Problems when verifing the rigid bodies dynamics of Mujoco #2590

ppap36 · 2025-04-21T14:42:16Z

ppap36
Apr 21, 2025

Intro

Hi!

I am a student at BIT, I use MuJoCo for my research on rigid bodies dynamics.

My setup

Mujoco 3.3.1
Python

My question

When I verified the rigid body dynamics of Mujoco as per the official documentation, I ran into a problem:

I set up a cube with six degrees of freedom in a gravity-free environment, and at the same time, I applied a force of 0.05N to the x-axis and y-axis of the cube in the first 1 second of the experiment, after which the cube was no longer subjected to any external force.

After that, I recorded the joint acceleration qacc, joint velocity qvel, and the inertia matrix I of the cube to attempt to verify the rigid body dynamics.

I expect the formula to be validated as:

The first term of the formula can be regarded as the inertial force qfrc_inertia, while the second term can be regarded as the Coriolis force plus the centrifugal force qfrc_coriolis. The sum of them is supposed to be equal to 0.

However, there is always a gap in the results.

Thank you very much for your answer!

Minimal model and/or code that explain my question

If you encountered the issue in a complex model, please simplify it as much as possible (while still reproducing the issue).

Model:

minimal XML

<mujoco>
    <option gravity="0 0 0" timestep="0.00001" />
    <compiler angle="radian" autolimits="true" eulerseq="xyz" />
    <worldbody>
        <body name="test_cube" pos="0 0 0" quat="1 0 0 0">

            <joint name="slide_x_joint" type="slide" axis="1 0 0"  damping="0.001"/>
            <joint name="slide_y_joint" type="slide" axis="0 1 0"  damping="0.01"/>
            <joint name="slide_z_joint" type="slide" axis="0 0 1"  damping="0.001"/>
            <joint name="tbase_rotx"   axis="1 0 0"    damping="0.0000" />
            <joint name="tbase_roty"   axis="0 1 0"    damping="0.0000" />
            <joint name="tbase_rotz"   axis="0 0 1"    damping="0.0000" />            
           
            <geom name="test_cube"  type="box" size="0.1 0.1 0.1" />
            <!-- <inertial pos="0 0 0" diaginertia="0.001667 0.001667 0.001667" mass="1"/> -->
            <site name="body_site" />
        </body>

    </worldbody>
    <sensor>
        <framequat name="robot_base_quat" objtype="site" objname="body_site"/>
        <!-- <framepos name="robot_base_pos" body="test_cube"/> -->
    </sensor>

</mujoco>

Code:

import mujoco
import mujoco.viewer
import numpy as np

def compute_inertial(I, qacc):
    return I @ qacc

def compute_coriolis(I, v):

    Iv = I @ v
    omega=v[3:]
    vl = v[:3]
    return -np.concatenate([
        np.cross(omega, Iv[:3]) + np.cross(vl, Iv[3:]),
        np.cross(omega, Iv[3:])
    ])

# THIS PATH SHOULD BE YOUR PATH OF THE MODEL
model = mujoco.MjModel.from_xml_path("/home/zha/1workspace/mujoco/test/MujocoTutorials/new_task/test/dynamic_test/dynamic.xml")
data = mujoco.MjData(model)
viewer= mujoco.viewer.launch_passive(model=model, data=data)

mujoco.mj_resetData(model, data)  # Reset state and time.


mujoco.mj_resetData(model, data)
mujoco.mjv_defaultFreeCamera(model, viewer.cam)

joint_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_JOINT, "tbase_rotx")
print_interval = 0.1  
next_print_time = 0.0
nv = model.nv
dst = np.zeros((nv, nv), dtype=np.float64)


while viewer.is_running():
    if data.time >= next_print_time:
        # print the qacc and qvel and q
        print(data.qpos[3:6],data.qvel[3:6],data.qacc[3:6])
        # compute the inertial matrix
        mujoco.mj_fullM(model, dst, data.qM)
        print("Dense inertia matrix:")
        for i in range(nv):
            print(dst[i,:])

        # compute the inertial force
        qfrc_inertia = compute_inertial(dst, data.qacc)
        # compute the coriolis + centripedal force
        qfrc_coriolis = compute_coriolis(dst, data.qvel)

        print("intertia",qfrc_inertia)
        print("coriolis", qfrc_coriolis)
        print("sum",qfrc_inertia + qfrc_coriolis)


        next_print_time += print_interval
    
    if(data.time < 1):
        data.qfrc_applied[joint_id] = 0.001
        data.qfrc_applied[joint_id+1] = 0.001
        data.qfrc_applied[joint_id+2] = 0.00
    else:
        data.qfrc_applied[joint_id] = 0
        data.qfrc_applied[joint_id+1] = 0
        data.qfrc_applied[joint_id+2] = 0

    mujoco.mj_step(model, data)

    viewer.sync()

Confirmations

I searched the latest documentation thoroughly before posting.
I searched previous Issues and Discussions, I am certain this has not been raised before.

Answered by v-r-a

Apr 22, 2025

Gist: Three slide joints followed by three hinge joints will produce the same effect as a freejoint. But their time rates are not the same.

When you define hinges one after another, you perform sequential body-fixed rotations, each in different frames, making them Euler angles. The rate of change of Euler angles is not the body's angular velocity. You can find the mapping Jacobian between the rate of change of Euler angles and the angular velocity, e.g., see: 4.8 in Spong, M. W., Hutchinson, S., & Vidyasagar, M. (2006). Robot Modeling and Control. Wiley.

But, I guess that is not the point here since you want to verify the conservation of angular momentum. You need the angular velocity of …

View full answer

v-r-a · 2025-04-21T16:47:50Z

v-r-a
Apr 21, 2025

Hi, I am a MuJoCo user.

I tried debugging your code, but it seems a bit tough to me.

Here is a simpler code on the lines of your code that verifies the conservation of angular momentum. Torques are applied for the first five seconds, then it is set to zero. The rate of change of angular momentum goes to zero after 5s, as expected.

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

myxml="""
<mujoco>
    <option gravity="0 0 0" timestep="0.002" />
    <worldbody>
        <body name="test_cube">
            <freejoint/>           
            <geom type="box" size="0.1 0.1 0.1" />
        </body>
    </worldbody>
</mujoco>
"""
model = mujoco.MjModel.from_xml_string(myxml)
data = mujoco.MjData(model)
bodyID = mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_BODY,"test_cube")
mujoco.mj_resetData(model, data)
nv = model.nv
dst = np.zeros((nv, nv), dtype=np.float64)

viewer = mujoco.viewer.launch_passive(model=model, data=data)
while viewer.is_running():

    tstart = data.time
    while (data.time - tstart) < 1/60:
        mujoco.mj_step(model,data)
        # Apply some torques to the cube
        if data.time < 5:
            data.xfrc_applied[bodyID][3:6] = 0.1*np.array([1., 1., 1.])
            print(data.xfrc_applied)
        else:
            data.xfrc_applied[bodyID][3:6] = np.array([0., 0., 0.])

    # Calculations
    mujoco.mj_fullM(model, dst, data.qM)
    # compute the inertial force
    Iwdot = dst[3:6, 3:6] @ data.qacc[3:6]
    # compute the coriolis + centripedal force
    wxIw = np.cross(data.qvel[3:6], dst[3:6, 3:6] @ data.qvel[3:6])
    print(Iwdot)
    print(wxIw)
    print(Iwdot + wxIw)
    # all the above quantities are expressed in the body-fixed frame

    viewer.sync()
    sleep(0.015)

1 reply

ppap36 Apr 22, 2025
Author

Thanks for your quick response.

I tested your code and compared it with mine. I found the biggest difference between our code is that you set the joint of the cube as freejoint and apply the forces by xfrc_applied, however I set the joint of the cube as hinge & slide and apply the forces by qfrc_applied

You can check the difference by running the code below:

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

myxml="""
<mujoco>
    <option gravity="0 0 0" timestep="0.002" />
    <worldbody>
        <body name="test_cube">
            <freejoint/>           
            <geom type="box" size="0.1 0.1 0.1"/>
        </body>
    </worldbody>
</mujoco>
"""
myxml_withjoint="""
<mujoco>
    <option gravity="0 0 0" timestep="0.002" />
    <worldbody>
        <body name="test_cube">
            <joint name="slide_x_joint" type="slide" axis="1 0 0"  damping="0.0000"/>
            <joint name="slide_y_joint" type="slide" axis="0 1 0"  damping="0.0000"/>
            <joint name="slide_z_joint" type="slide" axis="0 0 1"  damping="0.0000"/>
            <joint name="tbase_rotx" type="hinge"    axis="1 0 0"  damping="0.0000"/>
            <joint name="tbase_roty" type="hinge"    axis="0 1 0"  damping="0.0000"/>
            <joint name="tbase_rotz" type="hinge"    axis="0 0 1"  damping="0.0000"/>                        
            <geom type="box" size="0.1 0.1 0.1"/>
        </body>
    </worldbody>
</mujoco>
"""

test_withjoint = 1
if test_withjoint:
    model = mujoco.MjModel.from_xml_string(myxml_withjoint)
else:
    model = mujoco.MjModel.from_xml_string(myxml)

data = mujoco.MjData(model)
bodyID = mujoco.mj_name2id(model,mujoco.mjtObj.mjOBJ_BODY,"test_cube")
mujoco.mj_resetData(model, data)
nv = model.nv
dst = np.zeros((nv, nv), dtype=np.float64)

viewer = mujoco.viewer.launch_passive(model=model, data=data)
while viewer.is_running():

    tstart = data.time
    while (data.time - tstart) < 1/60:
        mujoco.mj_step(model,data)
        # Apply some torques to the cube
        if data.time < 5:
            if test_withjoint:
                data.qfrc_applied[3:6] = 0.01*np.array([1., 1., 1.])
            else:
                data.xfrc_applied[bodyID][3:6] = 0.01*np.array([1, 1., 1.])
            
            print(data.xfrc_applied)
        else:
            if test_withjoint:
                data.qfrc_applied[3:6] = np.array([0., 0., 0.])
            else:
                data.xfrc_applied[bodyID][3:6] = np.array([0., 0., 0.])

    # Calculations
    mujoco.mj_fullM(model, dst, data.qM)
    # compute the inertial force
    Iw = dst[3:6, 3:6] @ data.qacc[3:6]
    # compute the coriolis + centripedal force
    wxIw = np.cross(data.qvel[3:6], dst[3:6, 3:6] @ data.qvel[3:6])
    
    print("qvel",data.qvel[3:6])
    print("Iqvel",dst[3:6, 3:6] @ data.qvel[3:6])
    for i in range(3):
        print(dst[3+i,3:])
    print("Iqacc",Iw)
    print("wxIw",wxIw)
    print("plus",Iw + wxIw)
    
    # all the above quantities are expressed in the body-fixed frame

    viewer.sync()
    sleep(0.015)

It turns out that the cube with xfrc_applied has stable angular velocities(data.time>5). but the cube with qfrc_applied has variable angular velocities(data.time>5).

I'm thinking about which result conforms to the laws of physics in the real world, and what causes their differences. If you have any new insights, I look forward to your reply.

v-r-a · 2025-04-22T05:01:12Z

v-r-a
Apr 22, 2025

Gist: Three slide joints followed by three hinge joints will produce the same effect as a freejoint. But their time rates are not the same.

When you define hinges one after another, you perform sequential body-fixed rotations, each in different frames, making them Euler angles. The rate of change of Euler angles is not the body's angular velocity. You can find the mapping Jacobian between the rate of change of Euler angles and the angular velocity, e.g., see: 4.8 in Spong, M. W., Hutchinson, S., & Vidyasagar, M. (2006). Robot Modeling and Control. Wiley.

But, I guess that is not the point here since you want to verify the conservation of angular momentum. You need the angular velocity of the body. You can get that by defining a suitable sensor. Or, since our model is just a single body--->

If you define a freejoint, the corresponding rate of change is directly the linear velocity and angular velocity (in the body-fixed frame of the cube) of the cube.

I am not sure where to point in the documentation for further reading, see issue #691 or Floating objects.

1 reply

ppap36 Apr 22, 2025
Author

Thank you for pointing my mistake, I will check the files mentioned above.

gyq-china · 2025-04-22T09:29:17Z

gyq-china
Apr 22, 2025

Hello,
I am also a student and have used mujoco to verify dynamics.
I don't think sum should be 0, because you are applying the qfrc_applied, and secondly

3 replies

gyq-china Apr 22, 2025

In this formula you are applying n_c (I think so), so according to your code (if you only apply 1 to rotx) the result should be [1 0 0]

v-r-a Apr 22, 2025

Perfect, you can observe that in the code output too. The sum is exactly equal to the applied torques.

We were expecting it to be zero once we stopped applying external moments.

ppap36 Apr 22, 2025
Author

    if(data.time < 1):
        data.qfrc_applied[joint_id] = 0.001
        data.qfrc_applied[joint_id+1] = 0.001
        data.qfrc_applied[joint_id+2] = 0.00
    else:
        data.qfrc_applied[joint_id] = 0
        data.qfrc_applied[joint_id+1] = 0
        data.qfrc_applied[joint_id+2] = 0

I am sorry I didn't make it clear. When data.time<1, the qfrc_applied is regarded as the initial force input. So n_c is applied according to your formula.
I want to verify the dynamic of the test cube when data.time>=1.

Thank you for your reply

Provide feedback

Saved searches

Use saved searches to filter your results more quickly

Uh oh!

Problems when verifing the rigid bodies dynamics of Mujoco #2590

Uh oh!

{{title}}

Uh oh!

Replies: 3 comments 5 replies

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{editor}}'s edit

{{editor}}'s edit

Uh oh!

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{title}}

Uh oh!

Uh oh!

{{title}}

Uh oh!

Select a reply

Uh oh!

Problems when verifing the rigid bodies dynamics of Mujoco #2590

Uh oh!

ppap36 Apr 21, 2025

Intro

My setup

My question

Minimal model and/or code that explain my question

Confirmations

Replies: 3 comments · 5 replies

Uh oh!

Uh oh!

v-r-a Apr 21, 2025

Uh oh!

ppap36 Apr 22, 2025 Author

Uh oh!

v-r-a Apr 22, 2025

Uh oh!

ppap36 Apr 22, 2025 Author

Uh oh!

gyq-china Apr 22, 2025

Uh oh!

gyq-china Apr 22, 2025

Uh oh!

v-r-a Apr 22, 2025

Uh oh!

ppap36 Apr 22, 2025 Author

ppap36
Apr 21, 2025

Replies: 3 comments 5 replies

v-r-a
Apr 21, 2025

ppap36 Apr 22, 2025
Author

v-r-a
Apr 22, 2025

ppap36 Apr 22, 2025
Author

gyq-china
Apr 22, 2025

ppap36 Apr 22, 2025
Author