crocoddyl  1.9.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 
20 namespace crocoddyl {
21 
58 template <typename _Scalar>
60  public:
61  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
62 
63  typedef _Scalar Scalar;
67  typedef typename MathBase::VectorXs VectorXs;
68 
76  ActionModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nu, const std::size_t nr = 0);
77  virtual ~ActionModelAbstractTpl();
78 
86  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
87  const Eigen::Ref<const VectorXs>& u) = 0;
88 
98  virtual void calc(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
99 
111  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
112  const Eigen::Ref<const VectorXs>& u) = 0;
113 
123  virtual void calcDiff(const boost::shared_ptr<ActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
124 
130  virtual boost::shared_ptr<ActionDataAbstract> createData();
131 
135  virtual bool checkData(const boost::shared_ptr<ActionDataAbstract>& data);
136 
149  virtual void quasiStatic(const boost::shared_ptr<ActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
150  const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter = 100,
151  const Scalar tol = Scalar(1e-9));
152 
164  VectorXs quasiStatic_x(const boost::shared_ptr<ActionDataAbstract>& data, const VectorXs& x,
165  const std::size_t maxiter = 100, const Scalar tol = Scalar(1e-9));
166 
170  std::size_t get_nu() const;
171 
175  std::size_t get_nr() const;
176 
180  const boost::shared_ptr<StateAbstract>& get_state() const;
181 
185  const VectorXs& get_u_lb() const;
186 
190  const VectorXs& get_u_ub() const;
191 
196 
200  void set_u_lb(const VectorXs& u_lb);
201 
205  void set_u_ub(const VectorXs& u_ub);
206 
210  template <class Scalar>
211  friend std::ostream& operator<<(std::ostream& os, const ActionModelAbstractTpl<Scalar>& model);
212 
218  virtual void print(std::ostream& os) const;
219 
220  protected:
221  std::size_t nu_;
222  std::size_t nr_;
223  boost::shared_ptr<StateAbstract> state_;
224  VectorXs unone_;
225  VectorXs u_lb_;
226  VectorXs u_ub_;
228 
233 };
234 
235 template <typename _Scalar>
237  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
238 
239  typedef _Scalar Scalar;
241  typedef typename MathBase::VectorXs VectorXs;
242  typedef typename MathBase::MatrixXs MatrixXs;
243 
244  template <template <typename Scalar> class Model>
245  explicit ActionDataAbstractTpl(Model<Scalar>* const model)
246  : cost(Scalar(0.)),
247  xnext(model->get_state()->get_nx()),
248  Fx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
249  Fu(model->get_state()->get_ndx(), model->get_nu()),
250  r(model->get_nr()),
251  Lx(model->get_state()->get_ndx()),
252  Lu(model->get_nu()),
253  Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
254  Lxu(model->get_state()->get_ndx(), model->get_nu()),
255  Luu(model->get_nu(), model->get_nu()) {
256  xnext.setZero();
257  Fx.setZero();
258  Fu.setZero();
259  r.setZero();
260  Lx.setZero();
261  Lu.setZero();
262  Lxx.setZero();
263  Lxu.setZero();
264  Luu.setZero();
265  }
266  virtual ~ActionDataAbstractTpl() {}
267 
268  Scalar cost;
269  VectorXs xnext;
270  MatrixXs Fx;
271  MatrixXs Fu;
272  VectorXs r;
273  VectorXs Lx;
274  VectorXs Lu;
275  MatrixXs Lxx;
276  MatrixXs Lxu;
277  MatrixXs Luu;
278 };
279 
280 } // namespace crocoddyl
281 
282 /* --- Details -------------------------------------------------------------- */
283 /* --- Details -------------------------------------------------------------- */
284 /* --- Details -------------------------------------------------------------- */
285 #include "crocoddyl/core/action-base.hxx"
286 
287 #endif // CROCODDYL_CORE_ACTION_BASE_HPP_
Abstract class for action model.
Definition: action-base.hpp:59
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.
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.
virtual void calc(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the total cost value for nodes that depends only on the state.
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 void calcDiff(const boost::shared_ptr< ActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the cost functions with respect to the state only.
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:42
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.