Support mesh optimization params in mesh inertial calculator (#2770)

Signed-off-by: Ian Chen <ichen@openrobotics.org>

Author: iche033

src/MeshInertiaCalculator.cc

@@ -25,9 +25,8 @@
 
 #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>
@@ -42,15 +41,15 @@ using namespace sim;
 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,62 +186,65 @@ 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();
 
   if (sdfMesh == std::nullopt)
   {
     gzerr << "Could not calculate inertia for mesh "
-    "as it std::nullopt" << std::endl;
+              "as mesh SDF is std::nullopt" << std::endl;
     _errors.push_back({sdf::ErrorCode::FATAL_ERROR,
-        "Could not calculate mesh inertia as mesh object is"
+        "Could not calculate mesh inertia as mesh SDF is"
         "std::nullopt"});
     return std::nullopt;
   }
 
-  auto fullPath = asFullPath(sdfMesh->Uri(), sdfMesh->FilePath());
-
-  if (fullPath.empty())
+  const common::Mesh *mesh = loadMesh(*sdfMesh);
+  if (!mesh)
   {
-    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"});
+    gzerr << "Failed to load mesh: " << sdfMesh->Uri() << std::endl;
+    _errors.push_back({sdf::ErrorCode::FATAL_ERROR,
+        "Could not calculate mesh inertia as mesh is not loaded."});
     return std::nullopt;
   }
 
-  // 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;
+    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);
+    // 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);
+    // Calculate mesh mass properties
+    this->CalculateMassProperties(meshTriangles, density,
+      meshMassMatrix, centreOfMass);
 
-  gz::math::Inertiald meshInertial;
+    if (meshMassMatrix.IsValid())
+      meshInertial += gz::math::Inertiald(meshMassMatrix, centreOfMass);
+  }
 
-  if (!meshInertial.SetMassMatrix(meshMassMatrix))
+  if (meshInertial.MassMatrix().Mass() <= 0.0 ||
+      !meshInertial.MassMatrix().IsValid())
   {
+    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;
+    _errors.push_back({sdf::ErrorCode::WARNING,
+        "Could not calculate valid mesh inertia"});
     return std::nullopt;
   }
-  else
-  {
-    meshInertial.SetPose(centreOfMass);
-    return meshInertial;
-  }
+
+  return meshInertial;
 }

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>
@@ -88,7 +86,7 @@ namespace gz
         public: 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

test/integration/mesh_inertia_calculation.cc

@@ -16,6 +16,7 @@
  */
 
 #include <gtest/gtest.h>
+#include <cstddef>
 #include <optional>
 #include <string>
 
@@ -207,3 +208,89 @@ TEST(MeshInertiaCalculationTest,
   EXPECT_EQ(link.WorldInertialPose(*ecm).value(),
     gz::math::Pose3d(0, 0, 1, 0, 0, 0));
 }
+
+TEST(MeshInertiaCalculationTest, CylinderColladaOptimizedMeshInertiaCalculation)
+{
+  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_convex_decomposition"),
+    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_convex_decomposition");
+  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);
+
+  // Check the Inertia Matrix within a larger tolerance since we are
+  // comparing a mesh cylinder made of convex hulls with an ideal cylinder.
+  // For values more closer to the ideal, a higher number convex decomposition
+  // paramers would be required in the mesh sdf.
+  double ixxyyzzTol = meshInertial.MassMatrix().DiagonalMoments().Max() * 0.1;
+  gz::math::Vector3d actualIxxyyzz(link.WorldInertiaMatrix(*ecm).value()(0, 0),
+                                   link.WorldInertiaMatrix(*ecm).value()(1, 1),
+                                   link.WorldInertiaMatrix(*ecm).value()(2, 2));
+  gz::math::Vector3d actualIxyxzyz(link.WorldInertiaMatrix(*ecm).value()(0, 1),
+                                   link.WorldInertiaMatrix(*ecm).value()(0, 2),
+                                   link.WorldInertiaMatrix(*ecm).value()(1, 2));
+  EXPECT_TRUE(actualIxxyyzz.Equal(meshInertial.MassMatrix().DiagonalMoments(),
+              ixxyyzzTol));
+  EXPECT_TRUE(actualIxyxzyz.Equal(
+              meshInertial.MassMatrix().OffDiagonalMoments(), 3.5));
+  // Check the Inertial Pose and Link Pose
+  EXPECT_EQ(link.WorldPose(*ecm).value(), gz::math::Pose3d::Zero);
+  EXPECT_TRUE(link.WorldInertialPose(*ecm).value().Equal(
+              gz::math::Pose3d::Zero, 1e-2));
+}

test/worlds/mesh_inertia_calculation.sdf

@@ -6,7 +6,6 @@
       <real_time_factor>1.0</real_time_factor>
     </physics>
 
-
     <include>
       <uri>cylinder_dae</uri>
       <name>cylinder_dae</name>
@@ -18,5 +17,11 @@
       <name>cylinder_dae_bottom_origin</name>
       <pose>4 0 0 0 0 0</pose>
     </include>
+
+    <include>
+      <uri>cylinder_dae_convex_decomposition</uri>
+      <name>cylinder_dae_convex_decomposition</name>
+      <pose>8 0 0 0 0 0</pose>
+    </include>
   </world>
 </sdf>

test/worlds/models/cylinder_dae_convex_decomposition/meshes/cylinder.dae

@@ -0,0 +1,16 @@
+<?xml version="1.0"?>
+
+<model>
+  <name>cylinder_dae_convex_decomposition</name>
+  <version>1.0</version>
+  <sdf version="1.11">model.sdf</sdf>
+
+  <author>
+    <name>Ian Chen</name>
+    <email></email>
+  </author>
+
+  <description>
+    A model of a cylinder collada mesh with convex decomposition
+  </description>
+</model>

test/worlds/models/cylinder_dae_convex_decomposition/model.config

@@ -0,0 +1,37 @@
+<?xml version="1.0" ?>
+<sdf version="1.11">
+  <model name="cylinder_dae_convex_decomposition">
+    <link name="cylinder_dae_convex_decomposition">
+      <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 optimization="convex_decomposition">
+            <convex_decomposition>
+              <max_convex_hulls>16</max_convex_hulls>
+              <voxel_resolution>200000</voxel_resolution>
+            </convex_decomposition>
+            <uri>meshes/cylinder.dae</uri>
+          </mesh>
+        </geometry>
+      </collision>
+      <visual name="cylinder_visual">
+        <pose>0 0 0 0 0 0</pose>
+        <geometry>
+          <mesh>
+            <uri>meshes/cylinder.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>