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>
39 class IntegratedActionModelRKTpl
40  : public IntegratedActionModelAbstractTpl<_Scalar> {
41  public:
42  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
43 
44  typedef _Scalar Scalar;
45  typedef MathBaseTpl<Scalar> MathBase;
46  typedef IntegratedActionModelAbstractTpl<Scalar> Base;
47  typedef IntegratedActionDataRKTpl<Scalar> Data;
48  typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
49  typedef DifferentialActionModelAbstractTpl<Scalar>
50  DifferentialActionModelAbstract;
51  typedef ControlParametrizationModelAbstractTpl<Scalar>
52  ControlParametrizationModelAbstract;
53  typedef typename MathBase::VectorXs VectorXs;
54  typedef typename MathBase::MatrixXs MatrixXs;
55 
65  IntegratedActionModelRKTpl(
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 
82  IntegratedActionModelRKTpl(
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_;
188  using Base::with_cost_residual_;
189 
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>
202 struct IntegratedActionDataRKTpl
203  : public IntegratedActionDataAbstractTpl<_Scalar> {
204  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
205 
206  typedef _Scalar Scalar;
207  typedef MathBaseTpl<Scalar> MathBase;
208  typedef IntegratedActionDataAbstractTpl<Scalar> Base;
209  typedef DifferentialActionDataAbstractTpl<Scalar>
210  DifferentialActionDataAbstract;
211  typedef ControlParametrizationDataAbstractTpl<Scalar>
212  ControlParametrizationDataAbstract;
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> >
277  differential;
278  std::vector<boost::shared_ptr<ControlParametrizationDataAbstract> >
279  control;
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>
289  dki_dx;
290  std::vector<MatrixXs>
292  dki_du;
293 
295  std::vector<MatrixXs>
296  dyi_dx;
297  std::vector<MatrixXs>
299  dyi_du;
300 
302  std::vector<VectorXs>
303  dli_dx;
304  std::vector<VectorXs>
306  dli_du;
307 
309  std::vector<MatrixXs>
310  ddli_ddx;
311  std::vector<MatrixXs>
313  ddli_ddw;
314  std::vector<MatrixXs> ddli_ddu;
317  std::vector<MatrixXs>
320  ddli_dxdw;
321  std::vector<MatrixXs>
324  ddli_dxdu;
325  std::vector<MatrixXs>
328  ddli_dwdu;
329 
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_
crocoddyl::IntegratedActionModelAbstractTpl::differential_
boost::shared_ptr< DifferentialActionModelAbstract > differential_
Differential action model that is integrated.
Definition: integ-action-base.hpp:156
crocoddyl::IntegratedActionModelAbstractTpl::with_cost_residual_
bool with_cost_residual_
Flag indicating whether a cost residual is used.
Definition: integ-action-base.hpp:163
crocoddyl::IntegratedActionDataRKTpl::ddli_ddx
std::vector< MatrixXs > ddli_ddx
Definition: rk.hpp:310
crocoddyl::IntegratedActionDataRKTpl::dx
VectorXs dx
State rate.
Definition: rk.hpp:281
crocoddyl::IntegratedActionDataRKTpl::dyi_dx
std::vector< MatrixXs > dyi_dx
Definition: rk.hpp:296
crocoddyl::IntegratedActionDataRKTpl::dki_dx
std::vector< MatrixXs > dki_dx
Definition: rk.hpp:289
crocoddyl::IntegratedActionModelRKTpl::createData
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the RK integrator data.
crocoddyl::IntegratedActionDataRKTpl::dli_dx
std::vector< VectorXs > dli_dx
Definition: rk.hpp:303
crocoddyl::IntegratedActionDataRKTpl::dki_du
std::vector< MatrixXs > dki_du
Definition: rk.hpp:292
crocoddyl::IntegratedActionModelRKTpl::print
virtual void print(std::ostream &os) const
Print relevant information of the RK integrator model.
crocoddyl::IntegratedActionDataRKTpl::ddli_ddu
std::vector< MatrixXs > ddli_ddu
Definition: rk.hpp:316
crocoddyl::IntegratedActionDataRKTpl::ki
std::vector< VectorXs > ki
List of RK terms related to system dynamics.
Definition: rk.hpp:282
crocoddyl::IntegratedActionModelAbstractTpl::time_step2_
Scalar time_step2_
Square of the time step used for integration.
Definition: integ-action-base.hpp:161
crocoddyl::IntegratedActionModelAbstractTpl::time_step_
Scalar time_step_
Time step used for integration.
Definition: integ-action-base.hpp:160
crocoddyl::IntegratedActionModelRKTpl::get_ni
std::size_t get_ni() const
Return the number of nodes of the integrator.
crocoddyl::IntegratedActionDataRKTpl::ddli_ddw
std::vector< MatrixXs > ddli_ddw
Definition: rk.hpp:313
crocoddyl::IntegratedActionDataRKTpl::ddli_dxdu
std::vector< MatrixXs > ddli_dxdu
Definition: rk.hpp:324
crocoddyl::IntegratedActionModelRKTpl::calc
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.
crocoddyl::IntegratedActionDataRKTpl::control
std::vector< boost::shared_ptr< ControlParametrizationDataAbstract > > control
List of control parametrization data.
Definition: rk.hpp:279
crocoddyl::IntegratedActionModelRKTpl::quasiStatic
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.
crocoddyl::IntegratedActionModelRKTpl::checkData
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
crocoddyl::IntegratedActionDataRKTpl::differential
std::vector< boost::shared_ptr< DifferentialActionDataAbstract > > differential
List of differential model data.
Definition: rk.hpp:277
crocoddyl::IntegratedActionDataRKTpl::ws
std::vector< VectorXs > ws
Control inputs evaluated in the RK integration.
Definition: rk.hpp:285
crocoddyl::IntegratedActionDataRKTpl::dli_du
std::vector< VectorXs > dli_du
Definition: rk.hpp:306
crocoddyl::IntegratedActionModelAbstractTpl::control_
boost::shared_ptr< ControlParametrizationModelAbstract > control_
Model of the control parametrization.
Definition: integ-action-base.hpp:158
crocoddyl::IntegratedActionModelRKTpl::calcDiff
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.
crocoddyl::IntegratedActionDataRKTpl::ddli_dxdw
std::vector< MatrixXs > ddli_dxdw
Definition: rk.hpp:320
crocoddyl::IntegratedActionDataRKTpl::y
std::vector< VectorXs > y
List of states where f is evaluated in the RK integration.
Definition: rk.hpp:284
crocoddyl::IntegratedActionModelRKTpl::IntegratedActionModelRKTpl
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.
crocoddyl::IntegratedActionDataRKTpl::ddli_dwdu
std::vector< MatrixXs > ddli_dwdu
Definition: rk.hpp:328
crocoddyl::IntegratedActionDataRKTpl::dyi_du
std::vector< MatrixXs > dyi_du
Definition: rk.hpp:299