crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
diff-lqr.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_ACTIONS_DIFF_LQR_HPP_
10 #define CROCODDYL_CORE_ACTIONS_DIFF_LQR_HPP_
11 
12 #include <stdexcept>
13 
14 #include "crocoddyl/core/fwd.hpp"
15 #include "crocoddyl/core/diff-action-base.hpp"
16 #include "crocoddyl/core/states/euclidean.hpp"
17 
18 namespace crocoddyl {
19 
20 template <typename _Scalar>
22  public:
23  typedef _Scalar Scalar;
29  typedef typename MathBase::VectorXs VectorXs;
30  typedef typename MathBase::MatrixXs MatrixXs;
31 
32  DifferentialActionModelLQRTpl(const std::size_t nq, const std::size_t nu, const bool drift_free = true);
34 
35  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
36  const Eigen::Ref<const VectorXs>& u);
37  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
38  const Eigen::Ref<const VectorXs>& x);
39  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
40  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u);
41  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
42  const Eigen::Ref<const VectorXs>& x);
43  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
44  virtual bool checkData(const boost::shared_ptr<DifferentialActionDataAbstract>& data);
45 
46  const MatrixXs& get_Fq() const;
47  const MatrixXs& get_Fv() const;
48  const MatrixXs& get_Fu() const;
49  const VectorXs& get_f0() const;
50  const VectorXs& get_lx() const;
51  const VectorXs& get_lu() const;
52  const MatrixXs& get_Lxx() const;
53  const MatrixXs& get_Lxu() const;
54  const MatrixXs& get_Luu() const;
55 
56  void set_Fq(const MatrixXs& Fq);
57  void set_Fv(const MatrixXs& Fv);
58  void set_Fu(const MatrixXs& Fu);
59  void set_f0(const VectorXs& f0);
60  void set_lx(const VectorXs& lx);
61  void set_lu(const VectorXs& lu);
62  void set_Lxx(const MatrixXs& Lxx);
63  void set_Lxu(const MatrixXs& Lxu);
64  void set_Luu(const MatrixXs& Luu);
65 
71  virtual void print(std::ostream& os) const;
72 
73  protected:
74  using Base::nu_;
75  using Base::state_;
76 
77  private:
78  bool drift_free_;
79  MatrixXs Fq_;
80  MatrixXs Fv_;
81  MatrixXs Fu_;
82  VectorXs f0_;
83  MatrixXs Lxx_;
84  MatrixXs Lxu_;
85  MatrixXs Luu_;
86  VectorXs lx_;
87  VectorXs lu_;
88 };
89 
90 template <typename _Scalar>
92  typedef _Scalar Scalar;
95  typedef typename MathBase::VectorXs VectorXs;
96  typedef typename MathBase::MatrixXs MatrixXs;
97 
98  template <template <typename Scalar> class Model>
99  explicit DifferentialActionDataLQRTpl(Model<Scalar>* const model) : Base(model) {
100  // Setting the linear model and quadratic cost here because they are constant
101  Fx.leftCols(model->get_state()->get_nq()) = model->get_Fq();
102  Fx.rightCols(model->get_state()->get_nv()) = model->get_Fv();
103  Fu = model->get_Fu();
104  Lxx = model->get_Lxx();
105  Luu = model->get_Luu();
106  Lxu = model->get_Lxu();
107  }
108 
109  using Base::cost;
110  using Base::Fu;
111  using Base::Fx;
112  using Base::Lu;
113  using Base::Luu;
114  using Base::Lx;
115  using Base::Lxu;
116  using Base::Lxx;
117  using Base::r;
118  using Base::xout;
119 };
120 
121 } // namespace crocoddyl
122 
123 /* --- Details -------------------------------------------------------------- */
124 /* --- Details -------------------------------------------------------------- */
125 /* --- Details -------------------------------------------------------------- */
126 #include "crocoddyl/core/actions/diff-lqr.hxx"
127 #endif // CROCODDYL_CORE_ACTIONS_DIFF_LQR_HPP_
VectorXs Lx
Jacobian of the cost function.
MatrixXs Fx
Jacobian of the dynamics.
MatrixXs Lxx
Hessian of the cost function.
Abstract class for differential action model.
MatrixXs Luu
Hessian of the cost function.
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Checks that a specific data belongs to this model.
MatrixXs Lxu
Hessian of the cost function.
boost::shared_ptr< StateAbstract > state_
Model of the state.
virtual void print(std::ostream &os) const
Print relevant information of the LQR model.
MatrixXs Fu
Jacobian of the dynamics.
VectorXs Lu
Jacobian of the cost function.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.