[mujoco parser] Fix debug throw from RotationalInertia (#22401)

The InertiaCalculator is not supposed to throw, but could still throw
in debug mode based on how it was constructing an intermediate
RotationalInertia. Now we skip the intermediate check and properly
catch the error to display a helpful message in the parser.

Towards #20444.

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<std::string>(result)) {
           policy_.Warning(std::get<std::string>(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<std::string>(result)) {
+            policy_.Error(fmt::format(
+                "Failed to compute spatial inertia even for the convex hull of "
+                "{}.\n{}",
+                mesh.source().path().string(), std::get<std::string>(result)));
+          } else {
+            M_GGo_G_unitDensity = std::get<SpatialInertia<double>>(result);
+          }
           used_convex_hull_fallback_ = true;
         } else {
           M_GGo_G_unitDensity = std::get<SpatialInertia<double>>(result);

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"};  // #22412
 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<const char*, std::string_view> 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<const char*, std::string_view> 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<T>::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},

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);
 

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<double>::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<double>{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;
 }