Implement Eigen-like expression templates for spatial algebra computations

CHANGELOG.md

@@ -6,6 +6,9 @@ The format is based on [Keep a Changelog](https://keepachangelog.com/en/1.0.0/).
 
 ## [Unreleased]
 
+### Added
+- Add Eigen expression support to evaluate spatial algebra ([#2551](https://github.com/stack-of-tasks/pinocchio/pull/2551))
+
 ### Fixed
 - Fix mjcf Euler angle parsing: use xyz as a default value for eulerseq compiler option ([#2526](https://github.com/stack-of-tasks/pinocchio/pull/2526))
 - Fix aba explicit template instantiation ([#2541](https://github.com/stack-of-tasks/pinocchio/pull/2541))

CMakeLists.txt

@@ -460,7 +460,9 @@ add_subdirectory(unittest)
 add_subdirectory(examples)
 
 # --- BENCHMARKS ---------------------------------------------------------------
-add_subdirectory(benchmark)
+if(BUILD_BENCHMARK)
+  add_subdirectory(benchmark)
+endif()
 
 # --- PACKAGING ----------------------------------------------------------------
 macro(EXPORT_VARIABLE var_name var_value)

benchmark/CMakeLists.txt

@@ -7,72 +7,65 @@
 # ----------------------------------------------------
 add_custom_target(bench)
 
-macro(ADD_BENCH bench_name)
-  if(BUILD_BENCHMARK)
-    add_executable(${bench_name} ${bench_name}.cpp)
-  else(BUILD_BENCHMARK)
-    add_executable(${bench_name} EXCLUDE_FROM_ALL ${bench_name}.cpp)
-  endif(BUILD_BENCHMARK)
-  set(ExtraMacroArgs ${ARGN})
-  list(LENGTH ExtraMacroArgs NumExtraMacroArgs)
-  if(NumExtraMacroArgs GREATER 0)
-    set(link_to_main_lib ${ARGV1})
-    if(link_to_main_lib)
-      set_target_properties(${bench_name} PROPERTIES COMPILE_DEFINITIONS
-                                                     PINOCCHIO_MODEL_DIR="${PINOCCHIO_MODEL_DIR}")
-      target_link_libraries(${bench_name} PUBLIC ${PROJECT_NAME})
-    endif(link_to_main_lib)
-  endif()
+add_project_private_dependency(benchmark REQUIRED)
 
-  add_dependencies(bench ${bench_name})
-endmacro(ADD_BENCH)
+macro(ADD_PINOCCHIO_BENCH bench_name)
+  set(options HEADER_ONLY GOOGLE_BENCHMARK)
+  set(oneValueArgs)
+  set(multiValueArgs PACKAGES)
+  cmake_parse_arguments(unit_test "${options}" "${oneValueArgs}" "${multiValueArgs}" ${ARGN})
 
-macro(ADD_TEST_CFLAGS target flag)
-  set_property(
-    TARGET ${target}
-    APPEND_STRING
-    PROPERTY COMPILE_FLAGS " ${flag}")
-endmacro(ADD_TEST_CFLAGS)
+  add_executable(${bench_name} ${bench_name}.cpp)
 
-if(CPPAD_FOUND)
-  include_directories(${cppad_INCLUDE_DIR})
-endif(CPPAD_FOUND)
+  target_compile_definitions(${bench_name} PRIVATE PINOCCHIO_MODEL_DIR="${PINOCCHIO_MODEL_DIR}")
+  if(NOT unit_test_HEADER_ONLY)
+    target_link_libraries(${bench_name} PRIVATE pinocchio_default)
+  else()
+    target_link_libraries(${bench_name} PRIVATE pinocchio_headers)
+  endif()
 
-if(cppadcg_FOUND)
-  include_directories(${cppadcg_INCLUDE_DIR})
-endif(cppadcg_FOUND)
+  if(unit_test_GOOGLE_BENCHMARK)
+    target_link_libraries(${bench_name} PRIVATE benchmark::benchmark)
+  endif()
+
+  add_dependencies(bench ${bench_name})
+endmacro()
 
 # timings
 #
-add_bench(timings TRUE)
-if(cppadcg_FOUND)
-  add_bench(timings-cg TRUE)
-  target_link_libraries(timings-cg PUBLIC ${CMAKE_DL_LIBS} ${cppad_LIBRARY})
-endif(cppadcg_FOUND)
+add_pinocchio_bench(timings)
+target_link_libraries(timings PRIVATE pinocchio_parsers)
+if(BUILD_WITH_CODEGEN_SUPPORT)
+  add_pinocchio_bench(timings-cg)
+  target_link_libraries(timings-cg PRIVATE pinocchio_parsers pinocchio_cppadcg ${CMAKE_DL_LIBS})
+endif()
 
 if(BUILD_WITH_OPENMP_SUPPORT)
-  add_bench(timings-parallel TRUE)
-endif(BUILD_WITH_OPENMP_SUPPORT)
+  add_pinocchio_bench(timings-parallel)
+  target_link_libraries(timings-parallel PRIVATE pinocchio_parsers pinocchio_parallel
+                                                 pinocchio_collision)
+endif()
 
-# timings
+# timings cholesky
 #
-add_bench(timings-cholesky TRUE)
+add_pinocchio_bench(timings-cholesky)
+target_link_libraries(timings-cholesky PRIVATE pinocchio_parsers)
 
 # timings derivatives
-
-add_bench(timings-derivatives TRUE)
-if(CPPAD_FOUND)
+#
+add_pinocchio_bench(timings-derivatives)
+target_link_libraries(timings-derivatives PRIVATE pinocchio_parsers)
+if(BUILD_WITH_AUTODIFF_SUPPORT)
   # timings-cppad-jit
-  add_bench(timings-cppad-jit TRUE)
-  target_link_libraries(timings-derivatives PUBLIC ${cppad_LIBRARY})
-  target_link_libraries(timings-cppad-jit PUBLIC ${cppad_LIBRARY} ${CMAKE_DL_LIBS})
+  add_pinocchio_bench(timings-cppad-jit)
+  target_link_libraries(timings-derivatives PRIVATE pinocchio_cppad)
+  target_link_libraries(timings-cppad-jit PRIVATE pinocchio_cppad ${CMAKE_DL_LIBS})
   target_compile_definitions(timings-cppad-jit
-                             PUBLIC PINOCCHIO_CXX_COMPILER=\"${CMAKE_CXX_COMPILER}\")
+                             PRIVATE PINOCCHIO_CXX_COMPILER=\"${CMAKE_CXX_COMPILER}\")
 endif()
 
 # timings-eigen
-#
-add_bench(timings-eigen)
+add_pinocchio_bench(timings-eigen)
 modernize_target_link_libraries(
   timings-eigen
   SCOPE PUBLIC
@@ -81,16 +74,23 @@ modernize_target_link_libraries(
 
 # timings-geometry
 #
-if(URDFDOM_FOUND AND hpp-fcl_FOUND)
-  add_bench(timings-geometry TRUE)
-endif(URDFDOM_FOUND AND hpp-fcl_FOUND)
+if(BUILD_WITH_URDF_SUPPORT AND BUILD_WITH_COLLISION_SUPPORT)
+  add_pinocchio_bench(timings-geometry)
+  target_link_libraries(timings-geometry PRIVATE pinocchio_parsers pinocchio_collision)
+endif()
 
 # timings-jacobian
 #
-add_bench(timings-jacobian TRUE)
+add_pinocchio_bench(timings-jacobian)
+target_link_libraries(timings-jacobian PRIVATE pinocchio_parsers)
 
 # timings-contact-dynamics
 #
-add_bench(timings-delassus-operations TRUE)
-add_bench(timings-contact-dynamics TRUE)
-add_bench(timings-constrained-dynamics-derivatives TRUE)
+add_pinocchio_bench(timings-delassus-operations)
+target_link_libraries(timings-delassus-operations PRIVATE pinocchio_parsers)
+add_pinocchio_bench(timings-contact-dynamics)
+target_link_libraries(timings-contact-dynamics PRIVATE pinocchio_parsers)
+add_pinocchio_bench(timings-constrained-dynamics-derivatives)
+target_link_libraries(timings-constrained-dynamics-derivatives PRIVATE pinocchio_parsers)
+
+add_pinocchio_bench(timings-spatial HEADER_ONLY GOOGLE_BENCHMARK)

benchmark/timings-spatial.cpp

@@ -0,0 +1,78 @@
+#include <pinocchio/spatial/se3.hpp>
+
+#include <benchmark/benchmark.h>
+
+using SE3 = pinocchio::SE3Tpl<double>;
+
+// Standard SE3Tpl operation
+struct SE3TplAct
+{
+  EIGEN_DONT_INLINE void operator()(SE3 & amc, const SE3 & amb, const SE3 & bmc)
+  {
+    amc = amb * bmc;
+  }
+};
+
+// Use of SE3Expr without noalias
+struct SE3ExprAct
+{
+  EIGEN_DONT_INLINE void operator()(SE3 & amc, const SE3 & amb, const SE3 & bmc)
+  {
+    amc.expr() = amb.const_expr() * bmc.const_expr();
+  }
+};
+
+// Use of SE3Expr with noalias
+struct SE3ExprNoaliasAct
+{
+  EIGEN_DONT_INLINE void operator()(SE3 & amc, const SE3 & amb, const SE3 & bmc)
+  {
+    amc.expr().noalias() = amb.const_expr() * bmc.const_expr();
+  }
+};
+
+// Apply SE3 manualy with noalias
+struct SE3ManualAct
+{
+  EIGEN_DONT_INLINE void operator()(SE3 & amc, const SE3 & amb, const SE3 & bmc)
+  {
+    amc.rotation().noalias() = amb.rotation() * bmc.rotation();
+    amc.translation().noalias() = amb.translation();
+    amc.translation().noalias() += amb.rotation() * bmc.translation();
+  }
+};
+
+template<class Fun>
+void bench_se3_act(benchmark::State & state)
+{
+  Fun f;
+  SE3 amc;
+  SE3 amb(SE3::Random());
+  SE3 bmc(SE3::Random());
+  for (auto _ : state)
+  {
+    for (int64_t i = 0; i < state.range(0); ++i)
+    {
+      f(amc, amb, bmc);
+      benchmark::DoNotOptimize(amc);
+    }
+  }
+}
+
+static void CustomArguments(benchmark::internal::Benchmark * b)
+{
+  b->ArgNames({"iter"})->MinWarmUpTime(3.);
+  for (int64_t i = 1; i < 4; ++i)
+  {
+    std::vector<int64_t> args;
+    args.push_back(static_cast<int64_t>(std::pow(10, i)));
+    b->Args(args);
+  }
+}
+
+BENCHMARK(bench_se3_act<SE3TplAct>)->Apply(CustomArguments);
+BENCHMARK(bench_se3_act<SE3ExprAct>)->Apply(CustomArguments);
+BENCHMARK(bench_se3_act<SE3ExprNoaliasAct>)->Apply(CustomArguments);
+BENCHMARK(bench_se3_act<SE3ManualAct>)->Apply(CustomArguments);
+
+BENCHMARK_MAIN();

development/scripts/pixi/activation.bat

@@ -19,3 +19,4 @@ if not defined PINOCCHIO_OPENMP_SUPPORT (set PINOCCHIO_OPENMP_SUPPORT=OFF)
 if not defined PINOCCHIO_CODEGEN_SUPPORT (set PINOCCHIO_CODEGEN_SUPPORT=OFF)
 if not defined PINOCCHIO_SDF_SUPPORT (set PINOCCHIO_SDF_SUPPORT=OFF)
 if not defined PINOCCHIO_MPFR_SUPPORT (set PINOCCHIO_MPFR_SUPPORT=OFF)
+if not defined PINOCCHIO_BUILD_BENCHMARK (set PINOCCHIO_BUILD_BENCHMARK=OFF)

development/scripts/pixi/activation.sh

@@ -43,3 +43,4 @@ export PINOCCHIO_OPENMP_SUPPORT=${PINOCCHIO_OPENMP_SUPPORT:=OFF}
 export PINOCCHIO_CODEGEN_SUPPORT=${PINOCCHIO_CODEGEN_SUPPORT:=OFF}
 export PINOCCHIO_SDF_SUPPORT=${PINOCCHIO_SDF_SUPPORT:=OFF}
 export PINOCCHIO_MPFR_SUPPORT=${PINOCCHIO_MPFR_SUPPORT:=OFF}
+export PINOCCHIO_BUILD_BENCHMARK=${PINOCCHIO_BUILD_BENCHMARK:=OFF}

include/pinocchio/multibody/joint/joint-helical.hpp

@@ -84,6 +84,8 @@ namespace pinocchio
     typedef const typename Vector3::ConstantReturnType ConstLinearRef;
     typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
     typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
+    typedef typename traits<PlainType>::ExprType ExprType;
+    typedef typename traits<PlainType>::ConstExprType ConstExprType;
   }; // traits TransformHelicalTpl
 
   template<typename Scalar, int Options, int axis>

include/pinocchio/multibody/joint/joint-prismatic.hpp

@@ -220,6 +220,8 @@ namespace pinocchio
     typedef const Vector3 ConstLinearRef;
     typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
     typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
+    typedef typename traits<PlainType>::ExprType ExprType;
+    typedef typename traits<PlainType>::ConstExprType ConstExprType;
   }; // traits TransformPrismaticTpl
 
   template<typename Scalar, int Options, int axis>

include/pinocchio/multibody/joint/joint-revolute.hpp

@@ -85,6 +85,8 @@ namespace pinocchio
     typedef const typename Vector3::ConstantReturnType ConstLinearRef;
     typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
     typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
+    typedef typename traits<PlainType>::ExprType ExprType;
+    typedef typename traits<PlainType>::ConstExprType ConstExprType;
   }; // traits TransformRevoluteTpl
 
   template<typename Scalar, int Options, int axis>

include/pinocchio/multibody/joint/joint-translation.hpp

@@ -216,6 +216,8 @@ namespace pinocchio
     typedef const LinearType & ConstLinearRef;
     typedef typename traits<PlainType>::ActionMatrixType ActionMatrixType;
     typedef typename traits<PlainType>::HomogeneousMatrixType HomogeneousMatrixType;
+    typedef typename traits<PlainType>::ExprType ExprType;
+    typedef typename traits<PlainType>::ConstExprType ConstExprType;
   }; // traits TransformTranslationTpl
 
   template<typename Scalar, int Options>

include/pinocchio/spatial/fwd.hpp

@@ -38,6 +38,21 @@ namespace pinocchio
   template<typename Scalar, int Options = context::Options>
   struct SE3Tpl;
 
+  template<typename Derived>
+  struct SE3ExprBase;
+  template<typename Derived>
+  struct SE3ExprNoalias;
+  template<typename RotProduct, typename TransProduct>
+  struct SE3ExprProduct;
+  template<typename RotProduct, typename TransProduct>
+  SE3ExprProduct<RotProduct, TransProduct>
+  make_se3_expr_product(RotProduct rot_prod, TransProduct trans_prod);
+
+  template<typename _Scalar, int _Options>
+  struct SE3TplExpr;
+  template<typename _Scalar, int _Options>
+  struct SE3TplConstExpr;
+
   template<typename Derived>
   class MotionBase;
   template<typename Derived>

include/pinocchio/spatial/se3-base.hpp

@@ -214,6 +214,16 @@ namespace pinocchio
       derived().normalized();
     }
 
+    ExprType expr()
+    {
+      return derived().expr_impl();
+    }
+
+    ConstExprType const_expr() const
+    {
+      return derived().const_expr_impl();
+    }
+
   }; // struct SE3Base
 
 } // namespace pinocchio