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

[LHTLiu]

Hi!

My setup
Apple M1 Pro macOS Sonoma 14.6.1
PyCharm 2023.3.5 (Community Edition)
mujoco 3.2.2
pinocchio 3.2.0

**My question:**I use Pinocchio for my research on forward kinematics and jacobian matrix of mobile manipulator.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?

and I don't particularly understand the difference between the three methods in the ReferenceFrame, Can you describe it in more detail? which one is Flange's Jacobian matrix relative to the world coordinate system?

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)

thanks

@stephane-caron

[jcarpent]

Have you taken a look at the cheat sheet: https://github.com/stack-of-tasks/pinocchio/blob/master/doc/pinocchio_cheat_sheet.pdf?

[jcarpent]

It depends on the convention used in both software. In our case, the free-flyer velocity is expressed in the LOCAL frame of the joint, following the convention of Featherstone.

[LHTLiu]

thanks, @jcarpent . I want to know if I calculate the Jacobian matrix of a certain position on the robot relative to the world coordinate system, should the reference frame be pin.WORLD?

[LHTLiu]

@jcarpent hello，can you help me about the above question.thanks

[ManifoldFR]

The jefe is busy, we're quite sorry but we only have a limited time to answer questions due to our other duties, it's best not to press us 😃

[LHTLiu]

ok, i'm sorry, thanks