pinocchio  3.3.1
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
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 
19 namespace 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::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: fwd.hpp:122
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::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.
Definition: joint-configuration.hpp:193
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::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::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::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::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::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.
Definition: joint-configuration.hpp:363
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.
Definition: joint-configuration.hpp:70
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...
Definition: joint-configuration.hpp:742
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::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::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:471
pinocchio::DataTpl::dlambda_dq
MatrixXs dlambda_dq
Partial derivatives of the constraints forces with respect to the joint configuration,...
Definition: data.hpp:416
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:402
pinocchio::DataTpl::ddq_dv
RowMatrixXs ddq_dv
Partial derivative of the joint acceleration vector with respect to the joint velocity.
Definition: data.hpp:398
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:394
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