crocoddyl  1.3.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 
44  void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
45  void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
46 
47  void quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
48  const Eigen::Ref<const VectorXs>& x, const std::size_t& maxiter = 100, const Scalar& tol = 1e-9);
49  VectorXs quasiStatic_x(const boost::shared_ptr<ActionDataAbstract>& data, const VectorXs& x,
50  const std::size_t& maxiter = 100, const Scalar& tol = 1e-9);
51 
52  const std::size_t& get_nu() const;
53  const std::size_t& get_nr() const;
54  const boost::shared_ptr<StateAbstract>& get_state() const;
55 
56  const VectorXs& get_u_lb() const;
57  const VectorXs& get_u_ub() const;
58  bool const& get_has_control_limits() const;
59 
60  void set_u_lb(const VectorXs& u_lb);
61  void set_u_ub(const VectorXs& u_ub);
62 
63  protected:
64  std::size_t nu_;
65  std::size_t nr_;
66  boost::shared_ptr<StateAbstract> state_;
67  VectorXs unone_;
68  VectorXs u_lb_;
69  VectorXs u_ub_;
71 
72  void update_has_control_limits();
73 };
74 
75 template <typename _Scalar>
77  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
78 
79  typedef _Scalar Scalar;
81 
82  template <template <typename Scalar> class Model>
83  explicit ActionDataAbstractTpl(Model<Scalar>* const model)
84  : cost(Scalar(0.)),
85  xnext(model->get_state()->get_nx()),
86  r(model->get_nr()),
87  Fx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
88  Fu(model->get_state()->get_ndx(), model->get_nu()),
89  Lx(model->get_state()->get_ndx()),
90  Lu(model->get_nu()),
91  Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
92  Lxu(model->get_state()->get_ndx(), model->get_nu()),
93  Luu(model->get_nu(), model->get_nu()) {
94  xnext.setZero();
95  r.setZero();
96  Fx.setZero();
97  Fu.setZero();
98  Lx.setZero();
99  Lu.setZero();
100  Lxx.setZero();
101  Lxu.setZero();
102  Luu.setZero();
103  }
104  virtual ~ActionDataAbstractTpl() {}
105 
106  Scalar cost;
107  typename MathBase::VectorXs xnext;
108  typename MathBase::VectorXs r;
109  typename MathBase::MatrixXs Fx;
110  typename MathBase::MatrixXs Fu;
111  typename MathBase::VectorXs Lx;
112  typename MathBase::VectorXs Lu;
113  typename MathBase::MatrixXs Lxx;
114  typename MathBase::MatrixXs Lxu;
115  typename MathBase::MatrixXs Luu;
116 };
117 
118 } // namespace crocoddyl
119 
120 /* --- Details -------------------------------------------------------------- */
121 /* --- Details -------------------------------------------------------------- */
122 /* --- Details -------------------------------------------------------------- */
123 #include "crocoddyl/core/action-base.hxx"
124 
125 #endif // CROCODDYL_CORE_ACTION_BASE_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
This class abstract state representations.
Definition: fwd.hpp:71
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