crocoddyl  1.5.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
diff-action-base.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh
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 
47 template <typename _Scalar>
49  public:
50  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
51 
52  typedef _Scalar Scalar;
56  typedef typename MathBase::VectorXs VectorXs;
57  typedef typename MathBase::MatrixXs MatrixXs;
58 
66  DifferentialActionModelAbstractTpl(boost::shared_ptr<StateAbstract> state, const std::size_t& nu,
67  const std::size_t& nr = 0);
69 
77  virtual void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x,
78  const Eigen::Ref<const VectorXs>& u) = 0;
79 
91  virtual void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data,
92  const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& u) = 0;
93 
99  virtual boost::shared_ptr<DifferentialActionDataAbstract> createData();
100 
104  virtual bool checkData(const boost::shared_ptr<DifferentialActionDataAbstract>& data);
105 
112  void calc(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
113 
120  void calcDiff(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const Eigen::Ref<const VectorXs>& x);
121 
134  virtual void quasiStatic(const boost::shared_ptr<DifferentialActionDataAbstract>& data, Eigen::Ref<VectorXs> u,
135  const Eigen::Ref<const VectorXs>& x, const std::size_t& maxiter = 100,
136  const Scalar& tol = Scalar(1e-9));
137 
149  VectorXs quasiStatic_x(const boost::shared_ptr<DifferentialActionDataAbstract>& data, const VectorXs& x,
150  const std::size_t& maxiter = 100, const Scalar& tol = Scalar(1e-9));
151 
155  const std::size_t& get_nu() const;
156 
160  const std::size_t& get_nr() const;
161 
165  const boost::shared_ptr<StateAbstract>& get_state() const;
166 
170  const VectorXs& get_u_lb() const;
171 
175  const VectorXs& get_u_ub() const;
176 
180  bool const& get_has_control_limits() const;
181 
185  void set_u_lb(const VectorXs& u_lb);
186 
190  void set_u_ub(const VectorXs& u_ub);
191 
192  protected:
193  std::size_t nu_;
194  std::size_t nr_;
195  boost::shared_ptr<StateAbstract> state_;
196  VectorXs unone_;
197  VectorXs u_lb_;
198  VectorXs u_ub_;
200 
201  void update_has_control_limits();
202 };
203 
204 template <typename _Scalar>
206  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
207 
208  typedef _Scalar Scalar;
210  typedef typename MathBase::VectorXs VectorXs;
211  typedef typename MathBase::MatrixXs MatrixXs;
212 
213  template <template <typename Scalar> class Model>
214  explicit DifferentialActionDataAbstractTpl(Model<Scalar>* const model)
215  : cost(0.),
216  xout(model->get_state()->get_nv()),
217  Fx(model->get_state()->get_nv(), model->get_state()->get_ndx()),
218  Fu(model->get_state()->get_nv(), model->get_nu()),
219  r(model->get_nr()),
220  Lx(model->get_state()->get_ndx()),
221  Lu(model->get_nu()),
222  Lxx(model->get_state()->get_ndx(), model->get_state()->get_ndx()),
223  Lxu(model->get_state()->get_ndx(), model->get_nu()),
224  Luu(model->get_nu(), model->get_nu()) {
225  xout.setZero();
226  r.setZero();
227  Fx.setZero();
228  Fu.setZero();
229  Lx.setZero();
230  Lu.setZero();
231  Lxx.setZero();
232  Lxu.setZero();
233  Luu.setZero();
234  }
235  virtual ~DifferentialActionDataAbstractTpl() {}
236 
237  Scalar cost;
238  VectorXs xout;
239  MatrixXs Fx;
240  MatrixXs Fu;
241  VectorXs r;
242  VectorXs Lx;
243  VectorXs Lu;
244  MatrixXs Lxx;
245  MatrixXs Lxu;
246  MatrixXs Luu;
247 };
248 
249 } // namespace crocoddyl
250 
251 /* --- Details -------------------------------------------------------------- */
252 /* --- Details -------------------------------------------------------------- */
253 /* --- Details -------------------------------------------------------------- */
254 #include "crocoddyl/core/diff-action-base.hxx"
255 
256 #endif // CROCODDYL_CORE_DIFF_ACTION_BASE_HPP_
bool const & get_has_control_limits() const
Indicates if there are defined control limits.
const boost::shared_ptr< StateAbstract > & get_state() const
Return the state.
const VectorXs & get_u_ub() const
Return the control upper bound.
const std::size_t & get_nr() const
Return the dimension of the cost-residual vector.
const std::size_t & get_nu() const
Return the dimension of the control input.
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.
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.
Abstract class for the state representation.
Definition: fwd.hpp:71
void set_u_lb(const VectorXs &u_lb)
Modify the control lower bounds.
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 boost::shared_ptr< DifferentialActionDataAbstract > createData()
Create the differential action data.
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.
boost::shared_ptr< StateAbstract > state_
Model of 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.
void set_u_ub(const VectorXs &u_ub)
Modify the control upper bounds.
const VectorXs & get_u_lb() const
Return the control lower bound.