9 #ifndef CROCODDYL_CORE_STATE_BASE_HPP_ 10 #define CROCODDYL_CORE_STATE_BASE_HPP_ 16 #include "crocoddyl/core/fwd.hpp" 17 #include "crocoddyl/core/mathbase.hpp" 18 #include "crocoddyl/core/utils/exception.hpp" 22 enum Jcomponent { both = 0, first = 1, second = 2 };
23 enum AssignmentOp { setto, addto, rmfrom };
25 inline bool is_a_Jcomponent(Jcomponent firstsecond) {
26 return (firstsecond == first || firstsecond == second || firstsecond == both);
29 inline bool is_a_AssignmentOp(AssignmentOp op) {
return (op == setto || op == addto || op == rmfrom); }
44 template <
typename _Scalar>
45 class StateAbstractTpl {
47 EIGEN_MAKE_ALIGNED_OPERATOR_NEW
49 typedef _Scalar Scalar;
50 typedef MathBaseTpl<Scalar> MathBase;
51 typedef typename MathBase::VectorXs VectorXs;
52 typedef typename MathBase::MatrixXs MatrixXs;
67 virtual VectorXs
zero()
const = 0;
72 virtual VectorXs
rand()
const = 0;
89 virtual void diff(
const Eigen::Ref<const VectorXs>& x0,
const Eigen::Ref<const VectorXs>& x1,
90 Eigen::Ref<VectorXs> dxout)
const = 0;
107 virtual void integrate(
const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& dx,
108 Eigen::Ref<VectorXs> xout)
const = 0;
144 virtual void Jdiff(
const Eigen::Ref<const VectorXs>& x0,
const Eigen::Ref<const VectorXs>& x1,
145 Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
146 const Jcomponent firstsecond = both)
const = 0;
180 virtual void Jintegrate(
const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& dx,
181 Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond,
182 const Jcomponent firstsecond = both,
const AssignmentOp op = setto)
const = 0;
196 virtual void JintegrateTransport(
const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& dx,
197 Eigen::Ref<MatrixXs> Jin,
const Jcomponent firstsecond)
const = 0;
206 VectorXs
diff_dx(
const Eigen::Ref<const VectorXs>& x0,
const Eigen::Ref<const VectorXs>& x1);
215 VectorXs
integrate_x(
const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& dx);
224 std::vector<MatrixXs>
Jdiff_Js(
const Eigen::Ref<const VectorXs>& x0,
const Eigen::Ref<const VectorXs>& x1,
225 const Jcomponent firstsecond = both);
234 std::vector<MatrixXs>
Jintegrate_Js(
const Eigen::Ref<const VectorXs>& x,
const Eigen::Ref<const VectorXs>& dx,
235 const Jcomponent firstsecond = both);
240 std::size_t
get_nx()
const;
250 std::size_t
get_nq()
const;
255 std::size_t
get_nv()
const;
260 const VectorXs&
get_lb()
const;
265 const VectorXs&
get_ub()
const;
275 void set_lb(
const VectorXs& lb);
280 void set_ub(
const VectorXs& ub);
283 void update_has_limits();
299 #include "crocoddyl/core/state-base.hxx" 301 #endif // CROCODDYL_CORE_STATE_BASE_HPP_ 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.
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.
std::size_t get_ndx() const
Return the dimension of the tangent space of the state manifold.
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 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)
std::size_t get_nq() const
Return the dimension of the configuration tuple.
StateAbstractTpl(const std::size_t nx, const std::size_t ndx)
Initialize the state dimensions.
bool has_limits_
Indicates whether any of the state limits is finite.
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 get_nx() const
Return the dimension of the state tuple.
std::size_t nx_
State dimension.
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.
std::size_t get_nv() const
Return the dimension of tangent space of the configuration manifold.
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.
std::size_t nq_
Configuration dimension.
const VectorXs & get_ub() const
Return the state upper bound.
bool get_has_limits() const
Indicate if the state has defined limits.
virtual VectorXs rand() const =0
Generate a random state.