Go to the documentation of this file.
30 int main(
int argc,
const char ** argv)
32 using namespace Eigen;
37 const int NBT = 1000 * 100;
40 std::cout <<
"(the time score in debug mode is not relevant) " << std::endl;
51 const std::string ff_option = argv[2];
52 if (ff_option ==
"-no-ff")
63 std::cout <<
"nq = " <<
model.nq << std::endl;
64 std::cout <<
"nv = " <<
model.nv << std::endl;
65 std::cout <<
"--" << std::endl;
69 const std::string RF =
"RLEG_ANKLE_R";
70 const std::string LF =
"LLEG_ANKLE_R";
75 contact_models_6D6D.push_back(ci_RF);
76 contact_models_6D6D.push_back(ci_LF);
81 contact_datas_6D6D.push_back(cd_RF);
83 contact_datas_6D6D.push_back(cd_LF);
85 VectorXd qmax = Eigen::VectorXd::Ones(
model.nq);
104 rnea_derivatives_code_gen.
initLib();
105 rnea_derivatives_code_gen.
loadLib();
108 aba_derivatives_code_gen.
initLib();
109 aba_derivatives_code_gen.
loadLib();
112 model, contact_models_6D6D);
113 constraint_dynamics_derivatives_code_gen.
initLib();
114 constraint_dynamics_derivatives_code_gen.
loadLib();
121 for (
size_t i = 0;
i < NBT; ++
i)
124 qdots[
i] = Eigen::VectorXd::Random(
model.nv);
125 qddots[
i] = Eigen::VectorXd::Random(
model.nv);
126 taus[
i] = Eigen::VectorXd::Random(
model.nv);
134 std::cout <<
"RNEA = \t\t";
135 timer.
toc(std::cout, NBT);
140 rnea_code_gen.
evalFunction(
qs[_smooth], qdots[_smooth], qddots[_smooth]);
142 std::cout <<
"RNEA generated = \t\t";
143 timer.
toc(std::cout, NBT);
148 rnea_code_gen.
evalJacobian(
qs[_smooth], qdots[_smooth], qddots[_smooth]);
150 std::cout <<
"RNEA partial derivatives auto diff + code gen = \t\t";
151 timer.
toc(std::cout, NBT);
156 rnea_derivatives_code_gen.
evalFunction(
qs[_smooth], qdots[_smooth], qddots[_smooth]);
158 std::cout <<
"RNEA partial derivatives code gen = \t\t";
159 timer.
toc(std::cout, NBT);
173 std::cout <<
"CRBA = \t\t";
174 timer.
toc(std::cout, NBT);
181 std::cout <<
"CRBA generated = \t\t";
182 timer.
toc(std::cout, NBT);
189 std::cout <<
"Minv = \t\t";
190 timer.
toc(std::cout, NBT);
197 std::cout <<
"Minv generated = \t\t";
198 timer.
toc(std::cout, NBT);
205 std::cout <<
"ABA = \t\t";
206 timer.
toc(std::cout, NBT);
211 aba_code_gen.
evalFunction(
qs[_smooth], qdots[_smooth], taus[_smooth]);
213 std::cout <<
"ABA generated = \t\t";
214 timer.
toc(std::cout, NBT);
219 aba_code_gen.
evalJacobian(
qs[_smooth], qdots[_smooth], taus[_smooth]);
221 std::cout <<
"ABA partial derivatives auto diff + code gen = \t\t";
222 timer.
toc(std::cout, NBT);
227 aba_derivatives_code_gen.
evalFunction(
qs[_smooth], qdots[_smooth], qddots[_smooth]);
229 std::cout <<
"ABA partial derivatives code gen = \t\t";
230 timer.
toc(std::cout, NBT);
237 model,
data,
qs[_smooth], qdots[_smooth], qddots[_smooth], contact_models_6D6D,
240 model,
data, contact_models_6D6D, contact_datas_6D6D);
242 std::cout <<
"contact dynamics derivatives 6D,6D = \t\t";
243 timer.
toc(std::cout, NBT);
249 qs[_smooth], qdots[_smooth], qddots[_smooth]);
251 std::cout <<
"contact dynamics derivatives 6D,6D code gen = \t\t";
252 timer.
toc(std::cout, NBT);
void evalFunction(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVector1 > &v, const Eigen::MatrixBase< TangentVector2 > &tau)
void computeConstraintDynamicsDerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_data, const ProximalSettingsTpl< Scalar > &settings, const Eigen::MatrixBase< MatrixType1 > &ddq_partial_dq, const Eigen::MatrixBase< MatrixType2 > &ddq_partial_dv, const Eigen::MatrixBase< MatrixType3 > &ddq_partial_dtau, const Eigen::MatrixBase< MatrixType4 > &lambda_partial_dq, const Eigen::MatrixBase< MatrixType5 > &lambda_partial_dv, const Eigen::MatrixBase< MatrixType6 > &lambda_partial_dtau)
void evalFunction(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVector1 > &v, const Eigen::MatrixBase< TangentVector2 > &a)
void evalFunction(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVector1 > &v, const Eigen::MatrixBase< TangentVector2 > &a)
void humanoidRandom(ModelTpl< Scalar, Options, JointCollectionTpl > &model, bool usingFF=true)
Create a humanoid kinematic tree with 6-DOF limbs and random joint placements.
JointModelFreeFlyerTpl< Scalar > JointModelFreeFlyer
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 >::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...
int main(int argc, const char **argv)
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.
@ CONTACT_6D
Point contact model.
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 evalFunction(const Eigen::MatrixBase< ConfigVectorType > &q)
void loadLib(const bool generate_if_not_exist=true, const std::string &gcc_path="/usr/bin/gcc", const std::string &compile_options="-Ofast")
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...
void initConstraintDynamics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, Allocator > &contact_models)
Init the forward dynamics data according to the contact information contained in contact_models.
void evalFunction(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVector1 > &v, const Eigen::MatrixBase< TangentVector2 > &tau)
#define PINOCCHIO_STD_VECTOR_WITH_EIGEN_ALLOCATOR(T)
void evalJacobian(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVector1 > &v, const Eigen::MatrixBase< TangentVector2 > &tau)
std::vector< T, Eigen::aligned_allocator< T > > aligned_vector
void evalJacobian(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVector1 > &v, const Eigen::MatrixBase< TangentVector2 > &a)
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...
void evalFunction(const Eigen::MatrixBase< ConfigVectorType > &q)
void evalFunction(const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVector1 > &v, const Eigen::MatrixBase< TangentVector2 > &tau)
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & constraintDynamics(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 std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_datas, ProximalSettingsTpl< Scalar > &settings)
Computes the forward dynamics with contact constraints according to a given list of Contact informati...
Main pinocchio namespace.
#define PINOCCHIO_MODEL_DIR
pinocchio
Author(s):
autogenerated on Sat Jun 1 2024 02:40:38