GCC Code Coverage Report

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