MCT-352

ROBOTICS
Week 4
INSTRUCTOR: DR. HASHIM IQBAL
 Week 4 Agenda
Rotation
◦ Properties of Rotation Matrix
◦ Interpretation of Rotation Matrix
◦ Composition of Rotation
◦ Parameterization of Rotation
Rigid Motions
◦ Homogeneous Transformations
 ^𝑨
𝒛
^𝑩
𝒛
{ 𝑩} { 𝑨} ^
𝒚𝑨
^
𝒚𝑩 ^
𝒙𝑩
^
𝒙𝑨

^𝑨
^𝑩 𝒛
𝒛
°
𝟗𝟎
^
𝒚𝑩 ^
𝒙𝑩
^
𝒚𝑨

^
𝒙𝑨
 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 
 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
 0 0 1  or y is obtained.

• Example 3: given the following rotation, plot frames {A} and {B}
^𝑧 𝐴
0  1 0 
A
RB  0 0  1
 1 0 0 
 Rotation Matrix
• Example:
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)
Rotation Matrix
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 
 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
 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!

 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)

det( R) 
2
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
 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)

For a matrix R to be a rotation matrix, it must belong

to SO(3): it must satisfy RRT=I and det(R)=+1
 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
Interpretations of the Rotation Matrix
a) Representation of the Orientation

• It describes the relative orientation between 2 reference

frames

:
• Interpretation : Represents frame {B} in terms of frame {A}
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})
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:
P

- Mapping of the point between frames:

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 
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 p = (0, , 1) an angle 30° about axis x
1

 0 
1 0 0  1 0

𝒑2 
R(30 , x)  0 cos(30 )  sin(30 )   0 3
2
 1 
2

 
𝒑1  0 sin(30 ) cos(30 )   0 1 3 
𝜃  2 2

 0
 
p 2  R (30 , x)p1  1 
 3
 
Composition of Rotations
 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:
◦ Point from frame {B} to frame {A}: A
p  A RB B p
◦ Point from frame {C} to frame {B}: 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 AR :
C

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 {
 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)
z0 2° Final: z0
z1 z2 z1 y2 z2 y2
y1 𝜃2 y1
𝜃 1 y0 y0
x0 x2 x0 Ry (1 ) Rz ( 2 )
x1 Ry (1 ) x1 Rz ( 2 ) x2
 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° y1
z1 𝜃2
z2 y2
y1
𝜃 1 y0 y0
x0 x0
x1 Ry (1 ) x2 Rz ( 2 )
 Composition of Rotations
Example 3
Consider the following rigid body

Then (2 cases):
First, apply a rotation of 90° about z.
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 ) R Rz (90 ) Ry (90 )
a) current y

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

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

Then (2 cases):
First, apply a rotation of 90° about y.
a) Apply a rotation of 90° about the current (“new”) z axis
Rz (90 )
b) Apply a rotation of 90° about the fixed z axis a) current z

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

Ry (90 )

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

b) fixed z

Images from B. Siciliano et al, Robotics Modelling, Planning and Control, 2009
 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 (  )
Parameterization of Rotations
Parameterizations of the Orientation
The orientation (rotation) in space has 3 degrees of freedom
The rotation matrix has 9 elements it is redundant

Parameterization
◦ Objective: be less redundant
◦ Represent orientation (rotation) using less than 9 elements

Usual parameterizations:
◦ Euler angles (3 angles)
◦ Roll, pitch, yaw angles (3 angles)
◦ Axis/angle representation (3 elements)
 Euler Angles
They represent the orientation using 3 angles

The angles are originated from 3 successive rotations about the current (mobile)
axes

Possible combination of rotations:

XYZ XZY XZX XYX 12 in total
YXZ YZX YXY YZY
ZXY ZYZ ZXZ ZYX
◦ Note: there cannot be successive rotations about parallel axes

Most used Euler angles:

◦ ZYZ
◦ ZXZ
◦ ZYX (Fick angles)
◦ YZX (Helmholtz angles)
 Euler Angles
Euler Angles ZYZ
Originated from the following sequence of rotations:
1. A rotation of ϕ1 about axis z
2. A rotation of the new frame an angle ϕ2 about axis y’ (current axis)
3. A rotation of the new frame an angle ϕ3 about axis z’’ (current axis)
z
z' z' z '' yf
𝜙1 y' zf
𝜙 3
y'
y y ''

x x' x '' xf
x'

Equivalent rotation matrix:

Rotation
matrix from
ZYZ Euler
angles
 Euler Angles
Euler Angles ZYZ
ZYZ Euler angles from rotation matrix

◦ Given a matrix find the ZYZ Euler angles

◦ Using the previous matrix:

X X
X X

◦ Comparing and solving:

+ if ϕ2 from 0 to π
– if ϕ2 from -π to 0

sin ϕ2 ≠ 0

Note: always 2 different

solutions
 Euler Angles
Euler Angles ZYZ
Singularities (of Euler Angles ZYZ)
◦ They occur when sin ϕ2= 0; that is when:
ϕ2= 0, π, 2π, ...
◦ When ϕ2= 0 (sin ϕ2= 0 and cos ϕ2= 1):

Only ϕ1 + ϕ3 can be found

◦ When ϕ2= π (sin ϕ2= 0 and cos ϕ2= -1):

Only ϕ3 - ϕ1 can be found

 Euler Angles
Euler Angles ZYZ
Examples
a) Find the rotation matrix equivalent to the ZYZ Euler angles (30°, 50°, 90°)

[ ]
− 0 . 5 −0 . 557 0 . 663
𝑅=𝑅 𝑧 ( 30 ° ) 𝑅 𝑦 ( 50 ° ) 𝑅 𝑧 ( 90 ° ) = 0 . 866 −0 . 321 0 . 383
0 0 . 766 0 . 643
b) Find the rotation matrix equivalent to the ZYZ Euler angles (-150°, -50°, -90°)

[ ]
− 0 .5 − 0 .557 0 .663
𝑅=𝑅 𝑧 (−150 °) 𝑅 𝑦 (−50 °) 𝑅 𝑧 (−90 °)= 0 . 866 − 0 . 321 0 .383
0 0 .766 0 .643
c) Given the previous matrix, find the possible ZYZ Euler angles
 Roll, Pitch, Yaw Angles
Originated from the following sequence of rotations:
1. A rotation of ϕy about the fixed x axis (yaw)
2. A rotation of ϕp about the fixed y axis (pitch)
3. A rotation of ϕr about the fixed z axis (roll)
z
r
p
y y
x

• Equivalent rotation matrix: https://www.grc.nasa.gov/www/k-12/airplane/Images/

rotations.gif

Rotation
matrix from
roll, pitch,
yaw angles
 Roll, Pitch, Yaw Angles
RPY angles from rotation matrix

◦ Given a matrix find the RPY angles

◦ Using the previous matrix:

X X
X X

◦ Comparing and solving:

+ if ϕp from - π/2 to π/2
– if ϕp from π/2 to 3π/2

cos ϕp ≠ 0

Note: always 2 different

solutions
 Roll, Pitch, Yaw Angles
Singularities (of the RPY representation):
◦ They appear when cos ϕp= 0; that is when:

ϕp= -π/2, π/2

◦ When ϕp= π/2 (cos ϕp= 0 and sin ϕp= 1):

Only ϕr - ϕy can be found

◦ When ϕp= -π/2 (cos ϕp= 0 and sin ϕp= -1):

Only ϕr + ϕy can be found

 Roll, Pitch, Yaw Angles
Example
Given the following rotation matrix, find the two equivalent sets of Roll, Pitch, Yaw
angles
  0.5  0.557 0.663
R  0.866  0.321 0.383
 0 0.766 0.643

Solution

 p atan2(0, 0.7662  0.6432 ) 0  p atan2(0,  0.7662  0.6432 ) 180

 0.866  0.5   0.866  0.5 

r atan2  ,  120 r atan2  ,   60
 1 1    1  1 

 0.766 0.643   0.766 0.643 

 y atan2  ,  50  y atan2  ,   130
 1 1    1  1 
 Axis/Angle Representation
Rotation about an Arbitrary Axis

__ ^ ^ ^
k k x i  k y j  k z k

sin  
k y
2 2

k x k y
cos   k x
2 2

k x k y
2 2
sin   k x k y
cos  k z

September 2019 MCT UET FSD 36

 Axis/Angle Representation
Rotation about an Arbitrary Axis
◦ Rotation of vector k “–α” degrees about ‘Z0’ axis
◦ Rotation of vector k “–β” degrees about ‘Y0’ axis
◦ Rotation of object “θ” degrees about ‘k’ –axis
i.e. about ‘Z0’ –axis
◦ Rotation of vector k “β” degrees about ‘Y0’ axis
◦ Rotation of vector k “α ” degrees about ‘Z0’ axis

𝑅 𝑘, 𝜃 =¿R z , R y , R z k , R y ,   R z ,  
0 0 0 0 0
 Axis/angle Representation
sin  
k y
2 2

kx k y
cos   k x
2 2

kx k y
2 2
sin   k x k y
cos  k z

𝑘=(𝑘𝑥 , 𝑘 𝑦 ,𝑘 𝑧 )

Angle: Axis:

Always use
atan2
Axis/angle Representation
Example
Compute the axis and angle equivalent to the following rotation matrix
 0.926  0.304 0.226 
R  0.363 0.881  0.304 
  0.107 0.363 0.926 

Solution
  (0.363  0.304) 2  (  0.107  0.226) 2  (0.363  0.304) 2 0.926  0.881  0.926  1 
 atan2  , 
 2 2 
 
 1 1.733 
 atan2   , 
 2 2 

- Using the positive root: - Using the negative root:

 30   30
 0.363  0.304   0.667   0.363  0.304    0.667 
1  0.226  0.107   0.333  1  0.226  0.107    0.333 
ω ω
2sin 30     2sin  30     
 0.363  0.304   0.667   0.363  0.304    0.667 
 Rigid Motion
Homogeneous Transformations
 Homogeneous Transformations
P zB
B
P p yB
zA
A
p
A
tB
xB
yA A
t B : Origin of frame B
with respect to
xA frame A
A
Point P of frame {B} in frame {A}: p  At B  A RB B p
In a more compact way:
 A p   A RB A
tB   B p  A
p
     Homogeneo

 1   0 1  1  B
p us
coordinates
A
p A
TB
B
p
Homogeneous
transformation
matrix
 Homogeneous Transformations
They represent the position and orientation (pose) of a frame with respect to
another frame
Parts Rotation
(orientation)
 r11 r12 r13 t x 
r r22 r23 t y   R t  translation

T  21
(position)
 
 r31 r32 r33 t z   0 1
 
0 0 0 1

(SE = Special Euclidean)

Mathematically, they belong to SE(3):
◦ They can also be viewed from the perspective of projective geometry

 R 0  I t
Pure transformations T   T  
 0 1   0 1
Pure rotation Pure translation
 Homogeneous Transformations
Pure Transformations
Pure rotations: Pure translations:
1 0 0 0 1 0 0 d
 0 cos   sin  0  0 1 0 0 
Rot x ( )  Transx (d ) 
 0 sin  cos  0 0 0 1 0
   
0 0 0 1 0 0 0 1

 cos  0 sin  0 1 0 0 0
 0 1 0 0  0 1 0 d 
Rot y ( )  Trans y (d ) 
  sin  0 cos  0 0 0 1 0
   
 0 0 0 1 0 0 0 1

 cos   sin  0 0 1 0 0 0
 sin  cos  0 0  0 1 0 0 
Rot z ( )  Transz (d ) 
 0 0 1 0 0 0 1 d
   
 0 0 0 1 0 0 0 1
 Homogeneous Transformations
Composition
1. When a transformation is applied with respect to the fixed frame:
- A pre-multiplication is used

2. When a transformation is applied with respect to the mobile (current, new)

frame
- A post-multiplication is used
Example
A frame {A} is rotated 90° about x, and then it is translated a vector (6, -2, 10) with respect
to the fixed (initial) frame. Find the homogeneous transformation that describes {B} with
respect to {A}
A
TB Trans (6,  2,10) Rot x (90 )

1 0 0 6  1 0 0 0  1 0 0 6
0 1 0  2   0 cos 90  sin 90 0   0 0  1  2 
 
0 0 1 10   0 sin 90 cos 90 0  0 1 0 10 
    
0 0 0 1  0 0 0 1  0 0 0 1
 Homogeneous Transformations
Composition
Example
Find the homogeneous transformation matrix that represents a rotation of an angle α
about the x axis, followed by a translation of b units along the new x axis, followed by
a translation of d units along the new z axis, followed by a rotation of an angle θ about
the new z axis

Solution
A
TB Rot x ( )Transx (b)Trans z (d ) Rot z ( )

 cos   sin  0 b 
 cos  sin  cos  cos   sin   d sin  

 sin  sin  sin  cos  cos  d cos  
 
 0 0 0 1 
 Homogeneous Transformations
Composition
Decomposition in pure transformations
◦ Any homogeneous transformation can be decomposed in 2 components:

 R t  I t   R 0
T      0 1
 0 1  0 1  
translatio rotation
nt R

Interpretations of the (de)composition of T

◦ Interpretation 1:
1. First, it applies a translation of t units
2. Then, it applies a rotation R with respect to the new frame

◦ Interpretation 2:
1. First, it applies a rotation R
2. Then, it applies a translation of t units with respect to the fixed (initial) frame
 Homogeneous Transformations
Uses
1. They represent the pose (position+orientation) of a frame (rigid body) with
respect to another frame

2. They change the reference frame in which a point is represented (using a

linear relation):

A
p ATB B
p

◦ Note: the point must be represented using homogeneous coordinates (its notation
uses ~)

3. They apply a transformation (rotation + translation) to a point in the same

reference frame
 Homogeneous Transformations
Uses
• Example 1
Consider frame {A} and {B}. Point P in frame {B} is given by (2, 0, 1),
find its coordinates with respect to frame {A} using a homogeneous
transformation matrix
P zB

zA xB

4
yB

6
yA
xA
Solution
0 1 0  4  2   4
  4
 p   RB
A A A
tB   p 
B
 A p    1 0 0 6   0   4 
         A
p  4 
 1   0 1  1   1 0 0 1 0   1  1 
      1 
0 0 0 1   1  1 
 Homogeneous Transformations
Uses
Example 2
A frame {A} is rotated 90° about x, and then it is translated a vector (6, -2, 10) with
respect to the fixed (initial) frame. Consider a point p = (-5, 2, -12) with respect to the
new frame {B}. Determine the coordinates of that point with respect to the initial
frame
Solution
- Homogeneous transformation
1 0 0 6  1 0 0 0  1 0 0 6
A
0 1 0  2   0 cos 90  sin 90 0   0 0  1  2 
TB Trans (6,  2,10) Rot x (90 )  
0 0 1 10   0 sin 90 cos 90 0  0 1 0 10 
    
0 0 0 1  0 0 0 1  0 0 0 1

- Point after transformation:

1 0 0 6   5   1
0 0  1  2   2   10 
A
p ATB B p  
0 1 0 10  12  12 
  
    
0 0 0 1 1  1
 Homogeneous Transformations
Example 3 Uses

A frame {A} is translated a vector (6, -2, 10) and then it is rotated 90° about axis x of
the fixed (initial) frame. Consider a point with coordinates (-5, 2, -12) with respect to
the new frame ({B}). Find the coordinates of that point with respect to the initial
frame

Solution
- Homogeneous transformation
1 0 0 0  1 0 0 6  1 0 0 6 
 0 cos 90  sin 90 0   0 1 0  2   0 0  1  10 
A
TB Rot x (90 )Trans (6,  2,10)  0 sin 90 cos 90 0  0 0

1 10   0 1 0  2 
    
0 0 0 1  0 0 0 1  0 0 0 1 

- Transformed point:
1 0 0 6    5   1
0 0  1  10   2   2 
A
p TB p 
A B

0 1 0  2    12   0 
    
0 0 0 1   1   1
 Homogeneous Transformations
Properties
Inverse of a homogeneous transformation:

 R t  R T
 RT t 
T   T  1  
 0 1  0 1 

◦ Why?
Solving for BP B
A A A
p  t B  RB pB
p  A RBT A p  A RBT At B

Product of homogeneous transformations:

 R1 t1   R2 t2   R1 R2 R1t 2  t1 
T1   T2  T1T2 
 0 1  0 1   0 1 
It is not commutative
 Homogeneous Transformations
Example:
Consider that the transformations of the belt and of the robot base with respect to a
reference frame {W} are known. The transformation of the object with respect to the
belt, as well as the transformation of the end effector with respect to the robot base
are also known.
a) Find the pose of the object with respect to the base of the robot
b) Find the pose of the object with respect to the end effector
End
effector
{E}

object
{O}

{F}
belt
{B}
base
{W}
world
 Homogeneous Transformations
Example:
Consider that the transformations of the belt and of the robot base with respect to a
reference frame {W} are known. The transformation of the object with respect to the
belt, as well as the transformation of the end effector with respect to the robot base
are also known.
a) Find the pose of the object with respect to the base of the robot
b) Find the pose of the object with respect to the end effector
Solution
W
- Known transformations: TF , W TB , FTO , BTE
a) Desired pose (in terms of the known transformations): BTO
B
TO  BTW  W TO   W TB 1   T  T 
W
F
F
O

b) Desired pose (in terms of the known transformations): ETO

TO  ETW  W TO   W TE 1  W TF  FTO      T  T 

1
TB  BTE 
E W W F
F O

 TE   TB   TF  FTO 
B 1 W 1 W
 Homogeneous Transformations
Example:
The figure shows a robot whose base is 1m away
from the base of the table. The table is 1m height and
its surface is a square. Frame {1} is fixed on a corner
of the table. A 20 cm cube is located on the middle of
the table, and it has frame {2} attached to its center.
A camera is located 2m above the table, just over the
cube, and it has frame {3} attached to it.

a) Find the homogeneous transformations that

relate each of these frames with the base system
{0}

b) Find the homogeneous transformation that

relates the cube frame {2} with respect to the
camera frame {3}
 Homogeneous Transformations
Example:
Solution
a) By inspection, the homogeneous transformations that relate each of the frames with respect
to the base frame {0} are:
1 0 0 0 1 0 0  0.5 0 1 0 0.5
0 1 0 1  0 1 0 1.5  1 0 0 1.5 
0
T1  0
T2  0
T3 
0 0 1 1 0 0 1 1  0 0 1 3 
     
0 0 0 1 0 0 0 1  0 0 0 1 

T2  3T0 0T2  0T3 1 0T2

3

b) Using the composition of transformations:

0 1 0  1.5  1 0 0  0.5  0 1 0 0
1 0 0 0.5   0 1 0 1.5   1 0 0 0 
 
0 0  1 3  0 0 1 1  0 0  1 2
    
0 0 0 1  0 0 0 1  0 0 0 1
Kinematics of Robot Manipulators

{n}

qi+1

Relation between joints (qi)

and the pose
(position/orientation) of some
point (e.g.: frame {n})
qi
{0}
 Kinematics of Robot Manipulators
Forward and Inverse formulations
x  f (q)

Forward
Joint kinematics Operation
space al space
q (q1 , , qn ) x ( x, y, z,  ,  , )
Inverse
kinematics

q  f  1 ( x)

◦ Forward kinematics
Given a joint configuration, find the pose (position/orientation) of some part of the
robot (e.g. end effector)
◦ Inverse Kinematics
Find the joint configuration that achieves a certain pose (position/orientation) of
some part of the robot
 Kinematics of Robot Manipulators
Forward and Inverse formulations

T=?

, q ,=?
, q5 6

, q , q 3, q 4
q1 2

What values must q have to

Given q, where is the end
achieve the shown pose of
effector?
the end effector?

Forward kinematics Inverse kinematics

 Forward Kinematics
Example
6 dof (degrees-of-freedom) robot

Forward kinematics: position and

orientation of the end effector

From: Springer Handbook of Robotics,

2nd Ed. (2016)
 Forward Kinematics
Example (robot R-R): given q1, q2 , where is the end effector?

ŷ y {e} x - Position of the end effector

y 
x l1 cos(q1 )  l2 cos(q1  q2 )
l2
y l1 sin(q1 )  l2 sin(q1  q2 )
l1 q2
- Orientation of the end effector
{0} q1  q1  q2
x x̂
 x  l1 cos(q1 )  l2 cos(q1  q2 ) 
Position and orientation:x  y  x  l1 sin(q1 )  l2 sin(q1  q2 ) 
About     q1  q2 
axis z

 cos( q1  q2 )  sin( q1  q2 ) 0 l1 cos( q1 )  l2 cos(q1  q2 ) 

 sin(q  q ) cos( q  q ) 0 l1 sin( q1 )  l2 sin(q1  q2 ) 
As a homogeneous 0
Te  1 2 1 2
transformation  0 0 1 0 
matrix  
 0 0 0 1 

Note: the reference frame of the end effector was arbitrarily assigned
 Forward Kinematics
Example (robot R-R): given q1, q2 , where is the end effector?

ŷ y {e} x - Position of the end effector

y 
x l1 cos(q1 )  l2 cos(q1  q2 )
l2
y l1 sin(q1 )  l2 sin(q1  q2 )
l1 q2
- Orientation of the end effector
{0} q1  q1  q2
x x̂
 x  l1 cos(q1 )  l2 cos(q1  q2 ) 
x  y 
Position and orientation: x  l1 sin(q1 )  l2 sin(q1  q2 ) 
    q1  q2 

 q1 
Joint vector: q   x  f (q)
 q2 
Forward Kinematics
For an Open Chain
1 {2}
{1}
T2
{n  1}
…
0
T1 n 1
Tn
{0}

Base Efector
final {n}

Homogeneous transformation of every frame with respect to the

previous one:
Previous frame i 1 Every T (usually) depends of a
Ti Current frame single joint (variable)

Forward kinematics: 0frame

Tn  0 T{n}
1 T2    
1with respect
... n 2
Tn 1
ton  1frame
Tn  
{0}Pose of the end effector
with respect to the robot
base
Forward Kinematics
Frame for the End Effector
(almost) By convention:

o
[y] n {e}: end effector frame
[x]

{e} o sometimes called

s (sliding plane of
jaws)
a [z]
- a: approach vector (aligned with the roll axis and pointing
outwards)
- o: orientation vector (in the direction of motion of the gripper jaws)
- n: normal vector (orthogonal to the plane defined by o and a)

Homogeneous transformation matrix:

 nx ox a x px 
n oy ay p y 
0
Te  y
Pose of the end effector with  nz oz az pz  (px, py, pz): position of
respect to the robot base  
0 0 0 1  {e} with respect to
the base
Denavit-Hartenberg Convention
Introduction
Notation: Denavit - Hartenberg = DH
It describes forward kinematics using 4 parameter for each
joint: θi, di, ai, αi
Example: y3 x3
z3
x2 Artic. i di θi ai αi
y5 x4 1 450 180+q1 -150 90
z5 y4 y2 z2
x5 z4 2 0 90+q2 600 0
3 0 180+q3 -200 90
z6 y6
4 640 180+q4 0 90
x6
5 0 180+q5 0 90
x1 z1
6 0 q6 0 0
y1 z0

Fanuc M-10iA x0 y0
Denavit-Hartenberg Convention
Introduction

It is a systematic (and classical) method used to describe forward

kinematics of manipulators

Procedure
1. Determine 1 frame per joint (based on some rules)
2. Determine 4 parameters (θi, di, ai, αi) that describe the pose between
every two reference frames (based on rules)
3. Using the 4 parameters (per joint) compute the homogeneous
transformation matrices
→ Determine the pose of the end effector with respect to the base (product of the
homogeneous transformation matrices)

There are 2 conventions

- Standard DH: it is the most used convention (we will use it)
- Modified DH: less common (introduced by J.J. Craig*)
*J.J. Craig. Introduction to Robotics: Mechanics and Control. 1st edition (1986)
Denavit-Hartenberg Convention
(1) Assigning a Reference Frame

Number joint axes: 1 to n (from the base to the end effector) showing the
axis of motion for each joint

Joint 2 Joint
(q2) Joint
Link i- i+1 (qi+1)
1 n (qn)
1 ki
il nk Lin
Joint 1

Lin
Joint

k
(q1)

n
i-1 (q
i )
-1

Joint
Link 0 i (qi) End
(base) effector
Denavit-Hartenberg Convention
(1) Assigning a Reference Frame

Number joint axes: 1 to n (from the base to the end effector) showing the
axis of motion for each joint

Base reference frame: assign frame {0} to the base, with axis z0 along
the axis of motion of joint 1 (arbitrary origin)
→ Axis x0 is arbitrary, and axis y0 completes the frame (right-hand rule)

q3
z0 q2
q1 y0

x0
Denavit-Hartenberg Convention
(1) Assigning a Reference Frame

Axis zi: align zi with the axis of motion of joint i+1

Origin of frame {i}:

a) At the intersection of zi & zi-1, or

b) At the intersection of zi with the common normal between zi & zi-1

→ If zi & zi-1 are parallel, arbitrarily chose any normal
q3
z0 q2 z2
q1 y0 z1

x0
Denavit-Hartenberg Convention
(1) Assigning a Reference Frame

Axis xi: assign xi in the direction of zi-1 x zi. If (zi-1 & zi) are parallel, assign xi
along the common normal between zi-1 & zi

Axis yi: assign yi to complete the frame (following the right hand rule)

q3
z0 q2
q1 y0 y2
z1 y1
z2
x0 x1 x2

Note: actually, xi can be assigned in the direction of ±(zi-1 x zi).

Denavit-Hartenberg Convention
(1) Assigning a Reference Frame

End effector frame {n}:

- xn must be orthogonal to zn-1 and it must intersect it (the origin of the frame is
usually at the end of the kinematic chain)
- Usually zn goes in the same direction as zn-1 pointing outwards
- yn completes the frame (right hand rule)

q3 z3
z0 q2 x3
q1 y0 y2
z1 y1
z2 y3

x0 x1 x2 n=3
 Denavit-Hartenberg Convention
(2) Assigning the DH Parameters

Joint Parameters

Joint angle (θi): rotation angle from axis xi-1 to axis xi about axis zi-1
→ It is the joint variable if the i-th joint is revolute

Joint displacement (di): distance from the origin of frame {i-1} to the
intersection of axis zi-1 to axis xi along axis zi-1
→ It is the joint variable if the i-th joint is prismatic

Link Parameters (constants)

Link length (ai): distance from the intersection of axis zi-1 and axis xi to
the origin of frame {i} along axis xi

Link rotation angle (αi): rotation angle from axis zi-1 to axis zi about
axis xi
Denavit-Hartenberg Convention
(2) Assigning the DH Parameters
joint joint
qi-1 qi joint
qi+1

zi
di zi-1
xi-1 xi
θi joint between
links

Joint parameters

• di: distance from the origin of {i-1} to the [intersection of zi-1 with xi] along zi-1
• θi: rotation angle from xi-1 to xi about zi-1
Note: di, θi have sign (they can be + or -)
Denavit-Hartenberg Convention
(2) Assigning the DH Parameters
joint joint
qi-1 qi joint
qi+1

Angle between
αi joint axis
zi
ai
zi-1
Displacement xi
xi-1 between
joint axis

Link parameters
• ai: distance from [the intersection of zi-1 with xi] to the origin of {i} along xi
• αi: angle from zi-1 to zi about xi
Note: ai, αi have sign (they can be + or -)
Denavit-Hartenberg Convention
(2) Assigning the DH Parameters
joint joint
qi-1 qi joint
qi+1

αi
zi
ai
di zi-1
xi-1 xi
θi

Summary
• di: distance from the origin of {i-1} to the [intersection of zi-1 with xi]
along zi-1
• θi: rotation angle from xi-1 to xi about zi-1
• ai: distance from [the intersection of zi-1 with xi] to the origin of {i}
along xi
 Denavit-Hartenberg Convention
(2) Assigning the DH Parameters

Example
q3 z3
z0 q2 x3
q1 y0 y2
z1 y1
y3 l3
z2
x0 x1 x2
l1 l2
Joint i di θi ai αi
1 0 q1 l1 0
2 0 180+q2 -l2 90
3 l3 180+q3 0 0

di: distance from the origin of {i-1} to [the intersection of zi-1 to xi] along
zi-1
θi: rotation angle from xi-1 to xi about zi-1
ai: distance from [the intersection of zi-1 with xi] to the origin of {i} along
xi
 Denavit-Hartenberg Convention
(2) Assigning the DH Parameters

Example
q3 z3
z0 q2 x3
q1 y0 y2
z1 y1
y3 l3
z2
x0 x1 x2
l1 l2

Note:
Joint i di θi ai αi Joint i di θi ai αi
Home
1 0 q1 l1 0 Sometimes 1 0 q1 l1 0 0
2 0 180+q2 -l2 90 written as
2 0 q2 -l2 90 180
3 l3 180+q3 0 0
3 l3 q3 0 0 180
Sometimes the “home” is omitted
 Denavit-Hartenberg Convention
(3) Homogeneous Transformation
Objective: take frame {i-1} to frame {i}
1. First take {i-1} to {G} qi+1
qi
zg αi
{G} xg zi
ai
di zi-1
xi-1 xi
{i}
θi
{i-1}

- Rotate an angle θi about zi-1

- Translate a distance di along zi-1
 cos i  sin i 0 0
 sin  cos i 0 0  It varies
i 1
TG (i , di ) Rot zi 1 (i ) Trzi 1 (di )  i
according to the
 0 0 1 di 
  joint value (θi, di)
 0 0 0 1
Note: if first there is a translation and then a rotation, the result is the same (Why?)