5 #include "pinocchio/multibody/model.hpp" 6 #include "pinocchio/multibody/data.hpp" 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" 20 #include "pinocchio/parsers/urdf.hpp" 21 #include "pinocchio/parsers/sample-models.hpp" 25 #include "pinocchio/utils/timer.hpp" 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;
183 if(argc>1) filename = argv[1];
188 const std::string ff_option = argv[2];
189 if(ff_option ==
"-no-ff")
193 if( filename ==
"HS")
200 std::cout <<
"nq = " << model.
nq << std::endl;
205 const VectorXd qmax = Eigen::VectorXd::Ones(model.
nq);
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);
221 rnea(model,data,
qs[_smooth],qdots[_smooth],qddots[_smooth]);
223 std::cout <<
"RNEA = \t\t"; timer.
toc(std::cout,NBT);
230 std::cout <<
"NLE = \t\t"; timer.
toc(std::cout,NBT);
235 rnea(model,data,
qs[_smooth],qdots[_smooth],Eigen::VectorXd::Zero(model.
nv));
237 std::cout <<
"NLE via RNEA = \t\t"; timer.
toc(std::cout,NBT);
242 crba(model,data,
qs[_smooth]);
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);
263 crba(model,data,
qs[_smooth]);
268 std::cout <<
"Branch Induced Sparsity Cholesky = \t" << (total/NBT)
272 Eigen::LDLT<Eigen::MatrixXd> Mldlt(data.
M);
275 crba(model,data,
qs[_smooth]);
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);
309 centerOfMass(model,data,
qs[_smooth], qdots[_smooth], qddots[_smooth],
true);
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);
357 ccrba(model,data,
qs[_smooth],qdots[_smooth]);
359 std::cout <<
"CCRBA = \t"; timer.
toc(std::cout,NBT);
364 aba(model,data,
qs[_smooth],qdots[_smooth], qddots[_smooth]);
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;
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
ModelTpl< Scalar, Options, JointCollectionTpl > Model
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)
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
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)
#define PINOCCHIO_UNUSED_VARIABLE(var)
Helper to declare that a parameter is unused.
DataTpl< Scalar, Options, JointCollectionTpl > Data
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
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)
JointDataVector joints
Vector of pinocchio::JointData associated to the pinocchio::JointModel stored in model, encapsulated in JointDataAccessor.
DataTpl< Scalar, Options, JointCollectionTpl > Data
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
static EIGEN_DONT_INLINE void algo(const JointModelBase< JointModel > &)
void emptyForwardPassUnaryVisitNoData(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data)
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 > &)
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 > &)
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
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...
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.
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)
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...
DataTpl< Scalar, Options, JointCollectionTpl > Data
JointModelFreeFlyerTpl< double > JointModelFreeFlyer
static void algo(const JointModelBase< JointModel > &, JointDataBase< typename JointModel::JointDataDerived > &)
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)
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
JointCollectionTpl & model
int nq
Dimension of the configuration vector representation.
ModelTpl< Scalar, Options, JointCollectionTpl > Model
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...