Add failing test showing that model framedata calculation is incorrect

Signed-off-by: Addisu Z. Taddese <addisu@openrobotics.org>
gazebosim · Jun 5, 2024 · 27deae9 · 27deae9
1 parent 84054fa
commit 27deae9
Show file tree

Hide file tree

Showing 2 changed files with 5 additions and 1 deletion.
diff --git a/test/common_test/kinematic_features.cc b/test/common_test/kinematic_features.cc
@@ -80,7 +80,8 @@ struct KinematicFeaturesList : gz::physics::FeatureList<
     gz::physics::GetJointFromModel,
     gz::physics::JointFrameSemantics,
     gz::physics::LinkFrameSemantics,
-    gz::physics::ShapeFrameSemantics
+    gz::physics::ShapeFrameSemantics,
+    gz::physics::ModelFrameSemantics
 > { };
 
 using KinematicFeaturesTestTypes =
@@ -207,6 +208,8 @@ TYPED_TEST(KinematicFeaturesTest, LinkFrameSemanticsPose)
     ASSERT_NE(nullptr, linkCol);
 
     gz::math::Pose3d actualModelPose(1, 0, 0, 0, 0, 0);
+    auto modelFrameData = model->FrameDataRelativeToWorld();
+    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);

diff --git a/test/common_test/worlds/pose_offset.sdf b/test/common_test/worlds/pose_offset.sdf
@@ -6,6 +6,7 @@
       <link name="base">
         <pose>0 1 0 0 0 0</pose>
         <inertial>
+          <pose>0 0 3 0 0 0</pose>
           <mass>100</mass>
         </inertial>
         <collision name="base_collision">