From 681a21ab2d3205c80aca19d4618168e340b73183 Mon Sep 17 00:00:00 2001 From: nepfaff Date: Mon, 7 Jul 2025 18:09:12 -0400 Subject: [PATCH 1/2] Support SDF files in make_drake_compatible_model.py --- manipulation/make_drake_compatible_model.py | 98 +++++++++++++++++++-- 1 file changed, 91 insertions(+), 7 deletions(-) diff --git a/manipulation/make_drake_compatible_model.py b/manipulation/make_drake_compatible_model.py index d34c0612..238f6dce 100644 --- a/manipulation/make_drake_compatible_model.py +++ b/manipulation/make_drake_compatible_model.py @@ -78,7 +78,10 @@ def _convert_mesh( return output_url, output_path # Load the mesh - mesh_or_scene = trimesh.load(path) + try: + mesh_or_scene = trimesh.load(path) + except Exception as e: + raise ValueError(f"Failed to load mesh {path}:\n{e}") if isinstance(mesh_or_scene, trimesh.Scene): meshes = [mesh for mesh in mesh_or_scene.geometry.values()] else: @@ -205,7 +208,83 @@ def _convert_urdf( def _convert_sdf( input_filename: str, output_filename: str, package_map: PackageMap, overwrite: bool ) -> None: - raise NotImplementedError("SDF format is not supported yet, but it will be soon.") + """Convert an SDF file to be compatible with Drake. + + Args: + input_filename: The path to the input SDF file. + output_filename: The path where the converted SDF file will be saved. + package_map: The PackageMap to use. + overwrite: Whether to overwrite existing files. + """ + with open(input_filename, "r") as file: + sdf_content = file.read() + + # Remove XML comments to avoid matching filenames inside them + sdf_content_no_comments = re.sub(r"", "", sdf_content, flags=re.DOTALL) + + # Parse the XML to properly handle SDF structure + try: + root = etree.fromstring(sdf_content_no_comments) + except etree.XMLSyntaxError as e: + raise ValueError(f"Invalid SDF XML syntax in {input_filename}: {e}") + + # Find all mesh elements with uri containing .stl, .dae, or .obj extensions + mesh_elements = root.xpath(".//mesh[uri]") + + # Filter to only those with target file extensions + target_extensions = (".stl", ".dae", ".obj") + filtered_mesh_elements = [] + for mesh_element in mesh_elements: + uri_element = mesh_element.find("uri") + if uri_element is not None and uri_element.text: + uri_text = uri_element.text.strip().lower() + if any(uri_text.endswith(ext) for ext in target_extensions): + filtered_mesh_elements.append(mesh_element) + + for mesh_element in filtered_mesh_elements: + uri_element = mesh_element.find("uri") + mesh_url = uri_element.text.strip() + + # Handle scale element + scale_element = mesh_element.find("scale") + scale = None + if scale_element is not None: + scale_values = scale_element.text.strip().split() + if len(scale_values) == 3: + scale = [float(s) for s in scale_values] + if len(set(scale)) == 1 and all(s > 0 for s in scale): + # Uniform positive scaling is supported natively by Drake + scale = None + else: + scale_element.text = "1 1 1" + + # Don't need to convert .obj files with no scale or uniform scale + if mesh_url.lower().endswith(".obj") and scale is None: + continue + + # Resolve the mesh path + if mesh_url.lower().startswith("package://") or mesh_url.lower().startswith( + "file://" + ): + mesh_path = package_map.ResolveUrl(mesh_url) + else: + mesh_path = os.path.join(os.path.dirname(input_filename), mesh_url) + + # Convert the mesh + output_mesh_url, _ = _convert_mesh( + url=mesh_url, path=mesh_path, scale=scale, overwrite=overwrite + ) + + # Update the URI element text + uri_element.text = output_mesh_url + + # Convert back to string + output_content = etree.tostring(root, encoding="unicode", pretty_print=True) + + with open(output_filename, "w") as file: + file.write(output_content) + + print(f"Converted SDF file '{input_filename}' to '{output_filename}'.") def _process_includes( @@ -331,7 +410,8 @@ def _convert_mjcf( os.path.abspath(input_filename) ): warnings.warn( - f"Output path {os.path.dirname(output_filename)} differs from input path {os.path.dirname(input_filename)}. " + f"Output path {os.path.dirname(output_filename)} differs from input path " + f"{os.path.dirname(input_filename)}. " "This may cause issues with relative paths in the MJCF file." ) @@ -377,7 +457,8 @@ def process_defaults(element, parent_class="main"): # Get class name, default to parent_class if not specified class_name = element.get("class", parent_class) - # Process this default element's children (which define defaults for specific types) + # Process this default element's children (which define defaults for specific + # types) for child in element: if child.tag != "default": # Only process non-default children # Create key from class name and element type @@ -493,7 +574,9 @@ def process_defaults(element, parent_class="main"): and mesh_to_material[mesh_name] != material_name ): raise AssertionError( - f"Mesh {mesh_name} was already associated with {mesh_to_material[mesh_name]}. We don't handle multiple materials assigned to the same mesh yet." + f"Mesh {mesh_name} was already associated with " + f"{mesh_to_material[mesh_name]}. We don't handle multiple " + "materials assigned to the same mesh yet." ) mesh_to_material[mesh_name] = material_name @@ -591,7 +674,8 @@ def process_defaults(element, parent_class="main"): if parent.tag == "worldbody": break elif parent.tag == "body": - # Then the plane would have been parsed as a dynamic collision element in Drake. Replace it with a large box. + # Then the plane would have been parsed as a dynamic collision element + # in Drake. Replace it with a large box. geom.attrib["size"] = "1000 1000 1" if "pos" in geom.attrib: pos = [float(value) for value in geom.attrib["pos"].split()] @@ -623,7 +707,7 @@ def MakeDrakeCompatibleModel( overwrite: bool = False, remap_mujoco_geometry_groups: dict[int, int] = {}, ) -> None: - """Converts a model file (currently .urdf or .xml)to be compatible with the + """Converts a model file (currently .urdf, .sdf, or .xml) to be compatible with the Drake multibody parsers. For all models: From 412507d5ccd93614e52e8a3146259c041256d8d8 Mon Sep 17 00:00:00 2001 From: nepfaff Date: Sun, 13 Jul 2025 09:48:19 -0400 Subject: [PATCH 2/2] Add test for make_drake_compatible_models SDF conversion --- manipulation/test/models/.gitignore | 3 ++ manipulation/test/models/test.sdf | 44 +++++++++++++++++++ .../test/test_make_drake_compatible_model.py | 35 ++++++++++++++- 3 files changed, 80 insertions(+), 2 deletions(-) create mode 100644 manipulation/test/models/.gitignore create mode 100644 manipulation/test/models/test.sdf diff --git a/manipulation/test/models/.gitignore b/manipulation/test/models/.gitignore new file mode 100644 index 00000000..a1555a6b --- /dev/null +++ b/manipulation/test/models/.gitignore @@ -0,0 +1,3 @@ +# Ignore files written by test_make_drake_compatible_model.py +cube_from* +test_modified* diff --git a/manipulation/test/models/test.sdf b/manipulation/test/models/test.sdf new file mode 100644 index 00000000..d2c25f35 --- /dev/null +++ b/manipulation/test/models/test.sdf @@ -0,0 +1,44 @@ + + + + + + + + cube.stl + + + + + + + cube.dae + + + + + + + cube.obj + + + + + + + file://replace_me_in_test_with_absolute_path/cube.obj + 1 2 3 + + + + + + + package://manipulation_test_models/cube.obj + -1 1 1 + + + + + + diff --git a/manipulation/test/test_make_drake_compatible_model.py b/manipulation/test/test_make_drake_compatible_model.py index b447c854..f1075de6 100644 --- a/manipulation/test/test_make_drake_compatible_model.py +++ b/manipulation/test/test_make_drake_compatible_model.py @@ -68,8 +68,39 @@ def test_urdf(self): os.remove(output_filename) def test_sdf(self): - # TODO(russt): Implement this. - pass + original_filename = FindResource("test/models/test.sdf") + input_filename = original_filename.replace(".sdf", "_modified.sdf") + with open(original_filename, "r") as file: + original_content = file.read() + modified_content = original_content.replace( + "replace_me_in_test_with_absolute_path", os.path.dirname(input_filename) + ) + with open(input_filename, "w") as file: + file.write(modified_content) + output_filename = tempfile.mktemp(suffix=".sdf") + package_map = PackageMap() + package_map.AddPackageXml(filename=FindResource("test/models/package.xml")) + MakeDrakeCompatibleModel( + input_filename=input_filename, + output_filename=output_filename, + package_map=package_map, + ) + self.assertTrue(os.path.exists(output_filename)) + with open(output_filename, "r") as f: + output_content = f.read() + self.assertIn("cube_from_stl.obj", output_content) + self.assertIn("cube_from_dae.obj", output_content) + self.assertIn("cube.obj", output_content) + self.assertIn( + f"file://{os.path.dirname(input_filename)}/cube_from_obj_scaled_1_2_3.obj", + output_content, + ) + self.assertIn( + "package://manipulation_test_models/cube_from_obj_scaled_n1_1_1.obj", + output_content, + ) + # Clean up the temp file + os.remove(output_filename) def test_mjcf(self): input_filename = FindResource("test/models/test.xml")