Robot Modeling and the Forward Kinematic Solution

ME 4135Lecture Series 4

Looking Closely at the T0n Matrix

At the end of our discussion of “Robot Mapping” we found that the T0

n 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:1 2

0 0 1 1n n

nT T T T

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, T0

n = A1*A2*A3

While for a full functioned 6 DOF robot (arm and wrist) would be: T0

n = 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: Two translations (in a controlled sense) Two rotations (also in a controlled sense)

A rotation and translation WRT the Zi-1

These are called the Joint Parameters A rotation and translation WRT the Xi

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 don’t 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:

os 0 00 0

0 0 1 00 0 0 1

C SinSin Cos

1 0 0 00 1 0 00 0 10 0 0 1

d

Returning to the 4 Frame Operators:

3rd is a Translation Along X or:

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

1 0 00 1 0 00 0 1 00 0 0 1

a

1 0 0 00 00 00 0 0 1

Cos SinSin Cos

The Overall Effect is:

os 0 0 1 0 0 0 1 0 0 1 0 0 00 0 0 1 0 0 0 1 0 0 0 0

0 0 1 0 0 0 1 0 0 1 0 0 00 0 0 1 0 0 0 1 0 0 0 1 0 0 0 1

C Sin aSin Cos Cos Sin

d Sin Cos

Simplifying this Matrix Product:

00 0 0 1

i i i i i i i

i i i i i i i

i i i

C S C S S a CS C C C S a S

S C d

This matrix is the general transformation relating each and every of the frame pairs

along a robot structure

So, Since We Can Control the Building of this Set Of Frames, What Are The Rules?

We will employ a method called the Denavit-Hartenberg 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 Zi-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. Then Repeat Steps 3 to 5 for i = 1 through i = n-1:

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

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

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) i as angle about Zi-1 between X’s2) di as distance along Zi-1 3) i as angle about Xi between Z’s4) ai as distance along Xi

8) Form HTM matrices A1, A2, … An by substituting , d, and a into the general model

9) 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 model’s starting point

To achieve this kinematic home, rotate the model about moveable axes (Zi-1’s) to align X’s

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 won’t contain joint axes

Applying D-H to a General Case:

Case 1: Like a part of a biological arm

Case 2: Like a part of a R-P manipulator

And Note:

General Case: Considering Link i Connects Frames: i-1 and I and

includes Joint i

This information allowed us to ‘Build’ The L.P. (link parameter) Table seen here

Frames Link Var d a S C S C

i -1 to i i R + 37

17.5 u 47.8 u 17.8 0.306 .952 S( + 37) C( + 37)

Leads to an Ai Matrix:

( 37 ) .952 ( 37 ) .306 ( 37 ) 47.8* ( 37 )( 37 ) .952 ( 37 ) .306 ( 37 ) 47.8 ( 37 )0 0.306 .952 17.50 0 0 1

C S S CS C C S

Lindeke’s Folly – The “Plastic Handed” Robot

Frame Skeleton for Prismatic Hand Robot

LP Table:Frames Link Var d a S C S C

0 → 1 1 R 10 0 -90 -1 0 S1 C1

1 → 2 2 R 2-1 6 0 0 1 S2 C2

2 → 3 3 R 30.5 0 90 1 0 S3 C3

3 → 4 4 P 0 d4 + 4.25 0 0 0 1 S4 C4

4 → n 5 R 51 0 0 0 1 S5 C5

Depends on Location of n-frame!

Leading to 5 Ai Matrices

1

1 0 1 01 0 1 00 1 0 00 0 0 1

C SS C

A

3

3 0 3 03 0 3 00 1 0 .50 0 0 1

C SS C

A

2

2 2 0 6 22 2 0 6 20 0 1 10 0 0 1

C S CS C S

A

44

1 0 0 00 1 0 00 0 1 4.250 0 0 1

Ad

#5 is:

5

5 5 0 05 5 0 00 0 1 10 0 0 1

C SS C

A

Now, Lets Form the FKS: T0

n = A1*A2*A3*A4*A5 Best to use a software: MathCad,

Mathematica, etc.

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 or MathCad Simplify for A2*A3 then Evaluate for A1*{A2*A3}*A4*A5)

Basically then: T0n = A1*{A2A3}*A4*A5

Extracts from Math Cad Analyses:

Continuing

Continuing

In Tabular Form: Nx: C23C1C5 - S1S5

Ny: C1S5 + C23C5S1

Nz: -S23C5

Ox: -C23C1S5 - S1C5

Oy: C1C5 - C23S1S5

Oz: S23S5

Ax: S23C1

Ay: S1S23

Az: C23

Dx: S23C1 + 6C1C2 + 0.5S1 + S23C1(d4+4.25) Dy: S23S1 + 6S1C2 - 0.5C1 + S23S1(d4+4.25) Dz: C23 - 6S2 + C23(d4+4.25)

Finalizing the FKS – perform a physical verification

Physical verification means to plug known angles into the variables and compute the Ai’s and FKS against the Frame Skeleton

Physical Verification in MathCad:

Physical Verification of FKS:

Another? 6dof Articulating Arm – (The Figure Contains Frame Skelton)

LP Table

Frames Link Var d a S C S C 0 → 1 1 R 1 0 0 90 -1 0 S1 C1

1 → 2 2 R 2 0 a2 0 0 1 S2 C2

2 → 3 3 R 3 0 a3 0 0 1 S3 C3

3 → 4 4 R 4 0 a4 -90 -1 0 S4 C4

4 → 5 5 R 5 0 0 90 1 0 S5 C5

5 → 6 6 R 6* d6 0 0 0 1 S6 C6

* With End Frame in Better Kinematic Home, here, as shown it could be (6 - 90), which is a problem!

A Matrices

1

1 0 1 01 0 1 00 1 0 00 0 0 1

C SS C

A

2

22

2 2 0 22 2 0 20 0 1 00 0 0 1

C S a CS C a S

A

3

33

3 3 0 33 3 0 30 0 1 00 0 0 1

C S a CS C a S

A

4

44

4 0 4 44 0 4 40 1 0 00 0 0 1

C S a CS C a S

A

A Matrices, cont.

5

5 0 5 05 0 5 00 1 0 00 0 0 1

C SS C

A

66

6 6 0 06 6 0 00 0 10 0 0 1

C SS C

Ad

Leads To:

FKS of:

0 1 2 3 4 5 6nT A A A A A A

PreProcess After Preprocessing A2-A3-A4, to collect

angular effect, the FKS must be reordered as A1-A234-A5-A6 and Simplified

Solving for FKS

Pre-process {A2*A3*A4} with Evaluate Simplify

They are the planer arm issue as in the previous robot model

Then Form: A1* {A2*A3*A4}*A5*A6

Evaluate for FKS!

Simplifies to(in the expected Tabular Form):

nx = C1·(C5·C6·C234 - S6·S234) - S1·S5·C6ny = C1·S5·C6 + S1·(C5·C6·C234 - S6·S234)nz = S6·C234 + C5·C6·S234

ox = S1·S5·S6 - C1·(C5·S6·C234 + C6·S234)oy = - C1·S5·S6 - S1·(C5·S6·C234 + C6·S234)oz = C6·C234 - C5·S6·S234

ax = C1·S5·C234 + S1·C5ay = S1·S5·C234 - C1·C5az = S5·S234

dx = C1·(C234·(d6·S5 + a4) + a3·C23 + a2·C2) + d6·S1·C5dy = S1·(C234·(d6·S5 + a4) + a3·C23 + a2·C2) - d6·C1·C5dz = S234·(d6·S5 + a4) + a3·S23 + a2·S2

Verify the Model

Again, substitute known’s (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·(C5·C6·C234 - S6·S234) - S1·S5·C6 = 1(1-0) – 0 = 1 ny = C1·S5·C6 + S1·(C5·C6·C234 - S6·S234) = 0+ 0(1 – 0) = 0 nz = S6·C234 + C5·C6·S234 = 0 + 0 = 0

ox = S1·S5·S6 - C1·(C5·S6·C234 + C6·S234) = 0 – 1(0 + 0) = 0 oy = - C1·S5·S6 - S1·(C5·S6·C234 + C6·S234) = -0 – 0(0 + 0) = 0 oz = C6·C234 - C5·S6·S234 = 1 – 0 = 1

ax = C1·S5·C234 + S1·C5 = 0 + 0 = 0 ay = S1·S5·C234 - C1·C5 = 0 – 1 = -1 az = S5·S234 = 0

dx = C1·(C234·(d6·S5 + a4) + a3·C23 + a2·C2) + d6·S1·C5= 1*(1(0 + a4) + a3 + a2) + 0 = a4 + a3 + a2 so here is (.2+1.5+1.5 = 3.2 m)

dy = S1·(C234·(d6·S5 + a4) + a3·C23 + a2·C2) - d6·C1·C5= 0(1(0 + a4) + a3 + a2) – d6 = -d6 so here is -.25 m

dz = S234·(d6·S5 + a4) + a3·S23 + a2·S2= 0(0 + a4) + 0 + 0 = 0

And Physical Verification w/ Math Cad: