Is there a way to start a simulation with a link (with a low stiffness coefficient) at a certain compression?

Question

I am trying to model links as springs so I have added drake:point_contact_stiffness parameters to the URDF. However, I want to control how much the link is deflected at the start of the simulation. Is there a way to start the simulation with the link at an arbitrary deflection?

I tried to influence this value by moving a mass on the link up and down, so I changed the URDF to include a prismatic joint between the leg link and hip mass. In order to get the link to start a bit compressed, I hoped I could "push" the hip mass onto the leg link using the prismatic joint. In simulation, this did not work very well and the leg link and hip mass would fly apart from each other.

Here is an example on how the collision dynamics were added to the ground and leg links:

<visual>
  <origin xyz="0 0 -.25" rpy="0 0 0"/>
  <geometry>
    <box size="10 1 .5" />
  </geometry>
  <material name="green">
    <color rgba="0 1 0 1" />
  </material>
</visual>

<collision name="collision_ground">
  <origin xyz="0 0 -.25" rpy="0 0 0"/>
  <geometry>
    <box size="10 1 .5" />
  </geometry>
  <drake:proximity_properties>
     <drake:hunt_crossley_dissipation value="0.1"/>
     <drake:point_contact_stiffness value="10000"/>>
    </drake:proximity_properties>
</collision> 

And here I am trying to simulate the walker model for 0.9 second from an initial condition. Ideally, here I would like the leg link to start with some compression.

builder = DiagramBuilder()
compass_walker, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0.0)
Parser(compass_walker).AddModelFromFile(file_name="compass.urdf")

compass_walker.Finalize()
compass_walker.set_name("cw")

meshcat.Delete()
visualizer = MeshcatVisualizer.AddToBuilder(builder, scene_graph, meshcat)
visualizer.set_name("visualizer")
logger = LogVectorOutput(compass_walker.get_state_output_port(), builder)


# finish building the block diagram
diagram = builder.Build()
simulator = Simulator(diagram)


context = simulator.get_mutable_context()
plant_context = compass_walker.GetMyMutableContextFromRoot(context)

context.SetContinuousState([0.0, 0.0, 0.156, -0.312, 0.0, 0.0, -0.3975, -0.3065]) #8 states


visualizer.StartRecording()
simulator.AdvanceTo(0.908)
visualizer.PublishRecording()

log = logger.FindLog(context)

Answer 1

If you want to "model a link as a spring", I would think that you would want to have two links that are connected to each other via a prismatic joint and a LinearSpringDamper between them. Then setting the initial state of the prismatic joint gives you a way to set the initial length of the spring.

Note: you'll need to have non-zero mass on either side of the spring if either side of the spring is a "leaf" in the kinematic tree -- the very simple legged robot models that have massless toes typically have to be done manually and don't use the multibody dynamics engine. For instance, the spring-loaded inverted pendulum model in the underactuated notes is derived by hand for this reason; the dynamics of that model are not well-defined except when the foot is in contact with the ground, and we have to explicitly ignore physics in the aerial phase.

You're right that the soft contact model in the ground also implements a spring stiffness (typically this would be higher than the stiffness of any internal springs). This will apply forces in the normal direction of the ground, not along the axis of your leg, so it would be a coarse approximation of a leg spring at best. If you do go with this approach, then setting the initial position of the robot to achieve a certain penetration into the ground could be done by solving an InverseKinematics problem.