Really Basic Things About SE2

An SE2 transformation has a matrix representation, composed of a rotation matrix \(R(\theta) \in \text{SO2}\) and a translation vector \(d \in \mathbb{R}^{2}\)

\begin{equation} T = \begin{bmatrix} R(\theta) & d \\ 0 & 1 \end{bmatrix} \end{equation}

Recall that a rotation matrix is orthornomal (so \(R^{-1} = R^{T}\)), and that for SO2 \(R(\theta)^{T} = R(-\theta)\).

Notation

A common notation is \(T_{\textrm{f1\_f2}}\), meaning that \(T\) transforms points in frame f2 to points in frame f1. For example, let f1=w, i.e., the world frame, and leg f2=ego, i.e., the ego frame of a robot (also called the base_link in the ROS world). The given a point \(p_{ego}\) expressed in the ego frame, that point is transformed to the world frame by

\begin{equation} p_{w} = \xformlmul{w}{egp} p_{\cancel{\textrm{ego}}} \end{equation}

Similarly, SE2 transforms can be composed. Using analogous notation, let \(T_{\text{ego\_wheel}}\) be the transform from the ego frame to the wheel frame. Then the transform of the wheel in the world frame is

\begin{equation} T_{w\_wheel} = \xformlmul{w}{ego} \xformrmul{ego}{wheel} \end{equation}

This notational convention of \(\xform{to}{from}\) allows one to simply cross out adjacent frames in a multiplication to arrive at the correct interpretation of a transform composition. Further, the inverse of transform simply flips the position of the frames,

\begin{equation} \xform{ego}{w} = (\xform{w}{ego})^{{-1}}. \end{equation}

Inverses

The inverse matrix of an SE2 transform is

\begin{equation} T^{-1} = \begin{bmatrix} R(\theta) & d \\ 0 & 1 \end{bmatrix} ^{-1} =\begin{bmatrix} R(\theta)^{-1} & -R(\theta)^{-1}d \\ 0 & 1 \end{bmatrix} \end{equation}

which is easy to verify by direct multiplication.

Given two robot poses at time \(t_1\) and \(t_2\), \(\xform{w}{ego1}\) and \(\xform{w}{ego2}\), the error between these poses is

\begin{align} \xform{ego1}{ego2} &= T_{\text{w\_ego1}}^{-1} T_{\text{w\_ego2}} \\ &= \xformlmul{ego1}{w} \xformrmul{w}{ego2} \\ &= \begin{bmatrix} R(\theta_{1})^{-1} & -R(\theta_{1})^{-1}d_{1} \\ 0 & 1 \end{bmatrix} ^{-1} \begin{bmatrix} R(\theta_{2}) & d_{2} \\ 0 & 1 \end{bmatrix} = \begin{bmatrix} R(\theta_{2} - \theta_{1}) & R(\theta_{1})^{-1}(d_{2} - d_{1}) \\ 0 & 1 \end{bmatrix} \end{align}

Note that

  1. The difference in angles is \(\theta_{2} - \theta_{1}\) and
  2. The distances between the frames is

    \begin{equation} ||R(\theta_1)(d_2 - d_1)||_2 = ||(d_2 - d_1)||_2 \end{equation}

    because \(||R(\theta_1)||_2 = 1\).

Time Derivative

Suppose that \(\theta\) is a linear function of \(t\), \(\theta(t) = \omega t\). Then

\begin{align} \dot R(\theta(t)) &= \diff{}{t} \begin{bmatrix} -\sin(\theta(t)) & -\cos(\theta(t)) \\ \cos(\theta(t)) & \sin(\theta(t)) \end{bmatrix} \omega \\ &= \begin{bmatrix} \cos(\theta(t)) & -\sin(\theta(t)) \\ \sin(\theta(t)) & \cos(\theta(t)) \end{bmatrix} \begin{bmatrix}0 & -\omega \\ \omega & 0\end{bmatrix} \\ &= R(\theta) \hat{\omega} \\ &= \hat{\omega} R(\theta) \end{align}

Further Reading

I found these lecture notes to be quite useful: Rigid Body Kinematics (Slides)

A micro Lie theory for state estimation in robotics by Joan SolĂ , Jeremie Deray, Dinesh Atchuthan is a very useful tutorial paper.

For a discussion of representating and transforming splines of SE3 poses, see my article here.