crocoddyl  1.3.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
diff-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_DIFF_ACTION_BASE_HPP_
10 #define CROCODDYL_CORE_DIFF_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/to-string.hpp"
19 
20 namespace crocoddyl {
21 
42 template <typename _Scalar>
44  public:
45  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
46 
47  typedef _Scalar Scalar;
51  typedef typename MathBase::VectorXs VectorXs;
52  typedef typename MathBase::MatrixXs MatrixXs;
53 
54  DifferentialActionModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t& nu,
55  const std::size_t& nr = 0);
57 
58  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
59  const Eigen::Ref<const VectorXs>& u) = 0;
60  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
61  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) = 0;
62  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
63 
64  void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
65  void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
66 
67  const std::size_t& get_nu() const;
68  const std::size_t& get_nr() const;
69  const boost::shared_ptr<StateAbstract>& get_state() const;
70 
71  const VectorXs& get_u_lb() const;
72  const VectorXs& get_u_ub() const;
73  bool const& get_has_control_limits() const;
74 
75  void set_u_lb(const VectorXs& u_lb);
76  void set_u_ub(const VectorXs& u_ub);
77 
78  protected:
79  std::size_t nu_;
80  std::size_t nr_;
81  boost::shared_ptr<StateAbstract> state_;
82  VectorXs unone_;
83  VectorXs u_lb_;
84  VectorXs u_ub_;
86 
87  void update_has_control_limits();
88 };
89 
90 template <typename _Scalar>
92  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
93 
94  typedef _Scalar Scalar;
96  typedef typename MathBase::VectorXs VectorXs;
97  typedef typename MathBase::MatrixXs MatrixXs;
98 
99  template <template <typename Scalar> class Model>
100  explicit DifferentialActionDataAbstractTpl(Model<Scalar>* const model)
101  : cost(0.),
102  xout(model->get_state()->get_nv()),
103  Fx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
104  Fu(model->get_state()->get_nv(), model->get_nu()),
105  r(model->get_nr()),
106  Lx(model->get_state()->get_ndx()),
107  Lu(model->get_nu()),
108  Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
109  Lxu(model->get_state()->get_ndx(), model->get_nu()),
110  Luu(model->get_nu(), model->get_nu()) {
111  xout.setZero();
112  r.setZero();
113  Fx.setZero();
114  Fu.setZero();
115  Lx.setZero();
116  Lu.setZero();
117  Lxx.setZero();
118  Lxu.setZero();
119  Luu.setZero();
120  }
121  virtual ~DifferentialActionDataAbstractTpl() {}
122 
123  Scalar cost;
124  VectorXs xout;
125  MatrixXs Fx;
126  MatrixXs Fu;
127  VectorXs r;
128  VectorXs Lx;
129  VectorXs Lu;
130  MatrixXs Lxx;
131  MatrixXs Lxu;
132  MatrixXs Luu;
133 };
134 
135 } // namespace crocoddyl
136 
137 /* --- Details -------------------------------------------------------------- */
138 /* --- Details -------------------------------------------------------------- */
139 /* --- Details -------------------------------------------------------------- */
140 #include "crocoddyl/core/diff-action-base.hxx"
141 
142 #endif // CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
This class DifferentialActionModelAbstract represents a first-order ODE, i.e. where and represents ...
std::size_t nr_
Dimension of the cost residual.
This class abstract state representations.
Definition: fwd.hpp:71
bool has_control_limits_
Indicates whether any of the control limits is finite.
boost::shared_ptr< StateAbstract > state_
Model of the state.