crocoddyl  1.6.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
lqr.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2020, 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_LQR_HPP_
10 #define CROCODDYL_CORE_ACTIONS_LQR_HPP_
11 
12 #include <stdexcept>
13 
14 #include "crocoddyl/core/fwd.hpp"
15 #include "crocoddyl/core/action-base.hpp"
16 #include "crocoddyl/core/states/euclidean.hpp"
17 
18 namespace crocoddyl {
19 
20 template <typename _Scalar>
21 class ActionModelLQRTpl : public ActionModelAbstractTpl<_Scalar> {
22  public:
23  typedef _Scalar Scalar;
29  typedef typename MathBase::VectorXs VectorXs;
30  typedef typename MathBase::MatrixXs MatrixXs;
31 
32  ActionModelLQRTpl(const std::size_t& nx, const std::size_t& nu, bool drift_free = true);
33  virtual ~ActionModelLQRTpl();
34 
35  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
36  const Eigen::Ref<const VectorXs>& u);
37  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
38  const Eigen::Ref<const VectorXs>& u);
39  virtual boost::shared_ptr<ActionDataAbstract> createData();
40  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
41 
42  const MatrixXs& get_Fx() const;
43  const MatrixXs& get_Fu() const;
44  const VectorXs& get_f0() const;
45  const VectorXs& get_lx() const;
46  const VectorXs& get_lu() const;
47  const MatrixXs& get_Lxx() const;
48  const MatrixXs& get_Lxu() const;
49  const MatrixXs& get_Luu() const;
50 
51  void set_Fx(const MatrixXs& Fx);
52  void set_Fu(const MatrixXs& Fu);
53  void set_f0(const VectorXs& f0);
54  void set_lx(const VectorXs& lx);
55  void set_lu(const VectorXs& lu);
56  void set_Lxx(const MatrixXs& Lxx);
57  void set_Lxu(const MatrixXs& Lxu);
58  void set_Luu(const MatrixXs& Luu);
59 
60  protected:
62  using Base::nr_;
63  using Base::nu_;
64  using Base::state_;
65  using Base::u_lb_;
66  using Base::u_ub_;
67  using Base::unone_;
68 
69  private:
70  bool drift_free_;
71  MatrixXs Fx_;
72  MatrixXs Fu_;
73  VectorXs f0_;
74  MatrixXs Lxx_;
75  MatrixXs Lxu_;
76  MatrixXs Luu_;
77  VectorXs lx_;
78  VectorXs lu_;
79 };
80 
81 template <typename _Scalar>
82 struct ActionDataLQRTpl : public ActionDataAbstractTpl<_Scalar> {
83  typedef _Scalar Scalar;
86 
87  template <template <typename Scalar> class Model>
88  explicit ActionDataLQRTpl(Model<Scalar>* const model) : Base(model) {
89  // Setting the linear model and quadratic cost here because they are constant
90  Fx = model->get_Fx();
91  Fu = model->get_Fu();
92  Lxx = model->get_Lxx();
93  Luu = model->get_Luu();
94  Lxu = model->get_Lxu();
95  }
96 
97  using Base::cost;
98  using Base::Fu;
99  using Base::Fx;
100  using Base::Lu;
101  using Base::Luu;
102  using Base::Lx;
103  using Base::Lxu;
104  using Base::Lxx;
105  using Base::r;
106  using Base::xnext;
107 };
108 
109 } // namespace crocoddyl
110 
111 /* --- Details -------------------------------------------------------------- */
112 /* --- Details -------------------------------------------------------------- */
113 /* --- Details -------------------------------------------------------------- */
114 #include "crocoddyl/core/actions/lqr.hxx"
115 
116 #endif // CROCODDYL_CORE_ACTIONS_LQR_HPP_
crocoddyl::ActionDataAbstractTpl
Definition: action-base.hpp:218
crocoddyl::ActionModelAbstractTpl::has_control_limits_
bool has_control_limits_
Indicates whether any of the control limits is finite.
Definition: action-base.hpp:209
crocoddyl::ActionDataAbstractTpl::Lxu
MatrixXs Lxu
Hessian of the cost function.
Definition: action-base.hpp:258
crocoddyl::MathBaseTpl< Scalar >
crocoddyl::ActionModelLQRTpl::checkData
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
crocoddyl::ActionModelLQRTpl::calc
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the next state and cost value.
crocoddyl::ActionModelLQRTpl
Definition: lqr.hpp:21
crocoddyl::ActionModelAbstractTpl::nu_
std::size_t nu_
Control dimension.
Definition: action-base.hpp:203
crocoddyl::ActionDataAbstractTpl::Lx
VectorXs Lx
Jacobian of the cost function.
Definition: action-base.hpp:255
crocoddyl::ActionModelLQRTpl::calcDiff
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the dynamics and cost functions.
crocoddyl::ActionDataAbstractTpl::xnext
VectorXs xnext
evolution state
Definition: action-base.hpp:251
crocoddyl::StateVectorTpl
Definition: fwd.hpp:109
crocoddyl::ActionModelAbstractTpl
Abstract class for action model.
Definition: action-base.hpp:60
crocoddyl::ActionDataAbstractTpl::cost
Scalar cost
cost value
Definition: action-base.hpp:250
crocoddyl::ActionModelLQRTpl::createData
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
crocoddyl::ActionDataAbstractTpl::r
VectorXs r
Cost residual.
Definition: action-base.hpp:254
crocoddyl::ActionModelAbstractTpl::state_
boost::shared_ptr< StateAbstract > state_
Model of the state.
Definition: action-base.hpp:205
crocoddyl::ActionModelAbstractTpl::unone_
VectorXs unone_
Neutral state.
Definition: action-base.hpp:206
crocoddyl::ActionDataAbstractTpl::Fx
MatrixXs Fx
Jacobian of the dynamics.
Definition: action-base.hpp:252
crocoddyl::ActionModelAbstractTpl::u_lb_
VectorXs u_lb_
Lower control limits.
Definition: action-base.hpp:207
crocoddyl::ActionModelAbstractTpl::u_ub_
VectorXs u_ub_
Upper control limits.
Definition: action-base.hpp:208
crocoddyl::ActionDataLQRTpl
Definition: lqr.hpp:82
crocoddyl::ActionDataAbstractTpl::Lu
VectorXs Lu
Jacobian of the cost function.
Definition: action-base.hpp:256
crocoddyl::ActionDataAbstractTpl::Lxx
MatrixXs Lxx
Hessian of the cost function.
Definition: action-base.hpp:257
crocoddyl::ActionDataAbstractTpl::Fu
MatrixXs Fu
Jacobian of the dynamics.
Definition: action-base.hpp:253
crocoddyl::ActionModelAbstractTpl::nr_
std::size_t nr_
Dimension of the cost residual.
Definition: action-base.hpp:204
crocoddyl::ActionDataAbstractTpl::Luu
MatrixXs Luu
Hessian of the cost function.
Definition: action-base.hpp:259