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::ADCongigVectorType ADCongigVectorType;
29 typedef typename Base::ADTangentVectorType ADTangentVectorType;
30 typedef typename Base::MatrixXs MatrixXs;
31 typedef typename Base::VectorXs VectorXs;
38 ad_q = ADCongigVectorType(model.
nq); ad_q =
neutral(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;
103 using Base::ad_model;
113 MatrixXs dtau_dq, dtau_dv, dtau_da;
115 ADCongigVectorType ad_q, ad_q_plus;
116 ADTangentVectorType ad_dq, ad_v, ad_a;
119 template<
typename _Scalar>
123 typedef typename Base::Scalar Scalar;
126 typedef typename Base::ADCongigVectorType ADCongigVectorType;
127 typedef typename Base::ADTangentVectorType ADTangentVectorType;
128 typedef typename Base::MatrixXs MatrixXs;
129 typedef typename Base::VectorXs VectorXs;
136 ad_q = ADCongigVectorType(model.
nq); ad_q =
neutral(model);
137 ad_v = ADTangentVectorType(model.
nv); ad_v.setZero();
138 ad_tau = ADTangentVectorType(model.
nv); ad_tau.setZero();
142 da_dq = MatrixXs::Zero(model.
nv,model.
nq);
143 da_dv = MatrixXs::Zero(model.
nv,model.
nv);
144 da_dtau = MatrixXs::Zero(model.
nv,model.
nv);
149 CppAD::Independent(ad_X);
151 Eigen::DenseIndex it = 0;
152 ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
153 ad_v = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
154 ad_tau = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
159 ad_fun.Dependent(ad_X,ad_Y);
160 ad_fun.optimize(
"no_compare_op");
163 using Base::evalFunction;
164 template<
typename ConfigVectorType,
typename TangentVector1,
typename TangentVector2>
165 void evalFunction(
const Eigen::MatrixBase<ConfigVectorType> & q,
166 const Eigen::MatrixBase<TangentVector1> & v,
167 const Eigen::MatrixBase<TangentVector2> & tau)
170 Eigen::DenseIndex it = 0;
171 x.segment(it,ad_model.nq) = q; it += ad_model.nq;
172 x.segment(it,ad_model.nv) = v; it += ad_model.nv;
173 x.segment(it,ad_model.nv) = tau; it += ad_model.nv;
179 using Base::evalJacobian;
180 template<
typename ConfigVectorType,
typename TangentVector1,
typename TangentVector2>
181 void evalJacobian(
const Eigen::MatrixBase<ConfigVectorType> & q,
182 const Eigen::MatrixBase<TangentVector1> & v,
183 const Eigen::MatrixBase<TangentVector2> & tau)
186 Eigen::DenseIndex it = 0;
187 x.segment(it,ad_model.nq) = q; it += ad_model.nq;
188 x.segment(it,ad_model.nv) = v; it += ad_model.nv;
189 x.segment(it,ad_model.nv) = tau; it += ad_model.nv;
194 da_dq = Base::jac.middleCols(it,ad_model.nq); it += ad_model.nq;
195 da_dv = Base::jac.middleCols(it,ad_model.nv); it += ad_model.nv;
196 da_dtau = Base::jac.middleCols(it,ad_model.nv); it += ad_model.nv;
201 using Base::ad_model;
211 MatrixXs da_dq,da_dv,da_dtau;
213 ADCongigVectorType ad_q, ad_q_plus;
214 ADTangentVectorType ad_dq, ad_v, ad_tau;
217 template<
typename _Scalar>
221 typedef typename Base::Scalar Scalar;
224 typedef typename Base::ADCongigVectorType ADCongigVectorType;
225 typedef typename Base::ADTangentVectorType ADTangentVectorType;
226 typedef typename Base::MatrixXs MatrixXs;
227 typedef typename Base::VectorXs VectorXs;
234 ad_q = ADCongigVectorType(model.
nq); ad_q =
neutral(model);
238 M = MatrixXs::Zero(model.
nv,model.
nq);
245 CppAD::Independent(ad_X);
247 Eigen::DenseIndex it = 0;
248 ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
251 Eigen::DenseIndex it_Y = 0;
253 for(Eigen::DenseIndex i = 0; i < ad_model.nv; ++i)
255 for(Eigen::DenseIndex j = i; j < ad_model.nv; ++j)
257 ad_Y[it_Y++] = ad_data.M(i,j);
263 ad_fun.Dependent(ad_X,ad_Y);
264 ad_fun.optimize(
"no_compare_op");
267 template<
typename ConfigVectorType>
268 void evalFunction(
const Eigen::MatrixBase<ConfigVectorType> & q)
271 Eigen::DenseIndex it = 0;
272 x.segment(it,ad_model.nq) = q; it += ad_model.nq;
274 Base::evalFunction(x);
277 Eigen::DenseIndex it_Y = 0;
278 for(Eigen::DenseIndex i = 0; i < ad_model.nv; ++i)
280 for(Eigen::DenseIndex j = i; j < ad_model.nv; ++j)
282 M(i,j) = Base::y[it_Y++];
291 using Base::ad_model;
302 ADCongigVectorType ad_q;
305 template<
typename _Scalar>
309 typedef typename Base::Scalar Scalar;
312 typedef typename Base::ADCongigVectorType ADCongigVectorType;
313 typedef typename Base::ADTangentVectorType ADTangentVectorType;
314 typedef typename Base::MatrixXs MatrixXs;
315 typedef typename Base::VectorXs VectorXs;
322 ad_q = ADCongigVectorType(model.
nq); ad_q =
neutral(model);
326 Minv = MatrixXs::Zero(model.
nv,model.
nq);
333 CppAD::Independent(ad_X);
335 Eigen::DenseIndex it = 0;
336 ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
339 Eigen::DenseIndex it_Y = 0;
340 for(Eigen::DenseIndex i = 0; i < ad_model.nv; ++i)
342 for(Eigen::DenseIndex j = i; j < ad_model.nv; ++j)
344 ad_Y[it_Y++] = ad_data.Minv(i,j);
350 ad_fun.Dependent(ad_X,ad_Y);
351 ad_fun.optimize(
"no_compare_op");
354 template<
typename ConfigVectorType>
355 void evalFunction(
const Eigen::MatrixBase<ConfigVectorType> & q)
358 Eigen::DenseIndex it = 0;
359 x.segment(it,ad_model.nq) = q; it += ad_model.nq;
361 Base::evalFunction(x);
364 Eigen::DenseIndex it_Y = 0;
365 for(Eigen::DenseIndex i = 0; i < ad_model.nv; ++i)
367 for(Eigen::DenseIndex j = i; j < ad_model.nv; ++j)
369 Minv(i,j) = Base::y[it_Y++];
376 using Base::ad_model;
387 ADCongigVectorType ad_q;
390 template<
typename _Scalar>
394 typedef typename Base::Scalar Scalar;
397 typedef typename Base::ADCongigVectorType ADCongigVectorType;
398 typedef typename Base::ADTangentVectorType ADTangentVectorType;
399 typedef typename Base::MatrixXs MatrixXs;
400 typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXs) RowMatrixXs;
401 typedef typename Base::VectorXs VectorXs;
404 typedef typename ADData::MatrixXs ADMatrixXs;
405 typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(ADMatrixXs) RowADMatrixXs;
409 const std::string &
library_name =
"cg_partial_rnea_eval")
412 ad_q = ADCongigVectorType(model.
nq); ad_q =
neutral(model);
413 ad_v = ADTangentVectorType(model.
nv); ad_v.setZero();
414 ad_a = ADTangentVectorType(model.
nv); ad_a.setZero();
419 ad_dtau_dq = ADMatrixXs::Zero(model.
nv,model.
nv);
420 ad_dtau_dv = ADMatrixXs::Zero(model.
nv,model.
nv);
421 ad_dtau_da = ADMatrixXs::Zero(model.
nv,model.
nv);
423 dtau_dq = MatrixXs::Zero(model.
nv,model.
nv);
424 dtau_dv = MatrixXs::Zero(model.
nv,model.
nv);
425 dtau_da = MatrixXs::Zero(model.
nv,model.
nv);
432 CppAD::Independent(ad_X);
434 Eigen::DenseIndex it = 0;
435 ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
436 ad_v = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
437 ad_a = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
441 ad_dtau_dq,ad_dtau_dv,ad_dtau_da);
445 Eigen::DenseIndex it_Y = 0;
446 Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dtau_dq;
447 it_Y += ad_model.nv*ad_model.nv;
448 Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dtau_dv;
449 it_Y += ad_model.nv*ad_model.nv;
450 Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dtau_da;
451 it_Y += ad_model.nv*ad_model.nv;
453 ad_fun.Dependent(ad_X,ad_Y);
454 ad_fun.optimize(
"no_compare_op");
457 template<
typename ConfigVectorType,
typename TangentVector1,
typename TangentVector2>
458 void evalFunction(
const Eigen::MatrixBase<ConfigVectorType> & q,
459 const Eigen::MatrixBase<TangentVector1> & v,
460 const Eigen::MatrixBase<TangentVector2> & a)
463 Eigen::DenseIndex it_x = 0;
464 x.segment(it_x,ad_model.nq) = q; it_x += ad_model.nq;
465 x.segment(it_x,ad_model.nq) = v; it_x += ad_model.nv;
466 x.segment(it_x,ad_model.nq) = a; it_x += ad_model.nv;
468 Base::evalFunction(x);
471 Eigen::DenseIndex it_y = 0;
472 dtau_dq = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
473 it_y += ad_model.nv*ad_model.nv;
474 dtau_dv = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
475 it_y += ad_model.nv*ad_model.nv;
476 dtau_da = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
477 it_y += ad_model.nv*ad_model.nv;
483 using Base::ad_model;
491 VectorXs partial_derivatives;
492 ADMatrixXs ad_dtau_dq, ad_dtau_dv, ad_dtau_da;
493 MatrixXs dtau_dq, dtau_dv, dtau_da;
495 ADCongigVectorType ad_q;
496 ADTangentVectorType ad_v, ad_a;
499 template<
typename _Scalar>
503 typedef typename Base::Scalar Scalar;
506 typedef typename Base::ADCongigVectorType ADCongigVectorType;
507 typedef typename Base::ADTangentVectorType ADTangentVectorType;
508 typedef typename Base::MatrixXs MatrixXs;
509 typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXs) RowMatrixXs;
510 typedef typename Base::VectorXs VectorXs;
513 typedef typename ADData::MatrixXs ADMatrixXs;
514 typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(ADMatrixXs) RowADMatrixXs;
518 const std::string &
library_name =
"cg_partial_aba_eval")
521 ad_q = ADCongigVectorType(model.
nq); ad_q =
neutral(model);
522 ad_v = ADTangentVectorType(model.
nv); ad_v.setZero();
523 ad_tau = ADTangentVectorType(model.
nv); ad_tau.setZero();
528 ad_dddq_dq = ADMatrixXs::Zero(model.
nv,model.
nv);
529 ad_dddq_dv = ADMatrixXs::Zero(model.
nv,model.
nv);
530 ad_dddq_dtau = ADMatrixXs::Zero(model.
nv,model.
nv);
532 dddq_dq = MatrixXs::Zero(model.
nv,model.
nv);
533 dddq_dv = MatrixXs::Zero(model.
nv,model.
nv);
534 dddq_dtau = MatrixXs::Zero(model.
nv,model.
nv);
541 CppAD::Independent(ad_X);
543 Eigen::DenseIndex it = 0;
544 ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
545 ad_v = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
546 ad_tau = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
550 ad_dddq_dq,ad_dddq_dv,ad_dddq_dtau);
554 Eigen::DenseIndex it_Y = 0;
555 Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dddq_dq;
556 it_Y += ad_model.nv*ad_model.nv;
557 Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dddq_dv;
558 it_Y += ad_model.nv*ad_model.nv;
559 Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dddq_dtau;
560 it_Y += ad_model.nv*ad_model.nv;
562 ad_fun.Dependent(ad_X,ad_Y);
563 ad_fun.optimize(
"no_compare_op");
566 template<
typename ConfigVectorType,
typename TangentVector1,
typename TangentVector2>
567 void evalFunction(
const Eigen::MatrixBase<ConfigVectorType> & q,
568 const Eigen::MatrixBase<TangentVector1> & v,
569 const Eigen::MatrixBase<TangentVector2> & tau)
572 Eigen::DenseIndex it_x = 0;
573 x.segment(it_x,ad_model.nq) = q; it_x += ad_model.nq;
574 x.segment(it_x,ad_model.nq) = v; it_x += ad_model.nv;
575 x.segment(it_x,ad_model.nq) = tau; it_x += ad_model.nv;
577 Base::evalFunction(x);
580 Eigen::DenseIndex it_y = 0;
581 dddq_dq = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
582 it_y += ad_model.nv*ad_model.nv;
583 dddq_dv = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
584 it_y += ad_model.nv*ad_model.nv;
585 dddq_dtau = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
586 it_y += ad_model.nv*ad_model.nv;
592 using Base::ad_model;
600 VectorXs partial_derivatives;
601 ADMatrixXs ad_dddq_dq, ad_dddq_dv, ad_dddq_dtau;
602 MatrixXs dddq_dq, dddq_dv, dddq_dtau;
604 ADCongigVectorType ad_q;
605 ADTangentVectorType ad_v, ad_tau;
610 #endif // ifdef PINOCCHIO_WITH_CPPADCG_SUPPORT 612 #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.