dyn::State#

rb::dyn::State is the mutable state container passed into the rigid-body model computations. It stores joint vectors, intermediate kinematic quantities, and base-motion terms used by the dynamics layer.

Header

Header

#include <rby1-sdk/dynamics/state.h>

Declaration

Namespace

rb::dyn

Kind

class template

Primary role

Hold joint vectors, gravity, base twist, and intermediate dynamic state.

Public Attributes

Field group

Meaning

Notes

q, qdot, qddot, tau

Joint position, velocity, acceleration, and torque vectors.

The main dynamic inputs and outputs.

T, V, Vdot, S, E, F

Intermediate rigid-body quantities.

Mostly useful after model computations run.

V0 and Vdot0

Base motion terms.

Relevant for floating-base or moving-base setups.

Public Member Functions

Method group

Purpose

Notes

GetQ*(), GetTau(), GetV0(), GetVdot0()

Read the main state vectors.

Useful when state is owned by another subsystem.

SetQ*(), SetTau(), SetV0(), SetVdot0(), SetGravity(...)

Write the main state vectors and gravity.

Call before running model computations.

GetJointNames(), GetLinkNames(), GetBaseLinkIdx()

Inspect the model-index mapping bound into the state.

Useful for debugging and analysis.

Detailed Reference

template<int DOF>
class State#

Public Types

template<typename T, int N>
using ContainerType = typename std::conditional_t<(N > 0), std::array<T, (unsigned int)N>, std::vector<T>>#

Public Functions

inline unsigned int GetBaseLinkIdx() const#
inline ContainerType<std::string, DOF> GetJointNames() const#
inline std::vector<std::string> GetLinkNames() const#
inline Eigen::Vector<double, DOF> GetQ() const#
inline Eigen::Vector<double, DOF> GetQddot() const#
inline Eigen::Vector<double, DOF> GetQdot() const#
inline const Eigen::Vector<double, DOF> GetTau() const#
inline const math::se3v::MatrixType &GetV0() const#
inline const math::se3v::MatrixType &GetVdot0() const#
inline void SetGravity(const math::se3v::MatrixType &gravity)#
template<typename Derived>
inline void SetQ(const Eigen::MatrixBase<Derived> &new_q)#
template<typename Derived>
inline void SetQddot(const Eigen::MatrixBase<Derived> &new_qddot)#
template<typename Derived>
inline void SetQdot(const Eigen::MatrixBase<Derived> &new_qdot)#
template<typename Derived>
inline void SetTau(const Eigen::MatrixBase<Derived> &new_tau)#
inline void SetV0(const math::se3v::MatrixType &new_V0)#
inline void SetVdot0(const math::se3v::MatrixType &new_Vdot0)#

Public Members

ContainerType<math::SE3::MatrixType, DOF> E = {}#
Eigen::Matrix<double, 6, DOF> F#
Eigen::Vector<double, DOF> q#
Eigen::Vector<double, DOF> qddot#
Eigen::Vector<double, DOF> qdot#
Eigen::Matrix<double, 6, DOF> S#
ContainerType<math::SE3::MatrixType, DOF> T = {}#
Eigen::Vector<double, DOF> tau#
Eigen::Matrix<double, 6, DOF> V#
math::se3v::MatrixType V0 = {math::se3v::MatrixType::Zero()}#
Eigen::Matrix<double, 6, DOF> Vdot#
math::se3v::MatrixType Vdot0 = {math::se3v::MatrixType::Zero()}#

Friends

friend class Robot

Related Types

Examples

  • dynamics/load_urdf.cpp

  • dynamics/gravity_term.cpp