crocoddyl  1.8.1
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/action-base.hpp"
14 #include "crocoddyl/core/diff-action-base.hpp"
15 
16 namespace crocoddyl {
17 
18 template <typename _Scalar>
20  public:
21  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22 
23  typedef _Scalar Scalar;
29  typedef typename MathBase::VectorXs VectorXs;
30  typedef typename MathBase::MatrixXs MatrixXs;
31 
32  IntegratedActionModelEulerTpl(boost::shared_ptr<DifferentialActionModelAbstract> model,
33  const Scalar time_step = Scalar(1e-3), const bool with_cost_residual = true);
35 
36  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
37  const Eigen::Ref<const VectorXs>& u);
38  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
39  const Eigen::Ref<const VectorXs>& u);
40  virtual boost::shared_ptr<ActionDataAbstract> createData();
41  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
42 
43  virtual void quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
44  const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter = 100,
45  const Scalar tol = Scalar(1e-9));
46 
47  const boost::shared_ptr<DifferentialActionModelAbstract>& get_differential() const;
48  const Scalar get_dt() const;
49 
50  void set_dt(const Scalar dt);
51  void set_differential(boost::shared_ptr<DifferentialActionModelAbstract> model);
52 
58  virtual void print(std::ostream& os) const;
59 
60  protected:
61  using Base::nr_;
62  using Base::nu_;
63  using Base::state_;
64  using Base::unone_;
65 
66  private:
67  boost::shared_ptr<DifferentialActionModelAbstract> differential_;
68  Scalar time_step_;
69  Scalar time_step2_;
70  bool with_cost_residual_;
71  bool enable_integration_;
72 };
73 
74 template <typename _Scalar>
76  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
77 
78  typedef _Scalar Scalar;
81  typedef typename MathBase::VectorXs VectorXs;
82  typedef typename MathBase::MatrixXs MatrixXs;
83 
84  template <template <typename Scalar> class Model>
85  explicit IntegratedActionDataEulerTpl(Model<Scalar>* const model) : Base(model) {
86  differential = model->get_differential()->createData();
87  const std::size_t ndx = model->get_state()->get_ndx();
88  dx = VectorXs::Zero(ndx);
89  }
90  virtual ~IntegratedActionDataEulerTpl() {}
91 
92  boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> > differential;
93  VectorXs dx;
94 
95  using Base::cost;
96  using Base::Fu;
97  using Base::Fx;
98  using Base::Lu;
99  using Base::Luu;
100  using Base::Lx;
101  using Base::Lxu;
102  using Base::Lxx;
103  using Base::r;
104  using Base::xnext;
105 };
106 
107 } // namespace crocoddyl
108 
109 /* --- Details -------------------------------------------------------------- */
110 /* --- Details -------------------------------------------------------------- */
111 /* --- Details -------------------------------------------------------------- */
112 #include "crocoddyl/core/integrator/euler.hxx"
113 
114 #endif // CROCODDYL_CORE_INTEGRATOR_EULER_HPP_
Abstract class for action model.
Definition: action-base.hpp:60
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Control dimension.
VectorXs unone_
Neutral state.
std::size_t nr_
Dimension of the cost residual.
Abstract class for differential action model.
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)
Compute the next state and cost value.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)
Compute the derivatives of the dynamics and cost functions.
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the action 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.
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.
VectorXs r
Cost residual.