GCC Code Coverage Report

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