crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
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_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, const 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 calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
38  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
39  const Eigen::Ref<const VectorXs>& u);
40  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
41  virtual boost::shared_ptr<ActionDataAbstract> createData();
42  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
43 
44  const MatrixXs& get_Fx() const;
45  const MatrixXs& get_Fu() const;
46  const VectorXs& get_f0() const;
47  const VectorXs& get_lx() const;
48  const VectorXs& get_lu() const;
49  const MatrixXs& get_Lxx() const;
50  const MatrixXs& get_Lxu() const;
51  const MatrixXs& get_Luu() const;
52 
53  void set_Fx(const MatrixXs& Fx);
54  void set_Fu(const MatrixXs& Fu);
55  void set_f0(const VectorXs& f0);
56  void set_lx(const VectorXs& lx);
57  void set_lu(const VectorXs& lu);
58  void set_Lxx(const MatrixXs& Lxx);
59  void set_Lxu(const MatrixXs& Lxu);
60  void set_Luu(const MatrixXs& Luu);
61 
67  virtual void print(std::ostream& os) const;
68 
69  protected:
70  using Base::nu_;
71  using Base::state_;
72 
73  private:
74  bool drift_free_;
75  MatrixXs Fx_;
76  MatrixXs Fu_;
77  VectorXs f0_;
78  MatrixXs Lxx_;
79  MatrixXs Lxu_;
80  MatrixXs Luu_;
81  VectorXs lx_;
82  VectorXs lu_;
83 };
84 
85 template <typename _Scalar>
86 struct ActionDataLQRTpl : public ActionDataAbstractTpl<_Scalar> {
87  typedef _Scalar Scalar;
90  typedef typename MathBase::VectorXs VectorXs;
91 
92  template <template <typename Scalar> class Model>
93  explicit ActionDataLQRTpl(Model<Scalar>* const model)
94  : Base(model),
95  Luu_u_tmp(VectorXs::Zero(static_cast<Eigen::Index>(model->get_nu()))),
96  Lxx_x_tmp(VectorXs::Zero(static_cast<Eigen::Index>(model->get_state()->get_ndx()))) {
97  // Setting the linear model and quadratic cost here because they are constant
98  Fx = model->get_Fx();
99  Fu = model->get_Fu();
100  Lxx = model->get_Lxx();
101  Luu = model->get_Luu();
102  Lxu = model->get_Lxu();
103  }
104 
105  using Base::cost;
106  using Base::Fu;
107  using Base::Fx;
108  using Base::Lu;
109  using Base::Luu;
110  using Base::Lx;
111  using Base::Lxu;
112  using Base::Lxx;
113  using Base::r;
114  using Base::xnext;
115  VectorXs Luu_u_tmp; // Temporary variable for storing Hessian-vector product (size: nu)
116  VectorXs Lxx_x_tmp; // Temporary variable for storing Hessian-vector product (size: nx)
117 };
118 
119 } // namespace crocoddyl
120 
121 /* --- Details -------------------------------------------------------------- */
122 /* --- Details -------------------------------------------------------------- */
123 /* --- Details -------------------------------------------------------------- */
124 #include "crocoddyl/core/actions/lqr.hxx"
125 
126 #endif // CROCODDYL_CORE_ACTIONS_LQR_HPP_
Abstract class for action model.
Definition: action-base.hpp:59
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Control dimension.
virtual void print(std::ostream &os) const
Print relevant information of the LQR model.
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.
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.
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the total cost value for nodes that depends only on the state.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the cost functions with respect to the state only.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
VectorXs xnext
evolution state
MatrixXs Fx
Jacobian of the dynamics.
MatrixXs Fu
Jacobian of the dynamics.
MatrixXs Luu
Hessian of the cost function.
VectorXs Lx
Jacobian of the cost function.
MatrixXs Lxx
Hessian of the cost function.
VectorXs Lu
Jacobian of the cost function.
MatrixXs Lxu
Hessian of the cost function.
VectorXs r
Cost residual.
MatrixXs Fx
Jacobian of the dynamics.
MatrixXs Fu
Jacobian of the dynamics.
MatrixXs Luu
Hessian of the cost function.
MatrixXs Lxx
Hessian of the cost function.
MatrixXs Lxu
Hessian of the cost function.