GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/residual-base.hpp
Date: 2025-05-13 10:30:51
Exec Total Coverage
Lines: 0 24 0.0%
Branches: 0 46 0.0%
Line Branch Exec Source
1 ///////////////////////////////////////////////////////////////////////////////
2 // BSD 3-Clause License
3 //
4 // Copyright (C) 2021-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_RESIDUAL_BASE_HPP_
10 #define CROCODDYL_CORE_RESIDUAL_BASE_HPP_
11
12 #include "crocoddyl/core/activation-base.hpp"
13 #include "crocoddyl/core/cost-base.hpp"
14 #include "crocoddyl/core/data-collector-base.hpp"
15 #include "crocoddyl/core/fwd.hpp"
16 #include "crocoddyl/core/state-base.hpp"
17
18 namespace crocoddyl {
19
20 class ResidualModelBase {
21 public:
22 ✗ virtual ~ResidualModelBase() = default;
23
24 ✗ CROCODDYL_BASE_CAST(ResidualModelBase, ResidualModelAbstractTpl)
25 };
26
27 /**
28 * @brief Abstract class for residual models
29 *
30 * A residual model defines a vector function \f$\mathbf{r}(\mathbf{x},
31 * \mathbf{u})\mathbb{R}^{nr}\f$ where `nr` describes its dimension in the
32 * Euclidean space. This function depends on the state point
33 * \f$\mathbf{x}\in\mathcal{X}\f$, which lies in the state manifold described
34 * with a `nq`-tuple, its velocity \f$\dot{\mathbf{x}}\in
35 * T_{\mathbf{x}}\mathcal{X}\f$ that belongs to the tangent space with `nv`
36 * dimension, and the control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$. The
37 * residual function can used across cost and constraint models.
38 *
39 * The main computations are carring out in `calc` and `calcDiff` routines.
40 * `calc` computes the residual vector and `calcDiff` computes the Jacobians of
41 * the residual function. Additionally, it is important to note that
42 * `calcDiff()` computes the Jacobians using the latest stored values by
43 * `calc()`. Thus, we need to first run `calc()`.
44 *
45 * \sa `StateAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
46 */
47 template <typename _Scalar>
48 class ResidualModelAbstractTpl : public ResidualModelBase {
49 public:
50 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
51 ✗ CROCODDYL_BASE_DERIVED_CAST(ResidualModelBase, ResidualModelAbstractTpl)
52
53 typedef _Scalar Scalar;
54 typedef MathBaseTpl<Scalar> MathBase;
55 typedef ResidualDataAbstractTpl<Scalar> ResidualDataAbstract;
56 typedef CostDataAbstractTpl<Scalar> CostDataAbstract;
57 typedef ActivationDataAbstractTpl<Scalar> ActivationDataAbstract;
58 typedef StateAbstractTpl<Scalar> StateAbstract;
59 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
60 typedef typename MathBase::VectorXs VectorXs;
61 typedef typename MathBase::MatrixXs MatrixXs;
62 typedef typename MathBase::DiagonalMatrixXs DiagonalMatrixXs;
63
64 /**
65 * @brief Initialize the residual model
66 *
67 * @param[in] state State of the system
68 * @param[in] nr Dimension of residual vector
69 * @param[in] nu Dimension of control vector
70 * @param[in] q_dependent Define if the residual function depends on q
71 * (default true)
72 * @param[in] v_dependent Define if the residual function depends on v
73 * (default true)
74 * @param[in] u_dependent Define if the residual function depends on u
75 * (default true)
76 */
77 ResidualModelAbstractTpl(std::shared_ptr<StateAbstract> state,
78 const std::size_t nr, const std::size_t nu,
79 const bool q_dependent = true,
80 const bool v_dependent = true,
81 const bool u_dependent = true);
82
83 /**
84 * @copybrief ResidualModelAbstractTpl()
85 *
86 * The default `nu` value is obtained from `StateAbstractTpl::get_nv()`.
87 *
88 * @param[in] state State of the system
89 * @param[in] nr Dimension of residual vector
90 * @param[in] q_dependent Define if the residual function depends on q
91 * (default true)
92 * @param[in] v_dependent Define if the residual function depends on v
93 * (default true)
94 * @param[in] u_dependent Define if the residual function depends on u
95 * (default true)
96 */
97 ResidualModelAbstractTpl(std::shared_ptr<StateAbstract> state,
98 const std::size_t nr, const bool q_dependent = true,
99 const bool v_dependent = true,
100 const bool u_dependent = true);
101 ✗ virtual ~ResidualModelAbstractTpl() = default;
102
103 /**
104 * @brief Compute the residual vector
105 *
106 * @param[in] data Residual data
107 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
108 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
109 */
110 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
111 const Eigen::Ref<const VectorXs>& x,
112 const Eigen::Ref<const VectorXs>& u);
113
114 /**
115 * @brief Compute the residual vector for nodes that depends only on the state
116 *
117 * It updates the residual vector based on the state only. This function is
118 * used in the terminal nodes of an optimal control problem.
119 *
120 * @param[in] data Residual data
121 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
122 */
123 virtual void calc(const std::shared_ptr<ResidualDataAbstract>& data,
124 const Eigen::Ref<const VectorXs>& x);
125
126 /**
127 * @brief Compute the Jacobian of the residual vector
128 *
129 * It computes the Jacobian the residual function. It assumes that `calc()`
130 * has been run first.
131 *
132 * @param[in] data Residual data
133 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
134 * @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
135 */
136 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
137 const Eigen::Ref<const VectorXs>& x,
138 const Eigen::Ref<const VectorXs>& u);
139
140 /**
141 * @brief Compute the Jacobian of the residual functions with respect to the
142 * state only
143 *
144 * It updates the Jacobian of the residual function based on the state only.
145 * This function is used in the terminal nodes of an optimal control problem.
146 *
147 * @param[in] data Residual data
148 * @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
149 */
150 virtual void calcDiff(const std::shared_ptr<ResidualDataAbstract>& data,
151 const Eigen::Ref<const VectorXs>& x);
152
153 /**
154 * @brief Create the residual data
155 *
156 * The default data contains objects to store the values of the residual
157 * vector and their Jacobians. However, it is possible to specialize this
158 * function if we need to create additional data, for instance, to avoid
159 * dynamic memory allocation.
160 *
161 * @param data Data collector
162 * @return the residual data
163 */
164 virtual std::shared_ptr<ResidualDataAbstract> createData(
165 DataCollectorAbstract* const data);
166
167 /**
168 * @brief Compute the derivative of the cost function
169 *
170 * This function assumes that the derivatives of the activation and residual
171 * are computed via calcDiff functions.
172 *
173 * @param cdata Cost data
174 * @param rdata Residual data
175 * @param adata Activation data
176 * @param update_u Update the derivative of the cost function w.r.t. to the
177 * control if True.
178 */
179 virtual void calcCostDiff(
180 const std::shared_ptr<CostDataAbstract>& cdata,
181 const std::shared_ptr<ResidualDataAbstract>& rdata,
182 const std::shared_ptr<ActivationDataAbstract>& adata,
183 const bool update_u = true);
184
185 /**
186 * @brief Return the state
187 */
188 const std::shared_ptr<StateAbstract>& get_state() const;
189
190 /**
191 * @brief Return the dimension of the residual vector
192 */
193 std::size_t get_nr() const;
194
195 /**
196 * @brief Return the dimension of the control input
197 */
198 std::size_t get_nu() const;
199
200 /**
201 * @brief Return true if the residual function depends on q
202 */
203 bool get_q_dependent() const;
204
205 /**
206 * @brief Return true if the residual function depends on v
207 */
208 bool get_v_dependent() const;
209
210 /**
211 * @brief Return true if the residual function depends on u
212 */
213 bool get_u_dependent() const;
214
215 /**
216 * @brief Print information on the residual model
217 */
218 template <class Scalar>
219 friend std::ostream& operator<<(
220 std::ostream& os, const ResidualModelAbstractTpl<Scalar>& model);
221
222 /**
223 * @brief Print relevant information of the residual model
224 *
225 * @param[out] os Output stream object
226 */
227 virtual void print(std::ostream& os) const;
228
229 protected:
230 std::shared_ptr<StateAbstract> state_; //!< State description
231 std::size_t nr_; //!< Residual vector dimension
232 std::size_t nu_; //!< Control dimension
233 VectorXs unone_; //!< No control vector
234 bool q_dependent_; //!< Label that indicates if the residual function depends
235 //!< on q
236 bool v_dependent_; //!< Label that indicates if the residual function depends
237 //!< on v
238 bool u_dependent_; //!< Label that indicates if the residual function depends
239 //!< on u
240 ✗ ResidualModelAbstractTpl()
241 ✗ : state_(nullptr),
242 ✗ nr_(0),
243 ✗ nu_(0),
244 ✗ q_dependent_(false),
245 ✗ v_dependent_(false),
246 ✗ u_dependent_(false) {};
247 };
248
249 template <typename _Scalar>
250 struct ResidualDataAbstractTpl {
251 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
252
253 typedef _Scalar Scalar;
254 typedef MathBaseTpl<Scalar> MathBase;
255 typedef DataCollectorAbstractTpl<Scalar> DataCollectorAbstract;
256 typedef typename MathBase::VectorXs VectorXs;
257 typedef typename MathBase::MatrixXs MatrixXs;
258
259 template <template <typename Scalar> class Model>
260 ✗ ResidualDataAbstractTpl(Model<Scalar>* const model,
261 DataCollectorAbstract* const data)
262 ✗ : shared(data),
263 ✗ r(model->get_nr()),
264 ✗ Rx(model->get_nr(), model->get_state()->get_ndx()),
265 ✗ Ru(model->get_nr(), model->get_nu()),
266 ✗ Arr_Rx(model->get_nr(), model->get_state()->get_ndx()),
267 ✗ Arr_Ru(model->get_nr(), model->get_nu()) {
268 ✗ r.setZero();
269 ✗ Rx.setZero();
270 ✗ Ru.setZero();
271 ✗ Arr_Rx.setZero();
272 ✗ Arr_Ru.setZero();
273 }
274 ✗ virtual ~ResidualDataAbstractTpl() = default;
275
276 DataCollectorAbstract* shared; //!< Shared data allocated by the action model
277 VectorXs r; //!< Residual vector
278 MatrixXs Rx; //!< Jacobian of the residual vector with respect the state
279 MatrixXs Ru; //!< Jacobian of the residual vector with respect the control
280 MatrixXs Arr_Rx;
281 MatrixXs Arr_Ru;
282 };
283
284 } // namespace crocoddyl
285
286 /* --- Details -------------------------------------------------------------- */
287 /* --- Details -------------------------------------------------------------- */
288 /* --- Details -------------------------------------------------------------- */
289 #include "crocoddyl/core/residual-base.hxx"
290
291 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ResidualModelAbstractTpl)
292 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ResidualDataAbstractTpl)
293
294 #endif // CROCODDYL_CORE_RESIDUAL_BASE_HPP_
295