Auto-inertial calculations don't allow zero density

[scpeters]

Environment

• OS Version: macOS 13.5.2-x86_64
• Source or binary build? source build of sdf14 as of 2ba6470

Description

• Expected behavior: auto-inertial calculation should allow collisions with zero density if there is at least one collision with non-zero density
• Actual behavior: an error is thrown

Error Code 19: Msg: Inertia Calculated for collision: box_col_zero_density is invalid.

Steps to reproduce

1. Build 747e555 (branch scpeters/auto_inertial_zero_density) from source, which has modified a unit test to show this behavior
2. Run UNIT_World_TEST

Output

[ RUN      ] DOMWorld.ResolveAutoInertials
/Users/scpeters/ws/sdformat/src/sdformat/src/World_TEST.cc:552: Failure
Value of: errors.empty()
  Actual: false
Expected: true
Error Code 19: Msg: Inertia Calculated for collision: box_col_zero_density is invalid.

/Users/scpeters/ws/sdformat/src/sdformat/src/World_TEST.cc:580: Failure
Value of: errors.empty()
  Actual: false
Expected: true
Error Code 19: Msg: Inertia Calculated for collision: box_col_zero_density is invalid.

[  FAILED  ] DOMWorld.ResolveAutoInertials (63 ms)

[scpeters]

the following might work:

diff --git a/src/Collision.cc b/src/Collision.cc
index cf03c565..46337d51 100644
--- a/src/Collision.cc
+++ b/src/Collision.cc
@@ -303,6 +303,13 @@ void Collision::CalculateInertial(
     );
   }
 
+  if (density == 0)
+  {
+    // Set to default Inertial, which has zero mass and moments of inertia.
+    _inertial = gz::math::Inertiald();
+    return;
+  }
+
   auto geomInertial =
     this->dataPtr->geom.CalculateInertial(_errors, _config,
       density, this->dataPtr->autoInertiaParams);
diff --git a/src/Link.cc b/src/Link.cc
index c3d772dd..57e68055 100644
--- a/src/Link.cc
+++ b/src/Link.cc
@@ -646,6 +646,13 @@ void Link::ResolveAutoInertials(sdf::Errors &_errors,
       totalInertia = totalInertia + collisionInertia;
     }
 
+    if (!totalInertia.MassMatrix().IsValid())
+    {
+      _errors.push_back({ErrorCode::LINK_INERTIA_INVALID,
+          "Inertia Calculated for link: " +
+          this->dataPtr->name + " is invalid."});
+    }
+
     this->dataPtr->inertial = totalInertia;
 
     // If CalculateInertial() was called with SAVE_CALCULATION