An Introduction to

Robot Kinematics

Renata Melamud

Kinematics studies the motion of bodies

An Example - The PUMA 560

4
1

There are two more

joints on the end
effector (the
gripper)

The PUMA 560 has SIX revolute joints

A revolute joint has ONE degree of freedom ( 1 DOF) that is
defined by its angle

Other basic joints

Revolute Joint
1 DOF ( Variable - )

Prismatic Joint
1 DOF (linear) (Variables - d)

Spherical Joint
3 DOF ( Variables - 1, 2, 3)

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

Quick Math Review

Dot Product:

ax
a
y

Geometric Representation:

A B A B cos

Matrix Representation:

bx
b
y

ax bx
AB

a xb x a y b y

ay by

Unit Vector
Vector in the direction of a chosen vector but whose magnitude is 1.

B
uB
B

B
uB

Quick Matrix Review

Matrix Multiplication:
An (m x n) matrix A and an (n x p) matrix B, can be multiplied since
the number of columns of A is equal to the number of rows of B.
Non-Commutative Multiplication
AB is NOT equal to BA

a b e
c d g

f ae bg

h ce dg

af bh
cf dh

Matrix Addition:
a b e
c d g

f a e

h c g

b f
d h

Basic Transformations
Moving Between Coordinate Frames
Translation Along the X-Axis
Y

O
(VN,VO)

NO

XY

P
X

Px
Px = distance between the XY and NO coordinate planes
Notation:

XY

VX
Y

NO

VN
O

Px
P

VO

Writing V XY in terms of V NO
Y

NO

VX

P
X

V XY

VN

PX V N
NO

O
V

VO
N

Translation along the X-Axis and Y-Axis

Y
NO

V
Y

VX

PX
X

V XY P V NO

PX V N

O
P

V
Y

XY

Px

PY

VO

Using Basis Vectors

Basis vectors are unit vectors that point along a coordinate axis
O

n
o

Unit vector along the N-Axis

Unit vector along the N-Axis

V NO

Magnitude of the VNO vector

NO

V NO

VO

VN

NO
V NO cos V NO n
V N V cos
O
NO
NO
NO
V V sin V cos(90 ) V o

Rotation (around the Z-Axis)

Y

VY
N

VX

= Angle of rotation between the XY and NO coordinate axis

V XY

VX
Y

V NO

VN
O

Unit vector along X-Axis

Can be considered with respect to

the XY coordinates or NO coordinates
O

V XY

V NO

VY

VX

V X V XY cos V NO cos V NO x

(V n V o ) x
N

(Substituting for VNO using the N and O

components of the vector)

V X V N (x n ) V O (x o)
V N (cos) V O (cos( 90))
V N (cos) V O (sin)

Similarly.

V Y V NO sin V NO cos(90 ) V NO y

V Y (V N n V O o ) y
V Y V N (y n ) V O (y o )
V N (cos(90 )) V O (cos)
V N (sin) V O (cos)
So.

V V (cos) V (sin)
V Y V N (sin) V O (cos)
X

VX

V XY

Written in Matrix Form

XY

V X cos sin
Y

sin
cos
V

VN
O
V

Rotation Matrix about the z-axis

Y1
(VN,VO)

Y0

VNO

VXY

X1

Translation along P followed by rotation by

X0

XY

Px
cos
Y

P
sin
V
y
VX

sin V N

cos V O

(Note : Px, Py are relative to the original coordinate frame. Translation followed by
rotation is different than rotation followed by translation.)

In other words, knowing the coordinates of a point (V N,VO) in some coordinate

frame (NO) you can find the position of that point relative to your original
coordinate frame (X0Y0).

HOMOGENEOUS REPRESENTATION
Putting it all into a Matrix

XY

Px
cos
Y

P
sin
V
y

sin V N

cos V O

VX

VX

V
1

VX

VY
1

cos
H sin
0

sin

Px
cos
Py sin
1
0
cos
sin
0
sin
cos
0

cos
0

sin
cos
0

What we found by doing a

translation and a rotation

0 V N

0 V O
1 1

Px V N

Py V O
1 1

Padding with 0s and 1s

Simplifying into a matrix form

Px

Py
1

Homogenous Matrix for a Translation in

XY plane, followed by a Rotation around
the z-axis

Rotation Matrices in 3D OK,lets return from

homogenous repn

cos
R z sin
0

sin
cos

cos
R y
0
sin
1
R z 0
0

0
cos
sin

0
0
1
0

0
0

Rotation around the Z-Axis

sin
0

Rotation around the Y-Axis

cos
0
sin
cos

Rotation around the X-Axis

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

1
0
H
0

0
1
0
0

0 Px
0 Py
1 Pz

0 1

Z
Y
O

N
X
Z
A

Rotation without translation

nx
n
y
nz

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.

Homogeneous Continued.

V XY

XY

VN
O
V
H A
V

nx
n
y
nz

ox
oy
oz
0

The (n,o,a) position of a point relative to the current

coordinate frame you are in.

ax
ay
az
0

Px
Py
Pz

VN
O
V
VA

V X n x V N o x V O a x V A Px

The rotation and translation part can be combined into a single homogeneous
matrix IF and ONLY IF both are relative to the same coordinate frame.

Finding the Homogeneous Matrix

EX.

WN
O
W
WA

T
P

X
Z
WX
Y
W
WZ

WI

Point relative to the

X-Y-Z frame

W
WK

Pi n i
Pj n j
Pk n k

oi
oj
ok

J
W

K
W

WN

Point relative to the

I-J-K frame

ai W

a j W O
a k W A
N

WI

O
W

A
W

ni
n
j

J
W

W K nk

1 0

Point relative to the

N-O-A frame

oi

ai

oj

aj

ok

ak

Pi

Pj
Pk

1

WN

WO
WA
1

WN

O
W
WA

T
X

P
A

W
X

Y
W

Z
W

Tx i x
Ty i y
Tz i z

kx W

k y W J
k z W k

jx
jy
jz

WX

Substituting for W
WK

WX

ix
i
y

Y
W

WZ iz

1 0

ix
i
y

Y
W

Z
W iz

jx

kx

jy

ky

jz

kz

Tx
Ty
Tz

1

Tx

Ty
Tz

1

jx

kx

jy

ky

jz

kz

Pi

Pj
Pk

1

ni

oi

ai

nj

oj

aj

nk

ok

ak

WI

WJ

WK

WN

WO
WA
1

 WX

WN

Y
O
W
W

WZ
WA

1
1

ix
i
y

kx

jy

ky

iz

jz

kz

jx

Tx
Ty
Tz

1

Pi

ni

oi

ai

nj

oj

aj

nk

ok

ak

Pk

Pj

Product of the two matrices

Notice that H can also be written as:

1 0 0 Tx i x
0 1 0 T i
y y
H
0 0 1 Tz i z

0
0
0
1

jx

kx

jy

ky

jz

kz

0 1
0 0
0 0

1 0

Pi n i
1 0 Pj n j
0 1 Pk n k

0 0 1 0
0 0

oi

ai

oj

aj

ok

ak

0
0
0

H = (Translation relative to the XYZ frame) * (Rotation relative to the XYZ frame)
* (Translation relative to the IJK frame) * (Rotation relative to the IJK frame)

The Homogeneous Matrix is a concatenation of numerous

translations and rotations
J
I

WN

T
X
K

P
A

O
W
WA

One more variation on finding H:

H=

(Rotate so that the X-axis is aligned with T)

* ( Translate along the new t-axis by || T || (magnitude of T))
* ( Rotate so that the t-axis is aligned with P)
* ( Translate along the p-axis by || P || )
* ( Rotate so that the p-axis is aligned with the O-axis)

This method might seem a bit confusing, but its actually an easier way to
solve our problem given the information we have. Here is an example

Forward Kinematics

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.

Example Problem:
You are 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

X3

X2

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

1

Y1

X1

Y0

1
X0

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
(l1, 0). Multiplying H by that position vector will give you the
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.

More on Forward Kinematics

Denavit - Hartenberg Parameters

Denavit-Hartenberg Notation

Z(i - 1)
Y(i -1)

X(i -1)

Yi
a(i - 1 )

Zi
Xi

di

ai

( i - 1)

IDEA: Each joint is assigned a coordinate frame. Using the DenavitHartenberg notation, you need 4 parameters to describe how a frame (i)
relates to a previous frame ( i -1 ).
THE PARAMETERS/VARIABLES:

, a , d,

The Parameters

Z(i - 1)
Y(i -1)

X(i -1)
( i - 1)

Yi
a(i - 1 )

Zi
Xi

di

ai

You can
align the
two axis
just using
the 4
parameters

1) a(i-1)
Technical Definition: a(i-1) is the length of the perpendicular between the joint
axes. The joint axes is the axes around which revolution takes place which are the
Z(i-1) and Z(i) axes. These two axes can be viewed as lines in space. The common
perpendicular is the shortest line between the two axis-lines and is perpendicular
to both axis-lines.

a(i-1) cont...
Visual Approach - A way to visualize the link parameter a(i-1) is to imagine an
expanding cylinder whose axis is the Z(i-1) axis - when the cylinder just touches the
joint axis i the radius of the cylinder is equal to a(i-1). (Manipulator Kinematics)
Its Usually on the Diagram Approach - If the diagram already specifies the
various coordinate frames, then the common perpendicular is usually the X (i-1)
axis. So a(i-1) is just the displacement along the X(i-1) to move from the (i-1) frame
to the i frame.
If the link is prismatic, then a(i-1)
is a variable, not a parameter.

Z(i - 1)
Y(i -1)
X(i -1)
( i - 1)

Yi
a(i - 1 )

di

Zi
Xi

ai
i

2) (i-1)
Technical Definition: Amount of rotation around the common perpendicular so that
the joint axes are parallel.
i.e. How much you have to rotate around the X (i-1) axis so that the Z(i-1) is pointing in
the same direction as the Zi axis. Positive rotation follows the right hand rule.

3) d(i-1)
Technical Definition: The displacement
along the Zi axis needed to align the a(i-1)
common perpendicular to the ai common
perpendicular.
In other words, displacement along the
Zi to align the X(i-1) and Xi axes.

Z(i - 1)
Y(i -1)
X(i -1)

Yi Z
i
a(i - 1 )

di

Xi

( i - 1)

4) i
Amount of rotation around the Zi axis needed to align the X(i-1) axis with the Xi

ai
i

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.

Put the transformation here

Z(i Y(i -1)

1)

X(i -1)
( i -

Y Zi
a(i -

X a
i

1)

1)

3 Revolute Joints

Y2

Z1

Z0

X2
X0

d2

X1

Y0

Y1

a0

Denavit-Hartenberg Link
Parameter Table

a1
i

(i-1)

a(i-1)

di

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.

a0

-90

a1

d2

Notice that the table has two uses:

Y2

Z1

Z0

X2
X0

X1

Y0

Y1

a0

a1

X 0 Y0 Z 0

V X2
Y2
V

V Z2

(i-1)

a(i-1)

di

a0

-90

a1

d2

d2

T ( 0T)( 01T)(12T)
Note: T is the D-H matrix with (i-1) = 0 and i = 1.

(i-1)

a(i-1)

di

a0

cos1
sin
1
0

1
0

-90

a1

sin1
cos1
0
0

d2

0 a0
0 0
0 0

0 1

This is a translation by a0 followed by a

rotation around the Z1 axis

cos 0 sin 0
sin
cos 0
0

0T
0
0

0
0

0
0
1
0

0
0
0

This is just a rotation around the Z0 axis

cos 2

0
1

2T
sin 2

sin 2
0
cos 2
0

0 a1
1 d 2
0 0

0 1

This is a translation by a1 and then d2

followed by a rotation around the X2 and
Z2 axis

T ( 0T)( 01T)( 12T)

Inverse Kinematics
From Position to Angles

A Simple Example

Revolute and
Prismatic Joints
Combined

Finding :

y
arctan( )
x

More Specifically:

(x , y)

y
arctan 2( )
x

1
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

The Geometric Solution

2

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

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 b)(sin a )

x l1 c1 l2 c1 2
l1 c1 l2 c1c 2 l2 s1s2
c1 (l1 l2 c 2 ) s1 (l2 s2 )
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

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

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

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