GCC Code Coverage Report

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