search

RobotLocomotion / drake

Model-based design and verification for robotics.
https://drake.mit.edu
Other
3.25k stars 1.25k forks source link

MuJoCo imports differ from MuJoCo in placement of joints #20367

Closed drewhamiltonasdf closed 10 months ago

drewhamiltonasdf commented 11 months ago

What happened?

It seems that MuJoCo imports both joints and inertial masses as an offset from the body it is nested in. Drake on the other hand seems to compute the joint location with respect to the previous body.

For example:

<body name='link1' pos="0 0.1 0.6" >
            <joint name="joint1" axis="1 0.1 0.1" pos="0.00 0.12 0.62"/>

Drake will currently interpret this as the joint1 frame being offset from link1 frame by [0.0, 0.02, 0.02]: On the other hand, MuJoCo would interpret it as and offset of [0.0 0.12 0.62] a very significant difference.

This example is not a great way to specify body/joint locations, but it is incorrect nonetheless and could lead to some significant issues. Interestingly, Drake seems to import the geometry objects and inertial masses correctly parented to the parent body frame, but gets the joint position wrong. I assume this has to do with how the parenting works in other formats, where the joint is the parent of the body instead of the other way around.

I hope that makes sense. I can provide a sample xml and minimal Drake code / MuJoCo code to see the difference if it is helpful.

Here is a (very gross) xml to demonstrate this:

<mujoco model="Planar3Link">

  <worldbody>

        <body name='link1' pos="0 0.1 0.6" >
            <joint name="joint1" axis="1 0.1 0.1" pos="0.00 0.12 0.62"/>
            <inertial mass="0.1" pos="0 0.3 0" fullinertia="0.01 0.01 0.01 0.001 0.001 0.001"/>

            <geom type="sphere" size="0.15" pos="0.0 0.02 0.02" material="red"/>
            <geom type="sphere" size="0.11" pos="0.0 0.3 0" material="blue"/>
            <geom type="cylinder" size="0.1 0.1" fromto="0 0.0 0.0 0 0.5 0" material="white"/>

            <body name="link2" pos = "0 0.5 0">
                <joint name="joint2" type="hinge" axis="1 0.2 0" pos="0.03 0.52 0.0"/>
                <inertial mass="0.1" pos="0 0.1 0" fullinertia="0.02 0.02 0.02 0.002 0.002 0.002"/>

                <geom type="sphere" size="0.15" pos="0.03 0.02 0" material="red"/>
                <geom type="sphere" size="0.11" pos="0 0.1 0" material="blue"/>
                <geom type="cylinder" size="0.1 0.1" fromto="0 0 0 0 0.75 0" material="white"/>

                <body name="link3" pos = "0.0 0.75 0.0">
                    <joint name="joint3" type="hinge" axis="1 0.03 0.05" pos="0.04 0.78 0.0" />
                    <inertial mass="0.1" pos="0.0 0.2 0.0" fullinertia="0.03 0.03 0.03 0.002 0.002 0.002"/>
                    <geom type="sphere" size="0.15" pos="0.04 0.03 0" material="red"/>
                    <geom type="sphere" size="0.11" pos="0.0 0.2 0" material="blue"/>
                    <geom type="cylinder" size="0.1 0.1" fromto="0 0 0 0 0.5 0" material="white"/>

                </body>

            </body>
        </body>

  </worldbody>

  <actuator>
    <!-- <motor name="cart-motor" joint="cart"/> -->
    <motor name="motor1" joint="joint1"/>
    <motor name="motor2" joint="joint2"/>
    <motor name="motor3" joint="joint3"/>
  </actuator>

</mujoco>

It will throw a few unsupported material warnings but load fine so you can move the joints in meshcat, and just to make things easy on you, here's minimal code to load this in MuJoCo (python):

import mujoco
import mujoco.viewer
import time
import numpy as np

MODEL_XML = """
<?xml version="1.0" ?>
<mujoco model="Planar3Link">
    <compiler autolimits="true"/>

    <option gravity="0 0 0" timestep="0.02" />
    <asset>
        <material name="blue" rgba="0 0 1 1" />
        <material name="green" rgba="0 1 0 1" />
        <material name="red" rgba="1 0 0 1" />
        <material name="white" rgba="1 1 1 1" />
    </asset>
    <worldbody>
        <geom type="plane" size="10 10 0.01" pos="0 0 0.0" rgba=".9 0 0 1" />
        <light diffuse=".5 .5 .5" pos="3 3 3" dir="-1 -1 -1" />      

        <body name='link1' pos="0 0.1 0.6" >
            <joint name="joint1" axis="1 0.1 0.1" pos="0.00 0.12 0.62" stiffness="100"  damping="10"/>
            <inertial mass="0.1" pos="0 0.3 0" fullinertia="0.01 0.01 0.01 0.001 0.001 0.001"/>
            <geom type="sphere" size="0.15" pos="0.0 0.02 0.02" material="red"/>
            <geom type="sphere" size="0.11" pos="0.0 0.3 0" material="blue"/>
            <geom type="cylinder" size="0.1 0.1" fromto="0 0.0 0.0 0 0.5 0" material="white"/>

            <body name="link2" pos = "0 0.5 0">
                <joint name="joint2" type="hinge" axis="1 0.2 0" pos="0.03 0.52 0.0" stiffness="100"  damping="10"/>
                <inertial mass="0.1" pos="0 0.1 0" fullinertia="0.02 0.02 0.02 0.002 0.002 0.002"/>
                <geom type="sphere" size="0.15" pos="0.03 0.02 0" material="red"/>
                <geom type="sphere" size="0.11" pos="0 0.1 0" material="blue"/>
                <geom type="cylinder" size="0.1 0.1" fromto="0 0 0 0 0.75 0" material="white"/>

                <body name="link3" pos = "0 0.75 0">
                    <joint name="joint3" type="hinge" axis="1 0.03 0.05" pos="0.04 0.78 0.0" stiffness="100"  damping="10"/>
                    <inertial mass="0.1" pos="0.0 0.2 0.0" fullinertia="0.03 0.03 0.03 0.002 0.002 0.002"/>                    <geom type="sphere" size="0.12" pos="0.0 0.4 0" material="red"/>
                    <geom type="sphere" size="0.15" pos="0.04 0.03 0" material="red"/>
                    <geom type="sphere" size="0.11" pos="0.0 0.2 0" material="blue"/>
                    <geom type="cylinder" size="0.1 0.1" fromto="0 0 0 0 0.5 0" material="white"/>
                </body>

            </body>

        </body>
  </worldbody>

  <actuator>
    <position name="motor1" joint="joint1" gear="10 10 10" ctrlrange="-20 20"/>
    <position name="motor2" joint="joint2" gear="10 10 10" ctrlrange="-20 20"/>
    <position name="motor3" joint="joint3" gear="10 10 10" ctrlrange="-20 20"/>
  </actuator>

</mujoco>
"""

model = mujoco.MjModel.from_xml_string(MODEL_XML)
data = mujoco.MjData(model)

# create the viewer object
#viewer = mujoco_viewer.MujocoViewer(model, data, width=800, height=600, hide_menus=True)
renderer = mujoco.Renderer(model, 400, 600)
viewer = mujoco.viewer.launch_passive(model, data)
viewer.cam.distance = model.stat.extent * 2

data.qpos = [1,1,1]
data.qvel = [0,0,0]
mujoco.mj_forward(model, data)

M = np.zeros((3,3))
mujoco.mj_fullM(model, M, data.qM)
print(M)

# simulate and render
while viewer.is_running():
    step_start = time.time()
    mujoco.mj_step(model, data)
    viewer.sync()

    # Rudimentary time keeping, will drift relative to wall clock.
    time_until_next_step = model.opt.timestep - (time.time() - step_start)
    if time_until_next_step > 0:
      time.sleep(time_until_next_step)

# close
viewer.close()

That mass matrix printed above will, naturally, be quite different than the one printed in Drake.

Version

No response

What operating system are you using?

Ubuntu 22.04

What installation option are you using?

pip install drake

Relevant log output

No response

RussTedrake commented 10 months ago

Thanks for the clear write-up and the example code!

I agree with your assessment. In fact, I see the same problem even more clearly in the dm_control acrobot.xml example.

from pydrake.all import ModelVisualizer, StartMeshcat, PackageMap

visualizer = ModelVisualizer(meshcat=meshcat)
visualizer.parser().package_map().AddRemote(
    package_name="dm_control",
    params=PackageMap.RemoteParams(
        urls=[
            f"https://github.com/google-deepmind/dm_control/archive/refs/tags/1.0.15.tar.gz"
        ],
        sha256=("bac091b18689330a99b7c18ddf86baa916527f5e4ab8e3ded0c8caff1dab2048"),
        strip_prefix="dm_control-1.0.15/",
    ),
)
visualizer.AddModels(url="package://dm_control/dm_control/suite/acrobot.xml")
plant = visualizer.parser().plant()
visualizer.Run(loop_once=not running_as_notebook)
meshcat.DeleteAddedControls()

Drake is clearly putting the joint at the origin, but mujoco is clearly putting it at [0,0,2].

As far as I can tell, drake is following the mujoco xml documentation, but the behavior in mujoco is not following the xml documentation? Please check me on this. The body/joint docs say

This element creates a joint. As explained in Kinematic tree, a joint creates motion degrees of freedom between the body where it is defined and the body’s parent.

and then also

pos: real(3), “0 0 0” Position of the joint, specified in the frame of the parent body. For free joints this attribute is ignored.

I guess if one interprets the second usage of the term "parent body" to mean the current body (the parent of this joint), not the parent of the body, then things work out ok. Presumably the axis element is ok, since it is by definition the same in the parent and child bodies.

drewhamiltonasdf commented 10 months ago

100%. I think this is the issue. I only noticed because I was really trying to break something I'm working on and was spending a lot of time loading URDF's, SDF's, and MuJoCo representations. I mentioned in the reviewable that I don't quite have time to look, but I trust if it's passing tests it's good to go. Thanks for the fix!