crocoddyl  1.9.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/integ-action-base.hpp"
14 #include "crocoddyl/core/utils/deprecate.hpp"
15 
16 namespace crocoddyl {
17 
31 template <typename _Scalar>
33  public:
34  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
35 
36  typedef _Scalar Scalar;
43  typedef typename MathBase::VectorXs VectorXs;
44  typedef typename MathBase::MatrixXs MatrixXs;
45 
54  DEPRECATED("Use IntegratedActionModelRK",
55  IntegratedActionModelRK4Tpl(boost::shared_ptr<DifferentialActionModelAbstract> model,
56  boost::shared_ptr<ControlParametrizationModelAbstract> control,
57  const Scalar time_step = Scalar(1e-3), const bool with_cost_residual = true);)
58 
68  DEPRECATED("Use IntegratedActionModelRK",
69  IntegratedActionModelRK4Tpl(boost::shared_ptr<DifferentialActionModelAbstract> model,
70  const Scalar time_step = Scalar(1e-3), const bool with_cost_residual = true);)
71  virtual ~IntegratedActionModelRK4Tpl();
72 
80  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
81  const Eigen::Ref<const VectorXs>& u);
82 
92  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
93 
101  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
102  const Eigen::Ref<const VectorXs>& u);
103 
113  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
114 
120  virtual boost::shared_ptr<ActionDataAbstract> createData();
121 
125  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
126 
139  virtual void quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
140  const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter = 100,
141  const Scalar tol = Scalar(1e-9));
142 
148  virtual void print(std::ostream& os) const;
149 
150  protected:
151  using Base::control_;
152  using Base::differential_;
153  using Base::nu_;
154  using Base::state_;
155  using Base::time_step2_;
156  using Base::time_step_;
158 
159  private:
160  std::array<Scalar, 4> rk4_c_;
161 };
162 
163 template <typename _Scalar>
165  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
166 
167  typedef _Scalar Scalar;
172  typedef typename MathBase::VectorXs VectorXs;
173  typedef typename MathBase::MatrixXs MatrixXs;
174 
175  template <template <typename Scalar> class Model>
176  explicit IntegratedActionDataRK4Tpl(Model<Scalar>* const model)
177  : Base(model),
178  integral(4, Scalar(0.)),
179  dx(model->get_state()->get_ndx()),
180  ki(4, VectorXs::Zero(model->get_state()->get_ndx())),
181  y(4, VectorXs::Zero(model->get_state()->get_nx())),
182  ws(4, VectorXs::Zero(model->get_control()->get_nw())),
183  dx_rk4(4, VectorXs::Zero(model->get_state()->get_ndx())),
184  dki_dx(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
185  dki_du(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
186  dyi_dx(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
187  dyi_du(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
188  dli_dx(4, VectorXs::Zero(model->get_state()->get_ndx())),
189  dli_du(4, VectorXs::Zero(model->get_nu())),
190  ddli_ddx(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
191  ddli_ddw(4, MatrixXs::Zero(model->get_control()->get_nw(), model->get_control()->get_nw())),
192  ddli_ddu(4, MatrixXs::Zero(model->get_nu(), model->get_nu())),
193  ddli_dxdw(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_control()->get_nw())),
194  ddli_dxdu(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
195  ddli_dwdu(4, MatrixXs::Zero(model->get_control()->get_nw(), model->get_nu())),
196  Luu_partialx(4, MatrixXs::Zero(model->get_nu(), model->get_nu())),
197  Lxu_i(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
198  Lxx_partialx(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
199  Lxx_partialu(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())) {
200  dx.setZero();
201 
202  for (std::size_t i = 0; i < 4; ++i) {
203  differential.push_back(
204  boost::shared_ptr<DifferentialActionDataAbstract>(model->get_differential()->createData()));
205  control.push_back(boost::shared_ptr<ControlParametrizationDataAbstract>(model->get_control()->createData()));
206  }
207 
208  const std::size_t nv = model->get_state()->get_nv();
209  dyi_dx[0].diagonal().setOnes();
210  dki_dx[0].topRightCorner(nv, nv).diagonal().setOnes();
211  }
212  virtual ~IntegratedActionDataRK4Tpl() {}
213 
214  std::vector<boost::shared_ptr<DifferentialActionDataAbstract> > differential;
215  std::vector<boost::shared_ptr<ControlParametrizationDataAbstract> >
217  std::vector<Scalar> integral;
218  VectorXs dx;
219  std::vector<VectorXs> ki;
220  std::vector<VectorXs> y;
221  std::vector<VectorXs> ws;
222  std::vector<VectorXs> dx_rk4;
223 
224  std::vector<MatrixXs>
226  std::vector<MatrixXs> dki_du;
228 
229  std::vector<MatrixXs>
231  std::vector<MatrixXs> dyi_du;
233 
234  std::vector<VectorXs>
236  std::vector<VectorXs> dli_du;
238 
239  std::vector<MatrixXs> ddli_ddx;
241  std::vector<MatrixXs> ddli_ddw;
243  std::vector<MatrixXs> ddli_ddu;
245  std::vector<MatrixXs> ddli_dxdw;
247  std::vector<MatrixXs> ddli_dxdu;
249  std::vector<MatrixXs> ddli_dwdu;
251 
252  std::vector<MatrixXs> Luu_partialx;
253  std::vector<MatrixXs> Lxu_i;
254  std::vector<MatrixXs> Lxx_partialx;
255  std::vector<MatrixXs> Lxx_partialu;
256 
257  using Base::cost;
258  using Base::Fu;
259  using Base::Fx;
260  using Base::Lu;
261  using Base::Luu;
262  using Base::Lx;
263  using Base::Lxu;
264  using Base::Lxx;
265  using Base::r;
266  using Base::xnext;
267 };
268 
269 } // namespace crocoddyl
270 
271 /* --- Details -------------------------------------------------------------- */
272 /* --- Details -------------------------------------------------------------- */
273 /* --- Details -------------------------------------------------------------- */
274 #include "crocoddyl/core/integrator/rk4.hxx"
275 
276 #endif // CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
Abstract class for action model.
Definition: action-base.hpp:59
Abstract class for the control trajectory parametrization.
Abstract class for differential action model.
Abstract class for an integrated action model.
bool with_cost_residual_
Flag indicating whether a cost residual is used.
Scalar time_step_
Time step used for integration.
boost::shared_ptr< DifferentialActionModelAbstract > differential_
Differential action model that is integrated.
boost::shared_ptr< ControlParametrizationModelAbstract > control_
Model of the control parametrization.
boost::shared_ptr< StateAbstract > state_
< Dimension of the control
std::size_t nu_
< Dimension of the cost residual
Scalar time_step2_
Square of the time step used for integration.
Standard RK4 integrator.
Definition: rk4.hpp:32
virtual void print(std::ostream &os) const
Print relevant information of the RK4 integrator model.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Integrate the differential action model using RK4 scheme.
DEPRECATED("Use IntegratedActionModelRK", IntegratedActionModelRK4Tpl(boost::shared_ptr< DifferentialActionModelAbstract > model, boost::shared_ptr< ControlParametrizationModelAbstract > control, const Scalar time_step=Scalar(1e-3), const bool with_cost_residual=true);) DEPRECATED("Use IntegratedActionModelRK"
Initialize the RK4 integrator.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the partial derivatives of the RK4 integrator.
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)
Integrate the total cost value for nodes that depends only on the state using RK4 scheme.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the partial derivatives of the cost.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the RK4 integrator 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.
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.
std::vector< MatrixXs > ddli_ddx
Definition: rk4.hpp:239
std::vector< VectorXs > ki
List of RK4 terms related to system dynamics.
Definition: rk4.hpp:219
std::vector< MatrixXs > dki_du
Definition: rk4.hpp:226
std::vector< MatrixXs > dki_dx
List of partial derivatives of RK4 nodes with respect to the state of the RK4 integration....
Definition: rk4.hpp:225
std::vector< MatrixXs > ddli_ddu
Definition: rk4.hpp:243
std::vector< MatrixXs > ddli_dxdw
Definition: rk4.hpp:245
std::vector< MatrixXs > ddli_ddw
Definition: rk4.hpp:241
std::vector< boost::shared_ptr< DifferentialActionDataAbstract > > differential
List of differential model data.
Definition: rk4.hpp:214
std::vector< MatrixXs > ddli_dwdu
Definition: rk4.hpp:249
std::vector< MatrixXs > dyi_du
Definition: rk4.hpp:231
std::vector< VectorXs > y
List of states where f is evaluated in the RK4 integration.
Definition: rk4.hpp:220
std::vector< VectorXs > dli_du
Definition: rk4.hpp:236
std::vector< MatrixXs > ddli_dxdu
Definition: rk4.hpp:247
std::vector< VectorXs > dli_dx
List of partial derivatives of the cost with respect to the state of the RK4 integration....
Definition: rk4.hpp:235
std::vector< boost::shared_ptr< ControlParametrizationDataAbstract > > control
List of control parametrization data.
Definition: rk4.hpp:216
std::vector< MatrixXs > dyi_dx
List of partial derivatives of RK4 dynamics with respect to the state of the RK4 integrator....
Definition: rk4.hpp:230
std::vector< VectorXs > ws
Control inputs evaluated in the RK4 integration.
Definition: rk4.hpp:221