9 #ifndef CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
10 #define CROCODDYL_CORE_INTEGRATOR_RK4_HPP_
12 #include "crocoddyl/core/fwd.hpp"
13 #include "crocoddyl/core/integ-action-base.hpp"
14 #include "crocoddyl/core/utils/deprecate.hpp"
31 template <
typename _Scalar>
32 class IntegratedActionModelRK4Tpl :
public IntegratedActionModelAbstractTpl<_Scalar> {
34 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
36 typedef _Scalar Scalar;
37 typedef MathBaseTpl<Scalar> MathBase;
38 typedef IntegratedActionModelAbstractTpl<Scalar> Base;
39 typedef IntegratedActionDataRK4Tpl<Scalar> Data;
40 typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
41 typedef DifferentialActionModelAbstractTpl<Scalar> DifferentialActionModelAbstract;
42 typedef ControlParametrizationModelAbstractTpl<Scalar> ControlParametrizationModelAbstract;
43 typedef typename MathBase::VectorXs VectorXs;
44 typedef typename MathBase::MatrixXs MatrixXs;
55 IntegratedActionModelRK4Tpl(boost::shared_ptr<DifferentialActionModelAbstract> model,
56 boost::shared_ptr<ControlParametrizationModelAbstract> control,
57 const Scalar time_step = Scalar(1e-3),
const bool with_cost_residual =
true);)
69 IntegratedActionModelRK4Tpl(boost::shared_ptr<DifferentialActionModelAbstract> model,
70 const Scalar time_step = Scalar(1e-3),
const bool with_cost_residual =
true);)
71 virtual ~IntegratedActionModelRK4Tpl();
80 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
81 const Eigen::Ref<const VectorXs>& u);
92 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
101 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
102 const Eigen::Ref<const VectorXs>& u);
113 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
120 virtual boost::shared_ptr<ActionDataAbstract>
createData();
125 virtual bool checkData(
const boost::shared_ptr<ActionDataAbstract>& data);
139 virtual void quasiStatic(
const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
140 const Eigen::Ref<const VectorXs>& x,
const std::size_t maxiter = 100,
141 const Scalar tol = Scalar(1e-9));
148 virtual void print(std::ostream& os)
const;
160 std::array<Scalar, 4> rk4_c_;
163 template <
typename _Scalar>
164 struct IntegratedActionDataRK4Tpl :
public IntegratedActionDataAbstractTpl<_Scalar> {
165 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
167 typedef _Scalar Scalar;
168 typedef MathBaseTpl<Scalar> MathBase;
169 typedef IntegratedActionDataAbstractTpl<Scalar> Base;
170 typedef DifferentialActionDataAbstractTpl<Scalar> DifferentialActionDataAbstract;
171 typedef ControlParametrizationDataAbstractTpl<Scalar> ControlParametrizationDataAbstract;
172 typedef typename MathBase::VectorXs VectorXs;
173 typedef typename MathBase::MatrixXs MatrixXs;
175 template <
template <
typename Scalar>
class Model>
176 explicit IntegratedActionDataRK4Tpl(Model<Scalar>*
const model)
178 integral(4, Scalar(0.)),
179 dx(model->get_state()->get_ndx()),
180 ki(4, VectorXs::Zero(model->get_state()->get_ndx())),
181 y(4, VectorXs::Zero(model->get_state()->get_nx())),
182 ws(4, VectorXs::Zero(model->get_control()->get_nw())),
183 dx_rk4(4, VectorXs::Zero(model->get_state()->get_ndx())),
184 dki_dx(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
185 dki_du(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
186 dyi_dx(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
187 dyi_du(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
188 dli_dx(4, VectorXs::Zero(model->get_state()->get_ndx())),
189 dli_du(4, VectorXs::Zero(model->get_nu())),
190 ddli_ddx(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
191 ddli_ddw(4, MatrixXs::Zero(model->get_control()->get_nw(), model->get_control()->get_nw())),
192 ddli_ddu(4, MatrixXs::Zero(model->get_nu(), model->get_nu())),
193 ddli_dxdw(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_control()->get_nw())),
194 ddli_dxdu(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
195 ddli_dwdu(4, MatrixXs::Zero(model->get_control()->get_nw(), model->get_nu())),
196 Luu_partialx(4, MatrixXs::Zero(model->get_nu(), model->get_nu())),
197 Lxu_i(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())),
198 Lxx_partialx(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_state()->get_ndx())),
199 Lxx_partialu(4, MatrixXs::Zero(model->get_state()->get_ndx(), model->get_nu())) {
202 for (std::size_t i = 0; i < 4; ++i) {
204 boost::shared_ptr<DifferentialActionDataAbstract>(model->get_differential()->createData()));
205 control.push_back(boost::shared_ptr<ControlParametrizationDataAbstract>(model->get_control()->createData()));
208 const std::size_t nv = model->get_state()->get_nv();
209 dyi_dx[0].diagonal().setOnes();
210 dki_dx[0].topRightCorner(nv, nv).diagonal().setOnes();
212 virtual ~IntegratedActionDataRK4Tpl() {}
214 std::vector<boost::shared_ptr<DifferentialActionDataAbstract> >
differential;
215 std::vector<boost::shared_ptr<ControlParametrizationDataAbstract> >
217 std::vector<Scalar> integral;
219 std::vector<VectorXs>
ki;
220 std::vector<VectorXs>
y;
221 std::vector<VectorXs>
ws;
222 std::vector<VectorXs> dx_rk4;
224 std::vector<MatrixXs>
229 std::vector<MatrixXs>
234 std::vector<VectorXs>
252 std::vector<MatrixXs> Luu_partialx;
253 std::vector<MatrixXs> Lxu_i;
254 std::vector<MatrixXs> Lxx_partialx;
255 std::vector<MatrixXs> Lxx_partialu;
274 #include "crocoddyl/core/integrator/rk4.hxx"
276 #endif // CROCODDYL_CORE_INTEGRATOR_RK4_HPP_