9 #ifndef CROCODDYL_CORE_INTEGRATOR_RK4_HPP_ 10 #define CROCODDYL_CORE_INTEGRATOR_RK4_HPP_ 12 #include "crocoddyl/core/fwd.hpp" 13 #include "crocoddyl/core/action-base.hpp" 14 #include "crocoddyl/core/diff-action-base.hpp" 18 template <
typename _Scalar>
19 class IntegratedActionModelRK4Tpl :
public ActionModelAbstractTpl<_Scalar> {
21 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 typedef _Scalar Scalar;
24 typedef MathBaseTpl<Scalar> MathBase;
25 typedef ActionModelAbstractTpl<Scalar> Base;
26 typedef IntegratedActionDataRK4Tpl<Scalar> Data;
27 typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
28 typedef DifferentialActionModelAbstractTpl<Scalar> DifferentialActionModelAbstract;
29 typedef typename MathBase::VectorXs VectorXs;
30 typedef typename MathBase::MatrixXs MatrixXs;
32 IntegratedActionModelRK4Tpl(boost::shared_ptr<DifferentialActionModelAbstract> model,
33 const Scalar time_step = Scalar(1e-3),
const bool with_cost_residual =
true);
34 virtual ~IntegratedActionModelRK4Tpl();
36 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
37 const Eigen::Ref<const VectorXs>& u);
38 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
39 const Eigen::Ref<const VectorXs>& u);
40 virtual boost::shared_ptr<ActionDataAbstract>
createData();
41 virtual bool checkData(
const boost::shared_ptr<ActionDataAbstract>& data);
43 virtual void quasiStatic(
const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
44 const Eigen::Ref<const VectorXs>& x,
const std::size_t maxiter = 100,
45 const Scalar tol = Scalar(1e-9));
47 const boost::shared_ptr<DifferentialActionModelAbstract>& get_differential()
const;
48 const Scalar get_dt()
const;
50 void set_dt(
const Scalar dt);
51 void set_differential(boost::shared_ptr<DifferentialActionModelAbstract> model);
58 virtual void print(std::ostream& os)
const;
67 boost::shared_ptr<DifferentialActionModelAbstract> differential_;
69 std::vector<Scalar> rk4_c_;
70 bool with_cost_residual_;
71 bool enable_integration_;
74 template <
typename _Scalar>
75 struct IntegratedActionDataRK4Tpl :
public ActionDataAbstractTpl<_Scalar> {
76 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
78 typedef _Scalar Scalar;
79 typedef MathBaseTpl<Scalar> MathBase;
80 typedef ActionDataAbstractTpl<Scalar> Base;
81 typedef typename MathBase::VectorXs VectorXs;
82 typedef typename MathBase::MatrixXs MatrixXs;
84 template <
template <
typename Scalar>
class Model>
85 explicit IntegratedActionDataRK4Tpl(Model<Scalar>*
const model) : Base(model) {
86 const std::size_t ndx = model->get_state()->get_ndx();
87 const std::size_t nx = model->get_state()->get_nx();
88 const std::size_t nv = model->get_state()->get_nv();
89 const std::size_t nu = model->get_nu();
91 for (std::size_t i = 0; i < 4; ++i) {
92 differential.push_back(
93 boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> >(model->get_differential()->createData()));
96 dx = VectorXs::Zero(ndx);
97 integral = std::vector<Scalar>(4, Scalar(0.));
99 ki = std::vector<VectorXs>(4, VectorXs::Zero(ndx));
100 y = std::vector<VectorXs>(4, VectorXs::Zero(nx));
101 dx_rk4 = std::vector<VectorXs>(4, VectorXs::Zero(ndx));
103 dki_dx = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
104 dki_du = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, nu));
105 dyi_dx = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
106 dyi_du = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, nu));
107 dki_dy = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
109 dli_dx = std::vector<VectorXs>(4, VectorXs::Zero(ndx));
110 dli_du = std::vector<VectorXs>(4, VectorXs::Zero(nu));
111 ddli_ddx = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
112 ddli_ddu = std::vector<MatrixXs>(4, MatrixXs::Zero(nu, nu));
113 ddli_dxdu = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, nu));
114 Luu_partialx = std::vector<MatrixXs>(4, MatrixXs::Zero(nu, nu));
115 Lxx_partialx = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
116 Lxx_partialu = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, nu));
118 dyi_dx[0].diagonal().array() = (Scalar)1;
119 for (std::size_t i = 0; i < 4; ++i) {
120 dki_dy[i].topRightCorner(nv, nv).diagonal().array() = (Scalar)1;
123 virtual ~IntegratedActionDataRK4Tpl() {}
126 std::vector<boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> > > differential;
127 std::vector<Scalar> integral;
128 std::vector<VectorXs> ki;
129 std::vector<VectorXs> y;
130 std::vector<VectorXs> dx_rk4;
132 std::vector<MatrixXs> dki_dx;
133 std::vector<MatrixXs> dki_du;
134 std::vector<MatrixXs> dyi_dx;
135 std::vector<MatrixXs> dyi_du;
136 std::vector<MatrixXs> dki_dy;
138 std::vector<VectorXs> dli_dx;
139 std::vector<VectorXs> dli_du;
140 std::vector<MatrixXs> ddli_ddx;
141 std::vector<MatrixXs> ddli_ddu;
142 std::vector<MatrixXs> ddli_dxdu;
143 std::vector<MatrixXs> Luu_partialx;
144 std::vector<MatrixXs> Lxx_partialx;
145 std::vector<MatrixXs> Lxx_partialu;
164 #include "crocoddyl/core/integrator/rk4.hxx" 166 #endif // CROCODDYL_CORE_INTEGRATOR_RK4_HPP_ virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
std::size_t nu_
Control dimension.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
VectorXs unone_
Neutral state.
boost::shared_ptr< StateAbstract > state_
Model of the state.
virtual void print(std::ostream &os) const
Print relevant information of the Runge-Kutta 4 integrator model.
std::size_t nr_
Dimension of the cost residual.