crocoddyl  1.7.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/to-string.hpp"
19 #include "crocoddyl/core/utils/math.hpp"
20 
21 namespace crocoddyl {
22 
53 template <typename _Scalar>
55  public:
56  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
57 
58  typedef _Scalar Scalar;
62  typedef typename MathBase::VectorXs VectorXs;
63  typedef typename MathBase::MatrixXs MatrixXs;
64 
72  DifferentialActionModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t nu,
73  const std::size_t nr = 0);
75 
83  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
84  const Eigen::Ref<const VectorXs>& u) = 0;
85 
97  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
98  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) = 0;
99 
105  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
106 
110  virtual bool checkData(const boost::shared_ptr<DifferentialActionDataAbstract>& data);
111 
118  void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
119 
126  void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
127 
140  virtual void quasiStatic(const boost::shared_ptr<DifferentialActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
141  const Eigen::Ref<const VectorXs>& x, const std::size_t maxiter = 100,
142  const Scalar tol = Scalar(1e-9));
143 
155  VectorXs quasiStatic_x(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const VectorXs& x,
156  const std::size_t maxiter = 100, const Scalar tol = Scalar(1e-9));
157 
161  std::size_t get_nu() const;
162 
166  std::size_t get_nr() const;
167 
171  const boost::shared_ptr<StateAbstract>& get_state() const;
172 
176  const VectorXs& get_u_lb() const;
177 
181  const VectorXs& get_u_ub() const;
182 
186  bool get_has_control_limits() const;
187 
191  void set_u_lb(const VectorXs& u_lb);
192 
196  void set_u_ub(const VectorXs& u_ub);
197 
201  template <class Scalar>
202  friend std::ostream& operator<<(std::ostream& os,
203  const DifferentialActionModelAbstractTpl<Scalar>& diff_action_model);
204 
205  protected:
206  std::size_t nu_;
207  std::size_t nr_;
208  boost::shared_ptr<StateAbstract> state_;
209  VectorXs unone_;
210  VectorXs u_lb_;
211  VectorXs u_ub_;
213 
214  void update_has_control_limits();
215 };
216 
217 template <typename _Scalar>
219  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
220 
221  typedef _Scalar Scalar;
223  typedef typename MathBase::VectorXs VectorXs;
224  typedef typename MathBase::MatrixXs MatrixXs;
225 
226  template <template <typename Scalar> class Model>
227  explicit DifferentialActionDataAbstractTpl(Model<Scalar>* const model)
228  : cost(0.),
229  xout(model->get_state()->get_nv()),
230  Fx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
231  Fu(model->get_state()->get_nv(), model->get_nu()),
232  r(model->get_nr()),
233  Lx(model->get_state()->get_ndx()),
234  Lu(model->get_nu()),
235  Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
236  Lxu(model->get_state()->get_ndx(), model->get_nu()),
237  Luu(model->get_nu(), model->get_nu()) {
238  xout.setZero();
239  r.setZero();
240  Fx.setZero();
241  Fu.setZero();
242  Lx.setZero();
243  Lu.setZero();
244  Lxx.setZero();
245  Lxu.setZero();
246  Luu.setZero();
247  }
248  virtual ~DifferentialActionDataAbstractTpl() {}
249 
250  Scalar cost;
251  VectorXs xout;
252  MatrixXs Fx;
253  MatrixXs Fu;
254  VectorXs r;
255  VectorXs Lx;
256  VectorXs Lu;
257  MatrixXs Lxx;
258  MatrixXs Lxu;
259  MatrixXs Luu;
260 };
261 
262 } // namespace crocoddyl
263 
264 /* --- Details -------------------------------------------------------------- */
265 /* --- Details -------------------------------------------------------------- */
266 /* --- Details -------------------------------------------------------------- */
267 #include "crocoddyl/core/diff-action-base.hxx"
268 
269 #endif // CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
VectorXs Lx
Jacobian of the cost function.
MatrixXs Fx
Jacobian of the dynamics.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
const VectorXs & get_u_ub() const
Return the control upper bound.
MatrixXs Lxx
Hessian of the cost function.
Abstract class for differential action model.
MatrixXs Luu
Hessian of the cost function.
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.
friend std::ostream & operator<<(std::ostream &os, const DifferentialActionModelAbstractTpl< Scalar > &diff_action_model)
Print information on the DifferentialActionModel.
Abstract class for the state representation.
Definition: fwd.hpp:101
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_lb(const VectorXs &u_lb)
Modify the control lower bounds.
std::size_t get_nr() const
Return the dimension of the cost-residual vector.
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 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.
std::size_t get_nu() const
Return the dimension of the control input.
virtual boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
MatrixXs Lxu
Hessian of the cost function.
boost::shared_ptr< StateAbstract > state_
Model of the state.
bool get_has_control_limits() const
Indicates if there are defined control limits.
VectorXs quasiStatic_x(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const VectorXs &x, const std::size_t maxiter=100, const Scalar tol=Scalar(1e-9))
void set_u_ub(const VectorXs &u_ub)
Modify the control upper bounds.
MatrixXs Fu
Jacobian of the dynamics.
VectorXs Lu
Jacobian of the cost function.
const VectorXs & get_u_lb() const
Return the control lower bound.