From 4374ae7bd003202152ec9f74101ffbf0780528c5 Mon Sep 17 00:00:00 2001 From: Voldivh Date: Wed, 25 Oct 2023 15:27:21 -0500 Subject: [PATCH 1/6] Implements a method to get the link inertia Signed-off-by: Voldivh --- include/gz/sim/Link.hh | 8 ++++++++ python/src/gz/sim/Link.cc | 3 +++ python/test/link_TEST.py | 6 +++++- src/Link.cc | 17 ++++++++++++++++- test/integration/link.cc | 29 +++++++++++++++++++++++++++++ 5 files changed, 61 insertions(+), 2 deletions(-) diff --git a/include/gz/sim/Link.hh b/include/gz/sim/Link.hh index d67a17d4de..842e287025 100644 --- a/include/gz/sim/Link.hh +++ b/include/gz/sim/Link.hh @@ -22,6 +22,7 @@ #include #include +#include #include #include #include @@ -173,6 +174,13 @@ namespace gz public: std::optional WorldPose( const EntityComponentManager &_ecm) const; + /// \brief Get the world inertia of the link. + /// \param[in] _ecm Entity-component manager. + /// \return Inertia of the link pose in world frame or nullopt + /// if the link does not have the component components::Inertial. + public: std::optional WorldInertial( + const EntityComponentManager &_ecm) const; + /// \brief Get the world pose of the link inertia. /// \param[in] _ecm Entity-component manager. /// \return Inertial pose in world frame or nullopt if the diff --git a/python/src/gz/sim/Link.cc b/python/src/gz/sim/Link.cc index 51a6b15807..1c8bb8381d 100644 --- a/python/src/gz/sim/Link.cc +++ b/python/src/gz/sim/Link.cc @@ -79,6 +79,9 @@ void defineSimLink(py::object module) .def("world_pose", &gz::sim::Link::WorldPose, py::arg("ecm"), "Get the pose of the link frame in the world coordinate frame.") + .def("world_inertial", &gz::sim::Link::WorldInertial, + py::arg("ecm"), + "Get the inertia of the link in the world frame.") .def("world_inertial_pose", &gz::sim::Link::WorldInertialPose, py::arg("ecm"), "Get the world pose of the link inertia.") diff --git a/python/test/link_TEST.py b/python/test/link_TEST.py index 35071869e1..94cc03491b 100755 --- a/python/test/link_TEST.py +++ b/python/test/link_TEST.py @@ -18,7 +18,7 @@ from gz_test_deps.common import set_verbosity from gz_test_deps.sim import K_NULL_ENTITY, TestFixture, Link, Model, World, world_entity -from gz_test_deps.math import Matrix3d, Vector3d, Pose3d +from gz_test_deps.math import Inertiald, Matrix3d, Vector3d, Pose3d class TestModel(unittest.TestCase): post_iterations = 0 @@ -59,6 +59,10 @@ def on_pre_udpate_cb(_info, _ecm): # Visuals Test self.assertNotEqual(K_NULL_ENTITY, link.visual_by_name(_ecm, 'visual_test')) self.assertEqual(1, link.visual_count(_ecm)) + # World Inertial Test + self.assertEqual(Pose3d(), link.world_inertial(_ecm).pose()) + self.assertEqual(Matrix3d(1,0,0,0,1,0,0,0,1), link.world_inertial(_ecm).moi()) + self.assertEqual(10.0, link.world_inertial(_ecm).mass_matrix().mass()) # World Inertial Pose Test. self.assertEqual(Pose3d(), link.world_inertial_pose(_ecm)) # World Velocity Test diff --git a/src/Link.cc b/src/Link.cc index 426c57e780..fe5d844d7b 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -15,7 +15,6 @@ * */ -#include #include #include #include @@ -190,6 +189,22 @@ std::optional Link::WorldPose( .value_or(sim::worldPose(this->dataPtr->id, _ecm)); } +////////////////////////////////////////////////// +std::optional Link::WorldInertial( + const EntityComponentManager &_ecm) const +{ + auto inertial = _ecm.Component(this->dataPtr->id); + auto worldPose = _ecm.ComponentData(this->dataPtr->id) + .value_or(sim::worldPose(this->dataPtr->id, _ecm)); + + if (!inertial) + return std::nullopt; + + const math::Pose3d &comWorldPose = + worldPose * inertial->Data().Pose(); + return std::make_optional(math::Inertiald(inertial->Data().MassMatrix(), comWorldPose)); +} + ////////////////////////////////////////////////// std::optional Link::WorldInertialPose( const EntityComponentManager &_ecm) const diff --git a/test/integration/link.cc b/test/integration/link.cc index 6603633ae0..245716dace 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -351,6 +351,35 @@ TEST_F(LinkIntegrationTest, LinkAccelerations) ecm.Component(eLink)); } +////////////////////////////////////////////////// +TEST_F(LinkIntegrationTest, LinkInertia) +{ + EntityComponentManager ecm; + EventManager eventMgr; + SdfEntityCreator creator(ecm, eventMgr); + + auto eLink = ecm.CreateEntity(); + ecm.CreateComponent(eLink, components::Link()); + + Link link(eLink); + EXPECT_EQ(eLink, link.Entity()); + + ASSERT_TRUE(link.Valid(ecm)); + + // Before we add components, pose functions should return nullopt + EXPECT_EQ(std::nullopt, link.WorldInertial(ecm)); + + 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}; + 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()); +} + ////////////////////////////////////////////////// TEST_F(LinkIntegrationTest, LinkInertiaMatrix) { From 44a8869362e86464a6857aaaa2331e4c63e4c274 Mon Sep 17 00:00:00 2001 From: Voldivh Date: Wed, 25 Oct 2023 15:36:57 -0500 Subject: [PATCH 2/6] Corrects line length according to cpplint Signed-off-by: Voldivh --- src/Link.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/src/Link.cc b/src/Link.cc index fe5d844d7b..4fef211baf 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -202,7 +202,8 @@ std::optional Link::WorldInertial( const math::Pose3d &comWorldPose = worldPose * inertial->Data().Pose(); - return std::make_optional(math::Inertiald(inertial->Data().MassMatrix(), comWorldPose)); + return std::make_optional( + math::Inertiald(inertial->Data().MassMatrix(), comWorldPose)); } ////////////////////////////////////////////////// From 7151c65e957c318ff9205f99c7b5db62e0768899 Mon Sep 17 00:00:00 2001 From: Voldivh Date: Fri, 27 Oct 2023 12:33:42 -0500 Subject: [PATCH 3/6] Adds whitespaces between series of numbers Signed-off-by: Voldivh --- python/test/link_TEST.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/python/test/link_TEST.py b/python/test/link_TEST.py index 94cc03491b..c5994b5e1e 100755 --- a/python/test/link_TEST.py +++ b/python/test/link_TEST.py @@ -61,7 +61,7 @@ def on_pre_udpate_cb(_info, _ecm): self.assertEqual(1, link.visual_count(_ecm)) # World Inertial Test self.assertEqual(Pose3d(), link.world_inertial(_ecm).pose()) - self.assertEqual(Matrix3d(1,0,0,0,1,0,0,0,1), link.world_inertial(_ecm).moi()) + self.assertEqual(Matrix3d(1, 0, 0, 0, 1, 0, 0, 0, 1), link.world_inertial(_ecm).moi()) self.assertEqual(10.0, link.world_inertial(_ecm).mass_matrix().mass()) # World Inertial Pose Test. self.assertEqual(Pose3d(), link.world_inertial_pose(_ecm)) From 5f107009bfa2d8fd468f5a994271386404c77032 Mon Sep 17 00:00:00 2001 From: Voldivh Date: Fri, 27 Oct 2023 15:19:44 -0500 Subject: [PATCH 4/6] 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()); } ////////////////////////////////////////////////// From 06287649383f57d1dd3d8bf05e37b51d00cd0bf2 Mon Sep 17 00:00:00 2001 From: Voldivh Date: Fri, 27 Oct 2023 15:22:07 -0500 Subject: [PATCH 5/6] Removes whiteline Signed-off-by: Voldivh --- test/integration/link.cc | 1 - 1 file changed, 1 deletion(-) diff --git a/test/integration/link.cc b/test/integration/link.cc index 6f9f3081ba..5536658b18 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -375,7 +375,6 @@ TEST_F(LinkIntegrationTest, LinkInertia) // 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)); From 03be14fdd25da01c914b87d6445657c1204f4435 Mon Sep 17 00:00:00 2001 From: Voldivh Date: Fri, 27 Oct 2023 15:40:04 -0500 Subject: [PATCH 6/6] Reduce line length for cpplint Signed-off-by: Voldivh --- test/integration/link.cc | 3 ++- 1 file changed, 2 insertions(+), 1 deletion(-) diff --git a/test/integration/link.cc b/test/integration/link.cc index 5536658b18..9e140a9499 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -382,7 +382,8 @@ TEST_F(LinkIntegrationTest, LinkInertia) ASSERT_TRUE(link.WorldInertial(ecm)); EXPECT_EQ(10.0, link.WorldInertial(ecm).value().MassMatrix().Mass()); - EXPECT_EQ(linkWorldPose * inertiaPose, link.WorldInertial(ecm).value().Pose()); + EXPECT_EQ(linkWorldPose * inertiaPose, + link.WorldInertial(ecm).value().Pose()); } //////////////////////////////////////////////////