Representing Spatial Velocities

I’ve been going through Russ Tedrake’s notes on robotics, which got me thinking about their so-called monogram notation. Basically, this is the notation that represents an $SE(3)$ transformation as $^aX^b$ — more on that shortly. There is a recent review paper that points to several variations on this notation, and my postdoc advisor Gregory Chirikjian wrote a paper on it as well. Tedrake’s monogram notation looks very elegant until you get to spatial velocities, at which point strange artifacts such as the “cross product” will appear. $\newcommand{\mf}[1]{\mathbf{#1}}$ $\newcommand{\mono}[4]{ { ^{#2} #1 ^{#3} _{#4} } }$

This got me thinking about the connections between the monogram notation and abstract Lie groups. The result of this deliberation was a new notation for spatial velocities that bridges the gap between abstract Lie groups and their applications to robotics and computer graphics. The notation I present here has the following characteristics:

• it is consistent with the monogram notation (with a minor caveat, noted in the appendix )
• it makes the transformation rules for spatial velocities look like those of points, vectors, and frames, and
• (for those familiar with abstract Lie groups) it emphasizes the role of the adjoint representation of $SE(3)$.

Chapter 1. The Frame Bundle

First, let’s think about the space in which frames live. Given a point $\mf p\in \mathbb R^3$, a frame $f$ at $\mf p^f$ is an ordered set of $3$ linearly independent vectors, $(\mf f_1, \mf f_2, \mf f_3)$, along with the base point $\mf p^f$. The frame is visualized as arrows starting at $\mf p$ and facing outwards.¹ The collection of all frames of $\mathbb R^3$ makes up the frame bundle, $\mathrm F\mathbb R^3$. There is a map $\pi:\mathrm F\mathbb R^3\rightarrow \mathbb R^3$ that maps $f$ to its base point $\mf p^f$, giving us a fiber bundle structure.

The contents of the frame $f\in\mathrm F\mathbb R^3$ can be stacked into a matrix as follows:

$$ f=\begin{bmatrix} \mf f_1 & \mf f_2 & \mf f_3 & \mf p^f\\ 0 & 0 & 0 & 1 \end{bmatrix}. $$

Meanwhile, an element $(A, \mf p)$ of the semi-direct product group $G\coloneq GL(3)\ltimes\mathbb R^3$² can be written as

$$ (A, \mf p) = \begin{bmatrix} A & \mf p\\ \mf 0 & 1 \end{bmatrix}. $$

The transformation $(A, \mf p)$ acts on $f$ from the right, giving us another frame that we denote as $g$:

$$ \begin{align} f \cdot (A, \mf p) &= \begin{bmatrix} \mf f_1 & \mf f_2 & \mf f_3 & \mf p^f\\ 0 & 0 & 0 & 1 \end{bmatrix}\begin{bmatrix} A & \mf p\\ \mf 0 & 1 \end{bmatrix}\\ &\eqcolon \begin{bmatrix} \mf g_1 & \mf g_2 & \mf g_3 & \mf p^g\\ 0 & 0 & 0 & 1 \end{bmatrix}\\ &=g. \end{align} $$

Conversely, given a pair of frames $f,g$, there is a unique transformation ${\mono Xfg~}\coloneq(\mono Afg~, \mono {\mf p}fgf)$ in $G$ that takes $f$ to $g$. Equivalently, ${^fX^g}$ is the frame $g$ as seen from $f$. As we will see, this notation lets us write the transformation rule for frames as $\mono Xeg~ = \mono Xef~ \mono Xfg~$.

${^fX^g}$ is the transformation that takes $f$ to $g$
equivalently, it's $g$ "as seen from" $f$

Chapter 2. Transformation Rules

In the last chapter, we implicitly assumed that there is a well-defined “origin frame”, which we will call $e$. This is the frame that, when represented as a $4\times 4$ matrix, becomes the identity matrix. For our purposes, we can assume that this is a frame that is stationary with respect to Earth 🌏, and located at a convenient location. Perhaps $e$ is located at (and aligned with) the lower-left corner of my desk. Let $f$ be another frame that is attached to the fan on my ceiling.

We can describe the vector that goes from $e$ to $f$ as $\mono {\mf p}{e}{f}{~}$. Expressing this vector in the basis of $f$, we have

$$ \mono {\mf p}{e}{f}{~} = \mono {\mf p}{e}{f}{\mf f_1} \mf f_1 + \mono {\mf p}{e}{f}{\mf f_2} \mf f_2 + \mono {\mf p}{e}{f}{\mf f_3} \mf f_3, $$

whose coefficients we can represent as a vector:

$$ \mono {\mf p}{e}{f}{f}\coloneq\begin{bmatrix} \mono {\mf p}{e}{f}{\mf f_1}\\ \mono {\mf p}{e}{f}{\mf f_2}\\ \mono {\mf p}{e}{f}{\mf f_3} \end{bmatrix}. $$

We could also represent $\mono {\mf p}{e}{f}{~}$ in the origin (i.e., Earth) frame:

$$ \begin{align} \mono {\mf p}{e}{f}{~} &= \mono {\mf p}{e}{f}{\mf e_1} \mf e_1 + \mono {\mf p}{e}{f}{\mf e_2} \mf e_2 + \mono {\mf p}{e}{f}{\mf e_3} \mf e_3\\ &= \begin{bmatrix} \mono {\mf p}{e}{f}{\mf e_1}\\ \mono {\mf p}{e}{f}{\mf e_2}\\ \mono {\mf p}{e}{f}{\mf e_3} \end{bmatrix} \ \\ &\eqcolon \mono {\mf p}{e}{f}{e}. \end{align} $$

As in Tedrake’s notes, if we omit a supercript/subscript, we mean the origin (or Earth) frame: $\mono {\mf p}{}{f}{}\coloneq\mono {\mf p}{e}{f}{e}$. This means that $\mono {\mf p}{}{}{f}$ is the zero vector, and $\mono X{}{}{}$ the identity matrix!

Notice that $f$ has the same matrix as $\mono X{}{f}{}$. We can view $f$ both as a frame and as a transformation from $e$ to $f$. Similarly, $\mf p^f$ can be viewed both as a point in $\mathbb R^3$ and as the vector going from the origin to $\mf p^f$. The underlying reason is that, in order to describe to you which point (or frame) I am talking about, I need to first choose an origin (or Earth frame).³

We have the following identities for frames $f,g,h,$ and $k$.

(Vector Addition) $\mono {\mf p}{f}{h}{k} = \mono {\mf p}{f}{g}{k} + \mono {\mf p}{g}{h}{k}$
(Additive Inverse) $\mono {\mf p}{f}{g}{k} = - \mono {\mf p}{g}{f}{k}$
(Inverse Transformation) $\mono X{f}{g}{} = (\mono X{g}{f}{})^{-1}$

Using these facts, we can derive the transformation rule for points. Let’s relate how the frames $e$ and $f$ see the point $\mf p^g$:

$$ \begin{align} \mono {\mf p}~g~ &= \mono {\mf p}~f~ + \mono {\mf p}fg e\\ &= \mono {\mf p}~f~ + \mono {\mf p}fg{\mf f_1}\mf f_1 + \mono {\mf p}fg{\mf f_2}\mf f_2 + \mono {\mf p}fg{\mf f_3}\mf f_3\\ &= \mono {\mf p}~f~ + \mono{A}~f~ \mono {\mf p}fg{f}, \end{align} $$

$$ \text{or}\quad \begin{bmatrix} \mono {\mf p}e g e\\ 1 \end{bmatrix} = \mono Xef~ \begin{bmatrix} \mono {\mf p}fg{f}\\ 1 \end{bmatrix} $$

where $\mono X~f~=(\mono{A}~f~, \mono {\mf p}~f~)$ and $\mono{A}~f~\in GL(3)$ is the matrix

$$ \mono{A}~f~ = \begin{bmatrix} \mf f_1 & \mf f_2 & \mf f_3 \end{bmatrix}. $$

What we wrote above is just the last column of the formula…

(Composition) $\mono Xeg~ = \mono Xef~ \mono Xfg~$

While $\mono Xef~$ translates between how $e$ and $f$ each see the frame $g$, we may be interested in translating between how $e$ and $f$ see the vector going from $f$ to $g$, which is $\mono {\mf p}fg~=\mono {\mf p}~g~ - \mono {\mf p}~f~$. We already know what this looks like:

$$ \mono {\mf p}fge = \mono{A}ef~\mono {\mf p}fgf. $$

When $e$ and $f$ are both positively-oriented orthogonal frames, $\mono{A}ef~$ is an orthogonal matrix, so we will write it as $\mono{R}ef~$ instead:

(Rotation-of-Basis) $\mono {\mf p}fge = \mono{R}ef~\mono {\mf p}fgf$

In this case, $\mono Xef~=(\mono{R}ef~, \mono {\mf p}efe)\in SE(3)$. These formulae hold if we replace $e$ with $h$; I just used $e$ to simplify the presentation.

Chapter 3. Velocities

Suppose we have curves $f :\mathbb R\rightarrow \mathrm F\mathbb R^3$ and $g :\mathbb R\rightarrow \mathrm F\mathbb R^3$, such that $f(t)$ and $g(t)$ are moving w.r.t. the origin frame $e$. Our task it to describe their velocities at time $t$.

Setting up the notation. I introduce the following notation for derivatives:

$$ \begin{align} (\mono X{f}{g}~)' &\coloneq \frac{d}{d\tau}{\mono X{f(t+\tau)}{g(t+\tau)}~}\Big\vert_{\tau=0}\\ \mono X{f}{g'}~ &\coloneq \frac{d}{d\tau}{\mono X{f(t)}{g(t+\tau)}~}\Big\vert_{\tau=0}\\ \mono X{f'}{g}~ &\coloneq \frac{d}{d\tau}{\mono X{f(t+\tau)}{g(t)}~}\Big\vert_{\tau=0} \end{align} $$

which means that $(\mono Xgg~)' = \mf 0$, and so on. Each of these quantities is the component-wise derivative of a time-dependent matrix. By differentiating the identity

$$ \mono X{f(t)}{g(t+\tau)}~ \mono X{g(t+\tau)}{f(t)}~ = \mf I, $$

we get $\mono Xf{g'}~=-\mono Xfg~\,\mono X{g'}f~\,\mono Xfg~$. In particular, $\mono Xg{g'}~=-\mono X{g'}g~$. I will not be using this notation much, but I found it to be very useful for deriving the forthcoming results.

Basic physics tells us that, in order to even define what velocity is, we need to decide which frame we consider to be stationary. We will write $\mono{\Lambda}fgk$ to denote the velocity of $g$ as seen from $k$, with $f$ considered to be the stationary/reference frame. It is of the form

$$ \mono{\Lambda}fgk = \begin{bmatrix} \mono{\Omega}fgk & \mono{\mf v}fgk\\ \mf 0 & 0 \end{bmatrix}\in \mathfrak{se}(3), $$

where $\mono{\Omega}fgk\in\mathfrak{so}(3)$ is a $3\times 3$ skew-symmetric matrix. As we will see, $\mono{\mf v}fgk$ is not the same as Tedrake’s $\mono{\mf v}{f}{g}{k}$; what I write as $\mono{\mf v}fgk$ is what Tedrake writes as $\mono{\mf v}{f}{g_f}{k}$. I will justify my choice of notation shortly.

Using the notation. We denote the velocity of $g$ as seen from itself, with $e$ considered as the stationary frame, as

(Left-Invariant Velocity) $ \mono{\Lambda}{}gg\coloneq \mono Xg{g'}~ $

It follows that $\mono X{g'}g~ = - \mono{\Lambda}{}gg$. Similar to how left-invariant vector fields work on abstract Lie groups, we have

$$ \begin{align} \mono X~{g'}~=\frac{d}{d\tau}{\mono X{e}{g(t)}~ \mono X{g(t)}{g(t+\tau)}~}\Big\vert_{\tau=0} = \mono X{}g~ \mono{\Lambda}{}gg. \end{align} $$

Thus, $\mono{\Lambda}{}gg=(\mono X{}g~)^{-1}\,\mono X{}{g'}~$ is indeed the left-invariant velocity of $g$, with $e$ considered to be the stationary/reference frame. What is then the right-invariant velocity? It is the velocity of $g$ as seen from $e$, with $e$ considered the stationary frame:

$$ \mono{\Lambda}ege\coloneq \mono X{e}{g}~ \mono{\Lambda}egg \,\mono Xge~. $$

More generally, the adjoint representation gives us the velocity as seen from another frame. Recalling the definition of the adjoint representation , $\mathrm{Ad}_X:\mathfrak{se}(3)\rightarrow\mathfrak{se}(3)$ for each $X\in SE(3)$, we have

(Frame-Change for Velocities) $$ \begin{align} \mono{\Lambda}ghe &\coloneq \mono Xef~ \,\mono{\Lambda}ghf \,\mono Xfe~\\ &=\mathrm{Ad}_{\mono X{e}{f}{}}(\mono{\Lambda}ghf) \end{align} $$

The nice symmetric notation helps us remember this formula, although I like the $\mathrm{Ad}$ notation since it makes the transformation rules look like those of vectors and frames.

Here is the big payoff of this notation. Velocities add like vectors, as long as they are expressed in the same frame:

(Addition of Velocities) $ \mono{\Lambda}fhk = \mono{\Lambda}fgk + \mono{\Lambda}ghk $

You are encouraged to prove this! As a hint, notice that it is sufficient to prove this for the case of $k=f$. As an example of how to apply this formula, you can verify that

$$ \mono{\Lambda}{f}{h}{h} = \mono{\Lambda}{}{h}{h} - \mathrm{Ad}_{\mono X{h}{f}{}}(\mono{\Lambda}{}{f}{f}), $$

or the left-invariant velocity of $\mono X{f}{h}{}$ is the velocity of $h$ minus the velocity of $f$ (as seen from $h$).

Chapter 4. Hat & Vee Maps

For an introduction to the hat and vee maps, see the books by Chirikjian or Barfoot, or Appendix A of my preprint . Basically, $\mathfrak{so}(3)$ has a certain choice of basis that lets us map skew-symmetric matrices to vectors in $\mathbb R^3$ using the vee map:

$$ \begin{align} \begin{bmatrix} 0 & -\omega_z & \omega_y\\ \omega_z & 0 & -\omega_x \\ -\omega_y & \omega_x & 0 \end{bmatrix}^\vee = \begin{bmatrix} \omega_x\\ \omega_y\\ \omega_z \end{bmatrix}, \end{align} $$

and the hat map is its inverse, so that $\mono{\Omega}efg^\vee = \mono{\boldsymbol\omega}efg$ and $\mono{\boldsymbol\omega}efg^\wedge = \mono{\Omega}efg$. The reason for this peculiar choice of basis is that it defines a Lie algebra isomorphism from $\mathfrak{so}(3)$ to $(\mathbb R^3, \times)$, where $\times$ is the cross product:

$$ \begin{align} \Omega_1 \Omega_2 - \Omega_2 \Omega_1 = ({\boldsymbol\omega}_1 \times {\boldsymbol\omega}_2)^\wedge \end{align} $$

and has interesting properties, such as $(\boldsymbol\omega^\wedge) \mf p = {\boldsymbol\omega}\times \mf p$. We can extend this to define hat and vee maps for $\mathfrak{se}(3)$:

$$ (\mono{\Lambda}{f}{g}{k})^\vee=\begin{bmatrix} \mono{\boldsymbol\omega}{f}{g}{k}\\ \mono{\mf v}{f}{g}{k} \end{bmatrix} $$

which is the vector that Tedrake writes as $\mono{\mathrm V}{f}{g_f}{k}$ (recall that my $\mono{\mf v}{f}{g}{k}$ is Tedrake’s $\mono{\mf v}{f}{g_f}{k}$). This vee map satisfies the following property; basically the choice of basis for $\mathfrak{se}(3)$ lets us write the adjoint action as a matrix:

$$ \begin{align} \textrm{Ad}_{\mono Xhg~}(\mono \Lambda efg) = \left( \begin{bmatrix} \mono Rhg~ & \mf 0 \\ (\mono {\mf p}hgh)^{\hspace{-2pt}^\wedge} \,\mono Rhg~ & \mono Rhg~ \end{bmatrix} \begin{bmatrix} \mono {\boldsymbol\omega}efg \\ \mono {\mf v}efg \end{bmatrix} \right)^\wedge \end{align} $$

Yes, that's a weird-looking matrix, and it is not clear why we are hitting $\mono {\mf p}hgh$ with the hat map. But it works! Using the cross-product properties given above, we get the formulae $$ \begin{align} \mono {\boldsymbol\omega}efh &= \mono Rhg~ \mono {\boldsymbol\omega}efg\\ \mono {\mf v}efh &= \mono {\mf p}hgh\times (\mono Rhg~ \mono {\boldsymbol\omega}efg) + \mono Rhg~ \mono {\mf v}efg \\ &=\mono {\mf p}hgh\times \mono {\boldsymbol\omega}efh + \mono Rhg~ \mono {\mf v}efg . \end{align} $$

This is where our notation departs from that of Tedrake. In our notation, the addition of velocities formula looks like

$$ \begin{align} \mono {\boldsymbol\omega}fhk &= \mono {\boldsymbol\omega}fgk + \mono {\boldsymbol\omega}ghk\\ \mono {\mf v}fhk &= \mono {\mf v}fgk + \mono {\mf v}ghk, \end{align} $$

both angular and translational velocities add when they are expressed in the same frame.

---

Appendix A. The Two Definitions

This begs the question of what the term $\mono {\mf v}ABC$ even means, in Tedrake’s notation. In his notation, what I am claiming is that $ \mono{\mf v}{A}{C_A}A = \mono{\mf v}{A}{B_A}A + \mono{\mf v}{B}{C_B}A $, which is true! In other words, my $\mono{\mf v}{A}{B}A$ is what Tedrake writes as $\mono{\mf v}{A}{B_A}A$. Note that $\mono{\boldsymbol\omega}ABA=\mono{\boldsymbol\omega}A{B_A}A$ in both of our notations, but

You can draw some diagrams to see that his choice for representing velocities is very reasonable from a physical standpoint. However, the notation I choose here is more compatible with the group structure of $SE(3)$, and makes the transformation rules look more natural.

Appendix B. Group Actions

An action of $G$ on $X$ is said to be…

• effective (or faithful) if $g\cdot x=x$ for all $x \in X$ implies $g=e$. Equivalently, the mapping $G\mapsto \textrm{Aut}(X)$ is injective.

  “Everyone (in $G$) is doing something somewhere (in $X$)"

• free if $g\cdot x=x$ for some $x$ already implies that $g=e$. Given an element $g\in G\backslash\lbrace e\rbrace$, its action has no fixed points (is fixed-point free!)

  “Everyone's doing something everywhere"

• transitive if any two points are connected by a group action. Consider the task of taking $x_1\in X$ to $x_2\in X$…

  (Given an arbitrary task) “It will be done by someone"

• regular if transitive and free. This makes $X$ a principal homogeneous $G$-space or a $G$-torsor ; the group element in the definition of transitivity is unique

  (Given an arbitrary task) “I know just the person!"