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>
34 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
36 typedef _Scalar Scalar;
43 typedef typename MathBase::VectorXs VectorXs;
44 typedef typename MathBase::MatrixXs MatrixXs;
56 boost::shared_ptr<ControlParametrizationModelAbstract> control,
57 const Scalar time_step = Scalar(1e-3),
const bool with_cost_residual =
true);)
70 const Scalar time_step = Scalar(1e-3),
const bool with_cost_residual =
true);)
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);
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>
165 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
167 typedef _Scalar Scalar;
172 typedef typename MathBase::VectorXs VectorXs;
173 typedef typename MathBase::MatrixXs MatrixXs;
175 template <
template <
typename Scalar>
class 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();
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"
Abstract class for action model.
Abstract class for the control trajectory parametrization.
Abstract class for differential action model.
Abstract class for an integrated action model.
bool with_cost_residual_
Flag indicating whether a cost residual is used.
Scalar time_step_
Time step used for integration.
boost::shared_ptr< DifferentialActionModelAbstract > differential_
Differential action model that is integrated.
boost::shared_ptr< ControlParametrizationModelAbstract > control_
Model of the control parametrization.
boost::shared_ptr< StateAbstract > state_
< Dimension of the control
std::size_t nu_
< Dimension of the cost residual
Scalar time_step2_
Square of the time step used for integration.
virtual void print(std::ostream &os) const
Print relevant information of the RK4 integrator model.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Integrate the differential action model using RK4 scheme.
DEPRECATED("Use IntegratedActionModelRK", IntegratedActionModelRK4Tpl(boost::shared_ptr< DifferentialActionModelAbstract > model, boost::shared_ptr< ControlParametrizationModelAbstract > control, const Scalar time_step=Scalar(1e-3), const bool with_cost_residual=true);) DEPRECATED("Use IntegratedActionModelRK"
Initialize the RK4 integrator.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the partial derivatives of the RK4 integrator.
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Integrate the total cost value for nodes that depends only on the state using RK4 scheme.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the partial derivatives of the cost.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the RK4 integrator 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.
std::vector< MatrixXs > ddli_ddx
std::vector< VectorXs > ki
List of RK4 terms related to system dynamics.
std::vector< MatrixXs > dki_du
std::vector< MatrixXs > dki_dx
List of partial derivatives of RK4 nodes with respect to the state of the RK4 integration....
std::vector< MatrixXs > ddli_ddu
std::vector< MatrixXs > ddli_dxdw
std::vector< MatrixXs > ddli_ddw
std::vector< boost::shared_ptr< DifferentialActionDataAbstract > > differential
List of differential model data.
std::vector< MatrixXs > ddli_dwdu
std::vector< MatrixXs > dyi_du
std::vector< VectorXs > y
List of states where f is evaluated in the RK4 integration.
std::vector< VectorXs > dli_du
std::vector< MatrixXs > ddli_dxdu
std::vector< VectorXs > dli_dx
List of partial derivatives of the cost with respect to the state of the RK4 integration....
std::vector< boost::shared_ptr< ControlParametrizationDataAbstract > > control
List of control parametrization data.
std::vector< MatrixXs > dyi_dx
List of partial derivatives of RK4 dynamics with respect to the state of the RK4 integrator....
std::vector< VectorXs > ws
Control inputs evaluated in the RK4 integration.