Unified Notation

Author:: Onur Tuncer, PhD
Date:: May 13^th, 2026

Goal 

A single consistent notation that:

uses spatial algebra (Featherstone notation) for rigid-body dynamics,
treats configuration variables as living on a (product) manifold,
supports Lie-group-aware integration (RKMK) without ad-hoc constraint fixes (e.g. re-orthonormalization).

Unification is achieved by interpreting the Featherstone 6D spatial vectors as coordinates of the Lie algebra \(\Lie{se}(3)\) (and its dual \(\Lie{se}(3)^{\*}\)), and poses as elements of \(\SE(3)\).

State as a product manifold 

For a single rigid body, choose the configuration

\[\begin{split}g \in \SE(3), \qquad g = \begin{bmatrix} R & p\\ 0 & 1 \end{bmatrix}_{4x4}, \quad R\in \SO(3),\ p\in\R^3.\end{split}\]

A typical flight / GNC state includes additional Euclidean components (velocities, biases, masses, aerodynamic states, etc.):

\[x = (g,\ y) \in \SE(3) \times \R^m.\]

For multi-body systems (or multiple frames), configuration becomes a product manifold:

\[x \in \underbrace{\SE(3)\times\cdots\times\SE(3)}_{N\ \text{bodies / frames}} \times \R^m.\]

This is a product manifold viewpoint: Lie-group factors for configuration, Euclidean factors for everything else.

Spatial motion vectors as coordinates of \(\Lie{se}(3)\)

A Featherstone spatial motion vector (twist) is

\[\begin{split}\twist = \begin{bmatrix} \omega\\ v \end{bmatrix}\in\R^6, \quad \omega\in\R^3,\ v\in\R^3.\end{split}\]

We identify this with an element of the Lie algebra \(\Lie{se}(3)\) via the hat map:

\[\begin{split}(\cdot)^\wedge:\R^6\to \Lie{se}(3),\qquad \twist^\wedge \;=\; \begin{bmatrix} [\omega]_\times & v\\ 0 & 0 \end{bmatrix}_{4x4}.\end{split}\]

In contrast, \((\cdot)^\vee\) maps \(\Lie{se}(3)\) back to \(\R^6\).

Key unification point:

Spatial motion vectors are simply coordinates for \(\Lie{se}(3)\). Spatial forces (wrenches) are the coordinates for the dual space \(\Lie{se}(3)^{\*}\):

\[\begin{split}\wrench = \begin{bmatrix} n\\ f \end{bmatrix}\in\R^6 \quad\leftrightarrow\quad \Lie{se}(3)^{\*}.\end{split}\]

Kinematics on \(\SE(3)\) using spatial velocity 

Write kinematics in left-trivialized (body) form:

\[\dot g = g\,\twist^\wedge.\]

This equation is the clean bridge between Lie theory and Featherstone notation:

\(g\in\SE(3)\) is the pose (position + orientation),
\(\twist\in\R^6\) is the spatial twist (body spatial velocity),
\(\twist^\wedge\in\Lie{se}(3)\) is the corresponding Lie algebra element.

Featherstone cross operators as \(\ad\) and \(\ad^{\*}\)

Featherstone defines bi-linear operators:

motion cross product: \(\twist \times (\cdot)\) mapping \(\R^6\to \R^6\),
force cross product: \(\twist \times^{\*} (\cdot)\) mapping \(\R^6\to \R^6\).

In Lie notation these are precisely the adjoint actions:

\[\ad_{\twist}(\cdot)\ \equiv\ \twist \times (\cdot), \qquad \ad_{\twist}^{\*}(\cdot)\ \equiv\ \twist \times^{\*}(\cdot).\]

Therefore “spatial algebra dynamics” can be re-written in a Lie-algebraic form without changing the underlying computations.

Rigid-body dynamics in unified form 

Let \(I\in\R^{6\times 6}\) be the spatial inertia, \(\twist\) the body twist, and \(\wrench\) the net wrench. The standard Featherstone equation

\[I\,\dot{\twist} + \twist\times^{\*}(I\twist) = \wrench\]

is equivalently

\[I\,\dot{\twist} + \ad_{\twist}^{\*}\!\big(I\twist\big) = \wrench.\]

This is the central “unified” dynamics statement:

pose lives on \(\SE(3)\),
velocities live in \(\Lie{se}(3)\cong\R^6\),
forces live in \(\Lie{se}(3)^{\*}\cong\R^6\),
coupling terms use \(\ad^{\*}\), matching Featherstone’s cross operator.

ODE on the product manifold \(\SE(3)\times\R^m\)

A general coupled system can be written as

\[\dot g = g\,\Omega(g,y)^\wedge,\qquad \dot y = f(g,y),\]

where \(\Omega(g,y)\in\R^6\) is a spatial twist (often produced by vehicle dynamics + constraints + controls) and \(f(g,y)\in\R^m\) is the Euclidean part (biases, masses, actuator states, aero coefficients, etc.).

RKMK integration in the same notation 

Runge–Kutta–Munthe-Kaas (RKMK) integrates the Lie-group factor by evolving an algebra increment

\[\eta \in \Lie{se}(3)\quad\text{(or its coordinate vector in }\R^6\text{)}\]

and updating the pose using the group exponential:

\[g_{n+1} = g_n\,\Exp(\eta_{n+1}).\]

The Euclidean part advances by standard Runge–Kutta addition:

\[y_{n+1} = y_n + \Delta y.\]

Stage reconstruction on \(\SE(3)\times\R^m\)

Let \((a_{ij},b_i,c_i)\) be a Runge–Kutta tableau with \(s\) stages and step size \(h\). Maintain stage variables

\[\eta_i \in \Lie{se}(3),\qquad \rho_i\in\R^m,\]

and define stage states

\[g_i = g_n\,\Exp(\eta_i),\qquad y_i = y_n + \rho_i.\]

Stage rates and \(\dexp^{-1}\)

RKMK evaluates the vector field in the algebra (a vector space) via the inverse differential of the exponential:

\[K_i = \dexp_{\eta_i}^{-1}\!\big(\Omega(g_i,y_i)\big)\in \Lie{se}(3), \qquad k_i = f(g_i,y_i)\in\R^m.\]

(If one stores algebra elements as 6-vectors, \(K_i\) and \(\Omega\) can be interpreted as \(\R^6\) coordinates.)

Explicit stage accumulation 

For an explicit Runge–Kutta tableau:

\[\eta_i = h\sum_{j=1}^{i-1} a_{ij}K_j,\qquad \rho_i = h\sum_{j=1}^{i-1} a_{ij}k_j.\]

Then the final update is

\[\eta_{n+1} = h\sum_{i=1}^{s} b_i K_i,\qquad y_{n+1} = y_n + h\sum_{i=1}^{s} b_i k_i,\qquad g_{n+1} = g_n\,\Exp(\eta_{n+1}).\]

Implicit R-K methods, on the other hand, require the solution of a coupled system of equations.

Practical Ramifications 

This unified notation means:

One can keep poses on \(\SE(3)\) by construction (no re-orthonormalization hacks).
One keeps spatial dynamics in Featherstone form (fast, standard, and physically interpretable).
One can swap integrators (RK4, Radau IIA, Gauss) while preserving the manifold structure.
One’s EKF propagation naturally uses tangent-space coordinates (algebra vectors) consistent with the manifold.

Note: \(\R^m\) is also a Lie group 

The additive group \((\R^m,+)\) is a Lie group with trivial exponential and \(\dexp\). So the whole state space \(\SE(3)\times\R^m\) can be viewed as a direct-product Lie group. In implementation, \(y\) is still kept in plain vector form because its Lie machinery is trivial.

Appendix 

Hat/Vee Operators: From a Twist in \(\mathbb{R}^6\) to \(\Lie{se}(3)\)

This appendix explains the “hat” operator \((\cdot)^\wedge\) used throughout the document to map a 6D spatial motion vector (twist) into the Lie algebra \(\Lie{se}(3)\), and its inverse “vee” operator \((\cdot)^\vee\) mapping back to coordinates in \(\R^6\).

The hat map on \(\R^3\): \(\omega \mapsto [\omega]_\times\)

For \(\omega = (\omega_x,\omega_y,\omega_z)^\top \in \R^3\), define the skew-symmetric matrix

\[\begin{split}[\omega]_\times \;=\; \begin{bmatrix} 0 & -\omega_z & \omega_y\\ \omega_z & 0 & -\omega_x\\ -\omega_y & \omega_x & 0 \end{bmatrix} \in \Lie{so}(3).\end{split}\]

It is characterized by the identity

\[[\omega]_\times\,a \;=\; \omega \times a, \qquad \forall\, a\in\R^3,\]

so “multiplication by \([\omega]_\times\)” is exactly the linear map “cross with \(\omega\).”

The hat map on \(\R^6\): \((\omega,v)\mapsto \Lie{se}(3)\)

A twist (spatial motion vector) is represented in coordinates by

\[\begin{split}\twist = \begin{bmatrix}\omega\\ v\end{bmatrix}\in\R^6, \qquad \omega,v\in\R^3.\end{split}\]

The hat map \((\cdot)^\wedge:\R^6\to \Lie{se}(3)\) packs these coordinates into the standard matrix representation of \(\Lie{se}(3)\):

\[\begin{split}\twist^\wedge \;=\; \begin{bmatrix} [\omega]_\times & v\\ 0 & 0 \end{bmatrix}_{4x4} \in \Lie{se}(3).\end{split}\]

Thus, \(\omega\) occupies the rotational (skew) block, and \(v\) occupies the translational column.

Why this mapping is natural: recovering the kinematics

Let the pose be

\[\begin{split}g = \begin{bmatrix} R & p\\ 0 & 1 \end{bmatrix} \in \SE(3), \qquad R\in\SO(3),\ p\in\R^3.\end{split}\]

Using left-trivialized (body) kinematics,

\[\dot g = g\,\twist^\wedge,\]

and substituting \(\twist^\wedge\) gives

\[\begin{split}\dot g = \begin{bmatrix} R & p\\ 0 & 1 \end{bmatrix} \begin{bmatrix} [\omega]_\times & v\\ 0 & 0 \end{bmatrix} = \begin{bmatrix} R[\omega]_\times & Rv\\ 0 & 0 \end{bmatrix}.\end{split}\]

Equating blocks yields the familiar rigid-body kinematics

\[\dot R = R[\omega]_\times, \qquad \dot p = Rv.\]

So in the left-trivialized convention, \(\omega\) and \(v\) are expressed in the body frame, and are mapped into the inertial/world frame by left-multiplication with \(R\).

The vee map: extracting coordinates back to \(\R^6\)

The inverse map \((\cdot)^\vee:\Lie{se}(3)\to\R^6\) simply unpacks the matrix into its coordinate vector:

\[\begin{split}\left( \begin{bmatrix} [\omega]_\times & v\\ 0 & 0 \end{bmatrix} \right)^\vee = \begin{bmatrix}\omega\\ v\end{bmatrix}.\end{split}\]

(Here \(\omega\) is recovered from the skew block via the standard identification between \(\Lie{so}(3)\) and \(\R^3\).)

Remark on conventions (body vs. spatial)

Equation \(\dot g = g\,\twist^\wedge\) corresponds to a body twist (left-trivialized velocity). If instead one uses a spatial twist (right-trivialized velocity) expressed in world coordinates rather than body coordinates, the kinematics take the form

\[\dot g = \twist^\wedge g,\]

and the coordinate interpretation of \((\omega,v)\) changes accordingly. If the world frame is an inertial frame then it is an “inertial twist”. The document adopts the left-trivialized convention to match the common “body” form used in many Lie-group integrators and in Featherstone-style rigid-body dynamics.