Implements a method to get the link inertia

include/gz/sim/Link.hh

@@ -22,6 +22,7 @@
 #include <string>
 #include <vector>
 
+#include <gz/math/Inertial.hh>
 #include <gz/math/Matrix3.hh>
 #include <gz/math/Pose3.hh>
 #include <gz/math/Quaternion.hh>
@@ -173,6 +174,13 @@ namespace gz
  public: std::optional<math::Pose3d> 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<math::Inertiald> 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

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.")

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

src/Link.cc

@@ -15,7 +15,6 @@
  *
  */
 
-#include <gz/math/Inertial.hh>
 #include <gz/math/Matrix3.hh>
 #include <gz/math/Pose3.hh>
 #include <gz/math/Vector3.hh>
@@ -190,6 +189,23 @@ std::optional<math::Pose3d> Link::WorldPose(
  .value_or(sim::worldPose(this->dataPtr->id, _ecm));
 }
 
+//////////////////////////////////////////////////
+std::optional<math::Inertiald> Link::WorldInertial(
+ const EntityComponentManager &_ecm) const
+{
+ auto inertial = _ecm.Component<components::Inertial>(this->dataPtr->id);
+ auto worldPose = _ecm.ComponentData<components::WorldPose>(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<math::Pose3d> Link::WorldInertialPose(
  const EntityComponentManager &_ecm) const

test/integration/link.cc

@@ -351,6 +351,35 @@ TEST_F(LinkIntegrationTest, LinkAccelerations)
  ecm.Component<components::WorldAngularAcceleration>(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)
 {