crocoddyl  1.4.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
action-base.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_ACTION_BASE_HPP_
10 #define CROCODDYL_CORE_ACTION_BASE_HPP_
11 
12 #include <stdexcept>
13 #include <boost/shared_ptr.hpp>
14 #include <boost/make_shared.hpp>
15 
16 #include "crocoddyl/core/fwd.hpp"
17 #include "crocoddyl/core/state-base.hpp"
18 #include "crocoddyl/core/utils/math.hpp"
19 #include "crocoddyl/core/utils/to-string.hpp"
20 
21 namespace crocoddyl {
22 
23 template <typename _Scalar>
25  public:
26  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
27 
28  typedef _Scalar Scalar;
32  typedef typename MathBase::VectorXs VectorXs;
33 
34  ActionModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t& nu, const std::size_t& nr = 0);
35 
36  virtual ~ActionModelAbstractTpl();
37 
38  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
39  const Eigen::Ref<const VectorXs>& u) = 0;
40  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
41  const Eigen::Ref<const VectorXs>& u) = 0;
42  virtual boost::shared_ptr<ActionDataAbstract> createData();
43  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
44 
45  void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
46  void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
47 
48  void quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
49  const Eigen::Ref<const VectorXs>& x, const std::size_t& maxiter = 100, const Scalar& tol = 1e-9);
50  VectorXs quasiStatic_x(const boost::shared_ptr<ActionDataAbstract>& data, const VectorXs& x,
51  const std::size_t& maxiter = 100, const Scalar& tol = 1e-9);
52 
53  const std::size_t& get_nu() const;
54  const std::size_t& get_nr() const;
55  const boost::shared_ptr<StateAbstract>& get_state() const;
56 
57  const VectorXs& get_u_lb() const;
58  const VectorXs& get_u_ub() const;
59  bool const& get_has_control_limits() const;
60 
61  void set_u_lb(const VectorXs& u_lb);
62  void set_u_ub(const VectorXs& u_ub);
63 
64  protected:
65  std::size_t nu_;
66  std::size_t nr_;
67  boost::shared_ptr<StateAbstract> state_;
68  VectorXs unone_;
69  VectorXs u_lb_;
70  VectorXs u_ub_;
72 
73  void update_has_control_limits();
74 };
75 
76 template <typename _Scalar>
78  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
79 
80  typedef _Scalar Scalar;
82 
83  template <template <typename Scalar> class Model>
84  explicit ActionDataAbstractTpl(Model<Scalar>* const model)
85  : cost(Scalar(0.)),
86  xnext(model->get_state()->get_nx()),
87  r(model->get_nr()),
88  Fx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
89  Fu(model->get_state()->get_ndx(), model->get_nu()),
90  Lx(model->get_state()->get_ndx()),
91  Lu(model->get_nu()),
92  Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
93  Lxu(model->get_state()->get_ndx(), model->get_nu()),
94  Luu(model->get_nu(), model->get_nu()) {
95  xnext.setZero();
96  r.setZero();
97  Fx.setZero();
98  Fu.setZero();
99  Lx.setZero();
100  Lu.setZero();
101  Lxx.setZero();
102  Lxu.setZero();
103  Luu.setZero();
104  }
105  virtual ~ActionDataAbstractTpl() {}
106 
107  Scalar cost;
108  typename MathBase::VectorXs xnext;
109  typename MathBase::VectorXs r;
110  typename MathBase::MatrixXs Fx;
111  typename MathBase::MatrixXs Fu;
112  typename MathBase::VectorXs Lx;
113  typename MathBase::VectorXs Lu;
114  typename MathBase::MatrixXs Lxx;
115  typename MathBase::MatrixXs Lxu;
116  typename MathBase::MatrixXs Luu;
117 };
118 
119 } // namespace crocoddyl
120 
121 /* --- Details -------------------------------------------------------------- */
122 /* --- Details -------------------------------------------------------------- */
123 /* --- Details -------------------------------------------------------------- */
124 #include "crocoddyl/core/action-base.hxx"
125 
126 #endif // CROCODDYL_CORE_ACTION_BASE_HPP_
bool has_control_limits_
Indicates whether any of the control limits is finite.
Definition: action-base.hpp:71
std::size_t nu_
Control dimension.
Definition: action-base.hpp:65
Abstract class for the state representation.
Definition: fwd.hpp:71
VectorXs unone_
Neutral state.
Definition: action-base.hpp:68
VectorXs u_ub_
Upper control limits.
Definition: action-base.hpp:70
VectorXs u_lb_
Lower control limits.
Definition: action-base.hpp:69
boost::shared_ptr< StateAbstract > state_
Model of the state.
Definition: action-base.hpp:67
std::size_t nr_
Dimension of the cost residual.
Definition: action-base.hpp:66