crocoddyl  1.5.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>
24 class ActionModelAbstractTpl {
25  public:
26  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
27 
28  typedef _Scalar Scalar;
29  typedef MathBaseTpl<Scalar> MathBase;
30  typedef ActionDataAbstractTpl<Scalar> ActionDataAbstract;
31  typedef StateAbstractTpl<Scalar> StateAbstract;
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 
60  virtual void quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
61  const Eigen::Ref<const VectorXs>& x, const std::size_t& maxiter = 100,
62  const Scalar& tol = Scalar(1e-9));
63 
75  VectorXs quasiStatic_x(const boost::shared_ptr<ActionDataAbstract>& data, const VectorXs& x,
76  const std::size_t& maxiter = 100, const Scalar& tol = Scalar(1e-9));
77 
78  const std::size_t& get_nu() const;
79  const std::size_t& get_nr() const;
80  const boost::shared_ptr<StateAbstract>& get_state() const;
81 
82  const VectorXs& get_u_lb() const;
83  const VectorXs& get_u_ub() const;
84  bool const& get_has_control_limits() const;
85 
86  void set_u_lb(const VectorXs& u_lb);
87  void set_u_ub(const VectorXs& u_ub);
88 
89  protected:
90  std::size_t nu_;
91  std::size_t nr_;
92  boost::shared_ptr<StateAbstract> state_;
93  VectorXs unone_;
94  VectorXs u_lb_;
95  VectorXs u_ub_;
96  bool has_control_limits_;
97 
98  void update_has_control_limits();
99 };
100 
101 template <typename _Scalar>
102 struct ActionDataAbstractTpl {
103  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
104 
105  typedef _Scalar Scalar;
106  typedef MathBaseTpl<Scalar> MathBase;
107 
108  template <template <typename Scalar> class Model>
109  explicit ActionDataAbstractTpl(Model<Scalar>* const model)
110  : cost(Scalar(0.)),
111  xnext(model->get_state()->get_nx()),
112  r(model->get_nr()),
113  Fx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
114  Fu(model->get_state()->get_ndx(), model->get_nu()),
115  Lx(model->get_state()->get_ndx()),
116  Lu(model->get_nu()),
117  Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
118  Lxu(model->get_state()->get_ndx(), model->get_nu()),
119  Luu(model->get_nu(), model->get_nu()) {
120  xnext.setZero();
121  r.setZero();
122  Fx.setZero();
123  Fu.setZero();
124  Lx.setZero();
125  Lu.setZero();
126  Lxx.setZero();
127  Lxu.setZero();
128  Luu.setZero();
129  }
130  virtual ~ActionDataAbstractTpl() {}
131 
132  Scalar cost;
133  typename MathBase::VectorXs xnext;
134  typename MathBase::VectorXs r;
135  typename MathBase::MatrixXs Fx;
136  typename MathBase::MatrixXs Fu;
137  typename MathBase::VectorXs Lx;
138  typename MathBase::VectorXs Lu;
139  typename MathBase::MatrixXs Lxx;
140  typename MathBase::MatrixXs Lxu;
141  typename MathBase::MatrixXs Luu;
142 };
143 
144 } // namespace crocoddyl
145 
146 /* --- Details -------------------------------------------------------------- */
147 /* --- Details -------------------------------------------------------------- */
148 /* --- Details -------------------------------------------------------------- */
149 #include "crocoddyl/core/action-base.hxx"
150 
151 #endif // CROCODDYL_CORE_ACTION_BASE_HPP_
crocoddyl::ActionDataAbstractTpl
Definition: action-base.hpp:102
crocoddyl::ActionModelAbstractTpl::has_control_limits_
bool has_control_limits_
Indicates whether any of the control limits is finite.
Definition: action-base.hpp:96
crocoddyl::MathBaseTpl< Scalar >
crocoddyl::ActionModelAbstractTpl::nu_
std::size_t nu_
Control dimension.
Definition: action-base.hpp:90
crocoddyl::ActionModelAbstractTpl
Definition: action-base.hpp:24
crocoddyl::ActionModelAbstractTpl::state_
boost::shared_ptr< StateAbstract > state_
Model of the state.
Definition: action-base.hpp:92
crocoddyl::ActionModelAbstractTpl::quasiStatic_x
VectorXs quasiStatic_x(const boost::shared_ptr< ActionDataAbstract > &data, const VectorXs &x, const std::size_t &maxiter=100, const Scalar &tol=Scalar(1e-9))
crocoddyl::ActionModelAbstractTpl::unone_
VectorXs unone_
Neutral state.
Definition: action-base.hpp:93
crocoddyl::ActionModelAbstractTpl::u_lb_
VectorXs u_lb_
Lower control limits.
Definition: action-base.hpp:94
crocoddyl::ActionModelAbstractTpl::u_ub_
VectorXs u_ub_
Upper control limits.
Definition: action-base.hpp:95
crocoddyl::StateAbstractTpl
Abstract class for the state representation.
Definition: fwd.hpp:71
crocoddyl::ActionModelAbstractTpl::quasiStatic
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.
crocoddyl::ActionModelAbstractTpl::nr_
std::size_t nr_
Dimension of the cost residual.
Definition: action-base.hpp:91