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"
18enum RKType { two = 2, three = 3, four = 4 };
34template <
typename _Scalar>
37 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39 typedef _Scalar Scalar;
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);
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));
156 virtual void print(std::ostream& os)
const;
171 void set_rk_type(
const RKType rktype);
173 std::vector<Scalar> rk_c_;
177template <
typename _Scalar>
179 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
181 typedef _Scalar Scalar;
186 typedef typename MathBase::VectorXs VectorXs;
187 typedef typename MathBase::MatrixXs MatrixXs;
189 template <
template <
typename Scalar>
class 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();
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"
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.
IntegratedActionModelRKTpl(boost::shared_ptr< DifferentialActionModelAbstract > model, const RKType rktype, const Scalar time_step=Scalar(1e-3), const bool with_cost_residual=true)
Initialize the RK integrator.
virtual void print(std::ostream &os) const
Print relevant information of the RK integrator model.
IntegratedActionModelRKTpl(boost::shared_ptr< DifferentialActionModelAbstract > model, boost::shared_ptr< ControlParametrizationModelAbstract > control, const RKType rktype, const Scalar time_step=Scalar(1e-3), const bool with_cost_residual=true)
Initialize the RK integrator.
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 RK scheme.
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 RK integrator.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the RK integrator data.
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 RK scheme.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the partial derivatives of the cost.
std::size_t get_ni() const
Return the number of nodes of the integrator.
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 RK terms related to system dynamics.
std::vector< MatrixXs > dki_du
std::vector< MatrixXs > dki_dx
List of partial derivatives of RK nodes with respect to the state of the RK 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 RK 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 RK integration....
std::vector< boost::shared_ptr< ControlParametrizationDataAbstract > > control
List of control parametrization data.
std::vector< MatrixXs > dyi_dx
List of partial derivatives of RK dynamics with respect to the state of the RK integrator....
std::vector< VectorXs > ws
Control inputs evaluated in the RK integration.