crocoddyl  1.9.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
diff-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_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/math.hpp"
19 
20 namespace crocoddyl {
21 
52 template <typename _Scalar>
54  public:
55  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
56 
57  typedef _Scalar Scalar;
61  typedef typename MathBase::VectorXs VectorXs;
62  typedef typename MathBase::MatrixXs MatrixXs;
63 
71  DifferentialActionModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nu,
72  const std::size_t nr = 0);
74 
82  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
83  const Eigen::Ref<const VectorXs>& u) = 0;
84 
94  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
95  const Eigen::Ref<const VectorXs>& x);
96 
108  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
109  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) = 0;
110 
120  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
121  const Eigen::Ref<const VectorXs>& x);
122 
128  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
129 
133  virtual bool checkData(const boost::shared_ptr<DifferentialActionDataAbstract>& data);
134 
147  virtual void quasiStatic(const boost::shared_ptr<DifferentialActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
148  const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter = 100,
149  const Scalar tol = Scalar(1e-9));
150 
162  VectorXs quasiStatic_x(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const VectorXs& x,
163  const std::size_t maxiter = 100, const Scalar tol = Scalar(1e-9));
164 
168  std::size_t get_nu() const;
169 
173  std::size_t get_nr() const;
174 
178  const boost::shared_ptr<StateAbstract>& get_state() const;
179 
183  const VectorXs& get_u_lb() const;
184 
188  const VectorXs& get_u_ub() const;
189 
194 
198  void set_u_lb(const VectorXs& u_lb);
199 
203  void set_u_ub(const VectorXs& u_ub);
204 
208  template <class Scalar>
209  friend std::ostream& operator<<(std::ostream& os, const DifferentialActionModelAbstractTpl<Scalar>& model);
210 
216  virtual void print(std::ostream& os) const;
217 
218  protected:
219  std::size_t nu_;
220  std::size_t nr_;
221  boost::shared_ptr<StateAbstract> state_;
222  VectorXs unone_;
223  VectorXs u_lb_;
224  VectorXs u_ub_;
226 
227  void update_has_control_limits();
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 DifferentialActionDataAbstractTpl(Model<Scalar>* const model)
241  : cost(0.),
242  xout(model->get_state()->get_nv()),
243  Fx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
244  Fu(model->get_state()->get_nv(), 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  xout.setZero();
252  r.setZero();
253  Fx.setZero();
254  Fu.setZero();
255  Lx.setZero();
256  Lu.setZero();
257  Lxx.setZero();
258  Lxu.setZero();
259  Luu.setZero();
260  }
262 
263  Scalar cost;
264  VectorXs xout;
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/diff-action-base.hxx"
281 
282 #endif // CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
Abstract class for differential action model.
virtual void calc(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the total cost value for nodes that depends only on the state.
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.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
VectorXs quasiStatic_x(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const VectorXs &x, const std::size_t maxiter=100, const Scalar tol=Scalar(1e-9))
DifferentialActionModelAbstractTpl(boost::shared_ptr< StateAbstract > state, const std::size_t nu, const std::size_t nr=0)
Initialize the differential action model.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
virtual void print(std::ostream &os) const
Print relevant information of the differential 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.
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.
friend std::ostream & operator<<(std::ostream &os, const DifferentialActionModelAbstractTpl< Scalar > &model)
Print information on the differential action model.
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.
boost::shared_ptr< StateAbstract > state_
Model of the state.
std::size_t nr_
Dimension of the cost residual.
std::size_t get_nr() const
Return the dimension of the cost-residual vector.
virtual void quasiStatic(const boost::shared_ptr< DifferentialActionDataAbstract > &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.
void set_u_lb(const VectorXs &u_lb)
Modify the control lower bounds.
virtual bool checkData(const boost::shared_ptr< DifferentialActionDataAbstract > &data)
Checks that a specific data belongs to this model.
const VectorXs & get_u_lb() const
Return the control lower bound.
virtual void calcDiff(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
Compute the derivatives of the cost functions with respect to the state only.
std::size_t get_nu() const
Return the dimension of the control input.
Abstract class for the state representation.
Definition: state-base.hpp:42
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.