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"
31template <
typename _Scalar>
34 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
36 typedef _Scalar Scalar;
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);
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;
159template <
typename _Scalar>
161 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
163 typedef _Scalar Scalar;
168 typedef typename MathBase::VectorXs VectorXs;
169 typedef typename MathBase::MatrixXs MatrixXs;
171 template <
template <
typename Scalar>
class 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());
184 boost::shared_ptr<ControlParametrizationDataAbstract>
control;
206#include "crocoddyl/core/integrator/euler.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.
Symplectic Euler integrator.
IntegratedActionModelEulerTpl(boost::shared_ptr< DifferentialActionModelAbstract > model, const Scalar time_step=Scalar(1e-3), const bool with_cost_residual=true)
Initialize the symplectic Euler integrator.
virtual void print(std::ostream &os) const
Print relevant information of the Euler 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 symplectic Euler scheme.
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 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.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the symplectic Euler 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 symplectic Euler scheme...
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the partial derivatives of the cost.
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.
MatrixXs Lwu
Hessian of the cost function with respect to the control input (w) and control parameters (u)
boost::shared_ptr< DifferentialActionDataAbstract > differential
Differential model data.
boost::shared_ptr< ControlParametrizationDataAbstract > control
Control parametrization data.