GCC Code Coverage Report

Line	Branch	Exec	Source
1			///////////////////////////////////////////////////////////////////////////////
2			// BSD 3-Clause License
3			//
4			// Copyright (C) 2019-2025, LAAS-CNRS, University of Edinburgh,
5			// University of Oxford, Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#ifndef CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
11			#define CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
12
13			#include "crocoddyl/core/fwd.hpp"
14			#include "crocoddyl/core/state-base.hpp"
15
16			namespace crocoddyl {
17
18			class DifferentialActionModelBase {
19			public:
20		✗	virtual ~DifferentialActionModelBase() = default;
21
22		✗	CROCODDYL_BASE_CAST(DifferentialActionModelBase,
23			DifferentialActionModelAbstractTpl)
24			};
25
26			/**
27			* @brief Abstract class for differential action model
28			*
29			* A differential action model combines dynamics, cost and constraints models.
30			* We can use it in each node of our optimal control problem thanks to dedicated
31			* integration rules (e.g., `IntegratedActionModelEulerTpl` or
32			* `IntegratedActionModelRKTpl`). These integrated action models produce action
33			* models (`ActionModelAbstractTpl`). Thus, every time that we want to describe
34			* a problem, we need to provide ways of computing the dynamics, cost,
35			* constraints functions and their derivatives. All these are described inside
36			* the differential action model.
37			*
38			* Concretely speaking, the differential action model is the time-continuous
39			* version of an action model, i.e., \f[ \begin{aligned}
40			* &\dot{\mathbf{v}} = \mathbf{f}(\mathbf{q}, \mathbf{v}, \mathbf{u}),
41			* &\textrm{(dynamics)}\\
42			* &\ell(\mathbf{q}, \mathbf{v},\mathbf{u}) = \int_0^{\delta t}
43			* a(\mathbf{r}(\mathbf{q}, \mathbf{v},\mathbf{u}))\,dt,
44			* &\textrm{(cost)}\\
45			* &\mathbf{g}(\mathbf{q}, \mathbf{v},\mathbf{u})<\mathbf{0},
46			* &\textrm{(inequality constraint)}\\
47			* &\mathbf{h}(\mathbf{q}, \mathbf{v},\mathbf{u})=\mathbf{0}, &\textrm{(equality
48			* constraint)} \end{aligned} \f] where
49			* - the configuration \f$\mathbf{q}\in\mathcal{Q}\f$ lies in the configuration
50			* manifold described with a `nq`-tuple,
51			* - the velocity \f$\mathbf{v}\in T_{\mathbf{q}}\mathcal{Q}\f$ is the tangent
52			* vector to the configuration manifold with `nv` dimension,
53			* - the control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$ is an Euclidean
54			* vector,
55			* - \f$\mathbf{r}(\cdot)\f$ and \f$a(\cdot)\f$ are the residual and activation
56			* functions (see `ResidualModelAbstractTpl` and `ActivationModelAbstractTpl`,
57			* respectively),
58			* - \f$\mathbf{g}(\cdot)\in\mathbb{R}^{ng}\f$ and
59			* \f$\mathbf{h}(\cdot)\in\mathbb{R}^{nh}\f$ are the inequality and equality
60			* vector functions, respectively.
61			*
62			* Both configuration and velocity describe the system space
63			* \f$\mathbf{x}=(\mathbf{q}, \mathbf{v})\in\mathcal{X}\f$ which lies in the
64			* state manifold. Note that the acceleration \f$\dot{\mathbf{v}}\in
65			* T_{\mathbf{q}}\mathcal{Q}\f$ lies also in the tangent space of the
66			* configuration manifold. The computation of these equations are carried out
67			* inside `calc()` function. In short, this function computes the system
68			* acceleration, cost and constraints values (also called constraints
69			* violations). This procedure is equivalent to running a forward pass of the
70			* action model.
71			*
72			* However, during numerical optimization, we also need to run backward passes
73			* of the differential action model. These calculations are performed by
74			* `calcDiff()`. In short, this function builds a linear-quadratic approximation
75			* of the differential action model, i.e., \f[ \begin{aligned}
76			* &\delta\dot{\mathbf{v}} =
77			* \mathbf{f_{q}}\delta\mathbf{q}+\mathbf{f_{v}}\delta\mathbf{v}+\mathbf{f_{u}}\delta\mathbf{u},
78			* &\textrm{(dynamics)}\\
79			* &\ell(\delta\mathbf{q},\delta\mathbf{v},\delta\mathbf{u}) = \begin{bmatrix}1
80			* \\ \delta\mathbf{q} \\ \delta\mathbf{v}
81			* \\ \delta\mathbf{u}\end{bmatrix}^T \begin{bmatrix}0 & \mathbf{\ell_q}^T &
82			* \mathbf{\ell_v}^T & \mathbf{\ell_u}^T \\ \mathbf{\ell_q} & \mathbf{\ell_{qq}}
83			* &
84			* \mathbf{\ell_{qv}} & \mathbf{\ell_{uq}}^T \\
85			* \mathbf{\ell_v} & \mathbf{\ell_{vq}} & \mathbf{\ell_{vv}} &
86			* \mathbf{\ell_{uv}}^T \\
87			* \mathbf{\ell_u} & \mathbf{\ell_{uq}} & \mathbf{\ell_{uv}} &
88			* \mathbf{\ell_{uu}}\end{bmatrix} \begin{bmatrix}1 \\ \delta\mathbf{q}
89			* \\ \delta\mathbf{v} \\
90			* \delta\mathbf{u}\end{bmatrix}, &\textrm{(cost)}\\
91			* &\mathbf{g_q}\delta\mathbf{q}+\mathbf{g_v}\delta\mathbf{v}+\mathbf{g_u}\delta\mathbf{u}\leq\mathbf{0},
92			* &\textrm{(inequality constraints)}\\
93			* &\mathbf{h_q}\delta\mathbf{q}+\mathbf{h_v}\delta\mathbf{v}+\mathbf{h_u}\delta\mathbf{u}=\mathbf{0},
94			* &\textrm{(equality constraints)} \end{aligned} \f] where
95			* - \f$\mathbf{f_x}=(\mathbf{f_q};\,\, \mathbf{f_v})\in\mathbb{R}^{nv\times
96			* ndx}\f$ and \f$\mathbf{f_u}\in\mathbb{R}^{nv\times nu}\f$ are the Jacobians
97			* of the dynamics,
98			* - \f$\mathbf{\ell_x}=(\mathbf{\ell_q};\,\,
99			* \mathbf{\ell_v})\in\mathbb{R}^{ndx}\f$ and
100			* \f$\mathbf{\ell_u}\in\mathbb{R}^{nu}\f$ are the Jacobians of the cost
101			* function,
102			* - \f$\mathbf{\ell_{xx}}=(\mathbf{\ell_{qq}}\,\, \mathbf{\ell_{qv}};\,\,
103			* \mathbf{\ell_{vq}}\, \mathbf{\ell_{vv}})\in\mathbb{R}^{ndx\times ndx}\f$,
104			* \f$\mathbf{\ell_{xu}}=(\mathbf{\ell_q};\,\,
105			* \mathbf{\ell_v})\in\mathbb{R}^{ndx\times nu}\f$ and
106			* \f$\mathbf{\ell_{uu}}\in\mathbb{R}^{nu\times nu}\f$ are the Hessians of the
107			* cost function,
108			* - \f$\mathbf{g_x}=(\mathbf{g_q};\,\, \mathbf{g_v})\in\mathbb{R}^{ng\times
109			* ndx}\f$ and \f$\mathbf{g_u}\in\mathbb{R}^{ng\times nu}\f$ are the Jacobians
110			* of the inequality constraints, and
111			* - \f$\mathbf{h_x}=(\mathbf{h_q};\,\, \mathbf{h_v})\in\mathbb{R}^{nh\times
112			* ndx}\f$ and \f$\mathbf{h_u}\in\mathbb{R}^{nh\times nu}\f$ are the Jacobians
113			* of the equality constraints.
114			*
115			* Additionally, it is important to note that `calcDiff()` computes the
116			* derivatives using the latest stored values by `calc()`. Thus, we need to
117			* first run `calc()`.
118			*
119			* \sa `ActionModelAbstractTpl`, `calc()`, `calcDiff()`, `createData()`
120			*/
121			template <typename _Scalar>
122			class DifferentialActionModelAbstractTpl : public DifferentialActionModelBase {
123			public:
124			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
125
126			typedef _Scalar Scalar;
127			typedef typename ScalarSelector<Scalar>::type ScalarType;
128			typedef MathBaseTpl<Scalar> MathBase;
129			typedef DifferentialActionDataAbstractTpl<Scalar>
130			DifferentialActionDataAbstract;
131			typedef StateAbstractTpl<Scalar> StateAbstract;
132			typedef typename MathBase::VectorXs VectorXs;
133			typedef typename MathBase::MatrixXs MatrixXs;
134
135			/**
136			* @brief Initialize the differential action model
137			*
138			* @param[in] state State description
139			* @param[in] nu Dimension of control vector
140			* @param[in] nr Dimension of cost-residual vector
141			* @param[in] ng Number of inequality constraints (default 0)
142			* @param[in] nh Number of equality constraints (default 0)
143			* @param[in] ng_T Number of inequality terminal constraints (default 0)
144			* @param[in] nh_T Number of equality terminal constraints (default 0)
145			*/
146			DifferentialActionModelAbstractTpl(std::shared_ptr<StateAbstract> state,
147			const std::size_t nu,
148			const std::size_t nr = 0,
149			const std::size_t ng = 0,
150			const std::size_t nh = 0,
151			const std::size_t ng_T = 0,
152			const std::size_t nh_T = 0);
153		✗	virtual ~DifferentialActionModelAbstractTpl() = default;
154
155			/**
156			* @brief Compute the system acceleration and cost value
157			*
158			* @param[in] data Differential action data
159			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
160			* @param[in] u Control input \f$\mathbf{u}\in\mathbb{R}^{nu}\f$
161			*/
162			virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
163			const Eigen::Ref<const VectorXs>& x,
164			const Eigen::Ref<const VectorXs>& u) = 0;
165
166			/**
167			* @brief Compute the total cost value for nodes that depends only on the
168			* state
169			*
170			* It updates the total cost and the system acceleration is not updated as the
171			* control input is undefined. This function is used in the terminal nodes of
172			* an optimal control problem.
173			*
174			* @param[in] data Differential action data
175			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
176			*/
177			virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
178			const Eigen::Ref<const VectorXs>& x);
179
180			/**
181			* @brief Compute the derivatives of the dynamics and cost functions
182			*
183			* It computes the partial derivatives of the dynamical system and the cost
184			* function. It assumes that `calc()` has been run first. This function builds
185			* a quadratic approximation of the time-continuous action model (i.e.
186			* dynamical system and cost function).
187			*
188			* @param[in] data Differential action 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			virtual void calcDiff(
193			const std::shared_ptr<DifferentialActionDataAbstract>& data,
194			const Eigen::Ref<const VectorXs>& x,
195			const Eigen::Ref<const VectorXs>& u) = 0;
196
197			/**
198			* @brief Compute the derivatives of the cost functions with respect to the
199			* state only
200			*
201			* It updates the derivatives of the cost function with respect to the state
202			* only. This function is used in the terminal nodes of an optimal control
203			* problem.
204			*
205			* @param[in] data Differential action data
206			* @param[in] x State point \f$\mathbf{x}\in\mathbb{R}^{ndx}\f$
207			*/
208			virtual void calcDiff(
209			const std::shared_ptr<DifferentialActionDataAbstract>& data,
210			const Eigen::Ref<const VectorXs>& x);
211
212			/**
213			* @brief Create the differential action data
214			*
215			* @return the differential action data
216			*/
217			virtual std::shared_ptr<DifferentialActionDataAbstract> createData();
218
219			/**
220			* @brief Checks that a specific data belongs to this model
221			*/
222			virtual bool checkData(
223			const std::shared_ptr<DifferentialActionDataAbstract>& data);
224
225			/**
226			* @brief Computes the quasic static commands
227			*
228			* The quasic static commands are the ones produced for a the reference
229			* posture as an equilibrium point, i.e. for
230			* \f$\mathbf{f}(\mathbf{q},\mathbf{v}=\mathbf{0},\mathbf{u})=\mathbf{0}\f$
231			*
232			* @param[in] data Differential action data
233			* @param[out] u Quasic static commands
234			* @param[in] x State point (velocity has to be zero)
235			* @param[in] maxiter Maximum allowed number of iterations
236			* @param[in] tol Tolerance
237			*/
238			virtual void quasiStatic(
239			const std::shared_ptr<DifferentialActionDataAbstract>& data,
240			Eigen::Ref<VectorXs> u, const Eigen::Ref<const VectorXs>& x,
241			const std::size_t maxiter = 100, const Scalar tol = Scalar(1e-9));
242
243			/**
244			* @copybrief quasicStatic()
245			*
246			* @copydetails quasicStatic()
247			*
248			* @param[in] data Differential action data
249			* @param[in] x State point (velocity has to be zero)
250			* @param[in] maxiter Maximum allowed number of iterations
251			* @param[in] tol Tolerance
252			* @return Quasic static commands
253			*/
254			VectorXs quasiStatic_x(
255			const std::shared_ptr<DifferentialActionDataAbstract>& data,
256			const VectorXs& x, const std::size_t maxiter = 100,
257			const Scalar tol = Scalar(1e-9));
258
259			/**
260			* @brief Return the dimension of the control input
261			*/
262			std::size_t get_nu() const;
263
264			/**
265			* @brief Return the dimension of the cost-residual vector
266			*/
267			std::size_t get_nr() const;
268
269			/**
270			* @brief Return the number of inequality constraints
271			*/
272			virtual std::size_t get_ng() const;
273
274			/**
275			* @brief Return the number of equality constraints
276			*/
277			virtual std::size_t get_nh() const;
278
279			/**
280			* @brief Return the number of inequality terminal constraints
281			*/
282			virtual std::size_t get_ng_T() const;
283
284			/**
285			* @brief Return the number of equality terminal constraints
286			*/
287			virtual std::size_t get_nh_T() const;
288
289			/**
290			* @brief Return the state
291			*/
292			const std::shared_ptr<StateAbstract>& get_state() const;
293
294			/**
295			* @brief Return the lower bound of the inequality constraints
296			*/
297			virtual const VectorXs& get_g_lb() const;
298
299			/**
300			* @brief Return the upper bound of the inequality constraints
301			*/
302			virtual const VectorXs& get_g_ub() const;
303
304			/**
305			* @brief Return the control lower bound
306			*/
307			const VectorXs& get_u_lb() const;
308
309			/**
310			* @brief Return the control upper bound
311			*/
312			const VectorXs& get_u_ub() const;
313
314			/**
315			* @brief Indicates if there are defined control limits
316			*/
317			bool get_has_control_limits() const;
318
319			/**
320			* @brief Modify the lower bound of the inequality constraints
321			*/
322			void set_g_lb(const VectorXs& g_lb);
323
324			/**
325			* @brief Modify the upper bound of the inequality constraints
326			*/
327			void set_g_ub(const VectorXs& g_ub);
328
329			/**
330			* @brief Modify the control lower bounds
331			*/
332			void set_u_lb(const VectorXs& u_lb);
333
334			/**
335			* @brief Modify the control upper bounds
336			*/
337			void set_u_ub(const VectorXs& u_ub);
338
339			/**
340			* @brief Print information on the differential action model
341			*/
342			template <class Scalar>
343			friend std::ostream& operator<<(
344			std::ostream& os,
345			const DifferentialActionModelAbstractTpl<Scalar>& model);
346
347			/**
348			* @brief Print relevant information of the differential action model
349			*
350			* @param[out] os Output stream object
351			*/
352			virtual void print(std::ostream& os) const;
353
354			private:
355			std::size_t ng_internal_; //!< Internal object for storing the number of
356			//!< inequality constraints
357			std::size_t nh_internal_; //!< Internal object for storing the number of
358			//!< equality constraints
359
360			protected:
361			std::size_t nu_; //!< Control dimension
362			std::size_t nr_; //!< Dimension of the cost residual
363			std::size_t ng_; //!< Number of inequality constraints
364			std::size_t nh_; //!< Number of equality constraints
365			std::size_t ng_T_; //!< Number of inequality terminal constraints
366			std::size_t nh_T_; //!< Number of equality terminal constraints
367			std::shared_ptr<StateAbstract> state_; //!< Model of the state
368			VectorXs unone_; //!< Neutral state
369			VectorXs g_lb_; //!< Lower bound of the inequality constraints
370			VectorXs g_ub_; //!< Lower bound of the inequality constraints
371			VectorXs u_lb_; //!< Lower control limits
372			VectorXs u_ub_; //!< Upper control limits
373			bool has_control_limits_; //!< Indicates whether any of the control limits is
374			//!< finite
375		✗	DifferentialActionModelAbstractTpl()
376		✗	: nu_(0), nr_(0), ng_(0), nh_(0), ng_T_(0), nh_T_(0), state_(nullptr) {}
377
378			/**
379			* @brief Update the status of the control limits (i.e. if there are defined
380			* limits)
381			*/
382			void update_has_control_limits();
383
384			template <class Scalar>
385			friend class IntegratedActionModelAbstractTpl;
386			template <class Scalar>
387			friend class ConstraintModelManagerTpl;
388			};
389
390			template <typename _Scalar>
391			struct DifferentialActionDataAbstractTpl {
392			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
393
394			typedef _Scalar Scalar;
395			typedef MathBaseTpl<Scalar> MathBase;
396			typedef typename MathBase::VectorXs VectorXs;
397			typedef typename MathBase::MatrixXs MatrixXs;
398
399			template <template <typename Scalar> class Model>
400		✗	explicit DifferentialActionDataAbstractTpl(Model<Scalar>* const model)
401		✗	: cost(Scalar(0.)),
402		✗	xout(model->get_state()->get_nv()),
403		✗	Fx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
404		✗	Fu(model->get_state()->get_nv(), model->get_nu()),
405		✗	r(model->get_nr()),
406		✗	Lx(model->get_state()->get_ndx()),
407		✗	Lu(model->get_nu()),
408		✗	Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
409		✗	Lxu(model->get_state()->get_ndx(), model->get_nu()),
410		✗	Luu(model->get_nu(), model->get_nu()),
411		✗	g(model->get_ng() > model->get_ng_T() ? model->get_ng()
412		✗	: model->get_ng_T()),
413		✗	Gx(model->get_ng() > model->get_ng_T() ? model->get_ng()
414		✗	: model->get_ng_T(),
415		✗	model->get_state()->get_ndx()),
416		✗	Gu(model->get_ng() > model->get_ng_T() ? model->get_ng()
417		✗	: model->get_ng_T(),
418		✗	model->get_nu()),
419		✗	h(model->get_nh() > model->get_nh_T() ? model->get_nh()
420		✗	: model->get_nh_T()),
421		✗	Hx(model->get_nh() > model->get_nh_T() ? model->get_nh()
422		✗	: model->get_nh_T(),
423		✗	model->get_state()->get_ndx()),
424		✗	Hu(model->get_nh() > model->get_nh_T() ? model->get_nh()
425		✗	: model->get_nh_T(),
426		✗	model->get_nu()) {
427		✗	xout.setZero();
428		✗	Fx.setZero();
429		✗	Fu.setZero();
430		✗	r.setZero();
431		✗	Lx.setZero();
432		✗	Lu.setZero();
433		✗	Lxx.setZero();
434		✗	Lxu.setZero();
435		✗	Luu.setZero();
436		✗	g.setZero();
437		✗	Gx.setZero();
438		✗	Gu.setZero();
439		✗	h.setZero();
440		✗	Hx.setZero();
441		✗	Hu.setZero();
442		✗	}
443		✗	virtual ~DifferentialActionDataAbstractTpl() = default;
444
445			Scalar cost; //!< cost value
446			VectorXs xout; //!< evolution state
447			MatrixXs Fx; //!< Jacobian of the dynamics w.r.t. the state \f$\mathbf{x}\f$
448			MatrixXs
449			Fu; //!< Jacobian of the dynamics w.r.t. the control \f$\mathbf{u}\f$
450			VectorXs r; //!< Cost residual
451			VectorXs Lx; //!< Jacobian of the cost w.r.t. the state \f$\mathbf{x}\f$
452			VectorXs Lu; //!< Jacobian of the cost w.r.t. the control \f$\mathbf{u}\f$
453			MatrixXs Lxx; //!< Hessian of the cost w.r.t. the state \f$\mathbf{x}\f$
454			MatrixXs Lxu; //!< Hessian of the cost w.r.t. the state \f$\mathbf{x}\f$ and
455			//!< control u
456			MatrixXs Luu; //!< Hessian of the cost w.r.t. the control \f$\mathbf{u}\f$
457			VectorXs g; //!< Inequality constraint values
458			MatrixXs Gx; //!< Jacobian of the inequality constraint w.r.t. the state
459			//!< \f$\mathbf{x}\f$
460			MatrixXs Gu; //!< Jacobian of the inequality constraint w.r.t. the control
461			//!< \f$\mathbf{u}\f$
462			VectorXs h; //!< Equality constraint values
463			MatrixXs Hx; //!< Jacobian of the equality constraint w.r.t. the state
464			//!< \f$\mathbf{x}\f$
465			MatrixXs Hu; //!< Jacobian of the equality constraint w.r.t the control
466			//!< \f$\mathbf{u}\f$
467			};
468
469			} // namespace crocoddyl
470
471			/* --- Details -------------------------------------------------------------- */
472			/* --- Details -------------------------------------------------------------- */
473			/* --- Details -------------------------------------------------------------- */
474			#include "crocoddyl/core/diff-action-base.hxx"
475
476			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
477			crocoddyl::DifferentialActionModelAbstractTpl)
478			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
479			crocoddyl::DifferentialActionDataAbstractTpl)
480
481			#endif // CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
482