10 #ifndef CROCODDYL_CORE_INTEGRATOR_RK_HPP_
11 #define CROCODDYL_CORE_INTEGRATOR_RK_HPP_
13 #include "crocoddyl/core/fwd.hpp"
14 #include "crocoddyl/core/integ-action-base.hpp"
18 enum RKType { two = 2, three = 3, four = 4 };
34 template <
typename _Scalar>
35 class IntegratedActionModelRKTpl :
public IntegratedActionModelAbstractTpl<_Scalar> {
37 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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;
59 boost::shared_ptr<ControlParametrizationModelAbstract> control,
const RKType rktype,
60 const Scalar time_step = Scalar(1e-3),
const bool with_cost_residual =
true);
73 const Scalar time_step = Scalar(1e-3),
const bool with_cost_residual =
true);
83 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
84 const Eigen::Ref<const VectorXs>& u);
95 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
104 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
105 const Eigen::Ref<const VectorXs>& u);
116 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
123 virtual boost::shared_ptr<ActionDataAbstract>
createData();
128 virtual bool checkData(
const boost::shared_ptr<ActionDataAbstract>& data);
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));
149 std::size_t
get_ni()
const;
156 virtual void print(std::ostream& os)
const;
171 void set_rk_type(
const RKType rktype);
173 std::vector<Scalar> rk_c_;
177 template <
typename _Scalar>
178 struct IntegratedActionDataRKTpl :
public IntegratedActionDataAbstractTpl<_Scalar> {
179 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
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;
189 template <
template <
typename Scalar>
class Model>
190 explicit IntegratedActionDataRKTpl(Model<Scalar>*
const 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())) {
216 for (std::size_t i = 0; i < model->get_ni(); ++i) {
218 boost::shared_ptr<DifferentialActionDataAbstract>(model->get_differential()->createData()));
219 control.push_back(boost::shared_ptr<ControlParametrizationDataAbstract>(model->get_control()->createData()));
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();
226 virtual ~IntegratedActionDataRKTpl() {}
228 std::vector<boost::shared_ptr<DifferentialActionDataAbstract> >
differential;
229 std::vector<boost::shared_ptr<ControlParametrizationDataAbstract> >
231 std::vector<Scalar> integral;
233 std::vector<VectorXs>
ki;
234 std::vector<VectorXs>
y;
235 std::vector<VectorXs>
ws;
236 std::vector<VectorXs> dx_rk;
238 std::vector<MatrixXs>
243 std::vector<MatrixXs>
248 std::vector<VectorXs>
266 std::vector<MatrixXs> Luu_partialx;
267 std::vector<MatrixXs> Lxu_i;
268 std::vector<MatrixXs> Lxx_partialx;
269 std::vector<MatrixXs> Lxx_partialu;
288 #include "crocoddyl/core/integrator/rk.hxx"
290 #endif // CROCODDYL_CORE_INTEGRATOR_RK4_HPP_