Open scpeters opened 1 year 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
Environment
sdf14
as of 2ba6470e8f9cc3e1ee3aec19df574f18bbb10185Description
Steps to reproduce
UNIT_World_TEST
Output