From 942f5140b620ef753d8598d921ee798e4e04031d Mon Sep 17 00:00:00 2001 From: castor639 <316019788+castor639@users.noreply.github.com> Date: Tue, 1 Sep 2026 18:35:17 +0000 Subject: [PATCH] [parsing] Resolve MuJoCo nested include assets relative to included XML Match MuJoCo by tracking each include file's directory through nested includes and resolving mesh asset paths against that base when the main-MJCF-relative path does not exist. --- multibody/parsing/detail_mujoco_parser.cc | 88 +++++++++++++++---- .../parsing/test/detail_mujoco_parser_test.cc | 26 ++++++ .../nested/assets/box.obj | 26 ++++++ .../nested/assets/meshes.xml | 3 + .../nested_include_assets/nested/mid.xml | 8 ++ .../nested_include_assets/scene.xml | 3 + 6 files changed, 139 insertions(+), 15 deletions(-) create mode 100644 multibody/parsing/test/mujoco_parser_test/nested_include_assets/nested/assets/box.obj create mode 100644 multibody/parsing/test/mujoco_parser_test/nested_include_assets/nested/assets/meshes.xml create mode 100644 multibody/parsing/test/mujoco_parser_test/nested_include_assets/nested/mid.xml create mode 100644 multibody/parsing/test/mujoco_parser_test/nested_include_assets/scene.xml diff --git a/multibody/parsing/detail_mujoco_parser.cc b/multibody/parsing/detail_mujoco_parser.cc index bab05dc17fc9..3bd4c8493acd 100644 --- a/multibody/parsing/detail_mujoco_parser.cc +++ b/multibody/parsing/detail_mujoco_parser.cc @@ -1417,21 +1417,39 @@ class MujocoParser { attribute is set and contains an absolute path, the full path is the meshdir appended with the file name; (3) the full path is the path to the main MJCF model file, appended with the value of meshdir if - set, appended with the file name. */ + set, appended with the file name. + + In addition (matching MuJoCo's include behavior), if the mesh was + contributed by an included MJCF file and the path from (2)/(3) does not + exist, the file is resolved relative to that included file's + directory. */ // TODO(russt): Support strippath. if (!filename.is_absolute()) { + std::filesystem::path from_compiler_or_main; if (meshdir_) { if (meshdir_->is_absolute()) { - filename = *meshdir_ / filename; + from_compiler_or_main = *meshdir_ / filename; } else { - filename = main_mjcf_path_ / *meshdir_ / filename; + from_compiler_or_main = main_mjcf_path_ / *meshdir_ / filename; } } else { - filename = main_mjcf_path_ / filename; + from_compiler_or_main = main_mjcf_path_ / filename; + } + from_compiler_or_main = + std::filesystem::weakly_canonical(from_compiler_or_main); + if (std::filesystem::exists(from_compiler_or_main)) { + filename = from_compiler_or_main; + } else if (auto it = element_include_dir_.find(mesh_node); + it != element_include_dir_.end()) { + filename = + std::filesystem::weakly_canonical(it->second / filename); + } else { + filename = from_compiler_or_main; } + } else { + filename = std::filesystem::weakly_canonical(filename); } - filename = std::filesystem::weakly_canonical(filename); if (std::filesystem::exists(filename)) { std::string extension = filename.extension(); @@ -1861,12 +1879,37 @@ class MujocoParser { WarnUnsupportedElement(*node, "distance"); // removed in MuJoCo 2.2.2 } + // Records that `node` (and its non-include descendants) originated from an + // included MJCF file in `include_dir`, so relative asset paths can be + // resolved against that directory. + void MarkElementIncludeDir(XMLElement* node, + const std::filesystem::path& include_dir) { + DRAKE_DEMAND(node != nullptr); + if (std::string(node->Value()) == "include") { + // Nested includes are expanded separately with their own base path. + return; + } + element_include_dir_[node] = include_dir; + for (XMLElement* child = node->FirstChildElement(); child; + child = child->NextSiblingElement()) { + MarkElementIncludeDir(child, include_dir); + } + } + // Updates node by recursively replacing any elements under it with - // the children of the named file's root element. + // the children of the named file's root element. Nested includes and relative + // asset paths are resolved relative to the including file's directory + // (matching MuJoCo), with a fallback to the main MJCF directory. void ExpandIncludeTags(XMLElement* node, const std::filesystem::path& parent_mjcf_path) { DRAKE_DEMAND(node != nullptr); + std::filesystem::path base_path = parent_mjcf_path; + if (auto it = element_include_dir_.find(node); + it != element_include_dir_.end()) { + base_path = it->second; + } + // Process the current node if it's an tag. if (std::string(node->Value()) == "include") { std::string file; @@ -1875,11 +1918,18 @@ class MujocoParser { return; } - // From the MJCF docs: "The name of the XML file to be included. The file - // location is relative to the directory of the main MJCF file. If the - // file is not in the same directory, it should be prefixed with a - // relative path." - std::filesystem::path filename = parent_mjcf_path / file; + // MuJoCo resolves include paths relative to the main MJCF directory + // (legacy), and also relative to the including file's directory for + // nested includes. + std::filesystem::path filename(file); + if (!filename.is_absolute()) { + const std::filesystem::path from_main = main_mjcf_path_ / filename; + if (std::filesystem::exists(from_main)) { + filename = from_main; + } else { + filename = base_path / filename; + } + } filename = std::filesystem::absolute(filename); log()->debug("Processing included file: {}", filename.string()); @@ -1897,6 +1947,8 @@ class MujocoParser { return; } + const std::filesystem::path include_dir = filename.parent_path(); + // Insert the children of the root element of the included file. XMLDocument* xml_doc = node->GetDocument(); XMLElement* parent = node->Parent()->ToElement(); @@ -1906,7 +1958,9 @@ class MujocoParser { while (child) { // Insert the child node after the include node (or after the last // inserted node). - parent->InsertAfterChild(node_in_parent, child->DeepClone(xml_doc)); + XMLElement* cloned = child->DeepClone(xml_doc)->ToElement(); + MarkElementIncludeDir(cloned, include_dir); + parent->InsertAfterChild(node_in_parent, cloned); node_in_parent = node_in_parent->NextSiblingElement(); child = child->NextSiblingElement(); } @@ -1917,7 +1971,7 @@ class MujocoParser { // Recurse on child elements. XMLElement* child = node->FirstChildElement(); while (child) { - ExpandIncludeTags(child, parent_mjcf_path); + ExpandIncludeTags(child, base_path); XMLElement* to_delete = (std::string(child->Value()) == "include") ? child : nullptr; child = child->NextSiblingElement(); @@ -1929,8 +1983,9 @@ class MujocoParser { } } - // Assets without an absolute path are referenced relative to the "main MJCF - // model file" path, `main_mjcf_path`. + // Assets without an absolute path are referenced relative to the main MJCF + // model file path (`main_mjcf_path`), or relative to the included MJCF that + // declared them when that main-relative path does not exist. std::pair, std::string> Parse( const std::string& model_name_in, const std::optional& parent_model_name, @@ -2077,6 +2132,9 @@ class MujocoParser { geometry::SceneGraph* const scene_graph_; ModelInstanceIndex model_instance_{}; std::filesystem::path main_mjcf_path_{}; + // For elements that originated from an d file, the directory of + // that included file (used to resolve relative include and asset paths). + std::map element_include_dir_{}; bool autolimits_{true}; enum Angle { kRadian, kDegree }; Angle angle_{kDegree}; diff --git a/multibody/parsing/test/detail_mujoco_parser_test.cc b/multibody/parsing/test/detail_mujoco_parser_test.cc index 7d2d1d19f7ae..357ac5683f09 100644 --- a/multibody/parsing/test/detail_mujoco_parser_test.cc +++ b/multibody/parsing/test/detail_mujoco_parser_test.cc @@ -628,6 +628,32 @@ TEST_F(MujocoParserTest, Include) { EXPECT_EQ(plant_->CalcTotalMass(*context), 2.0); } +// Regression test for https://github.com/RobotLocomotion/drake/issues/22638: +// mesh assets declared in nested includes must resolve relative to the +// included XML (RB-Y1-style layouts), not only relative to the main MJCF. +TEST_F(MujocoParserTest, NestedIncludeRelativeAssets) { + const std::string scene_file = FindResourceOrThrow( + "drake/multibody/parsing/test/mujoco_parser_test/nested_include_assets/" + "scene.xml"); + const std::string expected_mesh = std::filesystem::canonical( + FindResourceOrThrow( + "drake/multibody/parsing/test/mujoco_parser_test/" + "nested_include_assets/nested/assets/box.obj")); + + AddAllModelsFromFile(scene_file, {}); + FlushDiagnostics(); + + const SceneGraphInspector& inspector = + scene_graph_->model_inspector(); + GeometryId geom_id = inspector.GetGeometryIdByName( + inspector.world_frame_id(), Role::kProximity, "box_geom"); + const auto* mesh = + dynamic_cast(&inspector.GetShape(geom_id)); + ASSERT_NE(mesh, nullptr); + DRAKE_DEMAND(mesh->source().is_path()); + EXPECT_EQ(mesh->source().path(), expected_mesh); +} + class BoxMeshTest : public MujocoParserTest { public: // Load and evaluate a box mesh, specified in various ways by caller-supplied diff --git a/multibody/parsing/test/mujoco_parser_test/nested_include_assets/nested/assets/box.obj b/multibody/parsing/test/mujoco_parser_test/nested_include_assets/nested/assets/box.obj new file mode 100644 index 000000000000..feb39adf159a --- /dev/null +++ b/multibody/parsing/test/mujoco_parser_test/nested_include_assets/nested/assets/box.obj @@ -0,0 +1,26 @@ +v 1.000000 -1.000000 -1.000000 +v 1.000000 -1.000000 1.000000 +v -1.000000 -1.000000 1.000000 +v -1.000000 -1.000000 -1.000000 +v 1.000000 1.000000 -1.000000 +v 1.000000 1.000000 1.000000 +v -1.000000 1.000000 1.000000 +v -1.000000 1.000000 -1.000000 +vn 0.0000 -1.0000 0.0000 +vn 0.0000 1.0000 0.0000 +vn 1.0000 -0.0000 0.0000 +vn 0.0000 -0.0000 1.0000 +vn -1.0000 -0.0000 -0.0000 +vn 0.0000 0.0000 -1.0000 +f 1//1 3//1 4//1 +f 8//2 6//2 5//2 +f 5//3 2//3 1//3 +f 6//4 3//4 2//4 +f 3//5 8//5 4//5 +f 1//6 8//6 5//6 +f 1//1 2//1 3//1 +f 8//2 7//2 6//2 +f 5//3 6//3 2//3 +f 6//4 7//4 3//4 +f 3//5 7//5 8//5 +f 1//6 4//6 8//6 diff --git a/multibody/parsing/test/mujoco_parser_test/nested_include_assets/nested/assets/meshes.xml b/multibody/parsing/test/mujoco_parser_test/nested_include_assets/nested/assets/meshes.xml new file mode 100644 index 000000000000..cdf8bbca503e --- /dev/null +++ b/multibody/parsing/test/mujoco_parser_test/nested_include_assets/nested/assets/meshes.xml @@ -0,0 +1,3 @@ + + + diff --git a/multibody/parsing/test/mujoco_parser_test/nested_include_assets/nested/mid.xml b/multibody/parsing/test/mujoco_parser_test/nested_include_assets/nested/mid.xml new file mode 100644 index 000000000000..d3ca4af26de1 --- /dev/null +++ b/multibody/parsing/test/mujoco_parser_test/nested_include_assets/nested/mid.xml @@ -0,0 +1,8 @@ + + + + + + + + diff --git a/multibody/parsing/test/mujoco_parser_test/nested_include_assets/scene.xml b/multibody/parsing/test/mujoco_parser_test/nested_include_assets/scene.xml new file mode 100644 index 000000000000..eeb9a207f141 --- /dev/null +++ b/multibody/parsing/test/mujoco_parser_test/nested_include_assets/scene.xml @@ -0,0 +1,3 @@ + + +