Template Class StateMultibodyTpl

Inheritance Relationships

Base Type

Class Documentation

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

State multibody representation.

A multibody state is described by the configuration point and its tangential velocity, or in other words, by the generalized position and velocity coordinates of a rigid-body system. For this state, we describe its operators: difference, integrates, transport and their derivatives for any Pinocchio model.

For more details about these operators, please read the documentation of the StateAbstractTpl class.

See also

diff(), integrate(), Jdiff(), Jintegrate() and JintegrateTransport()

Public Types

typedef MathBaseTpl<Scalar> MathBase
typedef StateAbstractTpl<Scalar> Base
typedef pinocchio::ModelTpl<Scalar> PinocchioModel
typedef MathBase::VectorXs VectorXs
typedef MathBase::MatrixXs MatrixXs

Public Functions

explicit StateMultibodyTpl(std::shared_ptr<PinocchioModel> model)

Initialize the multibody state.

Parameters:

model – [in] Pinocchio model

StateMultibodyTpl()
virtual ~StateMultibodyTpl()
virtual VectorXs zero() const override

Generate a zero state.

Note that the zero configuration is computed using pinocchio::neutral.

virtual VectorXs rand() const override

Generate a random state.

Note that the random configuration is computed using pinocchio::random which satisfies the manifold definition (e.g., the quaterion definition)

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 safe_diff(const Eigen::Ref<const VectorXs> &x0, const Eigen::Ref<const VectorXs> &x1, Eigen::Ref<VectorXs> dxout) const override

Sanitized diff operator that neutralises invalid components and propagates NaNs where the result cannot be trusted”.

Parameters:

  • x0 – Current state (size nx).

  • x1 – Next state (size nx)

  • dxout – x1 - x0 value with NaNs on invalid entries (size ndx)

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

Sanitized integrate operator that clamps invalid inputs and marks affected configuration or velocity entries with NaNs.

Parameters:

  • x – Current state (size nx).

  • dx – Displacement of the state (size ndx).

  • xout – x + dx value with NaNs on invalid entries (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.

const std::shared_ptr<PinocchioModel> &get_pinocchio() const

Return the Pinocchio model (i.e., model of the rigid body system)

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

Print relevant information of the state multibody.

Parameters:

os – [out] Output stream object

Public Members

EIGEN_MAKE_ALIGNED_OPERATOR_NEW typedef _Scalar Scalar

Protected Attributes

bool has_limits_

Indicates whether any of the state limits is finite.

VectorXs lb_

Lower state limits.

std::size_t ndx_

State rate dimension.

std::size_t nq_

Configuration dimension.

std::size_t nv_

Velocity dimension.

std::size_t nx_

State dimension.

VectorXs ub_

Upper state limits.