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>
37 class IntegratedActionModelRK4Tpl
38  : public IntegratedActionModelAbstractTpl<_Scalar> {
39  public:
40  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
41 
42  typedef _Scalar Scalar;
43  typedef MathBaseTpl<Scalar> MathBase;
44  typedef IntegratedActionModelAbstractTpl<Scalar> Base;
45  typedef IntegratedActionDataRK4Tpl<Scalar> Data;
46  typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
47  typedef DifferentialActionModelAbstractTpl<Scalar>
48  DifferentialActionModelAbstract;
49  typedef ControlParametrizationModelAbstractTpl<Scalar>
50  ControlParametrizationModelAbstract;
51  typedef typename MathBase::VectorXs VectorXs;
52  typedef typename MathBase::MatrixXs MatrixXs;
53 
62  DEPRECATED("Use IntegratedActionModelRK",
63  IntegratedActionModelRK4Tpl(
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",
80  IntegratedActionModelRK4Tpl(
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_;
181  using Base::with_cost_residual_;
182 
184  private:
185  std::array<Scalar, 4> rk4_c_;
186 };
187 
188 template <typename _Scalar>
189 struct IntegratedActionDataRK4Tpl
190  : public IntegratedActionDataAbstractTpl<_Scalar> {
191  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
192 
193  typedef _Scalar Scalar;
194  typedef MathBaseTpl<Scalar> MathBase;
195  typedef IntegratedActionDataAbstractTpl<Scalar> Base;
196  typedef DifferentialActionDataAbstractTpl<Scalar>
197  DifferentialActionDataAbstract;
198  typedef ControlParametrizationDataAbstractTpl<Scalar>
199  ControlParametrizationDataAbstract;
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> >
256  differential;
257  std::vector<boost::shared_ptr<ControlParametrizationDataAbstract> >
258  control;
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>
269  dki_dx;
270  std::vector<MatrixXs>
272  dki_du;
273 
275  std::vector<MatrixXs>
276  dyi_dx;
277  std::vector<MatrixXs>
279  dyi_du;
280 
282  std::vector<VectorXs>
283  dli_dx;
284  std::vector<VectorXs>
286  dli_du;
287 
289  std::vector<MatrixXs>
290  ddli_ddx;
291  std::vector<MatrixXs>
293  ddli_ddw;
294  std::vector<MatrixXs> ddli_ddu;
297  std::vector<MatrixXs>
300  ddli_dxdw;
301  std::vector<MatrixXs>
304  ddli_dxdu;
305  std::vector<MatrixXs>
308  ddli_dwdu;
309 
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_
crocoddyl::IntegratedActionModelAbstractTpl::differential_
boost::shared_ptr< DifferentialActionModelAbstract > differential_
Differential action model that is integrated.
Definition: integ-action-base.hpp:156
crocoddyl::IntegratedActionDataRK4Tpl::differential
std::vector< boost::shared_ptr< DifferentialActionDataAbstract > > differential
List of differential model data.
Definition: rk4.hpp:256
crocoddyl::IntegratedActionModelAbstractTpl::with_cost_residual_
bool with_cost_residual_
Flag indicating whether a cost residual is used.
Definition: integ-action-base.hpp:163
crocoddyl::IntegratedActionDataRK4Tpl::ddli_dwdu
std::vector< MatrixXs > ddli_dwdu
Definition: rk4.hpp:308
crocoddyl::IntegratedActionModelRK4Tpl::DEPRECATED
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.
crocoddyl::IntegratedActionDataRK4Tpl::y
std::vector< VectorXs > y
List of states where f is evaluated in the RK4 integration.
Definition: rk4.hpp:263
crocoddyl::IntegratedActionDataRK4Tpl::dyi_du
std::vector< MatrixXs > dyi_du
Definition: rk4.hpp:279
crocoddyl::IntegratedActionDataRK4Tpl::ddli_ddw
std::vector< MatrixXs > ddli_ddw
Definition: rk4.hpp:293
crocoddyl::IntegratedActionModelRK4Tpl::createData
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the RK4 integrator data.
crocoddyl::IntegratedActionDataRK4Tpl::control
std::vector< boost::shared_ptr< ControlParametrizationDataAbstract > > control
List of control parametrization data.
Definition: rk4.hpp:258
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::IntegratedActionModelRK4Tpl::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 RK4 integrator.
crocoddyl::IntegratedActionModelRK4Tpl::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::IntegratedActionDataRK4Tpl::dki_du
std::vector< MatrixXs > dki_du
Definition: rk4.hpp:272
crocoddyl::IntegratedActionModelRK4Tpl::print
virtual void print(std::ostream &os) const
Print relevant information of the RK4 integrator model.
crocoddyl::IntegratedActionDataRK4Tpl::dki_dx
std::vector< MatrixXs > dki_dx
Definition: rk4.hpp:269
crocoddyl::IntegratedActionDataRK4Tpl::ddli_ddu
std::vector< MatrixXs > ddli_ddu
Definition: rk4.hpp:296
crocoddyl::IntegratedActionModelRK4Tpl::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 RK4 scheme.
crocoddyl::IntegratedActionDataRK4Tpl::dyi_dx
std::vector< MatrixXs > dyi_dx
Definition: rk4.hpp:276
crocoddyl::IntegratedActionModelAbstractTpl::control_
boost::shared_ptr< ControlParametrizationModelAbstract > control_
Model of the control parametrization.
Definition: integ-action-base.hpp:158
crocoddyl::IntegratedActionDataRK4Tpl::ki
std::vector< VectorXs > ki
List of RK4 terms related to system dynamics.
Definition: rk4.hpp:261
crocoddyl::IntegratedActionDataRK4Tpl::ws
std::vector< VectorXs > ws
Control inputs evaluated in the RK4 integration.
Definition: rk4.hpp:265
crocoddyl::IntegratedActionDataRK4Tpl::dx
VectorXs dx
State rate.
Definition: rk4.hpp:260
crocoddyl::IntegratedActionDataRK4Tpl::dli_du
std::vector< VectorXs > dli_du
Definition: rk4.hpp:286
crocoddyl::IntegratedActionDataRK4Tpl::ddli_dxdw
std::vector< MatrixXs > ddli_dxdw
Definition: rk4.hpp:300
crocoddyl::IntegratedActionDataRK4Tpl::ddli_ddx
std::vector< MatrixXs > ddli_ddx
Definition: rk4.hpp:290
crocoddyl::IntegratedActionDataRK4Tpl::dli_dx
std::vector< VectorXs > dli_dx
Definition: rk4.hpp:283
crocoddyl::IntegratedActionDataRK4Tpl::ddli_dxdu
std::vector< MatrixXs > ddli_dxdu
Definition: rk4.hpp:304
crocoddyl::IntegratedActionModelRK4Tpl::checkData
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.