GCC Code Coverage Report

Directory: ./
File: include/crocoddyl/core/actions/diff-lqr.hpp
Date: 2025-06-03 08:14:12
Exec Total Coverage
Lines: 0 36 0.0%
Functions: 0 48 0.0%
Branches: 0 78 0.0%
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_DIFF_LQR_HPP_
11 #define CROCODDYL_CORE_ACTIONS_DIFF_LQR_HPP_
12
13 #include "crocoddyl/core/diff-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) differential action model
21 *
22 * A linear-quadratic regulator (LQR) action has a transition model of the form
23 * \f[ \begin{equation}
24 * \mathbf{\dot{v}} = \mathbf{A_q q + A_v v + 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 DifferentialActionModelLQRTpl
46 : public DifferentialActionModelAbstractTpl<_Scalar> {
47 public:
48 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
49 ✗ CROCODDYL_DERIVED_CAST(DifferentialActionModelBase,
50 DifferentialActionModelLQRTpl)
51
52 typedef _Scalar Scalar;
53 typedef MathBaseTpl<Scalar> MathBase;
54 typedef DifferentialActionModelAbstractTpl<Scalar> Base;
55 typedef DifferentialActionDataLQRTpl<Scalar> Data;
56 typedef StateVectorTpl<Scalar> StateVector;
57 typedef DifferentialActionDataAbstractTpl<Scalar>
58 DifferentialActionDataAbstract;
59 typedef typename MathBase::VectorXs VectorXs;
60 typedef typename MathBase::MatrixXs MatrixXs;
61
62 /**
63 * @brief Initialize the LQR action model
64 *
65 * @param[in] Aq Position matrix
66 * @param[in] Av Velocity matrix
67 * @param[in] B Input matrix
68 * @param[in] Q State weight matrix
69 * @param[in] R Input weight matrix
70 * @param[in] N State-input weight matrix
71 */
72 DifferentialActionModelLQRTpl(const MatrixXs& Aq, const MatrixXs& Av,
73 const MatrixXs& B, const MatrixXs& Q,
74 const MatrixXs& R, const MatrixXs& N);
75
76 /**
77 * @brief Initialize the LQR action model
78 *
79 * @param[in] Aq Position matrix
80 * @param[in] Av Velocity matrix
81 * @param[in] B Input matrix
82 * @param[in] Q State weight matrix
83 * @param[in] R Input weight matrix
84 * @param[in] N State-input weight matrix
85 * @param[in] f Dynamics drift
86 * @param[in] q State weight vector
87 * @param[in] r Input weight vector
88 */
89 DifferentialActionModelLQRTpl(const MatrixXs& Aq, const MatrixXs& Av,
90 const MatrixXs& B, const MatrixXs& Q,
91 const MatrixXs& R, const MatrixXs& N,
92 const VectorXs& f, const VectorXs& q,
93 const VectorXs& r);
94
95 /**
96 * @brief Initialize the LQR action model
97 *
98 * @param[in] Aq Position matrix
99 * @param[in] Av Velocity matrix
100 * @param[in] B Input matrix
101 * @param[in] Q State weight matrix
102 * @param[in] R Input weight matrix
103 * @param[in] N State-input weight matrix
104 * @param[in] G State-input inequality constraint matrix
105 * @param[in] H State-input equality constraint matrix
106 * @param[in] f Dynamics drift
107 * @param[in] q State weight vector
108 * @param[in] r Input weight vector
109 * @param[in] g State-input inequality constraint bias
110 * @param[in] h State-input equality constraint bias
111 */
112 DifferentialActionModelLQRTpl(const MatrixXs& Aq, const MatrixXs& Av,
113 const MatrixXs& B, const MatrixXs& Q,
114 const MatrixXs& R, const MatrixXs& N,
115 const MatrixXs& G, const MatrixXs& H,
116 const VectorXs& f, const VectorXs& q,
117 const VectorXs& r, const VectorXs& g,
118 const VectorXs& h);
119
120 /**
121 * @brief Initialize the LQR action model
122 *
123 * @param[in] nq Dimension of position vector
124 * @param[in] nu Dimension of control vector
125 * @param[in] drif_free Enable / disable the bias term of the linear dynamics
126 * (default true)
127 */
128 DifferentialActionModelLQRTpl(const std::size_t nq, const std::size_t nu,
129 const bool drift_free = true);
130
131 /** @brief Copy constructor */
132 DifferentialActionModelLQRTpl(const DifferentialActionModelLQRTpl& copy);
133
134 ✗ virtual ~DifferentialActionModelLQRTpl() = default;
135
136 virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
137 const Eigen::Ref<const VectorXs>& x,
138 const Eigen::Ref<const VectorXs>& u) override;
139 virtual void calc(const std::shared_ptr<DifferentialActionDataAbstract>& data,
140 const Eigen::Ref<const VectorXs>& x) override;
141 virtual void calcDiff(
142 const std::shared_ptr<DifferentialActionDataAbstract>& data,
143 const Eigen::Ref<const VectorXs>& x,
144 const Eigen::Ref<const VectorXs>& u) override;
145 virtual void calcDiff(
146 const std::shared_ptr<DifferentialActionDataAbstract>& data,
147 const Eigen::Ref<const VectorXs>& x) override;
148 virtual std::shared_ptr<DifferentialActionDataAbstract> createData() override;
149
150 /**
151 * @brief Cast the differential-LQR model to a different scalar type.
152 *
153 * It is useful for operations requiring different precision or scalar types.
154 *
155 * @tparam NewScalar The new scalar type to cast to.
156 * @return DifferentialActionModelLQRTpl<NewScalar> A differential-action
157 * model with the new scalar type.
158 */
159 template <typename NewScalar>
160 DifferentialActionModelLQRTpl<NewScalar> cast() const;
161
162 virtual bool checkData(
163 const std::shared_ptr<DifferentialActionDataAbstract>& data) override;
164
165 /**
166 * @brief Create a random LQR model
167 *
168 * @param[in] nq Position dimension
169 * @param[in] nu Control dimension
170 * @param[in] ng Inequality constraint dimension (default 0)
171 * @param[in] nh Equality constraint dimension (default 0)
172 */
173 static DifferentialActionModelLQRTpl Random(const std::size_t nq,
174 const std::size_t nu,
175 const std::size_t ng = 0,
176 const std::size_t nh = 0);
177
178 /** @brief Return the position matrix */
179 const MatrixXs& get_Aq() const;
180
181 /** @brief Return the velocity matrix */
182 const MatrixXs& get_Av() const;
183
184 /** @brief Return the input matrix */
185 const MatrixXs& get_B() const;
186
187 /** @brief Return the dynamics drift */
188 const VectorXs& get_f() const;
189
190 /** @brief Return the state weight matrix */
191 const MatrixXs& get_Q() const;
192
193 /** @brief Return the input weight matrix */
194 const MatrixXs& get_R() const;
195
196 /** @brief Return the state-input weight matrix */
197 const MatrixXs& get_N() const;
198
199 /** @brief Return the state-input inequality constraint matrix */
200 const MatrixXs& get_G() const;
201
202 /** @brief Return the state-input equality constraint matrix */
203 const MatrixXs& get_H() const;
204
205 /** @brief Return the state weight vector */
206 const VectorXs& get_q() const;
207
208 /** @brief Return the input weight vector */
209 const VectorXs& get_r() const;
210
211 /** @brief Return the state-input inequality constraint bias */
212 const VectorXs& get_g() const;
213
214 /** @brief Return the state-input equality constraint bias */
215 const VectorXs& get_h() const;
216
217 /**
218 * @brief Modify the LQR action model
219 *
220 * @param[in] Aq Position matrix
221 * @param[in] Av Velocity matrix
222 * @param[in] B Input matrix
223 * @param[in] Q State weight matrix
224 * @param[in] R Input weight matrix
225 * @param[in] N State-input weight matrix
226 * @param[in] G State-input inequality constraint matrix
227 * @param[in] H State-input equality constraint matrix
228 * @param[in] f Dynamics drift
229 * @param[in] q State weight vector
230 * @param[in] r Input weight vector
231 * @param[in] g State-input inequality constraint bias
232 * @param[in] h State-input equality constraint bias
233 */
234 void set_LQR(const MatrixXs& Aq, const MatrixXs& Av, const MatrixXs& B,
235 const MatrixXs& Q, const MatrixXs& R, const MatrixXs& N,
236 const MatrixXs& G, const MatrixXs& H, const VectorXs& f,
237 const VectorXs& q, const VectorXs& r, const VectorXs& g,
238 const VectorXs& h);
239
240 ✗ DEPRECATED("Use get_Aq", const MatrixXs& get_Fq() const { return get_Aq(); })
241 ✗ DEPRECATED("Use get_Av", const MatrixXs& get_Fv() const { return get_Av(); })
242 ✗ DEPRECATED("Use get_B", const MatrixXs& get_Fu() const { return get_B(); })
243 ✗ DEPRECATED("Use get_f", const VectorXs& get_f0() const { return get_f(); })
244 ✗ DEPRECATED("Use get_q", const VectorXs& get_lx() const { return get_q(); })
245 ✗ DEPRECATED("Use get_r", const VectorXs& get_lu() const { return get_r(); })
246 ✗ DEPRECATED("Use get_Q", const MatrixXs& get_Lxx() const { return get_Q(); })
247 ✗ DEPRECATED("Use get_N", const MatrixXs& get_Lxu() const { return get_N(); })
248 ✗ DEPRECATED("Use get_R", const MatrixXs& get_Luu() const { return get_R(); })
249 ✗ DEPRECATED(
250 "Use set_LQR", void set_Fq(const MatrixXs& Aq) {
251 set_LQR(Aq, Av_, B_, Q_, R_, N_, G_, H_, f_, q_, r_, g_, h_);
252 })
253 ✗ DEPRECATED(
254 "Use set_LQR", void set_Fv(const MatrixXs& Av) {
255 set_LQR(Aq_, Av, B_, Q_, R_, N_, G_, H_, f_, q_, r_, g_, h_);
256 })
257 ✗ DEPRECATED(
258 "Use set_LQR", void set_Fu(const MatrixXs& B) {
259 set_LQR(Aq_, Av_, B, Q_, R_, N_, G_, H_, f_, q_, r_, g_, h_);
260 })
261 ✗ DEPRECATED(
262 "Use set_LQR", void set_f0(const VectorXs& f) {
263 set_LQR(Aq_, Av_, B_, Q_, R_, N_, G_, H_, f, q_, r_, g_, h_);
264 })
265 ✗ DEPRECATED(
266 "Use set_LQR", void set_lx(const VectorXs& q) {
267 set_LQR(Aq_, Av_, B_, Q_, R_, N_, G_, H_, f_, q, r_, g_, h_);
268 })
269 ✗ DEPRECATED(
270 "Use set_LQR", void set_lu(const VectorXs& r) {
271 set_LQR(Aq_, Av_, B_, Q_, R_, N_, G_, H_, f_, q_, r, g_, h_);
272 })
273 ✗ DEPRECATED(
274 "Use set_LQR", void set_Lxx(const MatrixXs& Q) {
275 set_LQR(Aq_, Av_, B_, Q, R_, N_, G_, H_, f_, q_, r_, g_, h_);
276 })
277 ✗ DEPRECATED(
278 "Use set_LQR", void set_Lxu(const MatrixXs& N) {
279 set_LQR(Aq_, Av_, B_, Q_, R_, N, G_, H_, f_, q_, r_, g_, h_);
280 })
281 ✗ DEPRECATED(
282 "Use set_LQR", void set_Luu(const MatrixXs& R) {
283 set_LQR(Aq_, Av_, B_, Q_, R, N_, G_, H_, f_, q_, r_, g_, h_);
284 })
285
286 /**
287 * @brief Print relevant information of the LQR model
288 *
289 * @param[out] os Output stream object
290 */
291 virtual void print(std::ostream& os) const override;
292
293 protected:
294 using Base::ng_; //!< Equality constraint dimension
295 using Base::nh_; //!< Inequality constraint dimension
296 using Base::nu_; //!< Control dimension
297 using Base::state_; //!< Model of the state
298
299 private:
300 MatrixXs Aq_;
301 MatrixXs Av_;
302 MatrixXs B_;
303 MatrixXs Q_;
304 MatrixXs R_;
305 MatrixXs N_;
306 MatrixXs G_;
307 MatrixXs H_;
308 VectorXs f_;
309 VectorXs q_;
310 VectorXs r_;
311 VectorXs g_;
312 VectorXs h_;
313 MatrixXs L_;
314 bool drift_free_;
315 bool updated_lqr_;
316 };
317
318 template <typename _Scalar>
319 struct DifferentialActionDataLQRTpl
320 : public DifferentialActionDataAbstractTpl<_Scalar> {
321 typedef _Scalar Scalar;
322 typedef MathBaseTpl<Scalar> MathBase;
323 typedef DifferentialActionDataAbstractTpl<Scalar> Base;
324 typedef typename MathBase::VectorXs VectorXs;
325 typedef typename MathBase::MatrixXs MatrixXs;
326
327 template <template <typename Scalar> class Model>
328 ✗ explicit DifferentialActionDataLQRTpl(Model<Scalar>* const model)
329 ✗ : Base(model) {
330 // Setting the linear model and quadratic cost as they are constant
331 ✗ const std::size_t nq = model->get_state()->get_nq();
332 ✗ const std::size_t nu = model->get_nu();
333 ✗ Fx.leftCols(nq) = model->get_Aq();
334 ✗ Fx.rightCols(nq) = model->get_Av();
335 ✗ Fu = model->get_B();
336 ✗ Lxx = model->get_Q();
337 ✗ Luu = model->get_R();
338 ✗ Lxu = model->get_N();
339 ✗ Gx = model->get_G().leftCols(2 * nq);
340 ✗ Gu = model->get_G().rightCols(nu);
341 ✗ Hx = model->get_H().leftCols(2 * nq);
342 ✗ Hu = model->get_H().rightCols(nu);
343 ✗ }
344 ✗ virtual ~DifferentialActionDataLQRTpl() = default;
345
346 using Base::cost;
347 using Base::Fu;
348 using Base::Fx;
349 using Base::Gu;
350 using Base::Gx;
351 using Base::Hu;
352 using Base::Hx;
353 using Base::Lu;
354 using Base::Luu;
355 using Base::Lx;
356 using Base::Lxu;
357 using Base::Lxx;
358 using Base::r;
359 using Base::xout;
360 };
361
362 } // namespace crocoddyl
363
364 /* --- Details -------------------------------------------------------------- */
365 /* --- Details -------------------------------------------------------------- */
366 /* --- Details -------------------------------------------------------------- */
367 #include "crocoddyl/core/actions/diff-lqr.hxx"
368
369 CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
370 crocoddyl::DifferentialActionModelLQRTpl)
371 CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
372 crocoddyl::DifferentialActionDataLQRTpl)
373
374 #endif // CROCODDYL_CORE_ACTIONS_DIFF_LQR_HPP_
375