Merge pull request #569 from jcarpent/devel

Version 2.0.0-alpha

.travis @ 083fa2cb

-Subproject commit 00d90b0f0c3aa0579667d963cc91cae4290add3c
+Subproject commit 083fa2cb0fa4ad594926d9bb3d246075e62ce9ee

.travis.yml

@@ -3,6 +3,8 @@ python:
   - "2.7"
 sudo: required
 dist: trusty
+git:
+  depth: false
 compiler:
   - gcc
 # - clang

CMakeLists.txt

@@ -26,6 +26,7 @@ INCLUDE(cmake/test.cmake)
 SET(PROJECT_NAME pinocchio)
 SET(PROJECT_DESCRIPTION "Rigid multi body dynamics algorithms")
 SET(PROJECT_URL "http://github.com/stack-of-tasks/pinocchio")
+SET(PROJECT_CUSTOM_HEADER_EXTENSION "hpp")
 OPTION(INSTALL_DOCUMENTATION "Generate and install the documentation" ON)
 
 SET(DOXYGEN_USE_MATHJAX YES)
@@ -83,9 +84,24 @@ ENDMACRO(TAG_LIBRARY_VERSION)
 # --- DEPENDANCIES -----------------------------------
 # ----------------------------------------------------
 ADD_REQUIRED_DEPENDENCY("eigen3 >= 3.0.5")
-ADD_OPTIONAL_DEPENDENCY("metapod >= 1.0.7")
 ADD_OPTIONAL_DEPENDENCY("urdfdom >= 0.2.0")
 ADD_OPTIONAL_DEPENDENCY("hpp-fcl >= 0.5.1")
+ADD_OPTIONAL_DEPENDENCY("cppad")
+
+IF(CPPAD_FOUND)
+  ADD_DEFINITIONS(-DPINOCCHIO_WITH_CPPAD_SUPPORT)
+  PKG_CONFIG_APPEND_CFLAGS("-DPINOCCHIO_WITH_CPPAD_SUPPORT")
+  IF(NOT ${CPPAD_VERSION} VERSION_GREATER "3.3.0")
+    ADD_DEFINITIONS(-DPINOCCHIO_CPPAD_REQUIRES_MATRIX_BASE_PLUGIN)
+    PKG_CONFIG_APPEND_CFLAGS("-DPINOCCHIO_CPPAD_REQUIRES_MATRIX_BASE_PLUGIN")
+  ENDIF(NOT ${CPPAD_VERSION} VERSION_GREATER "3.3.0")
+
+  ADD_OPTIONAL_DEPENDENCY("cppadcg")
+  IF(CPPADCG_FOUND)
+    ADD_DEFINITIONS(-DPINOCCHIO_WITH_CPPADCG_SUPPORT)
+    PKG_CONFIG_APPEND_CFLAGS("-DPINOCCHIO_WITH_CPPADCG_SUPPORT")
+  ENDIF(CPPADCG_FOUND)
+ENDIF(CPPAD_FOUND)
 
 # Special care of urdfdom version
 IF(URDFDOM_FOUND)
@@ -155,19 +171,29 @@ SET(${PROJECT_NAME}_MATH_HEADERS
   math/sincos.hpp
   math/quaternion.hpp
   math/matrix.hpp
+  math/cppad.hpp
+  math/cppadcg.hpp
   )
 
 SET(${PROJECT_NAME}_UTILS_HEADERS
   utils/axis-label.hpp
+  utils/eigen-fix.hpp
   utils/timer.hpp
   utils/string-generator.hpp
   utils/file-explorer.hpp
   utils/version.hpp
   )
 
+SET(${PROJECT_NAME}_CODEGEN_HEADERS
+  codegen/code-generator-base.hpp
+  codegen/code-generator-algo.hpp
+  )
+
 SET(${PROJECT_NAME}_SPATIAL_HEADERS
   spatial/symmetric3.hpp
   spatial/se3.hpp
+  spatial/se3-base.hpp
+  spatial/se3-tpl.hpp
   spatial/motion.hpp
   spatial/motion-base.hpp
   spatial/motion-dense.hpp
@@ -185,6 +211,7 @@ SET(${PROJECT_NAME}_SPATIAL_HEADERS
   spatial/act-on-set.hpp
   spatial/act-on-set.hxx
   spatial/explog.hpp
+  spatial/explog-quaternion.hpp
   spatial/cartesian-axis.hpp
   spatial/spatial-axis.hpp
   )
@@ -192,6 +219,7 @@ SET(${PROJECT_NAME}_SPATIAL_HEADERS
 SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
   multibody/joint/fwd.hpp
   multibody/joint/joint-base.hpp
+  multibody/joint/joint-collection.hpp
   multibody/joint/joint-revolute.hpp
   multibody/joint/joint-revolute-unaligned.hpp
   multibody/joint/joint-revolute-unbounded.hpp
@@ -202,8 +230,7 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
   multibody/joint/joint-planar.hpp
   multibody/joint/joint-translation.hpp
   multibody/joint/joint-free-flyer.hpp
-  multibody/joint/joint-variant.hpp
-  multibody/joint/joint.hpp
+  multibody/joint/joint-generic.hpp
   multibody/joint/joint-basic-visitors.hpp
   multibody/joint/joint-basic-visitors.hxx
   multibody/joint/joint-composite.hpp
@@ -211,14 +238,17 @@ SET(${PROJECT_NAME}_MULTIBODY_JOINT_HEADERS
   ) 
 
 SET(${PROJECT_NAME}_MULTIBODY_LIEGROUP_HEADERS
+  multibody/liegroup/fwd.hpp
   multibody/liegroup/liegroup.hpp
   multibody/liegroup/liegroup-algo.hpp
   multibody/liegroup/liegroup-algo.hxx
+  multibody/liegroup/liegroup-collection.hpp
+  multibody/liegroup/liegroup-generic.hpp
   multibody/liegroup/liegroup-variant.hpp
   multibody/liegroup/liegroup-variant-visitors.hpp
   multibody/liegroup/liegroup-variant-visitors.hxx
-  multibody/liegroup/operation-base.hpp
-  multibody/liegroup/operation-base.hxx
+  multibody/liegroup/liegroup-base.hpp
+  multibody/liegroup/liegroup-base.hxx
   multibody/liegroup/vector-space.hpp
   multibody/liegroup/cartesian-product.hpp
   multibody/liegroup/special-orthogonal.hpp
@@ -244,6 +274,8 @@ SET(${PROJECT_NAME}_ALGORITHM_HEADERS
   algorithm/aba-derivatives.hxx
   algorithm/centroidal.hpp
   algorithm/centroidal.hxx
+  algorithm/centroidal-derivatives.hpp
+  algorithm/centroidal-derivatives.hxx
   algorithm/rnea.hpp
   algorithm/rnea.hxx
   algorithm/rnea-derivatives.hpp
@@ -285,11 +317,14 @@ SET(${PROJECT_NAME}_PARSERS_HEADERS
   parsers/sample-models.hpp
   parsers/utils.hpp
   parsers/srdf.hpp
+  parsers/srdf.hxx
   ) 
 
 IF(URDFDOM_FOUND)
   LIST(APPEND ${PROJECT_NAME}_PARSERS_HEADERS
     parsers/urdf.hpp
+    parsers/urdf/model.hxx
+    parsers/urdf/geometry.hxx
     parsers/urdf/utils.hpp
     parsers/urdf/types.hpp
     )
@@ -337,9 +372,12 @@ SET(HEADERS
   ${${PROJECT_NAME}_PARSERS_HEADERS}
   ${${PROJECT_NAME}_ALGORITHM_HEADERS}
   ${${PROJECT_NAME}_CONTAINER_HEADERS}
+  ${${PROJECT_NAME}_CODEGEN_HEADERS}
   exception.hpp
   assert.hpp
   macros.hpp
+  eigen-macros.hpp
+  fwd.hpp
  )
 LIST(REMOVE_DUPLICATES HEADERS)
 
@@ -355,6 +393,7 @@ 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/algorithm")
 MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/container")
+MAKE_DIRECTORY("${${PROJECT_NAME}_BINARY_DIR}/include/pinocchio/codegen")
 
 FOREACH(header ${HEADERS})
   GET_FILENAME_COMPONENT(headerName ${header} NAME)

NEWS

-New in 1.0.1, 04/30/2015:
-* Added computation of non-linear-effects
-* Now handle fixed joint when parsing urdf files and keep trace of fixed bodies
-* Added Lua parser to build models
-* Added planar and translation joints
-* Fixed function se3ToXYZQUAT used with python
\ No newline at end of file

benchmark/CMakeLists.txt

@@ -20,23 +20,32 @@
 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_SOURCE_DIR="${${PROJECT_NAME}_SOURCE_DIR}")
-    TARGET_LINK_LIBRARIES(${bench_name} ${PROJECT_NAME})
-    PKG_CONFIG_USE_DEPENDENCY(${bench_name} eigen3)
-  ENDIF(link_to_main_lib)
-ENDIF()
+  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_SOURCE_DIR="${${PROJECT_NAME}_SOURCE_DIR}")
+      TARGET_LINK_LIBRARIES(${bench_name} ${PROJECT_NAME})
+      PKG_CONFIG_USE_DEPENDENCY(${bench_name} eigen3)
 
-ADD_DEPENDENCIES(bench ${bench_name})
+      IF(HPP_FCL_FOUND)
+        PKG_CONFIG_USE_DEPENDENCY(${bench_name} hpp-fcl)
+      ENDIF(HPP_FCL_FOUND)
+
+      IF(URDFDOM_FOUND)
+        PKG_CONFIG_USE_DEPENDENCY(${bench_name} urdfdom)
+      ENDIF(URDFDOM_FOUND)
+
+    ENDIF(link_to_main_lib)
+  ENDIF()
+  
+  ADD_DEPENDENCIES(bench ${bench_name})
 ENDMACRO(ADD_BENCH)
 
 MACRO(ADD_TEST_CFLAGS target flag)
@@ -46,15 +55,27 @@ ENDMACRO(ADD_TEST_CFLAGS)
 # timings
 # 
 ADD_BENCH(timings TRUE)
+IF(CPPADCG_FOUND)
+  ADD_BENCH(timings-cg TRUE)
+  SET_PROPERTY(TARGET timings-cg PROPERTY CXX_STANDARD 11)
+  PKG_CONFIG_USE_DEPENDENCY(timings-cg "cppadcg")
+ENDIF(CPPADCG_FOUND)
 
 # timings
 # 
 ADD_BENCH(timings-cholesky TRUE)
 
 # timings derivatives
-# 
+ 
 ADD_BENCH(timings-derivatives TRUE)
 TARGET_LINK_LIBRARIES(timings-derivatives ${PROJECT_NAME})
+IF(CPPAD_FOUND)
+  PKG_CONFIG_USE_DEPENDENCY(timings-derivatives "cppad")
+ENDIF(CPPAD_FOUND)
+IF(CPPADCG_FOUND)
+  SET_PROPERTY(TARGET timings-derivatives PROPERTY CXX_STANDARD 11)
+  PKG_CONFIG_USE_DEPENDENCY(timings-derivatives "cppadcg")
+ENDIF(CPPADCG_FOUND)
 
 # timings-eigen
 # 
@@ -64,7 +85,6 @@ ADD_BENCH(timings-eigen)
 # 
 IF(URDFDOM_FOUND AND HPP_FCL_FOUND)
   ADD_BENCH(timings-geometry TRUE)
-  PKG_CONFIG_USE_DEPENDENCY(timings-geometry hpp-fcl)
 ENDIF(URDFDOM_FOUND AND HPP_FCL_FOUND)
 
 

benchmark/timings-cg.cpp

+//
+// Copyright (c) 2018 CNRS
+//
+// This file is part of Pinocchio
+// Pinocchio is free software: you can redistribute it
+// and/or modify it under the terms of the GNU Lesser General Public
+// License as published by the Free Software Foundation, either version
+// 3 of the License, or (at your option) any later version.
+//
+// Pinocchio is distributed in the hope that it will be
+// useful, but WITHOUT ANY WARRANTY; without even the implied warranty
+// of MERCHANTABILITY or FITNESS FOR A PARTICULAR PURPOSE. See the GNU
+// General Lesser Public License for more details. You should have
+// received a copy of the GNU Lesser General Public License along with
+// Pinocchio If not, see
+// <http://www.gnu.org/licenses/>.
+
+#include "pinocchio/algorithm/crba.hpp"
+#include "pinocchio/algorithm/centroidal.hpp"
+#include "pinocchio/algorithm/aba.hpp"
+#include "pinocchio/algorithm/rnea.hpp"
+#include "pinocchio/algorithm/cholesky.hpp"
+#include "pinocchio/algorithm/jacobian.hpp"
+#include "pinocchio/algorithm/center-of-mass.hpp"
+#include "pinocchio/algorithm/compute-all-terms.hpp"
+#include "pinocchio/algorithm/kinematics.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
+#include "pinocchio/container/aligned-vector.hpp"
+
+#include "pinocchio/codegen/code-generator-algo.hpp"
+
+#include <iostream>
+
+#include "pinocchio/utils/timer.hpp"
+
+int main(int argc, const char ** argv)
+{
+  using namespace Eigen;
+  using namespace se3;
+
+  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
+    
+  se3::Model model;
+
+  std::string filename = PINOCCHIO_SOURCE_DIR"/models/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 == "H")
+    se3::buildModels::humanoidRandom(model,true);
+  else
+    if(with_ff)
+      se3::urdf::buildModel(filename,JointModelFreeFlyer(),model);
+    else
+      se3::urdf::buildModel(filename,model);
+  
+  std::cout << "nq = " << model.nq << std::endl;
+  std::cout << "nv = " << model.nv << std::endl;
+  std::cout << "--" << std::endl;
+
+  se3::Data data(model);
+  
+  CodeGenRNEA<double> rnea_code_gen(model);
+  rnea_code_gen.initLib();
+  rnea_code_gen.loadLib();
+  
+  CodeGenABA<double> aba_code_gen(model);
+  aba_code_gen.initLib();
+  aba_code_gen.loadLib();
+  
+  CodeGenCRBA<double> crba_code_gen(model);
+  crba_code_gen.initLib();
+  crba_code_gen.loadLib();
+  
+  CodeGenMinv<double> minv_code_gen(model);
+  minv_code_gen.initLib();
+  minv_code_gen.loadLib();
+  
+  CodeGenRNEADerivatives<double> rnea_derivatives_code_gen(model);
+  rnea_derivatives_code_gen.initLib();
+  rnea_derivatives_code_gen.loadLib();
+  
+  CodeGenABADerivatives<double> aba_derivatives_code_gen(model);
+  aba_derivatives_code_gen.initLib();
+  aba_derivatives_code_gen.loadLib();
+
+  se3::container::aligned_vector<VectorXd> qs     (NBT);
+  se3::container::aligned_vector<VectorXd> qdots  (NBT);
+  se3::container::aligned_vector<VectorXd> qddots (NBT);
+  se3::container::aligned_vector<VectorXd> taus (NBT);
+  
+  for(size_t i=0;i<NBT;++i)
+  {
+    qs[i]     = Eigen::VectorXd::Random(model.nq);
+    qs[i].segment<4>(3) /= qs[i].segment<4>(3).norm();
+    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)
+  {
+    rnea(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth]);
+  }
+  std::cout << "RNEA = \t\t"; timer.toc(std::cout,NBT);
+
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    rnea_code_gen.evalFunction(qs[_smooth],qdots[_smooth],qddots[_smooth]);
+  }
+  std::cout << "RNEA generated = \t\t"; timer.toc(std::cout,NBT);
+
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    rnea_code_gen.evalJacobian(qs[_smooth],qdots[_smooth],qddots[_smooth]);
+  }
+  std::cout << "RNEA partial derivatives auto diff + code gen = \t\t"; timer.toc(std::cout,NBT);
+  
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    rnea_derivatives_code_gen.evalFunction(qs[_smooth],qdots[_smooth],qddots[_smooth]);
+  }
+  std::cout << "RNEA partial derivatives code gen = \t\t"; timer.toc(std::cout,NBT);
+  
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    aba(model,data,qs[_smooth],qdots[_smooth],taus[_smooth]);
+  }
+  timer.toc();
+  
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    crba(model,data,qs[_smooth]);
+  }
+  std::cout << "CRBA = \t\t"; timer.toc(std::cout,NBT);
+  
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    crba_code_gen.evalFunction(qs[_smooth]);
+  }
+  std::cout << "CRBA generated = \t\t"; timer.toc(std::cout,NBT);
+  
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    computeMinverse(model,data,qs[_smooth]);
+  }
+  std::cout << "Minv = \t\t"; timer.toc(std::cout,NBT);
+  
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    minv_code_gen.evalFunction(qs[_smooth]);
+  }
+  std::cout << "Minv generated = \t\t"; timer.toc(std::cout,NBT);
+  
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    aba(model,data,qs[_smooth],qdots[_smooth],taus[_smooth]);
+  }
+  std::cout << "ABA = \t\t"; timer.toc(std::cout,NBT);
+
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    aba_code_gen.evalFunction(qs[_smooth],qdots[_smooth],taus[_smooth]);
+  }
+  std::cout << "ABA generated = \t\t"; timer.toc(std::cout,NBT);
+
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    aba_code_gen.evalJacobian(qs[_smooth],qdots[_smooth],taus[_smooth]);
+  }
+  std::cout << "ABA partial derivatives auto diff + code gen = \t\t"; timer.toc(std::cout,NBT);
+  
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    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);
+  
+  return 0;
+}

benchmark/timings-derivatives.cpp

@@ -15,11 +15,6 @@
 // Pinocchio If not, see
 // <http://www.gnu.org/licenses/>.
 
-#include "pinocchio/spatial/fwd.hpp"
-#include "pinocchio/spatial/se3.hpp"
-#include "pinocchio/multibody/visitor.hpp"
-#include "pinocchio/multibody/model.hpp"
-#include "pinocchio/multibody/data.hpp"
 #include "pinocchio/algorithm/joint-configuration.hpp"
 #include "pinocchio/algorithm/kinematics.hpp"
 #include "pinocchio/algorithm/kinematics-derivatives.hpp"
@@ -37,14 +32,19 @@
 
 #include "pinocchio/utils/timer.hpp"
 
+template<typename Matrix1, typename Matrix2, typename Matrix3>
 void rnea_fd(const se3::Model & model, se3::Data & data_fd,
              const Eigen::VectorXd & q,
              const Eigen::VectorXd & v,
              const Eigen::VectorXd & a,
-             Eigen::MatrixXd & drnea_dq,
-             Eigen::MatrixXd & drnea_dv,
-             Eigen::MatrixXd & drnea_da)
+             const Eigen::MatrixBase<Matrix1> & _drnea_dq,
+             const Eigen::MatrixBase<Matrix2> & _drnea_dv,
+             const Eigen::MatrixBase<Matrix3> & _drnea_da)
 {
+  Matrix1 & drnea_dq = EIGEN_CONST_CAST(Matrix1,_drnea_dq);
+  Matrix2 & drnea_dv = EIGEN_CONST_CAST(Matrix2,_drnea_dv);
+  Matrix3 & drnea_da = EIGEN_CONST_CAST(Matrix3,_drnea_da);
+  
   using namespace Eigen;
   VectorXd v_eps(VectorXd::Zero(model.nv));
   VectorXd q_plus(model.nq);
@@ -77,8 +77,8 @@ void rnea_fd(const se3::Model & model, se3::Data & data_fd,
   
   // dRNEA/da
   drnea_da = crba(model,data_fd,q);
-  drnea_da.triangularView<Eigen::StrictlyLower>()
-  = drnea_da.transpose().triangularView<Eigen::StrictlyLower>();
+  drnea_da.template triangularView<Eigen::StrictlyLower>()
+  = drnea_da.transpose().template triangularView<Eigen::StrictlyLower>();
   
 }
 
@@ -88,7 +88,7 @@ void aba_fd(const se3::Model & model, se3::Data & data_fd,
             const Eigen::VectorXd & tau,
             Eigen::MatrixXd & daba_dq,
             Eigen::MatrixXd & daba_dv,
-            se3::Data::RowMatrixXd & daba_dtau)
+            se3::Data::RowMatrixXs & daba_dtau)
 {
   using namespace Eigen;
   VectorXd v_eps(VectorXd::Zero(model.nv));
@@ -152,11 +152,10 @@ int main(int argc, const char ** argv)
     
   if( filename == "HS") 
     buildModels::humanoidRandom(model,true);
-  else if( filename == "H2" )
-    buildModels::humanoid2d(model);
   else
     if(with_ff)
       se3::urdf::buildModel(filename,JointModelFreeFlyer(),model);
+//      se3::urdf::buildModel(filename,JointModelRX(),model);
     else
       se3::urdf::buildModel(filename,model);
   std::cout << "nq = " << model.nq << std::endl;
@@ -180,14 +179,14 @@ int main(int argc, const char ** argv)
     qddots[i] = Eigen::VectorXd::Random(model.nv);
     taus[i] = Eigen::VectorXd::Random(model.nv);
   }
-
-  MatrixXd drnea_dq(MatrixXd::Zero(model.nv,model.nv));
-  MatrixXd drnea_dv(MatrixXd::Zero(model.nv,model.nv));
+  
+  EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXd) drnea_dq(MatrixXd::Zero(model.nv,model.nv));
+  EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXd) drnea_dv(MatrixXd::Zero(model.nv,model.nv));
   MatrixXd drnea_da(MatrixXd::Zero(model.nv,model.nv));
  
   MatrixXd daba_dq(MatrixXd::Zero(model.nv,model.nv));
   MatrixXd daba_dv(MatrixXd::Zero(model.nv,model.nv));
-  Data::RowMatrixXd daba_dtau(Data::RowMatrixXd::Zero(model.nv,model.nv));
+  Data::RowMatrixXs daba_dtau(Data::RowMatrixXs::Zero(model.nv,model.nv));
   
   timer.tic();
   SMOOTH(NBT)
@@ -195,14 +194,14 @@ int main(int argc, const char ** argv)
     forwardKinematics(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth]);
   }
   std::cout << "FK= \t\t"; timer.toc(std::cout,NBT);
-  
+
   timer.tic();
   SMOOTH(NBT)
   {
     computeForwardKinematicsDerivatives(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth]);
   }
   std::cout << "FK derivatives= \t\t"; timer.toc(std::cout,NBT);
-  
+
   timer.tic();
   SMOOTH(NBT)
   {
@@ -217,15 +216,15 @@ int main(int argc, const char ** argv)
                            drnea_dq,drnea_dv,drnea_da);
   }
   std::cout << "RNEA derivatives= \t\t"; timer.toc(std::cout,NBT);
-  
+
   timer.tic();
-  SMOOTH(NBT)
+  SMOOTH(NBT/100)
   {
     rnea_fd(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth],
             drnea_dq,drnea_dv,drnea_da);
   }
-  std::cout << "RNEA finite differences= \t\t"; timer.toc(std::cout,NBT);
-  
+  std::cout << "RNEA finite differences= \t\t"; timer.toc(std::cout,NBT/100);
+
   timer.tic();
   SMOOTH(NBT)
   {
@@ -248,7 +247,7 @@ int main(int argc, const char ** argv)
            daba_dq,daba_dv,daba_dtau);
   }
   std::cout << "ABA finite differences= \t\t"; timer.toc(std::cout,NBT);
-  
+
   timer.tic();
   SMOOTH(NBT)
   {

benchmark/timings.cpp

@@ -56,13 +56,25 @@ int main(int argc, const char ** argv)
 
   std::string filename = PINOCCHIO_SOURCE_DIR"/models/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") 
     se3::buildModels::humanoidRandom(model,true);
-  else if( filename == "H2" )
-    se3::buildModels::humanoid2d(model);
   else
-    se3::urdf::buildModel(filename,JointModelFreeFlyer(),model);
+    if(with_ff)
+      se3::urdf::buildModel(filename,JointModelFreeFlyer(),model);
+    else
+      se3::urdf::buildModel(filename,model);
   std::cout << "nq = " << model.nq << std::endl;
+  
+