Why are the first three rows and columns of the Jacobian matrix calculated using Pinocchio and MuJoCo inconsistent, while the others are consistent?

[LHTLiu]

Intro

Hi!

I am a PhD student at Beihang University, I use MuJoCo for my research on forward kinematics and jacobian matrix of mobile manipulator.

My setup

Apple M1 Pro macOS Sonoma 14.6.1

PyCharm 2023.3.5 (Community Edition)

mujoco 3.2.2

My question

I used the same urdf file (xml file), used pinocchio and mujoco to calculate the forward kinematics and Jacobian matrix of the same position of robot (first the floating joint and then franka's robot with a total of 15 degrees of freedom (6+7+2fingers)), and found that the forward kinematics were consistent, but the first three rows and first three columns of the Jacobian matrix were inconsistent, and the remaining They are all consistent, what’s going on?

thanks

@yuvaltassa

Minimal model and/or code that explain my question

pinocchio

import sys
import pinocchio as pin
from pinocchio.visualize import MeshcatVisualizer
import numpy as np
# 除了安装对应的库，还需要安装 pip install meshcat
# conda env: Test_MM_FK_Jacobian_env

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

# urdf_path = "/home/lht/LHT/Test_MM_FK_Jacobian/Test_MM_FK_Jacobian/mm_base_and_franka_pinocchio.urdf"
urdf_path = "mm_base_and_franka_pinocchio.urdf"

model: pin.Model = pin.buildModelFromUrdf(urdf_path)
visual_model = pin.buildGeomFromUrdf(model, urdf_path, pin.GeometryType.VISUAL)
collision_model = pin.buildGeomFromUrdf(model, urdf_path, pin.GeometryType.COLLISION)

data: pin.Data = model.createData()

viz = MeshcatVisualizer(model, collision_model, visual_model)

try:
    viz.initViewer(open=True)
    viz.loadViewerModel()
except ImportError as error:
    print(error)
    sys.exit(0)


# 原来是planar类型的时候，输入的是x，y的位移、z轴转动角度的cos、sin。
# q = pin.randomConfiguration(model)
# q1 = pin.neutral(model)
# theta = -np.pi / 3
# q = np.array([1, 1, np.cos(theta), np.sin(theta),
#               0, 0, 0, -np.pi / 2, 0, np.pi / 2, np.pi / 4,
#               0, 0])


# urdf中的base相对于世界坐标系是floating类型，输入是xyz的位移，还有旋转对应的四元数；floating关节类型需要7个输入。
# 好像修改floating关节的位置量，并不会影响雅可比矩阵。也就是整机的雅可比矩阵和floating的位置量没关系，和旋转角度有关系。
base_x = 0.8
base_y = 0.9
base_z = 1.0
translation = 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])

quaternion = pin.Quaternion(rotation_matrix)

print(translation[0])
print(translation[1])
print(translation[2])

print('\n')

print(quaternion.x)
print(quaternion.y)
print(quaternion.z)
print(quaternion.w)
q = np.array([translation[0], translation[1], translation[2], quaternion.x, quaternion.y, quaternion.z, quaternion.w,
              # 0, 0, 0, -np.pi / 2, 0, np.pi / 2, 0,
              0.1, 0.2, 0.3, 0.4, 0.5, 0.6, 0.7,
              0, 0])

print(model.nq)

viz.display(q)
# viz.displayVisuals(True)

print("q=", q)

# 计算hand_tcp的正运动学
Hand_tcp_Id = model.getFrameId("fr3_hand_tcp")
Flange_Id = model.getFrameId("Flange")  # 法兰末端,no hand,跟matlab对应的时候计算这个就可以。
joint7_Id = model.getJointId("fr3_joint7")

pin.forwardKinematics(model, data, q)
pin.updateFramePlacements(model, data)
hand_tcp_pose = data.oMf[Hand_tcp_Id]
Flange_pose = data.oMf[Flange_Id]
joint7_pose = data.oMi[joint7_Id]
print("hand_tcp_pose \n", hand_tcp_pose)
print("Flange_pose \n", Flange_pose)
print("joint7_pose \n", joint7_pose)

# 计算hand_tcp的Jacobian
pin.computeJointJacobians(model, data, q)
Hand_tcp_Jaco = pin.getFrameJacobian(model, data, Hand_tcp_Id, pin.LOCAL_WORLD_ALIGNED)
print("Hand_tcp_Jaco \n", Hand_tcp_Jaco)

Flange_Jaco = pin.getFrameJacobian(model, data, Flange_Id, pin.LOCAL_WORLD_ALIGNED)
print("Flange_Jaco \n", Flange_Jaco)

# 计算joint7的Jacobian
Joint7_Jaco = pin.getJointJacobian(model, data, joint7_Id, pin.LOCAL_WORLD_ALIGNED)
print("Joint7_Jaco \n", Joint7_Jaco)

input("Press Enter to continue...")

mujoco

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等方式，如下。


# 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 的全局位置:\n", Flange_site_pos)
print("Flange_site 的旋转矩阵:\n", Flange_site_rot)

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


# 计算 Joint7 的运动学和雅可比矩阵。这个例子中，link7和joint7是一样的位置
# 目前直接计算link7的信息(fr3_link7)
Link7_id = m.body('fr3_link7').id

Link7_pos = d.xpos[Link7_id]
Link7_rot = d.xmat[Link7_id].reshape(3, 3)
print("Link7 的全局位置:\n", Link7_pos)
print("Link7 的旋转矩阵:\n", Link7_rot)

link7_jacp = np.zeros((3, m.nv))
link7_jacr = np.zeros((3, m.nv))
mujoco.mj_jacBody(m, d, link7_jacp, link7_jacr, Link7_id)
Link7_jacobian = np.vstack((link7_jacp, link7_jacr))

print("带有floating joint的 Link7 的雅可比矩阵（相对于world）: \n", Link7_jacobian)

Confirmations

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

[kevinzakka]

Hi @LHTLiu, MuJoCo computes a Jacobian in LOCAL_WORLD_ALIGNED, aka a Jacobian whose frame of reference is centered at the local frame but aligned with the world frame. To obtain a Jacobian expressed in the local frame, aka a body Jacobian, you need to left-multiply by Adjoint[T_fw] where T_fw is the transform of the world frame w expressed in the desired frame f.

Here is some code to illustrate how to match forward kinematics in MuJoCo and Pinocchio:

import mujoco
import numpy as np
import pinocchio as pin
from robot_descriptions.loaders.mujoco import (
    load_robot_description as mj_load_description,
)
from robot_descriptions.loaders.pinocchio import (
    load_robot_description as pin_load_description,
)

np.set_printoptions(precision=2, threshold=1e-5, suppress=True)

def skew(x):
    wx, wy, wz = x
    return np.array(
        [
            [0.0, -wz, wy],
            [wz, 0.0, -wx],
            [-wy, wx, 0.0],
        ]
    )


def adjoint(R, t):
    assert R.shape == (3, 3)
    assert t.shape == (3,)
    A = np.block(
        [
            [R, np.zeros((3, 3))],
            [skew(t) @ R, R],
        ]
    )
    assert A.shape == (6, 6)
    return A


def inverse(R, t):
    assert R.shape == (3, 3)
    assert t.shape == (3,)
    R_inv = R.T
    t_inv = -R_inv @ t
    return R_inv, t_inv

# Sample a random joint configuration
q = 0.1 * np.pi * (2.0 * np.random.random((9,)) - 1.0)
print(q.shape)  # (9 because 7 + 2 gripper).

# Pinocchio model and data.
robot = pin_load_description("panda_description")
model_pin = robot.model
data_pin = robot.data

# Run forward kinematics.
pin.computeJointJacobians(model_pin, data_pin, q)
pin.updateFramePlacements(model_pin, data_pin)
frame_id = model_pin.getFrameId("panda_link7")

# Mujoco model and data.
print("Loading mujoco model...")
model_mj = mj_load_description("panda_mj_description")
data_mj = mujoco.MjData(model_mj)

# Run forward kinematics.
data_mj.qpos = q
mujoco.mj_kinematics(model_mj, data_mj)
mujoco.mj_comPos(model_mj, data_mj)

# Forward kinematics: check frame transforms are identical.

# Pin.
T_sb_pin = data_pin.oMf[frame_id].copy()
t_sb_pin = T_sb_pin.np[:3, 3]
R_sb_pin = T_sb_pin.np[:3, :3]
# Mujoco.
t_sb = data_mj.body("link7").xpos.copy()
R_sb = data_mj.body("link7").xmat.copy().reshape(3, 3)
np.testing.assert_allclose(t_sb, t_sb_pin, atol=1e-7)
np.testing.assert_allclose(R_sb, R_sb_pin, atol=1e-7)

# Jacobian: check that mujoco's jac matches pinocchio's jac in frame: LOCAL_WORLD_ALIGNED.
jac_pin_local_world_aligned: np.ndarray = pin.getFrameJacobian(
    model_pin, data_pin, frame_id, pin.ReferenceFrame.LOCAL_WORLD_ALIGNED
)
jac_muj = np.empty((6, model_mj.nv))
mujoco.mj_jacBody(model_mj, data_mj, jac_muj[:3], jac_muj[3:], model_mj.body("link7").id)
np.testing.assert_allclose(jac_pin_local_world_aligned, jac_muj, atol=1e-7)

# How to transform MuJoCo's jacobian so that it matches pinocchio's LOCAL frame.
jac_pin_local: np.ndarray = pin.getFrameJacobian(model_pin, data_pin, frame_id, pin.ReferenceFrame.LOCAL)

R_bs, t_bs = inverse(R_sb, t_sb)
A_bs = adjoint(R_bs, np.zeros(3))
jac_muj_local = A_bs @ jac_muj

np.testing.assert_allclose(jac_muj_local, jac_pin_local, atol=1e-7)

[LHTLiu]

Hello, @kevinzakka . Thank you for your careful reply, but I am still a little confused. Among the Jacobian matrices calculated by pinocchio and mujoco that I mentioned before, which one is Flange's Jacobian matrix relative to the world coordinate system?

[kevinzakka]

@LHTLiu Upon closer inspection, it looks like you are comparing MuJoCo's Jacobian with Pinocchio's using LOCAL_WORLD_ALIGNED. The problem seems to arise when a floating joint is introduced (which was not present in my example). Can you confirm that this is your question?

[LHTLiu]

Hello, @kevinzakka , I want to compute the Flange w.r.t. the world frame, and I don't understand your code. Among the codes I provided at the beginning, which one is the correct Jacobian matrix of flange relative to the world coordinate system? Is it necessary to use pin.WORLD instead of pin.LOCAL WORLD ALIGNED in pinocchio? but? I use pin.WORLD to calculate the Jacobian matrix of flange, but the result feels even more wrong?