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" 18 enum RKType { two = 2, three = 3, four = 4 };
34 template <
typename _Scalar>
35 class IntegratedActionModelRKTpl :
public IntegratedActionModelAbstractTpl<_Scalar> {
37 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
39 typedef _Scalar Scalar;
40 typedef MathBaseTpl<Scalar> MathBase;
41 typedef IntegratedActionModelAbstractTpl<Scalar> Base;
42 typedef IntegratedActionDataRKTpl<Scalar> Data;
43 typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
44 typedef DifferentialActionModelAbstractTpl<Scalar> DifferentialActionModelAbstract;
45 typedef ControlParametrizationModelAbstractTpl<Scalar> ControlParametrizationModelAbstract;
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);
123 virtual boost::shared_ptr<ActionDataAbstract>
createData();
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));
149 std::size_t
get_ni()
const;
156 virtual void print(std::ostream& os)
const;
171 void set_rk_type(
const RKType rktype);
173 std::vector<Scalar> rk_c_;
177 template <
typename _Scalar>
178 struct IntegratedActionDataRKTpl :
public IntegratedActionDataAbstractTpl<_Scalar> {
179 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
181 typedef _Scalar Scalar;
182 typedef MathBaseTpl<Scalar> MathBase;
183 typedef IntegratedActionDataAbstractTpl<Scalar> Base;
184 typedef DifferentialActionDataAbstractTpl<Scalar> DifferentialActionDataAbstract;
185 typedef ControlParametrizationDataAbstractTpl<Scalar> ControlParametrizationDataAbstract;
186 typedef typename MathBase::VectorXs VectorXs;
187 typedef typename MathBase::MatrixXs MatrixXs;
189 template <
template <
typename Scalar>
class Model>
190 explicit IntegratedActionDataRKTpl(Model<Scalar>*
const model)
192 integral(model->
get_ni(), Scalar(0.)),
202 dli_dx(model->
get_ni(), VectorXs::Zero(model->
get_state()->get_ndx())),
203 dli_du(model->
get_ni(), VectorXs::Zero(model->
get_nu())),
216 for (std::size_t i = 0; i < model->get_ni(); ++i) {
217 differential.push_back(
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();
226 virtual ~IntegratedActionDataRKTpl() {}
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>
254 std::vector<MatrixXs> ddli_ddw;
256 std::vector<MatrixXs> ddli_ddu;
258 std::vector<MatrixXs> ddli_dxdw;
260 std::vector<MatrixXs> ddli_dxdu;
262 std::vector<MatrixXs> ddli_dwdu;
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" 290 #endif // CROCODDYL_CORE_INTEGRATOR_RK4_HPP_ std::vector< MatrixXs > dki_du
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
Scalar time_step_
Time step used for integration.
std::size_t nu_
Control dimension.
bool with_cost_residual_
Flag indicating whether a cost residual is used.
std::size_t get_nu() const
Return the dimension of the control input.
std::vector< boost::shared_ptr< DifferentialActionDataAbstract > > differential
List of differential model data.
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.
const boost::shared_ptr< ControlParametrizationModelAbstract > & get_control() const
Return the control parametrization model associated to this integrated action model.
std::vector< VectorXs > ws
Control inputs evaluated in the RK integration.
std::vector< VectorXs > ki
List of RK terms related to system dynamics.
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
boost::shared_ptr< ControlParametrizationModelAbstract > control_
Model of the control parametrization.
std::vector< MatrixXs > dyi_dx
List of partial derivatives of RK dynamics with respect to the state of the RK integrator. dyi/dx.
std::vector< VectorXs > dli_dx
List of partial derivatives of the cost with respect to the state of the RK integration. dli_dx.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the RK integrator data.
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 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.
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.
std::vector< MatrixXs > dyi_du
Scalar time_step2_
Square of the time step used for integration.
boost::shared_ptr< DifferentialActionModelAbstract > differential_
Differential action model that is integrated.
std::size_t get_ni() const
Return the number of nodes of the integrator.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::vector< VectorXs > y
List of states where f is evaluated in the RK integration.
std::vector< MatrixXs > dki_dx
List of partial derivatives of RK nodes with respect to the state of the RK integration. dki/dx.
std::vector< boost::shared_ptr< ControlParametrizationDataAbstract > > control
List of control parametrization data.
std::vector< MatrixXs > ddli_ddx
std::vector< VectorXs > dli_du
virtual void print(std::ostream &os) const
Print relevant information of the RK integrator model.