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>
32 class IntegratedActionModelRK4Tpl :
public IntegratedActionModelAbstractTpl<_Scalar> {
34 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
36 typedef _Scalar Scalar;
37 typedef MathBaseTpl<Scalar> MathBase;
38 typedef IntegratedActionModelAbstractTpl<Scalar> Base;
39 typedef IntegratedActionDataRK4Tpl<Scalar> Data;
40 typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
41 typedef DifferentialActionModelAbstractTpl<Scalar> DifferentialActionModelAbstract;
42 typedef ControlParametrizationModelAbstractTpl<Scalar> ControlParametrizationModelAbstract;
43 typedef typename MathBase::VectorXs VectorXs;
44 typedef typename MathBase::MatrixXs MatrixXs;
55 IntegratedActionModelRK4Tpl(boost::shared_ptr<DifferentialActionModelAbstract> model,
56 boost::shared_ptr<ControlParametrizationModelAbstract> control,
57 const Scalar time_step = Scalar(1e-3),
const bool with_cost_residual =
true);)
69 IntegratedActionModelRK4Tpl(boost::shared_ptr<DifferentialActionModelAbstract> model,
70 const Scalar time_step = Scalar(1e-3),
const bool with_cost_residual =
true);)
71 virtual ~IntegratedActionModelRK4Tpl();
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);
120 virtual boost::shared_ptr<ActionDataAbstract>
createData();
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>
164 struct IntegratedActionDataRK4Tpl :
public IntegratedActionDataAbstractTpl<_Scalar> {
165 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
167 typedef _Scalar Scalar;
168 typedef MathBaseTpl<Scalar> MathBase;
169 typedef IntegratedActionDataAbstractTpl<Scalar> Base;
170 typedef DifferentialActionDataAbstractTpl<Scalar> DifferentialActionDataAbstract;
171 typedef ControlParametrizationDataAbstractTpl<Scalar> ControlParametrizationDataAbstract;
172 typedef typename MathBase::VectorXs VectorXs;
173 typedef typename MathBase::MatrixXs MatrixXs;
175 template <
template <
typename Scalar>
class Model>
176 explicit IntegratedActionDataRK4Tpl(Model<Scalar>*
const model)
178 integral(4, Scalar(0.)),
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())),
192 ddli_ddu(4, MatrixXs::Zero(model->
get_nu(), model->
get_nu())),
194 ddli_dxdu(4, MatrixXs::Zero(model->
get_state()->get_ndx(), 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) {
203 differential.push_back(
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();
212 virtual ~IntegratedActionDataRK4Tpl() {}
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>
240 std::vector<MatrixXs> ddli_ddw;
242 std::vector<MatrixXs> ddli_ddu;
244 std::vector<MatrixXs> ddli_dxdw;
246 std::vector<MatrixXs> ddli_dxdu;
248 std::vector<MatrixXs> ddli_dwdu;
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" 276 #endif // CROCODDYL_CORE_INTEGRATOR_RK4_HPP_ virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
std::vector< boost::shared_ptr< ControlParametrizationDataAbstract > > control
List of control parametrization data.
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.
const boost::shared_ptr< ControlParametrizationModelAbstract > & get_control() const
Return the control parametrization model associated to this integrated action model.
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.
std::vector< VectorXs > y
List of states where f is evaluated in the RK4 integration.
std::vector< MatrixXs > dki_du
std::vector< boost::shared_ptr< DifferentialActionDataAbstract > > differential
List of differential model data.
std::vector< VectorXs > ws
Control inputs evaluated in the RK4 integration.
boost::shared_ptr< ControlParametrizationModelAbstract > control_
Model of the control parametrization.
std::vector< MatrixXs > dyi_dx
List of partial derivatives of RK4 dynamics with respect to the state of the RK4 integrator. dyi/dx.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the RK4 integrator data.
std::vector< VectorXs > dli_du
std::vector< MatrixXs > dki_dx
List of partial derivatives of RK4 nodes with respect to the state of the RK4 integration. dki/dx.
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.
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.
std::vector< VectorXs > dli_dx
List of partial derivatives of the cost with respect to the state of the RK4 integration. dli_dx.
boost::shared_ptr< StateAbstract > state_
Model of the state.
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.
std::vector< VectorXs > ki
List of RK4 terms related to system dynamics.
virtual void print(std::ostream &os) const
Print relevant information of the RK4 integrator model.
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.
std::vector< MatrixXs > ddli_ddx