Implements a method to get the link inertia

Signed-off-by: Voldivh <[email protected]>
gazebosim · Oct 25, 2023 · 4374ae7 · 4374ae7
1 parent 71d965a
commit 4374ae7
Show file tree

Hide file tree

Showing 5 changed files with 61 additions and 2 deletions.
diff --git a/include/gz/sim/Link.hh b/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

diff --git 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
@@ -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
@@ -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,22 @@ 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

diff --git a/test/integration/link.cc b/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)
 {