L2 Rigid Body Motion - OXBR Website

License: CC-BY-NC-SA 4.0

Author: Murilo M. Marinho (murilo.marinho@manchester.ac.uk)

Pre-requisites for the learner¶

The user of this notebook is expected to have prior knowledge in

All the content and pre-requisites of lesson 1.

I found an issue¶

Thank you! Please report it at https://github.com/MarinhoLab/OpenExecutableBooksRobotics/issues

Latex Macros¶

$\providecommand{\myvec}[1]{{\mathbf{\boldsymbol{{#1}}}}}$ $\providecommand{\mymatrix}[1]{{\mathbf{\boldsymbol{{#1}}}}}$

\providecommand{\myvec}[1]{{\mathbf{\boldsymbol{{#1}}}}}
\providecommand{\mymatrix}[1]{{\mathbf{\boldsymbol{{#1}}}}}

Installing pre-requisites¶

%%capture
%pip install numpy
%pip install numpy --break-system-packages

Imports¶

import numpy as np
from math import pi, sin, cos

Rigid bodies¶

If the relative transformation between all points of a given object remain the same regardless of motion, it is a rigid body.

In other words, the object has no flexibility and the motion of the entire body can be prescribed by its position and orientation with respect to a given reference frame.

This tends to be the initial topic of robotics textbooks. That is because we can use this to derive the equations of motion for many classes of robots and objects from first principles.

Reference frames¶

Positions/translations and orientations/rotations of objects are always defined with respect to a reference frame. Reference frames can be attached to rigid bodies or at fixed locations in space. Reference frames are usually defined in such way to make the mathematical derivations simpler.

In this tutorial, the World (or neutral) reference frame has the following notation

\mathcal{F}.

(1)

Unless otherwise stated, a given position/orientation/pose is given with respect to the World frame. When other frames are needed we usually rely on notations such as

\mathcal{F}',\mathcal{F}''

(2)

when frames are sequential or

\mathcal{F}_a,\mathcal{F}_b

(3)

when relationships are more complex.

2D Position/translation¶

Positions/translations in 2D can be uniquely defined as any

\myvec{p} \in \mathbb{R}^2.

(4)

Hence, if we would like to define

\myvec{p} = \left[\begin{array}{ccc} x \\ y \end{array}\right] =\left[\begin{array}{ccc} 1 \\ 2 \end{array}\right],

(5)

we can do so programmatically with the following piece of code.

It is common for column and row vectors to not be distinguishable in Python with numpy. That is in general convenient but can cause problems when the dimension is important, so always pay close attention to what each function expects as input.

p = np.array([1.0, 2.0])

print(f"p={p}")

p=[1. 2.]

Composition of translations¶

Sequential translations, such as

\mathbb{R}^2 \ni \myvec{p}_i = \left[\begin{array}{ccc} x_i \\ y_i \end{array}\right],

(6)

with

i \in \mathbb{N}

(7)

can be composed with sequential additions

\myvec{p} = \myvec{p}_{0} + \myvec{p}_{1} + \myvec{p}_{2} + \myvec{p}_{3}.

(8)

p0 = np.array([1.0, 2.0])
p1 = np.array([2.0, 3.0])
p2 = np.array([3.0, 4.0])
p3 = np.array([4.0, 5.0])

p = p0 + p1 + p2 + p3

print(f"p={p}")

p=[10. 14.]

Inverse translation¶

The inverse translation can be obtained by subtractions and the element of no translation is the zero vector, that is, if we unwind all translations we’re back to the origin of the reference frame

\myvec{p}' = \myvec{p} - \myvec{p}_{0} - \myvec{p}_{1} - \myvec{p}_{2} - \myvec{p}_{3} = \left[\begin{array}{ccc} 0 \\ 0 \end{array}\right].

(9)

p0 = np.array([1.0, 2.0])
p1 = np.array([2.0, 3.0])
p2 = np.array([3.0, 4.0])
p3 = np.array([4.0, 5.0])
p = p0 + p1 + p2 + p3

p_ = p - p0 - p1 - p2 - p3

print(f"p_={p_}")

p_=[0. 0.]

2D orientation/rotation¶

Orientations/rotations in 2D can be defined in many different ways. In this tutorial, we will address the special orthogonal group for two dimensions, i.e., SO(2). These rotations are defined, in this representation, as a matrix

\mymatrix{R} \in \mathbb{R}^{2 \times 2}.

(10)

The identity rotation means no rotation. For any frame $\mathcal{F}_a$ ,

\mymatrix{R}^{a}_{a} = \mymatrix{I}_2.

(11)

An element of SO(2) is simply a matrix with the correct properties, therefore we can define one directly in numpy. For

\mymatrix{R}(\theta) = \left[\begin{array}{ccc} \cos{\theta} & -\sin{\theta} \\ \sin{\theta} & \cos{\theta} \end{array}\right],

(12)

when $\theta = \frac{\pi}{2}$ , we have the following equivalent piece of code.

θ = pi/2

R = np.array([[cos(θ),-sin(θ)],
              [sin(θ), cos(θ)]])

print(f"R={R}")

R=[[ 6.123234e-17 -1.000000e+00]
 [ 1.000000e+00  6.123234e-17]]

2D poses (combined translation/orientation)¶

2D poses can be represented using elements of SE(2). A translation followed by a rotation can be combined into a single $\mymatrix{H}\in\mathbb{R}^{3 \times 3}$ with the following structure

\mymatrix{H}(x,y,\theta) = \left[\begin{array}{ccc} 1 & 0 & x \\ 0 & 1 & y \\ 0 & 0 & 1 \end{array}\right] \left[\begin{array}{ccc} \cos{\theta} & -\sin{\theta} & 0 \\ \sin{\theta} & \cos{\theta} & 0 \\ 0 & 0 & 1 \end{array}\right] = \left[\begin{array}{ccc} \cos{\theta} & -\sin{\theta} & x \\ \sin{\theta} & \cos{\theta} & y \\ 0 & 0 & 1 \end{array}\right].

(13)

The translation and rotation order is extremelly important. Check the exercises at the end of this lesson.

For $\theta = \frac{\pi}{2}$ , $x = 0.1$ , and $y = 0.2$ , we have the following equivalent piece of code.

θ = pi/2
x = 0.1
y = 0.2

H = np.array([[cos(θ),-sin(θ), x],
              [sin(θ), cos(θ), y],
              [0,      0,      1]])


print(f"H = {H}")

H = [[ 6.123234e-17 -1.000000e+00  1.000000e-01]
 [ 1.000000e+00  6.123234e-17  2.000000e-01]
 [ 0.000000e+00  0.000000e+00  1.000000e+00]]

3D Position/translation¶

The 3D position/translations are a trivial extention of the 2D ones with one extra dimension.

\mathbb{R}^3 \ni \myvec{p}_i = \left[\begin{array}{ccc} x_i \\ y_i \\ z_i \end{array}\right].

(14)

There is nothing surprising in terms of properties, so we will move on to SO(3).

3D orientation/rotation¶

For rotational matrices in 3D, we usually compose basic rotations. For rotations about the basis vectors, we have

\mymatrix{R}(z,\theta) = \left[\begin{array}{ccc} \cos{\theta} & -\sin{\theta} & 0 \\ \sin{\theta} & \cos{\theta} & 0 \\ 0 & 0 & 1 \end{array}\right],

(15)

\mymatrix{R}(y,\theta) = \left[\begin{array}{ccc} \cos{\theta} & 0 & \sin{\theta} \\ 0 & 1 & 0 \\ -\sin{\theta} & 0 & \cos{\theta} \end{array}\right],

(16)

\mymatrix{R}(x,\theta) = \left[\begin{array}{ccc} 1 & 0 & 0 \\ 0 & \cos{\theta} & -\sin{\theta} \\ 0 & \sin{\theta} & \cos{\theta} \end{array}\right].

(17)

Rz = np.array([[cos(θ),-sin(θ), 0],
               [sin(θ), cos(θ), 0],
               [0,      0,      1]])

Ry = np.array([[ cos(θ), 0, sin(θ)],
               [      0, 1, 0],
               [-sin(θ), 0, cos(θ)]])

Rx = np.array([[1,      0,  0],
               [0, cos(θ), -sin(θ)],
               [0, sin(θ),  cos(θ)]])

# A rotation about z
print(f"Rz={Rz}")

# A rotation about y
print(f"Ry={Ry}")

# A rotation about x
print(f"Rx={Rx}")

Rz=[[ 6.123234e-17 -1.000000e+00  0.000000e+00]
 [ 1.000000e+00  6.123234e-17  0.000000e+00]
 [ 0.000000e+00  0.000000e+00  1.000000e+00]]
Ry=[[ 6.123234e-17  0.000000e+00  1.000000e+00]
 [ 0.000000e+00  1.000000e+00  0.000000e+00]
 [-1.000000e+00  0.000000e+00  6.123234e-17]]
Rx=[[ 1.000000e+00  0.000000e+00  0.000000e+00]
 [ 0.000000e+00  6.123234e-17 -1.000000e+00]
 [ 0.000000e+00  1.000000e+00  6.123234e-17]]

Compositions of rotations in SO(3)¶

Compositions in SO(3) follow the same rules as SO(2), where sequential rotations are represented by right multiplications. For instance,

\mymatrix{R}^0_c = \mymatrix{R}^0_a\mymatrix{R}^a_b\mymatrix{R}^b_c.

(18)

This can be represented by the following equivalent piece of code using sample angles.

θ0_a = pi/4
R0_a = np.array([[cos(θ0_a),-sin(θ0_a), 0],
               [sin(θ0_a), cos(θ0_a), 0],
               [0,      0,        1]])

θa_b = -pi/2
Ra_b = np.array([[ cos(θa_b), 0, sin(θa_b)],
                 [         0, 1, 0],
                 [-sin(θa_b), 0, cos(θa_b)]])

θb_c = pi/8
Rb_c = np.array([[1,         0,  0],
                 [0, cos(θb_c), -sin(θb_c)],
                 [0, sin(θb_c),  cos(θb_c)]])

R0_c = R0_a @ Ra_b @ Rb_c
print(f"R0_c={R0_c}")

R0_c=[[ 4.32978028e-17 -9.23879533e-01 -3.82683432e-01]
 [ 4.32978028e-17  3.82683432e-01 -9.23879533e-01]
 [ 1.00000000e+00  2.34326020e-17  5.65713056e-17]]

Inverse rotations in SO(3)¶

Inverse operations in SO(3) are analogous to the inversions in SO(2), which are simply matrix transpositions.

For example, we can recover $\mymatrix{R}^0_a$ from $\mymatrix{R}^0_c$ using inverse relative rotations as follows.

\begin{align} \mymatrix{R}^0_a &= \mymatrix{R}^0_c \mymatrix{R}^c_b \mymatrix{R}^b_a \\ \mymatrix{R}^0_a &= \mymatrix{R}^0_c (\mymatrix{R}^b_c)^T (\mymatrix{R}^a_b)^T \\ \end{align}

(19)

In the example code below, we store this alternative calculation in R0_a_ to compare it with R0_a.

R0_a_ = R0_c @ Rb_c.T @ Ra_b.T

if np.allclose(R0_a, R0_a_):
    print('The results are pretty much the same!')
else:
    print('The results are too different.')

The results are pretty much the same!

3D poses¶

In 3D, a translation followed by a rotation in the current frame is represented with elements in SE(3). These elements are matrices $\mymatrix{H}\in\mathbb{R}^{4 \times 4}$ with the following structure

\mymatrix{H}(\myvec{t},\mymatrix{R}) = \left[\begin{array}{ccc} \mymatrix{I} & \myvec{t} \\ \myvec{0} & 1 \end{array}\right] \left[\begin{array}{ccc} \mymatrix{R} & \myvec{0} \\ \myvec{0} & 1 \end{array}\right] = \left[\begin{array}{ccc} \mymatrix{R} & \myvec{t} \\ \myvec{0} & 1 \end{array}\right].

(20)

The translation and rotation order is extremelly important.

Sequential pose transformations¶

We can also perform pose transformations using sequential right multiplications when they are with respect to the current frame. Note also that we can verify the lack of commutativity on these transformations.

For example, consider a translation in 3D along the World frame, represented by the homogenous transformation matrix below.

\mymatrix{H}^0_a = \mymatrix{H}_a = \left[\begin{array}{ccc} 1 & 0 & 0 & 1 \\ 0 & 1 & 0 & 2 \\ 0 & 0 & 1 & 3 \\ 0 & 0 & 0 & 1 \end{array}\right].

(21)

Consider a rotation in 3D about the current frame, represented by the homogenous transformation matrix below.

\mymatrix{H}^a_b = \left[\begin{array}{ccc} \cos{\theta_{ab}} & -\sin{\theta_{ab}} & 0 & 0 \\ \sin{\theta_{ab}} & \cos{\theta_{ab}} & 0 & 0 \\ 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 1 \end{array}\right].

(22)

We can calculate their sequential combination as follows.

\mymatrix{H}^0_b = \mymatrix{H}_b = \mymatrix{H}_a\mymatrix{H}^a_b

(23)

We can compute this result with $\theta_{ab} = \frac{\pi}{4}$ as shown in the piece of code below. We also show that

\mymatrix{H}_a\mymatrix{H}^a_b \neq \mymatrix{H}^a_b\mymatrix{H}_a,

(24)

further reinforcing the importance of the order of operations.

# An SE(3) translation
x = 1 # distance in metres
y = 2 # distance in metres
z = 3 # distance in metres

H0_a =  np.array([[1, 0, 0, x],
                  [0, 1, 0, y],
                  [0, 0, 1, z],
                  [0, 0, 0, 1]])

# An SE(3) rotation
θab = pi/4 # angle in radians

Ha_b = np.array([[cos(θab), -sin(θab), 0,  0],
                 [sin(θab),  cos(θab), 0,  0],
                 [0,         0,        1,  0],
                 [0,         0,        0,  1]])

# H0_a then Ha_b
H0_b = H0_a @ Ha_b

# Ha_b then Ha
H_wrong = Ha_b @ H0_a

if np.isclose(H0_b,H_wrong).all():
    print('The results are close!')
else:
    print('The results are far, therefore the operation is not commutative')

The results are far, therefore the operation is not commutative

Inverse transformations¶

As you have learned in theory, the inverse transformation can be found as

\mymatrix{H^{-1}}(\myvec{t},\mymatrix{R}) = \left[\begin{array}{ccc} \mymatrix{R}^T & -\mymatrix{R}^T\myvec{t} \\ \myvec{0} & 1 \end{array}\right],

(25)

so it is important to remember to never invert the matrix with general matrix inversion algorithms. Using the matricial properties leads to a simpler, faster, and more accurate inversion. The only “trick” is to obtain the submatrix corresponding to the rotation matrix.

# Extract R from H
R0_b = H0_b[0:3,0:3] # The 3x3 Rotation matrix
t0_b = H0_b[0:3,3].reshape((3,1)) # The 3x1 translation vector at the forth column. Reshape it into a column vector.

A =  R0_b.T
B = -R0_b.T @ t0_b
C = np.array([0,0,0])
D = np.array([1])

# This is how you can use `np.block` to build a matrix like so
# | A B |
# | C D |
H0_b_inv = np.block([[A, B],
                     [C, D]])

print(f"H0_b_inv={H0_b_inv}")

print(f"H0_b @ H0_b_inv = {H0_b @ H0_b_inv}")

H0_b_inv=[[ 0.70710678  0.70710678  0.         -2.12132034]
 [-0.70710678  0.70710678  0.         -0.70710678]
 [ 0.          0.          1.         -3.        ]
 [ 0.          0.          0.          1.        ]]
H0_b @ H0_b_inv = [[1.00000000e+00 1.01465364e-17 0.00000000e+00 1.11022302e-16]
 [1.01465364e-17 1.00000000e+00 0.00000000e+00 0.00000000e+00]
 [0.00000000e+00 0.00000000e+00 1.00000000e+00 0.00000000e+00]
 [0.00000000e+00 0.00000000e+00 0.00000000e+00 1.00000000e+00]]

Exercises¶

Consider that the numpy and math modules are already imported as shown earlier in this lesson.

Exercise a¶

For $\theta_a = \frac{\pi}{4}$ , calculate

\mymatrix{R}^0_{a} = \mymatrix{R}_{a} = R(\theta_a) \in SO(2)

(26)

and store it in the variable R_a shown in the cell below.

θ_a = pi/4.0 # As given in the exercise

R_a = None # Replace None with your solution to this exercise.

Exercise b¶

Calculate the result of a rotation of $\theta_{b1} = \frac{\pi}{12}$ followed by a rotation of $\theta_{b2} = -\frac{\pi}{2}$ , in the current frame, using elements of SO(2).

Store the result in the variable R_b shown in the cell below.

θ_b1 = pi/12.0 # As given in the exercise
θ_b2 = -pi/2.0 # As given in the exercise

R_b = None # Replace None with your solution to this exercise.

Exercise c¶

Consider the translation

\myvec{p}_c = \left[\begin{array}{ccc} x_c \\ y_c \end{array}\right],

(27)

and the rotation

R(\theta_c) = \left[\begin{array}{ccc} \cos{\theta_c} & -\sin{\theta_c} \\ \sin{\theta_c} & \cos{\theta_c} \end{array}\right].

(28)

Starting at the World frame, calculate the homogenous transformation representing the rotation $R(\theta_c)$ , about the World frame, followed by the translation $\myvec{p}_c$ , in the current frame.

Consider $\theta_c = \frac{\pi}{3}$ , $x_c = 2$ , $y_c = 5$ , and store the result in the variable H_c shown in the cell below.

θ_c = pi/3.0 # As given in the exercise
x_c = 2.0 # As given in the exercise
y_c = 5.0 # As given in the exercise

H_c = None # Replace None with your solution to this exercise.

Exercise d¶

Consider the same variables as in Exercise c by replacing the subscripts with d. Calculate, instead, the translation followed by the rotation. Store the result in the variable H_d shown in the cell below.

Is H_c the same as H_d? What does that indicate?

θ_d = pi/3.0 # As given in the exercise
x_d = 2.0 # As given in the exercise
y_d = 5.0 # As given in the exercise

H_d = None # Replace None with your solution to this exercise.

Extra challenge(s)¶

Let a rotation represented by a SO(2) element have a time-varying angle of $\theta(t) = \sin(t) + 2\cos(t)$ .
- Write down its general form in SO(2) so that all four elements are clearly visible.
- Using this written down solution, compute the SO(2) representation at $t=10$ .
Using SE(3) elements, calculate the final rigid body motion after four sequential transformations.
- The first transformation is a rotation of $\theta=\frac{\pi}{10}$ about the $z$ -axis of the World frame.
- The second transformation is a translation of $d=0.3$ about the $z$ -axis of the current frame.
- The third transformation is a translation of $a=0.5$ about the $x$ -axis of the current frame.
- The forth and last transformation is a rotation of $\alpha=-\frac{\pi}{2}$ about the $x$ -axis of the current frame.