Merge pull request #2218 from gazebosim/voldivh/world_inertial

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
@@ -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)

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,41 @@ 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(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)
 {