Fix "more than one orientation" error in mujoco parser (#22387)

Towards #20444.
RobotLocomotion · Jan 5, 2025 · 5ade0b6 · 5ade0b6
1 parent e97dc74
commit 5ade0b6
Show file tree

Hide file tree

Showing 3 changed files with 67 additions and 36 deletions.
diff --git a/multibody/parsing/detail_mujoco_parser.cc b/multibody/parsing/detail_mujoco_parser.cc
@@ -49,16 +49,8 @@ using tinyxml2::XMLNode;
 
 namespace {
 
-// Any attributes from `default` that are not specified in `node` will be added
-// to `node`.
-void ApplyDefaultAttributes(const XMLElement& default_node, XMLElement* node) {
-  for (const XMLAttribute* default_attr = default_node.FirstAttribute();
-       default_attr != nullptr; default_attr = default_attr->Next()) {
-    if (!node->Attribute(default_attr->Name())) {
-      node->SetAttribute(default_attr->Name(), default_attr->Value());
-    }
-  }
-}
+constexpr std::array kOrientationAttributes = {  // BR
+    "quat", "axisangle", "euler", "xyaxes", "zaxis"};
 
 class MujocoParser {
  public:
@@ -72,30 +64,68 @@ class MujocoParser {
     unused(workspace_);
   }
 
+  void ErrorIfMoreThanOneOrientation(const XMLElement& node) {
+    int num_orientation_attrs = 0;
+    for (const char* attr : kOrientationAttributes) {
+      if (node.Attribute(attr) != nullptr) {
+        ++num_orientation_attrs;
+      }
+    }
+    if (num_orientation_attrs > 1) {
+      std::string attributes;
+      for (const XMLAttribute* attr = node.FirstAttribute(); attr;
+            attr = attr->Next()) {
+        attributes += fmt::format("{}={} ", attr->Name(),
+                                  fmt_debug_string(attr->Value()));
+      }
+      Error(node,
+            fmt::format(
+                "Element {} has more than one orientation attribute specified. "
+                "There must be no more than one instance of `quat`, "
+                "`axisangle`, `euler`, `xyaxes`, or `zaxis`. "
+                "Attributes: [{}]",
+                node.Name(), attributes));
+    }
+  }
+
+  // Any attributes from `default` that are not specified in `node` will be
+  // added to `node`. For orientation attributes, we do not apply orientation
+  // attribute defaults if the node already has an orientation attribute.
+  void ApplyDefaultAttributes(const XMLElement& default_node,
+                              XMLElement* node) {
+    bool node_has_orientation_attr = false;
+    for (const char* attr : kOrientationAttributes) {
+      if (node->Attribute(attr) != nullptr) {
+        node_has_orientation_attr = true;
+        break;
+      }
+    }
+
+    for (const XMLAttribute* default_attr = default_node.FirstAttribute();
+         default_attr != nullptr; default_attr = default_attr->Next()) {
+      if (!node->Attribute(default_attr->Name())) {
+        if (node_has_orientation_attr &&
+            std::find_if(kOrientationAttributes.cbegin(),
+                         kOrientationAttributes.cend(),
+                         [&default_attr](const char* attr) {
+                           return strcmp(attr, default_attr->Name()) == 0;
+                         }) != kOrientationAttributes.cend()) {
+          // Don't apply default orientation attributes if the node already has
+          // an orientation attribute.
+          continue;
+        }
+        node->SetAttribute(default_attr->Name(), default_attr->Value());
+      }
+    }
+  }
+
   RigidTransformd ParseTransform(
       XMLElement* node, const RigidTransformd& X_default = RigidTransformd{}) {
     Vector3d pos(X_default.translation());
     ParseVectorAttribute(node, "pos", &pos);
+    ErrorIfMoreThanOneOrientation(*node);
 
-    // Check that only one of the orientation variants are supplied:
-    const int num_orientation_attrs =
-        (node->Attribute("quat") != nullptr ? 1 : 0) +
-        (node->Attribute("axisangle") != nullptr ? 1 : 0) +
-        (node->Attribute("euler") != nullptr ? 1 : 0) +
-        (node->Attribute("xyaxes") != nullptr ? 1 : 0) +
-        (node->Attribute("zaxis") != nullptr ? 1 : 0);
-    if (num_orientation_attrs > 1) {
-      Error(
-          *node,
-          fmt::format(
-              "Element {} has more than one orientation attribute specified "
-              "(perhaps through defaults). There must be no more than one "
-              "instance of `quat`, `axisangle`, `euler`, `xyaxes`, or `zaxis`.",
-              node->Name()));
-      return {};
-    }
-
-    {
+    if (node->Attribute("quat") != nullptr) {
       Vector4d quat;  // MuJoCo uses w,x,y,z order.
       if (ParseVectorAttribute(node, "quat", &quat)) {
         return RigidTransformd(
@@ -1228,7 +1258,7 @@ class MujocoParser {
         std::filesystem::path filename(file);
 
         /* Adapted from the mujoco docs: The full path to a file is determined
-        as follows. If the strippath compiler option is “true”, all path
+        as follows. If the strippath compiler option is "true", all path
         information from the file name is removed. The following checks are
         then applied in order: (1) if the file name contains an absolute path,
         it is used without further changes; (2) if the compiler meshdir

diff --git a/multibody/parsing/test/detail_mujoco_parser_examples_test.cc b/multibody/parsing/test/detail_mujoco_parser_examples_test.cc
@@ -95,8 +95,6 @@ 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 kMoreThanOneOrientation{
-    ".*more than one orientation.*"};
 constexpr std::string_view kCapsuleSize{".*size attribute for capsule geom.*"};
 }  // namespace KnownErrors
 
@@ -137,8 +135,8 @@ const std::pair<const char*, std::string_view> mujoco_menagerie_models[] = {
     {"agility_cassie/scene", KnownErrors::kNonUniformScale},
     {"aloha/aloha", kItWorks},
     {"aloha/scene", kItWorks},
-    {"anybotics_anymal_b/anymal_b", KnownErrors::kMoreThanOneOrientation},
-    {"anybotics_anymal_b/scene", KnownErrors::kMoreThanOneOrientation},
+    {"anybotics_anymal_b/anymal_b", kItWorks},
+    {"anybotics_anymal_b/scene", kItWorks},
     {"anybotics_anymal_c/anymal_c", kItWorks},
     {"anybotics_anymal_c/anymal_c_mjx", kItWorks},
     {"anybotics_anymal_c/scene", kItWorks},

diff --git a/multibody/parsing/test/detail_mujoco_parser_test.cc b/multibody/parsing/test/detail_mujoco_parser_test.cc
@@ -373,6 +373,8 @@ TEST_F(MujocoParserTest, GeometryPose) {
     <geom name="fromto_cylinder" fromto="-1 -3 -3 -1 -1 -3"
           quat="0.2 0.4 0.3 .1" pos="1 2 3" type="cylinder" size="0.1" />
     <geom name="from_default" type="sphere" size="0.1" class="default_pose" />
+    <geom name="from_default2" type="sphere" size="0.1" class="default_pose"
+          euler="30 45 60"/> <!-- euler should overwrite quat -->
   </worldbody>
 </mujoco>
 )""";
@@ -443,6 +445,9 @@ TEST_F(MujocoParserTest, GeometryPose) {
             RigidTransformd(RotationMatrixd::MakeXRotation(-M_PI / 2.0), -p));
   CheckPose("from_default",
             RigidTransformd(Eigen::Quaternion<double>{0, 1, 0, 0}, p));
+  CheckPose(
+      "from_default2",
+      RigidTransformd(RollPitchYawd{M_PI / 6.0, M_PI / 4.0, M_PI / 3.0}, p));
 
   CheckPose(
       "axisangle_rad",
@@ -462,8 +467,6 @@ TEST_F(MujocoParserTest, GeometryPose) {
                             p));
 }
 
-
-
 TEST_F(MujocoParserTest, GeometryPoseErrors) {
   const std::string xml = R"""(
 <mujoco model="test">