Motion representations

Rotation representations described a body’s orientation, angular velocity, and finite rotation. This section adds position. A pose pairs a rotation with a translation. A twist pairs an angular velocity with a linear velocity. A screw motion is the exponential of a twist. In Lie I, these are the group $SE(3)$, its Lie algebra $\mathfrak{se}(3)$, and the exponential between them. The general case is in three dimensions. Paragraphs marked In the plane. give the special case. The presentation follows section 3.3 of Lynch and Park (Kevin M. Lynch, 2017).

​

Poses in SE(3)

A homogeneous transformation matrix describes a pose: $$T = \begin{bmatrix} R & \mathbf{p} \\ 0 & 1 \end{bmatrix} \in SE(3) .$$ The matrix encodes both rotation and translation. It acts on homogeneous coordinates. A point $(\mathbf{x}, 1)$ is rotated and then translated. A free vector $(\mathbf{v}, 0)$ is only rotated because its zero cancels $\mathbf{p}$. The same matrix therefore handles both kinds of object. The inverse of a homogeneous transformation matrix is $$T^{-1} = \begin{bmatrix} R^T & -R^T \mathbf{p} \\ 0 & 1 \end{bmatrix} ,$$ and transformations compose by matrix multiplication, $T_{ac} = T_{ab} T_{bc}$. Composing two poses shows the interaction between translation and rotation: $$T_1 T_2 = \begin{bmatrix} R_1 & p_1 \\ 0 & 1 \end{bmatrix} \begin{bmatrix} R_2 & p_2 \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} R_1 R_2 & p_1 + R_1 p_2 \\ 0 & 1 \end{bmatrix} .$$ The orientations combine by multiplying their rotations. The first rotation acts on the second translation, producing the term $R_1 p_2$. As a manifold, SE(3) looks like $\mathbb{R}^3 \times SO(3)$, a position paired with an orientation. Its group structure also includes the action of rotation on translation. This structure is called a semidirect product and is written $SE(3) = \mathbb{R}^3 \rtimes SO(3)$. In the plane. The pose is a $3 \times 3$ matrix, and the configuration is the triple $(\theta, x, y)$: $$T = \begin{bmatrix} R(\theta) & \mathbf{p} \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} \cos\theta & -\sin\theta & x \\ \sin\theta & \cos\theta & y \\ 0 & 0 & 1 \end{bmatrix} \in SE(2) .$$ For a differential-drive robot such as the TurtleBot, $(x, y)$ is the position of the chassis and $\theta$ is its heading. The headings $\theta$ and $\theta + 2\pi$ are identical. The matrix $R(\theta)$ converts coordinates between the robot and world frames. A velocity $v_b = (v, 0)$ along the robot’s heading becomes $v_s = R(\theta)\,v_b = (v\cos\theta, v\sin\theta)$ in the world frame. SE(2) is the semidirect product $\mathbb{R}^2 \rtimes SO(2)$.

​

Twists

A twist combines the angular and linear velocity of a body in one six-dimensional vector: $$\mathcal{V} = \begin{bmatrix} \omega \\ v \end{bmatrix} \in \mathbb{R}^6 , \qquad [\mathcal{V}] = \begin{bmatrix} [\omega] & v \\ 0 & 0 \end{bmatrix} \in \mathfrak{se}(3) .$$ $\mathfrak{se}(3)$ is the Lie algebra of $SE(3)$. As with angular velocity, you can express a twist in two frames. The body twist comes from $[\mathcal{V}_b] = T^{-1}\dot{T}$. The space twist comes from $[\mathcal{V}_s] = \dot{T} T^{-1}$.

• $v_b = R^\top \dot{\mathbf{p}}$ is the velocity of the body-frame origin, written in the coordinates of $\{b\}$.
• $v_s = \dot{\mathbf{p}} - \omega_s \times \mathbf{p}$ is the velocity of the point of the body currently at the origin of $\{s\}$, written in $\{s\}$. That point may lie outside the physical body. You can imagine extending the body far enough to contain it.

The adjoint converts between the two: $$[\mathrm{Ad}_T] = \begin{bmatrix} R & 0 \\ [\mathbf{p}]R & R \end{bmatrix} , \qquad \mathcal{V}_s = [\mathrm{Ad}_T]\,\mathcal{V}_b .$$ Calculus on Lie groups derives this matrix. In the plane. A twist has three numbers, $\mathcal{V} = (\omega, v_x, v_y) \in \mathbb{R}^3$, and both matrix forms belong to $\mathfrak{se}(2)$: $$T^{-1} \dot T = \begin{bmatrix} [\omega_b] & \mathbf{v}_b \\ 0 & 0 \end{bmatrix}, \qquad \dot T T^{-1} = \begin{bmatrix} [\omega_s] & \mathbf{v}_s \\ 0 & 0 \end{bmatrix}, \qquad \omega_b = \omega_s = \dot\theta .$$ The linear parts are $\mathbf{v}_b = R^\top \dot{\mathbf{p}}$ and $\mathbf{v}_s = \dot{\mathbf{p}} - [\omega]\mathbf{p} = (\dot x + \dot\theta\, y,\ \dot y - \dot\theta\, x)$. The body twist is related to the rate of change of the configuration $q = (\theta, x, y)$ by $$\mathcal{V}_b = \begin{bmatrix} \omega_b \\ v_{bx} \\ v_{by} \end{bmatrix} = \begin{bmatrix} 1 & 0 & 0 \\ 0 & \cos\theta & \sin\theta \\ 0 & -\sin\theta & \cos\theta \end{bmatrix} \begin{bmatrix} \dot\theta \\ \dot x \\ \dot y \end{bmatrix} .$$

​

Screw motion

Exponential coordinates extend from rotations to poses. Let $\mathcal{S} = (\omega, v) \in \mathbb{R}^6$ be a screw axis. The displacement by $\theta$ along it is $$e^{[\mathcal{S}]\theta} = \begin{bmatrix} e^{[\omega]\theta} & G(\theta)v \\ 0 & 1 \end{bmatrix} , \qquad G(\theta) = I\theta + (1 - \cos\theta)[\omega] + (\theta - \sin\theta)[\omega]^2 .$$ Here $G(\theta)$ is Lynch and Park’s notation. It is unrelated to the generator $G$ of Lie I. Given a transformation $T = (R, \mathbf{p})$, the matrix logarithm reverses this map. First, $\log R$ gives $[\omega]\theta$. Then $v = G(\theta)^{-1} \mathbf{p}$. Thus $[\mathcal{S}]\theta = \log T$. Every pose can be reached by one screw motion, a rotation about an axis combined with a translation along it. In the plane. Any planar displacement is a rotation about a fixed point $\mathbf{s}$. The figure shows such a motion. (a) The frame $\{d\}$ is fixed to an elliptical rigid body and initially coincides with $\{s\}$. A rotation by $R$ followed by a translation by $\mathbf{p}$ moves it to $\{d'\}$, which coincides with the stationary frame $\{b\}$. The pair $(R, \mathbf{p})$ represents $\{b\}$ in $\{s\}$. The same transformation takes the frame $\{c\}$, also attached to the rigid body, to $\{c'\}$. Transformation 1 rotates $\{c\}$ about the origin of $\{s\}$. Transformation 2 translates the frame by $\mathbf{p}$, expressed in $\{s\}$. (b) The rotation and translation can occur simultaneously. The displacement is then a rotation of $\beta = 90^\circ$ about a fixed point $\mathbf{s}$. Two motions produce the same displacement. In the first, the body rotates by $R$ about the origin of $\{s\}$ and then translates by $\mathbf{p}$. In the second, it rotates by $\beta = 90^\circ$ about $\mathbf{s} = (0, 2)$. Both motions end at the same pose. The three screw coordinates $(\beta, s_x, s_y)$ parametrize the displacement. Here, $(s_x, s_y) = (0, 2)$ gives the coordinates of the point $\mathbf{s}$ in the fixed frame $\{s\}$. This point is the screw axis out of the page.

​

From velocity to motion

For rotations, angular velocity and the exponential form this sequence: $$R(t) \xrightarrow{\;\text{differentiate}\;} \dot R \xrightarrow{\;R^{-1}\;} [\omega] \xrightarrow{\;\exp([\omega]\Delta t)\;} R(t + \Delta t) .$$ Multiplication by $R^{-1}$ extracts the local motion. The exponential converts that motion into a finite rotation. In rotation representations, an animation of a tangent plane on a sphere depicts the same wrapping process for the sphere $S^2$. For poses, the corresponding quantities are $T$, the twist $[\mathcal{V}_b]$, and $e^{[\mathcal{V}_b]\Delta t}$. A body with a constant twist moves along a screw. In the plane. Consider the TurtleBot pose from poses in SE(3).

1. Velocity. The robot travels forward at speed $v$ and turns at rate $\omega$. Its wheels cannot slide sideways, so its velocity in the body frame is the twist $\mathcal{V}_b = (\omega, v, 0)$. This order follows Lynch and Park: turning rate comes first, forward speed second, and sideways speed last. The sideways component is zero. In matrix form, the twist is $$[\mathcal{V}_b] = \begin{bmatrix} 0 & -\omega & v \\ \omega & 0 & 0 \\ 0 & 0 & 0 \end{bmatrix} \in \mathfrak{se}(2) .$$ This matrix belongs to the Lie algebra. In the notation of Lie I, $[\mathcal{V}_b]$ is $\tau^\wedge$.
2. Motion. During a short time interval $\Delta t$, update the pose with $$T_{k+1} = T_k \exp\big([\mathcal{V}_b]\,\Delta t\big) .$$

Wheeled robots derives $(v, \omega)$ from the wheel speeds and applies this update to odometry. The chassis can point in any direction and reach any position. At each instant, the constraint $v_{by} = 0$ allows it to move only forward, backward, or turn.

​

References

• Kevin M. Lynch, F. (2017). Modern Robotics.

---