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			// Heriot-Watt University
6			// Copyright note valid unless otherwise stated in individual files.
7			// All rights reserved.
8			///////////////////////////////////////////////////////////////////////////////
9
10			#ifndef CROCODDYL_CORE_ACTIONS_LQR_HPP_
11			#define CROCODDYL_CORE_ACTIONS_LQR_HPP_
12
13			#include "crocoddyl/core/action-base.hpp"
14			#include "crocoddyl/core/fwd.hpp"
15			#include "crocoddyl/core/states/euclidean.hpp"
16
17			namespace crocoddyl {
18
19			/**
20			* @brief Linear-quadratic regulator (LQR) action model
21			*
22			* A linear-quadratic regulator (LQR) action has a transition model of the form
23			* \f[ \begin{equation}
24			* \mathbf{x}^' = \mathbf{A x + B u + f}.
25			* \end{equation} \f]
26			* Its cost function is quadratic of the form:
27			* \f[ \begin{equation}
28			* \ell(\mathbf{x},\mathbf{u}) = \begin{bmatrix}1
29			* \\ \mathbf{x} \\ \mathbf{u}\end{bmatrix}^T \begin{bmatrix}0 &
30			* \mathbf{q}^T & \mathbf{r}^T \\ \mathbf{q} & \mathbf{Q}
31			* &
32			* \mathbf{N}^T \\
33			* \mathbf{r} & \mathbf{N} & \mathbf{R}\end{bmatrix}
34			* \begin{bmatrix}1 \\ \mathbf{x} \\
35			* \mathbf{u}\end{bmatrix}
36			* \end{equation} \f]
37			* and the linear equality and inequality constraints has the form:
38			* \f[ \begin{aligned}
39			* \mathbf{g(x,u)} = \mathbf{G}\begin{bmatrix} \mathbf{x} \\ \mathbf{u}
40			* \end{bmatrix} [x,u] + \mathbf{g} \leq \mathbf{0}
41			* &\mathbf{h(x,u)} = \mathbf{H}\begin{bmatrix} \mathbf{x} \\ \mathbf{u}
42			* \end{bmatrix} [x,u] + \mathbf{h} \end{aligned} \f]
43			*/
44			template <typename _Scalar>
45			class ActionModelLQRTpl : public ActionModelAbstractTpl<_Scalar> {
46			public:
47			EIGEN_MAKE_ALIGNED_OPERATOR_NEW
48		✗	CROCODDYL_DERIVED_CAST(ActionModelBase, ActionModelLQRTpl)
49
50			typedef _Scalar Scalar;
51			typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
52			typedef ActionModelAbstractTpl<Scalar> Base;
53			typedef ActionDataLQRTpl<Scalar> Data;
54			typedef StateVectorTpl<Scalar> StateVector;
55			typedef MathBaseTpl<Scalar> MathBase;
56			typedef typename MathBase::VectorXs VectorXs;
57			typedef typename MathBase::MatrixXs MatrixXs;
58
59			/**
60			* @brief Initialize the LQR action model
61			*
62			* @param[in] A State matrix
63			* @param[in] B Input matrix
64			* @param[in] Q State weight matrix
65			* @param[in] R Input weight matrix
66			* @param[in] N State-input weight matrix
67			*/
68			ActionModelLQRTpl(const MatrixXs& A, const MatrixXs& B, const MatrixXs& Q,
69			const MatrixXs& R, const MatrixXs& N);
70
71			/**
72			* @brief Initialize the LQR action model
73			*
74			* @param[in] A State matrix
75			* @param[in] B Input matrix
76			* @param[in] Q State weight matrix
77			* @param[in] R Input weight matrix
78			* @param[in] N State-input weight matrix
79			* @param[in] f Dynamics drift
80			* @param[in] q State weight vector
81			* @param[in] r Input weight vector
82			*/
83			ActionModelLQRTpl(const MatrixXs& A, const MatrixXs& B, const MatrixXs& Q,
84			const MatrixXs& R, const MatrixXs& N, const VectorXs& f,
85			const VectorXs& q, const VectorXs& r);
86
87			/**
88			* @brief Initialize the LQR action model
89			*
90			* @param[in] A State matrix
91			* @param[in] B Input matrix
92			* @param[in] Q State weight matrix
93			* @param[in] R Input weight matrix
94			* @param[in] N State-input weight matrix
95			* @param[in] G State-input inequality constraint matrix
96			* @param[in] H State-input equality constraint matrix
97			* @param[in] f Dynamics drift
98			* @param[in] q State weight vector
99			* @param[in] r Input weight vector
100			* @param[in] g State-input inequality constraint bias
101			* @param[in] h State-input equality constraint bias
102			*/
103			ActionModelLQRTpl(const MatrixXs& A, const MatrixXs& B, const MatrixXs& Q,
104			const MatrixXs& R, const MatrixXs& N, const MatrixXs& G,
105			const MatrixXs& H, const VectorXs& f, const VectorXs& q,
106			const VectorXs& r, const VectorXs& g, const VectorXs& h);
107
108			/**
109			* @brief Initialize the LQR action model
110			*
111			* @param[in] nx Dimension of state vector
112			* @param[in] nu Dimension of control vector
113			* @param[in] drif_free Enable / disable the bias term of the linear dynamics
114			* (default true)
115			*/
116			ActionModelLQRTpl(const std::size_t nx, const std::size_t nu,
117			const bool drift_free = true);
118
119			/** @brief Copy constructor */
120			ActionModelLQRTpl(const ActionModelLQRTpl& copy);
121
122		✗	virtual ~ActionModelLQRTpl() = default;
123
124			virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
125			const Eigen::Ref<const VectorXs>& x,
126			const Eigen::Ref<const VectorXs>& u) override;
127			virtual void calc(const std::shared_ptr<ActionDataAbstract>& data,
128			const Eigen::Ref<const VectorXs>& x) override;
129			virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
130			const Eigen::Ref<const VectorXs>& x,
131			const Eigen::Ref<const VectorXs>& u) override;
132			virtual void calcDiff(const std::shared_ptr<ActionDataAbstract>& data,
133			const Eigen::Ref<const VectorXs>& x) override;
134			virtual std::shared_ptr<ActionDataAbstract> createData() override;
135
136			/**
137			* @brief Cast the LQR model to a different scalar type.
138			*
139			* It is useful for operations requiring different precision or scalar types.
140			*
141			* @tparam NewScalar The new scalar type to cast to.
142			* @return ActionModelLQRTpl<NewScalar> A action model with the
143			* new scalar type.
144			*/
145			template <typename NewScalar>
146			ActionModelLQRTpl<NewScalar> cast() const;
147
148			virtual bool checkData(
149			const std::shared_ptr<ActionDataAbstract>& data) override;
150
151			/**
152			* @brief Create a random LQR model
153			*
154			* @param[in] nx State dimension
155			* @param[in] nu Control dimension
156			* @param[in] ng Inequality constraint dimension (default 0)
157			* @param[in] nh Equality constraint dimension (defaul 0)
158			*/
159			static ActionModelLQRTpl Random(const std::size_t nx, const std::size_t nu,
160			const std::size_t ng = 0,
161			const std::size_t nh = 0);
162
163			/** @brief Return the state matrix */
164			const MatrixXs& get_A() const;
165
166			/** @brief Return the input matrix */
167			const MatrixXs& get_B() const;
168
169			/** @brief Return the dynamics drift */
170			const VectorXs& get_f() const;
171
172			/** @brief Return the state weight matrix */
173			const MatrixXs& get_Q() const;
174
175			/** @brief Return the input weight matrix */
176			const MatrixXs& get_R() const;
177
178			/** @brief Return the state-input weight matrix */
179			const MatrixXs& get_N() const;
180
181			/** @brief Return the state-input inequality constraint matrix */
182			const MatrixXs& get_G() const;
183
184			/** @brief Return the state-input equality constraint matrix */
185			const MatrixXs& get_H() const;
186
187			/** @brief Return the state weight vector */
188			const VectorXs& get_q() const;
189
190			/** @brief Return the input weight vector */
191			const VectorXs& get_r() const;
192
193			/** @brief Return the state-input inequality constraint bias */
194			const VectorXs& get_g() const;
195
196			/** @brief Return the state-input equality constraint bias */
197			const VectorXs& get_h() const;
198
199			/**
200			* @brief Modify the LQR action model
201			*
202			* @param[in] A State matrix
203			* @param[in] B Input matrix
204			* @param[in] Q State weight matrix
205			* @param[in] R Input weight matrix
206			* @param[in] N State-input weight matrix
207			* @param[in] G State-input inequality constraint matrix
208			* @param[in] H State-input equality constraint matrix
209			* @param[in] f Dynamics drift
210			* @param[in] q State weight vector
211			* @param[in] r Input weight vector
212			* @param[in] g State-input inequality constraint bias
213			* @param[in] h State-input equality constraint bias
214			*/
215			void set_LQR(const MatrixXs& A, const MatrixXs& B, const MatrixXs& Q,
216			const MatrixXs& R, const MatrixXs& N, const MatrixXs& G,
217			const MatrixXs& H, const VectorXs& f, const VectorXs& q,
218			const VectorXs& r, const VectorXs& g, const VectorXs& h);
219
220		✗	DEPRECATED("Use get_A", const MatrixXs& get_Fx() const { return get_A(); })
221		✗	DEPRECATED("Use get_B", const MatrixXs& get_Fu() const { return get_B(); })
222		✗	DEPRECATED("Use get_f", const VectorXs& get_f0() const { return get_f(); })
223		✗	DEPRECATED("Use get_q", const VectorXs& get_lx() const { return get_q(); })
224		✗	DEPRECATED("Use get_r", const VectorXs& get_lu() const { return get_r(); })
225		✗	DEPRECATED("Use get_Q", const MatrixXs& get_Lxx() const { return get_Q(); })
226		✗	DEPRECATED("Use get_R", const MatrixXs& get_Lxu() const { return get_R(); })
227		✗	DEPRECATED("Use get_N", const MatrixXs& get_Luu() const { return get_N(); })
228		✗	DEPRECATED(
229			"Use set_LQR", void set_Fx(const MatrixXs& A) {
230			set_LQR(A, B_, Q_, R_, N_, G_, H_, f_, q_, r_, g_, h_);
231			})
232		✗	DEPRECATED(
233			"Use set_LQR", void set_Fu(const MatrixXs& B) {
234			set_LQR(A_, B, Q_, R_, N_, G_, H_, f_, q_, r_, g_, h_);
235			})
236		✗	DEPRECATED(
237			"Use set_LQR", void set_f0(const VectorXs& f) {
238			set_LQR(A_, B_, Q_, R_, N_, G_, H_, f, q_, r_, g_, h_);
239			})
240		✗	DEPRECATED(
241			"Use set_LQR", void set_lx(const VectorXs& q) {
242			set_LQR(A_, B_, Q_, R_, N_, G_, H_, f_, q, r_, g_, h_);
243			})
244		✗	DEPRECATED(
245			"Use set_LQR", void set_lu(const VectorXs& r) {
246			set_LQR(A_, B_, Q_, R_, N_, G_, H_, f_, q_, r, g_, h_);
247			})
248		✗	DEPRECATED(
249			"Use set_LQR", void set_Lxx(const MatrixXs& Q) {
250			set_LQR(A_, B_, Q, R_, N_, G_, H_, f_, q_, r_, g_, h_);
251			})
252		✗	DEPRECATED(
253			"Use set_LQR", void set_Luu(const MatrixXs& R) {
254			set_LQR(A_, B_, Q_, R, N_, G_, H_, f_, q_, r_, g_, h_);
255			})
256		✗	DEPRECATED(
257			"Use set_LQR", void set_Lxu(const MatrixXs& N) {
258			set_LQR(A_, B_, Q_, R_, N, G_, H_, f_, q_, r_, g_, h_);
259			})
260
261			/**
262			* @brief Print relevant information of the LQR model
263			*
264			* @param[out] os Output stream object
265			*/
266			virtual void print(std::ostream& os) const override;
267
268			protected:
269			using Base::ng_; //!< Equality constraint dimension
270			using Base::nh_; //!< Inequality constraint dimension
271			using Base::nu_; //!< Control dimension
272			using Base::state_; //!< Model of the state
273
274			private:
275			MatrixXs A_;
276			MatrixXs B_;
277			MatrixXs Q_;
278			MatrixXs R_;
279			MatrixXs N_;
280			MatrixXs G_;
281			MatrixXs H_;
282			VectorXs f_;
283			VectorXs q_;
284			VectorXs r_;
285			VectorXs g_;
286			VectorXs h_;
287			MatrixXs L_;
288			bool drift_free_;
289			bool updated_lqr_;
290			};
291
292			template <typename _Scalar>
293			struct ActionDataLQRTpl : public ActionDataAbstractTpl<_Scalar> {
294			typedef _Scalar Scalar;
295			typedef MathBaseTpl<Scalar> MathBase;
296			typedef ActionDataAbstractTpl<Scalar> Base;
297			typedef typename MathBase::VectorXs VectorXs;
298
299			template <template <typename Scalar> class Model>
300		✗	explicit ActionDataLQRTpl(Model<Scalar>* const model)
301			: Base(model),
302		✗	R_u_tmp(VectorXs::Zero(static_cast<Eigen::Index>(model->get_nu()))),
303		✗	Q_x_tmp(VectorXs::Zero(
304		✗	static_cast<Eigen::Index>(model->get_state()->get_ndx()))) {
305			// Setting the linear model and quadratic cost as they are constant
306		✗	const std::size_t nq = model->get_state()->get_nq();
307		✗	const std::size_t nu = model->get_nu();
308		✗	Fx = model->get_A();
309		✗	Fu = model->get_B();
310		✗	Lxx = model->get_Q();
311		✗	Luu = model->get_R();
312		✗	Lxu = model->get_N();
313		✗	Gx = model->get_G().leftCols(2 * nq);
314		✗	Gu = model->get_G().rightCols(nu);
315		✗	Hx = model->get_H().leftCols(2 * nq);
316		✗	Hu = model->get_H().rightCols(nu);
317		✗	}
318		✗	virtual ~ActionDataLQRTpl() = default;
319
320			using Base::cost;
321			using Base::Fu;
322			using Base::Fx;
323			using Base::Gu;
324			using Base::Gx;
325			using Base::Hu;
326			using Base::Hx;
327			using Base::Lu;
328			using Base::Luu;
329			using Base::Lx;
330			using Base::Lxu;
331			using Base::Lxx;
332			using Base::r;
333			using Base::xnext;
334
335			VectorXs R_u_tmp; // Temporary variable for storing Hessian-vector product
336			// (size: nu)
337			VectorXs Q_x_tmp; // Temporary variable for storing Hessian-vector product
338			// (size: nx)
339			};
340
341			} // namespace crocoddyl
342
343			/* --- Details -------------------------------------------------------------- */
344			/* --- Details -------------------------------------------------------------- */
345			/* --- Details -------------------------------------------------------------- */
346			#include "crocoddyl/core/actions/lqr.hxx"
347
348			CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ActionModelLQRTpl)
349			CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ActionDataLQRTpl)
350
351			#endif // CROCODDYL_CORE_ACTIONS_LQR_HPP_
352