Joint limits in Drake

[amcastro-tri]

Currently (the discrete) MultibodyPlant uses soft constraints to impose joint position limits through a penalty method.

As reported in #12775 and recently in SO, sometimes MBP fails to find the right set of penalty parameters to enforce these limits.

There is a distinction to be made here. Soft constraints are not a compliant model of joint limits, but rather a way to regularize the hard constraints we want such that they become more tractable to our solvers. I believe this distinction is important since our user facing APIs will (should) reflect these abstractions.

When attempting to solve #12775 by providing users knobs to tweak these penalty parameters, I relized that I was probably opening a can of worms. The reason being that I'd be mixing the concept of soft constraints (a trick used by our solver, could potentially change in the future) vs. a modeling choice (should not change under the hood if a user decided on a specific physical model). This has even spawned a discussion in our internal Slack about the danger of breaking our current simulations.

Proposed solution

What I propose is not a full solution, but rather a first step towards a final solution. And, specifically, it addresses the immediate need of having a plan B when something like #12775 or SO comes up.

What I propose is:

1. Do not modify what MBP currently does with its internal penalty method. This goes along the idea of backwards compatibility with sims we run today.
2. MBP at least prints a warning message to the screen when it detects a situation like those reported in #12775 or SO. Doable actually in both of those cases.
3. Implement a physical CompliantJointLimit, a ForceElement, that users can add to their model if something like 2) happens to them.

Related

• Towards #12911. All forces in MBP should be a ForceElement and our future API should allow requests such as; "give me all force elements that apply to this body/joint".
• Similarly #12942, and the tracking of energy budgets.
• SDF parsing of compliant limits is supported.

[sherm1]

I like the idea of using a compliant force element to enforce joint limits. Simbody uses that method: here is the doxygen for it. (Note the Hunt & Crossley-like dissipation model to avoid a force discontinuity at the limit.)

A joint limit force element can serve two masters as does the bushing element:

• it can be used as a penalty method to enforce a hard constraint (with some relaxation), and
• it can be used to model a physical element in a mechanism, like a rubber stop.

For our purposes the constraint penalty use is most common. I favor a very simple default for this like the sdf file uses -- just pick a moderately stiff material with plenty of damping and document it (sdf uses k=1e8 d=1).

[jwnimmer-tri]

FYI #2517 is re: enforcing velocity limits.

[xuchenhan-tri]

Did #17083 fix this?

[jwnimmer-tri]

Maybe not? See https://github.com/RobotLocomotion/drake/issues/17731#issuecomment-1495891482.

[RussTedrake]

Here is another URDF/code snippet. It fails with TAMSI (still the default, though I know that's changing), but works with the SAP variants. (to reproduce, just comment out the line with set_discrete_contact_approximation). @amcastro-tri -- is this expected?

from pydrake.all import MultibodyPlant, Parser, Simulator, namedview, DiscreteContactApproximation

plant = MultibodyPlant(time_step=0.001)
plant.set_discrete_contact_approximation(DiscreteContactApproximation.kLagged)
parser = Parser(plant)
parser.AddModels("quadrotor_pole.urdf")
plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base_link"))
plant.Finalize()
PositionView = namedview("PositionView", plant.GetPositionNames(False, False))

print(f"q_min = {PositionView(plant.GetPositionLowerLimits())}")
print(f"q_max = {PositionView(plant.GetPositionUpperLimits())}")

simulator = Simulator(plant)
context = simulator.get_mutable_context()

q = PositionView(plant.GetPositions(context))
q.theta_x = 0.4
q.theta_y = 0.4
plant.SetPositions(context, q[:])
print(f"q(t=0) = {q}")
simulator.AdvanceTo(2.0)
q = PositionView(plant.GetPositions(context))
print(f"q(t=2.0) = {q}")

quadrotor_pole.urdf:

<?xml version="1.0" ?>

<!-- adapted from Daniel Mellinger, Nathan Michael, Vijay Kumar, "Trajectory Generation and Control for Precise Aggressive Maneuvers with Quadrotors" -->

<robot xmlns="http://drake.mit.edu"
 xmlns:xsi="http://www.w3.org/2001/XMLSchema-instance"
 xsi:schemaLocation="http://drake.mit.edu ../../../../pods/drake/doc/drakeURDF.xsd" name="quadrotor_pole">
  <link name="base_link">
    <inertial>
      <mass value="0.5"/>
      <origin xyz="0 0 0"/>
      <inertia ixx="0.0023" ixy="0.0" ixz="0.0" iyy="0.0023" iyz="0.0" izz="0.004"/>
    </inertial>
    <visual>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <mesh filename="quadrotor_base.obj" scale=".1 .1 .1"/>
      </geometry>
      <material>
         <color rgba="1 1 0 1" />
       </material>
    </visual>
    <!-- note: the original hector quadrotor urdf had a (simplified, but still complex) collision mesh, too -->
    <collision>
      <origin rpy="0 0 0" xyz="0 0 0"/>
      <geometry>
        <cylinder radius=".3" length=".06"/>
      </geometry>
    </collision>      
  </link>
  <frame link="base_link" name="body" rpy="0 0 0" xyz="0 0 0" />

  <force_element name="prop1">
    <propellor lower_limit="0" upper_limit="10" scale_factor_thrust="1.0" scale_factor_moment="0.0245">
      <parent link="base_link"/>
      <origin xyz=".1750 0 0"/>
      <axis xyz="0 0 1"/>
    </propellor>
  </force_element>

  <force_element name="prop2">
    <propellor lower_limit="0" upper_limit="10" scale_factor_thrust="1.0" scale_factor_moment="-0.0245">
      <parent link="base_link"/>
      <origin xyz="0 .1750 0 "/>
      <axis xyz="0 0 1"/>
    </propellor>
  </force_element>

  <force_element name="prop3">
    <propellor lower_limit="0" upper_limit="10" scale_factor_thrust="1.0" scale_factor_moment="0.0245">
      <parent link="base_link"/>
      <origin xyz="-.1750 0 0"/>
      <axis xyz="0 0 1"/>
    </propellor>
  </force_element>

  <force_element name="prop4">
    <propellor lower_limit="0" upper_limit="10" scale_factor_thrust="1.0" scale_factor_moment="-0.0245">
      <parent link="base_link"/>
      <origin xyz="0 -.1750 0"/>
      <axis xyz="0 0 1"/>
    </propellor>
  </force_element>

  <link name="pole_arm">
    <inertial>
        <mass value="0.01"/>
        <origin xyz="0 0 0.4"/>
        <inertia ixx="0.000209" ixy="0.0" ixz="0.0" iyy="0.000209" iyz="0.0" izz="0.0000005"/>
    </inertial>

    <visual name="pole_arm_viz">
    <origin xyz="0 0 0.28" rpy="0 0 0" />
    <geometry>
        <cylinder length=".5" radius="0.01" />
    </geometry>
    <material><diffuse>0 0 0 1</diffuse></material>
    </visual>
  </link>

  <link name="pole_arm_1"></link>
  <link name="pole_arm_2"></link>

  <joint name="theta_x" type="revolute">
    <parent link="pole_arm_1" />
    <child link="pole_arm_2" />
    <axis xyz="1 0 0" />
    <limit lower="-0.5" upper="0.5"/>
  </joint>

  <joint name="theta_y" type="revolute">
    <parent link="pole_arm_2" />
    <child link="pole_arm" />
    <axis xyz="0 1 0" />
    <limit lower="-0.5" upper="0.5"/>
  </joint>

  <joint name="ball_joint" type="fixed">
    <parent link="base_link" />
    <child link="pole_arm_1" />
    <origin xyz="0 0 0" rpy="0 0 0" />
  </joint>

</robot>