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>
32 class IntegratedActionModelRK4Tpl : public IntegratedActionModelAbstractTpl<_Scalar> {
33  public:
34  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
35 
36  typedef _Scalar Scalar;
37  typedef MathBaseTpl<Scalar> MathBase;
38  typedef IntegratedActionModelAbstractTpl<Scalar> Base;
39  typedef IntegratedActionDataRK4Tpl<Scalar> Data;
40  typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
41  typedef DifferentialActionModelAbstractTpl<Scalar> DifferentialActionModelAbstract;
42  typedef ControlParametrizationModelAbstractTpl<Scalar> ControlParametrizationModelAbstract;
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>
164 struct IntegratedActionDataRK4Tpl : public IntegratedActionDataAbstractTpl<_Scalar> {
165  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
166 
167  typedef _Scalar Scalar;
168  typedef MathBaseTpl<Scalar> MathBase;
169  typedef IntegratedActionDataAbstractTpl<Scalar> Base;
170  typedef DifferentialActionDataAbstractTpl<Scalar> DifferentialActionDataAbstract;
171  typedef ControlParametrizationDataAbstractTpl<Scalar> ControlParametrizationDataAbstract;
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;
227 
229  std::vector<MatrixXs>
231  std::vector<MatrixXs> dyi_du;
232 
234  std::vector<VectorXs>
236  std::vector<VectorXs> dli_du;
237 
239  std::vector<MatrixXs> ddli_ddx;
240  std::vector<MatrixXs> ddli_ddw;
242  std::vector<MatrixXs> ddli_ddu;
244  std::vector<MatrixXs> ddli_dxdw;
246  std::vector<MatrixXs> ddli_dxdu;
248  std::vector<MatrixXs> ddli_dwdu;
250 
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_
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
std::vector< boost::shared_ptr< ControlParametrizationDataAbstract > > control
List of control parametrization data.
Definition: rk4.hpp:216
std::vector< MatrixXs > ddli_ddw
Definition: rk4.hpp:241
MatrixXs Lxx
Hessian of the cost function.
Scalar time_step_
Time step used for integration.
std::size_t nu_
Control dimension.
bool with_cost_residual_
Flag indicating whether a cost residual is used.
VectorXs xnext
evolution state
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.
std::vector< VectorXs > y
List of states where f is evaluated in the RK4 integration.
Definition: rk4.hpp:220
std::vector< MatrixXs > dki_du
Definition: rk4.hpp:226
std::vector< boost::shared_ptr< DifferentialActionDataAbstract > > differential
List of differential model data.
Definition: rk4.hpp:214
std::vector< VectorXs > ws
Control inputs evaluated in the RK4 integration.
Definition: rk4.hpp:221
boost::shared_ptr< ControlParametrizationModelAbstract > control_
Model of the control parametrization.
std::vector< MatrixXs > dyi_dx
List of partial derivatives of RK4 dynamics with respect to the state of the RK4 integrator. dyi/dx.
Definition: rk4.hpp:230
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the RK4 integrator data.
std::vector< VectorXs > dli_du
Definition: rk4.hpp:236
VectorXs r
Cost residual.
VectorXs Lx
Jacobian of the cost function.
std::vector< MatrixXs > ddli_dxdu
Definition: rk4.hpp:247
std::vector< MatrixXs > dki_dx
List of partial derivatives of RK4 nodes with respect to the state of the RK4 integration. dki/dx.
Definition: rk4.hpp:225
std::vector< MatrixXs > ddli_ddu
Definition: rk4.hpp:243
std::vector< MatrixXs > dyi_du
Definition: rk4.hpp:231
MatrixXs Fx
Jacobian of the dynamics.
VectorXs Lu
Jacobian of the cost function.
Scalar time_step2_
Square of the time step used for integration.
boost::shared_ptr< DifferentialActionModelAbstract > differential_
Differential action model that is integrated.
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.
std::vector< VectorXs > dli_dx
List of partial derivatives of the cost with respect to the state of the RK4 integration. dli_dx.
Definition: rk4.hpp:235
MatrixXs Luu
Hessian of the cost function.
boost::shared_ptr< StateAbstract > state_
Model of the state.
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.
std::vector< MatrixXs > ddli_dwdu
Definition: rk4.hpp:249
std::vector< VectorXs > ki
List of RK4 terms related to system dynamics.
Definition: rk4.hpp:219
VectorXs dx
State rate.
Definition: rk4.hpp:218
virtual void print(std::ostream &os) const
Print relevant information of the RK4 integrator model.
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.
MatrixXs Lxu
Hessian of the cost function.
std::vector< MatrixXs > ddli_ddx
Definition: rk4.hpp:239
std::vector< MatrixXs > ddli_dxdw
Definition: rk4.hpp:245
MatrixXs Fu
Jacobian of the dynamics.