crocoddyl  1.8.1
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 
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, const DifferentialActionModelAbstractTpl<Scalar>& model);
203 
209  virtual void print(std::ostream& os) const;
210 
211  protected:
212  std::size_t nu_;
213  std::size_t nr_;
214  boost::shared_ptr<StateAbstract> state_;
215  VectorXs unone_;
216  VectorXs u_lb_;
217  VectorXs u_ub_;
219 
220  void update_has_control_limits();
221 };
222 
223 template <typename _Scalar>
225  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
226 
227  typedef _Scalar Scalar;
229  typedef typename MathBase::VectorXs VectorXs;
230  typedef typename MathBase::MatrixXs MatrixXs;
231 
232  template <template <typename Scalar> class Model>
233  explicit DifferentialActionDataAbstractTpl(Model<Scalar>* const model)
234  : cost(0.),
235  xout(model->get_state()->get_nv()),
236  Fx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
237  Fu(model->get_state()->get_nv(), model->get_nu()),
238  r(model->get_nr()),
239  Lx(model->get_state()->get_ndx()),
240  Lu(model->get_nu()),
241  Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
242  Lxu(model->get_state()->get_ndx(), model->get_nu()),
243  Luu(model->get_nu(), model->get_nu()) {
244  xout.setZero();
245  r.setZero();
246  Fx.setZero();
247  Fu.setZero();
248  Lx.setZero();
249  Lu.setZero();
250  Lxx.setZero();
251  Lxu.setZero();
252  Luu.setZero();
253  }
255 
256  Scalar cost;
257  VectorXs xout;
258  MatrixXs Fx;
259  MatrixXs Fu;
260  VectorXs r;
261  VectorXs Lx;
262  VectorXs Lu;
263  MatrixXs Lxx;
264  MatrixXs Lxu;
265  MatrixXs Luu;
266 };
267 
268 } // namespace crocoddyl
269 
270 /* --- Details -------------------------------------------------------------- */
271 /* --- Details -------------------------------------------------------------- */
272 /* --- Details -------------------------------------------------------------- */
273 #include "crocoddyl/core/diff-action-base.hxx"
274 
275 #endif // CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
Abstract class for differential action model.
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.
void calcDiff(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
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.
void calc(const boost::shared_ptr< DifferentialActionDataAbstract > &data, const Eigen::Ref< const VectorXs > &x)
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.
std::size_t get_nu() const
Return the dimension of the control input.
Abstract class for the state representation.
Definition: state-base.hpp:45
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.