bullet-featherstone: Support nested models

[iche033]

🎉 New feature

Closes #546

Summary

Extended bullet-featurestone implementation to support these features related to nested models:

• ConstructSdfJoint (limited to creating world joints)
• ConstructSdfNestedModel (bullet-featherstone: implement ConstructSdfNestedModel feature #546)
• GetNestedModelFromModel
• WorldModelFeature (bullet-featherstone: implement WorldModelFeature feature #547)
• RemoveEntities

The nested models are currently added to a single multibody since bullet-featherstone does not seem to support joints between 2 different multibodies. The nested model just keeps a shared ptr to the rootLink's btMultibody.

Test it

With the addition of the above features, more common tests are now being run using bullet-feathersone plugin. Note that I had to skip a few of tests in free_joint_features and joint_features because the bullet-featherstone implementation does not support 1) multiple floating bodies / sub-trees yet (#550) and 2) joints between two different models.

Here's an example nested model world file I use for testing. To run:

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

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.

[codecov]

Codecov Report

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

Project coverage is 79.06%. Comparing base (ea90ada) to head (d08d0ef).

Files	Patch %	Lines
bullet-featherstone/src/SDFFeatures.cc	76.45%	73 Missing ⚠️
...ullet-featherstone/src/EntityManagementFeatures.cc	71.69%	15 Missing ⚠️
bullet-featherstone/src/Base.hh	93.05%	5 Missing ⚠️

Additional details and impacted files

@@               Coverage Diff               @@
##           gz-physics6     #574      +/-   ##
===============================================
+ Coverage        78.52%   79.06%   +0.54%     
===============================================
  Files              140      140              
  Lines             7670     7950     +280     
===============================================
+ Hits              6023     6286     +263     
- Misses            1647     1664      +17     

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

[scpeters]

It's a little weird that the WorldModelAPI test in world_features.cc passes even though it's not actually creating the joints between //world/models. When I load world_joint_test.sdf in gz sim with bullet-featherstone, I see the following error messages, which aren't obvious to interpret but do indicate that something is wrong:

[Err] [Physics.cc:1710] Attempting to create a new joint [j1], but the physics engine doesn't support constructing joints at runtime.
[Err] [Physics.cc:1710] Attempting to create a new joint [j2], but the physics engine doesn't support constructing joints at runtime.

I also tried loading the nested_model.sdf example world from gz-sim, which I believe is not supported because it has multiple kinematic trees, but it results in a comprehensible error message that is obscured by infinite console spamming:

[Err] [SDFFeatures.cc:534] Multiple subt-trees / floating links detected in a model. This is not supported in bullet-featherstone implementation yet.
[Wrn] [Physics.cc:1285] Cannot create a new link [link_01] because the physics engine plugin does not support link construction during runtime
[Wrn] [Physics.cc:1372] Collision's parent entity [13] not found on link map.
[Err] [Physics.cc:2894] Internal error: link [13] not in entity map
[Err] [Physics.cc:3050] Internal error: entity [13] not in entity map
...
other messages
...
[Err] [Physics.cc:2894] Internal error: link [13] not in entity map
[Err] [Physics.cc:2894] Internal error: link [13] not in entity map
[Err] [Physics.cc:2894] Internal error: link [13] not in entity map
[Err] [Physics.cc:2894] Internal error: link [13] not in entity map
...

This may be a gz-sim problem but if there's a way to mitigate it in gz-physics, that would be helpful to users.

I also get infinite console spam when loading the nested_model_joint_positions.sdf example world from gz-sim unless I remove model4

[iche033]

It's a little weird that the WorldModelAPI test in world_features.cc passes even though it's not actually creating the joints between //world/models

yeah the WorldModelAPI test checks the parent-child relationship between world and models only. It does not actually require a world joint to be present for a model to be a child of the 'world model'.

When I load world_joint_test.sdf in gz sim with bullet-featherstone, I see the following error messages, which aren't obvious to interpret but do indicate that something is wrong:

I added support for the ConstructSdfJoint feature which is used by gz-sim for creating a //world/joint. This new function is currently also only limited to creating world joints (and not other joints between or within models). 2c85f5e

I also tried loading the nested_model.sdf example world from gz-sim, which I believe is not supported because it has multiple kinematic trees, but it results in a comprehensible error message that is obscured by infinite console spamming:

I added necessary logic to add all remaining links (floating bodies / other subtrees) in gz-physics but without bullet structures. This is just for book-keeping and prevents gz-sim from printing warning messages when trying to access these links. Added warning msgs to let users know that these links are non-functional. 2c85f5e and 06016b7

I also get infinite console spam when loading the nested_model_joint_positions.sdf example world from gz-sim unless I remove model4

Fixed issue with an empty top level model that has no links. 2c85f5e

[iche033]

model4 is now next to model1 on the far left, but sunk into the ground and bounces away when the simulation starts

Fixed a todo item and that resolved the pose issue of model4 which occurs when the root link is inside a nested model (as opposed to a top level model). 575e73b

[scpeters]

overall it looks good; I left a few minor comments and will approve once they are resolved

[scpeters]

nit: I think this check could be removed, since RemoveModelImpl performs the same check for the existence of the model

[iche033]

removed. ca1715e

[scpeters]

well, now I think I was wrong, since I didn't notice that we are dereferencing model on the next line. Those two lines from ca1715e should probably come back. Thanks for your patience

[iche033]

ah yes, reverted change. d08d0ef

[iche033]

overall it looks good; I left a few minor comments and will approve once they are resolved

thanks for the review! Addressed all comments.

[scpeters]

also, one small patch to improve test coverage:

diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc
index 640e185c..e1e68c0f 100644
--- a/test/common_test/world_features.cc
+++ b/test/common_test/world_features.cc
@@ -434,6 +434,10 @@ TEST_F(WorldNestedModelTest, WorldConstructNestedModel)
     gz::physics::ForwardStep::State state;
     gz::physics::ForwardStep::Output output;
 
+    // Check invalid input to RemoveNestedModel method
+    EXPECT_FALSE(worldModel->RemoveNestedModel(1));
+    EXPECT_FALSE(worldModel->RemoveNestedModel("invalid"));
+
     // Check that we can remove models via RemoveNestedModel
     EXPECT_TRUE(worldModel->RemoveNestedModel(0));
     EXPECT_TRUE(nestedModel->Removed());

[scpeters]

just two remaining comments: a suggested revert of one of my suggestions and a patch to improve coverage

[iche033]

also, one small patch to improve test coverage:

Added, thanks. d08d0ef