gazebosim / sdformat

Simulation Description Format (SDFormat) parser and description files.
http://sdformat.org
Apache License 2.0
166 stars 92 forks source link

Auto-inertial calculations don't allow zero density #1328

Open scpeters opened 1 year ago

scpeters commented 1 year ago

Environment

Description

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

Steps to reproduce

  1. Build https://github.com/gazebosim/sdformat/commit/747e5553be5f05bc47eb4f27180720dd4481f803 (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 commented 12 months ago

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