fix removing model with constraints in bullet-featherstone (#577)

Signed-off-by: Ian Chen <[email protected]>
gazebosim · Nov 15, 2023 · 53e1ee5 · 53e1ee5
1 parent 1e2bcdd
commit 53e1ee5
Show file tree

Hide file tree

Showing 3 changed files with 40 additions and 15 deletions.
diff --git a/bullet-featherstone/src/Base.hh b/bullet-featherstone/src/Base.hh
@@ -441,14 +441,25 @@ class Base : public Implements3d<FeatureList<Feature>>
     world->modelNameToEntityId.erase(model->name);
 
     // Remove all constraints related to this model
-    for (auto constraint_index : model->external_constraints)
+    for (const auto jointID : model->jointEntityIds)
     {
-      const auto joint = this->joints.at(constraint_index);
-      const auto &constraint =
-        std::get<std::unique_ptr<btMultiBodyConstraint>>(joint->identifier);
-      world->world->removeMultiBodyConstraint(constraint.get());
-      this->joints.erase(constraint_index);
+      const auto joint = this->joints.at(jointID);
+      if (joint->motor)
+      {
+        world->world->removeMultiBodyConstraint(joint->motor.get());
+      }
+      if (joint->fixedConstraint)
+      {
+        world->world->removeMultiBodyConstraint(joint->fixedConstraint.get());
+      }
+      if (joint->jointLimits)
+      {
+        world->world->removeMultiBodyConstraint(joint->jointLimits.get());
+      }
+      this->joints.erase(jointID);
     }
+    // \todo(iche033) Remove external constraints related to this model
+    // (model->external_constraints) once this is supported
 
     world->world->removeMultiBody(model->body.get());
     for (const auto linkID : model->linkEntityIds)
@@ -464,9 +475,6 @@ class Base : public Implements3d<FeatureList<Feature>>
       this->links.erase(linkID);
     }
 
-    for (const auto jointID : model->jointEntityIds)
-      this->joints.erase(jointID);
-
     this->models.erase(_modelID);
 
     return true;

diff --git a/test/common_test/world_features.cc b/test/common_test/world_features.cc
@@ -358,11 +358,12 @@ TEST_F(WorldModelTest, WorldModelAPI)
 
 struct WorldNestedModelFeatureList : gz::physics::FeatureList<
   GravityFeatures,
-  gz::physics::WorldModelFeature,
-  gz::physics::RemoveEntities,
+  gz::physics::ForwardStep,
   gz::physics::GetNestedModelFromModel,
   gz::physics::sdf::ConstructSdfModel,
-  gz::physics::sdf::ConstructSdfNestedModel
+  gz::physics::sdf::ConstructSdfNestedModel,
+  gz::physics::RemoveEntities,
+  gz::physics::WorldModelFeature
 > { };
 
 
@@ -408,13 +409,26 @@ TEST_F(WorldNestedModelTest, WorldConstructNestedModel)
     ASSERT_NE(nullptr, nestedModel);
     EXPECT_EQ("parent_model", nestedModel->GetName());
 
+    gz::physics::ForwardStep::Input input;
+    gz::physics::ForwardStep::State state;
+    gz::physics::ForwardStep::Output output;
+
     // Check that we can remove models via RemoveNestedModel
     EXPECT_TRUE(worldModel->RemoveNestedModel(0));
     EXPECT_TRUE(nestedModel->Removed());
     EXPECT_EQ(0u, world->GetModelCount());
     EXPECT_EQ(0u, worldModel->GetNestedModelCount());
     EXPECT_EQ(nullptr, worldModel->GetNestedModel(0));
     EXPECT_EQ(nullptr, worldModel->GetNestedModel("parent_model"));
+
+    // verify we can step the world after model removal
+    const size_t numSteps = 1000;
+    for (size_t i = 0; i < numSteps; ++i)
+    {
+      world->Step(output, state, input);
+    }
+    EXPECT_EQ(0u, world->GetModelCount());
+    EXPECT_EQ(0u, worldModel->GetNestedModelCount());
   }
 }
 

diff --git a/test/common_test/worlds/world_single_nested_model.sdf b/test/common_test/worlds/world_single_nested_model.sdf
@@ -49,9 +49,12 @@
         </visual>
       </link>
 
-      <joint name="joint1" type="fixed">
-         <parent>link1</parent>
-         <child>nested_model::nested_link1</child>
+      <joint name="joint1" type="revolute">
+        <parent>link1</parent>
+        <child>nested_model::nested_link1</child>
+        <axis>
+          <xyz>0 0 1</xyz>
+        </axis>
       </joint>
     </model>
   </world>