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¶

As briefly defined in the past lecture, 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, it 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.

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. The neutral reference frame has the following notation

\mathcal{F}.

(1)

Unless otherwise stated, a given position/orientation/pose is given with respect to that 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 with

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 or outputs.

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 rotations 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). As you have learned in theory, the rotations are defined, in this representation, as a matrix

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

(10)

where the identity rotation means no rotation, hence, 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

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

θ = 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). As you have learned in theory, translations and rotations 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} \cos{\theta} & -\sin{\theta} & x \\ \sin{\theta} & \cos{\theta} & y \\ 0 & 0 & 1 \end{array}\right],

(13)

that for $\theta = \frac{\pi}{2}$ , $x = 0.1$ , and $y = 0.2$ , we have

θ = 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, being defined in $\mathbb{R}^3$ .

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

(14)

This is straightforward and there’s nothing surprising in terms of properties, so we will move on to SO(3).

3D orientation/rotation¶

You have seen the general formulation of rotational matrices in 3D. 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 of SO(3) follow the same rules as SO(2), where sequential frame transformations as right multiplications

\mymatrix{R}_c = \mymatrix{R}_a\mymatrix{R}^a_b\mymatrix{R}^b_c,

(18)

for instance

θa = pi/4
Ra = np.array([[cos(θa),-sin(θa), 0],
               [sin(θa), cos(θ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)]])

Rc = Ra @ Ra_b @ Rb_c
print(f"Rc={Rc}")

Rc=[[ 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.

Ra_ = Rc @ Rb_c.T @ Ra_b.T 

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

The results are pretty much the same!

3D poses¶

3D poses are represented in SE(3) with matrices $\mymatrix{H}\in\mathbb{R}^{4 \times 4}$ with the following structure

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

(19)

Sequential transformations¶

We can also perform transformations using sequential multiplications, as for SO(3). Note also that we can verify the lack of commutativity on these transformations.

# An SE(3) translation
x = 1
y = 2
z = 3
θza = pi/4
Ha =  np.array([[cos(θza),-sin(θza), 0, 1],
                [sin(θza), cos(θza), 0, 2],
                [0,        0,        1, 3],
                [0,        0,        0, 1]])

# An SE(3) rotation
θxb = pi/4
Hb = np.array([[1, 0,        0,       0],
               [0, cos(θxb), -sin(θxb), 0],
               [0, sin(θxb),  cos(θxb), 0],
               [0, 0,        0,       1]])

# Ha then Hb
Hc = Ha @ Hb 

# Hb then Ha
Hc_ = Hb @ Ha 

if np.isclose(Hc,Hc_).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],

(20)

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

# Extract Rc from Hc
Rc = Hc[0:3,0:3] # The 3x3 Rotation matrix
tc = Hc[0:3,3].reshape((3,1)) # The 3x1 translation vector at the forth column. Reshape it into a column vector.

A =  Rc.T
B = -Rc.T@tc
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 |
Hc_inv = np.block([[A, B], 
                   [C, D]])

print(f"Rc_inv={Hc_inv}")

print(f"Rc @ Rc_inv = {Hc @ Hc_inv}")

Rc_inv=[[ 0.70710678  0.70710678  0.         -2.12132034]
 [-0.5         0.5         0.70710678 -2.62132034]
 [ 0.5        -0.5         0.70710678 -1.62132034]
 [ 0.          0.          0.          1.        ]]


Rc @ Rc_inv = [[ 1.00000000e+00  0.00000000e+00 -2.29934717e-17  2.22044605e-16]
 [ 0.00000000e+00  1.00000000e+00  0.00000000e+00  0.00000000e+00]
 [-2.29934717e-17  0.00000000e+00  1.00000000e+00 -4.44089210e-16]
 [ 0.00000000e+00  0.00000000e+00  0.00000000e+00  1.00000000e+00]]