Crocoddyl
 
Loading...
Searching...
No Matches
diff-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_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
17namespace crocoddyl {
18
44template <typename _Scalar>
46 : public DifferentialActionModelAbstractTpl<_Scalar> {
47 public:
48 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
49 CROCODDYL_DERIVED_CAST(DifferentialActionModelBase,
51
52 typedef _Scalar Scalar;
59 typedef typename MathBase::VectorXs VectorXs;
60 typedef typename MathBase::MatrixXs MatrixXs;
61
72 DifferentialActionModelLQRTpl(const MatrixXs& Aq, const MatrixXs& Av,
73 const MatrixXs& B, const MatrixXs& Q,
74 const MatrixXs& R, const MatrixXs& N);
75
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
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
128 DifferentialActionModelLQRTpl(const std::size_t nq, const std::size_t nu,
129 const bool drift_free = true);
130
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
159 template <typename NewScalar>
161
162 virtual bool checkData(
163 const std::shared_ptr<DifferentialActionDataAbstract>& data) override;
164
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
179 const MatrixXs& get_Aq() const;
180
182 const MatrixXs& get_Av() const;
183
185 const MatrixXs& get_B() const;
186
188 const VectorXs& get_f() const;
189
191 const MatrixXs& get_Q() const;
192
194 const MatrixXs& get_R() const;
195
197 const MatrixXs& get_N() const;
198
200 const MatrixXs& get_G() const;
201
203 const MatrixXs& get_H() const;
204
206 const VectorXs& get_q() const;
207
209 const VectorXs& get_r() const;
210
212 const VectorXs& get_g() const;
213
215 const VectorXs& get_h() const;
216
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
291 virtual void print(std::ostream& os) const override;
292
293 protected:
294 using Base::ng_;
295 using Base::nh_;
296 using Base::nu_;
297 using Base::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
318template <typename _Scalar>
320 : public DifferentialActionDataAbstractTpl<_Scalar> {
321 typedef _Scalar Scalar;
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
369CROCODDYL_DECLARE_EXTERN_TEMPLATE_CLASS(
371CROCODDYL_DECLARE_EXTERN_TEMPLATE_STRUCT(
373
374#endif // CROCODDYL_CORE_ACTIONS_DIFF_LQR_HPP_
Abstract class for differential action model.
std::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nh_
Number of equality constraints.
std::size_t ng_
Number of inequality constraints.
Linear-quadratic regulator (LQR) differential action model.
Definition diff-lqr.hpp:46
virtual bool checkData(const std::shared_ptr< DifferentialActionDataAbstract > &data) override
Checks that a specific data belongs to this model.
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 calcDiff(const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the derivatives of the cost functions with respect to the state only.
void set_LQR(const MatrixXs &Aq, const MatrixXs &Av, 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.
virtual void calc(const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u) override
Compute the system acceleration and cost value.
const MatrixXs & get_R() const
Return the input weight matrix.
static DifferentialActionModelLQRTpl Random(const std::size_t nq, const std::size_t nu, const std::size_t ng=0, const std::size_t nh=0)
Create a random LQR model.
const VectorXs & get_g() const
Return the state-input inequality constraint bias.
virtual std::shared_ptr< DifferentialActionDataAbstract > createData() override
Create the differential action data.
DifferentialActionModelLQRTpl(const MatrixXs &Aq, const MatrixXs &Av, const MatrixXs &B, const MatrixXs &Q, const MatrixXs &R, const MatrixXs &N)
Initialize the LQR action model.
const VectorXs & get_f() const
Return the dynamics drift.
DifferentialActionModelLQRTpl< NewScalar > cast() const
Cast the differential-LQR model to a different scalar type.
const MatrixXs & get_Av() const
Return the velocity matrix.
DifferentialActionModelLQRTpl(const DifferentialActionModelLQRTpl &copy)
Copy constructor.
const VectorXs & get_h() const
Return the state-input equality constraint bias.
const MatrixXs & get_Aq() const
Return the position matrix.
virtual void calc(const std::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x) override
Compute the total cost value for nodes that depends only on the state.
DifferentialActionModelLQRTpl(const MatrixXs &Aq, const MatrixXs &Av, 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.
const MatrixXs & get_B() const
Return the input matrix.
DifferentialActionModelLQRTpl(const std::size_t nq, const std::size_t nu, const bool drift_free=true)
Initialize the LQR action model.
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.
DifferentialActionModelLQRTpl(const MatrixXs &Aq, const MatrixXs &Av, 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.
virtual void calcDiff(const std::shared_ptr< DifferentialActionDataAbstract > &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_N() const
Return the state-input weight matrix.
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 .