crocoddyl  1.3.0
Contact RObot COntrol by Differential DYnamic programming Library (Crocoddyl)
state-base.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2018-2020, LAAS-CNRS, University of Edinburgh, INRIA
5 // Copyright note valid unless otherwise stated in individual files.
6 // All rights reserved.
8 
9 #ifndef CROCODDYL_CORE_STATE_BASE_HPP_
10 #define CROCODDYL_CORE_STATE_BASE_HPP_
11 
12 #include <vector>
13 #include <string>
14 #include <stdexcept>
15 
16 #include "crocoddyl/core/fwd.hpp"
17 #include "crocoddyl/core/mathbase.hpp"
18 #include "crocoddyl/core/utils/exception.hpp"
19 
20 namespace crocoddyl {
21 
22 enum Jcomponent { both = 0, first = 1, second = 2 };
23 enum AssignmentOp { setto, addto, rmfrom };
24 
25 inline bool is_a_Jcomponent(Jcomponent firstsecond) {
26  return (firstsecond == first || firstsecond == second || firstsecond == both);
27 }
28 
29 inline bool is_a_AssignmentOp(AssignmentOp op) { return (op == setto || op == addto || op == rmfrom); }
30 
42 template <typename _Scalar>
43 class StateAbstractTpl {
44  public:
45  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
46 
47  typedef _Scalar Scalar;
48  typedef MathBaseTpl<Scalar> MathBase;
49  typedef typename MathBase::VectorXs VectorXs;
50  typedef typename MathBase::MatrixXs MatrixXs;
51 
58  StateAbstractTpl(const std::size_t& nx, const std::size_t& ndx);
60  virtual ~StateAbstractTpl();
61 
65  virtual VectorXs zero() const = 0;
66 
70  virtual VectorXs rand() const = 0;
71 
87  virtual void diff(const Eigen::Ref<const VectorXs>& x0, const Eigen::Ref<const VectorXs>& x1,
88  Eigen::Ref<VectorXs> dxout) const = 0;
89 
105  virtual void integrate(const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
106  Eigen::Ref<VectorXs> xout) const = 0;
107 
142  virtual void Jdiff(const Eigen::Ref<const VectorXs>& x0, const Eigen::Ref<const VectorXs>& x1,
143  Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
144  const Jcomponent firstsecond = both) const = 0;
145 
178  virtual void Jintegrate(const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
179  Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
180  const Jcomponent firstsecond = both, const AssignmentOp op = setto) const = 0;
181 
194  virtual void JintegrateTransport(const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
195  Eigen::Ref<MatrixXs> Jin, const Jcomponent firstsecond) const = 0;
196 
204  VectorXs diff_dx(const Eigen::Ref<const VectorXs>& x0, const Eigen::Ref<const VectorXs>& x1);
205 
213  VectorXs integrate_x(const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx);
214 
222  std::vector<MatrixXs> Jdiff_Js(const Eigen::Ref<const VectorXs>& x0, const Eigen::Ref<const VectorXs>& x1,
223  const Jcomponent firstsecond = both);
224 
232  std::vector<MatrixXs> Jintegrate_Js(const Eigen::Ref<const VectorXs>& x, const Eigen::Ref<const VectorXs>& dx,
233  const Jcomponent firstsecond = both);
234 
238  const std::size_t& get_nx() const;
239 
243  const std::size_t& get_ndx() const;
244 
248  const std::size_t& get_nq() const;
249 
253  const std::size_t& get_nv() const;
254 
258  const VectorXs& get_lb() const;
259 
263  const VectorXs& get_ub() const;
264 
268  bool const& get_has_limits() const;
269 
273  void set_lb(const VectorXs& lb);
274 
278  void set_ub(const VectorXs& ub);
279 
280  protected:
281  void update_has_limits();
282 
283  std::size_t nx_;
284  std::size_t ndx_;
285  std::size_t nq_;
286  std::size_t nv_;
287  VectorXs lb_;
288  VectorXs ub_;
289  bool has_limits_;
290 };
291 
292 } // namespace crocoddyl
293 
294 /* --- Details -------------------------------------------------------------- */
295 /* --- Details -------------------------------------------------------------- */
296 /* --- Details -------------------------------------------------------------- */
297 #include "crocoddyl/core/state-base.hxx"
298 
299 #endif // CROCODDYL_CORE_STATE_BASE_HPP_
const std::size_t & get_ndx() const
Return the dimension of the tangent space of the state manifold.
void set_lb(const VectorXs &lb)
Modify the state lower bound.
void set_ub(const VectorXs &ub)
Modify the state upper bound.
VectorXs lb_
Lower state limits.
Definition: state-base.hpp:287
virtual void diff(const Eigen::Ref< const VectorXs > &x0, const Eigen::Ref< const VectorXs > &x1, Eigen::Ref< VectorXs > dxout) const =0
Compute the state manifold differentiation.
VectorXs ub_
Upper state limits.
Definition: state-base.hpp:288
virtual void Jintegrate(const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &dx, Eigen::Ref< MatrixXs > Jfirst, Eigen::Ref< MatrixXs > Jsecond, const Jcomponent firstsecond=both, const AssignmentOp op=setto) const =0
Compute the Jacobian of the state manifold integration.
const VectorXs & get_lb() const
Return the state lower bound.
virtual VectorXs zero() const =0
Generate a zero reference state.
std::vector< MatrixXs > Jintegrate_Js(const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &dx, const Jcomponent firstsecond=both)
std::vector< MatrixXs > Jdiff_Js(const Eigen::Ref< const VectorXs > &x0, const Eigen::Ref< const VectorXs > &x1, const Jcomponent firstsecond=both)
bool has_limits_
Indicates whether any of the state limits is finite.
Definition: state-base.hpp:289
StateAbstractTpl(const std::size_t &nx, const std::size_t &ndx)
Initialize the state dimensions.
bool const & get_has_limits() const
Indicate if the state has defined limits.
const std::size_t & get_nx() const
Return the dimension of the state tuple.
virtual void Jdiff(const Eigen::Ref< const VectorXs > &x0, const Eigen::Ref< const VectorXs > &x1, Eigen::Ref< MatrixXs > Jfirst, Eigen::Ref< MatrixXs > Jsecond, const Jcomponent firstsecond=both) const =0
Compute the Jacobian of the state manifold differentiation.
VectorXs diff_dx(const Eigen::Ref< const VectorXs > &x0, const Eigen::Ref< const VectorXs > &x1)
std::size_t nx_
State dimension.
Definition: state-base.hpp:283
virtual void integrate(const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &dx, Eigen::Ref< VectorXs > xout) const =0
Compute the state manifold integration.
std::size_t nv_
Velocity dimension.
Definition: state-base.hpp:286
VectorXs integrate_x(const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &dx)
virtual void JintegrateTransport(const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &dx, Eigen::Ref< MatrixXs > Jin, const Jcomponent firstsecond) const =0
Parallel transport from x + dx to x.
std::size_t ndx_
State rate dimension.
Definition: state-base.hpp:284
std::size_t nq_
Configuration dimension.
Definition: state-base.hpp:285
const VectorXs & get_ub() const
Return the state upper bound.
virtual VectorXs rand() const =0
Generate a random reference state.
const std::size_t & get_nv() const
Return the dimension of tangent space of the configuration manifold.
const std::size_t & get_nq() const
Return the dimension of the configuration tuple.