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..4d6be57631 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 @@ -76,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/src/Link.cc b/src/Link.cc index 426c57e780..4fef211baf 100644 --- a/src/Link.cc +++ b/src/Link.cc @@ -15,7 +15,6 @@ * */ -#include #include #include #include @@ -190,6 +189,23 @@ 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..9e140a9499 100644 --- a/test/integration/link.cc +++ b/test/integration/link.cc @@ -351,6 +351,41 @@ 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(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 * inertiaPose, + link.WorldInertial(ecm).value().Pose()); +} + ////////////////////////////////////////////////// TEST_F(LinkIntegrationTest, LinkInertiaMatrix) {