Why are the calculation results of mj jac and mj jac Site inconsistent?

[LHTLiu]

Intro

Hi!

I am a student, I use MuJoCo for computing Jacobian matrix with mj_jac and mj_jacSite.

My setup

Apple M1 Pro macOS Sonoma 14.6.1

PyCharm 2023.3.5 (Community Edition)

mujoco 3.2.2

My question

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

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

# 将 mm_base_and_franka_mujoco.urdf 拖到 MuJoCo 软件中，点击（Save xml）生成的 xml文件，mujoco好像不能直接导入urdf文件
# 跟pinocchio类似，使用类型注释，要不 m 这个根本索引不出来。
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])

# 根据CoppleiaSim中的规定，角度采用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]

print(joint_angles)

# 设置关节位置，为所有关节赋值
d.qpos[:len(joint_angles)] = joint_angles

# 计算正向运动学，更新仿真状态。
mujoco.mj_forward(m, d)


# mujoco中的xml文件，没有pinochhio中利用的urdf文件中的固定关节类型，转换的时候直接给合并了。
# 在mujoco中计算正运动学和雅可比矩阵的时候，好像只有joint和body，想要计算其他位置，没想想办法

# 1、手动转化，直接在python代码中进行，设置偏移量，乘以rotation matrix等方式，如下。
#    Fr3_Link7_id = m.body("fr3_link7").id
#    上述得到的body的id的方式和下面的方式是一样的，只是上面的.id索引不出来，但是可以正常使用
Fr3_Link7_id = mujoco.mj_name2id(m, mujoco.mjtObj.mjOBJ_BODY, "fr3_link7")

Fr3_Link7_pos = d.xpos[Fr3_Link7_id]
Fr3_Link7_rot = d.xmat[Fr3_Link7_id].reshape(3, 3)

# hand 在 fr3_link7 局部坐标系中的偏移量
Flange_2_link7_offset = np.array([0, 0, 0.107])
# 将局部偏移量转换到世界坐标系
world_hand_offset = np.dot(Fr3_Link7_rot, Flange_2_link7_offset)
# hand 在世界坐标系中的位置 = fr3_link7 的位置 + 偏移
Flange_pos = Fr3_Link7_pos + world_hand_offset
Flange_rot = Fr3_Link7_rot

print(Flange_pos)
print(Flange_rot)

# 2、修改生成的xml文件，手动添加了site标签。

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))  # vstack函数将多个数组沿垂直（行）方向堆叠的函数
print("带有floating joint的Flange_site处的雅可比矩阵（相对于world）: \n", Flange_site_jacobian)

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

I want to calculate the jacobian matrix of Flange relative to the world coordinate system. Why are the results calculated by mj_jac and mj_jacSite inconsistent?

@yuvaltassa Thanks

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>
  <worldbody>
    <light pos="0 0 1"/>
    <geom type="sphere" size="1" rgba="1 0 0 1"/>
  </worldbody>
</mujoco>

Code:

import mujoco
import mediapy as media
model = mujoco.MjModel.from_xml_string(xml)
data = mujoco.MjData(model)

with mujoco.Renderer(model) as renderer:
  mujoco.mj_forward(model, data)
  renderer.update_scene(data)

  media.show_image(renderer.render())

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]

Sorry, I don't understand where the inconsistency is? Here is the implementation of mj_jacSite, it is literally one line:

// compute site Jacobian
void mj_jacSite(const mjModel* m, const mjData* d, mjtNum* jacp, mjtNum* jacr, int site) {
  mj_jac(m, d, jacp, jacr, d->site_xpos + 3*site, m->site_bodyid[site]);
}

[yuvaltassa]

Please make a minimal model and code that can reproduce the issue, and maybe we'll be able to help.

[LHTLiu]

i use the following code to compute the jacobian matrix the Flange_site related to the world frame,

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(Flange_site_jacobian)

i would like to achieve the same result to the function mj_jac . What should the last two inputs of this function be?

I tried inputting point as the position of the Flange in the world coordinate system and body is the original Flange_site_id, but the results were both 0. I am confused about the meaning of the last two input parameters.

Thanks @yuvaltassa

[yuvaltassa]

As you can see from the docs and from the code sample I sent above, point is in the world frame and the id is the body id, so you want something like model.site("Flange_site").bodyid.

[LHTLiu]

I seem to understand, is it because mj_jac cannot calculate the Jacobian matrix of site type?
@yuvaltassa

[yuvaltassa]

mj_jac works with bodies, sites are attached to bodies. If you want the jacobian of a site with mj_jac you need to specify the site's parent body and tell mj_jac where the site is.