CHAPTER 4

ROBOT KINEMATICS
Abd Razak Ahmad Ext 134
Electrical Engineering Section
UniKL-BMI
badri@bmi.unikl.edu.my

The Robot System

Kinematics

Hardware
Mechanical Design
Actuators

Dynamics
Control System
Task Planning

Software

Sensors

Manipulators

Cartesian: PPP

Robot Configuration:

Cylindrical: RPP

Spherical: RRP

Hand coordinate:
SCARA: RRP
Articulated: RRR

(Selective Compliance
Assembly Robot Arm)

n: normal vector; s: sliding vector;

a: approach vector, normal to the
tool mounting plate

Manipulators
Robot Specifications
Number of Axes
Major axes, (1-3) => Position the
wrist
Minor axes, (4-6) => Orient the tool
Redundant, (7-n) => reaching
around obstacles, avoiding
undesirable configuration
Degree of Freedom (DOF)
Workspace
Payload (load capacity)
Precision v.s. Repeatability
Which one is more important?

An Example - The PUMA 560

2
3

6
5

There are two more

joints on the end
effector (the
gripper)

1
The PUMA 560 has SIX revolute joints
ONE revolute joint has ONE degree of freedom ( 1 DOF) that is
defined by its angle

Kinematic Considerations
Using kinematics to describe the spatial
configuration of a robot gives us two
approaches:
Forward Kinematics. (direct)
Given the joint angles for the robot, what is the
orientation and position of the end effector?

Inverse Kinematics.
Given a desired end effector position what are
the joint angles to achieve this?

What is Kinematics
Forward kinematics

Given joint variables (angles)

q (q1 , q2 , q3 , q4 , q5 , q6 , qn )

Y ( x, y, z , O, A, T )
End-effector position and orientation, - Formula?

y
x

What is Kinematics
Inverse kinematics
End effector position
and orientation
( x, y, z , O, A, T )

q (q1 , q2 , q3 , q4 , q5 , q6 , qn )
Joint variables -Formula?

y
x

Example 1
Forward kinematics
x0 l cos Coordinate position (x, y)
y0 l sin

y0

in terms of joint angle,

cos 1 ( x0 / l )

Joint angle in terms

of coordinate position

x1

Inverse kinematics

y1

x0

Preliminary
Robot Frames

World frame - reference

Joint frame
z
y
Tool frame
z x
work piece

y
W

Co-ordinate Frames

y Right-handed
Co-ordinate frame

Camera Frame
x

Tool Frame
x

Link Frame
x

Goal Frame
x

Base
x Frame
11

Kinematic Relationship
Between two frames we have a kinematic
relationship - basically a translation and a
rotation.
z

x
This relationship is mathematically
represented by a 4 4 Homogeneous
Transformation Matrix.

We are interested in two kinematics topics

Forward Kinematics (angles to position)
What you are given:

The length of each link

The angle of each joint

What you can find:

The position of any point

(i.e. its (x, y, z) coordinates

Inverse Kinematics (position to angles)

What you are given:

The length of each link

The position of some point on the robot

What you can find:

The angles of each joint needed to obtain

that position

Axis and Orientation Terminology :

Global/
Reference
Coordinate

Moving/
Derived/
local
Coordinate

Tools
Orientation - I

Tools
Orientation - II

Tools
Orientation - III

n: normal

Y: Yaw

n: normal

s: sliding

P: Pitch

a: approach

R: Roll

Reference Origin = O

and

Derived Origin = O

o: orientation

a: approach

Orientation Convention

Orientation Convention

Orientation Convention

Orientation Convention
In three dimensions, the six DOFs of a
rigid body are sometimes described
using these nautical names:

Moving up and down (heaving);

Moving left and right (swaying);
Moving forward and backward (surging);
Tilting forward and backward (pitching);
Turning left and right (yawing);
Tilting side to side (rolling).

Orientation Convention
Try to move your hand accordingly !

Roll
(z, a-axis)

Yaw
(x, n-axis)
s, y
a, z
n, x

Pitch
(y, s-axis)

End-effector or
Tool Front View

Preliminary
Coordinate Transformation
Reference coordinate frame
oxyz

Point represented in OXYZ:

P = 2x + 5y + 7z

Vector representation in OXYZ :

Pxyz p x i x p y jy p z k z

y
7

2=px x=ix 5=py y=jy 7=pz z=kz

Pxyz [ p x , p y , p z ]

5
O

i, j, k are unit vector along x, y, z

Preliminary

px i x P

p y jy P
pz k z P

Preliminary
Coordinate Transformation
Reference coordinate frame
Oxyz
Body-attached frame Ouvw

Point represented in OXYZ:

Pxyz p x i x p y jy p z k z

w
v

Pxyz [ p x , p y , p z ]

Point represented in Ouvw:

Puvw pu i u pv jv pw k w

Two frames coincide ==>

Therefore

O, O

pu p x pv p y pw p z
Pxyz = Puvw

Preliminary
Oxyz and Ouvw are coordinate systems

P is the point in the space

P
v

Pxyz p x i x p y jy p z k z

y
u

Puvw pu i u pv jv pw k w

x
O,O

i, j, k are unit vector along x, y, z and u, v, w directions

Preliminary
Properties: Dot Product

3
x
y
R
Let and be arbitrary vectors in
and be
the angle from x to y , then

x y x y cos
Properties of orthonormal coordinate frame
Mutually perpendicular

i j 0

i k 0

k j 0

Unit vectors

| i | 1

| j | 1

| k | 1

Preliminary
Coordinate Transformation

Rotation only

Pxyz p x i x p y jy p z k z

Puvw pu i u pv jv pw k w
But now

y
v

Pxyz = Puvw

Pxyz RPuvw
How to relate the coordinate in these two frames?

Preliminary
Basic Rotation
p x , p y , and p z

represent the projections

of P onto OX, OY, OZ axes, respectively

Since

P pu i u pv jv pw k w

p x i x P i x i u pu i x jv pv i x k w pw
p y jy P jy i u pu jy jv pv jy k w pw

p z k z P k z i u pu k z jv pv k z k w pw

Preliminary
Basic Rotation Matrix
px i x i u
p j i
y y u
p z k z i u
ix iu
R jy i u
k z i u

i x jv
j y jv
k z jv

i x jv
j y jv
k z jv
ix k w
jy k w

k z k w

i x k w pu

j y k w pv
k z k w pw

Pxyz RPuvw

Preliminary
Basic Rotation Matrix
p x i x i u i x jv i x k w
p j i

j
j

k
y
y
u
y
v
y
w

p z k z i u k z jv k z k w
Rotation about x-axis with w
0
1 0

Rot ( x, ) 0 C S
0 S C
x

pu
p
v
pw
z

P v

Rotation about z-axis

y

jy

P u

jv

kw

kz

iu

ix

Preliminary
Rotation about x axis with

0
px 1
p 0 cos
y
p z 0 sin

sin

pu
p
v

cos pw

P v

p x pu

p y pv cos pw sin
p z pv sin pw cos

u
x

Basic Rotation Matrices

Rotation about x-axis with

1 0
Rot ( x, ) 0 C
0 S

0
S
C

Rotation about y-axis with

C
Rot ( y, ) 0
S

0
1

C
Rot ( z , ) S
0

S
C

Rotation about z-axis with

Pxyz RPuvw

S
0

0 C

0
0

Rotation Matrices in 3D OK,lets return

from homogenous representation
cos
R z sin
0

cos
R y
0
sin

1
R x 0
0

sin
cos
0

0
1
0

0
cos
sin

0
0
1

Rotation around the Z, W, R, a -Axis

sin

0
cos

0
sin
cos

Rotation around the Y, V, P, s-Axis

Rotation around the X, U, Y, n-Axis

Preliminary
Basic Rotation Matrix
ix iu
R jy i u
k z i u

i x jv
j y jv
k z jv

ix k w
jy k w
k z k w

Pxyz RPuvw

Obtain the coordinate of Puvw from the

Dot products are commutative!
coordinate of Pxyz
pu i u i x
p j i
v v x
pw k w i x

i u jy
jv j y
k w jy

i u k z px

jv k z p y
k w k z p z

QR R T R R 1 R I 3

Puvw QPxyz

Q R 1 R T

<== 3X3 identity matrix

Example 2
A point auvw (4,3,2) is attached to a rotating
frame, the frame rotates 60 degree about the
OZ axis of the reference frame. Find the
coordinates of the point relative to the
reference frame after the rotation.

a xyz Rot ( z ,60)auvw Rauvw

0.5 0.866 0 4 0.598

0.866
0.5
0 3 4.964
0
0
1 2
2

Example 3

- Theta
- Phi
Psi

A point a xyz (4,3,2) is the coordinate w.r.t.

the reference coordinate system, find the
corresponding point auvw w.r.t. the
rotated OU-V-W coordinate system if it
has been rotated 60 degree about OZ
axis.
auvw Rot ( z ,60)T a xyz Qa xyz R T a xyz

0.5
0.866 0 4 4.598

0.866 0.5 0 3 1.964

0
0
1 2
2

MATLAB - Robotic Toolbox

>> rotz(pi/3)
ans =
0.5000 -0.8660
0.8660 0.5000
0
0
0
0

0
0
1.0000
0

0
0
0
1.0000

Composite Rotation Matrix

A sequence of finite rotations
matrix multiplications do not commute
rules:
if rotating coordinate OUVW is rotating about
principal axis of OXYZ frame, then Pre-multiply
the previous (resultant) rotation matrix with an
appropriate basic rotation matrix
if rotating coordinate OUVW is rotating about its
own principal axes, then post-multiply the
previous (resultant) rotation matrix with an
appropriate basic rotation matrix

Example 4
Find the rotation matrix for the
following operations:
Rotation about OY axis
Rotation about OW axis
Rotation about OU axis
R Rot ( y, ) Rot ( w, ) Rot (u , )

Answer...

C
0
- S

0 S C
1 0 S
0 C 0

S
C
0

0
0

1 0
0 C

1 0

0
S
C

Example 4
Find the rotation matrix for the
following operations:
Rotation about OY axis, Rotation about OW axis and
Rotation about OU axis
Answer... Pre-multiply if rotate about the OXYZ axes
Post-multiply if rotate about the OUVW axes
CC
S
SC

SS CSC
CC
SSC CS

CSS SC

CS

CC SSS

Homogeneous Transformations
33 Rotational
Matrix

r1 r2 r3

r4 r5 r6

r7 r8 r9

0 0 0

1 3 Perspective

3 1 Translation

Global Scale

Homogeneous Matrices in 3D
H is a 4x4 matrix that can describe a translation, rotation, or both in one matrix
O
Y
N

P
X

Translation without rotation

Z

1
0
H
0

Y
O

N
X
Z
A

Rotation without translation

nx
n
y
nz

0
1
0
0

0 Px
0 Py
1 Pz

0 1

ox
oy

ax
ay

oz
0

az
0

0
0

Rotation part:
Could be rotation around z-axis,
x-axis, y-axis or a combination of
the three.

Example 5
Translation along Z-axis with h:
1 0 0 0
0 1 0 0

Trans ( z , h)
0 0 1 h

0
0
0
1

x 1 0
y 0 1

z 0 0

1 0 0

0
0
1
0

0
0
h

pu pu
p p
v
v

pw pw h

1
1

P
y

w
v

v
O, O

O, O

Example 6
Rotation about the X-axis by
Rot ( x, )

1 0
0 C

x
y

z

1

1 0
0 C

0
S

S
0

C
0

P v

u
x

0 pu
0 pv
0 pw

1 1

Homogeneous Transformation
Composite Homogeneous
Transformation Matrix
Rules:
Transformation (rotation/translation)
w.r.t (X,Y,Z) (OLD FRAME), using premultiplication
Transformation (rotation/translation)
w.r.t (U,V,W) (NEW FRAME), using postmultiplication

Example 7
Find the homogeneous transformation
matrix (T) for the following operations:

Rotation about OX axis

Translation of a along OX axis
Translation of d along OZ axis
Rotation of about OZ axis
Answer :

C
S

S
C
0

0 0
0 0
1 0

0 1

1 0 0 0
0 1 0 0
0 0 1 d

0 0 0 1

T Rz , Tz ,d Tx ,a Rx ,
1 0 0 a
0 1 0 0
0 0 1 0

0 0 0 1

0 C

0
S

0
0
0

Example 8
Find the homogeneous transformation
matrix (T) for the following operations:

Rotation about OX axis

Translation of a along OX axis
Translation of d along OZ axis
Rotation of about OW axis
Answer :

1 0 0 0 1
0 1 0 0 0

0 0 1 d 0

0 0 0 1 0

0 0 a
1 0 0
0 1 0

0 0 1

T Tz ,d Tx ,a Rx , Rw,
1

0 C

0
0
0

1

0 0
1 0

0 1

0 0

Homogeneous Representation
A frame in space
(Geometric Interpretation)

R33
F
0

P31

nx
n
y

sx
sy

ax
ay

sz
0

az
0

nz

P( px , p y , pz )
a (z)

px
p y
pz

Principal axis n w.r.t. the reference coordinate system

s(y)
n (X)
y

Homogeneous Transformation
Translation

1 0 0 d x nx
0 1 0 d n
y
Fnew
y
0 0 1 d z nz

0
0
0
1

0
nx s x a x p x d x
n s a

d
y
y
y
y
y

nz s z a z p z d z

0
0
0
1

sx
sy

ax
ay

sz

az

px
p y
pz

s
n

s
n
y

Fnew Trans (d x , d y , d z ) Fold

>> a=rotx(0)
a=
1
0
0
0
1
0
0
0
1
0
0
0
>> plotT1(a)

0
0
0
1
T position and orientation

1
0.8

0.6
0.4
0.2
0
1
0.8
0.6
0.4
0.2
y

0.2

0.4
x

0.6

0.8

Homogeneous Transformation
Composite Homogeneous Transformation Matrix
z1
z0

z2
y1

A1

y0

x2
1

A2

x1

x0
i 1

y2

A2 A1 A2
0

?
Ai

Transformation matrix for

adjacent coordinate frames
Chain product of successive
coordinate transformation matrices

Rotate x, 600 (Rotx(600)

T position and orientation

0.8

0.6

0.4
1

0.2

0
1

0.5
0.5

-0.5
y

-1

Translate x=2 (transl(2,0,0)

T position and orientation

1
0.8

0.6
0.4
0.2

0
0.5

3
2

0
1

-0.5
y

-1

Further translate (2,3,2)

T position and orientation

5
4

0
4

3
3

2
1
y

1
-1

Rotation and Translation

T position and orientation

T position and orientation

3
2
z

3
2.5

0
4

1.5
1

0.5
0
4

2
1

4
3
3

2
2

1
y

1
-1

-1

2
x

Matlab codes for rotation and

translation Robot Toolbox
>>a=rotx(0)
plotT1(a)
b=rotx(pi/3)
plotT1(b)
c=transl(2,0,0)*b
plotT1(c)
d=transl(2,3,2)*c
plotT1(d)
plotT21(a,d)
plotT21(b,c)
plotT21(c,d)

Example 8
For the figure shown below, find the 4x4 homogeneous
transformation matrices i 1 Ai and 0 Ai for i=1, 2, 3, 4, 5
c

z3

y3

z5
x5

y5

x0

nz

sz

az

z2

y4

x2
x1
y1

y0

ax
ay

z1
z0

sx
sy

z4
x4

x3

nx
n
y

y2

px
p y
pz

0
1 0 0
0 0 1 e c
0

A1
0 1 0 a d

0
0
0
1

b
0 1 0
0 0 1 a d
1

A2
1 0 0
0

0
0
0
1

0
Can you find the answer by observation based
1
0
on the geometric interpretation of
A2
0
homogeneous transformation matrix?

1
0
0
0

0 b
0 e c
1
0

0
1

Examples :
Theoretical and graphical examples

Orientation Representation
R33
F
0

P31

Rotation matrix representation needs 9

elements to completely describe the
orientation of a rotating rigid body.
Any easy way?
Euler Angles Representation

Orientation Representation
Euler Angles Representation ( , , )
Many different types
Description of Euler angle representations
Euler Angle I

Euler Angle II

Roll-Pitch-Yaw

Sequence

about OZ axis

about OZ axis

about OX axis

of

about OU axis

about OV axis

about OY axis

Rotations about OW axis

about OW axis

about OZ axis

Euler Angle I, Animated

w'= z
w'"= w"

v'"
v"
v'

y
u'"
u' =u"

Orientation Representation
Euler Angle I
cos

Rz sin
0

Rw''

cos

sin
0

sin

cos
0

0 , Ru '
1

sin

cos

0
1

0 cos
0 sin

sin ,
cos

Euler Angle I
Resultant Eulerian rotation matrix:

R Rz Ru ' Rw''

cos cos

sin sin cos

sin cos
cos sin cos

sin sin

cos sin
sin cos cos
sin sin
cos cos cos
cos sin

sin sin

cos sin

cos

Euler Angle II, Animated

w'= z
w"'= w"

Notetheopposite
(clockwise)senseofthe
thirdrotation,.

v"'
v' =v"
y
u"'
x

u'

u"

Orientation Representation
Matrix with Euler Angle II
sin sin

cos cos cos

cos sin
sin cos cos

cos sin

sin cos
sin cos cos
cos cos
sin cos cos
sin sin

Quiz: How to get this matrix ?

cos sin

sin sin

cos

Orientation Representation
Description of Roll Pitch Yaw
Z
- Phi roll angle
Theta pitch angle
Psi yaw angle

X
Quiz: How to get rotation matrix ?

Review
Composite Homogeneous Transformation
Matrix
Rules:
Transformation (rotation/translation) w.r.t.
(X,Y,Z) (OLD FRAME), using premultiplication
Transformation (rotation/translation) w.r.t.
(U,V,W) (NEW FRAME), using postmultiplication

Review
Homogeneous Representation
A point in R 3 space
p x Homogeneous coordinate of P w.r.t. OXYZ
p
P( p x ,
P y
z
pz
a

1
3
A frame in R space

n s a P
F

0
0
0
1

nx
n
y
nz

sx
sy
sz
0

ax
ay
az
0

px
p y
pz

p y , pz )
s
n
y

Review
Orientation Representation
(Roll-Pitch-Yaw)
Description of Yaw, Pitch,
Roll premultiply !

A rotation of about the

yaw
OX axis ( Rx , ) -- yaw

A rotation of about the

OY axis ( R y , ) -- pitch
A rotation of about the X
OZ axis ( Rz , ) -- roll

roll

pitch

Quiz 1
How to get the resultant rotation matrix for YPR?

T Rz , R y , Rx ,
C
S

S
C
0
0

0
0
1
0

0
0
0

C
0

0 S
1 0
0 C
0 0

0
0
0

1 0
0 C

0 S

0 0

0
S
C
0

0
0
0

Quiz 2
Geometric Interpretation?

R33
T
0

P31

Orientation of OUVW coordinate

frame w.r.t. OXYZ frame
Position of the origin of OUVW
coordinate frame w.r.t. OXYZ frame

Inverse Homogeneous Matrix?

1

RT

RT P

Inverse of the rotation submatrix

is equivalent to its transpose
Position of the origin of OXYZ
reference frame w.r.t. OUVW frame

RT P

R P R T R 0
I 44
0 1

0 1

R
1
T T
0

Inverse Rotation Matrix R and

Transformation Matrix T
The inverse of rotation matrix R and
Homogeneous Transformation Matrix T

Kinematics Model
Forward (direct) Kinematics
z
q (q1 , q2 , qn )
J o in t
v a r ia b le s
D ir e c t K in e m a t ic s

P o s it io n a n d O r ie n t a t io n
o f th e e n d -e ffe c to r

y
I n v e r s e K in e m a t ic s

Inverse Kinematics Y ( x, y, z, , , )

Exercise : Prove that

1

Q R R QR R R R R I 3
T

R
1
T T
0

R P

1
T
T
R
P

R P
R R 0

I 44

1 0 1 0 1
T

Robot Links and Joints

Denavit-Hartenberg Convention
Number the joints from 1 to n starting with the base and
ending with the end-effector.
Establish the base coordinate system. Establish a righthanded orthonormal coordinate system ( X 0 , Y0 , Z 0 ) at the
supporting base with Z 0 axis lying along the axis of
motion of joint 1.
Establish joint axis. Align the Zi with the axis of motion
(rotary or sliding) of joint i+1.

Denavit-Hartenberg Convention ctd.

Establish the origin of the ith coordinate system. Locate
the origin of the ith coordinate at the intersection of the Z i
& Zi-1 or at the intersection of common normal between
the Zi & Zi-1 axes and the Zi axis.
Establish Xi axis. Establish X i ( Z i 1 Z i ) / Z i 1 Z i
or
along the common normal between the Zi-1 & Zi axes
when they are parallel.
Yi ( Z i X i ) / Z i X i
Establish Yi axis. Assign
to
complete the right-handed coordinate system.
Find the link and joint parameters

Link and Joint Parameters

Joint angle i : the angle of rotation from the Xi-1 axis to the
Xi axis about the Zi-1 axis. It is the joint variable if joint i is
rotary.

di

Joint distance
: the distance from the origin of the (i-1)
coordinate system to the intersection of the Zi-1 axis and
the Xi axis along the Zi-1 axis. It is the joint variable if joint i
is prismatic.

ai

Link length : the distance from the intersection of the Zi-1

axis and the Xi axis to the origin of the ith coordinate
system along the Xi axis.

Link twist angle : the angle of rotation from the Zi-1 axis
to the Zi axis about the Xi axis.

Transformation between i-1 and i

Transformation between i-1 and i

More detail

Example I
3 Revolute Joints

Z3
Z1

Z0

Joint 3

Y0

Link 1
Joint 1

O0

Link 2

X0

Joint 2

O1

X3

d2
X1

O2 X

Y2

a0

O3

Y1

a1

Link Coordinate Frames

Assign Link Coordinate Frames:
To describe the geometry of robot motion, we assign a
Cartesian coordinate frame (Oi, Xi,Yi,Zi) to each link, as follows:

establish a right-handed orthonormal coordinate frame

O0 at the supporting base with Z 0 lying along joint 1
motion axis.
the Zi axis is directed along the axis of motion of joint (i
+ 1), that is, link (i + 1) rotates about or translates
along Zi;
Z1

Z0
Y0

O0 X0

Joint 3
Y1

Link 1
Joint 1

Z3

Link 2

Joint 2

O1 X1 O2 X2

a1

X3

d2

Y2

a0

O3

Link Coordinate Frames

Locate the origin of the ith coordinate at the
intersection of the Zi & Zi-1 or at the intersection of
common normal between the Zi & Zi-1 axes and the
Zi axis.
the Xi axis lies along the common normal from the
Zi-1 axis to the Zi axis X i ( Z i 1 Z i ) / Z i 1 Z i
, (if Zi1 is parallel to Zi, then Xi is specified arbitrarily,
subject only to Xi being perpendicular toZ Zi);
Z0
Y0

Joint 1

Z1

Joint 3
Y1

O3

X3

d2
O0 X0

Joint 2

O1 X1 O2 X2
Y2

a0

a1

Link Coordinate Frames

Assign Yi ( Z i X i ) / Z i X i
to complete the righthanded coordinate system.
The hand coordinate frame is specified by the
geometry of the end-effector. Normally, On
establish Zn along the direction of Zn-1 axis and
pointing away from the robot; establish Xn
such that it is normal to both Zn-1 and Zn axes.
Assign Yn to complete the right-handed
Z3
coordinate system.
Z1

Z0
Y0

Joint 1

Joint 3
Y1

O3

X3

d2
O0 X0

Joint 2

O1 X1 O2 X2
Y2

a0

a1

Example I
Z1

Z0
Y0

Joint 1

Z3

Joint 3
Y1

O3

X3

d2
O0 X0

Joint 2

O1 X1 O2 X2

D-H Link Parameter Table

Joint i

ai

di

a0

a0
a1
i : rotation angle from Zi-1 to Zi about Xi

-90

a1

ai

d2

Y2

di

: distance from intersection of Zi-1 & Xi

to origin of i coordinate along Xi
: distance from origin of (i-1) coordinate to intersection of Z i-1 & Xi along Zi-1

i : rotation angle from X to X about Z

i-1
i
i-1

Z3

Z1

Z0
Y0

Joint 3
Y1

X3

O3

d2

oint 1

O0 X0

Joint 2

O1 X1 O2 X2
Y2

a0

a
d

a1

Joint i

ai

di

a0

-90

a1

d2

- angle twist for z axis

- distance traveled by z-axis along x-axis
- distance traveled by x-axis along z-axis
- angle at z-axis

Joint i

ai

di

a0

-90

a1

d2

Y2

3 Revolute Joints

Z1

Z0

X2
X0

d2

X1

Y0

Y1

a0
Notice that the table has two uses:
1) To describe the robot with its
variables and parameters.
2) To describe some state of the
robot by having a numerical values
for the variables.

a1

Denavit-Hartenberg Link
Parameter Table

(i-1)

a(i-1)

di

a0

-90

a1

d2

2
89

Transformation between i-1 and i

Four successive elementary transformations are
required to relate the i-th coordinate frame to the
(i-1)-th coordinate frame:
Rotate about the Z i-1 axis an angle of i to align the X
i-1 axis with the X i axis.
Translate along the Z i-1 axis a distance of di, to bring
Xi-1 and Xi axes into coincidence.
Translate along the Xi axis a distance of ai to bring the
two origins Oi-1 and Oi as well as the X axis into
coincidence.
Rotate about the Xi axis an angle of i ( in the righthanded sense), to bring the two coordinates into
coincidence.

In Short :
Rotate z-axis to align with
the x-axis

Slide along z-axis

di

Slide along x-axis

ai

Rotate x-axis to align with z-axis -

Develop a D-H Parameter Table

Joint i

a0

- 90

a1

Matrix transform :

Transformation between i-1 and i

D-H transformation matrix for adjacent
coordinate frames, i and i-1.
The position and orientation of the i-th frame
coordinate can be expressed in the (i-1)th frame by
the following homogeneous transformation matrix:
Source coordinate

Ti i 1 T ( zi 1 , d i ) R ( zi 1 , i )T ( xi , ai ) R ( xi , i )
Reference
Coordinate

C i
S
i

C i S i
C i C i
S i

S i S i
S i C i
C i

ai C i
ai S i
di

Matlab codes Robot Toolbox

% create robot object R with the DH parameter below :
% alpha A Theta D
%
0
a0 0
0
% -90
a1 0
0
%
0
0 0
d2
% L = LINK([alpha A theta D])
L1 = LINK([0 1 0 0])
% a0 = 1
L2 = LINK([-pi/2 1 0 0]) % a1 = 1
L3 = LINK([0 0 0 1.5]) % d2 = 1.5
R=robot({L1 L2 L3}) % create IBot01 object
drivebot(R)
plot(R)

Try this ?

Example II: PUMA 260

1

O1

Z1

1.

Number the joints

2.

Establish base frame

3.

Establish joint axis Zi

4.

Locate origin, (intersect.

of Zi & Zi-1) OR (intersect
of common normal & Zi )

X1
Z2 Z6
5.
Y1
O2
Y3
Z
Z4
O3
X 2 5 6 Y6
Y2
5
O
Z0
6
Y
Y5
X3 4
t
O5
X5 X6
O4 Z 3
X4
PUMA 260

Establish Xi,Yi

X i ( Z i 1 Z i ) / Z i 1 Z i
Yi ( Z i X i ) / Z i X i

Link Parameters
J

O1

Z1

2
3

i
1
2
3
4
5
6

ai d i

-90 0

13

90

-l

4
-90 0 8
X1
Z2 Z6
Y1
5
90 0 0
O
Y3 2
Z
Z4 6
0
0 t
O3
X 2 5 6 Y6
Y2
5
O
i : angle from X to X
Z0
6
i-1
i
Y5
X 3 Y4
about Zi-1
O5
X5 X6

i : angle from Zi-1 to Zi

O4 Z 3

X4

about Xi
ai : distance from intersection
of Zi-1 & Xi to Oi along Xi

Joint distance d i : distance from Oi-1 to intersection of Zi-1 & Xi along Zi-1

PUMA ROBOT : OPEN CHAIN

Stanford Arm : Old Design

Robotic Toolbox

Robotic Toolbox

Robotic Toolbox

Robotic Toolbox

Announcement !
Next week (W9) is for mini-Project
week
No need to attend tutorial class
Do your own work on mini-project
Mini project check on Assembly will
be done together with programming
check on week 11.
Laboratory II will start on week 10.

Kinematic Equations
q (q1 , q2 , qn )

Forward Kinematics

Given joint variables

To get end-effector position & orientation

Homogeneous matrix

Y ( x, y, z , , , )

T0n

specifies the location of the ith coordinate frame w.r.t.

the base coordinate system
chain product of successive coordinate transformation
i
T
matrices of i 1
Position

T0n T01T12 Tnn1

Orientation
matrix

R0n

vector

P
n s a P0

1 0 0 0 1
n
0

Kinematics Equations
Other representations
reference from, tool frame
0
Treftool Bref
T0n H ntool

Yaw-Pitch-Roll representation for orientation

T Rz , R y , Rx ,
C
S

S
C
0
0

0
0
1
0

0
0
0

C
0

0 S
1 0
0 C
0 0

0
0
0

1 0
0 C

0 S

0 0

0
S
C
0

0
0
0

Solving forward kinematics

Forward kinematics

px
p
y

nx
n
T y
nz

pz

5
6

Transformation Matrix

sx
sy
sz
0

ax
ay
az
0

px
p y
pz

- Phi : roll angle

Theta : pitch angle
Psi : yaw angle

Solving forward kinematics

Yaw-Pitch-Roll representation for orientation
CC CSS SC
SC SSS CC
T0n
S
CS

0
0

T
n
0

Problem?

nx
n
y
nz

sx

ax

sy

ay

sz
0

az
0

px

p y
pz

CSC SS
SSC CS
CC
0

sin 1 (nz )

az
cos (
)
cos
nx
1
cos (
)
cos

Solution is inconsistent and illconditioned!!

px
p y
pz

atan2(y,x)

a tan 2( y, x)

0 90
90 180

180

90

90 0

for x and y
for x and y
for x and y
for x and y

Yaw-Pitch-Roll Representation
T Rz , R y , Rx ,
C
S

S
C
0
0

0
0
0

sx

ax

sy

ay

nz

sz

az

C
0

0
0
1
0

nx
n
y

0
0

0 S
1 0
0 C
0 0

0
0
0

1 0
0 C

0 S

0 0

0
S
C
0

0
0
0

Yaw-Pitch-Roll Representation
1
z ,

R T R y , Rx ,
C
0

C
S

C
0

0 0
0 0

1 0

0 1

0 S
1 0
0 C
0 0

0
0
0

nx
ny
nz
0

sx
sy
sz
0

1 0
0 C

ax
ay
az
0

0
0
0

(Equation A)

Yaw-Pitch-Roll Representation
Compare LHS and RHS of Equation A, we have:

sin nx cos n y 0

a tan 2(n y , nx )

cos nx sin n y cos

nz sin

a tan 2(nz , cos nz sin n y )

sin s x cos s y cos

sin a x cos a y sin

a tan 2(sin a x cos a y , sin s x cos s y )

Kinematic Model
Steps to derive kinematics model:
Assign D-H coordinates frames
Find link parameters
Transformation matrices of adjacent
joints
Calculate Kinematics Matrix
When necessary, Euler angle
representation


a
d

Example
Y0

Joint 1

Z3

Z1

Z0

- angle twist for z axis

- distance traveled by z-axis along x-axis
- distance traveled by x-axis along z-axis
- angle at z-axis
Joint 3

Y1

O3

X3

d2
O0 X0

Joint 2

O1 X1 O2 X2
Y2

a0

a1

Joint i

ai

di

a0

-90

a1

d2

Transformation between i-1 and i

D-H transformation matrix for adjacent
coordinate frames, i and i-1.
The position and orientation of the i-th frame
coordinate can be expressed in the (i-1)th frame by
the following homogeneous transformation matrix:
Source coordinate

Ti i 1 T ( zi 1 , d i ) R ( zi 1 , i )T ( xi , ai ) R ( xi , i )
Reference
Coordinate

C i
S
i

C i S i
C i C i
S i

S i S i
S i C i
C i

ai C i
ai S i
di

More detail

Example
Joint i

ai

di

a0

2
3

Ti i1

C i
S
i

-90
0

a1

C i S i
C i C i
S i
0

d2
S i S i
S i C i
C i
0

T03 (T 01)(T 21)(T 23)

1
2
ai C i
ai S i
di

a0 cos 0

cos 0
sin
0
T 01
0

sin 0

cos 0
0

0
1

a0 sin 0

cos1
sin
1
2
T 1

0
0
1
0

cos 2
sin
2
T 23

sin 1
cos 1
0
0

sin 2
cos 2
0
0

a1 cos 1
a1 sin 1

0
0
1
0

0
0
d2

Exercise
Find the Homogeneous
Transformation Matrix of
T03 = T01 T12 T23

Example: Puma 560

Example: Puma 560

Link Coordinate Parameters

PUMA 560 robot arm link coordinate parameters

Example: Puma 560

Example: Puma 560

Exercise :
Specify the parameter table for IGBot-01
from the data given.
Find the D-H transformation matrix for
adjacent coordinate frames, i and i-1.
Finally, find the Homogeneous
Transformation matrix, T, for the IGBot01

IGBot-01

Work Envelope of IGBot-01

Link Configuration of IGBot-01

Link 3
Link 2

Link 4

Joint 3 (R)
Elbow
Joint 4 (T)
Wrist
Joint 2 (R)
Shoulder

Link 1
Joint 1 (R)
Base

Link 1 = 9.5 cm
Link 2 = 11.5 cm
Link 3 = 8.5 cm
Link 4 = 15.0 cm

IGBot-01 specifications :
Link #

Joint Type

Link Dimension

Joint Angle Range

Base

Revolute

-177.50 to +177.50

Shoulder

Revolute

11.5 cm

-450 to +900

Elbow

Revolute

8.5 cm

-600 to +900

Wrist

Revolute

5.0 cm

-1800 to +1800

Gripper

IGBot-01
Link parameter table for IGBot-01
Joint #

di

ai

d1

-900

a1

a2

-900

d2

Joint #

di

ai

d1

-900

a1

a2

-900

d2

OR - IGBot-01
Link parameter table for IGBot-01
Joint #

di

ai

d1

900

a1

a2

900

d2

IGBot01 - simulation
% create robot object IGBot01 with the DH
% parameter below :
% alpha A Theta D
%
-90 0 0
d1
%
0 a1 0
0
%
-90 a2 0
0
%
0 0 0
d2
%L = LINK([alpha A theta D])
L1 = LINK([-pi/2 0 0 1])
% d1 = 1
L2 = LINK([0 1 0 0])
% a1 = 1
L3 = LINK([-pi/2 0 0 0])
% a2 = 0
L4 = LINK([0 0 0 1])
% d2 = 1
IGBot01=robot({L1 L2 L3 L4})
% create robot IGBot01
drivebot(IGBot01)
%simulate
plot(IGBot01)
%plot
T=fkine(IGBot01, [0 0 pi/2 0])
%derive forward kinematics
Q=ikine(IGBot01, T)
%inverse kinematics
rpy=tr2rpy(T)
%derive roll/pitch/yaw

Inverse Kinematics
Given a desired position (P)
& orientation (R) of the end- z
effector

q (q1 , q2 , qn )
Find the joint variables which
can bring the robot the
desired configuration

y
x

Inverse Kinematics
More difficult
(x , y)

l2
l2

l1
l1

Systematic closed-form
solution in general is not
available
Solution not unique
Redundant robot
Elbow-up/elbow-down
configuration
Robot dependent

Inverse Kinematics
Transformation Matrix
nx
n
T y
nz

sx
sy
sz
0

ax
ay
az
0

px
p y
T01T12T23T34T45T56
pz

Special cases make the closed-form arm solution possible:

1

2
3

4
5

6

1.

Three adjacent joint axes intersecting (PUMA, Stanford)

2.

Three adjacent joint axes parallel to one another (MINIMOVER)

The Denavit-Hartenberg Matrix

cos i
sin i cos (i 1)
sin i sin (i 1)
0

sin i
cos i cos (i 1)
cos i sin (i 1)
0

0
sin (i 1)
cos (i 1)
0

a(i 1)

sin (i 1) d i
cos (i 1) d i

Just like the Homogeneous Matrix, the Denavit-Hartenberg Matrix is a

transformation matrix from one coordinate frame to the next. Using a series of
D-H Matrix multiplications and the D-H Parameter table, the final result is a
transformation matrix from some frame to your initial frame.

141

A Simple Example of Inverse Kinematics

- Finding angles given coordinate x and y

Revolute and
Prismatic Joints
Combined

Finding

y
arctan( )
x

More Specifically:

(x , y)

y
arctan 2( )
x

Finding S:

S (x 2 y 2 )

arctan2() specifies that its in the

first quadrant

Thank you!
z
z
z

x
z

y
x

y
x

The Situation:
You have a robotic arm that
starts out aligned with the xo-axis.
You tell the first link to move by 1
and the second link to move by 2.
The Quest:
What is the position of the
end of the robotic arm?

Solution:
1. Geometric Approach
This might be the easiest solution for the simple situation. However,
notice that the angles are measured relative to the direction of the previous
link. (The first link is the exception. The angle is measured relative to its
initial position.) For robots with more links and whose arm extends into 3
dimensions the geometry gets much more tedious.
2. Algebraic Approach
Involves coordinate transformations.

144

Geometry of planar robot

Link #

Joint Type

Link Dimension

Joint Range

Revolute

l1

-900 - +900

Revolute

l2

-1800 - +1800

Revolute

l3

-1800 - +1800

Example Problem:
You have a three link arm that starts out aligned in the x-axis. Each link
has lengths l1, l2, l3, respectively. You tell the first one to move by 1 , and so on as
the diagram suggests. Find the Homogeneous matrix to get the position of the
yellow dot in the X0Y0 frame.
Y3

3
Y2

X2

Y1

X1

1
X0

X3

H = Rz( 1 ) * Tx1(l1) * Rz( 2 ) * Tx2(l2) * Rz( 3 )

i.e. Rotating by 1 will put you in the X1Y1 frame.
Translate in the along the X1 axis by l1.
Rotating by 2 will put you in the X2Y2 frame.
and so on until you are in the X3Y3 frame.
The position of the yellow dot relative to the X 3Y3 frame is
(l3, 0). Multiplying H by that position vector will give you the
146
coordinates of the yellow point relative the the X 0Y0 frame.

Slight variation on the last solution:

Make the yellow dot the origin of a new coordinate X 4Y4 frame
Y3
Y4

3
Y2

2
X2

X3

3
X4

H = Rz( 1 ) * Tx1(l1) * Rz( 2 ) * Tx2(l2) * Rz( 3 ) * Tx3(l3)

1

This takes you from the X0Y0 frame to the X4Y4 frame.

X1

Y0

Y1

1
X0

X
Y

0
0
H
0

1

The position of the yellow dot relative to the X 4Y4 frame

is (0,0).
Notice that multiplying by the (0,0,0,1) vector will
equal the last column of the H matrix.

The Denavit-Hartenberg Matrix

cos i
sin i cos (i 1)
sin i sin (i 1)
0

sin i
cos i cos (i 1)
cos i sin (i 1)
0

0
sin (i 1)
cos (i 1)
0

a(i 1)

sin (i 1) d i
cos (i 1) d i

Just like the Homogeneous Matrix, the Denavit-Hartenberg Matrix is a

transformation matrix from one coordinate frame to the next. Using a series of
D-H Matrix multiplications and the D-H Parameter table, the final result is a
transformation matrix from some frame to your initial frame.

148

A Simple Example of Inverse Kinematics

- Finding angles given coordinate x and y

Revolute and
Prismatic Joints
Combined

Finding

y
arctan( )
x

More Specifically:

(x , y)

y
arctan 2( )
x

Finding S:

S (x 2 y 2 )

arctan2() specifies that its in the

first quadrant

Inverse Kinematics of a Two Link Manipulator

(x , y)

Find:

l2

l1

Given: l1, l2 , x , y
1, 2

Redundancy:
A unique solution to this problem
does not exist. Notice, that using the
givens two solutions are possible.
Sometimes no solution is possible.
(x , y)
l2
l2

l1
l1

150

The Geometric Solution

l2

(x , y)

Using the Law of Cosines:

c 2 a 2 b 2 2ab cos C
2

( x 2 y 2 ) l1 l2 2l1l2 cos(180 2 )
cos(180 2 ) cos( 2 )
2

l1

Using the Law of Cosines:

sin B sin C

b
c
sin 1 sin(180 2 )
sin( 2 )

l2
x 2 y2
x 2 y2

1 1
y
arctan 2
x

x 2 y 2 l1 l2
cos( 2 )
2l1l2

x 2 y 2 l12 l2 2

2 arccos

2l1l2

Redundant since 2 could be in the

first or fourth quadrant.

Redundancy caused since 2 has two possible

values

1 arcsin

l sin( )
2
2

y
arctan 2
x
x 2 y 2
151

The Algebraic Solution

2

l2

(x , y)

c1 cos1
c1 2 cos( 2 1 )
(1) x l1 c1 l2 c1 2

l1

(2) y l1 s1 l2 sin 1 2

(3) 1 2

(1) 2 (2) 2 x 2 y 2

l1 c1 l2 (c1 2 ) 2 2l1l2 c1 (c1 2 ) l1 s1 l2 (sin1 2 ) 2 2l1l2 s1 (sin1 2 )

l1 l2 2l1l2 c1 (c1 2 ) s1 (sin1 2 )
2

l1 l2 2l1l2 c 2

Only Unknown

x 2 y 2 l12 l2 2

2 arccos

2
l
l
1 2

Note :
cos(ab) (cos a )(cos b) (sin a )(sin b)

sin( ab) (cos a)(sin b) (cos b152

)(sin a )

x l1 c1 l2 c1 2
l1 c1 l2 c1c 2 l2 s1s2
c1 (l1 l2 c 2 ) s1 (l2 s2 )

Note :
cos(a b) (cos a )(cos b) (sin a )(sin b)

sin( a b) (cos a )(sin b) (cos b)(sin a )

y l1 s1 l2 sin 1 2
l1 s1 l2 s1c 2 l2 s 2 c1
c1 (l2 s 2 ) s1 (l1 l2 c 2 )
c1

x s1 (l 2 s 2 )
(l1 l 2 c 2 )

x s1 (l 2 s 2 )
y
(l 2 s 2 ) s1 (l1 l 2 c 2 )
(l1 l 2 c 2 )

s1

We know what 2 is from the previous

slide. We need to solve for 1 . Now
we have two equations and two
unknowns (sin 1 and cos 1 )
Substituting for c1 and simplifying
many times

1
x l 2 s 2 s1 (l12 l 2 2 2l1l 2 c 2 )
(l1 l 2 c 2 )
y(l1 l 2 c 2 ) x l 2 s 2
2

x y

Notice this is the law of cosines

and can be replaced by x2+ y2

y(l1 l 2 c 2 ) x l 2 s 2

1 arcsin

x y