Rigid Body Transformation

Suppose we have a rigid body frame $b$ rotating and translating in a fix world frame $w$, the transformation from $b$ to $w$ is $T^{wb} = T_{trans}T_{rot}$, then we have the following properties:

  • $RR^{wb}$ rotates rigid body orientation $R^{wb}$ in $w$ frame

  • $R^{wb}R$ rotates rigid body orientation $R^{wb}$ in $b$ frame

  • $TT^{wb}$ rotates and translate rigid body pose $T^{wb}$ in $w$ frame

    • $T_{trans}T_{rot}T^{wb}$ rotates and translate rigid body pose $T^{wb}$ in $w$ frame
  • $T^{wb}T$ rotates and translate rigid body pose $T^{wb}$ in $b$ frame
    • $T^{wb}T_{trans}T_{rot}$ rotates and translate rigid body pose $T^{wb}$ in $b$ frame
In [ ]:

In the following example, I will show you how to rotate the frame in $w$ and $b$ to the get same pose. For rotating in $w$, we first rotate yellow frame at origin around z-axis in w frame 45 degree to get the cyan coordinate. Then we will do 45 degree rotation around y and then around x. Finall we should get a pose $R_{wb^{'}} = R_{roll}R_{pitch}R_{yaw}I$ where $I$ is the identity matrix. For rotating in $b$, we should get a pose $R_{wb^{''}} = IR_{roll}R_{pitch}R_{yaw}$ where $I$ is the identity matrix.

In [55]:
Out[55]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:
In [ ]:

Now we let's think about both rotation and translation. $TT_{wb}$ rotates $T_{wb}$ in $w$ frame while $T_{wb}T$ rotates $T_{wb}$ in $b$ frame. As shown in the video below, you won't get the same pose from $TT_{wb} and T_{wb}T$. Cyan represents rotations in $w$ frame and purple represents rotations in $b$ frame.

How do I animate a rigid body transformation? I actually break $T$ into two parts: $T = T_{trans}T_{rot}$. $T_{trans}$ is a pure translation and $T_{rot}$ is a pure rotation. So I can animate the rigid body transformation in two steps with a linear velocity and angular velocity separately.

In [56]:
Out[56]:

Firstly, we will move the rigid body in $w$ frame, i.e. $T_{wb^{'}} = TT_{wb}$. In this case, we will do $T_{rot}$ first then $T_{trans}$.

In [ ]:

Then, we will move the rigid body in $b$ frame, i.e. $T_{wb^{''}} = T_{wb}T$. In this case, we will do $T_{trans}$ first then $T_{rot}$.

In [ ]:

Now think about what angle axis $\omega$, rotation angle $\theta$, and translation $t$ can bring $T_{wb}$ back to $I$. Of course, you can inverse the transformation matrix. But if you understand how rotating in body frame, you can easily figure this out by first find translation vector and then rotation components.

In [ ]:

But can you animate it with a velocity containing both linear velocity and angular velocity? This was a new thing I learned which is called Twist. Like angular velocity can be represented by a $3x1$ vector, twist can be represented by a 6x1 vector. In the video below, I first show how to move the purple frame back to origin by translating and rotating in its $b$ frame in two steps. Then I show an animation moving cyan frame back to origin smoothly with a combined linear and angular velocity.

In [58]:
Out[58]:
In [ ]:

Time Derivative of Rotation Matrices

We have a rotation matrix $R^{wb}$ and we are interested to know the time derivative of it as $\dot{R^{wb}}$. The rotation matrix has 9 elements, however we know that the degree of freedom of the angular velocity is 3 which can be represented as a magnitude value $\theta$ and a normalized direction vector $\omega$. Then the question is how do we get $\dot{R^{wb}}$?

Firstly, we specify the rotation vector $\omega_b$ in the body frame. Then in the body frame $b$, for each axis we can get $\dot{v} = [\omega_b]_\times v$. Put them into a matrix form, we can get $$ \begin{align} \dot{I_b} = [\omega_b]_\times \begin{bmatrix} 1, &0, &0 \\ 0, &1, &0 \\ 0, &0, &1 \end{bmatrix} \end{align} $$

The math notation might be very rigorous here, but the main idea is that each column is the linear velocity of an axis. If we integrate over time, we can get $R^{bb'} = \int{\dot{I_b(t)}}dt = \int{[\omega_b]_\times}dt$, then the new rotation matrix in the world frame will be $R^{wb'} = R^{wb}R^{bb'} = \int{\dot{R^{wb}}}dt$.

Then we found $\dot{R_{wb}} = R_{wb}[\omega_b]_\times$.

Summary

In this blog, I emphasize only on using $\omega_b$ because it can be measured by IMU in its body frame.

$$ \begin{align} \dot{R_{wb}} &= R_{wb}[\omega_b]_\times \\ \dot{R_{bw}} &= -[\omega_b]_\times R_{bw} \end{align} $$
This blog is converted from rigid-body-transformation.ipynb
Written on December 18, 2021