From babb43fd4a8b4470968edf9f7152460a7eb1d0de Mon Sep 17 00:00:00 2001 From: Ali K Date: Wed, 11 Dec 2024 01:28:45 -0800 Subject: [PATCH 1/6] add base body and other fixes to conversion --- urdf2mjcf/convert.py | 86 ++++++++++++++++++++++++++++++++++++++------ 1 file changed, 75 insertions(+), 11 deletions(-) diff --git a/urdf2mjcf/convert.py b/urdf2mjcf/convert.py index e2fd10d..1189dc4 100644 --- a/urdf2mjcf/convert.py +++ b/urdf2mjcf/convert.py @@ -205,12 +205,32 @@ def add_root_body(root: ET.Element) -> None: }, ) - # Move existing bodies and geoms under root_body + # Create base body + base_body = ET.SubElement( + root_body, + "body", + attrib={"name": "base"}, + ) + + # Add inertial properties to base body + ET.SubElement( + base_body, + "inertial", + attrib={ + # TODO: How to compute these values? + "pos": "0.00648939 0.00390843 -0.180571", + "quat": "0.999495 -0.0317223 -0.00110485 0.00149824", + "mass": "18.9034", + "diaginertia": "1.33012 0.801658 0.559678", + }, + ) + + # Move existing bodies and geoms under base_body elements_to_move = list(worldbody) for elem in elements_to_move: if elem.tag in {"body", "geom"}: worldbody.remove(elem) - root_body.append(elem) + base_body.append(elem) worldbody.append(root_body) @@ -449,21 +469,64 @@ def add_visual_geom_logic(root: ET.Element) -> None: body.insert(index + 1, new_geom) -def add_default_position(root: ET.Element, default_position: List[float]) -> None: - """Add a keyframe to the root element. +def add_statistics(root: ET.Element, temp_mjcf_path: Path) -> None: + """Add statistics element to the MJCF file using computed values. Args: root: The root element of the MJCF file. - default_position: The default position of the robot. + temp_mjcf_path: Path to the temporary MJCF file to compute statistics from. """ - actuators = root.find("actuator") - if actuators is None: - raise ValueError("No actuators found in the MJCF file.") + # Compute the statistics from the model + model = mujoco.MjModel.from_xml_path(str(temp_mjcf_path)) + center_str = " ".join(map(str, model.stat.center)) + + statistic = ET.Element( + "statistic", {"meansize": str(model.stat.meansize), "extent": str(model.stat.extent), "center": center_str} + ) + + # Insert after default + default_el = root.find("default") + idx = list(root).index(default_el) + root.insert(idx + 1, statistic) + + +def calculate_dof_from_xml(root: ET.Element) -> int: + """Calculate the total DOF (qpos size) of a robot based on the XML tree. - num_actuators = len(list(actuators.iter("motor"))) - if len(default_position) != num_actuators: - raise ValueError(f"Default position must have {num_actuators} values, got {len(default_position)}.") + Free joints add 7 DOFs, and joints with a 'range' attribute add 1 DOF each. + + Args: + root: Root element of the MJCF XML. + + Returns: + Total DOF (qpos size). + """ + dof = 0 + + # Count free joints + for _ in root.iter("freejoint"): + dof += 7 + + # Count joints with a 'range' attribute + for joint in root.iter("joint"): + if "range" in joint.attrib: + dof += 1 + + return dof + + +def add_default_position(root: ET.Element, default_position: List[float]) -> None: + """Add a keyframe to the root element with the default start position. + + Args: + root: The root element of the MJCF file. + default_position: The default positions of the robot. + """ + num_dof = calculate_dof_from_xml(root) + if len(default_position) != num_dof: + raise ValueError(f"Default position must have {num_dof} values, got {len(default_position)}.") + # Add the keyframe with the default position keyframe = ET.Element("keyframe") key = ET.SubElement(keyframe, "key") key.set("name", "default") @@ -559,6 +622,7 @@ def convert_urdf_to_mjcf( add_default(root) add_compiler(root) add_option(root) + add_statistics(root, temp_mjcf_path) add_assets(root) add_cameras(root, distance=camera_distance, height_offset=camera_height_offset) add_root_body(root) From 62234cb144a938d3e7113b3c0799ec7d4b836550 Mon Sep 17 00:00:00 2001 From: Ali K Date: Wed, 11 Dec 2024 10:24:08 -0800 Subject: [PATCH 2/6] type check for lint --- urdf2mjcf/convert.py | 11 ++++++----- 1 file changed, 6 insertions(+), 5 deletions(-) diff --git a/urdf2mjcf/convert.py b/urdf2mjcf/convert.py index 1189dc4..e75eb18 100644 --- a/urdf2mjcf/convert.py +++ b/urdf2mjcf/convert.py @@ -476,7 +476,7 @@ def add_statistics(root: ET.Element, temp_mjcf_path: Path) -> None: root: The root element of the MJCF file. temp_mjcf_path: Path to the temporary MJCF file to compute statistics from. """ - # Compute the statistics from the model + # Compute the statistics directly from mujoco model = mujoco.MjModel.from_xml_path(str(temp_mjcf_path)) center_str = " ".join(map(str, model.stat.center)) @@ -484,10 +484,11 @@ def add_statistics(root: ET.Element, temp_mjcf_path: Path) -> None: "statistic", {"meansize": str(model.stat.meansize), "extent": str(model.stat.extent), "center": center_str} ) - # Insert after default - default_el = root.find("default") - idx = list(root).index(default_el) - root.insert(idx + 1, statistic) + # Insert after default if it exists, otherwise append + if isinstance(existing_element := root.find("default"), ET.Element): + root.insert(list(root).index(existing_element) + 1, statistic) + else: + root.append(statistic) def calculate_dof_from_xml(root: ET.Element) -> int: From 268f32310e716cdcaa5921306fc19491bdd52aff Mon Sep 17 00:00:00 2001 From: Ali K Date: Wed, 11 Dec 2024 10:56:56 -0800 Subject: [PATCH 3/6] update num dofs in test_conversion --- tests/test_conversion.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/tests/test_conversion.py b/tests/test_conversion.py index dd293ec..5ac144a 100644 --- a/tests/test_conversion.py +++ b/tests/test_conversion.py @@ -31,7 +31,7 @@ def test_conversion_no_frc_limit(tmpdir: Path) -> None: mjcf_path=mjcf_path, no_frc_limit=True, copy_meshes=False, - default_position=[0.0] * 10, + default_position=[0.0] * 17, ) # Compare the outputted MJCF with the expected XML From 3d279e861d5a9d477ee9ad136d21d72de5a7832c Mon Sep 17 00:00:00 2001 From: Ali K Date: Wed, 11 Dec 2024 10:58:26 -0800 Subject: [PATCH 4/6] add stats, "base" body and correct num-dofs in robot_test.xml --- tests/sample/robot_test.xml | 145 +++++++++++++++++++----------------- 1 file changed, 75 insertions(+), 70 deletions(-) diff --git a/tests/sample/robot_test.xml b/tests/sample/robot_test.xml index 23e5bc1..143c378 100644 --- a/tests/sample/robot_test.xml +++ b/tests/sample/robot_test.xml @@ -13,6 +13,8 @@ + + @@ -39,79 +41,82 @@ - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + + - - - - - - - - - - - - - - - - - - - - - - - - - - + + + + + + + + + + + + + + + + + + + + + + + + + + @@ -169,6 +174,6 @@ - + \ No newline at end of file From d5d5f4c47715ab5b40dbbc1f67004008a600885b Mon Sep 17 00:00:00 2001 From: Ali K Date: Wed, 11 Dec 2024 16:25:16 -0800 Subject: [PATCH 5/6] remove hardcoded base values and statistics from GT test xml file --- tests/sample/robot_test.xml | 5 +---- 1 file changed, 1 insertion(+), 4 deletions(-) diff --git a/tests/sample/robot_test.xml b/tests/sample/robot_test.xml index 143c378..ba559cf 100644 --- a/tests/sample/robot_test.xml +++ b/tests/sample/robot_test.xml @@ -13,8 +13,6 @@ - - @@ -41,8 +39,7 @@ - - + From 67d8ea3f08616442282cc3286e48151d0f277e8d Mon Sep 17 00:00:00 2001 From: Ali K Date: Wed, 11 Dec 2024 16:30:30 -0800 Subject: [PATCH 6/6] remove hardcoded intertial values and statistics --- urdf2mjcf/convert.py | 38 +++----------------------------------- 1 file changed, 3 insertions(+), 35 deletions(-) diff --git a/urdf2mjcf/convert.py b/urdf2mjcf/convert.py index e75eb18..6a4bf6f 100644 --- a/urdf2mjcf/convert.py +++ b/urdf2mjcf/convert.py @@ -209,19 +209,10 @@ def add_root_body(root: ET.Element) -> None: base_body = ET.SubElement( root_body, "body", - attrib={"name": "base"}, - ) - - # Add inertial properties to base body - ET.SubElement( - base_body, - "inertial", attrib={ - # TODO: How to compute these values? - "pos": "0.00648939 0.00390843 -0.180571", - "quat": "0.999495 -0.0317223 -0.00110485 0.00149824", - "mass": "18.9034", - "diaginertia": "1.33012 0.801658 0.559678", + "name": "base", + "pos": "0 0 0", + "quat": "1 0 0 0", }, ) @@ -469,28 +460,6 @@ def add_visual_geom_logic(root: ET.Element) -> None: body.insert(index + 1, new_geom) -def add_statistics(root: ET.Element, temp_mjcf_path: Path) -> None: - """Add statistics element to the MJCF file using computed values. - - Args: - root: The root element of the MJCF file. - temp_mjcf_path: Path to the temporary MJCF file to compute statistics from. - """ - # Compute the statistics directly from mujoco - model = mujoco.MjModel.from_xml_path(str(temp_mjcf_path)) - center_str = " ".join(map(str, model.stat.center)) - - statistic = ET.Element( - "statistic", {"meansize": str(model.stat.meansize), "extent": str(model.stat.extent), "center": center_str} - ) - - # Insert after default if it exists, otherwise append - if isinstance(existing_element := root.find("default"), ET.Element): - root.insert(list(root).index(existing_element) + 1, statistic) - else: - root.append(statistic) - - def calculate_dof_from_xml(root: ET.Element) -> int: """Calculate the total DOF (qpos size) of a robot based on the XML tree. @@ -623,7 +592,6 @@ def convert_urdf_to_mjcf( add_default(root) add_compiler(root) add_option(root) - add_statistics(root, temp_mjcf_path) add_assets(root) add_cameras(root, distance=camera_distance, height_offset=camera_height_offset) add_root_body(root)