Kinematics studies the motion of rigid body systems, which is a fundamental building block of robotics. Expressing kinematic quantities in the Plucker coordinates results in compact notations that include both rotation and translation in a unified fashion that is suitable for matrix operations. To facilitate the mathematical operations, spatial kinematics vector algebra introduced some spatial transformations that are discussed less often in classic robotics, physics, and dynamics textbooks. Nevertheless, they are nothing more than vector physics written in matrix format.
This post summarizes some transformations used in spatial kinematics. For each of them, this post describes their definition, context, physical interpretation, typical applications, alternative notations, and some algebraic properties for computer implementation. For more details, please refer to 2017@Lynch , 2007@Featherstone , and 2007@Siciliano .
In this post and most posts on this website, if not otherwise stated, we employ and denote
as defined and described in Ch. 3 of 2017@Lynch .
Given a vector $$a = \begin{bmatrix} a_1 & a_2 & a_3 \end{bmatrix}^T \in \mathbb{R}^3$$ a matrix in the form of $$ [a] \triangleq \begin{bmatrix} 0 & -a_3 & a_2 \\ a_3 & 0 & -a_1 \\ -a_2 & a_1 & 0 \end{bmatrix} \in so(3) \in \mathbb{R}^{3\times3} $$ is the skew-symmetric matrix representation of $a$. In addition to $[a]$, it's also common denoted as $[a]_{\times}$, $a\times$, or $S(a)$. Note that $[a] = -[a]^T$.
The set of all such skew-symmetric matrices is called $so(3)$, also known as the Lie algebra of the Lie group $SO(3)$. The mathematical definition and concept of an algebra and the Lie algebra will be discussed in a separate post.
Such a representation can be employed to facilitate rigid body motion computation by converting vector algebra to matrix algebra. Some typically usage are as follows.
Equation $\dot{R} = [\omega_s] R$ can be physically interpreted as follows:
For vectors $a, b \in \mathbb{R}^3$ $$ a \times b = [a]b = -[b]a = -b \times a \label{eq:ss-order} $$
For $\omega \in \mathbb{R}^3$ and $R\in SO(3)$ $$ R[\omega]R^T = [R\omega] \label{eq:ss-rot} $$
Given a twist $$\mathcal{V} = \begin{bmatrix} \omega \\ v \end{bmatrix} \in \mathsf{M}^6 \in \mathbb{R}^6$$ the matrix in the form of $$ [\mathcal{V}] \triangleq \begin{bmatrix} [\omega] & v \\ 0 & 0 \end{bmatrix} \in se(3) \in \mathbb{R}^{4\times4} $$ is the $se(3)$ matrix representation of twist $\mathcal{V}$, analogous to that of angular velocity and its $so(3)$ representation, $[\omega]$ is the $so(3)$ representation of $\omega$.
The set of all such matrices is called $se(3)$, also known as the Lie algebra of the Lie group $SE(3)$.
Similar to the $so(3)$ algebra, the $se(3)$ algebra can also facilitate spatial rigid body motion in matrix form. Some typically usage are as follows.
To facilitate physical interpretation, the following discussion is based on the context below. For a visual illustration and further explanation of the following context, please refer to Fig. 3.17 in 2017@Lynch .
Given a twist $\mathcal{V}_{b,\{B\}}$ of point $b$ on the rigid body expressed in frame $\{B\}$, if one wants to express the twist of the same point in frame $\{S\}$, one just need to rotate both its angular and linear components $$ \mathcal{V}_{b,\{S\}} = \begin{bmatrix} R \, \omega_{b,\{B\}} \\ R \, v_{b, \{B\}} \end{bmatrix} = \begin{bmatrix} R & 0 \\ 0 & R \end{bmatrix} \begin{bmatrix} \omega_{b,\{B\}} \\ v_{b, \{B\}} \end{bmatrix} = \begin{bmatrix} R & 0 \\ 0 & R \end{bmatrix} \mathcal{V}_{b,\{B\}} = \text{rot}(R) \mathcal{V}_{b,\{B\}} $$ And for a wrench of the same operation, it's the same, $$ \mathcal{F}_{b,\{S\}} = \text{rot}(R) \mathcal{F}_{b,\{B\}} $$ where $\text{rot}(R)$ denotes the spatial rotation.
Given a twist $\mathcal{V}_{b,\{B\}}$ of point $b$ on the rigid body expressed in frame $\{B\}$, if one wants to find the twist at point $s$ on the same rigid body and express is in frame $\{S\}$, one has to do the following steps:
Explicitly writing out Step 1 gives $$ \mathcal{V}_{s,\{B\}} = \begin{bmatrix} \omega_{s,\{B\}} \\ v_{s,\{B\}} \end{bmatrix} = \begin{bmatrix} \omega_{b,\{B\}} \\ v_{b,\{B\}} + \omega_{b,\{B\}} \times \overrightarrow{sb} \end{bmatrix} = \begin{bmatrix} \omega_{b,\{B\}} \\ -p \times \omega_{b,\{B\}} + v_{b,\{B\}} \end{bmatrix} = \begin{bmatrix} I & 0 \\ -[p] & I \end{bmatrix} \begin{bmatrix} \omega_{b,\{B\}} \\ v_{b,\{B\}} \end{bmatrix} = \begin{bmatrix} I & 0 \\ -[p] & I \end{bmatrix} \mathcal{V}_{b,\{B\}} $$ Continuing with Step 2 gives $$ \mathcal{V}_{s,\{S\}} = \text{rot}(R)\mathcal{V}_{s,\{B\}} = \begin{bmatrix} R & 0 \\ 0 & R \end{bmatrix} \mathcal{V}_{s,\{B\}} = \begin{bmatrix} R & 0 \\ 0 & R \end{bmatrix} \begin{bmatrix} I & 0 \\ -[p] & I \end{bmatrix} \mathcal{V}_{b,\{B\}} = \begin{bmatrix} R & 0 \\ -R[p] & R \end{bmatrix} \mathcal{V}_{b,\{B\}} $$ In short-hand notations $$ \mathcal{V}_s = \begin{bmatrix} R & 0 \\ -R[p] & R \end{bmatrix} \mathcal{V}_b = X(T) \mathcal{V}_b $$ where $X(T)$ denotes the plucker transformation for twists, which can be computed equivalently by $$ X(T) = \begin{bmatrix} R & 0 \\ -R[p] & R \end{bmatrix} = \begin{bmatrix} R & 0 \\ R[p]^T & R \end{bmatrix} = \begin{bmatrix} R & 0 \\ [p]R & R \end{bmatrix} $$ by utilizing properties in \eqref{eq:ss-order} and \eqref{eq:ss-rot}.
Notice that power is a frame independent quantity, that is $$\mathcal{V}_s^T\mathcal{F}_s = \mathcal{V}_b^T\mathcal{F}_b$$ one can find the Plucker transformation of a wrench in the exact same context described above to be $$ \mathcal{F}_s = X^F(T) \mathcal{F}_b $$ where $X^F(T)$ is the Plucker transformation for wrenches, And $$X^F(T) = X^{-T}(T) = X^T(T^{-1})$$
Some alternative notations for Plucker transformations for twists (motion vectors, $\mathsf{M}^6$) and wrenches (force vectors, $\mathsf{F}^6$) are listed as follows. $$ X(T), X^F(T) \longrightarrow \sideset{^b}{}{X_a}, \sideset{^b}{}{X^F_a} \longrightarrow [\text{Ad}_{T}], [\text{Ad}_{T^{-1}}]^T \longrightarrow \text{Ad}_{T}, \text{Ad}_{T^{-1}}^T $$ where the operator denoted by $X(T), \sideset{^b}{}{X_a}, [\text{Ad}_{T}], \text{Ad}_T$ is called the adjoint representation of $T(R, p) \in SE(3)$ or the adjoint map associated with the homogeneous transformation $T$.
Note that the above implicitly assumes a generic $T = \sideset{^b}{}{T_a} \rightarrow (R, p)$, and hence $T^{-1} = \sideset{^a}{}{T_b} \rightarrow (R^T, -p)$.
Due to the physical nature of the above spatial transformations, there exists a few noticeable algebraic properties. In practical implementation, they can be applied to simplify numerical computation, in contrast to conducting raw matrix operations in $\mathbb{R}^{6\times6}$. In addition to using $X(T)$ to denote the Plucker transformation where $T(R,p) \in SE(3)$, the following also uses $X(R,p)$ to denote the same plucker transformation.
Generic to both $\mathsf{M}^6$, $\mathsf{F}^6$, and all spatial transformations, the following properties hold, which can be directly induced from $SE(3)$ properties due to the close correspondence between Plucker and homogeneous transformations: $$X^{-1}(T) = X(T^{-1}) = X(R^T, -p)$$ $$X_1(T_1)X_2(T_2) = X(T_1T_2) = X(R_1R_2, p_1 + R_1 p_2) $$
Specific to $\mathsf{M}^6$, $\mathsf{F}^6$, and different spatial transformations, there exists the following properties: $$X^F(T) = X^{-T}(T)$$ $$ X(R, p) \mathcal{V} = X \Big(R\omega, R(v - [p]\omega) \Big) \label{eq:xm-v} $$ $$ X^{F}(R, p) \mathcal{F} = X^F \Big(R(m - [p]f), R f \Big) \label{eq:xf-f} $$
In 2007@Featherstone and 2007@Siciliano , the concept of spatial acceleration is also introduced. It is defined as the rate of change of spatial velocity $$ \mathbf{a} = \begin{bmatrix} \dot{\omega} \\ \dot{v}_O \end{bmatrix} = \begin{bmatrix} \dot{\omega} \\ \ddot{r} - \omega \times \dot{r} \end{bmatrix} \in \mathbb{R}^6 $$ where $r$ is a position vector giving the position of the body-fixed point at $O$ relative to any fixed point, $\dot{v}_O$ is the derivative of $v_O$ taking $O$ to be space-fixed. And the classical acceleration is defined as $$ \mathbf{a}' = \begin{bmatrix} \dot{\omega} \\ \dot{v}'_O \end{bmatrix} = \begin{bmatrix} \dot{\omega} \\ \ddot{r} \end{bmatrix} = \mathbf{a} + \begin{bmatrix} 0 \\ \omega \times v_O \end{bmatrix} \in \mathbb{R}^6 $$ where $\dot{v}_O'$ is the derivative of $v_O$ taking $O$ to be body-fixed. They are related by $$ \mathbf{a}' = \mathbf{a} + \begin{bmatrix} 0 \\ \omega \times v_O \end{bmatrix} $$
Please refer to 2007@Featherstone and 2007@Siciliano for more details. In addition, contents in NatGeo@Coriolis provide a good visual illustration for the difference in Coriolis effect when taking space and body frames as reference.
The counterpart of spatial kinematics and transformations is spatial dynamics and their transformations. In addition, the advantage of spatial accelerations are that in the process of certain computation, one can directly taking derivative of twist without having to consider Coriolis or centrifugal terms, then consider them together at a later stage when classical acceleration of a certain point on the body is needed, for example, when computing the derivative of robot Jacobian.
In order to keep this post short and focused, spatial dynamics and their transformations, and further discussion on spatial accelerations and robot Jacobian derivative will be included in separate posts.