Use //link/inertial/density for auto-inertials

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();