timings.cpp
Go to the documentation of this file.
1 //
2 // Copyright (c) 2015-2025 CNRS INRIA
3 //
4 
5 #include "model-fixture.hpp"
6 
9 
21 
24 
25 #include <benchmark/benchmark.h>
26 
27 #include <iostream>
28 
32 
33 namespace pinocchio
34 {
35  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
37  : fusion::
38  JointUnaryVisitorBase<EmptyForwardStepUnaryVisit<Scalar, Options, JointCollectionTpl>, int>
39  {
42 
44 
45  // Function should not be empty to allow PINOCCHIO_DONT_INLINE to work
46  template<typename JointModel>
47  PINOCCHIO_DONT_INLINE static int
49  {
50  return 0;
51  }
52  };
53 
54  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
58  {
59  assert(model.check(data) && "data is not consistent with model.");
60 
61  typedef
64 
65  int sum = 0;
66  for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
67  {
68  sum += Algo::run(model.joints[i], data.joints[i]);
69  benchmark::DoNotOptimize(sum);
70  }
71  }
72 
73  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
76  EmptyForwardStepUnaryVisitNoData<Scalar, Options, JointCollectionTpl>,
77  int>
78  {
81 
83 
84  // Function should not be empty to allow PINOCCHIO_DONT_INLINE to work
85  template<typename JointModel>
87  {
88  return 0;
89  }
90  };
91 
92  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
96  {
98  assert(model.check(data) && "data is not consistent with model.");
99 
100  typedef
103 
104  int sum = 0;
105  for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
106  {
107  sum += Algo::run(model.joints[i]);
108  benchmark::DoNotOptimize(sum);
109  }
110  }
111 
112  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
114  : fusion::
115  JointBinaryVisitorBase<EmptyForwardStepBinaryVisit<Scalar, Options, JointCollectionTpl>, int>
116  {
119 
121 
122  // Function should not be empty to allow PINOCCHIO_DONT_INLINE to work
123  template<typename JointModel1, typename JointModel2>
129  {
130  return 0;
131  }
132  };
133 
134  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
138  {
139  assert(model.check(data) && "data is not consistent with model.");
140 
141  typedef
144 
145  int sum = 0;
146  for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
147  {
148  sum += Algo::run(model.joints[i], model.joints[i], data.joints[i], data.joints[i]);
149  benchmark::DoNotOptimize(sum);
150  }
151  }
152 
153  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
156  EmptyForwardStepBinaryVisitNoData<Scalar, Options, JointCollectionTpl>,
157  int>
158  {
161 
163 
164  // Function should not be empty to allow PINOCCHIO_DONT_INLINE to work
165  template<typename JointModel1, typename JointModel2>
166  PINOCCHIO_DONT_INLINE static int
168  {
169  return 0;
170  }
171  };
172 
173  template<typename Scalar, int Options, template<typename, int> class JointCollectionTpl>
177  {
179  assert(model.check(data) && "data is not consistent with model.");
180 
181  typedef
184 
185  int sum = 0;
186  for (JointIndex i = 1; i < (JointIndex)model.njoints; ++i)
187  {
188  sum += Algo::run(model.joints[i], model.joints[i]);
189  benchmark::DoNotOptimize(sum);
190  }
191  }
192 } // namespace pinocchio
193 
194 static void CustomArguments(benchmark::internal::Benchmark * b)
195 {
196  b->MinWarmUpTime(3.);
197 }
198 
199 // RNEA
200 
202  const pinocchio::Model & model,
204  const Eigen::VectorXd & q,
205  const Eigen::VectorXd & v,
206  const Eigen::VectorXd & a)
207 {
208  pinocchio::rnea(model, data, q, v, a);
209 }
210 BENCHMARK_DEFINE_F(ModelFixture, RNEA)(benchmark::State & st)
211 {
212  for (auto _ : st)
213  {
214  rneaCall(model, data, q, v, a);
215  }
216 }
218 
219 // nonLinearEffects
220 
222  const pinocchio::Model & model,
224  const Eigen::VectorXd & q,
225  const Eigen::VectorXd & v)
226 {
228 }
229 BENCHMARK_DEFINE_F(ModelFixture, NLE)(benchmark::State & st)
230 {
231  for (auto _ : st)
232  {
234  }
235 }
237 
238 // nonLinearEffects via RNEA
239 
241  const pinocchio::Model & model,
243  const Eigen::VectorXd & q,
244  const Eigen::VectorXd & v)
245 {
246  pinocchio::rnea(model, data, q, v, Eigen::VectorXd::Zero(model.nv));
247 }
248 BENCHMARK_DEFINE_F(ModelFixture, NLE_via_RNEA)(benchmark::State & st)
249 {
250  for (auto _ : st)
251  {
253  }
254 }
255 BENCHMARK_REGISTER_F(ModelFixture, NLE_via_RNEA)->Apply(CustomArguments);
256 
257 // CRBA Local
258 
259 PINOCCHIO_DONT_INLINE static void
260 crbaLocalCall(const pinocchio::Model & model, pinocchio::Data & data, const Eigen::VectorXd & q)
261 {
263 }
264 BENCHMARK_DEFINE_F(ModelFixture, CRBA_LOCAL)(benchmark::State & st)
265 {
266  for (auto _ : st)
267  {
269  }
270 }
272 
273 // CRBA World
274 
275 PINOCCHIO_DONT_INLINE static void
276 crbaWorldCall(const pinocchio::Model & model, pinocchio::Data & data, const Eigen::VectorXd & q)
277 {
279 }
280 BENCHMARK_DEFINE_F(ModelFixture, CRBA_WORLD)(benchmark::State & st)
281 {
282  for (auto _ : st)
283  {
285  }
286 }
288 
289 // computeAllTerms
290 
292  const pinocchio::Model & model,
294  const Eigen::VectorXd & q,
295  const Eigen::VectorXd & v)
296 {
298 }
299 BENCHMARK_DEFINE_F(ModelFixture, COMPUTE_ALL_TERMS)(benchmark::State & st)
300 {
301  for (auto _ : st)
302  {
304  }
305 }
306 BENCHMARK_REGISTER_F(ModelFixture, COMPUTE_ALL_TERMS)->Apply(CustomArguments);
307 
308 // choleskyDecompose
309 
310 PINOCCHIO_DONT_INLINE static void
312 {
314 }
315 BENCHMARK_DEFINE_F(ModelFixture, CHOLESKY_DECOMPOSE)(benchmark::State & st)
316 {
318  for (auto _ : st)
319  {
321  }
322 }
323 BENCHMARK_REGISTER_F(ModelFixture, CHOLESKY_DECOMPOSE)->Apply(CustomArguments);
324 
325 // Dense choleskyDecompose
326 
327 PINOCCHIO_DONT_INLINE static void
328 denseCholeksyDecomposeCall(Eigen::LDLT<Eigen::MatrixXd> & M_ldlt, const pinocchio::Data & data)
329 {
330  M_ldlt.compute(data.M);
331 }
332 BENCHMARK_DEFINE_F(ModelFixture, DENSE_CHOLESKY_DECOMPOSE)(benchmark::State & st)
333 {
335  data.M.triangularView<Eigen::StrictlyLower>() =
336  data.M.transpose().triangularView<Eigen::StrictlyLower>();
337  Eigen::LDLT<Eigen::MatrixXd> M_ldlt(data.M);
338  for (auto _ : st)
339  {
341  }
342 }
343 BENCHMARK_REGISTER_F(ModelFixture, DENSE_CHOLESKY_DECOMPOSE)->Apply(CustomArguments);
344 
345 // computeJointJacobians
346 
348  const pinocchio::Model & model, pinocchio::Data & data, const Eigen::VectorXd & q)
349 {
351 }
352 BENCHMARK_DEFINE_F(ModelFixture, COMPUTE_JOINT_JACOBIANS)(benchmark::State & st)
353 {
354  for (auto _ : st)
355  {
357  }
358 }
359 BENCHMARK_REGISTER_F(ModelFixture, COMPUTE_JOINT_JACOBIANS)->Apply(CustomArguments);
360 
361 // computeJointJacobiansTimeVariation
362 
364  const pinocchio::Model & model,
366  const Eigen::VectorXd & q,
367  const Eigen::VectorXd & v)
368 {
370 }
371 BENCHMARK_DEFINE_F(ModelFixture, COMPUTE_JOINT_JACOBIANS_TIME_VARIATION)(benchmark::State & st)
372 {
373  for (auto _ : st)
374  {
376  }
377 }
378 BENCHMARK_REGISTER_F(ModelFixture, COMPUTE_JOINT_JACOBIANS_TIME_VARIATION)->Apply(CustomArguments);
379 
380 // jacobianCenterOfMass
381 
383  const pinocchio::Model & model, pinocchio::Data & data, const Eigen::VectorXd & q)
384 {
386 }
387 BENCHMARK_DEFINE_F(ModelFixture, JACOBIAN_CENTER_OF_MASS)(benchmark::State & st)
388 {
389  for (auto _ : st)
390  {
392  }
393 }
394 BENCHMARK_REGISTER_F(ModelFixture, JACOBIAN_CENTER_OF_MASS)->Apply(CustomArguments);
395 
396 // centerOfMass
397 
399  const pinocchio::Model & model,
401  const Eigen::VectorXd & q,
402  const Eigen::VectorXd & v,
403  const Eigen::VectorXd & a)
404 {
405  pinocchio::centerOfMass(model, data, q, v, a, true);
406 }
407 BENCHMARK_DEFINE_F(ModelFixture, CENTER_OF_MASS)(benchmark::State & st)
408 {
409  for (auto _ : st)
410  {
412  }
413 }
414 BENCHMARK_REGISTER_F(ModelFixture, CENTER_OF_MASS)->Apply(CustomArguments);
415 
416 // forwardKinematicsQ
417 
419  const pinocchio::Model & model, pinocchio::Data & data, const Eigen::VectorXd & q)
420 {
422 }
423 BENCHMARK_DEFINE_F(ModelFixture, FORWARD_KINEMATICS_Q)(benchmark::State & st)
424 {
425  for (auto _ : st)
426  {
428  }
429 }
430 BENCHMARK_REGISTER_F(ModelFixture, FORWARD_KINEMATICS_Q)->Apply(CustomArguments);
431 
432 // forwardKinematicsQV
433 
435  const pinocchio::Model & model,
437  const Eigen::VectorXd & q,
438  const Eigen::VectorXd & v)
439 {
441 }
442 BENCHMARK_DEFINE_F(ModelFixture, FORWARD_KINEMATICS_Q_V)(benchmark::State & st)
443 {
444  for (auto _ : st)
445  {
447  }
448 }
449 BENCHMARK_REGISTER_F(ModelFixture, FORWARD_KINEMATICS_Q_V)->Apply(CustomArguments);
450 
451 // forwardKinematicsQVA
452 
454  const pinocchio::Model & model,
456  const Eigen::VectorXd & q,
457  const Eigen::VectorXd & v,
458  const Eigen::VectorXd & a)
459 {
461 }
462 BENCHMARK_DEFINE_F(ModelFixture, FORWARD_KINEMATICS_Q_V_A)(benchmark::State & st)
463 {
464  for (auto _ : st)
465  {
467  }
468 }
469 BENCHMARK_REGISTER_F(ModelFixture, FORWARD_KINEMATICS_Q_V_A)->Apply(CustomArguments);
470 
471 // framesForwardKinematics
472 
474  const pinocchio::Model & model, pinocchio::Data & data, const Eigen::VectorXd & q)
475 {
477 }
478 BENCHMARK_DEFINE_F(ModelFixture, FRAME_FORWARD_KINEMATICS)(benchmark::State & st)
479 {
480  for (auto _ : st)
481  {
483  }
484 }
485 BENCHMARK_REGISTER_F(ModelFixture, FRAME_FORWARD_KINEMATICS)->Apply(CustomArguments);
486 
487 // updateFramePlacements
488 
489 PINOCCHIO_DONT_INLINE static void
491 {
493 }
494 BENCHMARK_DEFINE_F(ModelFixture, UPDATE_FRAME_PLACEMENTS)(benchmark::State & st)
495 {
497  for (auto _ : st)
498  {
500  }
501 }
502 BENCHMARK_REGISTER_F(ModelFixture, UPDATE_FRAME_PLACEMENTS)->Apply(CustomArguments);
503 
504 // CCRBA
505 
507  const pinocchio::Model & model,
509  const Eigen::VectorXd & q,
510  const Eigen::VectorXd & v)
511 {
513 }
514 BENCHMARK_DEFINE_F(ModelFixture, CCRBA)(benchmark::State & st)
515 {
516  for (auto _ : st)
517  {
518  ccrbaCall(model, data, q, v);
519  }
520 }
522 
523 // ABA Local
524 
526  const pinocchio::Model & model,
528  const Eigen::VectorXd & q,
529  const Eigen::VectorXd & v,
530  const Eigen::VectorXd & tau)
531 {
533 }
534 BENCHMARK_DEFINE_F(ModelFixture, ABA_LOCAL)(benchmark::State & st)
535 {
536  for (auto _ : st)
537  {
538  abaLocalCall(model, data, q, v, tau);
539  }
540 }
542 
543 // ABA World
544 
546  const pinocchio::Model & model,
548  const Eigen::VectorXd & q,
549  const Eigen::VectorXd & v,
550  const Eigen::VectorXd & tau)
551 {
553 }
554 BENCHMARK_DEFINE_F(ModelFixture, ABA_WORLD)(benchmark::State & st)
555 {
556  for (auto _ : st)
557  {
558  abaWorldCall(model, data, q, v, tau);
559  }
560 }
562 
563 // computeCoriolisMatrix
564 
566  const pinocchio::Model & model,
568  const Eigen::VectorXd & q,
569  const Eigen::VectorXd & v)
570 {
572 }
573 BENCHMARK_DEFINE_F(ModelFixture, COMPUTE_CORIOLIS_MATRIX)(benchmark::State & st)
574 {
575  for (auto _ : st)
576  {
578  }
579 }
580 BENCHMARK_REGISTER_F(ModelFixture, COMPUTE_CORIOLIS_MATRIX)->Apply(CustomArguments);
581 
582 // computeMinverseQ
583 
585  const pinocchio::Model & model, pinocchio::Data & data, const Eigen::VectorXd & q)
586 {
588 }
589 BENCHMARK_DEFINE_F(ModelFixture, COMPUTE_M_INVERSE_Q)(benchmark::State & st)
590 {
591  for (auto _ : st)
592  {
594  }
595 }
596 BENCHMARK_REGISTER_F(ModelFixture, COMPUTE_M_INVERSE_Q)->Apply(CustomArguments);
597 
598 // computeMinverse
599 
600 PINOCCHIO_DONT_INLINE static void
602 {
604 }
605 BENCHMARK_DEFINE_F(ModelFixture, COMPUTE_M_INVERSE)(benchmark::State & st)
606 {
608  for (auto _ : st)
609  {
611  }
612 }
613 BENCHMARK_REGISTER_F(ModelFixture, COMPUTE_M_INVERSE)->Apply(CustomArguments);
614 
615 // emptyForwardPassUnaryVisit
616 
617 PINOCCHIO_DONT_INLINE static void
619 {
621 }
622 BENCHMARK_DEFINE_F(ModelFixture, EMPTY_FORWARD_PASS_UNARY_VISIT)(benchmark::State & st)
623 {
624  for (auto _ : st)
625  {
627  }
628 }
629 BENCHMARK_REGISTER_F(ModelFixture, EMPTY_FORWARD_PASS_UNARY_VISIT)->Apply(CustomArguments);
630 
631 // emptyForwardPassUnaryVisitNoData
632 
633 PINOCCHIO_DONT_INLINE static void
635 {
637 }
638 BENCHMARK_DEFINE_F(ModelFixture, EMPTY_FORWARD_PASS_UNARY_VISIT_NO_DATA)(benchmark::State & st)
639 {
640  for (auto _ : st)
641  {
643  }
644 }
645 BENCHMARK_REGISTER_F(ModelFixture, EMPTY_FORWARD_PASS_UNARY_VISIT_NO_DATA)->Apply(CustomArguments);
646 
647 // emptyForwardPassBinaryVisit
648 
649 PINOCCHIO_DONT_INLINE static void
651 {
653 }
654 BENCHMARK_DEFINE_F(ModelFixture, EMPTY_FORWARD_PASS_BINARY_VISIT)(benchmark::State & st)
655 {
656  for (auto _ : st)
657  {
659  }
660 }
661 BENCHMARK_REGISTER_F(ModelFixture, EMPTY_FORWARD_PASS_BINARY_VISIT)->Apply(CustomArguments);
662 
663 // emptyForwardPassBinaryVisitNoData
664 
665 PINOCCHIO_DONT_INLINE static void
667 {
669 }
670 BENCHMARK_DEFINE_F(ModelFixture, EMPTY_FORWARD_PASS_BINARY_VISIT_NO_DATA)(benchmark::State & st)
671 {
672  for (auto _ : st)
673  {
675  }
676 }
677 BENCHMARK_REGISTER_F(ModelFixture, EMPTY_FORWARD_PASS_BINARY_VISIT_NO_DATA)->Apply(CustomArguments);
678 
pinocchio::EmptyForwardStepBinaryVisit::Data
pinocchio::DataTpl< Scalar, Options, JointCollectionTpl > Data
Definition: timings.cpp:118
pinocchio::emptyForwardPassBinaryVisit
static void emptyForwardPassBinaryVisit(const pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl > &model, pinocchio::DataTpl< Scalar, Options, JointCollectionTpl > &data)
Definition: timings.cpp:135
frames.hpp
rneaCall
static PINOCCHIO_DONT_INLINE void rneaCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &a)
Definition: timings.cpp:201
pinocchio::EmptyForwardStepBinaryVisit::Model
pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl > Model
Definition: timings.cpp:117
pinocchio::DataTpl
Definition: context/generic.hpp:25
emptyForwardPassUnaryVisitNoDataCall
static PINOCCHIO_DONT_INLINE void emptyForwardPassUnaryVisitNoDataCall(const pinocchio::Model &model, pinocchio::Data &data)
Definition: timings.cpp:634
PINOCCHIO_DONT_INLINE
#define PINOCCHIO_DONT_INLINE
Function attribute to forbid inlining. This is a compiler hint that can be not respected.
Definition: include/pinocchio/macros.hpp:53
pinocchio::forwardKinematics
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.
compute-all-terms.hpp
omniidl_be_python_with_docstring.run
def run(tree, args)
Definition: cmake/hpp/idl/omniidl_be_python_with_docstring.py:140
pinocchio::EmptyForwardStepBinaryVisitNoData::Model
pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl > Model
Definition: timings.cpp:159
pinocchio::JointModelBase
Definition: joint-model-base.hpp:78
pinocchio::EmptyForwardStepBinaryVisit::ArgsType
fusion::NoArg ArgsType
Definition: timings.cpp:120
computeJointJacobiansCall
static PINOCCHIO_DONT_INLINE void computeJointJacobiansCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q)
Definition: timings.cpp:347
pinocchio::EmptyForwardStepUnaryVisitNoData::Model
pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl > Model
Definition: timings.cpp:79
ModelFixture
Definition: model-fixture.hpp:37
computeMinverseQCall
static PINOCCHIO_DONT_INLINE void computeMinverseQCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q)
Definition: timings.cpp:584
pinocchio::Convention::WORLD
@ WORLD
computeCoriolisMatrixCall
static PINOCCHIO_DONT_INLINE void computeCoriolisMatrixCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
Definition: timings.cpp:565
pinocchio::JointDataBase
Definition: joint-data-base.hpp:161
inverse-kinematics.i
int i
Definition: inverse-kinematics.py:17
abaWorldCall
static PINOCCHIO_DONT_INLINE void abaWorldCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &tau)
Definition: timings.cpp:545
kinematics.hpp
model.hpp
setup.data
data
Definition: cmake/cython/setup.in.py:48
choleskyDecomposeCall
static PINOCCHIO_DONT_INLINE void choleskyDecomposeCall(const pinocchio::Model &model, pinocchio::Data &data)
Definition: timings.cpp:311
pinocchio::EmptyForwardStepUnaryVisit::Data
pinocchio::DataTpl< Scalar, Options, JointCollectionTpl > Data
Definition: timings.cpp:41
nonLinearEffectsViaRNEACall
static PINOCCHIO_DONT_INLINE void nonLinearEffectsViaRNEACall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
Definition: timings.cpp:240
pinocchio::EmptyForwardStepBinaryVisitNoData::Data
pinocchio::DataTpl< Scalar, Options, JointCollectionTpl > Data
Definition: timings.cpp:160
pinocchio::emptyForwardPassUnaryVisitNoData
static void emptyForwardPassUnaryVisitNoData(const pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl > &model, pinocchio::DataTpl< Scalar, Options, JointCollectionTpl > &data)
Definition: timings.cpp:93
pinocchio::fusion::JointBinaryVisitorBase
Base structure for Binary visitation of two JointModels. This structure provides runners to call the ...
Definition: joint-binary-visitor.hpp:27
pinocchio::crba
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Convention convention=Convention::LOCAL)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
pinocchio::computeJointJacobiansTimeVariation
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...
pinocchio::JointModelFreeFlyerTpl
Definition: multibody/joint/fwd.hpp:110
BENCHMARK_REGISTER_F
BENCHMARK_REGISTER_F(ModelFixture, RNEA) -> Apply(CustomArguments)
PINOCCHIO_BENCHMARK_MAIN
PINOCCHIO_BENCHMARK_MAIN()
pinocchio::aba
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, const Convention convention=Convention::LOCAL)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
pinocchio::computeJointJacobians
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....
emptyForwardPassBinaryVisitNoDataCall
static PINOCCHIO_DONT_INLINE void emptyForwardPassBinaryVisitNoDataCall(const pinocchio::Model &model, pinocchio::Data &data)
Definition: timings.cpp:666
BENCHMARK_DEFINE_F
BENCHMARK_DEFINE_F(ModelFixture, RNEA)(benchmark
Definition: timings.cpp:210
pinocchio::EmptyForwardStepUnaryVisitNoData::Data
pinocchio::DataTpl< Scalar, Options, JointCollectionTpl > Data
Definition: timings.cpp:80
pinocchio::EmptyForwardStepBinaryVisit
Definition: timings.cpp:113
pinocchio::EmptyForwardStepBinaryVisitNoData
Definition: timings.cpp:154
denseCholeksyDecomposeCall
static PINOCCHIO_DONT_INLINE void denseCholeksyDecomposeCall(Eigen::LDLT< Eigen::MatrixXd > &M_ldlt, const pinocchio::Data &data)
Definition: timings.cpp:328
pinocchio::EmptyForwardStepUnaryVisitNoData::algo
static PINOCCHIO_DONT_INLINE int algo(const JointModelBase< JointModel > &)
Definition: timings.cpp:86
pinocchio::fusion::JointUnaryVisitorBase
Base structure for Unary visitation of a JointModel. This structure provides runners to call the righ...
Definition: joint-unary-visitor.hpp:25
rnea.hpp
b
Vec3f b
pinocchio::EmptyForwardStepBinaryVisitNoData::ArgsType
fusion::NoArg ArgsType
Definition: timings.cpp:162
pinocchio::computeMinverse
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.
aba.hpp
anymal-simulation.model
model
Definition: anymal-simulation.py:8
pinocchio::ModelTpl::JointIndex
pinocchio::JointIndex JointIndex
Definition: multibody/model.hpp:67
forward-dynamics-derivatives.tau
tau
Definition: forward-dynamics-derivatives.py:25
center-of-mass.hpp
pinocchio::EmptyForwardStepUnaryVisit::algo
static PINOCCHIO_DONT_INLINE int algo(const JointModelBase< JointModel > &, JointDataBase< typename JointModel::JointDataDerived > &)
Definition: timings.cpp:48
joint-configuration.hpp
crbaLocalCall
static PINOCCHIO_DONT_INLINE void crbaLocalCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q)
Definition: timings.cpp:260
computeJointJacobiansTimeVariationCall
static PINOCCHIO_DONT_INLINE void computeJointJacobiansTimeVariationCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
Definition: timings.cpp:363
pinocchio::updateFramePlacements
void updateFramePlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Updates the position of each frame contained in the model.
model-fixture.hpp
ccrbaCall
static PINOCCHIO_DONT_INLINE void ccrbaCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
Definition: timings.cpp:506
pinocchio::jacobianCenterOfMass
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...
pinocchio::EmptyForwardStepUnaryVisit::ArgsType
fusion::NoArg ArgsType
Definition: timings.cpp:43
urdf.hpp
crbaWorldCall
static PINOCCHIO_DONT_INLINE void crbaWorldCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q)
Definition: timings.cpp:276
cartpole.v
v
Definition: cartpole.py:153
data.hpp
pinocchio::ccrba
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...
emptyForwardPassBinaryVisitCall
static PINOCCHIO_DONT_INLINE void emptyForwardPassBinaryVisitCall(const pinocchio::Model &model, pinocchio::Data &data)
Definition: timings.cpp:650
forwardKinematicsQVCall
static PINOCCHIO_DONT_INLINE void forwardKinematicsQVCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
Definition: timings.cpp:434
inverse-dynamics._
_
Definition: inverse-dynamics.py:22
pinocchio::EmptyForwardStepBinaryVisitNoData::algo
static PINOCCHIO_DONT_INLINE int algo(const JointModelBase< JointModel1 > &, const JointModelBase< JointModel2 > &)
Definition: timings.cpp:167
pinocchio::nonLinearEffects
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),...
q
q
framesForwardKinematicsCall
static PINOCCHIO_DONT_INLINE void framesForwardKinematicsCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q)
Definition: timings.cpp:473
pinocchio::framesForwardKinematics
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....
forwardKinematicsQCall
static PINOCCHIO_DONT_INLINE void forwardKinematicsQCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q)
Definition: timings.cpp:418
CustomArguments
static void CustomArguments(benchmark::internal::Benchmark *b)
Definition: timings.cpp:194
a
Vec3f a
cholesky.hpp
pinocchio::EmptyForwardStepUnaryVisitNoData::ArgsType
fusion::NoArg ArgsType
Definition: timings.cpp:82
pinocchio::cholesky::decompose
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.
abaLocalCall
static PINOCCHIO_DONT_INLINE void abaLocalCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &tau)
Definition: timings.cpp:525
centroidal.hpp
pinocchio::rnea
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & rnea(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 > &a)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
pinocchio::EmptyForwardStepBinaryVisit::algo
static PINOCCHIO_DONT_INLINE int algo(const JointModelBase< JointModel1 > &, const JointModelBase< JointModel2 > &, JointDataBase< typename JointModel1::JointDataDerived > &, JointDataBase< typename JointModel2::JointDataDerived > &)
Definition: timings.cpp:124
emptyForwardPassUnaryVisitCall
static PINOCCHIO_DONT_INLINE void emptyForwardPassUnaryVisitCall(const pinocchio::Model &model, pinocchio::Data &data)
Definition: timings.cpp:618
pinocchio::emptyForwardPassBinaryVisitNoData
static void emptyForwardPassBinaryVisitNoData(const pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl > &model, pinocchio::DataTpl< Scalar, Options, JointCollectionTpl > &data)
Definition: timings.cpp:174
pinocchio::Convention::LOCAL
@ LOCAL
pinocchio::emptyForwardPassUnaryVisit
static void emptyForwardPassUnaryVisit(const pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl > &model, pinocchio::DataTpl< Scalar, Options, JointCollectionTpl > &data)
Definition: timings.cpp:55
pinocchio::EmptyForwardStepUnaryVisit::Model
pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl > Model
Definition: timings.cpp:40
pinocchio::fusion::NoArg
boost::blank NoArg
Definition: fusion.hpp:20
pinocchio::JointIndex
Index JointIndex
Definition: multibody/fwd.hpp:26
pinocchio::EmptyForwardStepUnaryVisit
Definition: timings.cpp:36
computeMinverseCall
static PINOCCHIO_DONT_INLINE void computeMinverseCall(const pinocchio::Model &model, pinocchio::Data &data)
Definition: timings.cpp:601
pinocchio::JointCollectionDefaultTpl
Definition: context/generic.hpp:15
sample-models.hpp
updateFramePlacementsCall
static PINOCCHIO_DONT_INLINE void updateFramePlacementsCall(const pinocchio::Model &model, pinocchio::Data &data)
Definition: timings.cpp:490
nonLinearEffectsCall
static PINOCCHIO_DONT_INLINE void nonLinearEffectsCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
Definition: timings.cpp:221
jacobian.hpp
pinocchio::ModelTpl
Definition: context/generic.hpp:20
forwardKinematicsQVACall
static PINOCCHIO_DONT_INLINE void forwardKinematicsQVACall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &a)
Definition: timings.cpp:453
pinocchio::computeCoriolisMatrix
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:
pinocchio::EmptyForwardStepUnaryVisitNoData
Definition: timings.cpp:74
crba.hpp
pinocchio::computeAllTerms
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...
jacobianCenterOfMassCall
static PINOCCHIO_DONT_INLINE void jacobianCenterOfMassCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q)
Definition: timings.cpp:382
centerOfMassCall
static PINOCCHIO_DONT_INLINE void centerOfMassCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v, const Eigen::VectorXd &a)
Definition: timings.cpp:398
pinocchio::model
JointCollectionTpl & model
Definition: joint-configuration.hpp:1082
pinocchio
Main pinocchio namespace.
Definition: timings.cpp:33
PINOCCHIO_UNUSED_VARIABLE
#define PINOCCHIO_UNUSED_VARIABLE(var)
Helper to declare that a parameter is unused.
Definition: include/pinocchio/macros.hpp:76
pinocchio::centerOfMass
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....
computeAllTermsCall
static PINOCCHIO_DONT_INLINE void computeAllTermsCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
Definition: timings.cpp:291


pinocchio
Author(s):
autogenerated on Mon Apr 7 2025 02:41:45