diff --git a/include/sdf/ParserConfig.hh b/include/sdf/ParserConfig.hh index a3741ef6f..782085432 100644 --- a/include/sdf/ParserConfig.hh +++ b/include/sdf/ParserConfig.hh @@ -69,6 +69,21 @@ enum class ConfigureResolveAutoInertials SAVE_CALCULATION_IN_ELEMENT, }; +/// \enum CalculateInertialFailurePolicyType +/// \brief Configuration options of how CalculateInertial() failures should +/// be handled. +enum class CalculateInertialFailurePolicyType +{ + /// \brief If this value is used, failures of Geometry::CalculateInertial() + /// will result in a LINK_INERTIA_INVALID error with no inertial values + /// written. + ERR, + + /// \brief If this value is used, failures of Geometry::CalculateInertial() + /// will result in default inertial values used and a WARNING. + WARN_AND_USE_DEFAULT_INERTIAL, +}; + // Forward declare private data class. class ParserConfigPrivate; @@ -192,6 +207,19 @@ class SDFORMAT_VISIBLE ParserConfig public: void SetCalculateInertialConfiguration( ConfigureResolveAutoInertials _configuration); + /// \brief Get the current policy for handling failures of the + /// CalculateInertial() function. By default an error is reported. + /// \return Current set value of the CalculateInertialFailurePolicyType enum + public: CalculateInertialFailurePolicyType + CalculateInertialFailurePolicy() const; + + /// \brief Set the policy for handling failures of the CalculateInertial() + /// function + /// \param[in] _policy The policy to set for handling failures of the + /// CalculateInertial() function + public: void SetCalculateInertialFailurePolicy( + CalculateInertialFailurePolicyType _policy); + /// \brief Registers a custom model parser. /// \param[in] _modelParser Callback as described in /// sdf/InterfaceElements.hh. diff --git a/src/Collision.cc b/src/Collision.cc index 07ad12805..088beb7e7 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -322,9 +322,27 @@ void Collision::CalculateInertial( if (!geomInertial) { - _errors.push_back({ErrorCode::LINK_INERTIA_INVALID, - "Inertia Calculated for collision: " + - this->dataPtr->name + " is invalid."}); + if (_config.CalculateInertialFailurePolicy() == + CalculateInertialFailurePolicyType::WARN_AND_USE_DEFAULT_INERTIAL) + { + Error err( + sdf::ErrorCode::WARNING, + "Inertia Calculated for collision: " + this->dataPtr->name + + " is invalid, using default inertial values."); + enforceConfigurablePolicyCondition(_config.WarningsPolicy(), err, + _errors); + + using namespace gz::math; + _inertial = Inertiald(MassMatrix3d(1, Vector3d::One, Vector3d::Zero), + Pose3d::Zero); + } + else if (_config.CalculateInertialFailurePolicy() == + CalculateInertialFailurePolicyType::ERR) + { + _errors.push_back({ErrorCode::LINK_INERTIA_INVALID, + "Inertia Calculated for collision: " + + this->dataPtr->name + " is invalid."}); + } } else { diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index aaa0cd2dc..ab73bb22c 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -398,6 +398,61 @@ TEST(DOMCollision, CalculateInertialPoseNotRelativeToLink) EXPECT_EQ(expectedInertial.Pose(), link->Inertial().Pose()); } +///////////////////////////////////////////////// +TEST(DOMCollision, CollisionCalculateInertialFailurePolicy) +{ + sdf::Collision collision; + + sdf::ElementPtr sdf(new sdf::Element()); + collision.Load(sdf); + + const sdf::ParserConfig sdfParserConfig; + sdf::Geometry geom; + sdf::Mesh mesh; + geom.SetType(sdf::GeometryType::MESH); + geom.SetMeshShape(mesh); + collision.SetGeom(geom); + + sdf::ParserConfig config; + sdf::Errors errors; + sdf::CustomInertiaCalcProperties inertiaCalcProps; + + // Custom inertia calculator that returns null inertial + auto customMeshInertiaCalculator = []( + sdf::Errors &, + const sdf::CustomInertiaCalcProperties &) + -> std::optional + { + return std::nullopt; + }; + config.RegisterCustomInertiaCalc(customMeshInertiaCalculator); + + // With default inertial failure policy, there should be an error when the + // mesh inertial calculator returns null inertial values. + gz::math::Inertiald collisionInertial; + collision.CalculateInertial(errors, collisionInertial, config); + ASSERT_EQ(1u, errors.size()) << errors; + EXPECT_EQ(errors[0].Code(), sdf::ErrorCode::LINK_INERTIA_INVALID); + const gz::math::Inertiald empty; + EXPECT_EQ(empty, collisionInertial); + + // Set inertial failure policy to use default inertial values on failure and + // verify that there are no more errors. + errors.clear(); + config.SetCalculateInertialFailurePolicy( + sdf::CalculateInertialFailurePolicyType::WARN_AND_USE_DEFAULT_INERTIAL); + collision.CalculateInertial(errors, collisionInertial, config); + EXPECT_TRUE(errors.empty()) << errors; + + // Verify default inertial values are returned. + gz::math::Inertiald defaultInertial; + defaultInertial.SetMassMatrix( + gz::math::MassMatrix3d(1.0, + gz::math::Vector3d::One, + gz::math::Vector3d::Zero)); + EXPECT_EQ(collisionInertial, defaultInertial); +} + ///////////////////////////////////////////////// TEST(DOMCollision, ToElement) { diff --git a/src/ParserConfig.cc b/src/ParserConfig.cc index d3c60e7af..658e8ef37 100644 --- a/src/ParserConfig.cc +++ b/src/ParserConfig.cc @@ -44,6 +44,11 @@ class sdf::ParserConfig::Implementation /// to behave behave differently than the `warningsPolicy`. public: std::optional deprecatedElementsPolicy; + /// \brief Policy that is set for handling failures of the + /// CalculateInertial() function. By default errors are reported. + public: CalculateInertialFailurePolicyType calculateInertialFailurePolicy = + CalculateInertialFailurePolicyType::ERR; + /// \brief Configuration that is set for the CalculateInertial() function /// By default it is set to SAVE_CALCULATION to preserve the behavior of /// Root::Load() generating complete inertial information. @@ -167,6 +172,20 @@ EnforcementPolicy ParserConfig::DeprecatedElementsPolicy() const this->dataPtr->warningsPolicy); } +///////////////////////////////////////////////// +CalculateInertialFailurePolicyType + ParserConfig::CalculateInertialFailurePolicy() const +{ + return this->dataPtr->calculateInertialFailurePolicy; +} + +///////////////////////////////////////////////// +void ParserConfig::SetCalculateInertialFailurePolicy( + CalculateInertialFailurePolicyType _policy) +{ + this->dataPtr->calculateInertialFailurePolicy = _policy; +} + ///////////////////////////////////////////////// ConfigureResolveAutoInertials ParserConfig::CalculateInertialConfiguration() const diff --git a/src/ParserConfig_TEST.cc b/src/ParserConfig_TEST.cc index 314a05fae..35bdb1cbe 100644 --- a/src/ParserConfig_TEST.cc +++ b/src/ParserConfig_TEST.cc @@ -68,6 +68,14 @@ TEST(ParserConfig, Construction) EXPECT_EQ(sdf::ConfigureResolveAutoInertials::SAVE_CALCULATION_IN_ELEMENT, config.CalculateInertialConfiguration()); + EXPECT_EQ(sdf::CalculateInertialFailurePolicyType::ERR, + config.CalculateInertialFailurePolicy()); + config.SetCalculateInertialFailurePolicy( + sdf::CalculateInertialFailurePolicyType::WARN_AND_USE_DEFAULT_INERTIAL); + EXPECT_EQ( + sdf::CalculateInertialFailurePolicyType::WARN_AND_USE_DEFAULT_INERTIAL, + config.CalculateInertialFailurePolicy()); + EXPECT_FALSE(config.URDFPreserveFixedJoint()); EXPECT_FALSE(config.StoreResolvedURIs()); }