Crocoddyl
 
Loading...
Searching...
No Matches
action.hpp
1
3// BSD 3-Clause License
4//
5// Copyright (C) 2019-2025, LAAS-CNRS, INRIA, University of Edinburgh,
6// Heriot-Watt University
7// Copyright note valid unless otherwise stated in individual files.
8// All rights reserved.
10
11#ifndef CROCODDYL_CORE_CODEGEN_ACTION_HPP_
12#define CROCODDYL_CORE_CODEGEN_ACTION_HPP_
13
14#ifdef CROCODDYL_WITH_CODEGEN
15
16#include "crocoddyl/core/action-base.hpp"
17#include "crocoddyl/core/utils/stop-watch.hpp"
18
19namespace crocoddyl {
20
21template <typename Scalar>
22std::unique_ptr<CppAD::ADFun<CppAD::cg::CG<Scalar>>> clone_adfun(
23 const CppAD::ADFun<CppAD::cg::CG<Scalar>>& original) {
24 auto cloned = std::make_unique<CppAD::ADFun<CppAD::cg::CG<Scalar>>>();
25 *cloned = original; // Use assignment operator to copy the function
26 return cloned;
27}
28
29template <typename FromScalar, typename ToScalar>
30std::function<
31 void(std::shared_ptr<ActionModelAbstractTpl<ToScalar>>,
32 const Eigen::Ref<const typename MathBaseTpl<ToScalar>::VectorXs>&)>
33cast_function(
34 const std::function<void(
35 std::shared_ptr<ActionModelAbstractTpl<FromScalar>>,
36 const Eigen::Ref<const typename MathBaseTpl<FromScalar>::VectorXs>&)>&
37 fn) {
38 return [fn](std::shared_ptr<ActionModelAbstractTpl<ToScalar>> to_base,
39 const Eigen::Ref<const typename MathBaseTpl<ToScalar>::VectorXs>&
40 to_vector) {
41 // Convert arguments
42 const std::shared_ptr<ActionModelAbstractTpl<FromScalar>>& from_base =
43 to_base->template cast<FromScalar>();
44 const typename MathBaseTpl<FromScalar>::VectorXs from_vector =
45 to_vector.template cast<FromScalar>();
46 // Call the original function with converted arguments
47 fn(from_base, from_vector);
48 };
49}
50
51enum CompilerType { GCC = 0, CLANG };
52
53template <typename Scalar>
54struct ActionDataCodeGenTpl;
55
56template <typename _Scalar>
57class ActionModelCodeGenTpl : public ActionModelAbstractTpl<_Scalar> {
58 public:
59 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
60 CROCODDYL_DERIVED_FLOATINGPOINT_CAST(ActionModelBase, ActionModelCodeGenTpl)
61
62 typedef _Scalar Scalar;
63 typedef MathBaseTpl<Scalar> MathBase;
64 typedef ActionModelAbstractTpl<Scalar> Base;
65 typedef ActionDataCodeGenTpl<Scalar> Data;
66 typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
67 typedef typename MathBase::VectorXs VectorXs;
68 typedef typename MathBase::MatrixXs MatrixXs;
69
70 typedef CppAD::cg::CG<Scalar> CGScalar;
71 typedef CppAD::AD<CGScalar> ADScalar;
72 typedef MathBaseTpl<ADScalar> ADMathBase;
73 typedef ActionModelAbstractTpl<ADScalar> ADBase;
74 typedef ActionDataAbstractTpl<ADScalar> ADActionDataAbstract;
75 typedef typename ADMathBase::VectorXs ADVectorXs;
76 typedef typename ADMathBase::MatrixXs ADMatrixXs;
77 typedef CppAD::ADFun<CGScalar> ADFun;
78 typedef CppAD::cg::ModelCSourceGen<Scalar> CSourceGen;
79 typedef CppAD::cg::ModelLibraryCSourceGen<Scalar> LibraryCSourceGen;
80 typedef CppAD::cg::DynamicModelLibraryProcessor<Scalar> LibraryProcessor;
81 typedef CppAD::cg::DynamicLib<Scalar> DynamicLib;
82 typedef CppAD::cg::GenericModel<Scalar> GenericModel;
83 typedef CppAD::cg::LinuxDynamicLib<Scalar> LinuxDynamicLib;
84 typedef CppAD::cg::system::SystemInfo<> SystemInfo;
85 typedef std::function<void(std::shared_ptr<ADBase>,
86 const Eigen::Ref<const ADVectorXs>&)>
87 ParamsEnvironment;
88
104 ActionModelCodeGenTpl(
105 std::shared_ptr<Base> model, const std::string& lib_fname,
106 bool autodiff = false, const std::size_t np = 0,
107 ParamsEnvironment updateParams = EmptyParamsEnv,
108 CompilerType compiler = CLANG,
109 const std::string& compile_options = "-Ofast -march=native");
110
126 ActionModelCodeGenTpl(
127 std::shared_ptr<ADBase> ad_model, const std::string& lib_fname,
128 bool autodiff = false, const std::size_t np = 0,
129 ParamsEnvironment updateParams = EmptyParamsEnv,
130 CompilerType compiler = CLANG,
131 const std::string& compile_options = "-Ofast -march=native");
132
140 ActionModelCodeGenTpl(const std::string& lib_fname,
141 std::shared_ptr<Base> model);
142
150 ActionModelCodeGenTpl(const std::string& lib_fname,
151 std::shared_ptr<ADBase> ad_model);
152
157 ActionModelCodeGenTpl(const ActionModelCodeGenTpl<Scalar>& other);
158
159 virtual ~ActionModelCodeGenTpl() = default;
160
164 void initLib();
165
169 void compileLib();
170
177 bool existLib(const std::string& lib_fname) const;
178
184 void loadLib(const std::string& lib_fname);
185
192 void update_p(const std::shared_ptr<ActionDataAbstract>& data,
193 const Eigen::Ref<const VectorXs>& p) const;
194
202 virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
203 const Eigen::Ref<const VectorXs>& x,
204 const Eigen::Ref<const VectorXs>& u) override;
205
210 virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
211 const Eigen::Ref<const VectorXs>& x) override;
212
226 virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
227 const Eigen::Ref<const VectorXs>& x,
228 const Eigen::Ref<const VectorXs>& u) override;
229
234 virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
235 const Eigen::Ref<const VectorXs>& x) override;
236
242 virtual std::shared_ptr<ActionDataAbstract> createData() override;
243
247 virtual bool checkData(
248 const std::shared_ptr<ActionDataAbstract>& data) override;
249
263 virtual void quasiStatic(const std::shared_ptr<ActionDataAbstract>& data,
264 Eigen::Ref<VectorXs> u,
265 const Eigen::Ref<const VectorXs>& x,
266 const std::size_t /*maxiter = 100*/,
267 const Scalar /*tol*/) override;
268
278 template <typename NewScalar>
279 ActionModelCodeGenTpl<NewScalar> cast() const;
280
284 const std::shared_ptr<Base>& get_model() const;
285
286 std::size_t get_np() const;
287
291 virtual std::size_t get_ng() const override;
292
296 virtual std::size_t get_nh() const override;
297
301 virtual std::size_t get_ng_T() const override;
302
306 virtual std::size_t get_nh_T() const override;
307
311 virtual const VectorXs& get_g_lb() const override;
312
316 virtual const VectorXs& get_g_ub() const override;
317
322 std::size_t get_nX() const;
323
328 std::size_t get_nX_T() const;
329
334 std::size_t get_nX3() const;
335
340 std::size_t get_nY1() const;
341
346 std::size_t get_nY1_T() const;
347
352 std::size_t get_nY2() const;
353
358 std::size_t get_nY2_T() const;
359
364 std::size_t get_nY3() const;
365
371 virtual void print(std::ostream& os) const override;
372
373 protected:
374 ActionModelCodeGenTpl();
375
376 using Base::ng_;
377 using Base::ng_T_;
378 using Base::nh_;
379 using Base::nh_T_;
380 using Base::nu_;
381 using Base::state_;
382
383 std::shared_ptr<Base> model_;
384 std::shared_ptr<ADBase>
385 ad_model_;
386 std::shared_ptr<ADActionDataAbstract>
387 ad_data_;
388 bool autodiff_;
390
391 std::size_t np_;
393 std::size_t nX_;
395 std::size_t nX_T_;
397 std::size_t nX3_;
399 std::size_t
400 nY1_;
401 std::size_t nY1_T_;
403 std::size_t nY2_;
405 std::size_t nY2_T_;
407 std::size_t nY3_;
409 ADVectorXs ad_X_;
411 ADVectorXs ad_X_T_;
413 ADVectorXs ad_X3_;
415 ADVectorXs ad_Y1_;
416 ADVectorXs ad_Y1_T_;
418 ADVectorXs
419 ad_Y2_;
420 ADVectorXs ad_Y2_T_;
422 ADVectorXs
423 ad_Y3_;
424
425 const std::string Y1fun_name_;
426 const std::string
427 Y1Tfun_name_;
428 const std::string Y2fun_name_;
429 const std::string
430 Y2Tfun_name_;
431 const std::string Y3fun_name_;
432 const std::string lib_fname_;
433 CompilerType compiler_type_;
434 const std::string compile_options_;
435
436 ParamsEnvironment updateParams_; // Lambda function that updates parameter
437 // variables before starting record.
438
439 std::unique_ptr<ADFun> ad_calc_;
440 std::unique_ptr<ADFun>
441 ad_calc_T_;
442 std::unique_ptr<ADFun>
443 ad_calcDiff_;
444 std::unique_ptr<ADFun> ad_calcDiff_T_;
446 std::unique_ptr<ADFun>
447 ad_quasiStatic_;
448 std::unique_ptr<CSourceGen>
449 calcCG_;
450 std::unique_ptr<CSourceGen>
451 calcCG_T_;
453 std::unique_ptr<CSourceGen>
454 calcDiffCG_;
455 std::unique_ptr<CSourceGen>
456 calcDiffCG_T_;
458 std::unique_ptr<CSourceGen> quasiStaticCG_;
460 std::unique_ptr<LibraryCSourceGen>
461 libCG_;
462 std::unique_ptr<LibraryProcessor>
463 dynLibManager_;
464 std::unique_ptr<DynamicLib> dynLib_;
465 std::unique_ptr<GenericModel> calcFun_;
466 std::unique_ptr<GenericModel>
467 calcFun_T_;
468 std::unique_ptr<GenericModel>
469 calcDiffFun_;
470 std::unique_ptr<GenericModel>
471 calcDiffFun_T_;
472 std::unique_ptr<GenericModel>
473 quasiStaticFun_;
474
475 private:
476 void recordCalc();
477 void recordCalc_T();
478 void recordCalcDiff();
479 void recordCalcDiff_T();
480 void recordQuasiStatic();
481
482 void tapeCalcOutput();
483 void tapeCalcOutput_T();
484 void tapeCalcDiffOutput();
485 void tapeCalcDiffOutput_T();
486
487 VectorXs wCostHess_;
488 VectorXs wCostHess_T_;
489
490 static void EmptyParamsEnv(std::shared_ptr<ADBase>,
491 const Eigen::Ref<const ADVectorXs>&);
492};
493
494template <typename _Scalar>
495struct ActionDataCodeGenTpl : public ActionDataAbstractTpl<_Scalar> {
496 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
497
498 typedef _Scalar Scalar;
499 typedef MathBaseTpl<Scalar> MathBase;
500 typedef ActionDataAbstractTpl<Scalar> Base;
501 typedef typename MathBase::VectorXs VectorXs;
502 typedef typename MathBase::MatrixXs MatrixXs;
503
504 template <template <typename Scalar> class Model>
505 explicit ActionDataCodeGenTpl(Model<Scalar>* const model) : Base(model) {
506 ActionModelCodeGenTpl<Scalar>* m =
507 static_cast<ActionModelCodeGenTpl<Scalar>*>(model);
508 X.resize(m->get_nX());
509 X_T.resize(m->get_nX_T());
510 X3.resize(m->get_nX3());
511 Y1.resize(m->get_nY1());
512 J1.resize(m->get_nY1() * m->get_nX());
513 H1.resize(m->get_nX() * m->get_nX());
514 Y1_T.resize(m->get_nY1_T());
515 J1_T.resize(m->get_nY1_T() * m->get_nX_T());
516 H1_T.resize(m->get_nX_T() * m->get_nX_T());
517 Y2.resize(m->get_nY2());
518 Y2_T.resize(m->get_nY2_T());
519 Y3.resize(m->get_nY3());
520 X.setZero();
521 X_T.setZero();
522 X3.setZero();
523 Y1.setZero();
524 J1.setZero();
525 H1.setZero();
526 Y1_T.setZero();
527 J1_T.setZero();
528 H1_T.setZero();
529 Y2.setZero();
530 Y2_T.setZero();
531 Y3.setZero();
532 }
533
534 using Base::cost;
535 using Base::Fu;
536 using Base::Fx;
537 using Base::g;
538 using Base::Gu;
539 using Base::Gx;
540 using Base::h;
541 using Base::Hu;
542 using Base::Hx;
543 using Base::Lu;
544 using Base::Luu;
545 using Base::Lx;
546 using Base::Lxu;
547 using Base::Lxx;
548 using Base::r;
549 using Base::xnext;
550
551 VectorXs
552 X;
553 VectorXs X_T;
555 VectorXs X3;
556 VectorXs Y1;
557 VectorXs J1;
558 VectorXs H1;
559 VectorXs Y1_T;
560 VectorXs J1_T;
561 VectorXs H1_T;
563 VectorXs Y2;
564 VectorXs Y2_T;
566 VectorXs Y3;
567
568 template <template <typename Scalar> class Model>
569 void set_Y1(Model<Scalar>* const model) {
570 const std::size_t nx = model->get_state()->get_nx();
571 const std::size_t ng = model->get_ng();
572 const std::size_t nh = model->get_nh();
573 Eigen::DenseIndex it_Y1 = 0;
574 cost = Y1[it_Y1];
575 it_Y1 += 1;
576 xnext = Y1.segment(it_Y1, nx);
577 it_Y1 += nx;
578 g = Y1.segment(it_Y1, ng);
579 it_Y1 += ng;
580 h = Y1.segment(it_Y1, nh);
581 }
582
583 template <template <typename Scalar> class Model>
584 void set_D1(Model<Scalar>* const model) {
585 const std::size_t ndx = model->get_state()->get_ndx();
586 const std::size_t nu = model->get_nu();
587 const std::size_t ng = model->get_ng();
588 const std::size_t nh = model->get_nh();
589 const std::size_t np = model->get_np();
590 const std::size_t nxu = ndx + nu + np;
591 Eigen::DenseIndex it_J1 = 0;
592 Lx = Eigen::Map<VectorXs>(J1.data() + it_J1, ndx);
593 it_J1 += ndx;
594 Lu = Eigen::Map<VectorXs>(J1.data() + it_J1, nu);
595 it_J1 += nu + np;
596 Eigen::Map<MatrixXs> J1_map(J1.data() + it_J1, nxu, ndx);
597 Fx = J1_map.topRows(ndx).transpose();
598 Fu = J1_map.middleRows(ndx, nu).transpose();
599 it_J1 += ndx * nxu;
600 Eigen::Map<MatrixXs> G_map(J1.data() + it_J1, nxu, ng);
601 Gx = G_map.topRows(ndx).transpose();
602 Gu = G_map.middleRows(ndx, nu).transpose();
603 it_J1 += ng * nxu;
604 Eigen::Map<MatrixXs> H_map(J1.data() + it_J1, nxu, nh);
605 Hx = H_map.topRows(ndx).transpose();
606 Hu = H_map.middleRows(ndx, nu).transpose();
607 Eigen::Map<MatrixXs> H1_map(H1.data(), nxu, nxu);
608 Lxx = H1_map.topLeftCorner(ndx, ndx);
609 Luu = H1_map.middleCols(ndx, nu).middleRows(ndx, nu);
610 Lxu = H1_map.middleCols(ndx, nu).topRows(ndx);
611 }
612
613 template <template <typename Scalar> class Model>
614 void set_Y1_T(Model<Scalar>* const model) {
615 const std::size_t ng = model->get_ng_T();
616 const std::size_t nh = model->get_nh_T();
617 Eigen::DenseIndex it_Y1 = 0;
618 cost = Y1_T[it_Y1];
619 it_Y1 += 1;
620 g = Y1_T.segment(it_Y1, ng);
621 it_Y1 += ng;
622 h = Y1_T.segment(it_Y1, nh);
623 }
624
625 template <template <typename Scalar> class Model>
626 void set_D1_T(Model<Scalar>* const model) {
627 const std::size_t ndx = model->get_state()->get_ndx();
628 const std::size_t ng = model->get_ng();
629 const std::size_t np = model->get_np();
630 const std::size_t nxp = ndx + np;
631 Eigen::DenseIndex it_J1 = 0;
632 Lx = Eigen::Map<VectorXs>(J1_T.data() + it_J1, ndx);
633 it_J1 += nxp;
634 Gx = Eigen::Map<MatrixXs>(J1_T.data() + it_J1, nxp, ng)
635 .topRows(ndx)
636 .transpose();
637 it_J1 += ng * nxp;
638 Hx = Eigen::Map<MatrixXs>(J1_T.data() + it_J1, nxp, ng)
639 .topRows(ndx)
640 .transpose();
641 Lxx = Eigen::Map<MatrixXs>(H1_T.data(), nxp, nxp).topLeftCorner(ndx, ndx);
642 }
643
644 template <template <typename Scalar> class Model>
645 void set_Y2(Model<Scalar>* const model) {
646 const std::size_t ndx = model->get_state()->get_ndx();
647 const std::size_t nu = model->get_nu();
648 const std::size_t ng = model->get_ng();
649 const std::size_t nh = model->get_nh();
650 Eigen::DenseIndex it_Y2 = 0;
651 Fx = Eigen::Map<MatrixXs>(Y2.data() + it_Y2, ndx, ndx);
652 it_Y2 += ndx * ndx;
653 Fu = Eigen::Map<MatrixXs>(Y2.data() + it_Y2, ndx, nu);
654 it_Y2 += ndx * nu;
655 Lx = Eigen::Map<VectorXs>(Y2.data() + it_Y2, ndx);
656 it_Y2 += ndx;
657 Lu = Eigen::Map<VectorXs>(Y2.data() + it_Y2, nu);
658 it_Y2 += nu;
659 Lxx = Eigen::Map<MatrixXs>(Y2.data() + it_Y2, ndx, ndx);
660 it_Y2 += ndx * ndx;
661 Lxu = Eigen::Map<MatrixXs>(Y2.data() + it_Y2, ndx, nu);
662 it_Y2 += ndx * nu;
663 Luu = Eigen::Map<MatrixXs>(Y2.data() + it_Y2, nu, nu);
664 it_Y2 += nu * nu;
665 Gx = Eigen::Map<MatrixXs>(Y2.data() + it_Y2, ng, ndx);
666 it_Y2 += ng * ndx;
667 Gu = Eigen::Map<MatrixXs>(Y2.data() + it_Y2, ng, nu);
668 it_Y2 += ng * nu;
669 Hx = Eigen::Map<MatrixXs>(Y2.data() + it_Y2, nh, ndx);
670 it_Y2 += nh * ndx;
671 Hu = Eigen::Map<MatrixXs>(Y2.data() + it_Y2, nh, nu);
672 }
673
674 template <template <typename Scalar> class Model>
675 void set_Y2_T(Model<Scalar>* const model) {
676 const std::size_t ndx = model->get_state()->get_ndx();
677 const std::size_t ng = model->get_ng_T();
678 const std::size_t nh = model->get_nh_T();
679 Eigen::DenseIndex it_Y2 = 0;
680 Lx = Eigen::Map<VectorXs>(Y2_T.data() + it_Y2, ndx);
681 it_Y2 += ndx;
682 Lxx = Eigen::Map<MatrixXs>(Y2_T.data() + it_Y2, ndx, ndx);
683 it_Y2 += ndx * ndx;
684 Gx = Eigen::Map<MatrixXs>(Y2_T.data() + it_Y2, ng, ndx);
685 it_Y2 += ng * ndx;
686 Hx = Eigen::Map<MatrixXs>(Y2_T.data() + it_Y2, nh, ndx);
687 }
688};
689
690} // namespace crocoddyl
691
692/* --- Details -------------------------------------------------------------- */
693/* --- Details -------------------------------------------------------------- */
694/* --- Details -------------------------------------------------------------- */
695#include "crocoddyl/core/codegen/action.hxx"
696
697CROCODDYL_DECLARE_FLOATINGPOINT_EXTERN_TEMPLATE_CLASS(
698 crocoddyl::ActionModelCodeGenTpl)
699CROCODDYL_DECLARE_FLOATINGPOINT_EXTERN_TEMPLATE_STRUCT(
700 crocoddyl::ActionDataCodeGenTpl)
701
702#endif // CROCODDYL_WITH_CODEGEN
703
704#endif // CROCODDYL_CORE_CODEGEN_ACTION_HPP_