| Directory: | ./ |
|---|---|
| File: | include/crocoddyl/core/integrator/rk.hpp |
| Date: | 2025-03-26 19:23:43 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 53 | 54 | 98.1% |
| Branches: | 159 | 318 | 50.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2019-2025, University of Edinburgh, University of Trento, | ||
| 5 | // LAAS-CNRS, IRI: CSIC-UPC, Heriot-Watt University | ||
| 6 | // Copyright note valid unless otherwise stated in individual files. | ||
| 7 | // All rights reserved. | ||
| 8 | /////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | #ifndef CROCODDYL_CORE_INTEGRATOR_RK_HPP_ | ||
| 11 | #define CROCODDYL_CORE_INTEGRATOR_RK_HPP_ | ||
| 12 | |||
| 13 | #include "crocoddyl/core/fwd.hpp" | ||
| 14 | #include "crocoddyl/core/integ-action-base.hpp" | ||
| 15 | |||
| 16 | namespace crocoddyl { | ||
| 17 | |||
| 18 | enum RKType { two = 2, three = 3, four = 4 }; | ||
| 19 | |||
| 20 | /** | ||
| 21 | * @brief Standard RK integrator | ||
| 22 | * | ||
| 23 | * It applies a standard RK integration schemes to a differential (i.e., | ||
| 24 | * continuous time) action model. The available integrators are: RK2, RK3, and | ||
| 25 | * RK4. | ||
| 26 | * | ||
| 27 | * This standard RK scheme introduces also the possibility to parametrize the | ||
| 28 | * control trajectory inside an integration step, for instance using | ||
| 29 | * polynomials. This requires introducing some notation to clarify the | ||
| 30 | * difference between the control inputs of the differential model and the | ||
| 31 | * control inputs to the integrated model. We have decided to use | ||
| 32 | * \f$\mathbf{w}\f$ to refer to the control inputs of the differential model and | ||
| 33 | * \f$\mathbf{u}\f$ for the control inputs of the integrated action model. | ||
| 34 | * | ||
| 35 | * \sa `IntegratedActionModelAbstractTpl`, `calc()`, `calcDiff()`, | ||
| 36 | * `createData()` | ||
| 37 | */ | ||
| 38 | template <typename _Scalar> | ||
| 39 | class IntegratedActionModelRKTpl | ||
| 40 | : public IntegratedActionModelAbstractTpl<_Scalar> { | ||
| 41 | public: | ||
| 42 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 43 | ✗ | CROCODDYL_DERIVED_CAST(ActionModelBase, IntegratedActionModelRKTpl) | |
| 44 | |||
| 45 | typedef _Scalar Scalar; | ||
| 46 | typedef MathBaseTpl<Scalar> MathBase; | ||
| 47 | typedef IntegratedActionModelAbstractTpl<Scalar> Base; | ||
| 48 | typedef IntegratedActionDataRKTpl<Scalar> Data; | ||
| 49 | typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract; | ||
| 50 | typedef DifferentialActionModelAbstractTpl<Scalar> | ||
| 51 | DifferentialActionModelAbstract; | ||
| 52 | typedef DifferentialActionDataAbstractTpl<Scalar> | ||
| 53 | DifferentialActionDataAbstract; | ||
| 54 | typedef ControlParametrizationModelAbstractTpl<Scalar> | ||
| 55 | ControlParametrizationModelAbstract; | ||
| 56 | typedef ControlParametrizationDataAbstractTpl<Scalar> | ||
| 57 | ControlParametrizationDataAbstract; | ||
| 58 | typedef typename MathBase::VectorXs VectorXs; | ||
| 59 | typedef typename MathBase::MatrixXs MatrixXs; | ||
| 60 | |||
| 61 | /** | ||
| 62 | * @brief Initialize the RK integrator | ||
| 63 | * | ||
| 64 | * @param[in] model Differential action model | ||
| 65 | * @param[in] control Control parametrization | ||
| 66 | * @param[in] rktype Type of RK integrator | ||
| 67 | * @param[in] time_step Step time (default 1e-3) | ||
| 68 | * @param[in] with_cost_residual Compute cost residual (default true) | ||
| 69 | */ | ||
| 70 | IntegratedActionModelRKTpl( | ||
| 71 | std::shared_ptr<DifferentialActionModelAbstract> model, | ||
| 72 | std::shared_ptr<ControlParametrizationModelAbstract> control, | ||
| 73 | const RKType rktype, const Scalar time_step = Scalar(1e-3), | ||
| 74 | const bool with_cost_residual = true); | ||
| 75 | |||
| 76 | /** | ||
| 77 | * @brief Initialize the RK integrator | ||
| 78 | * | ||
| 79 | * This initialization uses `ControlParametrizationPolyZeroTpl` for the | ||
| 80 | * control parametrization. | ||
| 81 | * | ||
| 82 | * @param[in] model Differential action model | ||
| 83 | * @param[in] rktype Type of RK integrator | ||
| 84 | * @param[in] time_step Step time (default 1e-3) | ||
| 85 | * @param[in] with_cost_residual Compute cost residual (default true) | ||
| 86 | */ | ||
| 87 | IntegratedActionModelRKTpl( | ||
| 88 | std::shared_ptr<DifferentialActionModelAbstract> model, | ||
| 89 | const RKType rktype, const Scalar time_step = Scalar(1e-3), | ||
| 90 | const bool with_cost_residual = true); | ||
| 91 | 1668 | virtual ~IntegratedActionModelRKTpl() = default; | |
| 92 | |||
| 93 | /** | ||
| 94 | * @brief Integrate the differential action model using RK scheme | ||
| 95 | * | ||
| 96 | * @param[in] data RK integrator data | ||
| 97 | * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ | ||
| 98 | * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ | ||
| 99 | */ | ||
| 100 | virtual void calc(const std::shared_ptr<ActionDataAbstract>& data, | ||
| 101 | const Eigen::Ref<const VectorXs>& x, | ||
| 102 | const Eigen::Ref<const VectorXs>& u) override; | ||
| 103 | |||
| 104 | /** | ||
| 105 | * @brief Integrate the total cost value for nodes that depends only on the | ||
| 106 | * state using RK scheme | ||
| 107 | * | ||
| 108 | * It computes the total cost and defines the next state as the current one. | ||
| 109 | * This function is used in the terminal nodes of an optimal control problem. | ||
| 110 | * | ||
| 111 | * @param[in] data RK integrator data | ||
| 112 | * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ | ||
| 113 | */ | ||
| 114 | virtual void calc(const std::shared_ptr<ActionDataAbstract>& data, | ||
| 115 | const Eigen::Ref<const VectorXs>& x) override; | ||
| 116 | |||
| 117 | /** | ||
| 118 | * @brief Compute the partial derivatives of the RK integrator | ||
| 119 | * | ||
| 120 | * @param[in] data RK integrator data | ||
| 121 | * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ | ||
| 122 | * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ | ||
| 123 | */ | ||
| 124 | virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data, | ||
| 125 | const Eigen::Ref<const VectorXs>& x, | ||
| 126 | const Eigen::Ref<const VectorXs>& u) override; | ||
| 127 | |||
| 128 | /** | ||
| 129 | * @brief Compute the partial derivatives of the cost | ||
| 130 | * | ||
| 131 | * It updates the derivatives of the cost function with respect to the state | ||
| 132 | * only. This function is used in the terminal nodes of an optimal control | ||
| 133 | * problem. | ||
| 134 | * | ||
| 135 | * @param[in] data RK integrator data | ||
| 136 | * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ | ||
| 137 | */ | ||
| 138 | virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data, | ||
| 139 | const Eigen::Ref<const VectorXs>& x) override; | ||
| 140 | |||
| 141 | /** | ||
| 142 | * @brief Create the RK integrator data | ||
| 143 | * | ||
| 144 | * @return the RK integrator data | ||
| 145 | */ | ||
| 146 | virtual std::shared_ptr<ActionDataAbstract> createData() override; | ||
| 147 | |||
| 148 | /** | ||
| 149 | * @brief Cast the RK integrated-action model to a different scalar type. | ||
| 150 | * | ||
| 151 | * It is useful for operations requiring different precision or scalar types. | ||
| 152 | * | ||
| 153 | * @tparam NewScalar The new scalar type to cast to. | ||
| 154 | * @return IntegratedActionModelRKTpl<NewScalar> An action model with the | ||
| 155 | * new scalar type. | ||
| 156 | */ | ||
| 157 | template <typename NewScalar> | ||
| 158 | IntegratedActionModelRKTpl<NewScalar> cast() const; | ||
| 159 | |||
| 160 | /** | ||
| 161 | * @brief Checks that a specific data belongs to this model | ||
| 162 | */ | ||
| 163 | virtual bool checkData( | ||
| 164 | const std::shared_ptr<ActionDataAbstract>& data) override; | ||
| 165 | |||
| 166 | /** | ||
| 167 | * @brief Computes the quasic static commands | ||
| 168 | * | ||
| 169 | * The quasic static commands are the ones produced for a the reference | ||
| 170 | * posture as an equilibrium point, i.e. for | ||
| 171 | * \f$\mathbf{f^q_x}\delta\mathbf{q}+\mathbf{f_u}\delta\mathbf{u}=\mathbf{0}\f$ | ||
| 172 | * | ||
| 173 | * @param[in] data RK integrator data | ||
| 174 | * @param[out] u Quasic static commands | ||
| 175 | * @param[in] x State point (velocity has to be zero) | ||
| 176 | * @param[in] maxiter Maximum allowed number of iterations | ||
| 177 | * @param[in] tol Tolerance | ||
| 178 | */ | ||
| 179 | virtual void quasiStatic(const std::shared_ptr<ActionDataAbstract>& data, | ||
| 180 | Eigen::Ref<VectorXs> u, | ||
| 181 | const Eigen::Ref<const VectorXs>& x, | ||
| 182 | const std::size_t maxiter = 100, | ||
| 183 | const Scalar tol = Scalar(1e-9)) override; | ||
| 184 | |||
| 185 | /** | ||
| 186 | * @brief Return the number of nodes of the integrator | ||
| 187 | */ | ||
| 188 | std::size_t get_ni() const; | ||
| 189 | |||
| 190 | /** | ||
| 191 | * @brief Print relevant information of the RK integrator model | ||
| 192 | * | ||
| 193 | * @param[out] os Output stream object | ||
| 194 | */ | ||
| 195 | virtual void print(std::ostream& os) const override; | ||
| 196 | |||
| 197 | protected: | ||
| 198 | using Base::control_; //!< Control parametrization | ||
| 199 | using Base::differential_; //!< Differential action model | ||
| 200 | using Base::ng_; //!< Number of inequality constraints | ||
| 201 | using Base::nh_; //!< Number of equality constraints | ||
| 202 | using Base::nu_; //!< Dimension of the control | ||
| 203 | using Base::state_; //!< Model of the state | ||
| 204 | using Base::time_step2_; //!< Square of the time step used for integration | ||
| 205 | using Base::time_step_; //!< Time step used for integration | ||
| 206 | using Base::with_cost_residual_; //!< Flag indicating whether a cost residual | ||
| 207 | //!< is used | ||
| 208 | |||
| 209 | private: | ||
| 210 | /** | ||
| 211 | * @brief Modify the RK type | ||
| 212 | */ | ||
| 213 | void set_rk_type(const RKType rktype); | ||
| 214 | |||
| 215 | RKType rk_type_; | ||
| 216 | std::vector<Scalar> rk_c_; | ||
| 217 | std::size_t ni_; | ||
| 218 | }; | ||
| 219 | |||
| 220 | template <typename _Scalar> | ||
| 221 | struct IntegratedActionDataRKTpl | ||
| 222 | : public IntegratedActionDataAbstractTpl<_Scalar> { | ||
| 223 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 224 | |||
| 225 | typedef _Scalar Scalar; | ||
| 226 | typedef MathBaseTpl<Scalar> MathBase; | ||
| 227 | typedef IntegratedActionDataAbstractTpl<Scalar> Base; | ||
| 228 | typedef DifferentialActionDataAbstractTpl<Scalar> | ||
| 229 | DifferentialActionDataAbstract; | ||
| 230 | typedef ControlParametrizationDataAbstractTpl<Scalar> | ||
| 231 | ControlParametrizationDataAbstract; | ||
| 232 | typedef typename MathBase::VectorXs VectorXs; | ||
| 233 | typedef typename MathBase::MatrixXs MatrixXs; | ||
| 234 | |||
| 235 | template <template <typename Scalar> class Model> | ||
| 236 | 31571 | explicit IntegratedActionDataRKTpl(Model<Scalar>* const model) | |
| 237 | : Base(model), | ||
| 238 |
2/4✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
|
31571 | integral(model->get_ni(), Scalar(0.)), |
| 239 |
3/6✓ Branch 1 taken 31557 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
31571 | dx(model->get_state()->get_ndx()), |
| 240 |
6/12✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 31557 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 31557 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 31557 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 31557 times.
✗ Branch 19 not taken.
|
31571 | ki(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())), |
| 241 |
6/12✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 31557 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 31557 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 31557 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 31557 times.
✗ Branch 19 not taken.
|
31571 | y(model->get_ni(), VectorXs::Zero(model->get_state()->get_nx())), |
| 242 |
6/12✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 31557 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 31557 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 31557 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 31557 times.
✗ Branch 19 not taken.
|
31571 | ws(model->get_ni(), VectorXs::Zero(model->get_control()->get_nw())), |
| 243 |
6/12✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 31557 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 31557 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 31557 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 31557 times.
✗ Branch 19 not taken.
|
31571 | dx_rk(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())), |
| 244 |
5/10✓ Branch 3 taken 31557 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 31557 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 31557 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 31557 times.
✗ Branch 16 not taken.
|
63142 | dki_dx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), |
| 245 |
3/6✓ Branch 1 taken 31557 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
31571 | model->get_state()->get_ndx())), |
| 246 |
3/6✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
63142 | dki_du(model->get_ni(), |
| 247 |
4/8✓ Branch 1 taken 31557 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31557 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 31557 times.
✗ Branch 12 not taken.
|
31571 | MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())), |
| 248 |
5/10✓ Branch 3 taken 31557 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 31557 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 31557 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 31557 times.
✗ Branch 16 not taken.
|
63142 | dyi_dx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), |
| 249 |
3/6✓ Branch 1 taken 31557 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
31571 | model->get_state()->get_ndx())), |
| 250 |
3/6✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
63142 | dyi_du(model->get_ni(), |
| 251 |
4/8✓ Branch 1 taken 31557 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31557 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 31557 times.
✗ Branch 12 not taken.
|
31571 | MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())), |
| 252 |
6/12✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 6 taken 31557 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 31557 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 31557 times.
✗ Branch 16 not taken.
✓ Branch 18 taken 31557 times.
✗ Branch 19 not taken.
|
31571 | dli_dx(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())), |
| 253 |
5/10✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 31557 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 31557 times.
✗ Branch 15 not taken.
|
31571 | dli_du(model->get_ni(), VectorXs::Zero(model->get_nu())), |
| 254 |
3/6✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
63142 | ddli_ddx(model->get_ni(), |
| 255 |
2/4✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
|
31571 | MatrixXs::Zero(model->get_state()->get_ndx(), |
| 256 |
3/6✓ Branch 1 taken 31557 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
31571 | model->get_state()->get_ndx())), |
| 257 |
3/6✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
63142 | ddli_ddw(model->get_ni(), |
| 258 |
2/4✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
|
31571 | MatrixXs::Zero(model->get_control()->get_nw(), |
| 259 |
3/6✓ Branch 1 taken 31557 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
31571 | model->get_control()->get_nw())), |
| 260 |
3/6✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
63142 | ddli_ddu(model->get_ni(), |
| 261 |
3/6✓ Branch 1 taken 31557 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31557 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31557 times.
✗ Branch 8 not taken.
|
31571 | MatrixXs::Zero(model->get_nu(), model->get_nu())), |
| 262 |
3/6✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
63142 | ddli_dxdw(model->get_ni(), |
| 263 |
2/4✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
|
31571 | MatrixXs::Zero(model->get_state()->get_ndx(), |
| 264 |
3/6✓ Branch 1 taken 31557 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
31571 | model->get_control()->get_nw())), |
| 265 |
5/10✓ Branch 3 taken 31557 times.
✗ Branch 4 not taken.
✓ Branch 6 taken 31557 times.
✗ Branch 7 not taken.
✓ Branch 9 taken 31557 times.
✗ Branch 10 not taken.
✓ Branch 12 taken 31557 times.
✗ Branch 13 not taken.
✓ Branch 15 taken 31557 times.
✗ Branch 16 not taken.
|
63142 | ddli_dxdu(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), |
| 266 |
2/4✓ Branch 1 taken 31557 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31557 times.
✗ Branch 5 not taken.
|
31571 | model->get_nu())), |
| 267 |
3/6✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
63142 | ddli_dwdu( |
| 268 | model->get_ni(), | ||
| 269 |
4/8✓ Branch 1 taken 31557 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31557 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 31557 times.
✗ Branch 12 not taken.
|
31571 | MatrixXs::Zero(model->get_control()->get_nw(), model->get_nu())), |
| 270 |
3/6✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
63142 | Luu_partialx(model->get_ni(), |
| 271 |
3/6✓ Branch 1 taken 31557 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31557 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31557 times.
✗ Branch 8 not taken.
|
31571 | MatrixXs::Zero(model->get_nu(), model->get_nu())), |
| 272 |
3/6✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
63142 | Lxu_i(model->get_ni(), |
| 273 |
4/8✓ Branch 1 taken 31557 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 31557 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 31557 times.
✗ Branch 12 not taken.
|
31571 | MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())), |
| 274 |
3/6✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
63142 | Lxx_partialx(model->get_ni(), |
| 275 |
2/4✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
|
31571 | MatrixXs::Zero(model->get_state()->get_ndx(), |
| 276 |
3/6✓ Branch 1 taken 31557 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
31571 | model->get_state()->get_ndx())), |
| 277 |
3/6✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
63142 | Lxx_partialu( |
| 278 | model->get_ni(), | ||
| 279 |
4/8✓ Branch 4 taken 31557 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 31557 times.
✗ Branch 8 not taken.
✓ Branch 11 taken 31557 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 31557 times.
✗ Branch 15 not taken.
|
63142 | MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())) { |
| 280 |
1/2✓ Branch 1 taken 31557 times.
✗ Branch 2 not taken.
|
31571 | dx.setZero(); |
| 281 | |||
| 282 |
3/4✓ Branch 1 taken 128544 times.
✗ Branch 2 not taken.
✓ Branch 3 taken 96987 times.
✓ Branch 4 taken 31557 times.
|
128610 | for (std::size_t i = 0; i < model->get_ni(); ++i) { |
| 283 |
1/2✓ Branch 1 taken 96987 times.
✗ Branch 2 not taken.
|
97039 | differential.push_back(std::shared_ptr<DifferentialActionDataAbstract>( |
| 284 |
2/4✓ Branch 1 taken 96987 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 96987 times.
✗ Branch 6 not taken.
|
97039 | model->get_differential()->createData())); |
| 285 |
1/2✓ Branch 1 taken 96987 times.
✗ Branch 2 not taken.
|
97039 | control.push_back(std::shared_ptr<ControlParametrizationDataAbstract>( |
| 286 |
2/4✓ Branch 1 taken 96987 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 96987 times.
✗ Branch 6 not taken.
|
97039 | model->get_control()->createData())); |
| 287 | } | ||
| 288 | |||
| 289 |
2/4✓ Branch 1 taken 31557 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
|
31571 | const std::size_t nv = model->get_state()->get_nv(); |
| 290 |
2/4✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
|
31571 | dyi_dx[0].diagonal().setOnes(); |
| 291 |
3/6✓ Branch 2 taken 31557 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 31557 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 31557 times.
✗ Branch 9 not taken.
|
31571 | dki_dx[0].topRightCorner(nv, nv).diagonal().setOnes(); |
| 292 | 31571 | } | |
| 293 | 63102 | virtual ~IntegratedActionDataRKTpl() = default; | |
| 294 | |||
| 295 | std::vector<std::shared_ptr<DifferentialActionDataAbstract> > | ||
| 296 | differential; //!< List of differential model data | ||
| 297 | std::vector<std::shared_ptr<ControlParametrizationDataAbstract> > | ||
| 298 | control; //!< List of control parametrization data | ||
| 299 | std::vector<Scalar> integral; | ||
| 300 | VectorXs dx; //!< State rate | ||
| 301 | std::vector<VectorXs> ki; //!< List of RK terms related to system dynamics | ||
| 302 | std::vector<VectorXs> | ||
| 303 | y; //!< List of states where f is evaluated in the RK integration | ||
| 304 | std::vector<VectorXs> ws; //!< Control inputs evaluated in the RK integration | ||
| 305 | std::vector<VectorXs> dx_rk; | ||
| 306 | |||
| 307 | std::vector<MatrixXs> | ||
| 308 | dki_dx; //!< List of partial derivatives of RK nodes with respect to the | ||
| 309 | //!< state of the RK integration. dki/dx | ||
| 310 | std::vector<MatrixXs> | ||
| 311 | dki_du; //!< List of partial derivatives of RK nodes with respect to the | ||
| 312 | //!< control parameters of the RK integration. dki/du | ||
| 313 | |||
| 314 | std::vector<MatrixXs> | ||
| 315 | dyi_dx; //!< List of partial derivatives of RK dynamics with respect to | ||
| 316 | //!< the state of the RK integrator. dyi/dx | ||
| 317 | std::vector<MatrixXs> | ||
| 318 | dyi_du; //!< List of partial derivatives of RK dynamics with respect to | ||
| 319 | //!< the control parameters of the RK integrator. dyi/du | ||
| 320 | |||
| 321 | std::vector<VectorXs> | ||
| 322 | dli_dx; //!< List of partial derivatives of the cost with respect to the | ||
| 323 | //!< state of the RK integration. dli_dx | ||
| 324 | std::vector<VectorXs> | ||
| 325 | dli_du; //!< List of partial derivatives of the cost with respect to the | ||
| 326 | //!< control input of the RK integration. dli_du | ||
| 327 | |||
| 328 | std::vector<MatrixXs> | ||
| 329 | ddli_ddx; //!< List of second partial derivatives of the cost with | ||
| 330 | //!< respect to the state of the RK integration. ddli_ddx | ||
| 331 | std::vector<MatrixXs> | ||
| 332 | ddli_ddw; //!< List of second partial derivatives of the cost with | ||
| 333 | //!< respect to the control parameters of the RK integration. | ||
| 334 | //!< ddli_ddw | ||
| 335 | std::vector<MatrixXs> ddli_ddu; //!< List of second partial derivatives of | ||
| 336 | //!< the cost with respect to the control | ||
| 337 | //!< input of the RK integration. ddli_ddu | ||
| 338 | std::vector<MatrixXs> | ||
| 339 | ddli_dxdw; //!< List of second partial derivatives of the cost with | ||
| 340 | //!< respect to the state and control input of the RK | ||
| 341 | //!< integration. ddli_dxdw | ||
| 342 | std::vector<MatrixXs> | ||
| 343 | ddli_dxdu; //!< List of second partial derivatives of the cost with | ||
| 344 | //!< respect to the state and control parameters of the RK | ||
| 345 | //!< integration. ddli_dxdu | ||
| 346 | std::vector<MatrixXs> | ||
| 347 | ddli_dwdu; //!< List of second partial derivatives of the cost with | ||
| 348 | //!< respect to the control parameters and inputs control of | ||
| 349 | //!< the RK integration. ddli_dwdu | ||
| 350 | |||
| 351 | std::vector<MatrixXs> Luu_partialx; | ||
| 352 | std::vector<MatrixXs> Lxu_i; | ||
| 353 | std::vector<MatrixXs> Lxx_partialx; | ||
| 354 | std::vector<MatrixXs> Lxx_partialu; | ||
| 355 | |||
| 356 | using Base::cost; | ||
| 357 | using Base::Fu; | ||
| 358 | using Base::Fx; | ||
| 359 | using Base::Lu; | ||
| 360 | using Base::Luu; | ||
| 361 | using Base::Lx; | ||
| 362 | using Base::Lxu; | ||
| 363 | using Base::Lxx; | ||
| 364 | using Base::r; | ||
| 365 | using Base::xnext; | ||
| 366 | }; | ||
| 367 | |||
| 368 | } // namespace crocoddyl | ||
| 369 | |||
| 370 | /* --- Details -------------------------------------------------------------- */ | ||
| 371 | /* --- Details -------------------------------------------------------------- */ | ||
| 372 | /* --- Details -------------------------------------------------------------- */ | ||
| 373 | #include "crocoddyl/core/integrator/rk.hxx" | ||
| 374 | |||
| 375 | CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::IntegratedActionModelRKTpl) | ||
| 376 | CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::IntegratedActionDataRKTpl) | ||
| 377 | |||
| 378 | #endif // CROCODDYL_CORE_INTEGRATOR_RK4_HPP_ | ||
| 379 |