Pinocchio: Computation of Time-Derivative of Jacobian (Jd)

[gsutanto]

Dear Pinocchio Authors,

I have a question:
suppose if R is the orientation of the end-effector w.r.t. robot base frame as rotation matrix, and if local_J is the result of pinocchio.jointJacobian(), then the end-effector Jacobian w.r.t. base frame is (similar to the example/demo https://gepettoweb.laas.fr/doc/stack-of-tasks/pinocchio/master/doxygen-html/md_doc_b-examples_i-inverse-kinematics.html ):
J = [R, zeros(3,3); zeros(3,3), R] * local_J
How do we compute the time derivative of the end-effector Jacobian w.r.t. base frame Jd?
I was thinking to do Jd = [R, zeros(3,3); zeros(3,3), R] * local_Jd (local_Jd obtained from pinocchio.getJointJacobianTimeVariation() ) , but I realized that R is also varying with time (not a constant), so there shall be an additional term that depends on the time derivative of R...
So, how do we compute Jd properly?

I need Jd for computing end-effector acceleration xdd = Jd * qd + J * qdd.

P.S.:
(1) I also noticed that pinocchio.getFrameJacobian(self.pinoc_model, self.pinoc_data, self.pinoc_model.getFrameId('iiwa_link_ee'), pinocchio.ReferenceFrame.WORLD) is not the same as J = [R, zeros(3,3); zeros(3,3), R] * local_J computed above.
(2) I have asked Nicolas Mansard via email about this, please see his response below. (I put the question here, as per his request.)

Best Regards,

Giovanni

[gsutanto]

Nicolas Mansard's response by email is as follows:
"Dear Giovanni,

please post your question on github, as a new issue.

Don't compute the Jdot for computing the coriolis acceleration of the hand. Rather use the method to compute the acceleration of the bodies with qddot = 0.

I also suggest that you follow the spirit of Featherstone and Murray (which drives the implementation of Pinocchio) and work in 6D with either spatial velocity in the local frame (i.e. linear velocity of the local frame expressed using the basis of the local frame) or in the world frame (i.e. linear velocity of the virtual point attached to the body but passing through the origin of the world, expressed in the world basis)."

[gsutanto]

Dear Nicolas and Pinocchio Authors,

Do you mean something like this:
````

suppose q, qd, and qdd are joint position/configuration, joint velocity,

and joint acceleration, respectively

import pinocchio as pinoc

pinoc.computeForwardKinematicsDerivatives(pinoc_model, pinoc_data, q, qd, qdd)
pinoc_spatial_linear_vel = pinoc_data.ov[end_effector_idx].linear
pinoc_spatial_angular_vel = pinoc_data.ov[end_effector_idx].angular
pinoc_spatial_linear_acc = pinoc_data.oa[end_effector_idx].linear
pinoc_spatial_angular_acc = pinoc_data.oa[end_effector_idx].angular
````
?

I realized that the above computations are spatial velocities, i.e. if the end effector pose is expressed by rotation R and translation p, the end effector linear velocity p_dot, and the end effector angular velocity omega, then (according to Murray et al.'s "A Mathematical Intro. to Robotic Manipulation" book, equation (2.53)):
pinoc_spatial_linear_vel == -cross(omega, p) + p_dot
pinoc_spatial_angular_vel == omega
Is this correct?

So, if the above is correct, I may get away of computing end-effector (spatial) velocities and accelerations, without computing the time-derivative of Jacobian (Jd).

But what if I need Jd for something else, e.g. for implementing Gauss' (Force-based) Operational Space Controller like in the equation (56) of the paper "Operational Space Control: A Theoretical and Empirical Comparison" by Nakanishi et al.? How to compute Jd properly for this? Can you provide a code snippet/example/demo for this, similar to the inverse kinematics demo that you provided?

Thanks in advance!

P.S.: Also, I assume [p_dot; omega] == [R, zeros(3,3); zeros(3,3), R] * local_J * qd , if qd is joint velocity, and local_J is obtained from pinocchio.jointJacobian(), correct?

Best Regards,

Giovanni

[jcarpent]

Dear @gsutanto,
First of all, thanks a lot for using Pinocchio in your own research activity.

In Pinocchio, you have two functions called computeJointJacobiansTimeVariation which performs general computations and getJointJacobianTimeVariation which retrieves the Jacobian time derivative for your joint either in the local or the world frame.

Is this reply provides some helpfull directions?

[gabrielebndn]

Hi @gsutanto,

I also noticed that pinocchio.getFrameJacobian(self.pinoc_model, self.pinoc_data, self.pinoc_model.getFrameId('iiwa_link_ee'), pinocchio.ReferenceFrame.WORLD) is not the same as J = [R, zeros(3,3); zeros(3,3), R] * local_J computed above.

It is not. In Pinocchio, we tend to use Featherstone's spatial notation for velocities. With pinocchio.ReferenceFrame.WORLD we mean the velocity of the point at the origin of the world frame, expressed in the world frame itself.
The correct identity would be world_J = [R, skew(p)*R; zeros(3,3), R] * local_J.
The matrix J = [R, zeros(3,3); zeros(3,3), R] * local_J can be computed using pinocchio.ReferenceFrame.LOCAL_WORLD_ALIGNED. This option was added in v2.1.4.

In other words, see this test:

import pinocchio as pin
pin.switchToNumpyMatrix()

from pinocchio.utils import zero, skew, isapprox
import numpy as np

model = pin.buildSampleModelManipulator()
data = model.createData()

q = pin.randomConfiguration(model)
idx = model.getFrameId('effector_body')

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

M = data.oMf[idx]
R = M.rotation
p = M.translation
local_J = pin.frameJacobian(model,data,q,idx)

local_J_2 = pin.getFrameJacobian(model,data,idx,pin.ReferenceFrame.LOCAL)

world_J = pin.getFrameJacobian(model,data,idx,pin.ReferenceFrame.WORLD)

local_walign_J = pin.getFrameJacobian(model,data,idx,pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)

assert isapprox(local_J_2,local_J)

l2w = np.vstack([np.hstack([R,skew(p)*R]) , np.hstack([zero([3,3]),R])])
assert isapprox(l2w,M.toActionMatrix())
assert isapprox(l2w*local_J,world_J)

l2lwa = np.vstack([np.hstack([R,zero([3,3])]) , np.hstack([zero([3,3]),R])])
assert isapprox(l2lwa*local_J,local_walign_J)

P.S.: Also, I assume [p_dot; omega] == [R, zeros(3,3); zeros(3,3), R] * local_J * qd , if qd is joint velocity, and local_J is obtained from pinocchio.jointJacobian(), correct?

Yes, where p_dot and omega are the velocity of the point in the world frame. Notice that this lays outside of Featherstone's spatial notation. See above.

[jcarpent]

@gsutanto Do you have any feedback on our replies?

[gsutanto]

Dear @jcarpent and @gabrielebndn ,

Thanks for your response. Sorry I have been busy the past two days. I will have one more test tomorrow (Monday) on my side. If I have further questions, I will post it here. Otherwise I will close this case. Thanks so much!

Best Regards,

Giovanni Sutanto

[gsutanto]

Dear @jcarpent ,

Sorry for the delay, I tried the following:

# q is joint configuration/position
# qd is joint velocity
# qdd is joint acceleration
# R is rotation matrix of the end-effector w.r.t. robot base

pin.computeJointJacobians(model, data, q)
local_walign_J = pin.getJointJacobian(model, data,
                                      model.getJointId('iiwa_joint_7'),
                                      pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
local_J = pin.getJointJacobian(model, data,
                               model.getJointId('iiwa_joint_7'),
                               pin.ReferenceFrame.LOCAL)

pin.computeJointJacobiansTimeVariation(model, data, q, qd)
local_walign_Jd = pin.getJointJacobianTimeVariation(model, data,
                                                    model.getJointId('iiwa_joint_7'),
                                                    pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)
local_Jd = pin.getJointJacobianTimeVariation(model, data,
                                             model.getJointId('iiwa_joint_7'),
                                             pin.ReferenceFrame.LOCAL)

l2lwa = np.vstack([np.hstack([R,zero([3,3])]) , np.hstack([zero([3,3]),R])])

I checked that the relationship:
local_walign_J == matmul(l2lwa, local_J)
holds, but the relationship:
local_walign_Jd == matmul(l2lwa, local_Jd)
does not hold.

I guess this makes sense, because for the computation of local_walign_Jd should be equal to matmul(l2lwa, local_Jd) + (an additional term that depends on local_J and the time derivative of l2lwa (which includes time derivative of R)). Am I right?

Also, I guess to compute the end-effector acceleration w.r.t. the robot base (this lays outside of Featherstone's spatial notation, as @gabrielebndn says), I can compute:
[p_doubledot; omega_dot] = matmul(local_walign_Jd, qd) + matmul(local_walign_J, qdd)
Am I right?

Please clarify. Thanks so much!

Best Regards,

Giovanni Sutanto

[jcarpent]

@gsutanto Did you update the code to the current devel branch?
The support of LOCAL_WORLD_ALIGNED is new for JacobianTimeVariation.

By the way, how do you install pinocchio on your computer?

[jcarpent]

Following discussions in #906, I will provide an new release in the next few hours. It will solve your issue.

[gsutanto]

@gsutanto Did you update the code to the current devel branch?
The support of LOCAL_WORLD_ALIGNED is new for JacobianTimeVariation.

By the way, how do you install pinocchio on your computer?

I follow the instruction in https://stack-of-tasks.github.io/pinocchio/download.html , the Linux version, with:
sudo apt install robotpkg-py35-pinocchio

With the new release, are you saying that the relationship:
local_walign_Jd == matmul(l2lwa, local_Jd)
should hold?

[jcarpent]

The release is not yet available. The code you need is currently on the devel branch of the project.
I'm making a new release which will be shortly available.

Unfortunately, you will have to way for the robotpkg version.
On the other side, you will be able to install it by using Conda. Is it OK for you?

[gsutanto]

Hi @jcarpent ,

I see. Yeah, conda is fine (is there already installation instruction with conda?). Right now it's not critical for me, it can wait for a 2-3 days or so, and it's still fine for me.

But on the theoretical side, I have a few questions:
[1] Is local_Jd == time_derivative_of(local_J)?
[2] If local_Jd == time_derivative_of(local_J) is true, then isn't local_walign_Jd == matmul(l2lwa, local_Jd) + matmul(time_derivative_of(l2lwa), local_J)?

[gsutanto]

Dear @jcarpent ,

Is there an update on this case?

[jcarpent]

@gsutanto, sorry for my late reply, but those days are very full from my side.
I will answer to your request (I haven't read fully yet) on Monday. Is it OK?

[gsutanto]

OK

[jcarpent]

Dear @gsutanto,

Sorry for the delay, but the last two weeks were very busy for me.
To answer your questions, the answer to [1] is YES while the answer to [2] is NO.

I provided a code snippet to help you in the understanding below.
The idea is that J_lwa is just the transformed Jacobian from the local Jacobian by applying the LOCAL to WORLD action of the relative transform SE3((0,0,0),R) between the two frames.
It is then the same thing for the Jacobian times derivatives, which does not take into account the motion of the frame. This very related to the notion of SPATIAL velocity, as detailed by Featherstone in his book.

from __future__ import print_function

import pinocchio as pin
import eigenpy
import numpy as np
eigenpy.switchToNumpyMatrix()

model = pin.buildSampleModelManipulator()
data = model.createData()
data_plus = model.createData()

q = pin.randomConfiguration(model);
#print('q: ',q.T)
v = np.matrix(np.random.rand(model.nv,1))
a = np.matrix(np.random.rand(model.nv,1))

eps = 1e-8
q_plus = pin.integrate(model,q,v*eps)
#print('q_plus: ',q_plus.T)

joint_id = 6
pin.computeJointJacobiansTimeVariation(model,data,q,v)

J_local = pin.getJointJacobian(model,data,joint_id,pin.ReferenceFrame.LOCAL)
#print('J_local:\n',J_local)
dJ_local = pin.getJointJacobianTimeVariation(model,data,joint_id,pin.ReferenceFrame.LOCAL)

pin.computeJointJacobians(model,data_plus,q_plus)
iMi_plus = data.oMi[joint_id].inverse() * data_plus.oMi[joint_id]
J_local_plus = pin.getJointJacobian(model,data_plus,joint_id,pin.ReferenceFrame.LOCAL)
#print('J_local_plus:\n',J_local_plus)

dJ_local_fd = (iMi_plus.toActionMatrix()*J_local_plus-J_local)/eps
print('dJ_fd:\n',dJ_local_fd)
print('dJ:\n',dJ_local)

pin.forwardKinematics(model,data,q,v,a)
spatial_acc = J_local*a + dJ_local*v

# Question 1
print('spatial_acc: ', spatial_acc.T)
print('spatial_acc ref: ', data.a[joint_id].vector.T)

dJ_lwa = pin.getJointJacobianTimeVariation(model,data,joint_id,pin.ReferenceFrame.LOCAL_WORLD_ALIGNED)

M_lwa = data.oMi[joint_id].copy()
M_lwa.translation = np.matrix(np.zeros((3,1)))

dM_lwa = M_lwa.act(data.v[joint_id])
dM_lwa.linear = np.matrix(np.zeros((3,1)))

dJ_other_lwa = M_lwa.toActionMatrix() * dJ_local

# Question 2
print('dJ_other_lwa:\n',dJ_other_lwa)
print('dJ_lwa:\n',dJ_lwa)

[jcarpent]

@gsutanto I will close this issue as it seems to be solved. Feel free to reopen.