Question on pinocchio ( how to require jacobian about floating joint )

[LHTLiu]

I built a floating type joint and then a franka robot (7 joints + 2 fingers). I want to solve the Jacobian matrix. What do the first six columns of this matrix mean?

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/fr3_with_hand.urdf"
urdf_path = "../Test_MM_FK_Jacobian/fr3_with_hand.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)
viz.loadViewerModel()



# 原来是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.4
base_y = 0.5
base_z = 0.6
translation = np.array([base_x, base_y, base_z])

# 根据CoppleiaSim中的规定，角度采用xyz欧拉角，一定要注意。
xyz_euler_x = 0.1
xyz_euler_y = 0.2
xyz_euler_z = 0.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")
Joint8_Id = model.getFrameId("fr3_joint8")  # 法兰末端,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]
Joint8_pose = data.oMf[Joint8_Id]
joint7_pose = data.oMi[joint7_Id]
print("hand_tcp_pose \n", hand_tcp_pose)
print("Joint8_pose \n", Joint8_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)

Joint8_Jaco = pin.getFrameJacobian(model, data, Joint8_Id, pin.LOCAL_WORLD_ALIGNED)
print("Joint8_Jaco \n", Joint8_Jaco)

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

the jacobian of Joint8

And why does the position change in the floating joint not affect the Jacobian matrix?

Thnaks.

@stephane-caron

[jorisv]

Hello @LHTLiu,

I don't get why you have highlight 9 last columns of the Joint8_Jaco matrix.
Floating joint can be seen in the 6 first column of this jacobian.

As you can see there is a bunch of zero on the bottom left corner that show that floating base translation has no effect on the rotational velocity of Joint 8.

Also, the last column are 0 maybe because joint 8 is before the last 2 joints. Then, the last two joint don't affect how joint 8 move.

[LHTLiu]

hello，thanks for your reply.

Indeed highlighting the last 9 columns may mislead you.

Are the first six columns of this Jcaobian Jacobian matrices of floating joint ？

If yes, then are the first 6 columns the differentials of position and direction respectively? I still have a question. I remember that the definition of the Jacobian matrix is ​​the partial derivative of the position and attitude in the terminal Cartesian coordinates with respect to each joint space. Can this be considered for floating joints?

I used the same urdf file (xml file), used pinocchio and mujoco to calculate the forward kinematics and Jacobian matrix of the same position, 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?