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