From ef2b6ca4cb51612ded559b6a6e127a99d32cd1ce Mon Sep 17 00:00:00 2001 From: Nathan Dunkelberger Date: Thu, 16 Oct 2025 12:51:42 -0500 Subject: [PATCH 01/10] fixing the free joint conversion to include mass and inertial properties for the root body --- scripts/make_mjcf_from_robot_description.py | 97 ++++++++++++++++----- 1 file changed, 76 insertions(+), 21 deletions(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 5e06a41..e812f38 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -31,7 +31,7 @@ import numpy as np -from urdf_parser_py.urdf import URDF +from urdf_parser_py.urdf import URDF, Joint, Link, Pose from ament_index_python.packages import get_package_share_directory from xml.dom import minidom @@ -649,9 +649,11 @@ def parse_inputs_xml(filename=None): return raw_inputs, processed_inputs -def add_free_joint(dom, urdf, joint_name="floating_base_joint"): +def fix_free_joint(dom, urdf, joint_name="floating_base_joint"): """ - Optionally adds a free joint to the base of the robot for non-fixed based systems. + This change is on the mjcf side, and replaces the "free" joint type with a freejoint tag. + This is a special item which explicitl sets all siffness/damping to 0. + https://mujoco.readthedocs.io/en/stable/XMLreference.html#body-freejoint """ robot = URDF.from_xml_string(urdf) root_link = robot.get_root() @@ -659,25 +661,19 @@ def add_free_joint(dom, urdf, joint_name="floating_base_joint"): print("Not adding a free joint because world is the URDF root") return - # get the world body - world_body = dom.getElementsByTagName("worldbody")[0] + # Find all joint elements + joints = dom.getElementsByTagName("joint") - # make a new body with our base - root_body = dom.createElement("body") - root_body.setAttribute("name", root_link) + # Locate the one with name="virtual_base_joint" of type="free" + for joint in joints: + if joint.getAttribute("name") == "virtual_base_joint" and joint.getAttribute("type") == "free": + # Create the new freejoint element + new_joint = dom.createElement("freejoint") + new_joint.setAttribute("name", joint_name) - # make a free joint under that body - free_joint = dom.createElement("freejoint") - free_joint.setAttribute("name", joint_name) - root_body.appendChild(free_joint) - - # move the previous body underneath the new body and joint - while world_body.hasChildNodes(): - child = world_body.firstChild - world_body.removeChild(child) - root_body.appendChild(child) - - world_body.appendChild(root_body) + # Replace the old joint with the new one + joint.parentNode.replaceChild(new_joint, joint) + break return dom @@ -948,7 +944,7 @@ def fix_mujoco_description( dom = minidom.parse(full_filepath) if request_add_free_joint: - dom = add_free_joint(dom, urdf) + dom = fix_free_joint(dom, urdf) # Update and add the new fixed assets dom = update_obj_assets(dom, output_filepath, mesh_info_dict) @@ -1081,6 +1077,61 @@ def get_parent_chain(link, transform=PyKDL.Frame.Identity()): return results +def add_urdf_free_joint(urdf): + """ + Adds a free joint to the top of the urdf. This makes Mujoco create a + floating joint so that a base is free to move, like on an AMR. + """ + + # get the old root link + robot = URDF.from_xml_string(urdf) + old_root = robot.get_root() + + urdf_dom = minidom.parseString(urdf) + + # Get the root element + robot_elem = urdf_dom.getElementsByTagName("robot")[0] + + ################################### + # virtual base link + virtual_link = urdf_dom.createElement("link") + virtual_link.setAttribute("name", "virtual_base") + + ################################### + # joint of virtual base link to dummy link + virtual_joint = urdf_dom.createElement("joint") + virtual_joint.setAttribute("name", "virtual_base_joint") + virtual_joint.setAttribute("type", "floating") + + # + parent_elem = urdf_dom.createElement("parent") + parent_elem.setAttribute("link", "virtual_base") + virtual_joint.appendChild(parent_elem) + + # + child_elem = urdf_dom.createElement("child") + child_elem.setAttribute("link", old_root) # replace with your real root link name + virtual_joint.appendChild(child_elem) + + # + origin_elem = urdf_dom.createElement("origin") + origin_elem.setAttribute("xyz", "0 0 0") + origin_elem.setAttribute("rpy", "0 0 0") + virtual_joint.appendChild(origin_elem) + + # Insert the elements at the top of the robot definition + robot_elem.insertBefore(virtual_joint, robot_elem.firstChild) + robot_elem.insertBefore(virtual_link, robot_elem.firstChild) + + # Use minidom to format the string with line breaks and indentation + formatted_xml = urdf_dom.toprettyxml(indent=" ") + + # Remove extra newlines that minidom adds after each tag + formatted_xml = "\n".join([line for line in formatted_xml.splitlines() if line.strip()]) + + return formatted_xml + + def main(args=None): parser = argparse.ArgumentParser(description="Convert a full URDF to MJCF for use in Mujoco") @@ -1125,6 +1176,10 @@ def main(args=None): # Grab the output directory and ensure it ends with '/' output_filepath = os.path.join(parsed_args.output, "") + # Add a free joint to the urdf + if request_add_free_joint: + urdf = add_urdf_free_joint(urdf) + # Add required mujoco tags to the starting URDF xml_data = add_mujoco_info(urdf) From 6bc387068bbbe55e2742761519ac635dc3e87c42 Mon Sep 17 00:00:00 2001 From: Nathan Dunkelberger Date: Thu, 16 Oct 2025 12:54:41 -0500 Subject: [PATCH 02/10] fix precommit --- scripts/make_mjcf_from_robot_description.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index e812f38..5b399ce 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -31,7 +31,7 @@ import numpy as np -from urdf_parser_py.urdf import URDF, Joint, Link, Pose +from urdf_parser_py.urdf import URDF from ament_index_python.packages import get_package_share_directory from xml.dom import minidom From c082e8c87f58446e4acd3a37cad15f4e379093eb Mon Sep 17 00:00:00 2001 From: Nathan Dunkelberger <138718889+ndunkelb-nasa@users.noreply.github.com> Date: Fri, 17 Oct 2025 08:47:45 -0500 Subject: [PATCH 03/10] Fix typos in freejoint description Co-authored-by: Erik Holum --- scripts/make_mjcf_from_robot_description.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 5b399ce..0a96414 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -652,7 +652,7 @@ def parse_inputs_xml(filename=None): def fix_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 explicitl sets all siffness/damping to 0. + This is a special item which explicitly sets all stiffness/damping to 0. https://mujoco.readthedocs.io/en/stable/XMLreference.html#body-freejoint """ robot = URDF.from_xml_string(urdf) From a2ba9e3162724ef1d295670a644f2af544cc3916 Mon Sep 17 00:00:00 2001 From: Nathan Dunkelberger Date: Fri, 17 Oct 2025 09:19:25 -0500 Subject: [PATCH 04/10] adding check to see if root is world --- scripts/make_mjcf_from_robot_description.py | 4 ++++ 1 file changed, 4 insertions(+) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 0a96414..969d411 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -1087,6 +1087,10 @@ def add_urdf_free_joint(urdf): robot = URDF.from_xml_string(urdf) old_root = robot.get_root() + if old_root == "world": + print("Not adding a free joint because world is the URDF root") + return + urdf_dom = minidom.parseString(urdf) # Get the root element From 53b577d4535ac3d75bd11ea6d8770d5d00cf6372 Mon Sep 17 00:00:00 2001 From: Nathan Dunkelberger Date: Fri, 17 Oct 2025 09:26:00 -0500 Subject: [PATCH 05/10] raise exception if no virtual joint was found --- scripts/make_mjcf_from_robot_description.py | 6 ++++++ 1 file changed, 6 insertions(+) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index 969d411..aad5f70 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -664,6 +664,8 @@ def fix_free_joint(dom, urdf, joint_name="floating_base_joint"): # Find all joint elements joints = dom.getElementsByTagName("joint") + succesfully_fixed = False + # Locate the one with name="virtual_base_joint" of type="free" for joint in joints: if joint.getAttribute("name") == "virtual_base_joint" and joint.getAttribute("type") == "free": @@ -673,8 +675,12 @@ def fix_free_joint(dom, urdf, joint_name="floating_base_joint"): # Replace the old joint with the new one joint.parentNode.replaceChild(new_joint, joint) + succesfully_fixed = True break + if not succesfully_fixed: + raise ValueError("Did not find a joint of name virtual_base_joint and type free. What did you just do????") + return dom From 6bea8823ea9538204d9849cb5b4883ce39dc6cac Mon Sep 17 00:00:00 2001 From: Nathan Dunkelberger Date: Fri, 17 Oct 2025 10:03:10 -0500 Subject: [PATCH 06/10] disabling rangefinder visuals at startup --- src/mujoco_system_interface.cpp | 5 +++++ 1 file changed, 5 insertions(+) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index f8b8826..4254c28 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -429,6 +429,11 @@ hardware_interface::CallbackReturn MujocoSystemInterface::on_init(const hardware return hardware_interface::CallbackReturn::FAILURE; } + // Disable the rangefinder flag at startup so that we don't get the yellow lines. + // We can still turn this on manually if desired. + auto rangefinder_flag_ptr = sim_->opt.flags + mjVIS_RANGEFINDER; + *rangefinder_flag_ptr = false; + // When the interface is activated, we start the physics engine. physics_thread_ = std::thread([this]() { // Load the simulation and do an initial forward pass From 614c32d3df51b58c3cac7bb4a22e9a6217b63bda Mon Sep 17 00:00:00 2001 From: Nathan Dunkelberger Date: Fri, 17 Oct 2025 10:05:23 -0500 Subject: [PATCH 07/10] updating argument description --- scripts/make_mjcf_from_robot_description.py | 5 ++++- 1 file changed, 4 insertions(+), 1 deletion(-) diff --git a/scripts/make_mjcf_from_robot_description.py b/scripts/make_mjcf_from_robot_description.py index aad5f70..270ca6b 100755 --- a/scripts/make_mjcf_from_robot_description.py +++ b/scripts/make_mjcf_from_robot_description.py @@ -1158,7 +1158,10 @@ 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( - "-f", "--add_free_joint", action="store_true", help="Adds a free joint as the base link for mobile robots" + "-f", + "--add_free_joint", + action="store_true", + help="Adds a free joint before the root link of the robot in the urdf before conversion", ) # remove ros args to make argparser heppy From b84df8586e7f5373d8c56e6defef78f43e884248 Mon Sep 17 00:00:00 2001 From: Nathan Dunkelberger <138718889+ndunkelb-nasa@users.noreply.github.com> Date: Fri, 17 Oct 2025 10:31:32 -0500 Subject: [PATCH 08/10] switch to managing as array instead of pointer arithmetic Co-authored-by: Erik Holum --- src/mujoco_system_interface.cpp | 3 +-- 1 file changed, 1 insertion(+), 2 deletions(-) diff --git a/src/mujoco_system_interface.cpp b/src/mujoco_system_interface.cpp index 4254c28..6b6087b 100644 --- a/src/mujoco_system_interface.cpp +++ b/src/mujoco_system_interface.cpp @@ -431,8 +431,7 @@ hardware_interface::CallbackReturn MujocoSystemInterface::on_init(const hardware // Disable the rangefinder flag at startup so that we don't get the yellow lines. // We can still turn this on manually if desired. - auto rangefinder_flag_ptr = sim_->opt.flags + mjVIS_RANGEFINDER; - *rangefinder_flag_ptr = false; + sim_->opt.flags[mjVIS_RANGEFINDER] = false; // When the interface is activated, we start the physics engine. physics_thread_ = std::thread([this]() { From 52fdd990528a7cf2d03ae145ee5469c5108f29f0 Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Thu, 6 Nov 2025 15:21:20 -0500 Subject: [PATCH 09/10] Ensure the camera's rendering context will support the maximum camera resolutions --- src/mujoco_cameras.cpp | 17 +++++++++++++---- 1 file changed, 13 insertions(+), 4 deletions(-) diff --git a/src/mujoco_cameras.cpp b/src/mujoco_cameras.cpp index 6a865af..9bb7278 100644 --- a/src/mujoco_cameras.cpp +++ b/src/mujoco_cameras.cpp @@ -164,14 +164,23 @@ void MujocoCameras::update_loop() // Turn rangefinder rendering off so we don't get rays in camera images mjv_opt_.flags[mjtVisFlag::mjVIS_RANGEFINDER] = 0; + // Initialize data for camera rendering + mj_camera_data_ = mj_makeData(mj_model_); + RCLCPP_INFO(node_->get_logger(), "Starting the camera rendering loop, publishing at %f Hz", camera_publish_rate_); + // create scene and context mjv_makeScene(mj_model_, &mjv_scn_, 2000); mjr_makeContext(mj_model_, &mjr_con_, mjFONTSCALE_150); - // Initialize data for camera rendering - mj_camera_data_ = mj_makeData(mj_model_); - RCLCPP_INFO_STREAM(node_->get_logger(), - "Starting the camera rendering loop, publishing at " << camera_publish_rate_ << " Hz"); + // Ensure the context will support the largest cameras + int max_width = 1, max_height = 1; + for (const auto& cam : cameras_) + { + max_width = std::max(max_width, static_cast(cam.width)); + max_height = std::max(max_height, static_cast(cam.height)); + } + mjr_resizeOffscreen(max_width, max_height, &mjr_con_); + RCLCPP_INFO(node_->get_logger(), "Resized offscreen buffer to %d x %d", max_width, max_height); // TODO: Support per-camera publish rates? rclcpp::Rate rate(camera_publish_rate_); From 47e3e69d57728bb4c8fe90892bdd73773608af0a Mon Sep 17 00:00:00 2001 From: Erik Holum Date: Thu, 6 Nov 2025 15:33:57 -0500 Subject: [PATCH 10/10] Bump the test camera's image res --- test/test_resources/test_robot.xml | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/test/test_resources/test_robot.xml b/test/test_resources/test_robot.xml index 5f1a39b..f504d99 100644 --- a/test/test_resources/test_robot.xml +++ b/test/test_resources/test_robot.xml @@ -17,7 +17,7 @@ - +