Bugfix: Fixed Centre of Mass and Inertia Matrix Calculation Bug `Mesh…

…InertiaCalculator::CalculateMassProperties()` function (gazebosim#2182) * Added function to transform inertia matrix to COM using parallel axis theorem * Corrected mismatch of values in ixz and iyz calculation from integral * Added doc for TransformInertiaMatrixToCOM() function * Added correction for rotational offset of inertia matrix after transformation * Added cylinder with origin at bottom to test world * Fixed center of mass & inertia matrix calculation in CalculateMassProperties() * Added test for inertia calculation of cylinder with origin at the bottom * Updated input params of TransformInertiaMatrixToCOM() to const ref * Updated tutorial doc * Removed unused functions from the code --------- Signed-off-by: Jasmeet Singh <[email protected]>
jrutgeer · Nov 13, 2023 · 9db8061 · 9db8061
1 parent a30dfff
commit 9db8061
Show file tree

Hide file tree

Showing 8 changed files with 224 additions and 79 deletions.
diff --git a/src/MeshInertiaCalculator.cc b/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;
@@ -72,41 +73,12 @@ void MeshInertiaCalculator::GetMeshTriangles(
   }
 }
 
-//////////////////////////////////////////////////
-void MeshInertiaCalculator::CalculateMeshCentroid(
-  gz::math::Pose3d &_centreOfMass,
-  std::vector<Triangle> &_triangles)
-{
-  gz::math::Vector3d centroid = gz::math::Vector3d::Zero;
-  gz::math::Vector3d triangleCross = gz::math::Vector3d::Zero;
-  double totalMeshArea = 0.0;
-  double triangleArea = 0.0;
-
-  for (Triangle &triangle : _triangles)
-  {
-    // Calculate the area of the triangle using half of
-    // cross product magnitude
-    triangleCross =
-      (triangle.v1 - triangle.v0).Cross(triangle.v2 - triangle.v0);
-    triangleArea = triangleCross.Length() / 2;
-
-    centroid = centroid + (triangle.centroid * triangleArea);
-    totalMeshArea = totalMeshArea + triangleArea;
-  }
-
-  centroid = centroid / totalMeshArea;
-
-  _centreOfMass.SetX(centroid.X());
-  _centreOfMass.SetY(centroid.Y());
-  _centreOfMass.SetZ(centroid.Z());
-}
-
 //////////////////////////////////////////////////
 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 +157,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 +221,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;
 

diff --git a/src/MeshInertiaCalculator.hh b/src/MeshInertiaCalculator.hh
@@ -90,19 +90,6 @@ namespace gz
           const gz::math::Vector3d &_meshScale,
           const gz::common::Mesh* _mesh);
 
-        /// \brief Function to calculate the centroid of the mesh. Since
-        /// uniform density is considered for the mesh, the centroid value
-        /// is used as the centre of mass.
-        /// The mesh centroid is calculated by the average of the
-        /// centroid of mesh triangle weighted by the area of the
-        /// respective triangle
-        /// \param[out] _centreOfMass A gz::math::Pose3d object to hold
-        /// calculated centroid (centre of mass) of the mesh
-        /// \param[in] _triangles A vector with all the triangles of the
-        /// mesh represented as instances of the Triangle struct
-        public: void CalculateMeshCentroid(gz::math::Pose3d &_centreOfMass,
-          std::vector<Triangle> &_triangles);
-
         /// \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

diff --git a/test/integration/mesh_inertia_calculation.cc b/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));
+}
diff --git a/test/worlds/mesh_inertia_calculation.sdf b/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>
diff --git a/test/worlds/models/cylinder_dae_bottom_origin/meshes/cylinder_bottom_origin.dae b/test/worlds/models/cylinder_dae_bottom_origin/meshes/cylinder_bottom_origin.dae
diff --git a/test/worlds/models/cylinder_dae_bottom_origin/model.config b/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>
diff --git a/test/worlds/models/cylinder_dae_bottom_origin/model.sdf b/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>
diff --git a/tutorials/auto_inertia_calculation.md b/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