Attempt to correct mass matrix in mesh inertia calculator

src/CMakeLists.txt

@@ -114,6 +114,7 @@ set (gtest_sources
   Joint_TEST.cc
   Light_TEST.cc
   Link_TEST.cc
+  MeshInertiaCalculator_TEST.cc
   Model_TEST.cc
   Primitives_TEST.cc
   SdfEntityCreator_TEST.cc

src/MeshInertiaCalculator.cc

@@ -25,32 +25,75 @@
 
 #include <gz/sim/Util.hh>
 
-#include <gz/common/graphics.hh>
-#include <gz/common/Mesh.hh>
 #include <gz/common/MeshManager.hh>
+#include <gz/common/SubMesh.hh>
 
 #include <gz/math/Vector3.hh>
 #include <gz/math/Pose3.hh>
 #include <gz/math/MassMatrix3.hh>
+#include <gz/math/Helpers.hh>
 #include <gz/math/Inertial.hh>
 #include <gz/math/Quaternion.hh>
 
 using namespace gz;
 using namespace sim;
 
+constexpr double MeshInertiaCalculator::kPrincipalMomentPercentTol;
+
+//////////////////////////////////////////////////
+bool MeshInertiaCalculator::CorrectMassMatrix(
+    math::MassMatrix3d &_massMatrix, double _tol)
+{
+  if (_massMatrix.IsValid())
+    return true;
+
+  if (!_massMatrix.IsPositive())
+    return false;
+
+  math::Quaterniond principalAxesOffset = _massMatrix.PrincipalAxesOffset();
+  math::Vector3d principalMoments = _massMatrix.PrincipalMoments();
+  math::sort3(principalMoments[0], principalMoments[1], principalMoments[2]);
+
+  // Check if principal moments are within tol of satisfying the
+  // triangle inequality
+  const double ratio = (principalMoments[0] + principalMoments[1])
+                 / principalMoments[2];
+  if ((1.0 - ratio) > _tol)
+  {
+    // The error is outside of tol so do not attempt to correct the mass matrix.
+    return false;
+  }
+  // Scale the principal moments to satisfy the triangle inequality
+  const double scale = 1.0 / ratio;
+  math::Vector3d correctedPrincipalMoments(
+      principalMoments[0] * scale,
+      principalMoments[1] * scale,
+      principalMoments[2]);
+
+  // Recompute mass matrx with corrected principal moments.
+  math::MassMatrix3d correctedPrincipalMassMatrix(_massMatrix.Mass(),
+      correctedPrincipalMoments, math::Vector3d::Zero);
+  math::Inertiald correctedPrincipalMassMatrixWithAxesOffset(
+      correctedPrincipalMassMatrix,
+      math::Pose3d(math::Vector3d::Zero, principalAxesOffset));
+  _massMatrix.SetMoi(correctedPrincipalMassMatrixWithAxesOffset.Moi());
+
+  return true;
+}
+
 //////////////////////////////////////////////////
 void MeshInertiaCalculator::GetMeshTriangles(
   std::vector<Triangle> &_triangles,
   const gz::math::Vector3d &_meshScale,
-  const gz::common::Mesh* _mesh)
+  const gz::common::SubMesh* _subMesh)
 {
   // Get the vertices & indices of the mesh
   double* vertArray = nullptr;
   int* indArray = nullptr;
-  _mesh->FillArrays(&vertArray, &indArray);
+  _subMesh->FillArrays(&vertArray, &indArray);
 
   // Add check to see if size of the ind array is divisible by 3
-  for (unsigned int i = 0; i < _mesh->IndexCount(); i += 3)
+  for (unsigned int i = 0; i < _subMesh->IndexCount(); i += 3)
   {
     Triangle triangle;
     triangle.v0.X() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i])];
@@ -187,10 +230,9 @@ void MeshInertiaCalculator::CalculateMassProperties(
 
 //////////////////////////////////////////////////
 std::optional<gz::math::Inertiald> MeshInertiaCalculator::operator()
-  (sdf::Errors& _errors,
+  (sdf::Errors &_errors,
   const sdf::CustomInertiaCalcProperties& _calculatorParams)
 {
-  const gz::common::Mesh *mesh = nullptr;
   const double density = _calculatorParams.Density();
 
   auto sdfMesh = _calculatorParams.Mesh();
@@ -205,44 +247,55 @@ std::optional<gz::math::Inertiald> MeshInertiaCalculator::operator()
     return std::nullopt;
   }
 
-  auto fullPath = asFullPath(sdfMesh->Uri(), sdfMesh->FilePath());
-
-  if (fullPath.empty())
-  {
-    gzerr << "Mesh geometry missing uri" << std::endl;
-    _errors.push_back({sdf::ErrorCode::URI_INVALID,
-        "Attempting to load the mesh but the URI seems to be incorrect"});
-    return std::nullopt;
-  }
+  const common::Mesh *mesh = loadMesh(*sdfMesh);
 
-  // Load the Mesh
-  gz::common::MeshManager *meshManager = gz::common::MeshManager::Instance();
-  mesh = meshManager->Load(fullPath);
-  if (!mesh)
+  // Compute inertia for each submesh then sum up to get the final inertia
+  // values.
+  gz::math::Inertiald meshInertial;
+  for (unsigned int i = 0; i < mesh->SubMeshCount(); ++i)
   {
-    gzerr << "Failed to load mesh: " << fullPath << std::endl;
-    return std::nullopt;
+    std::vector<Triangle> meshTriangles;
+    gz::math::MassMatrix3d meshMassMatrix;
+    gz::math::Pose3d centreOfMass;
+
+    // Create a list of Triangle objects from the mesh vertices and indices
+    auto submesh = mesh->SubMeshByIndex(i).lock();
+    this->GetMeshTriangles(meshTriangles, sdfMesh->Scale(), submesh.get());
+
+    // Calculate mesh mass properties
+    this->CalculateMassProperties(meshTriangles, density,
+      meshMassMatrix, centreOfMass);
+
+    if (!meshMassMatrix.IsValid())
+    {
+      gzwarn << "Auto-calculated mass matrix is invalid for mesh: "
+             << mesh->Name() << ", submesh index: " << i << "."
+             << std::endl;
+      if (!this->CorrectMassMatrix(meshMassMatrix))
+      {
+        gzwarn << "  Unable to correct mass matrix. Skipping submesh."
+               << std::endl;
+        continue;
+      }
+      gzwarn << "  Successfully corrected mass matrix."
+             << std::endl;
+    }
+    meshInertial += gz::math::Inertiald(meshMassMatrix, centreOfMass);
   }
-  std::vector<Triangle> meshTriangles;
-  gz::math::MassMatrix3d meshMassMatrix;
-  gz::math::Pose3d centreOfMass;
-
-  // Create a list of Triangle objects from the mesh vertices and indices
-  this->GetMeshTriangles(meshTriangles, sdfMesh->Scale(), mesh);
-
-  // Calculate mesh mass properties
-  this->CalculateMassProperties(meshTriangles, density,
-    meshMassMatrix, centreOfMass);
 
-  gz::math::Inertiald meshInertial;
-
-  if (!meshInertial.SetMassMatrix(meshMassMatrix))
+  if (meshInertial.MassMatrix().Mass() > 0.0 &&
+      meshInertial.MassMatrix().IsValid())
   {
-    return std::nullopt;
+    return meshInertial;
   }
   else
   {
-    meshInertial.SetPose(centreOfMass);
-    return meshInertial;
+    gzerr << "Failed to computed valid inertia in MeshInertiaCalculator. "
+          << "Ensure that the mesh is water tight, or try optimizing the mesh "
+          << "by setting the //mesh/@optimization attribute in SDF to "
+          << "`convex_hull` or `convex_decomposition`."
+          << std::endl;
   }
+
+  return std::nullopt;
 }

src/MeshInertiaCalculator.hh

@@ -28,9 +28,7 @@
 #include <gz/sim/Util.hh>
 #include <gz/sim/config.hh>
 
-#include <gz/common/graphics.hh>
-#include <gz/common/Mesh.hh>
-#include <gz/common/MeshManager.hh>
+#include <gz/common/SubMesh.hh>
 
 #include <gz/math/Vector3.hh>
 #include <gz/math/Pose3.hh>
@@ -70,11 +68,21 @@ namespace gz
       /// The calculation method used in this class is described here:
       /// https://www.geometrictools.com/Documentation/PolyhedralMassProperties.pdf
       /// and it works on triangle water-tight meshes for simple polyhedron
-      class MeshInertiaCalculator
+      class GZ_SIM_VISIBLE MeshInertiaCalculator
       {
         /// \brief Constructor
         public: MeshInertiaCalculator() = default;
 
+        /// \brief Function to correct an invalid mass matrix. The mass matrix
+        /// to be corrected needs to be positive definite and within a small
+        /// tolerance of satisfying the triangle inequality test. If the above
+        /// conditions are not satisfied, the mass matrix will not be corrected.
+        /// \param[in, out] _massMatrix Mass matrix to correct
+        /// \return True if the mass matrix is corrected, false otherwise.
+        public: static bool CorrectMassMatrix(
+            gz::math::MassMatrix3d &_massMatrix,
+            double tol = kPrincipalMomentPercentTol);
+
         /// \brief Function to get the vertices & indices of the given mesh
         /// & convert them into instances of the Triangle struct
         /// Each triangle represents a triangle in the mesh & is added
@@ -85,10 +93,10 @@ namespace gz
         /// \param[in] _meshScale A vector with the scaling factor
         /// of all the 3 axes
         /// \param[in] _mesh Mesh object
-        public: void GetMeshTriangles(
+        public: static void GetMeshTriangles(
           std::vector<Triangle> &_triangles,
           const gz::math::Vector3d &_meshScale,
-          const gz::common::Mesh* _mesh);
+          const gz::common::SubMesh* _mesh);
 
         /// \brief Function that calculates the mass, mass matrix & centre of
         /// mass of a mesh using a vector of Triangles of the mesh
@@ -98,7 +106,7 @@ namespace gz
         /// moment of inertia of the mesh
         /// \param[out] _inertiaOrigin Pose3d object to hold the origin about
         /// which the inertia tensor was calculated
-        public: void CalculateMassProperties(
+        public: static void CalculateMassProperties(
           const std::vector<Triangle>& _triangles,
           double _density,
           gz::math::MassMatrix3d& _massMatrix,
@@ -116,6 +124,10 @@ namespace gz
         public: std::optional<gz::math::Inertiald> operator()(
           sdf::Errors& _errors,
           const sdf::CustomInertiaCalcProperties& _calculatorParams);
+
+        /// \brief Percentage error tolerance allowed when testing if principal
+        /// moments of a mass matrix satify the triangle inequality.
+        public: static constexpr double kPrincipalMomentPercentTol = 0.05;
       };
     }
   }

src/MeshInertiaCalculator_TEST.cc

@@ -0,0 +1,83 @@
+/*
+ * Copyright (C) 2025 Open Source Robotics Foundation
+ *
+ * Licensed under the Apache License, Version 2.0 (the "License");
+ * you may not use this file except in compliance with the License.
+ * You may obtain a copy of the License at
+ *
+ *     http://www.apache.org/licenses/LICENSE-2.0
+ *
+ * Unless required by applicable law or agreed to in writing, software
+ * distributed under the License is distributed on an "AS IS" BASIS,
+ * WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+ * See the License for the specific language governing permissions and
+ * limitations under the License.
+ *
+*/
+
+#include <gtest/gtest.h>
+
+#include <gz/math/MassMatrix3.hh>
+
+#include "MeshInertiaCalculator.hh"
+
+using namespace gz;
+using namespace sim;
+
+/////////////////////////////////////////////////
+TEST(MeshInertiaCalculator, CorrectMassMatrix)
+{
+  // Verify a mass matrix with a small error can be corrected
+  math::MassMatrix3d massMatrix(55.0,
+                                math::Vector3d(7, 15, 23),
+                                math::Vector3d::Zero);
+  EXPECT_FALSE(massMatrix.IsValid());
+  EXPECT_TRUE(massMatrix.IsPositive());
+  EXPECT_TRUE(MeshInertiaCalculator::CorrectMassMatrix(massMatrix));
+  EXPECT_TRUE(massMatrix.IsValid());
+
+  // Verify a mass matrix with non-zero off-diagonal moments can be corrected
+  massMatrix = math::MassMatrix3d(1.0,
+                                  math::Vector3d(2.0, 2.0, 2.0),
+                                  math::Vector3d(-1, 0, -0.1));
+  EXPECT_FALSE(massMatrix.IsValid());
+  EXPECT_TRUE(massMatrix.IsPositive());
+  EXPECT_TRUE(MeshInertiaCalculator::CorrectMassMatrix(massMatrix));
+  EXPECT_TRUE(massMatrix.IsValid());
+
+  // Verify a mass matrix with a large error can not be corrected.
+  massMatrix = math::MassMatrix3d(15.0,
+                                  math::Vector3d(1, 2, 23),
+                                  math::Vector3d::Zero);
+  EXPECT_FALSE(massMatrix.IsValid());
+  EXPECT_TRUE(massMatrix.IsPositive());
+  EXPECT_FALSE(MeshInertiaCalculator::CorrectMassMatrix(massMatrix));
+  EXPECT_FALSE(massMatrix.IsValid());
+
+  // Verify a mass matrix with non positive-definite inertia matrix can not
+  // be corrected.
+  massMatrix = math::MassMatrix3d(15.0,
+                                  math::Vector3d(12, -15, 23),
+                                  math::Vector3d::Zero);
+  EXPECT_FALSE(massMatrix.IsValid());
+  EXPECT_FALSE(massMatrix.IsPositive());
+  EXPECT_FALSE(MeshInertiaCalculator::CorrectMassMatrix(massMatrix));
+  EXPECT_FALSE(massMatrix.IsPositive());
+  EXPECT_FALSE(massMatrix.IsValid());
+
+  // Verify a mass matrix with no error can be corrected. The mass
+  // matrix should keep the original values.
+  massMatrix = math::MassMatrix3d(15.0,
+                                  math::Vector3d(12, 15, 23),
+                                  math::Vector3d::Zero);
+  math::MassMatrix3d originalMassMatrix = massMatrix;
+  EXPECT_TRUE(massMatrix.IsValid());
+  EXPECT_TRUE(MeshInertiaCalculator::CorrectMassMatrix(massMatrix));
+  EXPECT_TRUE(massMatrix.IsValid());
+  EXPECT_DOUBLE_EQ(originalMassMatrix.Mass(),
+              massMatrix.Mass());
+  EXPECT_EQ(originalMassMatrix.DiagonalMoments(),
+            massMatrix.DiagonalMoments());
+  EXPECT_EQ(originalMassMatrix.OffDiagonalMoments(),
+            massMatrix.OffDiagonalMoments());
+}