From 5f107009bfa2d8fd468f5a994271386404c77032 Mon Sep 17 00:00:00 2001 From: Voldivh Date: Fri, 27 Oct 2023 15:19:44 -0500 Subject: [PATCH] Modifies test for new method Signed-off-by: Voldivh --- python/test/link_TEST.py | 2 +- test/integration/link.cc | 12 +++++++++--- 2 files changed, 10 insertions(+), 4 deletions(-) diff --git a/python/test/link_TEST.py b/python/test/link_TEST.py index c5994b5e1e..4d6be57631 100755 --- 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 index 245716dace..6f9f3081ba 100644 --- 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()); } //////////////////////////////////////////////////