| Directory: | ./ |
|---|---|
| File: | include/crocoddyl/core/costs/cost-sum.hpp |
| Date: | 2025-03-26 19:23:43 |
| Exec | Total | Coverage | |
|---|---|---|---|
| Lines: | 49 | 79 | 62.0% |
| Branches: | 59 | 246 | 24.0% |
| Line | Branch | Exec | Source |
|---|---|---|---|
| 1 | /////////////////////////////////////////////////////////////////////////////// | ||
| 2 | // BSD 3-Clause License | ||
| 3 | // | ||
| 4 | // Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh, | ||
| 5 | // Heriot-Watt University | ||
| 6 | // Copyright note valid unless otherwise stated in individual files. | ||
| 7 | // All rights reserved. | ||
| 8 | /////////////////////////////////////////////////////////////////////////////// | ||
| 9 | |||
| 10 | #ifndef CROCODDYL_CORE_COSTS_COST_SUM_HPP_ | ||
| 11 | #define CROCODDYL_CORE_COSTS_COST_SUM_HPP_ | ||
| 12 | |||
| 13 | #include <map> | ||
| 14 | #include <set> | ||
| 15 | #include <string> | ||
| 16 | #include <utility> | ||
| 17 | |||
| 18 | #include "crocoddyl/core/cost-base.hpp" | ||
| 19 | #include "crocoddyl/core/fwd.hpp" | ||
| 20 | |||
| 21 | namespace crocoddyl { | ||
| 22 | |||
| 23 | template <typename _Scalar> | ||
| 24 | struct CostItemTpl { | ||
| 25 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 26 | |||
| 27 | typedef _Scalar Scalar; | ||
| 28 | typedef CostModelAbstractTpl<Scalar> CostModelAbstract; | ||
| 29 | |||
| 30 | ✗ | CostItemTpl() {} | |
| 31 | 6121 | CostItemTpl(const std::string& name, std::shared_ptr<CostModelAbstract> cost, | |
| 32 | const Scalar weight, const bool active = true) | ||
| 33 | 6121 | : name(name), cost(cost), weight(weight), active(active) {} | |
| 34 | |||
| 35 | template <typename NewScalar> | ||
| 36 | ✗ | CostItemTpl<NewScalar> cast() const { | |
| 37 | typedef CostItemTpl<NewScalar> ReturnType; | ||
| 38 | ✗ | ReturnType ret(name, cost->template cast<NewScalar>(), | |
| 39 | ✗ | scalar_cast<NewScalar>(weight), active); | |
| 40 | ✗ | return ret; | |
| 41 | } | ||
| 42 | |||
| 43 | /** | ||
| 44 | * @brief Print information on the cost item | ||
| 45 | */ | ||
| 46 | ✗ | friend std::ostream& operator<<(std::ostream& os, | |
| 47 | const CostItemTpl<Scalar>& model) { | ||
| 48 | ✗ | os << "{w=" << model.weight << ", " << *model.cost << "}"; | |
| 49 | ✗ | return os; | |
| 50 | } | ||
| 51 | |||
| 52 | std::string name; | ||
| 53 | std::shared_ptr<CostModelAbstract> cost; | ||
| 54 | Scalar weight; | ||
| 55 | bool active; | ||
| 56 | }; | ||
| 57 | |||
| 58 | /** | ||
| 59 | * @brief Summation of individual cost models | ||
| 60 | * | ||
| 61 | * This class serves to manage a set of added cost models. The cost functions | ||
| 62 | * might active or inactive, with this approach we avoid dynamic allocation of | ||
| 63 | * memory. Each cost model is added through `addCost`, where the weight and its | ||
| 64 | * status can be defined. | ||
| 65 | * | ||
| 66 | * The main computations are carring out in `calc()` and `calcDiff()` routines. | ||
| 67 | * `calc()` computes the costs (and its residuals) and `calcDiff()` computes the | ||
| 68 | * derivatives of the cost functions (and its residuals). Concretely speaking, | ||
| 69 | * `calcDiff()` builds a linear-quadratic approximation of the total cost | ||
| 70 | * function with the form: \f$\mathbf{\ell_x}\in\mathbb{R}^{ndx}\f$, | ||
| 71 | * \f$\mathbf{\ell_u}\in\mathbb{R}^{nu}\f$, | ||
| 72 | * \f$\mathbf{\ell_{xx}}\in\mathbb{R}^{ndx\times ndx}\f$, | ||
| 73 | * \f$\mathbf{\ell_{xu}}\in\mathbb{R}^{ndx\times nu}\f$, | ||
| 74 | * \f$\mathbf{\ell_{uu}}\in\mathbb{R}^{nu\times nu}\f$ are the Jacobians and | ||
| 75 | * Hessians, respectively. | ||
| 76 | * | ||
| 77 | * \sa `CostModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()` | ||
| 78 | */ | ||
| 79 | template <typename _Scalar> | ||
| 80 | class CostModelSumTpl { | ||
| 81 | public: | ||
| 82 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 83 | |||
| 84 | typedef _Scalar Scalar; | ||
| 85 | typedef MathBaseTpl<Scalar> MathBase; | ||
| 86 | typedef CostDataSumTpl<Scalar> CostDataSum; | ||
| 87 | typedef StateAbstractTpl<Scalar> StateAbstract; | ||
| 88 | typedef CostModelAbstractTpl<Scalar> CostModelAbstract; | ||
| 89 | typedef CostDataAbstractTpl<Scalar> CostDataAbstract; | ||
| 90 | typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract; | ||
| 91 | typedef CostItemTpl<Scalar> CostItem; | ||
| 92 | typedef typename MathBase::VectorXs VectorXs; | ||
| 93 | typedef typename MathBase::MatrixXs MatrixXs; | ||
| 94 | |||
| 95 | typedef std::map<std::string, std::shared_ptr<CostItem> > CostModelContainer; | ||
| 96 | typedef std::map<std::string, std::shared_ptr<CostDataAbstract> > | ||
| 97 | CostDataContainer; | ||
| 98 | |||
| 99 | /** | ||
| 100 | * @brief Initialize the cost-sum model | ||
| 101 | * | ||
| 102 | * @param[in] state State description | ||
| 103 | * @param[in] nu Dimension of control vector | ||
| 104 | */ | ||
| 105 | CostModelSumTpl(std::shared_ptr<StateAbstract> state, const std::size_t nu); | ||
| 106 | |||
| 107 | /** | ||
| 108 | * @brief Initialize the cost-sum model | ||
| 109 | * | ||
| 110 | * The default `nu` value is obtained from `StateAbstractTpl::get_nv()`. | ||
| 111 | * | ||
| 112 | * @param[in] state State description | ||
| 113 | */ | ||
| 114 | explicit CostModelSumTpl(std::shared_ptr<StateAbstract> state); | ||
| 115 | ~CostModelSumTpl(); | ||
| 116 | |||
| 117 | /** | ||
| 118 | * @brief Add a cost item | ||
| 119 | * | ||
| 120 | * @param[in] name Cost name | ||
| 121 | * @param[in] cost Cost model | ||
| 122 | * @param[in] weight Cost weight | ||
| 123 | * @param[in] active True if the cost is activated (default true) | ||
| 124 | */ | ||
| 125 | void addCost(const std::string& name, std::shared_ptr<CostModelAbstract> cost, | ||
| 126 | const Scalar weight, const bool active = true); | ||
| 127 | |||
| 128 | /** | ||
| 129 | * @brief Remove a cost item | ||
| 130 | * | ||
| 131 | * @param[in] name Cost name | ||
| 132 | */ | ||
| 133 | void removeCost(const std::string& name); | ||
| 134 | |||
| 135 | /** | ||
| 136 | * @brief Change the cost status | ||
| 137 | * | ||
| 138 | * @param[in] name Cost name | ||
| 139 | * @param[in] active Cost status (true for active and false for inactive) | ||
| 140 | */ | ||
| 141 | void changeCostStatus(const std::string& name, const bool active); | ||
| 142 | |||
| 143 | /** | ||
| 144 | * @brief Compute the total cost value | ||
| 145 | * | ||
| 146 | * @param[in] data Cost data | ||
| 147 | * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ | ||
| 148 | * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ | ||
| 149 | */ | ||
| 150 | void calc(const std::shared_ptr<CostDataSum>& data, | ||
| 151 | const Eigen::Ref<const VectorXs>& x, | ||
| 152 | const Eigen::Ref<const VectorXs>& u); | ||
| 153 | |||
| 154 | /** | ||
| 155 | * @brief Compute the total cost value for nodes that depends only on the | ||
| 156 | * state | ||
| 157 | * | ||
| 158 | * It updates the total cost based on the state only. This function is used in | ||
| 159 | * the terminal nodes of an optimal control problem. | ||
| 160 | * | ||
| 161 | * @param[in] data Cost data | ||
| 162 | * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ | ||
| 163 | */ | ||
| 164 | void calc(const std::shared_ptr<CostDataSum>& data, | ||
| 165 | const Eigen::Ref<const VectorXs>& x); | ||
| 166 | |||
| 167 | /** | ||
| 168 | * @brief Compute the Jacobian and Hessian of the total cost | ||
| 169 | * | ||
| 170 | * @param[in] data Cost data | ||
| 171 | * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ | ||
| 172 | * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ | ||
| 173 | */ | ||
| 174 | void calcDiff(const std::shared_ptr<CostDataSum>& data, | ||
| 175 | const Eigen::Ref<const VectorXs>& x, | ||
| 176 | const Eigen::Ref<const VectorXs>& u); | ||
| 177 | |||
| 178 | /** | ||
| 179 | * @brief Compute the Jacobian and Hessian of the total cost for nodes that | ||
| 180 | * depends on the state only | ||
| 181 | * | ||
| 182 | * It updates the Jacobian and Hessian of the total cost based on the state | ||
| 183 | * only. This function is used in the terminal nodes of an optimal control | ||
| 184 | * problem. | ||
| 185 | * | ||
| 186 | * @param[in] data Cost data | ||
| 187 | * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$ | ||
| 188 | * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ | ||
| 189 | */ | ||
| 190 | void calcDiff(const std::shared_ptr<CostDataSum>& data, | ||
| 191 | const Eigen::Ref<const VectorXs>& x); | ||
| 192 | |||
| 193 | /** | ||
| 194 | * @brief Create the cost data | ||
| 195 | * | ||
| 196 | * The default data contains objects to store the values of the cost, residual | ||
| 197 | * vector and their derivatives (first and second order derivatives). However, | ||
| 198 | * it is possible to specialize this function is we need to create additional | ||
| 199 | * data, for instance, to avoid dynamic memory allocation. | ||
| 200 | * | ||
| 201 | * @param data Data collector | ||
| 202 | * @return the cost data | ||
| 203 | */ | ||
| 204 | std::shared_ptr<CostDataSum> createData(DataCollectorAbstract* const data); | ||
| 205 | |||
| 206 | /** | ||
| 207 | * @brief Cast the cost-sum model to a different scalar type. | ||
| 208 | * | ||
| 209 | * It is useful for operations requiring different precision or scalar types. | ||
| 210 | * | ||
| 211 | * @tparam NewScalar The new scalar type to cast to. | ||
| 212 | * @return CostModelSumTpl<NewScalar> A cost-sum model with the | ||
| 213 | * new scalar type. | ||
| 214 | */ | ||
| 215 | template <typename NewScalar> | ||
| 216 | CostModelSumTpl<NewScalar> cast() const; | ||
| 217 | |||
| 218 | /** | ||
| 219 | * @brief Return the state | ||
| 220 | */ | ||
| 221 | const std::shared_ptr<StateAbstract>& get_state() const; | ||
| 222 | |||
| 223 | /** | ||
| 224 | * @brief Return the stack of cost models | ||
| 225 | */ | ||
| 226 | const CostModelContainer& get_costs() const; | ||
| 227 | |||
| 228 | /** | ||
| 229 | * @brief Return the dimension of the control input | ||
| 230 | */ | ||
| 231 | std::size_t get_nu() const; | ||
| 232 | |||
| 233 | /** | ||
| 234 | * @brief Return the dimension of the active residual vector | ||
| 235 | */ | ||
| 236 | std::size_t get_nr() const; | ||
| 237 | |||
| 238 | /** | ||
| 239 | * @brief Return the dimension of the total residual vector | ||
| 240 | */ | ||
| 241 | std::size_t get_nr_total() const; | ||
| 242 | |||
| 243 | /** | ||
| 244 | * @brief Return the names of the set of active costs | ||
| 245 | */ | ||
| 246 | const std::set<std::string>& get_active_set() const; | ||
| 247 | |||
| 248 | /** | ||
| 249 | * @brief Return the names of the set of inactive costs | ||
| 250 | */ | ||
| 251 | const std::set<std::string>& get_inactive_set() const; | ||
| 252 | |||
| 253 | ✗ | DEPRECATED( | |
| 254 | "get_active() is deprecated and will be replaced with get_active_set()", | ||
| 255 | const std::vector<std::string>& get_active() { | ||
| 256 | active_.clear(); | ||
| 257 | active_.reserve(active_set_.size()); | ||
| 258 | for (const auto& contact : active_set_) { | ||
| 259 | active_.push_back(contact); | ||
| 260 | } | ||
| 261 | return active_; | ||
| 262 | };) | ||
| 263 | |||
| 264 | ✗ | DEPRECATED( | |
| 265 | "get_inactive() is deprecated and will be replaced with " | ||
| 266 | "get_inactive_set()", | ||
| 267 | const std::vector<std::string>& get_inactive() { | ||
| 268 | inactive_.clear(); | ||
| 269 | inactive_.reserve(inactive_set_.size()); | ||
| 270 | for (const auto& contact : inactive_set_) { | ||
| 271 | inactive_.push_back(contact); | ||
| 272 | } | ||
| 273 | return inactive_; | ||
| 274 | };) | ||
| 275 | |||
| 276 | /** | ||
| 277 | * @brief Return the status of a given cost name | ||
| 278 | * | ||
| 279 | * @param[in] name Cost name | ||
| 280 | */ | ||
| 281 | bool getCostStatus(const std::string& name) const; | ||
| 282 | |||
| 283 | /** | ||
| 284 | * @brief Print information on the stack of costs | ||
| 285 | */ | ||
| 286 | template <class Scalar> | ||
| 287 | friend std::ostream& operator<<(std::ostream& os, | ||
| 288 | const CostModelSumTpl<Scalar>& model); | ||
| 289 | |||
| 290 | private: | ||
| 291 | std::shared_ptr<StateAbstract> state_; //!< State description | ||
| 292 | CostModelContainer costs_; //!< Stack of cost items | ||
| 293 | std::size_t nu_; //!< Dimension of the control input | ||
| 294 | std::size_t nr_; //!< Dimension of the active residual vector | ||
| 295 | std::size_t nr_total_; //!< Dimension of the total residual vector | ||
| 296 | std::set<std::string> active_set_; //!< Names of the active set of cost items | ||
| 297 | std::set<std::string> | ||
| 298 | inactive_set_; //!< Names of the inactive set of cost items | ||
| 299 | |||
| 300 | // Vector variants. These are to maintain the API compatibility for the | ||
| 301 | // deprecated syntax. These will be removed in future versions along with | ||
| 302 | // get_active() / get_inactive() | ||
| 303 | std::vector<std::string> active_; | ||
| 304 | std::vector<std::string> inactive_; | ||
| 305 | }; | ||
| 306 | |||
| 307 | template <typename _Scalar> | ||
| 308 | struct CostDataSumTpl { | ||
| 309 | EIGEN_MAKE_ALIGNED_OPERATOR_NEW | ||
| 310 | |||
| 311 | typedef _Scalar Scalar; | ||
| 312 | typedef MathBaseTpl<Scalar> MathBase; | ||
| 313 | typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract; | ||
| 314 | typedef CostItemTpl<Scalar> CostItem; | ||
| 315 | typedef typename MathBase::VectorXs VectorXs; | ||
| 316 | typedef typename MathBase::MatrixXs MatrixXs; | ||
| 317 | |||
| 318 | template <template <typename Scalar> class Model> | ||
| 319 | 96510 | CostDataSumTpl(Model<Scalar>* const model, DataCollectorAbstract* const data) | |
| 320 |
1/2✓ Branch 4 taken 95880 times.
✗ Branch 5 not taken.
|
96510 | : Lx_internal(model->get_state()->get_ndx()), |
| 321 |
2/4✓ Branch 1 taken 95880 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 95880 times.
✗ Branch 5 not taken.
|
96510 | Lu_internal(model->get_nu()), |
| 322 |
2/4✓ Branch 2 taken 95880 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 95880 times.
✗ Branch 6 not taken.
|
96510 | Lxx_internal(model->get_state()->get_ndx(), |
| 323 |
3/6✓ Branch 1 taken 95880 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 95880 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 95880 times.
✗ Branch 9 not taken.
|
96510 | model->get_state()->get_ndx()), |
| 324 |
4/8✓ Branch 1 taken 95880 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 95880 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 95880 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 95880 times.
✗ Branch 12 not taken.
|
96510 | Lxu_internal(model->get_state()->get_ndx(), model->get_nu()), |
| 325 |
3/6✓ Branch 1 taken 95880 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 95880 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 95880 times.
✗ Branch 8 not taken.
|
96510 | Luu_internal(model->get_nu(), model->get_nu()), |
| 326 | 96510 | shared(data), | |
| 327 | 96510 | cost(Scalar(0.)), | |
| 328 |
5/10✓ Branch 1 taken 95880 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 95880 times.
✗ Branch 5 not taken.
✓ Branch 8 taken 95880 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 95880 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 95880 times.
✗ Branch 15 not taken.
|
96510 | Lx(Lx_internal.data(), model->get_state()->get_ndx()), |
| 329 |
4/8✓ Branch 1 taken 95880 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 95880 times.
✗ Branch 5 not taken.
✓ Branch 7 taken 95880 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 95880 times.
✗ Branch 11 not taken.
|
96510 | Lu(Lu_internal.data(), model->get_nu()), |
| 330 |
4/8✓ Branch 1 taken 95880 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 95880 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 95880 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 95880 times.
✗ Branch 12 not taken.
|
193020 | Lxx(Lxx_internal.data(), model->get_state()->get_ndx(), |
| 331 |
3/6✓ Branch 1 taken 95880 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 95880 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 95880 times.
✗ Branch 9 not taken.
|
96510 | model->get_state()->get_ndx()), |
| 332 |
4/8✓ Branch 1 taken 95880 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 95880 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 95880 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 95880 times.
✗ Branch 12 not taken.
|
193020 | Lxu(Lxu_internal.data(), model->get_state()->get_ndx(), |
| 333 |
2/4✓ Branch 1 taken 95880 times.
✗ Branch 2 not taken.
✓ Branch 4 taken 95880 times.
✗ Branch 5 not taken.
|
96510 | model->get_nu()), |
| 334 |
5/10✓ Branch 2 taken 95880 times.
✗ Branch 3 not taken.
✓ Branch 5 taken 95880 times.
✗ Branch 6 not taken.
✓ Branch 8 taken 95880 times.
✗ Branch 9 not taken.
✓ Branch 11 taken 95880 times.
✗ Branch 12 not taken.
✓ Branch 14 taken 95880 times.
✗ Branch 15 not taken.
|
193020 | Luu(Luu_internal.data(), model->get_nu(), model->get_nu()) { |
| 335 |
1/2✓ Branch 1 taken 95880 times.
✗ Branch 2 not taken.
|
96510 | Lx.setZero(); |
| 336 |
1/2✓ Branch 1 taken 95880 times.
✗ Branch 2 not taken.
|
96510 | Lu.setZero(); |
| 337 |
1/2✓ Branch 1 taken 95880 times.
✗ Branch 2 not taken.
|
96510 | Lxx.setZero(); |
| 338 |
1/2✓ Branch 1 taken 95880 times.
✗ Branch 2 not taken.
|
96510 | Lxu.setZero(); |
| 339 |
1/2✓ Branch 1 taken 95880 times.
✗ Branch 2 not taken.
|
96510 | Luu.setZero(); |
| 340 | 96510 | for (typename CostModelSumTpl<Scalar>::CostModelContainer::const_iterator | |
| 341 |
1/2✓ Branch 1 taken 95880 times.
✗ Branch 2 not taken.
|
96510 | it = model->get_costs().begin(); |
| 342 |
3/4✓ Branch 1 taken 530557 times.
✗ Branch 2 not taken.
✓ Branch 5 taken 434677 times.
✓ Branch 6 taken 95880 times.
|
532913 | it != model->get_costs().end(); ++it) { |
| 343 | 436403 | const std::shared_ptr<CostItem>& item = it->second; | |
| 344 |
3/6✓ Branch 3 taken 434677 times.
✗ Branch 4 not taken.
✓ Branch 7 taken 434677 times.
✗ Branch 8 not taken.
✓ Branch 10 taken 434677 times.
✗ Branch 11 not taken.
|
436403 | costs.insert(std::make_pair(item->name, item->cost->createData(data))); |
| 345 | } | ||
| 346 | 96510 | } | |
| 347 | |||
| 348 | template <class ActionData> | ||
| 349 | 96120 | void shareMemory(ActionData* const data) { | |
| 350 | // Save memory by setting the internal variables with null dimension | ||
| 351 | 96120 | Lx_internal.resize(0); | |
| 352 | 96120 | Lu_internal.resize(0); | |
| 353 | 96120 | Lxx_internal.resize(0, 0); | |
| 354 | 96120 | Lxu_internal.resize(0, 0); | |
| 355 | 96120 | Luu_internal.resize(0, 0); | |
| 356 | // Share memory with the differential action data | ||
| 357 |
1/2✓ Branch 5 taken 95520 times.
✗ Branch 6 not taken.
|
96120 | new (&Lx) Eigen::Map<VectorXs>(data->Lx.data(), data->Lx.size()); |
| 358 |
1/2✓ Branch 5 taken 95520 times.
✗ Branch 6 not taken.
|
96120 | new (&Lu) Eigen::Map<VectorXs>(data->Lu.data(), data->Lu.size()); |
| 359 |
1/2✓ Branch 5 taken 95520 times.
✗ Branch 6 not taken.
|
192240 | new (&Lxx) Eigen::Map<MatrixXs>(data->Lxx.data(), data->Lxx.rows(), |
| 360 | 96120 | data->Lxx.cols()); | |
| 361 |
1/2✓ Branch 5 taken 95520 times.
✗ Branch 6 not taken.
|
192240 | new (&Lxu) Eigen::Map<MatrixXs>(data->Lxu.data(), data->Lxu.rows(), |
| 362 | 96120 | data->Lxu.cols()); | |
| 363 |
1/2✓ Branch 5 taken 95520 times.
✗ Branch 6 not taken.
|
192240 | new (&Luu) Eigen::Map<MatrixXs>(data->Luu.data(), data->Luu.rows(), |
| 364 | 96120 | data->Luu.cols()); | |
| 365 | 96120 | } | |
| 366 | |||
| 367 | 7 | VectorXs get_Lx() const { return Lx; } | |
| 368 | 7 | VectorXs get_Lu() const { return Lu; } | |
| 369 | 7 | MatrixXs get_Lxx() const { return Lxx; } | |
| 370 | 7 | MatrixXs get_Lxu() const { return Lxu; } | |
| 371 | 7 | MatrixXs get_Luu() const { return Luu; } | |
| 372 | |||
| 373 | ✗ | void set_Lx(const VectorXs& _Lx) { | |
| 374 | ✗ | if (Lx.size() != _Lx.size()) { | |
| 375 | ✗ | throw_pretty( | |
| 376 | "Invalid argument: " << "Lx has wrong dimension (it should be " + | ||
| 377 | std::to_string(Lx.size()) + ")"); | ||
| 378 | } | ||
| 379 | ✗ | Lx = _Lx; | |
| 380 | } | ||
| 381 | ✗ | void set_Lu(const VectorXs& _Lu) { | |
| 382 | ✗ | if (Lu.size() != _Lu.size()) { | |
| 383 | ✗ | throw_pretty( | |
| 384 | "Invalid argument: " << "Lu has wrong dimension (it should be " + | ||
| 385 | std::to_string(Lu.size()) + ")"); | ||
| 386 | } | ||
| 387 | ✗ | Lu = _Lu; | |
| 388 | } | ||
| 389 | ✗ | void set_Lxx(const MatrixXs& _Lxx) { | |
| 390 | ✗ | if (Lxx.rows() != _Lxx.rows() || Lxx.cols() != _Lxx.cols()) { | |
| 391 | ✗ | throw_pretty( | |
| 392 | "Invalid argument: " << "Lxx has wrong dimension (it should be " + | ||
| 393 | std::to_string(Lxx.rows()) + ", " + | ||
| 394 | std::to_string(Lxx.cols()) + ")"); | ||
| 395 | } | ||
| 396 | ✗ | Lxx = _Lxx; | |
| 397 | } | ||
| 398 | ✗ | void set_Lxu(const MatrixXs& _Lxu) { | |
| 399 | ✗ | if (Lxu.rows() != _Lxu.rows() || Lxu.cols() != _Lxu.cols()) { | |
| 400 | ✗ | throw_pretty( | |
| 401 | "Invalid argument: " << "Lxu has wrong dimension (it should be " + | ||
| 402 | std::to_string(Lxu.rows()) + ", " + | ||
| 403 | std::to_string(Lxu.cols()) + ")"); | ||
| 404 | } | ||
| 405 | ✗ | Lxu = _Lxu; | |
| 406 | } | ||
| 407 | ✗ | void set_Luu(const MatrixXs& _Luu) { | |
| 408 | ✗ | if (Luu.rows() != _Luu.rows() || Luu.cols() != _Luu.cols()) { | |
| 409 | ✗ | throw_pretty( | |
| 410 | "Invalid argument: " << "Luu has wrong dimension (it should be " + | ||
| 411 | std::to_string(Luu.rows()) + ", " + | ||
| 412 | std::to_string(Luu.cols()) + ")"); | ||
| 413 | } | ||
| 414 | ✗ | Luu = _Luu; | |
| 415 | } | ||
| 416 | |||
| 417 | // Creates internal data in case we don't share it externally | ||
| 418 | VectorXs Lx_internal; | ||
| 419 | VectorXs Lu_internal; | ||
| 420 | MatrixXs Lxx_internal; | ||
| 421 | MatrixXs Lxu_internal; | ||
| 422 | MatrixXs Luu_internal; | ||
| 423 | |||
| 424 | typename CostModelSumTpl<Scalar>::CostDataContainer costs; | ||
| 425 | DataCollectorAbstract* shared; | ||
| 426 | Scalar cost; | ||
| 427 | Eigen::Map<VectorXs> Lx; | ||
| 428 | Eigen::Map<VectorXs> Lu; | ||
| 429 | Eigen::Map<MatrixXs> Lxx; | ||
| 430 | Eigen::Map<MatrixXs> Lxu; | ||
| 431 | Eigen::Map<MatrixXs> Luu; | ||
| 432 | }; | ||
| 433 | |||
| 434 | } // namespace crocoddyl | ||
| 435 | |||
| 436 | /* --- Details -------------------------------------------------------------- */ | ||
| 437 | /* --- Details -------------------------------------------------------------- */ | ||
| 438 | /* --- Details -------------------------------------------------------------- */ | ||
| 439 | #include "crocoddyl/core/costs/cost-sum.hxx" | ||
| 440 | |||
| 441 | CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::CostItemTpl) | ||
| 442 | CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::CostModelSumTpl) | ||
| 443 | CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::CostDataSumTpl) | ||
| 444 | |||
| 445 | #endif // CROCODDYL_CORE_COSTS_COST_SUM_HPP_ | ||
| 446 |