fix model frame data

Signed-off-by: Ian Chen <[email protected]>
gazebosim · Jun 6, 2024 · 8bcb176 · 8bcb176
1 parent 081c672
commit 8bcb176
Show file tree

Hide file tree

Showing 2 changed files with 7 additions and 21 deletions.
diff --git a/dartsim/src/KinematicsFeatures.cc b/dartsim/src/KinematicsFeatures.cc
@@ -64,14 +64,6 @@ FrameData3d KinematicsFeatures::FrameDataRelativeToWorld(
 const dart::dynamics::Frame *KinematicsFeatures::SelectFrame(
     const FrameID &_id) const
 {
-  const auto model_it = this->models.idToObject.find(_id.ID());
-  if (model_it != this->models.idToObject.end())
-  {
-    // This is a model FreeGroup frame, so we'll use the first root link as the
-    // frame
-    return model_it->second->model->getRootBodyNode();
-  }
-
   auto framesIt = this->frames.find(_id.ID());
   if (framesIt == this->frames.end())
   {

diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc
@@ -217,11 +217,8 @@ TYPED_TEST(KinematicFeaturesTest, LinkFrameSemanticsPose)
 
     gz::math::Pose3d actualModelPose(1, 0, 0, 0, 0, 0);
     auto modelFrameData = model->FrameDataRelativeToWorld();
-    if (this->PhysicsEngineName(name) == "bullet-featherstone")
-    {
-      EXPECT_EQ(actualModelPose,
-                gz::math::eigen3::convert(modelFrameData.pose));
-    }
+    EXPECT_EQ(actualModelPose,
+              gz::math::eigen3::convert(modelFrameData.pose));
     auto baseLinkFrameData = baseLink->FrameDataRelativeToWorld();
     auto baseLinkPose = gz::math::eigen3::convert(baseLinkFrameData.pose);
     gz::math::Pose3d actualLinkLocalPose(0, 1, 0, 0, 0, 0);
@@ -254,14 +251,11 @@ TYPED_TEST(KinematicFeaturesTest, LinkFrameSemanticsPose)
         linkColPose.Rot().Euler());
 
     gz::math::Pose3d actualNestedModelLocalPose(0, 0, 1, 0, 0, 0);
-    if (this->PhysicsEngineName(name) == "bullet-featherstone")
-    {
-      auto nestedModelFrameData = nestedModel->FrameDataRelativeToWorld();
-      gz::math::Pose3d expectedNestedModelWorldPose =
-          actualModelPose * actualNestedModelLocalPose;
-      EXPECT_EQ(expectedNestedModelWorldPose,
-          gz::math::eigen3::convert(nestedModelFrameData.pose));
-    }
+    auto nestedModelFrameData = nestedModel->FrameDataRelativeToWorld();
+    gz::math::Pose3d expectedNestedModelWorldPose =
+        actualModelPose * actualNestedModelLocalPose;
+    EXPECT_EQ(expectedNestedModelWorldPose,
+        gz::math::eigen3::convert(nestedModelFrameData.pose));
 
     auto nestedLinkFrameData = nestedLink->FrameDataRelativeToWorld();
     auto nestedLinkPose = gz::math::eigen3::convert(nestedLinkFrameData.pose);