Go to the documentation of this file.
30 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
39 template<
typename Jo
intModel>
48 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
52 assert(
model.check(
data) &&
"data is not consistent with model.");
63 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
72 template<
typename Jo
intModel>
80 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
85 assert(
model.check(
data) &&
"data is not consistent with model.");
96 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
105 template<
typename Jo
intModel1,
typename Jo
intModel2>
116 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
120 assert(
model.check(
data) &&
"data is not consistent with model.");
132 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
141 template<
typename Jo
intModel1,
typename Jo
intModel2>
150 template<
typename Scalar,
int Options,
template<
typename,
int>
class JointCollectionTpl>
155 assert(
model.check(
data) &&
"data is not consistent with model.");
167 int main(
int argc,
const char ** argv)
169 using namespace Eigen;
174 const int NBT = 1000*100;
177 std::cout <<
"(the time score in debug mode is not relevant) " << std::endl;
188 const std::string ff_option = argv[2];
189 if(ff_option ==
"-no-ff")
200 std::cout <<
"nq = " <<
model.nq << std::endl;
210 for(
size_t i=0;
i<NBT;++
i)
213 qdots[
i] = Eigen::VectorXd::Random(
model.nv);
214 qddots[
i] = Eigen::VectorXd::Random(
model.nv);
223 std::cout <<
"RNEA = \t\t"; timer.
toc(std::cout,NBT);
230 std::cout <<
"NLE = \t\t"; timer.
toc(std::cout,NBT);
237 std::cout <<
"NLE via RNEA = \t\t"; timer.
toc(std::cout,NBT);
244 std::cout <<
"CRBA = \t\t"; timer.
toc(std::cout,NBT);
251 std::cout <<
"CRBA minimal = \t\t"; timer.
toc(std::cout,NBT);
258 std::cout <<
"computeAllTerms = \t\t"; timer.
toc(std::cout,NBT);
268 std::cout <<
"Branch Induced Sparsity Cholesky = \t" << (total/NBT)
272 Eigen::LDLT<Eigen::MatrixXd> Mldlt(
data.M);
276 data.M.triangularView<Eigen::StrictlyLower>()
277 =
data.M.transpose().triangularView<Eigen::StrictlyLower>();
279 Mldlt.compute(
data.M);
282 std::cout <<
"Dense Eigen Cholesky = \t" << (total/NBT)
290 std::cout <<
"Jacobian = \t"; timer.
toc(std::cout,NBT);
297 std::cout <<
"Jacobian Time Variation = \t"; timer.
toc(std::cout,NBT);
304 std::cout <<
"COM+Jcom = \t"; timer.
toc(std::cout,NBT);
311 std::cout <<
"COM+vCOM+aCOM = \t"; timer.
toc(std::cout,NBT);
318 std::cout <<
"Zero Order Kinematics = \t"; timer.
toc(std::cout,NBT);
326 std::cout <<
"First Order Kinematics = \t"; timer.
toc(std::cout,NBT);
333 std::cout <<
"Second Order Kinematics = \t"; timer.
toc(std::cout,NBT);
343 std::cout <<
"Update Frame Placement = \t" << (total/NBT)
352 std::cout <<
"Zero Order Frames Kinematics = \t"; timer.
toc(std::cout,NBT);
359 std::cout <<
"CCRBA = \t"; timer.
toc(std::cout,NBT);
366 std::cout <<
"ABA = \t"; timer.
toc(std::cout,NBT);
373 std::cout <<
"Empty Forward Pass Unary visit (no computations) = \t"; timer.
toc(std::cout,NBT);
380 std::cout <<
"Empty Forward Pass Unary visit No Data (no computations) = \t"; timer.
toc(std::cout,NBT);
387 std::cout <<
"Empty Forward Pass Binary visit (no computations) = \t"; timer.
toc(std::cout,NBT);
394 std::cout <<
"Empty Forward Pass Binary visit No Data (no computations) = \t"; timer.
toc(std::cout,NBT);
401 std::cout <<
"Coriolis Matrix = \t"; timer.
toc(std::cout,NBT);
408 std::cout <<
"Minv = \t"; timer.
toc(std::cout,NBT);
410 std::cout <<
"--" << std::endl;
void emptyForwardPassUnaryVisit(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
Eigen::Matrix< Scalar, Eigen::Dynamic, 1 > VectorXd
DataTpl< Scalar, Options, JointCollectionTpl > Data
ModelTpl< Scalar, Options, JointCollectionTpl > Model
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 >::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...
DataTpl< Scalar, Options, JointCollectionTpl > Data
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...
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
Base structure for Binary visitation of two JointModels. This structure provides runners to call the ...
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 std::string unitName(Unit u)
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.
Base structure for Unary visitation of a JointModel. This structure provides runners to call the righ...
JointModelFreeFlyerTpl< double > JointModelFreeFlyer
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 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:
DataTpl< Scalar, Options, JointCollectionTpl > Data
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...
static EIGEN_DONT_INLINE void algo(const JointModelBase< JointModel1 > &, const JointModelBase< JointModel2 > &)
ModelTpl< Scalar, Options, JointCollectionTpl > Model
static EIGEN_DONT_INLINE void algo(const JointModelBase< JointModel1 > &, const JointModelBase< JointModel2 > &, JointDataBase< typename JointModel1::JointDataDerived > &, JointDataBase< typename JointModel2::JointDataDerived > &)
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...
ModelTpl< Scalar, Options, JointCollectionTpl > Model
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....
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.
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....
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),...
int main(int argc, const char **argv)
void emptyForwardPassUnaryVisitNoData(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
void emptyForwardPassBinaryVisit(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
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....
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...
void emptyForwardPassBinaryVisitNoData(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
static void algo(const JointModelBase< JointModel > &, JointDataBase< typename JointModel::JointDataDerived > &)
#define PINOCCHIO_ALIGNED_STD_VECTOR(Type)
DataTpl< Scalar, Options, JointCollectionTpl > Data
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...
ModelTpl< Scalar, Options, JointCollectionTpl > Model
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...
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...
JointCollectionTpl & model
Main pinocchio namespace.
#define PINOCCHIO_UNUSED_VARIABLE(var)
Helper to declare that a parameter is unused.
#define PINOCCHIO_MODEL_DIR
static EIGEN_DONT_INLINE void algo(const JointModelBase< JointModel > &)
pinocchio
Author(s):
autogenerated on Sun Apr 28 2024 02:41:53