Callback based API for Implementing Custom Moment of Inertia Calculat…

…ors for Meshes (#1304)

Added functions to register custom calculator and get custom calculator in ParserConfig.
Added an InterfaceMoiCalculator class that can be used to send appropriate data to the calculator from sdformat.
Added a <moi_calculator_params> tag that can be used to access custom calculator params from the SDF by using tags with namespaces. (See this for reference)
Added MassMatrix() function to sdf::Mesh class and updated the Collision::MassMatrix() function to pass the parser config as well as sdf::ElementPtr to <moi_calculator_params> tag.

---------

Signed-off-by: Jasmeet Singh <[email protected]>

include/sdf/Collision.hh

@@ -86,6 +86,18 @@ namespace sdf
     /// \param[in] _density Density of the collision.
     public: void SetDensity(double _density);
 
+    /// \brief Get the ElementPtr to the <auto_inertia_params> element
+    /// This element can be used as a parent element to hold user-defined
+    /// params for the custom moment of inertia calculator.
+    /// \return ElementPtr object for the <auto_inertia_params> element.
+    public: sdf::ElementPtr AutoInertiaParams() const;
+
+    /// \brief Function to set the auto inertia params using a
+    /// sdf ElementPtr object
+    /// \param[in] _autoInertiaParams ElementPtr to <auto_inertia_params>
+    /// element
+    public: void SetAutoInertiaParams(const sdf::ElementPtr _autoInertiaParams);
+
     /// \brief Get a pointer to the collisions's geometry.
     /// \return The collision's geometry.
     public: const Geometry *Geom() const;

include/sdf/CustomInertiaCalcProperties.hh

@@ -0,0 +1,88 @@
+/*
+ * Copyright 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 SDF_CUSTOM_INERTIA_CALC_PROPERTIES_HH_
+#define SDF_CUSTOM_INERTIA_CALC_PROPERTIES_HH_
+
+#include <optional>
+
+#include <gz/utils/ImplPtr.hh>
+#include <gz/math/Inertial.hh>
+
+#include "sdf/Element.hh"
+#include "sdf/Mesh.hh"
+#include "sdf/config.hh"
+#include "sdf/Types.hh"
+#include "sdf/Error.hh"
+
+namespace sdf
+{
+inline namespace SDF_VERSION_NAMESPACE
+{
+
+// Forward Declarations
+class Mesh;
+
+class SDFORMAT_VISIBLE CustomInertiaCalcProperties
+{
+  /// \brief Default Constructor
+  public: CustomInertiaCalcProperties();
+
+  /// \brief Constructor with mesh properties
+  /// \param[in] _density Double density value
+  /// \param[in] _mesh sdf::Mesh object
+  /// \param[in] _calculatorParams sdf::ElementPtr for calculator params element
+  public: CustomInertiaCalcProperties(const double _density,
+              const sdf::Mesh _mesh,
+              const sdf::ElementPtr _calculatorParams);
+
+  /// \brief Get the density of the mesh.
+  /// \return Double density of the mesh.
+  public: double Density() const;
+
+  /// \brief Function to set the density of the interface object
+  /// \param[in] _density Double density value
+  public: void SetDensity(double _density);
+
+  /// \brief Get the reference to the mesh oject being used.
+  /// \return Reference to the sdf::Mesh object.
+  public: const std::optional<sdf::Mesh> &Mesh() const;
+
+  /// \brief Function to set the mesh object
+  /// \param[in] _mesh sdf::Mesh object
+  public: void SetMesh(sdf::Mesh &_mesh);
+
+  /// \brief Get the reference to the <auto_inertia_params> sdf element.
+  /// User defined calculator params can be accessed through this element
+  /// \return sdf::ElementPtr for the tag
+  public: const sdf::ElementPtr AutoInertiaParams() const;
+
+  /// \brief Function to set the calculator params sdf element object
+  /// \param[in] _autoInertiaParamsElem sdf::ElementPtr for calculator params
+  public: void SetAutoInertiaParams(sdf::ElementPtr _autoInertiaParamsElem);
+
+  /// \brief Private data pointer.
+  GZ_UTILS_IMPL_PTR(dataPtr)
+};
+
+using CustomInertiaCalculator =
+    std::function<std::optional<gz::math::Inertiald>(sdf::Errors &,
+        const sdf::CustomInertiaCalcProperties &)>;
+}
+}
+
+#endif

include/sdf/Geometry.hh

@@ -213,16 +213,16 @@ 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
+    /// \brief Calculate and return the Inertial 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.
+    /// \param[in] _autoInertiaParams ElementPtr to <auto_inertia_params>
     /// \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);
+      sdf::Errors &_errors, const ParserConfig &_config,
+      double _density, sdf::ElementPtr _autoInertiaParams);
 
     /// \brief Get a pointer to the SDF element that was used during
     /// load.

include/sdf/Mesh.hh

@@ -18,8 +18,12 @@
 #define SDF_MESH_HH_
 
 #include <string>
+#include <optional>
+
 #include <gz/math/Vector3.hh>
+#include <gz/math/Inertial.hh>
 #include <gz/utils/ImplPtr.hh>
+#include <sdf/CustomInertiaCalcProperties.hh>
 #include <sdf/Element.hh>
 #include <sdf/Error.hh>
 #include <sdf/sdf_config.h>
@@ -104,6 +108,20 @@ namespace sdf
     /// \param[in] _center True to center the submesh.
     public: void SetCenterSubmesh(const bool _center);
 
+    /// \brief Calculate and return the Inertial values for the Mesh
+    /// \param[out] _errors A vector of Errors object. Each object
+    /// would contain an error code and an error message.
+    /// \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
+    /// \return A std::optional with gz::math::Inertiald object or std::nullopt
+    public: std::optional<gz::math::Inertiald>
+            CalculateInertial(sdf::Errors &_errors,
+                              double _density,
+                              const sdf::ElementPtr _autoInertiaParams,
+                              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
     /// not been called.

include/sdf/ParserConfig.hh

@@ -27,6 +27,7 @@
 
 #include "sdf/Error.hh"
 #include "sdf/InterfaceElements.hh"
+#include "sdf/CustomInertiaCalcProperties.hh"
 #include "sdf/sdf_config.h"
 #include "sdf/system_util.hh"
 
@@ -194,6 +195,16 @@ class SDFORMAT_VISIBLE ParserConfig
   /// \return Vector of registered model parser callbacks.
   public: const std::vector<CustomModelParser> &CustomModelParsers() const;
 
+  /// \brief Registers a custom Moment of Inertia Calculator for Meshes
+  /// \param[in] _inertiaCalculator Callback with signature as described in
+  /// sdf/CustomInertiaCalcProperties.hh.
+  public: void RegisterCustomInertiaCalc(
+      CustomInertiaCalculator _inertiaCalculator);
+
+  /// \brief Get the registered custom mesh MOI Calculator
+  /// \return registered mesh MOI Calculator.
+  public: const CustomInertiaCalculator &CustomInertiaCalc() const;
+
   /// \brief Set the preserveFixedJoint flag.
   /// \param[in] _preserveFixedJoint True to preserve fixed joints, false to
   /// reduce the fixed joints and merge the child link into the parent.

sdf/1.11/collision.sdf

@@ -22,6 +22,10 @@
     </description>
   </element>
 
+  <element name="auto_inertia_params" required="0">
+    <description>Parent tag to hold user-defined custom params for mesh inertia calculator</description>
+  </element>
+
   <include filename="pose.sdf" required="0"/>
 
   <include filename="geometry.sdf" required="1"/>

sdf/1.11/inertial.sdf

@@ -25,6 +25,14 @@
     </description>
   </element>
 
+  <element name="auto_inertia_params" required="0">
+    <description>
+      Parent tag to hold user-defined custom params for mesh inertia calculator
+      The elements used under this would be overwritten by the elements in auto_inertia_params
+      in collision.
+    </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

src/Collision.cc

@@ -55,6 +55,9 @@ class sdf::Collision::Implementation
   /// \brief True if density was set during load from sdf.
   public: bool densitySetAtLoad = false;
 
+  /// \brief SDF element pointer to <moi_calculator_params> tag
+  public: sdf::ElementPtr autoInertiaParams{nullptr};
+
   /// \brief The SDF element pointer used during load.
   public: sdf::ElementPtr sdf;
 
@@ -157,6 +160,18 @@ void Collision::SetDensity(double _density)
   this->dataPtr->density = _density;
 }
 
+/////////////////////////////////////////////////
+sdf::ElementPtr Collision::AutoInertiaParams() const
+{
+  return this->dataPtr->autoInertiaParams;
+}
+
+/////////////////////////////////////////////////
+void Collision::SetAutoInertiaParams(const sdf::ElementPtr _autoInertiaParams)
+{
+  this->dataPtr->autoInertiaParams = _autoInertiaParams;
+}
+
 /////////////////////////////////////////////////
 const Geometry *Collision::Geom() const
 {
@@ -248,9 +263,15 @@ void Collision::CalculateInertial(
     );
   }
 
+  if (this->dataPtr->sdf->HasElement("auto_inertia_params"))
+  {
+    this->dataPtr->autoInertiaParams =
+      this->dataPtr->sdf->GetElement("auto_inertia_params");
+  }
+
   auto geomInertial =
     this->dataPtr->geom.CalculateInertial(_errors, _config,
-      this->dataPtr->density);
+      this->dataPtr->density, this->dataPtr->autoInertiaParams);
 
   if (!geomInertial)
   {

src/Collision_TEST.cc

@@ -53,9 +53,18 @@ TEST(DOMcollision, Construction)
     EXPECT_FALSE(semanticPose.Resolve(pose).empty());
   }
 
+  EXPECT_EQ(collision.Density(), 1000.0);
   collision.SetDensity(1240.0);
   EXPECT_DOUBLE_EQ(collision.Density(), 1240.0);
 
+  EXPECT_EQ(collision.AutoInertiaParams(), nullptr);
+  sdf::ElementPtr autoInertialParamsElem(new sdf::Element());
+  autoInertialParamsElem->SetName("auto_inertial_params");
+  collision.SetAutoInertiaParams(autoInertialParamsElem);
+  EXPECT_EQ(collision.AutoInertiaParams(), autoInertialParamsElem);
+  EXPECT_EQ(collision.AutoInertiaParams()->GetName(),
+    autoInertialParamsElem->GetName());
+
   collision.SetRawPose({-10, -20, -30, GZ_PI, GZ_PI, GZ_PI});
   EXPECT_EQ(gz::math::Pose3d(-10, -20, -30, GZ_PI, GZ_PI, GZ_PI),
             collision.RawPose());
@@ -270,6 +279,60 @@ TEST(DOMCollision, CorrectBoxCollisionCalculateInertial)
   EXPECT_EQ(expectedInertial.Pose(), link->Inertial().Pose());
 }
 
+/////////////////////////////////////////////////
+TEST(DOMCollision, CalculateInertialWithAutoInertiaParamsElement)
+{
+  // This example considers a custom voxel-based inertia calculator
+  // <auto_inertial_params> element is used to provide properties
+  // for the custom calculator like voxel size, grid type, etc.
+  std::string sdf = "<?xml version=\"1.0\"?>"
+  " <sdf version=\"1.11\">"
+  "   <model name='shapes'>"
+  "     <link name='link'>"
+  "       <inertial auto='true' />"
+  "       <collision name='box_col'>"
+  "         <auto_inertia_params>"
+  "           <gz:voxel_size>0.01</gz:voxel_size>"
+  "           <gz:voxel_grid_type>float</gz:voxel_grid_type>"
+  "         </auto_inertia_params>"
+  "         <density>1240.0</density>"
+  "         <geometry>"
+  "           <box>"
+  "             <size>2 2 2</size>"
+  "           </box>"
+  "         </geometry>"
+  "       </collision>"
+  "     </link>"
+  "   </model>"
+  " </sdf>";
+
+  sdf::Root root;
+  const sdf::ParserConfig sdfParserConfig;
+  sdf::Errors errors = root.LoadSdfString(sdf, sdfParserConfig);
+  EXPECT_TRUE(errors.empty());
+  EXPECT_NE(nullptr, root.Element());
+
+  const sdf::Model *model = root.Model();
+  const sdf::Link *link = model->LinkByIndex(0);
+  const sdf::Collision *collision = link->CollisionByIndex(0);
+
+  root.ResolveAutoInertials(errors, sdfParserConfig);
+  EXPECT_TRUE(errors.empty());
+
+  sdf::ElementPtr autoInertiaParamsElem = collision->AutoInertiaParams();
+
+  // <auto_inertial_params> element is used as parent element for custom
+  // intertia calculator params. Custom elements have to be defined with a
+  // namespace prefix(gz in this case). More about this can be found in the
+  // following proposal:
+  // http://sdformat.org/tutorials?tut=custom_elements_attributes_proposal&cat=pose_semantics_docs&
+  double voxelSize = autoInertiaParamsElem->Get<double>("gz:voxel_size");
+  std::string voxelGridType =
+    autoInertiaParamsElem->Get<std::string>("gz:voxel_grid_type");
+  EXPECT_EQ("float", voxelGridType);
+  EXPECT_DOUBLE_EQ(0.01, voxelSize);
+}
+
 /////////////////////////////////////////////////
 TEST(DOMCollision, CalculateInertialPoseNotRelativeToLink)
 {