Compute partial derivatives of jacobian columns w.r.t. to joint positions AKA Hessians

[traversaro]

@S-Dafarra asked about this. This derivative have the nice properties that everything boils down to a lot of 6D cross product between all the joint motion subspace vectors.

References on this:

• Equation 31 of https://www.researchgate.net/profile/Gianluca_Garofalo2/publication/259899572_On_the_closed_form_computation_of_the_dynamic_matrices_and_their_differentiations/links/59707389a6fdccc6c973a8e1/On-the-closed-form-computation-of-the-dynamic-matrices-and-their-differentiations.pdf
• Equation 13 of https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=240619

Also check the iKin implementation of Hessians: https://github.com/robotology/icub-main/blob/master/src/libraries/iKin/src/iKinFwd.cpp#L1124 .

[S-Dafarra]

What about the floating base part? Should we consider a 6dof joint there? Also, is there any possibility to use these computations also for the matrix mapping base velocity and joints velocity to the robot momentum?

[S-Dafarra]

Just to provide another source, Equation 13 of https://ieeexplore.ieee.org/stamp/stamp.jsp?tp=&arnumber=240619 corresponds to Equation 37 of https://ieeexplore.ieee.org/stamp/stamp.jsp?arnumber=1554272

[traversaro]

It depends what you need to compute for the floating base part. Depending on the choice of the frame velocity representation, the joint part of the jacobian may or may not depend on the floating base position/orientation, while for the derivatives of the adjoint transform (i.e. that may be the base part of the jacobian, again depending on the velocity representation) the procedure is a bit more complicated. If you write down formally the quantity you want to derive, I think it will be easier to explain. For the robot momentum it is a bit more complicated because you have to account for all the composite rigid body inertia present in the formula of the momentum jacobian.

[traversaro]

FYI @mpbos This is a bit hard to understand know, but it is definitely related to your master thesis work.

[traversaro]

Related code in Pinocchio:

• https://github.com/stack-of-tasks/pinocchio/blob/v1.2.9/bindings/python/scripts/derivative/dcrba.py
• https://github.com/stack-of-tasks/pinocchio/blob/devel/src/algorithm/kinematics-derivatives.hxx

Related code in DART (warning: time derivative):

• https://github.com/dartsim/dart/blob/a206b1995a4c4ee96de691351cb109b808d0a347/dart/dynamics/BodyNode.cpp#L2398

Related code in KDL (warning: time derivative):

• https://github.com/orocos/orocos_kinematics_dynamics/blob/master/orocos_kdl/src/chainjnttojacdotsolver.cpp#L117 (related to https://github.com/orocos/orocos_kinematics_dynamics/blob/master/orocos_kdl/src/chainjnttojacdotsolver.hpp#L37 )

[S-Dafarra]

Aren't those related to time derivative? :thinking:

[traversaro]

Yep, but if you expand the use of any velocity with $\mathrm{v} = J \nu$, you almost obtain the partial derivatives w.r.t. to the joint positions.

[jcarpent]

Dear iDynTree developers,

I have implemented such kind of derivatives here: https://github.com/stack-of-tasks/pinocchio/blob/devel/src/algorithm/kinematics-derivatives.hxx

I never explicitly compute the tensor H of dimension 6 x nv x nv (where nv is the dimension of the tangent vector - aka joint velocity) but rather H*v (v being the joint velocity). If you have any question on the code, I will be pleased to help you.

Best,

Justin

[traversaro]

Thanks @jcarpent for the useful insight and references.

[S-Dafarra]

Thanks @jcarpent! I will definitely go though that. I updated the above comment to have the references all together.

By any chance, do you have any document about the mathematical derivation you implemented?

[jcarpent]

A bit is explained here: https://hal.laas.fr/hal-01790971 When deriving RNEA derivatives, you can get for free the derivatives of the kinematics too. I will try to make these things clearer in a companion document that I need to write.

If you have any questions, I will be pleased to answer them.

[S-Dafarra]

Thanks again!

[DanielePucci]

I think that sooner or later we have to implement the Hessian. Let's discuss this in view of the current lab activities, and understand a possible timeline

CC @S-Dafarra

[traversaro]

Related issue in pinocchio: https://github.com/stack-of-tasks/pinocchio/issues/867 .