Kinematics Hessian -- Jacobian derivatives

[Leph]

First, I would like to thank you all a lot for your great open source work and implementation.

The analytical derivatives of the dynamics are a very valuable feature of the library. Is there a way using the currently implemented Pinnochio primitives to efficiently compute the kinematics Hessian -- the partial derivatives of a Jacobian of a frame ; which is a tensor of (6 x nv x nv) ?

Thanks very much.

Best,

[jcarpent]

@Leph Thanks for your interests in our project.

There is no yet a public interface for it. I plan to add in the future. If it is important for your applications, I can implement it in the very next days.

Otherwise, you can still use the automatic differentiation layer with CppAD to do that from the classic Jacobians. Are you familiar with such things?

[Leph]

@jcarpent, thanks for your answer.

I have already prototyped my code with classic numerical finite differences. Thanks to the already implemented partial derivatives, the Hessian is the last piece of the puzzle preventing me to reach the 1ms computation time.

This is not highly critical, but I will be happy to experiment it as soon as it will be included.

Thank you again.

[jcarpent]

Ok. I may implement them soon.

[nmansard]

Thanks @traversaro for referring to this discussion in the idyntree project. @Leph: are you sure you need H directly, and not for computing a quantity that we be easier to compute directly. I don't know the context of this question, but for an example: if you want H to finally compute H.v_q (the product of the tensor with the robot velocity), then we may already have this quantity computed. And it would likely be cheaper to compute it that way.

[Leph]

@nmansard, I am differentiating the equation of motion under contacts and using it within a QP formulation. You are right, I am actually looking for a kind of Hessian-vector product. It would have been indeed much simplier, but unfortunately, the differentation ends up with a "H.lambda" tensor product against the first tensor dimention instead of the classical last one (resulting with an (nv x nv) matrix instead of a (6 x nv)).

This is equivalent to compute nv times the regular matrix-product: (dJ/dq_i)^T.lambda

[nmansard]

Do you have some maths to describe what you are doing, maybe in a scientific paper? How is that different from equation (12) of this paper https://hal.archives-ouvertes.fr/hal-01851596v2/document

[jcarpent]

@nmansard, I am differentiating the equation of motion under contacts and using it within a QP formulation. You are right, I am actually looking for a kind of Hessian-vector product. It would have been indeed much simplier, but unfortunately, the differentation ends up with a "H.lambda" tensor product against the first tensor dimention instead of the classical last one (resulting with an (nv x nv) matrix instead of a (6 x nv)).

This is equivalent to compute nv times the regular matrix-product: (dJ/dq_i)^T.lambda

I think your current is also related to #648. I will provide soon all the API for dealing with contacts dynamics and so on, in the correct manner with Pinocchio.

[jcarpent]

Indeed, for the Hessian part, the idea is really to provide the tensor * vector operation. Not really to provide the tensor in its own which is often not necessary.

[Leph]

@nmansard When I linearize the following term from the equation of motion: J(q+dq)^T.lambda I get (by noting H=dJ/dq): (H.dq)^T.lambda which can then be rewritten as M.dq with M(i,j) = Sum_k (H(k,i,j)).lambda(k). I am trying to see if there is a way to compute M efficently.

@jcarpent, you're right. from eq.10a of your paper "Analytical Derivatives of Rigid Body Dynamics Algorithms", the term dJ^T/dq.lambda computed by the derivatives of rnea(q,0,0,lambda) with zero gravity might be very close from what I am looking for.

[Leph]

I do confirm that the derivative of the RNEA algorithm with external forces allows to access to the Hessian-wrench product I was interested.

Computations with RBDL finite differences: 0.500ms With Pinocchio differentiated RNEA: 0.060ms

Thank you very much.

[jcarpent]

Computations with RBDL finite differences: 0.500ms With Pinocchio differentiated RNEA: 0.060ms

It seems your timings remain huge with Pinocchio. Can you provide more details?

[Leph]

The "Pinoccio" timing is taking into account the convertion from Pinocchio to RBDL format. The raw RNEADerivatives call is around 0.035ms on my laptop.

[jcarpent]

What is the robot? It still seems huge.

[Leph]

This is the humanoid Valkyrie model. No much different than Atlas. nv = 32.

[jcarpent]

You should be able to run them at 0.015 ms on standard laptop computers.

[markusgft]

@jcarpent, I don't want to open a new issue since it seems related to the above discussion... I would like to add my vote here for a standalone-interface to the hessian of the kinematics! This would be a very useful feature, ideally in isolation from the above mentioned Hessian-external-wrench product!

[jcarpent]

@markusgft Sure. Which operations do you need? Do you need access to the elements of the matrix?

[markusgft]

@jcarpent thanks for the quick reply! Yes, ideally I need access to the full derivative tensor dJ/dq (i.e. the derivative of a frame-jacobian w.r.t. the joint angles). Is this somehow doable?

[jmirabel]

As long as you have a function returning tensor * vector, you can easily build the hessian by calling the function with the canonical base of R^n.

I have no idea how slow this is compare to a function which would compute the hessian directly.

[markusgft]

yes, this is what I am currently using as intermediate solution... I simply call "pinocchio::computeJointJacobiansTimeVariation(model, data, jointConfig, jointVel);" N times using different unit joint velocities. This works, however it is not optimum speed ;)

[jcarpent]

Dear @markusgft, We have two options to expose the data structure for the Kinematic Hessian:

• either as en Eigen::Matrix<Data::Matrix6x,Eigen::Dynamic,1> (compatible with all Eigen versions)
• or as Eigen::Tensor which requires C++11

I would vote for the second option, which will be a long term solution I would say. What is your opinion?

[markusgft]

I would vote for the second option, which will be a long term solution I would say. What is your opinion?

I second that! Thanks a lot!

[oliwiermelon]

Dear @jcarpent , Sorry to be reopening this old issue but I have a related question.

Context: In my problem formulation, I am optimising for end-effector position p and velocity pdot, which I then express in the base frame (B). Then, I obtain the Jacobian matrix J(q) using getFrameJacobian(..., EE_FRAME, LOCAL, ...) which relates joint velocities qdot to pdot_B. For the optimisation, I am deriving the partial derivatives where I have to account for the changing Jacobian.

Question: Like Markus, I am looking for the kinematic Hessian (tensor) dJ(q)/dq (i.e., the derivative of an end-effector frame Jacobian with respect to the joint angles). If I was using the joint Jacobian (from getJointJacobian(), relating joint velocities to spatial velocities of a joint frame), I could use getJointKinematicHessian(); unfortunately, there does not appear to be an equivalent method getFrameKinematicHessian(). Can you please advise me if there is a way of deriving this quantity/obtaining it through some algorithm?

[josyulakrishna]

Dear @jcarpent , Sorry to be reopening this old issue but I have a related question.

Context: In my problem formulation, I am optimising for end-effector position p and velocity pdot, which I then express in the base frame (B). Then, I obtain the Jacobian matrix J(q) using getFrameJacobian(..., EE_FRAME, LOCAL, ...) which relates joint velocities qdot to pdot_B. For the optimisation, I am deriving the partial derivatives where I have to account for the changing Jacobian.

Question: Like Markus, I am looking for the kinematic Hessian (tensor) dJ(q)/dq (i.e., the derivative of an end-effector frame Jacobian with respect to the joint angles). If I was using the joint Jacobian (from getJointJacobian(), relating joint velocities to spatial velocities of a joint frame), I could use getJointKinematicHessian(); unfortunately, there does not appear to be an equivalent method getFrameKinematicHessian(). Can you please advise me if there is a way of deriving this quantity/obtaining it through some algorithm?

@oliwiermelon Hello, I'm having to compute the same quantity dJ(q)/dq, did you find a solution for this? Thank you