The Rotation State

As mentioned in AbstractFunction, RobotDynamics can work with non-Euclidean state vectors, or state vectors whose composition rule is not simple addition. The most common example of non-Euclidean state vectors in robotics is that of 3D rotations. Frequently our state vectors include a 3D rotation together with normal Euclidean states such as position, linear or angular velocities, etc. The RotationStateStateVectorType represents this type of state vector. In general, this represents a state vector of the following form:

\[\begin{bmatrix} x_1 \\ q_1 \\ x_2 \\ q_2 \\ \vdots \\ x_{N-1} \\ q_N \\ x_N \end{bmatrix}\]

where $x_k \in \mathbb{R}^{n_k}$ and $q_k \in SO(3)$. Any of the $n_k$ can be zero.

This state is described by the LieState struct:

RobotDynamics.LieState — Type
LieState{R,P}

Specifies a state with rotational components mixed in with standard vector components. All rotational components are assumed to be parameterizations of 3D rotations.

Parameters

  • R <: Rotation is the rotational representation used in the state vector. Must have

params(::Type{R}) defined, which returns the number of parameters used by the rotation, as well a constructor that takes each parameter as a separate scalar argument.

  • P <: Tuple{Vararg{Int}} is a tuple of integers specifying the partitioning of the state

vector. Each element of P specifies the length of the vector component between the rotational components, and P[1] and P[end] specify the number of vector states at the beginning and end of the state vector.

Examples

If we want to construct a state vector like the following: [v3, q, v2, q, v3] where v2 and v3 and vector components of length 2 and 3, respectively, and q is a 4-dimensional unit quaternion. The LieState for this state vector would be LieState{UnitQuaternion{Float64},3,2,3}. The length should be (3+4+2+4+3) = 16, which can be verified by length(s::LieState).

Constructors

LieState(::Type{R}, P::Tuple{Vararg{Int}})
LieState(::Type{R}, p1::Int, p2::Int, p3::Int...)
RobotDynamics.QuatState — Function
QuatState(n::Int, Q::StaticVector{<:Any,Int})
QuatState(n::Int, Q::NTuple{<:Any,Int})

Create a n-dimensional LieState assuming R = UnitQuaternion{Float64} and Q[i] is the first index of each quaternion in the state vector.

Example

If we want to construct a state vector like the following: [v3, q, v2, q, v3] where v2 and v3 and vector components of length 2 and 3, respectively, and q is a 4-dimensional unit quaternion. Since the first quaternion starts at index 4, and the second starts at index 10, Q = [4,10]. The entire length of the vector is n = 16 = 3 + 4 + 2 + 4 + 3, so we would call QuatState(16, SA[4,10]).

The LieGroupModel type

To simplify the definition of models whose state vector is a RotationState, we provide the abstract LieGroupModel type:

RobotDynamics.LieGroupModel — Type
LieGroupModel <: ContinuousDynamics

Abstraction of a dynamical system whose state contains at least one arbitrary rotation.

Usage

To use this model, you only need to define the following functions:

  • control_dim(model::MyLieGroupModel)
  • LieState(::MyLieGroupModel)
  • Either dynamics! or dynamics

where LieState(model) should return a LieState for your model, describing how the state vector is composed of Euclidean and 3D rotations. Note that this function should only be a function of the type, not the actual struct itself (i.e. your method definition should look like the one above).

Single rigid bodies

A lot of robotic systems, such as airplanes, quadrotors, underwater vehicles, satellites, etc., can be described as a single rigid body subject to external forces and actuators. RobotDynamics provides the RigidBody model type for this type of system:

RobotDynamics.RigidBody — Type
RigidBody{R<:Rotation} <: LieGroupModel

Abstraction of a dynamical system with free-body dynamics, with a 12 or 13-dimensional state vector: [p; q; v; ω] where p is the 3D position, q is the 3 or 4-dimension attitude representation, v is the 3D linear velocity, and ω is the 3D angular velocity.

Interface

Any single-body system can leverage the RigidBody type by inheriting from it and defining the following interface:

forces(::MyRigidBody, x, u, [t])  # return the forces in the world frame
moments(::MyRigidBody, x, u, [t]) # return the moments in the body frame
inertia(::MyRigidBody, x, u) # return the 3x3 inertia matrix
mass(::MyRigidBody, x, u)  # return the mass as a real scalar

Instead of defining forces and moments you can define the higher-level wrenches function

wrenches(model::MyRigidbody, x, u, t)

Rotation Parameterization

A RigidBody model must specify the rotational representation being used. Any Rotations.Rotation{3} can be used, but we suggest one of the following:

  • UnitQuaternion
  • MRP
  • RodriguesParam

Working with state vectors for a RigidBody

Several methods are provided for working with the state vectors for a RigidBody. Also see the documentation for RBState which provides a unified representation for working with states for rigid bodies, which can be easily converted to and from the state vector representation for the given model.

Base.position — Method
position(x::RBState)
position(model::RigidBody, x::AbstractVector)

Return the 3-dimensional position of an rigid body as a SVector{3}.

RobotDynamics.orientation — Function
orientation(x::RBState)
orientation(model::RigidBody, x::AbstractVector)

Return the 3D orientation of a rigid body. Returns a Rotations.Rotation{3}.

RobotDynamics.linear_velocity — Function
linear_velocity(x::RBState)
linear_velocity(model::RigidBody, x::AbstractVector)

Return the 3D linear velocity of a rigid body as a SVector{3}.

RobotDynamics.angular_velocity — Function
angular_velocity(x::RBState)
angular_velocity(model::RigidBody, x::AbstractVector)

Return the 3D linear velocity of a rigid body as a SVector{3}.

RobotDynamics.build_state — Function
build_state(model::RigidBody{R}, x::RBState) where R
build_state(model::RigidBody{R}, x::AbstractVector) where R
build_state(model::RigidBody{R}, r, q, v, ω) where R

Build the state vector for model using the RBStatex. If R <: UnitQuaternion this returns x cast as an SVector, otherwise it will convert the quaternion in x to a rotation of type R.

Also accepts as arguments any arguments that can be passed to the constructor of RBState.

RobotDynamics.parse_state — Function
parse_state(model::RigidBody{R}, x, renorm=false)

Return the position, orientation, linear velocity, and angular velocity as separate vectors. The orientation will be of type R. If renorm=true and R <: UnitQuaternion the quaternion will be renormalized.

RobotDynamics.gen_inds — Function
gen_inds(model::RigidBody)

Generate a NamedTuple containing the indices of the position (r), orientation (q), linear velocity (v), and angular velocity (ω) from the state vector for model.

RobotDynamics.flipquat — Function
flipquat(model, x)

Flips the quaternion sign for a RigidBody{<:UnitQuaternion}.

The RBState type

When working with rigid bodies, the rotation can be represented a variety of methods and dealing with this ambiguity can be tedious. We provide the RBState type which represents a generic state for a rigid body, representing the orietation as a quaternion. It provides easy methods to convert to and from the state vector for a given RigidBody{R}.

RobotDynamics.RBState — Type
RBState{T} <: StaticVector{13,T}

Represents the state of a rigid body in 3D space, consisting of position, orientation, linear velocity and angular velocity, respresented as a vector stacked in that order, with the rotation represented as the 4 elements of a unit quaternion.

Implements the StaticArrays interface so can be treated as an SVector with additional methods.

Constructors

RBState{T}(r, q, v, ω)
RBState{T}(x)
RBState(r, q, v, ω)
RBState(x)

where r, v, and ω are three-dimensional vectors, q is either a Rotation or a four-dimenional vector representing the parameters of unit quaternion, and x is a 13-dimensional vector (or tuple),

Converting to a State Vector

An RBState can be converted to a state vector for a RigidBody using RBState(model::RBstate, x, [renorm=false])