Foundations of Robotics: 2018-I

- Topic 3 -
Forward Kinematics of Robot
Manipulators
Prof. Oscar E. Ramos, Ph.D.

- Week 3 -
 Objectives

• Understand basic concepts of forward kinematics

• Kinematically model robot manipulators using the

Denavit-Hartenberg parameters

• Kinematically model robot manipulators using arbitrary

homogeneous transformation matrices

2
 Outline

1. Introduction

2. Geometric method

3. Denavit-Hartenberg (DH) convention

4. Examples of Denavit-Hartenberg

- Forward Kinematics of Robot Manipulators -

Prof. Oscar E. Ramos, Ph.D. 3
 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}

4
 Kinematics of Robot Manipulators

• Forward and Inverse formulations

x  f (q)

Forward
Joint kinematics Operational
space 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
5
 Kinematics of Robot Manipulators

• Forward and Inverse formulations

T=?

What values must q have to

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

Forward kinematics Inverse kinematics

6
 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) 7

 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    
axis z
q1  q2

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 matrix  0 0 1 0 
 
 0 0 0 1 
Note: the reference frame of the end effector was arbitrarily assigned 8
 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 ) 
   q1  q2 

 q1 
Joint vector: q  x  f (q)
 q2 

9
 Forward Kinematics
• In a generic way: Joint configuration

x  f (q)

Position/orientation of some point* (e.g. end effector)

- f is different for different operational x3

points* x
- f is a (highly) nonlinear function
- x is usually with respect to the base

• f is usually represented using a

homogeneous transformation x1 x2
- It contains position and orientation
Baxter Robot

* Points x are usually called: operational points 10

 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: frame {n} with respect to frame {0}

Pose of the end effector with
0
Tn   0 T1  1T2  ...  n  2 Tn 1  n 1Tn  respect to the robot base

11
 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 ax px 
n oy ay p y  (px, py, pz): position of {e}
Pose of the end effector with
respect to the robot base
0
Te   y with respect to the base
 nz oz az pz 
 
0 0 0 1
12
 Forward Kinematics
Methods

• In general:
- Assign reference frames to links (not necessarily to every link)
- Relate frame {i} to frame {i-1}
- Apply compositions of transformations to obtain the end effector frame with
respect to the base

13
 Forward Kinematics
Methods

• Geometric Methods
- They assign arbitrary reference frames (by inspection)
- The transformation matrices (or screws) from frame to frame are different
(there is no general transformation matrix)
- Examples:
• Generic geometric methods
• Product of exponentials (PoE)

• Systematic Methods:
- They assign reference frames based on rules
- The relation between frames is given by some parameters (based on rules)
- Generic (similar) transformations are obtained between frames
- Examples:
• Denavit-Hartenberg (DH)
• Hayati-Roberts
• Khalil-Kleinfinger, etc.
14
 Outline

1. Introduction

2. Geometric method

3. Denavit-Hartenberg (DH) convention

4. Examples of Denavit-Hartenberg

- Forward Kinematics of Robot Manipulators -

Prof. Oscar E. Ramos, Ph.D. 15
 Geometric Method
Introduction

• Main Characteristic:
- It arbitrarily assigns reference frames

• Procedure:
1. Assign arbitrary frames to each link (they must move with the link) → The
motion is usually about the z axis or along it

2. Describe each frame with respect to the previous one (using homogeneous
transformation matrices)
3. Multiply the partial descriptions (of the kinematic chain) to obtain the end
effector with respect to the base frame

• Note:
- At the end a “standard” frame can be added to the end effector
- Steps 1 and 2 can be done link by link (first for link 1, then for link 2,…)

16
 Forward Kinematics: Geometric Method
2D Example: R-R Robot

ŷ y {e} x
y  Procedure:
l2 1. Assign frames (that move with each
link)
{1} q2 2. Relate a frame to the previous
l1
one (0T1 and 1Te)
{0} q1
x x̂

• 0T1: system {1} with respect to {0} = “take” {0} to {1}

- Rotate q1 about z: Rotz(q1) 0
T1  Rot z (q1 )Trx (l1 )
- Translate l1 along the resulting x (current frame): Trx(l1)

cos(q1 )  sin(q1 ) 0 0  1 0 0 l1  cos(q1 )  sin(q1 ) 0 l1 cos(q1 ) 

 sin(q ) cos(q ) 0 0  0 1 0 0   sin(q1 ) cos(q1 ) 0 l1 sin(q1 ) 
0
T1   1 1

 0 0 1 0  0 0 1 0  0 0 1 0 
    
 0 0 0 1  0 0 0 1  0 0 0 1 
17
 Forward Kinematics: Geometric Method
2D Example: R-R Robot

ŷ y {e} x
y  Procedure:
l2 1. Assign frames (that move with each
link)
{1} q2 2. Relate a frame to the previous
l1
one (0T1 and 1Te)
{0} q1
x x̂

• 1Te: system {e} with respect to {1} = “take” {1} to {e}

- Rotate q2 about z: Rotz(q2) 1
Te  Rot z (q2 )Trx (l2 )
- Translate l2 along the resulting x (current frame): Trx(l2)

cos(q2 )  sin(q2 ) 0 0  1 0 0 l2  cos( q2 )  sin(q2 ) 0 l2 cos(q2 ) 

 sin(q ) cos(q ) 0 0  0 1 0 0   sin(q2 ) cos(q2 ) 0 l2 sin(q2 ) 
1
Te   2 2

 0 0 1 0 0 0 1 0  0 0 1 0 
    
 0 0 0 1  0 0 0 1  0 0 0 1 
18
 Forward Kinematics: Geometric Method
2D Example: R-R Robot

Procedure:
3. Multipliy to obtain the final transformation matrix 0Te
0
Te   0 T1  1Te 
cos(q1 )  sin(q1 ) 0 l1 cos(q1 )  cos(q2 )  sin(q2 ) 0 l2 cos(q2 ) 
 sin(q ) cos(q ) 0 l1 sin(q1 )   sin(q2 ) cos(q2 ) 0 l2 sin(q2 ) 
0
Te   1 1

 0 0 1 0  0 0 1 0 
  
 0 0 0 1  0 0 0 1 
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 ) 
0
Te   1 2 1 2

 0 0 1 0 
 
 0 0 0 1 

Position and orientation of the end

effector with respect to the base frame
19
 Forward Kinematics: Geometric Method
3D Example: SCARA Robot

Adept’s SCARA Robot (cobra s800) ABB’s SCARA Robot (IRB910SC)

https://youtu.be/IRDJnwFDq88 https://youtu.be/97KX-j8Onu0

20
Forward Kinematics: Geometric Method
3D Example: SCARA Robot

Adept’s SCARA Robot Schematic model of a SCARA robot

21
 Forward Kinematics: Geometric Method
3D Example: SCARA Robot
1. Take {0} to {1}
q1 l3
- Translate {0} a distance l1 along z0. l2
- Rotate 180°+q1 about z0 to get to {1}
z1
y1 x1

0
T1  Trz (l1 ) Rot z (180  q1 )
l4
1 0 0 0  c1 s1 0 0
0 l1
1 0 0  s c1 0 0 
  1 z0
0 0 1 l1   0 0 1 0
   
0 0 0 1  0 0 0 1
y0
x0
 c1 s1 0 0 
s c1 0 0 
0
T1   1
 0 0 1 l1 
  Arbitrarily assign
 0 0 0 1 frames

Since translation and rotation are with respect to the

same axis, we have: Trz (l1 ) Rot z (q1 )  Rot z (q1 )Trz (l1 )
22
 Forward Kinematics: Geometric Method
3D Example: SCARA Robot
2. Take {1} to {2} q2
q1 l3
- Translate {1} a distance l2 along x1 l2 z2 y2
- Then rotate (-90°+q2) about the new z to get
to {2} z1
y1 x1 x2

1
T2  Trx (l2 ) Rot z (90  q2 )
l4
1 0 0 l2   s2 c2 0 0
l1
0 z0
 1 0 0   c s2 0 0 
 2
0 0 1 0  0 0 1 0
    y0
0 0 0 1  0 0 0 1 x0

 s2 c2 0 l2 
 c s2 0 0 
1
T2   2 Arbitrarily assign
 0 0 1 0
  frames
 0 0 0 1

In this case (as in general) the product is not commutative

23
 Forward Kinematics: Geometric Method
3D Example: SCARA Robot
3. Take {2} to {3} q2 q3
q1 l3 q4
Translate {2} a distance l3 along x2 to get to l2 z2
{3} y2

1 0 0 l3  z1 z3 y3
y1 x1 x2
0 1 0 0 
2
T3  Trx (l3 )  x3
0 0 1 0
 
0 0 0 1
z4 x4 l4
y4
4. Take {3} to {4} l1
z0
- Translate {3} a distance (-l4+q3) along z3
- Then, rotate (90°+q4) about z to get to {4} y0
x0

  s4 c4 0 0 
c Arbitrarily assign
3  s4 0 0 
T4  Trz (l4  q3 ) Rot z (90  q4 )  4 frames
 0 0 1 q3  l4 
 
 0 0 0 1 

24
 Forward Kinematics: Geometric Method
3D Example: SCARA Robot
5. Multiply: take {0} to {4} q2 q3
q1 l3 q4
Write the end effector {4} in terms of the l2 z2
base {0} → multiply the kinematic chain y2

z1 z3 y3
0
T4   T1  T2  T3  T4 
0 1 2 3
y1 x1 x2

x3

 c1 s1 0 0   s2 c2 0 l2   1 0 0 l3    s4 c4 0 0 
s c1 0 0   c2 0 0  0 1 0 0   c4 0  z4 x4
0
T4   1
s2  s4 0 l4
 0 0 1 l1   0 0 1 0  0 0 1 0  0 0 1 q3  l4  y4
     l1
 0 0 0 1  0 0 0 1  0 0 0 1  0 0 0 1  z0

 c124 s124 0 l3 s12  l2 c1 

x0
y0
s c124 0 l3c12  l2 s1 
0
T4   124
 0 0 1 l1  l4  q3 
 
 0 0 0 1 
Forward kinematics using arbitrary
reference frames

25
 Forward Kinematics: Geometric Method
3D Example: SCARA Robot
Optional: q2 q3
q1 l3 q4
The end effector convention can be used (axis n l2 z2 y2
= x, o = y, a = z) for frame {e}
z1 z3 y3
- Rotate {4} 180° about x4 y1 x1 x2

1 0 0 0  1 0 0 0 x3
 0 cos(180)  sin(180) 0   0 1 0 0 
4
Te    z4
 0 sin(180) cos(180) 0   0 0 1 0 x4 l4
    y4
0 0 0 1  0 0 0 1 l1
z0
- Forward kinematics:
y0 xe
x0
0
Te   T4  Te 
0 4
ye

ze
 c124  s124 0 l3 s12  l2 c1 
s c124 0 l3c12  l2 s1 
0
Te   124
 0 0 1 l1  l4  q3 
  Verify the result when the joint
 0 0 0 1  configuration is null (zeros)
26
 Forward Kinematics: Geometric Method
3D Example: SCARA Robot
Homogeneous Transformation Matrices using Python
Import sympy
from sympy.matrices import Matrix
matrices

def transl(x, y, z):

T = Matrix([[1,0,0,x],
Function for [0,1,0,y],
translation [0,0,1,z],
[0,0,0,1]])
return T

def trotx(ang):
T = Matrix([[1,0,0,0],
Function for [0,cos(ang),-sin(ang),0],
rotation about X [0,sin(ang),cos(ang),0],
[0,0,0,1]])
return T

def trotz(ang):
T = Matrix([[cos(ang),-sin(ang),0,0],
Function for [sin(ang), cos(ang),0,0],
rotation about Z [0,0,1,0],
[0,0,0,1]])
return T
27
 Forward Kinematics: Geometric Method
3D Example: SCARA Robot
Homogeneous Transformation Matrices using Python

Import sympy from sympy import *

q1, q2, q3, q4 = symbols("q1 q2 q3 q4")

l1, l2, l3, l4 = symbols("l1 l2 l3 l4")

Homogeneous T1 = transl(0,0,l1)*trotz(pi+q1)
transformations T2 = transl(l2,0,0)*trotz(-pi/2+q2)
and their products T3 = transl(l3,0,0)
T4 = transl(0,0,-l4+q3)*trotz(pi/2+q4)
T04 = simplify(T1*T2*T3*T4)
print(T04)

Using the Te = trotx(pi);

convention for the T0e = simplify(T04*Te);
end effector print(T0e)

When all joint

T0e.subs([(q1,0),(q2,0),(q3,0),(q4,0)])
values are zero

28
 Outline

1. Introduction

2. Geometric method

3. Denavit-Hartenberg (DH) convention

4. Examples of Denavit-Hartenberg

- Forward Kinematics of Robot Manipulators -

Prof. Oscar E. Ramos, Ph.D. 29
 Denavit-Hartenberg Convention
Introduction

• Notation: Denavit - Hartenberg = DH

• It describes forward kinematics using 4 parameter for each joint: θi, di,
a i, α i
• Example:
y3 x3
Artic. i di θi ai αi
z3 1 450 180+q1 -150 90
x2
2 0 90+q2 600 0
y5 x4
z5 y4 3 0 180+q3 -200 90
y2 z2
x5 z4
4 640 180+q4 0 90
5 0 180+q5 0 90
z6 y6
6 0 q6 0 0
x6
x1 z1
y1 z0
Fanuc M-10iA

x0 y0

30
 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) 31
 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 zi: align zi with the axis of motion of joint i+1

• Origin of frame {i}: find the origin of frame {i} at the intersection of zi & zi-1, or
at the intersection of zi with the common normal between zi & zi-1

• Axis xi: assign xi in the direction of zi-1 x zi. If they 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)

• End effector frame {n}: xn must be orthogonal to zn-1 and it must intersect it

32
 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
Joint
(q2) i+1 (qi+1)
n (qn)

Joint 1
(q1)

Joint
Link 0 i (qi) End
(base) effector

33
 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

34
 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

35
 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).

36
 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

37
 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

38
 Denavit-Hartenberg Convention
(2) Assigning the DH Parameters

joint joint
qi-1 qi joint
qi+1

zi
zi-1
xi-1 xi

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 -)

39
 Denavit-Hartenberg Convention
(2) Assigning the DH Parameters

joint joint
qi-1 qi joint
qi+1

Angle between
joint axis
zi
zi-1
xi-1 xi

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 -)

40
 Denavit-Hartenberg Convention
(2) Assigning the DH Parameters

joint joint
qi-1 qi joint
qi+1

zi
zi-1
xi-1 xi

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
• αi: angle from zi-1 to zi about xi

41
 Denavit-Hartenberg Convention
(2) Assigning the DH Parameters

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

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
αi: angle from zi-1 to zi about xi|
42
 Denavit-Hartenberg Convention
(2) Assigning the DH Parameters

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

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

43
 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
{G} xg zi
zi-1
xi-1 xi
{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 according
i 1
TG (i , d i )  Rot zi1 (i ) Trzi1 (di )   i
to the joint value
 0 0 1 di  (θi, di)
 
 0 0 0 1
Note: if first there is a translation and then a rotation, the result is the same (Why?) 44
 Denavit-Hartenberg Convention
(3) Homogeneous Transformation

Objective: take frame {i-1} to frame {i}

2. Then take {G} to {i}
qi+1
qi
zg
{G} xg zi
zi-1
xi-1 xi
{i}
{i-1}

- Translate a distance ai along xi

- Rotate an angle αi about xi

1 0 0 ai 
 0 cos   sin  i 0  Always
G
Ti ( i , ai )  Trxi (ai ) Rot xi ( i )   i
constant
 0 sin  i cos  i 0
 
0 0 0 1
Nota: if first there is a translation, and then a rotation, the result is the same 45
 Denavit-Hartenberg Convention
(3) Homogeneous Transformation

Objective: take frame {i-1} to frame {i}

i 1
Ti   i 1TG  G Ti 

 cos  i  sin  i 0 0  1 0 0 ai 
 sin  cos i 0 0  0 cos  i  sin  i 0 
i 1
Ti (i , di ,  i , ai )   i

 0 0 1 di  0 sin  i cos  i 0
  
 0 0 0 1  0 0 0 1

cos i  cos  i sin i sin  i sin  i ai cos  i 

 sin  cos  i cos  i  sin  i cos i ai sin i 
i 1
Ti ( i , d i ,  i , ai )   i

 0 sin  i cos  i di 
 
 0 0 0 1 

DH Homogeneous transformation matrix for the

adjacent reference frames {i-1} and {i}
46
 Denavit-Hartenberg Convention
(3) Homogeneous Transformation

Final result:
- End effector with respect to the base
- Using the product of homogeneous transformations

0
Tn   0 T1  1T2   n  2 Tn 1  n 1Tn 

47
 Denavit-Hartenberg Convention
(3) Homogeneous Transformation

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

cos q1  sin q1 0 l1 cos q1 

 sin q cos q1 0 l1 sin q1 
0
T1   1

 0 0 1 0 
 
 0 0 0 1 

 cos 180  q2  0 sin 180  q2  l2 cos 180  q2     cos q2 0  sin q2 l2 cos q2 
  
sin 180  q2  0  cos 180  q2  l2 sin 180  q2     sin q2 0 cos q2 l2 sin q2 
1
T2   
 0 1 0 0   0 1 0 0 
   
 0 0 0 1   0 0 0 1 

48
 Denavit-Hartenberg Convention
(3) Homogeneous Transformation

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

cos 180  q3   sin 180  q3  0 0    cos q3 sin q3 0 0

  
sin 180  q3  cos 180  q3  0 0    sin q3  cos q3 0 0 
2
T3   
 0 0 1 l3   0 0 1 l3 
   
 0 0 0 1  0 0 0 1

c12 c3 c12 s3  s12 l2 c12  l3 s12  l1c1 

c1 = cos(q1)
s c  s12 s3 c12 l3c12  l2 s12  l1s1  s1 = sin(q1)
0
T3   0 T1  1T2  2 T3    12 3
  s3 c3 0 0  c12 = cos(q1+q2)
  s12 = sin(q1+q2)
 0 0 0 1 

49
 Denavit-Hartenberg Convention
(3) Homogeneous Transformation

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

Initial position (shown in the figure):

- All joint variables are zero (q1 = 0, q2 = 0, q3 = 0)

1 0 0 l1   1 0 0 l2   1 0 0 0 1 0 0 l2  l1 
0 1 0 0  0 0 1 0   0 1 0 0  0 0 1 l3 
0
T1   1
T2   2
T3   0
T3  
0 0 1 0 0 1 0 0 0 0 1 l3  0 1 0 0 
       
0 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1 

Verify with the figure, giving “interpretations” to the homogeneous transformations

50
Denavit-Hartenberg Convention
Illustration

Note: the video uses r instead of a and no sub indices are added

https://youtu.be/rA9tm0gTln8

51
 Denavit-Hartenberg Convention
(Modified DH*)

• Introduced by J.J. Craig (Introduction to Robotics, Mechanics and Control)

• Main characteristics:
- Sometimes it creates confusion (when it is used without explicitly stating it)
- It uses axis zi at the i-th joint
- ai, αi: relate zi and zi+1 along xi
- di, θi: relate xi-1 to xi along axis zi

ci ci si si si ai ci 

  Frames at the
i 1  si ci ci  si ci ai si  end of the link
Standard: T  0 di 
i
si ci
 
 0 0 0 1 

 ci  si 0 ai 1 
 
i 1  c s ci1 ci  si1 di si1 
Modified: Ti   i1 i Frames at the
s s si1 ci ci1 di ci1  beginning of the
 i1 i 
 0 0 0 1  link

*Only for reference 52

 Outline

1. Introduction

2. Geometric method

3. Denavit-Hartenberg (DH) convention

4. Examples of Denavit-Hartenberg

- Forward Kinematics of Robot Manipulators -

Prof. Oscar E. Ramos, Ph.D. 53
 Example 1: DH of a SCARA Robot

1. Reference frames
q3
q2
q1 q4
l2 l3

• Number joint axis

• Base reference frame: z0 along the
axis of joint 1 (arbitrary origin,
arbitrary x0)
l4
l1
z0

y0
x0

54
 Example 1: DH of a SCARA Robot

1. Reference frames
q3
q2
q1 q4
l2 l3
z1 x1 • Axis zi: zi along the axis of joint i+1
y1
z2 y2 • Origin of frame {i}:
a) Intersection of zi & zi-1, or
x2 b) Intersection of zi with normal between zi & zi-1
(If zi & zi-1 parallel: arbitrary normal)
z3 l4
y3
l1 • Axis xi: in the direction of zi-1 x zi. If (zi-1 &
z0 zi) are parallel, xi along their common
x3
normal
y0
x0 • Axis yi: assign yi to complete the frame
(using the right hand rule)

55
 Example 1: DH of a SCARA Robot

1. Reference frames
q3
q2
q1 q4
l2 l3
z1 x1
y1
• End effector frame {n}:
z2 y2 - xn orthogonal to zn-1, intersecting it (origin at
the end of the chain)
x2
- zn in the direction of zn-1 pointing outwards
- yn completes the frame
z3 l4
y3
l1
z0 x3

y0 x4
x0
y4
z4

56
 Example 1: DH of a SCARA Robot

2. DH parameters
q3
q2
q1 q4
l2 l3 Joint i di θi ai αi
z1 x1 1 l1 180+q1 l2 0
y1
z2 2 0 -90+q2 l3 0
y2
3 -l4+q3 0 0 0
x2 4 0 90+q4 0 180

z3 l4
y3
l1
z0 x3 di: distance from {i-1} to [intersection of zi-1
with xi] along zi-1
y0 x4 θi: angle from xi-1 to xi alrededor de zi-1
x0
y4 ai: distance from [intersection of zi-1 wtih xi] to
z4 {i} along xi
αi: angle from zi-1 to zi about xi

57
 Example 1: DH of a SCARA Robot

3. Homogeneous Transformation Matrices

Joint i di θi ai αi
1 l1 180+q1 l2 0
2 0 -90+q2 l3 0
3 -l4+q3 0 0 0
4 0 90+q4 0 180

  cos q1 sin q1 0 l2 cos q1  1 0 0 0 

  sin q  cos q1 0 l2 sin q1  0 1 0 0 
0
T1 (q1 )   1 2
T3 ( q3 )  
 0 0 1 l1  0 0 1 q3  l4 
   
 0 0 0 1  0 0 0 1 

 sin q2 cos q2 0 l3 sin q2    sin q4 cos q4 0 0

  cos q sin q2 0 l3 cos q2   cos q sin q4 0 0 
1
T2 (q2 )   2 3
T4 ( q4 )   4
'
 0 0 1 0   0 0 1 0 
   
 0 0 0 1   0 0 0 1

58
 Example 1: DH of a SCARA Robot

3. Homogeneous Transformation Matrices

- End effector with respect to the base:

0
T4   0 T1  1T2  2 T3  3 T4  l2 l3

 c124  s124 0 l3 s12  l2 c1 

s c124 0 l3c12  l2 s1 
  124 '
 0 0 1 l1  l4  q3 
 
 0 0 0 1  l4
l1
z0
- For the initial configuration (q1 = q2 = q3 = q4 = 0):
y0 x4
x0
 1 0 0 l2  y4
0 1 0 l3  z4
0
T4   '
0 0 1 l1  l4  Robot in the initial configuration
 
0 0 0 1 

Verify by inspection, in the diagram Compare with the result obtained

using the geometric method
59
 Example 1: DH of a SCARA Robot

Homogeneous Transformation Matrices using Python (and Sympy)

Function: homogeneous
def dh(d,theta,a,alpha): # complete this
transformation from DH parameters

# Symbolic variables
q1, q2, q3, q4 = symbols("q1 q2 q3 q4")
l1, l2, l3, l4 = symbols("l1 l2 l3 l4")

Using symbolic variables to # Homogeneous transformations

compute each homogeneous T1 = dh(l1, pi+q1, l2, 0)
transformation T2 = dh(0,-pi/2+q2, l3, 0)
T3 = dh(-l4+q3, 0, 0, 0)
T4 = dh(0, pi/2+q4, 0, pi)

# Final homogeneous transformation

Tf = simplify(T1*T2*T3*T4)

For the configuration shown in the # Evaluation with specific values

figure Tf.subs([(q1,0),(q2,0),(q3,0),(q4,0)])

60
 Example 1: DH of a SCARA Robot

Note: Alternative representation of the DH parameters

Using a home position

Joint i di θi ai αi Joint i di θi ai αi Home

1 l1 180+q1 l2 0 1 l1 q1 l2 0 180°
2 0 -90+q2 l3 0 2 0 q2 l3 0 -90°
3 -l4+q3 0 0 0 3 q3 0 0 0 -l4
4 0 90+q4 0 180 4 0 q4 0 180 90°

The resulting homogeneous The resulting homogeneous

transformation contains the initial transformation does not contain the
configuration (shown in the figure) for initial configuration (shown in the
the zeroed joints figure) for the zeroed joints

Joints with the “home” value must

be used to obtain the configuration
shown in the figure
61
 Example 1: DH of a SCARA Robot

Verification using the Robotics Toolbox (of P. Corke) for MATLAB

matlabrc % To avoid problems with functions

addpath('path_to_robotics_toolbox/common')
addpath(‘path_to_robotics_toolbox/robot')

% Initial lengths
l1=1; l2=1; l3=1; l4=0.5;
It uses DH parameters
% Definition of the robot using DH (th,d,a,alfa,P/R)
without including the
L(1)=Link([0, l1, l2, 0, 0]);
initial configuration (the
L(2)=Link([0, 0, l3, 0, 0]);
joint variable q is set to 0)
L(3)=Link([0, 0, 0, 0, 1]);
L(4)=Link([0, 0, 0, pi, 0]); The value P/R means:
% Robot creation 0 = revolute joint
scara = SerialLink(L, 'name', 'scara'); 1 = prismatic joint

% Forward kinematics (example: home position)

scara.fkine([pi -pi/2 -l4 pi/2])

% Visualization (of the initial position)

scara.plot([pi -pi/2 -l4 pi/2],'workspace',...
[-2 2 -2 2 -1 2]);

http://petercorke.com/wordpress/toolboxes/robotics-toolbox 62
 Example 1: DH of a SCARA Robot

Verification using the Robotics Toolbox (of P. Corke) for MATLAB

l2 l3

l4
l1
z0

y0 x4
x0
y4
z4

63
 Example 2: DH of the Fanuc M-10iA Robot

Fanuc M-10iA robot

https://youtu.be/Vrxaz0_X3Qs

64
 Example 2: DH of the Fanuc M-10iA Robot

Lateral view Front view Isometric view

From: Datasheet M-10iA Series FANUC ROBOTICS 65

 Example 2: DH of the Fanuc M-10iA Robot
1. Reference frames

640

200

600

150

z0 z0 450

y0 x0
x0

- Number joint axis

- Reference frame of the base 66
 Example 2: DH of the Fanuc M-10iA Robot
1. Reference frames

z5 640
z3
z3
200
z5
z2
z4

600

150
z1

z0 z0 450

y0 x0
x0

- Axis zi
- Origin of frame {i} 67
 Example 2: DH of the Fanuc M-10iA Robot
1. Reference frames

z5 640
z3 x3 y 3 x5
x4
y4 z3 x3
x2
x4 x2 200
y5
z5 y4 y2
y2 z2
x5 z4

600

x1 150
z1 x1

y1 y1
z0 z0 450

y0 x0
x0

- Axis xi
- Axis yi 68
 Example 2: DH of the Fanuc M-10iA Robot
1. Reference frames

z5 640
z3 x3 y 3 x5
x4
y4 z3 x3
x2
x4 x2 200
y5 z6
z5 y4 y2
y2 z2 x6
x5 z4

600
z6 y6
x6 x1 150
z1 x1

y1 y1
z0 z0 450

y0 x0
x0

- Frame for the end effector

69
 Example 2: DH of the Fanuc M-10iA Robot
2. DH parameters
Joint i di θi ai αi Joint i di θi ai αi
1 450 180+q1 -150 90 4 640 180+q4 0 90
2 0 90+q2 600 0 5 0 180+q5 0 90
3 0 180+q3 -200 90 6 0 q6 0 0

640
z3 x3 y 3 z5
x4
x2 x5 y4 z3 x3
y5 x4 x2 200
z5 y4 z6
y2 z2 y2
x5 z4 x6

z6 y6 600
x6 x1
150
z1 x1
y1 z0 y1
z0 450

x0 y0 x0
70
 Example 2: DH of the Fanuc M-10iA Robot
3. Homogeneous transformation matrices
Joint i di θi ai αi Joint i di θi ai αi
1 450 180+q1 -150 90 4 640 180+q4 0 90
2 0 90+q2 600 0 5 0 180+q5 0 90
3 0 180+q3 -200 90 6 0 q6 0 0

  cos(q1 ) 0  sin(q1 ) 150 cos( q1 )    cos( q4 ) 0  sin(q4 ) 0 

  sin( q ) 0 cos( q1 ) 150sin(q1 )    sin(q ) 0 cos( q4 ) 0 
0
T1 ( q1 )   1 3
T4 ( q4 )   4

 0 1 0 450   0 1 0 640 
   
 0 0 0 1   0 0 0 1 

  sin(q2 )  cos(q2 ) 0 600sin(q2 )    cos( q5 ) 0  sin(q5 ) 0 

 cos( q )  sin(q ) 0 600 cos( q2 )    sin(q ) 0 cos( q5 ) 0 
1
T2 ( q2 )   2 2 4
T5 ( q5 )   5

 0 0 1 0   0 1 0 0
   
 0 0 0 1   0 0 0 1

  cos( q3 ) 0  sin( q3 ) 200 cos( q3 )  cos(q6 )  sin( q6 ) 0 0

  sin(q ) 0 cos(q 3 ) 200 sin( q3 )   sin( q ) cos(q ) 0 0 
2
T3 ( q3 )   3 5
T6 ( q6 )   6 6

 0 1 0 0   0 0 1 0
   
 0 0 0 1   0 0 0 1
71
 Example 2: DH of the Fanuc M-10iA Robot
3. Homogeneous transformation matrices

- End effector with respect to the base:

640
0
T6   0 T1  1T2  2 T3  3 T4  4 T5  5 T6 
z6 200
- For the initial configuration (zeroed joints): x6
q1  q2  q3  q4  q5  q6  0
600
0 0 1 790 
0 1 0 0  150
0
T6  
 1 0 0 1250 
  z0
0 0 0 1  450
It can be verified by inspection
(in the figure) x0

Robot in its initial configuration

72
 Example 2: DH of the Fanuc M-10iA Robot

Homogeneous transformation matrices using Python (and Sympy)

Function to get the homogeneous

def dh(d,theta,a,alpha): # complete this
transformation from DH parameters

# Symbolic variables
q1, q2, q3, q4, q5, q6 = symbols("q1 q2 q3
q4, q5, q6")
% Homogeneous transformations
Using symbolic variables to T1 = dh(450, pi+q1, -150, pi/2);
compute each homogeneous T2 = dh(0, q2+pi/2, 600, 0);
transformation T3 = dh(0, q3+pi, -200, pi/2);
T4 = dh(640, q4+pi, 0, pi/2);
T5 = dh(0, q5+pi, 0, pi/2);
T6 = dh(0, q6, 0, 0);
% Final homogeneous transformation
Tf = simplify(T1*T2*T3*T4*T5*T6)

% Evaluation with specific values

For the configuration shown in the Tf.subs([(q1,0),(q2,0),(q3,0),(q4,0),(q5,0),
figure (q6,0)])

73
 Example 2: DH of the Fanuc M-10iA Robot

Verification using the Robotics Toolbox (of P. Corke) for MATLAB

matlabrc % To avoid problems with functions

addpath('path_to_robotics_toolbox/common')
addpath('path_to_robotics_toolbox/robot')

% Definition of robot using DH (th,d,a,alfa)

L(1)=Link([0, 0.450, -0.150, pi/2]);
L(2)=Link([0, 0, 0.600, 0]);
L(3)=Link([0, 0, -0.200, pi/2]);
L(4)=Link([0, 0.640, 0, pi/2]);
L(5)=Link([0, 0, 0, pi/2]);
L(6)=Link([0, 0, 0, 0]);

% Creating the robot

fanuc = SerialLink(L, 'name', 'fanuc');

% Forward kinematics (example)

fanuc.fkine([pi pi/2 pi pi pi 0])

% Visualization
fanuc.plot([pi pi/2 pi pi pi 0]);

http://petercorke.com/wordpress/toolboxes/robotics-toolbox 74
 Example 2: DH of the Fanuc M-10iA Robot

Verification using the Robotics Toolbox (of P. Corke) for MATLAB

z6 y6
x6

z0

x0 y0

75
 References

• B. Siciliano, L. Sciavicco, L. Villani, y G. Oriolo. Robotics: modelling,

planning and control. Springer Science & Business Media, 2010
(Chapter 2.8-2.9)

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

Control. John Wiley & Sons, 2006 (Chapter 3.1-3.2)

76