Crocoddyl
rk.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2022, University of Edinburgh, University of Trento,
5 // LAAS-CNRS, IRI: CSIC-UPC, Heriot-Watt University
6 // Copyright note valid unless otherwise stated in individual files.
7 // All rights reserved.
9 
10 #ifndef CROCODDYL_CORE_INTEGRATOR_RK_HPP_
11 #define CROCODDYL_CORE_INTEGRATOR_RK_HPP_
12 
13 #include "crocoddyl/core/fwd.hpp"
14 #include "crocoddyl/core/integ-action-base.hpp"
15 
16 namespace crocoddyl {
17 
18 enum RKType { two = 2, three = 3, four = 4 };
19 
38 template <typename _Scalar>
40  : public IntegratedActionModelAbstractTpl<_Scalar> {
41  public:
42  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43 
44  typedef _Scalar Scalar;
53  typedef typename MathBase::VectorXs VectorXs;
54  typedef typename MathBase::MatrixXs MatrixXs;
55 
66  boost::shared_ptr<DifferentialActionModelAbstract> model,
67  boost::shared_ptr<ControlParametrizationModelAbstract> control,
68  const RKType rktype, const Scalar time_step = Scalar(1e-3),
69  const bool with_cost_residual = true);
70 
83  boost::shared_ptr<DifferentialActionModelAbstract> model,
84  const RKType rktype, const Scalar time_step = Scalar(1e-3),
85  const bool with_cost_residual = true);
86  virtual ~IntegratedActionModelRKTpl();
87 
95  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
96  const Eigen::Ref<const VectorXs>& x,
97  const Eigen::Ref<const VectorXs>& u);
98 
109  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data,
110  const Eigen::Ref<const VectorXs>& x);
111 
119  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
120  const Eigen::Ref<const VectorXs>& x,
121  const Eigen::Ref<const VectorXs>& u);
122 
133  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data,
134  const Eigen::Ref<const VectorXs>& x);
135 
141  virtual boost::shared_ptr<ActionDataAbstract> createData();
142 
146  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
147 
161  virtual void quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data,
162  Eigen::Ref<VectorXs> u,
163  const Eigen::Ref<const VectorXs>& x,
164  const std::size_t maxiter = 100,
165  const Scalar tol = Scalar(1e-9));
166 
170  std::size_t get_ni() const;
171 
177  virtual void print(std::ostream& os) const;
178 
179  protected:
180  using Base::control_;
181  using Base::differential_;
182  using Base::ng_;
183  using Base::nh_;
184  using Base::nu_;
185  using Base::state_;
186  using Base::time_step2_;
187  using Base::time_step_;
190 
191  private:
195  void set_rk_type(const RKType rktype);
196 
197  std::vector<Scalar> rk_c_;
198  std::size_t ni_;
199 };
200 
201 template <typename _Scalar>
203  : public IntegratedActionDataAbstractTpl<_Scalar> {
204  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
205 
206  typedef _Scalar Scalar;
213  typedef typename MathBase::VectorXs VectorXs;
214  typedef typename MathBase::MatrixXs MatrixXs;
215 
216  template <template <typename Scalar> class Model>
217  explicit IntegratedActionDataRKTpl(Model<Scalar>* const model)
218  : Base(model),
219  integral(model->get_ni(), Scalar(0.)),
220  dx(model->get_state()->get_ndx()),
221  ki(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
222  y(model->get_ni(), VectorXs::Zero(model->get_state()->get_nx())),
223  ws(model->get_ni(), VectorXs::Zero(model->get_control()->get_nw())),
224  dx_rk(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
225  dki_dx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(),
226  model->get_state()->get_ndx())),
227  dki_du(model->get_ni(),
228  MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
229  dyi_dx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(),
230  model->get_state()->get_ndx())),
231  dyi_du(model->get_ni(),
232  MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
233  dli_dx(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
234  dli_du(model->get_ni(), VectorXs::Zero(model->get_nu())),
235  ddli_ddx(model->get_ni(),
236  MatrixXs::Zero(model->get_state()->get_ndx(),
237  model->get_state()->get_ndx())),
238  ddli_ddw(model->get_ni(),
239  MatrixXs::Zero(model->get_control()->get_nw(),
240  model->get_control()->get_nw())),
241  ddli_ddu(model->get_ni(),
242  MatrixXs::Zero(model->get_nu(), model->get_nu())),
243  ddli_dxdw(model->get_ni(),
244  MatrixXs::Zero(model->get_state()->get_ndx(),
245  model->get_control()->get_nw())),
246  ddli_dxdu(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(),
247  model->get_nu())),
248  ddli_dwdu(
249  model->get_ni(),
250  MatrixXs::Zero(model->get_control()->get_nw(), model->get_nu())),
251  Luu_partialx(model->get_ni(),
252  MatrixXs::Zero(model->get_nu(), model->get_nu())),
253  Lxu_i(model->get_ni(),
254  MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
255  Lxx_partialx(model->get_ni(),
256  MatrixXs::Zero(model->get_state()->get_ndx(),
257  model->get_state()->get_ndx())),
258  Lxx_partialu(
259  model->get_ni(),
260  MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())) {
261  dx.setZero();
262 
263  for (std::size_t i = 0; i < model->get_ni(); ++i) {
264  differential.push_back(boost::shared_ptr<DifferentialActionDataAbstract>(
265  model->get_differential()->createData()));
266  control.push_back(boost::shared_ptr<ControlParametrizationDataAbstract>(
267  model->get_control()->createData()));
268  }
269 
270  const std::size_t nv = model->get_state()->get_nv();
271  dyi_dx[0].diagonal().setOnes();
272  dki_dx[0].topRightCorner(nv, nv).diagonal().setOnes();
273  }
274  virtual ~IntegratedActionDataRKTpl() {}
275 
276  std::vector<boost::shared_ptr<DifferentialActionDataAbstract> >
278  std::vector<boost::shared_ptr<ControlParametrizationDataAbstract> >
280  std::vector<Scalar> integral;
281  VectorXs dx;
282  std::vector<VectorXs> ki;
283  std::vector<VectorXs>
284  y;
285  std::vector<VectorXs> ws;
286  std::vector<VectorXs> dx_rk;
287 
288  std::vector<MatrixXs>
291  std::vector<MatrixXs>
294 
295  std::vector<MatrixXs>
298  std::vector<MatrixXs>
301 
302  std::vector<VectorXs>
305  std::vector<VectorXs>
308 
309  std::vector<MatrixXs>
312  std::vector<MatrixXs>
316  std::vector<MatrixXs> ddli_ddu;
319  std::vector<MatrixXs>
323  std::vector<MatrixXs>
327  std::vector<MatrixXs>
331 
332  std::vector<MatrixXs> Luu_partialx;
333  std::vector<MatrixXs> Lxu_i;
334  std::vector<MatrixXs> Lxx_partialx;
335  std::vector<MatrixXs> Lxx_partialu;
336 
337  using Base::cost;
338  using Base::Fu;
339  using Base::Fx;
340  using Base::Lu;
341  using Base::Luu;
342  using Base::Lx;
343  using Base::Lxu;
344  using Base::Lxx;
345  using Base::r;
346  using Base::xnext;
347 };
348 
349 } // namespace crocoddyl
350 
351 /* --- Details -------------------------------------------------------------- */
352 /* --- Details -------------------------------------------------------------- */
353 /* --- Details -------------------------------------------------------------- */
354 #include "crocoddyl/core/integrator/rk.hxx"
355 
356 #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 RK integrator.
Definition: rk.hpp:40
IntegratedActionModelRKTpl(boost::shared_ptr< DifferentialActionModelAbstract > model, const RKType rktype, const Scalar time_step=Scalar(1e-3), const bool with_cost_residual=true)
Initialize the RK integrator.
virtual void print(std::ostream &os) const
Print relevant information of the RK integrator model.
IntegratedActionModelRKTpl(boost::shared_ptr< DifferentialActionModelAbstract > model, boost::shared_ptr< ControlParametrizationModelAbstract > control, const RKType rktype, const Scalar time_step=Scalar(1e-3), const bool with_cost_residual=true)
Initialize the RK integrator.
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 RK scheme.
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 RK 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 RK scheme.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the partial derivatives of the cost.
std::size_t get_ni() const
Return the number of nodes of the integrator.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the RK 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: rk.hpp:310
std::vector< VectorXs > ki
List of RK terms related to system dynamics.
Definition: rk.hpp:282
std::vector< MatrixXs > dki_du
Definition: rk.hpp:292
std::vector< MatrixXs > dki_dx
Definition: rk.hpp:289
std::vector< MatrixXs > ddli_ddu
Definition: rk.hpp:316
std::vector< MatrixXs > ddli_dxdw
Definition: rk.hpp:320
std::vector< MatrixXs > ddli_ddw
Definition: rk.hpp:313
std::vector< boost::shared_ptr< DifferentialActionDataAbstract > > differential
List of differential model data.
Definition: rk.hpp:277
std::vector< MatrixXs > ddli_dwdu
Definition: rk.hpp:328
std::vector< MatrixXs > dyi_du
Definition: rk.hpp:299
std::vector< VectorXs > y
List of states where f is evaluated in the RK integration.
Definition: rk.hpp:284
std::vector< VectorXs > dli_du
Definition: rk.hpp:306
std::vector< MatrixXs > ddli_dxdu
Definition: rk.hpp:324
std::vector< VectorXs > dli_dx
Definition: rk.hpp:303
VectorXs dx
State rate.
Definition: rk.hpp:281
std::vector< boost::shared_ptr< ControlParametrizationDataAbstract > > control
List of control parametrization data.
Definition: rk.hpp:279
std::vector< MatrixXs > dyi_dx
Definition: rk.hpp:296
std::vector< VectorXs > ws
Control inputs evaluated in the RK integration.
Definition: rk.hpp:285