Modifies test for new method

Signed-off-by: Voldivh <[email protected]>
gazebosim · Oct 27, 2023 · 5f10700 · 5f10700
1 parent 7151c65
commit 5f10700
Show file tree

Hide file tree

Showing 2 changed files with 10 additions and 4 deletions.
diff --git a/python/test/link_TEST.py b/python/test/link_TEST.py
@@ -80,7 +80,7 @@ def on_pre_udpate_cb(_info, _ecm):
             self.assertEqual(Vector3d(), link.world_linear_acceleration(_ecm))
             self.assertEqual(Vector3d(), link.world_angular_acceleration(_ecm))
             # World Inertia Matrix Test
-            self.assertEqual(Matrix3d(1,0,0,0,1,0,0,0,1), link.world_inertia_matrix(_ecm))
+            self.assertEqual(Matrix3d(1, 0, 0, 0, 1, 0, 0, 0, 1), link.world_inertia_matrix(_ecm))
             # World Kinetic Energy Test
             self.assertEqual(0, link.world_kinetic_energy(_ecm))
             link.enable_velocity_checks(_ecm, False)

diff --git a/test/integration/link.cc b/test/integration/link.cc
@@ -371,13 +371,19 @@ TEST_F(LinkIntegrationTest, LinkInertia)
 
   math::MassMatrix3d linkMassMatrix(10.0, {0.4, 0.4, 0.4}, {0.02, 0.02, 0.02});
   math::Pose3d linkWorldPose;
-  linkWorldPose.Set(0.0, 0.1, 0.2, 0.0, GZ_PI_4, GZ_PI_2);
-  math::Inertiald linkInertial{linkMassMatrix, linkWorldPose};
+  linkWorldPose.Set(1.0, 0.0, 0.0, 0, 0, GZ_PI_4);
+  // This is the pose of the inertia frame relative to its parent link frame
+  math::Pose3d inertiaPose;
+  inertiaPose.Set(1.0, 2.0, 3.0, 0, GZ_PI_2, 0);
+
+  math::Inertiald linkInertial{linkMassMatrix, inertiaPose};
+
+  ecm.CreateComponent(eLink, components::WorldPose(linkWorldPose));
   ecm.CreateComponent(eLink, components::Inertial(linkInertial));
 
   ASSERT_TRUE(link.WorldInertial(ecm));
   EXPECT_EQ(10.0, link.WorldInertial(ecm).value().MassMatrix().Mass());
-  EXPECT_EQ(linkWorldPose, link.WorldInertial(ecm).value().Pose());
+  EXPECT_EQ(linkWorldPose * inertiaPose, link.WorldInertial(ecm).value().Pose());
 }
 
 //////////////////////////////////////////////////