crocoddyl  1.4.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 
46 template <typename _Scalar>
48  public:
49  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
50 
51  typedef _Scalar Scalar;
55  typedef typename MathBase::VectorXs VectorXs;
56  typedef typename MathBase::MatrixXs MatrixXs;
57 
65  DifferentialActionModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t& nu,
66  const std::size_t& nr = 0);
68 
76  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
77  const Eigen::Ref<const VectorXs>& u) = 0;
78 
90  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
91  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) = 0;
92 
98  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
99 
103  virtual bool checkData(const boost::shared_ptr<DifferentialActionDataAbstract>& data);
104 
111  void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
112 
119  void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
120 
124  const std::size_t& get_nu() const;
125 
129  const std::size_t& get_nr() const;
130 
134  const boost::shared_ptr<StateAbstract>& get_state() const;
135 
139  const VectorXs& get_u_lb() const;
140 
144  const VectorXs& get_u_ub() const;
145 
149  bool const& get_has_control_limits() const;
150 
154  void set_u_lb(const VectorXs& u_lb);
155 
159  void set_u_ub(const VectorXs& u_ub);
160 
161  protected:
162  std::size_t nu_;
163  std::size_t nr_;
164  boost::shared_ptr<StateAbstract> state_;
165  VectorXs unone_;
166  VectorXs u_lb_;
167  VectorXs u_ub_;
169 
170  void update_has_control_limits();
171 };
172 
173 template <typename _Scalar>
175  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
176 
177  typedef _Scalar Scalar;
179  typedef typename MathBase::VectorXs VectorXs;
180  typedef typename MathBase::MatrixXs MatrixXs;
181 
182  template <template <typename Scalar> class Model>
183  explicit DifferentialActionDataAbstractTpl(Model<Scalar>* const model)
184  : cost(0.),
185  xout(model->get_state()->get_nv()),
186  Fx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
187  Fu(model->get_state()->get_nv(), model->get_nu()),
188  r(model->get_nr()),
189  Lx(model->get_state()->get_ndx()),
190  Lu(model->get_nu()),
191  Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
192  Lxu(model->get_state()->get_ndx(), model->get_nu()),
193  Luu(model->get_nu(), model->get_nu()) {
194  xout.setZero();
195  r.setZero();
196  Fx.setZero();
197  Fu.setZero();
198  Lx.setZero();
199  Lu.setZero();
200  Lxx.setZero();
201  Lxu.setZero();
202  Luu.setZero();
203  }
204  virtual ~DifferentialActionDataAbstractTpl() {}
205 
206  Scalar cost;
207  VectorXs xout;
208  MatrixXs Fx;
209  MatrixXs Fu;
210  VectorXs r;
211  VectorXs Lx;
212  VectorXs Lu;
213  MatrixXs Lxx;
214  MatrixXs Lxu;
215  MatrixXs Luu;
216 };
217 
218 } // namespace crocoddyl
219 
220 /* --- Details -------------------------------------------------------------- */
221 /* --- Details -------------------------------------------------------------- */
222 /* --- Details -------------------------------------------------------------- */
223 #include "crocoddyl/core/diff-action-base.hxx"
224 
225 #endif // CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
bool const & get_has_control_limits() const
Indicates if there are defined control limits.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
const VectorXs & get_u_ub() const
Return the control upper bound.
const std::size_t & get_nr() const
Return the dimension of the cost-residual vector.
const std::size_t & get_nu() const
Return the dimension of the control input.
Abstract class for differential action model.
virtual void calcDiff(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0
Compute the derivatives of the dynamics and cost functions.
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Checks that a specific data belongs to this model.
std::size_t nr_
Dimension of the cost residual.
Abstract class for the state representation.
Definition: fwd.hpp:71
void set_u_lb(const VectorXs &u_lb)
Modify the control lower bounds.
virtual void calc(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0
Compute the system acceleration and cost value.
bool has_control_limits_
Indicates whether any of the control limits is finite.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
boost::shared_ptr< StateAbstract > state_
Model of the state.
DifferentialActionModelAbstractTpl(boost::shared_ptr< StateAbstract > state, const std::size_t &nu, const std::size_t &nr=0)
Initialize the differential action model.
void set_u_ub(const VectorXs &u_ub)
Modify the control upper bounds.
const VectorXs & get_u_lb() const
Return the control lower bound.