crocoddyl  1.8.1
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 
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_
Abstract class for action model.
Definition: action-base.hpp:60
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
VectorXs u_lb_
Lower control limits.
VectorXs u_ub_
Upper control limits.
virtual void print(std::ostream &os) const
Print relevant information of the action model.
void set_u_ub(const VectorXs &u_ub)
Modify the control upper bounds.
const VectorXs & get_u_ub() const
Return the control upper bound.
bool has_control_limits_
Indicates whether any of the control limits is finite.
bool get_has_control_limits() const
Indicates if there are defined control limits.
VectorXs quasiStatic_x(const boost::shared_ptr< ActionDataAbstract > &data, const VectorXs &x, const std::size_t maxiter=100, const Scalar tol=Scalar(1e-9))
void update_has_control_limits()
Update the status of the control limits (i.e. if there are defined limits)
ActionModelAbstractTpl(boost::shared_ptr< StateAbstract > state, const std::size_t nu, const std::size_t nr=0)
Initialize the action model.
void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the next state and cost value.
void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the dynamics and cost functions.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nu_
Control dimension.
VectorXs unone_
Neutral state.
std::size_t nr_
Dimension of the cost residual.
virtual bool checkData(const boost::shared_ptr< ActionDataAbstract > &data)
Checks that a specific data belongs to this model.
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.
std::size_t get_nr() const
Return the dimension of the cost-residual vector.
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.
void set_u_lb(const VectorXs &u_lb)
Modify the control lower bounds.
friend std::ostream & operator<<(std::ostream &os, const ActionModelAbstractTpl< Scalar > &model)
Print information on the action model.
const VectorXs & get_u_lb() const
Return the control lower bound.
virtual boost::shared_ptr< ActionDataAbstract > createData()
Create the action data.
std::size_t get_nu() const
Return the dimension of the control input.
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.
Abstract class for the state representation.
Definition: state-base.hpp:45
VectorXs xnext
evolution state
MatrixXs Fx
Jacobian of the dynamics.
MatrixXs Fu
Jacobian of the dynamics.
MatrixXs Luu
Hessian of the cost function.
VectorXs Lx
Jacobian of the cost function.
MatrixXs Lxx
Hessian of the cost function.
VectorXs Lu
Jacobian of the cost function.
MatrixXs Lxu
Hessian of the cost function.
VectorXs r
Cost residual.