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