crocoddyl  1.8.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
action-base.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-2021, LAAS-CNRS, University of Edinburgh, University of Oxford
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 
59 template <typename _Scalar>
61  public:
62  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
63 
64  typedef _Scalar Scalar;
68  typedef typename MathBase::VectorXs VectorXs;
69 
77  ActionModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nu, const std::size_t nr = 0);
78  virtual ~ActionModelAbstractTpl();
79 
87  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
88  const Eigen::Ref<const VectorXs>& u) = 0;
89 
101  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
102  const Eigen::Ref<const VectorXs>& u) = 0;
103 
109  virtual boost::shared_ptr<ActionDataAbstract> createData();
110 
114  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
115 
122  void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
123 
130  void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
131 
144  virtual void quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
145  const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter = 100,
146  const Scalar tol = Scalar(1e-9));
147 
159  VectorXs quasiStatic_x(const boost::shared_ptr<ActionDataAbstract>& data, const VectorXs& x,
160  const std::size_t maxiter = 100, const Scalar tol = Scalar(1e-9));
161 
165  std::size_t get_nu() const;
166 
170  std::size_t get_nr() const;
171 
175  const boost::shared_ptr<StateAbstract>& get_state() const;
176 
180  const VectorXs& get_u_lb() const;
181 
185  const VectorXs& get_u_ub() const;
186 
190  bool get_has_control_limits() const;
191 
195  void set_u_lb(const VectorXs& u_lb);
196 
200  void set_u_ub(const VectorXs& u_ub);
201 
205  template <class Scalar>
206  friend std::ostream& operator<<(std::ostream& os, const ActionModelAbstractTpl<Scalar>& model);
207 
213  virtual void print(std::ostream& os) const;
214 
215  protected:
216  std::size_t nu_;
217  std::size_t nr_;
218  boost::shared_ptr<StateAbstract> state_;
219  VectorXs unone_;
220  VectorXs u_lb_;
221  VectorXs u_ub_;
223 
228 };
229 
230 template <typename _Scalar>
232  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
233 
234  typedef _Scalar Scalar;
236  typedef typename MathBase::VectorXs VectorXs;
237  typedef typename MathBase::MatrixXs MatrixXs;
238 
239  template <template <typename Scalar> class Model>
240  explicit ActionDataAbstractTpl(Model<Scalar>* const model)
241  : cost(Scalar(0.)),
242  xnext(model->get_state()->get_nx()),
243  Fx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
244  Fu(model->get_state()->get_ndx(), model->get_nu()),
245  r(model->get_nr()),
246  Lx(model->get_state()->get_ndx()),
247  Lu(model->get_nu()),
248  Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
249  Lxu(model->get_state()->get_ndx(), model->get_nu()),
250  Luu(model->get_nu(), model->get_nu()) {
251  xnext.setZero();
252  Fx.setZero();
253  Fu.setZero();
254  r.setZero();
255  Lx.setZero();
256  Lu.setZero();
257  Lxx.setZero();
258  Lxu.setZero();
259  Luu.setZero();
260  }
261  virtual ~ActionDataAbstractTpl() {}
262 
263  Scalar cost;
264  VectorXs xnext;
265  MatrixXs Fx;
266  MatrixXs Fu;
267  VectorXs r;
268  VectorXs Lx;
269  VectorXs Lu;
270  MatrixXs Lxx;
271  MatrixXs Lxu;
272  MatrixXs Luu;
273 };
274 
275 } // namespace crocoddyl
276 
277 /* --- Details -------------------------------------------------------------- */
278 /* --- Details -------------------------------------------------------------- */
279 /* --- Details -------------------------------------------------------------- */
280 #include "crocoddyl/core/action-base.hxx"
281 
282 #endif // CROCODDYL_CORE_ACTION_BASE_HPP_
void set_u_lb(const VectorXs &u_lb)
Modify the control lower bounds.
Abstract class for action model.
Definition: action-base.hpp:60
bool has_control_limits_
Indicates whether any of the control limits is finite.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
MatrixXs Lxx
Hessian of the cost function.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
std::size_t nu_
Control dimension.
void update_has_control_limits()
Update the status of the control limits (i.e. if there are defined limits)
VectorXs xnext
evolution state
std::size_t get_nu() const
Return the dimension of the control input.
Abstract class for the state representation.
Definition: fwd.hpp:112
bool get_has_control_limits() const
Indicates if there are defined control limits.
void set_u_ub(const VectorXs &u_ub)
Modify the control upper bounds.
const VectorXs & get_u_lb() const
Return the control lower bound.
VectorXs quasiStatic_x(const boost::shared_ptr< ActionDataAbstract > &data, const VectorXs &x, const std::size_t maxiter=100, const Scalar tol=Scalar(1e-9))
std::size_t get_nr() const
Return the dimension of the cost-residual vector.
VectorXs r
Cost residual.
VectorXs Lx
Jacobian of the cost function.
VectorXs unone_
Neutral state.
MatrixXs Fx
Jacobian of the dynamics.
VectorXs Lu
Jacobian of the cost function.
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
ActionModelAbstractTpl(boost::shared_ptr< StateAbstract > state, const std::size_t nu, const std::size_t nr=0)
Initialize the action model.
VectorXs u_ub_
Upper control limits.
MatrixXs Luu
Hessian of the cost function.
VectorXs u_lb_
Lower control limits.
boost::shared_ptr< StateAbstract > state_
Model of the state.
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.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0
Compute the next state and cost value.
virtual void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &u)=0
Compute the derivatives of the dynamics and cost functions.
virtual void print(std::ostream &os) const
Print relevant information of the action model.
MatrixXs Lxu
Hessian of the cost function.
std::size_t nr_
Dimension of the cost residual.
MatrixXs Fu
Jacobian of the dynamics.
const VectorXs & get_u_ub() const
Return the control upper bound.