GCC Code Coverage Report

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