timings-cg.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2018 CNRS
3 //
4 
5 #include "pinocchio/codegen/cppadcg.hpp"
6 
7 #include "pinocchio/algorithm/joint-configuration.hpp"
8 #include "pinocchio/algorithm/crba.hpp"
9 #include "pinocchio/algorithm/centroidal.hpp"
10 #include "pinocchio/algorithm/aba.hpp"
11 #include "pinocchio/algorithm/rnea.hpp"
12 #include "pinocchio/algorithm/cholesky.hpp"
13 #include "pinocchio/algorithm/jacobian.hpp"
14 #include "pinocchio/algorithm/center-of-mass.hpp"
15 #include "pinocchio/algorithm/compute-all-terms.hpp"
16 #include "pinocchio/algorithm/kinematics.hpp"
17 #include "pinocchio/parsers/urdf.hpp"
18 #include "pinocchio/parsers/sample-models.hpp"
19 #include "pinocchio/container/aligned-vector.hpp"
20 
21 #include "pinocchio/codegen/code-generator-algo.hpp"
22 
23 #include <iostream>
24 
25 #include "pinocchio/utils/timer.hpp"
26 
27 int main(int argc, const char ** argv)
28 {
29  using namespace Eigen;
30  using namespace pinocchio;
31 
33  #ifdef NDEBUG
34  const int NBT = 1000*100;
35  #else
36  const int NBT = 1;
37  std::cout << "(the time score in debug mode is not relevant) " << std::endl;
38  #endif
39 
41  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
42  if(argc>1) filename = argv[1];
43 
44  bool with_ff = true;
45  if(argc>2)
46  {
47  const std::string ff_option = argv[2];
48  if(ff_option == "-no-ff")
49  with_ff = false;
50  }
51 
52  if( filename == "H")
54  else
55  if(with_ff)
57  else
58  pinocchio::urdf::buildModel(filename,model);
59 
60  std::cout << "nq = " << model.nq << std::endl;
61  std::cout << "nv = " << model.nv << std::endl;
62  std::cout << "--" << std::endl;
63 
64  pinocchio::Data data(model);
65  VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
66 
67  CodeGenRNEA<double> rnea_code_gen(model);
68  rnea_code_gen.initLib();
69  rnea_code_gen.loadLib();
70 
71  CodeGenABA<double> aba_code_gen(model);
72  aba_code_gen.initLib();
73  aba_code_gen.loadLib();
74 
75  CodeGenCRBA<double> crba_code_gen(model);
76  crba_code_gen.initLib();
77  crba_code_gen.loadLib();
78 
79  CodeGenMinv<double> minv_code_gen(model);
80  minv_code_gen.initLib();
81  minv_code_gen.loadLib();
82 
83  CodeGenRNEADerivatives<double> rnea_derivatives_code_gen(model);
84  rnea_derivatives_code_gen.initLib();
85  rnea_derivatives_code_gen.loadLib();
86 
87  CodeGenABADerivatives<double> aba_derivatives_code_gen(model);
88  aba_derivatives_code_gen.initLib();
89  aba_derivatives_code_gen.loadLib();
90 
95 
96  for(size_t i=0;i<NBT;++i)
97  {
98  qs[i] = randomConfiguration(model,-qmax,qmax);
99  qdots[i] = Eigen::VectorXd::Random(model.nv);
100  qddots[i] = Eigen::VectorXd::Random(model.nv);
101  taus[i] = Eigen::VectorXd::Random(model.nv);
102  }
103 
104  timer.tic();
105  SMOOTH(NBT)
106  {
107  rnea(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth]);
108  }
109  std::cout << "RNEA = \t\t"; timer.toc(std::cout,NBT);
110 
111  timer.tic();
112  SMOOTH(NBT)
113  {
114  rnea_code_gen.evalFunction(qs[_smooth],qdots[_smooth],qddots[_smooth]);
115  }
116  std::cout << "RNEA generated = \t\t"; timer.toc(std::cout,NBT);
117 
118  timer.tic();
119  SMOOTH(NBT)
120  {
121  rnea_code_gen.evalJacobian(qs[_smooth],qdots[_smooth],qddots[_smooth]);
122  }
123  std::cout << "RNEA partial derivatives auto diff + code gen = \t\t"; timer.toc(std::cout,NBT);
124 
125  timer.tic();
126  SMOOTH(NBT)
127  {
128  rnea_derivatives_code_gen.evalFunction(qs[_smooth],qdots[_smooth],qddots[_smooth]);
129  }
130  std::cout << "RNEA partial derivatives code gen = \t\t"; timer.toc(std::cout,NBT);
131 
132  timer.tic();
133  SMOOTH(NBT)
134  {
135  aba(model,data,qs[_smooth],qdots[_smooth],taus[_smooth]);
136  }
137  timer.toc();
138 
139  timer.tic();
140  SMOOTH(NBT)
141  {
142  crba(model,data,qs[_smooth]);
143  }
144  std::cout << "CRBA = \t\t"; timer.toc(std::cout,NBT);
145 
146  timer.tic();
147  SMOOTH(NBT)
148  {
149  crba_code_gen.evalFunction(qs[_smooth]);
150  }
151  std::cout << "CRBA generated = \t\t"; timer.toc(std::cout,NBT);
152 
153  timer.tic();
154  SMOOTH(NBT)
155  {
156  computeMinverse(model,data,qs[_smooth]);
157  }
158  std::cout << "Minv = \t\t"; timer.toc(std::cout,NBT);
159 
160  timer.tic();
161  SMOOTH(NBT)
162  {
163  minv_code_gen.evalFunction(qs[_smooth]);
164  }
165  std::cout << "Minv generated = \t\t"; timer.toc(std::cout,NBT);
166 
167  timer.tic();
168  SMOOTH(NBT)
169  {
170  aba(model,data,qs[_smooth],qdots[_smooth],taus[_smooth]);
171  }
172  std::cout << "ABA = \t\t"; timer.toc(std::cout,NBT);
173 
174  timer.tic();
175  SMOOTH(NBT)
176  {
177  aba_code_gen.evalFunction(qs[_smooth],qdots[_smooth],taus[_smooth]);
178  }
179  std::cout << "ABA generated = \t\t"; timer.toc(std::cout,NBT);
180 
181  timer.tic();
182  SMOOTH(NBT)
183  {
184  aba_code_gen.evalJacobian(qs[_smooth],qdots[_smooth],taus[_smooth]);
185  }
186  std::cout << "ABA partial derivatives auto diff + code gen = \t\t"; timer.toc(std::cout,NBT);
187 
188  timer.tic();
189  SMOOTH(NBT)
190  {
191  aba_derivatives_code_gen.evalFunction(qs[_smooth],qdots[_smooth],qddots[_smooth]);
192  }
193  std::cout << "ABA partial derivatives code gen = \t\t"; timer.toc(std::cout,NBT);
194 
195  return 0;
196 }
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & aba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
double toc()
Definition: timer.hpp:68
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
const DataTpl< Scalar, Options, JointCollectionTpl >::RowMatrixXs & computeMinverse(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the inverse of the joint space inertia matrix using Articulated Body formulation.
void rnea(const int num_threads, ModelPoolTpl< Scalar, Options, JointCollectionTpl > &pool, const Eigen::MatrixBase< ConfigVectorPool > &q, const Eigen::MatrixBase< TangentVectorPool1 > &v, const Eigen::MatrixBase< TangentVectorPool2 > &a, const Eigen::MatrixBase< TangentVectorPool3 > &tau)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
void evalFunction(const Eigen::MatrixBase< ConfigVectorType > &q)
void randomConfiguration(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &lowerLimits, const Eigen::MatrixBase< ConfigVectorIn2 > &upperLimits, const Eigen::MatrixBase< ReturnType > &qout)
Generate a configuration vector uniformly sampled among provided limits.
#define PINOCCHIO_MODEL_DIR
void tic()
Definition: timer.hpp:63
void evalFunction(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVector1 > &v, const Eigen::MatrixBase< TangentVector2 > &a)
void loadLib(const bool generate_if_not_exist=true)
Specialization of an std::vector with an aligned allocator. This specialization might be used when th...
void evalFunction(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVector1 > &v, const Eigen::MatrixBase< TangentVector2 > &tau)
void evalFunction(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVector1 > &v, const Eigen::MatrixBase< TangentVector2 > &a)
void evalJacobian(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVector1 > &v, const Eigen::MatrixBase< TangentVector2 > &tau)
#define SMOOTH(s)
Definition: timer.hpp:38
ModelTpl< Scalar, Options, JointCollectionTpl > & buildModel(const std::string &filename, const typename ModelTpl< Scalar, Options, JointCollectionTpl >::JointModel &rootJoint, ModelTpl< Scalar, Options, JointCollectionTpl > &model, const bool verbose=false)
Build the model from a URDF file with a particular joint as root of the model tree inside the model g...
void evalFunction(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVector1 > &v, const Eigen::MatrixBase< TangentVector2 > &tau)
Main pinocchio namespace.
Definition: timings.cpp:28
int nv
Dimension of the velocity vector space.
filename
JointModelFreeFlyerTpl< double > JointModelFreeFlyer
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
Definition: conversions.cpp:14
void evalFunction(const Eigen::MatrixBase< ConfigVectorType > &q)
int nq
Dimension of the configuration vector representation.
void evalJacobian(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVector1 > &v, const Eigen::MatrixBase< TangentVector2 > &a)
int main(int argc, const char **argv)
Definition: timings-cg.cpp:27


pinocchio
Author(s):
autogenerated on Fri Jun 23 2023 02:38:33