Skip to main content
Version: 8

Creating a MuJoCo Config from a URDF

To create a Mujoco world for your MoveIt Pro config to run in simulation, you will need to create a MuJoCo Modeling XML File (MJCF) that defines the robot and the environment. The easiest way to create this file is by starting with your existing URDF file.

This how-to guide will walk through the URDF to MJCF migration process.

Configuration package references:

MJCF references:

Steps​

Retrieve the rendered URDF of the robot​

  1. Run the non-MuJoCo MoveIt Pro site config:

    moveit_pro run -v

  2. Enter the MoveIt Pro docker container:

    moveit_pro shell

  3. Copy the auto-generated URDF file from the /robot_description topic to a file.

    ros2 topic echo --once --field data --full-length /robot_description | head -n -1 > ~/user_ws/robot_description.urdf

  4. You may also have to rectify the filename paths for the mesh files. Example:

    • From: <mesh filename="file:///home/USERNAME/user_ws/install/kortex_description/share/kortex_description/arms/gen3/7dof/meshes/base_link.dae"/>
    • To: <mesh filename="package://kortex_description/arms/gen3/7dof/meshes/base_link.dae"/>

Create the MuJoCo config​

Create a new config based on the site config called [robot_name]_mujoco_config and create the [robot_name]_mujoco_config/description/mujoco directory.

Inside of the mujoco directory create and populate the following:

  • assets directory:
    • Add in all of the .stl collision files for the robot and gripper.
    • If the visual files are currently in .stl or .dae files, use Blender to convert them to .obj files.
      • NOTE 1: Make sure the axes are aligned correctly when you export the obj files from blender. You may need to change the "Up Axis" to Z and the "Forward Axis" to X when you export.
      • NOTE 2: May need to use the obj2mjcf tool to create .obj files that have material information embedded in them. This tool can also perform convex mesh decomposition if needed.
  • [robot_name]_mujoco.xml MJCF file:
    • You can convert a URDF to MJCF in XML via either of the following 2 methods:
      1. Using the MuJoCo binaries, which can be installed via the instructions in the MuJoCo Getting started docs: ./compile <robot_name>.urdf <robot_name>.xml
      2. Using the MuJoCo pip package, which can be installed via pip install mujoco: python3 -c "import mujoco; model = mujoco.MjModel.from_xml_path('<robot_name>.urdf'); mujoco.mj_saveLastXML('<robot_name>.xml', model)"
    • ROS 2 package resolution does not work, so you may have to do some find and replace if your URDF uses ROS style references to mesh locations.
    • Confirm all the links from the robot.urdf file are present.
    • Use .stl assets to define collision geometries and obj files to define visual geometries.
    • Add actuators and tune.
    • NOTE: You can preview your robot model and tune the actuators using python3 -m mujoco --mjcf=/path/to/robot.xml
  • scene.xml MJCF file.
    • Include robot MJCF file.
    • Add sites if the simulation includes a scene camera.

The file structure beneath the description directory should resemble the following:

description
├── mujoco
│   ├── assets
│   │   ├── base_link.obj
│   │   ├── base_link.STL
│   │   └── ...
│   ├── robot.xml
│   └── scene.xml
└── robot_description.xacro

Use the MujocoSystem plugin in your robot description Xacro file​

Change the xacro defining your robot's <ros2_control>/<hardware> tag to use the picknik_mujoco_ros/MujocoSystem plugin. Refer to the example in our lab_sim example here.

We recommend exposing mujoco_model as a xacro argument to enable quickly switching the robot between simulated worlds.

Optional Params​

MuJoCo Interactive Viewer​

You can launch MuJoCo's interactive viewer (in passive mode) on startup and safely use it in parallel with commands sent by your Objectives. Do this by setting the mujoco_viewer parameter to true in the above MujocoSystem plugin.

Learn more about the interactive viewer's capabilities in this video.

Hand of God

MujoCo Keyframes​

You can start the simulation with a specific keyframe (including initial joint values) as specified in the MJCF file:

  1. Capture the keyframe the qpos entry for a keyframe by launching with the MuJoCo interactive viewer, expanding the Simulation left sidebar, and clicking Copy pose.

  2. Paste the keyframe output to your MJCF file. Optionally add a name if you paste in multiple keyframes.

      <keyframe>
        <key
          name='zeros'
          qpos='0 0 0 0 0 0'
        />
        <key
          name='ones'
          qpos='1 1 1 1 1 1'
        />
      </keyframe>
      ...
    </mujoco>

    Some configurations may also require recording the ctrl values, but this is rare.

  3. If keyframes are specified in the MJCF, the simulation will default to the first keyframe.

  4. You can launch a specific, named keyframe by referencing it in the xacro defining your robot's <ros2_control>/<hardware> tag:

      <ros2_control>
        <hardware>
          <plugin>picknik_mujoco_ros/MujocoSystem</plugin>
          ...
          <param name="mujoco_keyframe">ones</param>
        </hardware>
      </ros2_control>

    Alternatively, using the interactive viewer, you can load a specific keyframe by expanding the Simulation left sidebar, dragging the Key value to the index of the keyframe you want to load, and clicking Load Key.

tip

For complex scenes with many objects, the keyframe can be quite large. The starting joint values of the robot will be the last X items in the keyframe qpos, where X is the ordered list of joints in the interactive viewer's Joint right sidebar. You can use this knowledge to quickly iterate additional keyframes from an initial keyframe.

Ignore Joint Validation​

  • By default, the simulation enforces that every joint in the URDF exists in the MJCF model. You may want to skip this in cases where the joint isn't intended to be simulated, e.g. mock joints.

  • A list of joints to ignore can be specified using the ignore_joint_validation parameter.

      <ros2_control>
        <hardware>
          <plugin>picknik_mujoco_ros/MujocoSystem</plugin>
          ...
          <param name="ignore_joint_validation">linear_x_joint linear_y_joint rotational_yaw_joint</param>
        </hardware>
      </ros2_control>

Simulated Sensors​

note

All <site> tags with a name attribute defined in the MJCF file will be picked up by the MuJoCo ros2_control plugin and published to tf2. The transforms will be published in world frame. Like a physical sensor, our simulated sensors do not know where they are mounted and do not include this transform information along with their sensor data. A controller and/or broadcaster is typically configured with the additional information of what frame the sensor has been mounted at.

Simulated Force Torque Sensor​

  1. Add a site for the FTS pose in the MJCF file. The simulator will publish the transform of the site at the rate defined by tf_publish_rate. Refer to the tcp_fts_sensor site in our lab_sim example here.
  2. Add <force> and <torque> tags referencing the site in the MJCF file. The created hardware interface will have the same name as the site. Refer to the tags referencing tcp_fts_sensor in our lab_sim example here.
  3. Optionally, add a FTS broadcaster to your ros2_control configuration yaml to publish the values to a ROS topic. Refer to the force_torque_sensor_broadcaster declaration and definition in our lab_sim example here.
  4. Optionally, add a VelocityForceController to your ros2_control configuration yaml to directly leverage the simulated hardware interface. Refer to the velocity_force_controller declaration and definition in our lab_sim example here.

Simulated Depth Camera Sensor​

  1. Add a <camera> sensor tag and a <site> tag corresponding to the camera frame and camera optical frame, respectively. Refer to the wrist_camera and wrist_camera_optical_frame tags in our lab_sim example here.

    Camera Frame Convention

    • Positive X points to right of camera.
    • Positive Y points up towards the top of the camera.
    • Positive Z points to behind the camera.

    Camera Optical Frame Convention

    The camera optical frame should be set to the <camera> position rotated 180 degrees about its x-axis.

  2. Add a render_publish_rate param in the ros2_control hardware section of the robot description file. Refer to the example in our lab_sim example here.

tip

When using MuJoCo's interactive viewer, you can quickly obtain the <camera> and <site> definitions for a scene camera by adjusting your viewpoint and copying the equivalent camera pose using the Copy camera button under the Rendering tab on the left hand side of the viewer. You can paste the camera position in your XML description, which will contain a position (pos) and orientation (xyaxes). Be sure to add the name, fovy, mode, and resolution, as shown in the example above. To determine the camera optical frame, multiply the camera xyaxes orientation by 1 1 1 -1 -1 -1 and use the same position as the camera.

Simulated Lidar Sensor​

  1. Add a site for each lidar laser angle using the <replicate> tag in the MJCF along with a <rangefinder>. Ex:

      <replicate count="360" sep="-" offset="0 0 0" euler="0.0174533 0 0">
          <site name="lidar" size="0.01" pos="-0.075 0.0 0.0" quat="0.0 0.0 0.0 1.0"/>
      </replicate>
    ...
      <sensor>
        <rangefinder site="lidar" user="0.1 -3.14159 3.14159 0.0174533 0.1 6.0"/>
      </sensor>

  2. Add a lidar_publish_rate param in the ros2_control hardware section of the robot description file. Ex:

      <ros2_control>
        <hardware>
          <plugin>picknik_mujoco_ros/MujocoSystem</plugin>
          ...
          <param name="lidar_publish_rate">10</param>
        </hardware>
      </ros2_control>

Other configuration package changes​

  • In the [robot_name]_mujoco_config/config/config.yaml file, make sure to change all package references from [robot_name]_base_config to [robot_name]_mujoco_config.

XML files in multiple packages​

Mujoco does not have the equivalent of URDF’s of package:// package finding. The location of the top level XML defines the location from which all file references are made. The simulate binary does not even respect the folder structures in XML include tags, so when first building your XML it’s easiest to just put all the STL and include XMLs in the same folder.

The picknik_accessories package contains reusable xml scenes and components. To use them with an XML in another package, the symlinks from picknik_accessories can be copied over to the package’s install folder that contains the top level XML. See here.

Troubleshooting​

See our Digital Twin Troubleshooting page here.

Naming conventions​

We will be generating a lot of files that have similar names with slightly different names.

ROBOT_COMPONENT_macro.urdf.xacro

  • For example picknik_2f_gripper_macro.urdf.xacro
  • "macro" indicates this file only includes xacro macros and including the file will not generate any xml without running one of its macros
  • "urdf" indicates that xacro will be generating URDF xml, as opposed to ros2_control xml tags

ROBOT_model.urdf.xacro

  • For example picknik_tridex_model.urdf.xacro where the robot model is the "PickNik Tridex"
  • "model" indicates this is a "top level" xacro file that will generate the xml for an entire robot when invoked or included

ROBOT_DETAILED.xml

  • For example picknik_tridex_2f.xml
  • Xacro enables you to pass arguments to easily generate variations of a robot model, for example one that only has vacuum tools and one that only has two finger tools. Formats that capture a specific arrangement of the robot, such as URDF, MoveIt SRDF, ros2_control controllers, and Mujoco xml, should include the details in their name to reduce confusion.

Source of moveit and ros2_control configuration files​

Should they be in the ROBOT_sim package? Or the _description package?
The place(s) for these might vary. Some manufacturers might only provide a basic ROS package with xacro and mesh files for the URDF. Others might only provide ros2_control/ files, while others might provide files for ros2_control, moveit, and nav.

If you are combining multiple pieces of hardware (e.g. a COTS arm from one vendor and a COTS arm from another vendor), you will likely have the config files for the combined system in your ROBOT_sim package.

If you are making your own hardware, then you might have a _description package with all of your xacro, mesh, and config file info there. In that case, you will only have the MoveIt Pro config.yaml file in your ROBOT_sim's config/ folder.

1. Setting up Xacro from URDF​

We start with just URDF and meshes

~/user_ws contents:

urdf/
  random_name.urdf
meshes/
  base_link.stl
  link1.stl

a. Optionally install git lfs​

Good for when we are dealing with mesh files

b. Copy Docker and ROS 2 boilerplate package with rviz viewing launch file​

Clone our ROS 2 boilerplate and move the files into their corresponding directories.

Additionally, add a world link and connect it to the base link of your robot.

You can now previous and test out joint motion via rviz.

🐙
moveit_pro configure
moveit_pro build
ros2 launch ROBOT_description view_robot.launch

Test that joint limits and range of motion make sense.

~/user_ws contents:

Dockerfile
docker-compose.yaml
ROBOT_description/
  description/
    meshes/
      base_link.stl
      link1.stl
  urdf/
    random_name.urdf #now with world to base_link joint
  launch/
    view_robot.launch # from URDF
    view_robot.rviz
  CMakeLists.txt
  package.xml
  LICENSE
  READEME.md
  .gitattributes # Set up for mesh files at least

c. Convert URDF to Xacro file(s)​

Not always required, but very common. Especially if you want any sort of modularity for

  • what ros2_control should expect to talk to (mujoco sim v. hardware)
  • end effector type (if you have some robots with vacuum grippers, some with finger grippers, etc)

Generate a URDF from your xacro file(s):

🐙
colcon build --packages-select ROBOT_description
xacro ~/user_ws/ROBOT_description/urdf/ROBOT_model.urdf.xacro arg:=value > ~/user_ws/ROBOT_description/urdf/ROBOT_DETAILED.test.urdf

Update the view_robot.launch to use your xacro, then launch and verify that the xacro joints still move as expected. If you have a dual arm robot, you may need to introduce a "reflect" parameter.

~/user_ws contents:

Dockerfile
docker-compose.yaml
ROBOT_description/
  description/
    meshes/
      base_link.stl
      link1.stl
  urdf/
    ROBOT_model.urdf.xacro
    ROBOT_COMPONENT_macro.urdf.xacro
    random_name.urdf # to be deleted
  launch/
    view_robot.launch # now loads .xacro instead of .urdf
    view_robot.rviz
  CMakeLists.txt
  package.xml
  LICENSE
  READEME.md

2. Setting up Mujoco xml from URDF​

a. Install Mujoco Binaries​

Install https://mujoco.readthedocs.io/en/stable/programming/index.html#getting-started in your ~/user_ws/

~/user_ws contents:

mujoco-3.3.1/
  bin/
  ...

b. Move files into a flat directory​

Mujoco currently does not have a built-in mechanism for resolving package:// URIs. The provided ./compile tool we will use here will strip the package and relative file path from items in a ROS package specified via package URI, such as meshes. To work around this, we will copy everything into a flat directory at ~/user_ws/temp to do the initial conversion.

This conversion guide currently only covers using a single set of meshes for both collision and visualization in Mujoco. If your URDF uses different meshes for the visual and collision settings, copy the meshes used for collisions.

🐙
cd ~/user_ws
mkdir temp
find . -name "meshes" -type d
cp ./src/ROBOT_description/meshes/collision/* temp/
cp ./src/ROBOT_description/urdf/ROBOT_DETAILED.test.urdf temp/

~/user_ws contents:

temp/
  ROBOT_DETAILED.test.urdf
  base_link.STL
  link1.stl
  ...

c. Start ./compile loop​

Now we will start running the "compile" binary and resolve some possible warnings and errors. At the end of this step we will have the beginning of a functional mujoco model.

🐙
~/user_ws/mujoco-3.3.1/bin/compile ~/user_ws/temp/ROBOT_DEAILED.test.urdf
~/user_ws/temp/ROBOT_DETAILED.xml

Possible WARNING: Geom with duplicate name​

warning

Geom with duplicate name '' encountered in URDF, creating an unnamed geom.

This is expected, as most URDFs do not name these tags.

<collision name="link1_collision">
  <geometry>
    <box size="1 1 1"/>
  </geometry>
</collision>

<visual name="link1_visual">
  <geometry>
    <box size="1 1 1"/>
  </geometry>
</visual>

We do not need mujoco to name the geoms so this is fine.

Possible ERROR: Error opening file​

danger

Error opening file 'temp/base_link.STL': No such file or directory

This is typically a sign you forgot to copy a file into your flat "temp" directory.

Possible ERROR: Mesh decimation​

To improve collision checking performance, Mujoco limits the number of faces that can be in a mesh file.

danger

🐙 Error: number of faces should be between 1 and 200000 in STL file 'temp/base_link.stl'; perhaps this is an ASCII file? Element name 'body', id 0

We have a built in tool for decimating meshes to reduce the number of faces. You may need to run it multiple times for large files.

🐙
cd ~/user_ws/temp/
decimate_mesh base_link.stl base_link.stl

Possible ERROR: Missing inertia​

Some URDF generating tools may only generate partial dynamics information in the exported URDF. This is important information for physics engines and must be included.

warning

Geom with duplicate name '' encountered in URDF, creating an unnamed geom. Error: mass and inertia of moving bodies must be larger than mjMINVAL Element name 'left_arm_wrist_link_x', id 11

Either:

  • Generate missing values in CAD software and copy over to xacro
    • Regenerate URDF in temp folder
  • If the missing values cannot be easily generated, delete the partially populated inertial tag. Mujoco will by default provide a guess of the values via inertiafromgeom. You can use these values initially and resolve the inaccuracies later.
    • You can get a sanity check of the inertia values it generates via the Inertia Rendering capability.
    • TODO would be nice to find a way to extract these estimates, so we could have the inertia matrix, but set our own mass estimate (to a low value) to make the actuators more responsive.

Inertia Rendering

Change

<link name="left_arm_wrist_link_x"/>

To

<link name="left_arm_wrist_link_x">
   <inertial>
     <origin rpy="0 0 0" xyz="0 0 0"/>
     <mass value="0.05"/>
     <inertia ixx="1.0E-05" ixy="0" ixz="0" iyy="1.0E-05" iyz="0" izz="1.0E-05"/>
   </inertial>
 </link>

Iterate through ./compile until all these errors are resolved the xml file is generated

~/user_ws contents:

Dockerfile
docker-compose.yaml
ROBOT_description/
  description/
    meshes/
      base_link.stl # decimated mesh
      link1.stl # decimated mesh
  urdf/
    ROBOT_model.urdf.xacro # new inertia values
    ROBOT_COMPONENT_macro.urdf.xacro # new inertia values
  launch/
    view_robot.launch # from xacro
    view_robot.rviz
  CMakeLists.txt
  package.xml
  LICENSE
  READEME.md
temp/
  ROBOT_DETAILED.test.urdf # with new inertia values
  base_link.stl # decimated mesh
  link1.stl # decimated mesh
  ROBOT_DETAILED.xml # hopefully

d. Simulate your robot and watch it fall​

Next we will use the mujoco simulate binary to start making actuation work

🐙
~/user_ws/mujoco-3.3.1/bin/simulate ~/user_ws/temp/ROBOT_DETAILED.xml

This should launch the mujoco viewer without any errors in the UI, but the robot will immediately flop and hang under its base link.

e. Add gravcomp to bodies​

To simulate arms with brakes that hold the arm in place when it is not actively commanded, we add gravity compensation to robot bodies. This also makes tuning actuator strength simpler.

This script will automatically add gravity compensation to all links. This is fine as you should only have robot bodies (we still want gravity to act normally on elements of the environment).

🐙
~/user_ws/helper-scripts/add-gravcomp.sh ~/user_ws/temp/ROBOT_DETAILED.xml

You can hit "Reload" in the left sidebar of the simulator to reload the xml file - no need to exit and relaunch. ADD PIC

Now the robot should not fall over. However, we can not yet command the robot - the Control entries in the right sidebar will be empty

Control

~/user_ws contents:

helper-scripts/
  add-gravcomp.sh
temp/
  ROBOT_DETAILED.test.urdf
  base_link.stl
  link1.stl
  ROBOT_DETAILED.xml # now with gravcomp

f. Use implicitfast integration​

The default Euler integration typically does not perform well for the instructions in this guide. Use implicitfast integration with multi contact consideration.

<mujoco>
  <option integrator="implicitfast">
    <flag multiccd="enable" />
  </option>
...
helper-scripts/
  add-gravcomp.sh
temp/
  ROBOT_DETAILED.test.urdf
  base_link.stl
  link1.stl
  ROBOT_DETAILED.xml # now with implicitfast and multiccd

g. Add & tune actuators​

Again, if your xml file only contains robot elements, and not environment elements, then it is easy to extract every joint and then make a corresponding actuator for it.

🐙
~/user_ws/helper-scripts/extract_joints.sh ~/user_ws/temp/ROBOT_DETAILED.xml ~/user_ws/temp/extracted-joints.xml

Paste output into your ROBOT_DETAILED.xml

🐙
<joint name="feet_joint" pos="0 0 0" axis="0 0 -1" range="-3.14 3.14"/>
...

Reformat to be actuators

<mujoco>
  <actuator>
    <position joint="feet_joint" name="feet_joint" ctrlrange="-3.14 3.14" kp="1000" forcelimited="false" dampratio="1"/>
  ...
  </actuator>
</mujoco>
  • <joint name= ➡️ <position joint=
  • range= ➡️ ctrlrange=
  • add name="" matching joint=""
  • /> ➡️ kp="1000" forcelimited="false" dampratio="1"/>

Now click Reload in the left sidebar
The right sidebar's "Control" entry should now have entries.
Command some values, see how well it tracks and settles
Control

Recommendation: kp and dampratio tuning - still testing out these recs​

  • Small robot arms / fingers:
    • kp: 10-100
    • dampratio: 1-1.5
  • Medium-scale limbs / manipulators:
    • kp: 100-500
    • dampratio: 0.7-1
  • Large or high-inertia systems:
    • kp: 500-2000
    • dampratio: 0.6-0.9
  • Start with 1.0 (critical damping)??? YES? NO?. It's a good baseline.
    • If you want faster response, reduce dampratio (e.g. 0.5).
    • If you see oscillations or jitter, increase it toward 1.2–1.5.
    • Keep consistent units: since dampratio is used alongside kp, make sure kp is realistic for your joint scale and mass.

After making changes, Reload and re-test.

Recommendation: Remove body joint force limits​

info

If the robot is still very weak after increasing kp to >10,00, it may be because your Mujoco joints have limits on the force that can be applied to them (by the actuators) which are too conservative. Check for "actuatorfrcrange" attributes in your joint entries. These are generated if your URDF joint entries have a non-zero effort limit, for example below:

 <joint name="feet_joint" type="revolute">
    <origin rpy="-3.1416 0 0" xyz="0 0 0.0156"/>
    <parent link="base_link"/>
    <child link="feet_link"/>
    <axis xyz="0 0 -1"/>
    <limit effort="100" lower="-3.14" upper="3.14" velocity="0"/>
  </joint>

Consider increasing or removing these “actuatorfrcrange” statements from the Xacro and XML.

Note to self: I have not converted a URDF with effort tags myself yet.

Recommendation: Set mass to low value​

I see this in some of our configs, how often is this needed?

Possible ERROR: colliding adjacent meshes​

If certain actuators do not move at all despite the control value sent, it could be colliding with another link. Enable "Contact Point" visualization as well as "Convex Hull" visualization. Look for a cylinder as you command the joint, which indicates a collision.

Contact Point

There are two ways to resolve this
1. via exclude (body to body) tag

<mujoco>
  <contact>
    <exclude body1="base_link" body2="J1_link" />
    <exclude body1="fork" body2="J5_link" />
    <exclude body1="fork" body2="J6_link" />
    <exclude body1="fork" body2="head" />
  </contact>
</mujoco>

2. via contype and conaffinity tags (for things attached to "world" - geom to geom)
URDF meshes connected to one another via a urdf fixed joint are combined under the same mujoco body by the compile tool. This means your base_link, and any other fixed link attached to it, are all part of the mujoco "world" body, as will be any environment asset. We still want assets in the environment to interact with the base link, so we use contype and conaffinity, which can be set per-geom instead of per-body.

<mujoco>
  <worldbody>
    <geom type="mesh" rgba="0.898039 0.917647 0.929412 1" mesh="base_link" contype="1" conaffinity="2"/>
    <body name="feet_link" pos="0 0 0.0156" quat="-3.67321e-06 -1 0 0" gravcomp="1">
      <inertial pos="3.959e-07 -0.031579 -0.19136" quat="0.694431 -0.133517 0.133511 0.694344" mass="43.789" diaginertia="0.453199 0.385 0.281311"/>
      <joint name="feet_joint" pos="0 0 0" axis="0 0 -1" range="-3.14 3.14"/>
      <geom type="mesh" rgba="0.89804 0.91765 0.92941 1" mesh="feet_link" contype="1" conaffinity="2"/>

Alternatively, we could wrap the base_link in its own body and use the exclude tag approach.

Iterate through these recommendations and you should end up with responsive actuators.

~/user_ws contents:

helper-scripts/
  add-gravcomp.sh
temp/
  ROBOT_DETAILED.test.urdf
  base_link.stl
  link1.stl
  ROBOT_DETAILED.xml # now with working actuators

h. Check for meshes that were not convex hulls and are not poorly converted​

Left sidebar ---> header ??? ---> Convex Hulls
Rendering
Make sure convex hulls make sense.
Otherwise decompose - we have a guide here for that.

ROBOT_description/
  description/
   ROBOT_DETAILED.xml
    meshes/
      base_link.stl
      link1.stl # decomposed mesh
temp/
  ROBOT_DETAILED.test.urdf
  base_link.stl
  link1.stl # decomposed mesh
  ROBOT_DETAILED.xml

i. Configure robot sensors​

FTS
wrist cameras
etc
https://docs.picknik.ai/how_to/configuration_tutorials/migrate_to_mujoco_config/#simulated-sensors

temp/  
  ROBOT_DETAILED.test.urdf  
  base_link.stl  
  link1.stl  
  ROBOT_DETAILED.xml # now with sensors

j. Copy mujoco xml and decimated/decomposed meshes back to ROBOT_description​

ROBOT_description/  
  description/  
   ROBOT_DETAILED.xml  
    meshes/  
      base_link.stl # decimated mesh  
      link1.stl # decimated mesh  
  urdf/  
    ROBOT_model.urdf.xacro # new inertia values  
    ROBOT_COMPONENT_macro.urdf.xacro # new inertia values  
  launch/  
    view_robot.launch  
    view_robot.rviz  
  CMakeLists.txt  
  package.xml  
  LICENSE  
  READEME.md

3. Set up ros2_control and MoveIt config files​

a. Make ros2_control macro xacro for urdf tags​

Should be a way to leverage \~/user_ws/helper-scripts/extract_joints.sh \~/user_ws/ROBOT_description/description/ROBOT_DETAILED.xml
to auto generate this so it isn't manual.

~/user_ws contents:

Dockerfile
docker-compose.yaml
ROBOT_description/
  description/
    meshes/
      base_link.stl
      link1.stl
    ROBOT_DETAILED.xml
  urdf/
    ROBOT_model.urdf.xacro # now calls ros2_control xacro macro
    ROBOT_COMPONENT_macro.urdf.xacro
    ROBOT_macro.ros2_control.xacro
  launch/
    view_robot.launch
    view_robot.rviz
  CMakeLists.txt
  package.xml
  LICENSE
  READEME.md
helper-scripts/
  add-gravcomp.sh
  extract_joints.sh

b. Make ros2_control controllers file​

Manually create and populate with Pro controllers

~/user_ws contents:

Dockerfile
docker-compose.yaml
ROBOT_description/
  config/ # or ROBOT_DETAILED_config/ if you expect to have variations of gripper type, etc
    ros2_control/
      controllers.yaml
  description/
    meshes/
      base_link.stl
      link1.stl
    ROBOT_DETAILED.xml
  urdf/
    ROBOT_model.urdf.xacro # now calls ros2_control xacro macro
    ROBOT_COMPONENT_macro.urdf.xacro
    ROBOT_macro.ros2_control.xacro
  launch/
    view_robot.launch
    view_robot.rviz
  CMakeLists.txt
  package.xml
  LICENSE
  READEME.md

c. Generate MoveIt SRDF ACM​

Two options

  1. Use MSA - create the .moveit-assistant or whatever the file is called so it will let you save your changes. Only use the SRDF tab and only save the new SRDF file - no other changes.
  2. Use CLI - Watch out for https://github.com/moveit/moveit2/issues/2915
🐙
ros2 run moveit_setup_assistant collisions_updater --urdf <PATH_TO_URDF> --srdf <PATH_TO_OLD_SRDF> --output out.srdf --trials 100000 --default --always --min-collision-fraction 0.99 --verbose

~/user_ws contents:

Dockerfile
docker-compose.yaml
ROBOT_description/
  config/
    moveit/
      groups.srdf # Just ACM
    ros2_control/
      controllers.yaml
  description/
    meshes/
      base_link.stl
      link1.stl
    ROBOT_DETAILED.xml
  urdf/
    ROBOT_model.urdf.xacro
    ROBOT_COMPONENT_macro.urdf.xacro
    ROBOT_macro.ros2_control.xacro
  launch/
    view_robot.launch # from xacro
    view_robot.rviz
  CMakeLists.txt
  package.xml
  LICENSE
  READEME.md

d. Manually define groups in SRDF​

~/user_ws contents:

Dockerfile
docker-compose.yaml
ROBOT_description/
  config/
    moveit/
      groups.srdf # Now with group definitions
    ros2_control/
      controllers.yaml
  description/
    meshes/
      base_link.stl
      link1.stl
    ROBOT_DETAILED.xml
  urdf/
    ROBOT_model.urdf.xacro
    ROBOT_COMPONENT_macro.urdf.xacro
    ROBOT_macro.ros2_control.xacro
  launch/
    view_robot.launch # from xacro
    view_robot.rviz
  CMakeLists.txt
  package.xml
  LICENSE
  READEME.md

e. Optional? Not needed? Add passive_joint entries to srdf​

Only needed for nav bases?
https://docs.picknik.ai/how_to/configuration_tutorials/add_nav2/

f. Manually create ompl file​

~/user_ws contents:

Dockerfile
docker-compose.yaml
ROBOT_description/
  config/
    moveit/
      groups.srdf
      ompl_planning.yaml
    ros2_control/
      controllers.yaml
  description/
    meshes/
      base_link.stl
      link1.stl
    ROBOT_DETAILED.xml
  urdf/
    ROBOT_model.urdf.xacro
    ROBOT_COMPONENT_macro.urdf.xacro
    ROBOT_macro.ros2_control.xacro
  launch/
    view_robot.launch # from xacro
    view_robot.rviz
  CMakeLists.txt
  package.xml
  LICENSE
  READEME.md

g. Manually create PoseIK kinematics file​

~/user_ws contents:

Dockerfile
docker-compose.yaml
ROBOT_description/
  config/
    moveit/
      groups.srdf
      ompl_planning.yaml
      pose_ik_distance.yaml 
    ros2_control/
      controllers.yaml
  description/
    meshes/
      base_link.stl
      link1.stl
    ROBOT_DETAILED.xml
  urdf/
    ROBOT_model.urdf.xacro
    ROBOT_COMPONENT_macro.urdf.xacro
    ROBOT_macro.ros2_control.xacro
  launch/
    view_robot.launch # from xacro
    view_robot.rviz
  CMakeLists.txt
  package.xml
  LICENSE
  READEME.md

h. Create servo.yaml file​

I copy lab_sim and start manually replacing things.

~/user_ws contents:

Dockerfile
docker-compose.yaml
ROBOT_description/
  config/
    moveit/
      groups.srdf
      ompl_planning.yaml
      pose_ik_distance.yaml 
      servo.yaml
    ros2_control/
      controllers.yaml
  description/
    meshes/
      base_link.stl
      link1.stl
    ROBOT_DETAILED.xml
  urdf/
    ROBOT_model.urdf.xacro
    ROBOT_COMPONENT_macro.urdf.xacro
    ROBOT_macro.ros2_control.xacro
  launch/
    view_robot.launch # from xacro
    view_robot.rviz
  CMakeLists.txt
  package.xml
  LICENSE
  READEME.md

i. Create joint_limits.yaml file​

Can probably write a script to use the extracted joints to make these.

~/user_ws contents:

Dockerfile
docker-compose.yaml
ROBOT_description/
  config/
    moveit/
      groups.srdf
      joint_limits.yaml
      ompl_planning.yaml
      pose_ik_distance.yaml 
      servo.yaml
    ros2_control/
      controllers.yaml
  description/
    meshes/
      base_link.stl
      link1.stl
    ROBOT_DETAILED.xml
  urdf/
    ROBOT_model.urdf.xacro
    ROBOT_COMPONENT_macro.urdf.xacro
    ROBOT_macro.ros2_control.xacro
  launch/
    view_robot.launch # from xacro
    view_robot.rviz
  CMakeLists.txt
  package.xml
  LICENSE
  READEME.md

4. Set up Pro config files​

a. Copy over ROBOT_sim boilerplate package​

~/user_ws contents:

ROBOT_sim/
  config/
    config.yaml
  description/
    meshes/
  launch/
    agent_bridge.launch
    moveit_pro.launch.xml
  objectives/
  waypoints/
    waypoints.yaml
  CMakeLists.txt
  package.xml
  LICENSE
  README.md
  .gitattributes

b. Create ROBOT_sim TASK-scene.xml​

<mujoco model="gen3_proto1">
  <option integrator="implicitfast">
    <flag multiccd="enable" />
  </option>

  <include file="ROBOT_DETAILED.xml" />

  <compiler angle="radian"/>
  <asset>
    <texture
      type="2d"
      name="groundplane"
      builtin="checker"
      mark="edge"
      rgb1="0.2 0.3 0.4"
      rgb2="0.1 0.2 0.3"
      markrgb="0.8 0.8 0.8"
      width="300"
      height="300"
    />
    <material
      name="groundplane"
      texture="groundplane"
      texuniform="true"
      texrepeat="5 5"
      reflectance="0.2"
    />
  </asset>
  <statistic center="0.3 0 0.4" extent="0.8" />
  <visual>
    <headlight diffuse="0.6 0.6 0.6" ambient="0.1 0.1 0.1" specular="0 0 0" />
    <rgba haze="0.15 0.25 0.35 1" />
    <global azimuth="120" elevation="-20" />
    <map zfar="50" />
  </visual>
  <worldbody>
    <light pos="0 0 2.5" dir="0 0 -1" directional="true" />
    <geom
      name="floor"
      pos="0 0 0"
      size="0 0 0.05"
      type="plane"
      material="groundplane"
    />
  </worldbody>
</mujoco>

~/user_ws contents:

Dockerfile
docker-compose.yaml
ROBOT_description/
  ROBOT_DETAILED_config/
    moveit/
      groups.srdf
      joint_limits.yaml
      ompl_planning.yaml
      pose_ik_distance.yaml 
      servo.yaml
    ros2_control/
      controllers.yaml
  description/
    meshes/
      base_link.stl
      link1.stl
    ROBOT_DETAILED.xml
  urdf/
    ROBOT_model.urdf.xacro
    ROBOT_COMPONENT_macro.urdf.xacro
    ROBOT_macro.ros2_control.xacro
  launch/
    view_robot.launch # from xacro
    view_robot.rviz
  CMakeLists.txt
  package.xml
  LICENSE
  READEME.md
helper-scripts/
  add-gravcomp.sh
  extract_joints.sh
ROBOT_sim/
  config/
    config.yaml
  description/
    TASK-scene.xml
  launch/
    agent_bridge.launch
    moveit_pro.launch.xml
  objectives/
  waypoints/
    waypoints.yaml
  CMakeLists.txt
  package.xml
  LICENSE
  README.md

Optional - add scene geometry​

Tables, widget, doodads, etc

~/user_ws contents:

Dockerfile
docker-compose.yaml
ROBOT_description/
  config/
    moveit/
      groups.srdf
      joint_limits.yaml
      ompl_planning.yaml
      pose_ik_distance.yaml 
      servo.yaml
    ros2_control/
      controllers.yaml
  description/
    meshes/
      base_link.stl
      link1.stl
    ROBOT_DETAILED.xml
  urdf/
    ROBOT_model.urdf.xacro
    ROBOT_COMPONENT_macro.urdf.xacro
    ROBOT_macro.ros2_control.xacro
  launch/
    view_robot.launch # from xacro
    view_robot.rviz
  CMakeLists.txt
  package.xml
  LICENSE
  READEME.md
helper-scripts/
  add-gravcomp.sh
  extract_joints.sh
ROBOT_sim/
  config/
    config.yaml
  description/
    meshes/
      interesting_object.stl
    TASK-scene.xml
  launch/
    agent_bridge.launch
    moveit_pro.launch.xml
  objectives/
  waypoints/
    waypoints.yaml
  CMakeLists.txt
  package.xml
  LICENSE
  README.md

Optional - configure scene camera​

https://docs.picknik.ai/how_to/configuration_tutorials/migrate_to_mujoco_config/#simulated-depth-camera-sensor

Optional - configure keyframes​

https://docs.picknik.ai/how_to/configuration_tutorials/migrate_to_mujoco_config/#mujoco-keyframes

c. Configure ROBOT_sim CMakeLists.txt to install ROBOT_description mujoco xml & mesh files into its description and meshes folder​

# Specify all packages we will install files from here to populate '_DIR' path
find_package(ROBOT_description REQUIRED)

...

# Mujoco models in this package use relative paths to assets such as meshes and textures
# To simplify building relative paths to assets in other packages, we will copy those assets into this project's file structure

# Paths we will copy from - the assets we want to easily reference in our Mujoco model
# set(PICKNIK_ACCESSORIES_PATH "${picknik_accessories_DIR}/../mujoco_assets/")
set(ROBOT_MESHES_PATH "${ROBOT_description_DIR}/../meshes/")
set(ROBOT_MODEL_PATH "${ROBOT_description_DIR}/../description/")

# Path we will copy to - the common folder in this project to copy all Mujoco related files to
set(MUJOCO_MESHES_DIR "share/${PROJECT_NAME}/meshes/")
set(MUJOCO_MODEL_DIR "share/${PROJECT_NAME}/description/")

message(STATUS"ROBOT_MESHES_PATH ${ROBOT_MESHES_PATH}")
message(STATUS"ROBOT_MODEL_PATH ${ROBOT_MODEL_PATH}")
message(STATUS"MUJOCO_MESHES_DIR ${MUJOCO_MESHES_DIR}")
message(STATUS"MUJOCO_MODEL_DIR ${MUJOCO_MODEL_DIR}")

# Copy all contents into the installed description folder
install(DIRECTORY "${ROBOT_MESHES_PATH}"
        DESTINATION "${MUJOCO_MESHES_DIR}"
        FILES_MATCHING PATTERN "*")
install(DIRECTORY "${ROBOT_MODEL_PATH}"
        DESTINATION "${MUJOCO_MODEL_DIR}"
        FILES_MATCHING PATTERN "*")
🐙
cd ~/user_ws
colcon build --packages-select ROBOT_sim
find . -name ROBOT_DETAILED.xml
find . -name base_link.stl

~/user_ws contents:

Dockerfile
docker-compose.yaml
ROBOT_description/
  config/
    moveit/
      groups.srdf
      joint_limits.yaml
      ompl_planning.yaml
      pose_ik_distance.yaml 
      servo.yaml
    ros2_control/
      controllers.yaml
  description/
    meshes/
      base_link.stl
      link1.stl
    ROBOT_DETAILED.xml
  urdf/
    ROBOT_model.urdf.xacro
    ROBOT_COMPONENT_macro.urdf.xacro
    ROBOT_macro.ros2_control.xacro
  launch/
    view_robot.launch # from xacro
    view_robot.rviz
  CMakeLists.txt
  package.xml
  LICENSE
  READEME.md
helper-scripts/
  add-gravcomp.sh
  extract_joints.sh
ROBOT_sim/
  config/
    config.yaml
  description/
    TASK-scene.xml
  launch/
    agent_bridge.launch
    moveit_pro.launch.xml
  objectives/
  waypoints/
    waypoints.yaml
  CMakeLists.txt
  package.xml
  LICENSE
  README.md

d. Expose mujoco model settings in xacro files​

This is so that the config.yaml can be quickly updated to start a different mujoco world or tun on/off the mujoco viewer.

ROBOT_model.urdf.xacro

 <robot>
  <!-- Use $() for xacro processing that must be done at runtime (roslaunch or CLI arguments, filepaths, etc)  -->
  <!-- Use ${} for xacro processing that is not done at runtime (passing values between xacro files) -->
  <xacro:arg name="left_gripper_type" default="default_vacuum" />
  <xacro:arg name="right_gripper_type" default="default_vacuum" />
  <xacro:arg name="base_type" default="none" />
  <!-- hardware_interface options: sim, physical (TODO), or mock (TODO) -->
  <xacro:arg name="hardware_interface" default="sim" />
  <xacro:arg name="mujoco_model_package" default="rapid_rad_base_config" />
  <xacro:arg
    name="mujoco_model_relpath"
    default="description/simple-scene.xml"
  />
  <xacro:arg name="mujoco_viewer" default="true" />

~/user_ws contents:

Dockerfile
docker-compose.yaml
ROBOT_description/
  config/
    moveit/
      groups.srdf
      joint_limits.yaml
      ompl_planning.yaml
      pose_ik_distance.yaml 
      servo.yaml
    ros2_control/
      controllers.yaml
  description/
    meshes/
      base_link.stl
      link1.stl
    ROBOT_DETAILED.xml
  urdf/
    ROBOT_model.urdf.xacro
    ROBOT_COMPONENT_macro.urdf.xacro
    ROBOT_macro.ros2_control.xacro
  launch/
    view_robot.launch # from xacro
    view_robot.rviz
  CMakeLists.txt
  package.xml
  LICENSE
  READEME.md
helper-scripts/
  add-gravcomp.sh
  extract_joints.sh
ROBOT_sim/
  config/
    config.yaml
  description/
    TASK-scene.xml
  launch/
    agent_bridge.launch
    moveit_pro.launch.xml
  objectives/
  waypoints/
    waypoints.yaml
  CMakeLists.txt
  package.xml
  LICENSE
  README.md

e. Configure "hardware" dictionary entries in config.yaml​

# Baseline hardware configuration parameters for MoveIt Pro.
# [Required]
hardware:
  # Set simulated to false if you are using this as a configuration for real hardware.
  # This allows users to switch between mock and real hardware by changing a single parameter with config inheritance.
  # [Required]
  simulated: true
  # If simulated is false, launch the following file:
  # [Optional, defaults to a blank launch file if not specified]
  robot_driver_persist_launch_file:
    package: "moveit_studio_agent"
    path: "launch/blank.launch.py"

  # If the MoveIt Pro Agent should launch the ros2 controller node.
  # [Optional, default=True]
  launch_control_node: True

  # If the MoveIt Pro Agent should launch the robot state publisher.
  # This should be false if you are launching the robot state publisher as part of drivers.
  # [Optional, default=True]
  launch_robot_state_publisher: True

  # Parameters used to configure the robot description through XACRO.
  # A URDF and SRDF are both required.
  # [Required]
  robot_description:
    urdf:
      package: "factory_sim"
      path: "description/picknik_fanuc.xacro"
    srdf:
      package: "factory_sim"
      path: "config/moveit/picknik_fanuc.srdf"
    # Specify any additional parameters required for the URDF.
    # Many of these are specific to the descriptions packages, and can be customized as needed.
    # [Optional]
    urdf_params:
      - hardware_interface: "sim"
      - mujoco_model_package: "ROBOT_sim"
      - mujoco_model_relpath: "description/TASK-scene.xml"
      - mujoco_viewer: "true"

f. Configure "moveit_params" dictionary entries in config.yaml​

TODO - unclear which are required in 7.x

# Configuration files for MoveIt.
# For more information, refer to https://moveit.picknik.ai/main/doc/how_to_guides/moveit_configuration/moveit_configuration_tutorial.html
# [Required]
moveit_params:
  # Used by the Waypoint Manager to save joint states from this joint group.
  joint_group_name: "whole_body"

  joint_limits:
    package: "ROBOT_description"
    path: "config/moveit/joint_limits.yaml"
  kinematics:
    package: "ROBOT_description"
    path: "config/moveit/pose_ik_speed.yaml"
  ompl_planning:
    package: "ROBOT_description"
    path: "config/moveit/ompl_planning.yaml"
  servo:
    package: "ROBOT_description"
    path: "config/moveit/servo.yaml"
  # servo_kinematics:
  #   package: "ROBOT_description"
  #   path: "config/moveit/pose_ik_speed.yaml"
  # servo_joint_limits:
  #   package: "ROBOT_description"
  #   path: "config/moveit/joint_limits.yaml"

g. Configure "ros2_control" dictionary entries in config.yaml​

# Configuration for launching ros2_control processes.
# [Required, if using ros2_control]
ros2_control:
  config:
    package: "ROBOT_description"
    path: "config/ros2_control/controllers.yaml"
  # MoveIt Pro will load and activate these controllers at start up to ensure they are available.
  # If not specified, it is up to the user to ensure the appropriate controllers are active and available
  # for running the application.
  # [Optional, default=[]]
  controllers_active_at_startup:
    - "joint_trajectory_controller"
    - "joint_state_broadcaster"
    - "tool_attach_controller"
    - "suction_cup_controller"
  # Load but do not start these controllers so they can be activated later if needed.
  # [Optional, default=[]]
  controllers_inactive_at_startup:
    - "servo_controller"
  # Any controllers here will not be spawned by MoveIt Pro.
  # [Optional, default=[]]
  controllers_not_managed: []
  # Optionally configure remapping rules to let multiple controllers receive commands on the same topic.
  # [Optional, default=[]]
  controller_shared_topics: []

h. Configure "objectives" dictionary entries in config.yaml​

make blank waypoints.yaml file

# Configuration for loading behaviors and objectives.
# [Required]
objectives:
  # List of plugins for loading custom behaviors.
  # [Required]
  behavior_loader_plugins:
    # This plugin will load the core MoveIt Pro Behaviors.
    # Add additional plugin loaders as needed.
    core:
      - "moveit_studio::behaviors::CoreBehaviorsLoader"
      - "moveit_studio::behaviors::MTCCoreBehaviorsLoader"
      - "moveit_studio::behaviors::ServoBehaviorsLoader"
      - "moveit_studio::behaviors::VisionBehaviorsLoader"
  # Specify source folder for objectives
  # [Required]
  objective_library_paths:
    core:
      package_name: "ROBOT_sim"
      relative_path: "objectives"
  # Specify the location of the saved waypoints file.
  # [Required]
  waypoints_file:
    package_name: "ROBOT_sim"
    relative_path: "waypoints/waypoints.yaml"

i. Launch! 🤞🤞​

Launch pro and iteratively check terminal output for errors in finding and/or loading files.

moveit_pro build user_workspace
moveit_pro run -v -c ROBOT_sim

5. Configure Teleop objectives​

If the Pro UI loads and you can see the 3D visualization of your robot, then we are ready to start configuring teleoperation objectives:
https://docs.picknik.ai/how_to/configuration_tutorials/configure_custom_robot/#configuring-robot-description-and-semantics

~/user_ws contents:

Dockerfile
docker-compose.yaml
ROBOT_description/
  config/
    moveit/
      groups.srdf
      joint_limits.yaml
      ompl_planning.yaml
      pose_ik_distance.yaml 
      servo.yaml
    ros2_control/
      controllers.yaml
  description/
    meshes/
      base_link.stl
      link1.stl
    ROBOT_DETAILED.xml
  urdf/
    ROBOT_model.urdf.xacro
    ROBOT_COMPONENT_macro.urdf.xacro
    ROBOT_macro.ros2_control.xacro
  launch/
    view_robot.launch # from xacro
    view_robot.rviz
  CMakeLists.txt
  package.xml
  LICENSE
  READEME.md
helper-scripts/
  add-gravcomp.sh
  extract_joints.sh
ROBOT_sim/
  config/
    config.yaml
  description/
    TASK-scene.xml
  launch/
    agent_bridge.launch
    moveit_pro.launch.xml
  objectives/
    clear_snapshot.xml
    disengage_grasp.xml
    engage_grasp.xml
    interpolate_to_joint_State.xml
    move_to_joint_state.xml
    move_to_pose_with_approval.xml
    request_teleoperation.xml
    reset_planning_scene.xml
    take_snapshot.xml
    teleoperation.xml
  waypoints/
    waypoints.yaml
  CMakeLists.txt
  package.xml
  LICENSE
  README.md

Final directory contents​

Dockerfile
docker-compose.yaml
helper-scripts/
  add-gravcomp.sh
  extract_joints.sh
mujoco-3.3.1/
  bin/
  ...
ROBOT_description/
  config/
    moveit/
      groups.srdf
      joint_limits.yaml
      ompl_planning.yaml
      pose_ik_distance.yaml 
      servo.yaml
    ros2_control/
      controllers.yaml
  description/
    meshes/
      base_link.stl
      link1.stl
    ROBOT_DETAILED.xml
  urdf/
    ROBOT_model.urdf.xacro
    ROBOT_COMPONENT_macro.urdf.xacro
    ROBOT_macro.ros2_control.xacro
  launch/
    view_robot.launch
    view_robot.rviz
  CMakeLists.txt
  package.xml
  LICENSE
  READEME.md
helper-scripts/
  add-gravcomp.sh
  extract_joints.sh
ROBOT_sim/
  config/
    config.yaml
  description/
    TASK-scene.xml
  launch/
    agent_bridge.launch
    moveit_pro.launch.xml
  objectives/
    clear_snapshot.xml
    disengage_grasp.xml
    engage_grasp.xml
    interpolate_to_joint_State.xml
    move_to_joint_state.xml
    move_to_pose_with_approval.xml
    request_teleoperation.xml
    reset_planning_scene.xml
    take_snapshot.xml
    teleoperation.xml
  waypoints/
    waypoints.yaml
  CMakeLists.txt
  package.xml
  LICENSE
  README.md
temp/
  ROBOT_DETAILED.test.urdf
  base_link.stl
  link1.stl
  ROBOT_DETAILED.xml