Crocoddyl
rk4.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2022, LAAS-CNRS, IRI: CSIC-UPC, University of Edinburgh
5 // Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #ifndef CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
11 #define CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
12 
13 #include "crocoddyl/core/fwd.hpp"
14 #include "crocoddyl/core/integ-action-base.hpp"
15 #include "crocoddyl/core/utils/deprecate.hpp"
16 
17 namespace crocoddyl {
18 
36 template <typename _Scalar>
38  : public IntegratedActionModelAbstractTpl<_Scalar> {
39  public:
40  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41 
42  typedef _Scalar Scalar;
51  typedef typename MathBase::VectorXs VectorXs;
52  typedef typename MathBase::MatrixXs MatrixXs;
53 
62  DEPRECATED("Use IntegratedActionModelRK",
64  boost::shared_ptr<DifferentialActionModelAbstract> model,
65  boost::shared_ptr<ControlParametrizationModelAbstract> control,
66  const Scalar time_step = Scalar(1e-3),
67  const bool with_cost_residual = true);)
68 
79  DEPRECATED("Use IntegratedActionModelRK",
81  boost::shared_ptr<DifferentialActionModelAbstract> model,
82  const Scalar time_step = Scalar(1e-3),
83  const bool with_cost_residual = true);)
84  virtual ~IntegratedActionModelRK4Tpl();
85 
93  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
94  const Eigen::Ref<const VectorXs>& x,
95  const Eigen::Ref<const VectorXs>& u);
96 
107  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
108  const Eigen::Ref<const VectorXs>& x);
109 
117  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
118  const Eigen::Ref<const VectorXs>& x,
119  const Eigen::Ref<const VectorXs>& u);
120 
131  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
132  const Eigen::Ref<const VectorXs>& x);
133 
139  virtual boost::shared_ptr<ActionDataAbstract> createData();
140 
144  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
145 
159  virtual void quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data,
160  Eigen::Ref<VectorXs> u,
161  const Eigen::Ref<const VectorXs>& x,
162  const std::size_t maxiter = 100,
163  const Scalar tol = Scalar(1e-9));
164 
170  virtual void print(std::ostream& os) const;
171 
172  protected:
173  using Base::control_;
174  using Base::differential_;
175  using Base::ng_;
176  using Base::nh_;
177  using Base::nu_;
178  using Base::state_;
179  using Base::time_step2_;
180  using Base::time_step_;
183 
184  private:
185  std::array<Scalar, 4> rk4_c_;
186 };
187 
188 template <typename _Scalar>
190  : public IntegratedActionDataAbstractTpl<_Scalar> {
191  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
192 
193  typedef _Scalar Scalar;
200  typedef typename MathBase::VectorXs VectorXs;
201  typedef typename MathBase::MatrixXs MatrixXs;
202 
203  template <template <typename Scalar> class Model>
204  explicit IntegratedActionDataRK4Tpl(Model<Scalar>* const model)
205  : Base(model),
206  integral(4, Scalar(0.)),
207  dx(model->get_state()->get_ndx()),
208  ki(4, VectorXs::Zero(model->get_state()->get_ndx())),
209  y(4, VectorXs::Zero(model->get_state()->get_nx())),
210  ws(4, VectorXs::Zero(model->get_control()->get_nw())),
211  dx_rk4(4, VectorXs::Zero(model->get_state()->get_ndx())),
212  dki_dx(4, MatrixXs::Zero(model->get_state()->get_ndx(),
213  model->get_state()->get_ndx())),
214  dki_du(4,
215  MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
216  dyi_dx(4, MatrixXs::Zero(model->get_state()->get_ndx(),
217  model->get_state()->get_ndx())),
218  dyi_du(4,
219  MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
220  dli_dx(4, VectorXs::Zero(model->get_state()->get_ndx())),
221  dli_du(4, VectorXs::Zero(model->get_nu())),
222  ddli_ddx(4, MatrixXs::Zero(model->get_state()->get_ndx(),
223  model->get_state()->get_ndx())),
224  ddli_ddw(4, MatrixXs::Zero(model->get_control()->get_nw(),
225  model->get_control()->get_nw())),
226  ddli_ddu(4, MatrixXs::Zero(model->get_nu(), model->get_nu())),
227  ddli_dxdw(4, MatrixXs::Zero(model->get_state()->get_ndx(),
228  model->get_control()->get_nw())),
229  ddli_dxdu(
230  4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
231  ddli_dwdu(
232  4, MatrixXs::Zero(model->get_control()->get_nw(), model->get_nu())),
233  Luu_partialx(4, MatrixXs::Zero(model->get_nu(), model->get_nu())),
234  Lxu_i(4,
235  MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
236  Lxx_partialx(4, MatrixXs::Zero(model->get_state()->get_ndx(),
237  model->get_state()->get_ndx())),
238  Lxx_partialu(
239  4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())) {
240  dx.setZero();
241 
242  for (std::size_t i = 0; i < 4; ++i) {
243  differential.push_back(boost::shared_ptr<DifferentialActionDataAbstract>(
244  model->get_differential()->createData()));
245  control.push_back(boost::shared_ptr<ControlParametrizationDataAbstract>(
246  model->get_control()->createData()));
247  }
248 
249  const std::size_t nv = model->get_state()->get_nv();
250  dyi_dx[0].diagonal().setOnes();
251  dki_dx[0].topRightCorner(nv, nv).diagonal().setOnes();
252  }
253  virtual ~IntegratedActionDataRK4Tpl() {}
254 
255  std::vector<boost::shared_ptr<DifferentialActionDataAbstract> >
257  std::vector<boost::shared_ptr<ControlParametrizationDataAbstract> >
259  std::vector<Scalar> integral;
260  VectorXs dx;
261  std::vector<VectorXs> ki;
262  std::vector<VectorXs>
263  y;
264  std::vector<VectorXs>
265  ws;
266  std::vector<VectorXs> dx_rk4;
267 
268  std::vector<MatrixXs>
271  std::vector<MatrixXs>
274 
275  std::vector<MatrixXs>
278  std::vector<MatrixXs>
281 
282  std::vector<VectorXs>
285  std::vector<VectorXs>
288 
289  std::vector<MatrixXs>
292  std::vector<MatrixXs>
296  std::vector<MatrixXs> ddli_ddu;
299  std::vector<MatrixXs>
303  std::vector<MatrixXs>
307  std::vector<MatrixXs>
311 
312  std::vector<MatrixXs> Luu_partialx;
313  std::vector<MatrixXs> Lxu_i;
314  std::vector<MatrixXs> Lxx_partialx;
315  std::vector<MatrixXs> Lxx_partialu;
316 
317  using Base::cost;
318  using Base::Fu;
319  using Base::Fx;
320  using Base::Lu;
321  using Base::Luu;
322  using Base::Lx;
323  using Base::Lxu;
324  using Base::Lxx;
325  using Base::r;
326  using Base::xnext;
327 };
328 
329 } // namespace crocoddyl
330 
331 /* --- Details -------------------------------------------------------------- */
332 /* --- Details -------------------------------------------------------------- */
333 /* --- Details -------------------------------------------------------------- */
334 #include "crocoddyl/core/integrator/rk4.hxx"
335 
336 #endif // CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
Abstract class for action model.
Definition: action-base.hpp:95
std::size_t nh_
Number of equality constraints.
std::size_t ng_
Number of inequality constraints.
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:38
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 w.r.t. the state .
MatrixXs Fu
Jacobian of the dynamics w.r.t. the control .
MatrixXs Luu
Hessian of the cost w.r.t. the control .
VectorXs Lx
Jacobian of the cost w.r.t. the state .
MatrixXs Lxx
Hessian of the cost w.r.t. the state .
VectorXs Lu
Jacobian of the cost w.r.t. the control .
std::vector< MatrixXs > ddli_ddx
Definition: rk4.hpp:290
std::vector< VectorXs > ki
List of RK4 terms related to system dynamics.
Definition: rk4.hpp:261
std::vector< MatrixXs > dki_du
Definition: rk4.hpp:272
std::vector< MatrixXs > dki_dx
Definition: rk4.hpp:269
std::vector< MatrixXs > ddli_ddu
Definition: rk4.hpp:296
std::vector< MatrixXs > ddli_dxdw
Definition: rk4.hpp:300
std::vector< MatrixXs > ddli_ddw
Definition: rk4.hpp:293
std::vector< boost::shared_ptr< DifferentialActionDataAbstract > > differential
List of differential model data.
Definition: rk4.hpp:256
std::vector< MatrixXs > ddli_dwdu
Definition: rk4.hpp:308
std::vector< MatrixXs > dyi_du
Definition: rk4.hpp:279
std::vector< VectorXs > y
List of states where f is evaluated in the RK4 integration.
Definition: rk4.hpp:263
std::vector< VectorXs > dli_du
Definition: rk4.hpp:286
std::vector< MatrixXs > ddli_dxdu
Definition: rk4.hpp:304
std::vector< VectorXs > dli_dx
Definition: rk4.hpp:283
std::vector< boost::shared_ptr< ControlParametrizationDataAbstract > > control
List of control parametrization data.
Definition: rk4.hpp:258
std::vector< MatrixXs > dyi_dx
Definition: rk4.hpp:276
std::vector< VectorXs > ws
Control inputs evaluated in the RK4 integration.
Definition: rk4.hpp:265