Lab 3: D-H representation, forward and inverse kinematics

ECE 4330
Rafael Resende Marcondes ID: 14256580

1. Introduction

The Denavit-Hartenberg (D-H) representation can be used to model robot links and

joint, giving a matrix that is a function of the joint angles and the link distances. That is

especially useful to achieve the forward and the inverse kinematics of a specific robot.

The forward kinematics is the process of calculating the position and orientation of the

end-effector of the robot given that you know the desired joint angles. So it is easily

obtained once you have the D-H matrix of the robot.

The inverse kinematics represent the inverse process of the forward kinematics: given

that the position and orientation of the robots end-effector is known, the goal is to find

the joint angles of the robot to achieve that position.

Knowing that, the goals of this laboratory are:

Utilize the D-H representation of the PUMA 260 to solve the forward kinematics.

Verify the result obtained for the forward kinematics comparing it to the actual

position given a certain joint configuration.

Solve the inverse kinematics for the PUMA 260 given a certain XYZOAT.

Verify the result of the inverse kinematics using the robot.

2. Procedure and Results

This lab consists of two different parts that are explained below:
 Part 1: Forward Kinematics

In this first part the D-H table of the PUMA 260 robot was used to obtain the A matrix,

and that matrix was then used to find the forward kinematics of the robot.

Figure 1 - D-H representation of the PUMA 260 robot

 The Figure 1 shows the D-H representation for the PUMA 260 where all joint angles

are equal to zero. With this representation it is possible to build the D-H table as follow:

i i di ai i
1 1 0 0 -90
2 2 2 7/8 + 2 1/16 8 0
3 3 0 0 90
4 4 d4 0 -90
5 5 0 0 90
6 6 d6 0 0

And that table gives six matrix, one for each line as follow:

1 0 1 0 2 2 0 203.22
1 0 1 0 2 2 0 203.22
1 = [ ] 2 = [ ]
0 1 0 0 0 1 0 125.8
0 0 0 1 0 0 0 1

4 0 4 0 5 0 5 0
4 0 4 0 5 0 5 0
4 = [ ] 5 = [ ]
0 1 203.2 0 0 1 0 0
0 0 0 1 0 0 0 1

3 0 3 0 6 6 0 0
3 0 3 0 6 6 0 0
3 = [ ] 6 = [ ]
0 1 0 0 0 0 1 55.93
0 0 0 1 0 0 0 1

Finally, multiplying these six matrix we get the final D-H matrix. This matrix gives us

the transformation of the first joint onto the sixth joint.

Since these calculations are very complex the software MatLab was used to assist into

obtaining the final matrix. For that, the command sym was used. The code on this first

part was the following:

T1 = 30;
T2 = 30;
T3 = 30;
T4 = 30;
T5 = 30;
T6 = 30;

c1 = sym (cos(degtorad(T1)));
s1 = sym (sin(degtorad(T1)));
c2 = sym (cos(degtorad(T2)));
s2 = sym (sin(degtorad(T2)));
c3 = sym (cos(degtorad(T3)));
s3 = sym (sin(degtorad(T3)));
c4 = sym (cos(degtorad(T4)));
s4 = sym (sin(degtorad(T4)));
c5 = sym (cos(degtorad(T5)));
s5 = sym (sin(degtorad(T5)));
c6 = sym (cos(degtorad(T6)));
s6 = sym (sin(degtorad(T6)));

d2 = 125.8;
d3 = 0;
A1 = [c1 0 -s1 0; s1 0 c1 0;
0 -1 0 0; 0 0 0 1]
A2 = [c2 -s2 0 203.2*c2; s2 c2 0 203.2*s2;
0 0 1 d2; 0 0 0 1]
A3 = [c3 0 s3 0; s3 0 -c3 0;
0 1 0 0; 0 0 0 1]
A4 = [c4 0 -s4 0; s4 0 c4 0;
0 -1 0 203.2; 0 0 0 1]
A5 = [c5 0 s5 0; s5 0 -c5 0;
0 1 0 0; 0 0 0 1]
A6 = [c6 -s6 0 0; s6 c6 0 0;
0 0 1 55.93; 0 0 0 1]

A_final = double(A1*A2*A3*A4*A5*A6)

Tr = atan2(-(A_final(3,2)),A_final(3,1))
Or = atan2(-(A_final(1,3)),A_final(2,3))
Ar = atan2((A_final(2,2)-sin(Or)*cos(Tr))/(-
cos(Or)*sin(Tr)),A_final(1,3)/sin(Or))

T = radtodeg(Tr)
O = radtodeg(Or)
A = radtodeg(Ar)
X = A_final(1,4)
Y = A_final(2,4)
Z = A_final(3,4)

object_xy = sprintf('Puma_MOVETO_JOINTS %f, %f, %f, %f, %f, %f', T1, T2, T3,
T4, T5, T6);

system(object_xy);
 The matrix (A_final) found is as follow, for the joints angles set to be all 30:

A_final =

-0.5558 -0.4291 0.7120 281.7232

0.6121 0.3683 0.6998 324.0599
-0.5625 0.8248 0.0580 3.2447
0 0 0 1.0000

After finding the A_final matrix the values of XYZ were already defined, being the first

three elements of the last column respectively.

The next step was to find the values of OAT, which is a little trickier. To do that, we

need to compare the matrix A_final found with the OAT matrix for the PUMA 260 robot

that is derived by the definition of these angles.

+ +
60 = [ + + ]

That way we can use the arc tangent to find the desired angles, which was done on

the following excerpt of the code:

Tr = atan2(-(A_final(3,2)),A_final(3,1))
Or = atan2(-(A_final(1,3)),A_final(2,3))
Ar = atan2((A_final(2,2)-sin(Or)*cos(Tr))/(-
cos(Or)*sin(Tr)),A_final(1,3)/sin(Or))

The values found were in radians, so to find the correspondent in degrees they were

multiplied by 180/pi.

The final values found for the joints angles set to be all 30 are:

T= X=
55.7053 281.7232
O= Y=
134.5025 324.0599
A= Z=
-3.3258 3.2447
 To verify if they were correct, the robot was commanded to go to the position that

represents that configuration, using the command PUMA_MOVETO_JOINTS. When the

robot was in the right place, the position on the end-effector was read using the command

HERE OBJECT, as shown on figure 2.

Figure 2 - Position of the PUMA 260 when the joints angles are 30

As can be seen, the values found and the actual values for the position of the robot

are very similar, meaning that the procedure used was correct.

The same was done to three other joint configurations, leading to the following results:

Joint angles set to 20

T= X=
34.8424 301.5890
O= Y=
117.8338 250.6054
A= Z=
-30.8811 114.8683
 Figure 3 - Position of the PUMA 260 when the joints angles are 20

Joint angles set to 10

Figure 4 - Position of the PUMA 260 when the joints angles are 10
As can be seen from the examples, all the cases tested worked well, resulting in an

small error.

Part 2: Inverse kinematics

In this second part of the lab, the inverse kinematics algorithm learnt in class was

tested, to try to find the joint angles given a XYZ-OAT.

Following the algorithm, the first three joint angles should be able to be calculated

after finding the position of joint 4, which is direct since its direction is the same of the

joint 6.
 That way, the values of Px, Py and Pz above could be found.

After some mathematical work, we find that the first joint angle should be:

To test this expression, the MatLab code below was used and tested for the value of

XYZ-OAT found in Part 1 (joints angles 10, 20 and 30).

%% teta 1

Teta1 = atan2(py*sqrt(px^2+py^2-d2^2)-px*d2,px*sqrt(px^2+py^2-d2^2)+py*d2)
Teta1d = rad2deg(Teta1)

The answer obtained was:

Teta1d = 10.3245 when the expected was 10

Teta1d = 21.0124 when the expected was 20
Teta1d = 31.9621 when the expected was 30

So we can conclude that this expression is correct.

The second joint angle is a function of the third one, so we tried to solve the third first.

Following the procedures, we find the expression:

2 + 2 + 2 22 32 22 + 42 = = 22 4 3 + 22 3 3

Since 3 is 0 for the robot used, the joint 3 angle should be obtained by

1
2 + 2 + 2 22 22 + 42
3 = sin ( )
22 4
 The answer for that was 30 when it should be 10, so we conclude that the fact of

3 being 0 makes the deduction of the angles totally different from the learnt on class.

Another solutions were looked on the web and even on the textbook, but none was simple

enough to be used nor gave the correct answer.

3. Conclusion

In this laboratory it was possible to obtain the forward kinematics of the PUMA 260
using the D-H representation. The results obtained on this part of the lab were very
satisfactory being almost equal to the real position of the robot for all angles tested. The
second part of the lab, which was to find the inverse kinematics, showed a lot of
complications, many derived from the fact that the robot configuration had been changed
during a revision. Being that way, just the first joint angle could be found, so a solution is
still being searched. Besides the problems found, it was possible to learn the principles
of the forward and the inverse kinematics of the PUMA 260 and apply most of them.