From 21da7219d849501cff802ce00dca0de83af49cc3 Mon Sep 17 00:00:00 2001
From: Russ Tedrake <russt@mit.edu>
Date: Sun, 5 Jan 2025 15:30:30 -0500
Subject: [PATCH] Mujoco parser: improve error message for "size from mesh"
 (#22389)

Towards #20444.
---
 multibody/parsing/detail_mujoco_parser.cc     | 30 ++++++++++++-------
 .../detail_mujoco_parser_examples_test.cc     | 10 ++++---
 .../parsing/test/detail_mujoco_parser_test.cc |  6 ++++
 3 files changed, 31 insertions(+), 15 deletions(-)

diff --git a/multibody/parsing/detail_mujoco_parser.cc b/multibody/parsing/detail_mujoco_parser.cc
index abcd22631310..ce87c7c938b4 100644
--- a/multibody/parsing/detail_mujoco_parser.cc
+++ b/multibody/parsing/detail_mujoco_parser.cc
@@ -713,6 +713,24 @@ class MujocoParser {
     }
 
     std::string mesh;
+    bool has_mesh_attribute = ParseStringAttribute(node, "mesh", &mesh);
+    if (has_mesh_attribute && type != "mesh") {
+      if (type == "sphere" || type == "capsule" || type == "cylinder" ||
+          type == "ellipsoid" || type == "box") {
+        Error(*node,
+              fmt::format(
+                  "geom {} specified type '{}' but also has a mesh attribute. "
+                  "The intended behavior is to compute the size of the shape "
+                  "from the mesh, but this is not supported yet (#22372).",
+                  geom.name, type));
+        return geom;
+      } else {
+        Error(*node, fmt::format("geom {} specified a 'mesh', but this is not "
+                                 "allowed for type '{}'.",
+                                 geom.name, type));
+        return geom;
+      }
+    }
     if (type == "plane") {
       // We interpret the MuJoCo infinite plane as a half-space.
       geom.shape = std::make_unique<geometry::HalfSpace>();
@@ -801,7 +819,7 @@ class MujocoParser {
             size[0] * 2.0, size[1] * 2.0, size[2] * 2.0);
       }
     } else if (type == "mesh") {
-      if (!ParseStringAttribute(node, "mesh", &mesh)) {
+      if (!has_mesh_attribute) {
         Error(*node, fmt::format("geom {} specified type 'mesh', but did not "
                                  "set the mesh attribute",
                                  geom.name));
@@ -826,16 +844,6 @@ class MujocoParser {
       return geom;
     }
 
-    if (type != "mesh") {
-      // TODO(russt): Support the mesh tag for non-mesh geometry. (Presumably
-      // this is how they specify different visual + collision geometry).
-      WarnUnsupportedAttribute(*node, "mesh");
-      /* From the MuJoCo docs: Note that mesh assets can also be referenced
-      from other geom types, causing primitive shapes to be fitted; see below.
-      The size is determined by the mesh asset and the geom size parameters are
-      ignored. */
-    }
-
     int contype{1};
     int conaffinity{1};
     int condim{3};
diff --git a/multibody/parsing/test/detail_mujoco_parser_examples_test.cc b/multibody/parsing/test/detail_mujoco_parser_examples_test.cc
index 34c59cb0a9f2..b637e20690d2 100644
--- a/multibody/parsing/test/detail_mujoco_parser_examples_test.cc
+++ b/multibody/parsing/test/detail_mujoco_parser_examples_test.cc
@@ -95,7 +95,8 @@ constexpr std::string_view kItWorks{""};
 constexpr std::string_view kSkipMe{"skip me"};
 namespace KnownErrors {
 constexpr std::string_view kNonUniformScale{".*non-uniform scale.*"};  // #22046
-constexpr std::string_view kCapsuleSize{".*size attribute for capsule geom.*"};
+constexpr std::string_view kSizeFromMesh =
+    ".*size of the shape from the mesh.*";  // #22372
 }  // namespace KnownErrors
 
 class MujocoMenagerieTest : public MujocoParserExamplesTest,
@@ -149,8 +150,9 @@ 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},  // works, but too slow in debug mode.
-    {"flybody/scene", kSkipMe},     // works, but too slow in debug mode.
+    {"flybody/fruitfly",
+     kSkipMe},                   // kSizeFromMesh, but too slow in debug mode.
+    {"flybody/scene", kSkipMe},  // kSizeFromMesh, but too slow in debug mode.
     {"franka_emika_panda/hand", kItWorks},
     {"franka_emika_panda/mjx_panda", kItWorks},
     {"franka_emika_panda/mjx_scene", kItWorks},
@@ -211,7 +213,7 @@ const std::pair<const char*, std::string_view> mujoco_menagerie_models[] = {
     {"pal_tiago_dual/tiago_dual_motor", KnownErrors::kNonUniformScale},
     {"pal_tiago_dual/tiago_dual_position", KnownErrors::kNonUniformScale},
     {"pal_tiago_dual/tiago_dual_velocity", KnownErrors::kNonUniformScale},
-    {"realsense_d435i/d435i", KnownErrors::kCapsuleSize},
+    {"realsense_d435i/d435i", KnownErrors::kSizeFromMesh},
     {"rethink_robotics_sawyer/scene", kItWorks},
     {"rethink_robotics_sawyer/sawyer", kItWorks},
     {"robotiq_2f85/2f85", kItWorks},
diff --git a/multibody/parsing/test/detail_mujoco_parser_test.cc b/multibody/parsing/test/detail_mujoco_parser_test.cc
index 8d1d81c298cc..4cf8e8c91c4c 100644
--- a/multibody/parsing/test/detail_mujoco_parser_test.cc
+++ b/multibody/parsing/test/detail_mujoco_parser_test.cc
@@ -1483,6 +1483,8 @@ TEST_F(MujocoParserTest, GeomErrors) {
       <geom type="mesh"/>
       <geom type="mesh" mesh="nonsense"/>
       <geom type="hfield"/>
+      <geom type="capsule" mesh="nonsense"/>
+      <geom type="plane" mesh="nonsense"/>
     </body>
   </worldbody>
 </mujoco>
@@ -1515,6 +1517,10 @@ TEST_F(MujocoParserTest, GeomErrors) {
       ".*size.*box.*must have.*three elements.*"));
   EXPECT_THAT(TakeError(), MatchesRegex(
       ".*mesh.*did not set the mesh attribute.*"));
+  EXPECT_THAT(TakeError(), MatchesRegex(
+      ".*size of the shape from the mesh.*"));
+  EXPECT_THAT(TakeError(), MatchesRegex(
+      ".*specified a 'mesh', but this is not allowed for type 'plane'.*"));
 }
 
 TEST_F(MujocoParserTest, Motor) {