crocoddyl  1.3.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
euler.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
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 
42  const boost::shared_ptr<DifferentialActionModelAbstract>& get_differential() const;
43  const Scalar& get_dt() const;
44 
45  void set_dt(const Scalar& dt);
46  void set_differential(boost::shared_ptr<DifferentialActionModelAbstract> model);
47 
48  protected:
50  using Base::nr_;
51  using Base::nu_;
52  using Base::state_;
53  using Base::u_lb_;
54  using Base::u_ub_;
55  using Base::unone_;
56 
57  private:
58  boost::shared_ptr<DifferentialActionModelAbstract> differential_;
59  Scalar time_step_;
60  Scalar time_step2_;
61  bool with_cost_residual_;
62  bool enable_integration_;
63 };
64 
65 template <typename _Scalar>
66 struct IntegratedActionDataEulerTpl : public ActionDataAbstractTpl<_Scalar> {
67  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
68 
69  typedef _Scalar Scalar;
70  typedef MathBaseTpl<Scalar> MathBase;
71  typedef ActionDataAbstractTpl<Scalar> Base;
72  typedef typename MathBase::VectorXs VectorXs;
73  typedef typename MathBase::MatrixXs MatrixXs;
74 
75  template <template <typename Scalar> class Model>
76  explicit IntegratedActionDataEulerTpl(Model<Scalar>* const model) : Base(model) {
77  differential = model->get_differential()->createData();
78  const std::size_t& ndx = model->get_state()->get_ndx();
79  dx = VectorXs::Zero(ndx);
80  }
81  ~IntegratedActionDataEulerTpl() {}
82 
83  boost::shared_ptr<DifferentialActionDataAbstractTpl<Scalar> > differential;
84  VectorXs dx;
85 
86  using Base::cost;
87  using Base::Fu;
88  using Base::Fx;
89  using Base::Lu;
90  using Base::Luu;
91  using Base::Lx;
92  using Base::Lxu;
93  using Base::Lxx;
94  using Base::r;
95  using Base::xnext;
96 };
97 
98 } // namespace crocoddyl
99 
100 /* --- Details -------------------------------------------------------------- */
101 /* --- Details -------------------------------------------------------------- */
102 /* --- Details -------------------------------------------------------------- */
103 #include "crocoddyl/core/integrator/euler.hxx"
104 
105 #endif // CROCODDYL_CORE_INTEGRATOR_EULER_HPP_
bool has_control_limits_
Indicates whether any of the control limits is finite.
Definition: action-base.hpp:70
std::size_t nu_
Control dimension.
Definition: action-base.hpp:64
VectorXs unone_
Neutral state.
Definition: action-base.hpp:67
VectorXs u_ub_
Upper control limits.
Definition: action-base.hpp:69
VectorXs u_lb_
Lower control limits.
Definition: action-base.hpp:68
boost::shared_ptr< StateAbstract > state_
Model of the state.
Definition: action-base.hpp:66
std::size_t nr_
Dimension of the cost residual.
Definition: action-base.hpp:65