get vel and j*qd are inconsistent？

[LHTLiu]

Intro

I am a student, I use MuJoCo for computing Jacobian matrix with free joint.

My setup

Apple M1 Pro macOS Sonoma 14.6.1

PyCharm 2023.3.5 (Community Edition)

mujoco 3.2.2

My question

I verified the Jacobian matrix and used two methods: direct calculation speed and j*qd, but the speed obtained was inconsistent.

joint_angles = [trans[0], trans[1], trans[2], quat.w, quat.x, quat.y, quat.z,
                0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7,
                0, 0]
qd = np.zeros(m.nv)
qdd = np.zeros(m.nv)
qd[0] = 0.1
qd[1] = 0.2
qd[2] = 0.3
qd[3] = 0.4
qd[4] = 0.5
qd[5] = 0.6
qd[6] = 0.7
qd[7] = 0.8

d.qpos[:len(joint_angles)] = joint_angles
d.qvel[:len(qd)] = qd
d.qacc[:len(qdd)] = qdd

mujoco.mj_forward(m, d)

mujoco.mj_jacSite(m, d, Flange_site_jacp, Flange_site_jacr, Flange_site_id)
Flange_site_jacobian = np.vstack((Flange_site_jacp, Flange_site_jacr)) 
print("带有floating joint的Flange_site处的雅可比矩阵（相对于world）: \n", Flange_site_jacobian)

v_Flange = np.zeros(6)
mujoco.mj_objectVelocity(m,d,mjtObj.mjOBJ_SITE,Flange_site_id,v_Flange,0)
print(v_Flange)
print(Flange_site_jacobian @ qd)

@yuvaltassa Thanks

Minimal model and/or code that explain my question

No response

Confirmations

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

[yuvaltassa]

This is not reproducible. Please make a simple, minimal model so that the model and your code together constitute a Minimal Reproducing Example.

[LHTLiu]

Thank you for your suggestion.

the complete code is shown below

import numpy as np
import pinocchio as pin
import mujoco.viewer
from mujoco import mjtObj

np.set_printoptions(suppress=True, precision=4, linewidth=np.inf)

m: mujoco.MjModel = mujoco.MjModel.from_xml_path("./mm_base_and_franka_mujoco.xml")
d: mujoco.MjData = mujoco.MjData(m)

base_x = 0.8
base_y = 0.9
base_z = 1.0
trans = np.array([base_x, base_y, base_z])

# 根据CoppeliaSim中的规定，角度采用xyz欧拉角，一定要注意。
xyz_euler_x = 1.1
xyz_euler_y = 1.2
xyz_euler_z = 1.3
xyz_euler = np.array([xyz_euler_x, xyz_euler_y, xyz_euler_z])

rotation_matrix = pin.utils.rotate('x', xyz_euler[0]) @ \
                  pin.utils.rotate('y', xyz_euler[1]) @ \
                  pin.utils.rotate('z', xyz_euler[2])

quat = pin.Quaternion(rotation_matrix)

joint_angles = [trans[0], trans[1], trans[2], quat.w, quat.x, quat.y, quat.z,
                0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7,
                0, 0]

qd = np.zeros(m.nv)
qdd = np.zeros(m.nv)
qd[0] = 0.1
qd[1] = 0.2
qd[2] = 0.3
qd[3] = 0.4
qd[4] = 0.5
qd[5] = 0.6
qd[6] = 0.7
qd[7] = 0.8

print(joint_angles)

d.qpos[:len(joint_angles)] = joint_angles
d.qvel[:len(qd)] = qd
d.qacc[:len(qdd)] = qdd

mujoco.mj_forward(m, d)

Flange_site_id = m.site('Flange_site').id
Flange_site_pos = d.site_xpos[Flange_site_id]
Flange_site_rot = d.site_xmat[Flange_site_id].reshape(3, 3)
print("Flange_site 的全局位置:", Flange_site_pos)
print("Flange_site 的旋转矩阵:\n", Flange_site_rot)

Flange_site_jacp = np.zeros((3, m.nv))
Flange_site_jacr = np.zeros((3, m.nv))
mujoco.mj_jacSite(m, d, Flange_site_jacp, Flange_site_jacr, Flange_site_id)
Flange_site_jacobian = np.vstack((Flange_site_jacp, Flange_site_jacr))
print("带有floating joint的Flange_site处的雅可比矩阵（相对于world）: \n", Flange_site_jacobian)


v_Flange = np.zeros(6)
mujoco.mj_objectVelocity(m,d,mjtObj.mjOBJ_SITE,Flange_site_id,v_Flange,0)
print("v_Flange_via_objectVelocity", v_Flange)

print("v_Flange_via_j*qd", Flange_site_jacobian @ qd)

the result is shown below

Why do the two results in the red box above differ

Thanks @yuvaltassa

[yuvaltassa]

That is still not minimal or reproducible. I don't have access to your mm_base_and_franka_mujoco.xml Please make a very simple model that does not require binary assets to load. Apparently all you need is a single free joint? Or maybe a body with the free joint and a child body with a hinge or some. Make this simple model and I will take a look.

[LHTLiu]

Test_MM_FK_Jacobian.zip

this is reproducible after changing some files paths.

thanks @yuvaltassa

[yuvaltassa]

It is reproducible, but not minimal. Please make a new minimal model that has only the things that are required to reproduce you issue (e.g. no meshes, minimal number of joints etc.). The model should fit in 10 or 20 lines of XML. Thanks.