pinocchio  3.6.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
casadi-algo.hpp
1 //
2 // Copyright (c) 2021 INRIA
3 //
4 
5 #ifndef __pinocchio_autodiff_code_generator_algo_hpp__
6 #define __pinocchio_autodiff_code_generator_algo_hpp__
7 
8 #include <casadi/core/casadi_types.hpp>
9 #include <casadi/core/code_generator.hpp>
10 
11 #include "pinocchio/autodiff/casadi.hpp"
12 #include "pinocchio/algorithm/aba.hpp"
13 #include "pinocchio/algorithm/aba-derivatives.hpp"
14 #include "pinocchio/algorithm/joint-configuration.hpp"
15 #include "pinocchio/algorithm/constrained-dynamics-derivatives.hpp"
16 #include "pinocchio/algorithm/constrained-dynamics.hpp"
17 
18 namespace pinocchio
19 {
20  namespace casadi
21  {
22 
23  template<typename _Scalar>
24  struct AutoDiffAlgoBase
25  {
26  typedef _Scalar Scalar;
27  typedef ::casadi::SX ADScalar;
28  typedef ::casadi::SXVector ADSVector;
29  typedef ::casadi::DM DMMatrix;
30  typedef ::casadi::DMVector DMVector;
31  enum
32  {
33  Options = 0
34  };
35 
36  typedef pinocchio::ModelTpl<Scalar, Options> Model;
37  typedef pinocchio::DataTpl<Scalar, Options> Data;
38  typedef pinocchio::ModelTpl<ADScalar, Options> ADModel;
39  typedef pinocchio::DataTpl<ADScalar, Options> ADData;
40 
41  typedef typename Model::ConfigVectorType ConfigVectorType;
42  typedef typename Model::TangentVectorType TangentVectorType;
43  typedef typename ADModel::ConfigVectorType ADConfigVectorType;
44  typedef typename ADModel::TangentVectorType ADTangentVectorType;
45 
46  typedef typename Data::MatrixXs MatrixXs;
47  typedef typename Data::VectorXs VectorXs;
48  typedef typename Data::RowMatrixXs RowMatrixXs;
49 
50  typedef ::casadi::Function ADFun;
51 
52  AutoDiffAlgoBase(
53  const Model & model,
54  const std::string & filename,
55  const std::string & libname,
56  const std::string & fun_name)
57  : ad_model(model.template cast<ADScalar>())
58  , ad_data(ad_model)
59  , filename(filename)
60  , libname(libname)
61  , fun_name(fun_name)
62  , cg_generated(filename)
63  , build_forward(true)
64  , build_jacobian(true)
65  {
66  }
67 
68  virtual ~AutoDiffAlgoBase()
69  {
70  }
71 
73  virtual void buildMap() = 0;
74 
75  void initLib()
76  {
77  buildMap();
78  // Generated code;
79  cg_generated.add(ad_fun);
80  fun_operation_count = ad_fun.n_instructions() - ad_fun.nnz_in() - ad_fun.nnz_out();
81  if (build_jacobian)
82  {
83  cg_generated.add(ad_fun_derivs);
84  fun_derivs_operation_count =
85  ad_fun_derivs.n_instructions() - ad_fun_derivs.nnz_in() - ad_fun_derivs.nnz_out();
86  }
87  cg_generated.generate();
88  }
89 
90  bool existLib() const
91  {
92  std::ifstream file((libname + ".so").c_str());
93  return file.good();
94  }
95 
96  void compileLib()
97  {
98  std::string compile_command =
99  "clang -fPIC -shared -Ofast -DNDEBUG " + filename + ".c -o " + libname + ".so";
100 
101 #ifdef __APPLE__
102  compile_command += " -isysroot "
103  "/Applications/Xcode.app/Contents/Developer/Platforms/MacOSX.platform/"
104  "Developer/SDKs/MacOSX.sdk";
105 #endif
106 
107  const int flag = system(compile_command.c_str());
108  if (flag != 0)
109  {
110  std::cerr << "Compilation failed" << std::endl; // TODO: raised here
111  }
112  }
113 
114  void loadLib(const bool generate_if_not_exist = true)
115  {
116  if (!existLib() && generate_if_not_exist)
117  compileLib();
118 
119  fun = ::casadi::external(fun_name, "./" + libname + ".so");
120  if (build_jacobian)
121  {
122  fun_derivs = ::casadi::external(fun_name + "_derivs", "./" + libname + ".so");
123  }
124  }
125 
126  casadi_int getFunOperationCount() const
127  {
128  return fun_operation_count;
129  }
130 
131  casadi_int getFunDerivsOperationCount() const
132  {
133  return fun_derivs_operation_count;
134  }
135 
136  protected:
137  ADModel ad_model;
138  ADData ad_data;
139  std::string filename, libname, fun_name;
140  ::casadi::CodeGenerator cg_generated;
141 
143  bool build_forward;
145  bool build_jacobian;
146 
147  ADFun ad_fun, ad_fun_derivs;
148  ADFun fun, fun_derivs;
149  std::vector<DMMatrix> fun_output, fun_output_derivs;
150  casadi_int fun_operation_count, fun_derivs_operation_count;
151  };
152 
153  template<typename _Scalar>
154  struct AutoDiffABA : public AutoDiffAlgoBase<_Scalar>
155  {
156  typedef AutoDiffAlgoBase<_Scalar> Base;
157  typedef typename Base::Scalar Scalar;
158 
159  typedef typename Base::TangentVectorType TangentVectorType;
160  typedef typename Base::RowMatrixXs RowMatrixXs;
161  typedef typename Base::VectorXs VectorXs;
162  typedef typename Base::MatrixXs MatrixXs;
163 
164  typedef typename Base::ADFun ADFun;
165  typedef typename Base::DMVector DMVector;
166  typedef typename Base::DMMatrix DMMatrix;
167  typedef typename Base::ADScalar ADScalar;
168  typedef typename Base::ADSVector ADSVector;
169  typedef typename Base::ADConfigVectorType ADConfigVectorType;
170  typedef typename Base::ADTangentVectorType ADTangentVectorType;
171 
172  explicit AutoDiffABA(
173  const Model & model,
174  const std::string & filename = "casadi_aba",
175  const std::string & libname = "libcasadi_cg_aba",
176  const std::string & fun_name = "eval_f")
177  : Base(model, filename, libname, fun_name)
178  , ddq(model.nv)
179  , ddq_dq(model.nv, model.nv)
180  , ddq_dv(model.nv, model.nv)
181  , ddq_dtau(model.nv, model.nv)
182  , cs_q(ADScalar::sym("q", model.nq))
183  , cs_v(ADScalar::sym("v", model.nv))
184  , cs_tau(ADScalar::sym("tau", model.nv))
185  , cs_v_int(ADScalar::sym("v_inc", model.nv))
186  , cs_ddq(model.nv, 1)
187  , q_ad(model.nq)
188  , q_int_ad(model.nq)
189  , v_ad(model.nv)
190  , v_int_ad(model.nv)
191  , tau_ad(model.nv)
192  , q_vec((size_t)model.nq)
193  , v_vec((size_t)model.nv)
194  , v_int_vec((size_t)model.nv)
195  , tau_vec((size_t)model.nv)
196  {
197  q_ad = Eigen::Map<ADConfigVectorType>(
198  static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);
199  v_int_ad = Eigen::Map<ADConfigVectorType>(
200  static_cast<std::vector<ADScalar>>(cs_v_int).data(), model.nv, 1);
201  v_ad = Eigen::Map<ADTangentVectorType>(
202  static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);
203  tau_ad = Eigen::Map<ADTangentVectorType>(
204  static_cast<std::vector<ADScalar>>(cs_tau).data(), model.nv, 1);
205 
206  Eigen::Map<TangentVectorType>(v_int_vec.data(), model.nv, 1).setZero();
207  }
208 
209  virtual ~AutoDiffABA()
210  {
211  }
212 
213  virtual void buildMap()
214  {
215  // Integrate q + v_int = q_int
216  pinocchio::integrate(ad_model, q_ad, v_int_ad, q_int_ad);
217  // Run ABA with new q_int
218  pinocchio::aba(ad_model, ad_data, q_int_ad, v_ad, tau_ad, Convention::WORLD);
219  // Copy Output
220  pinocchio::casadi::copy(ad_data.ddq, cs_ddq);
221 
222  cs_ddq_dq = jacobian(cs_ddq, cs_v_int);
223  cs_ddq_dv = jacobian(cs_ddq, cs_v);
224  cs_ddq_dtau = jacobian(cs_ddq, cs_tau);
225 
226  ad_fun = ADFun(fun_name, ADSVector{cs_q, cs_v_int, cs_v, cs_tau}, ADSVector{cs_ddq});
227 
228  ad_fun_derivs = ADFun(
229  fun_name + "_derivs", ADSVector{cs_q, cs_v_int, cs_v, cs_tau},
230  ADSVector{cs_ddq_dq, cs_ddq_dv, cs_ddq_dtau});
231  }
232 
233  template<typename ConfigVectorType1, typename TangentVectorType1, typename TangentVectorType2>
234  void evalFunction(
235  const Eigen::MatrixBase<ConfigVectorType1> & q,
236  const Eigen::MatrixBase<TangentVectorType1> & v,
237  const Eigen::MatrixBase<TangentVectorType2> & tau)
238  {
239  Eigen::Map<ConfigVectorType1>(q_vec.data(), ad_model.nq, 1) = q;
240  Eigen::Map<TangentVectorType1>(v_vec.data(), ad_model.nv, 1) = v;
241  Eigen::Map<TangentVectorType2>(tau_vec.data(), ad_model.nv, 1) = tau;
242  fun_output = fun(DMVector{q_vec, v_int_vec, v_vec, tau_vec});
243  ddq = Eigen::Map<TangentVectorType>(
244  static_cast<std::vector<Scalar>>(fun_output[0]).data(), ad_model.nv, 1);
245  }
246 
247  template<typename ConfigVectorType1, typename TangentVectorType1, typename TangentVectorType2>
248  void evalJacobian(
249  const Eigen::MatrixBase<ConfigVectorType1> & q,
250  const Eigen::MatrixBase<TangentVectorType1> & v,
251  const Eigen::MatrixBase<TangentVectorType2> & tau)
252  {
253  Eigen::Map<ConfigVectorType1>(q_vec.data(), ad_model.nq, 1) = q;
254  Eigen::Map<TangentVectorType1>(v_vec.data(), ad_model.nv, 1) = v;
255  Eigen::Map<TangentVectorType2>(tau_vec.data(), ad_model.nv, 1) = tau;
256  fun_output_derivs = fun_derivs(DMVector{q_vec, v_int_vec, v_vec, tau_vec});
257 
258  ddq_dq = Eigen::Map<MatrixXs>(
259  static_cast<std::vector<Scalar>>(fun_output_derivs[0]).data(), ad_model.nv, ad_model.nv);
260  ddq_dv = Eigen::Map<MatrixXs>(
261  static_cast<std::vector<Scalar>>(fun_output_derivs[1]).data(), ad_model.nv, ad_model.nv);
262  ddq_dtau = Eigen::Map<MatrixXs>(
263  static_cast<std::vector<Scalar>>(fun_output_derivs[2]).data(), ad_model.nv, ad_model.nv);
264  }
265 
266  TangentVectorType ddq;
267  RowMatrixXs ddq_dq, ddq_dv, ddq_dtau;
268 
269  protected:
270  using Base::ad_data;
271  using Base::ad_model;
272  using Base::cg_generated;
273  using Base::filename;
274  using Base::fun_name;
275  using Base::libname;
276 
277  using Base::ad_fun;
278  using Base::ad_fun_derivs;
279  using Base::fun;
280  using Base::fun_derivs;
281  using Base::fun_output;
282  using Base::fun_output_derivs;
283 
284  ADScalar cs_q, cs_v, cs_tau, cs_v_int, cs_ddq;
285 
286  // Derivatives
287  ADScalar cs_ddq_dq, cs_ddq_dv, cs_ddq_dtau;
288 
289  ADConfigVectorType q_ad, q_int_ad;
290  ADTangentVectorType v_ad, v_int_ad, tau_ad;
291 
292  std::vector<Scalar> q_vec, v_vec, v_int_vec, tau_vec;
293  };
294 
295  template<typename _Scalar>
296  struct AutoDiffABADerivatives : public AutoDiffAlgoBase<_Scalar>
297  {
298 
299  typedef AutoDiffAlgoBase<_Scalar> Base;
300  typedef typename Base::Scalar Scalar;
301 
302  typedef typename Base::TangentVectorType TangentVectorType;
303  typedef typename Base::RowMatrixXs RowMatrixXs;
304  typedef typename Base::VectorXs VectorXs;
305  typedef typename Base::MatrixXs MatrixXs;
306 
307  typedef typename Base::ADFun ADFun;
308  typedef typename Base::DMVector DMVector;
309  typedef typename Base::DMMatrix DMMatrix;
310  typedef typename Base::ADScalar ADScalar;
311  typedef typename Base::ADSVector ADSVector;
312  typedef typename Base::ADConfigVectorType ADConfigVectorType;
313  typedef typename Base::ADTangentVectorType ADTangentVectorType;
314 
315  explicit AutoDiffABADerivatives(
316  const Model & model,
317  const std::string & filename = "casadi_abaDerivs",
318  const std::string & libname = "libcasadi_cg_abaDerivs",
319  const std::string & fun_name = "eval_f")
320  : Base(model, filename, libname, fun_name)
321  , cs_q(ADScalar::sym("q", model.nq))
322  , cs_v(ADScalar::sym("v", model.nv))
323  , cs_tau(ADScalar::sym("tau", model.nv))
324  , q_ad(model.nq)
325  , v_ad(model.nv)
326  , tau_ad(model.nv)
327  , cs_ddq(model.nv, 1)
328  , q_vec((size_t)model.nq)
329  , v_vec((size_t)model.nv)
330  , tau_vec((size_t)model.nv)
331  , ddq(model.nv)
332  , ddq_dq(model.nv, model.nv)
333  , ddq_dv(model.nv, model.nv)
334  , ddq_dtau(model.nv, model.nv)
335  {
336  q_ad = Eigen::Map<ADConfigVectorType>(
337  static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);
338  v_ad = Eigen::Map<ADTangentVectorType>(
339  static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);
340  tau_ad = Eigen::Map<ADTangentVectorType>(
341  static_cast<std::vector<ADScalar>>(cs_tau).data(), model.nv, 1);
342 
343  Base::build_jacobian = false;
344  }
345 
346  virtual ~AutoDiffABADerivatives()
347  {
348  }
349 
350  virtual void buildMap()
351  {
352  // Run ABA with new q_int
353  pinocchio::computeABADerivatives(ad_model, ad_data, q_ad, v_ad, tau_ad);
354  // Copy Output
355  ad_data.Minv.template triangularView<Eigen::StrictlyLower>() =
356  ad_data.Minv.transpose().template triangularView<Eigen::StrictlyLower>();
357  pinocchio::casadi::copy(ad_data.ddq, cs_ddq);
358  pinocchio::casadi::copy(ad_data.ddq_dq, cs_ddq_dq);
359  pinocchio::casadi::copy(ad_data.ddq_dv, cs_ddq_dv);
360  pinocchio::casadi::copy(ad_data.Minv, cs_ddq_dtau);
361 
362  ad_fun = ADFun(
363  fun_name, ADSVector{cs_q, cs_v, cs_tau},
364  ADSVector{cs_ddq, cs_ddq_dq, cs_ddq_dv, cs_ddq_dtau});
365  }
366 
367  template<typename ConfigVectorType1, typename TangentVectorType1, typename TangentVectorType2>
368  void evalFunction(
369  const Eigen::MatrixBase<ConfigVectorType1> & q,
370  const Eigen::MatrixBase<TangentVectorType1> & v,
371  const Eigen::MatrixBase<TangentVectorType2> & tau)
372  {
373  Eigen::Map<ConfigVectorType1>(q_vec.data(), ad_model.nq, 1) = q;
374  Eigen::Map<TangentVectorType1>(v_vec.data(), ad_model.nv, 1) = v;
375  Eigen::Map<TangentVectorType2>(tau_vec.data(), ad_model.nv, 1) = tau;
376  fun_output = fun(DMVector{q_vec, v_vec, tau_vec});
377 
378  ddq = Eigen::Map<TangentVectorType>(
379  static_cast<std::vector<Scalar>>(fun_output[0]).data(), ad_model.nv, 1);
380  ddq_dq = Eigen::Map<MatrixXs>(
381  static_cast<std::vector<Scalar>>(fun_output[1]).data(), ad_model.nv, ad_model.nv);
382  ddq_dv = Eigen::Map<MatrixXs>(
383  static_cast<std::vector<Scalar>>(fun_output[2]).data(), ad_model.nv, ad_model.nv);
384  ddq_dtau = Eigen::Map<MatrixXs>(
385  static_cast<std::vector<Scalar>>(fun_output[3]).data(), ad_model.nv, ad_model.nv);
386  }
387 
388  TangentVectorType ddq;
389  RowMatrixXs ddq_dq, ddq_dv, ddq_dtau;
390 
391  protected:
392  using Base::ad_data;
393  using Base::ad_model;
394  using Base::cg_generated;
395  using Base::filename;
396  using Base::fun_name;
397  using Base::libname;
398 
399  using Base::ad_fun;
400  using Base::ad_fun_derivs;
401  using Base::fun;
402  using Base::fun_derivs;
403  using Base::fun_output;
404  using Base::fun_output_derivs;
405 
406  ADScalar cs_q, cs_v, cs_tau, cs_ddq;
407  // Derivatives
408  ADScalar cs_ddq_dq, cs_ddq_dv, cs_ddq_dtau;
409 
410  ADConfigVectorType q_ad;
411  ADTangentVectorType v_ad, tau_ad;
412  std::vector<Scalar> q_vec, v_vec, tau_vec;
413  };
414 
415  template<typename _Scalar>
416  struct AutoDiffConstraintDynamics : public AutoDiffAlgoBase<_Scalar>
417  {
418  typedef AutoDiffAlgoBase<_Scalar> Base;
419  typedef typename Base::Scalar Scalar;
420  typedef typename Base::ADScalar ADScalar;
421  typedef typename Base::ADSVector ADSVector;
422 
423  typedef typename Base::TangentVectorType TangentVectorType;
424  typedef typename Base::RowMatrixXs RowMatrixXs;
425  typedef typename Base::VectorXs VectorXs;
426  typedef typename Base::MatrixXs MatrixXs;
427 
428  typedef typename Base::ADFun ADFun;
429  typedef typename Base::DMVector DMVector;
430  typedef typename Base::DMMatrix DMMatrix;
431  typedef typename Base::ADConfigVectorType ADConfigVectorType;
432  typedef typename Base::ADTangentVectorType ADTangentVectorType;
433 
434  typedef typename pinocchio::RigidConstraintModelTpl<Scalar, Base::Options> ConstraintModel;
435  typedef Eigen::aligned_allocator<ConstraintModel> ConstraintModelAllocator;
436  typedef typename std::vector<ConstraintModel, ConstraintModelAllocator> ConstraintModelVector;
437  typedef typename pinocchio::RigidConstraintDataTpl<Scalar, Base::Options> ConstraintData;
438  typedef Eigen::aligned_allocator<ConstraintData> ConstraintDataAllocator;
439  typedef typename std::vector<ConstraintData, ConstraintDataAllocator> ConstraintDataVector;
440 
441  typedef typename pinocchio::RigidConstraintModelTpl<ADScalar, Base::Options>
442  ADConstraintModel;
443  typedef Eigen::aligned_allocator<ADConstraintModel> ADConstraintModelAllocator;
444  typedef typename std::vector<ADConstraintModel, ADConstraintModelAllocator>
445  ADConstraintModelVector;
446  typedef typename pinocchio::RigidConstraintDataTpl<ADScalar, Base::Options> ADConstraintData;
447  typedef Eigen::aligned_allocator<ADConstraintData> ADConstraintDataAllocator;
448  typedef typename std::vector<ADConstraintData, ADConstraintDataAllocator>
449  ADConstraintDataVector;
450 
451  static Eigen::DenseIndex constraintDim(const ConstraintModelVector & contact_models)
452  {
453  Eigen::DenseIndex num_total_constraints = 0;
454  for (typename ConstraintModelVector::const_iterator it = contact_models.begin();
455  it != contact_models.end(); ++it)
456  {
457  PINOCCHIO_CHECK_INPUT_ARGUMENT(
458  it->size() > 0, "The dimension of the constraint must be positive");
459  num_total_constraints += it->size();
460  }
461  return num_total_constraints;
462  }
463 
464  explicit AutoDiffConstraintDynamics(
465  const Model & model,
466  const ConstraintModelVector & contact_models,
467  const std::string & filename = "casadi_contactDyn",
468  const std::string & libname = "libcasadi_cg_contactDyn",
469  const std::string & fun_name = "eval_f")
470  : Base(model, filename, libname, fun_name)
471  , nc(constraintDim(contact_models))
472  , cs_q(ADScalar::sym("q", model.nq))
473  , cs_v(ADScalar::sym("v", model.nv))
474  , cs_tau(ADScalar::sym("tau", model.nv))
475  , cs_v_int(ADScalar::sym("v_inc", model.nv))
476  , q_ad(model.nq)
477  , q_int_ad(model.nq)
478  , v_ad(model.nv)
479  , v_int_ad(model.nv)
480  , tau_ad(model.nv)
481  , cs_ddq(model.nv, 1)
482  , cs_lambda_c(model.nv, 1)
483  , q_vec((size_t)model.nq)
484  , v_vec((size_t)model.nv)
485  , v_int_vec((size_t)model.nv)
486  , tau_vec((size_t)model.nv)
487  , ddq(model.nv)
488  , ddq_dq(model.nv, model.nv)
489  , ddq_dv(model.nv, model.nv)
490  , ddq_dtau(model.nv, model.nv)
491  {
492  lambda_c.resize(nc);
493  lambda_c.setZero();
494  dlambda_dq.resize(nc, model.nv);
495  dlambda_dq.setZero();
496  dlambda_dv.resize(nc, model.nv);
497  dlambda_dv.setZero();
498  dlambda_dtau.resize(nc, model.nv);
499  dlambda_dtau.setZero();
500  q_ad = Eigen::Map<ADConfigVectorType>(
501  static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);
502  v_int_ad = Eigen::Map<ADConfigVectorType>(
503  static_cast<std::vector<ADScalar>>(cs_v_int).data(), model.nv, 1);
504  v_ad = Eigen::Map<ADTangentVectorType>(
505  static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);
506  tau_ad = Eigen::Map<ADTangentVectorType>(
507  static_cast<std::vector<ADScalar>>(cs_tau).data(), model.nv, 1);
508 
509  Eigen::Map<TangentVectorType>(v_int_vec.data(), model.nv, 1).setZero();
510 
511  for (int k = 0; k < contact_models.size(); ++k)
512  {
513  ad_contact_models.push_back(contact_models[k].template cast<ADScalar>());
514  ad_contact_datas.push_back(ADConstraintData(ad_contact_models[k]));
515  }
516  }
517 
518  virtual ~AutoDiffConstraintDynamics()
519  {
520  }
521 
522  virtual void buildMap()
523  {
524  pinocchio::initConstraintDynamics(ad_model, ad_data, ad_contact_models);
525  // Integrate q + v_int = q_int
526  pinocchio::integrate(ad_model, q_ad, v_int_ad, q_int_ad);
527  // Run contact dynamics with new q_int
528  pinocchio::constraintDynamics(
529  ad_model, ad_data, q_int_ad, v_ad, tau_ad, ad_contact_models, ad_contact_datas);
530  // Copy Output
531  pinocchio::casadi::copy(ad_data.ddq, cs_ddq);
532  pinocchio::casadi::copy(ad_data.lambda_c, cs_lambda_c);
533 
534  cs_ddq_dq = jacobian(cs_ddq, cs_v_int);
535  cs_ddq_dv = jacobian(cs_ddq, cs_v);
536  cs_ddq_dtau = jacobian(cs_ddq, cs_tau);
537 
538  cs_lambda_dq = jacobian(cs_lambda_c, cs_v_int);
539  cs_lambda_dv = jacobian(cs_lambda_c, cs_v);
540  cs_lambda_dtau = jacobian(cs_lambda_c, cs_tau);
541 
542  ad_fun =
543  ADFun(fun_name, ADSVector{cs_q, cs_v_int, cs_v, cs_tau}, ADSVector{cs_ddq, cs_lambda_c});
544  ad_fun_derivs = ADFun(
545  fun_name + "_derivs", ADSVector{cs_q, cs_v_int, cs_v, cs_tau},
546  ADSVector{cs_ddq_dq, cs_ddq_dv, cs_ddq_dtau, cs_lambda_dq, cs_lambda_dv, cs_lambda_dtau});
547  }
548 
549  template<typename ConfigVectorType1, typename TangentVectorType1, typename TangentVectorType2>
550  void evalFunction(
551  const Eigen::MatrixBase<ConfigVectorType1> & q,
552  const Eigen::MatrixBase<TangentVectorType1> & v,
553  const Eigen::MatrixBase<TangentVectorType2> & tau)
554  {
555  Eigen::Map<ConfigVectorType1>(q_vec.data(), ad_model.nq, 1) = q;
556  Eigen::Map<TangentVectorType1>(v_vec.data(), ad_model.nv, 1) = v;
557  Eigen::Map<TangentVectorType2>(tau_vec.data(), ad_model.nv, 1) = tau;
558  fun_output = fun(DMVector{q_vec, v_int_vec, v_vec, tau_vec});
559  ddq = Eigen::Map<TangentVectorType>(
560  static_cast<std::vector<Scalar>>(fun_output[0]).data(), ad_model.nv, 1);
561  lambda_c = Eigen::Map<TangentVectorType>(
562  static_cast<std::vector<Scalar>>(fun_output[1]).data(), nc, 1);
563  }
564 
565  template<typename ConfigVectorType1, typename TangentVectorType1, typename TangentVectorType2>
566  void evalJacobian(
567  const Eigen::MatrixBase<ConfigVectorType1> & q,
568  const Eigen::MatrixBase<TangentVectorType1> & v,
569  const Eigen::MatrixBase<TangentVectorType2> & tau)
570  {
571  Eigen::Map<ConfigVectorType1>(q_vec.data(), ad_model.nq, 1) = q;
572  Eigen::Map<TangentVectorType1>(v_vec.data(), ad_model.nv, 1) = v;
573  Eigen::Map<TangentVectorType2>(tau_vec.data(), ad_model.nv, 1) = tau;
574  fun_output_derivs = fun_derivs(DMVector{q_vec, v_int_vec, v_vec, tau_vec});
575  ddq_dq = Eigen::Map<MatrixXs>(
576  static_cast<std::vector<Scalar>>(fun_output_derivs[0]).data(), ad_model.nv, ad_model.nv);
577  ddq_dv = Eigen::Map<MatrixXs>(
578  static_cast<std::vector<Scalar>>(fun_output_derivs[1]).data(), ad_model.nv, ad_model.nv);
579  ddq_dtau = Eigen::Map<MatrixXs>(
580  static_cast<std::vector<Scalar>>(fun_output_derivs[2]).data(), ad_model.nv, ad_model.nv);
581  dlambda_dq = Eigen::Map<MatrixXs>(
582  static_cast<std::vector<Scalar>>(fun_output_derivs[3]).data(), nc, ad_model.nv);
583  dlambda_dv = Eigen::Map<MatrixXs>(
584  static_cast<std::vector<Scalar>>(fun_output_derivs[4]).data(), nc, ad_model.nv);
585  dlambda_dtau = Eigen::Map<MatrixXs>(
586  static_cast<std::vector<Scalar>>(fun_output_derivs[5]).data(), nc, ad_model.nv);
587  }
588 
589  TangentVectorType ddq;
590  VectorXs lambda_c;
591  RowMatrixXs ddq_dq, ddq_dv, ddq_dtau, dlambda_dq, dlambda_dv, dlambda_dtau;
592 
593  protected:
594  using Base::ad_data;
595  using Base::ad_model;
596  using Base::cg_generated;
597  using Base::filename;
598  using Base::fun_name;
599  using Base::libname;
600 
601  using Base::ad_fun;
602  using Base::ad_fun_derivs;
603  using Base::fun;
604  using Base::fun_derivs;
605  using Base::fun_output;
606  using Base::fun_output_derivs;
607 
608  ADConstraintModelVector ad_contact_models;
609  ADConstraintDataVector ad_contact_datas;
610 
611  Eigen::DenseIndex nc;
612  ADScalar cs_q, cs_v, cs_tau, cs_v_int, cs_ddq, cs_lambda_c;
613 
614  // Derivatives
615  ADScalar cs_ddq_dq, cs_ddq_dv, cs_ddq_dtau, cs_lambda_dq, cs_lambda_dv, cs_lambda_dtau;
616 
617  ADConfigVectorType q_ad, q_int_ad;
618  ADTangentVectorType v_ad, v_int_ad, tau_ad;
619 
620  std::vector<Scalar> q_vec, v_vec, v_int_vec, tau_vec;
621  };
622 
623  template<typename _Scalar>
624  struct AutoDiffConstraintDynamicsDerivatives : public AutoDiffAlgoBase<_Scalar>
625  {
626  typedef AutoDiffAlgoBase<_Scalar> Base;
627  typedef typename Base::Scalar Scalar;
628  typedef typename Base::ADScalar ADScalar;
629  typedef typename Base::ADSVector ADSVector;
630 
631  typedef typename Base::TangentVectorType TangentVectorType;
632  typedef typename Base::RowMatrixXs RowMatrixXs;
633  typedef typename Base::VectorXs VectorXs;
634  typedef typename Base::MatrixXs MatrixXs;
635 
636  typedef typename Base::ADFun ADFun;
637  typedef typename Base::DMVector DMVector;
638  typedef typename Base::DMMatrix DMMatrix;
639  typedef typename Base::ADConfigVectorType ADConfigVectorType;
640  typedef typename Base::ADTangentVectorType ADTangentVectorType;
641 
642  typedef typename pinocchio::RigidConstraintModelTpl<Scalar, Base::Options> ConstraintModel;
643  typedef Eigen::aligned_allocator<ConstraintModel> ConstraintModelAllocator;
644  typedef typename std::vector<ConstraintModel, ConstraintModelAllocator> ConstraintModelVector;
645  typedef typename pinocchio::RigidConstraintDataTpl<Scalar, Base::Options> ConstraintData;
646  typedef Eigen::aligned_allocator<ConstraintData> ConstraintDataAllocator;
647  typedef typename std::vector<ConstraintData, ConstraintDataAllocator> ConstraintDataVector;
648 
649  typedef typename pinocchio::RigidConstraintModelTpl<ADScalar, Base::Options>
650  ADConstraintModel;
651  typedef Eigen::aligned_allocator<ADConstraintModel> ADConstraintModelAllocator;
652  typedef typename std::vector<ADConstraintModel, ADConstraintModelAllocator>
653  ADConstraintModelVector;
654  typedef typename pinocchio::RigidConstraintDataTpl<ADScalar, Base::Options> ADConstraintData;
655  typedef Eigen::aligned_allocator<ADConstraintData> ADConstraintDataAllocator;
656  typedef typename std::vector<ADConstraintData, ADConstraintDataAllocator>
657  ADConstraintDataVector;
658 
659  static Eigen::DenseIndex constraintDim(const ConstraintModelVector & contact_models)
660  {
661  Eigen::DenseIndex num_total_constraints = 0;
662  for (typename ConstraintModelVector::const_iterator it = contact_models.begin();
663  it != contact_models.end(); ++it)
664  {
665  PINOCCHIO_CHECK_INPUT_ARGUMENT(
666  it->size() > 0, "The dimension of the constraint must be positive");
667  num_total_constraints += it->size();
668  }
669  return num_total_constraints;
670  }
671 
672  explicit AutoDiffConstraintDynamicsDerivatives(
673  const Model & model,
674  const ConstraintModelVector & contact_models,
675  const std::string & filename = "casadi_contactDynDerivs",
676  const std::string & libname = "libcasadi_cg_contactDynDerivs",
677  const std::string & fun_name = "eval_f")
678  : Base(model, filename, libname, fun_name)
679  , nc(constraintDim(contact_models))
680  , cs_q(ADScalar::sym("q", model.nq))
681  , cs_v(ADScalar::sym("v", model.nv))
682  , cs_tau(ADScalar::sym("tau", model.nv))
683  , q_ad(model.nq)
684  , v_ad(model.nv)
685  , tau_ad(model.nv)
686  , cs_ddq(model.nv, 1)
687  , cs_lambda_c(model.nv, 1)
688  , q_vec((size_t)model.nq)
689  , v_vec((size_t)model.nv)
690  , tau_vec((size_t)model.nv)
691  , ddq(model.nv)
692  , ddq_dq(model.nv, model.nv)
693  , ddq_dv(model.nv, model.nv)
694  , ddq_dtau(model.nv, model.nv)
695  {
696  lambda_c.resize(nc);
697  lambda_c.setZero();
698  dlambda_dq.resize(nc, model.nv);
699  dlambda_dq.setZero();
700  dlambda_dv.resize(nc, model.nv);
701  dlambda_dv.setZero();
702  dlambda_dtau.resize(nc, model.nv);
703  dlambda_dtau.setZero();
704 
705  q_ad = Eigen::Map<ADConfigVectorType>(
706  static_cast<std::vector<ADScalar>>(cs_q).data(), model.nq, 1);
707  v_ad = Eigen::Map<ADTangentVectorType>(
708  static_cast<std::vector<ADScalar>>(cs_v).data(), model.nv, 1);
709  tau_ad = Eigen::Map<ADTangentVectorType>(
710  static_cast<std::vector<ADScalar>>(cs_tau).data(), model.nv, 1);
711 
712  for (int k = 0; k < contact_models.size(); ++k)
713  {
714  ad_contact_models.push_back(contact_models[k].template cast<ADScalar>());
715  ad_contact_datas.push_back(ADConstraintData(ad_contact_models[k]));
716  }
717 
718  Base::build_jacobian = false;
719  }
720 
721  virtual ~AutoDiffConstraintDynamicsDerivatives()
722  {
723  }
724 
725  virtual void buildMap()
726  {
727  pinocchio::initConstraintDynamics(ad_model, ad_data, ad_contact_models);
728  pinocchio::constraintDynamics(
729  ad_model, ad_data, q_ad, v_ad, tau_ad, ad_contact_models, ad_contact_datas);
730  pinocchio::computeConstraintDynamicsDerivatives(
731  ad_model, ad_data, ad_contact_models, ad_contact_datas);
732  // Copy Output
733  pinocchio::casadi::copy(ad_data.ddq, cs_ddq);
734  pinocchio::casadi::copy(ad_data.lambda_c, cs_lambda_c);
735  pinocchio::casadi::copy(ad_data.ddq_dq, cs_ddq_dq);
736  pinocchio::casadi::copy(ad_data.ddq_dv, cs_ddq_dv);
737  pinocchio::casadi::copy(ad_data.ddq_dtau, cs_ddq_dtau);
738  pinocchio::casadi::copy(ad_data.dlambda_dq, cs_lambda_dq);
739  pinocchio::casadi::copy(ad_data.dlambda_dv, cs_lambda_dv);
740  pinocchio::casadi::copy(ad_data.dlambda_dtau, cs_lambda_dtau);
741 
742  ad_fun = ADFun(
743  fun_name, ADSVector{cs_q, cs_v, cs_tau},
744  ADSVector{
745  cs_ddq, cs_lambda_c, cs_ddq_dq, cs_ddq_dv, cs_ddq_dtau, cs_lambda_dq, cs_lambda_dv,
746  cs_lambda_dtau});
747  }
748 
749  template<typename ConfigVectorType1, typename TangentVectorType1, typename TangentVectorType2>
750  void evalFunction(
751  const Eigen::MatrixBase<ConfigVectorType1> & q,
752  const Eigen::MatrixBase<TangentVectorType1> & v,
753  const Eigen::MatrixBase<TangentVectorType2> & tau)
754  {
755  Eigen::Map<ConfigVectorType1>(q_vec.data(), ad_model.nq, 1) = q;
756  Eigen::Map<TangentVectorType1>(v_vec.data(), ad_model.nv, 1) = v;
757  Eigen::Map<TangentVectorType2>(tau_vec.data(), ad_model.nv, 1) = tau;
758  fun_output = fun(DMVector{q_vec, v_vec, tau_vec});
759  ddq = Eigen::Map<TangentVectorType>(
760  static_cast<std::vector<Scalar>>(fun_output[0]).data(), ad_model.nv, 1);
761  lambda_c = Eigen::Map<TangentVectorType>(
762  static_cast<std::vector<Scalar>>(fun_output[1]).data(), nc, 1);
763  ddq_dq = Eigen::Map<MatrixXs>(
764  static_cast<std::vector<Scalar>>(fun_output[2]).data(), ad_model.nv, ad_model.nv);
765  ddq_dv = Eigen::Map<MatrixXs>(
766  static_cast<std::vector<Scalar>>(fun_output[3]).data(), ad_model.nv, ad_model.nv);
767  ddq_dtau = Eigen::Map<MatrixXs>(
768  static_cast<std::vector<Scalar>>(fun_output[4]).data(), ad_model.nv, ad_model.nv);
769  dlambda_dq = Eigen::Map<MatrixXs>(
770  static_cast<std::vector<Scalar>>(fun_output[5]).data(), nc, ad_model.nv);
771  dlambda_dv = Eigen::Map<MatrixXs>(
772  static_cast<std::vector<Scalar>>(fun_output[6]).data(), nc, ad_model.nv);
773  dlambda_dtau = Eigen::Map<MatrixXs>(
774  static_cast<std::vector<Scalar>>(fun_output[7]).data(), nc, ad_model.nv);
775  }
776 
777  TangentVectorType ddq, lambda_c;
778  RowMatrixXs ddq_dq, ddq_dv, ddq_dtau, dlambda_dq, dlambda_dv, dlambda_dtau;
779 
780  protected:
781  using Base::ad_data;
782  using Base::ad_model;
783  using Base::cg_generated;
784  using Base::filename;
785  using Base::fun_name;
786  using Base::libname;
787 
788  using Base::ad_fun;
789  using Base::ad_fun_derivs;
790  using Base::fun;
791  using Base::fun_derivs;
792  using Base::fun_output;
793  using Base::fun_output_derivs;
794  ADConstraintModelVector ad_contact_models;
795  ADConstraintDataVector ad_contact_datas;
796 
797  Eigen::DenseIndex nc;
798  ADScalar cs_q, cs_v, cs_tau, cs_ddq, cs_lambda_c;
799 
800  // Derivatives
801  ADScalar cs_ddq_dq, cs_ddq_dv, cs_ddq_dtau, cs_lambda_dq, cs_lambda_dv, cs_lambda_dtau;
802 
803  ADConfigVectorType q_ad;
804  ADTangentVectorType v_ad, tau_ad;
805 
806  std::vector<Scalar> q_vec, v_vec, tau_vec;
807  };
808 
809  } // namespace casadi
810 
811 } // namespace pinocchio
812 
813 #endif // ifndef __pinocchio_autodiff_code_generator_algo_hpp__
pinocchio::Convention::WORLD
@ WORLD
pinocchio
Main pinocchio namespace.
Definition: treeview.dox:11
pinocchio::aba
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & aba(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const Convention convention=Convention::LOCAL)
The Articulated-Body algorithm. It computes the forward dynamics, aka the joint accelerations given t...
pinocchio::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::constraintDynamics
const DataTpl< Scalar, Options, JointCollectionTpl >::TangentVectorType & constraintDynamics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType1 > &v, const Eigen::MatrixBase< TangentVectorType2 > &tau, const std::vector< RigidConstraintModelTpl< Scalar, Options >, ConstraintModelAllocator > &contact_models, std::vector< RigidConstraintDataTpl< Scalar, Options >, ConstraintDataAllocator > &contact_datas, ProximalSettingsTpl< Scalar > &settings)
Computes the forward dynamics with contact constraints according to a given list of contact informati...
pinocchio::integrate
void integrate(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, const Eigen::MatrixBase< ConfigVectorType > &q, const Eigen::MatrixBase< TangentVectorType > &v, const Eigen::MatrixBase< ReturnType > &qout)
Integrate a configuration vector for the specified model for a tangent vector during one unit time.
Definition: joint-configuration.hpp:70
pinocchio::initConstraintDynamics
void initConstraintDynamics(const ModelTpl< Scalar, Options, JointCollectionTpl > &model, DataTpl< Scalar, Options, JointCollectionTpl > &data, const std::vector< RigidConstraintModelTpl< Scalar, Options >, Allocator > &contact_models)
Init the forward dynamics data according to the contact information contained in contact_models.
pinocchio::DataTpl::lambda_c
VectorXs lambda_c
Lagrange Multipliers corresponding to the contact forces in pinocchio::forwardDynamics.
Definition: data.hpp:485
pinocchio::DataTpl::dlambda_dq
MatrixXs dlambda_dq
Partial derivatives of the constraints forces with respect to the joint configuration,...
Definition: data.hpp:430
pinocchio::DataTpl::ddq_dtau
RowMatrixXs ddq_dtau
Partial derivative of the joint acceleration vector with respect to the joint torques.
Definition: data.hpp:416
pinocchio::DataTpl::ddq_dv
RowMatrixXs ddq_dv
Partial derivative of the joint acceleration vector with respect to the joint velocity.
Definition: data.hpp:412
pinocchio::DataTpl::ddq
TangentVectorType ddq
The joint accelerations computed from ABA.
Definition: data.hpp:256
pinocchio::DataTpl::Minv
RowMatrixXs Minv
The inverse of the joint space inertia matrix (a square matrix of dim model.nv).
Definition: data.hpp:202
pinocchio::DataTpl::ddq_dq
RowMatrixXs ddq_dq
Partial derivative of the joint acceleration vector with respect to the joint configuration.
Definition: data.hpp:408
pinocchio::ModelTpl::TangentVectorType
VectorXs TangentVectorType
Dense vectorized version of a joint tangent vector (e.g. velocity, acceleration, etc)....
Definition: model.hpp:95
pinocchio::ModelTpl::nq
int nq
Dimension of the configuration vector representation.
Definition: model.hpp:98
pinocchio::ModelTpl::nv
int nv
Dimension of the velocity vector space.
Definition: model.hpp:101
pinocchio::ModelTpl::ConfigVectorType
VectorXs ConfigVectorType
Dense vectorized version of a joint configuration vector.
Definition: model.hpp:87
pinocchio::casadi::AutoDiffABADerivatives::buildMap
virtual void buildMap()
build the mapping Y = f(X)
Definition: casadi-algo.hpp:350
pinocchio::casadi::AutoDiffABA::buildMap
virtual void buildMap()
build the mapping Y = f(X)
Definition: casadi-algo.hpp:213
pinocchio::casadi::AutoDiffAlgoBase::buildMap
virtual void buildMap()=0
build the mapping Y = f(X)
pinocchio::casadi::AutoDiffAlgoBase::build_jacobian
bool build_jacobian
Options to build or not the Jacobian of he function.
Definition: casadi-algo.hpp:145
pinocchio::casadi::AutoDiffAlgoBase::build_forward
bool build_forward
Options to generate or not the source code for the evaluation function.
Definition: casadi-algo.hpp:143
pinocchio::casadi::AutoDiffConstraintDynamicsDerivatives::buildMap
virtual void buildMap()
build the mapping Y = f(X)
Definition: casadi-algo.hpp:725
pinocchio::casadi::AutoDiffConstraintDynamics::buildMap
virtual void buildMap()
build the mapping Y = f(X)
Definition: casadi-algo.hpp:522