9 #ifndef CROCODDYL_CORE_INTEGRATOR_EULER_HPP_ 10 #define CROCODDYL_CORE_INTEGRATOR_EULER_HPP_ 12 #include "crocoddyl/core/fwd.hpp" 13 #include "crocoddyl/core/integ-action-base.hpp" 31 template <
typename _Scalar>
32 class IntegratedActionModelEulerTpl :
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 IntegratedActionDataEulerTpl<Scalar> Data;
40 typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
41 typedef DifferentialActionModelAbstractTpl<Scalar> DifferentialActionModelAbstract;
42 typedef ControlParametrizationModelAbstractTpl<Scalar> ControlParametrizationModelAbstract;
43 typedef ControlParametrizationDataAbstractTpl<Scalar> ControlParametrizationDataAbstract;
44 typedef typename MathBase::VectorXs VectorXs;
45 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);
69 const Scalar time_step = Scalar(1e-3),
const bool with_cost_residual =
true);
79 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
80 const Eigen::Ref<const VectorXs>& u);
91 virtual void calc(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
100 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x,
101 const Eigen::Ref<const VectorXs>& u);
112 virtual void calcDiff(
const boost::shared_ptr<ActionDataAbstract>& data,
const Eigen::Ref<const VectorXs>& x);
119 virtual boost::shared_ptr<ActionDataAbstract>
createData();
124 virtual bool checkData(
const boost::shared_ptr<ActionDataAbstract>& data);
138 virtual void quasiStatic(
const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
139 const Eigen::Ref<const VectorXs>& x,
const std::size_t maxiter = 100,
140 const Scalar tol = Scalar(1e-9));
147 virtual void print(std::ostream& os)
const;
159 template <
typename _Scalar>
160 struct IntegratedActionDataEulerTpl :
public IntegratedActionDataAbstractTpl<_Scalar> {
161 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
163 typedef _Scalar Scalar;
164 typedef MathBaseTpl<Scalar> MathBase;
165 typedef IntegratedActionDataAbstractTpl<Scalar> Base;
166 typedef DifferentialActionDataAbstractTpl<Scalar> DifferentialActionDataAbstract;
167 typedef ControlParametrizationDataAbstractTpl<Scalar> ControlParametrizationDataAbstract;
168 typedef typename MathBase::VectorXs VectorXs;
169 typedef typename MathBase::MatrixXs MatrixXs;
171 template <
template <
typename Scalar>
class Model>
172 explicit IntegratedActionDataEulerTpl(Model<Scalar>*
const model) : Base(model) {
174 control = model->get_control()->createData();
175 const std::size_t ndx = model->get_state()->get_ndx();
176 const std::size_t nv = model->get_state()->get_nv();
177 dx = VectorXs::Zero(ndx);
178 da_du = MatrixXs::Zero(nv, model->get_nu());
179 Lwu = MatrixXs::Zero(model->get_control()->get_nw(), model->get_nu());
181 virtual ~IntegratedActionDataEulerTpl() {}
184 boost::shared_ptr<ControlParametrizationDataAbstract>
control;
206 #include "crocoddyl/core/integrator/euler.hxx" 208 #endif // CROCODDYL_CORE_INTEGRATOR_EULER_HPP_ boost::shared_ptr< DifferentialActionDataAbstract > differential
Differential model data.
MatrixXs Lxx
Hessian of the cost function.
MatrixXs Lwu
Hessian of the cost function with respect to the control input (w) and control parameters (u) ...
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.
VectorXs xnext
evolution state
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the symplectic Euler data.
boost::shared_ptr< ControlParametrizationModelAbstract > control_
Model of the control parametrization.
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 symplectic Euler integrator.
VectorXs Lx
Jacobian of the cost function.
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 symplectic Euler scheme.
virtual void print(std::ostream &os) const
Print relevant information of the Euler integrator model.
MatrixXs Fx
Jacobian of the dynamics.
VectorXs Lu
Jacobian of the cost function.
Scalar time_step2_
Square of the time step used for integration.
boost::shared_ptr< DifferentialActionModelAbstract > differential_
Differential action model that is integrated.
MatrixXs Luu
Hessian of the cost function.
boost::shared_ptr< StateAbstract > state_
Model of the state.
IntegratedActionModelEulerTpl(boost::shared_ptr< DifferentialActionModelAbstract > model, boost::shared_ptr< ControlParametrizationModelAbstract > control, const Scalar time_step=Scalar(1e-3), const bool with_cost_residual=true)
Initialize the symplectic Euler integrator.
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
boost::shared_ptr< ControlParametrizationDataAbstract > control
Control parametrization data.
MatrixXs Lxu
Hessian of the cost function.
MatrixXs Fu
Jacobian of the dynamics.