GCC Code Coverage Report

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