fix for model with world joint, add test

Signed-off-by: Ian Chen <ichen@openrobotics.org>
gazebosim · Nov 11, 2024 · a7e6eb5 · a7e6eb5
1 parent 949b20e
commit a7e6eb5
Show file tree

Hide file tree

Showing 3 changed files with 71 additions and 8 deletions.
diff --git a/bullet-featherstone/src/FreeGroupFeatures.cc b/bullet-featherstone/src/FreeGroupFeatures.cc
@@ -85,17 +85,28 @@ Identity FreeGroupFeatures::FindFreeGroupForModel(
 {
   const auto *model = this->ReferenceInterface<ModelInfo>(_modelID);
 
-  // Also reject if the model is a child of a fixed constraint
-  // (detachable joint)
+  std::cerr <<  "joints " << this->joints.size() << std::endl;
   for (const auto & joint : this->joints)
   {
+    // Also reject if the model is a child of a fixed constraint
+    // (detachable joint)
     if (joint.second->fixedConstraint)
     {
       if (joint.second->fixedConstraint->getMultiBodyB() == model->body.get())
       {
         return this->GenerateInvalidId();
       }
     }
+    // Reject if the model has a world joint
+    if (std::size_t(joint.second->model) == std::size_t(_modelID))
+    {
+      const auto *identifier =
+          std::get_if<RootJoint>(&joint.second->identifier);
+      if (identifier)
+      {
+        return this->GenerateInvalidId();
+      }
+    }
   }
 
   return _modelID;

diff --git a/bullet-featherstone/src/SDFFeatures.cc b/bullet-featherstone/src/SDFFeatures.cc
@@ -578,6 +578,7 @@ Identity SDFFeatures::ConstructSdfModelImpl(
   // Add world joint
   if (structure.rootJoint)
   {
+    std::cerr << " adding root joint " << structure.rootJoint->Name() << std::endl;
     const auto &parentInfo = structure.parentOf.at(structure.rootLink);
     this->AddJoint(
           JointInfo{

diff --git a/test/common_test/free_joint_features.cc b/test/common_test/free_joint_features.cc
@@ -316,7 +316,7 @@ TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPosePrincipalAxesOffset)
   }
 }
 
-TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPoseStaticModel)
+TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPoseStaticAndFixedModel)
 {
   const std::string modelStr = R"(
     <sdf version="1.11">
@@ -335,6 +335,28 @@ TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPoseStaticModel)
       </model>
     </sdf>)";
 
+  const std::string modelWorldFixedStr = R"(
+    <sdf version="1.11">
+      <model name="sphere_world_fixed">
+        <static>false</static>
+        <pose>3 2 3.0 0 0 0</pose>
+        <link name="link">
+          <collision name="coll_sphere">
+            <geometry>
+              <sphere>
+                <radius>0.1</radius>
+              </sphere>
+            </geometry>
+          </collision>
+        </link>
+        <joint name="world_fixed" type="fixed">
+          <parent>world</parent>
+          <child>link</child>
+        </joint>
+      </model>
+    </sdf>)";
+
+
   for (const std::string &name : pluginNames)
   {
     std::cout << "Testing plugin: " << name << std::endl;
@@ -355,29 +377,58 @@ TEST_F(FreeGroupFeaturesTest, FreeGroupSetWorldPoseStaticModel)
     auto world = engine->ConstructWorld(*sdfWorld);
     ASSERT_NE(nullptr, world);
 
-    // create the model
+    // create the static model
     errors = root.LoadSdfString(modelStr);
     ASSERT_TRUE(errors.empty()) << errors;
     ASSERT_NE(nullptr, root.Model());
     world->ConstructModel(*root.Model());
 
-    auto model = world->GetModel("sphere");
-    ASSERT_NE(nullptr, model);
-    auto link = model->GetLink("link");
+    auto modelStatic = world->GetModel("sphere");
+    ASSERT_NE(nullptr, modelStatic);
+    auto link = modelStatic->GetLink("link");
     ASSERT_NE(nullptr, link);
     auto frameDataLink = link->FrameDataRelativeToWorld();
     EXPECT_EQ(gz::math::Pose3d(1, 2, 3, 0, 0, 0),
               gz::math::eigen3::convert(frameDataLink.pose));
 
     // get free group and set new pose
-    auto freeGroup = model->FindFreeGroup();
+    auto freeGroup = modelStatic->FindFreeGroup();
     ASSERT_NE(nullptr, freeGroup);
     gz::math::Pose3d newPose(4, 5, 6, 0, 0, 1.57);
     freeGroup->SetWorldPose(
         gz::math::eigen3::convert(newPose));
     frameDataLink = link->FrameDataRelativeToWorld();
+    // static model should move to new pose
     EXPECT_EQ(newPose,
               gz::math::eigen3::convert(frameDataLink.pose));
+
+    // create the model with world joint
+    errors = root.LoadSdfString(modelWorldFixedStr);
+    ASSERT_TRUE(errors.empty()) << errors;
+    ASSERT_NE(nullptr, root.Model());
+    world->ConstructModel(*root.Model());
+
+    auto modelFixed = world->GetModel("sphere_world_fixed");
+    ASSERT_NE(nullptr, modelFixed);
+    link = modelFixed->GetLink("link");
+    ASSERT_NE(nullptr, link);
+    frameDataLink = link->FrameDataRelativeToWorld();
+    gz::math::Pose3d origPose(3, 2, 3, 0, 0, 0);
+    EXPECT_EQ(origPose,
+              gz::math::eigen3::convert(frameDataLink.pose));
+
+    // get free group and set new pose
+    freeGroup = modelFixed->FindFreeGroup();
+    ASSERT_NE(nullptr, freeGroup);
+    freeGroup->SetWorldPose(
+        gz::math::eigen3::convert(newPose));
+    frameDataLink = link->FrameDataRelativeToWorld();
+    // world fixed model should not be able to move to new pose
+    if (this->PhysicsEngineName(name) != "tpe")
+    {
+      EXPECT_EQ(origPose,
+                gz::math::eigen3::convert(frameDataLink.pose));
+    }
   }
 }