From 38617b80612dffc9f748241395e8f8d1e0d4aac6 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Tue, 18 Nov 2025 11:30:30 +0100 Subject: [PATCH 01/28] added topic publisher --- scripts/make_mjcf_from_robot_description.py | 129 ++++++++++++++++---- 1 file changed, 104 insertions(+), 25 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 270ca6b..af1399f 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -628,25 +628,46 @@ def parse_inputs_xml(filename=None): dom = minidom.parse(filename) root = dom.documentElement - - # We only parse the direct children of the root node, which should be called - # "mujoco_defaults". - if root.tagName != "mujoco_inputs": - raise ValueError(f"Root tag in defaults xml must be 'mujoco_inputs', not {root.tagName}") - + raw_inputs = None processed_inputs = None - for child in root.childNodes: - if child.nodeType != child.ELEMENT_NODE: - continue - - if child.tagName == "raw_inputs": - raw_inputs = child - elif child.tagName == "processed_inputs": - processed_inputs = child + # Case 1: The file itself is mujoco_inputs.xml + if root.tagName == "mujoco_inputs": + for child in root.childNodes: + if child.nodeType != child.ELEMENT_NODE: + continue + if child.tagName == "raw_inputs": + raw_inputs = child + elif child.tagName == "processed_inputs": + processed_inputs = child + return raw_inputs, processed_inputs + + + # Case 2: It is a URDF with a block + if root.tagName == "robot": + mujoco_inputs_node = None + + # find + for child in root.childNodes: + if child.nodeType == child.ELEMENT_NODE and child.tagName == "mujoco_inputs": + mujoco_inputs_node = child + break + + if mujoco_inputs_node is None: + # URDF without mujoco_inputs → allowed + return None, None + + # parse children of + for child in mujoco_inputs_node.childNodes: + if child.nodeType != child.ELEMENT_NODE: + continue + if child.tagName == "raw_inputs": + raw_inputs = child + elif child.tagName == "processed_inputs": + processed_inputs = child - return raw_inputs, processed_inputs + return raw_inputs, processed_inputs def fix_free_joint(dom, urdf, joint_name="floating_base_joint"): @@ -1082,6 +1103,49 @@ def get_parent_chain(link, transform=PyKDL.Frame.Identity()): return results +def publish_model_on_topic(output_filepath, temp_dir, args=None): + + import rclpy + from rclpy.node import Node + from std_msgs.msg import String + from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy + + # --- Node ROS2 for model publishing MJCF --- + class MjcfPublisher(Node): + def __init__(self, mjcf_path): + super().__init__('mjcf_publisher') + + qos_profile = QoSProfile( + depth=1, + reliability=QoSReliabilityPolicy.RELIABLE, + durability=QoSDurabilityPolicy.TRANSIENT_LOCAL + ) + + self.publisher_ = self.create_publisher(String, 'mujoco_robot_description', qos_profile) + self.timer = self.create_timer(1.0, self.publish_mjcf) + self.mjcf_path = mjcf_path + + def publish_mjcf(self): + with open(self.mjcf_path, 'r') as f: + xml_content = f.read() + msg = String() + msg.data = xml_content + self.publisher_.publish(msg) + + rclpy.init(args=args) + mjcf_path = os.path.join(output_filepath, "mujoco_description_formatted.xml") + mjcf_node = MjcfPublisher(mjcf_path) + + try: + rclpy.spin(mjcf_node) + except KeyboardInterrupt: + pass + finally: + mjcf_node.destroy_node() + rclpy.shutdown() + if temp_dir is not None: + temp_dir.cleanup() + print("Temporary directory cleaned up.") def add_urdf_free_joint(urdf): """ @@ -1157,6 +1221,11 @@ def main(args=None): ) parser.add_argument("-o", "--output", default="mjcf_data", help="Generated output path") parser.add_argument("-c", "--convert_stl_to_obj", action="store_true", help="If we should convert .stls to .objs") + parser.add_argument( + "-s", "--save_only", + action="store_true", + help="Save the generated files permanently. Without this flag a temporary directory is used." + ) parser.add_argument( "-f", "--add_free_joint", @@ -1182,12 +1251,12 @@ def main(args=None): convert_stl_to_obj = parsed_args.convert_stl_to_obj - # Part inputs data - raw_inputs, processed_inputs = parse_inputs_xml(parsed_args.mujoco_inputs) - decompose_dict, cameras_dict, modify_element_dict, lidar_dict = get_processed_mujoco_inputs(processed_inputs) - - # Grab the output directory and ensure it ends with '/' - output_filepath = os.path.join(parsed_args.output, "") + if parsed_args.save_only: + output_filepath = os.path.join(parsed_args.output, "") + else: + temp_dir = tempfile.TemporaryDirectory() + output_filepath = os.path.join(temp_dir.name, "") + print(f"Using temporary directory: {output_filepath}") # Add a free joint to the urdf if request_add_free_joint: @@ -1200,10 +1269,6 @@ def main(args=None): # not sure if this is the best move... xml_data = remove_tag(xml_data, "collision") - xml_data = replace_package_names(xml_data) - mesh_info_dict = extract_mesh_info(xml_data) - xml_data = convert_to_objs(mesh_info_dict, output_filepath, xml_data, convert_stl_to_obj, decompose_dict) - print("writing data to robot_description_formatted.urdf") robot_description_filename = "robot_description_formatted.urdf" with open(output_filepath + "robot_description_formatted.urdf", "w") as file: @@ -1211,6 +1276,17 @@ def main(args=None): xml_data = "\n".join([line for line in xml_data.splitlines() if line.strip()]) file.write(xml_data) + # Part inputs data from urdf + if not parsed_args.mujoco_inputs: + parsed_args.mujoco_inputs= parsed_args.urdf + + raw_inputs, processed_inputs = parse_inputs_xml(parsed_args.mujoco_inputs) + decompose_dict, cameras_dict, modify_element_dict, lidar_dict = get_processed_mujoco_inputs(processed_inputs) + + xml_data = replace_package_names(xml_data) + mesh_info_dict = extract_mesh_info(xml_data) + xml_data = convert_to_objs(mesh_info_dict, output_filepath, xml_data, convert_stl_to_obj, decompose_dict) + model = mujoco.MjModel.from_xml_path(f"{output_filepath}{robot_description_filename}") mujoco.mj_saveLastXML(f"{output_filepath}mujoco_description.xml", model) @@ -1228,6 +1304,9 @@ def main(args=None): ) shutil.copy2(f'{get_package_share_directory("mujoco_ros2_simulation")}/resources/scene.xml', output_filepath) + + if not parsed_args.save_only: + publish_model_on_topic(output_filepath, temp_dir, args) if __name__ == "__main__": From 30cfe0c0f49e919afb27804d9026587c1e895b4c Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Tue, 18 Nov 2025 12:09:09 +0100 Subject: [PATCH 02/28] restored the order of the instructions in main --- scripts/make_mjcf_from_robot_description.py | 22 ++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index af1399f..488b294 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -1251,6 +1251,13 @@ def main(args=None): convert_stl_to_obj = parsed_args.convert_stl_to_obj + # Part inputs data from urdf + if not parsed_args.mujoco_inputs: + parsed_args.mujoco_inputs= parsed_args.urdf + + raw_inputs, processed_inputs = parse_inputs_xml(parsed_args.mujoco_inputs) + decompose_dict, cameras_dict, modify_element_dict, lidar_dict = get_processed_mujoco_inputs(processed_inputs) + if parsed_args.save_only: output_filepath = os.path.join(parsed_args.output, "") else: @@ -1269,6 +1276,10 @@ def main(args=None): # not sure if this is the best move... xml_data = remove_tag(xml_data, "collision") + xml_data = replace_package_names(xml_data) + mesh_info_dict = extract_mesh_info(xml_data) + xml_data = convert_to_objs(mesh_info_dict, output_filepath, xml_data, convert_stl_to_obj, decompose_dict) + print("writing data to robot_description_formatted.urdf") robot_description_filename = "robot_description_formatted.urdf" with open(output_filepath + "robot_description_formatted.urdf", "w") as file: @@ -1276,17 +1287,6 @@ def main(args=None): xml_data = "\n".join([line for line in xml_data.splitlines() if line.strip()]) file.write(xml_data) - # Part inputs data from urdf - if not parsed_args.mujoco_inputs: - parsed_args.mujoco_inputs= parsed_args.urdf - - raw_inputs, processed_inputs = parse_inputs_xml(parsed_args.mujoco_inputs) - decompose_dict, cameras_dict, modify_element_dict, lidar_dict = get_processed_mujoco_inputs(processed_inputs) - - xml_data = replace_package_names(xml_data) - mesh_info_dict = extract_mesh_info(xml_data) - xml_data = convert_to_objs(mesh_info_dict, output_filepath, xml_data, convert_stl_to_obj, decompose_dict) - model = mujoco.MjModel.from_xml_path(f"{output_filepath}{robot_description_filename}") mujoco.mj_saveLastXML(f"{output_filepath}mujoco_description.xml", model) From dfa82e4cd2b665a2f956b17eec8317a52550204e Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Thu, 20 Nov 2025 16:57:53 +0100 Subject: [PATCH 03/28] fixed mujoco model publish --- CMakeLists.txt | 2 +- scripts/make_mjcf_from_robot_description.py | 40 +++++++++++++-------- test/launch/test_robot.launch.py | 35 ++++++++++++++++++ 3 files changed, 62 insertions(+), 15 deletions(-) diff --git a/CMakeLists.txt b/CMakeLists.txt index f16a82b..86a6fb0 100644 --- a/CMakeLists.txt +++ b/CMakeLists.txt @@ -207,7 +207,7 @@ install(PROGRAMS ) install( - DIRECTORY resources + DIRECTORY resources scripts DESTINATION share/${PROJECT_NAME} ) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 488b294..da5ef67 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -42,12 +42,12 @@ COMPOSED_PATH_NAME = "full" -def add_mujoco_info(raw_xml): +def add_mujoco_info(raw_xml, output_filepath): dom = minidom.parseString(raw_xml) mujoco_element = dom.createElement("mujoco") compiler_element = dom.createElement("compiler") - compiler_element.setAttribute("meshdir", "assets") + compiler_element.setAttribute("meshdir", output_filepath+"/assets") compiler_element.setAttribute("balanceinertia", "true") compiler_element.setAttribute("discardvisual", "false") compiler_element.setAttribute("strippath", "false") @@ -1142,10 +1142,13 @@ def publish_mjcf(self): pass finally: mjcf_node.destroy_node() - rclpy.shutdown() + try: + rclpy.shutdown() + except Exception: + pass if temp_dir is not None: temp_dir.cleanup() - print("Temporary directory cleaned up.") + print("Temporary directory cleaned up.", flush=True) def add_urdf_free_joint(urdf): """ @@ -1251,15 +1254,9 @@ def main(args=None): convert_stl_to_obj = parsed_args.convert_stl_to_obj - # Part inputs data from urdf - if not parsed_args.mujoco_inputs: - parsed_args.mujoco_inputs= parsed_args.urdf - - raw_inputs, processed_inputs = parse_inputs_xml(parsed_args.mujoco_inputs) - decompose_dict, cameras_dict, modify_element_dict, lidar_dict = get_processed_mujoco_inputs(processed_inputs) - if parsed_args.save_only: output_filepath = os.path.join(parsed_args.output, "") + print(f"Using destination directory: {output_filepath}") else: temp_dir = tempfile.TemporaryDirectory() output_filepath = os.path.join(temp_dir.name, "") @@ -1269,8 +1266,25 @@ def main(args=None): if request_add_free_joint: urdf = add_urdf_free_joint(urdf) + # If exists the mujoco_input.xml get the input tags from that file, otherwise try to get them from URDF + if not parsed_args.mujoco_inputs: + if parsed_args.urdf: + parsed_args.mujoco_inputs= parsed_args.urdf + elif not parsed_args.save_only: + print("writing URDF model from string to robot_description.urdf") + robot_description_filename = "robot_description.urdf" + with open(output_filepath + "robot_description.urdf", "w") as file: + # Remove extra newlines that minidom adds after each tag + urdf_data = "\n".join([line for line in urdf.splitlines() if line.strip()]) + file.write(urdf_data) + parsed_args.mujoco_inputs= os.path.join(temp_dir.name, "robot_description.urdf") + + raw_inputs, processed_inputs = parse_inputs_xml(parsed_args.mujoco_inputs) + decompose_dict, cameras_dict, modify_element_dict, lidar_dict = get_processed_mujoco_inputs(processed_inputs) + + # Add required mujoco tags to the starting URDF - xml_data = add_mujoco_info(urdf) + xml_data = add_mujoco_info(urdf,output_filepath) # get rid of collision data, assuming the visual data is much better resolution. # not sure if this is the best move... @@ -1302,8 +1316,6 @@ def main(args=None): lidar_dict, request_add_free_joint, ) - - shutil.copy2(f'{get_package_share_directory("mujoco_ros2_simulation")}/resources/scene.xml', output_filepath) if not parsed_args.save_only: publish_model_on_topic(output_filepath, temp_dir, args) diff --git a/test/launch/test_robot.launch.py b/test/launch/test_robot.launch.py index c6b7880..c1aa337 100644 --- a/test/launch/test_robot.launch.py +++ b/test/launch/test_robot.launch.py @@ -28,6 +28,7 @@ from launch_ros.actions import Node from launch_ros.parameter_descriptions import ParameterValue, ParameterFile from launch_ros.substitutions import FindPackageShare +from launch.actions import ExecuteProcess def generate_launch_description(): @@ -56,6 +57,39 @@ def generate_launch_description(): robot_description = {"robot_description": ParameterValue(value=robot_description_content, value_type=str)} + converter_command = [ + "python3", + PathJoinSubstitution([ + FindPackageShare("mujoco_ros2_simulation"), + "scripts", + "make_mjcf_from_robot_description.py", + ]), + # "--urdf", PathJoinSubstitution([ + # FindPackageShare("mujoco_ros2_simulation"), + # "test_resources", + # "test_robot.urdf", + # ]), + # "--m", PathJoinSubstitution([ + # FindPackageShare("mujoco_ros2_simulation"), + # "test_resources", + # "kangaroo_inputs.xml", + # ]), + # "--o", PathJoinSubstitution([ + # FindPackageShare("mujoco_ros2_simulation"), + # "test_resources", + # "output_mjcf_command", + # ]), + # "--s", + ] + + converter_process = ExecuteProcess( + cmd=converter_command, + name="make_mjcf_from_robot_description", + output="screen", + # True to see the output + emulate_tty=True, + ) + controller_parameters = ParameterFile( PathJoinSubstitution([FindPackageShare("mujoco_ros2_simulation"), "config", "controllers.yaml"]), ) @@ -107,5 +141,6 @@ def generate_launch_description(): control_node, spawn_joint_state_broadcaster, spawn_position_controller, + converter_process, ] ) From 9e77ada34b5b1f2943615e0d8e46ce899c64e522 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Thu, 27 Nov 2025 17:54:23 +0100 Subject: [PATCH 04/28] Added arguments --- scripts/make_mjcf_from_robot_description.py | 25 +++++++++++++++++---- 1 file changed, 21 insertions(+), 4 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index da5ef67..bbc3d5d 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -1212,7 +1212,7 @@ def add_urdf_free_joint(urdf): def main(args=None): parser = argparse.ArgumentParser(description="Convert a full URDF to MJCF for use in Mujoco") - parser.add_argument("-u", "--urdf", required=False, help="Optionally pass an existing URDF file") + parser.add_argument("-u", "--urdf", required=False, default=None, help="Optionally pass an existing URDF file") parser.add_argument( "-r", "--robot_description", required=False, help="Optionally pass the robot description string" ) @@ -1220,9 +1220,11 @@ def main(args=None): "-m", "--mujoco_inputs", required=False, + default=None, help="Optionally specify a defaults xml for default settings, actuators, options, and additional sensors", ) parser.add_argument("-o", "--output", default="mjcf_data", help="Generated output path") + parser.add_argument("-p", "--publish_topic", required=False, default= None, help="Optionally specify the topic to publish the MuJoCo model") parser.add_argument("-c", "--convert_stl_to_obj", action="store_true", help="If we should convert .stls to .objs") parser.add_argument( "-s", "--save_only", @@ -1235,6 +1237,12 @@ def main(args=None): action="store_true", help="Adds a free joint before the root link of the robot in the urdf before conversion", ) + parser.add_argument( + "-a" ,"--asset_dir", + required=False, + default=None, + help="Optional path to a existing asset folder" + ) # remove ros args to make argparser heppy args_without_filename = sys.argv[1:] @@ -1243,12 +1251,21 @@ def main(args=None): parsed_args = parser.parse_args(args_without_filename) + # Load URDF from file, string, or topic + urdf_path = None if parsed_args.urdf: urdf = get_xml_from_file(parsed_args.urdf) - elif parsed_args.robot_description: - urdf = parsed_args.robot_description + urdf_path = parsed_args.urdf else: - urdf = get_urdf_from_rsp(args) + if parsed_args.robot_description: + urdf = parsed_args.robot_description + else: + urdf = get_urdf_from_rsp(args) + # Create a tempfile and store the URDF + tmp = tempfile.NamedTemporaryFile() + with open(tmp.name, 'w') as f: + f.write(urdf) + urdf_path = tmp.name request_add_free_joint = parsed_args.add_free_joint From 836a4475e92db6f6406bcb21ad687b521c8f8f5f Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Thu, 27 Nov 2025 17:55:57 +0100 Subject: [PATCH 05/28] Improved arguments management in main --- scripts/make_mjcf_from_robot_description.py | 32 ++++++++++----------- 1 file changed, 15 insertions(+), 17 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index bbc3d5d..1892393 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -1271,44 +1271,38 @@ def main(args=None): convert_stl_to_obj = parsed_args.convert_stl_to_obj + # Determine the path of the output directory + temp_dir=None if parsed_args.save_only: output_filepath = os.path.join(parsed_args.output, "") print(f"Using destination directory: {output_filepath}") - else: + elif parsed_args.publish_topic: temp_dir = tempfile.TemporaryDirectory() output_filepath = os.path.join(temp_dir.name, "") print(f"Using temporary directory: {output_filepath}") + else: + raise ValueError("You must specify at least one of the following options: --publish_topic or --save-only.") # Add a free joint to the urdf if request_add_free_joint: urdf = add_urdf_free_joint(urdf) # If exists the mujoco_input.xml get the input tags from that file, otherwise try to get them from URDF - if not parsed_args.mujoco_inputs: - if parsed_args.urdf: - parsed_args.mujoco_inputs= parsed_args.urdf - elif not parsed_args.save_only: - print("writing URDF model from string to robot_description.urdf") - robot_description_filename = "robot_description.urdf" - with open(output_filepath + "robot_description.urdf", "w") as file: - # Remove extra newlines that minidom adds after each tag - urdf_data = "\n".join([line for line in urdf.splitlines() if line.strip()]) - file.write(urdf_data) - parsed_args.mujoco_inputs= os.path.join(temp_dir.name, "robot_description.urdf") + mujoco_inputs_file = parsed_args.mujoco_inputs or urdf_path - raw_inputs, processed_inputs = parse_inputs_xml(parsed_args.mujoco_inputs) + raw_inputs, processed_inputs = parse_inputs_xml(mujoco_inputs_file) decompose_dict, cameras_dict, modify_element_dict, lidar_dict = get_processed_mujoco_inputs(processed_inputs) # Add required mujoco tags to the starting URDF - xml_data = add_mujoco_info(urdf,output_filepath) + xml_data = add_mujoco_info(urdf, output_filepath) # get rid of collision data, assuming the visual data is much better resolution. # not sure if this is the best move... xml_data = remove_tag(xml_data, "collision") xml_data = replace_package_names(xml_data) - mesh_info_dict = extract_mesh_info(xml_data) + mesh_info_dict = extract_mesh_info(xml_data, parsed_args.asset_dir, decompose_dict) xml_data = convert_to_objs(mesh_info_dict, output_filepath, xml_data, convert_stl_to_obj, decompose_dict) print("writing data to robot_description_formatted.urdf") @@ -1334,8 +1328,12 @@ def main(args=None): request_add_free_joint, ) - if not parsed_args.save_only: - publish_model_on_topic(output_filepath, temp_dir, args) + if parsed_args.publish_topic: + publish_model_on_topic(parsed_args.publish_topic, output_filepath, args) + + if temp_dir is not None: + temp_dir.cleanup() + print("Temporary directory cleaned up.", flush=True) if __name__ == "__main__": From 35b481d3dc40fb664cf2eb1a1d7d8fa3402dbb1a Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Thu, 27 Nov 2025 17:56:46 +0100 Subject: [PATCH 06/28] changed to general topic name --- scripts/make_mjcf_from_robot_description.py | 10 +++++----- 1 file changed, 5 insertions(+), 5 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 1892393..8210b8c 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -1103,13 +1103,16 @@ def get_parent_chain(link, transform=PyKDL.Frame.Identity()): return results -def publish_model_on_topic(output_filepath, temp_dir, args=None): +def publish_model_on_topic(publish_topic, output_filepath, args=None): import rclpy from rclpy.node import Node from std_msgs.msg import String from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy + # Remove leading slash if present + publish_topic = publish_topic.lstrip('/') + # --- Node ROS2 for model publishing MJCF --- class MjcfPublisher(Node): def __init__(self, mjcf_path): @@ -1121,7 +1124,7 @@ def __init__(self, mjcf_path): durability=QoSDurabilityPolicy.TRANSIENT_LOCAL ) - self.publisher_ = self.create_publisher(String, 'mujoco_robot_description', qos_profile) + self.publisher_ = self.create_publisher(String, publish_topic, qos_profile) self.timer = self.create_timer(1.0, self.publish_mjcf) self.mjcf_path = mjcf_path @@ -1146,9 +1149,6 @@ def publish_mjcf(self): rclpy.shutdown() except Exception: pass - if temp_dir is not None: - temp_dir.cleanup() - print("Temporary directory cleaned up.", flush=True) def add_urdf_free_joint(urdf): """ From 2f8f6ae9674c44b6c878f44ececfa378ce10e074 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Thu, 27 Nov 2025 17:58:40 +0100 Subject: [PATCH 07/28] Added logic to reuse existing mesh --- scripts/make_mjcf_from_robot_description.py | 105 +++++++++++++++----- 1 file changed, 81 insertions(+), 24 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 8210b8c..515788a 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -47,7 +47,7 @@ def add_mujoco_info(raw_xml, output_filepath): mujoco_element = dom.createElement("mujoco") compiler_element = dom.createElement("compiler") - compiler_element.setAttribute("meshdir", output_filepath+"/assets") + compiler_element.setAttribute("meshdir", os.path.join(output_filepath, "assets")) compiler_element.setAttribute("balanceinertia", "true") compiler_element.setAttribute("discardvisual", "false") compiler_element.setAttribute("strippath", "false") @@ -78,7 +78,7 @@ def remove_tag(xml_string, tag_to_remove): return xmldoc.toprettyxml() -def extract_mesh_info(raw_xml): +def extract_mesh_info(raw_xml, asset_dir, decompose_dict): robot = URDF.from_xml_string(raw_xml) mesh_info_dict = {} @@ -107,6 +107,28 @@ def resolve_color(visual): uri = geom.filename # full URI stem = pathlib.Path(uri).stem # filename without extension + + # + is_decomposed = False + if asset_dir: + if stem in decompose_dict: + # decomposes mesh + candidate_path = f"{asset_dir}/decomposed/{stem}/{stem}.obj" + if os.path.exists(candidate_path): + new_uri = candidate_path + is_decomposed = True + else: + new_uri = uri + else: + # full mesh + candidate_path = f"{asset_dir}/full/{stem}.obj" + if os.path.exists(candidate_path): + new_uri = candidate_path + else: + new_uri = uri + else: + new_uri = uri + scale = " ".join(f"{v}" for v in geom.scale) if geom.scale else "1.0 1.0 1.0" rgba = resolve_color(vis) @@ -114,7 +136,8 @@ def resolve_color(visual): mesh_info_dict.setdefault( stem, { - "filename": uri, + "is_decomposed": is_decomposed , + "filename": new_uri, "scale": scale, "color": rgba, }, @@ -178,11 +201,14 @@ def convert_to_objs(mesh_info_dict, directory, xml_data, convert_stl_to_obj, dec # Import the .stl or .dae file if filename_ext.lower() == ".stl": + if filename_no_ext in decompose_dict and not convert_stl_to_obj: + raise ValueError( + "The --convert_stl_to_obj argument must be specified to decompose .stl mesh") if convert_stl_to_obj: bpy.ops.wm.stl_import(filepath=full_filepath) # bring in file color from urdf - new_mat = bpy.data.materials.new(name="new_mat_color") + new_mat = bpy.data.materials.new(name=f"material_{filename_no_ext}") new_mat.diffuse_color = mesh_item["color"] o = bpy.context.selected_objects[0] o.active_material = new_mat @@ -197,6 +223,18 @@ def convert_to_objs(mesh_info_dict, directory, xml_data, convert_stl_to_obj, dec xml_data = xml_data.replace(full_filepath, f"{assets_relative_filepath}.stl") pass elif filename_ext.lower() == ".obj": + # If import external .obj import also the .mtl since it is needed for the obj2mjcf + if convert_stl_to_obj: + mesh_dir = os.path.splitext(full_filepath)[0] + mtl_file = mesh_dir+ ".mtl" + if os.path.exists(mtl_file): + shutil.copy2(mtl_file, f"{directory}assets/{assets_relative_filepath}.mtl") + else: + print(f"Not found {full_filepath}, obj2mjcf requires it.") + if mesh_item["is_decomposed"]: + if os.path.exists(mesh_dir): + dst_base = f"{directory}assets/"+f"{DECOMPOSED_PATH_NAME}/{filename_no_ext}/{filename_no_ext}" + shutil.copytree(mesh_dir, dst_base, dirs_exist_ok=True) shutil.copy2(full_filepath, f"{directory}assets/{assets_relative_filepath}.obj") xml_data = xml_data.replace(full_filepath, f"{assets_relative_filepath}.obj") pass @@ -272,7 +310,7 @@ def set_up_axis_to_z_up(dae_file_path): return modified_data -def run_obj2mjcf(output_filepath, decompose_dict): +def run_obj2mjcf(output_filepath, decompose_dict, mesh_info_dict): # remove the folders in the asset directory so that we are clean to run obj2mjcf with os.scandir(f"{output_filepath}assets/{COMPOSED_PATH_NAME}") as entries: for entry in entries: @@ -281,14 +319,21 @@ def run_obj2mjcf(output_filepath, decompose_dict): # remove the folders in the asset directory so that we are clean to run obj2mjcf top_level_path = f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}" + backup_dir = tempfile.mkdtemp() for item in os.listdir(top_level_path): - first_level_path = os.path.join(top_level_path, item) - if os.path.isdir(first_level_path): - # Now check inside this first-level directory - for sub_item in os.listdir(first_level_path): - second_level_path = os.path.join(first_level_path, sub_item) - if os.path.isdir(second_level_path): - shutil.rmtree(second_level_path) + # keep this folder + if mesh_info_dict[item]["is_decomposed"]: + src = os.path.join(top_level_path, item) + dst = os.path.join(backup_dir, item) + shutil.copytree(src, dst) + else: + first_level_path = os.path.join(top_level_path, item) + if os.path.isdir(first_level_path): + # Now check inside this first-level directory + for sub_item in os.listdir(first_level_path): + second_level_path = os.path.join(first_level_path, sub_item) + if os.path.isdir(second_level_path): + shutil.rmtree(second_level_path) # run obj2mjcf to generate folders of processed objs cmd = ["obj2mjcf", "--obj-dir", f"{output_filepath}assets/{COMPOSED_PATH_NAME}", "--save-mjcf"] @@ -296,17 +341,29 @@ def run_obj2mjcf(output_filepath, decompose_dict): # run obj2mjcf to generate folders of processed objs with decompose option for decomposed components for mesh_name, threshold in decompose_dict.items(): - cmd = [ - "obj2mjcf", - "--obj-dir", - f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/{mesh_name}", - "--save-mjcf", - "--decompose", - "--coacd-args.threshold", - threshold, - ] - subprocess.run(cmd) - + if not mesh_info_dict[mesh_name]["is_decomposed"]: + cmd = [ + "obj2mjcf", + "--obj-dir", + f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/{mesh_name}", + "--save-mjcf", + "--decompose", + "--coacd-args.threshold", + threshold, + ] + subprocess.run(cmd) + + for item in os.listdir(backup_dir): + src = os.path.join(backup_dir, item) + dst = os.path.join(top_level_path, item) + + if os.path.exists(dst): + shutil.rmtree(dst) # remove if obj2mjcf recreated something + + shutil.copytree(src, dst) + + shutil.rmtree(backup_dir) + def update_obj_assets(dom, output_filepath, mesh_info_dict): # Find the element @@ -965,7 +1022,7 @@ def fix_mujoco_description( # shutil.copy2(full_filepath, destination_file) # Run conversions for mjcf - run_obj2mjcf(output_filepath, decompose_dict) + run_obj2mjcf(output_filepath, decompose_dict, mesh_info_dict) # Parse the DAE file dom = minidom.parse(full_filepath) From 3062d5fc827952bf99b8bafb223f0bcaad35bc47 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Fri, 28 Nov 2025 13:19:26 +0100 Subject: [PATCH 08/28] Optimized using previous generated .obj --- scripts/make_mjcf_from_robot_description.py | 71 +++++++++++++-------- 1 file changed, 43 insertions(+), 28 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 515788a..fd8ec61 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -223,20 +223,19 @@ def convert_to_objs(mesh_info_dict, directory, xml_data, convert_stl_to_obj, dec xml_data = xml_data.replace(full_filepath, f"{assets_relative_filepath}.stl") pass elif filename_ext.lower() == ".obj": - # If import external .obj import also the .mtl since it is needed for the obj2mjcf if convert_stl_to_obj: + # If import PREVIOUS GENERATED .obj mesh_dir = os.path.splitext(full_filepath)[0] - mtl_file = mesh_dir+ ".mtl" - if os.path.exists(mtl_file): - shutil.copy2(mtl_file, f"{directory}assets/{assets_relative_filepath}.mtl") - else: - print(f"Not found {full_filepath}, obj2mjcf requires it.") - if mesh_item["is_decomposed"]: - if os.path.exists(mesh_dir): - dst_base = f"{directory}assets/"+f"{DECOMPOSED_PATH_NAME}/{filename_no_ext}/{filename_no_ext}" - shutil.copytree(mesh_dir, dst_base, dirs_exist_ok=True) - shutil.copy2(full_filepath, f"{directory}assets/{assets_relative_filepath}.obj") - xml_data = xml_data.replace(full_filepath, f"{assets_relative_filepath}.obj") + if os.path.exists(mesh_dir): + if mesh_item["is_decomposed"]: + dst_base = f"{directory}assets/"+f"{DECOMPOSED_PATH_NAME}/{mesh_name}/{mesh_name}" + else: + dst_base = f"{directory}assets/"+f"{COMPOSED_PATH_NAME}/{mesh_name}" + shutil.copytree(mesh_dir, dst_base, dirs_exist_ok=True) + else: + #If import .obj files from URDF + shutil.copy2(full_filepath, f"{directory}assets/{assets_relative_filepath}.obj") + xml_data = xml_data.replace(full_filepath, f"{assets_relative_filepath}.obj") pass # objs are ok as is elif filename_ext.lower() == ".dae": @@ -311,28 +310,35 @@ def set_up_axis_to_z_up(dae_file_path): def run_obj2mjcf(output_filepath, decompose_dict, mesh_info_dict): + backup_dir = tempfile.mkdtemp() + backup_composed = os.path.join(backup_dir, "composed") + backup_decomposed = os.path.join(backup_dir, "decomposed") + os.makedirs(backup_composed, exist_ok=True) + os.makedirs(backup_decomposed, exist_ok=True) + # remove the folders in the asset directory so that we are clean to run obj2mjcf with os.scandir(f"{output_filepath}assets/{COMPOSED_PATH_NAME}") as entries: for entry in entries: if entry.is_dir(): + src = entry.path + dst = os.path.join(backup_composed, entry.name) + shutil.copytree(src, dst) shutil.rmtree(entry.path) # remove the folders in the asset directory so that we are clean to run obj2mjcf top_level_path = f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}" - backup_dir = tempfile.mkdtemp() for item in os.listdir(top_level_path): - # keep this folder - if mesh_info_dict[item]["is_decomposed"]: - src = os.path.join(top_level_path, item) - dst = os.path.join(backup_dir, item) - shutil.copytree(src, dst) - else: first_level_path = os.path.join(top_level_path, item) if os.path.isdir(first_level_path): # Now check inside this first-level directory for sub_item in os.listdir(first_level_path): second_level_path = os.path.join(first_level_path, sub_item) if os.path.isdir(second_level_path): + # keep this folder + if mesh_info_dict[item]["is_decomposed"]: + src = second_level_path + dst = os.path.join(backup_decomposed, item) + shutil.copytree(src, dst) shutil.rmtree(second_level_path) # run obj2mjcf to generate folders of processed objs @@ -353,13 +359,19 @@ def run_obj2mjcf(output_filepath, decompose_dict, mesh_info_dict): ] subprocess.run(cmd) - for item in os.listdir(backup_dir): - src = os.path.join(backup_dir, item) - dst = os.path.join(top_level_path, item) - + for item in os.listdir(backup_decomposed): + src = os.path.join(backup_decomposed, item) + dst = os.path.join(f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/{item}", item) if os.path.exists(dst): - shutil.rmtree(dst) # remove if obj2mjcf recreated something + shutil.rmtree(dst) + shutil.copytree(src, dst) + # Restore composed meshes (if needed) + for item in os.listdir(backup_composed): + src = os.path.join(backup_composed, item) + dst = os.path.join(f"{output_filepath}assets/{COMPOSED_PATH_NAME}", item) + if os.path.exists(dst): + shutil.rmtree(dst) shutil.copytree(src, dst) shutil.rmtree(backup_dir) @@ -1280,7 +1292,7 @@ def main(args=None): default=None, help="Optionally specify a defaults xml for default settings, actuators, options, and additional sensors", ) - parser.add_argument("-o", "--output", default="mjcf_data", help="Generated output path") + parser.add_argument("-o", "--output", default=None, help="Generated output path") parser.add_argument("-p", "--publish_topic", required=False, default= None, help="Optionally specify the topic to publish the MuJoCo model") parser.add_argument("-c", "--convert_stl_to_obj", action="store_true", help="If we should convert .stls to .objs") parser.add_argument( @@ -1331,14 +1343,17 @@ def main(args=None): # Determine the path of the output directory temp_dir=None if parsed_args.save_only: - output_filepath = os.path.join(parsed_args.output, "") - print(f"Using destination directory: {output_filepath}") + if parsed_args.output: + output_filepath = os.path.join(parsed_args.output, "") + print(f"Using destination directory: {output_filepath}") + else: + raise ValueError(" --save-only is missing the path of the output folder.") elif parsed_args.publish_topic: temp_dir = tempfile.TemporaryDirectory() output_filepath = os.path.join(temp_dir.name, "") print(f"Using temporary directory: {output_filepath}") else: - raise ValueError("You must specify at least one of the following options: --publish_topic or --save-only.") + raise ValueError("You must specify at least one of the following options: --publish_topic or --save-only with the path of the folder.") # Add a free joint to the urdf if request_add_free_joint: From cfd3d65ca863f6cf1ea6516f2514dd9fe2e70860 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Mon, 1 Dec 2025 10:01:56 +0100 Subject: [PATCH 09/28] Removed unnecessary variable --- scripts/make_mjcf_from_robot_description.py | 17 ++++------------- 1 file changed, 4 insertions(+), 13 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index fd8ec61..2e3a290 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -108,15 +108,12 @@ def resolve_color(visual): uri = geom.filename # full URI stem = pathlib.Path(uri).stem # filename without extension - # - is_decomposed = False if asset_dir: if stem in decompose_dict: # decomposes mesh candidate_path = f"{asset_dir}/decomposed/{stem}/{stem}.obj" if os.path.exists(candidate_path): new_uri = candidate_path - is_decomposed = True else: new_uri = uri else: @@ -136,7 +133,6 @@ def resolve_color(visual): mesh_info_dict.setdefault( stem, { - "is_decomposed": is_decomposed , "filename": new_uri, "scale": scale, "color": rgba, @@ -227,10 +223,7 @@ def convert_to_objs(mesh_info_dict, directory, xml_data, convert_stl_to_obj, dec # If import PREVIOUS GENERATED .obj mesh_dir = os.path.splitext(full_filepath)[0] if os.path.exists(mesh_dir): - if mesh_item["is_decomposed"]: - dst_base = f"{directory}assets/"+f"{DECOMPOSED_PATH_NAME}/{mesh_name}/{mesh_name}" - else: - dst_base = f"{directory}assets/"+f"{COMPOSED_PATH_NAME}/{mesh_name}" + dst_base = f"{directory}assets/{assets_relative_filepath}" shutil.copytree(mesh_dir, dst_base, dirs_exist_ok=True) else: #If import .obj files from URDF @@ -335,10 +328,9 @@ def run_obj2mjcf(output_filepath, decompose_dict, mesh_info_dict): second_level_path = os.path.join(first_level_path, sub_item) if os.path.isdir(second_level_path): # keep this folder - if mesh_info_dict[item]["is_decomposed"]: - src = second_level_path - dst = os.path.join(backup_decomposed, item) - shutil.copytree(src, dst) + src = second_level_path + dst = os.path.join(backup_decomposed, item) + shutil.copytree(src, dst) shutil.rmtree(second_level_path) # run obj2mjcf to generate folders of processed objs @@ -347,7 +339,6 @@ def run_obj2mjcf(output_filepath, decompose_dict, mesh_info_dict): # run obj2mjcf to generate folders of processed objs with decompose option for decomposed components for mesh_name, threshold in decompose_dict.items(): - if not mesh_info_dict[mesh_name]["is_decomposed"]: cmd = [ "obj2mjcf", "--obj-dir", From c4787239b92b61c6bd65bb4938c1774819555c6e Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Mon, 1 Dec 2025 14:39:52 +0100 Subject: [PATCH 10/28] Using existing global variable --- scripts/make_mjcf_from_robot_description.py | 22 ++++++++++----------- 1 file changed, 11 insertions(+), 11 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 2e3a290..ecf0803 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -108,17 +108,15 @@ def resolve_color(visual): uri = geom.filename # full URI stem = pathlib.Path(uri).stem # filename without extension + # Path of the existing .stl or .obj file, if it already exists if asset_dir: if stem in decompose_dict: - # decomposes mesh - candidate_path = f"{asset_dir}/decomposed/{stem}/{stem}.obj" - if os.path.exists(candidate_path): - new_uri = candidate_path - else: - new_uri = uri + # decomposed mesh + candidate_path = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/{stem}/{stem}.obj" else: # full mesh - candidate_path = f"{asset_dir}/full/{stem}.obj" + candidate_path = f"{asset_dir}/{COMPOSED_PATH_NAME}/{stem}.obj" + print(f"Using existing obj for {stem}") if os.path.exists(candidate_path): new_uri = candidate_path else: @@ -302,10 +300,12 @@ def set_up_axis_to_z_up(dae_file_path): return modified_data -def run_obj2mjcf(output_filepath, decompose_dict, mesh_info_dict): +def run_obj2mjcf(output_filepath, decompose_dict): + + # Create temporal folder uused for the backup existing asset backup_dir = tempfile.mkdtemp() - backup_composed = os.path.join(backup_dir, "composed") - backup_decomposed = os.path.join(backup_dir, "decomposed") + backup_composed = os.path.join(backup_dir, COMPOSED_PATH_NAME) + backup_decomposed = os.path.join(backup_dir, DECOMPOSED_PATH_NAME) os.makedirs(backup_composed, exist_ok=True) os.makedirs(backup_decomposed, exist_ok=True) @@ -1025,7 +1025,7 @@ def fix_mujoco_description( # shutil.copy2(full_filepath, destination_file) # Run conversions for mjcf - run_obj2mjcf(output_filepath, decompose_dict, mesh_info_dict) + run_obj2mjcf(output_filepath, decompose_dict) # Parse the DAE file dom = minidom.parse(full_filepath) From 469dda5e717ce6a87dc0ff43c671ac2856b52677 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Mon, 1 Dec 2025 14:40:10 +0100 Subject: [PATCH 11/28] added arg error management --- scripts/make_mjcf_from_robot_description.py | 3 +++ 1 file changed, 3 insertions(+) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index ecf0803..928b9b9 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -1311,6 +1311,9 @@ def main(args=None): parsed_args = parser.parse_args(args_without_filename) + if parsed_args.asset_dir is not None and not parsed_args.convert_stl_to_obj: + parser.error("The argument --asset_dir needs --convert_stl_to_obj to be specified") + # Load URDF from file, string, or topic urdf_path = None if parsed_args.urdf: From c1396a4f458aa7c0478359bc5cca04009a374a0a Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Mon, 1 Dec 2025 16:09:17 +0100 Subject: [PATCH 12/28] Managed reusing second level generated folder --- scripts/make_mjcf_from_robot_description.py | 29 ++++++++++++--------- 1 file changed, 16 insertions(+), 13 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 928b9b9..69295c5 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -111,16 +111,15 @@ def resolve_color(visual): # Path of the existing .stl or .obj file, if it already exists if asset_dir: if stem in decompose_dict: - # decomposed mesh - candidate_path = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/{stem}/{stem}.obj" + candidate_path = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/{stem}/{stem}/{stem}.obj" else: - # full mesh - candidate_path = f"{asset_dir}/{COMPOSED_PATH_NAME}/{stem}.obj" + candidate_path = f"{asset_dir}/{COMPOSED_PATH_NAME}/{stem}/{stem}.obj" + + if os.path.exists(candidate_path): print(f"Using existing obj for {stem}") - if os.path.exists(candidate_path): - new_uri = candidate_path - else: - new_uri = uri + new_uri = candidate_path + else: + new_uri = uri else: new_uri = uri @@ -217,12 +216,16 @@ def convert_to_objs(mesh_info_dict, directory, xml_data, convert_stl_to_obj, dec xml_data = xml_data.replace(full_filepath, f"{assets_relative_filepath}.stl") pass elif filename_ext.lower() == ".obj": - if convert_stl_to_obj: + # mesh_dir = os.path.splitext(full_filepath)[0] + mesh_dir = os.path.dirname(os.path.splitext(full_filepath)[0]) + if convert_stl_to_obj and os.path.exists(mesh_dir): # If import PREVIOUS GENERATED .obj - mesh_dir = os.path.splitext(full_filepath)[0] - if os.path.exists(mesh_dir): - dst_base = f"{directory}assets/{assets_relative_filepath}" - shutil.copytree(mesh_dir, dst_base, dirs_exist_ok=True) + if filename_no_ext in decompose_dict: + dst_base= f"{directory}assets/{DECOMPOSED_PATH_NAME}/{filename_no_ext}/{filename_no_ext}/" + else: + dst_base = f"{directory}assets/{COMPOSED_PATH_NAME}/{filename_no_ext}" + shutil.copytree(mesh_dir, dst_base, dirs_exist_ok=True) + xml_data = xml_data.replace(full_filepath, f"{assets_relative_filepath}.obj") else: #If import .obj files from URDF shutil.copy2(full_filepath, f"{directory}assets/{assets_relative_filepath}.obj") From 8dc4c79798ade1c9decf1a5266e159676fd9fa68 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Mon, 1 Dec 2025 17:12:48 +0100 Subject: [PATCH 13/28] removed temporal folder --- scripts/make_mjcf_from_robot_description.py | 60 +++++++-------------- 1 file changed, 18 insertions(+), 42 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 69295c5..2d59b62 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -216,20 +216,10 @@ def convert_to_objs(mesh_info_dict, directory, xml_data, convert_stl_to_obj, dec xml_data = xml_data.replace(full_filepath, f"{assets_relative_filepath}.stl") pass elif filename_ext.lower() == ".obj": - # mesh_dir = os.path.splitext(full_filepath)[0] mesh_dir = os.path.dirname(os.path.splitext(full_filepath)[0]) - if convert_stl_to_obj and os.path.exists(mesh_dir): - # If import PREVIOUS GENERATED .obj - if filename_no_ext in decompose_dict: - dst_base= f"{directory}assets/{DECOMPOSED_PATH_NAME}/{filename_no_ext}/{filename_no_ext}/" - else: - dst_base = f"{directory}assets/{COMPOSED_PATH_NAME}/{filename_no_ext}" - shutil.copytree(mesh_dir, dst_base, dirs_exist_ok=True) - xml_data = xml_data.replace(full_filepath, f"{assets_relative_filepath}.obj") - else: + if not (convert_stl_to_obj and os.path.exists(mesh_dir)): #If import .obj files from URDF shutil.copy2(full_filepath, f"{directory}assets/{assets_relative_filepath}.obj") - xml_data = xml_data.replace(full_filepath, f"{assets_relative_filepath}.obj") pass # objs are ok as is elif filename_ext.lower() == ".dae": @@ -304,21 +294,11 @@ def set_up_axis_to_z_up(dae_file_path): def run_obj2mjcf(output_filepath, decompose_dict): - - # Create temporal folder uused for the backup existing asset - backup_dir = tempfile.mkdtemp() - backup_composed = os.path.join(backup_dir, COMPOSED_PATH_NAME) - backup_decomposed = os.path.join(backup_dir, DECOMPOSED_PATH_NAME) - os.makedirs(backup_composed, exist_ok=True) - os.makedirs(backup_decomposed, exist_ok=True) # remove the folders in the asset directory so that we are clean to run obj2mjcf with os.scandir(f"{output_filepath}assets/{COMPOSED_PATH_NAME}") as entries: for entry in entries: if entry.is_dir(): - src = entry.path - dst = os.path.join(backup_composed, entry.name) - shutil.copytree(src, dst) shutil.rmtree(entry.path) # remove the folders in the asset directory so that we are clean to run obj2mjcf @@ -330,10 +310,6 @@ def run_obj2mjcf(output_filepath, decompose_dict): for sub_item in os.listdir(first_level_path): second_level_path = os.path.join(first_level_path, sub_item) if os.path.isdir(second_level_path): - # keep this folder - src = second_level_path - dst = os.path.join(backup_decomposed, item) - shutil.copytree(src, dst) shutil.rmtree(second_level_path) # run obj2mjcf to generate folders of processed objs @@ -352,23 +328,6 @@ def run_obj2mjcf(output_filepath, decompose_dict): threshold, ] subprocess.run(cmd) - - for item in os.listdir(backup_decomposed): - src = os.path.join(backup_decomposed, item) - dst = os.path.join(f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/{item}", item) - if os.path.exists(dst): - shutil.rmtree(dst) - shutil.copytree(src, dst) - - # Restore composed meshes (if needed) - for item in os.listdir(backup_composed): - src = os.path.join(backup_composed, item) - dst = os.path.join(f"{output_filepath}assets/{COMPOSED_PATH_NAME}", item) - if os.path.exists(dst): - shutil.rmtree(dst) - shutil.copytree(src, dst) - - shutil.rmtree(backup_dir) def update_obj_assets(dom, output_filepath, mesh_info_dict): @@ -1030,6 +989,23 @@ def fix_mujoco_description( # Run conversions for mjcf run_obj2mjcf(output_filepath, decompose_dict) + #Copy existing folders to the final directory + for mesh_name in mesh_info_dict: + mesh_item = mesh_info_dict[mesh_name] + filename = os.path.basename(mesh_item["filename"]) + filename_no_ext = os.path.splitext(filename)[0] + filename_ext = os.path.splitext(filename)[1] + full = mesh_item["filename"] + if filename_ext.lower() == ".obj": + mesh_dir = os.path.dirname(os.path.splitext(full)[0]) + if os.path.exists(mesh_dir): + # # If import PREVIOUS GENERATED .obj + if filename_no_ext in decompose_dict: + dst_base= f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/{filename_no_ext}/{filename_no_ext}/" + else: + dst_base = f"{output_filepath}assets/{COMPOSED_PATH_NAME}/{filename_no_ext}" + shutil.copytree(mesh_dir, dst_base, dirs_exist_ok=True) + # Parse the DAE file dom = minidom.parse(full_filepath) From 6b3e0f26950c39f75973c0b21511c4184019678b Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Tue, 2 Dec 2025 10:59:05 +0100 Subject: [PATCH 14/28] Managed threshold check for decomposed meshes --- scripts/make_mjcf_from_robot_description.py | 44 +++++++++++++++++---- 1 file changed, 37 insertions(+), 7 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 2d59b62..8448b1f 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -28,6 +28,8 @@ import tempfile import PyKDL import sys +import json +import math import numpy as np @@ -111,15 +113,30 @@ def resolve_color(visual): # Path of the existing .stl or .obj file, if it already exists if asset_dir: if stem in decompose_dict: - candidate_path = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/{stem}/{stem}/{stem}.obj" + decomposed_obj_path = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/{stem}/{stem}/{stem}.obj" + settings_path = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/decomposition_thresholds.json" + + if os.path.exists(decomposed_obj_path) and os.path.exists(settings_path): + with open(settings_path, "r") as f: + data = json.load(f) + used_threshold = float(data.get(f"{stem}")) + # Use existing decomposed object only if it has the same thresold, otherwise regenerate it. + if math.isclose(used_threshold, float(decompose_dict[stem]), rel_tol=1e-9): + print(f"Using existing decomposed obj for {stem} with matching threshold {used_threshold}") + new_uri = decomposed_obj_path + else: + print(f"Existing decomposed obj for {stem} has different threshold {used_threshold} with respect to {decompose_dict[stem]}. Regenerating...") + new_uri = uri + else: + new_uri = uri else: - candidate_path = f"{asset_dir}/{COMPOSED_PATH_NAME}/{stem}/{stem}.obj" + composed_obj_path = f"{asset_dir}/{COMPOSED_PATH_NAME}/{stem}/{stem}.obj" - if os.path.exists(candidate_path): - print(f"Using existing obj for {stem}") - new_uri = candidate_path - else: - new_uri = uri + if os.path.exists(composed_obj_path): + print(f"Using existing obj for {stem}") + new_uri = composed_obj_path + else: + new_uri = uri else: new_uri = uri @@ -316,6 +333,14 @@ def run_obj2mjcf(output_filepath, decompose_dict): cmd = ["obj2mjcf", "--obj-dir", f"{output_filepath}assets/{COMPOSED_PATH_NAME}", "--save-mjcf"] subprocess.run(cmd) + thresholds_file = os.path.join(f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}", "decomposition_thresholds.json") + + if os.path.exists(thresholds_file): + with open(thresholds_file, "r") as f: + thresholds_data = json.load(f) + else: + thresholds_data = {} + # run obj2mjcf to generate folders of processed objs with decompose option for decomposed components for mesh_name, threshold in decompose_dict.items(): cmd = [ @@ -328,6 +353,11 @@ def run_obj2mjcf(output_filepath, decompose_dict): threshold, ] subprocess.run(cmd) + + thresholds_data[mesh_name] = float(threshold) + + with open(thresholds_file, "w") as f: + json.dump(thresholds_data, f, indent=4) def update_obj_assets(dom, output_filepath, mesh_info_dict): From 373fa1fc55ad632f033000edaa93a1c3c2ed43b8 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Thu, 4 Dec 2025 17:39:48 +0100 Subject: [PATCH 15/28] improved args and functions in main --- scripts/make_mjcf_from_robot_description.py | 52 +++++++++++---------- 1 file changed, 27 insertions(+), 25 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 8448b1f..7e1d8af 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -1292,13 +1292,13 @@ def main(args=None): default=None, help="Optionally specify a defaults xml for default settings, actuators, options, and additional sensors", ) - parser.add_argument("-o", "--output", default=None, help="Generated output path") + parser.add_argument("-o", "--output", default="mjcf_data", help="Generated output path") parser.add_argument("-p", "--publish_topic", required=False, default= None, help="Optionally specify the topic to publish the MuJoCo model") parser.add_argument("-c", "--convert_stl_to_obj", action="store_true", help="If we should convert .stls to .objs") parser.add_argument( "-s", "--save_only", action="store_true", - help="Save the generated files permanently. Without this flag a temporary directory is used." + help="Save files permanently on disk; without this flag, files go to a temporary directory" ) parser.add_argument( "-f", @@ -1310,7 +1310,13 @@ def main(args=None): "-a" ,"--asset_dir", required=False, default=None, - help="Optional path to a existing asset folder" + help="Optionally pass an existing folder with pre-generated OBJ meshes." + ) + parser.add_argument( + "--scene", + required=False, + default=None, + help="OOptionally pass an existing xml for the scene" ) # remove ros args to make argparser heppy @@ -1320,9 +1326,6 @@ def main(args=None): parsed_args = parser.parse_args(args_without_filename) - if parsed_args.asset_dir is not None and not parsed_args.convert_stl_to_obj: - parser.error("The argument --asset_dir needs --convert_stl_to_obj to be specified") - # Load URDF from file, string, or topic urdf_path = None if parsed_args.urdf: @@ -1343,34 +1346,35 @@ def main(args=None): convert_stl_to_obj = parsed_args.convert_stl_to_obj + # Use provided MuJoCo input or scene XML files if given; otherwise use the URDF. + mujoco_inputs_file = parsed_args.mujoco_inputs or urdf_path + mujoco_scene_file = parsed_args.scene or urdf_path + + raw_inputs, processed_inputs = parse_inputs_xml(mujoco_inputs_file) + scene_inputs = parse_scene_xml(mujoco_scene_file) + decompose_dict, cameras_dict, modify_element_dict, lidar_dict = get_processed_mujoco_inputs(processed_inputs) + + output_filepath = parsed_args.output + if not os.path.isabs(parsed_args.output) and parsed_args.publish_topic: + output_filepath = os.path.join(os.getcwd(), parsed_args.output) # Determine the path of the output directory - temp_dir=None if parsed_args.save_only: - if parsed_args.output: - output_filepath = os.path.join(parsed_args.output, "") - print(f"Using destination directory: {output_filepath}") - else: - raise ValueError(" --save-only is missing the path of the output folder.") + # Grab the output directory and ensure it ends with '/' + output_filepath = os.path.join(output_filepath, "") elif parsed_args.publish_topic: temp_dir = tempfile.TemporaryDirectory() output_filepath = os.path.join(temp_dir.name, "") - print(f"Using temporary directory: {output_filepath}") else: raise ValueError("You must specify at least one of the following options: --publish_topic or --save-only with the path of the folder.") # Add a free joint to the urdf if request_add_free_joint: urdf = add_urdf_free_joint(urdf) - - # If exists the mujoco_input.xml get the input tags from that file, otherwise try to get them from URDF - mujoco_inputs_file = parsed_args.mujoco_inputs or urdf_path - raw_inputs, processed_inputs = parse_inputs_xml(mujoco_inputs_file) - decompose_dict, cameras_dict, modify_element_dict, lidar_dict = get_processed_mujoco_inputs(processed_inputs) - + print(f"Using destination directory: {output_filepath}") # Add required mujoco tags to the starting URDF - xml_data = add_mujoco_info(urdf, output_filepath) + xml_data = add_mujoco_info(urdf, output_filepath, parsed_args.publish_topic) # get rid of collision data, assuming the visual data is much better resolution. # not sure if this is the best move... @@ -1395,6 +1399,7 @@ def main(args=None): output_filepath, mesh_info_dict, raw_inputs, + scene_inputs, urdf, decompose_dict, cameras_dict, @@ -1402,13 +1407,10 @@ def main(args=None): lidar_dict, request_add_free_joint, ) - + + # Publish the MuJoCo model to the specified topic if provided if parsed_args.publish_topic: publish_model_on_topic(parsed_args.publish_topic, output_filepath, args) - - if temp_dir is not None: - temp_dir.cleanup() - print("Temporary directory cleaned up.", flush=True) if __name__ == "__main__": From afdba2ef1c11580bebb1ec7cdc61bf1684612913 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Thu, 4 Dec 2025 17:41:47 +0100 Subject: [PATCH 16/28] fixed asset mesh path --- scripts/make_mjcf_from_robot_description.py | 11 +++++++++-- 1 file changed, 9 insertions(+), 2 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 7e1d8af..33b99a8 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -44,12 +44,19 @@ COMPOSED_PATH_NAME = "full" -def add_mujoco_info(raw_xml, output_filepath): +def add_mujoco_info(raw_xml, output_filepath, publish_topic): dom = minidom.parseString(raw_xml) mujoco_element = dom.createElement("mujoco") compiler_element = dom.createElement("compiler") - compiler_element.setAttribute("meshdir", os.path.join(output_filepath, "assets")) + + # Use relative path for fixed directory otherwise use absolute path + if not publish_topic: + meshdir = "assets" + else: + meshdir = os.path.join(output_filepath, "assets") + + compiler_element.setAttribute("meshdir", meshdir) compiler_element.setAttribute("balanceinertia", "true") compiler_element.setAttribute("discardvisual", "false") compiler_element.setAttribute("strippath", "false") From 01593e67732f9220db9c2afeae60d5c531733305 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Thu, 4 Dec 2025 17:42:48 +0100 Subject: [PATCH 17/28] improved identation --- scripts/make_mjcf_from_robot_description.py | 39 ++++++++++----------- 1 file changed, 19 insertions(+), 20 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 33b99a8..8fe0d25 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -318,7 +318,6 @@ def set_up_axis_to_z_up(dae_file_path): def run_obj2mjcf(output_filepath, decompose_dict): - # remove the folders in the asset directory so that we are clean to run obj2mjcf with os.scandir(f"{output_filepath}assets/{COMPOSED_PATH_NAME}") as entries: for entry in entries: @@ -328,13 +327,13 @@ def run_obj2mjcf(output_filepath, decompose_dict): # remove the folders in the asset directory so that we are clean to run obj2mjcf top_level_path = f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}" for item in os.listdir(top_level_path): - first_level_path = os.path.join(top_level_path, item) - if os.path.isdir(first_level_path): - # Now check inside this first-level directory - for sub_item in os.listdir(first_level_path): - second_level_path = os.path.join(first_level_path, sub_item) - if os.path.isdir(second_level_path): - shutil.rmtree(second_level_path) + first_level_path = os.path.join(top_level_path, item) + if os.path.isdir(first_level_path): + # Now check inside this first-level directory + for sub_item in os.listdir(first_level_path): + second_level_path = os.path.join(first_level_path, sub_item) + if os.path.isdir(second_level_path): + shutil.rmtree(second_level_path) # run obj2mjcf to generate folders of processed objs cmd = ["obj2mjcf", "--obj-dir", f"{output_filepath}assets/{COMPOSED_PATH_NAME}", "--save-mjcf"] @@ -350,18 +349,18 @@ def run_obj2mjcf(output_filepath, decompose_dict): # run obj2mjcf to generate folders of processed objs with decompose option for decomposed components for mesh_name, threshold in decompose_dict.items(): - cmd = [ - "obj2mjcf", - "--obj-dir", - f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/{mesh_name}", - "--save-mjcf", - "--decompose", - "--coacd-args.threshold", - threshold, - ] - subprocess.run(cmd) - - thresholds_data[mesh_name] = float(threshold) + cmd = [ + "obj2mjcf", + "--obj-dir", + f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/{mesh_name}", + "--save-mjcf", + "--decompose", + "--coacd-args.threshold", + threshold, + ] + subprocess.run(cmd) + + thresholds_data[mesh_name] = float(threshold) with open(thresholds_file, "w") as f: json.dump(thresholds_data, f, indent=4) From 0bf8cce38f18aa85276abcbf31c6a39143bc35b5 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Thu, 4 Dec 2025 17:44:36 +0100 Subject: [PATCH 18/28] added scene --- scripts/make_mjcf_from_robot_description.py | 67 +++++++++++++++++++-- 1 file changed, 61 insertions(+), 6 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 8fe0d25..bf0844b 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -518,14 +518,20 @@ def update_non_obj_assets(dom, output_filepath): return dom -def add_mujoco_inputs(dom, raw_inputs): +def add_mujoco_inputs(dom, raw_inputs, scene_inputs): """ - Copies all elements under the "raw_inputs" xml tag directly in the provided dom. - This is useful for adding things like actuators, options, or defaults. But any tag that + Copies all elements under the "raw_inputs" and "scene_inputs" XML tags directly in the provided dom. + This is useful for adding things like actuators, options, or defaults or scene-specific elements. But any tag that is supported in the MJCF can be added here. """ root = dom.documentElement + if scene_inputs: + for child in scene_inputs.childNodes: + if child.nodeType == child.ELEMENT_NODE: + imported_node = dom.importNode(child, True) + root.appendChild(imported_node) + if raw_inputs: for child in raw_inputs.childNodes: if child.nodeType == child.ELEMENT_NODE: @@ -728,7 +734,54 @@ def parse_inputs_xml(filename=None): return raw_inputs, processed_inputs -def fix_free_joint(dom, urdf, joint_name="floating_base_joint"): +def parse_scene_xml(filename=None): + """ + This script can accept the scene in the form of an xml file. This allows users to inject this data + into the MJCF that is not necessarily included in the URDF. + """ + + if not filename: + return None + + print(f"Parsing mujoco scene from: {filename}") + + dom = minidom.parse(filename) + root = dom.documentElement + + scene_inputs = None + + if root.tagName == "mujoco": + # The file itself is a standalone xml + scene_inputs = root + return scene_inputs + + elif root.tagName == "robot": + # The file is a URDF + mujoco_inputs_node = None + + # find + for child in root.childNodes: + if child.nodeType == child.ELEMENT_NODE and child.tagName == "mujoco_inputs": + mujoco_inputs_node = child + break + + if mujoco_inputs_node is None: + # URDF without mujoco_inputs is allowed + return None + + # parse children of + for child in mujoco_inputs_node.childNodes: + if child.nodeType != child.ELEMENT_NODE: + continue + if child.tagName == "scene": + scene_inputs = child + else: + raise ValueError( f"Root tag in file must be either 'mujoco_inputs' (standalone XML) or 'robot' (URDF), not '{root.tagName}'") + + return scene_inputs + + +def add_free_joint(dom, urdf, joint_name="floating_base_joint"): """ This change is on the mjcf side, and replaces the "free" joint type with a freejoint tag. This is a special item which explicitly sets all stiffness/damping to 0. @@ -1007,6 +1060,7 @@ def fix_mujoco_description( output_filepath, mesh_info_dict, raw_inputs, + scene_inputs, urdf, decompose_dict, cameras_dict, @@ -1046,14 +1100,14 @@ def fix_mujoco_description( dom = minidom.parse(full_filepath) if request_add_free_joint: - dom = fix_free_joint(dom, urdf) + dom = add_free_joint(dom, urdf) # Update and add the new fixed assets dom = update_obj_assets(dom, output_filepath, mesh_info_dict) dom = update_non_obj_assets(dom, output_filepath) # Add the mujoco input elements - dom = add_mujoco_inputs(dom, raw_inputs) + dom = add_mujoco_inputs(dom, raw_inputs, scene_inputs) # Add links as sites dom = add_links_as_sites(urdf, dom, request_add_free_joint) @@ -1178,6 +1232,7 @@ def get_parent_chain(link, transform=PyKDL.Frame.Identity()): return results + def publish_model_on_topic(publish_topic, output_filepath, args=None): import rclpy From 7bf23e3b52347d4b74ce449f7a09574bb06fcb21 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Thu, 4 Dec 2025 17:45:40 +0100 Subject: [PATCH 19/28] improved logic and structure for obj meshes --- scripts/make_mjcf_from_robot_description.py | 98 ++++++++++++--------- 1 file changed, 56 insertions(+), 42 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index bf0844b..8174c46 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -116,36 +116,38 @@ def resolve_color(visual): uri = geom.filename # full URI stem = pathlib.Path(uri).stem # filename without extension + + # Select the mesh file: use a pre-generated OBJ if available and valid; otherwise use the original + is_pre_generated = False + new_uri = uri # default fallback - # Path of the existing .stl or .obj file, if it already exists if asset_dir: if stem in decompose_dict: - decomposed_obj_path = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/{stem}/{stem}/{stem}.obj" - settings_path = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/decomposition_thresholds.json" - - if os.path.exists(decomposed_obj_path) and os.path.exists(settings_path): - with open(settings_path, "r") as f: - data = json.load(f) - used_threshold = float(data.get(f"{stem}")) + # Decomposed mesh: check if a pre-generated OBJ exists and threshold matches + mesh_file = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/{stem}/{stem}/{stem}.obj" + settings_file = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/decomposition_thresholds.json" + + if os.path.exists(mesh_file) and os.path.exists(settings_file): + try: + with open(settings_file, "r") as f: + data = json.load(f) + used_threshold = float(data.get(f"{stem}")) + except (FileNotFoundError, PermissionError, json.JSONDecodeError) as e: + print(f"Warning: could not read thresholds for {stem}: {e}") + used_threshold = None # Use existing decomposed object only if it has the same thresold, otherwise regenerate it. - if math.isclose(used_threshold, float(decompose_dict[stem]), rel_tol=1e-9): - print(f"Using existing decomposed obj for {stem} with matching threshold {used_threshold}") - new_uri = decomposed_obj_path + if used_threshold is not None and math.isclose(used_threshold, float(decompose_dict[stem]), rel_tol=1e-9): + new_uri = mesh_file + is_pre_generated = True else: - print(f"Existing decomposed obj for {stem} has different threshold {used_threshold} with respect to {decompose_dict[stem]}. Regenerating...") - new_uri = uri - else: - new_uri = uri + print(f"Existing decomposed obj for {stem} has different threshold {used_threshold} than required {decompose_dict[stem]}. Regenerating...") else: - composed_obj_path = f"{asset_dir}/{COMPOSED_PATH_NAME}/{stem}/{stem}.obj" + # Composed mesh: check if a pre-generated OBJ exists + mesh_file = f"{asset_dir}/{COMPOSED_PATH_NAME}/{stem}/{stem}.obj" - if os.path.exists(composed_obj_path): - print(f"Using existing obj for {stem}") - new_uri = composed_obj_path - else: - new_uri = uri - else: - new_uri = uri + if os.path.exists(mesh_file): + new_uri = mesh_file + is_pre_generated = True scale = " ".join(f"{v}" for v in geom.scale) if geom.scale else "1.0 1.0 1.0" rgba = resolve_color(vis) @@ -154,6 +156,7 @@ def resolve_color(visual): mesh_info_dict.setdefault( stem, { + "is_pre_generated": is_pre_generated, "filename": new_uri, "scale": scale, "color": rgba, @@ -240,10 +243,14 @@ def convert_to_objs(mesh_info_dict, directory, xml_data, convert_stl_to_obj, dec xml_data = xml_data.replace(full_filepath, f"{assets_relative_filepath}.stl") pass elif filename_ext.lower() == ".obj": - mesh_dir = os.path.dirname(os.path.splitext(full_filepath)[0]) - if not (convert_stl_to_obj and os.path.exists(mesh_dir)): - #If import .obj files from URDF + #If import .obj files from URDF + if not mesh_item["is_pre_generated"]: shutil.copy2(full_filepath, f"{directory}assets/{assets_relative_filepath}.obj") + # If the .obj depends on a mtl should copy also this + old_directory = os.path.dirname(mesh_item["filename"]) + if(os.path.exists(old_directory+"/material.mtl")): + final_path=os.path.dirname(assets_relative_filepath) + shutil.copy2(old_directory+"/material.mtl", f"{directory}assets/{final_path}/material.mtl") pass # objs are ok as is elif filename_ext.lower() == ".dae": @@ -1056,6 +1063,27 @@ def add_modifiers(dom, modify_element_dict): return dom +def copy_pre_generated_meshes(output_filepath, mesh_info_dict, decompose_dict): + """ + Copies pre-generated mesh folders into the final MJCF assets structure. + """ + + for mesh_name in mesh_info_dict: + mesh_item = mesh_info_dict[mesh_name] + filename = os.path.basename(mesh_item["filename"]) + filename_no_ext = os.path.splitext(filename)[0] + full_path = mesh_item["filename"] + mesh_dir = os.path.dirname(os.path.splitext(full_path)[0]) + + if mesh_item ["is_pre_generated"]: + if filename_no_ext in decompose_dict: + dst_base= f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/{filename_no_ext}/{filename_no_ext}/" + else: + dst_base = f"{output_filepath}assets/{COMPOSED_PATH_NAME}/{filename_no_ext}" + + shutil.copytree(mesh_dir, dst_base, dirs_exist_ok=True) + + def fix_mujoco_description( output_filepath, mesh_info_dict, @@ -1079,22 +1107,8 @@ def fix_mujoco_description( # Run conversions for mjcf run_obj2mjcf(output_filepath, decompose_dict) - #Copy existing folders to the final directory - for mesh_name in mesh_info_dict: - mesh_item = mesh_info_dict[mesh_name] - filename = os.path.basename(mesh_item["filename"]) - filename_no_ext = os.path.splitext(filename)[0] - filename_ext = os.path.splitext(filename)[1] - full = mesh_item["filename"] - if filename_ext.lower() == ".obj": - mesh_dir = os.path.dirname(os.path.splitext(full)[0]) - if os.path.exists(mesh_dir): - # # If import PREVIOUS GENERATED .obj - if filename_no_ext in decompose_dict: - dst_base= f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/{filename_no_ext}/{filename_no_ext}/" - else: - dst_base = f"{output_filepath}assets/{COMPOSED_PATH_NAME}/{filename_no_ext}" - shutil.copytree(mesh_dir, dst_base, dirs_exist_ok=True) + # Copy pre-geerated mesh folders to the final directory + copy_pre_generated_meshes(output_filepath, mesh_info_dict, decompose_dict) # Parse the DAE file dom = minidom.parse(full_filepath) From 2b0c2307c5a3b8c1a0bc543b9a410bc7c8ab9495 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Thu, 4 Dec 2025 17:47:35 +0100 Subject: [PATCH 20/28] fixed the comments --- scripts/make_mjcf_from_robot_description.py | 14 +++++++------- 1 file changed, 7 insertions(+), 7 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 8174c46..740d155 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -703,8 +703,8 @@ def parse_inputs_xml(filename=None): raw_inputs = None processed_inputs = None - # Case 1: The file itself is mujoco_inputs.xml if root.tagName == "mujoco_inputs": + # The file itself is a standalone xml for child in root.childNodes: if child.nodeType != child.ELEMENT_NODE: continue @@ -712,11 +712,9 @@ def parse_inputs_xml(filename=None): raw_inputs = child elif child.tagName == "processed_inputs": processed_inputs = child - return raw_inputs, processed_inputs - - # Case 2: It is a URDF with a block - if root.tagName == "robot": + elif root.tagName == "robot": + # The file is a URDF mujoco_inputs_node = None # find @@ -726,7 +724,7 @@ def parse_inputs_xml(filename=None): break if mujoco_inputs_node is None: - # URDF without mujoco_inputs → allowed + # URDF without mujoco_inputs is allowed return None, None # parse children of @@ -737,8 +735,10 @@ def parse_inputs_xml(filename=None): raw_inputs = child elif child.tagName == "processed_inputs": processed_inputs = child + else: + raise ValueError( f"Root tag in file must be either 'mujoco_inputs' (standalone XML) or 'robot' (URDF), not '{root.tagName}'") - return raw_inputs, processed_inputs + return raw_inputs, processed_inputs def parse_scene_xml(filename=None): From 1ee8b51956137113cbce8f9358cba94c6a69dd3c Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Dec 2025 22:44:54 +0100 Subject: [PATCH 21/28] apply pre-commit changes --- scripts/make_mjcf_from_robot_description.py | 120 +++++++++++--------- test/launch/test_robot.launch.py | 30 ++--- 2 files changed, 75 insertions(+), 75 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 740d155..ce40adf 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -116,7 +116,7 @@ def resolve_color(visual): uri = geom.filename # full URI stem = pathlib.Path(uri).stem # filename without extension - + # Select the mesh file: use a pre-generated OBJ if available and valid; otherwise use the original is_pre_generated = False new_uri = uri # default fallback @@ -129,25 +129,30 @@ def resolve_color(visual): if os.path.exists(mesh_file) and os.path.exists(settings_file): try: - with open(settings_file, "r") as f: + with open(settings_file) as f: data = json.load(f) used_threshold = float(data.get(f"{stem}")) except (FileNotFoundError, PermissionError, json.JSONDecodeError) as e: print(f"Warning: could not read thresholds for {stem}: {e}") used_threshold = None - # Use existing decomposed object only if it has the same thresold, otherwise regenerate it. - if used_threshold is not None and math.isclose(used_threshold, float(decompose_dict[stem]), rel_tol=1e-9): + # Use existing decomposed object only if it has the same threshold, otherwise regenerate it. + if used_threshold is not None and math.isclose( + used_threshold, float(decompose_dict[stem]), rel_tol=1e-9 + ): new_uri = mesh_file is_pre_generated = True else: - print(f"Existing decomposed obj for {stem} has different threshold {used_threshold} than required {decompose_dict[stem]}. Regenerating...") + print( + f"Existing decomposed obj for {stem} has different threshold {used_threshold} " + f"than required {decompose_dict[stem]}. Regenerating..." + ) else: # Composed mesh: check if a pre-generated OBJ exists mesh_file = f"{asset_dir}/{COMPOSED_PATH_NAME}/{stem}/{stem}.obj" if os.path.exists(mesh_file): new_uri = mesh_file - is_pre_generated = True + is_pre_generated = True scale = " ".join(f"{v}" for v in geom.scale) if geom.scale else "1.0 1.0 1.0" rgba = resolve_color(vis) @@ -222,8 +227,7 @@ def convert_to_objs(mesh_info_dict, directory, xml_data, convert_stl_to_obj, dec # Import the .stl or .dae file if filename_ext.lower() == ".stl": if filename_no_ext in decompose_dict and not convert_stl_to_obj: - raise ValueError( - "The --convert_stl_to_obj argument must be specified to decompose .stl mesh") + raise ValueError("The --convert_stl_to_obj argument must be specified to decompose .stl mesh") if convert_stl_to_obj: bpy.ops.wm.stl_import(filepath=full_filepath) @@ -243,14 +247,14 @@ def convert_to_objs(mesh_info_dict, directory, xml_data, convert_stl_to_obj, dec xml_data = xml_data.replace(full_filepath, f"{assets_relative_filepath}.stl") pass elif filename_ext.lower() == ".obj": - #If import .obj files from URDF + # If import .obj files from URDF if not mesh_item["is_pre_generated"]: shutil.copy2(full_filepath, f"{directory}assets/{assets_relative_filepath}.obj") # If the .obj depends on a mtl should copy also this old_directory = os.path.dirname(mesh_item["filename"]) - if(os.path.exists(old_directory+"/material.mtl")): - final_path=os.path.dirname(assets_relative_filepath) - shutil.copy2(old_directory+"/material.mtl", f"{directory}assets/{final_path}/material.mtl") + if os.path.exists(old_directory + "/material.mtl"): + final_path = os.path.dirname(assets_relative_filepath) + shutil.copy2(old_directory + "/material.mtl", f"{directory}assets/{final_path}/material.mtl") pass # objs are ok as is elif filename_ext.lower() == ".dae": @@ -349,7 +353,7 @@ def run_obj2mjcf(output_filepath, decompose_dict): thresholds_file = os.path.join(f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}", "decomposition_thresholds.json") if os.path.exists(thresholds_file): - with open(thresholds_file, "r") as f: + with open(thresholds_file) as f: thresholds_data = json.load(f) else: thresholds_data = {} @@ -368,10 +372,10 @@ def run_obj2mjcf(output_filepath, decompose_dict): subprocess.run(cmd) thresholds_data[mesh_name] = float(threshold) - + with open(thresholds_file, "w") as f: json.dump(thresholds_data, f, indent=4) - + def update_obj_assets(dom, output_filepath, mesh_info_dict): # Find the element @@ -699,7 +703,7 @@ def parse_inputs_xml(filename=None): dom = minidom.parse(filename) root = dom.documentElement - + raw_inputs = None processed_inputs = None @@ -714,7 +718,7 @@ def parse_inputs_xml(filename=None): processed_inputs = child elif root.tagName == "robot": - # The file is a URDF + # The file is a URDF mujoco_inputs_node = None # find @@ -736,7 +740,9 @@ def parse_inputs_xml(filename=None): elif child.tagName == "processed_inputs": processed_inputs = child else: - raise ValueError( f"Root tag in file must be either 'mujoco_inputs' (standalone XML) or 'robot' (URDF), not '{root.tagName}'") + raise ValueError( + f"Root tag in file must be either 'mujoco_inputs' (standalone XML) or 'robot' (URDF), not '{root.tagName}'" + ) return raw_inputs, processed_inputs @@ -746,7 +752,7 @@ def parse_scene_xml(filename=None): This script can accept the scene in the form of an xml file. This allows users to inject this data into the MJCF that is not necessarily included in the URDF. """ - + if not filename: return None @@ -754,7 +760,7 @@ def parse_scene_xml(filename=None): dom = minidom.parse(filename) root = dom.documentElement - + scene_inputs = None if root.tagName == "mujoco": @@ -763,7 +769,7 @@ def parse_scene_xml(filename=None): return scene_inputs elif root.tagName == "robot": - # The file is a URDF + # The file is a URDF mujoco_inputs_node = None # find @@ -783,9 +789,11 @@ def parse_scene_xml(filename=None): if child.tagName == "scene": scene_inputs = child else: - raise ValueError( f"Root tag in file must be either 'mujoco_inputs' (standalone XML) or 'robot' (URDF), not '{root.tagName}'") - - return scene_inputs + raise ValueError( + f"Root tag in file must be either 'mujoco_inputs' (standalone XML) or 'robot' (URDF), not '{root.tagName}'" + ) + + return scene_inputs def add_free_joint(dom, urdf, joint_name="floating_base_joint"): @@ -1075,12 +1083,12 @@ def copy_pre_generated_meshes(output_filepath, mesh_info_dict, decompose_dict): full_path = mesh_item["filename"] mesh_dir = os.path.dirname(os.path.splitext(full_path)[0]) - if mesh_item ["is_pre_generated"]: + if mesh_item["is_pre_generated"]: if filename_no_ext in decompose_dict: - dst_base= f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/{filename_no_ext}/{filename_no_ext}/" + dst_base = f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}/{filename_no_ext}/{filename_no_ext}/" else: dst_base = f"{output_filepath}assets/{COMPOSED_PATH_NAME}/{filename_no_ext}" - + shutil.copytree(mesh_dir, dst_base, dirs_exist_ok=True) @@ -1255,17 +1263,15 @@ def publish_model_on_topic(publish_topic, output_filepath, args=None): from rclpy.qos import QoSProfile, QoSDurabilityPolicy, QoSReliabilityPolicy # Remove leading slash if present - publish_topic = publish_topic.lstrip('/') - - # --- Node ROS2 for model publishing MJCF --- + publish_topic = publish_topic.lstrip("/") + + # --- Node ROS2 for model publishing MJCF --- class MjcfPublisher(Node): def __init__(self, mjcf_path): - super().__init__('mjcf_publisher') + super().__init__("mjcf_publisher") qos_profile = QoSProfile( - depth=1, - reliability=QoSReliabilityPolicy.RELIABLE, - durability=QoSDurabilityPolicy.TRANSIENT_LOCAL + depth=1, reliability=QoSReliabilityPolicy.RELIABLE, durability=QoSDurabilityPolicy.TRANSIENT_LOCAL ) self.publisher_ = self.create_publisher(String, publish_topic, qos_profile) @@ -1273,7 +1279,7 @@ def __init__(self, mjcf_path): self.mjcf_path = mjcf_path def publish_mjcf(self): - with open(self.mjcf_path, 'r') as f: + with open(self.mjcf_path) as f: xml_content = f.read() msg = String() msg.data = xml_content @@ -1294,6 +1300,7 @@ def publish_mjcf(self): except Exception: pass + def add_urdf_free_joint(urdf): """ Adds a free joint to the top of the urdf. This makes Mujoco create a @@ -1331,7 +1338,8 @@ def add_urdf_free_joint(urdf): # child_elem = urdf_dom.createElement("child") - child_elem.setAttribute("link", old_root) # replace with your real root link name + # replace with your real root link name + child_elem.setAttribute("link", old_root) virtual_joint.appendChild(child_elem) # @@ -1368,12 +1376,19 @@ def main(args=None): help="Optionally specify a defaults xml for default settings, actuators, options, and additional sensors", ) parser.add_argument("-o", "--output", default="mjcf_data", help="Generated output path") - parser.add_argument("-p", "--publish_topic", required=False, default= None, help="Optionally specify the topic to publish the MuJoCo model") + parser.add_argument( + "-p", + "--publish_topic", + required=False, + default=None, + help="Optionally specify the topic to publish the MuJoCo model", + ) parser.add_argument("-c", "--convert_stl_to_obj", action="store_true", help="If we should convert .stls to .objs") parser.add_argument( - "-s", "--save_only", + "-s", + "--save_only", action="store_true", - help="Save files permanently on disk; without this flag, files go to a temporary directory" + help="Save files permanently on disk; without this flag, files go to a temporary directory", ) parser.add_argument( "-f", @@ -1382,17 +1397,13 @@ def main(args=None): help="Adds a free joint before the root link of the robot in the urdf before conversion", ) parser.add_argument( - "-a" ,"--asset_dir", - required=False, - default=None, - help="Optionally pass an existing folder with pre-generated OBJ meshes." - ) - parser.add_argument( - "--scene", + "-a", + "--asset_dir", required=False, default=None, - help="OOptionally pass an existing xml for the scene" + help="Optionally pass an existing folder with pre-generated OBJ meshes.", ) + parser.add_argument("--scene", required=False, default=None, help="OOptionally pass an existing xml for the scene") # remove ros args to make argparser heppy args_without_filename = sys.argv[1:] @@ -1413,7 +1424,7 @@ def main(args=None): urdf = get_urdf_from_rsp(args) # Create a tempfile and store the URDF tmp = tempfile.NamedTemporaryFile() - with open(tmp.name, 'w') as f: + with open(tmp.name, "w") as f: f.write(urdf) urdf_path = tmp.name @@ -1424,7 +1435,7 @@ def main(args=None): # Use provided MuJoCo input or scene XML files if given; otherwise use the URDF. mujoco_inputs_file = parsed_args.mujoco_inputs or urdf_path mujoco_scene_file = parsed_args.scene or urdf_path - + raw_inputs, processed_inputs = parse_inputs_xml(mujoco_inputs_file) scene_inputs = parse_scene_xml(mujoco_scene_file) decompose_dict, cameras_dict, modify_element_dict, lidar_dict = get_processed_mujoco_inputs(processed_inputs) @@ -1432,7 +1443,7 @@ def main(args=None): output_filepath = parsed_args.output if not os.path.isabs(parsed_args.output) and parsed_args.publish_topic: output_filepath = os.path.join(os.getcwd(), parsed_args.output) - # Determine the path of the output directory + # Determine the path of the output directory if parsed_args.save_only: # Grab the output directory and ensure it ends with '/' output_filepath = os.path.join(output_filepath, "") @@ -1440,12 +1451,15 @@ def main(args=None): temp_dir = tempfile.TemporaryDirectory() output_filepath = os.path.join(temp_dir.name, "") else: - raise ValueError("You must specify at least one of the following options: --publish_topic or --save-only with the path of the folder.") + raise ValueError( + "You must specify at least one of the following options: " + "--publish_topic or --save-only with the path of the folder." + ) # Add a free joint to the urdf if request_add_free_joint: urdf = add_urdf_free_joint(urdf) - + print(f"Using destination directory: {output_filepath}") # Add required mujoco tags to the starting URDF @@ -1482,7 +1496,7 @@ def main(args=None): lidar_dict, request_add_free_joint, ) - + # Publish the MuJoCo model to the specified topic if provided if parsed_args.publish_topic: publish_model_on_topic(parsed_args.publish_topic, output_filepath, args) diff --git a/test/launch/test_robot.launch.py b/test/launch/test_robot.launch.py index c1aa337..360d368 100644 --- a/test/launch/test_robot.launch.py +++ b/test/launch/test_robot.launch.py @@ -59,34 +59,20 @@ def generate_launch_description(): converter_command = [ "python3", - PathJoinSubstitution([ - FindPackageShare("mujoco_ros2_simulation"), - "scripts", - "make_mjcf_from_robot_description.py", - ]), - # "--urdf", PathJoinSubstitution([ - # FindPackageShare("mujoco_ros2_simulation"), - # "test_resources", - # "test_robot.urdf", - # ]), - # "--m", PathJoinSubstitution([ - # FindPackageShare("mujoco_ros2_simulation"), - # "test_resources", - # "kangaroo_inputs.xml", - # ]), - # "--o", PathJoinSubstitution([ - # FindPackageShare("mujoco_ros2_simulation"), - # "test_resources", - # "output_mjcf_command", - # ]), - # "--s", + PathJoinSubstitution( + [ + FindPackageShare("mujoco_ros2_simulation"), + "scripts", + "make_mjcf_from_robot_description.py", + ] + ), ] converter_process = ExecuteProcess( cmd=converter_command, name="make_mjcf_from_robot_description", output="screen", - # True to see the output + # True to see the output emulate_tty=True, ) From b65d1c36e93c5195dd1d84a1bfc854860612c8ae Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Thu, 4 Dec 2025 23:35:24 +0100 Subject: [PATCH 22/28] minor typos --- scripts/make_mjcf_from_robot_description.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index ce40adf..43ab161 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -1403,9 +1403,9 @@ def main(args=None): default=None, help="Optionally pass an existing folder with pre-generated OBJ meshes.", ) - parser.add_argument("--scene", required=False, default=None, help="OOptionally pass an existing xml for the scene") + parser.add_argument("--scene", required=False, default=None, help="Optionally pass an existing xml for the scene") - # remove ros args to make argparser heppy + # remove ros args to make argparser happy args_without_filename = sys.argv[1:] while "--ros-args" in args_without_filename: args_without_filename.remove("--ros-args") From a878972c4ee1c846b962357467ccbdb04dc558bc Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Fri, 5 Dec 2025 12:49:58 +0100 Subject: [PATCH 23/28] change metatdata name and added check for paths --- scripts/make_mjcf_from_robot_description.py | 16 ++++++++++++++-- 1 file changed, 14 insertions(+), 2 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 43ab161..a3016dc 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -125,7 +125,7 @@ def resolve_color(visual): if stem in decompose_dict: # Decomposed mesh: check if a pre-generated OBJ exists and threshold matches mesh_file = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/{stem}/{stem}/{stem}.obj" - settings_file = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/decomposition_thresholds.json" + settings_file = f"{asset_dir}/{DECOMPOSED_PATH_NAME}/metadata.json" if os.path.exists(mesh_file) and os.path.exists(settings_file): try: @@ -350,7 +350,7 @@ def run_obj2mjcf(output_filepath, decompose_dict): cmd = ["obj2mjcf", "--obj-dir", f"{output_filepath}assets/{COMPOSED_PATH_NAME}", "--save-mjcf"] subprocess.run(cmd) - thresholds_file = os.path.join(f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}", "decomposition_thresholds.json") + thresholds_file = os.path.join(f"{output_filepath}assets/{DECOMPOSED_PATH_NAME}", "metadata.json") if os.path.exists(thresholds_file): with open(thresholds_file) as f: @@ -1456,6 +1456,18 @@ def main(args=None): "--publish_topic or --save-only with the path of the folder." ) + if parsed_args.asset_dir: + assets_filepath= parsed_args.asset_dir + if not os.path.isabs(parsed_args.asset_dir): + assets_filepath = os.path.join(os.getcwd(), parsed_args.asset_dir) + print(assets_filepath) + print(output_filepath+"assets") + + if output_filepath+"assets" == assets_filepath: + raise ValueError( + "Output folder must be different from (or not inside) the assets folder" + ) + # Add a free joint to the urdf if request_add_free_joint: urdf = add_urdf_free_joint(urdf) From 68b89948d8b2477d87cea0a55fb193825b16cca7 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Fri, 5 Dec 2025 15:18:30 +0100 Subject: [PATCH 24/28] Managed scene creation for different cases --- scripts/make_mjcf_from_robot_description.py | 66 +++++++++++++++++---- 1 file changed, 55 insertions(+), 11 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index a3016dc..de56d52 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -1361,6 +1361,39 @@ def add_urdf_free_joint(urdf): return formatted_xml +def write_mujoco_scene(scene_inputs, output_filepath): + from xml.dom.minidom import Document, Node + dom = Document() + + root = dom.createElement("mujoco") + root.setAttribute("model", "scene") + dom.appendChild(root) + + include_node = dom.createElement("include") + include_node.setAttribute("file", "mujoco_description_formatted.xml") + root.appendChild(include_node) + + if scene_inputs: + scene_node = None + if scene_inputs.tagName == "scene": + scene_node = scene_inputs + else: + for child in scene_inputs.childNodes: + if child.nodeType == Node.ELEMENT_NODE and child.tagName == "scene": + scene_node = child + break + + if scene_node: + for child in scene_node.childNodes: + if child.nodeType == Node.TEXT_NODE and not child.data.strip(): + continue + imported_node = dom.importNode(child, True) # deep copy + root.appendChild(imported_node) + + with open(output_filepath + "scene.xml", "w") as file: + file.write(dom.toprettyxml(indent=" ")) + + def main(args=None): parser = argparse.ArgumentParser(description="Convert a full URDF to MJCF for use in Mujoco") @@ -1432,14 +1465,6 @@ def main(args=None): convert_stl_to_obj = parsed_args.convert_stl_to_obj - # Use provided MuJoCo input or scene XML files if given; otherwise use the URDF. - mujoco_inputs_file = parsed_args.mujoco_inputs or urdf_path - mujoco_scene_file = parsed_args.scene or urdf_path - - raw_inputs, processed_inputs = parse_inputs_xml(mujoco_inputs_file) - scene_inputs = parse_scene_xml(mujoco_scene_file) - decompose_dict, cameras_dict, modify_element_dict, lidar_dict = get_processed_mujoco_inputs(processed_inputs) - output_filepath = parsed_args.output if not os.path.isabs(parsed_args.output) and parsed_args.publish_topic: output_filepath = os.path.join(os.getcwd(), parsed_args.output) @@ -1456,13 +1481,29 @@ def main(args=None): "--publish_topic or --save-only with the path of the folder." ) + # Use provided MuJoCo input or scene XML files if given; otherwise use the URDF. + mujoco_inputs_file = parsed_args.mujoco_inputs or urdf_path + mujoco_scene_file = parsed_args.scene or urdf_path + + raw_inputs, processed_inputs = parse_inputs_xml(mujoco_inputs_file) + + scene_inputs = None + if parsed_args.publish_topic or (parsed_args.save_only and not parsed_args.scene): + scene_inputs = parse_scene_xml(mujoco_scene_file) + + # Copy the scene tags from URDF to a separate xml il not publishing + if not parsed_args.publish_topic and parsed_args.save_only and scene_inputs: + print("Copying scene tags from URDF to a separate xml") + write_mujoco_scene(scene_inputs, output_filepath) + scene_inputs = None + + decompose_dict, cameras_dict, modify_element_dict, lidar_dict = get_processed_mujoco_inputs(processed_inputs) + + if parsed_args.asset_dir: assets_filepath= parsed_args.asset_dir if not os.path.isabs(parsed_args.asset_dir): assets_filepath = os.path.join(os.getcwd(), parsed_args.asset_dir) - print(assets_filepath) - print(output_filepath+"assets") - if output_filepath+"assets" == assets_filepath: raise ValueError( "Output folder must be different from (or not inside) the assets folder" @@ -1513,6 +1554,9 @@ def main(args=None): if parsed_args.publish_topic: publish_model_on_topic(parsed_args.publish_topic, output_filepath, args) + # Copy the existing scene.xml to the output folder if not publishing + if not parsed_args.publish_topic and parsed_args.save_only and parsed_args.scene: + shutil.copy2(f'{parsed_args.scene}', output_filepath) if __name__ == "__main__": main() From ee31f789ded3e01d97d8dc766bcad062331cca53 Mon Sep 17 00:00:00 2001 From: Ortisa Poci Date: Fri, 5 Dec 2025 15:23:28 +0100 Subject: [PATCH 25/28] Added comments --- scripts/make_mjcf_from_robot_description.py | 4 +++- 1 file changed, 3 insertions(+), 1 deletion(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index de56d52..faef75e 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -1369,6 +1369,7 @@ def write_mujoco_scene(scene_inputs, output_filepath): root.setAttribute("model", "scene") dom.appendChild(root) + # Add an tag for the mujoco description include_node = dom.createElement("include") include_node.setAttribute("file", "mujoco_description_formatted.xml") root.appendChild(include_node) @@ -1382,7 +1383,8 @@ def write_mujoco_scene(scene_inputs, output_filepath): if child.nodeType == Node.ELEMENT_NODE and child.tagName == "scene": scene_node = child break - + + # If a node was found, import all of its child nodes if scene_node: for child in scene_node.childNodes: if child.nodeType == Node.TEXT_NODE and not child.data.strip(): From be9f2831b5def7449cd99c47229069389608cfa8 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Fri, 5 Dec 2025 21:04:18 +0100 Subject: [PATCH 26/28] Add pre-commit fix --- scripts/make_mjcf_from_robot_description.py | 23 ++++++++++----------- 1 file changed, 11 insertions(+), 12 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index faef75e..fbe332b 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -1363,12 +1363,13 @@ def add_urdf_free_joint(urdf): def write_mujoco_scene(scene_inputs, output_filepath): from xml.dom.minidom import Document, Node + dom = Document() root = dom.createElement("mujoco") root.setAttribute("model", "scene") dom.appendChild(root) - + # Add an tag for the mujoco description include_node = dom.createElement("include") include_node.setAttribute("file", "mujoco_description_formatted.xml") @@ -1383,7 +1384,7 @@ def write_mujoco_scene(scene_inputs, output_filepath): if child.nodeType == Node.ELEMENT_NODE and child.tagName == "scene": scene_node = child break - + # If a node was found, import all of its child nodes if scene_node: for child in scene_node.childNodes: @@ -1488,11 +1489,11 @@ def main(args=None): mujoco_scene_file = parsed_args.scene or urdf_path raw_inputs, processed_inputs = parse_inputs_xml(mujoco_inputs_file) - + scene_inputs = None if parsed_args.publish_topic or (parsed_args.save_only and not parsed_args.scene): scene_inputs = parse_scene_xml(mujoco_scene_file) - + # Copy the scene tags from URDF to a separate xml il not publishing if not parsed_args.publish_topic and parsed_args.save_only and scene_inputs: print("Copying scene tags from URDF to a separate xml") @@ -1501,15 +1502,12 @@ def main(args=None): decompose_dict, cameras_dict, modify_element_dict, lidar_dict = get_processed_mujoco_inputs(processed_inputs) - if parsed_args.asset_dir: - assets_filepath= parsed_args.asset_dir + assets_filepath = parsed_args.asset_dir if not os.path.isabs(parsed_args.asset_dir): assets_filepath = os.path.join(os.getcwd(), parsed_args.asset_dir) - if output_filepath+"assets" == assets_filepath: - raise ValueError( - "Output folder must be different from (or not inside) the assets folder" - ) + if output_filepath + "assets" == assets_filepath: + raise ValueError("Output folder must be different from (or not inside) the assets folder") # Add a free joint to the urdf if request_add_free_joint: @@ -1556,9 +1554,10 @@ def main(args=None): if parsed_args.publish_topic: publish_model_on_topic(parsed_args.publish_topic, output_filepath, args) - # Copy the existing scene.xml to the output folder if not publishing + # Copy the existing scene.xml to the output folder if not publishing if not parsed_args.publish_topic and parsed_args.save_only and parsed_args.scene: - shutil.copy2(f'{parsed_args.scene}', output_filepath) + shutil.copy2(f"{parsed_args.scene}", output_filepath) + if __name__ == "__main__": main() From 1003cba47aebeb536b36af83a30e4158dd86aeca Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 10 Dec 2025 19:40:15 +0100 Subject: [PATCH 27/28] Apply suggestions from code review Co-authored-by: Erik Holum --- scripts/make_mjcf_from_robot_description.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index fbe332b..df55f7a 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -1481,7 +1481,7 @@ def main(args=None): else: raise ValueError( "You must specify at least one of the following options: " - "--publish_topic or --save-only with the path of the folder." + "--publish_topic or --save_only." ) # Use provided MuJoCo input or scene XML files if given; otherwise use the URDF. @@ -1506,7 +1506,7 @@ def main(args=None): assets_filepath = parsed_args.asset_dir if not os.path.isabs(parsed_args.asset_dir): assets_filepath = os.path.join(os.getcwd(), parsed_args.asset_dir) - if output_filepath + "assets" == assets_filepath: + if output_filepath + "assets" in assets_filepath: raise ValueError("Output folder must be different from (or not inside) the assets folder") # Add a free joint to the urdf From a92298ffc5a8879962226bcaec330b18f1a050a9 Mon Sep 17 00:00:00 2001 From: Sai Kishor Kothakota Date: Wed, 10 Dec 2025 19:41:58 +0100 Subject: [PATCH 28/28] Use try_shutdown --- scripts/make_mjcf_from_robot_description.py | 10 ++-------- 1 file changed, 2 insertions(+), 8 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index df55f7a..bcf216d 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -1295,10 +1295,7 @@ def publish_mjcf(self): pass finally: mjcf_node.destroy_node() - try: - rclpy.shutdown() - except Exception: - pass + rclpy.try_shutdown() def add_urdf_free_joint(urdf): @@ -1479,10 +1476,7 @@ def main(args=None): temp_dir = tempfile.TemporaryDirectory() output_filepath = os.path.join(temp_dir.name, "") else: - raise ValueError( - "You must specify at least one of the following options: " - "--publish_topic or --save_only." - ) + raise ValueError("You must specify at least one of the following options: " "--publish_topic or --save_only.") # Use provided MuJoCo input or scene XML files if given; otherwise use the URDF. mujoco_inputs_file = parsed_args.mujoco_inputs or urdf_path