search

gazebosim / gz-physics

Abstract physics interface designed to support simulation and rapid development of robot applications.
https://gazebosim.org
Apache License 2.0
65 stars 40 forks source link

bullet-featherstone: Support nested models #574

Closed iche033 closed 7 months ago

iche033 commented 10 months ago

🎉 New feature

Closes https://github.com/gazebosim/gz-physics/issues/546

Summary

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

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 (https://github.com/gazebosim/gz-physics/issues/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

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[bot] commented 10 months ago

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 :warning:
...ullet-featherstone/src/EntityManagementFeatures.cc 71.69% 15 Missing :warning:
bullet-featherstone/src/Base.hh 93.05% 5 Missing :warning:
Additional details and impacted files ```diff @@ 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 ```

:umbrella: View full report in Codecov by Sentry.
:loudspeaker: Have feedback on the report? Share it here.

iche033 commented 10 months ago

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

scpeters commented 10 months ago

I just built this against gz-sim7 and when running the nested_model_joint_positions.sdf, it seems that bullet-featherstone is not putting model4 in the correct initial pose.

dart: nested_model_joint_positions.sdf

gz sim -v 4 src/gz-sim/examples/worlds/nested_model_joint_positions.sdf

Screenshot 2023-11-30 at 4 33 06 PM

model4 is on the far right

bullet-featherstone: nested_model_joint_positions.sdf

gz sim -v 4 src/gz-sim/examples/worlds/nested_model_joint_positions.sdf --physics-engine gz-physics-bullet-featherstone-plugin

Screenshot 2023-11-30 at 4 33 31 PM

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

iche033 commented 10 months ago

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

iche033 commented 7 months ago

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 commented 7 months ago

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());
iche033 commented 7 months ago

also, one small patch to improve test coverage:

Added, thanks. d08d0ef