Crocoddyl
state-base.hpp
1 // BSD 3-Clause License
3 //
4 // Copyright (C) 2019-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 <stdexcept>
13 #include <string>
14 #include <vector>
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 
24 inline bool is_a_Jcomponent(Jcomponent firstsecond) {
25  return (firstsecond == first || firstsecond == second || firstsecond == both);
26 }
27 
45 template <typename _Scalar>
47  public:
48  EIGEN_MAKE_ALIGNED_OPERATOR_NEW
49 
50  typedef _Scalar Scalar;
52  typedef typename MathBase::VectorXs VectorXs;
53  typedef typename MathBase::MatrixXs MatrixXs;
54 
61  StateAbstractTpl(const std::size_t nx, const std::size_t ndx);
63  virtual ~StateAbstractTpl();
64 
68  virtual VectorXs zero() const = 0;
69 
73  virtual VectorXs rand() const = 0;
74 
92  virtual void diff(const Eigen::Ref<const VectorXs>& x0,
93  const Eigen::Ref<const VectorXs>& x1,
94  Eigen::Ref<VectorXs> dxout) const = 0;
95 
112  virtual void integrate(const Eigen::Ref<const VectorXs>& x,
113  const Eigen::Ref<const VectorXs>& dx,
114  Eigen::Ref<VectorXs> xout) const = 0;
115 
157  virtual void Jdiff(const Eigen::Ref<const VectorXs>& x0,
158  const Eigen::Ref<const VectorXs>& x1,
159  Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
160  const Jcomponent firstsecond = both) const = 0;
161 
201  virtual void Jintegrate(const Eigen::Ref<const VectorXs>& x,
202  const Eigen::Ref<const VectorXs>& dx,
203  Eigen::Ref<MatrixXs> Jfirst,
204  Eigen::Ref<MatrixXs> Jsecond,
205  const Jcomponent firstsecond = both,
206  const AssignmentOp op = setto) const = 0;
207 
222  virtual void JintegrateTransport(const Eigen::Ref<const VectorXs>& x,
223  const Eigen::Ref<const VectorXs>& dx,
224  Eigen::Ref<MatrixXs> Jin,
225  const Jcomponent firstsecond) const = 0;
226 
235  VectorXs diff_dx(const Eigen::Ref<const VectorXs>& x0,
236  const Eigen::Ref<const VectorXs>& x1);
237 
245  VectorXs integrate_x(const Eigen::Ref<const VectorXs>& x,
246  const Eigen::Ref<const VectorXs>& dx);
247 
255  std::vector<MatrixXs> Jdiff_Js(const Eigen::Ref<const VectorXs>& x0,
256  const Eigen::Ref<const VectorXs>& x1,
257  const Jcomponent firstsecond = both);
258 
266  std::vector<MatrixXs> Jintegrate_Js(const Eigen::Ref<const VectorXs>& x,
267  const Eigen::Ref<const VectorXs>& dx,
268  const Jcomponent firstsecond = both);
269 
273  std::size_t get_nx() const;
274 
278  std::size_t get_ndx() const;
279 
283  std::size_t get_nq() const;
284 
288  std::size_t get_nv() const;
289 
293  const VectorXs& get_lb() const;
294 
298  const VectorXs& get_ub() const;
299 
303  bool get_has_limits() const;
304 
308  void set_lb(const VectorXs& lb);
309 
313  void set_ub(const VectorXs& ub);
314 
315  protected:
316  void update_has_limits();
317 
318  std::size_t nx_;
319  std::size_t ndx_;
320  std::size_t nq_;
321  std::size_t nv_;
322  VectorXs lb_;
323  VectorXs ub_;
324  bool has_limits_;
325 };
326 
327 } // namespace crocoddyl
328 
329 /* --- Details -------------------------------------------------------------- */
330 /* --- Details -------------------------------------------------------------- */
331 /* --- Details -------------------------------------------------------------- */
332 #include "crocoddyl/core/state-base.hxx"
333 
334 #endif // CROCODDYL_CORE_STATE_BASE_HPP_
Abstract class for the state representation.
Definition: state-base.hpp:46
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.
std::vector< MatrixXs > Jdiff_Js(const Eigen::Ref< const VectorXs > &x0, const Eigen::Ref< const VectorXs > &x1, const Jcomponent firstsecond=both)
std::vector< MatrixXs > Jintegrate_Js(const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &dx, const Jcomponent firstsecond=both)
Compute the Jacobian of the state manifold integration.
VectorXs diff_dx(const Eigen::Ref< const VectorXs > &x0, const Eigen::Ref< const VectorXs > &x1)
Compute the state manifold differentiation.
virtual VectorXs rand() const =0
Generate a random state.
std::size_t nv_
Velocity dimension.
Definition: state-base.hpp:321
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 integrate(x, dx) to x.
std::size_t get_nq() const
Return the dimension of the configuration tuple.
std::size_t get_ndx() const
Return the dimension of the tangent space of the state manifold.
std::size_t get_nv() const
Return the dimension of tangent space of the configuration manifold.
const VectorXs & get_lb() const
Return the state lower bound.
std::size_t nx_
State dimension.
Definition: state-base.hpp:318
VectorXs integrate_x(const Eigen::Ref< const VectorXs > &x, const Eigen::Ref< const VectorXs > &dx)
Compute the state manifold integration.
const VectorXs & get_ub() const
Return the state upper bound.
bool has_limits_
Indicates whether any of the state limits is finite.
Definition: state-base.hpp:324
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.
virtual VectorXs zero() const =0
Generate a zero state.
StateAbstractTpl(const std::size_t nx, const std::size_t ndx)
Initialize the state dimensions.
std::size_t nq_
Configuration dimension.
Definition: state-base.hpp:320
bool get_has_limits() const
Indicate if the state has defined limits.
VectorXs lb_
Lower state limits.
Definition: state-base.hpp:322
void set_ub(const VectorXs &ub)
Modify the state upper bound.
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.
VectorXs ub_
Upper state limits.
Definition: state-base.hpp:323
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.
std::size_t get_nx() const
Return the dimension of the state tuple.
void set_lb(const VectorXs &lb)
Modify the state lower bound.
std::size_t ndx_
State rate dimension.
Definition: state-base.hpp:319