crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
rk.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, University of Edinburgh, University of Trento,
5 // LAAS-CNRS, IRI: CSIC-UPC
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 
34 template <typename _Scalar>
35 class IntegratedActionModelRKTpl : public IntegratedActionModelAbstractTpl<_Scalar> {
36  public:
37  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38 
39  typedef _Scalar Scalar;
40  typedef MathBaseTpl<Scalar> MathBase;
41  typedef IntegratedActionModelAbstractTpl<Scalar> Base;
42  typedef IntegratedActionDataRKTpl<Scalar> Data;
43  typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
44  typedef DifferentialActionModelAbstractTpl<Scalar> DifferentialActionModelAbstract;
45  typedef ControlParametrizationModelAbstractTpl<Scalar> ControlParametrizationModelAbstract;
46  typedef typename MathBase::VectorXs VectorXs;
47  typedef typename MathBase::MatrixXs MatrixXs;
48 
58  IntegratedActionModelRKTpl(boost::shared_ptr<DifferentialActionModelAbstract> model,
59  boost::shared_ptr<ControlParametrizationModelAbstract> control, const RKType rktype,
60  const Scalar time_step = Scalar(1e-3), const bool with_cost_residual = true);
61 
72  IntegratedActionModelRKTpl(boost::shared_ptr<DifferentialActionModelAbstract> model, const RKType rktype,
73  const Scalar time_step = Scalar(1e-3), const bool with_cost_residual = true);
74  virtual ~IntegratedActionModelRKTpl();
75 
83  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
84  const Eigen::Ref<const VectorXs>& u);
85 
95  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
96 
104  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
105  const Eigen::Ref<const VectorXs>& u);
106 
116  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
117 
123  virtual boost::shared_ptr<ActionDataAbstract> createData();
124 
128  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
129 
142  virtual void quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
143  const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter = 100,
144  const Scalar tol = Scalar(1e-9));
145 
149  std::size_t get_ni() const;
150 
156  virtual void print(std::ostream& os) const;
157 
158  protected:
159  using Base::control_;
160  using Base::differential_;
161  using Base::nu_;
162  using Base::state_;
163  using Base::time_step2_;
164  using Base::time_step_;
166 
167  private:
171  void set_rk_type(const RKType rktype);
172 
173  std::vector<Scalar> rk_c_;
174  std::size_t ni_;
175 };
176 
177 template <typename _Scalar>
178 struct IntegratedActionDataRKTpl : public IntegratedActionDataAbstractTpl<_Scalar> {
179  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
180 
181  typedef _Scalar Scalar;
182  typedef MathBaseTpl<Scalar> MathBase;
183  typedef IntegratedActionDataAbstractTpl<Scalar> Base;
184  typedef DifferentialActionDataAbstractTpl<Scalar> DifferentialActionDataAbstract;
185  typedef ControlParametrizationDataAbstractTpl<Scalar> ControlParametrizationDataAbstract;
186  typedef typename MathBase::VectorXs VectorXs;
187  typedef typename MathBase::MatrixXs MatrixXs;
188 
189  template <template <typename Scalar> class Model>
190  explicit IntegratedActionDataRKTpl(Model<Scalar>* const model)
191  : Base(model),
192  integral(model->get_ni(), Scalar(0.)),
193  dx(model->get_state()->get_ndx()),
194  ki(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
195  y(model->get_ni(), VectorXs::Zero(model->get_state()->get_nx())),
196  ws(model->get_ni(), VectorXs::Zero(model->get_control()->get_nw())),
197  dx_rk(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
198  dki_dx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
199  dki_du(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
200  dyi_dx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
201  dyi_du(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
202  dli_dx(model->get_ni(), VectorXs::Zero(model->get_state()->get_ndx())),
203  dli_du(model->get_ni(), VectorXs::Zero(model->get_nu())),
204  ddli_ddx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
205  ddli_ddw(model->get_ni(), MatrixXs::Zero(model->get_control()->get_nw(), model->get_control()->get_nw())),
206  ddli_ddu(model->get_ni(), MatrixXs::Zero(model->get_nu(), model->get_nu())),
207  ddli_dxdw(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_control()->get_nw())),
208  ddli_dxdu(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
209  ddli_dwdu(model->get_ni(), MatrixXs::Zero(model->get_control()->get_nw(), model->get_nu())),
210  Luu_partialx(model->get_ni(), MatrixXs::Zero(model->get_nu(), model->get_nu())),
211  Lxu_i(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
212  Lxx_partialx(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
213  Lxx_partialu(model->get_ni(), MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())) {
214  dx.setZero();
215 
216  for (std::size_t i = 0; i < model->get_ni(); ++i) {
217  differential.push_back(
218  boost::shared_ptr<DifferentialActionDataAbstract>(model->get_differential()->createData()));
219  control.push_back(boost::shared_ptr<ControlParametrizationDataAbstract>(model->get_control()->createData()));
220  }
221 
222  const std::size_t nv = model->get_state()->get_nv();
223  dyi_dx[0].diagonal().setOnes();
224  dki_dx[0].topRightCorner(nv, nv).diagonal().setOnes();
225  }
226  virtual ~IntegratedActionDataRKTpl() {}
227 
228  std::vector<boost::shared_ptr<DifferentialActionDataAbstract> > differential;
229  std::vector<boost::shared_ptr<ControlParametrizationDataAbstract> >
231  std::vector<Scalar> integral;
232  VectorXs dx;
233  std::vector<VectorXs> ki;
234  std::vector<VectorXs> y;
235  std::vector<VectorXs> ws;
236  std::vector<VectorXs> dx_rk;
237 
238  std::vector<MatrixXs>
240  std::vector<MatrixXs> dki_du;
241 
243  std::vector<MatrixXs>
245  std::vector<MatrixXs> dyi_du;
246 
248  std::vector<VectorXs>
250  std::vector<VectorXs> dli_du;
251 
253  std::vector<MatrixXs> ddli_ddx;
254  std::vector<MatrixXs> ddli_ddw;
256  std::vector<MatrixXs> ddli_ddu;
258  std::vector<MatrixXs> ddli_dxdw;
260  std::vector<MatrixXs> ddli_dxdu;
262  std::vector<MatrixXs> ddli_dwdu;
264 
266  std::vector<MatrixXs> Luu_partialx;
267  std::vector<MatrixXs> Lxu_i;
268  std::vector<MatrixXs> Lxx_partialx;
269  std::vector<MatrixXs> Lxx_partialu;
270 
271  using Base::cost;
272  using Base::Fu;
273  using Base::Fx;
274  using Base::Lu;
275  using Base::Luu;
276  using Base::Lx;
277  using Base::Lxu;
278  using Base::Lxx;
279  using Base::r;
280  using Base::xnext;
281 };
282 
283 } // namespace crocoddyl
284 
285 /* --- Details -------------------------------------------------------------- */
286 /* --- Details -------------------------------------------------------------- */
287 /* --- Details -------------------------------------------------------------- */
288 #include "crocoddyl/core/integrator/rk.hxx"
289 
290 #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:117
crocoddyl::IntegratedActionModelAbstractTpl::with_cost_residual_
bool with_cost_residual_
Flag indicating whether a cost residual is used.
Definition: integ-action-base.hpp:122
crocoddyl::IntegratedActionDataRKTpl::ddli_ddx
std::vector< MatrixXs > ddli_ddx
Definition: rk.hpp:253
crocoddyl::IntegratedActionDataRKTpl::dx
VectorXs dx
State rate.
Definition: rk.hpp:232
crocoddyl::IntegratedActionDataRKTpl::dyi_dx
std::vector< MatrixXs > dyi_dx
List of partial derivatives of RK dynamics with respect to the state of the RK integrator....
Definition: rk.hpp:244
crocoddyl::IntegratedActionDataRKTpl::dki_dx
std::vector< MatrixXs > dki_dx
List of partial derivatives of RK nodes with respect to the state of the RK integration....
Definition: rk.hpp:239
crocoddyl::IntegratedActionModelRKTpl::createData
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the RK integrator data.
crocoddyl::IntegratedActionDataRKTpl::dli_dx
std::vector< VectorXs > dli_dx
List of partial derivatives of the cost with respect to the state of the RK integration....
Definition: rk.hpp:249
crocoddyl::IntegratedActionDataRKTpl::dki_du
std::vector< MatrixXs > dki_du
Definition: rk.hpp:240
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:257
crocoddyl::IntegratedActionDataRKTpl::ki
std::vector< VectorXs > ki
List of RK terms related to system dynamics.
Definition: rk.hpp:233
crocoddyl::IntegratedActionModelAbstractTpl::time_step2_
Scalar time_step2_
Square of the time step used for integration.
Definition: integ-action-base.hpp:121
crocoddyl::IntegratedActionModelAbstractTpl::time_step_
Scalar time_step_
Time step used for integration.
Definition: integ-action-base.hpp:120
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:255
crocoddyl::IntegratedActionDataRKTpl::ddli_dxdu
std::vector< MatrixXs > ddli_dxdu
Definition: rk.hpp:261
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:230
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:228
crocoddyl::IntegratedActionDataRKTpl::ws
std::vector< VectorXs > ws
Control inputs evaluated in the RK integration.
Definition: rk.hpp:235
crocoddyl::IntegratedActionDataRKTpl::dli_du
std::vector< VectorXs > dli_du
Definition: rk.hpp:250
crocoddyl::IntegratedActionModelAbstractTpl::control_
boost::shared_ptr< ControlParametrizationModelAbstract > control_
Model of the control parametrization.
Definition: integ-action-base.hpp:118
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:259
crocoddyl::IntegratedActionDataRKTpl::y
std::vector< VectorXs > y
List of states where f is evaluated in the RK integration.
Definition: rk.hpp:234
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:263
crocoddyl::IntegratedActionDataRKTpl::dyi_du
std::vector< MatrixXs > dyi_du
Definition: rk.hpp:245