[Bench] Add bench for derivatives algo

benchmark/CMakeLists.txt

@@ -42,9 +42,12 @@ ENDMACRO(ADD_TEST_CFLAGS)
 # 
 ADD_BENCH(timings TRUE)
 
-# timings-eigen
+# timings derivatives
 # 
+ADD_BENCH(timings-derivatives TRUE)
+TARGET_LINK_LIBRARIES(timings-derivatives ${PROJECT_NAME})
 
+# timings-eigen
 # 
 ADD_BENCH(timings-eigen)
 

benchmark/timings-derivatives.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/spatial/fwd.hpp"
+#include "pinocchio/spatial/se3.hpp"
+#include "pinocchio/multibody/visitor.hpp"
+#include "pinocchio/multibody/model.hpp"
+#include "pinocchio/algorithm/rnea-derivatives.hpp"
+#include "pinocchio/parsers/urdf.hpp"
+#include "pinocchio/parsers/sample-models.hpp"
+#include "pinocchio/container/aligned-vector.hpp"
+
+#include <iostream>
+
+#include "pinocchio/tools/timer.hpp"
+
+int main(int argc, const char ** argv)
+{
+  using namespace Eigen;
+  using namespace se3;
+
+  StackTicToc timer(StackTicToc::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
+    
+  Model model;
+
+  std::string filename = PINOCCHIO_SOURCE_DIR"/models/simple_humanoid.urdf";
+  if(argc>1) filename = argv[1];
+  if( filename == "HS") 
+    buildModels::humanoidSimple(model,true);
+  else if( filename == "H2" )
+    buildModels::humanoid2d(model);
+  else
+    se3::urdf::buildModel(filename,JointModelFreeFlyer(),model);
+  std::cout << "nq = " << model.nq << std::endl;
+  std::cout << "nv = " << model.nv << std::endl;
+
+  Data data(model);
+  VectorXd q = VectorXd::Random(model.nq);
+  VectorXd qdot = VectorXd::Random(model.nv);
+  VectorXd qddot = VectorXd::Random(model.nv);
+
+  container::aligned_vector<VectorXd> qs     (NBT);
+  container::aligned_vector<VectorXd> qdots  (NBT);
+  container::aligned_vector<VectorXd> qddots (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);
+  }
+
+  MatrixXd drnea_dq(MatrixXd::Zero(model.nv,model.nv));
+  MatrixXd drnea_dv(MatrixXd::Zero(model.nv,model.nv));
+ 
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    computeRNEADerivatives(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth],drnea_dq,drnea_dv);
+  }
+  std::cout << "RNEA derivatives= \t\t"; timer.toc(std::cout,NBT);
+
+  std::cout << "--" << std::endl;
+  return 0;
+}