Proposal for Automatic Moments of Inertia Calculations

• Authors: Jasmeet Singh <jasmeet0915@gmail.com>, Addisu Taddese <addisu@openrobotics.org>, Dharini Dutia <dharini@openrobotics.org>
• Status: Draft
• SDFormat Version: 1.11
• libSDFormat Version: 14.X

Introduction

This proposal suggests adding new attributes and elements to support the automatic calculation of Moments of Inertia and Products of Inertia for a link in SDFormat 1.11. It also proposes adding support for parsing these elements and attributes in libsdformat14.

Setting physically plausible values for inertial parameters is crucial for an accurate simulation. However, these parameters are often complex to comprehend and visualize, and users may tend to enter wrong values leading to incorrect simulation. Therefore, native support for calculating inertial parameters through SDFormat specification would enable accurate simulations in simulators that use SDFormat.

Document summary

The proposal includes the following sections:

• Motivation: A short explanation to provide context regarding the problem statement and the need for this feature
• User Perspective: Describes the current usage and the proposed usage by describing the terms to be added in the SDFormat specification
• Proposed Implementation: Detailed explanation of the proposed implementation and modifications to be done in the C++ API of requried libraries like libsdformat.

Syntax

The proposal uses XPath syntax to describe elements and attributes concisely. For example, <model> tags are referred to as //model using XPath. XPath is even more concise for referring to nested tags and attributes. In the following example, <link> elements inside <model> tags are referenced as //model/link and model name attributes as //model/@name:

<model name="model_name">
  <link/>
</model>

Motivation

Currently, there are 2 major workflows used by the users to obtain the correct inertial parameters of their models:

• Using CAD software like Fusion360 or Solidworks. Many users design their robot models using such CAD software which provide plugins that automatically generate the URDF/SDFormat for their model. Such plugins handle the calculation of the inertial parameters. For example, Fusion360 provides the Fusion2URDF plugin which automatically generates a URDF with all the inertial parameters.

• Another way is to use 3rd-party Mesh Processing Software like Meshlab. Such softwares take the mesh file as an input and provide the inertial parameters as an output which can then be copied and pasted into the URDF/SDFormat file. This is also the method that was suggested in official Classic Gazebo docs.

Both of these ways create a dependency on external software and might be complicated for beginners. In case the user doesn't provide any inertial values, a default Mass Matrix is used with mass = 1.0 and Diagonal Elements = (1, 1, 1) which might not be best suited for all kinds of models. Native support for automatic inertia calculations directly into libsdformat would work as a better alternative to using the default values and facilitate the effortless generation of accurate simulations.

User Perspective

To specify the <inertial> element of a <link> in SDFormat, the user needs to add the <mass>, <pose>, and <inertia> tags. Here <mass> is the mass of the link and <pose> is the pose of the center of Mass with respect to the link frame. The <inertia> tag, on the other hand, needs to enclose the following 6 tags:

• <ixx> (Moment of Inertia)

• <iyy> (Moment of Inertia)

• <izz> (Moment of Inertia)

• <ixy> (Product of Inertia)

• <ixz> (Product of Inertia)

• <izy> (Product of Inertia)

This proposal suggests the addition of the following elements and attributes in SDFormat Spec:

1. //inertial/@auto attribute that would tell libsdformat to calculate the Inertial values (Mass, Mass Matrix & Inertial Pose) automatically for the respective link.

2. //collision/density element for specifying the density of the collision geometry of a link. This density values would be used to calculate the inertial parameters of the respective collision geometries. Adding this as part of the <collision> tag would allow a user to simulate links with different material types for different collisions. By default, the value of density would be set equal to that of water which is 1000 kg/m^3. A //link/inertial/density element would also be added in the spec to allow users to specify the density values on a link level instead of specifying the same values for each collision.

3. //collision/auto_inertia_params element would be added that can be used to provide some parameters or options for a custom inertia calculator. Similar to the density element above, a link-level //link/inertial/auto_inertia_params element would also be added. This would allow the user to provide inertia calculator parameters for the whole link while retaining the flexibility to specify different parameters for each collision. Custom elements and attributes using a namespace prefix can be used to provide the user-defined parameters for a custom calculator. More about this can be found in this proposal.

The example snippet below shows how the above proposed elements would be used in a SDFormat <link>:

<link name="robot_link">
  <inertial auto="true">
    <auto_inertia_params>
      <gz:voxel_size>0.01</gz:voxel_size>
    </auto_inertia_params>
  <collision name="collision">
    <density>*some_float_value*</density>
    <geometry>
      .
      .
    </geometry>
  </collision>
  <visual name="visual">
    <geometry>
      .
      .
    </geometry>
    <material>
      .
      .
    </material>
  </visual>
</link>

Proposed Implementation

Note: In the section below, the term primitive geometries is used to collectively describe Box, Capusle, Cylinder, Ellipsoid and Sphere geometries.

Some Architectural Considerations

Below are some key architectural considerations for the implementation of this feature:

• The parsing of the proposed SDFormat elements and the Moment of Inertia calculations for primitive geometries (Box, Cylinder, Sphere, Ellipsoid and Capsule) can be implemented as an integral part of libsdformat. This would help enable all simulators that rely on SDFormat to utilize this feature.

• In case of 3D meshes being used as geometries, a modular callback-based architecture can be followed where the user can integrate their custom Moments of Inertia calculator. Two methods were explored computing the inertial properties (mass, mass matrix and center of mass) of 3D meshes: a Voxelization-based and an integration-based numerical method.

• For links where <inertial> tag is not set, the inertial calculations would be omitted if <static> is set to true. By default it is set as \(I_{xx}=I_{yy}=I_{zz}=1\) and \(I_{xy}=I_{yz}=I_{xz}=0\).

• The collision geometry of the link would used for all the inertial calculations. In case of multiple collisions, the inertial pose of each collision would be set in the link frame (if not already done) and then inertials would be aggregated using the + operator from the gz::math::Inertial class. If, however, no collisions are provided, an ELEMENT_MISSING error would be thrown.

Implemetation for Primitive Geometries

A std::optional<gz::math::Inertiald> CalculateInertial(double density) function would be added to the classes of all the Geometry Types which would be supported by this feature (Box, Capsule, Cylinder, Ellipsoid, Sphere and Mesh). For all the types except Mesh, existing MassMatrix() functions from the gz::math class of the respective geometry would be used for their inertial calculations. An additional data member double density would be added to sdf::Collision along with getter and setter functinons to get/set the density value of each collision. The density value would be sent to CalculateInertial() functions as a param. In case of the primitive geometries, the density value would be used to create a gz::math::Material object before calling the MassMatrix() function of the respective geometry. Below is the implmentation of the Box::CalculateInertial() function as an example:

/// \brief Calculate and return the Mass Matrix values for the Box
/// \param[in] _density Density of the box in kg/m^3
/// \return A std::optional with gz::math::Inertiald object or std::nullopt
public: std::optional<gz::math::Inertiald>
        CalculateInertial(double _density);

/////////////////////////////////////////////////
std::optional<gz::math::Inertiald> Box::CalculateInertial(double _density)
{
  gz::math::Material material = gz::math::Material(_density);
  this->dataPtr->box.SetMaterial(material);

  auto boxMassMatrix = this->dataPtr->box.MassMatrix();

  if (!boxMassMatrix)
  {
    return std::nullopt;
  }
  else
  {
    gz::math::Inertiald boxInertial;
    boxInertial.SetMassMatrix(boxMassMatrix.value());
    return std::make_optional(boxInertial);
  }
}

Callback-based API for Mesh/Custom Inertia Calculator

A CustomInertiaCalcProperties class with the following members would be added:

class CustomInertiaCalcProperties::Implementation
{
  /// \brief Density of the mesh. 1000 kg/m^3 by default
  public: double density{1000.0};

  /// \brief Optional SDFormat mesh object. Default is std::nullopt
  public: std::optional<sdf::Mesh> mesh{std::nullopt};

  /// \brief SDFormat element pointer to <auto_inertia_params> tag.
  /// This can be used to access custom params for the
  /// Inertia Caluclator
  public: sdf::ElementPtr inertiaCalculatorParams{nullptr};
};

This class would act as an interface between libsdformat and the custom calculator to bridge the data like density, properties of the mesh and user-defined calculator params given through the SDF. A signature for the Custom Calculator function is provided through an alias:

using CustomInertiaCalculator = std::function<std::optional<gz::math::Inertiald>(sdf::Errors &, const sdf::CustomInertiaCalcProperties &)>;

Functions to get and register a custom inertia calculator is provided through the sdf::ParserConfig. Using all these additions, the Mesh::CalculateInertial() function would be implmented as follows:

  /// \brief Calculate and return the Mass Matrix values for the Mesh
  /// \param[in] density Density of the mesh in kg/m^3
  /// \param[in] _autoInertiaParams ElementPtr to
  /// <auto_inertia_params> element
  /// \param[in] _config Parser Configuration
  /// \param[out] _errors A vector of Errors object. Each object
  /// would contain an error code and an error message.
  /// \return A std::optional with gz::math::Inertiald object or std::nullopt
  public: std::optional<gz::math::Inertiald> CalculateInertial(double _density, const sdf::ElementPtr _autoInertiaParams, const ParserConfig &_config, sdf::Errors &_errors);

  //////////////////////////////////////////////////
  std::optional<gz::math::Inertiald> Mesh::CalculateInertial(double _density,
      const sdf::ElementPtr _autoInertiaParams, const ParserConfig &_config,
      sdf::Errors &_errors)
  {
    if (this->dataPtr->filePath.empty())
    {
      _errors.push_back({
        sdf::ErrorCode::WARNING,
        "File Path for the mesh was empty. Could not calculate inertia"});
      return std::nullopt;
    }

    const auto &customCalculator = _config.CustomInertiaCalc();

    sdf::CustomInertiaCalcProperties calcInterface = CustomInertiaCalcProperties(
      _density, *this, _autoInertiaParams);

    return customCalculator(_errors, calcInterface);
  }

Configuring the CalculateInertial() function behavior

CalculateInertial() functions are also added to the sdf::Root, sdf::World, sdf::Model, sdf::Link, sdf::Collision and sdf::Geometry classes. Calling the CalculateInertial() function of any class, in turn recursively calls the CalculateInertial() for each element below it down the chain. For eg, generally a user might call the Root::CalculateInertial() which further calls World::CalculateInertials() for all the worlds which in turn calls the Model::CaluclateInertials() for all the models in the world and so on until the final CalculateInertial() for the respective geometry type is called where the chain ends.

The sdf::ParserConfig object is sent down the CalculateInertial() chain as a function parameter and is used to get the configuration information like the registered Custom Inertia Calculator. An enum class would be added to the sdf::ParserConfig with values that would allow the user to configure the behavior of the CalculateInertial() function:

/// \enum ConfigureCalculateInertial
/// \brief Configuration options of how CalculateInertial() function
/// would be used
enum class ConfigureCalculateInertial
{
  /// \brief If this value is used, CalculateInertial() won't be 
  /// called from inside the Root::Load() function
  SKIP_CALCULATION_IN_LOAD,

  /// \brief If this values is used, CalculateInertial() would be 
  /// called and the computed inertial values would be saved 
  SAVE_CALCULATION
};

Setting values from the above enum for the sdf::ParserConfig object, the user can configure the CalculateInertial() functions. For eg: if the configuration is set to SKIP_CALCULATION_IN_LOAD (which would be the default configuration), then the Root::CalculateInertial() won't be called from within the Root::Load(). Since the inertia calculations require a valid PoseGraph to be built for resolving the inertial poses in the link frame, this configuration would allow the user to skip inertial calculation in load if they know the DOM object doesn't have a valid PoseGraph. Root:CalculateInertial() can then be called separately after the load is complete and a valid PoseGraph is available.

Proposed Mesh Inertia Calculation Methods

Voxelization-based method

Voxels are the 3D equivalent of a pixel in 2D^[1]. Voxels can be arranged in ‘Voxel Grids’ ^{[2, 3]} which are the 3D equivalent of a structured image using pixels.

During the voxelization of a point cloud or mesh, all the points in the 3D data are intersected with a Voxel Grid. The voxels which contain a point of the mesh are kept while others are zeroed out (discarded). This way, we are left with a voxelized mesh that closely resembles the original mesh.

Voxelization of meshes/point cloud data is widely used for mesh processing. It can be used for feature extraction, occupancy analysis, asset generation, surface simplification, etc.^[4]

Moment of Inertia Matrix Calculation

The Moment of Inertia Matrix of an object is a 3x3 symmetric matrix. This means for all elements in a Moment of Inertia Matrix, I:

$$ I_{ij} = I_{ji} $$

The diagonal elements of the matrix are denoted as \(I_{xx}\), \(I_{yy}\) and \(I_{zz}\) and are the Moments of Inertia of the object. The remaining 6 off-diagonal elements are called the Products of Inertia and their value depends on the symmetry of the object about the axes about which the MOI Tensor is being calculated. Only 3 values out of the 6 are needed since the matrix is symmetric.

Note: If the axis about which the MOI Tensor is calculated, is taken to be the principal axis of inertia, then the products of inertia become 0 and the matrix becomes a diagonal matrix.

All these values of MOI Tensor can be calculated as follows:

$$\begin{eqnarray} I_{11} = I_{xx} = \int (y^2 + z^2)dm \\ I_{22} = I_{yy} = \int (x^2 + z^2)dm \\ I_{33} = I_{zz} = \int (x^2 + y^2)dm \\ \end{eqnarray}$$

Here \(dm\) is the mass of an infinitesimal unit of the object and \(x, y, z\) are the distances of that unit from axes.

Similarly, the Products of Inertia can be calculated as:

$$\begin{eqnarray} I_{12} = I_{xy} = \int -xydm = I_{yx} = I_{21} \\ I_{13} = I_{xz} = \int -xzdm = I_{zx} = I_{31} \\ I_{23} = I_{yz} = \int -yzdm = I_{zy} = I_{32} \\ \end{eqnarray}$$

In this solution, the infinitesimal element of the object can be represented by the each Voxel. Instead of calculating the mass \(dm\) for each voxel, we will calculate the volume \(dv\) of each voxel using the voxel size (the simple volume formula of a cube can be used because voxels are cubes). Then mass for each element would be mass density, \(\rho\) multiplied by the volume, \(dv\). Considering the mass density to be constant and substituting in the above equations we get:

$$\begin{eqnarray} I_{11} = I_{xx} = \rho\int (y^2 + z^2)dv \\ I_{22} = I_{yy} = \rho\int (x^2 + z^2)dv \\ I_{33} = I_{zz} = \rho\int (x^2 + y^2)dv \\ I_{12} = I_{xy} = \rho\int -xydv = I_{yx} = I_{21} \\ I_{13} = I_{xz} = \rho\int -xzdv = I_{zx} = I_{31} \\ I_{23} = I_{yz} = \rho\int -yzdv = I_{zy} = I_{32} \\ \end{eqnarray}$$

Advantages of the Voxelization Approach

• Voxelization is a generalized approach that would work for all kinds of meshes (convex and non-convex meshes).
• It can be made configurable by allowing the user to set an appropriate voxel_size for the process. A smaller voxel_size would be comparitively computationally heavy but would provide closer to real world value for moment of inertia.
• Voxelization is more intuitive way of moment of inertia calculations as compared to other integral methods.

Integration-based numerical method

This method utilizes Gauss’s Theorem and Greene’s Theorem of integration. First, we convert volume integrals to surface integrals using Gauss’s Theorem and then surface integrals to line integrals using Greene’s Theorem^[5]. This method works for triangle meshes which are simple water-tight polyhedrons. Currently, the origin of the mesh being used needs to be set at the geometric center to obtain the correct value.

Some Key Points Regarding the Integration-based numerical method

These points are listed according to the reference implementation in gz-sim

• Water-tight triangle meshes are required for the Mesh Inertia Calculator.

• The inertia in this method is calculated about the mesh origin. Since the link inertia value needs to be about the Center of Mass, the mesh origin needs to be set at the Center of Mass (Centroid) of the mesh.

• Since this method utilizes the mesh's vertex data for calculations, a high vertex count would be needed for near-ideal values. However, it is recommended to use basic shapes with the geometry tag (Box, Capsule, Cylinder, Ellipsoid, and Sphere) as collision geometries to reduce the load of calculations. For eg: in this integration test a cylinder mesh with 4096 vertices was used which resulted in inertia values withing a 0.005 tolerance of ideal.

References

• [1]: https://en.wikipedia.org/wiki/Voxel

• [2]: https://davidstutz.de/efficiently-voxelizing-watertight-meshes-into-occupancy-grids-and-signed-distance-functions/\

• [3]: https://towardsdatascience.com/how-to-voxelize-meshes-and-point-clouds-in-python-ca94d403f81d

• [4]: https://blog.spatial.com/the-main-benefits-and-disadvantages-of-voxel-modeling

• [5]: https://www.geometrictools.com/Documentation/PolyhedralMassProperties.pdf