| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2021-2025, LAAS-CNRS, University of Edinburgh, | ||
| 5 | // University of Oxford, University of Trento, | ||
| 6 | // Heriot-Watt University | ||
| 7 | // Copyright note valid unless otherwise stated in individual files. | ||
| 8 | // All rights reserved. | ||
| 9 | /////////////////////////////////////////////////////////////////////////////// | ||
| 10 | |||
| 11 | #ifndef CROCODDYL_CORE_INTEGRATED_ACTION_BASE_HPP_ | ||
| 12 | #define CROCODDYL_CORE_INTEGRATED_ACTION_BASE_HPP_ | ||
| 13 | |||
| 14 | #include "crocoddyl/core/action-base.hpp" | ||
| 15 | #include "crocoddyl/core/control-base.hpp" | ||
| 16 | #include "crocoddyl/core/diff-action-base.hpp" | ||
| 17 | #include "crocoddyl/core/fwd.hpp" | ||
| 18 | #include "crocoddyl/core/utils/deprecate.hpp" | ||
| 19 | |||
| 20 | namespace crocoddyl { | ||
| 21 | |||
| 22 | /** | ||
| 23 | * @brief Abstract class for an integrated action model | ||
| 24 | * | ||
| 25 | * An integrated action model is a special kind of action model that is obtained | ||
| 26 | * by applying a numerical integration scheme to a differential (i.e., | ||
| 27 | * continuous time) action model. Different integration schemes can be | ||
| 28 | * implemented inheriting from this base class. | ||
| 29 | * | ||
| 30 | * The numerical integration introduces also the possibility to parametrize the | ||
| 31 | * control trajectory inside an integration step, for instance using | ||
| 32 | * polynomials. This requires introducing some notation to clarify the | ||
| 33 | * difference between the control inputs of the differential model and the | ||
| 34 | * control inputs to the integrated model. We have decided to use | ||
| 35 | * \f$\mathbf{w}\f$ to refer to the control inputs of the differential model and | ||
| 36 | * \f$\mathbf{u}\f$ for the control inputs of the integrated action model. | ||
| 37 | * | ||
| 38 | * \sa `calc()`, `calcDiff()`, `createData()` | ||
| 39 | */ | ||
| 40 | template <typename _Scalar> | ||
| 41 | class IntegratedActionModelAbstractTpl | ||
| 42 | : public ActionModelAbstractTpl<_Scalar> { | ||
| 43 | public: | ||
| 44 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 45 | |||
| 46 | typedef _Scalar Scalar; | ||
| 47 | typedef MathBaseTpl<Scalar> MathBase; | ||
| 48 | typedef ActionModelAbstractTpl<Scalar> Base; | ||
| 49 | typedef IntegratedActionDataAbstractTpl<Scalar> Data; | ||
| 50 | typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract; | ||
| 51 | typedef DifferentialActionModelAbstractTpl<Scalar> | ||
| 52 | DifferentialActionModelAbstract; | ||
| 53 | typedef ControlParametrizationModelAbstractTpl<Scalar> | ||
| 54 | ControlParametrizationModelAbstract; | ||
| 55 | typedef typename MathBase::VectorXs VectorXs; | ||
| 56 | typedef typename MathBase::MatrixXs MatrixXs; | ||
| 57 | |||
| 58 | /** | ||
| 59 | * @brief Initialize the integrator | ||
| 60 | * | ||
| 61 | * @param[in] model Differential action model | ||
| 62 | * @param[in] control Control parametrization | ||
| 63 | * @param[in] time_step Step time (default 1e-3) | ||
| 64 | * @param[in] with_cost_residual Compute cost residual (default true) | ||
| 65 | */ | ||
| 66 | IntegratedActionModelAbstractTpl( | ||
| 67 | std::shared_ptr<DifferentialActionModelAbstract> model, | ||
| 68 | std::shared_ptr<ControlParametrizationModelAbstract> control, | ||
| 69 | const Scalar time_step = Scalar(1e-3), | ||
| 70 | const bool with_cost_residual = true); | ||
| 71 | |||
| 72 | /** | ||
| 73 | * @brief Initialize the integrator | ||
| 74 | * | ||
| 75 | * This initialization uses `ControlParametrizationPolyZeroTpl` for the | ||
| 76 | * control parametrization. | ||
| 77 | * | ||
| 78 | * @param[in] model Differential action model | ||
| 79 | * @param[in] time_step Step time (default 1e-3) | ||
| 80 | * @param[in] with_cost_residual Compute cost residual (default true) | ||
| 81 | */ | ||
| 82 | IntegratedActionModelAbstractTpl( | ||
| 83 | std::shared_ptr<DifferentialActionModelAbstract> model, | ||
| 84 | const Scalar time_step = Scalar(1e-3), | ||
| 85 | const bool with_cost_residual = true); | ||
| 86 | ✗ | virtual ~IntegratedActionModelAbstractTpl() = default; | |
| 87 | |||
| 88 | /** | ||
| 89 | * @brief Create the integrator data | ||
| 90 | * | ||
| 91 | * @return the sympletic integrator data | ||
| 92 | */ | ||
| 93 | virtual std::shared_ptr<ActionDataAbstract> createData() override; | ||
| 94 | |||
| 95 | /** | ||
| 96 | * @brief Return the number of inequality constraints | ||
| 97 | */ | ||
| 98 | virtual std::size_t get_ng() const override; | ||
| 99 | |||
| 100 | /** | ||
| 101 | * @brief Return the number of equality constraints | ||
| 102 | */ | ||
| 103 | virtual std::size_t get_nh() const override; | ||
| 104 | |||
| 105 | /** | ||
| 106 | * @brief Return the number of inequality terminal constraints | ||
| 107 | */ | ||
| 108 | virtual std::size_t get_ng_T() const override; | ||
| 109 | |||
| 110 | /** | ||
| 111 | * @brief Return the number of equality terminal constraints | ||
| 112 | */ | ||
| 113 | virtual std::size_t get_nh_T() const override; | ||
| 114 | |||
| 115 | /** | ||
| 116 | * @brief Return the lower bound of the inequality constraints | ||
| 117 | */ | ||
| 118 | virtual const VectorXs& get_g_lb() const override; | ||
| 119 | |||
| 120 | /** | ||
| 121 | * @brief Return the upper bound of the inequality constraints | ||
| 122 | */ | ||
| 123 | virtual const VectorXs& get_g_ub() const override; | ||
| 124 | |||
| 125 | /** | ||
| 126 | * @brief Return the differential action model associated to this integrated | ||
| 127 | * action model | ||
| 128 | */ | ||
| 129 | const std::shared_ptr<DifferentialActionModelAbstract>& get_differential() | ||
| 130 | const; | ||
| 131 | |||
| 132 | /** | ||
| 133 | * @brief Return the control parametrization model associated to this | ||
| 134 | * integrated action model | ||
| 135 | */ | ||
| 136 | const std::shared_ptr<ControlParametrizationModelAbstract>& get_control() | ||
| 137 | const; | ||
| 138 | |||
| 139 | /** | ||
| 140 | * @brief Return the time step used for the integration | ||
| 141 | */ | ||
| 142 | const Scalar get_dt() const; | ||
| 143 | |||
| 144 | /** | ||
| 145 | * @brief Set the time step for the integration | ||
| 146 | */ | ||
| 147 | void set_dt(const Scalar dt); | ||
| 148 | |||
| 149 | DEPRECATED("The DifferentialActionModel should be set at construction time", | ||
| 150 | void set_differential( | ||
| 151 | std::shared_ptr<DifferentialActionModelAbstract> model)); | ||
| 152 | |||
| 153 | protected: | ||
| 154 | using Base::has_control_limits_; //!< Indicates whether any of the control | ||
| 155 | //!< limits are active | ||
| 156 | using Base::nr_; //!< Dimension of the cost residual | ||
| 157 | using Base::nu_; //!< Dimension of the control | ||
| 158 | using Base::state_; //!< Model of the state | ||
| 159 | using Base::u_lb_; //!< Lower control limits | ||
| 160 | using Base::u_ub_; //!< Upper control limits | ||
| 161 | |||
| 162 | ✗ | IntegratedActionModelAbstractTpl() | |
| 163 | ✗ | : differential_(nullptr), | |
| 164 | ✗ | control_(nullptr), | |
| 165 | ✗ | time_step_(0), | |
| 166 | ✗ | time_step2_(0), | |
| 167 | ✗ | with_cost_residual_(false) {} | |
| 168 | void init(); | ||
| 169 | |||
| 170 | std::shared_ptr<DifferentialActionModelAbstract> | ||
| 171 | differential_; //!< Differential action model that is integrated | ||
| 172 | std::shared_ptr<ControlParametrizationModelAbstract> | ||
| 173 | control_; //!< Model of the control parametrization | ||
| 174 | |||
| 175 | Scalar time_step_; //!< Time step used for integration | ||
| 176 | Scalar time_step2_; //!< Square of the time step used for integration | ||
| 177 | bool | ||
| 178 | with_cost_residual_; //!< Flag indicating whether a cost residual is used | ||
| 179 | }; | ||
| 180 | |||
| 181 | template <typename _Scalar> | ||
| 182 | struct IntegratedActionDataAbstractTpl : public ActionDataAbstractTpl<_Scalar> { | ||
| 183 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 184 | |||
| 185 | typedef _Scalar Scalar; | ||
| 186 | typedef MathBaseTpl<Scalar> MathBase; | ||
| 187 | typedef ActionDataAbstractTpl<Scalar> Base; | ||
| 188 | typedef typename MathBase::VectorXs VectorXs; | ||
| 189 | typedef typename MathBase::MatrixXs MatrixXs; | ||
| 190 | |||
| 191 | template <template <typename Scalar> class Model> | ||
| 192 | ✗ | explicit IntegratedActionDataAbstractTpl(Model<Scalar>* const model) | |
| 193 | ✗ | : Base(model) {} | |
| 194 | ✗ | virtual ~IntegratedActionDataAbstractTpl() = default; | |
| 195 | |||
| 196 | using Base::cost; | ||
| 197 | using Base::Fu; | ||
| 198 | using Base::Fx; | ||
| 199 | using Base::Lu; | ||
| 200 | using Base::Luu; | ||
| 201 | using Base::Lx; | ||
| 202 | using Base::Lxu; | ||
| 203 | using Base::Lxx; | ||
| 204 | using Base::r; | ||
| 205 | using Base::xnext; | ||
| 206 | }; | ||
| 207 | |||
| 208 | } // namespace crocoddyl | ||
| 209 | |||
| 210 | /* --- Details -------------------------------------------------------------- */ | ||
| 211 | /* --- Details -------------------------------------------------------------- */ | ||
| 212 | /* --- Details -------------------------------------------------------------- */ | ||
| 213 | #include "crocoddyl/core/integ-action-base.hxx" | ||
| 214 | |||
| 215 | CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS( | ||
| 216 | crocoddyl::IntegratedActionModelAbstractTpl) | ||
| 217 | CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT( | ||
| 218 | crocoddyl::IntegratedActionDataAbstractTpl) | ||
| 219 | |||
| 220 | #endif // CROCODDYL_CORE_INTEGRATED_ACTION_BASE_HPP_ | ||
| 221 |