crocoddyl  1.8.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
rk4.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, IRI: CSIC-UPC, University of Edinburgh
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 
58  virtual void print(std::ostream& os) const;
59 
60  protected:
61  using Base::nr_;
62  using Base::nu_;
63  using Base::state_;
64  using Base::unone_;
65 
66  private:
67  boost::shared_ptr<DifferentialActionModelAbstract> differential_;
68  Scalar time_step_;
69  std::vector<Scalar> rk4_c_;
70  bool with_cost_residual_;
71  bool enable_integration_;
72 };
73 
74 template <typename _Scalar>
75 struct IntegratedActionDataRK4Tpl : public ActionDataAbstractTpl<_Scalar> {
76  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
77 
78  typedef _Scalar Scalar;
79  typedef MathBaseTpl<Scalar> MathBase;
80  typedef ActionDataAbstractTpl<Scalar> Base;
81  typedef typename MathBase::VectorXs VectorXs;
82  typedef typename MathBase::MatrixXs MatrixXs;
83 
84  template <template <typename Scalar> class Model>
85  explicit IntegratedActionDataRK4Tpl(Model<Scalar>* const model) : Base(model) {
86  const std::size_t ndx = model->get_state()->get_ndx();
87  const std::size_t nx = model->get_state()->get_nx();
88  const std::size_t nv = model->get_state()->get_nv();
89  const std::size_t nu = model->get_nu();
90 
91  for (std::size_t i = 0; i < 4; ++i) {
92  differential.push_back(
93  boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >(model->get_differential()->createData()));
94  }
95 
96  dx = VectorXs::Zero(ndx);
97  integral = std::vector<Scalar>(4, Scalar(0.));
98 
99  ki = std::vector<VectorXs>(4, VectorXs::Zero(ndx));
100  y = std::vector<VectorXs>(4, VectorXs::Zero(nx));
101  dx_rk4 = std::vector<VectorXs>(4, VectorXs::Zero(ndx));
102 
103  dki_dx = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
104  dki_du = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, nu));
105  dyi_dx = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
106  dyi_du = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, nu));
107  dki_dy = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
108 
109  dli_dx = std::vector<VectorXs>(4, VectorXs::Zero(ndx));
110  dli_du = std::vector<VectorXs>(4, VectorXs::Zero(nu));
111  ddli_ddx = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
112  ddli_ddu = std::vector<MatrixXs>(4, MatrixXs::Zero(nu, nu));
113  ddli_dxdu = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, nu));
114  Luu_partialx = std::vector<MatrixXs>(4, MatrixXs::Zero(nu, nu));
115  Lxx_partialx = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
116  Lxx_partialu = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, nu));
117 
118  dyi_dx[0].diagonal().array() = (Scalar)1;
119  for (std::size_t i = 0; i < 4; ++i) {
120  dki_dy[i].topRightCorner(nv, nv).diagonal().array() = (Scalar)1;
121  }
122  }
123  virtual ~IntegratedActionDataRK4Tpl() {}
124 
125  VectorXs dx;
126  std::vector<boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> > > differential;
127  std::vector<Scalar> integral;
128  std::vector<VectorXs> ki;
129  std::vector<VectorXs> y;
130  std::vector<VectorXs> dx_rk4;
131 
132  std::vector<MatrixXs> dki_dx;
133  std::vector<MatrixXs> dki_du;
134  std::vector<MatrixXs> dyi_dx;
135  std::vector<MatrixXs> dyi_du;
136  std::vector<MatrixXs> dki_dy;
137 
138  std::vector<VectorXs> dli_dx;
139  std::vector<VectorXs> dli_du;
140  std::vector<MatrixXs> ddli_ddx;
141  std::vector<MatrixXs> ddli_ddu;
142  std::vector<MatrixXs> ddli_dxdu;
143  std::vector<MatrixXs> Luu_partialx;
144  std::vector<MatrixXs> Lxx_partialx;
145  std::vector<MatrixXs> Lxx_partialu;
146 
147  using Base::cost;
148  using Base::Fu;
149  using Base::Fx;
150  using Base::Lu;
151  using Base::Luu;
152  using Base::Lx;
153  using Base::Lxu;
154  using Base::Lxx;
155  using Base::r;
156  using Base::xnext;
157 };
158 
159 } // namespace crocoddyl
160 
161 /* --- Details -------------------------------------------------------------- */
162 /* --- Details -------------------------------------------------------------- */
163 /* --- Details -------------------------------------------------------------- */
164 #include "crocoddyl/core/integrator/rk4.hxx"
165 
166 #endif // CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
std::size_t nu_
Control dimension.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
VectorXs unone_
Neutral state.
boost::shared_ptr< StateAbstract > state_
Model of the state.
virtual void print(std::ostream &os) const
Print relevant information of the Runge-Kutta 4 integrator model.
std::size_t nr_
Dimension of the cost residual.