Implement Eigen-like expression templates for spatial algebra computations

[jorisv]

This PR aim to use Eigen expression to compute spatial algebra.

Our SE3 implementation (same for Motion, Force and Inertia) prevent to use the Eigen noalias fonction.
This prevent the compiler to optimize the code well.
Some benchmark had shown that we can accelerate the forward kinematics computation time by 1.5x by using this trick.

TODO V1

• Implement SE3ExprBase
• Implement SE3TplExpr
  • act
  • actInv
• Add unit test
• Create spatial benchmark
• Use SE3Expr in kinematics (0 order)
• Benchmark old vs new kinematics

TODO V2

• Find a way to mix SE3, Motion, Force and Inertia expression
• Manage Expression on Matrix to replace act-on-set.
• Use expression in Motion
• Use expression in Force
• Use expression in Inertia
• Use expression in kinematics
• Use expression in aba/crba/rnea
• Extend expression for derivatives algo

Benchmark results

Setup:

• Ubuntu 22.04
• governor set in performance mode
• no_turbo
• CPU: i9-13950HX

SE3

Release + march=native

-----------------------------------------------------------------------------------------------------------
Benchmark                                                                 Time             CPU   Iterations
-----------------------------------------------------------------------------------------------------------
bench_se3_act<SE3TplAct>/iter:10/min_warmup_time:3.000                  119 ns          119 ns      5875890
bench_se3_act<SE3TplAct>/iter:100/min_warmup_time:3.000                1190 ns         1190 ns       588352
bench_se3_act<SE3TplAct>/iter:1000/min_warmup_time:3.000              11887 ns        11887 ns        58888
bench_se3_act<SE3ExprAct>/iter:10/min_warmup_time:3.000                 128 ns          128 ns      5449576
bench_se3_act<SE3ExprAct>/iter:100/min_warmup_time:3.000               1283 ns         1283 ns       545477
bench_se3_act<SE3ExprAct>/iter:1000/min_warmup_time:3.000             12801 ns        12799 ns        54686
bench_se3_act<SE3ExprNoaliasAct>/iter:10/min_warmup_time:3.000         98.6 ns         98.6 ns      7090541
bench_se3_act<SE3ExprNoaliasAct>/iter:100/min_warmup_time:3.000         922 ns          922 ns       759320
bench_se3_act<SE3ExprNoaliasAct>/iter:1000/min_warmup_time:3.000       9014 ns         9014 ns        77645
bench_se3_act<SE3ManualAct>/iter:10/min_warmup_time:3.000              98.7 ns         98.7 ns      7079105
bench_se3_act<SE3ManualAct>/iter:100/min_warmup_time:3.000              922 ns          922 ns       758837
bench_se3_act<SE3ManualAct>/iter:1000/min_warmup_time:3.000            9017 ns         9017 ns        77600

Release

-----------------------------------------------------------------------------------------------------------
Benchmark                                                                 Time             CPU   Iterations
-----------------------------------------------------------------------------------------------------------
bench_se3_act<SE3TplAct>/iter:10/min_warmup_time:3.000                  157 ns          157 ns      4455383
bench_se3_act<SE3TplAct>/iter:100/min_warmup_time:3.000                1570 ns         1569 ns       446245
bench_se3_act<SE3TplAct>/iter:1000/min_warmup_time:3.000              13433 ns        13427 ns        44669
bench_se3_act<SE3ExprAct>/iter:10/min_warmup_time:3.000                 252 ns          252 ns      2865782
bench_se3_act<SE3ExprAct>/iter:100/min_warmup_time:3.000               2523 ns         2520 ns       277917
bench_se3_act<SE3ExprAct>/iter:1000/min_warmup_time:3.000             25173 ns        25168 ns        27810
bench_se3_act<SE3ExprNoaliasAct>/iter:10/min_warmup_time:3.000          150 ns          150 ns      4665029
bench_se3_act<SE3ExprNoaliasAct>/iter:100/min_warmup_time:3.000        1447 ns         1446 ns       484347
bench_se3_act<SE3ExprNoaliasAct>/iter:1000/min_warmup_time:3.000      14329 ns        14325 ns        48740
bench_se3_act<SE3ManualAct>/iter:10/min_warmup_time:3.000               150 ns          150 ns      4665644
bench_se3_act<SE3ManualAct>/iter:100/min_warmup_time:3.000             1446 ns         1444 ns       484621
bench_se3_act<SE3ManualAct>/iter:1000/min_warmup_time:3.000           14326 ns        14306 ns        48914

[ManifoldFR]

It's not really an Eigen expression, more "Eigen-like expression template".

[ManifoldFR]

I allowed myself to re-title the PR @jorisv.

Excellent initiative btw !

[ManifoldFR]

For the spatial benchmark, are we going to write it like we usually did for Pinocchio, or start switching to the more standard google bench ?

I vote for the latter option.

[jorisv]

@ManifoldFR We will write it using google benchmark. We plan to port all benchmark to this library in the future.

[ManifoldFR]

@ManifoldFR We will write it using google benchmark. We plan to port all benchmark to this library in the future.

Excellent, I feel it will make our bench code much more professional. Is the port of all the benchmarks somewhere on the roadmap/issue tracker? We could work on that on the side.

[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.

[jcarpent]

I would suggest using Expression in full name for clarity and readibility.

[jcarpent]

Could you split benchmark refactorization and expression implementation to ease the review process?

[jcarpent]

I would suggest a function make_product generic, that calls then a template specialization for readability.