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>
36  public:
37  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
38 
39  typedef _Scalar Scalar;
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>
179  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
180 
181  typedef _Scalar Scalar;
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;
242 
243  std::vector<MatrixXs>
245  std::vector<MatrixXs> dyi_du;
247 
248  std::vector<VectorXs>
250  std::vector<VectorXs> dli_du;
252 
253  std::vector<MatrixXs> ddli_ddx;
255  std::vector<MatrixXs> ddli_ddw;
257  std::vector<MatrixXs> ddli_ddu;
259  std::vector<MatrixXs> ddli_dxdw;
261  std::vector<MatrixXs> ddli_dxdu;
263  std::vector<MatrixXs> ddli_dwdu;
265 
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_
Abstract class for action model.
Definition: action-base.hpp:59
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:35
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.
MatrixXs Fu
Jacobian of the dynamics.
MatrixXs Luu
Hessian of the cost function.
VectorXs Lx
Jacobian of the cost function.
MatrixXs Lxx
Hessian of the cost function.
VectorXs Lu
Jacobian of the cost function.
MatrixXs Lxu
Hessian of the cost function.
std::vector< MatrixXs > ddli_ddx
Definition: rk.hpp:253
std::vector< VectorXs > ki
List of RK terms related to system dynamics.
Definition: rk.hpp:233
std::vector< MatrixXs > dki_du
Definition: rk.hpp:240
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
std::vector< MatrixXs > ddli_ddu
Definition: rk.hpp:257
std::vector< MatrixXs > ddli_dxdw
Definition: rk.hpp:259
std::vector< MatrixXs > ddli_ddw
Definition: rk.hpp:255
std::vector< boost::shared_ptr< DifferentialActionDataAbstract > > differential
List of differential model data.
Definition: rk.hpp:228
std::vector< MatrixXs > ddli_dwdu
Definition: rk.hpp:263
std::vector< MatrixXs > dyi_du
Definition: rk.hpp:245
std::vector< VectorXs > y
List of states where f is evaluated in the RK integration.
Definition: rk.hpp:234
std::vector< VectorXs > dli_du
Definition: rk.hpp:250
std::vector< MatrixXs > ddli_dxdu
Definition: rk.hpp:261
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
VectorXs dx
State rate.
Definition: rk.hpp:232
std::vector< boost::shared_ptr< ControlParametrizationDataAbstract > > control
List of control parametrization data.
Definition: rk.hpp:230
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
std::vector< VectorXs > ws
Control inputs evaluated in the RK integration.
Definition: rk.hpp:235