Go to the documentation of this file.
25 #include <benchmark/benchmark.h>
35 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
38 JointUnaryVisitorBase<EmptyForwardStepUnaryVisit<Scalar, Options, JointCollectionTpl>, int>
46 template<
typename Jo
intModel>
54 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
59 assert(
model.check(
data) &&
"data is not consistent with model.");
69 benchmark::DoNotOptimize(sum);
73 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
76 EmptyForwardStepUnaryVisitNoData<Scalar, Options, JointCollectionTpl>,
85 template<
typename Jo
intModel>
92 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
98 assert(
model.check(
data) &&
"data is not consistent with model.");
108 benchmark::DoNotOptimize(sum);
112 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
115 JointBinaryVisitorBase<EmptyForwardStepBinaryVisit<Scalar, Options, JointCollectionTpl>, int>
123 template<
typename Jo
intModel1,
typename Jo
intModel2>
134 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
139 assert(
model.check(
data) &&
"data is not consistent with model.");
149 benchmark::DoNotOptimize(sum);
153 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
156 EmptyForwardStepBinaryVisitNoData<Scalar, Options, JointCollectionTpl>,
165 template<
typename Jo
intModel1,
typename Jo
intModel2>
173 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
179 assert(
model.check(
data) &&
"data is not consistent with model.");
189 benchmark::DoNotOptimize(sum);
196 b->MinWarmUpTime(3.);
204 const Eigen::VectorXd & q,
205 const Eigen::VectorXd &
v,
206 const Eigen::VectorXd &
a)
224 const Eigen::VectorXd & q,
225 const Eigen::VectorXd &
v)
243 const Eigen::VectorXd & q,
244 const Eigen::VectorXd &
v)
294 const Eigen::VectorXd & q,
295 const Eigen::VectorXd &
v)
330 M_ldlt.compute(
data.M);
335 data.M.triangularView<Eigen::StrictlyLower>() =
336 data.M.transpose().triangularView<Eigen::StrictlyLower>();
337 Eigen::LDLT<Eigen::MatrixXd> M_ldlt(
data.M);
366 const Eigen::VectorXd & q,
367 const Eigen::VectorXd &
v)
401 const Eigen::VectorXd & q,
402 const Eigen::VectorXd &
v,
403 const Eigen::VectorXd &
a)
437 const Eigen::VectorXd & q,
438 const Eigen::VectorXd &
v)
456 const Eigen::VectorXd & q,
457 const Eigen::VectorXd &
v,
458 const Eigen::VectorXd &
a)
509 const Eigen::VectorXd & q,
510 const Eigen::VectorXd &
v)
528 const Eigen::VectorXd & q,
529 const Eigen::VectorXd &
v,
530 const Eigen::VectorXd &
tau)
548 const Eigen::VectorXd & q,
549 const Eigen::VectorXd &
v,
550 const Eigen::VectorXd &
tau)
568 const Eigen::VectorXd & q,
569 const Eigen::VectorXd &
v)
pinocchio::DataTpl< Scalar, Options, JointCollectionTpl > Data
static void emptyForwardPassBinaryVisit(const pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl > &model, pinocchio::DataTpl< Scalar, Options, JointCollectionTpl > &data)
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)
pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl > Model
static PINOCCHIO_DONT_INLINE void emptyForwardPassUnaryVisitNoDataCall(const pinocchio::Model &model, pinocchio::Data &data)
#define PINOCCHIO_DONT_INLINE
Function attribute to forbid inlining. This is a compiler hint that can be not respected.
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.
pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl > Model
static PINOCCHIO_DONT_INLINE void computeJointJacobiansCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q)
pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl > Model
static PINOCCHIO_DONT_INLINE void computeMinverseQCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q)
static PINOCCHIO_DONT_INLINE void computeCoriolisMatrixCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
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)
static PINOCCHIO_DONT_INLINE void choleskyDecomposeCall(const pinocchio::Model &model, pinocchio::Data &data)
pinocchio::DataTpl< Scalar, Options, JointCollectionTpl > Data
static PINOCCHIO_DONT_INLINE void nonLinearEffectsViaRNEACall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
pinocchio::DataTpl< Scalar, Options, JointCollectionTpl > Data
static void emptyForwardPassUnaryVisitNoData(const pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl > &model, pinocchio::DataTpl< Scalar, Options, JointCollectionTpl > &data)
Base structure for Binary visitation of two JointModels. This structure provides runners to call the ...
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...
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...
BENCHMARK_REGISTER_F(ModelFixture, RNEA) -> Apply(CustomArguments)
PINOCCHIO_BENCHMARK_MAIN()
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...
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....
static PINOCCHIO_DONT_INLINE void emptyForwardPassBinaryVisitNoDataCall(const pinocchio::Model &model, pinocchio::Data &data)
BENCHMARK_DEFINE_F(ModelFixture, RNEA)(benchmark
pinocchio::DataTpl< Scalar, Options, JointCollectionTpl > Data
static PINOCCHIO_DONT_INLINE void denseCholeksyDecomposeCall(Eigen::LDLT< Eigen::MatrixXd > &M_ldlt, const pinocchio::Data &data)
static PINOCCHIO_DONT_INLINE int algo(const JointModelBase< JointModel > &)
Base structure for Unary visitation of a JointModel. This structure provides runners to call the righ...
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.
pinocchio::JointIndex JointIndex
static PINOCCHIO_DONT_INLINE int algo(const JointModelBase< JointModel > &, JointDataBase< typename JointModel::JointDataDerived > &)
static PINOCCHIO_DONT_INLINE void crbaLocalCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q)
static PINOCCHIO_DONT_INLINE void computeJointJacobiansTimeVariationCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
void updateFramePlacements(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Updates the position of each frame contained in the model.
static PINOCCHIO_DONT_INLINE void ccrbaCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
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...
static PINOCCHIO_DONT_INLINE void crbaWorldCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q)
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 PINOCCHIO_DONT_INLINE void emptyForwardPassBinaryVisitCall(const pinocchio::Model &model, pinocchio::Data &data)
static PINOCCHIO_DONT_INLINE void forwardKinematicsQVCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
static PINOCCHIO_DONT_INLINE int algo(const JointModelBase< JointModel1 > &, const JointModelBase< JointModel2 > &)
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),...
static PINOCCHIO_DONT_INLINE void framesForwardKinematicsCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q)
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....
static PINOCCHIO_DONT_INLINE void forwardKinematicsQCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q)
static void CustomArguments(benchmark::internal::Benchmark *b)
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.
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)
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...
static PINOCCHIO_DONT_INLINE int algo(const JointModelBase< JointModel1 > &, const JointModelBase< JointModel2 > &, JointDataBase< typename JointModel1::JointDataDerived > &, JointDataBase< typename JointModel2::JointDataDerived > &)
static PINOCCHIO_DONT_INLINE void emptyForwardPassUnaryVisitCall(const pinocchio::Model &model, pinocchio::Data &data)
static void emptyForwardPassBinaryVisitNoData(const pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl > &model, pinocchio::DataTpl< Scalar, Options, JointCollectionTpl > &data)
static void emptyForwardPassUnaryVisit(const pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl > &model, pinocchio::DataTpl< Scalar, Options, JointCollectionTpl > &data)
pinocchio::ModelTpl< Scalar, Options, JointCollectionTpl > Model
static PINOCCHIO_DONT_INLINE void computeMinverseCall(const pinocchio::Model &model, pinocchio::Data &data)
static PINOCCHIO_DONT_INLINE void updateFramePlacementsCall(const pinocchio::Model &model, pinocchio::Data &data)
static PINOCCHIO_DONT_INLINE void nonLinearEffectsCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
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)
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:
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...
static PINOCCHIO_DONT_INLINE void jacobianCenterOfMassCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q)
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)
JointCollectionTpl & model
Main pinocchio namespace.
#define PINOCCHIO_UNUSED_VARIABLE(var)
Helper to declare that a parameter is unused.
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....
static PINOCCHIO_DONT_INLINE void computeAllTermsCall(const pinocchio::Model &model, pinocchio::Data &data, const Eigen::VectorXd &q, const Eigen::VectorXd &v)
pinocchio
Author(s):
autogenerated on Mon Apr 7 2025 02:41:45