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 @@ + + +