Automatic Moment of Inertia Calculations for Basic Shapes

[jasmeet0915]

🎉 New feature

Based on this proposal.

Summary

This PR adds Automatic Moment of Inertia Calculations for Basic Shapes (Box, Capsule, Cylinder, Ellipsoid and Sphere). As mentioned in the proposal the PR adds the following to the SDF Spec:

• //inertial/@auto attribute to enable automatic calculations
• //link/collision/density tag for mentioning the density for the collision

Note: This would require merging this PR which bumps the SDF Spec version to 1.11 for the addition of new tags

Demos & Usage Example

Demo 1: This demo shows 2 cylinders: one with default inertial values (right, green) and the other with automatic inertia calculations enabled (left, yellow).
The yellow cylinder uses <inertial auto="true" /> and <density>1240.0</density> for automatic calculations.

SDF snippet for the yellow cylinder

  <model name="cylinder2">
    <pose>0 4 1 0 0 0</pose>
    <link name="cylinder_link">
      <inertial auto="true" />
      <collision name="collision">
        <density>1240.0</density>
        <geometry>
          <cylinder>
            <radius>1</radius>
            <length>2</length>
          </cylinder>
        </geometry>
      </collision>
      <visual name="visual">
        <geometry>
          <cylinder>
            <radius>1</radius>
            <length>2</length>
          </cylinder>
        </geometry>
        <material>
          <diffuse>1.0 1.0 0.0 1.0</diffuse>
          <ambient>1.0 1.0 0.0 1.0</ambient>
          <specular>1.0 1.0 0.0 1.0</specular>
        </material>
      </visual>
    </link>
  </model>

Demo 2: This demo shows a model with a link having 2 collisions: a cube with a cylinder on top of it.

Here the model uses <inertial auto="true" /> with <density>2.0</density> for the box collision and <density>4.0</density> for the cylinder.

SDF snippet of the model in the demo

  <model name="compound_model">
    <pose>0 0 1.0 0 0 0</pose>
    <link name="compound_link">
      <inertial auto="true" />
      <collision name="box_collision">
        <pose>0 0 -0.5 0 0 0</pose>
          <density>2.0</density>
        <geometry>
          <box>
            <size>1 1 1</size>
          </box>
        </geometry>
      </collision>
      <collision name="cylinder_compound_collision">
        <pose>0 0 0.5 0 0 0</pose>
        <density>4</density>
        <geometry>
          <cylinder>
            <radius>0.5</radius>
            <length>1.0</length>
          </cylinder>
        </geometry>
      </collision>

      <visual name="cylinder_visual">
        <pose>0 0 0.5 0 0 0</pose>
        <geometry>
          <cylinder>
            <radius>0.5</radius>
            <length>1.0</length>
          </cylinder>
        </geometry>
        <material>
          <ambient>1 1 0 1</ambient>
          <diffuse>1 1 0 1</diffuse>
          <specular>1 1 0 1</specular>
        </material>
      </visual>
      <visual name="box_visual">
        <pose>0 0 -0.5 0 0 0</pose>
        <geometry>
          <box>
            <size>1 1 1</size>
          </box>
        </geometry>
        <material>
          <ambient>1 0 0 1</ambient>
          <diffuse>1 0 0 1</diffuse>
          <specular>1 0 0 1</specular>
        </material>
      </visual>
    </link>
  </model>

TODO:

• Enable the addition of the Inertias of multiple collisions (with something like what is done for the Capsule shape here). This is a work in progress here
• Make auto an attribute of <inertial> tag instead and calculate mass and inertial pose. Update the proposal accordingly.
• Return an error in case auto is set and link has no collisions
• Change <material_density> tag to something less verbose like just <density> or <mass_density>. Update the proposal accordingly.
• Change all MassMatrix functions to return gz::math::inertial as we are now calculating all inertial values (mass, mass matrix, inertial pose)
• Add an Enum to ParserConfig to configure the CalculateInertial() function. Update the function to use the enum from the config.
• Update spec to add //link/inertial/density element.
• Update the Proposed Implementation section in the proposal
• Add MassMatrix Methods to the Python API
• Add tests

Checklist

• Signed all commits for DCO
• Added tests
• Added example and/or tutorial
• Updated documentation (as needed)
• Updated migration guide (as needed)
• Consider updating Python bindings (if the library has them)
• codecheck passed (See contributing)
• All tests passed (See test coverage)
• While waiting for a review on your PR, please help review another open pull request to support the maintainers

Note to maintainers: Remember to use Squash-Merge and edit the commit message to match the pull request summary while retaining Signed-off-by messages.

[ahcorde]

why this is empty ? remove ?

[jasmeet0915]

As mentioned in the migration.md file in the 'Note on Backward Compatibility' section, the .convert files are used to facilitate forward conversion and are present in all the spec directories.

[ahcorde]

we should add these new methods to the Python API.

std::optional< gz::math::MassMatrix3d >  MassMatrix(const double _density);

[azeey]

Great work! And thanks for iterating patiently :)

All feedback seems to be addressed