Use //link/inertial/density for auto-inertials (#1335)

The density precedence for Collisions is now:
1. Density explicitly set in //collision/density
   or by Collision::SetDensity.
2. Density explicitly set in //link/inertial/density
   or by Link::SetDensity.
3. Default density in Collision::DensityDefault().

* Fix density handling in Collision::ToElement
* Add Link auto_inertia_params to API

Pass //link/inertial/auto_inertia_params ElementPtr to
Collision::CalculateInertial and use it if the
Collision does not have auto_inertia_params of its own.
Also add a test for precedence of AutoInertiaParams.

* Mesh: don't warn if FilePath is not set

The FilePath refers to the SDFormat file that the
Mesh object was parsed from, not the Mesh file itself,
so remove the unrelated warning.

Signed-off-by: Steve Peters <[email protected]>

Author: scpeters

include/sdf/Collision.hh

@@ -18,6 +18,7 @@
 #define SDF_COLLISION_HH_
 
 #include <memory>
+#include <optional>
 #include <string>
 #include <gz/math/Pose3.hh>
 #include <gz/math/Vector3.hh>
@@ -78,6 +79,11 @@ namespace sdf
     /// \param[in] _name Name of the collision.
     public: void SetName(const std::string &_name);
 
+    /// \brief Get the default density of a collision if its density is not
+    /// specified.
+    /// \return Default density.
+    public: static double DensityDefault();
+
     /// \brief Get the density of the collision.
     /// \return Density of the collision.
     public: double Density() const;
@@ -145,13 +151,32 @@ namespace sdf
     /// \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
+    /// \param[in] _config Custom parser configuration
     public: void CalculateInertial(sdf::Errors &_errors,
                                   gz::math::Inertiald &_inertial,
                                   const ParserConfig &_config);
 
+    /// \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[out] _inertial An inertial object which will be set with the
+    /// calculated inertial values
+    /// \param[in] _config Custom parser configuration
+    /// \param[in] _density An optional density value to override the default
+    /// collision density. This value is used instead of DefaultDensity()
+    // if this collision's density has not been explicitly set.
+    /// \param[in] _autoInertiaParams An ElementPtr to the auto_inertia_params
+    /// element to be used if the auto_inertia_params have not been set in this
+    /// collision.
+    public: void CalculateInertial(
+                    sdf::Errors &_errors,
+                    gz::math::Inertiald &_inertial,
+                    const ParserConfig &_config,
+                    const std::optional<double> &_density,
+                    sdf::ElementPtr _autoInertiaParams);
+
     /// \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

@@ -18,6 +18,7 @@
 #define SDF_LINK_HH_
 
 #include <memory>
+#include <optional>
 #include <string>
 #include <gz/math/Inertial.hh>
 #include <gz/math/Pose3.hh>
@@ -79,6 +80,27 @@ namespace sdf
     /// \param[in] _name Name of the link.
     public: void SetName(const std::string &_name);
 
+    /// \brief Get the density of the inertial if it has been set.
+    /// \return Density of the inertial if it has been set,
+    /// otherwise std::nullopt.
+    public: std::optional<double> Density() const;
+
+    /// \brief Set the density of the inertial.
+    /// \param[in] _density Density of the inertial.
+    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 the number of visuals.
     /// \return Number of visuals contained in this Link object.
     public: uint64_t VisualCount() const;

src/Collision.cc

@@ -49,13 +49,10 @@ class sdf::Collision::Implementation
   /// \brief The collision's surface parameters.
   public: sdf::Surface surface;
 
-  /// \brief Density of the collision. Default is 1000.0
-  public: double density{1000.0};
+  /// \brief Density of the collision if it has been set.
+  public: std::optional<double> density;
 
-  /// \brief True if density was set during load from sdf.
-  public: bool densitySetAtLoad = false;
-
-  /// \brief SDF element pointer to <moi_calculator_params> tag
+  /// \brief SDF element pointer to <auto_inertia_params> tag
   public: sdf::ElementPtr autoInertiaParams{nullptr};
 
   /// \brief The SDF element pointer used during load.
@@ -130,7 +127,6 @@ Errors Collision::Load(ElementPtr _sdf, const ParserConfig &_config)
   if (_sdf->HasElement("density"))
   {
     this->dataPtr->density = _sdf->Get<double>("density");
-    this->dataPtr->densitySetAtLoad = true;
   }
 
   // Load the auto_inertia_params element
@@ -155,10 +151,20 @@ void Collision::SetName(const std::string &_name)
   this->dataPtr->name = _name;
 }
 
+/////////////////////////////////////////////////
+double Collision::DensityDefault()
+{
+  return 1000.0;
+}
+
 /////////////////////////////////////////////////
 double Collision::Density() const
 {
-  return this->dataPtr->density;
+  if (this->dataPtr->density)
+  {
+    return *this->dataPtr->density;
+  }
+  return DensityDefault();
 }
 
 /////////////////////////////////////////////////
@@ -256,23 +262,59 @@ void Collision::CalculateInertial(
   gz::math::Inertiald &_inertial,
   const ParserConfig &_config)
 {
-  // Check if density was not set during load & send a warning
-  // about the default value being used
-  if (!this->dataPtr->densitySetAtLoad)
+  this->CalculateInertial(
+    _errors, _inertial, _config, std::nullopt, ElementPtr());
+}
+
+/////////////////////////////////////////////////
+void Collision::CalculateInertial(
+  sdf::Errors &_errors,
+  gz::math::Inertiald &_inertial,
+  const ParserConfig &_config,
+  const std::optional<double> &_density,
+  sdf::ElementPtr _autoInertiaParams)
+{
+  // Order of precedence for density:
+  double density;
+  // 1. Density explicitly set in this collision, either from the
+  // `//collision/density` element or from Collision::SetDensity.
+  if (this->dataPtr->density)
+  {
+    density = *this->dataPtr->density;
+  }
+  // 2. Density passed into this function, which likely comes from the
+  // `//link/inertial/density` element or from Link::SetDensity.
+  else if (_density)
+  {
+    density = *_density;
+  }
+  // 3. DensityDefault value.
+  else
   {
+    // If density was not explicitly set, send a warning
+    // about the default value being used
+    density = DensityDefault();
     Error densityMissingErr(
       ErrorCode::ELEMENT_MISSING,
       "Collision is missing a <density> child element. "
-      "Using a default density value of 1000.0 kg/m^3. "
+      "Using a default density value of " +
+      std::to_string(DensityDefault()) + " kg/m^3. "
     );
     enforceConfigurablePolicyCondition(
       _config.WarningsPolicy(), densityMissingErr, _errors
     );
   }
 
+  // If this Collision's auto inertia params have not been set, then use the
+  // params passed into this function.
+  sdf::ElementPtr autoInertiaParams = this->dataPtr->autoInertiaParams;
+  if (!autoInertiaParams)
+  {
+    autoInertiaParams = _autoInertiaParams;
+  }
   auto geomInertial =
     this->dataPtr->geom.CalculateInertial(_errors, _config,
-      this->dataPtr->density, this->dataPtr->autoInertiaParams);
+      density, autoInertiaParams);
 
   if (!geomInertial)
   {
@@ -309,6 +351,7 @@ sdf::ElementPtr Collision::Element() const
   return this->dataPtr->sdf;
 }
 
+/////////////////////////////////////////////////
 sdf::ElementPtr Collision::ToElement() const
 {
   sdf::Errors errors;
@@ -335,8 +378,11 @@ sdf::ElementPtr Collision::ToElement(sdf::Errors &_errors) const
   poseElem->Set<gz::math::Pose3d>(_errors, this->RawPose());
 
   // Set the density
-  sdf::ElementPtr densityElem = elem->GetElement("density", _errors);
-  densityElem->Set<double>(this->Density());
+  if (this->dataPtr->density.has_value())
+  {
+    sdf::ElementPtr densityElem = elem->GetElement("density", _errors);
+    densityElem->Set<double>(this->Density());
+  }
 
   // Set the geometry
   elem->InsertElement(this->dataPtr->geom.ToElement(_errors), true);

src/Collision_TEST.cc

@@ -38,6 +38,7 @@ TEST(DOMcollision, Construction)
   EXPECT_EQ(nullptr, collision.Element());
   EXPECT_TRUE(collision.Name().empty());
   EXPECT_EQ(collision.Density(), 1000.0);
+  EXPECT_EQ(collision.DensityDefault(), 1000.0);
 
   collision.SetName("test_collison");
   EXPECT_EQ(collision.Name(), "test_collison");
@@ -58,12 +59,12 @@ TEST(DOMcollision, Construction)
   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);
+  sdf::ElementPtr autoInertiaParamsElem(new sdf::Element());
+  autoInertiaParamsElem->SetName("auto_inertia_params");
+  collision.SetAutoInertiaParams(autoInertiaParamsElem);
+  EXPECT_EQ(collision.AutoInertiaParams(), autoInertiaParamsElem);
   EXPECT_EQ(collision.AutoInertiaParams()->GetName(),
-    autoInertialParamsElem->GetName());
+    autoInertiaParamsElem->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),
@@ -420,6 +421,8 @@ TEST(DOMCollision, ToElement)
 
   sdf::ElementPtr elem = collision.ToElement();
   ASSERT_NE(nullptr, elem);
+  // Expect no density element
+  EXPECT_FALSE(elem->HasElement("density"));
 
   sdf::Collision collision2;
   collision2.Load(elem);
@@ -434,6 +437,19 @@ TEST(DOMCollision, ToElement)
   ASSERT_NE(nullptr, surface2->Friction());
   ASSERT_NE(nullptr, surface2->Friction()->ODE());
   EXPECT_DOUBLE_EQ(1.23, surface2->Friction()->ODE()->Mu());
+
+  // Now set density in collision
+  const double kDensity = 1234.5;
+  collision.SetDensity(kDensity);
+  sdf::ElementPtr elemWithDensity = collision.ToElement();
+  ASSERT_NE(nullptr, elemWithDensity);
+  // Expect density element
+  ASSERT_TRUE(elemWithDensity->HasElement("density"));
+  EXPECT_DOUBLE_EQ(kDensity, elemWithDensity->Get<double>("density"));
+
+  sdf::Collision collision3;
+  collision3.Load(elem);
+  EXPECT_DOUBLE_EQ(kDensity, collision.Density());
 }
 
 /////////////////////////////////////////////////

src/Link.cc

@@ -69,6 +69,13 @@ class sdf::Link::Implementation
   /// \brief The projectors specified in this link.
   public: std::vector<Projector> projectors;
 
+  /// \brief Density of the inertial which will be used for auto-inertial
+  /// calculations if the collision density has not been set.
+  public: std::optional<double> density;
+
+  /// \brief SDF element pointer to <auto_inertia_params> tag
+  public: sdf::ElementPtr autoInertiaParams{nullptr};
+
   /// \brief The inertial information for this link.
   public: gz::math::Inertiald inertial {{1.0,
             gz::math::Vector3d::One, gz::math::Vector3d::Zero},
@@ -180,6 +187,18 @@ Errors Link::Load(ElementPtr _sdf, const ParserConfig &_config)
   {
     sdf::ElementPtr inertialElem = _sdf->GetElement("inertial");
 
+    if (inertialElem->HasElement("density"))
+    {
+      this->dataPtr->density = inertialElem->Get<double>("density");
+    }
+
+    // Load the auto_inertia_params element
+    if (inertialElem->HasElement("auto_inertia_params"))
+    {
+      this->dataPtr->autoInertiaParams =
+        inertialElem->GetElement("auto_inertia_params");
+    }
+
     if (inertialElem->Get<bool>("auto"))
     {
       this->dataPtr->autoInertia = true;
@@ -309,6 +328,30 @@ void Link::SetName(const std::string &_name)
   this->dataPtr->name = _name;
 }
 
+/////////////////////////////////////////////////
+std::optional<double> Link::Density() const
+{
+  return this->dataPtr->density;
+}
+
+/////////////////////////////////////////////////
+void Link::SetDensity(double _density)
+{
+  this->dataPtr->density = _density;
+}
+
+/////////////////////////////////////////////////
+sdf::ElementPtr Link::AutoInertiaParams() const
+{
+  return this->dataPtr->autoInertiaParams;
+}
+
+/////////////////////////////////////////////////
+void Link::SetAutoInertiaParams(const sdf::ElementPtr _autoInertiaParams)
+{
+  this->dataPtr->autoInertiaParams = _autoInertiaParams;
+}
+
 /////////////////////////////////////////////////
 uint64_t Link::VisualCount() const
 {
@@ -620,7 +663,9 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors,
     for (sdf::Collision &collision : this->dataPtr->collisions)
     {
       gz::math::Inertiald collisionInertia;
-      collision.CalculateInertial(_errors, collisionInertia, _config);
+      collision.CalculateInertial(_errors, collisionInertia, _config,
+                                  this->dataPtr->density,
+                                  this->dataPtr->autoInertiaParams);
       totalInertia = totalInertia + collisionInertia;
     }
 
@@ -942,6 +987,11 @@ sdf::ElementPtr Link::ToElement() const
   inertiaElem->GetElement("iyz")->Set(massMatrix.Iyz());
   inertiaElem->GetElement("izz")->Set(massMatrix.Izz());
 
+  if (this->dataPtr->density.has_value())
+  {
+    inertialElem->GetElement("density")->Set(*this->dataPtr->density);
+  }
+
   if (this->dataPtr->inertial.FluidAddedMass().has_value())
   {
     auto addedMass = this->dataPtr->inertial.FluidAddedMass().value();