Integrating Second-Order derivatives of dynamics in Pinocchio

[shubhamsingh91]

Hi Pinocchio developers We recently published our work on the second-order derivatives of Inverse dynamics for bodies with multi-DoF joints and fixed/floating base in the IROS 2022 conference. Link to the paper is below.

For accuracy and run-time check we also implemented the derivatives in Pinocchio (my own fork) and Featherstone's spatialv2 library. Now with the increasing use of DDP for optimization in legged robots, this could be a useful addition to Pinocchio, since Pinocchio is at the heart of Crocoddyl and probably other libraries. @cmastalli also mentioned that it'd be a good addition and will accelerate the use of DDP in this field. On a side note, we also extended this to get Forward dynamics derivatives w.r.t configuration and velocity.

So I was wondering, how do I go about contributing this to Pinocchio? Do I simply create a unit-test like the ones present for first-order derivatives and then open a PR? or Is there a better way for this? @jcarpent Could you guide me here?

Thanks for all the development you guys do.

https://arxiv.org/abs/2203.01497

[jcarpent]

Very nice initiative @shubhamsingh91. You should follow the same strategy as in previous developments in Pinocchio:

• having detailed and extended unit testings of internal routines
• compatible with all the joints using the CRTP and visitors classically done in Pinocchio
• having self-content commit messages
• documentation of the additional functions
• no API break
• align with the git history
• methods and functions templated by the Scalar types

I hope I do not forget something. By the way, do you have documentation for the forward dynamics wrt configuration and velocity vectors?

Justin

[shubhamsingh91]

Thanks for the quick response @jcarpent . Forward Dynamics derivatives are still in works. We are finalizing the algorithm and running some tests. I will put something on arxiv at the time of the PR to provide a background for the algorithm. For now I will work on the Inverse Dynamics derivatives and create a PR for that. Shubham

[shubhamsingh91]

@jcarpent Opened the PR. Feel free to ask any questions regarding the code. Thanks

[jcarpent]

Solved via #1860 and #1840.