Merge pull request #488 from jcarpent/topic/bench

Update benchmark

benchmark/CMakeLists.txt

 #
-# Copyright (c) 2015-2016 CNRS
+# Copyright (c) 2015-2018 CNRS
 #
 # This file is part of Pinocchio
 # pinocchio is free software: you can redistribute it
@@ -17,6 +17,8 @@
 # ----------------------------------------------------
 # --- BENCHMARK --------------------------------------
 # ----------------------------------------------------
+ADD_CUSTOM_TARGET(bench)
+
 MACRO(ADD_BENCH bench_name)
 IF(BUILD_BENCHMARK)
   ADD_EXECUTABLE(${bench_name} ${bench_name}.cpp)
@@ -33,6 +35,8 @@ IF(NumExtraMacroArgs GREATER 0)
     PKG_CONFIG_USE_DEPENDENCY(${bench_name} eigen3)
   ENDIF(link_to_main_lib)
 ENDIF()
+
+ADD_DEPENDENCIES(bench ${bench_name})
 ENDMACRO(ADD_BENCH)
 
 MACRO(ADD_TEST_CFLAGS target flag)

benchmark/timings-derivatives.cpp

@@ -21,6 +21,8 @@
 #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"
 #include "pinocchio/algorithm/rnea-derivatives.hpp"
 #include "pinocchio/algorithm/aba-derivatives.hpp"
 #include "pinocchio/algorithm/aba.hpp"
@@ -169,6 +171,7 @@ int main(int argc, const char ** argv)
   container::aligned_vector<VectorXd> qdots  (NBT);
   container::aligned_vector<VectorXd> qddots (NBT);
   container::aligned_vector<VectorXd> taus (NBT);
+  
   for(size_t i=0;i<NBT;++i)
   {
     qs[i]     = Eigen::VectorXd::Random(model.nq);
@@ -186,6 +189,20 @@ int main(int argc, const char ** argv)
   MatrixXd daba_dv(MatrixXd::Zero(model.nv,model.nv));
   Data::RowMatrixXd daba_dtau(Data::RowMatrixXd::Zero(model.nv,model.nv));
   
+  timer.tic();
+  SMOOTH(NBT)
+  {
+    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)
   {

cmake @ f7328a51

-Subproject commit d22de8c53c3507df785b1fb3ab260d9fdfb65233
+Subproject commit f7328a51032f970f64895770af719a6d0d775ef7

src/algorithm/kinematics-derivatives.hpp

@@ -59,8 +59,8 @@ namespace se3
   inline void getJointVelocityDerivatives(const Model & model,
                                           Data & data,
                                           const Model::JointIndex jointId,
-                                          Data::Matrix6x & partial_dq,
-                                          Data::Matrix6x & partial_dv);
+                                          Data::Matrix6x & v_partial_dq,
+                                          Data::Matrix6x & v_partial_dv);
   
   ///
   /// \brief Computes the partial derivaties of the spatial acceleration of a given with respect to

src/algorithm/kinematics-derivatives.hxx

@@ -121,8 +121,8 @@ namespace se3
                      se3::Data & data,
                      const SE3 & oMlast,
                      const Motion & vlast,
-                     Data::Matrix6x & partial_dq,
-                     Data::Matrix6x & partial_dv)
+                     Data::Matrix6x & v_partial_dq,
+                     Data::Matrix6x & v_partial_dv)
     {
       const Model::JointIndex & i = jmodel.id();
       const Model::JointIndex & parent = model.parents[i];
@@ -132,28 +132,28 @@ namespace se3
       ColsBlock Jcols = jmodel.jointCols(data.J);
       
       // dvec/dv
-      ColsBlock partial_dv_cols = jmodel.jointCols(partial_dv);
+      ColsBlock v_partial_dv_cols = jmodel.jointCols(v_partial_dv);
       if(rf == WORLD)
-        partial_dv_cols = Jcols;
+        v_partial_dv_cols = Jcols;
       else
-        motionSet::se3ActionInverse(oMlast,Jcols,partial_dv_cols);
+        motionSet::se3ActionInverse(oMlast,Jcols,v_partial_dv_cols);
 
       // dvec/dq
-      ColsBlock partial_dq_cols = jmodel.jointCols(partial_dq);
+      ColsBlock v_partial_dq_cols = jmodel.jointCols(v_partial_dq);
       if(rf == WORLD)
       {
         if(parent > 0)
           vtmp = data.ov[parent] - vlast;
         else
           vtmp = -vlast;
-        motionSet::motionAction(vtmp,Jcols,partial_dq_cols);
+        motionSet::motionAction(vtmp,Jcols,v_partial_dq_cols);
       }
       else
       {
         if(parent > 0)
         {
           vtmp = oMlast.actInv(data.ov[parent]);
-          motionSet::motionAction(vtmp,partial_dv_cols,partial_dq_cols);
+          motionSet::motionAction(vtmp,v_partial_dv_cols,v_partial_dq_cols);
         }
       }
       
@@ -166,11 +166,11 @@ namespace se3
   inline void getJointVelocityDerivatives(const Model & model,
                                           Data & data,
                                           const Model::JointIndex jointId,
-                                          Data::Matrix6x & partial_dq,
-                                          Data::Matrix6x & partial_dv)
+                                          Data::Matrix6x & v_partial_dq,
+                                          Data::Matrix6x & v_partial_dv)
   {
-    assert( partial_dq.cols() ==  model.nv );
-    assert( partial_dv.cols() ==  model.nv );
+    assert( v_partial_dq.cols() ==  model.nv );
+    assert( v_partial_dv.cols() ==  model.nv );
     assert(model.check(data) && "data is not consistent with model.");
     
     const SE3 & oMlast = data.oMi[jointId];
@@ -181,10 +181,11 @@ namespace se3
       JointVelocityDerivativesBackwardStep<rf>::run(model.joints[i],
                                                     typename JointVelocityDerivativesBackwardStep<rf>::ArgsType(model,data,
                                                                                                                 oMlast,vlast,
-                                                                                                                partial_dq,
-                                                                                                                partial_dv));
+                                                                                                                v_partial_dq,
+                                                                                                                v_partial_dv));
     }
   
+    // Set back ov[0] to a zero value
     data.ov[0].setZero();
   }