pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
code-generator-algo.hpp
1//
2// Copyright (c) 2018-2020 CNRS INRIA
3//
4
5#ifndef __pinocchio_codegen_code_generator_algo_hpp__
6#define __pinocchio_codegen_code_generator_algo_hpp__
7
8#include "pinocchio/codegen/code-generator-base.hpp"
9
10#include "pinocchio/algorithm/joint-configuration.hpp"
11#include "pinocchio/algorithm/crba.hpp"
12#include "pinocchio/algorithm/rnea.hpp"
13#include "pinocchio/algorithm/aba.hpp"
14#include "pinocchio/algorithm/rnea-derivatives.hpp"
15#include "pinocchio/algorithm/constrained-dynamics-derivatives.hpp"
16#include "pinocchio/algorithm/constrained-dynamics.hpp"
17#include "pinocchio/algorithm/aba-derivatives.hpp"
18
19namespace pinocchio
20{
21 template<typename _Scalar>
22 struct CodeGenRNEA : public CodeGenBase<_Scalar>
23 {
24 typedef CodeGenBase<_Scalar> Base;
25 typedef typename Base::Scalar Scalar;
26
27 typedef typename Base::Model Model;
28 typedef typename Base::ADConfigVectorType ADConfigVectorType;
29 typedef typename Base::ADTangentVectorType ADTangentVectorType;
30 typedef typename Base::MatrixXs MatrixXs;
31 typedef typename Base::VectorXs VectorXs;
32
33 CodeGenRNEA(
34 const Model & model,
35 const std::string & function_name = "rnea",
36 const std::string & library_name = "cg_rnea_eval")
37 : Base(model, model.nq + 2 * model.nv, model.nv, function_name, library_name)
38 {
39 ad_q = ADConfigVectorType(model.nq);
40 ad_q = neutral(ad_model);
41 ad_v = ADTangentVectorType(model.nv);
42 ad_v.setZero();
43 ad_a = ADTangentVectorType(model.nv);
44 ad_a.setZero();
45 x = VectorXs::Zero(Base::getInputDimension());
46 res = VectorXs::Zero(Base::getOutputDimension());
47
48 dtau_dq = MatrixXs::Zero(model.nv, model.nq);
49 dtau_dv = MatrixXs::Zero(model.nv, model.nv);
50 dtau_da = MatrixXs::Zero(model.nv, model.nv);
51 }
52
53 virtual ~CodeGenRNEA()
54 {
55 }
56
57 void buildMap()
58 {
59 CppAD::Independent(ad_X);
60
61 Eigen::DenseIndex it = 0;
62 ad_q = ad_X.segment(it, ad_model.nq);
63 it += ad_model.nq;
64 ad_v = ad_X.segment(it, ad_model.nv);
65 it += ad_model.nv;
66 ad_a = ad_X.segment(it, ad_model.nv);
67 it += ad_model.nv;
68
69 pinocchio::rnea(ad_model, ad_data, ad_q, ad_v, ad_a);
70
71 ad_Y = ad_data.tau;
72
73 ad_fun.Dependent(ad_X, ad_Y);
74 ad_fun.optimize("no_compare_op");
75 }
76
77 using Base::evalFunction;
78 template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
79 void evalFunction(
80 const Eigen::MatrixBase<ConfigVectorType> & q,
81 const Eigen::MatrixBase<TangentVector1> & v,
82 const Eigen::MatrixBase<TangentVector2> & a)
83 {
84 // fill x
85 Eigen::DenseIndex it = 0;
86 x.segment(it, ad_model.nq) = q;
87 it += ad_model.nq;
88 x.segment(it, ad_model.nv) = v;
89 it += ad_model.nv;
90 x.segment(it, ad_model.nv) = a;
91 it += ad_model.nv;
92
93 evalFunction(x);
94 res = Base::y;
95 }
96
97 using Base::evalJacobian;
98 template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
99 void evalJacobian(
100 const Eigen::MatrixBase<ConfigVectorType> & q,
101 const Eigen::MatrixBase<TangentVector1> & v,
102 const Eigen::MatrixBase<TangentVector2> & a)
103 {
104 // fill x
105 Eigen::DenseIndex it = 0;
106 x.segment(it, ad_model.nq) = q;
107 it += ad_model.nq;
108 x.segment(it, ad_model.nv) = v;
109 it += ad_model.nv;
110 x.segment(it, ad_model.nv) = a;
111 it += ad_model.nv;
112
113 evalJacobian(x);
114 it = 0;
115 dtau_dq = Base::jac.middleCols(it, ad_model.nq);
116 it += ad_model.nq;
117 dtau_dv = Base::jac.middleCols(it, ad_model.nv);
118 it += ad_model.nv;
119 dtau_da = Base::jac.middleCols(it, ad_model.nv);
120 it += ad_model.nv;
121 }
122
123 MatrixXs dtau_dq, dtau_dv, dtau_da;
124
125 protected:
126 using Base::ad_data;
127 using Base::ad_fun;
128 using Base::ad_model;
129 using Base::ad_X;
130 using Base::ad_Y;
131 using Base::jac;
132 using Base::y;
133
134 VectorXs x;
135 VectorXs res;
136
137 ADConfigVectorType ad_q, ad_q_plus;
138 ADTangentVectorType ad_dq, ad_v, ad_a;
139 };
140
141 template<typename _Scalar>
142 struct CodeGenABA : public CodeGenBase<_Scalar>
143 {
144 typedef CodeGenBase<_Scalar> Base;
145 typedef typename Base::Scalar Scalar;
146
147 typedef typename Base::Model Model;
148 typedef typename Base::ADConfigVectorType ADConfigVectorType;
149 typedef typename Base::ADTangentVectorType ADTangentVectorType;
150 typedef typename Base::MatrixXs MatrixXs;
151 typedef typename Base::VectorXs VectorXs;
152
153 CodeGenABA(
154 const Model & model,
155 const std::string & function_name = "aba",
156 const std::string & library_name = "cg_aba_eval")
157 : Base(model, model.nq + 2 * model.nv, model.nv, function_name, library_name)
158 {
159 ad_q = ADConfigVectorType(model.nq);
160 ad_q = neutral(ad_model);
161 ad_v = ADTangentVectorType(model.nv);
162 ad_v.setZero();
163 ad_tau = ADTangentVectorType(model.nv);
164 ad_tau.setZero();
165 x = VectorXs::Zero(Base::getInputDimension());
166 res = VectorXs::Zero(Base::getOutputDimension());
167
168 da_dq = MatrixXs::Zero(model.nv, model.nq);
169 da_dv = MatrixXs::Zero(model.nv, model.nv);
170 da_dtau = MatrixXs::Zero(model.nv, model.nv);
171 }
172
173 virtual ~CodeGenABA()
174 {
175 }
176
177 void buildMap()
178 {
179 CppAD::Independent(ad_X);
180
181 Eigen::DenseIndex it = 0;
182 ad_q = ad_X.segment(it, ad_model.nq);
183 it += ad_model.nq;
184 ad_v = ad_X.segment(it, ad_model.nv);
185 it += ad_model.nv;
186 ad_tau = ad_X.segment(it, ad_model.nv);
187 it += ad_model.nv;
188
189 pinocchio::aba(ad_model, ad_data, ad_q, ad_v, ad_tau, Convention::WORLD);
190 ad_Y = ad_data.ddq;
191
192 ad_fun.Dependent(ad_X, ad_Y);
193 ad_fun.optimize("no_compare_op");
194 }
195
196 using Base::evalFunction;
197 template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
198 void evalFunction(
199 const Eigen::MatrixBase<ConfigVectorType> & q,
200 const Eigen::MatrixBase<TangentVector1> & v,
201 const Eigen::MatrixBase<TangentVector2> & tau)
202 {
203 // fill x
204 Eigen::DenseIndex it = 0;
205 x.segment(it, ad_model.nq) = q;
206 it += ad_model.nq;
207 x.segment(it, ad_model.nv) = v;
208 it += ad_model.nv;
209 x.segment(it, ad_model.nv) = tau;
210 it += ad_model.nv;
211
212 evalFunction(x);
213 res = Base::y;
214 }
215
216 using Base::evalJacobian;
217 template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
218 void evalJacobian(
219 const Eigen::MatrixBase<ConfigVectorType> & q,
220 const Eigen::MatrixBase<TangentVector1> & v,
221 const Eigen::MatrixBase<TangentVector2> & tau)
222 {
223 // fill x
224 Eigen::DenseIndex it = 0;
225 x.segment(it, ad_model.nq) = q;
226 it += ad_model.nq;
227 x.segment(it, ad_model.nv) = v;
228 it += ad_model.nv;
229 x.segment(it, ad_model.nv) = tau;
230 it += ad_model.nv;
231
232 evalJacobian(x);
233
234 it = 0;
235 da_dq = Base::jac.middleCols(it, ad_model.nq);
236 it += ad_model.nq;
237 da_dv = Base::jac.middleCols(it, ad_model.nv);
238 it += ad_model.nv;
239 da_dtau = Base::jac.middleCols(it, ad_model.nv);
240 it += ad_model.nv;
241 }
242
243 MatrixXs da_dq, da_dv, da_dtau;
244
245 protected:
246 using Base::ad_data;
247 using Base::ad_fun;
248 using Base::ad_model;
249 using Base::ad_X;
250 using Base::ad_Y;
251 using Base::jac;
252 using Base::y;
253
254 VectorXs x;
255 VectorXs res;
256
257 ADConfigVectorType ad_q, ad_q_plus;
258 ADTangentVectorType ad_dq, ad_v, ad_tau;
259 };
260
261 template<typename _Scalar>
262 struct CodeGenCRBA : public CodeGenBase<_Scalar>
263 {
264 typedef CodeGenBase<_Scalar> Base;
265 typedef typename Base::Scalar Scalar;
266
267 typedef typename Base::Model Model;
268 typedef typename Base::ADConfigVectorType ADConfigVectorType;
269 typedef typename Base::ADTangentVectorType ADTangentVectorType;
270 typedef typename Base::MatrixXs MatrixXs;
271 typedef typename Base::VectorXs VectorXs;
272
273 CodeGenCRBA(
274 const Model & model,
275 const std::string & function_name = "crba",
276 const std::string & library_name = "cg_crba_eval")
277 : Base(model, model.nq, (model.nv * (model.nv + 1)) / 2, function_name, library_name)
278 {
279 ad_q = ADConfigVectorType(model.nq);
280 ad_q = neutral(ad_model);
281 x = VectorXs::Zero(Base::getInputDimension());
282 res = VectorXs::Zero(Base::getOutputDimension());
283
284 M = MatrixXs::Zero(model.nv, model.nq);
285
286 Base::build_jacobian = false;
287 }
288
289 virtual ~CodeGenCRBA()
290 {
291 }
292
293 void buildMap()
294 {
295 CppAD::Independent(ad_X);
296
297 Eigen::DenseIndex it = 0;
298 ad_q = ad_X.segment(it, ad_model.nq);
299 it += ad_model.nq;
300
301 pinocchio::crba(ad_model, ad_data, ad_q, pinocchio::Convention::WORLD);
302 Eigen::DenseIndex it_Y = 0;
303
304 for (Eigen::DenseIndex i = 0; i < ad_model.nv; ++i)
305 {
306 for (Eigen::DenseIndex j = i; j < ad_model.nv; ++j)
307 {
308 ad_Y[it_Y++] = ad_data.M(i, j);
309 }
310 }
311
312 assert(it_Y == Base::getOutputDimension());
313
314 ad_fun.Dependent(ad_X, ad_Y);
315 ad_fun.optimize("no_compare_op");
316 }
317
318 template<typename ConfigVectorType>
319 void evalFunction(const Eigen::MatrixBase<ConfigVectorType> & q)
320 {
321 // fill x
322 Eigen::DenseIndex it = 0;
323 x.segment(it, ad_model.nq) = q;
324 it += ad_model.nq;
325
326 Base::evalFunction(x);
327
328 // fill M
329 Eigen::DenseIndex it_Y = 0;
330 for (Eigen::DenseIndex i = 0; i < ad_model.nv; ++i)
331 {
332 for (Eigen::DenseIndex j = i; j < ad_model.nv; ++j)
333 {
334 M(i, j) = Base::y[it_Y++];
335 }
336 }
337
338 assert(it_Y == Base::getOutputDimension());
339 }
340
341 MatrixXs M;
342
343 protected:
344 using Base::ad_data;
345 using Base::ad_fun;
346 using Base::ad_model;
347 using Base::ad_X;
348 using Base::ad_Y;
349 using Base::y;
350
351 VectorXs x;
352 VectorXs res;
353
354 ADConfigVectorType ad_q;
355 };
356
357 template<typename _Scalar>
358 struct CodeGenMinv : public CodeGenBase<_Scalar>
359 {
360 typedef CodeGenBase<_Scalar> Base;
361 typedef typename Base::Scalar Scalar;
362
363 typedef typename Base::Model Model;
364 typedef typename Base::ADConfigVectorType ADConfigVectorType;
365 typedef typename Base::ADTangentVectorType ADTangentVectorType;
366 typedef typename Base::MatrixXs MatrixXs;
367 typedef typename Base::VectorXs VectorXs;
368
369 CodeGenMinv(
370 const Model & model,
371 const std::string & function_name = "minv",
372 const std::string & library_name = "cg_minv_eval")
373 : Base(model, model.nq, (model.nv * (model.nv + 1)) / 2, function_name, library_name)
374 {
375 ad_q = ADConfigVectorType(model.nq);
376 ad_q = neutral(ad_model);
377 x = VectorXs::Zero(Base::getInputDimension());
378 res = VectorXs::Zero(Base::getOutputDimension());
379
380 Minv = MatrixXs::Zero(model.nv, model.nq);
381
382 Base::build_jacobian = false;
383 }
384
385 virtual ~CodeGenMinv()
386 {
387 }
388
389 void buildMap()
390 {
391 CppAD::Independent(ad_X);
392
393 Eigen::DenseIndex it = 0;
394 ad_q = ad_X.segment(it, ad_model.nq);
395 it += ad_model.nq;
396
397 pinocchio::computeMinverse(ad_model, ad_data, ad_q);
398 Eigen::DenseIndex it_Y = 0;
399 for (Eigen::DenseIndex i = 0; i < ad_model.nv; ++i)
400 {
401 for (Eigen::DenseIndex j = i; j < ad_model.nv; ++j)
402 {
403 ad_Y[it_Y++] = ad_data.Minv(i, j);
404 }
405 }
406
407 assert(it_Y == Base::getOutputDimension());
408
409 ad_fun.Dependent(ad_X, ad_Y);
410 ad_fun.optimize("no_compare_op");
411 }
412
413 template<typename ConfigVectorType>
414 void evalFunction(const Eigen::MatrixBase<ConfigVectorType> & q)
415 {
416 // fill x
417 Eigen::DenseIndex it = 0;
418 x.segment(it, ad_model.nq) = q;
419 it += ad_model.nq;
420
421 Base::evalFunction(x);
422
423 // fill Minv
424 Eigen::DenseIndex it_Y = 0;
425 for (Eigen::DenseIndex i = 0; i < ad_model.nv; ++i)
426 {
427 for (Eigen::DenseIndex j = i; j < ad_model.nv; ++j)
428 {
429 Minv(i, j) = Base::y[it_Y++];
430 }
431 }
432 }
433
434 MatrixXs Minv;
435
436 protected:
437 using Base::ad_data;
438 using Base::ad_fun;
439 using Base::ad_model;
440 using Base::ad_X;
441 using Base::ad_Y;
442 using Base::y;
443
444 VectorXs x;
445 VectorXs res;
446
447 ADConfigVectorType ad_q;
448 };
449
450 template<typename _Scalar>
451 struct CodeGenRNEADerivatives : public CodeGenBase<_Scalar>
452 {
453 typedef CodeGenBase<_Scalar> Base;
454 typedef typename Base::Scalar Scalar;
455
456 typedef typename Base::Model Model;
457 typedef typename Base::ADConfigVectorType ADConfigVectorType;
458 typedef typename Base::ADTangentVectorType ADTangentVectorType;
459 typedef typename Base::MatrixXs MatrixXs;
460 typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXs) RowMatrixXs;
461 typedef typename Base::VectorXs VectorXs;
462
463 typedef typename Base::ADData ADData;
464 typedef typename ADData::MatrixXs ADMatrixXs;
465 typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(ADMatrixXs) RowADMatrixXs;
466
467 CodeGenRNEADerivatives(
468 const Model & model,
469 const std::string & function_name = "partial_rnea",
470 const std::string & library_name = "cg_partial_rnea_eval")
471 : Base(model, model.nq + 2 * model.nv, 3 * model.nv * model.nv, function_name, library_name)
472 {
473 ad_q = ADConfigVectorType(model.nq);
474 ad_q = neutral(ad_model);
475 ad_v = ADTangentVectorType(model.nv);
476 ad_v.setZero();
477 ad_a = ADTangentVectorType(model.nv);
478 ad_a.setZero();
479
480 x = VectorXs::Zero(Base::getInputDimension());
481
482 ad_dtau_dq = ADMatrixXs::Zero(model.nv, model.nv);
483 ad_dtau_dv = ADMatrixXs::Zero(model.nv, model.nv);
484 ad_dtau_da = ADMatrixXs::Zero(model.nv, model.nv);
485
486 dtau_dq = MatrixXs::Zero(model.nv, model.nv);
487 dtau_dv = MatrixXs::Zero(model.nv, model.nv);
488 dtau_da = MatrixXs::Zero(model.nv, model.nv);
489
490 Base::build_jacobian = false;
491 }
492
493 virtual ~CodeGenRNEADerivatives()
494 {
495 }
496
497 void buildMap()
498 {
499 CppAD::Independent(ad_X);
500
501 Eigen::DenseIndex it = 0;
502 ad_q = ad_X.segment(it, ad_model.nq);
503 it += ad_model.nq;
504 ad_v = ad_X.segment(it, ad_model.nv);
505 it += ad_model.nv;
506 ad_a = ad_X.segment(it, ad_model.nv);
507 it += ad_model.nv;
508
509 pinocchio::computeRNEADerivatives(
510 ad_model, ad_data, ad_q, ad_v, ad_a, ad_dtau_dq, ad_dtau_dv, ad_dtau_da);
511
512 assert(ad_Y.size() == Base::getOutputDimension());
513
514 Eigen::DenseIndex it_Y = 0;
515 Eigen::Map<RowADMatrixXs>(ad_Y.data() + it_Y, ad_model.nv, ad_model.nv) = ad_dtau_dq;
516 it_Y += ad_model.nv * ad_model.nv;
517 Eigen::Map<RowADMatrixXs>(ad_Y.data() + it_Y, ad_model.nv, ad_model.nv) = ad_dtau_dv;
518 it_Y += ad_model.nv * ad_model.nv;
519 Eigen::Map<RowADMatrixXs>(ad_Y.data() + it_Y, ad_model.nv, ad_model.nv) = ad_dtau_da;
520 it_Y += ad_model.nv * ad_model.nv;
521
522 ad_fun.Dependent(ad_X, ad_Y);
523 ad_fun.optimize("no_compare_op");
524 }
525
526 template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
527 void evalFunction(
528 const Eigen::MatrixBase<ConfigVectorType> & q,
529 const Eigen::MatrixBase<TangentVector1> & v,
530 const Eigen::MatrixBase<TangentVector2> & a)
531 {
532 // fill x
533 Eigen::DenseIndex it_x = 0;
534 x.segment(it_x, ad_model.nq) = q;
535 it_x += ad_model.nq;
536 x.segment(it_x, ad_model.nv) = v;
537 it_x += ad_model.nv;
538 x.segment(it_x, ad_model.nv) = a;
539 it_x += ad_model.nv;
540
541 Base::evalFunction(x);
542
543 // fill partial derivatives
544 Eigen::DenseIndex it_y = 0;
545 dtau_dq = Eigen::Map<RowMatrixXs>(Base::y.data() + it_y, ad_model.nv, ad_model.nv);
546 it_y += ad_model.nv * ad_model.nv;
547 dtau_dv = Eigen::Map<RowMatrixXs>(Base::y.data() + it_y, ad_model.nv, ad_model.nv);
548 it_y += ad_model.nv * ad_model.nv;
549 dtau_da = Eigen::Map<RowMatrixXs>(Base::y.data() + it_y, ad_model.nv, ad_model.nv);
550 it_y += ad_model.nv * ad_model.nv;
551 }
552
553 protected:
554 using Base::ad_data;
555 using Base::ad_fun;
556 using Base::ad_model;
557 using Base::ad_X;
558 using Base::ad_Y;
559 using Base::y;
560
561 VectorXs x;
562 ADMatrixXs ad_dtau_dq, ad_dtau_dv, ad_dtau_da;
563 MatrixXs dtau_dq, dtau_dv, dtau_da;
564
565 ADConfigVectorType ad_q;
566 ADTangentVectorType ad_v, ad_a;
567 };
568
569 template<typename _Scalar>
570 struct CodeGenABADerivatives : public CodeGenBase<_Scalar>
571 {
572 typedef CodeGenBase<_Scalar> Base;
573 typedef typename Base::Scalar Scalar;
574
575 typedef typename Base::Model Model;
576 typedef typename Base::ADConfigVectorType ADConfigVectorType;
577 typedef typename Base::ADTangentVectorType ADTangentVectorType;
578 typedef typename Base::MatrixXs MatrixXs;
579 typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXs) RowMatrixXs;
580 typedef typename Base::VectorXs VectorXs;
581
582 typedef typename Base::ADData ADData;
583 typedef typename ADData::MatrixXs ADMatrixXs;
584 typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(ADMatrixXs) RowADMatrixXs;
585
586 CodeGenABADerivatives(
587 const Model & model,
588 const std::string & function_name = "partial_aba",
589 const std::string & library_name = "cg_partial_aba_eval")
590 : Base(model, model.nq + 2 * model.nv, 3 * model.nv * model.nv, function_name, library_name)
591 {
592 ad_q = ADConfigVectorType(model.nq);
593 ad_q = neutral(ad_model);
594 ad_v = ADTangentVectorType(model.nv);
595 ad_v.setZero();
596 ad_tau = ADTangentVectorType(model.nv);
597 ad_tau.setZero();
598
599 x = VectorXs::Zero(Base::getInputDimension());
600
601 ad_dddq_dq = ADMatrixXs::Zero(model.nv, model.nv);
602 ad_dddq_dv = ADMatrixXs::Zero(model.nv, model.nv);
603 ad_dddq_dtau = ADMatrixXs::Zero(model.nv, model.nv);
604
605 dddq_dq = MatrixXs::Zero(model.nv, model.nv);
606 dddq_dv = MatrixXs::Zero(model.nv, model.nv);
607 dddq_dtau = MatrixXs::Zero(model.nv, model.nv);
608
609 Base::build_jacobian = false;
610 }
611
612 virtual ~CodeGenABADerivatives()
613 {
614 }
615
616 void buildMap()
617 {
618 CppAD::Independent(ad_X);
619
620 Eigen::DenseIndex it = 0;
621 ad_q = ad_X.segment(it, ad_model.nq);
622 it += ad_model.nq;
623 ad_v = ad_X.segment(it, ad_model.nv);
624 it += ad_model.nv;
625 ad_tau = ad_X.segment(it, ad_model.nv);
626 it += ad_model.nv;
627
628 pinocchio::computeABADerivatives(
629 ad_model, ad_data, ad_q, ad_v, ad_tau, ad_dddq_dq, ad_dddq_dv, ad_dddq_dtau);
630
631 assert(ad_Y.size() == Base::getOutputDimension());
632
633 Eigen::DenseIndex it_Y = 0;
634 Eigen::Map<RowADMatrixXs>(ad_Y.data() + it_Y, ad_model.nv, ad_model.nv) = ad_dddq_dq;
635 it_Y += ad_model.nv * ad_model.nv;
636 Eigen::Map<RowADMatrixXs>(ad_Y.data() + it_Y, ad_model.nv, ad_model.nv) = ad_dddq_dv;
637 it_Y += ad_model.nv * ad_model.nv;
638 Eigen::Map<RowADMatrixXs>(ad_Y.data() + it_Y, ad_model.nv, ad_model.nv) = ad_dddq_dtau;
639 it_Y += ad_model.nv * ad_model.nv;
640
641 ad_fun.Dependent(ad_X, ad_Y);
642 ad_fun.optimize("no_compare_op");
643 }
644
645 template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
646 void evalFunction(
647 const Eigen::MatrixBase<ConfigVectorType> & q,
648 const Eigen::MatrixBase<TangentVector1> & v,
649 const Eigen::MatrixBase<TangentVector2> & tau)
650 {
651 // fill x
652 Eigen::DenseIndex it_x = 0;
653 x.segment(it_x, ad_model.nq) = q;
654 it_x += ad_model.nq;
655 x.segment(it_x, ad_model.nv) = v;
656 it_x += ad_model.nv;
657 x.segment(it_x, ad_model.nv) = tau;
658 it_x += ad_model.nv;
659
660 Base::evalFunction(x);
661
662 // fill partial derivatives
663 Eigen::DenseIndex it_y = 0;
664 dddq_dq = Eigen::Map<RowMatrixXs>(Base::y.data() + it_y, ad_model.nv, ad_model.nv);
665 it_y += ad_model.nv * ad_model.nv;
666 dddq_dv = Eigen::Map<RowMatrixXs>(Base::y.data() + it_y, ad_model.nv, ad_model.nv);
667 it_y += ad_model.nv * ad_model.nv;
668 dddq_dtau = Eigen::Map<RowMatrixXs>(Base::y.data() + it_y, ad_model.nv, ad_model.nv);
669 it_y += ad_model.nv * ad_model.nv;
670 }
671
672 protected:
673 using Base::ad_data;
674 using Base::ad_fun;
675 using Base::ad_model;
676 using Base::ad_X;
677 using Base::ad_Y;
678 using Base::y;
679
680 VectorXs x;
681 ADMatrixXs ad_dddq_dq, ad_dddq_dv, ad_dddq_dtau;
682 MatrixXs dddq_dq, dddq_dv, dddq_dtau;
683
684 ADConfigVectorType ad_q;
685 ADTangentVectorType ad_v, ad_tau;
686 };
687
688 template<typename _Scalar>
689 struct CodeGenConstraintDynamicsDerivatives : public CodeGenBase<_Scalar>
690 {
691 typedef CodeGenBase<_Scalar> Base;
692 typedef typename Base::Scalar Scalar;
693 typedef typename Base::ADScalar ADScalar;
694
695 typedef typename Base::Model Model;
696
697 typedef typename Base::ADConfigVectorType ADConfigVectorType;
698 typedef typename Base::ADTangentVectorType ADTangentVectorType;
699 typedef typename Base::MatrixXs MatrixXs;
700 typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXs) RowMatrixXs;
701 typedef typename Base::VectorXs VectorXs;
702
703 typedef typename pinocchio::RigidConstraintModelTpl<Scalar, Base::Options> ContactModel;
704 typedef Eigen::aligned_allocator<ContactModel> ConstraintModelAllocator;
705 typedef typename std::vector<ContactModel, ConstraintModelAllocator> ContactModelVector;
706 typedef typename pinocchio::RigidConstraintDataTpl<Scalar, Base::Options> ContactData;
707 typedef Eigen::aligned_allocator<ContactData> ConstraintDataAllocator;
708 typedef typename std::vector<ContactData, ConstraintDataAllocator> ContactDataVector;
709
710 typedef typename pinocchio::RigidConstraintModelTpl<ADScalar, Base::Options> ADContactModel;
711 typedef Eigen::aligned_allocator<ADContactModel> ADConstraintModelAllocator;
712 typedef typename std::vector<ADContactModel, ADConstraintModelAllocator> ADContactModelVector;
713 typedef typename pinocchio::RigidConstraintDataTpl<ADScalar, Base::Options> ADContactData;
714 typedef Eigen::aligned_allocator<ADContactData> ADConstraintDataAllocator;
715 typedef typename std::vector<ADContactData, ADConstraintDataAllocator> ADContactDataVector;
716
717 typedef typename Base::ADData ADData;
718 typedef typename ADData::MatrixXs ADMatrixXs;
719 typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(ADMatrixXs) RowADMatrixXs;
720
721 Eigen::DenseIndex constraintDim(const ContactModelVector & contact_models) const
722 {
723 Eigen::DenseIndex num_total_constraints = 0;
724 for (typename ContactModelVector::const_iterator it = contact_models.begin();
725 it != contact_models.end(); ++it)
726 {
727 PINOCCHIO_CHECK_INPUT_ARGUMENT(
728 it->size() > 0, "The dimension of the constraint must be positive");
729 num_total_constraints += it->size();
730 }
731 return num_total_constraints;
732 }
733
734 CodeGenConstraintDynamicsDerivatives(
735 const Model & model,
736 const ContactModelVector & contact_models,
737 const std::string & function_name = "partial_constraintDynamics",
738 const std::string & library_name = "cg_partial_constraintDynamics_eval")
739 : Base(
740 model,
741 model.nq + 2 * model.nv,
742 3 * model.nv * model.nv + 3 * constraintDim(contact_models) * model.nv,
743 function_name,
744 library_name)
745 , nc(constraintDim(contact_models))
746 , dddq_dq(model.nv, model.nv)
747 , dddq_dv(model.nv, model.nv)
748 , dddq_dtau(model.nv, model.nv)
749 {
750 dlambda_dq.resize(nc, model.nv);
751 dlambda_dq.setZero();
752 dlambda_dv.resize(nc, model.nv);
753 dlambda_dv.setZero();
754 dlambda_dtau.resize(nc, model.nv);
755 dlambda_dtau.setZero();
756
757 ad_q = ADConfigVectorType(model.nq);
758 ad_q = neutral(ad_model);
759 ad_v = ADTangentVectorType(model.nv);
760 ad_v.setZero();
761 ad_tau = ADTangentVectorType(model.nv);
762 ad_tau.setZero();
763
764 x = VectorXs::Zero(Base::getInputDimension());
765
766 for (int k = 0; k < contact_models.size(); ++k)
767 {
768 ad_contact_models.push_back(contact_models[k].template cast<ADScalar>());
769 }
770
771 for (int k = 0; k < ad_contact_models.size(); ++k)
772 {
773 ad_contact_datas.push_back(ADContactData(ad_contact_models[k]));
774 }
775
776 pinocchio::initConstraintDynamics(ad_model, ad_data, ad_contact_models);
777 Base::build_jacobian = false;
778 }
779
780 virtual ~CodeGenConstraintDynamicsDerivatives()
781 {
782 }
783
784 void buildMap()
785 {
786 CppAD::Independent(ad_X);
787
788 Eigen::DenseIndex it = 0;
789 ad_q = ad_X.segment(it, ad_model.nq);
790 it += ad_model.nq;
791 ad_v = ad_X.segment(it, ad_model.nv);
792 it += ad_model.nv;
793 ad_tau = ad_X.segment(it, ad_model.nv);
794 it += ad_model.nv;
795
796 pinocchio::constraintDynamics(
797 ad_model, ad_data, ad_q, ad_v, ad_tau, ad_contact_models, ad_contact_datas);
798 pinocchio::computeConstraintDynamicsDerivatives(
799 ad_model, ad_data, ad_contact_models, ad_contact_datas);
800 assert(ad_Y.size() == Base::getOutputDimension());
801
802 Eigen::DenseIndex it_Y = 0;
803 Eigen::Map<RowADMatrixXs>(ad_Y.data() + it_Y, ad_model.nv, ad_model.nv) = ad_data.ddq_dq;
804 it_Y += ad_model.nv * ad_model.nv;
805 Eigen::Map<RowADMatrixXs>(ad_Y.data() + it_Y, ad_model.nv, ad_model.nv) = ad_data.ddq_dv;
806 it_Y += ad_model.nv * ad_model.nv;
807 Eigen::Map<RowADMatrixXs>(ad_Y.data() + it_Y, ad_model.nv, ad_model.nv) = ad_data.ddq_dtau;
808 it_Y += ad_model.nv * ad_model.nv;
809 Eigen::Map<ADMatrixXs>(ad_Y.data() + it_Y, nc, ad_model.nv) = ad_data.dlambda_dq;
810 it_Y += nc * ad_model.nv;
811 Eigen::Map<ADMatrixXs>(ad_Y.data() + it_Y, nc, ad_model.nv) = ad_data.dlambda_dv;
812 it_Y += nc * ad_model.nv;
813 Eigen::Map<ADMatrixXs>(ad_Y.data() + it_Y, nc, ad_model.nv) = ad_data.dlambda_dtau;
814 it_Y += nc * ad_model.nv;
815 ad_fun.Dependent(ad_X, ad_Y);
816 ad_fun.optimize("no_compare_op");
817 }
818
819 template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
820 void evalFunction(
821 const Eigen::MatrixBase<ConfigVectorType> & q,
822 const Eigen::MatrixBase<TangentVector1> & v,
823 const Eigen::MatrixBase<TangentVector2> & tau)
824 {
825 // fill x
826 Eigen::DenseIndex it_x = 0;
827 x.segment(it_x, ad_model.nq) = q;
828 it_x += ad_model.nq;
829 x.segment(it_x, ad_model.nv) = v;
830 it_x += ad_model.nv;
831 x.segment(it_x, ad_model.nv) = tau;
832 it_x += ad_model.nv;
833
834 Base::evalFunction(x);
835
836 // fill partial derivatives
837 Eigen::DenseIndex it_y = 0;
838 dddq_dq = Eigen::Map<RowMatrixXs>(Base::y.data() + it_y, ad_model.nv, ad_model.nv);
839 it_y += ad_model.nv * ad_model.nv;
840 dddq_dv = Eigen::Map<RowMatrixXs>(Base::y.data() + it_y, ad_model.nv, ad_model.nv);
841 it_y += ad_model.nv * ad_model.nv;
842 dddq_dtau = Eigen::Map<RowMatrixXs>(Base::y.data() + it_y, ad_model.nv, ad_model.nv);
843 it_y += ad_model.nv * ad_model.nv;
844 dlambda_dq = Eigen::Map<MatrixXs>(Base::y.data() + it_y, nc, ad_model.nv);
845 it_y += nc * ad_model.nv;
846 dlambda_dv = Eigen::Map<MatrixXs>(Base::y.data() + it_y, nc, ad_model.nv);
847 it_y += nc * ad_model.nv;
848 dlambda_dtau = Eigen::Map<MatrixXs>(Base::y.data() + it_y, nc, ad_model.nv);
849 it_y += nc * ad_model.nv;
850 }
851
852 protected:
853 using Base::ad_data;
854 using Base::ad_fun;
855 using Base::ad_model;
856 using Base::ad_X;
857 using Base::ad_Y;
858 using Base::y;
859
860 Eigen::DenseIndex nc;
861 ADContactModelVector ad_contact_models;
862 ADContactDataVector ad_contact_datas;
863
864 VectorXs x;
865 MatrixXs dddq_dq, dddq_dv, dddq_dtau;
866 MatrixXs dlambda_dq, dlambda_dv, dlambda_dtau;
867
868 ADConfigVectorType ad_q;
869 ADTangentVectorType ad_v, ad_tau;
870 };
871
872 template<typename _Scalar>
873 struct CodeGenConstraintDynamics : public CodeGenBase<_Scalar>
874 {
875 typedef CodeGenBase<_Scalar> Base;
876 typedef typename Base::Scalar Scalar;
877 typedef typename Base::ADScalar ADScalar;
878
879 typedef typename Base::Model Model;
880
881 typedef typename Base::ADConfigVectorType ADConfigVectorType;
882 typedef typename Base::ADTangentVectorType ADTangentVectorType;
883 typedef typename Base::MatrixXs MatrixXs;
884 typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXs) RowMatrixXs;
885 typedef typename Base::VectorXs VectorXs;
886
887 typedef typename pinocchio::RigidConstraintModelTpl<Scalar, Base::Options> ContactModel;
888 typedef Eigen::aligned_allocator<ContactModel> ConstraintModelAllocator;
889 typedef typename std::vector<ContactModel, ConstraintModelAllocator> ContactModelVector;
890 typedef typename pinocchio::RigidConstraintDataTpl<Scalar, Base::Options> ContactData;
891 typedef Eigen::aligned_allocator<ContactData> ConstraintDataAllocator;
892 typedef typename std::vector<ContactData, ConstraintDataAllocator> ContactDataVector;
893
894 typedef typename pinocchio::RigidConstraintModelTpl<ADScalar, Base::Options> ADContactModel;
895 typedef Eigen::aligned_allocator<ADContactModel> ADConstraintModelAllocator;
896 typedef typename std::vector<ADContactModel, ADConstraintModelAllocator> ADContactModelVector;
897 typedef typename pinocchio::RigidConstraintDataTpl<ADScalar, Base::Options> ADContactData;
898 typedef Eigen::aligned_allocator<ADContactData> ADConstraintDataAllocator;
899 typedef typename std::vector<ADContactData, ADConstraintDataAllocator> ADContactDataVector;
900
901 typedef typename Base::ADData ADData;
902 typedef typename ADData::MatrixXs ADMatrixXs;
903 typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(ADMatrixXs) RowADMatrixXs;
904
905 Eigen::DenseIndex constraintDim(const ContactModelVector & contact_models) const
906 {
907 Eigen::DenseIndex num_total_constraints = 0;
908 for (typename ContactModelVector::const_iterator it = contact_models.begin();
909 it != contact_models.end(); ++it)
910 {
911 PINOCCHIO_CHECK_INPUT_ARGUMENT(
912 it->size() > 0, "The dimension of the constraint must be positive");
913 num_total_constraints += it->size();
914 }
915 return num_total_constraints;
916 }
917
918 CodeGenConstraintDynamics(
919 const Model & model,
920 const ContactModelVector & contact_models,
921 const std::string & function_name = "constraintDynamics",
922 const std::string & library_name = "cg_constraintDynamics_eval")
923 : Base(
924 model,
925 model.nq + 2 * model.nv,
926 model.nv + constraintDim(contact_models),
927 function_name,
928 library_name)
929 , nc(constraintDim(contact_models))
930 , da_dq(MatrixXs::Zero(model.nv, model.nq))
931 , da_dv(MatrixXs::Zero(model.nv, model.nv))
932 , da_dtau(MatrixXs::Zero(model.nv, model.nv))
933 , ddq(model.nv)
934 {
935 lambda_c.resize(nc);
936 lambda_c.setZero();
937 dlambda_dq.resize(nc, model.nq);
938 dlambda_dq.setZero();
939 dlambda_dv.resize(nc, model.nv);
940 dlambda_dv.setZero();
941 dlambda_dtau.resize(nc, model.nv);
942 dlambda_dtau.setZero();
943
944 ad_q = ADConfigVectorType(model.nq);
945 ad_q = neutral(ad_model);
946 ad_v = ADTangentVectorType(model.nv);
947 ad_v.setZero();
948 ad_tau = ADTangentVectorType(model.nv);
949 ad_tau.setZero();
950
951 x = VectorXs::Zero(Base::getInputDimension());
952
953 for (int k = 0; k < contact_models.size(); ++k)
954 {
955 ad_contact_models.push_back(contact_models[k].template cast<ADScalar>());
956 }
957
958 for (int k = 0; k < ad_contact_models.size(); ++k)
959 {
960 ad_contact_datas.push_back(ADContactData(ad_contact_models[k]));
961 }
962 pinocchio::initConstraintDynamics(ad_model, ad_data, ad_contact_models);
963 }
964
965 virtual ~CodeGenConstraintDynamics()
966 {
967 }
968
969 void buildMap()
970 {
971 CppAD::Independent(ad_X);
972
973 Eigen::DenseIndex it = 0;
974 ad_q = ad_X.segment(it, ad_model.nq);
975 it += ad_model.nq;
976 ad_v = ad_X.segment(it, ad_model.nv);
977 it += ad_model.nv;
978 ad_tau = ad_X.segment(it, ad_model.nv);
979 it += ad_model.nv;
980
981 pinocchio::constraintDynamics(
982 ad_model, ad_data, ad_q, ad_v, ad_tau, ad_contact_models, ad_contact_datas);
983 ad_Y.head(ad_model.nv) = ad_data.ddq;
984 ad_Y.segment(ad_model.nv, nc) = ad_data.lambda_c;
985
986 ad_fun.Dependent(ad_X, ad_Y);
987 ad_fun.optimize("no_compare_op");
988 }
989
990 template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
991 void evalFunction(
992 const Eigen::MatrixBase<ConfigVectorType> & q,
993 const Eigen::MatrixBase<TangentVector1> & v,
994 const Eigen::MatrixBase<TangentVector2> & tau)
995 {
996 // fill x
997 Eigen::DenseIndex it_x = 0;
998 x.segment(it_x, ad_model.nq) = q;
999 it_x += ad_model.nq;
1000 x.segment(it_x, ad_model.nv) = v;
1001 it_x += ad_model.nv;
1002 x.segment(it_x, ad_model.nv) = tau;
1003 it_x += ad_model.nv;
1004
1005 Base::evalFunction(x);
1006
1007 Eigen::DenseIndex it_y = 0;
1008 ddq = Base::y.segment(it_y, ad_model.nv);
1009 it_y += ad_model.nv;
1010 lambda_c = Base::y.segment(it_y, nc);
1011 }
1012
1013 using Base::evalJacobian;
1014 template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
1015 void evalJacobian(
1016 const Eigen::MatrixBase<ConfigVectorType> & q,
1017 const Eigen::MatrixBase<TangentVector1> & v,
1018 const Eigen::MatrixBase<TangentVector2> & a)
1019 {
1020 // fill x
1021 Eigen::DenseIndex it = 0;
1022 x.segment(it, ad_model.nq) = q;
1023 it += ad_model.nq;
1024 x.segment(it, ad_model.nv) = v;
1025 it += ad_model.nv;
1026 x.segment(it, ad_model.nv) = a;
1027 it += ad_model.nv;
1028
1029 evalJacobian(x);
1030 it = 0;
1031 da_dq = Base::jac.middleCols(it, ad_model.nq).topRows(ad_model.nv);
1032 dlambda_dq = Base::jac.middleCols(it, ad_model.nq).bottomRows(nc);
1033 it += ad_model.nq;
1034 da_dv = Base::jac.middleCols(it, ad_model.nv).topRows(ad_model.nv);
1035 dlambda_dv = Base::jac.middleCols(it, ad_model.nv).bottomRows(nc);
1036 it += ad_model.nv;
1037 da_dtau = Base::jac.middleCols(it, ad_model.nv).topRows(ad_model.nv);
1038 dlambda_dtau = Base::jac.middleCols(it, ad_model.nv).bottomRows(nc);
1039 it += ad_model.nv;
1040 }
1041 VectorXs lambda_c, ddq;
1042 MatrixXs da_dq, da_dv, da_dtau;
1043 MatrixXs dlambda_dq, dlambda_dv, dlambda_dtau;
1044
1045 protected:
1046 using Base::ad_data;
1047 using Base::ad_fun;
1048 using Base::ad_model;
1049 using Base::ad_X;
1050 using Base::ad_Y;
1051 using Base::y;
1052
1053 ADContactModelVector ad_contact_models;
1054 ADContactDataVector ad_contact_datas;
1055
1056 Eigen::DenseIndex nc;
1057 VectorXs x;
1058
1059 ADConfigVectorType ad_q;
1060 ADTangentVectorType ad_v, ad_tau;
1061 };
1062
1063 template<typename _Scalar>
1064 struct CodeGenIntegrate : public CodeGenBase<_Scalar>
1065 {
1066 typedef CodeGenBase<_Scalar> Base;
1067 typedef typename Base::Scalar Scalar;
1068
1069 typedef typename Base::Model Model;
1070 typedef typename Base::ADConfigVectorType ADConfigVectorType;
1071 typedef typename Base::ADTangentVectorType ADTangentVectorType;
1072 typedef typename Base::MatrixXs MatrixXs;
1073 typedef typename Base::VectorXs VectorXs;
1074
1075 CodeGenIntegrate(
1076 const Model & model,
1077 const std::string & function_name = "integrate",
1078 const std::string & library_name = "cg_integrate_eval")
1079 : Base(model, model.nq + model.nv, model.nq, function_name, library_name)
1080 {
1081 ad_q = ADConfigVectorType(model.nq);
1082 ad_q = neutral(ad_model);
1083 ad_v = ADTangentVectorType(model.nv);
1084 ad_v.setZero();
1085 x = VectorXs::Zero(Base::getInputDimension());
1086 res = VectorXs::Zero(Base::getOutputDimension());
1087 }
1088
1089 void buildMap()
1090 {
1091 CppAD::Independent(ad_X);
1092
1093 Eigen::DenseIndex it = 0;
1094 ad_q = ad_X.segment(it, ad_model.nq);
1095 it += ad_model.nq;
1096 ad_v = ad_X.segment(it, ad_model.nv);
1097 pinocchio::integrate(ad_model, ad_q, ad_v, ad_Y);
1098
1099 ad_fun.Dependent(ad_X, ad_Y);
1100 ad_fun.optimize("no_compare_op");
1101 }
1102
1103 using Base::evalFunction;
1104 template<typename ConfigVectorType1, typename TangentVector, typename ConfigVectorType2>
1105 void evalFunction(
1106 const Eigen::MatrixBase<ConfigVectorType1> & q,
1107 const Eigen::MatrixBase<TangentVector> & v,
1108 const Eigen::MatrixBase<ConfigVectorType2> & qout)
1109 {
1110 // fill x
1111 Eigen::DenseIndex it = 0;
1112 x.segment(it, ad_model.nq) = q;
1113 it += ad_model.nq;
1114 x.segment(it, ad_model.nv) = v;
1115
1116 evalFunction(x);
1117 PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType2, qout) = Base::y;
1118 }
1119
1120 protected:
1121 using Base::ad_fun;
1122 using Base::ad_model;
1123 using Base::ad_X;
1124 using Base::ad_Y;
1125 using Base::y;
1126
1127 VectorXs x;
1128 VectorXs res;
1129
1130 ADConfigVectorType ad_q;
1131 ADTangentVectorType ad_v;
1132 };
1133
1134 template<typename _Scalar>
1135 struct CodeGenDifference : public CodeGenBase<_Scalar>
1136 {
1137 typedef CodeGenBase<_Scalar> Base;
1138 typedef typename Base::Scalar Scalar;
1139
1140 typedef typename Base::Model Model;
1141 typedef typename Base::ADConfigVectorType ADConfigVectorType;
1142 typedef typename Base::ADTangentVectorType ADTangentVectorType;
1143 typedef typename Base::MatrixXs MatrixXs;
1144 typedef typename Base::VectorXs VectorXs;
1145
1146 CodeGenDifference(
1147 const Model & model,
1148 const std::string & function_name = "difference",
1149 const std::string & library_name = "cg_difference_eval")
1150 : Base(model, 2 * model.nq, model.nv, function_name, library_name)
1151 {
1152 ad_q0 = ADConfigVectorType(model.nq);
1153 ad_q0 = neutral(ad_model);
1154 ad_q1 = ADConfigVectorType(model.nq);
1155 ad_q1 = neutral(ad_model);
1156 x = VectorXs::Zero(Base::getInputDimension());
1157 res = VectorXs::Zero(Base::getOutputDimension());
1158 }
1159
1160 void buildMap()
1161 {
1162 CppAD::Independent(ad_X);
1163
1164 Eigen::DenseIndex it = 0;
1165 ad_q0 = ad_X.segment(it, ad_model.nq);
1166 it += ad_model.nq;
1167 ad_q1 = ad_X.segment(it, ad_model.nq);
1168 pinocchio::difference(ad_model, ad_q0, ad_q1, ad_Y);
1169
1170 ad_fun.Dependent(ad_X, ad_Y);
1171 ad_fun.optimize("no_compare_op");
1172 }
1173
1174 using Base::evalFunction;
1175 template<typename ConfigVectorType1, typename ConfigVectorType2, typename TangentVector>
1176 void evalFunction(
1177 const Eigen::MatrixBase<ConfigVectorType1> & q0,
1178 const Eigen::MatrixBase<ConfigVectorType2> & q1,
1179 const Eigen::MatrixBase<TangentVector> & v)
1180 {
1181 // fill x
1182 Eigen::DenseIndex it = 0;
1183 x.segment(it, ad_model.nq) = q0;
1184 it += ad_model.nq;
1185 x.segment(it, ad_model.nq) = q1;
1186
1187 evalFunction(x);
1188 PINOCCHIO_EIGEN_CONST_CAST(TangentVector, v) = Base::y;
1189 }
1190
1191 protected:
1192 using Base::ad_fun;
1193 using Base::ad_model;
1194 using Base::ad_X;
1195 using Base::ad_Y;
1196 using Base::y;
1197
1198 VectorXs x;
1199 VectorXs res;
1200
1201 ADConfigVectorType ad_q0;
1202 ADConfigVectorType ad_q1;
1203 };
1204
1205 template<typename _Scalar>
1206 struct CodeGenDDifference : public CodeGenBase<_Scalar>
1207 {
1208 typedef CodeGenBase<_Scalar> Base;
1209 typedef typename Base::Scalar Scalar;
1210
1211 typedef typename Base::Model Model;
1212 typedef typename Base::ADConfigVectorType ADConfigVectorType;
1213 typedef typename Base::ADTangentVectorType ADTangentVectorType;
1214 typedef typename Base::ADVectorXs ADVectorXs;
1215 typedef typename Base::ADMatrixXs ADMatrixXs;
1216 typedef typename Base::MatrixXs MatrixXs;
1217 typedef typename Base::VectorXs VectorXs;
1218
1219 CodeGenDDifference(
1220 const Model & model,
1221 const std::string & function_name = "dDifference",
1222 const std::string & library_name = "cg_dDifference_eval")
1223 : Base(model, 2 * model.nq, 2 * model.nv * model.nv, function_name, library_name)
1224 {
1225 ad_q0 = neutral(ad_model);
1226 ad_q1 = neutral(ad_model);
1227 ad_J0 = ADMatrixXs::Zero(ad_model.nv, ad_model.nv);
1228 ad_J1 = ADMatrixXs::Zero(ad_model.nv, ad_model.nv);
1229 x = VectorXs::Zero(Base::getInputDimension());
1230 }
1231
1232 void buildMap()
1233 {
1234 CppAD::Independent(ad_X);
1235
1236 Eigen::DenseIndex it = 0;
1237 ad_q0 = ad_X.segment(it, ad_model.nq);
1238 it += ad_model.nq;
1239 ad_q1 = ad_X.segment(it, ad_model.nq);
1240 pinocchio::dDifference(ad_model, ad_q0, ad_q1, ad_J0, pinocchio::ARG0);
1241 pinocchio::dDifference(ad_model, ad_q0, ad_q1, ad_J1, pinocchio::ARG1);
1242
1243 Eigen::Map<ADMatrixXs>(ad_Y.data(), 2 * ad_model.nv, ad_model.nv).topRows(ad_model.nv) =
1244 ad_J0;
1245 Eigen::Map<ADMatrixXs>(ad_Y.data(), 2 * ad_model.nv, ad_model.nv).bottomRows(ad_model.nv) =
1246 ad_J1;
1247 ad_fun.Dependent(ad_X, ad_Y);
1248 ad_fun.optimize("no_compare_op");
1249 }
1250
1251 using Base::evalFunction;
1252 template<typename ConfigVectorType1, typename ConfigVectorType2, typename JacobianMatrix>
1253 void evalFunction(
1254 const Eigen::MatrixBase<ConfigVectorType1> & q0,
1255 const Eigen::MatrixBase<ConfigVectorType2> & q1,
1256 const Eigen::MatrixBase<JacobianMatrix> & J,
1257 const ArgumentPosition arg)
1258 {
1259 // fill x
1260 Eigen::DenseIndex it = 0;
1261 x.segment(it, ad_model.nq) = q0;
1262 it += ad_model.nq;
1263 x.segment(it, ad_model.nq) = q1;
1264
1265 evalFunction(x);
1266 switch (arg)
1267 {
1268 case pinocchio::ARG0:
1269 PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, J) =
1270 Eigen::Map<MatrixXs>(Base::y.data(), 2 * ad_model.nv, ad_model.nv).topRows(ad_model.nv);
1271 break;
1272 case pinocchio::ARG1:
1273 PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix, J) =
1274 Eigen::Map<MatrixXs>(Base::y.data(), 2 * ad_model.nv, ad_model.nv)
1275 .bottomRows(ad_model.nv);
1276 break;
1277 default:
1278 assert(false && "Wrong argument");
1279 }
1280 }
1281
1282 protected:
1283 using Base::ad_fun;
1284 using Base::ad_model;
1285 using Base::ad_X;
1286 using Base::ad_Y;
1287 using Base::y;
1288
1289 VectorXs x;
1290 ADConfigVectorType ad_q0;
1291 ADConfigVectorType ad_q1;
1292 ADMatrixXs ad_J0;
1293 ADMatrixXs ad_J1;
1294 };
1295
1296} // namespace pinocchio
1297
1298#endif // ifndef __pinocchio_codegen_code_generator_algo_hpp__
pinocchio::Convention::WORLD
@ WORLD
pinocchio
Main pinocchio namespace.
Definition treeview.dox:11
pinocchio::neutral
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
pinocchio::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition fwd.hpp:122
pinocchio::difference
void difference(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorIn1 > &q0, const Eigen::MatrixBase< ConfigVectorIn2 > &q1, const Eigen::MatrixBase< ReturnType > &dvout)
Compute the tangent vector that must be integrated during one unit time to go from q0 to q1.
pinocchio::initConstraintDynamics
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.
pinocchio::computeABADerivatives
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.
pinocchio::constraintDynamics
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...
pinocchio::computeRNEADerivatives
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...
pinocchio::rnea
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...
pinocchio::integrate
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time.
pinocchio::aba
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...
pinocchio::computeMinverse
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.
pinocchio::dDifference
void dDifference(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVector1 > &q0, const Eigen::MatrixBase< ConfigVector2 > &q1, const Eigen::MatrixBase< JacobianMatrix > &J, const ArgumentPosition arg)
Computes the Jacobian of a small variation of the configuration vector into the tangent space at iden...
pinocchio::crba
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...
pinocchio::CodeGenABADerivatives::buildMap
void buildMap()
build the mapping Y = f(X)
Definition code-generator-algo.hpp:616
pinocchio::CodeGenABA::buildMap
void buildMap()
build the mapping Y = f(X)
Definition code-generator-algo.hpp:177
pinocchio::CodeGenBase::getOutputDimension
Eigen::DenseIndex getOutputDimension() const
Dimension of the output vector.
Definition code-generator-base.hpp:175
pinocchio::CodeGenBase::getInputDimension
Eigen::DenseIndex getInputDimension() const
Dimension of the input vector.
Definition code-generator-base.hpp:170
pinocchio::CodeGenBase::build_jacobian
bool build_jacobian
Options to build or not the Jacobian of he function.
Definition code-generator-base.hpp:193
pinocchio::CodeGenBase::function_name
const std::string function_name
Name of the function.
Definition code-generator-base.hpp:185
pinocchio::CodeGenBase::library_name
const std::string library_name
Name of the library.
Definition code-generator-base.hpp:187
pinocchio::CodeGenCRBA::buildMap
void buildMap()
build the mapping Y = f(X)
Definition code-generator-algo.hpp:293
pinocchio::CodeGenConstraintDynamics::buildMap
void buildMap()
build the mapping Y = f(X)
Definition code-generator-algo.hpp:969
pinocchio::CodeGenDDifference::buildMap
void buildMap()
build the mapping Y = f(X)
Definition code-generator-algo.hpp:1232
pinocchio::CodeGenDifference::buildMap
void buildMap()
build the mapping Y = f(X)
Definition code-generator-algo.hpp:1160
pinocchio::CodeGenIntegrate::buildMap
void buildMap()
build the mapping Y = f(X)
Definition code-generator-algo.hpp:1089
pinocchio::CodeGenMinv::buildMap
void buildMap()
build the mapping Y = f(X)
Definition code-generator-algo.hpp:389
pinocchio::CodeGenRNEADerivatives::buildMap
void buildMap()
build the mapping Y = f(X)
Definition code-generator-algo.hpp:497
pinocchio::CodeGenRNEA::buildMap
void buildMap()
build the mapping Y = f(X)
Definition code-generator-algo.hpp:57
pinocchio::DataTpl::M
MatrixXs M
The joint space inertia matrix (a square matrix of dim model.nv).
Definition data.hpp:199
pinocchio::DataTpl::lambda_c
VectorXs lambda_c
Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
Definition data.hpp:485
pinocchio::DataTpl::dlambda_dq
MatrixXs dlambda_dq
Partial derivatives of the constraints forces with respect to the joint configuration,...
Definition data.hpp:430
pinocchio::DataTpl::tau
TangentVectorType tau
Vector of joint torques (dim model.nv).
Definition data.hpp:173
pinocchio::DataTpl::ddq_dtau
RowMatrixXs ddq_dtau
Partial derivative of the joint acceleration vector with respect to the joint torques.
Definition data.hpp:416
pinocchio::DataTpl::ddq_dv
RowMatrixXs ddq_dv
Partial derivative of the joint acceleration vector with respect to the joint velocity.
Definition data.hpp:412
pinocchio::DataTpl::ddq
TangentVectorType ddq
The joint accelerations computed from ABA.
Definition data.hpp:256
pinocchio::DataTpl::Minv
RowMatrixXs Minv
The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
Definition data.hpp:202
pinocchio::DataTpl::ddq_dq
RowMatrixXs ddq_dq
Partial derivative of the joint acceleration vector with respect to the joint configuration.
Definition data.hpp:408
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition model.hpp:98
pinocchio::ModelTpl::nv
int nv
Dimension of the velocity vector space.
Definition model.hpp:101