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