Skip to content

Commit 1509b0f

Browse files
iche033iche033
authored
Attempt to correct mass matrix in mesh inertia calculator (#2775)
Signed-off-by: Ian Chen <ichen@openrobotics.org>
1 parent 3b1448a commit 1509b0f

4 files changed

+191
-13
lines changed

‎src/CMakeLists.txt

+1
Original file line numberDiff line numberDiff line change
@@ -113,6 +113,7 @@ set (gtest_sources
113113
Joint_TEST.cc
114114
Light_TEST.cc
115115
Link_TEST.cc
116+
MeshInertiaCalculator_TEST.cc
116117
Model_TEST.cc
117118
Primitives_TEST.cc
118119
SdfEntityCreator_TEST.cc

‎src/MeshInertiaCalculator.cc

+68-10
Original file line numberDiff line numberDiff line change
@@ -17,6 +17,8 @@
1717

1818
#include "MeshInertiaCalculator.hh"
1919

20+
#include <algorithm>
21+
#include <numeric>
2022
#include <optional>
2123
#include <vector>
2224

@@ -37,11 +39,61 @@
3739
using namespace gz;
3840
using namespace sim;
3941

42+
//////////////////////////////////////////////////
43+
bool MeshInertiaCalculator::CorrectMassMatrix(
44+
math::MassMatrix3d &_massMatrix, double _tol)
45+
{
46+
if (_massMatrix.IsValid())
47+
return true;
48+
49+
if (!_massMatrix.IsPositive())
50+
return false;
51+
52+
math::Quaterniond principalAxesOffset = _massMatrix.PrincipalAxesOffset();
53+
math::Vector3d principalMoments = _massMatrix.PrincipalMoments();
54+
55+
// Track elements in principalMoments in ascending order using a sorted
56+
// indices array, i.e. sortedIndices[0] should point to the index in
57+
// principalMoments containing the smallest value, while sortedIndices[2]
58+
// should point to the index in principalMoments containing the largest value.
59+
std::vector<int> sortedIndices(3);
60+
std::iota(sortedIndices.begin(), sortedIndices.end(), 0);
61+
std::sort(sortedIndices.begin(), sortedIndices.end(), [&](int i, int j)
62+
{ return principalMoments[i] < principalMoments[j]; } );
63+
64+
// Check if principal moments are within tol of satisfying the
65+
// triangle inequality.
66+
const double ratio =
67+
(principalMoments[sortedIndices[0]] + principalMoments[sortedIndices[1]])
68+
/ principalMoments[sortedIndices[2]];
69+
if ((1.0 - ratio) > _tol)
70+
{
71+
// The error is outside of tol so do not attempt to correct the mass matrix.
72+
return false;
73+
}
74+
// Scale the 2 smaller elements in principalMoments so that they
75+
// satisfy the triangle inequality
76+
const double scale = 1.0 / ratio;
77+
math::Vector3d correctedPrincipalMoments = principalMoments;
78+
correctedPrincipalMoments[sortedIndices[0]] *= scale;
79+
correctedPrincipalMoments[sortedIndices[1]] *= scale;
80+
81+
// Recompute mass matrix with corrected principal moments.
82+
math::MassMatrix3d correctedPrincipalMassMatrix(_massMatrix.Mass(),
83+
correctedPrincipalMoments, math::Vector3d::Zero);
84+
math::Inertiald correctedPrincipalMassMatrixWithAxesOffset(
85+
correctedPrincipalMassMatrix,
86+
math::Pose3d(math::Vector3d::Zero, principalAxesOffset));
87+
_massMatrix.SetMoi(correctedPrincipalMassMatrixWithAxesOffset.Moi());
88+
89+
return true;
90+
}
91+
4092
//////////////////////////////////////////////////
4193
void MeshInertiaCalculator::GetMeshTriangles(
4294
std::vector<Triangle> &_triangles,
4395
const gz::math::Vector3d &_meshScale,
44-
const gz::common::SubMesh* _subMesh)
96+
const gz::common::SubMesh *_subMesh)
4597
{
4698
// Get the vertices & indices of the mesh
4799
double* vertArray = nullptr;
@@ -204,13 +256,6 @@ std::optional<gz::math::Inertiald> MeshInertiaCalculator::operator()
204256
}
205257

206258
const common::Mesh *mesh = loadMesh(*sdfMesh);
207-
if (!mesh)
208-
{
209-
gzerr << "Failed to load mesh: " << sdfMesh->Uri() << std::endl;
210-
_errors.push_back({sdf::ErrorCode::FATAL_ERROR,
211-
"Could not calculate mesh inertia as mesh is not loaded."});
212-
return std::nullopt;
213-
}
214259

215260
// Compute inertia for each submesh then sum up to get the final inertia
216261
// values.
@@ -229,8 +274,21 @@ std::optional<gz::math::Inertiald> MeshInertiaCalculator::operator()
229274
this->CalculateMassProperties(meshTriangles, density,
230275
meshMassMatrix, centreOfMass);
231276

232-
if (meshMassMatrix.IsValid())
233-
meshInertial += gz::math::Inertiald(meshMassMatrix, centreOfMass);
277+
if (!meshMassMatrix.IsValid())
278+
{
279+
gzwarn << "Auto-calculated mass matrix is invalid for mesh: "
280+
<< mesh->Name() << ", submesh index: " << i << "."
281+
<< std::endl;
282+
if (!this->CorrectMassMatrix(meshMassMatrix))
283+
{
284+
gzwarn << " Unable to correct mass matrix. Skipping submesh."
285+
<< std::endl;
286+
continue;
287+
}
288+
gzwarn << " Successfully corrected mass matrix."
289+
<< std::endl;
290+
}
291+
meshInertial += gz::math::Inertiald(meshMassMatrix, centreOfMass);
234292
}
235293

236294
if (meshInertial.MassMatrix().Mass() <= 0.0 ||

‎src/MeshInertiaCalculator.hh

+20-3
Original file line numberDiff line numberDiff line change
@@ -43,6 +43,10 @@ namespace gz
4343
// Inline bracket to help doxygen filtering.
4444
inline namespace GZ_SIM_VERSION_NAMESPACE
4545
{
46+
/// \brief Relative error tolerance allowed when testing if principal
47+
/// moments of a mass matrix satify the triangle inequality.
48+
constexpr double kPrincipalMomentRelativeTol = 0.05;
49+
4650
/// \struct Triangle gz/sim/MeshInertiaCalculator.hh
4751
/// \brief A struct to represent a triangle of the mesh
4852
/// An instance of the struct holds 3 Vector3D instances
@@ -68,11 +72,24 @@ namespace gz
6872
/// The calculation method used in this class is described here:
6973
/// https://www.geometrictools.com/Documentation/PolyhedralMassProperties.pdf
7074
/// and it works on triangle water-tight meshes for simple polyhedron
71-
class MeshInertiaCalculator
75+
class GZ_SIM_VISIBLE MeshInertiaCalculator
7276
{
7377
/// \brief Constructor
7478
public: MeshInertiaCalculator() = default;
7579

80+
/// \brief Function to correct an invalid mass matrix. The mass matrix
81+
/// to be corrected needs to be positive definite and within a small
82+
/// tolerance of satisfying the triangle inequality test. If the above
83+
/// conditions are not satisfied, the mass matrix will not be corrected.
84+
/// \param[in, out] _massMatrix Mass matrix to correct
85+
/// \param[in] _tol Relative error tolerance allowed when testing if
86+
/// principal moments of a mass matrix satify the triangle inequality.
87+
/// \return True if the mass matrix is already valid or successfully
88+
/// corrected, false otherwise.
89+
public: static bool CorrectMassMatrix(
90+
gz::math::MassMatrix3d &_massMatrix,
91+
double tol = kPrincipalMomentRelativeTol);
92+
7693
/// \brief Function to get the vertices & indices of the given mesh
7794
/// & convert them into instances of the Triangle struct
7895
/// Each triangle represents a triangle in the mesh & is added
@@ -83,7 +100,7 @@ namespace gz
83100
/// \param[in] _meshScale A vector with the scaling factor
84101
/// of all the 3 axes
85102
/// \param[in] _mesh Mesh object
86-
public: void GetMeshTriangles(
103+
public: static void GetMeshTriangles(
87104
std::vector<Triangle> &_triangles,
88105
const gz::math::Vector3d &_meshScale,
89106
const gz::common::SubMesh* _mesh);
@@ -96,7 +113,7 @@ namespace gz
96113
/// moment of inertia of the mesh
97114
/// \param[out] _inertiaOrigin Pose3d object to hold the origin about
98115
/// which the inertia tensor was calculated
99-
public: void CalculateMassProperties(
116+
public: static void CalculateMassProperties(
100117
const std::vector<Triangle>& _triangles,
101118
double _density,
102119
gz::math::MassMatrix3d& _massMatrix,

‎src/MeshInertiaCalculator_TEST.cc

+102
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,102 @@
1+
/*
2+
* Copyright (C) 2025 Open Source Robotics Foundation
3+
*
4+
* Licensed under the Apache License, Version 2.0 (the "License");
5+
* you may not use this file except in compliance with the License.
6+
* You may obtain a copy of the License at
7+
*
8+
* http://www.apache.org/licenses/LICENSE-2.0
9+
*
10+
* Unless required by applicable law or agreed to in writing, software
11+
* distributed under the License is distributed on an "AS IS" BASIS,
12+
* WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
13+
* See the License for the specific language governing permissions and
14+
* limitations under the License.
15+
*
16+
*/
17+
18+
#include <gtest/gtest.h>
19+
20+
#include <gz/math/MassMatrix3.hh>
21+
22+
#include "MeshInertiaCalculator.hh"
23+
24+
using namespace gz;
25+
using namespace sim;
26+
27+
/////////////////////////////////////////////////
28+
TEST(MeshInertiaCalculator, CorrectMassMatrix)
29+
{
30+
// Verify a mass matrix with a small error can be corrected
31+
math::MassMatrix3d massMatrix(55.0,
32+
math::Vector3d(7, 15, 23),
33+
math::Vector3d::Zero);
34+
EXPECT_FALSE(massMatrix.IsValid());
35+
EXPECT_TRUE(massMatrix.IsPositive());
36+
// Set a small tolerance for the triangle inequality test and expect that
37+
// the mass matrix can not be corrected.
38+
EXPECT_FALSE(MeshInertiaCalculator::CorrectMassMatrix(massMatrix, 0.01));
39+
// Verify successful correction with default tolerance.
40+
EXPECT_TRUE(MeshInertiaCalculator::CorrectMassMatrix(massMatrix));
41+
EXPECT_TRUE(massMatrix.IsValid());
42+
43+
// Verify a mass matrix with unordered diagonal moments and a small error
44+
// can be corrected, and that the elements in the corrected principal moments
45+
// maintain the same order.
46+
math::Vector3d ixxyyzz(15, 7, 23);
47+
massMatrix = math::MassMatrix3d(55.0,
48+
ixxyyzz,
49+
math::Vector3d::Zero);
50+
EXPECT_FALSE(massMatrix.IsValid());
51+
EXPECT_TRUE(massMatrix.IsPositive());
52+
EXPECT_TRUE(MeshInertiaCalculator::CorrectMassMatrix(massMatrix));
53+
EXPECT_TRUE(massMatrix.IsValid());
54+
EXPECT_NE(ixxyyzz, massMatrix.DiagonalMoments());
55+
EXPECT_LT(massMatrix.PrincipalMoments()[1], massMatrix.PrincipalMoments()[0]);
56+
EXPECT_LT(massMatrix.PrincipalMoments()[0], massMatrix.PrincipalMoments()[2]);
57+
58+
// Verify a mass matrix with non-zero off-diagonal moments can be corrected
59+
massMatrix = math::MassMatrix3d(1.0,
60+
math::Vector3d(2.0, 2.0, 2.0),
61+
math::Vector3d(-1, 0, -0.1));
62+
EXPECT_FALSE(massMatrix.IsValid());
63+
EXPECT_TRUE(massMatrix.IsPositive());
64+
EXPECT_TRUE(MeshInertiaCalculator::CorrectMassMatrix(massMatrix));
65+
EXPECT_TRUE(massMatrix.IsValid());
66+
67+
// Verify a mass matrix with a large error can not be corrected.
68+
massMatrix = math::MassMatrix3d(15.0,
69+
math::Vector3d(1, 2, 23),
70+
math::Vector3d::Zero);
71+
EXPECT_FALSE(massMatrix.IsValid());
72+
EXPECT_TRUE(massMatrix.IsPositive());
73+
EXPECT_FALSE(MeshInertiaCalculator::CorrectMassMatrix(massMatrix));
74+
EXPECT_FALSE(massMatrix.IsValid());
75+
76+
// Verify a mass matrix with non positive-definite inertia matrix can not
77+
// be corrected.
78+
massMatrix = math::MassMatrix3d(15.0,
79+
math::Vector3d(12, -15, 23),
80+
math::Vector3d::Zero);
81+
EXPECT_FALSE(massMatrix.IsValid());
82+
EXPECT_FALSE(massMatrix.IsPositive());
83+
EXPECT_FALSE(MeshInertiaCalculator::CorrectMassMatrix(massMatrix));
84+
EXPECT_FALSE(massMatrix.IsPositive());
85+
EXPECT_FALSE(massMatrix.IsValid());
86+
87+
// Verify that CorrectMassMatrix returns true when given a valid mass matrix
88+
// and does not change the mass matrix data.
89+
massMatrix = math::MassMatrix3d(15.0,
90+
math::Vector3d(12, 15, 23),
91+
math::Vector3d::Zero);
92+
math::MassMatrix3d originalMassMatrix = massMatrix;
93+
EXPECT_TRUE(massMatrix.IsValid());
94+
EXPECT_TRUE(MeshInertiaCalculator::CorrectMassMatrix(massMatrix));
95+
EXPECT_TRUE(massMatrix.IsValid());
96+
EXPECT_DOUBLE_EQ(originalMassMatrix.Mass(),
97+
massMatrix.Mass());
98+
EXPECT_EQ(originalMassMatrix.DiagonalMoments(),
99+
massMatrix.DiagonalMoments());
100+
EXPECT_EQ(originalMassMatrix.OffDiagonalMoments(),
101+
massMatrix.OffDiagonalMoments());
102+
}

0 commit comments

Comments
 (0)