GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/cost-base.hpp
Date: 2025-06-03 08:14:12
Exec Total Coverage
Lines: 0 28 0.0%
Functions: 0 29 0.0%
Branches: 0 65 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_COST_BASE_HPP_
11 #define CROCODDYL_CORE_COST_BASE_HPP_
12
13 #include "crocoddyl/core/activation-base.hpp"
14 #include "crocoddyl/core/activations/quadratic.hpp"
15 #include "crocoddyl/core/data-collector-base.hpp"
16 #include "crocoddyl/core/fwd.hpp"
17 #include "crocoddyl/core/residual-base.hpp"
18 #include "crocoddyl/core/state-base.hpp"
19 #include "crocoddyl/core/utils/deprecate.hpp"
20
21 namespace crocoddyl {
22
23 class CostModelBase {
24 public:
25 ✗ virtual ~CostModelBase() = default;
26
27 ✗ CROCODDYL_BASE_CAST(CostModelBase, CostModelAbstractTpl)
28 };
29
30 /**
31 * @brief Abstract class for cost models
32 *
33 * A cost model is defined by the scalar activation function \f$a(\cdot)\f$ and
34 * by the residual function \f$\mathbf{r}(\cdot)\f$ as follows: \f[
35 * \ell(\mathbf{x},\mathbf{u}) = a(\mathbf{r}(\mathbf{x}, \mathbf{u})), \f]
36 * where the residual function depends on the state point
37 * \f$\mathbf{x}\in\mathcal{X}\f$, which lies in the state manifold described
38 * with a `nx`-tuple, its velocity \f$\dot{\mathbf{x}}\in
39 * T_{\mathbf{x}}\mathcal{X}\f$ that belongs to the tangent space with `ndx`
40 * dimension, and the control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$. The
41 * residual vector is defined by \f$\mathbf{r}\in\mathbb{R}^{nr}\f$ where `nr`
42 * describes its dimension in the Euclidean space. On the other hand, the
43 * activation function builds a cost value based on the definition of the
44 * residual vector. The residual vector has to be specialized in a derived
45 * classes.
46 *
47 * The main computations are carring out in `calc()` and `calcDiff()` routines.
48 * `calc()` computes the cost (and its residual) and `calcDiff()` computes the
49 * derivatives of the cost function (and its residual). Concretely speaking,
50 * `calcDiff()` builds a linear-quadratic approximation of the cost function
51 * with the form: \f$\mathbf{l_x}\in\mathbb{R}^{ndx}\f$,
52 * \f$\mathbf{l_u}\in\mathbb{R}^{nu}\f$,
53 * \f$\mathbf{l_{xx}}\in\mathbb{R}^{ndx\times ndx}\f$,
54 * \f$\mathbf{l_{xu}}\in\mathbb{R}^{ndx\times nu}\f$,
55 * \f$\mathbf{l_{uu}}\in\mathbb{R}^{nu\times nu}\f$ are the Jacobians and
56 * Hessians, respectively. Additionally, it is important to note that
57 * `calcDiff()` computes the derivatives using the latest stored values by
58 * `calc()`. Thus, we need to first run `calc()`.
59 *
60 * \sa `ActivationModelAbstractTpl`, `ResidualModelAbstractTpl` `calc()`,
61 * `calcDiff()`, `createData()`
62 */
63 template <typename _Scalar>
64 class CostModelAbstractTpl : public CostModelBase {
65 public:
66 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
67
68 typedef _Scalar Scalar;
69 typedef MathBaseTpl<Scalar> MathBase;
70 typedef CostDataAbstractTpl<Scalar> CostDataAbstract;
71 typedef StateAbstractTpl<Scalar> StateAbstract;
72 typedef ActivationModelAbstractTpl<Scalar> ActivationModelAbstract;
73 typedef ResidualModelAbstractTpl<Scalar> ResidualModelAbstract;
74 typedef ActivationModelQuadTpl<Scalar> ActivationModelQuad;
75 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
76 typedef typename MathBase::VectorXs VectorXs;
77 typedef typename MathBase::MatrixXs MatrixXs;
78
79 /**
80 * @brief Initialize the cost model
81 *
82 * @param[in] state State of the dynamical system
83 * @param[in] activation Activation model
84 * @param[in] residual Residual model
85 */
86 CostModelAbstractTpl(std::shared_ptr<StateAbstract> state,
87 std::shared_ptr<ActivationModelAbstract> activation,
88 std::shared_ptr<ResidualModelAbstract> residual);
89
90 /**
91 * @brief Initialize the cost model
92 *
93 * @param[in] state State of the dynamical system
94 * @param[in] activation Activation model
95 * @param[in] nu Dimension of control vector
96 */
97 CostModelAbstractTpl(std::shared_ptr<StateAbstract> state,
98 std::shared_ptr<ActivationModelAbstract> activation,
99 const std::size_t nu);
100
101 /**
102 * @copybrief CostModelAbstractTpl()
103 *
104 * The default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
105 *
106 * @param[in] state State of the dynamical system
107 * @param[in] activation Activation model
108 */
109 CostModelAbstractTpl(std::shared_ptr<StateAbstract> state,
110 std::shared_ptr<ActivationModelAbstract> activation);
111
112 /**
113 * @copybrief CostModelAbstractTpl()
114 *
115 * We use `ActivationModelQuadTpl` as a default activation model (i.e.,
116 * \f$a=\frac{1}{2}\|\mathbf{r}\|^2\f$)
117 *
118 * @param[in] state State of the dynamical system
119 * @param[in] residual Residual model
120 */
121 CostModelAbstractTpl(std::shared_ptr<StateAbstract> state,
122 std::shared_ptr<ResidualModelAbstract> residual);
123
124 /**
125 * @copybrief CostModelAbstractTpl()
126 *
127 * We use `ActivationModelQuadTpl` as a default activation model (i.e.,
128 * \f$a=\frac{1}{2}\|\mathbf{r}\|^2\f$)
129 *
130 * @param[in] state State of the system
131 * @param[in] nr Dimension of residual vector
132 * @param[in] nu Dimension of control vector
133 */
134 CostModelAbstractTpl(std::shared_ptr<StateAbstract> state,
135 const std::size_t nr, const std::size_t nu);
136
137 /**
138 * @copybrief CostModelAbstractTpl()
139 *
140 * We use `ActivationModelQuadTpl` as a default activation model (i.e.,
141 * \f$a=\frac{1}{2}\|\mathbf{r}\|^2\f$). Furthermore, the default `nu` value
142 * is obtained from `StateAbstractTpl::get_nv()`.
143 *
144 * @param[in] state State of the dynamical system
145 * @param[in] nr Dimension of residual vector
146 * @param[in] nu Dimension of control vector
147 */
148 CostModelAbstractTpl(std::shared_ptr<StateAbstract> state,
149 const std::size_t nr);
150 ✗ virtual ~CostModelAbstractTpl() = default;
151
152 /**
153 * @brief Compute the cost value and its residual vector
154 *
155 * @param[in] data Cost data
156 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
157 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
158 */
159 virtual void calc(const std::shared_ptr<CostDataAbstract>& data,
160 const Eigen::Ref<const VectorXs>& x,
161 const Eigen::Ref<const VectorXs>& u) = 0;
162
163 /**
164 * @brief Compute the total cost value for nodes that depends only on the
165 * state
166 *
167 * It updates the total cost based on the state only. This function is used in
168 * the terminal nodes of an optimal control problem.
169 *
170 * @param[in] data Cost data
171 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
172 */
173 virtual void calc(const std::shared_ptr<CostDataAbstract>& data,
174 const Eigen::Ref<const VectorXs>& x);
175
176 /**
177 * @brief Compute the Jacobian and Hessian of cost and its residual vector
178 *
179 * It computes the Jacobian and Hessian of the cost function. It assumes that
180 * `calc()` has been run first.
181 *
182 * @param[in] data Cost data
183 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
184 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
185 */
186 virtual void calcDiff(const std::shared_ptr<CostDataAbstract>& data,
187 const Eigen::Ref<const VectorXs>& x,
188 const Eigen::Ref<const VectorXs>& u) = 0;
189
190 /**
191 * @brief Compute the Jacobian and Hessian of the cost functions with respect
192 * to the state only
193 *
194 * It updates the Jacobian and Hessian of the cost function based on the state
195 * only. This function is used in the terminal nodes of an optimal control
196 * problem.
197 *
198 * @param[in] data Cost data
199 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
200 */
201 virtual void calcDiff(const std::shared_ptr<CostDataAbstract>& data,
202 const Eigen::Ref<const VectorXs>& x);
203
204 /**
205 * @brief Create the cost data
206 *
207 * The default data contains objects to store the values of the cost, residual
208 * vector and their derivatives (first and second order derivatives). However,
209 * it is possible to specialize this function if we need to create additional
210 * data, for instance, to avoid dynamic memory allocation.
211 *
212 * @param data Data collector
213 * @return the cost data
214 */
215 virtual std::shared_ptr<CostDataAbstract> createData(
216 DataCollectorAbstract* const data);
217
218 /**
219 * @brief Return the state
220 */
221 const std::shared_ptr<StateAbstract>& get_state() const;
222
223 /**
224 * @brief Return the activation model
225 */
226 const std::shared_ptr<ActivationModelAbstract>& get_activation() const;
227
228 /**
229 * @brief Return the residual model
230 */
231 const std::shared_ptr<ResidualModelAbstract>& get_residual() const;
232
233 /**
234 * @brief Return the dimension of the control input
235 */
236 std::size_t get_nu() const;
237
238 /**
239 * @brief Print information on the cost model
240 */
241 template <class Scalar>
242 friend std::ostream& operator<<(std::ostream& os,
243 const CostModelAbstractTpl<Scalar>& model);
244
245 /**
246 * @brief Modify the cost reference
247 */
248 template <class ReferenceType>
249 void set_reference(ReferenceType ref);
250
251 /**
252 * @brief Return the cost reference
253 */
254 template <class ReferenceType>
255 ReferenceType get_reference();
256
257 /**
258 * @brief Print relevant information of the cost model
259 *
260 * @param[out] os Output stream object
261 */
262 virtual void print(std::ostream& os) const;
263
264 protected:
265 /**
266 * @copybrief set_reference()
267 */
268 virtual void set_referenceImpl(const std::type_info&, const void*);
269
270 /**
271 * @copybrief get_reference()
272 */
273 virtual void get_referenceImpl(const std::type_info&, void*);
274
275 std::shared_ptr<StateAbstract> state_; //!< State description
276 std::shared_ptr<ActivationModelAbstract> activation_; //!< Activation model
277 std::shared_ptr<ResidualModelAbstract> residual_; //!< Residual model
278 std::size_t nu_; //!< Control dimension
279 VectorXs unone_; //!< No control vector
280 ✗ CostModelAbstractTpl()
281 ✗ : state_(nullptr), activation_(nullptr), residual_(nullptr), nu_(0) {}
282 };
283
284 template <typename _Scalar>
285 struct CostDataAbstractTpl {
286 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
287
288 typedef _Scalar Scalar;
289 typedef MathBaseTpl<Scalar> MathBase;
290 typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
291 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
292 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
293 typedef typename MathBase::VectorXs VectorXs;
294 typedef typename MathBase::MatrixXs MatrixXs;
295
296 template <template <typename Scalar> class Model>
297 ✗ CostDataAbstractTpl(Model<Scalar>* const model,
298 DataCollectorAbstract* const data)
299 ✗ : shared(data),
300 ✗ activation(model->get_activation()->createData()),
301 ✗ residual(model->get_residual()->createData(data)),
302 ✗ cost(Scalar(0.)),
303 ✗ Lx(model->get_state()->get_ndx()),
304 ✗ Lu(model->get_nu()),
305 ✗ Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
306 ✗ Lxu(model->get_state()->get_ndx(), model->get_nu()),
307 ✗ Luu(model->get_nu(), model->get_nu()) {
308 ✗ Lx.setZero();
309 ✗ Lu.setZero();
310 ✗ Lxx.setZero();
311 ✗ Lxu.setZero();
312 ✗ Luu.setZero();
313 ✗ }
314 ✗ virtual ~CostDataAbstractTpl() = default;
315
316 ✗ DEPRECATED(
317 "Use residual.r", const VectorXs& get_r() const { return residual->r; };)
318 ✗ DEPRECATED(
319 "Use residual.Rx",
320 const MatrixXs& get_Rx() const { return residual->Rx; };)
321 ✗ DEPRECATED(
322 "Use residual.Ru",
323 const MatrixXs& get_Ru() const { return residual->Ru; };)
324 ✗ DEPRECATED(
325 "Use residual.r", void set_r(const VectorXs& r) { residual->r = r; };)
326 ✗ DEPRECATED(
327 "Use residual.Rx",
328 void set_Rx(const MatrixXs& Rx) { residual->Rx = Rx; };)
329 ✗ DEPRECATED(
330 "Use residual.Ru",
331 void set_Ru(const MatrixXs& Ru) { residual->Ru = Ru; };)
332
333 DataCollectorAbstract* shared;
334 std::shared_ptr<ActivationDataAbstract> activation;
335 std::shared_ptr<ResidualDataAbstract> residual;
336 Scalar cost;
337 VectorXs Lx;
338 VectorXs Lu;
339 MatrixXs Lxx;
340 MatrixXs Lxu;
341 MatrixXs Luu;
342 };
343
344 } // namespace crocoddyl
345
346 /* --- Details -------------------------------------------------------------- */
347 /* --- Details -------------------------------------------------------------- */
348 /* --- Details -------------------------------------------------------------- */
349 #include "crocoddyl/core/cost-base.hxx"
350
351 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::CostModelAbstractTpl)
352 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::CostDataAbstractTpl)
353
354 #endif // CROCODDYL_CORE_COST_BASE_HPP_
355