[parsing] Add a very bad mesh test for mujoco parser (#22414)

This tests the error path in the parser when even falling back to convex hull can't save us. Fix one control flow path bug in the parser found by this new test.
RobotLocomotion · Jan 8, 2025 · 9639247 · 9639247
1 parent 7723bab
commit 9639247
Show file tree

Hide file tree

Showing 3 changed files with 44 additions and 0 deletions.
diff --git a/multibody/parsing/detail_mujoco_parser.cc b/multibody/parsing/detail_mujoco_parser.cc
@@ -984,6 +984,7 @@ class MujocoParser {
       if (!M_GGo_G.IsPhysicallyValid()) {
         Error(*node, fmt::format("geom {} {}", geom.name,
                                  M_GGo_G.CriticizeNotPhysicallyValid()));
+        return geom;
       }
 
       // Shift spatial inertia from Go to Bo and express it in the B frame.

diff --git a/multibody/parsing/test/detail_mujoco_parser_test.cc b/multibody/parsing/test/detail_mujoco_parser_test.cc
@@ -772,6 +772,41 @@ TEST_F(MujocoParserTest, BadMeshSpatialInertiaFallback) {
   // that we're using it (and not choking).
 }
 
+// Some meshes are so broken that even falling back to a convex hull can't save
+// us. Verify we get a useful error message.
+TEST_F(MujocoParserTest, BadMeshSpatialInertiaTotalFail) {
+  // This obj is known to produce a negative volume in CalcSpatialInertia().
+  const RlocationOrError rlocation = FindRunfile(
+      "drake/multibody/parsing/test/neg_volume.obj");
+  ASSERT_EQ(rlocation.error, "");
+  const geometry::Mesh bad_mesh(rlocation.abspath, 1.0);
+
+  // We know the spatial inertia is bad because CalcSpatialInertia()
+  // throws. Keep this confirmation that the mesh in question truly is **bad**.
+  DRAKE_EXPECT_THROWS_MESSAGE(CalcSpatialInertia(bad_mesh, 1.0),
+          ".*volume.*is -1.*");
+
+  std::string xml = fmt::format(R"""(
+<mujoco model="test">
+  <asset>
+    <mesh name="box" file="{}"/>  </asset>
+  <worldbody>
+    <body name="body1">
+      <geom name="box_geom" type="mesh" mesh="box"/>
+    </body>
+  </worldbody>
+</mujoco>
+)""",  rlocation.abspath);
+
+  EXPECT_NO_THROW(AddModelFromString(xml, "test"));
+  EXPECT_THAT(TakeWarning(), MatchesRegex(".*volume.*is -1.*"));
+  EXPECT_THAT(TakeWarning(), MatchesRegex(".*fallback.*"));
+  // Note: These errors are reported as a result of attempted fallback to a
+  // convex hull.
+  EXPECT_THAT(TakeError(), MatchesRegex(".*even.*convex hull.*"));
+  EXPECT_THAT(TakeError(), MatchesRegex(".*IsPhysicallyValid.*"));
+}
+
 TEST_F(MujocoParserTest, MeshFileRelativePathFromFile) {
   const std::string file = FindResourceOrThrow(
       "drake/multibody/parsing/test/box_package/mjcfs/box.xml");

diff --git a/multibody/parsing/test/neg_volume.obj b/multibody/parsing/test/neg_volume.obj
@@ -0,0 +1,8 @@
+# An adversarially bad mesh that produces a negative volume in
+# CalcSpatialInertia(). The single triangle has an implied normal pointing
+# toward the origin.
+
+v 1 0 0
+v 1 2 0
+v 1 0 3
+f 3 2 1