[pinocchio3] commit for Preview of Pinocchio3

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

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

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

@@ -44,6 +44,7 @@ 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
     

.gitignore

@@ -4,4 +4,4 @@ Xcode*
 *.pyc
 coverage*
 .travis
-.vscode*
+.vscode*
\ No newline at end of file

CMakeLists.txt

@@ -63,6 +63,7 @@ OPTION(BUILD_ADVANCED_TESTING "Build the advanced tests (multiprecision, etc.) o
 
 # --- OPTIONAL DEPENDENCIES -------------------------
 OPTION(BUILD_WITH_URDF_SUPPORT "Build the library with the URDF format support" ON)
+OPTION(BUILD_WITH_SDF_SUPPORT "Build the library with the SDF format support" OFF)
 OPTION(BUILD_WITH_COLLISION_SUPPORT "Build the library with the collision support (required HPP-FCL)" OFF)
 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)
@@ -71,6 +72,7 @@ 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)
+OPTION(CHECK_RUNTIME_MALLOC "Check if some memory allocations are performed at runtime" OFF)
 
 IF(BUILD_WITH_COLLISION_SUPPORT)
   SET(BUILD_WITH_HPP_FCL_SUPPORT TRUE)
@@ -83,10 +85,16 @@ IF(BUILD_WITH_CODEGEN_SUPPORT)
 ENDIF()
 
 IF(INITIALIZE_WITH_NAN)
-  MESSAGE (STATUS "Initialize with NaN all the Eigen entries.")
+  MESSAGE(STATUS "Initialize with NaN all the Eigen entries.")
   ADD_DEFINITIONS(-DEIGEN_INITIALIZE_MATRICES_BY_NAN)
 ENDIF(INITIALIZE_WITH_NAN)
 
+IF(CHECK_RUNTIME_MALLOC)
+  MESSAGE(STATUS "Check if some memory allocations are performed at runtime.")
+  ADD_DEFINITIONS(-DPINOCCHIO_EIGEN_CHECK_MALLOC)
+  ADD_DEFINITIONS(-DEIGEN_RUNTIME_NO_MALLOC)
+ENDIF(CHECK_RUNTIME_MALLOC)
+
 MACRO(TAG_LIBRARY_VERSION target)
   SET_TARGET_PROPERTIES(${target} PROPERTIES SOVERSION ${PROJECT_VERSION})
 ENDMACRO(TAG_LIBRARY_VERSION)
@@ -116,6 +124,17 @@ IF(BUILD_WITH_URDF_SUPPORT)
   ENDIF(${urdfdom_VERSION} VERSION_GREATER "0.4.2")
 ENDIF(BUILD_WITH_URDF_SUPPORT)
 
+IF(BUILD_WITH_SDF_SUPPORT)
+  SET(CMAKE_CXX_STANDARD 11)
+  ADD_PROJECT_DEPENDENCY(SDFormat)
+  INCLUDE_DIRECTORIES(${SDFormat_INCLUDE_DIRS})
+  LINK_DIRECTORIES(${SDFormat_LIBRARY_DIRS})
+  ADD_DEFINITIONS(-DPINOCCHIO_WITH_SDF)
+  LIST(APPEND CFLAGS_DEPENDENCIES "-DPINOCCHIO_WITH_SDF")  
+ENDIF(BUILD_WITH_SDF_SUPPORT)
+
+
+
 IF(BUILD_WITH_AUTODIFF_SUPPORT)
   # Check first CppADCodeGen
   IF(BUILD_WITH_CODEGEN_SUPPORT)
@@ -187,6 +206,8 @@ ENDIF(BUILD_WITH_HPP_FCL_SUPPORT)
 ADD_DEFINITIONS(-DBOOST_MPL_LIMIT_LIST_SIZE=30)
 ADD_DEFINITIONS(-DBOOST_MPL_LIMIT_VECTOR_SIZE=30)
 
+LIST(APPEND CFLAGS_DEPENDENCIES "-DBOOST_MPL_LIMIT_LIST_SIZE=30")
+
 # Path to boost headers
 INCLUDE_DIRECTORIES(SYSTEM ${Boost_INCLUDE_DIRS})
 
@@ -208,6 +229,15 @@ IF(NOT BUILD_WITH_URDF_SUPPORT)
     )
 ENDIF(NOT BUILD_WITH_URDF_SUPPORT)
 
+IF(NOT BUILD_WITH_SDF_SUPPORT)
+  LIST(REMOVE_ITEM HEADERS
+    ${PROJECT_SOURCE_DIR}/src/parsers/sdf.hpp
+    ${PROJECT_SOURCE_DIR}/src/parsers/sdf/model.hxx
+    ${PROJECT_SOURCE_DIR}/src/parsers/sdf/geometry.hxx
+    )
+ENDIF(NOT BUILD_WITH_SDF_SUPPORT)
+
+
 IF(NOT BUILD_WITH_HPP_FCL_SUPPORT)
   LIST(REMOVE_ITEM HEADERS 
     ${PROJECT_SOURCE_DIR}/src/spatial/fcl-pinocchio-conversions.hpp
@@ -227,10 +257,12 @@ 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/parsers/sdf")
 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/algorithm/utils")
 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")

README.md

@@ -31,6 +31,39 @@ If you want to directly dive into **Pinocchio**, only one single line is suffici
 conda install pinocchio -c conda-forge
 </p>
 
+## Introducing Pinocchio 3
+**Pinocchio3** is released for development under the branch [pinocchio3-preview](https://github.com/stack-of-tasks/pinocchio/tree/pinocchio3-preview) on the main github repository.
+With **Pinocchio3**, multiple new features are introduced in **Pinocchio**, such as:
+  - Sparse Solution of Constrained Dynamics (Published in Robotics: Science and Systems 2021)
+  - Constrained Dynamics Derivatives (In pre-publishing stages)
+  - Constraint Models for handling loop constraints.
+  - Full casadi support in python and C++
+  - Increased support of CppAD and CppADCodeGen
+  - New SDF parser.
+  - and much more...
+
+**Pinocchio** developers are highly encouraged to check out the new features. However, please keep in mind that this remains a **development** branch, and thus the API between 2.9.x and 2.9.{x+1} could change without backward compatibility.
+
+The new constrained dynamics algorithm can be cited by the following publication:
+
+```bibtex
+@inproceedings{carpentier:hal-03271811,
+  TITLE = {{Proximal and Sparse Resolution of Constrained Dynamic Equations}},
+  AUTHOR = {Carpentier, Justin and Budhiraja, Rohan and Mansard, Nicolas},
+  URL = {https://hal.inria.fr/hal-03271811},
+  BOOKTITLE = {{Robotics: Science and Systems 2021}},
+  ADDRESS = {Austin / Virtual, United States},
+  YEAR = {2021},
+  MONTH = Jul,
+  PDF = {https://hal.inria.fr/hal-03271811/file/rss-proximal-and-sparse.pdf},
+  HAL_ID = {hal-03271811},
+  HAL_VERSION = {v1},
+}
+
+```
+
+
+
 ## Pinocchio main features
 
 **Pinocchio** is fast:

benchmark/CMakeLists.txt

@@ -84,3 +84,8 @@ ENDIF(URDFDOM_FOUND AND hpp-fcl_FOUND)
 # timings-jacobian
 #
 ADD_BENCH(timings-jacobian TRUE)
+
+# timings-constrained-dynamics
+#
+ADD_BENCH(timings-contact-dynamics TRUE)
+ADD_BENCH(timings-constrained-dynamics-derivatives TRUE)

benchmark/timings-cg.cpp

@@ -8,6 +8,9 @@
 #include "pinocchio/algorithm/crba.hpp"
 #include "pinocchio/algorithm/centroidal.hpp"
 #include "pinocchio/algorithm/aba.hpp"
+#include "pinocchio/algorithm/contact-dynamics-derivatives.hpp"
+#include "pinocchio/algorithm/contact-dynamics.hpp"
+#include "pinocchio/algorithm/contact-info.hpp"
 #include "pinocchio/algorithm/rnea.hpp"
 #include "pinocchio/algorithm/cholesky.hpp"
 #include "pinocchio/algorithm/jacobian.hpp"
@@ -62,6 +65,21 @@ int main(int argc, const char ** argv)
   std::cout << "--" << std::endl;
 
   pinocchio::Data data(model);
+
+  const std::string RF = "RLEG_ANKLE_R";
+  const std::string LF = "LLEG_ANKLE_R";
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D6D;
+
+  RigidConstraintModel ci_RF(CONTACT_6D,model.getFrameId(RF),WORLD);
+  RigidConstraintModel ci_LF(CONTACT_6D,model.getFrameId(LF),WORLD);
+  contact_models_6D6D.push_back(ci_RF);
+  contact_models_6D6D.push_back(ci_LF);
+
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_datas_6D6D;
+
+  RigidConstraintData cd_RF(ci_RF);  contact_datas_6D6D.push_back(cd_RF);
+  RigidConstraintData cd_LF(ci_LF);  contact_datas_6D6D.push_back(cd_LF);
+    
   VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
   
   CodeGenRNEA<double> rnea_code_gen(model);
@@ -87,7 +105,11 @@ int main(int argc, const char ** argv)
   CodeGenABADerivatives<double> aba_derivatives_code_gen(model);
   aba_derivatives_code_gen.initLib();
   aba_derivatives_code_gen.loadLib();
-
+  
+  CodeGenConstraintDynamicsDerivatives<double> constraint_dynamics_derivatives_code_gen(model, contact_models_6D6D);
+  constraint_dynamics_derivatives_code_gen.initLib();
+  constraint_dynamics_derivatives_code_gen.loadLib();
+  
   pinocchio::container::aligned_vector<VectorXd> qs     (NBT);
   pinocchio::container::aligned_vector<VectorXd> qdots  (NBT);
   pinocchio::container::aligned_vector<VectorXd> qddots (NBT);
@@ -191,6 +213,27 @@ int main(int argc, const char ** argv)
     aba_derivatives_code_gen.evalFunction(qs[_smooth],qdots[_smooth],qddots[_smooth]);
   }
   std::cout << "ABA partial derivatives code gen = \t\t"; timer.toc(std::cout,NBT);
+
+
+  pinocchio::initConstraintDynamics(model, data, contact_models_6D6D);
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    pinocchio::constraintDynamics(model, data, qs[_smooth],qdots[_smooth],qddots[_smooth],
+                               contact_models_6D6D, contact_datas_6D6D);
+    pinocchio::computeConstraintDynamicsDerivatives(model, data,
+                                                 contact_models_6D6D, contact_datas_6D6D);
+  }
+  std::cout << "contact dynamics derivatives 6D,6D = \t\t"; timer.toc(std::cout,NBT);
+
+
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    constraint_dynamics_derivatives_code_gen.evalFunction(qs[_smooth],qdots[_smooth],qddots[_smooth]);
+  }
+  std::cout << "contact dynamics derivatives 6D,6D code gen = \t\t"; timer.toc(std::cout,NBT);
+
   
   return 0;
 }

benchmark/timings-constrained-dynamics-derivatives.cpp

+//
+// Copyright (c) 2019-2020 LAAS-CNRS INRIA
+//
+
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/constrained-dynamics.hpp"
+#include "pinocchio/algorithm/aba-derivatives.hpp"
+#include "pinocchio/algorithm/contact-dynamics.hpp"
+#include "pinocchio/algorithm/constrained-dynamics-derivatives.hpp"
+#include "pinocchio/algorithm/cholesky.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
+
+#include <iostream>
+
+#include "pinocchio/utils/timer.hpp"
+
+int main(int argc, const char ** argv)
+{
+  using namespace Eigen;
+  using namespace pinocchio;
+  
+  PinocchioTicToc timer(PinocchioTicToc::US);
+#ifdef NDEBUG
+  const int NBT = 1000*100;
+#else
+  const int NBT = 1;
+  std::cout << "(the time score in debug mode is not relevant) " << std::endl;
+#endif
+  
+  // Build model
+  Model model;
+  
+  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
+  if(argc>1) filename = argv[1];
+  bool with_ff = true;
+  
+  if(argc>2)
+  {
+    const std::string ff_option = argv[2];
+    if(ff_option == "-no-ff")
+      with_ff = false;
+  }
+  
+  if( filename == "HS")
+    buildModels::humanoidRandom(model,true);
+  else
+    if(with_ff)
+      pinocchio::urdf::buildModel(filename,JointModelFreeFlyer(),model);
+  //      pinocchio::urdf::buildModel(filename,JointModelRX(),model);
+    else
+      pinocchio::urdf::buildModel(filename,model);
+  
+  const std::string RA = "RARM_LINK6";
+  const JointIndex RA_id = model.frames[model.getFrameId(RA)].parent;
+  const std::string LA = "LARM_LINK6";
+  const JointIndex LA_id = model.frames[model.getFrameId(LA)].parent;
+  const std::string RF = "RLEG_LINK6";
+  const JointIndex RF_id = model.frames[model.getFrameId(RF)].parent;
+  const std::string LF = "LLEG_LINK6";
+  const JointIndex LF_id = model.frames[model.getFrameId(LF)].parent;
+  
+  RigidConstraintModel ci_RF_6D(CONTACT_6D,RF_id,LOCAL);
+  RigidConstraintData cd_RF_6D(ci_RF_6D);
+  //RigidConstraintModel ci_RF_3D(CONTACT_3D,model.getJointId(RF),WORLD);
+  
+  RigidConstraintModel ci_LF_6D(CONTACT_6D,LF_id,LOCAL);
+  RigidConstraintData cd_LF_6D(ci_LF_6D);
+  // RigidConstraintModel ci_LF_3D(CONTACT_3D,model.getJointId(LF),WORLD);
+  
+  //RigidConstraintModel ci_RA_3D(CONTACT_3D,model.getJointId(RA),WORLD);
+  //RigidConstraintModel ci_LA_3D(CONTACT_3D,model.getJointId(LA),WORLD);
+  
+  // Define contact infos structure
+  const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_empty;
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_empty;
+
+  cholesky::ContactCholeskyDecomposition contact_chol_empty(model,contact_models_empty);
+  
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D;
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_6D;
+  contact_models_6D.push_back(ci_RF_6D);
+  contact_data_6D.push_back(cd_RF_6D);
+  
+  cholesky::ContactCholeskyDecomposition contact_chol_6D(model,contact_models_6D);
+  
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D6D;
+  contact_models_6D6D.push_back(ci_RF_6D);
+  contact_models_6D6D.push_back(ci_LF_6D);
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_6D6D;
+  contact_data_6D6D.push_back(cd_RF_6D);
+  contact_data_6D6D.push_back(cd_LF_6D);
+
+  cholesky::ContactCholeskyDecomposition contact_chol_6D6D(model,contact_models_6D6D);
+  
+  std::cout << "nq = " << model.nq << std::endl;
+  std::cout << "nv = " << model.nv << std::endl;
+  std::cout << "--" << std::endl;
+  
+  Data data(model);
+  VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
+  
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qs(NBT);
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qdots(NBT);
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qddots(NBT);
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) taus(NBT);
+  
+  for(size_t i=0;i<NBT;++i)
+  {
+    qs[i]     = randomConfiguration(model,-qmax,qmax);
+    qdots[i]  = Eigen::VectorXd::Random(model.nv);
+    qddots[i] = Eigen::VectorXd::Random(model.nv);
+    taus[i] = Eigen::VectorXd::Random(model.nv);
+  }
+
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    computeABADerivatives(model,data,qs[_smooth],qdots[_smooth],taus[_smooth]);
+  }
+  std::cout << "ABA derivatives= \t\t\t"; timer.toc(std::cout,NBT);
+
+  
+  double total_time = 0;  
+  initConstraintDynamics(model,data,contact_models_empty);
+  SMOOTH(NBT)
+  {
+    constraintDynamics(model,data,qs[_smooth],qdots[_smooth],taus[_smooth],contact_models_empty,contact_data_empty);
+    timer.tic();
+    computeConstraintDynamicsDerivatives(model, data,
+                                      contact_models_empty, contact_data_empty);
+    total_time += timer.toc(timer.DEFAULT_UNIT);
+  }
+  std::cout << "constraintDynamicsDerivs {} = \t\t" << (total_time/NBT)<<std::endl;
+
+  total_time = 0;  
+  initConstraintDynamics(model,data,contact_models_6D);
+  SMOOTH(NBT)
+  {
+    constraintDynamics(model,data,qs[_smooth],qdots[_smooth],taus[_smooth],contact_models_6D,contact_data_6D);
+    timer.tic();
+    computeConstraintDynamicsDerivatives(model,data,contact_models_6D,contact_data_6D);
+    total_time += timer.toc(timer.DEFAULT_UNIT);
+  }
+  std::cout << "constraintDynamicsDerivs {6D} = \t\t" << (total_time/NBT)<<std::endl;
+
+  total_time = 0;
+  initConstraintDynamics(model,data,contact_models_6D6D);
+  SMOOTH(NBT)
+  {
+    constraintDynamics(model,data,qs[_smooth],qdots[_smooth],taus[_smooth],contact_models_6D6D,contact_data_6D6D);
+    timer.tic();
+    computeConstraintDynamicsDerivatives(model,data, contact_models_6D6D, contact_data_6D6D);
+    total_time += timer.toc(timer.DEFAULT_UNIT);
+  }
+  std::cout << "constraintDynamicsDerivs {6D,6D} = \t" << (total_time/NBT)<<std::endl;  
+  return 0;
+}
+

benchmark/timings-contact-dynamics.cpp

+//
+// Copyright (c) 2019-2020 INRIA
+//
+
+#include "pinocchio/algorithm/joint-configuration.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/algorithm/kinematics-derivatives.hpp"
+#include "pinocchio/algorithm/rnea-derivatives.hpp"
+#include "pinocchio/algorithm/aba-derivatives.hpp"
+#include "pinocchio/algorithm/aba.hpp"
+#include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/contact-dynamics.hpp"
+#include "pinocchio/algorithm/constrained-dynamics.hpp"
+#include "pinocchio/algorithm/cholesky.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
+
+#include <iostream>
+
+#include "pinocchio/utils/timer.hpp"
+
+int main(int argc, const char ** argv)
+{
+  using namespace Eigen;
+  using namespace pinocchio;
+  
+  PinocchioTicToc timer(PinocchioTicToc::US);
+#ifdef NDEBUG
+  const int NBT = 1000*100;
+#else
+  const int NBT = 1;
+  std::cout << "(the time score in debug mode is not relevant) " << std::endl;
+#endif
+  
+  // Build model
+  Model model;
+  
+  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
+  if(argc>1) filename = argv[1];
+  bool with_ff = true;
+  
+  if(argc>2)
+  {
+    const std::string ff_option = argv[2];
+    if(ff_option == "-no-ff")
+      with_ff = false;
+  }
+  
+  if( filename == "HS")
+    buildModels::humanoidRandom(model,true);
+  else
+    if(with_ff)
+      pinocchio::urdf::buildModel(filename,JointModelFreeFlyer(),model);
+  //      pinocchio::urdf::buildModel(filename,JointModelRX(),model);
+    else
+      pinocchio::urdf::buildModel(filename,model);
+  
+  const std::string RA = "RARM_LINK6";
+  const JointIndex RA_id = model.frames[model.getFrameId(RA)].parent;
+  const std::string LA = "LARM_LINK6";
+  const JointIndex LA_id = model.frames[model.getFrameId(LA)].parent;
+  const std::string RF = "RLEG_LINK6";
+  const JointIndex RF_id = model.frames[model.getFrameId(RF)].parent;
+  const std::string LF = "LLEG_LINK6";
+  const JointIndex LF_id = model.frames[model.getFrameId(LF)].parent;
+  
+  RigidConstraintModel ci_RF_6D(CONTACT_6D,RF_id,LOCAL);
+  RigidConstraintModel ci_RF_3D(CONTACT_3D,RF_id,LOCAL);
+  
+  RigidConstraintModel ci_LF_6D(CONTACT_6D,LF_id,LOCAL);
+  RigidConstraintModel ci_LF_3D(CONTACT_3D,LF_id,LOCAL);
+  
+  RigidConstraintModel ci_RA_3D(CONTACT_3D,RA_id,LOCAL);
+  RigidConstraintModel ci_LA_3D(CONTACT_3D,LA_id,LOCAL);
+  
+  // Define contact infos structure
+  static const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_empty;
+  static PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_empty;
+  cholesky::ContactCholeskyDecomposition contact_chol_empty(model,contact_models_empty);
+  
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D;
+  contact_models_6D.push_back(ci_RF_6D);
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_6D;
+  contact_data_6D.push_back(RigidConstraintData(ci_RF_6D));
+  cholesky::ContactCholeskyDecomposition contact_chol_6D(model,contact_models_6D);
+  
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models_6D6D;
+  contact_models_6D6D.push_back(ci_RF_6D);
+  contact_models_6D6D.push_back(ci_LF_6D);
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintData) contact_data_6D6D;
+  contact_data_6D6D.push_back(RigidConstraintData(ci_RF_6D));
+  contact_data_6D6D.push_back(RigidConstraintData(ci_LF_6D));
+  cholesky::ContactCholeskyDecomposition contact_chol_6D6D(model,contact_models_6D6D);
+
+  ProximalSettings prox_settings;
+  prox_settings.max_iter = 10;
+  prox_settings.mu = 1e8;
+  
+  std::cout << "nq = " << model.nq << std::endl;
+  std::cout << "nv = " << model.nv << std::endl;
+  std::cout << "--" << std::endl;
+  
+  Data data(model);
+  VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
+  
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qs(NBT);
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qdots(NBT);
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) qddots(NBT);
+  PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(VectorXd) taus(NBT);
+  
+  static const PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(RigidConstraintModel) contact_models;
+  
+  for(size_t i=0;i<NBT;++i)
+  {
+    qs[i]     = randomConfiguration(model,-qmax,qmax);
+    qdots[i]  = Eigen::VectorXd::Random(model.nv);
+    qddots[i] = Eigen::VectorXd::Random(model.nv);
+    taus[i] = Eigen::VectorXd::Random(model.nv);
+  }
+  
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    aba(model,data,qs[_smooth],qdots[_smooth],taus[_smooth]);
+  }
+  std::cout << "ABA = \t\t"; timer.toc(std::cout,NBT);
+    
+  double total_time = 0;
+  SMOOTH(NBT)
+  {
+    crba(model,data,qs[_smooth]);
+    timer.tic();
+    cholesky::decompose(model,data);
+    total_time += timer.toc(timer.DEFAULT_UNIT);
+  }
+  std::cout << "Sparse Cholesky = \t\t" << (total_time/NBT)
+  << " " << timer.unitName(timer.DEFAULT_UNIT) <<std::endl;
+  
+  total_time = 0;
+  SMOOTH(NBT)
+  {
+    computeAllTerms(model,data,qs[_smooth],qdots[_smooth]);
+    timer.tic();
+    contact_chol_empty.compute(model,data,contact_models_empty,contact_data_empty);
+    total_time += timer.toc(timer.DEFAULT_UNIT);
+  }
+  std::cout << "contactCholesky {} = \t\t" << (total_time/NBT)
+  << " " << timer.unitName(timer.DEFAULT_UNIT) <<std::endl;
+  
+  total_time = 0;
+  MatrixXd H_inverse(contact_chol_empty.size(),contact_chol_empty.size());
+  SMOOTH(NBT)
+  {
+    computeAllTerms(model,data,qs[_smooth],qdots[_smooth]);
+    contact_chol_empty.compute(model,data,contact_models_empty,contact_data_empty);
+    timer.tic();
+    contact_chol_empty.inverse(H_inverse);
+    total_time += timer.toc(timer.DEFAULT_UNIT);
+  }
+  std::cout << "contactCholeskyInverse {} = \t\t" << (total_time/NBT)
+  << " " << timer.unitName(timer.DEFAULT_UNIT) <<std::endl;
+  
+  initConstraintDynamics(model,data,contact_models_empty);
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    constraintDynamics(model,data,qs[_smooth],qdots[_smooth],taus[_smooth],contact_models_empty,contact_data_empty);
+  }
+  std::cout << "constraintDynamics {} = \t\t"; timer.toc(std::cout,NBT);
+  
+  total_time = 0;
+  SMOOTH(NBT)
+  {
+    computeAllTerms(model,data,qs[_smooth],qdots[_smooth]);
+    timer.tic();
+    contact_chol_6D.compute(model,data,contact_models_6D,contact_data_6D);
+    total_time += timer.toc(timer.DEFAULT_UNIT);
+  }
+  std::cout << "contactCholesky {6D} = \t\t" << (total_time/NBT)
+  << " " << timer.unitName(timer.DEFAULT_UNIT) <<std::endl;
+  
+  total_time = 0;
+  H_inverse.resize(contact_chol_6D.size(),contact_chol_6D.size());
+  SMOOTH(NBT)
+  {
+    computeAllTerms(model,data,qs[_smooth],qdots[_smooth]);
+    contact_chol_6D.compute(model,data,contact_models_6D,contact_data_6D);
+    timer.tic();
+    contact_chol_6D.inverse(H_inverse);
+    total_time += timer.toc(timer.DEFAULT_UNIT);
+  }
+  std::cout << "contactCholeskyInverse {6D} = \t\t" << (total_time/NBT)
+  << " " << timer.unitName(timer.DEFAULT_UNIT) <<std::endl;
+  
+  MatrixXd J(contact_chol_6D.constraintDim(),model.nv);
+  J.setZero();
+  MatrixXd MJtJ_inv(model.nv+contact_chol_6D.constraintDim(),
+                    model.nv+contact_chol_6D.constraintDim());
+  MJtJ_inv.setZero();