Load iCub URDF models in MuJoCo

[traversaro]

MuJoCo is a physics engine that in theory is able to load URDF files (see https://mujoco.readthedocs.io/en/latest/modeling.html).

This issue is the place where people that want to use iCub models with MuJoCo can report issues, if any.

[GiulioRomualdi]

cc @xEnVrE

[traversaro]

@CarlottaSartore is doing some experience in loading a URDF of a model similar to an iCub in MuJoCo, so probably some tricks that she is learning may be useful also here.

[CarlottaSartore]

I did some trials with the stickbot model (https://github.com/icub-tech-iit/ergocub-gazebo-simulations/tree/master/models).

To load an urdf file in mujoco, it is possible to use the mujoco.MjModel.from_xml_string() method, already implemented in mujoco, but there are few issues:

• The main issues is that mujoco does not natively support having a floating based models, and transforms the first link of the chain into the world link.
• To avoid this issue, one must declare a new first link defined as world, and add a floating joint with parent the world link and child the base_link of the robot, to allow having a floating base robot.
• All the joints defined are assumed to be controlled, if not 'fixed', for this reason, one should change the joint from revolute to fixed, if do not want to control them and have them rigidly attached to the parent and child. Note that the floating joint should instead left as floating with zero torque, to obtain a floating base dynamic

I have implemented the following lines to do it programmatically, even though they can surely be improved.
The urdf_string is the original urdf, the joint_name_list is the list of controlled joint, the base_link is the name of the base link.

from urdfpy import URDF
from urdfpy import Joint
from urdfpy import Link

def get_mujoco_urdf_file(urdf_string, joint_name_list, base_link):
        robot = URDF.load(urdf_string)
        for item in robot.joints: 
            print(item)
            if item.name not in (joint_name_list): 
                item.joint_type = "fixed"
        world_joint = Joint("floating_joint","floating", "world", base_link)
        world_link = Link("world",None, None, None)
        robot._links.append(world_link)
        robot._joints.append(world_joint)
        temp_urdf = tempfile.NamedTemporaryFile(mode = 'w+')
        robot.save(temp_urdf.name)
        return temp_urdf

In addition, to be able to correctly see the robot, one should add the following line to the urdf (I have not implemented a function which automatically add these lines yet):
<mujoco> <compiler discardvisual="false"/> </mujoco>

Then you can load the urdf and save the mjcf.xml

import mujoco

urdf_mujoco_file = get_mujoco_urdf_file(urdf_string, joint_name_list, base_link))
urdf_string = urdf_mujoco_file.read()
model = mujoco.MjModel.from_xml_string(urdf_string)
mujoco.mj_saveLastXML(path_to_save,model)

Then, it will be possible to visualize the robot via python -m mujoco.viewer --mjcf=/path/to/some/mjcf.xml

Other useful things to add

• A floor to (for now I saw only how to add it in the mjcf.xml file)

<geom name="floor" size="0 0 .05" type="plane" material="grid" condim="3"/>

• The following lines for not having the black background and Improved appearance

  <asset>
  <texture type="skybox" builtin="gradient" rgb1=".3 .5 .7" rgb2="0 0 0" width="32" height="512"/>
  <texture name="body" type="cube" builtin="flat" mark="cross" width="127" height="1278" rgb1="0.8 0.6 0.4" rgb2="0.8 0.6 0.4" markrgb="1 1 1" random="0.01"/>
  <material name="body" texture="body" texuniform="true" rgba="0.8 0.6 .4 1"/>
  <texture name="grid" type="2d" builtin="checker" width="512" height="512" rgb1=".1 .2 .3" rgb2=".2 .3 .4"/>
  <material name="grid" texture="grid" texrepeat="1 1" texuniform="true" reflectance=".2"/>
  </asset>

[traversaro]

Thanks for the input @CarlottaSartore !

[CarlottaSartore]

In addition to the step previously listed, one should also add the motor entry for each actuated joint, to be able to control them by filling the MjData.ctrl.

The motors entry is could be as follow:

<actuator>
<motor name="r_shoulder_pitch" joint="r_shoulder_pitch" gear="1" ctrlrange="-100 100" />
<motor name="r_shoulder_roll" joint="r_shoulder_roll" gear="1" ctrlrange="-100 100" />
<motor name="r_shoulder_yaw" joint="r_shoulder_yaw" gear="1" ctrlrange="-100 100" />
<motor name="r_elbow" joint="r_elbow" gear="1" ctrlrange="-100 100" />
<motor name="l_shoulder_pitch" joint="l_shoulder_pitch" gear="1" ctrlrange="-100 100" />
<motor name="l_shoulder_roll" joint="l_shoulder_roll" gear="1" ctrlrange="-100 100" />
<motor name="l_shoulder_yaw" joint="l_shoulder_yaw" gear="1" ctrlrange="-100 100" />
<motor name="l_elbow" joint="l_elbow" gear="1" ctrlrange="-100 100" />
<motor name="r_hip_pitch" joint="r_hip_pitch" gear="1" ctrlrange="-100 100" />
<motor name="r_hip_roll" joint="r_hip_roll" gear="1" ctrlrange="-100 100" />
<motor name="r_hip_yaw" joint="r_hip_yaw" gear="1" ctrlrange="-100 100" />
<motor name="r_knee" joint="r_knee" gear="1" ctrlrange="-100 100" />
<motor name="r_ankle_pitch" joint="r_ankle_pitch" gear="1" ctrlrange="-100 100" />
<motor name="r_ankle_roll" joint="r_ankle_roll" gear="1" ctrlrange="-100 100" />
<motor name="l_hip_pitch" joint="l_hip_pitch" gear="1" ctrlrange="-100 100" />
<motor name="l_hip_roll" joint="l_hip_roll" gear="1" ctrlrange="-100 100" />
<motor name="l_hip_yaw" joint="l_hip_yaw" gear="1" ctrlrange="-100 100" />
<motor name="l_knee" joint="l_knee" gear="1" ctrlrange="-100 100" />
<motor name="l_ankle_pitch" joint="l_ankle_pitch" gear="1" ctrlrange="-100 100" />
<motor name="l_ankle_roll" joint="l_ankle_roll" gear="1" ctrlrange="-100 100" />
</actuator>

Note that the order in which the motors are defined should follow the order in which the joint are defined in the XML file

By defining such entries in the XML files, it is possible then to control the robot. One should pay anyhow attention to have the same ordering in filling the ctrl vector

[traversaro]

Apparently MuJoCo does not supports resolving package:// URIs, and so it is necessary to manually specify the relative path between the .urdf model and the actualy directory containing the meshes.

As an example, I tried to modify the iCub3 to be visualized in MuJoCo, first I installed mujoco and icub-models as in:

$ mamba create -n test155 icub-models mujoco
$ mamba activate test155

Then I tried to check if MuJoCo was able to load the meshes out of the box:

$ cd $CONDA_PREFIX/share/iCub/robots/iCubGazeboV3
$ python -m mujoco.viewer --mjcf=./model.urdf

And by doing, so there was an error related to the fact that ./sim_ecub_root_link_prt.stl could not be found. As the URI in the file is actually package://iCub/meshes/simmechanics/sim_icub3_root_link_prt.stl, I guess that MuJoCo strips all the directories and only considers the filename, so in meshdir, we should specify the relative dir between the model, contained in:

$CONDA_PREFIX/share/iCub/robots/iCubGazeboV3

and the meshes, that are contained in:

$CONDA_PREFIX/share/iCub/meshes/simmechanics

So I copied the file, and I modified the top of the file to contain the meshdir, i.e. :

$ cp ./model.urdf ./model_mujoco.urdf
$ code ./model_mujoco
# manual modifications
$ head -n 5 ./model_mujoco.urdf
<robot name="iCub">
  <mujoco>
    <compiler meshdir="../../meshes/simmechanics"/>
  </mujoco>
  <link name="root_link"> 
$  python -m mujoco.viewer --mjcf=./model_mujoco.urdf

The robot is doing strange things, but at least the meshes are correctly loaded:

[siddharthdeore]

@traversaro, last year, I wrote some throwaway snippets to automatically generate mjcf files from the original icub-models and (ergoCub)(https://github.com/icub-tech-iit/ergocub-software.git) models. The primary goal was to streamline the generation process without changing original descriptions. You can find the code repository at https://github.com/siddharthdeore/icub_mujoco.git.

Mujoco's collision checking algorithm excludes consecutive links connected by non-fixed(?) joints. So, it's necessary to exclude collision pairs. This can be achieved by either directly adding exclude XML tags in the original URDF (similar to Gazebo tags) or manually adding them in the generated XML. generate.py script automatically appends the necessary exclude tag along with actuator prameters.

[traversaro]

Thanks @siddharthdeore . Also @CarlottaSartore did some progress of this, I guess more and less around this lines of code https://github.com/ami-iit/comodo/blob/8cbd5f5bc6f65a1385a646188abf999844244d30/src/comodo/robotModel/robotModel.py#L190-L327 . It would be cool if we could consolidate these logic somewhere, so to offer a MuJoCo Menagerie (https://github.com/google-deepmind/mujoco_menagerie) like experience for users of *Cub (iCub, ergoCub) models. It seems that most of the effort in menagerie is related to import existing models from URDF, save them in MJCF and then iterate manually on the generated MJCF. However, that would not work for us, as we continuously modify our URDFs and create new robots, so we could not manage manually MJCF models, as they would quick diverse from the URDF models we store in here and in https://github.com/icub-tech-iit/ergocub-software .

[traversaro]

Thanks @siddharthdeore . Also @CarlottaSartore did some progress of this, I guess more and less around this lines of code https://github.com/ami-iit/comodo/blob/8cbd5f5bc6f65a1385a646188abf999844244d30/src/comodo/robotModel/robotModel.py#L190-L327 . It would be cool if we could consolidate these logic somewhere, so to offer a MuJoCo Menagerie (https://github.com/google-deepmind/mujoco_menagerie) like experience for users of *Cub (iCub, ergoCub) models. It seems that most of the effort in menagerie is related to import existing models from URDF, save them in MJCF and then iterate manually on the generated MJCF. However, that would not work for us, as we continuously modify our URDFs and create new robots, so we could not manage manually MJCF models, as they would quick diverse from the URDF models we store in here and in https://github.com/icub-tech-iit/ergocub-software .

Also ami-iit/jaxsim#83 contains related code.

[traversaro]

I discussed a bit with @GiulioRomualdi and @Giulero in the last days and played a bit on this in the weekend, I noted my observations in google-deepmind/mujoco#1432 (comment) . TL;DR: I think we need to have a mujoco-urdf-automatic-massaging library to simplify the process of loading URDF in MuJoCo. If instead we want to have some models that can be loaded out of the box in MuJoCo (for example being passed to python -m mujoco.viewer), probably we need to run the mujoco-urdf-automatic-massaging scripts as part of icub-model-generator/ergocub-software, and commit to the icub-models/ergocub-software the generated models. I do not think it is feasible to modify MuJoCo to load out of the box URDF models in a way that is satisfactory for us.

[traversaro]

This issue is interesting for simulating FT placed in the middle of the link as the one in iCub: google-deepmind/mujoco#1597 . fyi @giotherobot