crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
euler.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, University of Oxford
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_INTEGRATOR_EULER_HPP_
10 #define CROCODDYL_CORE_INTEGRATOR_EULER_HPP_
11 
12 #include "crocoddyl/core/fwd.hpp"
13 #include "crocoddyl/core/integ-action-base.hpp"
14 
15 namespace crocoddyl {
16 
31 template <typename _Scalar>
33  public:
34  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
35 
36  typedef _Scalar Scalar;
44  typedef typename MathBase::VectorXs VectorXs;
45  typedef typename MathBase::MatrixXs MatrixXs;
46 
55  IntegratedActionModelEulerTpl(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);
58 
68  IntegratedActionModelEulerTpl(boost::shared_ptr<DifferentialActionModelAbstract> model,
69  const Scalar time_step = Scalar(1e-3), const bool with_cost_residual = true);
71 
79  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
80  const Eigen::Ref<const VectorXs>& u);
81 
91  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
92 
100  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
101  const Eigen::Ref<const VectorXs>& u);
102 
112  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
113 
119  virtual boost::shared_ptr<ActionDataAbstract> createData();
120 
124  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
125 
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));
141 
147  virtual void print(std::ostream& os) const;
148 
149  protected:
150  using Base::control_;
151  using Base::differential_;
152  using Base::nu_;
153  using Base::state_;
154  using Base::time_step2_;
155  using Base::time_step_;
157 };
158 
159 template <typename _Scalar>
161  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
162 
163  typedef _Scalar Scalar;
168  typedef typename MathBase::VectorXs VectorXs;
169  typedef typename MathBase::MatrixXs MatrixXs;
170 
171  template <template <typename Scalar> class Model>
172  explicit IntegratedActionDataEulerTpl(Model<Scalar>* const model) : Base(model) {
173  differential = model->get_differential()->createData();
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());
180  }
181  virtual ~IntegratedActionDataEulerTpl() {}
182 
183  boost::shared_ptr<DifferentialActionDataAbstract> differential;
184  boost::shared_ptr<ControlParametrizationDataAbstract> control;
185  VectorXs dx;
186  MatrixXs da_du;
187  MatrixXs Lwu;
188 
189  using Base::cost;
190  using Base::Fu;
191  using Base::Fx;
192  using Base::Lu;
193  using Base::Luu;
194  using Base::Lx;
195  using Base::Lxu;
196  using Base::Lxx;
197  using Base::r;
198  using Base::xnext;
199 };
200 
201 } // namespace crocoddyl
202 
203 /* --- Details -------------------------------------------------------------- */
204 /* --- Details -------------------------------------------------------------- */
205 /* --- Details -------------------------------------------------------------- */
206 #include "crocoddyl/core/integrator/euler.hxx"
207 
208 #endif // CROCODDYL_CORE_INTEGRATOR_EULER_HPP_
Abstract class for action model.
Definition: action-base.hpp:59
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.
Definition: euler.hpp:32
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 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 boost::shared_ptr< ActionDataAbstract > createData()
Create the symplectic Euler data.
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.
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)
Definition: euler.hpp:187
boost::shared_ptr< DifferentialActionDataAbstract > differential
Differential model data.
Definition: euler.hpp:183
boost::shared_ptr< ControlParametrizationDataAbstract > control
Control parametrization data.
Definition: euler.hpp:184