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_
Abstract class for differential action model.
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Checks that a specific data belongs to this model.
boost::shared_ptr< StateAbstract > state_
Model of the state.
virtual void print(std::ostream &os) const
Print relevant information of the LQR model.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.