Template Class StateVectorTpl

Inheritance Relationships

Base Type

Class Documentation

template<typename _Scalar>
class StateVectorTpl : public crocoddyl::StateAbstractTpl<_Scalar>

Public Types

typedef MathBaseTpl<Scalar> MathBase
typedef MathBase::VectorXs VectorXs
typedef MathBase::MatrixXs MatrixXs

Public Functions

explicit StateVectorTpl(const std::size_t nx)
virtual ~StateVectorTpl()
virtual VectorXs zero() const override

Generate a zero state.

virtual VectorXs rand() const override

Generate a random state.

virtual void diff(const Eigen::Ref<const VectorXs> &x0, const Eigen::Ref<const VectorXs> &x1, Eigen::Ref<VectorXs> dxout) const override

Compute the state manifold differentiation.

The state differentiation is defined as:

\[\begin{equation*} \delta\mathbf{x} = \mathbf{x}_{1} \ominus \mathbf{x}_{0}, \end{equation*}\]
where \(\mathbf{x}_{1}\), \(\mathbf{x}_{0}\) are the current and previous state which lie in a manifold \(\mathcal{M}\), and \(\delta\mathbf{x} \in T_\mathbf{x} \mathcal{M}\) is the rate of change in the state in the tangent space of the manifold.

Parameters:

  • x0 – [in] Previous state point (size nx)

  • x1 – [in] Current state point (size nx)

  • dxout – [out] Difference between the current and previous state points (size ndx)

virtual void integrate(const Eigen::Ref<const VectorXs> &x, const Eigen::Ref<const VectorXs> &dx, Eigen::Ref<VectorXs> xout) const override

Compute the state manifold integration.

The state integration is defined as:

\[\begin{equation*} \mathbf{x}_{next} = \mathbf{x} \oplus \delta\mathbf{x}, \end{equation*}\]
where \(\mathbf{x}\), \(\mathbf{x}_{next}\) are the current and next state which lie in a manifold \(\mathcal{M}\), and \(\delta\mathbf{x} \in T_\mathbf{x} \mathcal{M}\) is the rate of change in the state in the tangent space of the manifold.

Parameters:

  • x – [in] State point (size nx)

  • dx – [in] Velocity vector (size ndx)

  • xout – [out] Next state point (size nx)

virtual void Jdiff(const Eigen::Ref<const VectorXs>&, const Eigen::Ref<const VectorXs>&, Eigen::Ref<MatrixXs> Jfirst, Eigen::Ref<MatrixXs> Jsecond, const Jcomponent firstsecond = both) const override

Compute the Jacobian of the state manifold differentiation.

The state differentiation is defined as:

\[\begin{equation*} \delta\mathbf{x} = \mathbf{x}_{1} \ominus \mathbf{x}_{0}, \end{equation*}\]
where \(\mathbf{x}_{1}\), \(\mathbf{x}_{0}\) are the current and previous state which lie in a manifold \(\mathcal{M}\), and \(\delta\mathbf{x} \in T_\mathbf{x} \mathcal{M}\) is the rate of change in the state in the tangent space of the manifold.

The Jacobians lie in the tangent space of manifold, i.e. \(\mathbb{R}^{\textrm{ndx}\times\textrm{ndx}}\). Note that the state is represented as a tuple of nx values and its dimension is ndx. Calling \(\boldsymbol{\Delta}(\mathbf{x}_{0}, \mathbf{x}_{1}) \), the difference function, these Jacobians satisfy the following relationships:

  • \(\boldsymbol{\Delta}(\mathbf{x}_{0},\mathbf{x}_{0}\oplus\delta\mathbf{y}) - \boldsymbol{\Delta}(\mathbf{x}_{0},\mathbf{x}_{1}) = \mathbf{J}_{\mathbf{x}_{1}}\delta\mathbf{y} + \mathbf{o}(\mathbf{x}_{0})\).

  • \(\boldsymbol{\Delta}(\mathbf{x}_{0}\oplus\delta\mathbf{y},\mathbf{x}_{1}) - \boldsymbol{\Delta}(\mathbf{x}_{0},\mathbf{x}_{1}) = \mathbf{J}_{\mathbf{x}_{0}}\delta\mathbf{y} + \mathbf{o}(\mathbf{x}_{0})\),

where \(\mathbf{J}_{\mathbf{x}_{1}}\) and \(\mathbf{J}_{\mathbf{x}_{0}}\) are the Jacobian with respect to the current and previous state, respectively.

Parameters:

  • x0 – [in] Previous state point (size nx)

  • x1 – [in] Current state point (size nx)

  • Jfirst – [out] Jacobian of the difference operation relative to the previous state point (size ndx \(\times\)ndx)

  • Jsecond – [out] Jacobian of the difference operation relative to the current state point (size ndx \(\times\)ndx)

  • firstsecond – [in] Argument (either x0 and / or x1) with respect to which the differentiation is performed.

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 = setto) const override

Compute the Jacobian of the state manifold integration.

The state integration is defined as:

\[\begin{equation*} \mathbf{x}_{next} = \mathbf{x} \oplus \delta\mathbf{x}, \end{equation*}\]
where \(\mathbf{x}\), \(\mathbf{x}_{next}\) are the current and next state which lie in a manifold \(\mathcal{M}\), and \(\delta\mathbf{x} \in T_\mathbf{x} \mathcal{M}\) is the rate of change in the state in the tangent space of the manifold.

The Jacobians lie in the tangent space of manifold, i.e. \(\mathbb{R}^{\textrm{ndx}\times\textrm{ndx}}\). Note that the state is represented as a tuple of nx values and its dimension is ndx. Calling \( \mathbf{f}(\mathbf{x}, \delta\mathbf{x}) \), the integrate function, these Jacobians satisfy the following relationships:

  • \(\mathbf{f}(\mathbf{x}\oplus\delta\mathbf{y},\delta\mathbf{x})\ominus\mathbf{f}(\mathbf{x},\delta\mathbf{x}) = \mathbf{J}_\mathbf{x}\delta\mathbf{y} + \mathbf{o}(\delta\mathbf{x})\).

  • \(\mathbf{f}(\mathbf{x},\delta\mathbf{x}+\delta\mathbf{y})\ominus\mathbf{f}(\mathbf{x},\delta\mathbf{x}) = \mathbf{J}_{\delta\mathbf{x}}\delta\mathbf{y} + \mathbf{o}(\delta\mathbf{x})\),

where \(\mathbf{J}_{\delta\mathbf{x}}\) and \(\mathbf{J}_{\mathbf{x}}\) are the Jacobian with respect to the state and velocity, respectively.

Parameters:

  • x – [in] State point (size nx)

  • dx – [in] Velocity vector (size ndx)

  • Jfirst – [out] Jacobian of the integration operation relative to the state point (size ndx \(\times\)ndx)

  • Jsecond – [out] Jacobian of the integration operation relative to the velocity vector (size ndx \(\times\)ndx)

  • firstsecond – [in] Argument (either x and / or dx) with respect to which the differentiation is performed

  • op – [in] Assignment operator which sets, adds, or removes the given Jacobian matrix

virtual void JintegrateTransport(const Eigen::Ref<const VectorXs> &x, const Eigen::Ref<const VectorXs> &dx, Eigen::Ref<MatrixXs> Jin, const Jcomponent firstsecond) const override

Parallel transport from integrate(x, dx) to x.

This function performs the parallel transportation of an input matrix whose columns are expressed in the tangent space at \(\mathbf{x}\oplus\delta\mathbf{x}\) to the tangent space at \(\mathbf{x}\) point.

Parameters:

  • x – [in] State point (size nx).

  • dx – [in] Velocity vector (size ndx)

  • Jin – [out] Input matrix (number of rows = nv).

  • firstsecond – [in] Argument (either x or dx) with respect to which the differentiation of Jintegrate is performed.

template<typename NewScalar>
StateVectorTpl<NewScalar> cast() const
virtual void print(std::ostream &os) const override

Print relevant information of the state vector.

Parameters:

os – [out] Output stream object

Public Members

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar