Attempt to correct mass matrix in mesh inertia calculator (backport #2775) (#2799)

mergify[bot] · iche033 · scpeters · web-flow · commit f8dac7240405 · 2025-03-03T16:00:57.000-08:00
* Attempt to correct mass matrix in mesh inertia calculator (#2775) Signed-off-by: Ian Chen <ichen@openrobotics.org> (cherry picked from commit 1509b0f) * split test Signed-off-by: Ian Chen <ichen@openrobotics.org> --------- Signed-off-by: Ian Chen <ichen@openrobotics.org> Co-authored-by: Ian Chen <ichen@openrobotics.org> Co-authored-by: Steve Peters <scpeters@openrobotics.org>
diff --git a/src/CMakeLists.txt b/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
diff --git a/src/MeshInertiaCalculator.cc b/src/MeshInertiaCalculator.cc
@@ -17,6 +17,8 @@
 
 #include "MeshInertiaCalculator.hh"
 
+#include <algorithm>
+#include <numeric>
 #include <optional>
 #include <vector>
 
@@ -37,11 +39,61 @@
 using namespace gz;
 using namespace sim;
 
+//////////////////////////////////////////////////
+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();
+
+  // Track elements in principalMoments in ascending order using a sorted
+  // indices array, i.e. sortedIndices[0] should point to the index in
+  // principalMoments containing the smallest value, while sortedIndices[2]
+  // should point to the index in principalMoments containing the largest value.
+  std::vector<int> sortedIndices(3);
+  std::iota(sortedIndices.begin(), sortedIndices.end(), 0);
+  std::sort(sortedIndices.begin(), sortedIndices.end(), [&](int i, int j)
+      { return principalMoments[i] < principalMoments[j]; } );
+
+  // Check if principal moments are within tol of satisfying the
+  // triangle inequality.
+  const double ratio =
+      (principalMoments[sortedIndices[0]] + principalMoments[sortedIndices[1]])
+      / principalMoments[sortedIndices[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 2 smaller elements in principalMoments so that they
+  // satisfy the triangle inequality
+  const double scale = 1.0 / ratio;
+  math::Vector3d correctedPrincipalMoments = principalMoments;
+  correctedPrincipalMoments[sortedIndices[0]] *= scale;
+  correctedPrincipalMoments[sortedIndices[1]] *= scale;
+
+  // Recompute mass matrix 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::SubMesh* _subMesh)
+  const gz::common::SubMesh *_subMesh)
 {
   // Get the vertices & indices of the mesh
   double* vertArray = nullptr;
@@ -229,8 +281,21 @@ std::optional<gz::math::Inertiald> MeshInertiaCalculator::operator()
     this->CalculateMassProperties(meshTriangles, density,
       meshMassMatrix, centreOfMass);
 
-    if (meshMassMatrix.IsValid())
-      meshInertial += gz::math::Inertiald(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);
   }
 
   if (meshInertial.MassMatrix().Mass() <= 0.0 ||
diff --git a/src/MeshInertiaCalculator.hh b/src/MeshInertiaCalculator.hh
@@ -43,6 +43,10 @@ namespace gz
     // Inline bracket to help doxygen filtering.
     inline namespace GZ_SIM_VERSION_NAMESPACE
     {
+      /// \brief Relative error tolerance allowed when testing if principal
+      /// moments of a mass matrix satify the triangle inequality.
+      constexpr double kPrincipalMomentRelativeTol = 0.05;
+
       /// \struct Triangle gz/sim/MeshInertiaCalculator.hh
       /// \brief A struct to represent a triangle of the mesh
       /// An instance of the struct holds 3 Vector3D instances
@@ -68,11 +72,24 @@ 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
+        /// \param[in] _tol Relative error tolerance allowed when testing if
+        /// principal moments of a mass matrix satify the triangle inequality.
+        /// \return True if the mass matrix is already valid or successfully
+        /// corrected, false otherwise.
+        public: static bool CorrectMassMatrix(
+            gz::math::MassMatrix3d &_massMatrix,
+            double tol = kPrincipalMomentRelativeTol);
+
         /// \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
@@ -83,7 +100,7 @@ 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::SubMesh* _mesh);
@@ -96,7 +113,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,
diff --git a/src/MeshInertiaCalculator_TEST.cc b/src/MeshInertiaCalculator_TEST.cc
@@ -0,0 +1,102 @@
+/*
+ * 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());
+  // Set a small tolerance for the triangle inequality test and expect that
+  // the mass matrix can not be corrected.
+  EXPECT_FALSE(MeshInertiaCalculator::CorrectMassMatrix(massMatrix, 0.01));
+  // Verify successful correction with default tolerance.
+  EXPECT_TRUE(MeshInertiaCalculator::CorrectMassMatrix(massMatrix));
+  EXPECT_TRUE(massMatrix.IsValid());
+
+  // Verify a mass matrix with unordered diagonal moments and a small error
+  // can be corrected, and that the elements in the corrected principal moments
+  // maintain the same order.
+  math::Vector3d ixxyyzz(15, 7, 23);
+  massMatrix = math::MassMatrix3d(55.0,
+                                  ixxyyzz,
+                                  math::Vector3d::Zero);
+  EXPECT_FALSE(massMatrix.IsValid());
+  EXPECT_TRUE(massMatrix.IsPositive());
+  EXPECT_TRUE(MeshInertiaCalculator::CorrectMassMatrix(massMatrix));
+  EXPECT_TRUE(massMatrix.IsValid());
+  EXPECT_NE(ixxyyzz, massMatrix.DiagonalMoments());
+  EXPECT_LT(massMatrix.PrincipalMoments()[1], massMatrix.PrincipalMoments()[0]);
+  EXPECT_LT(massMatrix.PrincipalMoments()[0], massMatrix.PrincipalMoments()[2]);
+
+  // 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 that CorrectMassMatrix returns true when given a valid mass matrix
+  // and does not change the mass matrix data.
+  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());
+}
diff --git a/test/integration/mesh_inertia_calculation.cc b/test/integration/mesh_inertia_calculation.cc
