Calculation issue of `getFrameJacobianTimeVariation`

[matheecs]

Bug description

I finded that the method getFrameJacobianTimeVariation will return a wrong $\dot{J}_c$ and get a $a_c$ which was different from the result of getFrameClassicalAcceleration. We need to use $\dot{J}_c = \dot{J}_s + \omega \times J_l$ (#1907) to calculate correct $\dot{J}_c$.

Code

These are the test code:

import pinocchio
import numpy as np

np.set_printoptions(precision=3, suppress=True)

model = pinocchio.buildSampleModelManipulator()
data = model.createData()
frame_idx = model.getFrameId(model.frames[-1].name)

q = np.random.rand((model.nv))
v = np.random.rand((model.nv))

pinocchio.forwardKinematics(model, data, q, v, 0*v)
pinocchio.computeJointJacobians(model, data, q)
pinocchio.computeJointJacobiansTimeVariation(model, data, q, v)
pinocchio.updateFramePlacements(model, data)

a_c = pinocchio.getFrameClassicalAcceleration(
    model, data, frame_idx, pinocchio.WORLD).vector
a_c_bug = pinocchio.getFrameJacobianTimeVariation(
    model, data, frame_idx, pinocchio.LOCAL_WORLD_ALIGNED) @ v

print(f'a_c:     {a_c}')
print(f'a_c_bug: {a_c_bug}')

# try to fix the bug: dJc = dJs - Js_linear x w
dJs = pinocchio.getFrameJacobianTimeVariation(
    model, data, frame_idx, pinocchio.WORLD)
Js = pinocchio.getFrameJacobian(
    model, data, frame_idx, pinocchio.WORLD)

J_linear_cross_w = np.zeros_like(dJs[:3, :])

w = pinocchio.getFrameVelocity(model, data, frame_idx, pinocchio.WORLD).angular
for col in range(model.nv):
    J_linear_cross_w[:, col] = np.cross(Js[:3, col], w)
dJc = dJs.copy()
dJc[:3, :] = dJs[:3, :] - J_linear_cross_w
print(f'a_c_fix: {dJc @ v}')

result:

a_c:     [ 0.713 -2.178  1.082 -0.499 -0.869  1.065]
a_c_bug: [-0.22   0.127  1.18  -0.499 -0.869  1.065]
a_c_fix: [ 0.713 -2.178  1.082 -0.499 -0.869  1.065]

the a_c and a_c_fix always are the same.

Additional context

I finded that getFrameJacobianTimeVariation will call translateJointJacobian. So the bug may coming from https://github.com/stack-of-tasks/pinocchio/blob/0caf0ca4d07e63834cdc420c703993662c59e01b/include/pinocchio/algorithm/jacobian.hxx#L197

which was inconsistant $\dot{J}_c = \dot{J}_s + \omega \times J_l$ (#1907)

System

• OS: Ubuntu 22.04.3 LTS
• Pinocchio version: 3

I would appreciate your help.

[jcarpent]

I think there is some slight confusion in what you have written. The thing is that getFrameJacobianTimeVariation returns $\frac{d\,J_s}{dt}$, which is the time derivative with respect to the spatial acceleration. In your context, you are interested in the classic point acceleration, which is not a spatial acceleration.

But, as you exposed, you can express the classical acceleration as $a_c = J_c \, \ddot{q} + \dot{J}_c \, \dot{q}$. And this given by the formula $\dot{J}_c = \dot{J}_s + \omega \times J_l$.

You can check this via the following Python scripts.

import pinocchio as pin
import numpy as np

np.set_printoptions(precision=3, suppress=True)

model = pin.buildSampleModelManipulator()
data = model.createData()
frame_idx = model.getFrameId(model.frames[-1].name)

q = np.random.rand((model.nv))
v = np.random.rand((model.nv))
v_dot = np.random.rand((model.nv))
# v_dot = np.zeros((model.nv)) 

pin.forwardKinematics(model, data, q, v, v_dot)
pin.computeJointJacobiansTimeVariation(model, data, q, v)
pin.updateFramePlacements(model, data)

a_c = pin.getFrameClassicalAcceleration(model, data, frame_idx, pin.LOCAL) # Classic acceleration
a_s = pin.getFrameAcceleration(model, data, frame_idx, pin.LOCAL) # Spatial acceleration
v_s = pin.getFrameVelocity(model, data, frame_idx, pin.LOCAL) 

## Check the relation between a_c and a_s
a_c_bis = a_s.copy()
a_c_bis.linear += np.cross(v_s.angular,v_s.linear)

assert a_c_bis.isApprox(a_c)

## Compute a_s from the Jacobians
J_s = pin.getFrameJacobian(model,data,frame_id=frame_idx, reference_frame=pin.LOCAL)
Jdot_s = pin.getFrameJacobianTimeVariation(model,data,frame_id=frame_idx, reference_frame=pin.LOCAL)
a_s_bis = pin.Motion(J_s @ v_dot + Jdot_s @ v) 

assert a_s_bis.isApprox(a_s)

## Compute Jacobians of the classic accelerations
J_c = J_s.copy()

w_cross = pin.skew(v_s.angular)

Jdot_c = Jdot_s.copy()
Jdot_c[:3,:] += w_cross @ J_c[:3,:]

a_c_bis = pin.Motion(J_c @ v_dot + Jdot_c @ v)

assert a_c_bis.isApprox(a_c)

[matheecs]

Thank you for your reply. One more question:

Why jacobians of the classical velocity J_c (in your scripts) always equal to jacobians of the spatial velocity J_s?

...
## Compute Jacobians of the classic accelerations
J_c = J_s.copy()
...

[ALICEYZ5]

@jcarpent I have two questions regarding time derivatives of Jacobian: 1). In your code posted above, Jdot_c is expressed in LOCAL frame right? How do I get Jdot_c in LOCAL_WORLD_ALIGNED frame? I used a)numerical derivative of Jc (rf: LOCAL_WORLD_ALIGNED): (Jc_current - Jc_previous)/dt, and, b) another rigid body kinematic/dynamic solver as ground truth, then I tried different ref frames using pinocchio, none of them close to the ground truth, while a) and b) are pretty close. 2). The first column from Jdot_s = pin.getFrameJacobianTimeVariation(model,data,frame_id=frame_idx, reference_frame=pin.LOCAL_WORLD_ALIGNED) are always zero, no matter what robot configuration. Is there a problem with this calculation. Thanks,

[jcarpent]

@ALICEYZ5 Please share you code before getting feedback from the Pinocchio's dev team

[ALICEYZ5]

The code I am using is based on the code you provided. To save your time, I understand getFrameJacobianTimeVariation returns the time derivative with respect to the spatial acceleration. My question is how to get JacobianTimeVariation with respect to the classic acceleration in World Frame.

import pinocchio as pin
import numpy as np

np.set_printoptions(precision=3, suppress=True)

model = pin.buildModelFromUrdf("/urdf_path/xx.urdf")
data = model.createData()
frame_idx = model.getFrameId(model.frames[-1].name)

q = np.array([0.5236,-0.7854,0,1.5708,0,0.6981,0.5236])
v = np.array([0.1,0.1,0.1,0.1,0.1,0.1,0.1])
v_dot = np.random.rand((model.nv))

pin.forwardKinematics(model, data, q, v, v_dot)
pin.computeJointJacobiansTimeVariation(model, data, q, v)
pin.updateFramePlacements(model, data)

J_s = pin.getFrameJacobian(model,data,frame_id=frame_idx, reference_frame=pin.LOCAL_WORLD_ALIGNED)
# compute the time derivative of the Jacobian in world frame
Jdot_s = pin.getFrameJacobianTimeVariation(model,data,frame_id=frame_idx, reference_frame=pin.LOCAL_WORLD_ALIGNED)

print(J_s)
print(Jdot_s)

[jcarpent]

Do you mean LOCAL_WORLD_ALIGN ?

[ALICEYZ5]

YES, JacobianTimeVariation with respect to the classic acceleration expressed in LOCAL_WORLD_ALIGN

[ALICEYZ5]

@jcarpent any updates?

[stephane-caron]

I believe the original issue was addressed by https://github.com/stack-of-tasks/pinocchio/issues/2141#issuecomment-1925393733, thus closing this thread as there is no action needed in Pinocchio itself.

There were some related questions open:

• One from @matheecs in https://github.com/stack-of-tasks/pinocchio/issues/2141#issuecomment-1925582254 and
• One from @ALICEYZ5 in https://github.com/stack-of-tasks/pinocchio/issues/2141#issuecomment-2068201550

Can you open your questions as separate topics in Discussions?

Issues are for bugs that should be fixed in Pinocchio, whereas Discussions are for general discussions and building understanding shared with the broader community.

[stephane-caron]

@ALICEYZ5 in the case of https://github.com/stack-of-tasks/pinocchio/issues/2141#issuecomment-2068201550 your code cannot be run as-is due to the undefined URDF. To maximize the chances that someone looks at your question, write your sample for an available model such as pin.buildSampleModelManipulator().