From 3705b1f8aada880950a2159e0fb6df0c4052f929 Mon Sep 17 00:00:00 2001 From: Ian Chen Date: Tue, 11 Feb 2025 22:16:35 +0000 Subject: [PATCH] revert Signed-off-by: Ian Chen --- src/Collision.cc | 5 +--- src/Collision_TEST.cc | 43 -------------------------------- src/Mesh.cc | 24 +++++++++++++----- src/Mesh_TEST.cc | 57 +++++++++++++++++++++++++++++++++++++++++-- 4 files changed, 74 insertions(+), 55 deletions(-) diff --git a/src/Collision.cc b/src/Collision.cc index ba2bab00c..07ad12805 100644 --- a/src/Collision.cc +++ b/src/Collision.cc @@ -324,10 +324,7 @@ void Collision::CalculateInertial( { _errors.push_back({ErrorCode::LINK_INERTIA_INVALID, "Inertia Calculated for collision: " + - this->dataPtr->name + " is invalid, using default inertial values."}); - _inertial = gz::math::Inertiald( - gz::math::MassMatrix3d(1, gz::math::Vector3d::One, - gz::math::Vector3d::Zero), gz::math::Pose3d::Zero); + this->dataPtr->name + " is invalid."}); } else { diff --git a/src/Collision_TEST.cc b/src/Collision_TEST.cc index 38d56b271..aaa0cd2dc 100644 --- a/src/Collision_TEST.cc +++ b/src/Collision_TEST.cc @@ -195,49 +195,6 @@ TEST(DOMcollision, SetSurface) EXPECT_EQ(collision.Surface()->Contact()->CollideBitmask(), 0x2); } -///////////////////////////////////////////////// -TEST(DOMCollision, IncorrectMeshCollisionCalculateInertial) -{ - 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 inertia - auto customMeshInertiaCalculator = []( - sdf::Errors &, - const sdf::CustomInertiaCalcProperties &) - -> std::optional - { - return std::nullopt; - }; - config.RegisterCustomInertiaCalc(customMeshInertiaCalculator); - - gz::math::Inertiald collisionInertial; - collision.CalculateInertial(errors, collisionInertial, config); - ASSERT_FALSE(errors.empty()); - - // Expect mesh class to handle null inertia and return - // default inertial values - gz::math::Inertiald defaultInertial; - defaultInertial.SetMassMatrix( - gz::math::MassMatrix3d(1.0, - gz::math::Vector3d::One, - gz::math::Vector3d::Zero)); - ASSERT_EQ(collisionInertial, defaultInertial); -} - ///////////////////////////////////////////////// TEST(DOMCollision, IncorrectBoxCollisionCalculateInertial) { diff --git a/src/Mesh.cc b/src/Mesh.cc index d2ee4b7a8..46442f279 100644 --- a/src/Mesh.cc +++ b/src/Mesh.cc @@ -376,6 +376,10 @@ std::optional Mesh::CalculateInertial(sdf::Errors &_errors, { const auto &customCalculator = _config.CustomInertiaCalc(); + auto defaultInertial = gz::math::Inertiald( + gz::math::MassMatrix3d(1, gz::math::Vector3d::One, + gz::math::Vector3d::Zero), + gz::math::Pose3d::Zero); if (!customCalculator) { Error err( @@ -385,17 +389,25 @@ std::optional Mesh::CalculateInertial(sdf::Errors &_errors, "inertial values."); enforceConfigurablePolicyCondition( _config.WarningsPolicy(), err, _errors); - - using namespace gz::math; - return Inertiald( - MassMatrix3d(1, Vector3d::One, Vector3d::Zero), - Pose3d::Zero); + return defaultInertial; } sdf::CustomInertiaCalcProperties calcInterface = CustomInertiaCalcProperties( _density, *this, _autoInertiaParams); - return customCalculator(_errors, calcInterface); + std::optional inertial = + customCalculator(_errors, calcInterface); + if (!inertial) + { + Error err( + sdf::ErrorCode::WARNING, + "Custom moment of inertia calculator for meshes produced invalid " + "inertia, using default inertial values."); + enforceConfigurablePolicyCondition( + _config.WarningsPolicy(), err, _errors); + return defaultInertial; + } + return inertial; } ///////////////////////////////////////////////// diff --git a/src/Mesh_TEST.cc b/src/Mesh_TEST.cc index 391283976..2ee0765dd 100644 --- a/src/Mesh_TEST.cc +++ b/src/Mesh_TEST.cc @@ -298,7 +298,15 @@ TEST(DOMMesh, CalcualteInertial) auto meshInertial = mesh.CalculateInertial(errors, density, autoInertiaParamsElem, config); ASSERT_FALSE(errors.empty()); - ASSERT_EQ(meshInertial, std::nullopt); + gz::math::Inertiald defaultInertial; + defaultInertial.SetMassMatrix( + gz::math::MassMatrix3d( + 1.0, + gz::math::Vector3d::One, + gz::math::Vector3d::Zero + ) + ); + ASSERT_EQ(defaultInertial, meshInertial); density = 1240.0; sdf::Errors errors2; @@ -353,7 +361,52 @@ TEST(DOMMesh, CalculateInertiaWithEmptyFilePath) auto meshInertial = mesh.CalculateInertial(errors, density, autoInertiaParamsElem, config); ASSERT_FALSE(errors.empty()); - ASSERT_EQ(meshInertial, std::nullopt); + gz::math::Inertiald defaultInertial; + defaultInertial.SetMassMatrix( + gz::math::MassMatrix3d( + 1.0, + gz::math::Vector3d::One, + gz::math::Vector3d::Zero + ) + ); + ASSERT_EQ(defaultInertial, meshInertial); +} + +///////////////////////////////////////////////// +TEST(DOMMesh, CalculateInertiaWithNullInertiaFromMeshCalculator) +{ + sdf::Mesh mesh; + sdf::ParserConfig config; + sdf::Errors errors; + sdf::CustomInertiaCalcProperties inertiaCalcProps; + + sdf::ElementPtr autoInertiaParamsElem(new sdf::Element()); + + // A custom inertia calculator that return nullopt + auto customMeshInertiaCalculator = []( + sdf::Errors &, + const sdf::CustomInertiaCalcProperties &) + -> std::optional + { + return std::nullopt; + }; + + config.RegisterCustomInertiaCalc(customMeshInertiaCalculator); + + double density = 1000; + auto meshInertial = mesh.CalculateInertial(errors, + density, autoInertiaParamsElem, config); + ASSERT_TRUE(errors.empty()); + + gz::math::Inertiald defaultInertial; + defaultInertial.SetMassMatrix( + gz::math::MassMatrix3d( + 1.0, + gz::math::Vector3d::One, + gz::math::Vector3d::Zero + ) + ); + ASSERT_EQ(defaultInertial, meshInertial); } /////////////////////////////////////////////////