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>
19 class IntegratedActionModelEulerTpl : public ActionModelAbstractTpl<_Scalar> {
20  public:
21  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
22 
23  typedef _Scalar Scalar;
24  typedef MathBaseTpl<Scalar> MathBase;
25  typedef ActionModelAbstractTpl<Scalar> Base;
26  typedef IntegratedActionDataEulerTpl<Scalar> Data;
27  typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
28  typedef DifferentialActionModelAbstractTpl<Scalar> DifferentialActionModelAbstract;
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);
34  virtual ~IntegratedActionModelEulerTpl();
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>
75 struct IntegratedActionDataEulerTpl : public ActionDataAbstractTpl<_Scalar> {
76  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
77 
78  typedef _Scalar Scalar;
79  typedef MathBaseTpl<Scalar> MathBase;
80  typedef ActionDataAbstractTpl<Scalar> Base;
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_
crocoddyl::ActionDataAbstractTpl::Lxu
MatrixXs Lxu
Hessian of the cost function.
Definition: action-base.hpp:271
crocoddyl::ActionModelAbstractTpl::nu_
std::size_t nu_
Control dimension.
Definition: action-base.hpp:216
crocoddyl::ActionDataAbstractTpl::Lx
VectorXs Lx
Jacobian of the cost function.
Definition: action-base.hpp:268
crocoddyl::ActionDataAbstractTpl::xnext
VectorXs xnext
evolution state
Definition: action-base.hpp:264
crocoddyl::IntegratedActionModelEulerTpl::print
virtual void print(std::ostream &os) const
Print relevant information of the Euler integrator model.
crocoddyl::ActionDataAbstractTpl::cost
Scalar cost
cost value
Definition: action-base.hpp:263
crocoddyl::ActionDataAbstractTpl::r
VectorXs r
Cost residual.
Definition: action-base.hpp:267
crocoddyl::ActionModelAbstractTpl::state_
boost::shared_ptr< StateAbstract > state_
Model of the state.
Definition: action-base.hpp:218
crocoddyl::ActionModelAbstractTpl::unone_
VectorXs unone_
Neutral state.
Definition: action-base.hpp:219
crocoddyl::ActionDataAbstractTpl::Fx
MatrixXs Fx
Jacobian of the dynamics.
Definition: action-base.hpp:265
crocoddyl::ActionDataAbstractTpl::Lu
VectorXs Lu
Jacobian of the cost function.
Definition: action-base.hpp:269
crocoddyl::ActionDataAbstractTpl::Lxx
MatrixXs Lxx
Hessian of the cost function.
Definition: action-base.hpp:270
crocoddyl::ActionDataAbstractTpl::Fu
MatrixXs Fu
Jacobian of the dynamics.
Definition: action-base.hpp:266
crocoddyl::ActionModelAbstractTpl::nr_
std::size_t nr_
Dimension of the cost residual.
Definition: action-base.hpp:217
crocoddyl::ActionDataAbstractTpl::Luu
MatrixXs Luu
Hessian of the cost function.
Definition: action-base.hpp:272