Robot Modeling and the Forward

Kinematic Solution
ME 3230
R. R. Lindeke

Looking Closely at the T0n Matrix

At the end of our discussion of Robot

Mapping we found that the T0n matrix
related the end of the arm frame (n) to its
base (0)

Thus it must contain information related to

the several joints of the robot
It is a 4x4 matrix populated by complex
equations for both position and orientation
(POSE)

Looking Closely at the T0n Matrix

To get at the equation set, we will

choose to add a series of coordinate
frames to each of the joint positions
The frame attached to the 1st joint is
labeled 0 the base frame! while
joint two has Frame 1, etc.
The last Frame is the end or n Frame

Looking Closely at the T0n Matrix

As we have seen earlier, we can

define a HTM (T(i-1)i) to the
transformation between any two
SO3 based frames

Thus we will find that the T0n is

given by a product formed by:

T T gT gL gT
n
0

1
0

2
1

n
n 1

Looking Closely at the T0n Matrix

For simplicity, we will redefine each of the of

these transforms (T(i-1)i) as Ai

Then, for a typical 3 DOF robot Arm,

T0n = A1*A2*A3

While for a full functioned 6 DOF robot (arm

and wrist) would be:
T0n = A1*A2*A3*A4*A5*A6
A1 to A3 explain the arm joint effect while A4 to
A6 explain the wrist joint effects

Looking At The Frame To Frame

Arrangements Building A Modeling Basis

When we move from one frame to

another, we will encounter:

A rotation and translation WRT the Z i-1

Two translations (in a controlled sense)

Two rotations (also in a controlled sense)
These are called the Joint Parameters

A rotation and translation WRT the X i

These are called the Link Parameters

A model of the Joint

Parameters

NOTE!!!

A model of the Link

Parameters
ai or

Talking Specifics Joint Parameters

i is an angle measured about the Zi-1 axis

from Xi-1 to Xi and is a variable for a
revolute joint its fixed for a Prismatic
Joint

di is a distance measured from the origin

of Frame(i-1) to the intersection of Zi-1 and
Xi and is a variable for a prismatic joint
its fixed for a Revolute Joint

Talking Specifics Link Parameters

ai (or li) is the Link length and measures the

distance from the intersection of Zi-1 to the
origin of Framei measured along Xi
i is the Twist angle which measures the
angle from Zi-1 to Zi as measured about Xi
Both of these parameters are fixed in value
regardless of the joint type.

A Further note: Fixed does not mean zero

degrees or zero length just that they dont
change

Very Important to note:

Two Design Axioms prevail in this modeling

approach

Axiom DH1: The Axis Xi must be designed to be

perpendicular to Zi-1

Axiom DH2: The Axis Xi must be designed to

intersect Zi-1

Thus, within reason we can design the

structure of the coordinate frames to simplify
the math (they are under our control!)

Returning to the 4 Frame-Pair

Operators:

1st is which is an
operation of pure
rotation about Z or:

2nd is d which is a
translation along Z
or:

C os
Sin

0
1
0

Sin
Cos
0
0

0
1
0
0

0
0
1
0

0
0
1
0

0
0

1
0
0

Returning to the 4 Frame

Operators:

3rd is a Translation
Along X or:

1
0

0
1

0
0

4th is which is a
pure Rotation
about X or:

0
1
0 Cos

0 Sin

0
0

0
Sin
Cos
0

0
0

The Overall Effect is:

C os
Sin
0
0

Sin
Cos
0
0

0 0
0 0

1 0

0 1

1 0 0 0
1 0
0 1 0 0
0 1

0 0 1 d
0 0

0 0 0 1
0 0

0
0

a
0
0

1
0
0 Cos
0
0

Sin
0

0
0
0
Sin

Cos 0

0
1

Simplifying this Matrix Product:

S i C i
C i C i

S i S i
C i S i

S i
0

C i
0

C i
S
i

This matrix is the general transformation

relating each and every of the frame pairs
along a robot structure

ai C i

ai S i

di

So, Since We Can Control the Building of

this Set Of Frames, What Are The Rules?

We will employ a method called the DenavitHartenberg Method

It is a Step-by-Step approach for modeling
each of the frames from the initial (or 0) frame
all the way to the n (or end) frame
This modeling technique makes each joint
axis (either rotation or extension) the Z-axis of
the appropriate frame (Z0 to Zn-1).
The Joint motion, thus, is taken W.R.T. the Z i-1
axis of the frame pair making up the specific
transformation matrix under design

The D-H Modeling Rules:

1)

Locate & Label the Joint Axes: Z0 to Zn-1

2)

Establish the Base Frame. Set Base Origin

anywhere on the Z0 axis. Choose X0 and Y0
conveniently and to form a right hand frame.
Locate the origin Oi where the common normal
to Zi-1 and Zi intersects Zi. If Zi intersects Zi-1
locate Oi at this intersection. If Zi-1 and Zi are
parallel, locate Oi at Joint i+1

3)

The D-H Modeling Rules:

4)

Establish Xi along the common normal between

Zi-1 and Zi through Oi, or in the direction Normal to the plane
Zi-1 Zi if these axes intersect

5)

Establish Yi to form a right hand system

Set i = i+1, if i<n loop back to step 3
(Repeat Steps 3 to 5 for I = 1 to I = n-1)

6)

Establish the End-Effector (n) frame: OnXnYnZn. Assuming

the n-th joint is revolute, set kn = a along the direction Zn-1.
Establish the origin On conveniently along Zn, typically
mounting point of gripper or tool. Set jn = o in the direction of
gripper closure (opening) and set in = n such that n = oxa.
Note if tool is not a simple gripper, set Xn and Yn conveniently
to form a right hand frame.

The D-H Modeling Rules:

7)

Create a table of Link parameters:

1)
2)
)
4)

8)

9)

i as angle about Zi-1 between Xs

di as distance along Zi-1
i as angle about Xi between Zs
ai as distance along Xi

Form HTM matrices A1, A2, An by

substituting , d, and a into the general
model
Form T0n = A1*A2**An

Some Issues to remember:

If you have parallel Z axes, the X axis of the second

frame runs perpendicularly between them
When working on a revolute joint, the model will be
simpler if the two X directions are in alignment at
Kinematic Home ie. Our models starting point
To achieve this kinematic home, rotate the model about
moveable axes (Zi-1s) to align Xs
Kinematic Home is not particularly critical for prismatic
joints
An ideal model will have n+1 frames
However, additional frames may be necessary these
are considered Dummy frames since they wont
contain joint axes

Applying D-H to a General Case:

General Case: Considering Link i

Connects Frames: i-1 and I and

includes Joint i

Frames

Link Var

i -1 to i

+ 37

17.5 u

47.8 u

17.8

0.306

.952

S( + 37)

C( + 37)

This information allowed us to Build

The L.P. (link parameter) Table as
seen here

Leads to an Ai Matrix:
C ( 37) .952 S ( 37) .306 S ( 37) 47.8 * C ( 37)
S ( 37) .952 C ( 37) .306 C ( 37) 47.8 S ( 37)

0
0.306
.952
17.5

0
0
0
1

Frame Skeleton for Prismatic

Hand Robot

LP Table:
Frames

Link

Var

0 1

-90

-1

S1

C1

1 2

-1

S2

C2

23

0.5

90

S3

C3

34

d4 + 4.25 0

S4

C4

4n

S5

C5

Depends on Location
of n(end)-frame!

Leading to 5 Ai Matrices
C1
S1

S1

C1

A1

0
0

C 2 S 2 0 6 C 2
S 2 C 2 0 6 S 2

A2
0
0
1
1

0
0
0
1

C3
S3
A3
0

0
1
0
A4
0

S3

0
1
0

C 3
0
0

0 0

1 0
0 1
0 0

0
0

.5

4.25 d 4

#5 is:
C5
S5
A5
0

S 5

C5
0
0

0
1
0

Now, Lets Form the FKS:

T0n = A1*A2*A3*A4*A5
Ill use a software: Mathematica

0
0

This value is
called the
Hand Span
and depends
on the Frame
Skeleton we
developed

Solving for FKS

Here we have a special case two of the

Joints are a planer arm revolute model i.e.
parallel, consecutive revolute joints
These are contained in the A2 and A3 Matrices
These should be pre-multiplied using a
trigonometric tool that recognizes the sum of
angle cases ((Full)Simplify in mathematica)
Basically then: T0n = A1*{A2A3}*A4*A5

Finalizing the FKS perform a

physical verification

Physical verification means to plug

known angles into the variables and
compute the Ais and FKS against the
Frame Skeleton

Another? 6dof Articulating Arm

(The Figure Contains Frame Skelton)

LP Table
Frame
s

Lin
k

Var

0 1

90

-1

S1

C1

1 2

a2

S2

C2

23

a3

S3

C3

34

a4

-90

-1

S4

C4

45

90

S5

C5

56

6*

d6

S6

C6

* With End Frame in Better Kinematic Home. Here,

as shown, it would be (6 - 90), which is a problem!

A Matrices
C1
S1
A1
0

0
0
1
0

0
C1 0

0
0

0
1
S1

C 3 S 3 0 a3 C 3
S 3 C 3 0 a S 3
3

A3
0
0 1
0

0 0
1
0

C 2 S 2
S 2 C2
A2
0
0

0
0

0 a2 C 2
0 a2 S 2

1
0

0
1

C 4 0 S 4 a4 C 4
S 4 0 C 4 a S 4
4

A4
0 1 0
0

0
0
0
1

A Matrices, cont.

C5
S5
A5
0

0
0 C 5 0

1
0
0

0
0
1
0

S5

C 6 S 6
S 6 C6
A6
0
0

0
0

0 0

0 0

1 d 6

0 1

At Better Kinematic Home!

Leads To:

FKS of:

T A1 g A2 gA3 gA4 PreProcess gA5 gA6

n
0

After Preprocessing A2-A3-A4, with

(Full)Simplify function, the FKS must be
reordered as A1-A234-A5-A6 and Simplified

Solving for FKS

Pre-process {A2*A3*A4} with Full

Simplify
They are the planer arm issue as in
the previous robot model
Then Form: A1* {A2*A3*A4}*A5*A6

Simplify for FKS!

Simplifies to
(in the expected Tabular Form):
nx = C1(C5C6C234 - S6S234) - S1S5C6
ny = C1S5C6 + S1(C5C6C234 - S6S234)
nz = S6C234 + C5C6S234
ox = S1S5S6 - C1(C5S6C234 + C6S234)
oy = - C1S5S6 - S1(C5S6C234 + C6S234)
oz = C6C234 - C5S6S234
ax = C1S5C234 + S1C5
ay = S1S5C234 - C1C5
az = S5S234
dx = C1(C234(d6S5 + a4) + a3C23 + a2C2) + d6S1C5
dy = S1(C234(d6S5 + a4) + a3C23 + a2C2) - d6C1C5
dz = S234(d6S5 + a4) + a3S23 + a2S2

Verify the Model

Again, substitute knowns (typically 0

or 0 units) for the variable kinematic
variables
Check each individual A matrix against
your robot frame skeleton sketch for
physical agreement
Check the simplified FKS against the
Frame skeleton for appropriateness

After Substitution:

nx = C1(C5C6C234 - S6S234) - S1S5C6 = 1(1-0) 0 = 1

ny = C1S5C6 + S1(C5C6C234 - S6S234) = 0+ 0(1 0) = 0
nz = S6C234 + C5C6S234 = 0 + 0 = 0
ox = S1S5S6 - C1(C5S6C234 + C6S234) = 0 1(0 + 0) = 0
oy = - C1S5S6 - S1(C5S6C234 + C6S234) = -0 0(0 + 0) = 0
oz = C6C234 - C5S6S234 = 1 0 = 1
ax = C1S5C234 + S1C5 = 0 + 0 = 0
ay = S1S5C234 - C1C5 = 0 1 = -1
az = S5S234 = 0
dx = C1(C234(d6S5 + a4) + a3C23 + a2C2) + d6S1C5
= 1*(1(0 + a4) + a3 + a2) + 0 = a4 + a3 + a2
dy = S1(C234(d6S5 + a4) + a3C23 + a2C2) - d6C1C5
= 0(1(0 + a4) + a3 + a2) d6 = -d6
dz = S234(d6S5 + a4) + a3S23 + a2S2
= 0(0 + a4) + 0 + 0 = 0