Ɵ α a d

A0-1 q1 -90 0 0
A1-2 q2 0 L1 0
A2-3 q3 90 L2 0
A3-4 q4 -90 0 0
A4-5 q5 90 0 0
A5-6 q6 0 0 L3

L1=410; q1=40;
L2=322; q2=50;
L3=15; q3=-25;
q4=40;
q5=50;
q6=-25;

M1 = [cosd(q1), 0, -sind(q1), 0;
sind(q1), 0, cosd(q1), 0;
0, -1, 0, 0;
0, 0, 0, 1];

M2= [cosd(q2), -sind(q2), 0, cosd(q2)*L1;

sind(q2), cosd(q2), 0, sind(q2)*L1;
0, 0, 1, 0;
0, 0, 0, 1];

R1 = M1 * M2
R1 =

0.4924 -0.5868 -0.6428 201.8856

0.4132 -0.4924 0.7660 169.4021
-0.7660 -0.6428 0 -314.0782
0 0 0 1.0000

M3= [cosd(q3), 0, sind(q3), cosd(q3)*L2;

sind(q3), 0, -cosd(q3), sind(q3)*L2;
0, 1, 0, 0;
0, 0, 0, 1];

R2 = R1 * M3
R2 =

0.6943 -0.6428 0.3237 425.4412

0.5826 0.7660 0.2717 356.9875
-0.4226 0 0.9063 -450.1613
0 0 0 1.0000

M4 = [cosd(q4), 0, -sind(q4), 0;
sind(q4), 0, cosd(q4), 0;
0, -1, 0, 0;
0, 0, 0, 1];

R3 = R2 * M4
R3 =

0.1187 -0.3237 -0.9387 425.4412

0.9387 -0.2717 0.2124 356.9875
-0.3237 -0.9063 0.2717 -450.1613
0 0 0 1.0000

M5 = [cosd(q5), 0, sind(q5), 0;
sind(q5), 0, -cosd(q5), 0;
0, 1, 0, 0;
0, 0, 0, 1];

R4 = R3 * M5
R4 =

-0.1717 -0.9387 0.2990 425.4412

0.3953 0.2124 0.8937 356.9875
-0.9024 0.2717 0.3346 -450.1613
0 0 0 1.0000

M6 = [cosd(q6), -sind(q6), 0, 0;
sind(q6), cosd(q6), 0, 0;
0, 0, 1, L3;
0, 0, 0, 1];
R5 = R4 * M6
R5 =

0.2411 -0.9233 0.2990 429.9262

0.2685 0.3595 0.8937 370.3928
-0.9326 -0.1352 0.3346 -445.1429
0 0 0 1.0000