bullet-featherstone: Improve mesh collision stability

[iche033]

🦟 Bug fix

Summary

Mesh collisions are quite unstable in our bullet-featherstone implementation. Sometimes they explode and fly away on contact. The bullet-featherstone's multibody constraint solve applies a large impulse (uncapped) when penetration occurs. To reduce this effect, we tune 2 parameters:

• collision margin - increased from 0.001 to 0.01
  • 0.01 is actually the default margin for GImpact meshes. The new value is also consistent with gazebo-classic which does not have margin set for GImpact meshes (so uses default value of 0.01)
• erp2 (contact constraint error reduction param) - Reduces from default value of 0.2 to 0.02. This affects the contact penetration impulse applied.

Here's original behavior:
erp = 0.2, margin = 0.001
Box meshes explode on contact

Here's testing with just increasing the margin and leaving the erp untouched:
erp = 0.2, margin = 0.01
Stability improved but there are still small bounces due to penetration

Here's the new behavior with improved stability even when the top box is dropped from higher z:
erp = 0.002, margin = 0.01

The downside of having a larger margin is that the mesh collision has a small gap when resting on top of one another.

This will need to be addressed

To test

To reproduce the tests above, first you'll need these 2 PRs that allow bullet-featherstone in gz-phsyics to load meshes from relative paths:

• Enable StoreResolvedURIs when loading SDF gz-sim#2323
• Resolve URIs relative to file path sdformat#1373

You can download the world and model files for testing:

mkdir -p box_mesh_test/box
wget https://gist.githubusercontent.com/iche033/9943a9bcf6ed9eede40ee3e9dbb4f8ad/raw/2f837c5221c3d56ee13f6191de4c0a449c36b0db/box_mesh_test.sdf
wget https://gist.githubusercontent.com/iche033/d13a8d8e0c05308f98ce1e5e105500ce/raw/681a2be4a2d53d36c9f0e415640258674ef977e5/model.sdf -O box_mesh_test/box/model.sdf
wget https://gist.githubusercontent.com/iche033/8c57b8882085dbce615419d5d472530c/raw/754f620ab034d218c0cda02453c4664968fe5935/box.obj -O box_mesh_test/box/box.obj
wget https://gist.githubusercontent.com/iche033/e59504e1b67f37f2ad30588d16fadb1b/raw/9a4dd30007345f017ca44f7da671eca6cddb9843/model.config -O box_mesh_test/box/model.config

Run gz-sim with bullet-featherstone plugin:

gz sim -v 4 box_mesh_test.sdf --physics-engine gz-physics-bullet-featherstone-plugin

Checklist

• Signed all commits for DCO
• Added tests
• 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.

[iche033]

It seems like a small erp2 value causes objects to be slight more unstable when two objects are attached by a joint. The new erp2 value caused a few vel checks in joint_features to fail and I had to increase tolerance for the tests to pass. bae4277. Let me know if that shouldn't be done.

[codecov]

Codecov Report

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

Project coverage is 79.07%. Comparing base (18e0dad) to head (801dbdd).

Files	Patch %	Lines
bullet-featherstone/src/SDFFeatures.cc	0.00%	1 Missing ⚠️

Additional details and impacted files

@@             Coverage Diff              @@
##           gz-physics6     #600   +/-   ##
============================================
  Coverage        79.06%   79.07%           
============================================
  Files              140      140           
  Lines             7950     7951    +1     
============================================
+ Hits              6286     6287    +1     
  Misses            1664     1664           

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

[shameekganguly]

nit: I'm slightly surprised that we need to increase the LinearVelocity.Z() thresholds for both body1 and body3 but not body2 with this change. I guess this is due to the weld joint constraint being imperfect in bullet? You could consider using a position and velocity based threshold here as well as I mentioned in my previous comment to make the check robust to future changes.

[iche033]

yeah I'm not sure why the constraints are more unstable. I tried using the while loop approach with position and vel thresholds but the checks are always satisfied (and loop exists) on the 1st iteration right after the joints are created since this is when the objects are most stable - if that makes sense. Given that the idea is to check that the objects are still at rest and stable after a few iterations, I reverted and kept the test the same as before. I just added position checks to make sure the boxes didn't move.