5 #ifndef __pinocchio_utils_code_generator_algo_hpp__ 6 #define __pinocchio_utils_code_generator_algo_hpp__ 8 #ifdef PINOCCHIO_WITH_CPPADCG_SUPPORT 10 #include "pinocchio/codegen/code-generator-base.hpp" 12 #include "pinocchio/algorithm/joint-configuration.hpp" 13 #include "pinocchio/algorithm/crba.hpp" 14 #include "pinocchio/algorithm/rnea.hpp" 15 #include "pinocchio/algorithm/aba.hpp" 16 #include "pinocchio/algorithm/rnea-derivatives.hpp" 17 #include "pinocchio/algorithm/aba-derivatives.hpp" 21 template<
typename _Scalar>
25 typedef typename Base::Scalar Scalar;
28 typedef typename Base::ADConfigVectorType ADConfigVectorType;
29 typedef typename Base::ADTangentVectorType ADTangentVectorType;
30 typedef typename Base::MatrixXs MatrixXs;
31 typedef typename Base::VectorXs VectorXs;
38 ad_q = ADConfigVectorType(model.
nq); ad_q =
neutral(ad_model);
39 ad_v = ADTangentVectorType(model.
nv); ad_v.setZero();
40 ad_a = ADTangentVectorType(model.
nv); ad_a.setZero();
44 dtau_dq = MatrixXs::Zero(model.
nv,model.
nq);
45 dtau_dv = MatrixXs::Zero(model.
nv,model.
nv);
46 dtau_da = MatrixXs::Zero(model.
nv,model.
nv);
51 CppAD::Independent(ad_X);
53 Eigen::DenseIndex it = 0;
54 ad_q = ad_X.segment(it,ad_model.
nq); it += ad_model.
nq;
55 ad_v = ad_X.segment(it,ad_model.
nv); it += ad_model.
nv;
56 ad_a = ad_X.segment(it,ad_model.
nv); it += ad_model.
nv;
62 ad_fun.Dependent(ad_X,ad_Y);
63 ad_fun.optimize(
"no_compare_op");
66 using Base::evalFunction;
67 template<
typename ConfigVectorType,
typename TangentVector1,
typename TangentVector2>
68 void evalFunction(
const Eigen::MatrixBase<ConfigVectorType> & q,
69 const Eigen::MatrixBase<TangentVector1> & v,
70 const Eigen::MatrixBase<TangentVector2> & a)
73 Eigen::DenseIndex it = 0;
74 x.segment(it,ad_model.
nq) = q; it += ad_model.
nq;
75 x.segment(it,ad_model.
nv) = v; it += ad_model.
nv;
76 x.segment(it,ad_model.
nv) = a; it += ad_model.
nv;
82 using Base::evalJacobian;
83 template<
typename ConfigVectorType,
typename TangentVector1,
typename TangentVector2>
84 void evalJacobian(
const Eigen::MatrixBase<ConfigVectorType> & q,
85 const Eigen::MatrixBase<TangentVector1> & v,
86 const Eigen::MatrixBase<TangentVector2> & a)
89 Eigen::DenseIndex it = 0;
90 x.segment(it,ad_model.
nq) = q; it += ad_model.
nq;
91 x.segment(it,ad_model.
nv) = v; it += ad_model.
nv;
92 x.segment(it,ad_model.
nv) = a; it += ad_model.
nv;
96 dtau_dq = Base::jac.middleCols(it,ad_model.
nq); it += ad_model.
nq;
97 dtau_dv = Base::jac.middleCols(it,ad_model.
nv); it += ad_model.
nv;
98 dtau_da = Base::jac.middleCols(it,ad_model.
nv); it += ad_model.
nv;
101 MatrixXs dtau_dq, dtau_dv, dtau_da;
105 using Base::ad_model;
116 ADConfigVectorType ad_q, ad_q_plus;
117 ADTangentVectorType ad_dq, ad_v, ad_a;
120 template<
typename _Scalar>
124 typedef typename Base::Scalar Scalar;
127 typedef typename Base::ADConfigVectorType ADConfigVectorType;
128 typedef typename Base::ADTangentVectorType ADTangentVectorType;
129 typedef typename Base::MatrixXs MatrixXs;
130 typedef typename Base::VectorXs VectorXs;
137 ad_q = ADConfigVectorType(model.
nq); ad_q =
neutral(ad_model);
138 ad_v = ADTangentVectorType(model.
nv); ad_v.setZero();
139 ad_tau = ADTangentVectorType(model.
nv); ad_tau.setZero();
143 da_dq = MatrixXs::Zero(model.
nv,model.
nq);
144 da_dv = MatrixXs::Zero(model.
nv,model.
nv);
145 da_dtau = MatrixXs::Zero(model.
nv,model.
nv);
150 CppAD::Independent(ad_X);
152 Eigen::DenseIndex it = 0;
153 ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
154 ad_v = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
155 ad_tau = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
160 ad_fun.Dependent(ad_X,ad_Y);
161 ad_fun.optimize(
"no_compare_op");
164 using Base::evalFunction;
165 template<
typename ConfigVectorType,
typename TangentVector1,
typename TangentVector2>
166 void evalFunction(
const Eigen::MatrixBase<ConfigVectorType> & q,
167 const Eigen::MatrixBase<TangentVector1> & v,
168 const Eigen::MatrixBase<TangentVector2> & tau)
171 Eigen::DenseIndex it = 0;
172 x.segment(it,ad_model.nq) = q; it += ad_model.nq;
173 x.segment(it,ad_model.nv) = v; it += ad_model.nv;
174 x.segment(it,ad_model.nv) = tau; it += ad_model.nv;
180 using Base::evalJacobian;
181 template<
typename ConfigVectorType,
typename TangentVector1,
typename TangentVector2>
182 void evalJacobian(
const Eigen::MatrixBase<ConfigVectorType> & q,
183 const Eigen::MatrixBase<TangentVector1> & v,
184 const Eigen::MatrixBase<TangentVector2> & tau)
187 Eigen::DenseIndex it = 0;
188 x.segment(it,ad_model.nq) = q; it += ad_model.nq;
189 x.segment(it,ad_model.nv) = v; it += ad_model.nv;
190 x.segment(it,ad_model.nv) = tau; it += ad_model.nv;
195 da_dq = Base::jac.middleCols(it,ad_model.nq); it += ad_model.nq;
196 da_dv = Base::jac.middleCols(it,ad_model.nv); it += ad_model.nv;
197 da_dtau = Base::jac.middleCols(it,ad_model.nv); it += ad_model.nv;
200 MatrixXs da_dq,da_dv,da_dtau;
204 using Base::ad_model;
215 ADConfigVectorType ad_q, ad_q_plus;
216 ADTangentVectorType ad_dq, ad_v, ad_tau;
219 template<
typename _Scalar>
223 typedef typename Base::Scalar Scalar;
226 typedef typename Base::ADConfigVectorType ADConfigVectorType;
227 typedef typename Base::ADTangentVectorType ADTangentVectorType;
228 typedef typename Base::MatrixXs MatrixXs;
229 typedef typename Base::VectorXs VectorXs;
236 ad_q = ADConfigVectorType(model.
nq); ad_q =
neutral(ad_model);
240 M = MatrixXs::Zero(model.
nv,model.
nq);
247 CppAD::Independent(ad_X);
249 Eigen::DenseIndex it = 0;
250 ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
253 Eigen::DenseIndex it_Y = 0;
255 for(Eigen::DenseIndex i = 0; i < ad_model.nv; ++i)
257 for(Eigen::DenseIndex j = i; j < ad_model.nv; ++j)
259 ad_Y[it_Y++] = ad_data.M(i,j);
265 ad_fun.Dependent(ad_X,ad_Y);
266 ad_fun.optimize(
"no_compare_op");
269 template<
typename ConfigVectorType>
270 void evalFunction(
const Eigen::MatrixBase<ConfigVectorType> & q)
273 Eigen::DenseIndex it = 0;
274 x.segment(it,ad_model.nq) = q; it += ad_model.nq;
276 Base::evalFunction(x);
279 Eigen::DenseIndex it_Y = 0;
280 for(Eigen::DenseIndex i = 0; i < ad_model.nv; ++i)
282 for(Eigen::DenseIndex j = i; j < ad_model.nv; ++j)
284 M(i,j) = Base::y[it_Y++];
295 using Base::ad_model;
305 ADConfigVectorType ad_q;
308 template<
typename _Scalar>
312 typedef typename Base::Scalar Scalar;
315 typedef typename Base::ADConfigVectorType ADConfigVectorType;
316 typedef typename Base::ADTangentVectorType ADTangentVectorType;
317 typedef typename Base::MatrixXs MatrixXs;
318 typedef typename Base::VectorXs VectorXs;
325 ad_q = ADConfigVectorType(model.
nq); ad_q =
neutral(ad_model);
329 Minv = MatrixXs::Zero(model.
nv,model.
nq);
336 CppAD::Independent(ad_X);
338 Eigen::DenseIndex it = 0;
339 ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
342 Eigen::DenseIndex it_Y = 0;
343 for(Eigen::DenseIndex i = 0; i < ad_model.nv; ++i)
345 for(Eigen::DenseIndex j = i; j < ad_model.nv; ++j)
347 ad_Y[it_Y++] = ad_data.Minv(i,j);
353 ad_fun.Dependent(ad_X,ad_Y);
354 ad_fun.optimize(
"no_compare_op");
357 template<
typename ConfigVectorType>
358 void evalFunction(
const Eigen::MatrixBase<ConfigVectorType> & q)
361 Eigen::DenseIndex it = 0;
362 x.segment(it,ad_model.nq) = q; it += ad_model.nq;
364 Base::evalFunction(x);
367 Eigen::DenseIndex it_Y = 0;
368 for(Eigen::DenseIndex i = 0; i < ad_model.nv; ++i)
370 for(Eigen::DenseIndex j = i; j < ad_model.nv; ++j)
372 Minv(i,j) = Base::y[it_Y++];
381 using Base::ad_model;
391 ADConfigVectorType ad_q;
394 template<
typename _Scalar>
398 typedef typename Base::Scalar Scalar;
401 typedef typename Base::ADConfigVectorType ADConfigVectorType;
402 typedef typename Base::ADTangentVectorType ADTangentVectorType;
403 typedef typename Base::MatrixXs MatrixXs;
404 typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXs) RowMatrixXs;
405 typedef typename Base::VectorXs VectorXs;
408 typedef typename ADData::MatrixXs ADMatrixXs;
409 typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(ADMatrixXs) RowADMatrixXs;
413 const std::string &
library_name =
"cg_partial_rnea_eval")
416 ad_q = ADConfigVectorType(model.
nq); ad_q =
neutral(ad_model);
417 ad_v = ADTangentVectorType(model.
nv); ad_v.setZero();
418 ad_a = ADTangentVectorType(model.
nv); ad_a.setZero();
423 ad_dtau_dq = ADMatrixXs::Zero(model.
nv,model.
nv);
424 ad_dtau_dv = ADMatrixXs::Zero(model.
nv,model.
nv);
425 ad_dtau_da = ADMatrixXs::Zero(model.
nv,model.
nv);
427 dtau_dq = MatrixXs::Zero(model.
nv,model.
nv);
428 dtau_dv = MatrixXs::Zero(model.
nv,model.
nv);
429 dtau_da = MatrixXs::Zero(model.
nv,model.
nv);
436 CppAD::Independent(ad_X);
438 Eigen::DenseIndex it = 0;
439 ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
440 ad_v = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
441 ad_a = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
445 ad_dtau_dq,ad_dtau_dv,ad_dtau_da);
449 Eigen::DenseIndex it_Y = 0;
450 Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dtau_dq;
451 it_Y += ad_model.nv*ad_model.nv;
452 Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dtau_dv;
453 it_Y += ad_model.nv*ad_model.nv;
454 Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dtau_da;
455 it_Y += ad_model.nv*ad_model.nv;
457 ad_fun.Dependent(ad_X,ad_Y);
458 ad_fun.optimize(
"no_compare_op");
461 template<
typename ConfigVectorType,
typename TangentVector1,
typename TangentVector2>
462 void evalFunction(
const Eigen::MatrixBase<ConfigVectorType> & q,
463 const Eigen::MatrixBase<TangentVector1> & v,
464 const Eigen::MatrixBase<TangentVector2> & a)
467 Eigen::DenseIndex it_x = 0;
468 x.segment(it_x,ad_model.nq) = q; it_x += ad_model.nq;
469 x.segment(it_x,ad_model.nq) = v; it_x += ad_model.nv;
470 x.segment(it_x,ad_model.nq) = a; it_x += ad_model.nv;
472 Base::evalFunction(x);
475 Eigen::DenseIndex it_y = 0;
476 dtau_dq = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
477 it_y += ad_model.nv*ad_model.nv;
478 dtau_dv = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
479 it_y += ad_model.nv*ad_model.nv;
480 dtau_da = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
481 it_y += ad_model.nv*ad_model.nv;
487 using Base::ad_model;
495 VectorXs partial_derivatives;
496 ADMatrixXs ad_dtau_dq, ad_dtau_dv, ad_dtau_da;
497 MatrixXs dtau_dq, dtau_dv, dtau_da;
499 ADConfigVectorType ad_q;
500 ADTangentVectorType ad_v, ad_a;
503 template<
typename _Scalar>
507 typedef typename Base::Scalar Scalar;
510 typedef typename Base::ADConfigVectorType ADConfigVectorType;
511 typedef typename Base::ADTangentVectorType ADTangentVectorType;
512 typedef typename Base::MatrixXs MatrixXs;
513 typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXs) RowMatrixXs;
514 typedef typename Base::VectorXs VectorXs;
517 typedef typename ADData::MatrixXs ADMatrixXs;
518 typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(ADMatrixXs) RowADMatrixXs;
522 const std::string &
library_name =
"cg_partial_aba_eval")
525 ad_q = ADConfigVectorType(model.
nq); ad_q =
neutral(ad_model);
526 ad_v = ADTangentVectorType(model.
nv); ad_v.setZero();
527 ad_tau = ADTangentVectorType(model.
nv); ad_tau.setZero();
532 ad_dddq_dq = ADMatrixXs::Zero(model.
nv,model.
nv);
533 ad_dddq_dv = ADMatrixXs::Zero(model.
nv,model.
nv);
534 ad_dddq_dtau = ADMatrixXs::Zero(model.
nv,model.
nv);
536 dddq_dq = MatrixXs::Zero(model.
nv,model.
nv);
537 dddq_dv = MatrixXs::Zero(model.
nv,model.
nv);
538 dddq_dtau = MatrixXs::Zero(model.
nv,model.
nv);
545 CppAD::Independent(ad_X);
547 Eigen::DenseIndex it = 0;
548 ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
549 ad_v = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
550 ad_tau = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
554 ad_dddq_dq,ad_dddq_dv,ad_dddq_dtau);
558 Eigen::DenseIndex it_Y = 0;
559 Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dddq_dq;
560 it_Y += ad_model.nv*ad_model.nv;
561 Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dddq_dv;
562 it_Y += ad_model.nv*ad_model.nv;
563 Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dddq_dtau;
564 it_Y += ad_model.nv*ad_model.nv;
566 ad_fun.Dependent(ad_X,ad_Y);
567 ad_fun.optimize(
"no_compare_op");
570 template<
typename ConfigVectorType,
typename TangentVector1,
typename TangentVector2>
571 void evalFunction(
const Eigen::MatrixBase<ConfigVectorType> & q,
572 const Eigen::MatrixBase<TangentVector1> & v,
573 const Eigen::MatrixBase<TangentVector2> & tau)
576 Eigen::DenseIndex it_x = 0;
577 x.segment(it_x,ad_model.nq) = q; it_x += ad_model.nq;
578 x.segment(it_x,ad_model.nq) = v; it_x += ad_model.nv;
579 x.segment(it_x,ad_model.nq) = tau; it_x += ad_model.nv;
581 Base::evalFunction(x);
584 Eigen::DenseIndex it_y = 0;
585 dddq_dq = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
586 it_y += ad_model.nv*ad_model.nv;
587 dddq_dv = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
588 it_y += ad_model.nv*ad_model.nv;
589 dddq_dtau = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
590 it_y += ad_model.nv*ad_model.nv;
596 using Base::ad_model;
604 VectorXs partial_derivatives;
605 ADMatrixXs ad_dddq_dq, ad_dddq_dv, ad_dddq_dtau;
606 MatrixXs dddq_dq, dddq_dv, dddq_dtau;
608 ADConfigVectorType ad_q;
609 ADTangentVectorType ad_v, ad_tau;
614 #endif // ifdef PINOCCHIO_WITH_CPPADCG_SUPPORT 616 #endif // ifndef __pinocchio_utils_code_generator_base_hpp__ 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...
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...
Eigen::DenseIndex getOutputDimension() const
Dimension of the output vector.
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.
const std::string function_name
Name of the function.
Eigen::DenseIndex getInputDimension() const
Dimension of the input vector.
void buildMap()
build the mapping Y = f(X)
void buildMap()
build the mapping Y = f(X)
void buildMap()
build the mapping Y = f(X)
int nv
Dimension of the velocity vector space.
void computeRNEADerivatives(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, const Eigen::MatrixBase< MatrixType1 > &rnea_partial_dq, const Eigen::MatrixBase< MatrixType2 > &rnea_partial_dv, const Eigen::MatrixBase< MatrixType3 > &rnea_partial_da)
Computes the partial derivatives of the Recursive Newton Euler Algorithms with respect to the joint c...
const std::string library_name
Name of the library.
bool build_jacobian
Options to build or not the Jacobian of he function.
void buildMap()
build the mapping Y = f(X)
Main pinocchio namespace.
TangentVectorType tau
Vector of joint torques (dim model.nv).
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
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 buildMap()
build the mapping Y = f(X)
void computeABADerivatives(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 Eigen::MatrixBase< MatrixType1 > &aba_partial_dq, const Eigen::MatrixBase< MatrixType2 > &aba_partial_dv, const Eigen::MatrixBase< MatrixType3 > &aba_partial_dtau)
The derivatives of the Articulated-Body algorithm.
void buildMap()
build the mapping Y = f(X)
int nq
Dimension of the configuration vector representation.