bullet-featherstone: Update fixed constraint behavior

[iche033]

🎉 New feature

Summary

There is an inconsistency between how fixed constraints work in dartsim and bullet-featherstone. Currently when a fixed constraint's parent link moves, the behavior in the two physics engine is:

• dartsim: child link is welded to parent and so it moves along with the parent -
• bullet-featherstone - corrects position errors and enforces constraint by moving both the parent and child links

Since there is desire to have both types of behavior so a new feature is added to toggle behavior of a fixed constraint:

• SetFixedJointWeldChildToParentFeature

By default, this property is true, i.e. child is always welded to parent link, so we do not break the fixed constraint behavior in dartsim. The bullet-featherstone fixed constraint implementation is updated to match this behavior. This is achieved by setting the pose of child at each iteration - not ideal but gives the expected behavior. Additionally, the bullet-featherstone plugin implements SetFixedJointWeldChildToParentFeature and lets users disable welding of links.

Update

• bullet-featherstone fixed joint implementation now calls setMaxAppliedImpulse to enforce fixed constraint.
• When the pose of the parent body of a fixed constraint is changed by FreeGroupSetWorldPose, the child pose is updated to enforce pose transform relative to the parent

Other changes to bullet-featherstone's fixed constraint implementation include:

• prevent collision between parent and child link if parent is static
• reverted some of the test/common_test/joint_features.cc changes done in bullet-featherstone: Improve mesh collision stability #600 now that fixed constraints are more stable

Added new tests:

• test attaching fixed joint between non-static and static links
• test behavior changes with the new API provdided by theSetFixedJointWeldChildToParentFeature

known issues:

• overlapping parent and child links held together by fixed constraint still do not behave as expected in bullet version < 3.07:
  • if both parent and child links are non-static, they will be unstable. The setIgnoreCollisionCheck API does not seem to have an effect
  • If parent link is static, the 2 links will not collide but child link still produces non-zero velocities.

Test it

Here's an example showing the behavior of fixed constraint in bullet-featherstone plugin before and after the changes. The two models are connected using the Detachable Joint plugin. The red box is the parent while the blue box is the child.

Before:

After (matches dartsim fixed constraint behavior):

Checklist

• Signed all commits for DCO
• Added tests
• Added example and/or tutorial
• Updated documentation (as needed)
• Updated migration guide (as needed)
• Consider updating Python bindings (if the library has them)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

[azeey]

It seems like this won't update children recursively. Consider 3 models A (static), B (dynamic), C (dynamic). If we first attach B->C, where B is the parent and then A->B, the result would be A (static), B (static), C (dynamic) whereas we expect all models to be static.

[iche033]

updated to recursively set collision flags and added a test. d9b0bdb

[azeey]

I tried adding the following here

    jointInfo->fixedConstraint->setMaxAppliedImpulse(btScalar(1e9));

and disabling fixedConstraintWeldChildToParent in Base.hh. By default the max impulse is 100. Setting a high value like this made it so that the constraint is satisfied with minimal oscillations. As we discussed, the transform tool does not apply forces on the bodies, so the resulting behavior cannot be relied upon to determine if the physics is correct. Instead, I set up a simulation where two boxes are attached by a detachable joint and the parent box is attached to a fixed base with a prismatic joint.

Test SDFormat file

<?xml version="1.0"?>
<sdf version="1.11">
  <world name="default">
  <light type="directional" name="sun">
      <cast_shadows>true</cast_shadows>
      <pose>0 0 10 0 0 0</pose>
      <diffuse>1 1 1 1</diffuse>
      <specular>0.5 0.5 0.5 1</specular>
      <attenuation>
        <range>1000</range>
        <constant>0.9</constant>
        <linear>0.01</linear>
        <quadratic>0.001</quadratic>
      </attenuation>
      <direction>-0.5 0.1 -0.9</direction>
    </light>

    <model name="ground_plane">
      <static>true</static>
      <link name="link">
        <collision name="collision">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
            </plane>
          </geometry>
        </collision>
        <visual name="visual">
          <geometry>
            <plane>
              <normal>0 0 1</normal>
              <size>100 100</size>
            </plane>
          </geometry>
          <material>
            <ambient>0.8 0.8 0.8 1</ambient>
            <diffuse>0.8 0.8 0.8 1</diffuse>
            <specular>0.8 0.8 0.8 1</specular>
          </material>
        </visual>
      </link>
    </model>
    <model name="M1">
      <joint name="j_base" type="fixed">
        <parent>world</parent>
        <child>base</child>
      </joint>
      <link name="base">
        <pose>0 0 0.05  0 0 0</pose>
        <inertial auto="true"/>
        <collision name="box_collision">
          <density>1000</density>
          <geometry>
            <box>
              <size>0.1 0.1 0.1</size>
            </box>
          </geometry>
        </collision>
        <visual name="box_visual">
          <geometry>
            <box>
              <size>0.1 0.1 0.1</size>
            </box>
          </geometry>
          <material>
            <ambient>1 0 0 0.1</ambient>
            <diffuse>1 0 1 0.1</diffuse>
            <specular>1 0 0 1</specular>
          </material>
        </visual>
      </link>
      <joint name="j1" type="prismatic">
        <parent>base</parent>
        <child>body</child>
        <axis>
          <xyz>0 1 0</xyz>
        </axis>
      </joint>
      <plugin
        filename="gz-sim-joint-position-controller-system"
        name="gz::sim::systems::JointPositionController">
        <joint_name>j1</joint_name>
        <p_gain>20000</p_gain>
        <d_gain>10000</d_gain>
        <cmd_max>10000</cmd_max>
        <cmd_min>-10000</cmd_min>
        <use_velocity_commands>true</use_velocity_commands>
        <topic>cmd_pos</topic>
      </plugin>
      <link name="body">
        <pose>1 0 0.25 0 0.0 0.0</pose>
        <inertial auto="true"/>
        <collision name="box_collision">
          <density>100</density>
          <geometry>
            <box>
              <size>1 1 0.5</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>0.1</mu>
                <mu2>0.1</mu2>
              </ode>
              <bullet>
                <friction>0.1</friction>
                <friction2>0.1</friction2>
              </bullet>
            </friction>
          </surface>
        </collision>

        <visual name="box_visual">
          <geometry>
            <box>
              <size>1 1 0.5</size>
            </box>
          </geometry>
          <material>
            <ambient>1 0 0 1</ambient>
            <diffuse>1 0 0 1</diffuse>
            <specular>1 0 0 1</specular>
          </material>
        </visual>
      </link>
      <plugin filename="gz-sim-detachable-joint-system"
              name="gz::sim::systems::DetachableJoint">
       <parent_link>body</parent_link>
       <child_model>M2</child_model>
       <child_link>body2</child_link>
      </plugin>
    </model>

    <model name="M2">
      <pose>3.3 0 0.25 0 0.0 0</pose>
      <link name="body2">
        <inertial auto="true"/>
        <collision name="box_collision">
          <density>100</density>
          <geometry>
            <box>
              <size>1 1 0.5</size>
            </box>
          </geometry>
          <surface>
            <friction>
              <ode>
                <mu>1</mu>
                <mu2>1</mu2>
              </ode>
              <bullet>
                <friction>1</friction>
                <friction2>1</friction2>
              </bullet>
            </friction>
          </surface>
        </collision>

        <visual name="box_visual">
          <geometry>
            <box>
              <size>1 1 0.5</size>
            </box>
          </geometry>
          <material>
            <ambient>0 1 0 1</ambient>
            <diffuse>0 1 0 1</diffuse>
            <specular>1 0 0 1</specular>
          </material>
        </visual>
      </link>
    </model>
  </world>
</sdf>

Test command

gz topic -t /cmd_pos -m gz.msgs.Double -p "data: 4.0" 

Here are the results:

• DART
  dart.webm

• Bullet-featherstone gz-physics7 branch
  bullet-default.webm

• Bullet-featherstone current PR
  current-PR.webm

• Bullet-featherstone proposed change: setting a high max impulse
  max_impulse.webm

I moved your changes in SimulationFeatures.cc directly into FreeGroupFeatures::SetFreeGroupWorldPose and that solved the transform tool issue.

[iche033]

ok thanks for testing. I revised this PR based on the suggestions. Changes are in d9b0bdb:

• added jointInfo->fixedConstraint->setMaxAppliedImpulse(btScalar(1e9)); when creating attaching joint
• moved code for enforcing child body pose to SetFreeGroupWorldPose and updated test
• removed SetFixedJointWeldChildToParent feature

[codecov]

Codecov Report

Attention: Patch coverage is 92.23301% with 8 lines in your changes are missing coverage. Please review.

Project coverage is 79.04%. Comparing base (92e02c3) to head (c921141).
Report is 16 commits behind head on gz-physics7.

❗ Current head c921141 differs from pull request most recent head a112092

Please upload reports for the commit a112092 to get more accurate results.

Files	Patch %	Lines
bullet-featherstone/src/JointFeatures.cc	92.42%	5 Missing ⚠️
bullet-featherstone/src/FreeGroupFeatures.cc	90.62%	3 Missing ⚠️

Additional details and impacted files

@@               Coverage Diff               @@
##           gz-physics7     #632      +/-   ##
===============================================
+ Coverage        78.32%   79.04%   +0.71%     
===============================================
  Files              140      140              
  Lines             8069     8240     +171     
===============================================
+ Hits              6320     6513     +193     
+ Misses            1749     1727      -22     

☔ View full report in Codecov by Sentry.
📢 Have feedback on the report? Share it here.

[azeey]

I have created a suggestion PR #646