Automatic Moment of Inertia Calculations for Basic Shapes (#1299)

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/inertial/density` tag for mentioning the density for the collision
`//link/collision/density` tag for overriding the value density set at the `//link/inertial/density`.
---------

Signed-off-by: Jasmeet Singh <jasmeet0915@gmail.com>

include/sdf/Box.hh

@@ -17,8 +17,11 @@
 #ifndef SDF_BOX_HH_
 #define SDF_BOX_HH_
 
+#include <optional>
+
 #include <gz/math/Box.hh>
 #include <gz/math/Vector3.hh>
+#include <gz/math/Inertial.hh>
 #include <gz/utils/ImplPtr.hh>
 #include <sdf/Error.hh>
 #include <sdf/Element.hh>
@@ -65,6 +68,14 @@ namespace sdf
     /// \return A reference to a gz::math::Boxd object.
     public: gz::math::Boxd &Shape();
 
+    /// \brief Calculate and return the Inertial values for the Box. In order
+    /// to calculate the inertial properties, the function mutates the object
+    /// by updating its material properties.
+    /// \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);
+
     /// \brief Create and return an SDF element filled with data from this
     /// box.
     /// Note that parameter passing functionality is not captured with this

include/sdf/Capsule.hh

@@ -17,7 +17,10 @@
 #ifndef SDF_CAPSULE_HH_
 #define SDF_CAPSULE_HH_
 
+#include <optional>
+
 #include <gz/math/Capsule.hh>
+#include <gz/math/Inertial.hh>
 #include <gz/utils/ImplPtr.hh>
 #include <sdf/Error.hh>
 #include <sdf/Element.hh>
@@ -72,6 +75,14 @@ namespace sdf
     /// \return A reference to a gz::math::Capsuled object.
     public: gz::math::Capsuled &Shape();
 
+    /// \brief Calculate and return the Inertial values for the Capsule.
+    /// In order to calculate the inertial properties, the function
+    /// mutates the object by updating its material properties.
+    /// \param[in] _density Density of the capsule 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);
+
     /// \brief Create and return an SDF element filled with data from this
     /// capsule.
     /// Note that parameter passing functionality is not captured with this

include/sdf/Collision.hh

@@ -20,12 +20,15 @@
 #include <memory>
 #include <string>
 #include <gz/math/Pose3.hh>
+#include <gz/math/Vector3.hh>
+#include <gz/math/Inertial.hh>
 #include <gz/utils/ImplPtr.hh>
 #include "sdf/Element.hh"
 #include "sdf/SemanticPose.hh"
 #include "sdf/Types.hh"
 #include "sdf/sdf_config.h"
 #include "sdf/system_util.hh"
+#include "sdf/ParserConfig.hh"
 
 namespace sdf
 {
@@ -75,6 +78,14 @@ namespace sdf
     /// \param[in] _name Name of the collision.
     public: void SetName(const std::string &_name);
 
+    /// \brief Get the density of the collision.
+    /// \return Density of the collision.
+    public: double Density() const;
+
+    /// \brief Set the density of the collision.
+    /// \param[in] _density Density of the collision.
+    public: void SetDensity(double _density);
+
     /// \brief Get a pointer to the collisions's geometry.
     /// \return The collision's geometry.
     public: const Geometry *Geom() const;
@@ -119,6 +130,16 @@ namespace sdf
     /// \return SemanticPose object for this link.
     public: sdf::SemanticPose SemanticPose() const;
 
+    /// \brief Calculate and return the MassMatrix for the collision
+    /// \param[out] _errors A vector of Errors objects. Each errors contains an
+    /// Error code and a message. An empty errors vector indicates no errors
+    /// \param[in] _config Custom parser configuration
+    /// \param[out] _inertial An inertial object which will be set with the
+    /// calculated inertial values
+    public: void CalculateInertial(sdf::Errors &_errors,
+                                  gz::math::Inertiald &_inertial,
+                                  const ParserConfig &_config);
+
     /// \brief Get a pointer to the SDF element that was used during
     /// load.
     /// \return SDF element pointer. The value will be nullptr if Load has

include/sdf/Cylinder.hh

@@ -17,7 +17,10 @@
 #ifndef SDF_CYLINDER_HH_
 #define SDF_CYLINDER_HH_
 
+#include <optional>
+
 #include <gz/math/Cylinder.hh>
+#include <gz/math/Inertial.hh>
 #include <gz/utils/ImplPtr.hh>
 #include <sdf/Error.hh>
 #include <sdf/Element.hh>
@@ -72,6 +75,14 @@ namespace sdf
     /// \return A reference to a gz::math::Cylinderd object.
     public: gz::math::Cylinderd &Shape();
 
+    /// \brief Calculate and return the Inertial values for the Cylinder. In
+    /// order to calculate the inertial properties, the function mutates the
+    /// object by updating its material properties.
+    /// \param[in] _density Density of the cylinder 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);
+
     /// \brief Create and return an SDF element filled with data from this
     /// cylinder.
     /// Note that parameter passing functionality is not captured with this

include/sdf/Ellipsoid.hh

@@ -17,6 +17,9 @@
 #ifndef SDF_ELLIPSOID_HH_
 #define SDF_ELLIPSOID_HH_
 
+#include <optional>
+
+#include <gz/math/Inertial.hh>
 #include <gz/math/Ellipsoid.hh>
 #include <gz/utils/ImplPtr.hh>
 #include <sdf/Error.hh>
@@ -64,6 +67,14 @@ namespace sdf
     /// \return A reference to a gz::math::Ellipsoidd object.
     public: gz::math::Ellipsoidd &Shape();
 
+    /// \brief Calculate and return the Inertial values for the Ellipsoid. In
+    /// order to calculate the inertial properties, the function mutates the
+    /// object by updating its material properties.
+    /// \param[in] _density Density of the ellipsoid 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);
+
     /// \brief Create and return an SDF element filled with data from this
     /// ellipsoid.
     /// Note that parameter passing functionality is not captured with this

include/sdf/Geometry.hh

@@ -18,11 +18,15 @@
 #define SDF_GEOMETRY_HH_
 
 #include <vector>
+#include <optional>
 
 #include <gz/utils/ImplPtr.hh>
+#include <gz/math/Inertial.hh>
 #include <sdf/Error.hh>
 #include <sdf/Element.hh>
 #include <sdf/sdf_config.h>
+#include <sdf/ParserConfig.hh>
+#include <sdf/Types.hh>
 
 namespace sdf
 {
@@ -209,6 +213,17 @@ namespace sdf
     /// \param[in] _heightmap The heightmap shape.
     public: void SetHeightmapShape(const Heightmap &_heightmap);
 
+    /// \brief Calculate and return the Mass Matrix values for the Geometry
+    /// \param[out] _errors A vector of Errors object. Each object
+    /// would contain an error code and an error message.
+    /// \param[in] _config Parser Config
+    /// \param[in] _density The density of the geometry element.
+    /// \return A std::optional with gz::math::Inertiald object or std::nullopt
+    public: std::optional<gz::math::Inertiald> CalculateInertial(
+      sdf::Errors &_errors,
+      const sdf::ParserConfig &_config,
+      double _density);
+
     /// \brief Get a pointer to the SDF element that was used during
     /// load.
     /// \return SDF element pointer. The value will be nullptr if Load has

include/sdf/Link.hh

@@ -27,6 +27,8 @@
 #include "sdf/Types.hh"
 #include "sdf/sdf_config.h"
 #include "sdf/system_util.hh"
+#include "sdf/ParserConfig.hh"
+#include "sdf/Error.hh"
 
 namespace sdf
 {
@@ -326,6 +328,16 @@ namespace sdf
     public: Errors ResolveInertial(gz::math::Inertiald &_inertial,
                                    const std::string &_resolveTo = "") const;
 
+    /// \brief Calculate & set inertial values(mass, mass matrix
+    /// & inertial pose) for the link. Inertial values can be provided
+    /// by the user through the SDF or can be calculated automatically
+    /// by setting the auto attribute to true.
+    /// \param[out] _errors A vector of Errors object. Each object
+    /// would contain an error code and an error message.
+    /// \param[in] _config Custom parser configuration
+    public: void ResolveAutoInertials(sdf::Errors &_errors,
+                                    const ParserConfig &_config);
+
     /// \brief Get the pose of the link. This is the pose of the link
     /// as specified in SDF (<link> <pose> ... </pose></link>).
     /// \return The pose of the link.
@@ -380,6 +392,32 @@ namespace sdf
     /// \sa Model::SetEnableWind(bool)
     public: void SetEnableWind(bool _enableWind);
 
+    /// \brief Check if the automatic calculation for the link inertial
+    /// is enabled or not.
+    /// \return True if automatic calculation is enabled. This can be done
+    /// setting the auto attribute of the <inertial> element of the link to
+    /// true or by setting the autoInertia member to true
+    /// using SetAutoInertia().
+    public: bool AutoInertia() const;
+
+    /// \brief Enable automatic inertial calculations by setting autoInertia
+    /// to true.
+    /// \param[in] _autoInertia True or False
+    public: void SetAutoInertia(bool _autoInertia);
+
+    /// \brief Check if the inertial values for this link were saved.
+    /// If true, the inertial values for this link wont be calculated
+    /// when CalculateInertial() is called. This value is set to true
+    /// when CalculateInertial() is called with SAVE_CALCULATION
+    /// configuration.
+    /// \return True if CalculateInertial() was called with SAVE_CALCULATION
+    /// configuration, false otherwise.
+    public: bool AutoInertiaSaved() const;
+
+    /// \brief Set the autoInertiaSaved() values
+    /// \param[in] _autoInertiaSaved True or False
+    public: void SetAutoInertiaSaved(bool _autoInertiaSaved);
+
     /// \brief Add a collision to the link.
     /// \param[in] _collision Collision to add.
     /// \return True if successful, false if a collision with the name already

include/sdf/Model.hh

@@ -508,6 +508,14 @@ namespace sdf
     /// \param[in] _plugin Plugin to add.
     public: void AddPlugin(const Plugin &_plugin);
 
+    /// \brief Calculate and set the inertials for all the links belonging
+    /// to the model object
+    /// \param[out] _errrors A vector of Errors objects. Each errors contains an
+    /// Error code and a message. An empty errors vector indicates no errors
+    /// \param[in] _config Custom parser configuration
+    public: void ResolveAutoInertials(sdf::Errors &_errors,
+                                    const ParserConfig &_config);
+
     /// \brief Give the scoped PoseRelativeToGraph to be used for resolving
     /// poses. This is private and is intended to be called by Root::Load or
     /// World::SetPoseRelativeToGraph if this is a standalone model and

include/sdf/ParserConfig.hh

@@ -48,6 +48,20 @@ enum class EnforcementPolicy
   LOG,
 };
 
+/// \enum ConfigureResolveAutoInertials
+/// \brief Configuration options of how CalculateInertial() function
+/// would be used
+enum class ConfigureResolveAutoInertials
+{
+  /// \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
+};
+
 // Forward declare private data class.
 class ParserConfigPrivate;
 
@@ -160,6 +174,17 @@ class SDFORMAT_VISIBLE ParserConfig
   /// \return The deperacted elements policy enum value
   public: EnforcementPolicy DeprecatedElementsPolicy() const;
 
+  /// \brief Get the current configuration for the CalculateInertial()
+  /// function
+  /// \return Current set value of the ConfigureResolveAutoInertials enum
+  public: ConfigureResolveAutoInertials CalculateInertialConfiguration() const;
+
+  /// \brief Set the configuration for the CalculateInertial() function
+  /// \param[in] _configuration The configuration to set for the
+  /// CalculateInertial() function
+  public: void SetCalculateInertialConfiguration(
+    ConfigureResolveAutoInertials _configuration);
+
   /// \brief Registers a custom model parser.
   /// \param[in] _modelParser Callback as described in
   /// sdf/InterfaceElements.hh.

include/sdf/Root.hh

@@ -226,6 +226,17 @@ namespace sdf
     /// an error code and message. An empty vector indicates no error.
     public: Errors UpdateGraphs();
 
+    /// \brief Calculate & set the inertial properties (mass, mass matrix
+    /// and inertial pose) for all the worlds & models that are children
+    /// of this Root object. This function must be called after calling
+    /// Root::Load() or UpdateGraphs() since it uses frame graphs to
+    /// resolve inertial pose for links and they would be automatically built
+    /// \param[out] _errors A vector of Errors objects. Each errors contains an
+    /// Error code and a message. An empty errors vector indicates no errors
+    /// \param[in] _config Custom parser configuration
+    public: void ResolveAutoInertials(sdf::Errors &_errors,
+      const ParserConfig &_config);
+
     /// \brief Create and return an SDF element filled with data from this
     /// root.
     /// Note that parameter passing functionality is not captured with this

include/sdf/Sphere.hh

@@ -17,6 +17,9 @@
 #ifndef SDF_SPHERE_HH_
 #define SDF_SPHERE_HH_
 
+#include <optional>
+
+#include <gz/math/Inertial.hh>
 #include <gz/math/Sphere.hh>
 #include <gz/utils/ImplPtr.hh>
 
@@ -65,6 +68,14 @@ namespace sdf
     /// not been called.
     public: sdf::ElementPtr Element() const;
 
+    /// \brief Calculate and return the Inertial values for the Sphere. In
+    /// order to calculate the inertial properties, the function mutates the
+    /// object by updating its material properties.
+    /// \param[in] _density Density of the sphere 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);
+
     /// \brief Create and return an SDF element filled with data from this
     /// sphere.
     /// Note that parameter passing functionality is not captured with this

include/sdf/World.hh

@@ -516,6 +516,14 @@ namespace sdf
     /// \param[in] _plugin Plugin to add.
     public: void AddPlugin(const Plugin &_plugin);
 
+    /// \brief Calculate and set the inertials for all the models in the world
+    /// object
+    /// \param[out] _errrors A vector of Errors objects. Each errors contains an
+    /// Error code and a message. An empty errors vector indicates no errors
+    /// \param[in] _config Custom parser configuration
+    public: void ResolveAutoInertials(sdf::Errors &_errors,
+                                    const ParserConfig &_config);
+
     /// \brief Give the Scoped PoseRelativeToGraph to be passed on to child
     /// entities for resolving poses. This is private and is intended to be
     /// called by Root::Load.

sdf/1.11/collision.sdf

@@ -14,6 +14,14 @@
     <description>Maximum number of contacts allowed between two entities. This value overrides the max_contacts element defined in physics.</description>
   </element>
 
+  <element name="density" type="double" default="1000.0" required="0">
+    <description>
+      Mass Density of the collision geometry. 
+      This is used to determine mass and inertia values during automatic calculation.
+      Default is the density of water 1000 kg/m^3.
+    </description>
+  </element>
+
   <include filename="pose.sdf" required="0"/>
 
   <include filename="geometry.sdf" required="1"/>

sdf/1.11/inertial.sdf

@@ -5,10 +5,26 @@
     properties, and optionally its fluid added mass.
   </description>
 
+  <attribute name="auto" type="bool" default="false" required="0">
+    <description>
+      Set to true if you want automatic computation for the moments of inertia (ixx, iyy, izz) 
+      and products of inertia(ixy, iyz, ixz). Default value is false.
+    </description>
+  </attribute>
+
   <element name="mass" type="double" default="1.0" required="0">
     <description>The mass of the link.</description>
   </element>
 
+  <element name="density" type="double" default="1000.0" required="0">
+    <description>
+      Mass Density of the collision geometry. 
+      This is used to determine mass and inertia values during automatic calculation.
+      This density value would be overwritten by the density value in collision.
+      Default is the density of water 1000 kg/m^3.
+    </description>
+  </element>
+
   <element name="pose" type="pose" default="0 0 0 0 0 0" required="0">
     <description>
       This pose (translation, rotation) describes the position and orientation