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