| Directory: | ./ |
|---|---|
| File: | include/crocoddyl/core/constraints/constraint-manager.hpp |
| Date: | 2025-03-26 19:23:43 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 77 | 114 | 67.5% |
| Branches: | 108 | 338 | 32.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2020-2025, University of Edinburgh, Heriot-Watt University | ||
| 5 | // Copyright note valid unless otherwise stated in individual files. | ||
| 6 | // All rights reserved. | ||
| 7 | /////////////////////////////////////////////////////////////////////////////// | ||
| 8 | |||
| 9 | #ifndef CROCODDYL_CORE_CONSTRAINTS_CONSTRAINT_MANAGER_HPP_ | ||
| 10 | #define CROCODDYL_CORE_CONSTRAINTS_CONSTRAINT_MANAGER_HPP_ | ||
| 11 | |||
| 12 | #include <map> | ||
| 13 | #include <set> | ||
| 14 | #include <string> | ||
| 15 | #include <utility> | ||
| 16 | |||
| 17 | #include "crocoddyl/core/constraint-base.hpp" | ||
| 18 | |||
| 19 | namespace crocoddyl { | ||
| 20 | |||
| 21 | template <typename _Scalar> | ||
| 22 | struct ConstraintItemTpl { | ||
| 23 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 24 | |||
| 25 | typedef _Scalar Scalar; | ||
| 26 | typedef ConstraintModelAbstractTpl<Scalar> ConstraintModelAbstract; | ||
| 27 | |||
| 28 | ✗ | ConstraintItemTpl() {} | |
| 29 | 2453 | ConstraintItemTpl(const std::string& name, | |
| 30 | std::shared_ptr<ConstraintModelAbstract> constraint, | ||
| 31 | bool active = true) | ||
| 32 | 2453 | : name(name), constraint(constraint), active(active) {} | |
| 33 | |||
| 34 | template <typename NewScalar> | ||
| 35 | ✗ | ConstraintItemTpl<NewScalar> cast() const { | |
| 36 | typedef ConstraintItemTpl<NewScalar> ReturnType; | ||
| 37 | ✗ | ReturnType ret(name, constraint->template cast<NewScalar>(), active); | |
| 38 | ✗ | return ret; | |
| 39 | } | ||
| 40 | |||
| 41 | /** | ||
| 42 | * @brief Print information on the constraint item | ||
| 43 | */ | ||
| 44 | ✗ | friend std::ostream& operator<<(std::ostream& os, | |
| 45 | const ConstraintItemTpl<Scalar>& model) { | ||
| 46 | ✗ | os << "{" << *model.constraint << "}"; | |
| 47 | ✗ | return os; | |
| 48 | } | ||
| 49 | |||
| 50 | std::string name; | ||
| 51 | std::shared_ptr<ConstraintModelAbstract> constraint; | ||
| 52 | bool active; | ||
| 53 | }; | ||
| 54 | |||
| 55 | /** | ||
| 56 | * @brief Manage the individual constraint models | ||
| 57 | * | ||
| 58 | * This class serves to manage a set of added constraint models. The constraint | ||
| 59 | * functions might active or inactive, with this approach we avoid dynamic | ||
| 60 | * allocation of memory. Each constraint model is added through `addConstraint`, | ||
| 61 | * where its status can be defined. | ||
| 62 | * | ||
| 63 | * The main computations are carring out in `calc` and `calcDiff` routines. | ||
| 64 | * `calc` computes the constraint residuals and `calcDiff` computes the | ||
| 65 | * Jacobians of the constraint functions. Concretely speaking, `calcDiff` builds | ||
| 66 | * a linear approximation of the total constraint function (both inequality and | ||
| 67 | * equality) with the form: \f$\mathbf{g_u}\in\mathbb{R}^{ng\times nu}\f$, | ||
| 68 | * \f$\mathbf{h_x}\in\mathbb{R}^{nh\times ndx}\f$ | ||
| 69 | * \f$\mathbf{h_u}\in\mathbb{R}^{nh\times nu}\f$. | ||
| 70 | * | ||
| 71 | * \sa `StateAbstractTpl`, `calc()`, `calcDiff()`, `createData()` | ||
| 72 | */ | ||
| 73 | template <typename _Scalar> | ||
| 74 | class ConstraintModelManagerTpl { | ||
| 75 | public: | ||
| 76 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 77 | |||
| 78 | typedef _Scalar Scalar; | ||
| 79 | typedef MathBaseTpl<Scalar> MathBase; | ||
| 80 | typedef ConstraintDataManagerTpl<Scalar> ConstraintDataManager; | ||
| 81 | typedef StateAbstractTpl<Scalar> StateAbstract; | ||
| 82 | typedef ConstraintModelAbstractTpl<Scalar> ConstraintModelAbstract; | ||
| 83 | typedef ConstraintDataAbstractTpl<Scalar> ConstraintDataAbstract; | ||
| 84 | typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract; | ||
| 85 | typedef ConstraintItemTpl<Scalar> ConstraintItem; | ||
| 86 | typedef typename MathBase::VectorXs VectorXs; | ||
| 87 | typedef typename MathBase::MatrixXs MatrixXs; | ||
| 88 | |||
| 89 | typedef std::map<std::string, std::shared_ptr<ConstraintItem> > | ||
| 90 | ConstraintModelContainer; | ||
| 91 | typedef std::map<std::string, std::shared_ptr<ConstraintDataAbstract> > | ||
| 92 | ConstraintDataContainer; | ||
| 93 | |||
| 94 | /** | ||
| 95 | * @brief Initialize the constraint-manager model | ||
| 96 | * | ||
| 97 | * @param[in] state State of the multibody system | ||
| 98 | * @param[in] nu Dimension of control vector | ||
| 99 | */ | ||
| 100 | ConstraintModelManagerTpl(std::shared_ptr<StateAbstract> state, | ||
| 101 | const std::size_t nu); | ||
| 102 | |||
| 103 | /** | ||
| 104 | * @brief Initialize the constraint-manager model | ||
| 105 | * | ||
| 106 | * The default `nu` value is obtained from `StateAbstractTpl::get_nv()`. | ||
| 107 | * | ||
| 108 | * @param[in] state State of the multibody system | ||
| 109 | */ | ||
| 110 | explicit ConstraintModelManagerTpl(std::shared_ptr<StateAbstract> state); | ||
| 111 | ~ConstraintModelManagerTpl(); | ||
| 112 | |||
| 113 | /** | ||
| 114 | * @brief Add a constraint item | ||
| 115 | * | ||
| 116 | * @param[in] name Constraint name | ||
| 117 | * @param[in] constraint Constraint model | ||
| 118 | * @param[in] weight Constraint weight | ||
| 119 | * @param[in] active True if the constraint is activated (default true) | ||
| 120 | */ | ||
| 121 | void addConstraint(const std::string& name, | ||
| 122 | std::shared_ptr<ConstraintModelAbstract> constraint, | ||
| 123 | const bool active = true); | ||
| 124 | |||
| 125 | /** | ||
| 126 | * @brief Remove a constraint item | ||
| 127 | * | ||
| 128 | * @param[in] name Constraint name | ||
| 129 | */ | ||
| 130 | void removeConstraint(const std::string& name); | ||
| 131 | |||
| 132 | /** | ||
| 133 | * @brief Change the constraint status | ||
| 134 | * | ||
| 135 | * @param[in] name Constraint name | ||
| 136 | * @param[in] active Constraint status (true for active and false for | ||
| 137 | * inactive) | ||
| 138 | */ | ||
| 139 | void changeConstraintStatus(const std::string& name, bool active); | ||
| 140 | |||
| 141 | /** | ||
| 142 | * @brief Compute the total constraint value | ||
| 143 | * | ||
| 144 | * @param[in] data Constraint data | ||
| 145 | * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ | ||
| 146 | * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ | ||
| 147 | */ | ||
| 148 | void calc(const std::shared_ptr<ConstraintDataManager>& data, | ||
| 149 | const Eigen::Ref<const VectorXs>& x, | ||
| 150 | const Eigen::Ref<const VectorXs>& u); | ||
| 151 | |||
| 152 | /** | ||
| 153 | * @brief Compute the total constraint value for nodes that depends only on | ||
| 154 | * the state | ||
| 155 | * | ||
| 156 | * It updates the constraint based on the state only. This function is | ||
| 157 | * commonly used in the terminal nodes of an optimal control problem. | ||
| 158 | * | ||
| 159 | * @param[in] data Constraint data | ||
| 160 | * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ | ||
| 161 | */ | ||
| 162 | void calc(const std::shared_ptr<ConstraintDataManager>& data, | ||
| 163 | const Eigen::Ref<const VectorXs>& x); | ||
| 164 | |||
| 165 | /** | ||
| 166 | * @brief Compute the Jacobian of the total constraint | ||
| 167 | * | ||
| 168 | * @param[in] data Constraint data | ||
| 169 | * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ | ||
| 170 | * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ | ||
| 171 | */ | ||
| 172 | void calcDiff(const std::shared_ptr<ConstraintDataManager>& data, | ||
| 173 | const Eigen::Ref<const VectorXs>& x, | ||
| 174 | const Eigen::Ref<const VectorXs>& u); | ||
| 175 | |||
| 176 | /** | ||
| 177 | * @brief Compute the Jacobian of the total constraint with respect to the | ||
| 178 | * state only | ||
| 179 | * | ||
| 180 | * It computes the Jacobian of the constraint function based on the state | ||
| 181 | * only. This function is commonly used in the terminal nodes of an optimal | ||
| 182 | * control problem. | ||
| 183 | * | ||
| 184 | * @param[in] data Constraint data | ||
| 185 | * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ | ||
| 186 | */ | ||
| 187 | void calcDiff(const std::shared_ptr<ConstraintDataManager>& data, | ||
| 188 | const Eigen::Ref<const VectorXs>& x); | ||
| 189 | |||
| 190 | /** | ||
| 191 | * @brief Create the constraint data | ||
| 192 | * | ||
| 193 | * The default data contains objects to store the values of the constraint and | ||
| 194 | * their derivatives (i.e. Jacobians). However, it is possible to specialize | ||
| 195 | * this function is we need to create additional data, for instance, to avoid | ||
| 196 | * dynamic memory allocation. | ||
| 197 | * | ||
| 198 | * @param data Data collector | ||
| 199 | * @return the constraint data | ||
| 200 | */ | ||
| 201 | std::shared_ptr<ConstraintDataManager> createData( | ||
| 202 | DataCollectorAbstract* const data); | ||
| 203 | |||
| 204 | /** | ||
| 205 | * @brief Cast the constraint-manager model to a different scalar type. | ||
| 206 | * | ||
| 207 | * It is useful for operations requiring different precision or scalar types. | ||
| 208 | * | ||
| 209 | * @tparam NewScalar The new scalar type to cast to. | ||
| 210 | * @return ConstraintModelManagerTpl<NewScalar> A constraint-manager model | ||
| 211 | * with the new scalar type. | ||
| 212 | */ | ||
| 213 | template <typename NewScalar> | ||
| 214 | ConstraintModelManagerTpl<NewScalar> cast() const; | ||
| 215 | |||
| 216 | /** | ||
| 217 | * @brief Return the state | ||
| 218 | */ | ||
| 219 | const std::shared_ptr<StateAbstract>& get_state() const; | ||
| 220 | |||
| 221 | /** | ||
| 222 | * @brief Return the stack of constraint models | ||
| 223 | */ | ||
| 224 | const ConstraintModelContainer& get_constraints() const; | ||
| 225 | |||
| 226 | /** | ||
| 227 | * @brief Return the dimension of the control input | ||
| 228 | */ | ||
| 229 | std::size_t get_nu() const; | ||
| 230 | |||
| 231 | /** | ||
| 232 | * @brief Return the number of active inequality constraints | ||
| 233 | */ | ||
| 234 | std::size_t get_ng() const; | ||
| 235 | |||
| 236 | /** | ||
| 237 | * @brief Return the number of active equality constraints | ||
| 238 | */ | ||
| 239 | std::size_t get_nh() const; | ||
| 240 | |||
| 241 | /** | ||
| 242 | * @brief Return the number of active inequality terminal constraints | ||
| 243 | */ | ||
| 244 | std::size_t get_ng_T() const; | ||
| 245 | |||
| 246 | /** | ||
| 247 | * @brief Return the number of active equality terminal constraints | ||
| 248 | */ | ||
| 249 | std::size_t get_nh_T() const; | ||
| 250 | |||
| 251 | /** | ||
| 252 | * @brief Return the names of the set of active constraints | ||
| 253 | */ | ||
| 254 | const std::set<std::string>& get_active_set() const; | ||
| 255 | |||
| 256 | /** | ||
| 257 | * @brief Return the names of the set of inactive constraints | ||
| 258 | */ | ||
| 259 | const std::set<std::string>& get_inactive_set() const; | ||
| 260 | |||
| 261 | /** | ||
| 262 | * @brief Return the state lower bound | ||
| 263 | */ | ||
| 264 | const VectorXs& get_lb() const; | ||
| 265 | |||
| 266 | /** | ||
| 267 | * @brief Return the state upper bound | ||
| 268 | */ | ||
| 269 | const VectorXs& get_ub() const; | ||
| 270 | |||
| 271 | /** | ||
| 272 | * @brief Return the status of a given constraint name | ||
| 273 | * | ||
| 274 | * @param[in] name Constraint name | ||
| 275 | */ | ||
| 276 | bool getConstraintStatus(const std::string& name) const; | ||
| 277 | |||
| 278 | /** | ||
| 279 | * @brief Print information on the stack of constraints | ||
| 280 | */ | ||
| 281 | template <class Scalar> | ||
| 282 | friend std::ostream& operator<<( | ||
| 283 | std::ostream& os, const ConstraintModelManagerTpl<Scalar>& model); | ||
| 284 | |||
| 285 | private: | ||
| 286 | std::shared_ptr<StateAbstract> state_; //!< State description | ||
| 287 | ConstraintModelContainer constraints_; //!< Stack of constraint items | ||
| 288 | VectorXs lb_; //!< Lower bound of the constraint | ||
| 289 | VectorXs ub_; //!< Upper bound of the constraint | ||
| 290 | std::size_t nu_; //!< Dimension of the control input | ||
| 291 | std::size_t ng_; //!< Number of the active inequality constraints | ||
| 292 | std::size_t nh_; //!< Number of the active equality constraints | ||
| 293 | std::size_t ng_T_; //!< Number of the active inequality terminal constraints | ||
| 294 | std::size_t nh_T_; //!< Number of the active equality terminal constraints | ||
| 295 | std::set<std::string> active_set_; //!< Names of the active constraint items | ||
| 296 | std::set<std::string> | ||
| 297 | inactive_set_; //!< Names of the inactive constraint items | ||
| 298 | }; | ||
| 299 | |||
| 300 | template <typename _Scalar> | ||
| 301 | struct ConstraintDataManagerTpl { | ||
| 302 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 303 | |||
| 304 | typedef _Scalar Scalar; | ||
| 305 | typedef MathBaseTpl<Scalar> MathBase; | ||
| 306 | typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract; | ||
| 307 | typedef ConstraintItemTpl<Scalar> ConstraintItem; | ||
| 308 | typedef typename MathBase::VectorXs VectorXs; | ||
| 309 | typedef typename MathBase::MatrixXs MatrixXs; | ||
| 310 | |||
| 311 | template <template <typename Scalar> class Model> | ||
| 312 | 50030 | ConstraintDataManagerTpl(Model<Scalar>* const model, | |
| 313 | DataCollectorAbstract* const data) | ||
| 314 |
1/2✓ Branch 2 taken 50024 times.
✗ Branch 3 not taken.
|
50030 | : g_internal(model->get_ng()), |
| 315 |
4/8✓ Branch 1 taken 50024 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50024 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50024 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50024 times.
✗ Branch 12 not taken.
|
50030 | Gx_internal(model->get_ng(), model->get_state()->get_ndx()), |
| 316 |
3/6✓ Branch 1 taken 50024 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50024 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50024 times.
✗ Branch 8 not taken.
|
50030 | Gu_internal(model->get_ng(), model->get_nu()), |
| 317 |
2/4✓ Branch 1 taken 50024 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50024 times.
✗ Branch 5 not taken.
|
50030 | h_internal(model->get_nh()), |
| 318 |
4/8✓ Branch 1 taken 50024 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 50024 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 50024 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 50024 times.
✗ Branch 12 not taken.
|
50030 | Hx_internal(model->get_nh(), model->get_state()->get_ndx()), |
| 319 |
3/6✓ Branch 1 taken 50024 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 50024 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 50024 times.
✗ Branch 8 not taken.
|
50030 | Hu_internal(model->get_nh(), model->get_nu()), |
| 320 | 50030 | shared(data), | |
| 321 |
7/11✓ Branch 1 taken 6 times.
✓ Branch 2 taken 50018 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✓ Branch 5 taken 50018 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 6 times.
✓ Branch 8 taken 50018 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
50030 | g(g_internal.data(), model->get_ng()), |
| 322 |
11/18✓ Branch 1 taken 6 times.
✓ Branch 2 taken 50018 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 50018 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 6 times.
✓ Branch 9 taken 50018 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 6 times.
✓ Branch 12 taken 50018 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 6 times.
✓ Branch 15 taken 50018 times.
✗ Branch 16 not taken.
✓ Branch 17 taken 6 times.
✗ Branch 18 not taken.
|
50030 | Gx(Gx_internal.data(), model->get_ng(), model->get_state()->get_ndx()), |
| 323 |
9/14✓ Branch 1 taken 6 times.
✓ Branch 2 taken 50018 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✓ Branch 5 taken 50018 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 6 times.
✓ Branch 8 taken 50018 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 6 times.
✓ Branch 11 taken 50018 times.
✗ Branch 12 not taken.
✓ Branch 13 taken 6 times.
✗ Branch 14 not taken.
|
50030 | Gu(Gu_internal.data(), model->get_ng(), model->get_nu()), |
| 324 |
7/11✓ Branch 1 taken 6 times.
✓ Branch 2 taken 50018 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✓ Branch 5 taken 50018 times.
✗ Branch 6 not taken.
✓ Branch 7 taken 6 times.
✓ Branch 8 taken 50018 times.
✗ Branch 9 not taken.
✓ Branch 10 taken 6 times.
✗ Branch 11 not taken.
|
50030 | h(h_internal.data(), model->get_nh()), |
| 325 |
11/18✓ Branch 1 taken 6 times.
✓ Branch 2 taken 50018 times.
✗ Branch 3 not taken.
✓ Branch 4 taken 6 times.
✗ Branch 5 not taken.
✓ Branch 6 taken 50018 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 6 times.
✓ Branch 9 taken 50018 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 6 times.
✓ Branch 12 taken 50018 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 6 times.
✓ Branch 15 taken 50018 times.
✗ Branch 16 not taken.
✓ Branch 17 taken 6 times.
✗ Branch 18 not taken.
|
50030 | Hx(Hx_internal.data(), model->get_nh(), model->get_state()->get_ndx()), |
| 326 |
9/14✓ Branch 2 taken 6 times.
✓ Branch 3 taken 50018 times.
✗ Branch 4 not taken.
✓ Branch 5 taken 6 times.
✓ Branch 6 taken 50018 times.
✗ Branch 7 not taken.
✓ Branch 8 taken 6 times.
✓ Branch 9 taken 50018 times.
✗ Branch 10 not taken.
✓ Branch 11 taken 6 times.
✓ Branch 12 taken 50018 times.
✗ Branch 13 not taken.
✓ Branch 14 taken 6 times.
✗ Branch 15 not taken.
|
100060 | Hu(Hu_internal.data(), model->get_nh(), model->get_nu()) { |
| 327 |
1/2✓ Branch 1 taken 50024 times.
✗ Branch 2 not taken.
|
50030 | g.setZero(); |
| 328 |
1/2✓ Branch 1 taken 50024 times.
✗ Branch 2 not taken.
|
50030 | Gx.setZero(); |
| 329 |
1/2✓ Branch 1 taken 50024 times.
✗ Branch 2 not taken.
|
50030 | Gu.setZero(); |
| 330 |
1/2✓ Branch 1 taken 50024 times.
✗ Branch 2 not taken.
|
50030 | h.setZero(); |
| 331 |
1/2✓ Branch 1 taken 50024 times.
✗ Branch 2 not taken.
|
50030 | Hx.setZero(); |
| 332 |
1/2✓ Branch 1 taken 50024 times.
✗ Branch 2 not taken.
|
50030 | Hu.setZero(); |
| 333 | 50030 | for (typename ConstraintModelManagerTpl< | |
| 334 | Scalar>::ConstraintModelContainer::const_iterator it = | ||
| 335 |
1/2✓ Branch 1 taken 50024 times.
✗ Branch 2 not taken.
|
50030 | model->get_constraints().begin(); |
| 336 |
3/4✓ Branch 1 taken 270672 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 220648 times.
✓ Branch 6 taken 50024 times.
|
270682 | it != model->get_constraints().end(); ++it) { |
| 337 | 220652 | const std::shared_ptr<ConstraintItem>& item = it->second; | |
| 338 |
1/2✓ Branch 1 taken 220648 times.
✗ Branch 2 not taken.
|
220652 | constraints.insert( |
| 339 |
2/4✓ Branch 3 taken 220648 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 220648 times.
✗ Branch 8 not taken.
|
220652 | std::make_pair(item->name, item->constraint->createData(data))); |
| 340 | } | ||
| 341 | 50030 | } | |
| 342 | |||
| 343 | template <class ActionData> | ||
| 344 | 49950 | void shareMemory(ActionData* const data) { | |
| 345 | // Save memory by setting the internal variables with null dimension | ||
| 346 | 49950 | g_internal.resize(0); | |
| 347 | 49950 | Gx_internal.resize(0, 0); | |
| 348 | 49950 | Gu_internal.resize(0, 0); | |
| 349 | 49950 | h_internal.resize(0); | |
| 350 | 49950 | Hx_internal.resize(0, 0); | |
| 351 | 49950 | Hu_internal.resize(0, 0); | |
| 352 | // Share memory with the differential action data | ||
| 353 |
1/2✓ Branch 5 taken 49946 times.
✗ Branch 6 not taken.
|
49950 | new (&g) Eigen::Map<VectorXs>(data->g.data(), data->g.size()); |
| 354 |
1/2✓ Branch 3 taken 49946 times.
✗ Branch 4 not taken.
|
49950 | new (&Gx) |
| 355 | 49950 | Eigen::Map<MatrixXs>(data->Gx.data(), data->Gx.rows(), data->Gx.cols()); | |
| 356 |
1/2✓ Branch 3 taken 49946 times.
✗ Branch 4 not taken.
|
49950 | new (&Gu) |
| 357 | 49950 | Eigen::Map<MatrixXs>(data->Gu.data(), data->Gu.rows(), data->Gu.cols()); | |
| 358 |
1/2✓ Branch 5 taken 49946 times.
✗ Branch 6 not taken.
|
49950 | new (&h) Eigen::Map<VectorXs>(data->h.data(), data->h.size()); |
| 359 |
1/2✓ Branch 3 taken 49946 times.
✗ Branch 4 not taken.
|
49950 | new (&Hx) |
| 360 | 49950 | Eigen::Map<MatrixXs>(data->Hx.data(), data->Hx.rows(), data->Hx.cols()); | |
| 361 |
1/2✓ Branch 3 taken 49946 times.
✗ Branch 4 not taken.
|
49950 | new (&Hu) |
| 362 | 49950 | Eigen::Map<MatrixXs>(data->Hu.data(), data->Hu.rows(), data->Hu.cols()); | |
| 363 | 49950 | } | |
| 364 | |||
| 365 | template <class Model> | ||
| 366 | 3 | void resize(Model* const model, const bool running_node = true) { | |
| 367 | 3 | const std::size_t ndx = model->get_state()->get_ndx(); | |
| 368 | 3 | const std::size_t nu = model->get_nu(); | |
| 369 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
|
3 | const std::size_t ng = running_node ? model->get_ng() : model->get_ng_T(); |
| 370 |
1/2✗ Branch 0 not taken.
✓ Branch 1 taken 3 times.
|
3 | const std::size_t nh = running_node ? model->get_nh() : model->get_nh_T(); |
| 371 |
1/2✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | new (&g) Eigen::Map<VectorXs>(g_internal.data(), ng); |
| 372 |
1/2✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | new (&Gx) Eigen::Map<MatrixXs>(Gx_internal.data(), ng, ndx); |
| 373 |
1/2✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | new (&Gu) Eigen::Map<MatrixXs>(Gu_internal.data(), ng, nu); |
| 374 |
1/2✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | new (&h) Eigen::Map<VectorXs>(h_internal.data(), nh); |
| 375 |
1/2✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | new (&Hx) Eigen::Map<MatrixXs>(Hx_internal.data(), nh, ndx); |
| 376 |
1/2✓ Branch 4 taken 3 times.
✗ Branch 5 not taken.
|
3 | new (&Hu) Eigen::Map<MatrixXs>(Hu_internal.data(), nh, nu); |
| 377 | 3 | } | |
| 378 | |||
| 379 | template <class ActionModel, class ActionData> | ||
| 380 | 48282 | void resize(ActionModel* const model, ActionData* const data, | |
| 381 | const bool running_node = true) { | ||
| 382 | 48282 | const std::size_t ndx = model->get_state()->get_ndx(); | |
| 383 | 48282 | const std::size_t nu = model->get_nu(); | |
| 384 |
2/2✓ Branch 0 taken 43621 times.
✓ Branch 1 taken 4661 times.
|
48282 | const std::size_t ng = running_node ? model->get_ng() : model->get_ng_T(); |
| 385 |
2/2✓ Branch 0 taken 43621 times.
✓ Branch 1 taken 4661 times.
|
48282 | const std::size_t nh = running_node ? model->get_nh() : model->get_nh_T(); |
| 386 | 48282 | data->g.conservativeResize(ng); | |
| 387 | 48282 | data->Gx.conservativeResize(ng, ndx); | |
| 388 | 48282 | data->Gu.conservativeResize(ng, nu); | |
| 389 | 48282 | data->h.conservativeResize(nh); | |
| 390 | 48282 | data->Hx.conservativeResize(nh, ndx); | |
| 391 | 48282 | data->Hu.conservativeResize(nh, nu); | |
| 392 |
1/2✓ Branch 4 taken 48282 times.
✗ Branch 5 not taken.
|
48282 | new (&g) Eigen::Map<VectorXs>(data->g.data(), ng); |
| 393 |
1/2✓ Branch 4 taken 48282 times.
✗ Branch 5 not taken.
|
48282 | new (&Gx) Eigen::Map<MatrixXs>(data->Gx.data(), ng, ndx); |
| 394 |
1/2✓ Branch 4 taken 48282 times.
✗ Branch 5 not taken.
|
48282 | new (&Gu) Eigen::Map<MatrixXs>(data->Gu.data(), ng, nu); |
| 395 |
1/2✓ Branch 4 taken 48282 times.
✗ Branch 5 not taken.
|
48282 | new (&h) Eigen::Map<VectorXs>(data->h.data(), nh); |
| 396 |
1/2✓ Branch 4 taken 48282 times.
✗ Branch 5 not taken.
|
48282 | new (&Hx) Eigen::Map<MatrixXs>(data->Hx.data(), nh, ndx); |
| 397 |
1/2✓ Branch 4 taken 48282 times.
✗ Branch 5 not taken.
|
48282 | new (&Hu) Eigen::Map<MatrixXs>(data->Hu.data(), nh, nu); |
| 398 | 48282 | } | |
| 399 | |||
| 400 | ✗ | VectorXs get_g() const { return g; } | |
| 401 | ✗ | MatrixXs get_Gx() const { return Gx; } | |
| 402 | ✗ | MatrixXs get_Gu() const { return Gu; } | |
| 403 | ✗ | VectorXs get_h() const { return h; } | |
| 404 | ✗ | MatrixXs get_Hx() const { return Hx; } | |
| 405 | ✗ | MatrixXs get_Hu() const { return Hu; } | |
| 406 | |||
| 407 | ✗ | void set_g(const VectorXs& _g) { | |
| 408 | ✗ | if (g.size() != _g.size()) { | |
| 409 | ✗ | throw_pretty( | |
| 410 | "Invalid argument: " << "g has wrong dimension (it should be " + | ||
| 411 | std::to_string(g.size()) + ")"); | ||
| 412 | } | ||
| 413 | ✗ | g = _g; | |
| 414 | } | ||
| 415 | ✗ | void set_Gx(const MatrixXs& _Gx) { | |
| 416 | ✗ | if (Gx.rows() != _Gx.rows() || Gx.cols() != _Gx.cols()) { | |
| 417 | ✗ | throw_pretty( | |
| 418 | "Invalid argument: " << "Gx has wrong dimension (it should be " + | ||
| 419 | std::to_string(Gx.rows()) + ", " + | ||
| 420 | std::to_string(Gx.cols()) + ")"); | ||
| 421 | } | ||
| 422 | ✗ | Gx = _Gx; | |
| 423 | } | ||
| 424 | ✗ | void set_Gu(const MatrixXs& _Gu) { | |
| 425 | ✗ | if (Gu.rows() != _Gu.rows() || Gu.cols() != _Gu.cols()) { | |
| 426 | ✗ | throw_pretty( | |
| 427 | "Invalid argument: " << "Gu has wrong dimension (it should be " + | ||
| 428 | std::to_string(Gu.rows()) + ", " + | ||
| 429 | std::to_string(Gu.cols()) + ")"); | ||
| 430 | } | ||
| 431 | ✗ | Gu = _Gu; | |
| 432 | } | ||
| 433 | ✗ | void set_h(const VectorXs& _h) { | |
| 434 | ✗ | if (h.size() != _h.size()) { | |
| 435 | ✗ | throw_pretty( | |
| 436 | "Invalid argument: " << "h has wrong dimension (it should be " + | ||
| 437 | std::to_string(h.size()) + ")"); | ||
| 438 | } | ||
| 439 | ✗ | h = _h; | |
| 440 | } | ||
| 441 | ✗ | void set_Hx(const MatrixXs& _Hx) { | |
| 442 | ✗ | if (Hx.rows() != _Hx.rows() || Hx.cols() != _Hx.cols()) { | |
| 443 | ✗ | throw_pretty( | |
| 444 | "Invalid argument: " << "Hx has wrong dimension (it should be " + | ||
| 445 | std::to_string(Hx.rows()) + ", " + | ||
| 446 | std::to_string(Hx.cols()) + ")"); | ||
| 447 | } | ||
| 448 | ✗ | Hx = _Hx; | |
| 449 | } | ||
| 450 | ✗ | void set_Hu(const MatrixXs& _Hu) { | |
| 451 | ✗ | if (Hu.rows() != _Hu.rows() || Hu.cols() != _Hu.cols()) { | |
| 452 | ✗ | throw_pretty( | |
| 453 | "Invalid argument: " << "Hu has wrong dimension (it should be " + | ||
| 454 | std::to_string(Hu.rows()) + ", " + | ||
| 455 | std::to_string(Hu.cols()) + ")"); | ||
| 456 | } | ||
| 457 | ✗ | Hu = _Hu; | |
| 458 | } | ||
| 459 | |||
| 460 | // Creates internal data in case we don't share it externally | ||
| 461 | VectorXs g_internal; | ||
| 462 | MatrixXs Gx_internal; | ||
| 463 | MatrixXs Gu_internal; | ||
| 464 | VectorXs h_internal; | ||
| 465 | MatrixXs Hx_internal; | ||
| 466 | MatrixXs Hu_internal; | ||
| 467 | |||
| 468 | typename ConstraintModelManagerTpl<Scalar>::ConstraintDataContainer | ||
| 469 | constraints; | ||
| 470 | DataCollectorAbstract* shared; | ||
| 471 | Eigen::Map<VectorXs> g; | ||
| 472 | Eigen::Map<MatrixXs> Gx; | ||
| 473 | Eigen::Map<MatrixXs> Gu; | ||
| 474 | Eigen::Map<VectorXs> h; | ||
| 475 | Eigen::Map<MatrixXs> Hx; | ||
| 476 | Eigen::Map<MatrixXs> Hu; | ||
| 477 | }; | ||
| 478 | |||
| 479 | } // namespace crocoddyl | ||
| 480 | |||
| 481 | /* --- Details -------------------------------------------------------------- */ | ||
| 482 | /* --- Details -------------------------------------------------------------- */ | ||
| 483 | /* --- Details -------------------------------------------------------------- */ | ||
| 484 | #include "crocoddyl/core/constraints/constraint-manager.hxx" | ||
| 485 | |||
| 486 | CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ConstraintItemTpl) | ||
| 487 | CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ConstraintModelManagerTpl) | ||
| 488 | CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ConstraintDataManagerTpl) | ||
| 489 | |||
| 490 | #endif // CROCODDYL_CORE_CONSTRAINTS_CONSTRAINT_MANAGER_HPP_ | ||
| 491 |