crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
rk4.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, IRI: CSIC-UPC
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
10 #define CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
11 
12 #include "crocoddyl/core/fwd.hpp"
13 #include "crocoddyl/core/action-base.hpp"
14 #include "crocoddyl/core/diff-action-base.hpp"
15 
16 namespace crocoddyl {
17 
18 template <typename _Scalar>
19 class IntegratedActionModelRK4Tpl : public ActionModelAbstractTpl<_Scalar> {
20  public:
21  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22 
23  typedef _Scalar Scalar;
24  typedef MathBaseTpl<Scalar> MathBase;
25  typedef ActionModelAbstractTpl<Scalar> Base;
26  typedef IntegratedActionDataRK4Tpl<Scalar> Data;
27  typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
28  typedef DifferentialActionModelAbstractTpl<Scalar> DifferentialActionModelAbstract;
29  typedef typename MathBase::VectorXs VectorXs;
30  typedef typename MathBase::MatrixXs MatrixXs;
31 
32  IntegratedActionModelRK4Tpl(boost::shared_ptr<DifferentialActionModelAbstract> model,
33  const Scalar& time_step = Scalar(1e-3), const bool& with_cost_residual = true);
34  virtual ~IntegratedActionModelRK4Tpl();
35 
36  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
37  const Eigen::Ref<const VectorXs>& u);
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 boost::shared_ptr<ActionDataAbstract> createData();
41  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
42 
43  virtual void quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
44  const Eigen::Ref<const VectorXs>& x, const std::size_t& maxiter = 100,
45  const Scalar& tol = Scalar(1e-9));
46 
47  const boost::shared_ptr<DifferentialActionModelAbstract>& get_differential() const;
48  const Scalar& get_dt() const;
49 
50  void set_dt(const Scalar& dt);
51  void set_differential(boost::shared_ptr<DifferentialActionModelAbstract> model);
52 
53  protected:
55  using Base::nr_;
56  using Base::nu_;
57  using Base::state_;
58  using Base::u_lb_;
59  using Base::u_ub_;
60  using Base::unone_;
61 
62  private:
63  boost::shared_ptr<DifferentialActionModelAbstract> differential_;
64  Scalar time_step_;
65  std::vector<Scalar> rk4_c_;
66  bool with_cost_residual_;
67  bool enable_integration_;
68 };
69 
70 template <typename _Scalar>
71 struct IntegratedActionDataRK4Tpl : public ActionDataAbstractTpl<_Scalar> {
72  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
73 
74  typedef _Scalar Scalar;
75  typedef MathBaseTpl<Scalar> MathBase;
76  typedef ActionDataAbstractTpl<Scalar> Base;
77  typedef typename MathBase::VectorXs VectorXs;
78  typedef typename MathBase::MatrixXs MatrixXs;
79 
80  template <template <typename Scalar> class Model>
81  explicit IntegratedActionDataRK4Tpl(Model<Scalar>* const model) : Base(model) {
82  const std::size_t& ndx = model->get_state()->get_ndx();
83  const std::size_t& nx = model->get_state()->get_nx();
84  const std::size_t& nv = model->get_state()->get_nv();
85  const std::size_t& nu = model->get_nu();
86 
87  for (std::size_t i = 0; i < 4; ++i) {
88  differential.push_back(
89  boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >(model->get_differential()->createData()));
90  }
91 
92  dx = VectorXs::Zero(ndx);
93  integral = std::vector<Scalar>(4, Scalar(0.));
94 
95  ki = std::vector<VectorXs>(4, VectorXs::Zero(ndx));
96  y = std::vector<VectorXs>(4, VectorXs::Zero(nx));
97  dx_rk4 = std::vector<VectorXs>(4, VectorXs::Zero(ndx));
98 
99  dki_dx = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
100  dki_du = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, nu));
101  dyi_dx = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
102  dyi_du = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, nu));
103  dki_dy = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
104 
105  dli_dx = std::vector<VectorXs>(4, VectorXs::Zero(ndx));
106  dli_du = std::vector<VectorXs>(4, VectorXs::Zero(nu));
107  ddli_ddx = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
108  ddli_ddu = std::vector<MatrixXs>(4, MatrixXs::Zero(nu, nu));
109  ddli_dxdu = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, nu));
110  Luu_partialx = std::vector<MatrixXs>(4, MatrixXs::Zero(nu, nu));
111  Lxx_partialx = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
112  Lxx_partialu = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, nu));
113 
114  dyi_dx[0].diagonal().array() = (Scalar)1;
115  for (std::size_t i = 0; i < 4; ++i) {
116  dki_dy[i].topRightCorner(nv, nv).diagonal().array() = (Scalar)1;
117  }
118  }
119  virtual ~IntegratedActionDataRK4Tpl() {}
120 
121  VectorXs dx;
122  std::vector<boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> > > differential;
123  std::vector<Scalar> integral;
124  std::vector<VectorXs> ki;
125  std::vector<VectorXs> y;
126  std::vector<VectorXs> dx_rk4;
127 
128  std::vector<MatrixXs> dki_dx;
129  std::vector<MatrixXs> dki_du;
130  std::vector<MatrixXs> dyi_dx;
131  std::vector<MatrixXs> dyi_du;
132  std::vector<MatrixXs> dki_dy;
133 
134  std::vector<VectorXs> dli_dx;
135  std::vector<VectorXs> dli_du;
136  std::vector<MatrixXs> ddli_ddx;
137  std::vector<MatrixXs> ddli_ddu;
138  std::vector<MatrixXs> ddli_dxdu;
139  std::vector<MatrixXs> Luu_partialx;
140  std::vector<MatrixXs> Lxx_partialx;
141  std::vector<MatrixXs> Lxx_partialu;
142 
143  using Base::cost;
144  using Base::Fu;
145  using Base::Fx;
146  using Base::Lu;
147  using Base::Luu;
148  using Base::Lx;
149  using Base::Lxu;
150  using Base::Lxx;
151  using Base::r;
152  using Base::xnext;
153 };
154 
155 } // namespace crocoddyl
156 
157 /* --- Details -------------------------------------------------------------- */
158 /* --- Details -------------------------------------------------------------- */
159 /* --- Details -------------------------------------------------------------- */
160 #include "crocoddyl/core/integrator/rk4.hxx"
161 
162 #endif // CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
bool has_control_limits_
Indicates whether any of the control limits is finite.
Definition: action-base.hpp:96
std::size_t nu_
Control dimension.
Definition: action-base.hpp:90
VectorXs unone_
Neutral state.
Definition: action-base.hpp:93
VectorXs u_ub_
Upper control limits.
Definition: action-base.hpp:95
VectorXs u_lb_
Lower control limits.
Definition: action-base.hpp:94
boost::shared_ptr< StateAbstract > state_
Model of the state.
Definition: action-base.hpp:92
std::size_t nr_
Dimension of the cost residual.
Definition: action-base.hpp:91
virtual void quasiStatic(const boost::shared_ptr< ActionDataAbstract > &data, Eigen::Ref< VectorXs > u, const Eigen::Ref< const VectorXs > &x, const std::size_t &maxiter=100, const Scalar &tol=Scalar(1e-9))
Computes the quasic static commands.