pinocchio  2.2.1-dirty
code-generator-algo.hpp
1 //
2 // Copyright (c) 2018-2019 CNRS INRIA
3 //
4 
5 #ifndef __pinocchio_utils_code_generator_algo_hpp__
6 #define __pinocchio_utils_code_generator_algo_hpp__
7 
8 #ifdef PINOCCHIO_WITH_CPPADCG_SUPPORT
9 
10 #include "pinocchio/codegen/code-generator-base.hpp"
11 
12 #include "pinocchio/algorithm/joint-configuration.hpp"
13 #include "pinocchio/algorithm/crba.hpp"
14 #include "pinocchio/algorithm/rnea.hpp"
15 #include "pinocchio/algorithm/aba.hpp"
16 #include "pinocchio/algorithm/rnea-derivatives.hpp"
17 #include "pinocchio/algorithm/aba-derivatives.hpp"
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::ADCongigVectorType ADCongigVectorType;
29  typedef typename Base::ADTangentVectorType ADTangentVectorType;
30  typedef typename Base::MatrixXs MatrixXs;
31  typedef typename Base::VectorXs VectorXs;
32 
33  CodeGenRNEA(const Model & model,
34  const std::string & function_name = "rnea",
35  const std::string & library_name = "cg_rnea_eval")
36  : Base(model,model.nq+2*model.nv,model.nv,function_name,library_name)
37  {
38  ad_q = ADCongigVectorType(model.nq); ad_q = neutral(model);
39  ad_v = ADTangentVectorType(model.nv); ad_v.setZero();
40  ad_a = ADTangentVectorType(model.nv); ad_a.setZero();
41  x = VectorXs::Zero(Base::getInputDimension());
42  res = VectorXs::Zero(Base::getOutputDimension());
43 
44  dtau_dq = MatrixXs::Zero(model.nv,model.nq);
45  dtau_dv = MatrixXs::Zero(model.nv,model.nv);
46  dtau_da = MatrixXs::Zero(model.nv,model.nv);
47  }
48 
49  void buildMap()
50  {
51  CppAD::Independent(ad_X);
52 
53  Eigen::DenseIndex it = 0;
54  ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
55  ad_v = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
56  ad_a = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
57 
58  pinocchio::rnea(ad_model,ad_data,ad_q,ad_v,ad_a);
59 
60  ad_Y = ad_data.tau;
61 
62  ad_fun.Dependent(ad_X,ad_Y);
63  ad_fun.optimize("no_compare_op");
64  }
65 
66  using Base::evalFunction;
67  template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
68  void evalFunction(const Eigen::MatrixBase<ConfigVectorType> & q,
69  const Eigen::MatrixBase<TangentVector1> & v,
70  const Eigen::MatrixBase<TangentVector2> & a)
71  {
72  // fill x
73  Eigen::DenseIndex it = 0;
74  x.segment(it,ad_model.nq) = q; it += ad_model.nq;
75  x.segment(it,ad_model.nv) = v; it += ad_model.nv;
76  x.segment(it,ad_model.nv) = a; it += ad_model.nv;
77 
78  evalFunction(x);
79  res = Base::y;
80  }
81 
82  using Base::evalJacobian;
83  template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
84  void evalJacobian(const Eigen::MatrixBase<ConfigVectorType> & q,
85  const Eigen::MatrixBase<TangentVector1> & v,
86  const Eigen::MatrixBase<TangentVector2> & a)
87  {
88  // fill x
89  Eigen::DenseIndex it = 0;
90  x.segment(it,ad_model.nq) = q; it += ad_model.nq;
91  x.segment(it,ad_model.nv) = v; it += ad_model.nv;
92  x.segment(it,ad_model.nv) = a; it += ad_model.nv;
93 
94  evalJacobian(x);
95  it = 0;
96  dtau_dq = Base::jac.middleCols(it,ad_model.nq); it += ad_model.nq;
97  dtau_dv = Base::jac.middleCols(it,ad_model.nv); it += ad_model.nv;
98  dtau_da = Base::jac.middleCols(it,ad_model.nv); it += ad_model.nv;
99  }
100 
101  protected:
102 
103  using Base::ad_model;
104  using Base::ad_data;
105  using Base::ad_fun;
106  using Base::ad_X;
107  using Base::ad_Y;
108  using Base::y;
109  using Base::jac;
110 
111  VectorXs x;
112  VectorXs res;
113  MatrixXs dtau_dq, dtau_dv, dtau_da;
114 
115  ADCongigVectorType ad_q, ad_q_plus;
116  ADTangentVectorType ad_dq, ad_v, ad_a;
117  };
118 
119  template<typename _Scalar>
120  struct CodeGenABA : public CodeGenBase<_Scalar>
121  {
122  typedef CodeGenBase<_Scalar> Base;
123  typedef typename Base::Scalar Scalar;
124 
125  typedef typename Base::Model Model;
126  typedef typename Base::ADCongigVectorType ADCongigVectorType;
127  typedef typename Base::ADTangentVectorType ADTangentVectorType;
128  typedef typename Base::MatrixXs MatrixXs;
129  typedef typename Base::VectorXs VectorXs;
130 
131  CodeGenABA(const Model & model,
132  const std::string & function_name = "aba",
133  const std::string & library_name = "cg_aba_eval")
134  : Base(model,model.nq+2*model.nv,model.nv,function_name,library_name)
135  {
136  ad_q = ADCongigVectorType(model.nq); ad_q = neutral(model);
137  ad_v = ADTangentVectorType(model.nv); ad_v.setZero();
138  ad_tau = ADTangentVectorType(model.nv); ad_tau.setZero();
139  x = VectorXs::Zero(Base::getInputDimension());
140  res = VectorXs::Zero(Base::getOutputDimension());
141 
142  da_dq = MatrixXs::Zero(model.nv,model.nq);
143  da_dv = MatrixXs::Zero(model.nv,model.nv);
144  da_dtau = MatrixXs::Zero(model.nv,model.nv);
145  }
146 
147  void buildMap()
148  {
149  CppAD::Independent(ad_X);
150 
151  Eigen::DenseIndex it = 0;
152  ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
153  ad_v = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
154  ad_tau = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
155 
156  pinocchio::aba(ad_model,ad_data,ad_q,ad_v,ad_tau);
157  ad_Y = ad_data.ddq;
158 
159  ad_fun.Dependent(ad_X,ad_Y);
160  ad_fun.optimize("no_compare_op");
161  }
162 
163  using Base::evalFunction;
164  template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
165  void evalFunction(const Eigen::MatrixBase<ConfigVectorType> & q,
166  const Eigen::MatrixBase<TangentVector1> & v,
167  const Eigen::MatrixBase<TangentVector2> & tau)
168  {
169  // fill x
170  Eigen::DenseIndex it = 0;
171  x.segment(it,ad_model.nq) = q; it += ad_model.nq;
172  x.segment(it,ad_model.nv) = v; it += ad_model.nv;
173  x.segment(it,ad_model.nv) = tau; it += ad_model.nv;
174 
175  evalFunction(x);
176  res = Base::y;
177  }
178 
179  using Base::evalJacobian;
180  template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
181  void evalJacobian(const Eigen::MatrixBase<ConfigVectorType> & q,
182  const Eigen::MatrixBase<TangentVector1> & v,
183  const Eigen::MatrixBase<TangentVector2> & tau)
184  {
185  // fill x
186  Eigen::DenseIndex it = 0;
187  x.segment(it,ad_model.nq) = q; it += ad_model.nq;
188  x.segment(it,ad_model.nv) = v; it += ad_model.nv;
189  x.segment(it,ad_model.nv) = tau; it += ad_model.nv;
190 
191  evalJacobian(x);
192 
193  it = 0;
194  da_dq = Base::jac.middleCols(it,ad_model.nq); it += ad_model.nq;
195  da_dv = Base::jac.middleCols(it,ad_model.nv); it += ad_model.nv;
196  da_dtau = Base::jac.middleCols(it,ad_model.nv); it += ad_model.nv;
197  }
198 
199  protected:
200 
201  using Base::ad_model;
202  using Base::ad_data;
203  using Base::ad_fun;
204  using Base::ad_X;
205  using Base::ad_Y;
206  using Base::y;
207  using Base::jac;
208 
209  VectorXs x;
210  VectorXs res;
211  MatrixXs da_dq,da_dv,da_dtau;
212 
213  ADCongigVectorType ad_q, ad_q_plus;
214  ADTangentVectorType ad_dq, ad_v, ad_tau;
215  };
216 
217  template<typename _Scalar>
218  struct CodeGenCRBA : public CodeGenBase<_Scalar>
219  {
220  typedef CodeGenBase<_Scalar> Base;
221  typedef typename Base::Scalar Scalar;
222 
223  typedef typename Base::Model Model;
224  typedef typename Base::ADCongigVectorType ADCongigVectorType;
225  typedef typename Base::ADTangentVectorType ADTangentVectorType;
226  typedef typename Base::MatrixXs MatrixXs;
227  typedef typename Base::VectorXs VectorXs;
228 
229  CodeGenCRBA(const Model & model,
230  const std::string & function_name = "crba",
231  const std::string & library_name = "cg_crba_eval")
232  : Base(model,model.nq,(model.nv*(model.nv+1))/2,function_name,library_name)
233  {
234  ad_q = ADCongigVectorType(model.nq); ad_q = neutral(model);
235  x = VectorXs::Zero(Base::getInputDimension());
236  res = VectorXs::Zero(Base::getOutputDimension());
237 
238  M = MatrixXs::Zero(model.nv,model.nq);
239 
240  Base::build_jacobian = false;
241  }
242 
243  void buildMap()
244  {
245  CppAD::Independent(ad_X);
246 
247  Eigen::DenseIndex it = 0;
248  ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
249 
250  pinocchio::crba(ad_model,ad_data,ad_q);
251  Eigen::DenseIndex it_Y = 0;
252 
253  for(Eigen::DenseIndex i = 0; i < ad_model.nv; ++i)
254  {
255  for(Eigen::DenseIndex j = i; j < ad_model.nv; ++j)
256  {
257  ad_Y[it_Y++] = ad_data.M(i,j);
258  }
259  }
260 
261  assert(it_Y == Base::getOutputDimension());
262 
263  ad_fun.Dependent(ad_X,ad_Y);
264  ad_fun.optimize("no_compare_op");
265  }
266 
267  template<typename ConfigVectorType>
268  void evalFunction(const Eigen::MatrixBase<ConfigVectorType> & q)
269  {
270  // fill x
271  Eigen::DenseIndex it = 0;
272  x.segment(it,ad_model.nq) = q; it += ad_model.nq;
273 
274  Base::evalFunction(x);
275 
276  // fill M
277  Eigen::DenseIndex it_Y = 0;
278  for(Eigen::DenseIndex i = 0; i < ad_model.nv; ++i)
279  {
280  for(Eigen::DenseIndex j = i; j < ad_model.nv; ++j)
281  {
282  M(i,j) = Base::y[it_Y++];
283  }
284  }
285 
286  assert(it_Y == Base::getOutputDimension());
287  }
288 
289  protected:
290 
291  using Base::ad_model;
292  using Base::ad_data;
293  using Base::ad_fun;
294  using Base::ad_X;
295  using Base::ad_Y;
296  using Base::y;
297 
298  VectorXs x;
299  VectorXs res;
300  MatrixXs M;
301 
302  ADCongigVectorType ad_q;
303  };
304 
305  template<typename _Scalar>
306  struct CodeGenMinv : public CodeGenBase<_Scalar>
307  {
308  typedef CodeGenBase<_Scalar> Base;
309  typedef typename Base::Scalar Scalar;
310 
311  typedef typename Base::Model Model;
312  typedef typename Base::ADCongigVectorType ADCongigVectorType;
313  typedef typename Base::ADTangentVectorType ADTangentVectorType;
314  typedef typename Base::MatrixXs MatrixXs;
315  typedef typename Base::VectorXs VectorXs;
316 
317  CodeGenMinv(const Model & model,
318  const std::string & function_name = "minv",
319  const std::string & library_name = "cg_minv_eval")
320  : Base(model,model.nq,(model.nv*(model.nv+1))/2,function_name,library_name)
321  {
322  ad_q = ADCongigVectorType(model.nq); ad_q = neutral(model);
323  x = VectorXs::Zero(Base::getInputDimension());
324  res = VectorXs::Zero(Base::getOutputDimension());
325 
326  Minv = MatrixXs::Zero(model.nv,model.nq);
327 
328  Base::build_jacobian = false;
329  }
330 
331  void buildMap()
332  {
333  CppAD::Independent(ad_X);
334 
335  Eigen::DenseIndex it = 0;
336  ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
337 
338  pinocchio::computeMinverse(ad_model,ad_data,ad_q);
339  Eigen::DenseIndex it_Y = 0;
340  for(Eigen::DenseIndex i = 0; i < ad_model.nv; ++i)
341  {
342  for(Eigen::DenseIndex j = i; j < ad_model.nv; ++j)
343  {
344  ad_Y[it_Y++] = ad_data.Minv(i,j);
345  }
346  }
347 
348  assert(it_Y == Base::getOutputDimension());
349 
350  ad_fun.Dependent(ad_X,ad_Y);
351  ad_fun.optimize("no_compare_op");
352  }
353 
354  template<typename ConfigVectorType>
355  void evalFunction(const Eigen::MatrixBase<ConfigVectorType> & q)
356  {
357  // fill x
358  Eigen::DenseIndex it = 0;
359  x.segment(it,ad_model.nq) = q; it += ad_model.nq;
360 
361  Base::evalFunction(x);
362 
363  // fill Minv
364  Eigen::DenseIndex it_Y = 0;
365  for(Eigen::DenseIndex i = 0; i < ad_model.nv; ++i)
366  {
367  for(Eigen::DenseIndex j = i; j < ad_model.nv; ++j)
368  {
369  Minv(i,j) = Base::y[it_Y++];
370  }
371  }
372  }
373 
374  protected:
375 
376  using Base::ad_model;
377  using Base::ad_data;
378  using Base::ad_fun;
379  using Base::ad_X;
380  using Base::ad_Y;
381  using Base::y;
382 
383  VectorXs x;
384  VectorXs res;
385  MatrixXs Minv;
386 
387  ADCongigVectorType ad_q;
388  };
389 
390  template<typename _Scalar>
391  struct CodeGenRNEADerivatives : public CodeGenBase<_Scalar>
392  {
393  typedef CodeGenBase<_Scalar> Base;
394  typedef typename Base::Scalar Scalar;
395 
396  typedef typename Base::Model Model;
397  typedef typename Base::ADCongigVectorType ADCongigVectorType;
398  typedef typename Base::ADTangentVectorType ADTangentVectorType;
399  typedef typename Base::MatrixXs MatrixXs;
400  typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXs) RowMatrixXs;
401  typedef typename Base::VectorXs VectorXs;
402 
403  typedef typename Base::ADData ADData;
404  typedef typename ADData::MatrixXs ADMatrixXs;
405  typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(ADMatrixXs) RowADMatrixXs;
406 
407  CodeGenRNEADerivatives(const Model & model,
408  const std::string & function_name = "partial_rnea",
409  const std::string & library_name = "cg_partial_rnea_eval")
410  : Base(model,model.nq+2*model.nv,3*model.nv*model.nv,function_name,library_name)
411  {
412  ad_q = ADCongigVectorType(model.nq); ad_q = neutral(model);
413  ad_v = ADTangentVectorType(model.nv); ad_v.setZero();
414  ad_a = ADTangentVectorType(model.nv); ad_a.setZero();
415 
416  x = VectorXs::Zero(Base::getInputDimension());
417  partial_derivatives = VectorXs::Zero(Base::getOutputDimension());
418 
419  ad_dtau_dq = ADMatrixXs::Zero(model.nv,model.nv);
420  ad_dtau_dv = ADMatrixXs::Zero(model.nv,model.nv);
421  ad_dtau_da = ADMatrixXs::Zero(model.nv,model.nv);
422 
423  dtau_dq = MatrixXs::Zero(model.nv,model.nv);
424  dtau_dv = MatrixXs::Zero(model.nv,model.nv);
425  dtau_da = MatrixXs::Zero(model.nv,model.nv);
426 
427  Base::build_jacobian = false;
428  }
429 
430  void buildMap()
431  {
432  CppAD::Independent(ad_X);
433 
434  Eigen::DenseIndex it = 0;
435  ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
436  ad_v = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
437  ad_a = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
438 
439  pinocchio::computeRNEADerivatives(ad_model,ad_data,
440  ad_q,ad_v,ad_a,
441  ad_dtau_dq,ad_dtau_dv,ad_dtau_da);
442 
443  assert(ad_Y.size() == Base::getOutputDimension());
444 
445  Eigen::DenseIndex it_Y = 0;
446  Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dtau_dq;
447  it_Y += ad_model.nv*ad_model.nv;
448  Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dtau_dv;
449  it_Y += ad_model.nv*ad_model.nv;
450  Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dtau_da;
451  it_Y += ad_model.nv*ad_model.nv;
452 
453  ad_fun.Dependent(ad_X,ad_Y);
454  ad_fun.optimize("no_compare_op");
455  }
456 
457  template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
458  void evalFunction(const Eigen::MatrixBase<ConfigVectorType> & q,
459  const Eigen::MatrixBase<TangentVector1> & v,
460  const Eigen::MatrixBase<TangentVector2> & a)
461  {
462  // fill x
463  Eigen::DenseIndex it_x = 0;
464  x.segment(it_x,ad_model.nq) = q; it_x += ad_model.nq;
465  x.segment(it_x,ad_model.nq) = v; it_x += ad_model.nv;
466  x.segment(it_x,ad_model.nq) = a; it_x += ad_model.nv;
467 
468  Base::evalFunction(x);
469 
470  // fill partial derivatives
471  Eigen::DenseIndex it_y = 0;
472  dtau_dq = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
473  it_y += ad_model.nv*ad_model.nv;
474  dtau_dv = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
475  it_y += ad_model.nv*ad_model.nv;
476  dtau_da = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
477  it_y += ad_model.nv*ad_model.nv;
478 
479  }
480 
481  protected:
482 
483  using Base::ad_model;
484  using Base::ad_data;
485  using Base::ad_fun;
486  using Base::ad_X;
487  using Base::ad_Y;
488  using Base::y;
489 
490  VectorXs x;
491  VectorXs partial_derivatives;
492  ADMatrixXs ad_dtau_dq, ad_dtau_dv, ad_dtau_da;
493  MatrixXs dtau_dq, dtau_dv, dtau_da;
494 
495  ADCongigVectorType ad_q;
496  ADTangentVectorType ad_v, ad_a;
497  };
498 
499  template<typename _Scalar>
500  struct CodeGenABADerivatives : public CodeGenBase<_Scalar>
501  {
502  typedef CodeGenBase<_Scalar> Base;
503  typedef typename Base::Scalar Scalar;
504 
505  typedef typename Base::Model Model;
506  typedef typename Base::ADCongigVectorType ADCongigVectorType;
507  typedef typename Base::ADTangentVectorType ADTangentVectorType;
508  typedef typename Base::MatrixXs MatrixXs;
509  typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXs) RowMatrixXs;
510  typedef typename Base::VectorXs VectorXs;
511 
512  typedef typename Base::ADData ADData;
513  typedef typename ADData::MatrixXs ADMatrixXs;
514  typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(ADMatrixXs) RowADMatrixXs;
515 
516  CodeGenABADerivatives(const Model & model,
517  const std::string & function_name = "partial_aba",
518  const std::string & library_name = "cg_partial_aba_eval")
519  : Base(model,model.nq+2*model.nv,3*model.nv*model.nv,function_name,library_name)
520  {
521  ad_q = ADCongigVectorType(model.nq); ad_q = neutral(model);
522  ad_v = ADTangentVectorType(model.nv); ad_v.setZero();
523  ad_tau = ADTangentVectorType(model.nv); ad_tau.setZero();
524 
525  x = VectorXs::Zero(Base::getInputDimension());
526  partial_derivatives = VectorXs::Zero(Base::getOutputDimension());
527 
528  ad_dddq_dq = ADMatrixXs::Zero(model.nv,model.nv);
529  ad_dddq_dv = ADMatrixXs::Zero(model.nv,model.nv);
530  ad_dddq_dtau = ADMatrixXs::Zero(model.nv,model.nv);
531 
532  dddq_dq = MatrixXs::Zero(model.nv,model.nv);
533  dddq_dv = MatrixXs::Zero(model.nv,model.nv);
534  dddq_dtau = MatrixXs::Zero(model.nv,model.nv);
535 
536  Base::build_jacobian = false;
537  }
538 
539  void buildMap()
540  {
541  CppAD::Independent(ad_X);
542 
543  Eigen::DenseIndex it = 0;
544  ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
545  ad_v = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
546  ad_tau = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
547 
548  pinocchio::computeABADerivatives(ad_model,ad_data,
549  ad_q,ad_v,ad_tau,
550  ad_dddq_dq,ad_dddq_dv,ad_dddq_dtau);
551 
552  assert(ad_Y.size() == Base::getOutputDimension());
553 
554  Eigen::DenseIndex it_Y = 0;
555  Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dddq_dq;
556  it_Y += ad_model.nv*ad_model.nv;
557  Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dddq_dv;
558  it_Y += ad_model.nv*ad_model.nv;
559  Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dddq_dtau;
560  it_Y += ad_model.nv*ad_model.nv;
561 
562  ad_fun.Dependent(ad_X,ad_Y);
563  ad_fun.optimize("no_compare_op");
564  }
565 
566  template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
567  void evalFunction(const Eigen::MatrixBase<ConfigVectorType> & q,
568  const Eigen::MatrixBase<TangentVector1> & v,
569  const Eigen::MatrixBase<TangentVector2> & tau)
570  {
571  // fill x
572  Eigen::DenseIndex it_x = 0;
573  x.segment(it_x,ad_model.nq) = q; it_x += ad_model.nq;
574  x.segment(it_x,ad_model.nq) = v; it_x += ad_model.nv;
575  x.segment(it_x,ad_model.nq) = tau; it_x += ad_model.nv;
576 
577  Base::evalFunction(x);
578 
579  // fill partial derivatives
580  Eigen::DenseIndex it_y = 0;
581  dddq_dq = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
582  it_y += ad_model.nv*ad_model.nv;
583  dddq_dv = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
584  it_y += ad_model.nv*ad_model.nv;
585  dddq_dtau = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
586  it_y += ad_model.nv*ad_model.nv;
587 
588  }
589 
590  protected:
591 
592  using Base::ad_model;
593  using Base::ad_data;
594  using Base::ad_fun;
595  using Base::ad_X;
596  using Base::ad_Y;
597  using Base::y;
598 
599  VectorXs x;
600  VectorXs partial_derivatives;
601  ADMatrixXs ad_dddq_dq, ad_dddq_dv, ad_dddq_dtau;
602  MatrixXs dddq_dq, dddq_dv, dddq_dtau;
603 
604  ADCongigVectorType ad_q;
605  ADTangentVectorType ad_v, ad_tau;
606  };
607 
608 } // namespace pinocchio
609 
610 #endif // ifdef PINOCCHIO_WITH_CPPADCG_SUPPORT
611 
612 #endif // ifndef __pinocchio_utils_code_generator_base_hpp__
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & aba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
const DataTpl< Scalar, Options, JointCollectionTpl >::MatrixXs & crba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the upper triangular part of the joint space inertia matrix M by using the Composite Rigid B...
Eigen::DenseIndex getOutputDimension() const
Dimension of the output vector.
const DataTpl< Scalar, Options, JointCollectionTpl >::RowMatrixXs & computeMinverse(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q)
Computes the inverse of the joint space inertia matrix using Articulated Body formulation.
const std::string function_name
Name of the function.
Eigen::DenseIndex getInputDimension() const
Dimension of the input vector.
void buildMap()
build the mapping Y = f(X)
void buildMap()
build the mapping Y = f(X)
void buildMap()
build the mapping Y = f(X)
int nv
Dimension of the velocity vector space.
void computeRNEADerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a, const Eigen::MatrixBase< MatrixType1 > &rnea_partial_dq, const Eigen::MatrixBase< MatrixType2 > &rnea_partial_dv, const Eigen::MatrixBase< MatrixType3 > &rnea_partial_da)
Computes the partial derivatives of the Recursive Newton Euler Algorithms with respect to the joint c...
const std::string library_name
Name of the library.
bool build_jacobian
Options to build or not the Jacobian of he function.
void buildMap()
build the mapping Y = f(X)
Main pinocchio namespace.
Definition: treeview.dox:24
TangentVectorType tau
Vector of joint torques (dim model.nv).
Definition: data.hpp:125
void neutral(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ReturnType > &qout)
Return the neutral configuration element related to the model configuration space.
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & rnea(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &a)
The Recursive Newton-Euler algorithm. It computes the inverse dynamics, aka the joint torques accordi...
void buildMap()
build the mapping Y = f(X)
void computeABADerivatives(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const Eigen::MatrixBase< MatrixType1 > &aba_partial_dq, const Eigen::MatrixBase< MatrixType2 > &aba_partial_dv, const Eigen::MatrixBase< MatrixType3 > &aba_partial_dtau)
The derivatives of the Articulated-Body algorithm.
void buildMap()
build the mapping Y = f(X)
int nq
Dimension of the configuration vector representation.