Some questions about the getFrameJacobian() function

[liuxiaoyiyixiaoliu]

I created a frame "LF_1" based on "joint_22", which has an offset from "joint_22". The code is as follows

model=pin.buildModelFromUrdf("/home/liuyi/humanoid_py/model/humanoid.urdf",pin.JointModelFreeFlyer())
tranF_1=pin.SE3.Identity()
tranF_1.translation=np.array([0.1063, -0.05, -0.05935])
model.addFrame(pin.Frame("LF_1",model.getJointId("joint_22"),model.getFrameId("joint_22"),tranF_1,pin.FrameType.JOINT,pin.Inertia.Zero()))
data= model.createData()
q=pin.neutral(model)
pin.forwardKinematics(model,data,q)
pin.updateFramePlacements(model, data)
pin.computeJointJacobians(model,data,q)

And printed the coordinates of a certain joint configuration,the results are as follows,it can be seen that they are different.

print("joint_22")
print(data.oMf[model.getFrameId("joint_22")])
print("LF_1")
print(data.oMf[model.getFrameId("LF_1")])

output:
joint_22
R =
1.97215e-31 0 1
-4.44089e-16 1 0
-1 -4.44089e-16 3.9443e-31
p = 2.34035e-17 0.1273 -1.30803

LF_1
R =
1.97215e-31 0 1
-4.44089e-16 1 0
-1 -4.44089e-16 3.9443e-31
p = -0.05935 0.0773 -1.41433

But when I solve the Jacobian matrix for the two coordinates, they are indeed the same,the code is as follows, the result is a matrix with all zeros，I have tried OP_FRAME, JOINT, FIXED_JOINT, and BODY in the FrameType attribute, but the problem still exists.

JL_1=pin.getFrameJacobian(model, data, model.getFrameId("LF_1"), pin.ReferenceFrame.WORLD)
JL_x=pin.getJointJacobian(model, data, model.getJointId("joint_22"), pin.ReferenceFrame.WORLD)
print("result")
print(JL_1-JL_x)

I think the linear velocity of the two coordinates should be different, that is, their partial Jacobians are different. Is there something wrong with my coordinate settings?

[florent-lamiraux]

Hello @liuxiaoyiyixiaoliu .
According to the documentation, https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/namespacepinocchio.html#a5aa19d265b05aaa782bb24ae4d8894f0, you need to call pinocchio::framesForwardKinematics after pinocchio::computeJointJacobians.

As a side remark, I cannot reproduce your issue without the robot urdf model.

[liuxiaoyiyixiaoliu]

Hello @florent-lamiraux , thank you for your reply, I tried the above method, but the problem still exists, this time I simplified the code and used the urdf file at '/example-robot-data/robots/ur_description/urdf/ur5_robot.urdf' ,code show as below：

import numpy as np
import pinocchio as pin
from os.path import abspath, dirname, join

model=pin.buildModelFromUrdf("./ur5_robot.urdf",pin.JointModelFreeFlyer())

tranF_1=pin.SE3.Identity()
tranF_1.translation=np.array([0, 0, 1])
model.addFrame(pin.Frame("LF_1",model.getJointId("wrist_3_joint"),model.getFrameId("wrist_3_joint"),tranF_1,pin.FrameType.JOINT,pin.Inertia.Zero()))
data= model.createData()
q=pin.neutral(model)

pin.computeJointJacobians(model,data,q)
pin.framesForwardKinematics(model,data,q)
pin.updateFramePlacements(model, data)

JL_1=pin.getFrameJacobian(model, data, model.getFrameId("LF_1"), pin.ReferenceFrame.WORLD)
JL_x=pin.getFrameJacobian(model, data, model.getFrameId("wrist_3_joint"), pin.ReferenceFrame.WORLD)

print(data.oMf[model.getFrameId("LF_1")])
print(data.oMf[model.getFrameId("wrist_3_joint")])
print(JL_1-JL_x)

The result of my run is that two different reference frames have the same Jacobian matrix

[florent-lamiraux]

Looking again at the documentation of getFrameJacobian I remember now. The velocities expressed in the columns of the Jacobian when passing pin.ReferenceFrame.WORLD are the linear velocity of the point of the frame coninciding with the origin of the world frame and the angular velocity. Both velocities are expressed in the world basis. As "wrist_3_joint" and "LF_1" are attached to the same rigid-body, the point coinciding with the origin of the world has the same velocity.

[liuxiaoyiyixiaoliu]

1.I don't know if this is the right way to understand it，The Jacobian matrix under pin.ReferenceFrame.WORLD has such a relationship:dot_x_world=J_world @dot_q_world，and pin.ReferenceFrame.local has this relationship：dot_x_local=J_local
@dot_q_local,Among them, the suffixes world and local represent the speed in global coordinates and local coordinates respectively.
2.I don’t quite understand why two points of the same rigid body have the same linear velocity. How to consider the influence of angular velocity?

[florent-lamiraux]

Do you understand better with this proposition of modification in the doc ?

[liuxiaoyiyixiaoliu]

Thank you for your response. Would it be possible to include some formulas to help clarify the mapping relationship between Cartesian space velocity and joint velocity, especially under different reference systems?

[florent-lamiraux]

I think this should be done in the "mathematical formulation" section of the documentation that is still to be written.
In short, LOCAL provides the velocity of the origin of the moving frame and the angular velocity expressed in the basis of the moving frame,
LOCAL_WORLD_ALIGNED provides the same velocities expressed in the world frame. I do not recommend you to use WORLD.

[fabinsch]

There is also a visualization of what @florent-lamiraux just explained on the cheat sheet which might help you to understand the difference of the reference frames.