Transcript Foward Kinematics-

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 Zi-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 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:
C os
 Sin

 0

 0
1
0

0

0
 Sin
Cos
0
0
0
1
0
0
0
1
0
0
0 0
0 0

1 0

0 1
0
0

d

1
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

0

0
0
1
0
0
0
1
0
0
0
1
0 Cos

0 Sin

0
0
0
 Sin
Cos
0
a
0

0

1
0
0

0

1
The Overall Effect is:
C os
 Sin

 0

 0
 Sin
Cos
0
0
0 0  1
0 0  0

1 0  0
 
0 1  0
0 0 0  1
1 0 0  0

0 1 d  0
 
0 0 1  0
0 0 a  1
0
1 0 0  0 Cos

0 1 0  0 Sin
 
0 0 1  0
0
0
 Sin
Cos
0
0
0

0

1
Simplifying this Matrix Product:
C i
 S
 i
 0

 0
 S i  C i
C i  C i
S i
S i  S i
C i  S i
C i
0
0
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 

1 
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
Zi-1 axis of the frame pair making up the
specific transformation matrix under design
The D-H Modeling Rules:
1)
2)
3)
Locate & Label the Joint Axes: Z0 to Zn-1
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
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)
3)
4)
8)
9)
i as angle about Zi-1 between X’s
di as distance along Zi-1
i as angle about Xi between Z’s
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 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:
General Case: Considering Link i

Connects Frames: i-1 and I and
includes Joint i
Frames
Link Var

d
a

S
C
S
C
i -1 to i
i
 + 37
17.5 u
47.8 u
17.8 
0.306
.952
S( + 37)
C( + 37)

R
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

d
a

S
C
S
C
0 1
1
R
1
0
0
-90
-1
0
S1
C1
1 2
2
R
2
-1
6
0
0
1
S2
C2
23
3
R
3
0.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
5
1
0
0
1
S5
C5
Depends on Location
of n(end)-frame!
0
Leading to 5 Ai Matrices
C1
 S1
A1  
0

0
0
0
1
 S1
C1
0
0
0
C 2  S 2
S 2 C2
A2  
 0
0

0
 0
0
0

0

1
0 6  C 2
0 6  S2

1
1 

0
1 
C 3
 S3
A3  
 0

 0
1
0
A4  
0

0
0
0
1
S3
C 3
0
0
0
0 0
1 0
0 1
0 0
0
0

.5

1
0


0

4.25  d 4 

1

#5 is:
C 5
 S5
A5  
 0

 0


S 5
C5
0
0
0
1
0
0
0
0

1

1
Now, Lets Form the FKS:
T0n = A1*A2*A3*A4*A5
I’ll use a software: Mathematica
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*{A2A3}*A4*A5
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
Another? 6dof Articulating Arm –
(The Figure Contains Frame Skelton)
LP Table
Frame
s
Lin
k
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 would be (6 - 90), which is a problem!
A Matrices
C1
 S1
A1  
0

0
0
S1 0 
C1 0 

0
0

0
1
C 2  S 2
S 2 C2
A2  
0
0

0
0
C 3  S 3
 S 3 C3
A3  
0
0

0
0
0 a3  C 3
0 a3  S 3 

1
0 

0
1 
 C 4 0  S 4 a4  C 4 
 S 4 0 C4 a  S 4
4

A4  
 0 1 0
0 


0
0
0
1


0
0
1
0 a2  C 2 
0 a2  S 2 

1
0 

0
1 
A Matrices, cont.
C 5
 S5
A5  
0

0
0 S 5 0
0 C 5 0 

1
0
0

0
0
1
C 6  S 6
 S 6 C6
A6  
0
0

0
0
0 0

0 0

1 d6 

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·(C5·C6·C234 - S6·S234) - S1·S5·C6
ny = 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·C5
ay = S1·S5·C234 - C1·C5
az = S5·S234
dx = C1·(C234·(d6·S5 + a4) + a3·C23 + a2·C2) + d6·S1·C5
dy = S1·(C234·(d6·S5 + a4) + a3·C23 + a2·C2) - d6·C1·C5
dz = 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
dy = S1·(C234·(d6·S5 + a4) + a3·C23 + a2·C2) - d6·C1·C5
= 0(1(0 + a4) + a3 + a2) – d6 = -d6
dz = S234·(d6·S5 + a4) + a3·S23 + a2·S2
= 0(0 + a4) + 0 + 0 = 0