diff --git a/mujoco_ros2_control/README.md b/mujoco_ros2_control/README.md index d5a14542..00c10f6d 100644 --- a/mujoco_ros2_control/README.md +++ b/mujoco_ros2_control/README.md @@ -16,7 +16,7 @@ This includes adding actuators, sensors, and cameras as needed to the MJCF XML. This conversion can be done either offline or at run time. We have built a *highly experimental* tool to automate URDF conversion. -For more information refer to the [documentation](https://github.com/ros-controls/mujoco_ros2_control/blob/main/mujoco_ros2_control/docs/TOOLS.md). +For more information refer to the [documentation](./docs/TOOLS.md). ## Hardware Interface Setup @@ -375,19 +375,18 @@ The lidar sensor is then configurable through ROS 2 control xacro with: ```xml - - - - + + + lidar_sensor_frame 0.025 - 12 + -0.3 + 0.3 0.05 10 /scan - ``` ## Simulation - Topics and Services diff --git a/mujoco_ros2_control/docs/TOOLS.md b/mujoco_ros2_control/docs/TOOLS.md index 96ceda71..9484668b 100644 --- a/mujoco_ros2_control/docs/TOOLS.md +++ b/mujoco_ros2_control/docs/TOOLS.md @@ -1,6 +1,5 @@ # URDF to MJCF Conversion - > [!WARNING] > This tool is hacky and _highly_ experimental! > Expect things to be broken. @@ -27,16 +26,18 @@ ros2 run mujoco_ros2_control make_mjcf_from_robot_description.py ros2 run mujoco_ros2_control robot_description_to_mjcf.sh ``` -When `robot_description_to_mjcf.sh` is first executed, it creates a Python virtual environment at `$ROS_HOME/ros2_control` and installs all necessary dependencies. Once set up, the script sources the environment and runs `make_mjcf_from_robot_description.py`. On subsequent runs, it reuses the existing virtual environment. +When `robot_description_to_mjcf.sh` is first executed, it creates a Python virtual environment at `$ROS_HOME/ros2_control` and installs all necessary dependencies. +Once set up, the script sources the environment and runs `make_mjcf_from_robot_description.py`. +On subsequent runs, it reuses the existing virtual environment. By default, the tool will pull a URDF from the `/robot_description` topic. However, this is configurable at execution time. A complete list of options is available from the argument parser: ```bash -$ ros2 run mujoco_ros2_control make_mjcf_from_robot_description.py -h -usage: make_mjcf_from_robot_description.py [-h] [-u URDF] [-r ROBOT_DESCRIPTION] [-m MUJOCO_INPUTS] [-o OUTPUT] [-p PUBLISH_TOPIC] [-c] [-s] - [-f] [--fuse | --no-fuse] [-a ASSET_DIR] [--scene SCENE] +$ ros2 run mujoco_ros2_control make_mjcf_from_robot_description.py -h +usage: make_mjcf_from_robot_description.py [-h] [-u URDF] [-r ROBOT_DESCRIPTION] [-m MUJOCO_INPUTS] [-o OUTPUT] [-p PUBLISH_TOPIC] [-c] [-s] [-f] + [--fuse | --no-fuse] [-a ASSET_DIR] [--scene SCENE] Convert a full URDF to MJCF for use in MuJoCo @@ -55,7 +56,7 @@ options: If we should convert .stls to .objs -s, --save_only Save files permanently on disk; without this flag, files go to a temporary directory -f, --add_free_joint Adds a free joint before the root link of the robot in the urdf before conversion - --fuse, --no-fuse Allows MuJoCo to merge static bodies. Use --no-fuse to prevent merging (Default: --fuse). + --fuse, --no-fuse Allows MuJoCo to merge static bodies. Use --no-fuse to prevent merging. -a ASSET_DIR, --asset_dir ASSET_DIR Optionally pass an existing folder with pre-generated OBJ meshes. --scene SCENE Optionally pass an existing xml for the scene @@ -80,21 +81,22 @@ The `/tmp/output/` directory will contain all necessary assets and MJCF files th They can also be adjusted as needed after the fact. ```bash -/opt/ros/${ROS_DISTRO}/opt/mujoco_vendor/bin/simulate /tmp/output/mujoco_description_formatted.xml +ros2 run mujoco_vendor simulate /tmp/output/mujoco_description_formatted.xml ``` Of note, the test robot has a good chunk of supported functionality, and we recommend using it as a guide. > [!NOTE] -> The `make_mjcf_from_robot_description.py` script requires `trimesh`, `mujoco`, and `obj2mjcf`. These must either be installed system-wide or available within a virtual environment that is sourced before running the command. - +> The `make_mjcf_from_robot_description.py` script requires `trimesh`, `mujoco`, and `obj2mjcf`. +> These must either be installed system-wide or available within a virtual environment that is sourced before running the command. > [!NOTE] -> This page focuses on generating MJCFs for robots. Please see additional documentation on [modeling and generating MJCFs for objects](./MODELING_TIPS.md). +> This page focuses on generating MJCFs for robots. +> Please see additional documentation on [modeling and generating MJCFs for objects](./MODELING_TIPS.md). ## Notes > [!NOTE] -> This has some heavy non-ROS dependencies that could probably be cleaned up: +> This has some heavy non-ROS dependencies that could probably be cleaned up: * MuJoCo Python API * trimesh - Python library for loading and using triangular meshes. @@ -106,6 +108,8 @@ Of note, the test robot has a good chunk of supported functionality, and we reco * reads a robot description URDF * add in mujoco tag that provides necessary info for conversion * replace package names from `package://` to absolute filepaths + * NOTE: duplicate mesh files will have an `_N` appended to them to avoid conflicts in the output `assets` folder + * E.g. if running multiple types of UR robots in one sim, there will be multiple `shoulder.dae` files * read absolute filepaths of all meshes and convert either dae or stl to obj using trimesh * put all of these meshes into an `assets/` folder under `mjcf_data/` relative to current working dir * modify filepaths again in urdf to point to `assets/` folder @@ -123,80 +127,89 @@ so the URDF -> MJCF conversion script can pick it up and inject the correspondin the generated MJCF. See [mujoco_ros2_control_demos/demo_resources/robot/test_robot.urdf](../../mujoco_ros2_control_demos/demo_resources/robot/test_robot.urdf) for a complete example. ### Top-level container -- Use a `` element inside your xacro/URDF. The converter looks for this element + +* Use a `` element inside your xacro/URDF. The converter looks for this element and copies or processes its children into the MJCF. #### Main sub-elements -- `raw_inputs`: arbitrary MJCF XML fragments that will get copied into the generated + +* `raw_inputs`: arbitrary MJCF XML fragments that will get copied into the generated MJCF. Use this for elements that don't require conversion (for example `option`, `default`, `actuator`, `tendon`, `equality`, simple `sensor` definitions, etc.). See `test_robot.urdf`. - - Can also exclude contacts between bodies using the [`contact/exclude` tag](https://mujoco.readthedocs.io/en/stable/XMLreference.html#contact-exclude). + * Can also exclude contacts between bodies using the [`contact/exclude` tag](https://mujoco.readthedocs.io/en/stable/XMLreference.html#contact-exclude). -- `processed_inputs`: convenience tags that the converter understands and processes into valid +* `processed_inputs`: convenience tags that the converter understands and processes into valid MJCF entries. Use these when the converter must transform (or) generate MJCF elements (for example, cameras, lidar rangefinders, mesh decomposition hints, or targeted modifications). Common processed tags (supported by the demo converter): - - `decompose_mesh` (attributes: `mesh_name`, `threshold`) — request mesh decomposition when + * `decompose_mesh` (attributes: `mesh_name`, `threshold`) — request mesh decomposition when running `obj2mjcf`/decompose step; useful when large meshes must be split for more robust collision hull generation. This is particularly useful for gripper fingers and anything a robot will interact with like object handles. - - `camera` (attributes: `site`, `name`, `fovy`, `mode`, `resolution`) — instructs the + * `camera` (attributes: `site`, `name`, `fovy`, `mode`, `resolution`) — instructs the converter to add a camera in the MJCF attached to the given URDF site (frame). The converter will fill position/quaternion from the URDF link pose. `resolution` is two integers separated by a space (e.g. `640 480`). The `name` must match the `sensor` name declared in the `ros2_control` block if you plan to publish images to ROS topics. - - `lidar` (attributes: `ref_site`, `sensor_name`, `min_angle`, `max_angle`, `angle_increment`) + * `lidar` (attributes: `ref_site`, `sensor_name`, `min_angle`, `max_angle`, `angle_increment`) — generates a set of MJCF `rangefinder` sensors placed around `ref_site`. The converter rotates rangefinders about the replicate frame's Y axis between `min_angle` and `max_angle` at the step size `angle_increment`. The generated rangefinders will be named by the `sensor_name` base (e.g. `rf-01`, `rf-02`, ...); ROS-facing `sensor` entries in the `ros2_control` section should use the same base name. - - `modify_element` (attributes: `type`, `name`, ...any MJCF attributes...) — finds the + * `modify_element` (attributes: `type`, `name`, ...any MJCF attributes...) — finds the generated MJCF element by `type` (for example `joint` or `body`) and `name` and set or overwrite the provided attributes. This is useful to tweak physics properties like `frictionloss`, `stiffness`, `damping`, `gravcomp`, etc. -- `scene`: scene-level MJCF fragments such as `asset`, `worldbody`, `visual` and small scene +* `scene`: scene-level MJCF fragments such as `asset`, `worldbody`, `visual` and small scene parameters. If present the converter will merge/insert it into the MJCF scene (camera lighting, ground textures, skybox definitions, etc.). Example: the `scene` block in `test_robot.urdf` adds a `groundplane` texture and a light in the MJCF. On the other hand, `scene.xml` can be parsed to the script using `--scene` arg to the script, inorder to generate the model including the scene configuration. - - Gravity can also be changed in the MuJoCo `scene`. - - For example, for Earth gravity: + * Gravity can also be changed in the MuJoCo `scene`. + * For example, for Earth gravity: + ```xml ``` - - If we wanted to experiment in Lunar gravity (one-sixth Earth gravity): + + * If we wanted to experiment in Lunar gravity (one-sixth Earth gravity): + ```xml ``` - - Changing the simulated gravity does not affect gravity compensation in the `processed_inputs` described above. + + * Changing the simulated gravity does not affect gravity compensation in the `processed_inputs` described above. ### Sensor and ROS mapping notes -- Define ROS-facing sensors inside the `ros2_control` tag (or anywhere in the URDF that your + +* Define ROS-facing sensors inside the `ros2_control` tag (or anywhere in the URDF that your robot description consumers expect). For cameras the `sensor` name in the URDF must match the `camera` `name` you placed in `processed_inputs` so the plugin can map the MJCF camera to a ROS topic (see `test_robot.urdf`). -- Lidar: the converter produces multiple MJCF `rangefinder` sensors. The URDF `sensor` for + +* Lidar: the converter produces multiple MJCF `rangefinder` sensors. The URDF `sensor` for the lidar should provide `angle_increment`, `min_angle`, `max_angle`, `range_min`, `range_max` and `laserscan_topic` parameters. The hardware interface will combine the set of generated rangefinders into a single ROS `LaserScan` message. ### Practical tips and conventions -- Use URDF/xacro frames (links) as reference `site` locations for cameras and sensors; the + +* Use URDF/xacro frames (links) as reference `site` locations for cameras and sensors; the converter will read the link pose and attach MJCF objects accordingly. -- Keep `raw_inputs` minimal and prefer `processed_inputs` for things that need conversion or +* Keep `raw_inputs` minimal and prefer `processed_inputs` for things that need conversion or generation (meshes, cameras, rangefinders) — processed inputs document intent and are easier to maintain than dropping raw MJCF fragments into the URDF. -- When matching sensors: make sure MJCF sensor names and URDF `sensor` names align — this is +* When matching sensors: make sure MJCF sensor names and URDF `sensor` names align — this is how runtime mapping and topics are resolved. **Where to look** -- Example usage in repo: [mujoco_ros2_control_demos/demo_resources/robot/test_robot.urdf](../../mujoco_ros2_control_demos/demo_resources/robot/test_robot.urdf). -- The conversion inputs file used by the demo: [mujoco_ros2_control_demos/demo_resources/mjcf_generation/test_inputs.xml](../../mujoco_ros2_control_demos/demo_resources/mjcf_generation/test_inputs.xml). +* Example usage in repo: [mujoco_ros2_control_demos/demo_resources/robot/test_robot.urdf](../../mujoco_ros2_control_demos/demo_resources/robot/test_robot.urdf). +* The conversion inputs file used by the demo: [mujoco_ros2_control_demos/demo_resources/mjcf_generation/test_inputs.xml](../../mujoco_ros2_control_demos/demo_resources/mjcf_generation/test_inputs.xml). ### Minimal example (camera + lidar) @@ -250,35 +263,35 @@ map MJCF sensors to ROS topics. The following lists the supported `processed_inputs` tags and their attributes. These are the attributes the demo converter recognizes; converters may extend this list. -- `decompose_mesh` - - Required: `mesh_name` (string) - - Optional: `threshold` (float) — convex decomposition threshold; smaller values produce +* `decompose_mesh` + * Required: `mesh_name` (string) + * Optional: `threshold` (float) — convex decomposition threshold; smaller values produce finer decomposition. Example: ``. -- `camera` - - Required: `site` (string) — URDF link/frame name to attach the camera to. - - Required: `name` (string) — camera name in MJCF; must match URDF `sensor` name used for +* `camera` + * Required: `site` (string) — URDF link/frame name to attach the camera to. + * Required: `name` (string) — camera name in MJCF; must match URDF `sensor` name used for ROS mapping. - - Optional: `fovy` (int/float) — vertical field of view in degrees (example: `58`). - - Optional: `mode` (string) — MJCF camera mode (commonly `fixed`). - - Optional: `resolution` (string) — two integers `" "` (example: `640 480`). - - Optional: any other args supported by the mjcf [camera tag](https://mujoco.readthedocs.io/en/stable/XMLreference.html#body-camera) - - Notes: Converter fills transform (position + quaternion) from the URDF link pose. - -- `lidar` - - Required: `ref_site` (string) — URDF frame used as reference for placing rangefinders. - - Required: `sensor_name` (string) — base name for generated MJCF rangefinders (e.g. + * Optional: `fovy` (int/float) — vertical field of view in degrees (example: `58`). + * Optional: `mode` (string) — MJCF camera mode (commonly `fixed`). + * Optional: `resolution` (string) — two integers `" "` (example: `640 480`). + * Optional: any other args supported by the mjcf [camera tag](https://mujoco.readthedocs.io/en/stable/XMLreference.html#body-camera) + * Notes: Converter fills transform (position + quaternion) from the URDF link pose. + +* `lidar` + * Required: `ref_site` (string) — URDF frame used as reference for placing rangefinders. + * Required: `sensor_name` (string) — base name for generated MJCF rangefinders (e.g. `rf` will produce `rf-01`, `rf-02`, ...). URDF `sensor` entries and ROS mapping should reference the same base name. - - Required: `min_angle` (float) — start angle in radians (default depends on converter). - - Required: `max_angle` (float) — end angle in radians. - - Required: `angle_increment` (float) — angular step between generated rangefinders. - - Notes: The converter creates multiple MJCF `rangefinder` sensors across the angle range; + * Required: `min_angle` (float) — start angle in radians (default depends on converter). + * Required: `max_angle` (float) — end angle in radians. + * Required: `angle_increment` (float) — angular step between generated rangefinders. + * Notes: The converter creates multiple MJCF `rangefinder` sensors across the angle range; the hardware interface merges them into a single ROS LaserScan. -- `modify_element` - - Required: `type` (string) — MJCF element type to target (e.g. `joint`, `body`). - - Required: `name` (string) — name attribute of the MJCF element to modify. - - Additional attributes: any MJCF attributes you want to set or overwrite (for example +* `modify_element` + * Required: `type` (string) — MJCF element type to target (e.g. `joint`, `body`). + * Required: `name` (string) — name attribute of the MJCF element to modify. + * Additional attributes: any MJCF attributes you want to set or overwrite (for example `frictionloss`, `damping`, `gravcomp`, `solimp`, `solref`, ...). - - Example: ``. + * Example: ``. diff --git a/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py b/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py index 1c189a24..9cb9fc42 100644 --- a/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py +++ b/mujoco_ros2_control/mujoco_ros2_control/urdf_to_mujoco_utils.py @@ -85,8 +85,30 @@ def remove_tag(xml_string, tag_to_remove): def extract_mesh_info(raw_xml, asset_dir, decompose_dict): + """ + Builds a dictionary of all unique visual meshes in the URDF and rewrites the + raw_xml so every mesh filename points at a disambiguated path. There are some + gotchas: + + - Different URDF meshes can share a filename stem, so colliding stems get an + "__N" suffix so each resolves to its own asset directory. For example, if + running multiple UR types there will be a shoulder/ and shoulder__1/ directory. + - This happens BEFORE the pregen lookup. So if the asset_dir already + contains shoulder/ and shoulder__1/ from a prior run, the second source + will grab shoulder__1. + - stem_to_original_uri tracks which source uri claimed each stem slot, in + an effort to avoid unnecessary copying. + - A subset-equality check on (is_pre_generated, filename, scale, color) dedupes + entries that genuinely match based on those criteria. + + Returns (mesh_info_dict, rewritten raw_xml). Each entry will have + carries {is_pre_generated, filename (source path), scale, color, and + new_filepath}. + """ + robot = URDF.from_xml_string(raw_xml) mesh_info_dict = {} + stem_to_original_uri = {} robot_materials = dict() for material in robot.materials: @@ -112,14 +134,20 @@ def resolve_color(visual): continue uri = geom.filename # full URI - stem = pathlib.Path(uri).stem # filename without extension + original_stem = pathlib.Path(uri).stem # NEW: remember original for disambiguation + stem = original_stem + counter = 0 + while stem in stem_to_original_uri and stem_to_original_uri[stem] != uri: + counter += 1 + stem = f"{original_stem}__{counter}" + stem_to_original_uri[stem] = uri # 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 if asset_dir: - if stem in decompose_dict: + if original_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}/metadata.json" @@ -134,7 +162,7 @@ def resolve_color(visual): used_threshold = None # 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 + used_threshold, float(decompose_dict[original_stem]), rel_tol=1e-9 ): new_uri = mesh_file is_pre_generated = True @@ -142,7 +170,7 @@ def resolve_color(visual): else: print( f"Existing decomposed obj for {stem} has different threshold {used_threshold} " - f"than required {decompose_dict[stem]}. Regenerating..." + f"than required {decompose_dict[original_stem]}. Regenerating..." ) else: # Composed mesh: check if a pre-generated OBJ exists @@ -156,16 +184,42 @@ def resolve_color(visual): scale = " ".join(f"{v}" for v in geom.scale) if geom.scale else "1.0 1.0 1.0" rgba = resolve_color(vis) - # If the same stem appears more than once, keep the first, or change as you prefer - mesh_info_dict.setdefault( - stem, - { + mesh_dict_value = { + "is_pre_generated": is_pre_generated, + "filename": new_uri, + "scale": scale, + "color": rgba, + } + + # check to see if the values we are trying to add already exist + existing_identifier = None + for key, value in mesh_info_dict.items(): + if mesh_dict_value.items() <= value.items(): + existing_identifier = key + break + + # if the values we want to add are not in the dictionary yet, add them + if existing_identifier is None: + # get the name of the new file so that we can reference it later, but grab correct + # pre generated asset if it exists + path_obj = pathlib.Path(new_uri) + if is_pre_generated: + new_filepath = str(path_obj.parent.parent / stem / (stem + path_obj.suffix)) + else: + new_filepath = str(path_obj.parent / (stem + path_obj.suffix)) + + # add the unique name to the dictionary + mesh_info_dict[stem] = { "is_pre_generated": is_pre_generated, "filename": new_uri, "scale": scale, "color": rgba, - }, - ) + "new_filepath": new_filepath, + } + + # if we changed the identifier, make sure we update it in the underlying file + if stem != pathlib.Path(new_uri).stem: + raw_xml = raw_xml.replace(new_uri, new_filepath) return mesh_info_dict, raw_xml diff --git a/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py b/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py index ddc22a23..872da787 100755 --- a/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py +++ b/mujoco_ros2_control/scripts/make_mjcf_from_robot_description.py @@ -83,18 +83,21 @@ def convert_to_objs(mesh_info_dict, directory, xml_data, convert_stl_to_obj, dec filename_no_ext = os.path.splitext(filename)[0] filename_ext = os.path.splitext(filename)[1] full_filepath = mesh_item["filename"] + new_filepath = mesh_item["new_filepath"] if full_filepath in converted_filenames: pass - print(f"processing {full_filepath}") + print(f"processing {full_filepath} (mesh_name={mesh_name})") # if we want to decompose the mesh, put it in decomposed filepath, otherwise put it in full - if filename_no_ext in decompose_dict: - assets_relative_filepath = f"{mrc.DECOMPOSED_PATH_NAME}/{filename_no_ext}/" + # Note: we use the mesh_name as the the unique rather than filename_no_ext to avoid collisions + # for commonly named elements like on UR robots. + if mesh_name in decompose_dict or filename_no_ext in decompose_dict: + assets_relative_filepath = f"{mrc.DECOMPOSED_PATH_NAME}/{mesh_name}/" os.makedirs(f"{directory}assets/{assets_relative_filepath}", exist_ok=True) else: assets_relative_filepath = f"{mrc.COMPOSED_PATH_NAME}/" - assets_relative_filepath += filename_no_ext + assets_relative_filepath += mesh_name output_path = f"{directory}assets/{assets_relative_filepath}.obj" @@ -119,11 +122,10 @@ def convert_to_objs(mesh_info_dict, directory, xml_data, convert_stl_to_obj, dec # Export to OBJ mesh.export(output_path, include_color=True, mtl_name=mtl_name) - xml_data = xml_data.replace(full_filepath, f"{assets_relative_filepath}.obj") - + xml_data = xml_data.replace(new_filepath, f"{assets_relative_filepath}.obj") else: shutil.copy2(full_filepath, f"{directory}assets/{assets_relative_filepath}.stl") - xml_data = xml_data.replace(full_filepath, f"{assets_relative_filepath}.stl") + xml_data = xml_data.replace(new_filepath, f"{assets_relative_filepath}.stl") pass elif filename_ext.lower() == ".obj": # If import .obj files from URDF @@ -171,21 +173,18 @@ def convert_to_objs(mesh_info_dict, directory, xml_data, convert_stl_to_obj, dec for geom in scene.geometry.values(): visual = geom.visual rgba = extract_rgba(visual) - new_mat = trimesh.visual.material.SimpleMaterial( diffuse=rgba[:3], alpha=rgba[3], glossiness=1000, specular=[0.2, 0.2, 0.2], ) - visual.material = new_mat # give the material a unique name so that it can be properly referenced mtl_modifier = f"{mesh_name}" mtl_name = "mtl_" + mtl_modifier mtl_filepath = os.path.dirname(output_path) + f"/{mtl_name}" - scene.export(output_path, include_color=True, mtl_name=mtl_name) # rename textures mrc.rename_material_textures(dir_path=os.path.dirname(output_path), modifier=mtl_modifier) @@ -215,7 +214,7 @@ def convert_to_objs(mesh_info_dict, directory, xml_data, convert_stl_to_obj, dec if os.path.exists(copied_image_file): os.remove(copied_image_file) - xml_data = xml_data.replace(full_filepath, f"{assets_relative_filepath}.obj") + xml_data = xml_data.replace(new_filepath, f"{assets_relative_filepath}.obj") else: print(f"Can't convert {full_filepath} \n\tOnly stl and dae file extensions are supported at the moment") print(f"extension: {filename_ext}")