diff --git a/multibody/parsing/detail_mujoco_parser.cc b/multibody/parsing/detail_mujoco_parser.cc index ce87c7c938b4..219f534e6a03 100644 --- a/multibody/parsing/detail_mujoco_parser.cc +++ b/multibody/parsing/detail_mujoco_parser.cc @@ -565,7 +565,7 @@ class MujocoParser { if (mesh_inertia_->contains(name_)) { M_GGo_G_unitDensity = mesh_inertia_->at(name_); } else { - const CalcSpatialInertiaResult result = + CalcSpatialInertiaResult result = CalcSpatialInertiaImpl(mesh, 1.0 /* density */); if (std::holds_alternative(result)) { policy_.Warning(std::get(result)); @@ -575,9 +575,17 @@ class MujocoParser { DRAKE_DEMAND(mesh.source().is_path()); // As with mujoco, failure leads to using the convex hull. // https://github.com/google-deepmind/mujoco/blob/df7ea3ed3350164d0f111c12870e46bc59439a96/src/user/user_mesh.cc#L1379-L1382 - M_GGo_G_unitDensity = CalcSpatialInertia( + result = CalcSpatialInertiaImpl( geometry::Convex(mesh.source().path(), mesh.scale()), 1.0 /* density */); + if (std::holds_alternative(result)) { + policy_.Error(fmt::format( + "Failed to compute spatial inertia even for the convex hull of " + "{}.\n{}", + mesh.source().path().string(), std::get(result))); + } else { + M_GGo_G_unitDensity = std::get>(result); + } used_convex_hull_fallback_ = true; } else { M_GGo_G_unitDensity = std::get>(result); diff --git a/multibody/parsing/test/detail_mujoco_parser_examples_test.cc b/multibody/parsing/test/detail_mujoco_parser_examples_test.cc index b637e20690d2..9e61e1675ca9 100644 --- a/multibody/parsing/test/detail_mujoco_parser_examples_test.cc +++ b/multibody/parsing/test/detail_mujoco_parser_examples_test.cc @@ -92,7 +92,7 @@ INSTANTIATE_TEST_SUITE_P(DeepMindControl, DeepMindControlTest, })); constexpr std::string_view kItWorks{""}; -constexpr std::string_view kSkipMe{"skip me"}; +constexpr std::string_view kTooSlow{"skip me"}; // especially in debug mode. namespace KnownErrors { constexpr std::string_view kNonUniformScale{".*non-uniform scale.*"}; // #22046 constexpr std::string_view kSizeFromMesh = @@ -107,7 +107,7 @@ TEST_P(MujocoMenagerieTest, MujocoMenagerie) { // Confirm successful parsing of the MuJoCo models in the DeepMind control // suite. auto [model, error_regex] = GetParam(); - if (error_regex == kSkipMe) { + if (error_regex == kTooSlow) { GTEST_SKIP_("Skipping this test case."); } const RlocationOrError rlocation = FindRunfile( @@ -150,21 +150,17 @@ const std::pair mujoco_menagerie_models[] = { {"boston_dynamics_spot/scene_arm", kItWorks}, {"boston_dynamics_spot/spot", kItWorks}, {"boston_dynamics_spot/spot_arm", kItWorks}, - {"flybody/fruitfly", - kSkipMe}, // kSizeFromMesh, but too slow in debug mode. - {"flybody/scene", kSkipMe}, // kSizeFromMesh, but too slow in debug mode. + {"flybody/fruitfly", kTooSlow}, // kSizeFromMesh + {"flybody/scene", kTooSlow}, // kSizeFromMesh {"franka_emika_panda/hand", kItWorks}, {"franka_emika_panda/mjx_panda", kItWorks}, {"franka_emika_panda/mjx_scene", kItWorks}, {"franka_emika_panda/mjx_single_cube", kItWorks}, - {"franka_emika_panda/panda", - kSkipMe}, // works, but too slow in debug mode. - {"franka_emika_panda/panda_nohand", - kSkipMe}, // works, but too slow in debug mode. - {"franka_emika_panda/scene", - kSkipMe}, // works, but too slow in debug mode. - {"franka_fr3/fr3", kSkipMe}, // works, but too slow in debug mode. - {"franka_fr3/scene", kSkipMe}, // works, but too slow in debug mode. + {"franka_emika_panda/panda", kTooSlow}, // kItWorks + {"franka_emika_panda/panda_nohand", kTooSlow}, // kItWorks + {"franka_emika_panda/scene", kTooSlow}, // kItWorks + {"franka_fr3/fr3", kTooSlow}, // kItWorks + {"franka_fr3/scene", kTooSlow}, // kItWorks {"google_barkour_v0/barkour_v0", kItWorks}, {"google_barkour_v0/barkour_v0_mjx", kItWorks}, {"google_barkour_v0/scene", kItWorks}, @@ -177,15 +173,10 @@ const std::pair mujoco_menagerie_models[] = { {"google_barkour_vb/scene_mjx", kItWorks}, {"google_robot/robot", kItWorks}, {"google_robot/scene", kItWorks}, - /* The hello_robot_stretch and hello_robot_stretch_3 models currently throw - in RotationalInertia::ThrowNotPhysicallyValid(), but only in Debug - mode. This is possibly due to the fact that the stl geometries are not - being parsed, so the proper inertias are not being computed. They _also_ - fail with KnownErrors::kNonUniformScale in release mode. */ - {"hello_robot_stretch/scene", kSkipMe}, - {"hello_robot_stretch/stretch", kSkipMe}, - {"hello_robot_stretch_3/scene", kSkipMe}, - {"hello_robot_stretch_3/stretch", kSkipMe}, + {"hello_robot_stretch/scene", kTooSlow}, // kNonUniformScale, + {"hello_robot_stretch/stretch", kTooSlow}, // kNonUniformScale, + {"hello_robot_stretch_3/scene", kTooSlow}, // kNonUniformScale, + {"hello_robot_stretch_3/stretch", kTooSlow}, // kNonUniformScale, {"kinova_gen3/gen3", kItWorks}, {"kinova_gen3/scene", kItWorks}, {"kuka_iiwa_14/iiwa14", kItWorks}, diff --git a/multibody/parsing/test/detail_mujoco_parser_test.cc b/multibody/parsing/test/detail_mujoco_parser_test.cc index 4cf8e8c91c4c..db8d97c656c1 100644 --- a/multibody/parsing/test/detail_mujoco_parser_test.cc +++ b/multibody/parsing/test/detail_mujoco_parser_test.cc @@ -744,7 +744,7 @@ TEST_F(MujocoParserTest, BadMeshSpatialInertiaFallback) { // This obj is known to produce a non-physical spatial inertia in // CalcSpatialInertia(). const RlocationOrError rlocation = FindRunfile( - "mujoco_menagerie_internal/hello_robot_stretch/assets/base_link_0.obj"); + "mujoco_menagerie_internal/hello_robot_stretch/assets/link_head_1.obj"); ASSERT_EQ(rlocation.error, ""); const geometry::Mesh bad_mesh(rlocation.abspath, 1.0); diff --git a/multibody/tree/geometry_spatial_inertia.cc b/multibody/tree/geometry_spatial_inertia.cc index df81329aaf48..d4847f7f2f5c 100644 --- a/multibody/tree/geometry_spatial_inertia.cc +++ b/multibody/tree/geometry_spatial_inertia.cc @@ -186,17 +186,17 @@ CalcSpatialInertiaResult CalcSpatialInertiaImpl( // and go get the six terms we actually care about. C /= volume; const double trace_C = C.trace(); - const UnitInertia G_GGo_G(trace_C - C(0, 0), trace_C - C(1, 1), - trace_C - C(2, 2), -C(1, 0), -C(2, 0), -C(2, 1)); + const UnitInertia G_GGo_G( + RotationalInertia::MakeFromMomentsAndProductsOfInertia( + trace_C - C(0, 0), trace_C - C(1, 1), trace_C - C(2, 2), -C(1, 0), + -C(2, 0), -C(2, 1), + /* skip_validity_check = */ true)); auto result = SpatialInertia{mass, p_GoGcm, G_GGo_G, /* skip_validity_check = */ true}; - if (!result.IsPhysicallyValid()) { - std::string message = result.CriticizeNotPhysicallyValid(); - if (!message.empty()) { - return message; - } - return result; + std::string message = result.CriticizeNotPhysicallyValid(); + if (!message.empty()) { + return message; } return result; }