Motivation
Rigid body motion is a fundamental component in robotics. The description of rigid body system motion
often involves spatial vector algebra and certain special Orthogonal, Euclidean, and Lie groups.
In the literature, different notations and terminologies exist to describe some of the same physical concepts,
which can be confusing at times.
This post summarizes some of the most important concepts in rigid body motion and lists common terminologies
employed to describe them. For further details, please refer to
2017@Lynch
,
2007@Featherstone
,
2007@Siciliano
, and
1998@Dam
.
Orientation
Represents either the orientation of a rigid body or a rotation transformation.
Commonly presented in the following forms.
-
Rotation matrices, $R \in SO(3) \in
\mathbb{R}^{3 \times 3}$
-
$SO(3)$ denotes the Special Orthogonal Group including all 3D rotation matrices,
-
with $RR^T=I$, $\text{det}R = 1$.
-
Euler ZYX angles / Intrinsic rotations,
$(\theta_z, \theta_y, \theta_x)
\in \mathbb{R}^3$
-
$R(\theta_z, \theta_y, \theta_x) = I \, \text{Rot}(\hat{z}, \theta_z), \text{Rot}(\hat{y},
\theta_y), \text{Rot}(\hat{x}, \theta_x) \in SO(3)$,
-
indicating 3 consecutive rotations along the Z-Y-X axes w.r.t the
body frame,
-
$\hat{\mathcal{X}}$ denotes a unit vector,
-
$\text{Rot}(\hat{\mathcal{X}}, \mathcal{\Theta})$ denotes a rotation along a unit axis
$\hat{\mathcal{X}}$ by $\mathcal{\Theta}$ amount.
-
Roll-Pitch-Yaw angles / Extrinsic rotations,
$(\theta_r, \theta_p,
\theta_y) \in \mathbb{R}^3$
-
$R(\theta_r, \theta_p, \theta_y) = \text{Rot}(\hat{z}, \theta_y), \text{Rot}(\hat{y},
\theta_p), \text{Rot}(\hat{x}, \theta_r) \, I \in SO(3)$,
-
indicating 3 consecutive rotations along the z-y-x axes w.r.t the space frame.
-
Exponential coordinates representation, power series, Rodrigues formula,
angle-axis, $(\hat{\omega}, \theta) \in (\mathbb{R}^3, \mathbb{R})$
-
a rotation with an angular velocity of $\omega$ over a period of time $\Delta_T$ can be represented as
- $\hat{\omega} = \frac{\omega}{||\omega||}, \theta = ||\omega|| \Delta_T$,
-
$R = \text{Rot}(\hat{\omega}, \theta) = e^{[\hat{\omega}] \theta} = I + \text{sin}\theta [\hat{\omega}]
+ (1-\text{cos}\theta) [\hat{\omega}]^2 \in SO(3)$,
-
$
R = e^{[\hat{\omega}] \theta}
= \sum_{k=0}^{\infty} \frac{1}{k!} \big( [\hat{\omega}]\theta \big)^k
= \sum_{k=0}^{\infty} \frac{\theta^k}{k!} [\hat{\omega}]^k
\in SO(3)
$,
-
$[\hat{\omega}] \in so(3) \in \mathbb{R}^{3\times3}$ denotes the skew-symmetric matrix
expanded by $\hat{\omega}$,
-
$[\hat{\omega}]\theta \in so(3) \rightleftharpoons e^{[\hat{\omega}] \theta} \in SO(3)$ are the
matrix logarithm and exponential of each other,
-
$so(3)$ is the Lie algebra on Lie group $SO(3)$, more on $so(3)$.
-
Unit quaternions, $q \in \mathbb{H}_1 \in
\mathbb{R}^4$
-
$\mathbb{H}$ denotes the set of all quaternions, named after the inverter Hamilton,
-
$\mathbb{H}_1$ denotes the set of all unit quaternions,
-
sometime articles use $q\in\mathsf{S}^3$, where $\mathsf{S}^3$ denotes the 3D unit sphere in
$\mathbb{R}^4$ unit quaternions lie in,
-
$q = \begin{bmatrix} q_0 & q_1 & q_2 & q_3 \end{bmatrix}^T$ with $||q|| = 1$,
-
$\text{Rot}(\hat{\omega}, \theta)$ is analogous to
$q = q(\hat{\omega}, \frac{\theta}{2}) =
\begin{bmatrix}
\text{cos}\frac{\theta}{2} & \hat{\omega} \, \text{sin}\frac{\theta}{2}
\end{bmatrix}^T$,
-
notice the "half-angle" correspondence between $SO(3)$ and $\mathbb{H}$, that is
$(R, \theta) \Longleftrightarrow (q, \frac{\theta}{2})$.
-
Cayley-Rodrigues parameters, $(\hat{\omega},
\theta) \in
(\mathbb{R}^3, \mathbb{R})$
-
$\text{Rot}(\hat{\omega}, \theta) \in SO(3)$,
-
$r = \hat{\omega} \, \text{tan}\frac{\theta}{2} \in \mathbb{R}^3$,
- if $\text{tr}R \neq -1$:
-
$R = (I - [r])(I + [r])^{-1}$
-
$[r] = (I + R)(I - R)^{-1}$
- if $\text{tr}R = -1$:
-
$R = -(I - [r])(I + [r])^{-1}$
-
$[r] = (I - R)(I + R)^{-1}$
Position
Represents either the position of a rigid body or a translation transformation.
Commonly denoted as $p \in \mathsf{E}^3 \in \mathbb{R}^3$, where $\mathsf{E}^3$ denotes the set of all 3-DOF
Euclidean vectors.
Pose
Represents either the pose of a rigid body or a homogeneous transformation. Commonly expressed in the following
forms.
-
Homogenous transformations, $T \in SE(3) \in
\mathbb{R}^{4 \times 4}$
-
$SE(3)$ denotes the Special Euclidean Group including all homogeneous transformations,
-
$T = T(R, p) = \begin{bmatrix} R & p \\ \mathbf{0} & 1\end{bmatrix}$, where $R\in SO(3)$,
$p\in\mathsf{E}^3$,
-
$T^{-1} = \begin{bmatrix} R^T & -R^T p \\ \mathbf{0} & 1 \end{bmatrix}$,
$T\begin{bmatrix} x \\ 1\end{bmatrix} = \begin{bmatrix} Rx + p \\ 1 \end{bmatrix}$,
$T_1T_2 = \begin{bmatrix} R_1R_2 & p_1 + R_1 p_2 \\ \mathbf{0} & 1 \end{bmatrix}$,
-
notice that the above results can simply numerical computation for $SE(3)$ matrices as
oppose to generic matrix algebra in $\mathbb{R}^{4\times4}$.
-
Exponential coordinates representations /
Chasles-Mozzi theorem,
$(\mathcal{S}, \theta) \in (\mathbb{R}^6, \mathbb{R})$
-
$e^{[\mathcal{S}]\theta} = T(R,p) \in SE(3)$,
-
$\mathcal{S} = \begin{bmatrix} \bar{\omega} \\ \bar{v} \end{bmatrix}$ is the normalized "unit" screw
axis, with either $||\bar{\omega}||=1$, or $\bar{\omega} = \mathbf{0}$ and $||\bar{v}|| = 1$,
-
$[\mathcal{S}] = \begin{bmatrix} \, [\bar{\omega}] & \bar{v} \, \\ \mathbf{0} & 0 \end{bmatrix} \in
se(3) \in \mathbb{R}^{4 \times 4}$,
-
$[\mathcal{S}]\theta \in se(3) \rightleftharpoons e^{[\mathcal{S}]\theta} \in SE(3)$ are the matrix
logarithm and exponential of each other,
-
$se(3)$ is the Lie algebra of the Lie group $SE(3)$, more on $se(3)$ will be discussed in a separate
post.
-
$\theta$ is the distance traveled along the screw axis to transform the frame from its initial pose
$I$ to the final pose $T$,
- if $||\bar{\omega}||=1$:
-
$e^{[\mathcal{S}]\theta} =
\begin{bmatrix}
\, e^{[\bar{\omega}]\theta} &
\big\{ I\theta + (1-\text{cos}\theta) [\bar{\omega}] + (\theta - \text{sin})\theta
[\bar{\omega}]^2 \big\} \bar{v}
\, \\
\mathbf{0} & 1
\end{bmatrix}$,
$\theta$ corresponds to the angle of rotation about the screw axis,
- if $\bar{\omega} = \mathbf{0}$ and
$||\bar{v}||=1$:
-
$e^{[\mathcal{S}]\theta} =
\begin{bmatrix}
\, I & \bar{v}\theta\, \\
\mathbf{0} & 1
\end{bmatrix}$,
$\theta$ corresponds to the linear distance traveled along the screw axis,
-
for more on screw axis, see Twist and Screw Axis.
Twist and Screw Axis
Twists represents a spatial motion consists of an angular velocity and a linear velocity. Commonly denoted as
$$
\mathcal{V} = \begin{bmatrix} \omega \\ v \end{bmatrix} \in \mathsf{M}^6 \in \mathbb{R}^6
\label{eq:twist}
$$
where $\mathsf{M}^6$ denotes the set of 6-DOF Euclidean motion vectors.
Angular Velocity vs. Twist Interpretation, Axis and Rate
-
For an angular velocity $\omega$, it can be interpreted as $\omega = \frac{\omega}{||\omega||} ||\omega|| =
\hat{\omega} \dot{\theta}$, where $\hat{\omega}$ is the unit rotation
axis, $\dot{\theta}$ is the rate of
rotation about
$\hat{\omega}$.
-
For a twist $\mathcal{V}$, it can be analogously interpreted as $\mathcal{V} = \mathcal{S} \dot{\theta}$,
where
$\mathcal{S}$ is the normalized screw axis,
$\dot{\theta}$ is the
rate of motion about $\mathcal{S}$.
Screw Axis Parameterization and Visualization
A screw axis $\mathcal{S} \in \mathbb{R}^6$ can be parameterized by $(q, \hat{s}, h) \in (\mathbb{R}^3,
\mathbb{R}^3, \mathbb{R})$. A twist can then be written as below.
$$
\mathcal{V}
= \begin{bmatrix} \omega \\ v \end{bmatrix}
= \begin{bmatrix} \, \hat{s}\dot{\theta} \\ -\hat{s}\dot{\theta} \times q + h\hat{s} \dot{\theta} \,
\end{bmatrix}
$$
See Fig. 3.19 in
2017@Lynch
for the meaning and visualization of
parameter $(q, \hat{s}, h)$ and the corresponding twist and screw axis.
Unit Rotation Axis vs. Normalized Screw Axis
To obtain the unit rotation axis for an angular velocity $\omega \in \mathbb{R}^3$, one can apply the following
formula.
$$\hat{\omega} = \frac{\omega}{||\omega||}$$
To obtain the normalized screw axis for a twist $\mathcal{V} = \begin{bmatrix} \omega \\ v \,
\end{bmatrix} \in \mathbb{R}^6$, there are two cases to consider.
-
If $\omega \neq 0$:
- the screw axis $\mathcal{S}$ is twist $\mathcal{V}$ normalized by the length of the
angular velocity vector,
- the rate of motion $\dot{\theta}$ is the angular velocity about the screw axis,
$$
\begin{align}
S &= \frac{\mathcal{V}}{||\omega||}
= (\frac{\omega}{||\omega||}, \frac{v}{||\omega||}) = (\bar{\omega}, \bar{v}) \\
\dot{\theta} &= ||\omega|| \\
\end{align}
$$
-
If $\omega = 0$:
- the screw axis $\mathcal{S}$ is twist $\mathcal{V}$ normalized by the length of the
linear velocity vector,
- the rate of motion $\dot{\theta}$ is the linear velocity along the screw axis,
$$
\begin{align}
S &= \frac{\mathcal{V}}{||v||} = (0, \frac{v}{||v||}) = (\bar{\omega}, \bar{v}) \\
\dot{\theta} &= ||v|| \\
\end{align}
$$
Which summarizes to:
$$\mathcal{V} = \begin{bmatrix} \omega \\ v \end{bmatrix} = \mathcal{S}\dot{\theta}$$
$$\mathcal {S} = \begin{bmatrix} \bar{\omega} \\ \bar{v} \end{bmatrix}$$
where the pair $(\bar{\omega}, \bar{v})$ denoting the normalized "unit" screw axis with either:
- $||\bar{\omega}|| = 1$ with $\dot{\theta} = ||\omega||$, or
- $\bar{\omega}=0$ and $||\bar{v}||=1$ with $\dot{\theta} = ||v||$.
Wrench
Represents the moment of force acting on a rigid body at some point $r$ in some frame $\{A\}$. Commonly denoted
as
$$
\mathcal{F} = \begin{bmatrix} m \\ f \end{bmatrix} = \begin{bmatrix} \,r \times f \, \\ f \end{bmatrix} \in
\mathsf{F}^6 \in \mathbb{R}^6
\label{eq:wrench}
$$
where $\mathsf{F}^6$ denotes the set of 6-DOF Euclidean wrench vectors.
Alternatively, sometimes a wrench is also denoted as
$$\mathcal{W} = \begin{bmatrix} m \\ f \end{bmatrix}$$
Plucker Coordinates
The basis of 6-DOF spatial vector representation of twist in \eqref{eq:twist} and wrench in \eqref{eq:wrench}
are also referred to as the Plucker Coordinates in the literature.
When an article mentions the Plucker Coordinates of a velocity, acceleration, jerk, or force of an object or a
rigid body, it often refers to their spatial vector form in $\mathbb{R}^6$.
In addition to representing rigid body motion, another important role of the concepts and terminologies
discussed in this post is to facilitate the description of rigid body kinematics and dynamics. More on that will
be discussed in separate posts.
This post only discusses $SO(3)$ and $SE(3)$ matrix exponential, for their logarithm, and quaternion
exponential and logarithm, see
Exponential and Logarithm in SO(3), SE(3), Quaternion Space for further discussion.