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>
21 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
23 typedef _Scalar Scalar;
29 typedef typename MathBase::VectorXs VectorXs;
30 typedef typename MathBase::MatrixXs MatrixXs;
33 const Scalar time_step = Scalar(1e-3),
const bool with_cost_residual =
true);
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>
76 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
78 typedef _Scalar Scalar;
81 typedef typename MathBase::VectorXs VectorXs;
82 typedef typename MathBase::MatrixXs MatrixXs;
84 template <
template <
typename Scalar>
class 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(
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;
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"
Abstract class for action model.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Control dimension.
VectorXs unone_
Neutral state.
std::size_t nr_
Dimension of the cost residual.
Abstract class for differential action model.
virtual void print(std::ostream &os) const
Print relevant information of the Runge-Kutta 4 integrator model.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the next state and cost value.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the dynamics and cost functions.
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
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.
VectorXs xnext
evolution state
MatrixXs Fx
Jacobian of the dynamics.
MatrixXs Fu
Jacobian of the dynamics.
MatrixXs Luu
Hessian of the cost function.
VectorXs Lx
Jacobian of the cost function.
MatrixXs Lxx
Hessian of the cost function.
VectorXs Lu
Jacobian of the cost function.
MatrixXs Lxu
Hessian of the cost function.