Added Mesh Moment of Inertia Calculator

CMakeLists.txt

@@ -94,6 +94,7 @@ gz_find_package(gz-common5
     events
     av
     io
+    graphics
   REQUIRED
 )
 set(GZ_COMMON_VER ${gz-common5_VERSION_MAJOR})

src/CMakeLists.txt

@@ -73,6 +73,7 @@ set (sources
   LevelManager.cc
   Light.cc
   Link.cc
+  MeshInertiaCalculator.cc
   Model.cc
   Primitives.cc
   SdfEntityCreator.cc
@@ -202,6 +203,7 @@ target_link_libraries(${PROJECT_LIBRARY_TARGET_NAME}
   gz-math${GZ_MATH_VER}
   gz-plugin${GZ_PLUGIN_VER}::core
   gz-common${GZ_COMMON_VER}::gz-common${GZ_COMMON_VER}
+  gz-common${GZ_COMMON_VER}::graphics
   gz-common${GZ_COMMON_VER}::profiler
   gz-fuel_tools${GZ_FUEL_TOOLS_VER}::gz-fuel_tools${GZ_FUEL_TOOLS_VER}
   gz-gui${GZ_GUI_VER}::gz-gui${GZ_GUI_VER}

src/MeshInertiaCalculator.cc

@@ -0,0 +1,231 @@
+/*
+ * Copyright (C) 2023 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 "MeshInertiaCalculator.hh"
+
+#include <optional>
+#include <vector>
+
+#include <sdf/CustomInertiaCalcProperties.hh>
+#include <sdf/Types.hh>
+
+#include <gz/sim/Util.hh>
+
+#include <gz/common/graphics.hh>
+#include <gz/common/Mesh.hh>
+#include <gz/common/MeshManager.hh>
+
+#include <gz/math/Vector3.hh>
+#include <gz/math/Pose3.hh>
+#include <gz/math/MassMatrix3.hh>
+#include <gz/math/Inertial.hh>
+
+using namespace gz;
+using namespace sim;
+
+//////////////////////////////////////////////////
+void MeshInertiaCalculator::GetMeshTriangles(
+    std::vector<Triangle>& _triangles,
+    const gz::common::Mesh* _mesh)
+{
+  // Get the vertices & indices of the mesh
+  double* vertArray = nullptr;
+  int* indArray = nullptr;
+  _mesh->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)
+  {
+    Triangle triangle;
+    triangle.v0.X() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i])];
+    triangle.v0.Y() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i] + 1)];
+    triangle.v0.Z() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i] + 2)];
+    triangle.v1.X() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i+1])];
+    triangle.v1.Y() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i+1] + 1)];
+    triangle.v1.Z() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i+1] + 2)];
+    triangle.v2.X() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i+2])];
+    triangle.v2.Y() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i+2] + 1)];
+    triangle.v2.Z() = vertArray[static_cast<ptrdiff_t>(3 * indArray[i+2] + 2)];
+    _triangles.push_back(triangle);
+  }
+}
+
+//////////////////////////////////////////////////
+void MeshInertiaCalculator::CalculateMassProperties(
+  const std::vector<Triangle>& _triangles,
+  double _density,
+  gz::math::MassMatrix3d& _massMatrix,
+  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,
+                                   1.0 / 60,  1.0 / 60, 1.0 / 60, 1.0 / 120,
+                                   1.0 / 120, 1.0 / 120};
+
+  // Number of triangles of in the mesh
+  std::size_t numTriangles = _triangles.size();
+
+  // Vector to store cross products of 2 vectors of the triangles
+  std::vector<gz::math::Vector3d> crosses(numTriangles);
+
+  // Caculating cross products of 2 vectors emerging from a common vertex
+  // This basically gives a vector normal to the plane of triangle
+  for (std::size_t i = 0; i < numTriangles; ++i)
+  {
+    crosses[i] =
+      (_triangles[i].v1 - _triangles[i].v0).Cross(
+        _triangles[i].v2 - _triangles[i].v0);
+  }
+
+  // Calculate subexpressions of the integral
+  std::vector<gz::math::Vector3d> f1(numTriangles), f2(numTriangles),
+  f3(numTriangles), g0(numTriangles), g1(numTriangles), g2(numTriangles);
+
+  for (std::size_t i = 0; i < numTriangles; ++i)
+  {
+      f1[i] = _triangles[i].v0 + _triangles[i].v1 + _triangles[i].v2;
+      f2[i] = _triangles[i].v0 * _triangles[i].v0 +
+              _triangles[i].v1 * _triangles[i].v1 +
+              _triangles[i].v0 * _triangles[i].v1 +
+              _triangles[i].v2 * f1[i];
+      f3[i] = _triangles[i].v0 * _triangles[i].v0 * _triangles[i].v0 +
+              _triangles[i].v0 * _triangles[i].v0 * _triangles[i].v1 +
+              _triangles[i].v0 * _triangles[i].v1 * _triangles[i].v1 +
+              _triangles[i].v1 * _triangles[i].v1 * _triangles[i].v1 +
+              _triangles[i].v2 * f2[i];
+      g0[i] = f2[i] + (_triangles[i].v0 + f1[i]) * (_triangles[i].v0);
+      g1[i] = f2[i] + (_triangles[i].v1 + f1[i]) * (_triangles[i].v1);
+      g2[i] = f2[i] + (_triangles[i].v2 + f1[i]) * (_triangles[i].v2);
+  }
+
+  // Calculate integral terms
+  std::vector<double> integral(10);
+  for (std::size_t i = 0; i < numTriangles; ++i)
+  {
+      double x0 = _triangles[i].v0.X();
+      double y0 = _triangles[i].v0.Y();
+      double z0 = _triangles[i].v0.Z();
+      double x1 = _triangles[i].v1.X();
+      double y1 = _triangles[i].v1.Y();
+      double z1 = _triangles[i].v1.Z();
+      double x2 = _triangles[i].v2.X();
+      double y2 = _triangles[i].v2.Y();
+      double z2 = _triangles[i].v2.Z();
+      integral[0] += crosses[i].X() * f1[i].X();
+      integral[1] += crosses[i].X() * f2[i].X();
+      integral[2] += crosses[i].Y() * f2[i].Y();
+      integral[3] += crosses[i].Z() * f2[i].Z();
+      integral[4] += crosses[i].X() * f3[i].X();
+      integral[5] += crosses[i].Y() * f3[i].Y();
+      integral[6] += crosses[i].Z() * f3[i].Z();
+      integral[7] += crosses[i].X() *
+        (y0 * g0[i].X() + y1 * g1[i].X() + y2 * g2[i].X());
+      integral[8] += crosses[i].Y() *
+        (z0 * g0[i].Y() + z1 * g1[i].Y() + z2 * g2[i].Y());
+      integral[9] += crosses[i].Z() *
+        (x0 * g0[i].Z() + x1 * g1[i].Z() + x2 * g2[i].Z());
+  }
+
+  for (int i = 0; i < 10; ++i)
+  {
+      integral[i] *= coefficients[i];
+  }
+
+  // Accumulate the result and add it to MassMatrix object of gz::math
+  double volume = integral[0];
+  double mass = volume * _density;
+  _centreOfMass.SetX(integral[1] / mass);
+  _centreOfMass.SetY(integral[2] / mass);
+  _centreOfMass.SetZ(integral[3] / mass);
+  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 *
+                (_centreOfMass.Y() * _centreOfMass.Y() +
+                _centreOfMass.Z() * _centreOfMass.Z()));
+  ixxyyzz.Y() = (integral[4] + integral[6] - mass *
+                (_centreOfMass.Z() * _centreOfMass.Z() +
+                _centreOfMass.X() * _centreOfMass.X()));
+  ixxyyzz.Z() = integral[4] + integral[5] - mass *
+                (_centreOfMass.X() * _centreOfMass.X() +
+                _centreOfMass.Y() * _centreOfMass.Y());
+
+  // Off Diagonal Elements of the Mass Matrix
+  ixyxzyz.X() = -(integral[7] - mass * _centreOfMass.X() * _centreOfMass.Y());
+  ixyxzyz.Y() = -(integral[8] - mass * _centreOfMass.Y() * _centreOfMass.Z());
+  ixyxzyz.Z() = -(integral[9] - mass * _centreOfMass.X() * _centreOfMass.Z());
+
+  // Set the values in the MassMatrix object
+  _massMatrix.SetMass(mass);
+  _massMatrix.SetDiagonalMoments(ixxyyzz * _density);
+  _massMatrix.SetOffDiagonalMoments(ixyxzyz * _density);
+}
+
+//////////////////////////////////////////////////
+std::optional<gz::math::Inertiald> MeshInertiaCalculator::operator()
+  (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;
+    _errors.push_back({sdf::ErrorCode::FATAL_ERROR,
+        "Could not calculate mesh inertia as mesh object is"
+        "std::nullopt"});
+    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;
+  }
+
+  // Load the Mesh
+  gz::common::MeshManager *meshManager = gz::common::MeshManager::Instance();
+  mesh = meshManager->Load(fullPath);
+  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, mesh);
+  this->CalculateMassProperties(meshTriangles, density,
+    meshMassMatrix, centreOfMass);
+  gz::math::Inertiald meshInertial;
+
+  if (!meshInertial.SetMassMatrix(meshMassMatrix))
+  {
+    return std::nullopt;
+  }
+  else
+  {
+    meshInertial.SetPose(centreOfMass);
+    return meshInertial;
+  }
+}

src/MeshInertiaCalculator.hh

@@ -0,0 +1,117 @@
+/*
+ * Copyright (C) 2023 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.
+ *
+ */
+
+#ifndef GZ_SIM_MESHINERTIACALCULATOR_HH_
+#define GZ_SIM_MESHINERTIACALCULATOR_HH_
+
+#include <optional>
+#include <vector>
+
+#include <sdf/CustomInertiaCalcProperties.hh>
+#include <sdf/Types.hh>
+
+#include <gz/sim/Export.hh>
+#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/math/Vector3.hh>
+#include <gz/math/Pose3.hh>
+#include <gz/math/MassMatrix3.hh>
+#include <gz/math/Inertial.hh>
+
+
+namespace gz
+{
+  namespace sim
+  {
+    // Inline bracket to help doxygen filtering.
+    inline namespace GZ_SIM_VERSION_NAMESPACE
+    {
+      /// \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
+      /// each of which represents a vertex of the triangle
+      struct Triangle
+      {
+        gz::math::Vector3d v0, v1, v2;
+      };
+
+      /// \class MeshInertiaCalculator gz/sim/MeshInertiaCalculator.hh
+      /// \brief Inertial Properties (Mass, Centre of Mass & Moments of
+      /// Inertia) calculator for 3D meshes.
+      ///
+      /// This class overloads the () operator, therefore, an instance
+      /// of this class registered with libsdformat as a Custom
+      /// Inertia Calculator. This would be used to calculate the
+      /// inertial properties of 3D mesh using SDFormat.
+      ///
+      /// 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
+      {
+        /// \brief Constructor
+        public: MeshInertiaCalculator() = default;
+
+        /// \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
+        /// to a vector
+        /// \param[out] _triangles A vector to hold all the Triangle
+        /// instances obtained
+        /// from the vertices & indices of the mesh
+        /// \param[in] _mesh Mesh object
+        public: void GetMeshTriangles(
+          std::vector<Triangle>& _triangles,
+          const gz::common::Mesh* _mesh);
+
+        /// \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
+        /// \param[in] _density Density of the mesh
+        /// \param[out] _massMatrix MassMatrix object to hold mass &
+        /// moment of inertia of the mesh
+        /// \param[out] _centreOfMass Pose3d object to hold the centre of
+        /// mass of the object
+        public: void CalculateMassProperties(
+          const std::vector<Triangle>& _triangles,
+          double _density,
+          gz::math::MassMatrix3d& _massMatrix,
+          gz::math::Pose3d& _centreOfMass);
+
+        /// \brief Overloaded () operator which allows an instance
+        /// of this class to be registered as a Custom Inertia
+        /// Calculator with libsdformat
+        /// \param[in] _errors A vector of Errors object. Each object
+        /// would contain an error code and an error message.
+        /// \param _calculatorParams An instance of
+        /// CustomInertiaCalcProperties. This instance can be used
+        /// to access the data like density,
+        /// properties of the mesh, etc.
+        public: std::optional<gz::math::Inertiald> operator()(
+          sdf::Errors& _errors,
+          const sdf::CustomInertiaCalcProperties& _calculatorParams);
+      };
+    }
+  }
+}
+
+#endif