Merge pull request #1420 from jcarpent/topic/collision

Enhance collision support + serialization + parallelization

.github/workflows/linux.yml

@@ -47,6 +47,7 @@ jobs:
         echo $APT_DEPENDENCIES
         sudo apt-get update -qq
         sudo apt-get install -qq curl cppcheck ${APT_DEPENDENCIES}
+        sudo apt install libomp-dev libomp5
     - name: Free disk space
       run: |
         sudo apt clean
@@ -72,7 +73,7 @@ jobs:
         export MAKEFLAGS="-j1"
         mkdir build
         cd build
-        cmake .. -DCMAKE_BUILD_TYPE=Debug -DBUILD_WITH_COLLISION_SUPPORT=ON -DBUILD_ADVANCED_TESTING=ON -DBUILD_WITH_CASADI_SUPPORT=ON -DPYTHON_EXECUTABLE=$(which python3)
+        cmake .. -DCMAKE_BUILD_TYPE=Debug -DBUILD_WITH_COLLISION_SUPPORT=ON -DBUILD_ADVANCED_TESTING=ON -DBUILD_WITH_CASADI_SUPPORT=ON -DPYTHON_EXECUTABLE=$(which python3) -DBUILD_WITH_OPENMP_SUPPORT=ON
         make
         make build_tests
         export CTEST_OUTPUT_ON_FAILURE=1

.github/workflows/windows-conda-clang.yml

@@ -2,9 +2,6 @@ name: Build Pinocchio for Windows (CLANG) via Conda
 on:
   pull_request:
   push:
-    branches:
-      - master
-      - devel
 
 jobs:
   build:

.github/workflows/windows-conda-v142.yml

@@ -2,9 +2,6 @@ name: Build Pinocchio for Windows (v142) via Conda
 on:
   pull_request:
   push:
-    branches:
-      - master
-      - devel
 
 jobs:
   build:
@@ -33,6 +30,10 @@ jobs:
     - name: Install cmake and update conda
       run: |
         conda install cmake -c main
+
+    - name: Display the path
+      run: echo %cd%
+      shell: cmd
     
     - name: Build Pinocchio
       shell: cmd /C CALL {0}
@@ -43,7 +44,6 @@ jobs:
         set Boost_ROOT=
         set BOOST_ROOT_1_69_0=
         set BOOST_ROOT_1_72_0=
-        set PATH=%PATH:C:\hostedtoolcache\windows\Boost\1.72.0;=%
 
         call "%programfiles(x86)%\Microsoft Visual Studio\2019\Enterprise\VC\Auxiliary\Build\vcvarsall.bat" amd64
     
@@ -61,14 +61,31 @@ jobs:
           -DBUILD_WITH_URDF_SUPPORT=ON ^
           -DBUILD_PYTHON_INTERFACE=OFF ^
           -DBUILD_WITH_COLLISION_SUPPORT=ON ^
+          -DBUILD_TESTING=ON ^
           ..
 
         :: Build
         cmake --build . --config Release --target install
 
+#    - name: Start SSH session
+#      uses: luchihoratiu/debug-via-ssh@main
+#      with:
+#        NGROK_AUTH_TOKEN: ${{ secrets.NGROK_AUTH_TOKEN }}
+#        SSH_PASS: ${{ secrets.SSH_PASS }}
+#
+    - name: Testing
+      shell: cmd /C CALL {0}
+      env:
+        ACTIONS_ALLOW_UNSECURE_COMMANDS: 'true'
+      run: |
         :: Testing
+        pushd build
         set PATH=%PATH%;%CONDA_PREFIX%\Lib\site-packages\pinocchio
-        ctest --output-on-failure -C Release -V
+        set PATH=%PATH%;%CONDA_PREFIX%\Library\Lib
+        set PATH=%PATH%;%CONDA_PREFIX%\Library\bin
+        .\unittest\Release\test-cpp-urdf.exe
+        .\examples\Release\example-cpp-geometry-models.exe
+        ctest --output-on-failure -C Release -V --repeat until-pass:2
 
 #        :: Test Python import
 #        cd ..

.gitmodules

@@ -5,5 +5,5 @@
 	path = .travis
 	url = https://github.com/jrl-umi3218/jrl-travis
 [submodule "models/others"]
-	path = models/others
+	path = models/example-robot-data
 	url = https://github.com/Gepetto/example-robot-data.git

CMakeLists.txt

 #
-# Copyright (c) 2015-2020 CNRS INRIA
+# Copyright (c) 2015-2021 CNRS INRIA
 # Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
 #
 
@@ -32,6 +32,10 @@ INCLUDE(${CMAKE_CURRENT_LIST_DIR}/cmake/boost.cmake)
 INCLUDE(${CMAKE_CURRENT_LIST_DIR}/cmake/python.cmake)
 INCLUDE(${CMAKE_CURRENT_LIST_DIR}/cmake/ide.cmake)
 INCLUDE(${CMAKE_CURRENT_LIST_DIR}/cmake/apple.cmake)
+IF(APPLE) # Use the handmade approach
+  SET(CMAKE_MODULE_PATH ${CMAKE_CURRENT_LIST_DIR}/cmake/find-external/OpenMP ${CMAKE_MODULE_PATH})
+ENDIF(APPLE)
+INCLUDE(CMakeDependentOption)
 
 # If needed, set CMake policy for APPLE systems
 APPLY_DEFAULT_APPLE_CONFIGURATION()
@@ -63,6 +67,8 @@ OPTION(BUILD_WITH_COLLISION_SUPPORT "Build the library with the collision suppor
 OPTION(BUILD_WITH_AUTODIFF_SUPPORT "Build the library with the automatic differentiation support (via CppAD)" OFF)
 OPTION(BUILD_WITH_CASADI_SUPPORT "Build the library with the support of CASADI" OFF)
 OPTION(BUILD_WITH_CODEGEN_SUPPORT "Build the library with the support of code generation (via CppADCodeGen)" OFF)
+OPTION(BUILD_WITH_OPENMP_SUPPORT "Build the library with the OpenMP support" OFF)
+cmake_dependent_option(LINK_PYTHON_INTERFACE_TO_OPENM "Link OpenMP to the Python interface" ON BUILD_WITH_OPENMP_SUPPORT OFF)
 
 OPTION(INITIALIZE_WITH_NAN "Initialize Eigen entries with NaN" OFF)
 
@@ -123,6 +129,10 @@ IF(BUILD_WITH_CASADI_SUPPORT)
   ADD_PROJECT_DEPENDENCY(casadi 3.4.5 REQUIRED PKG_CONFIG_REQUIRES "casadi >= 3.4.5")
 ENDIF(BUILD_WITH_CASADI_SUPPORT)
 
+IF(BUILD_WITH_OPENMP_SUPPORT)
+  FIND_PACKAGE(OpenMP REQUIRED)
+ENDIF(BUILD_WITH_OPENMP_SUPPORT)
+
 SET(BOOST_REQUIRED_COMPONENTS filesystem serialization system)
 
 SET_BOOST_DEFAULT_OPTIONS()
@@ -133,7 +143,7 @@ IF(BUILD_PYTHON_INTERFACE)
   MESSAGE(STATUS "The Python bindings of Pinocchio will be compiled along the main library. If you want to disable this feature, please set the option BUILD_PYTHON_INTERFACE to OFF.")
   FINDPYTHON(REQUIRED)
   SEARCH_FOR_BOOST_PYTHON(REQUIRED)
-  ADD_PROJECT_DEPENDENCY(eigenpy 2.5.0 REQUIRED)
+  ADD_PROJECT_DEPENDENCY(eigenpy 2.6.2 REQUIRED)
 ELSE(BUILD_PYTHON_INTERFACE)
   MESSAGE(STATUS "Pinocchio won't be compiled with its Python bindings. If you want to enable this feature, please set the option BUILD_PYTHON_INTERFACE to ON.")
 ENDIF(BUILD_PYTHON_INTERFACE)
@@ -141,7 +151,7 @@ ENDIF(BUILD_PYTHON_INTERFACE)
 IF(BUILD_WITH_HPP_FCL_SUPPORT)
   ADD_DEFINITIONS(-DPINOCCHIO_WITH_HPP_FCL)
   LIST(APPEND CFLAGS_DEPENDENCIES "-DPINOCCHIO_WITH_HPP_FCL")
-  ADD_PROJECT_DEPENDENCY(hpp-fcl 1.4.0 REQUIRED PKG_CONFIG_REQUIRES "hpp-fcl >= 1.4.0")
+  ADD_PROJECT_DEPENDENCY(hpp-fcl 1.7.1 REQUIRED PKG_CONFIG_REQUIRES "hpp-fcl >= 1.7.1")
   # Check whether hpp-fcl python bindings are available.
   SET(BUILD_WITH_HPP_FCL_PYTHON_BINDINGS FALSE)
   IF(BUILD_PYTHON_INTERFACE)
@@ -203,12 +213,14 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/spatial")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/joint")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/liegroup")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/pool")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/multibody/visitor")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/parsers/urdf")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/utils")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/serialization")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/algorithm")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/algorithm/parallel")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/container")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/codegen")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/autodiff")

benchmark/CMakeLists.txt

 #
-# Copyright (c) 2015-2020 CNRS INRIA
+# Copyright (c) 2015-2021 CNRS INRIA
 #
 
 # ----------------------------------------------------
@@ -51,6 +51,11 @@ IF(CPPADCG_FOUND)
   TARGET_LINK_LIBRARIES(timings-cg PUBLIC ${CMAKE_DL_LIBS})
 ENDIF(CPPADCG_FOUND)
 
+IF(BUILD_WITH_OPENMP_SUPPORT)
+  ADD_BENCH(timings-parallel TRUE)
+  TARGET_LINK_LIBRARIES(timings-parallel PRIVATE OpenMP::OpenMP_CXX)
+ENDIF(BUILD_WITH_OPENMP_SUPPORT)
+
 # timings
 # 
 ADD_BENCH(timings-cholesky TRUE)

benchmark/timings-parallel.cpp

+//
+// Copyright (c) 2021 INRIA
+//
+
+#include "pinocchio/algorithm/parallel/rnea.hpp"
+#include "pinocchio/algorithm/parallel/aba.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
+
+#ifdef PINOCCHIO_WITH_HPP_FCL
+  #include "pinocchio/algorithm/parallel/geometry.hpp"
+#endif
+
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/parsers/srdf.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
+
+#include <iostream>
+
+#include "pinocchio/utils/timer.hpp"
+
+#include <Eigen/StdVector>
+EIGEN_DEFINE_STL_VECTOR_SPECIALIZATION(Eigen::VectorXd)
+
+int main(int /*argc*/, const char ** /*argv*/)
+{
+  using namespace Eigen;
+  using namespace pinocchio;
+  
+  typedef Eigen::Matrix<bool,Eigen::Dynamic,1> VectorXb;
+//  typedef Eigen::Matrix<bool,Eigen::Dynamic,Eigen::Dynamic> MatrixXb;
+
+  PinocchioTicToc timer(PinocchioTicToc::US);
+  #ifdef NDEBUG
+  const int NBT = 4000;
+ 
+  #else
+    const int NBT = 1;
+    std::cout << "(the time score in debug mode is not relevant) " << std::endl;
+  #endif
+  
+  const int BATCH_SIZE = 256;
+  const int NUM_THREADS = omp_get_max_threads();
+    
+  const std::string filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/talos_data/robots/talos_reduced.urdf");
+  
+  pinocchio::Model model;
+  pinocchio::urdf::buildModel(filename,JointModelFreeFlyer(),model);
+  
+  std::cout << "nq = " << model.nq << std::endl;
+  std::cout << "nv = " << model.nv << std::endl;
+  std::cout << "name = " << model.name << std::endl;
+  
+#ifdef PINOCCHIO_WITH_HPP_FCL
+  const std::string package_path = PINOCCHIO_MODEL_DIR;
+  hpp::fcl::MeshLoaderPtr mesh_loader = boost::make_shared<hpp::fcl::CachedMeshLoader>();
+  const std::string srdf_filename = PINOCCHIO_MODEL_DIR + std::string("/example-robot-data/robots/talos_data/srdf/talos.srdf");
+  std::vector<std::string> package_paths(1,package_path);
+  pinocchio::GeometryModel geometry_model;
+  pinocchio::urdf::buildGeom(model,filename,COLLISION,geometry_model,package_paths,mesh_loader);
+  
+  geometry_model.addAllCollisionPairs();
+  pinocchio::srdf::removeCollisionPairs(model,geometry_model,srdf_filename,false);
+  
+  GeometryData geometry_data(geometry_model);
+  {
+    int num_active_collision_pairs = 0;
+    for(size_t k = 0; k < geometry_data.activeCollisionPairs.size(); ++k)
+    {
+      if(geometry_data.activeCollisionPairs[k])
+        num_active_collision_pairs++;
+    }
+    std::cout << "active collision pairs = " << num_active_collision_pairs << std::endl;
+  }
+#endif
+  
+  std::cout << "--" << std::endl;
+  std::cout << "NUM_THREADS: " << NUM_THREADS << std::endl;
+  std::cout << "BATCH_SIZE: " << BATCH_SIZE << std::endl;
+  std::cout << "--" << std::endl;
+  
+  pinocchio::Data data(model);
+  const VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
+
+  MatrixXd qs(model.nq,BATCH_SIZE);
+  MatrixXd vs(model.nv,BATCH_SIZE);
+  MatrixXd as(model.nv,BATCH_SIZE);
+  MatrixXd taus(model.nv,BATCH_SIZE);
+  MatrixXd res(model.nv,BATCH_SIZE);
+  
+//  VectorXb cols()
+  
+  PINOCCHIO_ALIGNED_STD_VECTOR(VectorXd) q_vec(NBT);
+  for(size_t i=0; i < NBT; ++i)
+  {
+    q_vec[i] = randomConfiguration(model,-qmax,qmax);
+  }
+  
+  for(Eigen::DenseIndex i=0; i < BATCH_SIZE; ++i)
+  {
+    qs.col(i) = randomConfiguration(model,-qmax,qmax);
+    vs.col(i) = Eigen::VectorXd::Random(model.nv);
+    as.col(i) = Eigen::VectorXd::Random(model.nv);
+    taus.col(i) = Eigen::VectorXd::Random(model.nv);
+  }
+  
+  ModelPool pool(model,NUM_THREADS);
+  
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    for(Eigen::DenseIndex i = 0; i < BATCH_SIZE; ++i)
+      res.col(i) = rnea(model,data,qs.col(i),vs.col(i),as.col(i));
+  }
+  std::cout << "mean RNEA = \t\t\t\t"; timer.toc(std::cout,NBT*BATCH_SIZE);
+
+  for(int num_threads = 1; num_threads <= NUM_THREADS; ++num_threads)
+  {
+    timer.tic();
+    SMOOTH(NBT)
+    {
+      rnea(num_threads,pool,qs,vs,as,res);
+    }
+    double elapsed = timer.toc()/(NBT*BATCH_SIZE);
+    std::stringstream ss;
+    ss << "mean RNEA pool (";
+    ss << num_threads;
+    ss << " threads) = \t\t";
+    ss << elapsed << " us" << std::endl;
+    std::cout << ss.str();
+  }
+  
+  std::cout << "--" << std::endl;
+  
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    for(Eigen::DenseIndex i = 0; i < BATCH_SIZE; ++i)
+      res.col(i) = aba(model,data,qs.col(i),vs.col(i),taus.col(i));
+  }
+  std::cout << "mean ABA = \t\t\t\t"; timer.toc(std::cout,NBT*BATCH_SIZE);
+
+  for(int num_threads = 1; num_threads <= NUM_THREADS; ++num_threads)
+  {
+    timer.tic();
+    SMOOTH(NBT)
+    {
+      aba(num_threads,pool,qs,vs,taus,res);
+    }
+    double elapsed = timer.toc()/(NBT*BATCH_SIZE);
+    std::stringstream ss;
+    ss << "mean ABA pool (";
+    ss << num_threads;
+    ss << " threads) = \t\t";
+    ss << elapsed << " us" << std::endl;
+    std::cout << ss.str();
+  }
+  
+#ifdef PINOCCHIO_WITH_HPP_FCL
+  std::cout << "--" << std::endl;
+  const int NBT_COLLISION = math::max(NBT,1);
+  timer.tic();
+  SMOOTH((size_t)NBT_COLLISION)
+  {
+    computeCollisions(model,data,geometry_model,geometry_data,q_vec[_smooth]);
+  }
+  std::cout << "non parallel collision = \t\t\t"; timer.toc(std::cout,NBT_COLLISION);
+
+  for(int num_threads = 1; num_threads <= NUM_THREADS; ++num_threads)
+  {
+    timer.tic();
+    SMOOTH((size_t)NBT_COLLISION)
+    {
+      computeCollisions(num_threads,model,data,geometry_model,geometry_data,q_vec[_smooth]);
+    }
+    double elapsed = timer.toc()/(NBT_COLLISION);
+    std::stringstream ss;
+    ss << "parallel collision (";
+    ss << num_threads;
+    ss << " threads) = \t\t";
+    ss << elapsed << " us" << std::endl;
+    std::cout << ss.str();
+  }
+  
+  std::cout << "--" << std::endl;
+  GeometryPool geometry_pool(model,geometry_model,NUM_THREADS);
+  VectorXb collision_res(BATCH_SIZE);
+  collision_res.fill(false);
+  
+  timer.tic();
+  SMOOTH((size_t)(NBT_COLLISION/BATCH_SIZE))
+  {
+    for(Eigen::DenseIndex i = 0; i < BATCH_SIZE; ++i)
+      computeCollisions(model,data,geometry_model,geometry_data,qs.col(i));
+  }
+  std::cout << "non parallel collision = \t\t\t"; timer.toc(std::cout,NBT_COLLISION);
+
+  for(int num_threads = 1; num_threads <= NUM_THREADS; ++num_threads)
+  {
+    timer.tic();
+    SMOOTH((size_t)(NBT_COLLISION/BATCH_SIZE))
+    {
+      computeCollisions(num_threads,geometry_pool,qs,collision_res);
+    }
+    double elapsed = timer.toc()/(NBT_COLLISION);
+    std::stringstream ss;
+    ss << "pool parallel collision (";
+    ss << num_threads;
+    ss << " threads) = \t\t";
+    ss << elapsed << " us" << std::endl;
+    std::cout << ss.str();
+  }
+#endif
+
+  std::cout << "--" << std::endl;
+  return 0;
+}

bindings/python/CMakeLists.txt

 # 
-# Copyright (c) 2015-2020 CNRS INRIA
+# Copyright (c) 2015-2021 CNRS INRIA
 # Copyright (c) 2015 Wandercraft, 86 rue de Paris 91400 Orsay, France.
 #
 
@@ -27,6 +27,15 @@ FUNCTION(REMOVE_PATH_FROM_LIST list_name path_name dest_list)
   SET(${dest_list} ${list_name_} PARENT_SCOPE)
 ENDFUNCTION(REMOVE_PATH_FROM_LIST)
 
+FUNCTION(LIST_FILTER list regular_expression dest_list)
+  FOREACH(elt ${list})  
+    IF(${elt} MATCHES ${regular_expression})
+      LIST(REMOVE_ITEM list ${elt})
+    ENDIF()
+  ENDFOREACH(elt ${list})
+  SET(${dest_list} ${list} PARENT_SCOPE)
+ENDFUNCTION(LIST_FILTER)
+
 # --- PYTHON TARGET --- #
 SET(PYWRAP ${PROJECT_NAME}_pywrap)
 SET(PYWRAP ${PYWRAP} PARENT_SCOPE)
@@ -64,6 +73,24 @@ IF(NOT BUILD_WITH_HPP_FCL_PYTHON_BINDINGS)
     )
 ENDIF(NOT BUILD_WITH_HPP_FCL_PYTHON_BINDINGS)
 
+IF(NOT BUILD_WITH_OPENMP_SUPPORT)
+  LIST_FILTER("${${PROJECT_NAME}_PYTHON_HEADERS}" "^multibody/pool" ${PROJECT_NAME}_PYTHON_HEADERS)
+  LIST_FILTER("${${PROJECT_NAME}_PYTHON_SOURCES}" "^multibody/pool" ${PROJECT_NAME}_PYTHON_SOURCES)
+  LIST_FILTER("${${PROJECT_NAME}_PYTHON_HEADERS}" "^algorithm/parallel" ${PROJECT_NAME}_PYTHON_HEADERS)
+  LIST_FILTER("${${PROJECT_NAME}_PYTHON_SOURCES}" "^algorithm/parallel" ${PROJECT_NAME}_PYTHON_SOURCES)
+  #LIST(FILTER ${PROJECT_NAME}_PYTHON_HEADERS EXCLUDE REGEX "^multibody/pool")
+  #LIST(FILTER ${PROJECT_NAME}_PYTHON_SOURCES EXCLUDE REGEX "^multibody/pool")
+  #LIST(FILTER ${PROJECT_NAME}_PYTHON_HEADERS EXCLUDE REGEX "^algorithm/parallel")
+  #LIST(FILTER ${PROJECT_NAME}_PYTHON_SOURCES EXCLUDE REGEX "^algorithm/parallel")
+ELSE(NOT BUILD_WITH_OPENMP_SUPPORT)
+  IF(NOT BUILD_WITH_HPP_FCL_SUPPORT)
+    LIST_FILTER("${${PROJECT_NAME}_PYTHON_HEADERS}" "^multibody/pool/geometry.hpp" ${PROJECT_NAME}_PYTHON_HEADERS)
+    LIST_FILTER("${${PROJECT_NAME}_PYTHON_SOURCES}" "^algorithm/parallel/geometry.cpp" ${PROJECT_NAME}_PYTHON_SOURCES)
+    #LIST(FILTER ${PROJECT_NAME}_PYTHON_HEADERS EXCLUDE REGEX "^multibody/pool/geometry.hpp")
+    #LIST(FILTER ${PROJECT_NAME}_PYTHON_SOURCES EXCLUDE REGEX "^algorithm/parallel/geometry.cpp")
+  ENDIF(NOT BUILD_WITH_HPP_FCL_SUPPORT)
+ENDIF(NOT BUILD_WITH_OPENMP_SUPPORT)
+
 LIST(APPEND HEADERS ${${PROJECT_NAME}_PYTHON_HEADERS})
 
 # Headers of the Python bindings
@@ -71,12 +98,16 @@ MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python"
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/spatial")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/multibody")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/multibody/joint")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/multibody/pool")
 IF(BUILD_WITH_HPP_FCL_PYTHON_BINDINGS)
   MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/multibody/fcl")
 ENDIF(BUILD_WITH_HPP_FCL_PYTHON_BINDINGS)
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/parsers")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/serialization")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/algorithm")
+IF(BUILD_WITH_OPENMP_SUPPORT)
+  MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/algorithm/parallel")
+ENDIF(BUILD_WITH_OPENMP_SUPPORT)
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/bindings/python/utils")
 SYMLINK_AND_INSTALL_HEADERS("${${PROJECT_NAME}_PYTHON_HEADERS}" "bindings/python")
 
@@ -94,8 +125,16 @@ IF(BUILD_PYTHON_INTERFACE)
   ADD_CUSTOM_TARGET(python)
   SET_TARGET_PROPERTIES(python PROPERTIES EXCLUDE_FROM_DEFAULT_BUILD True)
 
-  SET(PKG_CONFIG_PYWRAP_REQUIRES "eigenpy >= 2.2.0")
+  SET(PKG_CONFIG_PYWRAP_REQUIRES "eigenpy >= 2.6.2")
   ADD_LIBRARY(${PYWRAP} SHARED ${${PYWRAP}_SOURCES} ${${PYWRAP}_HEADERS})
+  IF(BUILD_WITH_OPENMP_SUPPORT)
+    TARGET_COMPILE_OPTIONS(${PYWRAP} PRIVATE ${OpenMP_CXX_FLAGS})
+    TARGET_COMPILE_DEFINITIONS(${PYWRAP} PRIVATE -DPINOCCHIO_PYTHON_INTERFACE_WITH_OPENMP)
+    TARGET_INCLUDE_DIRECTORIES(${PYWRAP} SYSTEM PRIVATE ${OpenMP_CXX_INCLUDE_DIR})
+    IF(LINK_PYTHON_INTERFACE_TO_OPENM)
+      TARGET_LINK_LIBRARIES(${PYWRAP} PRIVATE ${OpenMP_CXX_LIBRARIES})
+    ENDIF(LINK_PYTHON_INTERFACE_TO_OPENM)
+  ENDIF(BUILD_WITH_OPENMP_SUPPORT)
   ADD_DEPENDENCIES(python ${PYWRAP})
   
   SET_TARGET_PROPERTIES(${PYWRAP} PROPERTIES PREFIX "") # Remove lib prefix for the target
@@ -117,7 +156,7 @@ IF(BUILD_PYTHON_INTERFACE)
   TARGET_LINK_LIBRARIES(${PYWRAP} PUBLIC ${PROJECT_NAME} eigenpy::eigenpy)
   TARGET_LINK_BOOST_PYTHON(${PYWRAP} PUBLIC)
   IF(BUILD_WITH_HPP_FCL_PYTHON_BINDINGS)
-    TARGET_COMPILE_DEFINITIONS(${PYWRAP} PRIVATE -DPINOCCHIO_WITH_HPP_FCL_PYTHON_BINDINGS)
+    TARGET_COMPILE_DEFINITIONS(${PYWRAP} PRIVATE -DPINOCCHIO_PYTHON_INTERFACE_WITH_HPP_FCL_PYTHON_BINDINGS)
   ENDIF(BUILD_WITH_HPP_FCL_PYTHON_BINDINGS)
   IF(WIN32)
     TARGET_COMPILE_DEFINITIONS(${PYWRAP} PRIVATE -DNOMINMAX)

bindings/python/algorithm/expose-rnea.cpp

 //
-// Copyright (c) 2015-2020 CNRS INRIA
+// Copyright (c) 2015-2021 CNRS INRIA
 //
 
 #include "pinocchio/bindings/python/algorithm/algorithms.hpp"
@@ -17,7 +17,7 @@ namespace pinocchio
       
       bp::def("rnea",
               &rnea<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd,VectorXd>,
-              bp::args("model","Data","q","v","a"),
+              bp::args("model","data","q","v","a"),
               "Compute the RNEA, store the result in Data and return it.\n\n"
               "Parameters:\n"
               "\tmodel: model of the kinematic tree\n"
@@ -29,7 +29,7 @@ namespace pinocchio
 
       bp::def("rnea",
               &rnea<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd,VectorXd,Force>,
-              bp::args("model","Data","q","v","a","fext"),
+              bp::args("model","data","q","v","a","fext"),
               "Compute the RNEA with external forces, store the result in Data and return it.\n\n"
               "Parameters:\n"
               "\tmodel: model of the kinematic tree\n"
@@ -42,7 +42,7 @@ namespace pinocchio
 
       bp::def("nonLinearEffects",
               &nonLinearEffects<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
-              bp::args("model","Data","q","v"),
+              bp::args("model","data","q","v"),
               "Compute the Non Linear Effects (coriolis, centrifugal and gravitational effects), store the result in Data and return it.\n\n"
               "Parameters:\n"
               "\tmodel: model of the kinematic tree\n"
@@ -53,7 +53,7 @@ namespace pinocchio
 
       bp::def("computeGeneralizedGravity",
               &computeGeneralizedGravity<double,0,JointCollectionDefaultTpl,VectorXd>,
-              bp::args("model","Data","q"),
+              bp::args("model","data","q"),
               "Compute the generalized gravity contribution g(q) of the Lagrangian dynamics, store the result in data.g and return it.\n\n"
               "Parameters:\n"
               "\tmodel: model of the kinematic tree\n"
@@ -63,7 +63,7 @@ namespace pinocchio
 
       bp::def("computeStaticTorque",
               &computeStaticTorque<double,0,JointCollectionDefaultTpl,VectorXd>,
-              bp::args("model","Data","q","fext"),
+              bp::args("model","data","q","fext"),
               "Computes the generalized static torque contribution g(q) - J.T f_ext of the Lagrangian dynamics, store the result in data.tau and return it.\n\n"
               "Parameters:\n"
               "\tmodel: model of the kinematic tree\n"
@@ -74,7 +74,7 @@ namespace pinocchio
 
       bp::def("computeCoriolisMatrix",
               &computeCoriolisMatrix<double,0,JointCollectionDefaultTpl,VectorXd,VectorXd>,
-              bp::args("model","Data","q","v"),
+              bp::args("model","data","q","v"),
               "Compute the Coriolis Matrix C(q,v) of the Lagrangian dynamics, store the result in data.C and return it.\n\n"
               "Parameters:\n"
               "\tmodel: model of the kinematic tree\n"
@@ -85,7 +85,7 @@ namespace pinocchio
 
       bp::def("getCoriolisMatrix",
               &getCoriolisMatrix<double,0,JointCollectionDefaultTpl>,
-              bp::args("model","Data"),
+              bp::args("model","data"),
               "Retrives the Coriolis Matrix C(q,v) of the Lagrangian dynamics after calling one of the derivative algorithms, store the result in data.C and return it.\n\n"
               "Parameters:\n"
               "\tmodel: model of the kinematic tree\n"

bindings/python/algorithm/parallel/aba.cpp

+//
+// Copyright (c) 2021 INRIA
+//
+
+#include "pinocchio/bindings/python/algorithm/algorithms.hpp"
+#include "pinocchio/algorithm/parallel/aba.hpp"
+
+#include <eigenpy/eigen-from-python.hpp>
+
+namespace pinocchio
+{
+  namespace python
+  {
+  
+    static void aba_proxy_res(const int num_thread, ModelPool & pool,
+                               const Eigen::MatrixXd & q, const Eigen::MatrixXd & v, const Eigen::MatrixXd & tau,
+                               Eigen::Ref<Eigen::MatrixXd> a)
+    {
+      aba(num_thread,pool,q,v,tau,a);
+    }
+  
+    static Eigen::MatrixXd aba_proxy(const int num_thread, ModelPool & pool,
+                                      const Eigen::MatrixXd & q, const Eigen::MatrixXd & v, const Eigen::MatrixXd & tau)
+    {
+      Eigen::MatrixXd a(v.rows(),v.cols());
+      aba(num_thread,pool,q,v,tau,a);
+      return a;
+    }
+  
+    void exposeParallelABA()
+    {
+      namespace bp = boost::python;
+      
+      using namespace Eigen;
+      
+      bp::def("aba",
+              aba_proxy,
+              bp::args("num_thread","pool","q","v","a"),
+              "Computes in parallel the ABA and returns the result.\n\n"
+              "Parameters:\n"
+              "\tnum_thread: number of threads used for the computation\n"
+              "\tpool: pool of model/data\n"
+              "\tq: the joint configuration vector (size model.nq x batch_size)\n"
+              "\tv: the joint velocity vector (size model.nv x batch_size)\n"
+              "\ttau: the joint torque vector (size model.nv x batch_size)\n");