[benchmark] Fix benchmark random configuration

benchmark/timings-cg.cpp

@@ -2,6 +2,7 @@
 // Copyright (c) 2018 CNRS
 //
 
+#include "pinocchio/algorithm/joint-configuration.hpp"
 #include "pinocchio/algorithm/crba.hpp"
 #include "pinocchio/algorithm/centroidal.hpp"
 #include "pinocchio/algorithm/aba.hpp"
@@ -60,6 +61,7 @@ int main(int argc, const char ** argv)
   std::cout << "--" << std::endl;
 
   pinocchio::Data data(model);
+  VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
   
   CodeGenRNEA<double> rnea_code_gen(model);
   rnea_code_gen.initLib();
@@ -92,8 +94,7 @@ int main(int argc, const char ** argv)
   
   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();
+    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);

benchmark/timings-cholesky.cpp

@@ -7,6 +7,7 @@
 #include "pinocchio/multibody/visitor.hpp"
 #include "pinocchio/multibody/model.hpp"
 #include "pinocchio/multibody/data.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
 #include "pinocchio/algorithm/crba.hpp"
 #include "pinocchio/algorithm/aba.hpp"
 #include "pinocchio/algorithm/cholesky.hpp"
@@ -46,9 +47,7 @@ int main(int argc, const char ** argv)
   std::cout << "nq = " << model.nq << std::endl;
 
   pinocchio::Data data(model);
-  VectorXd q = VectorXd::Random(model.nq);
-  VectorXd qdot = VectorXd::Random(model.nv);
-  VectorXd qddot = VectorXd::Random(model.nv);
+  VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
   
   MatrixXd A(model.nv,model.nv), B(model.nv,model.nv);
   A.setZero(); B.setRandom();
@@ -58,7 +57,7 @@ int main(int argc, const char ** argv)
   std::vector<VectorXd> rhs (NBT);
   for(size_t i=0;i<NBT;++i)
   {
-    qs[i] = Eigen::VectorXd::Random(model.nq);
+    qs[i] = randomConfiguration(model,-qmax,qmax);
     lhs[i] = Eigen::VectorXd::Zero(model.nv);
     rhs[i] = Eigen::VectorXd::Random(model.nv);
   }
@@ -145,7 +144,7 @@ int main(int argc, const char ** argv)
   {
     computeMinverse(model,data,qs[_smooth]);
   }
-  std::cout << "M.inverse() = \t\t"; timer.toc(std::cout,NBT);
+  std::cout << "computeMinverse = \t\t"; timer.toc(std::cout,NBT);
   
   return 0;
 }

benchmark/timings-derivatives.cpp

@@ -149,9 +149,7 @@ int main(int argc, const char ** argv)
   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);
+  VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
 
   container::aligned_vector<VectorXd> qs     (NBT);
   container::aligned_vector<VectorXd> qdots  (NBT);
@@ -160,8 +158,7 @@ int main(int argc, const char ** argv)
   
   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();
+    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);

benchmark/timings-geometry.cpp

@@ -7,6 +7,7 @@
 #include "pinocchio/multibody/visitor.hpp"
 #include "pinocchio/multibody/model.hpp"
 #include "pinocchio/multibody/data.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
 #include "pinocchio/algorithm/crba.hpp"
 #include "pinocchio/algorithm/rnea.hpp"
 #include "pinocchio/algorithm/cholesky.hpp"
@@ -42,7 +43,7 @@ int main()
   
   std::string romeo_filename = PINOCCHIO_SOURCE_DIR"/models/romeo/romeo_description/urdf/romeo.urdf";
   std::vector < std::string > package_dirs;
-  std::string romeo_meshDir  = PINOCCHIO_SOURCE_DIR"/models/";
+  std::string romeo_meshDir  = PINOCCHIO_SOURCE_DIR"/models/romeo";
   package_dirs.push_back(romeo_meshDir);
 
   pinocchio::Model model;
@@ -54,6 +55,7 @@ int main()
    
   Data data(model);
   GeometryData geom_data(geom_model);
+  VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
 
 
   std::vector<VectorXd> qs_romeo     (NBT);
@@ -61,8 +63,7 @@ int main()
   std::vector<VectorXd> qddots_romeo (NBT);
   for(size_t i=0;i<NBT;++i)
   {
-    qs_romeo[i]     = Eigen::VectorXd::Random(model.nq);
-    qs_romeo[i].segment<4>(3) /= qs_romeo[i].segment<4>(3).norm();
+    qs_romeo[i]     = randomConfiguration(model,-qmax,qmax);
     qdots_romeo[i]  = Eigen::VectorXd::Random(model.nv);
     qddots_romeo[i] = Eigen::VectorXd::Random(model.nv);
   }

benchmark/timings.cpp

@@ -7,6 +7,7 @@
 #include "pinocchio/multibody/visitor.hpp"
 #include "pinocchio/multibody/model.hpp"
 #include "pinocchio/multibody/data.hpp"
+#include "pinocchio/algorithm/joint-configuration.hpp"
 #include "pinocchio/algorithm/crba.hpp"
 #include "pinocchio/algorithm/centroidal.hpp"
 #include "pinocchio/algorithm/aba.hpp"
@@ -64,17 +65,14 @@ int main(int argc, const char ** argv)
   
 
   pinocchio::Data data(model);
-  VectorXd q = VectorXd::Random(model.nq);
-  VectorXd qdot = VectorXd::Random(model.nv);
-  VectorXd qddot = VectorXd::Random(model.nv);
+  VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
 
   std::vector<VectorXd> qs     (NBT);
   std::vector<VectorXd> qdots  (NBT);
   std::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();
+      qs[i]     = randomConfiguration(model,-qmax,qmax);
       qdots[i]  = Eigen::VectorXd::Random(model.nv);
       qddots[i] = Eigen::VectorXd::Random(model.nv);
     }