How to calculate inverse dynamics in Mujoco

[dazhongtu]

Intro

Hi!

I am a undergrad student at HeFei university of technology, I use MuJoCo for my research on robot inverse dynamics.

My setup

Mujoco 3.3.0 python Ubuntu20.04

My question

Hello everyone, I’m using MuJoCo to solve the inverse dynamics of a robot. I input a trajectory to the robot and output the joint torques. I used three methods to compute the torques: data.actuator_force and data.qfrc_inverse.copy() give the same result, but the manually calculated dense_M @ data.qacc + data.qfrc_bias shows significant differences from the first two. According to the inverse dynamics formula, it should be:
τ=M(q) q¨ +C(q, q​ )+G(q)
Has anyone encountered similar issues? The robot model is UR10E.

Minimal model and/or code that explain my question

import time
import mujoco
import mujoco.viewer
import glfw
import numpy as np
import os
import matplotlib.pyplot as plt
from scipy.io import loadmat

def scroll_callback(window, xoffset, yoffset):
global cam
cam.distance *= 1 - 0.1 * yoffset

global cam
loaded_data = loadmat("omega_traj_joint.mat")

joint_q = loaded_data["joint_q"]
joint_qd = loaded_data["joint_qd"]
joint_qdd = loaded_data["joint_qdd"]

model = mujoco.MjModel.from_xml_path('/home/madoka/software/mujoco/mujoco-3.3.0/mujoco_menagerie-main/universal_robots_ur10e/scene.xml')
data = mujoco.MjData(model)

mujoco.mj_resetData(model, data)

data.qpos[:6] = joint_q[0,:]

print("All bodies in the model:")
for i in range(model.nbody):
body_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, i)
print(f"ID: {i}, Name: {body_name}")

if not glfw.init():
print("glfw not intilized")

window = glfw.create_window(1200, 900, 'UR10 Arm Control', None, None)
if not window:
glfw.terminate()

fixed_timestep = model.opt.timestep

end_effector_id = mujoco.mj_name2id(model, mujoco.mjtObj.mjOBJ_BODY, 'wrist_3_link')
scene = mujoco.MjvScene(model, maxgeom=10000)
glfw.make_context_current(window)

cam = mujoco.MjvCamera()
opt = mujoco.MjvOption()
mujoco.mjv_defaultCamera(cam)
mujoco.mjv_defaultOption(opt)
pert = mujoco.MjvPerturb()
con = mujoco.MjrContext(model, mujoco.mjtFontScale.mjFONTSCALE_150.value)

mujoco_q = np.zeros((joint_q.shape[0],6))
mujoco_qd = np.zeros((joint_q.shape[0],6))
mujoco_qdd = np.zeros((joint_q.shape[0],6))
torque = np.zeros((joint_q.shape[0],6))
torque2 = np.zeros((joint_q.shape[0],6))

for i in range(model.njnt):
joint_type = model.jnt_type[i]
qpos_start = model.jnt_qposadr[i]
qvel_start = model.jnt_dofadr[i]
i = -1;
start_time = data.time
current_time = data.time
glfw.set_scroll_callback(window, scroll_callback)

while not glfw.window_should_close(window) and current_time-start_time < 10:

mujoco.mj_forward(model, data)
end_effector_pos = data.body(end_effector_id).xpos

# initial_q[0] = initial_q[0] + 0.01
if i <  joint_q.shape[0]-1:
    i = i+1
    data.ctrl[:6] =  joint_q[i,:]
    # data.qpos[:6] = joint_q[i,:]

current_time = data.time
# print("time = {}, actuator_force == {}, i == {}".format(current_time-start_time,data.actuator_force,i))
mujoco.mj_step(model,data)
viewport = mujoco.MjrRect(0, 0, 1200, 900)
mujoco.mjv_updateScene(model, data, opt, pert, cam, mujoco.mjtCatBit.mjCAT_ALL.value, scene)
mujoco.mjr_render(viewport, scene, con)
mujoco.mj_step1(model, data)
mujoco.mj_inverse(model, data)
mujoco_q[i,:] = data.qpos.copy()
mujoco_qd[i,:] = data.qvel.copy()
mujoco_qdd[i,:] = data.qacc.copy()
torque[i,:] = data.actuator_force.copy()
# torque2[i,:] = data.qfrc_inverse.copy()
dense_M = np.zeros((model.nv,model.nv))
mujoco.mj_fullM(model, dense_M, data.qM)
torque2[i,:] = dense_M @ np.transpose(data.qacc.copy()) + data.qfrc_bias.copy()
glfw.swap_buffers(window)
glfw.poll_events()

glfw.terminate()

Confirmations

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

[v-r-a]

Hi, I am a MuJoCo user. I hope this helps you.

1. The equation of motion has a few more terms than what you have written down. Please see here. You should include the passive forces in your calculations since the menagerie ur10e model has joint damping defined.
2. You may also look at how computations happen step by step. Here

I have written a program while cutting down a lot of clutter. You can update your code based on the following:
The LHS and RHS of the equation of motion are evaluated at an instance. The LHS and RHS are in reference to this equation.

import numpy as np
import matplotlib.pyplot as plt

import mujoco
import mujoco.viewer as viewer


# Load the model (Update the path to your mujoco_menagerie directory)
# model = mujoco.MjModel.from_xml_path("../../model/mujoco_menagerie/universal_robots_ur10e/scene.xml")
model = mujoco.MjModel.from_xml_path("<PATH>/mujoco_menagerie/universal_robots_ur10e/scene.xml")
# Create the data
data = mujoco.MjData(model)

def main():

    # Set the home position of the robot to avoid awkward positions
    mujoco.mj_resetDataKeyframe(model, data, 0)

    # Getting to know the model
    print("All bodies in the model:")
    for i in range(model.nbody):
        body_name = mujoco.mj_id2name(model, mujoco.mjtObj.mjOBJ_BODY, i)
        print(f"ID: {i}, Name: {body_name}\n")

    # nDOF
    nv = model.nv   

    # Assume some instantaneous position, velocity and acceleration of the joints
    pos_list = np.array([0.1, -0.2, 0.3, 0.2, 0.3, 0.4]) # joint angles relative to the home position
    vel_list = np.array([0.5, 0.1, 0.3, 0.4, 0.5, 0.6])
    acc_list = np.array([-0.2, 0.3, -0.4, 0.5, -0.6, 0.7])
    
    # Set those in the MjData
    data.qpos[:nv] += pos_list
    data.qvel[:nv] = vel_list.copy()
    data.qacc[:nv] = acc_list.copy()

    # Create an offscreen renderer
    renderer = mujoco.Renderer(model)
    
    # Render the current scene
    mujoco.mj_fwdPosition(model, data) # calculate and update the positions of bodies so that renderer can show them
    renderer.update_scene(data)
    img = renderer.render()    

    # Display the image using matplotlib
    plt.imshow(img)
    plt.axis('off')
    plt.show()

    # Free the renderer
    renderer.close()

    # Compute the LHS of the equation of motion: M qddot + C(q, qdot) + passive forces
    mass_matrix = np.zeros((nv, nv))

    # Compute the mass matrix which is dependent only on q
    mujoco.mj_invPosition(model, data) # You can also try mj_fwdPosition() here...
    mujoco.mj_fullM(model, mass_matrix, data.qM)

    print("Mass/inertia matrix M:")
    mujoco.mju_printMat(mass_matrix)
    print("Joint positions q:")
    mujoco.mju_printMat(data.qpos)
    print("Joint velocities qdot:")
    mujoco.mju_printMat(data.qvel)
    print("Joint accelerations qddot:")
    mujoco.mju_printMat(data.qacc)

    Mqddot = mass_matrix @ data.qacc
    print("M * qddot:")
    mujoco.mju_printMat(Mqddot)

    # Compute the Coriolis, centrifugal, and gravitational forces
    # Bias forces and passive damping forces depend on q and qdot, hence computed at velocity stage
    mujoco.mj_invVelocity(model, data) # same as mj_fwdVelocity()
    bias_forces = data.qfrc_bias.copy() 
    print("Bias forces:")
    mujoco.mju_printMat(bias_forces)

    # Other passive forces (position and velocity dependent) such as damping, joint stiffness, etc.
    passive_forces = data.qfrc_passive.copy()
    print("Passive forces:")
    mujoco.mju_printMat(passive_forces)

    # Passive forces are subtracted here because
    # In MuJoCo's calculations, they are included in the RHS, as a part of tau (see General framework)
    LHS = Mqddot + bias_forces - passive_forces
    print("LHS: M*qddot + bias forces - passive forces")
    mujoco.mju_printMat(LHS)

    print("RHS: Joint torques required to maintain the expected q qdot and qddot")
    mujoco.mj_inverse(model, data)
    mujoco.mju_printMat(data.qfrc_inverse)

    print("Difference:")
    mujoco.mju_printMat(LHS - data.qfrc_inverse)


if __name__ == "__main__":
    main()

No collisions are happening/external forces are applied to the robot in this code; you can include the corresponding terms to further expand the verification, e.g., see this.

[dazhongtu]

I am truly grateful for your assistance. Your code has been immensely helpful to me. Additionally, during testing, I discovered that two parameters in the robot's XML file—damping="10" and armature="0.1"— affect mjdata.qfrc_inverse but do not influence the inverse dynamics calculation dense_M @ data.qacc + data.qfrc_bias
After setting both damping and armature to 0, the error between the two became negligible—even smaller by an order of magnitude compared to the error observed in Simscape simulations.
Thank you again for your invaluable support.