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);
63 boost::shared_ptr<DifferentialActionModelAbstract> differential_;
65 std::vector<Scalar> rk4_c_;
66 bool with_cost_residual_;
67 bool enable_integration_;
70 template <
typename _Scalar>
71 struct IntegratedActionDataRK4Tpl :
public ActionDataAbstractTpl<_Scalar> {
72 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
74 typedef _Scalar Scalar;
75 typedef MathBaseTpl<Scalar> MathBase;
76 typedef ActionDataAbstractTpl<Scalar> Base;
77 typedef typename MathBase::VectorXs VectorXs;
78 typedef typename MathBase::MatrixXs MatrixXs;
80 template <
template <
typename Scalar>
class Model>
81 explicit IntegratedActionDataRK4Tpl(Model<Scalar>*
const model) : Base(model) {
82 const std::size_t& ndx = model->get_state()->get_ndx();
83 const std::size_t& nx = model->get_state()->get_nx();
84 const std::size_t& nv = model->get_state()->get_nv();
85 const std::size_t& nu = model->get_nu();
87 for (std::size_t i = 0; i < 4; ++i) {
88 differential.push_back(
89 boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar>>(model->get_differential()->createData()));
92 dx = VectorXs::Zero(ndx);
93 integral = std::vector<Scalar>(4, Scalar(0.));
95 ki = std::vector<VectorXs>(4, VectorXs::Zero(ndx));
96 y = std::vector<VectorXs>(4, VectorXs::Zero(nx));
97 dx_rk4 = std::vector<VectorXs>(4, VectorXs::Zero(ndx));
99 dki_dx = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
100 dki_du = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, nu));
101 dyi_dx = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
102 dyi_du = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, nu));
103 dki_dy = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
105 dli_dx = std::vector<VectorXs>(4, VectorXs::Zero(ndx));
106 dli_du = std::vector<VectorXs>(4, VectorXs::Zero(nu));
107 ddli_ddx = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
108 ddli_ddu = std::vector<MatrixXs>(4, MatrixXs::Zero(nu, nu));
109 ddli_dxdu = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, nu));
110 Luu_partialx = std::vector<MatrixXs>(4, MatrixXs::Zero(nu, nu));
111 Lxx_partialx = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, ndx));
112 Lxx_partialu = std::vector<MatrixXs>(4, MatrixXs::Zero(ndx, nu));
114 dyi_dx[0].diagonal().array() = (Scalar)1;
115 for (std::size_t i = 0; i < 4; ++i) {
116 dki_dy[i].topRightCorner(nv, nv).diagonal().array() = (Scalar)1;
119 virtual ~IntegratedActionDataRK4Tpl() {}
122 std::vector<boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar>>> differential;
123 std::vector<Scalar> integral;
124 std::vector<VectorXs> ki;
125 std::vector<VectorXs> y;
126 std::vector<VectorXs> dx_rk4;
128 std::vector<MatrixXs> dki_dx;
129 std::vector<MatrixXs> dki_du;
130 std::vector<MatrixXs> dyi_dx;
131 std::vector<MatrixXs> dyi_du;
132 std::vector<MatrixXs> dki_dy;
134 std::vector<VectorXs> dli_dx;
135 std::vector<VectorXs> dli_du;
136 std::vector<MatrixXs> ddli_ddx;
137 std::vector<MatrixXs> ddli_ddu;
138 std::vector<MatrixXs> ddli_dxdu;
139 std::vector<MatrixXs> Luu_partialx;
140 std::vector<MatrixXs> Lxx_partialx;
141 std::vector<MatrixXs> Lxx_partialu;
160 #include "crocoddyl/core/integrator/rk4.hxx" 162 #endif // CROCODDYL_CORE_INTEGRATOR_RK4_HPP_ bool has_control_limits_
Indicates whether any of the control limits is finite.
std::size_t nu_
Control dimension.
VectorXs unone_
Neutral state.
VectorXs u_ub_
Upper control limits.
VectorXs u_lb_
Lower control limits.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nr_
Dimension of the cost residual.
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.