crocoddyl  1.8.1
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>
20  public:
21  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22 
23  typedef _Scalar Scalar;
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>
76  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
77 
78  typedef _Scalar Scalar;
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_
Abstract class for action model.
Definition: action-base.hpp:60
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Control dimension.
VectorXs unone_
Neutral state.
std::size_t nr_
Dimension of the cost residual.
Abstract class for differential action model.
virtual void print(std::ostream &os) const
Print relevant information of the Runge-Kutta 4 integrator 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 boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
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.
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.