timings.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2020 CNRS INRIA
3 //
4 
5 #include "pinocchio/multibody/model.hpp"
6 #include "pinocchio/multibody/data.hpp"
7 
8 #include "pinocchio/algorithm/joint-configuration.hpp"
9 #include "pinocchio/algorithm/frames.hpp"
10 #include "pinocchio/algorithm/crba.hpp"
11 #include "pinocchio/algorithm/centroidal.hpp"
12 #include "pinocchio/algorithm/aba.hpp"
13 #include "pinocchio/algorithm/rnea.hpp"
14 #include "pinocchio/algorithm/cholesky.hpp"
15 #include "pinocchio/algorithm/jacobian.hpp"
16 #include "pinocchio/algorithm/center-of-mass.hpp"
17 #include "pinocchio/algorithm/compute-all-terms.hpp"
18 #include "pinocchio/algorithm/kinematics.hpp"
19 
20 #include "pinocchio/parsers/urdf.hpp"
21 #include "pinocchio/parsers/sample-models.hpp"
22 
23 #include <iostream>
24 
25 #include "pinocchio/utils/timer.hpp"
26 
27 
28 namespace pinocchio
29 {
30  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
32  : fusion::JointUnaryVisitorBase< EmptyForwardStepUnaryVisit<Scalar,Options,JointCollectionTpl> >
33  {
36 
38 
39  template<typename JointModel>
40  static void algo(const JointModelBase<JointModel> &,
42  )
43  { // do nothing
44  }
45 
46  };
47 
48  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
51  {
52  assert(model.check(data) && "data is not consistent with model.");
53 
56 
57  for(JointIndex i=1; i < (JointIndex)model.njoints; ++i)
58  {
59  Algo::run(model.joints[i],data.joints[i]);
60  }
61  }
62 
63  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
65  : fusion::JointUnaryVisitorBase< EmptyForwardStepUnaryVisitNoData<Scalar,Options,JointCollectionTpl> >
66  {
69 
71 
72  template<typename JointModel>
73  EIGEN_DONT_INLINE
74  static void algo(const JointModelBase<JointModel> &)
75  { // do nothing
76  }
77 
78  };
79 
80  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
83  {
85  assert(model.check(data) && "data is not consistent with model.");
86 
89 
90  for(JointIndex i=1; i < (JointIndex)model.njoints; ++i)
91  {
92  Algo::run(model.joints[i]);
93  }
94  }
95 
96  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
98  : fusion::JointBinaryVisitorBase< EmptyForwardStepBinaryVisit<Scalar,Options,JointCollectionTpl> >
99  {
102 
104 
105  template<typename JointModel1, typename JointModel2>
106  EIGEN_DONT_INLINE
107  static void algo(const JointModelBase<JointModel1> &,
111  { // do nothing
112  }
113 
114  };
115 
116  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
119  {
120  assert(model.check(data) && "data is not consistent with model.");
121 
124 
125  for(JointIndex i=1; i < (JointIndex)model.njoints; ++i)
126  {
127  Algo::run(model.joints[i],model.joints[i],
128  data.joints[i],data.joints[i]);
129  }
130  }
131 
132  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
134  : fusion::JointBinaryVisitorBase< EmptyForwardStepBinaryVisitNoData<Scalar,Options,JointCollectionTpl> >
135  {
138 
140 
141  template<typename JointModel1, typename JointModel2>
142  EIGEN_DONT_INLINE
143  static void algo(const JointModelBase<JointModel1> &,
145  { // do nothing
146  }
147 
148  };
149 
150  template<typename Scalar, int Options, template<typename,int> class JointCollectionTpl>
153  {
155  assert(model.check(data) && "data is not consistent with model.");
156 
159 
160  for(JointIndex i=1; i < (JointIndex)model.njoints; ++i)
161  {
162  Algo::run(model.joints[i],model.joints[i]);
163  }
164  }
165 }
166 
167 int main(int argc, const char ** argv)
168 {
169  using namespace Eigen;
170  using namespace pinocchio;
171 
173  #ifdef NDEBUG
174  const int NBT = 1000*100;
175  #else
176  const int NBT = 1;
177  std::cout << "(the time score in debug mode is not relevant) " << std::endl;
178  #endif
179 
181 
182  std::string filename = PINOCCHIO_MODEL_DIR + std::string("/simple_humanoid.urdf");
183  if(argc>1) filename = argv[1];
184 
185  bool with_ff = true;
186  if(argc>2)
187  {
188  const std::string ff_option = argv[2];
189  if(ff_option == "-no-ff")
190  with_ff = false;
191  }
192 
193  if( filename == "HS")
195  else
196  if(with_ff)
198  else
199  pinocchio::urdf::buildModel(filename,model);
200  std::cout << "nq = " << model.nq << std::endl;
201 
202 
203 
204  pinocchio::Data data(model);
205  const VectorXd qmax = Eigen::VectorXd::Ones(model.nq);
206 
210  for(size_t i=0;i<NBT;++i)
211  {
212  qs[i] = randomConfiguration(model,-qmax,qmax);
213  qdots[i] = Eigen::VectorXd::Random(model.nv);
214  qddots[i] = Eigen::VectorXd::Random(model.nv);
215  }
216 
217 
218  timer.tic();
219  SMOOTH(NBT)
220  {
221  rnea(model,data,qs[_smooth],qdots[_smooth],qddots[_smooth]);
222  }
223  std::cout << "RNEA = \t\t"; timer.toc(std::cout,NBT);
224 
225  timer.tic();
226  SMOOTH(NBT)
227  {
228  nonLinearEffects(model,data,qs[_smooth],qdots[_smooth]);
229  }
230  std::cout << "NLE = \t\t"; timer.toc(std::cout,NBT);
231 
232  timer.tic();
233  SMOOTH(NBT)
234  {
235  rnea(model,data,qs[_smooth],qdots[_smooth],Eigen::VectorXd::Zero(model.nv));
236  }
237  std::cout << "NLE via RNEA = \t\t"; timer.toc(std::cout,NBT);
238 
239  timer.tic();
240  SMOOTH(NBT)
241  {
242  crba(model,data,qs[_smooth]);
243  }
244  std::cout << "CRBA = \t\t"; timer.toc(std::cout,NBT);
245 
246  timer.tic();
247  SMOOTH(NBT)
248  {
249  crbaMinimal(model,data,qs[_smooth]);
250  }
251  std::cout << "CRBA minimal = \t\t"; timer.toc(std::cout,NBT);
252 
253  timer.tic();
254  SMOOTH(NBT)
255  {
256  computeAllTerms(model,data,qs[_smooth],qdots[_smooth]);
257  }
258  std::cout << "computeAllTerms = \t\t"; timer.toc(std::cout,NBT);
259 
260  double total = 0;
261  SMOOTH(NBT)
262  {
263  crba(model,data,qs[_smooth]);
264  timer.tic();
265  cholesky::decompose(model,data);
266  total += timer.toc(timer.DEFAULT_UNIT);
267  }
268  std::cout << "Branch Induced Sparsity Cholesky = \t" << (total/NBT)
269  << " " << timer.unitName(timer.DEFAULT_UNIT) <<std::endl;
270 
271  total = 0;
272  Eigen::LDLT<Eigen::MatrixXd> Mldlt(data.M);
273  SMOOTH(NBT)
274  {
275  crba(model,data,qs[_smooth]);
276  data.M.triangularView<Eigen::StrictlyLower>()
277  = data.M.transpose().triangularView<Eigen::StrictlyLower>();
278  timer.tic();
279  Mldlt.compute(data.M);
280  total += timer.toc(timer.DEFAULT_UNIT);
281  }
282  std::cout << "Dense Eigen Cholesky = \t" << (total/NBT)
283  << " " << timer.unitName(timer.DEFAULT_UNIT) <<std::endl;
284 
285  timer.tic();
286  SMOOTH(NBT)
287  {
288  computeJointJacobians(model,data,qs[_smooth]);
289  }
290  std::cout << "Jacobian = \t"; timer.toc(std::cout,NBT);
291 
292  timer.tic();
293  SMOOTH(NBT)
294  {
295  computeJointJacobiansTimeVariation(model,data,qs[_smooth],qdots[_smooth]);
296  }
297  std::cout << "Jacobian Time Variation = \t"; timer.toc(std::cout,NBT);
298 
299  timer.tic();
300  SMOOTH(NBT)
301  {
302  jacobianCenterOfMass(model,data,qs[_smooth],true);
303  }
304  std::cout << "COM+Jcom = \t"; timer.toc(std::cout,NBT);
305 
306  timer.tic();
307  SMOOTH(NBT)
308  {
309  centerOfMass(model,data,qs[_smooth], qdots[_smooth], qddots[_smooth], true);
310  }
311  std::cout << "COM+vCOM+aCOM = \t"; timer.toc(std::cout,NBT);
312 
313  timer.tic();
314  SMOOTH(NBT)
315  {
316  forwardKinematics(model,data,qs[_smooth]);
317  }
318  std::cout << "Zero Order Kinematics = \t"; timer.toc(std::cout,NBT);
319 
320 
321  timer.tic();
322  SMOOTH(NBT)
323  {
324  forwardKinematics(model,data,qs[_smooth],qdots[_smooth]);
325  }
326  std::cout << "First Order Kinematics = \t"; timer.toc(std::cout,NBT);
327 
328  timer.tic();
329  SMOOTH(NBT)
330  {
331  forwardKinematics(model,data,qs[_smooth],qdots[_smooth], qddots[_smooth]);
332  }
333  std::cout << "Second Order Kinematics = \t"; timer.toc(std::cout,NBT);
334 
335  total = 0.;
336  SMOOTH(NBT)
337  {
338  forwardKinematics(model,data,qs[_smooth]);
339  timer.tic();
340  updateFramePlacements(model, data);
341  total += timer.toc(timer.DEFAULT_UNIT);
342  }
343  std::cout << "Update Frame Placement = \t" << (total/NBT)
344  << " " << timer.unitName(timer.DEFAULT_UNIT) << std::endl;
345 
346 
347  timer.tic();
348  SMOOTH(NBT)
349  {
350  framesForwardKinematics(model,data, qs[_smooth]);
351  }
352  std::cout << "Zero Order Frames Kinematics = \t"; timer.toc(std::cout,NBT);
353 
354  timer.tic();
355  SMOOTH(NBT)
356  {
357  ccrba(model,data,qs[_smooth],qdots[_smooth]);
358  }
359  std::cout << "CCRBA = \t"; timer.toc(std::cout,NBT);
360 
361  timer.tic();
362  SMOOTH(NBT)
363  {
364  aba(model,data,qs[_smooth],qdots[_smooth], qddots[_smooth]);
365  }
366  std::cout << "ABA = \t"; timer.toc(std::cout,NBT);
367 
368  timer.tic();
369  SMOOTH(NBT)
370  {
371  emptyForwardPassUnaryVisit(model,data);
372  }
373  std::cout << "Empty Forward Pass Unary visit (no computations) = \t"; timer.toc(std::cout,NBT);
374 
375  timer.tic();
376  SMOOTH(NBT)
377  {
379  }
380  std::cout << "Empty Forward Pass Unary visit No Data (no computations) = \t"; timer.toc(std::cout,NBT);
381 
382  timer.tic();
383  SMOOTH(NBT)
384  {
385  emptyForwardPassBinaryVisit(model,data);
386  }
387  std::cout << "Empty Forward Pass Binary visit (no computations) = \t"; timer.toc(std::cout,NBT);
388 
389  timer.tic();
390  SMOOTH(NBT)
391  {
393  }
394  std::cout << "Empty Forward Pass Binary visit No Data (no computations) = \t"; timer.toc(std::cout,NBT);
395 
396  timer.tic();
397  SMOOTH(NBT)
398  {
399  computeCoriolisMatrix(model,data,qs[_smooth],qdots[_smooth]);
400  }
401  std::cout << "Coriolis Matrix = \t"; timer.toc(std::cout,NBT);
402 
403  timer.tic();
404  SMOOTH(NBT)
405  {
406  computeMinverse(model,data,qs[_smooth]);
407  }
408  std::cout << "Minv = \t"; timer.toc(std::cout,NBT);
409 
410  std::cout << "--" << std::endl;
411  return 0;
412 }
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...
ModelTpl< Scalar, Options, JointCollectionTpl > Model
Definition: timings.cpp:67
ModelTpl< Scalar, Options, JointCollectionTpl > Model
Definition: timings.cpp:100
void computeAllTerms(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes efficiently all the terms needed for dynamic simulation. It is equivalent to the call at the...
void emptyForwardPassUnaryVisit(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Definition: timings.cpp:49
double toc()
Definition: timer.hpp:68
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix3x & jacobianCenterOfMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const bool computeSubtreeComs=true)
Computes both the jacobian and the the center of mass position of a given model according to a partic...
typedef PINOCCHIO_ALIGNED_STD_VECTOR(JointData) JointDataVector
Unit DEFAULT_UNIT
Definition: timer.hpp:50
JointModelVector joints
Model of joint i, encapsulated in a JointModelAccessor.
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...
int main(int argc, const char **argv)
Definition: timings.cpp:167
#define PINOCCHIO_UNUSED_VARIABLE(var)
Helper to declare that a parameter is unused.
Definition: src/macros.hpp:51
DataTpl< Scalar, Options, JointCollectionTpl > Data
Definition: timings.cpp:68
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...
Base structure for Binary visitation of two JointModels. This structure provides runners to call the ...
DataTpl< Scalar, Options, JointCollectionTpl > Data
Definition: timings.cpp:137
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & decompose(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Compute the Cholesky decomposition of the joint space inertia matrix M contained in data...
int njoints
Number of joints.
static std::string unitName(Unit u)
Definition: timer.hpp:52
JointDataVector joints
Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model, encapsulated in JointDataAccessor.
DataTpl< Scalar, Options, JointCollectionTpl > Data
Definition: timings.cpp:35
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
static EIGEN_DONT_INLINE void algo(const JointModelBase< JointModel > &)
Definition: timings.cpp:74
void emptyForwardPassUnaryVisitNoData(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Definition: timings.cpp:81
void forwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Update the joint placements according to the current joint configuration.
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & ccrba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the Centroidal Momentum Matrix, the Composite Ridig Body Inertia as well as the centroidal m...
static EIGEN_DONT_INLINE void algo(const JointModelBase< JointModel1 > &, const JointModelBase< JointModel2 > &)
Definition: timings.cpp:143
Base structure for Unary visitation of a JointModel. This structure provides runners to call the righ...
static EIGEN_DONT_INLINE void algo(const JointModelBase< JointModel1 > &, const JointModelBase< JointModel2 > &, JointDataBase< typename JointModel1::JointDataDerived > &, JointDataBase< typename JointModel2::JointDataDerived > &)
Definition: timings.cpp:107
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crbaMinimal(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...
bool check(const AlgorithmCheckerBase< D > &checker=AlgorithmCheckerBase< D >()) const
Check the validity of the attributes of Model with respect to the specification of some algorithms...
ModelTpl< Scalar, Options, JointCollectionTpl > Model
Definition: timings.cpp:34
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobiansTimeVariation(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the full model Jacobian variations with respect to time. It corresponds to dJ/dt which depen...
#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...
Main pinocchio namespace.
Definition: timings.cpp:28
const DataTpl< Scalar, Options, JointCollectionTpl >::Matrix6x & computeJointJacobians(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the full model Jacobian, i.e. the stack of all motion subspace expressed in the world frame...
int nv
Dimension of the velocity vector space.
void emptyForwardPassBinaryVisitNoData(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Definition: timings.cpp:151
void updateFramePlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Updates the position of each frame contained in the model.
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & computeCoriolisMatrix(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the Coriolis Matrix of the Lagrangian dynamics:
const DataTpl< Scalar, Options, JointCollectionTpl >::Vector3 & centerOfMass(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const bool computeSubtreeComs=true)
Computes the center of mass position of a given model according to a particular joint configuration...
filename
DataTpl< Scalar, Options, JointCollectionTpl > Data
Definition: timings.cpp:101
JointModelFreeFlyerTpl< double > JointModelFreeFlyer
static void algo(const JointModelBase< JointModel > &, JointDataBase< typename JointModel::JointDataDerived > &)
Definition: timings.cpp:40
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & nonLinearEffects(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v)
Computes the non-linear effects (Corriolis, centrifual and gravitationnal effects), also called the bias terms of the Lagrangian dynamics:
void emptyForwardPassBinaryVisit(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Definition: timings.cpp:117
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
JointCollectionTpl & model
int nq
Dimension of the configuration vector representation.
ModelTpl< Scalar, Options, JointCollectionTpl > Model
Definition: timings.cpp:136
void framesForwardKinematics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
First calls the forwardKinematics on the model, then computes the placement of each frame...


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