Topic/aba second order derivatives

[shubhamsingh91]

Thanks for taking the time to make and fill out this pull request!

Description

This PR (discussed in #2691) adds the second-order derivatives of the unconstrained Articulated Body Algorithm as derived in this paper (https://ieeexplore.ieee.org/abstract/document/10449483).
The specific algorithm is from Eq~46 from the paper, and more details can be found in Fig.13. Note that this algorithm uses the rnea-second-order-derivatives, which were contributed previously as a part of #1860.

Paper can be accessed directly on researchGate.

Output of timings-derivatives:

  Benchmark                                                        Time             CPU   Iterations
  --------------------------------------------------------------------------------------------------
  DerivativesFixture/FORWARD_KINEMATICS_Q_V_A                   3688 ns         3687 ns       189116
  DerivativesFixture/FORWARD_KINEMATICS_DERIVATIVES             4792 ns         4792 ns       152272
  DerivativesFixture/RNEA                                       5254 ns         5250 ns       132336
  DerivativesFixture/COMPUTE_RNEA_DERIVATIVES                  22267 ns        22267 ns        31468
  DerivativesFixture/COMUTE_RNEA_SECOND_ORDER_DERIVATIVES     164514 ns       164474 ns         4248
  DerivativesFixture/COMPUTE_ABA_SECOND_ORDER_DERIVATIVES    3486243 ns      3486084 ns          203
  DerivativesFixture/COMPUTE_RNEA_FD_DERIVATIVES              406536 ns       406510 ns         1724
  DerivativesFixture/ABA_LOCAL                                 13172 ns        13166 ns        51339
  DerivativesFixture/COMPUTE_ABA_DERIVATIVES                   49715 ns        49713 ns        14036
  DerivativesFixture/COMPUTE_ABA_DERIVATIVES_NO_Q_V_TAU        41965 ns        41963 ns        16760
  DerivativesFixture/COMPUTE_ABA_FD_DERIVATIVES               946073 ns       946021 ns          741
  DerivativesFixture/COMPUTE_M_INVERSE_Q                       10507 ns        10500 ns        66520
  DerivativesFixture/COMPUTE_M_INVERSE                          5036 ns         5036 ns       139458
  DerivativesFixture/COMPUTE_CHOLESKY_M_INVERSE                17365 ns        17364 ns        40261

Checklist

• I have run pre-commit run --all-files or pixi run lint
• I have performed a self-review of my own code
• I have commented my code where necessary
• I have made corresponding changes to the doxygen documentation
• I have added tests that prove my fix or feature works
• I have updated the CHANGELOG or added the "no changelog" label if it's a CI-related issue
• I have updated the README credits section

[github-actions]

👋 Hi,
This is a reminder message to assign an extra build label to this Pull Request if needed.
By default, this PR will be build with minimal build options (URDF support and Python bindings)
The possible extra labels are:

• build_collision (build Pinocchio with coal support)
• build_casadi (build Pinocchio with CasADi support)
• build_autodiff (build Pinocchio with CppAD support)
• build_codegen (build Pinocchio with CppADCodeGen support)
• build_extra (build Pinocchio with extra algorithms)
• build_mpfr (build Pinocchio with Boost.Multiprecision support)
• build_sdf (build Pinocchio with SDF parser)
• build_accelerate (build Pinocchio with APPLE Accelerate framework support)
• build_all (build Pinocchio with ALL the options stated above)

Thanks.
The Pinocchio development team.

[jorisv]

Hello @shubhamsingh91,

Thanks for the contribution. Can you add in your PR message a description of this contribution content ?
Also, did you use any AI tools to write some part of this PR ?

[jcarpent]

Thanks a lot @shubhamsingh91 for this very nice and helpful contribution. Will be highly useful for the entire community.

[shubhamsingh91]

@jorisv Yes! i ported over the core algorithm from my own implementation in a separate Pinocchio fork, a while back. I used Claude code to write some parts and formatting.
Could you apply the build_all label since this is a new templated algorithm?

[shubhamsingh91]

@AjSat @jorisv @jcarpent , I am ready for the review!