Foundations of Robotics: 2018-I

- Topic 2 -
Spatial Representations of
Rigid Bodies (Part I)
Prof. Oscar E. Ramos, Ph.D.

- Week 1 -
 Outline

1. Introduction

2. Position of a Rigid Body

3. Orientation of a Rigid Body

4. Rotation Matrix

4.1. Properties and SO(3)

4.2. Interpretations of the Rotation Matrix

4.3. Composition of Rotations

- Spatial Representations of Rigid Bodies (Part I) -

Prof. Oscar E. Ramos, Ph.D. 2
 Introduction
How to Move Towards an Object?

• Reference frames (coordinates) are assigned to the desired parts

• The position and orientation (of a frame) with respect to another (reference)
frame is obtained

End effector
{E}

object
{O}

{F}
conveyor
belt

{B}
base
3
 Introduction
Where is the Mobile Robot?

• Reference frames (coordinates) are assigned to the desired parts

• The position and orientation of the robot with respect to a reference frame is
obtained

inertial robot {R}

{I}

https://www.nasa.gov/images/content/482680main_PIA09201-full.jpg 4
 Introduction
Where are the Aerial Robots?

• Reference frames (coordinates) are assigned to the desired parts

• The position and orientation of the robots with respect to a reference frame is
obtained

{R3} {R1}
{R2}

{I}
inertial

5
 Introduction
How to interpret the sensor data?

• Reference frames (coordinates) are assigned to the desired parts

• The position and orientation of the sensor data with respect to a reference
frame on the robot is obtained

{R}
{I} {S}
sensor
robot

Image from: Kuindersma et al. Optimization-based Locomotion Planning, Estimation, and Control Design for the Atlas Humanoid Robot,
Autonomous Robots, July 2015 6
 Introduction
… And a Humanoid Robot?

• Several reference frames

(coordinates) are assigned to {He}
the desired parts (hands, feet,
head, waist, chest, etc)
{Ch}

{Hl}
• The position and orientation of {Wr}
these parts with respect to a {Hr}
reference frame is obtained

{Fl}
{I} {Fr}

HRP-2 at LAAS-CNRS (Toulouse) 7

 Outline

1. Introduction

2. Position of a Rigid Body

3. Orientation of a Rigid Body

4. Rotation Matrix

4.1. Properties and SO(3)

4.2. Interpretations of the Rotation Matrix

4.3. Composition of Rotations

- Spatial Representations of Rigid Bodies (Part I) -

Prof. Oscar E. Ramos, Ph.D. 8
 Position of a Rigid Body

• How to represent the position of a rigid body?

- Through the position of a point P fixed to the body
- Position vector: P  OP

ẑ
P

OP
ŷ
O
x̂

• How many coordinates are needed?

- In 2D: 2 coordinates (2 dof)
dof: degrees of freedom
- In 3D: 3 coordinates (3 dof)

• Representations: Cartesian, cylindrical, spherical coordinates

9
 Position of a Rigid Body

1. Cartesian coordinates

ẑ
P
x
z
ŷ p   y 
x
 z 
x̂
y

2. Cylindrical coordinates
ẑ x   cos 
P
  y   sin 
Relation with
z ŷ p    Cartesian
coordinates
 z    x2  y 2

   atan2( y, x)
x̂

10
 Position of a Rigid Body

3. Spherical coordinates
ẑ
P with:
r 
r 0  
 p    0    2
ŷ   r0


x̂

x  r sin  cos    r sin 

y  r sin  sin  c  
Relation with z  r cos  z  r cos 
Relation with
Cartesian cylindrical
coordinates coordinates
r  x2  y2  z 2 (  ,c , z ) r   2  z2
( x, y , z )
  cos 1  z / r    atan2(  , z )
  atan2( y, x)   c
11
 Outline

1. Introduction

2. Position of a Rigid Body

3. Orientation of a Rigid Body

4. Rotation Matrix

4.1. Properties and SO(3)

4.2. Interpretations of the Rotation Matrix

4.3. Composition of Rotations

- Spatial Representations of Rigid Bodies (Part I) -

Prof. Oscar E. Ramos, Ph.D. 12
 Orientation of a Rigid Body

• How to represent the orientation of a rigid body?

Relation
ẑ
between frames

{W}
ŷ
O
x̂

• How many coordinates are needed?

- In 2-D: 1 coordinate (1 dof)
- In 3-D: 3 coordinates (3 dof)
- In n-D: n(n-1)/2 coordinates

13
 Orientation of a Rigid Body

• In general:
- Orientation is not as straight-forward as position
- The problem is the topology of the space that describes the orientation (it is
not Euclidean)
• Example:
Description of the surface of a
2-D sphere in the 3-D space

• Representations of orientation:
- Rotation matrix (implicit representation)
• Exponential coordinates
- Parameterizations
• Axis/angle representation: 3 parameters (minimal)
• Roll, Pitch, Yaw angles: 3 parameters (minimal)
• Euler angles: 3 parameters (minimal)
• Quaternions: 4 parameters (implicit representation)

14
 Outline

1. Introduction

2. Position of a Rigid Body

3. Orientation of a Rigid Body

4. Rotation Matrix

4.1. Properties and SO(3)

4.2. Interpretations of the Rotation Matrix

4.3. Composition of Rotations

- Spatial Representations of Rigid Bodies (Part I) -

Prof. Oscar E. Ramos, Ph.D. 15
 Rotation Matrix
Frames

• A frame is completely represented by its axes

• Frame = coordinate system
• Example: Frames {A} and {B}




Remarks: - ^ represents a unit vector

- Superindex represents the reference frame (with respect to …)
16
 Rotation Matrix
Generic Rotation Matrix

With respect to {A}

• 2 frames: {A} and {B}
- Common origin
- Rotated
?
?
• Objective: describe frame {B} with
respect to frame {A} ?

• Coordinates of the axes of {B} with respect to {A} (using projections)

- In vector form …

17
 Rotation Matrix
Generic Rotation Matrix

• Coordinates of the axes of {B} with respect to {A}

- We want R so that the axes of {B} with respect to {A}:

- Matrix R is:

Generic rotation matrix

• Rotation Matrix of frame {B} with respect to frame {A}

Every column represents the axes (x, y, z) of frame {B}

with respect to frame {A}
18
 Rotation Matrix
Generic Rotation Matrix

• Frame {B} with respect to frame {A}

• Frame {A} with respect to frame {B}

T
B
RA   RB 
A

19
 Rotation Matrix
Generic Rotation Matrix

• Example 1:
Given the following
frames {A} and {B} …

a) Obtain the rotation of frame {B} with respect to frame {A}

cos(90) cos(180) cos(90)  0 1 0 

A
RB   cos(0) cos(90) cos(90)   1 0 0 
 
cos(90) cos(90) cos(0)  0 0 1 

b) Obtain the rotation of frame {A} with respect to frame {B}

 cos(90) cos(0) cos(90)   0 1 0 

B
RA  cos(180) cos(90) cos(90)    1 0 0 
   
 cos(90) cos(90) cos(0)   0 0 1 
20
 Rotation Matrix
Generic Rotation Matrix

• Example 2: obtain the rotation of frame {B} with respect to frame {A}

 cos( ) cos(90   ) cos(90) 

Rotation A
RB  cos(90   ) cos( ) cos(90) 
about z
 cos(90) cos(90) cos(0) 
cos   sin  0
Similarly, the
A
RB   sin  cos  0  rotation about x or y
 0 0 1  is obtained.

• Example 3: given the following rotation, plot frames {A} and {B}

 0 1 0 
A
RB   0 0 1
1 0 0 

21
 Rotation Matrix
Generic Rotation Matrix

• Example 4:
There is a table, and on the table there is a
cube. There is a camera over the center of the
cube and its frame is {3}. Find the rotation
matrix that relates frame {2} to the camera
frame {3}

0 1 0 
3
R2  1 0 0 
0 0 1

(Adapted from Spong et al, Robot Modeling and

Control, 2006)
22
Rotation Matrix
Elementary Rotations

Rotation about the x axis

1 0 0 
Rx ( )  0 cos   sin  
 
0 sin  cos  

Rotation about the y axis

 cos  0 sin  
Ry ( )   0 1 0 
 
  sin  0 cos  

Rotation about the z axis

cos   sin  0
Rz ( )   sin  cos  0 
 0 0 1 
23
 Outline

1. Introduction

2. Position of a Rigid Body

3. Orientation of a Rigid Body

4. Rotation Matrix

4.1. Main Properties and SO(3)

4.2. Interpretations of the Rotation Matrix

4.3. Composition of Rotations

- Spatial Representations of Rigid Bodies (Part I) -

Prof. Oscar E. Ramos, Ph.D. 24
 Properties of a Rotation Matrix

1. Transpose of a rotation matrix (R)

Every axes ( , , ̂ ) is a unit

vector and is perpendicular to
the other 2

• Note that matrix RT is also a rotation matrix:

• The transpose is equal to the inverse:

R is an orthogonal matrix

25
 Properties of a Rotation Matrix

1. Transpose of a rotation matrix (R)

• R is an orthogonal matrix:
→ It has orthogonal rows (and columns)

3 constraints

3 constraints

6 constraints

→ Its rows (and columns) are an orthonormal basis in ℝ

• R has 9 elements
• R has 6 constraints (due to the orthonormal columns)
• R has 9 – 6 = 3 degrees of freedom (only 3 dof for orientation)

The rotation matrix is a redundant matrix!

26
 Properties of a Rotation Matrix

2. Determinant of a rotation matrix (R)

RT R  I det( RT R)  det( I )
det( RT ) det( R )  1 Property: det( RT )  det( R)
2
 det( R)  1
det( R )  1
z Right-handed frame:
In a right-handed frame (normally used):
x y  z
yz  x
y
det( R )  1 zx  y
x

 x1 y1 z1 
det( R )  x1 ( y2 z3  y3 z2 )  x2 ( y1 z3  y3 z1 )  x3 ( y1 z2  y2 z1 )
R   x2 y2 z2 
 x3 y3 z3   x12  x2 2  x32  1

In a right-handed frame: y  z  ( y2 z3  y3 z2 , z1 y3  y1 z3 , y1 z2  z1 y2 )  ( x1 , x2 , x3 )
27
 Rotation Group: SO(3)

• The rotation group in ℝ is known as SO(3) and is defined as:

• SO = Special Orthogonal
- Orthogonal matrices
- Special because the determinant is +1 (positive)

• SO(3)
- It represents rotations of rigid bodies
- It is a mathematical formalism (a Lie group)

For a matrix R to be a rotation matrix, it must belong to SO(3):

it must satisfy RRT=I and det(R)=+1

28
 Properties of a Rotation Matrix

• Example 1
Do the following matrices represent rotation matrices?

a)  1 3 b)  1 3
 0    0  
 2 2   2 2 
R1   0 1 0  R2   0 1 0 
   
 3 0 
1   3 0 
1 
 2 2   2 2 

Solution
a) 1
det R1  It is not a rotation matrix
2

b) det R2  1
It is a rotation matrix
R2 R2T  I

29
 Properties of a Rotation Matrix

• Example 2
The following matrix is a rotation matrix. a) Compute its inverse, b) Verify that
it is a rotation matrix, c) Compute its transpose

 0.25 0.433 0.866 

R  0.866 0.50 0 
 0.433 0.75 0.50 

Solution
a) Since it is a rotation matrix, its transpose is its inverse. Thus the inverse is:

 0.25 0.866 0.433 

RT  R 1   0.433 0.50 0.75 
0.866 0 0.50 

b) To verify, we need to check that det(R) = +1, and RRT = I

c) Its transpose is equal to its inverse and has been computed in a)

30
 Outline

1. Introduction

2. Position of a Rigid Body

3. Orientation of a Rigid Body

4. Rotation Matrix
4.1. Main Properties and SO(3)

4.2 Interpretations of the Rotation Matrix

4.2. Composition of Rotations

- Spatial Representations of Rigid Bodies (Part I) -

Prof. Oscar E. Ramos, Ph.D. 31
 Interpretations of the Rotation Matrix
a) Representation of the Orientation

• It describes the relative orientation between 2 reference frames

:
• Interpretation 1: Represents frame {B} in terms of frame {A}

• Interpretation 2: Takes frame {A} to frame {B}

- We start with the “reference frame” {A} (which is canonic in {A})
- We end in frame {B} (given by coordinates of {B} in {A})

32
 Interpretations of the Rotation Matrix
b) Mapping between frames

• It represents the same point in different frames

Point in frame {A} Point in frame {B}

- We want: start with point in {B} and represent it in {A} (obtain the coordinates in {A})
33
 Interpretations of the Rotation Matrix
b) Mapping between frames

- Objective: start with point in {B} and represent it in {A} (obtain the coordinates in {A})

- How? Projecting the point on every axis of {A}

- In matrix form:

- Mapping of the point between frames:

34
 Interpretations of the Rotation Matrix
b) Mapping between frames

Example
a) Determine by inspection the coordinates of
point P in frame B
d2  d1 P
B
p   d1 
 0 
d2
b) Starting with point P in frame B, determine
its coordinates in frame A
 0 1 0 
- Rotation matrix: A
RB  1 0 0 
 0 0 1 

0 1 0   d 2   d1 
- Transformed point: A     
p  A RB B p  1 0 0   d1    d 2 
0 0 1   0   0 
35
 Interpretations of the Rotation Matrix
c) Rotation Operator

• It rotates a vector within the same reference frame

- In general: it rotates vector an angle θ about axis to obtain

- Example:
Find the resulting vector after rotating vector p1 = (0, 3, 1) an angle 30°
about axis x
 0 
1 0 0  1 0

 
R(30, x)  0 cos(30)  sin(30)   0 3
2
1 
2
 
0 sin(30) cos(30)  0 1 3 
 2 2

0
 
p 2  R (30, x)p1   1 
 3
 
36
 Outline

1. Introduction

2. Position of a Rigid Body

3. Orientation of a Rigid Body

4. Rotation Matrix
4.1. Main Properties and SO(3)

4.2. Interpretations of the Rotation Matrix

4.2. Composition of Rotations

- Spatial Representations of Rigid Bodies (Part I) -

Prof. Oscar E. Ramos, Ph.D. 37
 Composition of Rotations

• Consider a point in 3 frames: {A}, {B}, {C}

- Point P in frame {A}: Ap
- Point P in frame {B}: Bp
- Point P in frame {C}: Cp

• Relations:
A
- Point from frame {B} to frame {A}: p  A RB B p
B
- Point from frame {C} to frame {B}: p  B RC C p
- Point from frame {C} in frame {A}: A
p   A RB B RC  C p

• Composition of rotations:
A
RC  A RB B RC
Interpretations of ARC:
1) It rotates point P from {C} to {B} and then from {B} to {A}
2) It starts with frame {A}, then makes it coincident with {B}, and then with {C}
38
 Composition of Rotations
a) Rotation with Respect to the Current (mobile) System

• A post-multiplication with a rotation matrix is needed

• Example 1
Initial frame: {0}
1º: Rotate θ1 about axis y0: Ry(θ1) → we obtain frame {1}
2º: Rotate θ2 about axis z1 (of the current system {1}): Rz(θ2) → we obtain
frame {2}
Resulting rotation: R = Ry(θ1) Rz(θ2)

1° z0 2° Final: z0
z1 z2 z1 y2 z2 y2
y1 2
y1
y0 y0
1
x0 x2 x0 Ry (1 ) Rz ( 2 )
x1 Ry (1 ) x1 Rz ( 2 ) x2
39
 Composition of Rotations
b) Rotation with Respect to the Fixed Frame

• A pre-multiplication with a rotation matrix is needed

• Example 2
Initial frame: {0}
1º: Rotate θ1 about frame y0: Ry(θ1) → we obtain frame {1}
2º: Rotate θ2 about frame z0 (of the initial, fixed {0}): Rz(θ2) → we obtain
frame {2}
Resulting rotation: R = Rz(θ2) Ry(θ1)

1° z0 2° z0
z1 2

z2 y2
y1
y0 y0
1
x0 x0
x1 Ry (1 ) x2 Rz ( 2 )

40
 Composition of Rotations
Example 3
Consider the following rigid body

First, apply a rotation of 90° about z. Then (2 cases):

a) Apply a rotation of 90° about the current (“new”) y axis
b) Apply a rotation of 90° about the fixed y axis

Ry (90)
a) current y
R  Rz (90) Ry (90)

Rz (90)

R  Ry (90) Rz (90)
Ry (90)
b) fixed y

Images from B. Siciliano et al, Robotics Modelling, Planning and Control, 2009 41
 Composition of Rotations
Example 4
Consider the following rigid body

First, apply a rotation of 90° about y. Then (2 cases):

a) Apply a rotation of 90° about the current (“new”) z axis
b) Apply a rotation of 90° about the fixed z axis

Rz (90)
a) current z
R  Ry (90) Rz (90)

Ry (90)

R  Rz (90) Ry (90)

Rz (90)
b) fixed z

Images from B. Siciliano et al, Robotics Modelling, Planning and Control, 2009 42
 Composition of Rotations
Example 5
We apply the following rotations to a reference frame in the following order:
1. Rotation of θ about the current x axis
2. Rotation of ϕ about the current z axis
3. Rotation of α about the fixed z axis
4. Rotation of β about the current y axis
5. Rotation of γ about the fixed x axis
Write the expression of the resulting rotation matrix

Rx ( )
Rx ( ) Rz ( )
Rz ( ) Rx ( ) Rz ( )
Rz ( ) Rx ( ) Rz ( ) Ry (  )
R  Rx ( ) Rz ( ) Rx ( ) Rz ( ) Ry (  )

43
 References

• Siciliano, Bruno, Sciavicco, Lorenzo, Villani, Luigi, y Oriolo,

Giuseppe. Robotics: Modelling, Planning and Control. Springer Science &
Business Media, 2010 (Chapter 2.1-2.3)

• Spong, M. W., Hutchinson, S., & Vidyasagar, M. Robot Modeling and

Control, Jon Wiley & Sons, 2006 (Chapter 2.1-2.4)

• Murray, Richard. M., Li, Z., Sastry, S. S., & Sastry, S. S. A Mathematical
Introduction to Robotic Manipulation. CRC press, 1994 (Chapter 2.1)

44