pinocchio  2.4.4
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/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::ADConfigVectorType ADConfigVectorType;
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 = ADConfigVectorType(model.nq); ad_q = neutral(ad_model);
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  MatrixXs dtau_dq, dtau_dv, dtau_da;
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 
114  ADConfigVectorType ad_q, ad_q_plus;
115  ADTangentVectorType ad_dq, ad_v, ad_a;
116  };
117 
118  template<typename _Scalar>
119  struct CodeGenABA : public CodeGenBase<_Scalar>
120  {
121  typedef CodeGenBase<_Scalar> Base;
122  typedef typename Base::Scalar Scalar;
123 
124  typedef typename Base::Model Model;
125  typedef typename Base::ADConfigVectorType ADConfigVectorType;
126  typedef typename Base::ADTangentVectorType ADTangentVectorType;
127  typedef typename Base::MatrixXs MatrixXs;
128  typedef typename Base::VectorXs VectorXs;
129 
130  CodeGenABA(const Model & model,
131  const std::string & function_name = "aba",
132  const std::string & library_name = "cg_aba_eval")
133  : Base(model,model.nq+2*model.nv,model.nv,function_name,library_name)
134  {
135  ad_q = ADConfigVectorType(model.nq); ad_q = neutral(ad_model);
136  ad_v = ADTangentVectorType(model.nv); ad_v.setZero();
137  ad_tau = ADTangentVectorType(model.nv); ad_tau.setZero();
138  x = VectorXs::Zero(Base::getInputDimension());
139  res = VectorXs::Zero(Base::getOutputDimension());
140 
141  da_dq = MatrixXs::Zero(model.nv,model.nq);
142  da_dv = MatrixXs::Zero(model.nv,model.nv);
143  da_dtau = MatrixXs::Zero(model.nv,model.nv);
144  }
145 
146  void buildMap()
147  {
148  CppAD::Independent(ad_X);
149 
150  Eigen::DenseIndex it = 0;
151  ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
152  ad_v = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
153  ad_tau = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
154 
155  pinocchio::aba(ad_model,ad_data,ad_q,ad_v,ad_tau);
156  ad_Y = ad_data.ddq;
157 
158  ad_fun.Dependent(ad_X,ad_Y);
159  ad_fun.optimize("no_compare_op");
160  }
161 
162  using Base::evalFunction;
163  template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
164  void evalFunction(const Eigen::MatrixBase<ConfigVectorType> & q,
165  const Eigen::MatrixBase<TangentVector1> & v,
166  const Eigen::MatrixBase<TangentVector2> & tau)
167  {
168  // fill x
169  Eigen::DenseIndex it = 0;
170  x.segment(it,ad_model.nq) = q; it += ad_model.nq;
171  x.segment(it,ad_model.nv) = v; it += ad_model.nv;
172  x.segment(it,ad_model.nv) = tau; it += ad_model.nv;
173 
174  evalFunction(x);
175  res = Base::y;
176  }
177 
178  using Base::evalJacobian;
179  template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
180  void evalJacobian(const Eigen::MatrixBase<ConfigVectorType> & q,
181  const Eigen::MatrixBase<TangentVector1> & v,
182  const Eigen::MatrixBase<TangentVector2> & tau)
183  {
184  // fill x
185  Eigen::DenseIndex it = 0;
186  x.segment(it,ad_model.nq) = q; it += ad_model.nq;
187  x.segment(it,ad_model.nv) = v; it += ad_model.nv;
188  x.segment(it,ad_model.nv) = tau; it += ad_model.nv;
189 
190  evalJacobian(x);
191 
192  it = 0;
193  da_dq = Base::jac.middleCols(it,ad_model.nq); it += ad_model.nq;
194  da_dv = Base::jac.middleCols(it,ad_model.nv); it += ad_model.nv;
195  da_dtau = Base::jac.middleCols(it,ad_model.nv); it += ad_model.nv;
196  }
197 
198  MatrixXs da_dq,da_dv,da_dtau;
199 
200  protected:
201 
202  using Base::ad_model;
203  using Base::ad_data;
204  using Base::ad_fun;
205  using Base::ad_X;
206  using Base::ad_Y;
207  using Base::y;
208  using Base::jac;
209 
210  VectorXs x;
211  VectorXs res;
212 
213  ADConfigVectorType 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::ADConfigVectorType ADConfigVectorType;
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 = ADConfigVectorType(model.nq); ad_q = neutral(ad_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  MatrixXs M;
290 
291  protected:
292 
293  using Base::ad_model;
294  using Base::ad_data;
295  using Base::ad_fun;
296  using Base::ad_X;
297  using Base::ad_Y;
298  using Base::y;
299 
300  VectorXs x;
301  VectorXs res;
302 
303  ADConfigVectorType ad_q;
304  };
305 
306  template<typename _Scalar>
307  struct CodeGenMinv : public CodeGenBase<_Scalar>
308  {
309  typedef CodeGenBase<_Scalar> Base;
310  typedef typename Base::Scalar Scalar;
311 
312  typedef typename Base::Model Model;
313  typedef typename Base::ADConfigVectorType ADConfigVectorType;
314  typedef typename Base::ADTangentVectorType ADTangentVectorType;
315  typedef typename Base::MatrixXs MatrixXs;
316  typedef typename Base::VectorXs VectorXs;
317 
318  CodeGenMinv(const Model & model,
319  const std::string & function_name = "minv",
320  const std::string & library_name = "cg_minv_eval")
321  : Base(model,model.nq,(model.nv*(model.nv+1))/2,function_name,library_name)
322  {
323  ad_q = ADConfigVectorType(model.nq); ad_q = neutral(ad_model);
324  x = VectorXs::Zero(Base::getInputDimension());
325  res = VectorXs::Zero(Base::getOutputDimension());
326 
327  Minv = MatrixXs::Zero(model.nv,model.nq);
328 
329  Base::build_jacobian = false;
330  }
331 
332  void buildMap()
333  {
334  CppAD::Independent(ad_X);
335 
336  Eigen::DenseIndex it = 0;
337  ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
338 
339  pinocchio::computeMinverse(ad_model,ad_data,ad_q);
340  Eigen::DenseIndex it_Y = 0;
341  for(Eigen::DenseIndex i = 0; i < ad_model.nv; ++i)
342  {
343  for(Eigen::DenseIndex j = i; j < ad_model.nv; ++j)
344  {
345  ad_Y[it_Y++] = ad_data.Minv(i,j);
346  }
347  }
348 
349  assert(it_Y == Base::getOutputDimension());
350 
351  ad_fun.Dependent(ad_X,ad_Y);
352  ad_fun.optimize("no_compare_op");
353  }
354 
355  template<typename ConfigVectorType>
356  void evalFunction(const Eigen::MatrixBase<ConfigVectorType> & q)
357  {
358  // fill x
359  Eigen::DenseIndex it = 0;
360  x.segment(it,ad_model.nq) = q; it += ad_model.nq;
361 
362  Base::evalFunction(x);
363 
364  // fill Minv
365  Eigen::DenseIndex it_Y = 0;
366  for(Eigen::DenseIndex i = 0; i < ad_model.nv; ++i)
367  {
368  for(Eigen::DenseIndex j = i; j < ad_model.nv; ++j)
369  {
370  Minv(i,j) = Base::y[it_Y++];
371  }
372  }
373  }
374 
375  MatrixXs Minv;
376 
377  protected:
378 
379  using Base::ad_model;
380  using Base::ad_data;
381  using Base::ad_fun;
382  using Base::ad_X;
383  using Base::ad_Y;
384  using Base::y;
385 
386  VectorXs x;
387  VectorXs res;
388 
389  ADConfigVectorType ad_q;
390  };
391 
392  template<typename _Scalar>
393  struct CodeGenRNEADerivatives : public CodeGenBase<_Scalar>
394  {
395  typedef CodeGenBase<_Scalar> Base;
396  typedef typename Base::Scalar Scalar;
397 
398  typedef typename Base::Model Model;
399  typedef typename Base::ADConfigVectorType ADConfigVectorType;
400  typedef typename Base::ADTangentVectorType ADTangentVectorType;
401  typedef typename Base::MatrixXs MatrixXs;
402  typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXs) RowMatrixXs;
403  typedef typename Base::VectorXs VectorXs;
404 
405  typedef typename Base::ADData ADData;
406  typedef typename ADData::MatrixXs ADMatrixXs;
407  typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(ADMatrixXs) RowADMatrixXs;
408 
409  CodeGenRNEADerivatives(const Model & model,
410  const std::string & function_name = "partial_rnea",
411  const std::string & library_name = "cg_partial_rnea_eval")
412  : Base(model,model.nq+2*model.nv,3*model.nv*model.nv,function_name,library_name)
413  {
414  ad_q = ADConfigVectorType(model.nq); ad_q = neutral(ad_model);
415  ad_v = ADTangentVectorType(model.nv); ad_v.setZero();
416  ad_a = ADTangentVectorType(model.nv); ad_a.setZero();
417 
418  x = VectorXs::Zero(Base::getInputDimension());
419  partial_derivatives = VectorXs::Zero(Base::getOutputDimension());
420 
421  ad_dtau_dq = ADMatrixXs::Zero(model.nv,model.nv);
422  ad_dtau_dv = ADMatrixXs::Zero(model.nv,model.nv);
423  ad_dtau_da = ADMatrixXs::Zero(model.nv,model.nv);
424 
425  dtau_dq = MatrixXs::Zero(model.nv,model.nv);
426  dtau_dv = MatrixXs::Zero(model.nv,model.nv);
427  dtau_da = MatrixXs::Zero(model.nv,model.nv);
428 
429  Base::build_jacobian = false;
430  }
431 
432  void buildMap()
433  {
434  CppAD::Independent(ad_X);
435 
436  Eigen::DenseIndex it = 0;
437  ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
438  ad_v = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
439  ad_a = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
440 
441  pinocchio::computeRNEADerivatives(ad_model,ad_data,
442  ad_q,ad_v,ad_a,
443  ad_dtau_dq,ad_dtau_dv,ad_dtau_da);
444 
445  assert(ad_Y.size() == Base::getOutputDimension());
446 
447  Eigen::DenseIndex it_Y = 0;
448  Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dtau_dq;
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_dv;
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_da;
453  it_Y += ad_model.nv*ad_model.nv;
454 
455  ad_fun.Dependent(ad_X,ad_Y);
456  ad_fun.optimize("no_compare_op");
457  }
458 
459  template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
460  void evalFunction(const Eigen::MatrixBase<ConfigVectorType> & q,
461  const Eigen::MatrixBase<TangentVector1> & v,
462  const Eigen::MatrixBase<TangentVector2> & a)
463  {
464  // fill x
465  Eigen::DenseIndex it_x = 0;
466  x.segment(it_x,ad_model.nq) = q; it_x += ad_model.nq;
467  x.segment(it_x,ad_model.nq) = v; it_x += ad_model.nv;
468  x.segment(it_x,ad_model.nq) = a; it_x += ad_model.nv;
469 
470  Base::evalFunction(x);
471 
472  // fill partial derivatives
473  Eigen::DenseIndex it_y = 0;
474  dtau_dq = 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_dv = 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_da = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
479  it_y += ad_model.nv*ad_model.nv;
480 
481  }
482 
483  protected:
484 
485  using Base::ad_model;
486  using Base::ad_data;
487  using Base::ad_fun;
488  using Base::ad_X;
489  using Base::ad_Y;
490  using Base::y;
491 
492  VectorXs x;
493  VectorXs partial_derivatives;
494  ADMatrixXs ad_dtau_dq, ad_dtau_dv, ad_dtau_da;
495  MatrixXs dtau_dq, dtau_dv, dtau_da;
496 
497  ADConfigVectorType ad_q;
498  ADTangentVectorType ad_v, ad_a;
499  };
500 
501  template<typename _Scalar>
502  struct CodeGenABADerivatives : public CodeGenBase<_Scalar>
503  {
504  typedef CodeGenBase<_Scalar> Base;
505  typedef typename Base::Scalar Scalar;
506 
507  typedef typename Base::Model Model;
508  typedef typename Base::ADConfigVectorType ADConfigVectorType;
509  typedef typename Base::ADTangentVectorType ADTangentVectorType;
510  typedef typename Base::MatrixXs MatrixXs;
511  typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(MatrixXs) RowMatrixXs;
512  typedef typename Base::VectorXs VectorXs;
513 
514  typedef typename Base::ADData ADData;
515  typedef typename ADData::MatrixXs ADMatrixXs;
516  typedef typename PINOCCHIO_EIGEN_PLAIN_ROW_MAJOR_TYPE(ADMatrixXs) RowADMatrixXs;
517 
518  CodeGenABADerivatives(const Model & model,
519  const std::string & function_name = "partial_aba",
520  const std::string & library_name = "cg_partial_aba_eval")
521  : Base(model,model.nq+2*model.nv,3*model.nv*model.nv,function_name,library_name)
522  {
523  ad_q = ADConfigVectorType(model.nq); ad_q = neutral(ad_model);
524  ad_v = ADTangentVectorType(model.nv); ad_v.setZero();
525  ad_tau = ADTangentVectorType(model.nv); ad_tau.setZero();
526 
527  x = VectorXs::Zero(Base::getInputDimension());
528  partial_derivatives = VectorXs::Zero(Base::getOutputDimension());
529 
530  ad_dddq_dq = ADMatrixXs::Zero(model.nv,model.nv);
531  ad_dddq_dv = ADMatrixXs::Zero(model.nv,model.nv);
532  ad_dddq_dtau = ADMatrixXs::Zero(model.nv,model.nv);
533 
534  dddq_dq = MatrixXs::Zero(model.nv,model.nv);
535  dddq_dv = MatrixXs::Zero(model.nv,model.nv);
536  dddq_dtau = MatrixXs::Zero(model.nv,model.nv);
537 
538  Base::build_jacobian = false;
539  }
540 
541  void buildMap()
542  {
543  CppAD::Independent(ad_X);
544 
545  Eigen::DenseIndex it = 0;
546  ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
547  ad_v = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
548  ad_tau = ad_X.segment(it,ad_model.nv); it += ad_model.nv;
549 
550  pinocchio::computeABADerivatives(ad_model,ad_data,
551  ad_q,ad_v,ad_tau,
552  ad_dddq_dq,ad_dddq_dv,ad_dddq_dtau);
553 
554  assert(ad_Y.size() == Base::getOutputDimension());
555 
556  Eigen::DenseIndex it_Y = 0;
557  Eigen::Map<RowADMatrixXs>(ad_Y.data()+it_Y,ad_model.nv,ad_model.nv) = ad_dddq_dq;
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_dv;
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_dtau;
562  it_Y += ad_model.nv*ad_model.nv;
563 
564  ad_fun.Dependent(ad_X,ad_Y);
565  ad_fun.optimize("no_compare_op");
566  }
567 
568  template<typename ConfigVectorType, typename TangentVector1, typename TangentVector2>
569  void evalFunction(const Eigen::MatrixBase<ConfigVectorType> & q,
570  const Eigen::MatrixBase<TangentVector1> & v,
571  const Eigen::MatrixBase<TangentVector2> & tau)
572  {
573  // fill x
574  Eigen::DenseIndex it_x = 0;
575  x.segment(it_x,ad_model.nq) = q; it_x += ad_model.nq;
576  x.segment(it_x,ad_model.nq) = v; it_x += ad_model.nv;
577  x.segment(it_x,ad_model.nq) = tau; it_x += ad_model.nv;
578 
579  Base::evalFunction(x);
580 
581  // fill partial derivatives
582  Eigen::DenseIndex it_y = 0;
583  dddq_dq = 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_dv = 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_dtau = Eigen::Map<RowMatrixXs>(Base::y.data()+it_y,ad_model.nv,ad_model.nv);
588  it_y += ad_model.nv*ad_model.nv;
589 
590  }
591 
592  protected:
593 
594  using Base::ad_model;
595  using Base::ad_data;
596  using Base::ad_fun;
597  using Base::ad_X;
598  using Base::ad_Y;
599  using Base::y;
600 
601  VectorXs x;
602  VectorXs partial_derivatives;
603  ADMatrixXs ad_dddq_dq, ad_dddq_dv, ad_dddq_dtau;
604  MatrixXs dddq_dq, dddq_dv, dddq_dtau;
605 
606  ADConfigVectorType ad_q;
607  ADTangentVectorType ad_v, ad_tau;
608  };
609 
610  template<typename _Scalar>
611  struct CodeGenIntegrate : public CodeGenBase<_Scalar>
612  {
613  typedef CodeGenBase<_Scalar> Base;
614  typedef typename Base::Scalar Scalar;
615 
616  typedef typename Base::Model Model;
617  typedef typename Base::ADConfigVectorType ADConfigVectorType;
618  typedef typename Base::ADTangentVectorType ADTangentVectorType;
619  typedef typename Base::MatrixXs MatrixXs;
620  typedef typename Base::VectorXs VectorXs;
621 
622  CodeGenIntegrate(const Model & model,
623  const std::string & function_name = "integrate",
624  const std::string & library_name = "cg_integrate_eval")
625  : Base(model,model.nq+model.nv,model.nq,function_name,library_name)
626  {
627  ad_q = ADConfigVectorType(model.nq); ad_q = neutral(ad_model);
628  ad_v = ADTangentVectorType(model.nv); ad_v.setZero();
629  x = VectorXs::Zero(Base::getInputDimension());
630  res = VectorXs::Zero(Base::getOutputDimension());
631  }
632 
633  void buildMap()
634  {
635  CppAD::Independent(ad_X);
636 
637  Eigen::DenseIndex it = 0;
638  ad_q = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
639  ad_v = ad_X.segment(it,ad_model.nv);
640  pinocchio::integrate(ad_model,ad_q,ad_v,ad_Y);
641 
642  ad_fun.Dependent(ad_X,ad_Y);
643  ad_fun.optimize("no_compare_op");
644  }
645 
646  using Base::evalFunction;
647  template<typename ConfigVectorType1, typename TangentVector, typename ConfigVectorType2>
648  void evalFunction(const Eigen::MatrixBase<ConfigVectorType1> & q,
649  const Eigen::MatrixBase<TangentVector> & v,
650  const Eigen::MatrixBase<ConfigVectorType2> & qout)
651  {
652  // fill x
653  Eigen::DenseIndex it = 0;
654  x.segment(it,ad_model.nq) = q; it += ad_model.nq;
655  x.segment(it,ad_model.nv) = v;
656 
657  evalFunction(x);
658  PINOCCHIO_EIGEN_CONST_CAST(ConfigVectorType2,qout) = Base::y;
659  }
660 
661  protected:
662 
663  using Base::ad_model;
664  using Base::ad_fun;
665  using Base::ad_X;
666  using Base::ad_Y;
667  using Base::y;
668 
669  VectorXs x;
670  VectorXs res;
671 
672  ADConfigVectorType ad_q;
673  ADTangentVectorType ad_v;
674  };
675 
676 
677  template<typename _Scalar>
678  struct CodeGenDifference : public CodeGenBase<_Scalar>
679  {
680  typedef CodeGenBase<_Scalar> Base;
681  typedef typename Base::Scalar Scalar;
682 
683  typedef typename Base::Model Model;
684  typedef typename Base::ADConfigVectorType ADConfigVectorType;
685  typedef typename Base::ADTangentVectorType ADTangentVectorType;
686  typedef typename Base::MatrixXs MatrixXs;
687  typedef typename Base::VectorXs VectorXs;
688 
689  CodeGenDifference(const Model & model,
690  const std::string & function_name = "difference",
691  const std::string & library_name = "cg_difference_eval")
692  : Base(model,2*model.nq,model.nv,function_name,library_name)
693  {
694  ad_q0 = ADConfigVectorType(model.nq); ad_q0 = neutral(ad_model);
695  ad_q1 = ADConfigVectorType(model.nq); ad_q1 = neutral(ad_model);
696  x = VectorXs::Zero(Base::getInputDimension());
697  res = VectorXs::Zero(Base::getOutputDimension());
698  }
699 
700  void buildMap()
701  {
702  CppAD::Independent(ad_X);
703 
704  Eigen::DenseIndex it = 0;
705  ad_q0 = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
706  ad_q1 = ad_X.segment(it,ad_model.nq);
707  pinocchio::difference(ad_model,ad_q0,ad_q1,ad_Y);
708 
709  ad_fun.Dependent(ad_X,ad_Y);
710  ad_fun.optimize("no_compare_op");
711  }
712 
713  using Base::evalFunction;
714  template<typename ConfigVectorType1, typename ConfigVectorType2, typename TangentVector>
715  void evalFunction(const Eigen::MatrixBase<ConfigVectorType1> & q0,
716  const Eigen::MatrixBase<ConfigVectorType2> & q1,
717  const Eigen::MatrixBase<TangentVector> & v)
718  {
719  // fill x
720  Eigen::DenseIndex it = 0;
721  x.segment(it,ad_model.nq) = q0; it += ad_model.nq;
722  x.segment(it,ad_model.nq) = q1;
723 
724  evalFunction(x);
725  PINOCCHIO_EIGEN_CONST_CAST(TangentVector,v) = Base::y;
726  }
727 
728  protected:
729 
730  using Base::ad_model;
731  using Base::ad_fun;
732  using Base::ad_X;
733  using Base::ad_Y;
734  using Base::y;
735 
736  VectorXs x;
737  VectorXs res;
738 
739  ADConfigVectorType ad_q0;
740  ADConfigVectorType ad_q1;
741  };
742 
743 
744  template<typename _Scalar>
745  struct CodeGenDDifference : public CodeGenBase<_Scalar>
746  {
747  typedef CodeGenBase<_Scalar> Base;
748  typedef typename Base::Scalar Scalar;
749 
750  typedef typename Base::Model Model;
751  typedef typename Base::ADConfigVectorType ADConfigVectorType;
752  typedef typename Base::ADTangentVectorType ADTangentVectorType;
753  typedef typename Base::ADVectorXs ADVectorXs;
754  typedef typename Base::ADMatrixXs ADMatrixXs;
755  typedef typename Base::MatrixXs MatrixXs;
756  typedef typename Base::VectorXs VectorXs;
757 
758  CodeGenDDifference(const Model & model,
759  const std::string & function_name = "dDifference",
760  const std::string & library_name = "cg_dDifference_eval")
761  : Base(model,2*model.nq,2*model.nv*model.nv,function_name,library_name)
762  {
763  ad_q0 = neutral(ad_model);
764  ad_q1 = neutral(ad_model);
765  ad_J0 = ADMatrixXs::Zero(ad_model.nv, ad_model.nv);
766  ad_J1 = ADMatrixXs::Zero(ad_model.nv, ad_model.nv);
767  x = VectorXs::Zero(Base::getInputDimension());
768  }
769 
770  void buildMap()
771  {
772  CppAD::Independent(ad_X);
773 
774  Eigen::DenseIndex it = 0;
775  ad_q0 = ad_X.segment(it,ad_model.nq); it += ad_model.nq;
776  ad_q1 = ad_X.segment(it,ad_model.nq);
777  pinocchio::dDifference(ad_model,ad_q0,ad_q1,ad_J0,pinocchio::ARG0);
778  pinocchio::dDifference(ad_model,ad_q0,ad_q1,ad_J1,pinocchio::ARG1);
779 
780  Eigen::Map<ADMatrixXs>(ad_Y.data(), 2*ad_model.nv, ad_model.nv).topRows(ad_model.nv) = ad_J0;
781  Eigen::Map<ADMatrixXs>(ad_Y.data(), 2*ad_model.nv, ad_model.nv).bottomRows(ad_model.nv) = ad_J1;
782  ad_fun.Dependent(ad_X,ad_Y);
783  ad_fun.optimize("no_compare_op");
784  }
785 
786  using Base::evalFunction;
787  template<typename ConfigVectorType1, typename ConfigVectorType2, typename JacobianMatrix>
788  void evalFunction(const Eigen::MatrixBase<ConfigVectorType1> & q0,
789  const Eigen::MatrixBase<ConfigVectorType2> & q1,
790  const Eigen::MatrixBase<JacobianMatrix> & J,
791  const ArgumentPosition arg)
792  {
793  // fill x
794  Eigen::DenseIndex it = 0;
795  x.segment(it,ad_model.nq) = q0; it += ad_model.nq;
796  x.segment(it,ad_model.nq) = q1;
797 
798  evalFunction(x);
799  switch(arg)
800  {
801  case pinocchio::ARG0:
802  PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,J)
803  = Eigen::Map<MatrixXs>(Base::y.data(), 2*ad_model.nv, ad_model.nv).topRows(ad_model.nv);
804  break;
805  case pinocchio::ARG1:
806  PINOCCHIO_EIGEN_CONST_CAST(JacobianMatrix,J)
807  = Eigen::Map<MatrixXs>(Base::y.data(), 2*ad_model.nv, ad_model.nv).bottomRows(ad_model.nv);
808  break;
809  default:
810  assert(false && "Wrong argument");
811  }
812 
813  }
814 
815  protected:
816 
817  using Base::ad_model;
818  using Base::ad_fun;
819  using Base::ad_X;
820  using Base::ad_Y;
821  using Base::y;
822 
823  VectorXs x;
824  ADConfigVectorType ad_q0;
825  ADConfigVectorType ad_q1;
826  ADMatrixXs ad_J0;
827  ADMatrixXs ad_J1;
828  };
829 
830 } // namespace pinocchio
831 
832 #endif // ifndef __pinocchio_codegen_code_generator_algo_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::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::CodeGenBase::getOutputDimension
Eigen::DenseIndex getOutputDimension() const
Dimension of the output vector.
Definition: code-generator-base.hpp:144
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:152
pinocchio::CodeGenBase::getInputDimension
Eigen::DenseIndex getInputDimension() const
Dimension of the input vector.
Definition: code-generator-base.hpp:142
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:54
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:505
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:142
pinocchio::CodeGenDDifference::buildMap
void buildMap()
build the mapping Y = f(X)
Definition: code-generator-algo.hpp:770
pinocchio::CodeGenRNEADerivatives::buildMap
void buildMap()
build the mapping Y = f(X)
Definition: code-generator-algo.hpp:432
pinocchio::CodeGenCRBA::buildMap
void buildMap()
build the mapping Y = f(X)
Definition: code-generator-algo.hpp:243
pinocchio::CodeGenABADerivatives::buildMap
void buildMap()
build the mapping Y = f(X)
Definition: code-generator-algo.hpp:541
pinocchio::ModelTpl::nv
int nv
Dimension of the velocity vector space.
Definition: model.hpp:78
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::CodeGenBase::library_name
const std::string library_name
Name of the library.
Definition: code-generator-base.hpp:154
pinocchio::CodeGenBase::build_jacobian
bool build_jacobian
Options to build or not the Jacobian of he function.
Definition: code-generator-base.hpp:160
pinocchio::CodeGenMinv::buildMap
void buildMap()
build the mapping Y = f(X)
Definition: code-generator-algo.hpp:332
pinocchio::ArgumentPosition
ArgumentPosition
Argument position. Used as template parameter to refer to an argument.
Definition: fwd.hpp:50
pinocchio::CodeGenIntegrate::buildMap
void buildMap()
build the mapping Y = f(X)
Definition: code-generator-algo.hpp:633
pinocchio::CodeGenDifference::buildMap
void buildMap()
build the mapping Y = f(X)
Definition: code-generator-algo.hpp:700
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:24
pinocchio::DataTpl::tau
TangentVectorType tau
Vector of joint torques (dim model.nv).
Definition: data.hpp:128
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:257
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:146
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: model.hpp:75