Bugfix: Fixed Centre of Mass and Inertia Matrix Calculation Bug MeshInertiaCalculator::CalculateMassProperties() function

src/MeshInertiaCalculator.cc

@@ -33,6 +33,7 @@
 #include <gz/math/Pose3.hh>
 #include <gz/math/MassMatrix3.hh>
 #include <gz/math/Inertial.hh>
+#include <gz/math/Quaternion.hh>
 
 using namespace gz;
 using namespace sim;
@@ -84,6 +85,11 @@ void MeshInertiaCalculator::CalculateMeshCentroid(
 
   for (Triangle &triangle : _triangles)
   {
+    // TODO(jasmeet0915): Use weighted average of tetrahedron
+    // volumes instead of triangle areas for centroid value
+    // as that would provide better approximation to
+    // center of mass
+
     // Calculate the area of the triangle using half of
     // cross product magnitude
     triangleCross =
@@ -101,12 +107,61 @@ void MeshInertiaCalculator::CalculateMeshCentroid(
   _centreOfMass.SetZ(centroid.Z());
 }
 
+//////////////////////////////////////////////////
+void MeshInertiaCalculator::TransformInertiaMatrixToCOM(
+  gz::math::MassMatrix3d &_massMatrix,
+  const gz::math::Pose3d &_centreOfMass,
+  const gz::math::Pose3d &_inertiaOrigin
+)
+{
+  gz::math::Vector3d comRelativeToOrigin =
+    _centreOfMass.CoordPositionSub(_inertiaOrigin);
+
+  gz::math::Vector3d ixxyyzz = _massMatrix.DiagonalMoments();
+  gz::math::Vector3d ixyxzyz = _massMatrix.OffDiagonalMoments();
+  double mass = _massMatrix.Mass();
+
+  // Transform the Inertia Matrix to COM using the
+  // reverse of the Parallel Axis Theorem
+  ixxyyzz.X() = ixxyyzz.X() -
+                mass * (comRelativeToOrigin.Y() * comRelativeToOrigin.Y()
+                + comRelativeToOrigin.Z() * comRelativeToOrigin.Z());
+  ixxyyzz.Y() = ixxyyzz.Y() -
+                mass * (comRelativeToOrigin.X() * comRelativeToOrigin.X()
+                + comRelativeToOrigin.Z() * comRelativeToOrigin.Z());
+  ixxyyzz.Z() = ixxyyzz.Z() -
+                mass * (comRelativeToOrigin.X() * comRelativeToOrigin.X()
+                + comRelativeToOrigin.Y() * comRelativeToOrigin.Y());
+
+  ixyxzyz.X() = ixyxzyz.X() +
+                mass * comRelativeToOrigin.X() * comRelativeToOrigin.Y();
+  ixyxzyz.Y() = ixyxzyz.Y() +
+                mass * comRelativeToOrigin.X() * comRelativeToOrigin.Z();
+  ixyxzyz.Z() = ixyxzyz.Z() +
+                mass * comRelativeToOrigin.Y() * comRelativeToOrigin.Z();
+
+  _massMatrix.SetDiagonalMoments(ixxyyzz);
+  _massMatrix.SetOffDiagonalMoments(ixyxzyz);
+
+  gz::math::Quaterniond rotOffset = _massMatrix.PrincipalAxesOffset();
+
+  // If there is a rotational offset remove that
+  if (rotOffset != gz::math::Quaterniond::Identity)
+  {
+    // Since the Inertia Matrix of a body about the COM aligned with
+    // Prinicipal Axes will be diagonal, we can set off diagonal
+    // elements to a diagonal matrix if there is a rotational
+    // offset after initial transformation
+    _massMatrix.SetOffDiagonalMoments(gz::math::Vector3d::Zero);
+  }
+}
+
 //////////////////////////////////////////////////
 void MeshInertiaCalculator::CalculateMassProperties(
   const std::vector<Triangle>& _triangles,
   double _density,
   gz::math::MassMatrix3d& _massMatrix,
-  gz::math::Pose3d& _inertiaOrigin)
+  gz::math::Pose3d& _centreOfMass)
 {
   // Some coefficients for the calculation of integral terms
   const double coefficients[10] = {1.0 / 6,   1.0 / 24, 1.0 / 24, 1.0 / 24,
@@ -185,27 +240,27 @@ void MeshInertiaCalculator::CalculateMassProperties(
   // Accumulate the result and add it to MassMatrix object of gz::math
   double volume = integral[0];
   double mass = volume * _density;
-  _inertiaOrigin.SetX(integral[1] / mass);
-  _inertiaOrigin.SetY(integral[2] / mass);
-  _inertiaOrigin.SetZ(integral[3] / mass);
+  _centreOfMass.SetX(integral[1] / volume);
+  _centreOfMass.SetY(integral[2] / volume);
+  _centreOfMass.SetZ(integral[3] / volume);
   gz::math::Vector3d ixxyyzz = gz::math::Vector3d();
   gz::math::Vector3d ixyxzyz = gz::math::Vector3d();
 
   // Diagonal Elements of the Mass Matrix
-  ixxyyzz.X() = (integral[5] + integral[6] - mass *
-                (_inertiaOrigin.Y() * _inertiaOrigin.Y() +
-                _inertiaOrigin.Z() * _inertiaOrigin.Z()));
-  ixxyyzz.Y() = (integral[4] + integral[6] - mass *
-                (_inertiaOrigin.Z() * _inertiaOrigin.Z() +
-                _inertiaOrigin.X() * _inertiaOrigin.X()));
-  ixxyyzz.Z() = integral[4] + integral[5] - mass *
-                (_inertiaOrigin.X() * _inertiaOrigin.X() +
-                _inertiaOrigin.Y() * _inertiaOrigin.Y());
+  ixxyyzz.X() = (integral[5] + integral[6] - volume *
+                (_centreOfMass.Y() * _centreOfMass.Y() +
+                _centreOfMass.Z() * _centreOfMass.Z()));
+  ixxyyzz.Y() = (integral[4] + integral[6] - volume *
+                (_centreOfMass.Z() * _centreOfMass.Z() +
+                _centreOfMass.X() * _centreOfMass.X()));
+  ixxyyzz.Z() = integral[4] + integral[5] - volume *
+                (_centreOfMass.X() * _centreOfMass.X() +
+                _centreOfMass.Y() * _centreOfMass.Y());
 
   // Off Diagonal Elements of the Mass Matrix
-  ixyxzyz.X() = -(integral[7] - mass * _inertiaOrigin.X() * _inertiaOrigin.Y());
-  ixyxzyz.Y() = -(integral[8] - mass * _inertiaOrigin.Y() * _inertiaOrigin.Z());
-  ixyxzyz.Z() = -(integral[9] - mass * _inertiaOrigin.X() * _inertiaOrigin.Z());
+  ixyxzyz.X() = -(integral[7] - volume * _centreOfMass.X() * _centreOfMass.Y());
+  ixyxzyz.Y() = -(integral[9] - volume * _centreOfMass.X() * _centreOfMass.Z());
+  ixyxzyz.Z() = -(integral[8] - volume * _centreOfMass.Y() * _centreOfMass.Z());
 
   // Set the values in the MassMatrix object
   _massMatrix.SetMass(mass);
@@ -249,26 +304,13 @@ std::optional<gz::math::Inertiald> MeshInertiaCalculator::operator()
   std::vector<Triangle> meshTriangles;
   gz::math::MassMatrix3d meshMassMatrix;
   gz::math::Pose3d centreOfMass;
-  gz::math::Pose3d inertiaOrigin;
 
   // Create a list of Triangle objects from the mesh vertices and indices
   this->GetMeshTriangles(meshTriangles, sdfMesh->Scale(), mesh);
 
-  // Calculate the mesh centroid and set is as centre of mass
-  this->CalculateMeshCentroid(centreOfMass, meshTriangles);
-
   // Calculate mesh mass properties
   this->CalculateMassProperties(meshTriangles, density,
-    meshMassMatrix, inertiaOrigin);
-
-  if (inertiaOrigin != centreOfMass)
-  {
-    // TODO(jasmeet0915): tranform the calculated inertia matrix
-    // from inertia origin to centre of mass
-    gzwarn << "Calculated centroid does not match the mesh origin. "
-    "Inertia Tensor values won't be correct. Use mesh with origin at "
-    "geometric center." << std::endl;
-  }
+    meshMassMatrix, centreOfMass);
 
   gz::math::Inertiald meshInertial;
 

src/MeshInertiaCalculator.hh

@@ -103,6 +103,22 @@ namespace gz
         public: void CalculateMeshCentroid(gz::math::Pose3d &_centreOfMass,
           std::vector<Triangle> &_triangles);
 
+        /// \brief Function to transform the Inertia Matrix from a
+        /// given point to the Center of Mass. This function is used
+        /// in cases where the origin of the mesh is not at the centroid
+        /// (Center of Mass). The function uses the reverse of the
+        /// Parallel Axis Theorem for the transformation.
+        /// \param[out] _massMatrix MassMatrix object to hold mass &
+        /// moment of inertia of the mesh
+        /// \param[in] _centerOfMass The centroid (center of mass) of the mesh
+        /// to which the inertia matrix has to be transformed.
+        /// \param[in] _inertiaOrigin The point about which the inertia matrix
+        /// was calculated. This would be the origin of the mesh.
+        public: void TransformInertiaMatrixToCOM(
+          gz::math::MassMatrix3d &_massMatrix,
+          const gz::math::Pose3d &_centreOfMass,
+          const gz::math::Pose3d &_inertiaOrigin);
+
         /// \brief Function that calculates the mass, mass matrix & centre of
         /// mass of a mesh using a vector of Triangles of the mesh
         /// \param[in] _triangles A vector of all the Triangles of the mesh

test/integration/mesh_inertia_calculation.cc

@@ -54,8 +54,6 @@ TEST(MeshInertiaCalculationTest, CylinderColladaMeshInertiaCalculation)
 {
   size_t kIter = 100u;
 
-  gz::math::Pose3d linkPose(4, 4, 1, 0, 0, 0);
-  gz::math::Inertiald linkInertial;
   // Start server and run.
   gz::sim::ServerConfig serverConfig;
   serverConfig.SetSdfFile(common::joinPaths(
@@ -127,3 +125,85 @@ TEST(MeshInertiaCalculationTest, CylinderColladaMeshInertiaCalculation)
   EXPECT_EQ(link.WorldPose(*ecm).value(), gz::math::Pose3d::Zero);
   EXPECT_EQ(link.WorldInertialPose(*ecm).value(), gz::math::Pose3d::Zero);
 }
+
+TEST(MeshInertiaCalculationTest,
+  CylinderColladaMeshWithNonCenterOriginInertiaCalculation)
+{
+  size_t kIter = 100u;
+
+  // Start server and run.
+  gz::sim::ServerConfig serverConfig;
+  serverConfig.SetSdfFile(common::joinPaths(
+      PROJECT_SOURCE_PATH, "test", "worlds", "mesh_inertia_calculation.sdf"));
+
+  common::setenv(
+      "GZ_SIM_RESOURCE_PATH",
+      common::joinPaths(PROJECT_SOURCE_PATH, "test", "worlds", "models"));
+
+  gz::sim::Server server(serverConfig);
+
+  // Create a system just to get the ECM
+  EntityComponentManager *ecm;
+  test::Relay testSystem;
+  testSystem.OnPreUpdate(
+    [&](const UpdateInfo &, EntityComponentManager &_ecm)
+    {
+      ecm = &_ecm;
+    }
+  );
+  server.AddSystem(testSystem.systemPtr);
+
+  ASSERT_FALSE(server.Running());
+  ASSERT_FALSE(*server.Running(0));
+  ASSERT_TRUE(server.Run(true, kIter, false));
+  ASSERT_NE(nullptr, ecm);
+
+  // Get link of collada cylinder
+  gz::sim::Entity modelEntity = ecm->EntityByComponents(
+    gz::sim::components::Name("cylinder_dae_bottom_origin"),
+    gz::sim::components::Model()
+  );
+
+  gz::sim::Model model = gz::sim::Model(modelEntity);
+  ASSERT_TRUE(model.Valid(*ecm));
+
+  gz::sim::Entity linkEntity = model.LinkByName(*ecm,
+    "cylinder_dae_bottom_origin");
+  gz::sim::Link link = gz::sim::Link(linkEntity);
+  ASSERT_TRUE(link.Valid(*ecm));
+
+  // Enable checks for pose values
+  link.EnableVelocityChecks(*ecm);
+
+  ASSERT_NE(link.WorldInertiaMatrix(*ecm), std::nullopt);
+  ASSERT_NE(link.WorldInertialPose(*ecm), std::nullopt);
+  ASSERT_NE(link.WorldPose(*ecm), std::nullopt);
+
+  // The cylinder has a radius of 1m, length of 2m, and density of 1240 kg/m³.
+  // Volume: πr²h = 2π ≈ 6.283
+  // Mass: ρV = (1240.0) * 2π ≈ 7791.1497
+  // Ix = Iy : 1/12 * m(3r² + h²)  = m/12 * (3 + 4) ≈ 4544.83
+  // Iz : ½mr² ≈ 3895.57
+  gz::math::Inertiald meshInertial;
+  meshInertial.SetMassMatrix(gz::math::MassMatrix3d(
+      7791.1497,
+      gz::math::Vector3d(4544.83, 4544.83, 3895.57),
+      gz::math::Vector3d::Zero
+    ));
+    meshInertial.SetPose(gz::math::Pose3d::Zero);
+  gz::math::Matrix3 inertiaMatrix = meshInertial.Moi();
+
+  // Check the Inertia Matrix within a tolerance of 0.005 since we are
+  // comparing a mesh cylinder with an ideal cylinder. For values more closer
+  // to the ideal, a higher number of vertices would be required in mesh
+  EXPECT_TRUE(
+    link.WorldInertiaMatrix(*ecm).value().Equal(inertiaMatrix, 0.005));
+
+  // Check the Inertial Pose and Link Pose
+  EXPECT_EQ(link.WorldPose(*ecm).value(), gz::math::Pose3d::Zero);
+
+  // Since the height of cylinder is 2m and origin is at center of bottom face
+  // the center of mass (inertial pose) will be 1m above the ground
+  EXPECT_EQ(link.WorldInertialPose(*ecm).value(),
+    gz::math::Pose3d(0, 0, 1, 0, 0, 0));
+}

test/worlds/mesh_inertia_calculation.sdf

@@ -12,5 +12,11 @@
       <name>cylinder_dae</name>
       <pose>0 0 0 0 0 0</pose>
     </include>
+
+    <include>
+      <uri>cylinder_dae_bottom_origin</uri>
+      <name>cylinder_dae_bottom_origin</name>
+      <pose>4 0 0 0 0 0</pose>
+    </include>
   </world>
 </sdf>

test/worlds/models/cylinder_dae_bottom_origin/model.config

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>cylinder_dae_bottom_origin</name>
+  <version>1.0</version>
+  <sdf version="1.11">model.sdf</sdf>
+
+  <author>
+    <name>Jasmeet Singh</name>
+    <email>[email protected]</email>
+  </author>
+
+  <description>
+    A model of a Cylinder collada with mesh origin at the center of the bottom face
+  </description>
+</model>

test/worlds/models/cylinder_dae_bottom_origin/model.sdf

@@ -0,0 +1,33 @@
+<?xml version="1.0" ?>
+<sdf version="1.11">
+  <model name="cylinder_dae_bottom_origin">
+    <link name="cylinder_dae_bottom_origin">
+      <pose>0 0 0 0 0 0</pose>
+      <inertial auto="true" />
+      <collision name="cylinder_collision">
+        <density>1240.0</density>
+        <auto_inertia_params>
+          <gz:voxel_size>0.01</gz:voxel_size>
+        </auto_inertia_params>
+        <geometry>
+          <mesh>
+            <uri>meshes/cylinder_bottom_origin.dae</uri>
+          </mesh>
+        </geometry>
+      </collision>
+      <visual name="cylinder_visual">
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>meshes/cylinder_bottom_origin.dae</uri>
+          </mesh>
+        </geometry>
+        <material>
+          <diffuse>0.7 0.7 0.7 1.0</diffuse>
+          <ambient>0.7 0.7 0.7 1.0</ambient>
+          <specular>0.7 0.7 0.7 1.0</specular>
+        </material>
+      </visual>
+    </link>
+  </model>
+</sdf>

tutorials/auto_inertia_calculation.md

@@ -153,11 +153,6 @@ the center of mass in this case).
 
 Here are some key points to consider when using automatic inertia calculation with 3D Meshes:
  * Water-tight triangle meshes are required for the Mesh Inertia Calculator.
- * Currently, the mesh inertia is calculated about the mesh origin. Since the link
- inertia value needs to be about the Center of Mass, the mesh origin needs to be set
- at the Center of Mass (Centroid). Functions to transform the inertia matrix to the mesh
- centroid in case the mesh origin is set elsewhere are under development. Therefore, this
- should hopefully be fixed in the future.
  * Since the vertex data is used for inertia calculations, high vertex count would be
  needed for near ideal values. However, it is recommended to use basic shapes with the
  geometry tag (Box, Capsule, Cylinder, Ellipsoid and Sphere) as collision geometries get