Crocoddyl
 
Loading...
Searching...
No Matches
lqr.hpp
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.
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
17namespace crocoddyl {
18
44template <typename _Scalar>
46 public:
47 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
48 CROCODDYL_DERIVED_CAST(ActionModelBase, ActionModelLQRTpl)
49
50 typedef _Scalar Scalar;
56 typedef typename MathBase::VectorXs VectorXs;
57 typedef typename MathBase::MatrixXs MatrixXs;
58
68 ActionModelLQRTpl(const MatrixXs& A, const MatrixXs& B, const MatrixXs& Q,
69 const MatrixXs& R, const MatrixXs& N);
70
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
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
116 ActionModelLQRTpl(const std::size_t nx, const std::size_t nu,
117 const bool drift_free = true);
118
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
145 template <typename NewScalar>
147
148 virtual bool checkData(
149 const std::shared_ptr<ActionDataAbstract>& data) override;
150
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
164 const MatrixXs& get_A() const;
165
167 const MatrixXs& get_B() const;
168
170 const VectorXs& get_f() const;
171
173 const MatrixXs& get_Q() const;
174
176 const MatrixXs& get_R() const;
177
179 const MatrixXs& get_N() const;
180
182 const MatrixXs& get_G() const;
183
185 const MatrixXs& get_H() const;
186
188 const VectorXs& get_q() const;
189
191 const VectorXs& get_r() const;
192
194 const VectorXs& get_g() const;
195
197 const VectorXs& get_h() const;
198
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
266 virtual void print(std::ostream& os) const override;
267
268 protected:
269 using Base::ng_;
270 using Base::nh_;
271 using Base::nu_;
272 using Base::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
292template <typename _Scalar>
293struct ActionDataLQRTpl : public ActionDataAbstractTpl<_Scalar> {
294 typedef _Scalar Scalar;
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
348CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(crocoddyl::ActionModelLQRTpl)
349CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(crocoddyl::ActionDataLQRTpl)
350
351#endif // CROCODDYL_CORE_ACTIONS_LQR_HPP_
Abstract class for action model.
std::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Control dimension.
std::size_t nh_
Number of equality constraints.
std::size_t ng_
Number of inequality constraints.
Linear-quadratic regulator (LQR) action model.
Definition lqr.hpp:45
const MatrixXs & get_H() const
Return the state-input equality constraint matrix.
const VectorXs & get_r() const
Return the input weight vector.
const MatrixXs & get_Q() const
Return the state weight matrix.
const VectorXs & get_q() const
Return the state weight vector.
virtual void calc(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the next state and cost value.
virtual std::shared_ptr< ActionDataAbstract > createData() override
Create the action data.
const MatrixXs & get_R() const
Return the input weight matrix.
virtual void calcDiff(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the derivatives of the cost functions with respect to the state only.
virtual bool checkData(const std::shared_ptr< ActionDataAbstract > &data) override
Checks that a specific data belongs to this model.
void set_LQR(const MatrixXs &A, const MatrixXs &B, const MatrixXs &Q, const MatrixXs &R, const MatrixXs &N, const MatrixXs &G, const MatrixXs &H, const VectorXs &f, const VectorXs &q, const VectorXs &r, const VectorXs &g, const VectorXs &h)
Modify the LQR action model.
const VectorXs & get_g() const
Return the state-input inequality constraint bias.
ActionModelLQRTpl(const ActionModelLQRTpl &copy)
Copy constructor.
const VectorXs & get_f() const
Return the dynamics drift.
ActionModelLQRTpl(const MatrixXs &A, const MatrixXs &B, const MatrixXs &Q, const MatrixXs &R, const MatrixXs &N, const VectorXs &f, const VectorXs &q, const VectorXs &r)
Initialize the LQR action model.
ActionModelLQRTpl(const MatrixXs &A, const MatrixXs &B, const MatrixXs &Q, const MatrixXs &R, const MatrixXs &N)
Initialize the LQR action model.
const MatrixXs & get_A() const
Return the state matrix.
const VectorXs & get_h() const
Return the state-input equality constraint bias.
ActionModelLQRTpl(const MatrixXs &A, const MatrixXs &B, const MatrixXs &Q, const MatrixXs &R, const MatrixXs &N, const MatrixXs &G, const MatrixXs &H, const VectorXs &f, const VectorXs &q, const VectorXs &r, const VectorXs &g, const VectorXs &h)
Initialize the LQR action model.
ActionModelLQRTpl< NewScalar > cast() const
Cast the LQR model to a different scalar type.
static ActionModelLQRTpl Random(const std::size_t nx, const std::size_t nu, const std::size_t ng=0, const std::size_t nh=0)
Create a random LQR model.
virtual void calc(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the total cost value for nodes that depends only on the state.
const MatrixXs & get_B() const
Return the input matrix.
ActionModelLQRTpl(const std::size_t nx, const std::size_t nu, const bool drift_free=true)
Initialize the LQR action model.
virtual void calcDiff(const std::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the derivatives of the dynamics and cost functions.
const MatrixXs & get_G() const
Return the state-input inequality constraint matrix.
virtual void print(std::ostream &os) const override
Print relevant information of the LQR model.
const MatrixXs & get_N() const
Return the state-input weight matrix.
VectorXs xnext
evolution state
MatrixXs Fx
Jacobian of the dynamics w.r.t. the state .
MatrixXs Fu
Jacobian of the dynamics w.r.t. the control .
MatrixXs Luu
Hessian of the cost w.r.t. the control .
VectorXs Lx
Jacobian of the cost w.r.t. the state .
MatrixXs Lxx
Hessian of the cost w.r.t. the state .
VectorXs Lu
Jacobian of the cost w.r.t. the control .
MatrixXs Fx
Jacobian of the dynamics w.r.t. the state .
MatrixXs Fu
Jacobian of the dynamics w.r.t. the control .
MatrixXs Luu
Hessian of the cost w.r.t. the control .
MatrixXs Lxx
Hessian of the cost w.r.t. the state .