pinocchio  3.7.0
A fast and flexible implementation of Rigid Body Dynamics algorithms and their analytical derivatives
 
Loading...
Searching...
No Matches
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
18namespace 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::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::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.
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::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