Crocoddyl
Loading...
Searching...
No Matches
action.hpp
1
3// BSD 3-Clause License
4//
5// Copyright (C) 2019-2026, 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
53inline constexpr CompilerType defaultCompilerType() {
54#if defined(__clang__)
55 return CLANG;
56#elif defined(__GNUC__)
57 return GCC;
58#else
59 return CLANG;
60#endif
61}
62
63inline const char* compilerExecutable(CompilerType compiler) {
64 switch (compiler) {
65 case GCC:
66#ifdef CROCODDYL_CODEGEN_GCC_COMPILER_PATH
67 return CROCODDYL_CODEGEN_GCC_COMPILER_PATH;
68#else
69 return "/usr/bin/gcc";
70#endif
71 case CLANG:
72#ifdef CROCODDYL_CODEGEN_CLANG_COMPILER_PATH
73 return CROCODDYL_CODEGEN_CLANG_COMPILER_PATH;
74#else
75 return "/usr/bib/clang";
76#endif
77 }
78 return "cc";
79}
80
81template <typename Scalar>
82struct ActionDataCodeGenTpl;
83
84template <typename _Scalar>
85class ActionModelCodeGenTpl : public ActionModelAbstractTpl<_Scalar> {
86 public:
87 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
88 CROCODDYL_DERIVED_FLOATINGPOINT_CAST(ActionModelBase, ActionModelCodeGenTpl)
89
90 typedef _Scalar Scalar;
91 typedef MathBaseTpl<Scalar> MathBase;
92 typedef ActionModelAbstractTpl<Scalar> Base;
93 typedef ActionDataCodeGenTpl<Scalar> Data;
94 typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
95 typedef typename MathBase::VectorXs VectorXs;
96 typedef typename MathBase::MatrixXs MatrixXs;
97
98 typedef CppAD::cg::CG<Scalar> CGScalar;
99 typedef CppAD::AD<CGScalar> ADScalar;
100 typedef MathBaseTpl<ADScalar> ADMathBase;
101 typedef ActionModelAbstractTpl<ADScalar> ADBase;
102 typedef ActionDataAbstractTpl<ADScalar> ADActionDataAbstract;
103 typedef typename ADMathBase::VectorXs ADVectorXs;
104 typedef typename ADMathBase::MatrixXs ADMatrixXs;
105 typedef CppAD::ADFun<CGScalar> ADFun;
106 typedef CppAD::cg::ModelCSourceGen<Scalar> CSourceGen;
107 typedef CppAD::cg::ModelLibraryCSourceGen<Scalar> LibraryCSourceGen;
108 typedef CppAD::cg::DynamicModelLibraryProcessor<Scalar> LibraryProcessor;
109 typedef CppAD::cg::DynamicLib<Scalar> DynamicLib;
110 typedef CppAD::cg::GenericModel<Scalar> GenericModel;
111 typedef CppAD::cg::LinuxDynamicLib<Scalar> LinuxDynamicLib;
112 typedef CppAD::cg::system::SystemInfo<> SystemInfo;
113 typedef std::function<void(std::shared_ptr<ADBase>,
114 const Eigen::Ref<const ADVectorXs>&)>
115 ParamsEnvironment;
116
133 ActionModelCodeGenTpl(
134 std::shared_ptr<Base> model, const std::string& lib_fname,
135 bool autodiff = false, const std::size_t np = 0,
136 ParamsEnvironment updateParams = EmptyParamsEnv,
137 CompilerType compiler = defaultCompilerType(),
138 const std::string& compile_options = "-O3 -ffast-math -march=native");
139
156 ActionModelCodeGenTpl(
157 std::shared_ptr<ADBase> ad_model, const std::string& lib_fname,
158 bool autodiff = false, const std::size_t np = 0,
159 ParamsEnvironment updateParams = EmptyParamsEnv,
160 CompilerType compiler = defaultCompilerType(),
161 const std::string& compile_options = "-O3 -ffast-math -march=native");
162
170 ActionModelCodeGenTpl(const std::string& lib_fname,
171 std::shared_ptr<Base> model);
172
180 ActionModelCodeGenTpl(const std::string& lib_fname,
181 std::shared_ptr<ADBase> ad_model);
182
187 ActionModelCodeGenTpl(const ActionModelCodeGenTpl<Scalar>& other);
188
189 virtual ~ActionModelCodeGenTpl() = default;
190
194 void initLib();
195
199 void compileLib();
200
207 bool existLib(const std::string& lib_fname) const;
208
214 void loadLib(const std::string& lib_fname);
215
222 void update_p(const std::shared_ptr<ActionDataAbstract>& data,
223 const Eigen::Ref<const VectorXs>& p) const;
224
232 virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
233 const Eigen::Ref<const VectorXs>& x,
234 const Eigen::Ref<const VectorXs>& u) override;
235
240 virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
241 const Eigen::Ref<const VectorXs>& x) override;
242
256 virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
257 const Eigen::Ref<const VectorXs>& x,
258 const Eigen::Ref<const VectorXs>& u) override;
259
264 virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
265 const Eigen::Ref<const VectorXs>& x) override;
266
272 virtual std::shared_ptr<ActionDataAbstract> createData() override;
273
277 virtual bool checkData(
278 const std::shared_ptr<ActionDataAbstract>& data) override;
279
293 virtual void quasiStatic(const std::shared_ptr<ActionDataAbstract>& data,
294 Eigen::Ref<VectorXs> u,
295 const Eigen::Ref<const VectorXs>& x,
296 const std::size_t /*maxiter = 100*/,
297 const Scalar /*tol*/) override;
298
308 template <typename NewScalar>
309 ActionModelCodeGenTpl<NewScalar> cast() const;
310
314 const std::shared_ptr<Base>& get_model() const;
315
316 std::size_t get_np() const;
317
321 virtual std::size_t get_ng() const override;
322
326 virtual std::size_t get_nh() const override;
327
331 virtual std::size_t get_ng_T() const override;
332
336 virtual std::size_t get_nh_T() const override;
337
341 virtual const VectorXs& get_g_lb() const override;
342
346 virtual const VectorXs& get_g_ub() const override;
347
352 std::size_t get_nX() const;
353
358 std::size_t get_nX_T() const;
359
364 std::size_t get_nX3() const;
365
370 std::size_t get_nY1() const;
371
376 std::size_t get_nY1_T() const;
377
382 std::size_t get_nY2() const;
383
388 std::size_t get_nY2_T() const;
389
394 std::size_t get_nY3() const;
395
401 virtual void print(std::ostream& os) const override;
402
403 protected:
404 ActionModelCodeGenTpl();
405
406 using Base::ng_;
407 using Base::ng_T_;
408 using Base::nh_;
409 using Base::nh_T_;
410 using Base::nu_;
411 using Base::state_;
412
413 std::shared_ptr<Base> model_;
414 std::shared_ptr<ADBase>
415 ad_model_;
416 std::shared_ptr<ADActionDataAbstract>
417 ad_data_;
418 bool autodiff_;
420
421 std::size_t np_;
423 std::size_t nX_;
425 std::size_t nX_T_;
427 std::size_t nX3_;
429 std::size_t
430 nY1_;
431 std::size_t nY1_T_;
433 std::size_t nY2_;
435 std::size_t nY2_T_;
437 std::size_t nY3_;
439 ADVectorXs ad_X_;
441 ADVectorXs ad_X_T_;
443 ADVectorXs ad_X3_;
445 ADVectorXs ad_Y1_;
446 ADVectorXs ad_Y1_T_;
448 ADVectorXs
449 ad_Y2_;
450 ADVectorXs ad_Y2_T_;
452 ADVectorXs
453 ad_Y3_;
454
455 const std::string Y1fun_name_;
456 const std::string
457 Y1Tfun_name_;
458 const std::string Y2fun_name_;
459 const std::string
460 Y2Tfun_name_;
461 const std::string Y3fun_name_;
462 const std::string lib_fname_;
463 CompilerType compiler_type_;
464 const std::string compile_options_;
465
466 ParamsEnvironment updateParams_; // Lambda function that updates parameter
467 // variables before starting record.
468
469 std::unique_ptr<ADFun> ad_calc_;
470 std::unique_ptr<ADFun>
471 ad_calc_T_;
472 std::unique_ptr<ADFun>
473 ad_calcDiff_;
474 std::unique_ptr<ADFun> ad_calcDiff_T_;
476 std::unique_ptr<ADFun>
477 ad_quasiStatic_;
478 std::unique_ptr<CSourceGen>
479 calcCG_;
480 std::unique_ptr<CSourceGen>
481 calcCG_T_;
483 std::unique_ptr<CSourceGen>
484 calcDiffCG_;
485 std::unique_ptr<CSourceGen>
486 calcDiffCG_T_;
488 std::unique_ptr<CSourceGen> quasiStaticCG_;
490 std::unique_ptr<LibraryCSourceGen>
491 libCG_;
492 std::unique_ptr<LibraryProcessor>
493 dynLibManager_;
494 std::unique_ptr<DynamicLib> dynLib_;
495 std::unique_ptr<GenericModel> calcFun_;
496 std::unique_ptr<GenericModel>
497 calcFun_T_;
498 std::unique_ptr<GenericModel>
499 calcDiffFun_;
500 std::unique_ptr<GenericModel>
501 calcDiffFun_T_;
502 std::unique_ptr<GenericModel>
503 quasiStaticFun_;
504
505 private:
506 void recordCalc();
507 void recordCalc_T();
508 void recordCalcDiff();
509 void recordCalcDiff_T();
510 void recordQuasiStatic();
511
512 void tapeCalcOutput();
513 void tapeCalcOutput_T();
514 void tapeCalcDiffOutput();
515 void tapeCalcDiffOutput_T();
516
517 VectorXs wCostHess_;
518 VectorXs wCostHess_T_;
519
520 static void EmptyParamsEnv(std::shared_ptr<ADBase>,
521 const Eigen::Ref<const ADVectorXs>&);
522};
523
524template <typename _Scalar>
525struct ActionDataCodeGenTpl : public ActionDataAbstractTpl<_Scalar> {
526 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
527
528 typedef _Scalar Scalar;
529 typedef MathBaseTpl<Scalar> MathBase;
530 typedef ActionDataAbstractTpl<Scalar> Base;
531 typedef typename MathBase::VectorXs VectorXs;
532 typedef typename MathBase::MatrixXs MatrixXs;
533
534 template <template <typename Scalar> class Model>
535 explicit ActionDataCodeGenTpl(Model<Scalar>* const model) : Base(model) {
536 ActionModelCodeGenTpl<Scalar>* m =
537 static_cast<ActionModelCodeGenTpl<Scalar>*>(model);
538 X.resize(m->get_nX());
539 X_T.resize(m->get_nX_T());
540 X3.resize(m->get_nX3());
541 Y1.resize(m->get_nY1());
542 J1.resize(m->get_nY1() * m->get_nX());
543 H1.resize(m->get_nX() * m->get_nX());
544 Y1_T.resize(m->get_nY1_T());
545 J1_T.resize(m->get_nY1_T() * m->get_nX_T());
546 H1_T.resize(m->get_nX_T() * m->get_nX_T());
547 Y2.resize(m->get_nY2());
548 Y2_T.resize(m->get_nY2_T());
549 Y3.resize(m->get_nY3());
550 X.setZero();
551 X_T.setZero();
552 X3.setZero();
553 Y1.setZero();
554 J1.setZero();
555 H1.setZero();
556 Y1_T.setZero();
557 J1_T.setZero();
558 H1_T.setZero();
559 Y2.setZero();
560 Y2_T.setZero();
561 Y3.setZero();
562 }
563
564 using Base::cost;
565 using Base::Fu;
566 using Base::Fx;
567 using Base::g;
568 using Base::Gu;
569 using Base::Gx;
570 using Base::h;
571 using Base::Hu;
572 using Base::Hx;
573 using Base::Lu;
574 using Base::Luu;
575 using Base::Lx;
576 using Base::Lxu;
577 using Base::Lxx;
578 using Base::r;
579 using Base::xnext;
580
581 VectorXs
582 X;
583 VectorXs X_T;
585 VectorXs X3;
586 VectorXs Y1;
587 VectorXs J1;
588 VectorXs H1;
589 VectorXs Y1_T;
590 VectorXs J1_T;
591 VectorXs H1_T;
593 VectorXs Y2;
594 VectorXs Y2_T;
596 VectorXs Y3;
597
598 template <template <typename Scalar> class Model>
599 void set_Y1(Model<Scalar>* const model) {
600 const std::size_t nx = model->get_state()->get_nx();
601 const std::size_t ng = model->get_ng();
602 const std::size_t nh = model->get_nh();
603 Eigen::DenseIndex it_Y1 = 0;
604 cost = Y1[it_Y1];
605 it_Y1 += 1;
606 xnext = Y1.segment(it_Y1, nx);
607 it_Y1 += nx;
608 g = Y1.segment(it_Y1, ng);
609 it_Y1 += ng;
610 h = Y1.segment(it_Y1, nh);
611 }
612
613 template <template <typename Scalar> class Model>
614 void set_D1(Model<Scalar>* const model) {
615 const std::size_t ndx = model->get_state()->get_ndx();
616 const std::size_t nu = model->get_nu();
617 const std::size_t ng = model->get_ng();
618 const std::size_t nh = model->get_nh();
619 const std::size_t np = model->get_np();
620 const std::size_t nxu = ndx + nu + np;
621 Eigen::DenseIndex it_J1 = 0;
622 Lx = Eigen::Map<VectorXs>(J1.data() + it_J1, ndx);
623 it_J1 += ndx;
624 Lu = Eigen::Map<VectorXs>(J1.data() + it_J1, nu);
625 it_J1 += nu + np;
626 Eigen::Map<MatrixXs> J1_map(J1.data() + it_J1, nxu, ndx);
627 Fx = J1_map.topRows(ndx).transpose();
628 Fu = J1_map.middleRows(ndx, nu).transpose();
629 it_J1 += ndx * nxu;
630 Eigen::Map<MatrixXs> G_map(J1.data() + it_J1, nxu, ng);
631 Gx = G_map.topRows(ndx).transpose();
632 Gu = G_map.middleRows(ndx, nu).transpose();
633 it_J1 += ng * nxu;
634 Eigen::Map<MatrixXs> H_map(J1.data() + it_J1, nxu, nh);
635 Hx = H_map.topRows(ndx).transpose();
636 Hu = H_map.middleRows(ndx, nu).transpose();
637 Eigen::Map<MatrixXs> H1_map(H1.data(), nxu, nxu);
638 Lxx = H1_map.topLeftCorner(ndx, ndx);
639 Luu = H1_map.middleCols(ndx, nu).middleRows(ndx, nu);
640 Lxu = H1_map.middleCols(ndx, nu).topRows(ndx);
641 }
642
643 template <template <typename Scalar> class Model>
644 void set_Y1_T(Model<Scalar>* const model) {
645 const std::size_t ng = model->get_ng_T();
646 const std::size_t nh = model->get_nh_T();
647 Eigen::DenseIndex it_Y1 = 0;
648 cost = Y1_T[it_Y1];
649 it_Y1 += 1;
650 g = Y1_T.segment(it_Y1, ng);
651 it_Y1 += ng;
652 h = Y1_T.segment(it_Y1, nh);
653 }
654
655 template <template <typename Scalar> class Model>
656 void set_D1_T(Model<Scalar>* const model) {
657 const std::size_t ndx = model->get_state()->get_ndx();
658 const std::size_t ng = model->get_ng();
659 const std::size_t np = model->get_np();
660 const std::size_t nxp = ndx + np;
661 Eigen::DenseIndex it_J1 = 0;
662 Lx = Eigen::Map<VectorXs>(J1_T.data() + it_J1, ndx);
663 it_J1 += nxp;
664 Gx = Eigen::Map<MatrixXs>(J1_T.data() + it_J1, nxp, ng)
665 .topRows(ndx)
666 .transpose();
667 it_J1 += ng * nxp;
668 Hx = Eigen::Map<MatrixXs>(J1_T.data() + it_J1, nxp, ng)
669 .topRows(ndx)
670 .transpose();
671 Lxx = Eigen::Map<MatrixXs>(H1_T.data(), nxp, nxp).topLeftCorner(ndx, ndx);
672 }
673
674 template <template <typename Scalar> class Model>
675 void set_Y2(Model<Scalar>* const model) {
676 const std::size_t ndx = model->get_state()->get_ndx();
677 const std::size_t nu = model->get_nu();
678 const std::size_t ng = model->get_ng();
679 const std::size_t nh = model->get_nh();
680 Eigen::DenseIndex it_Y2 = 0;
681 Fx = Eigen::Map<MatrixXs>(Y2.data() + it_Y2, ndx, ndx);
682 it_Y2 += ndx * ndx;
683 Fu = Eigen::Map<MatrixXs>(Y2.data() + it_Y2, ndx, nu);
684 it_Y2 += ndx * nu;
685 Lx = Eigen::Map<VectorXs>(Y2.data() + it_Y2, ndx);
686 it_Y2 += ndx;
687 Lu = Eigen::Map<VectorXs>(Y2.data() + it_Y2, nu);
688 it_Y2 += nu;
689 Lxx = Eigen::Map<MatrixXs>(Y2.data() + it_Y2, ndx, ndx);
690 it_Y2 += ndx * ndx;
691 Lxu = Eigen::Map<MatrixXs>(Y2.data() + it_Y2, ndx, nu);
692 it_Y2 += ndx * nu;
693 Luu = Eigen::Map<MatrixXs>(Y2.data() + it_Y2, nu, nu);
694 it_Y2 += nu * nu;
695 Gx = Eigen::Map<MatrixXs>(Y2.data() + it_Y2, ng, ndx);
696 it_Y2 += ng * ndx;
697 Gu = Eigen::Map<MatrixXs>(Y2.data() + it_Y2, ng, nu);
698 it_Y2 += ng * nu;
699 Hx = Eigen::Map<MatrixXs>(Y2.data() + it_Y2, nh, ndx);
700 it_Y2 += nh * ndx;
701 Hu = Eigen::Map<MatrixXs>(Y2.data() + it_Y2, nh, nu);
702 }
703
704 template <template <typename Scalar> class Model>
705 void set_Y2_T(Model<Scalar>* const model) {
706 const std::size_t ndx = model->get_state()->get_ndx();
707 const std::size_t ng = model->get_ng_T();
708 const std::size_t nh = model->get_nh_T();
709 Eigen::DenseIndex it_Y2 = 0;
710 Lx = Eigen::Map<VectorXs>(Y2_T.data() + it_Y2, ndx);
711 it_Y2 += ndx;
712 Lxx = Eigen::Map<MatrixXs>(Y2_T.data() + it_Y2, ndx, ndx);
713 it_Y2 += ndx * ndx;
714 Gx = Eigen::Map<MatrixXs>(Y2_T.data() + it_Y2, ng, ndx);
715 it_Y2 += ng * ndx;
716 Hx = Eigen::Map<MatrixXs>(Y2_T.data() + it_Y2, nh, ndx);
717 }
718};
719
720} // namespace crocoddyl
721
722/* --- Details -------------------------------------------------------------- */
723/* --- Details -------------------------------------------------------------- */
724/* --- Details -------------------------------------------------------------- */
725#include "crocoddyl/core/codegen/action.hxx"
726
727#endif // CROCODDYL_WITH_CODEGEN
728
729#endif // CROCODDYL_CORE_CODEGEN_ACTION_HPP_
crocoddyl::ActionModelAbstractTpl
Abstract class for action model.
Definition action-base.hpp:97