Automatic Moment of Inertia Calculations for Basic Shapes

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,12 @@ namespace sdf
     /// \return A reference to a gz::math::Boxd object.
     public: gz::math::Boxd &Shape();
 
+    /// \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);
+
     /// \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,12 @@ namespace sdf
     /// \return A reference to a gz::math::Capsuled object.
     public: gz::math::Capsuled &Shape();
 
+    /// \brief Calculate and return the Mass Matrix values for the Capsule
+    /// \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,16 @@ namespace sdf
     /// \param[in] _name Name of the collision.
     public: void SetName(const std::string &_name);
 
+    /// \brief Get the density of the collision.
+    /// The density of the collision must be unique within the scope of a Link.
+    /// \return Density of the collision.
+    public: double Density() const;
+
+    /// \brief Set the density of the collision.
+    /// The density of the collision must be unique within the scope of a Link.
+    /// \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 +132,15 @@ namespace sdf
     /// \return SemanticPose object for this link.
     public: sdf::SemanticPose SemanticPose() const;
 
+    /// \brief Calculate and return the MassMatrix for the collision
+    /// \param[out] _inertial An inertial object which will be set with the
+    /// calculated inertial values
+    /// \param[in] _config Custom parser configuration
+    /// \return Errors, which is a vector of Error objects. Each Error includes
+    /// an error code and message. An empty vector indicates no error
+    public: Errors CalculateInertial(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,12 @@ namespace sdf
     /// \return A reference to a gz::math::Cylinderd object.
     public: gz::math::Cylinderd &Shape();
 
+    /// \brief Calculate and return the Mass Matrix values for the Cylinder
+    /// \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,12 @@ namespace sdf
     /// \return A reference to a gz::math::Ellipsoidd object.
     public: gz::math::Ellipsoidd &Shape();
 
+    /// \brief Calculate and return the Mass Matrix values for the Ellipsoid
+    /// \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[in] _density The density of the geometry element.
+    /// \param[in] _config Parser Config
+    /// \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::ParserConfig &_config,
+      sdf::Errors &_errors);
+
     /// \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 CalculateInertials(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,19 @@ namespace sdf
     /// \sa Model::SetEnableWind(bool)
     public: void SetEnableWind(bool _enableWind);
 
+    /// \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 _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 CalculateInertials(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 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
+};
+
 // 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 ConfigureCalculateInertial enum
+  public: ConfigureCalculateInertial CalculateInertialConfiguration() const;
+
+  /// \brief Set the configuration for the CalculateInertial() function
+  /// \param[in] _configuration The configuration to set for the
+  /// CalculateInertial() function
+  public: void SetCalculateInertialConfiguration(
+    ConfigureCalculateInertial _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 can be called after calling
+    /// UpdateGraphs() since it uses frame graphs to resolve inertial pose
+    /// for links.
+    /// \param[in] _config Custom parser configuration
+    /// \return Errors, which is a vector of Error objects.
+    /// Each Error includes an error code and message. An empty vector
+    /// indicates no error.
+    public: Errors CalculateInertials(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,12 @@ namespace sdf
     /// not been called.
     public: sdf::ElementPtr Element() const;
 
+    /// \brief Calculate and return the Mass Matrix values for the Sphere
+    /// \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 CalculateInertials(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,25 @@
     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.
+      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