Michał Grudziński

grupa 24A

Marcin Krawiec

Roboty przemysłowe laboratorium 2
Model geometryczny manipulatora robota przemysłowego

1. Schemat kinematyczny robota MOTOMAN – DA20
2. Tabela parametrów powyższego robota

3. Program wyznaczający macierze opisującą położenie i orientację efektora

% skrypt wylicza macierz przekształcenia jednorodnego

clear

all

% deklaracja symboli występujących w modelu

% zmienne qi tworzone są tylko dla współrzędnych obrotowych

syms

th1

th2

th3

th4

th5

th6

th7

d1

d3

d5

d7

a1

a3

syms

q1

q2

q3

q4

q5

q6

q7

% wyznaczenie symbolicznej postaci macierzy - funkcja mA

A1=mA(th1+sym(pi/18),d1,a1,sym(-pi/2))
A2=mA(th2,0,0,sym(pi/2))
A3=mA(th3,d3,a3,0)
A4=mA(th4,0,0,sym(pi/2))
A5=mA(th5,d5,0,sym(-pi/2))
A6=mA(th6,0,0,sym(pi/2))
A7=mA(th7,d7,0,0)

% mnozenie macierzy

T07=A1*A2*A3*A4*A5*A6*A7

% podstawienie nowych zmiennych za obrotowe zmienne złączowe

% do upraszczania zapisu macierzy przekształcenia jednorodnego

% zmienne thi lub ali sa zamieniane na qi

T07v=subs(T07,{th1 th2 th3 th4 th5 th6 th7},{q1 q2 q3 q4 q5 q6 q7})

% wskazanie zmiennych złączowych

% zmienne: th1 th2 th3 th4 th5 th6 th7 wskazuj? ‘1’-ki

zmie=[[1,0,0,0];[1,0,0,0];[1,0,0,0];[1,0,0,0];[1,0,0,0];[1,0,0,0];[1,0,0,0]]

% uproszczona postać macierzy przekształcenia jednorodnego

% dla celów interpretacji

T07u=zam(zmie,T07v)

% podstawienie wartości zmiennych złączowych

% i stałych parametrów do macierzy T0e

% należy podstawić dane odpowiednio w radianach lub metrach

T0en=subs(T07,{th1,a1,th2,a2,a3,th4,th5,th6,d6},{pi/6,0.6,pi/4,0.4,0.4,pi/3,pi/5,pi/4,0.1})

4.

Wyznaczone macierze:

T07 =

[ sin(th7)*(sin(th5)*(cos(th4)*(sin(th3)*sin(pi/18 + th1) - cos(th2)*cos(th3)*cos(pi/18 +

th1)) + sin(th4)*(cos(th3)*sin(pi/18 + th1) + cos(th2)*cos(pi/18 + th1)*sin(th3))) +

cos(th5)*cos(pi/18 + th1)*sin(th2)) - cos(th7)*(sin(th6)*(cos(th4)*(cos(th3)*sin(pi/18 +

th1) + cos(th2)*cos(pi/18 + th1)*sin(th3)) - sin(th4)*(sin(th3)*sin(pi/18 + th1) -

cos(th2)*cos(th3)*cos(pi/18 + th1))) + cos(th6)*(cos(th5)*(cos(th4)*(sin(th3)*sin(pi/18 +

th1) - cos(th2)*cos(th3)*cos(pi/18 + th1)) + sin(th4)*(cos(th3)*sin(pi/18 + th1) +

cos(th2)*cos(pi/18 + th1)*sin(th3))) - cos(pi/18 + th1)*sin(th2)*sin(th5))),

sin(th7)*(sin(th6)*(cos(th4)*(cos(th3)*sin(pi/18 + th1) + cos(th2)*cos(pi/18 +

th1)*sin(th3)) - sin(th4)*(sin(th3)*sin(pi/18 + th1) - cos(th2)*cos(th3)*cos(pi/18 + th1)))

+ cos(th6)*(cos(th5)*(cos(th4)*(sin(th3)*sin(pi/18 + th1) - cos(th2)*cos(th3)*cos(pi/18 +

th1)) + sin(th4)*(cos(th3)*sin(pi/18 + th1) + cos(th2)*cos(pi/18 + th1)*sin(th3))) -

cos(pi/18 + th1)*sin(th2)*sin(th5))) + cos(th7)*(sin(th5)*(cos(th4)*(sin(th3)*sin(pi/18 +

th1) - cos(th2)*cos(th3)*cos(pi/18 + th1)) + sin(th4)*(cos(th3)*sin(pi/18 + th1) +

cos(th2)*cos(pi/18 + th1)*sin(th3))) + cos(th5)*cos(pi/18 + th1)*sin(th2)),

cos(th6)*(cos(th4)*(cos(th3)*sin(pi/18 + th1) + cos(th2)*cos(pi/18 + th1)*sin(th3)) -

sin(th4)*(sin(th3)*sin(pi/18 + th1) - cos(th2)*cos(th3)*cos(pi/18 + th1))) -

sin(th6)*(cos(th5)*(cos(th4)*(sin(th3)*sin(pi/18 + th1) - cos(th2)*cos(th3)*cos(pi/18 +

th1)) + sin(th4)*(cos(th3)*sin(pi/18 + th1) + cos(th2)*cos(pi/18 + th1)*sin(th3))) -

cos(pi/18 + th1)*sin(th2)*sin(th5)), d7*(cos(th6)*(cos(th4)*(cos(th3)*sin(pi/18 + th1) +

cos(th2)*cos(pi/18 + th1)*sin(th3)) - sin(th4)*(sin(th3)*sin(pi/18 + th1) -

cos(th2)*cos(th3)*cos(pi/18 + th1))) - sin(th6)*(cos(th5)*(cos(th4)*(sin(th3)*sin(pi/18 +

th1) - cos(th2)*cos(th3)*cos(pi/18 + th1)) + sin(th4)*(cos(th3)*sin(pi/18 + th1) +

cos(th2)*cos(pi/18 + th1)*sin(th3))) - cos(pi/18 + th1)*sin(th2)*sin(th5))) + a1*cos(pi/18

+ th1) + d5*(cos(th4)*(cos(th3)*sin(pi/18 + th1) + cos(th2)*cos(pi/18 + th1)*sin(th3)) -

sin(th4)*(sin(th3)*sin(pi/18 + th1) - cos(th2)*cos(th3)*cos(pi/18 + th1))) + d3*cos(pi/18 +

th1)*sin(th2) - a3*sin(th3)*sin(pi/18 + th1) + a3*cos(th2)*cos(th3)*cos(pi/18 + th1)]

[ cos(th7)*(sin(th6)*(cos(th4)*(cos(th3)*cos(pi/18 + th1) - cos(th2)*sin(th3)*sin(pi/18 +

th1)) - sin(th4)*(cos(pi/18 + th1)*sin(th3) + cos(th2)*cos(th3)*sin(pi/18 + th1))) +

cos(th6)*(cos(th5)*(cos(th4)*(cos(pi/18 + th1)*sin(th3) + cos(th2)*cos(th3)*sin(pi/18 +

th1)) + sin(th4)*(cos(th3)*cos(pi/18 + th1) - cos(th2)*sin(th3)*sin(pi/18 + th1))) +

sin(th2)*sin(th5)*sin(pi/18 + th1))) - sin(th7)*(sin(th5)*(cos(th4)*(cos(pi/18 +

th1)*sin(th3) + cos(th2)*cos(th3)*sin(pi/18 + th1)) + sin(th4)*(cos(th3)*cos(pi/18 + th1) -

cos(th2)*sin(th3)*sin(pi/18 + th1))) - cos(th5)*sin(th2)*sin(pi/18 + th1)), -

sin(th7)*(sin(th6)*(cos(th4)*(cos(th3)*cos(pi/18 + th1) - cos(th2)*sin(th3)*sin(pi/18 +

th1)) - sin(th4)*(cos(pi/18 + th1)*sin(th3) + cos(th2)*cos(th3)*sin(pi/18 + th1))) +

cos(th6)*(cos(th5)*(cos(th4)*(cos(pi/18 + th1)*sin(th3) + cos(th2)*cos(th3)*sin(pi/18 +

th1)) + sin(th4)*(cos(th3)*cos(pi/18 + th1) - cos(th2)*sin(th3)*sin(pi/18 + th1))) +

sin(th2)*sin(th5)*sin(pi/18 + th1))) - cos(th7)*(sin(th5)*(cos(th4)*(cos(pi/18 +

th1)*sin(th3) + cos(th2)*cos(th3)*sin(pi/18 + th1)) + sin(th4)*(cos(th3)*cos(pi/18 + th1) -

cos(th2)*sin(th3)*sin(pi/18 + th1))) - cos(th5)*sin(th2)*sin(pi/18 + th1)),

sin(th6)*(cos(th5)*(cos(th4)*(cos(pi/18 + th1)*sin(th3) + cos(th2)*cos(th3)*sin(pi/18 +

th1)) + sin(th4)*(cos(th3)*cos(pi/18 + th1) - cos(th2)*sin(th3)*sin(pi/18 + th1))) +

sin(th2)*sin(th5)*sin(pi/18 + th1)) - cos(th6)*(cos(th4)*(cos(th3)*cos(pi/18 + th1) -

cos(th2)*sin(th3)*sin(pi/18 + th1)) - sin(th4)*(cos(pi/18 + th1)*sin(th3) +

cos(th2)*cos(th3)*sin(pi/18 + th1))), d7*(sin(th6)*(cos(th5)*(cos(th4)*(cos(pi/18 +

th1)*sin(th3) + cos(th2)*cos(th3)*sin(pi/18 + th1)) + sin(th4)*(cos(th3)*cos(pi/18 + th1) -

cos(th2)*sin(th3)*sin(pi/18 + th1))) + sin(th2)*sin(th5)*sin(pi/18 + th1)) -

cos(th6)*(cos(th4)*(cos(th3)*cos(pi/18 + th1) - cos(th2)*sin(th3)*sin(pi/18 + th1)) -

sin(th4)*(cos(pi/18 + th1)*sin(th3) + cos(th2)*cos(th3)*sin(pi/18 + th1)))) + a1*sin(pi/18

+ th1) - d5*(cos(th4)*(cos(th3)*cos(pi/18 + th1) - cos(th2)*sin(th3)*sin(pi/18 + th1)) -

sin(th4)*(cos(pi/18 + th1)*sin(th3) + cos(th2)*cos(th3)*sin(pi/18 + th1))) + a3*cos(pi/18 +

th1)*sin(th3) + d3*sin(th2)*sin(pi/18 + th1) + a3*cos(th2)*cos(th3)*sin(pi/18 + th1)]

[

sin(th7)*(cos(th2)*cos(th5) - sin(th5)*(sin(th2)*sin(th3)*sin(th4) -

cos(th3)*cos(th4)*sin(th2))) + cos(th7)*(cos(th6)*(cos(th2)*sin(th5) +

cos(th5)*(sin(th2)*sin(th3)*sin(th4) - cos(th3)*cos(th4)*sin(th2))) +

sin(th6)*(cos(th3)*sin(th2)*sin(th4) + cos(th4)*sin(th2)*sin(th3))),

cos(th7)*(cos(th2)*cos(th5) - sin(th5)*(sin(th2)*sin(th3)*sin(th4) -

cos(th3)*cos(th4)*sin(th2))) - sin(th7)*(cos(th6)*(cos(th2)*sin(th5) +

cos(th5)*(sin(th2)*sin(th3)*sin(th4) - cos(th3)*cos(th4)*sin(th2))) +

sin(th6)*(cos(th3)*sin(th2)*sin(th4) + cos(th4)*sin(th2)*sin(th3))),

sin(th6)*(cos(th2)*sin(th5) + cos(th5)*(sin(th2)*sin(th3)*sin(th4) -

cos(th3)*cos(th4)*sin(th2))) - cos(th6)*(cos(th3)*sin(th2)*sin(th4) +

cos(th4)*sin(th2)*sin(th3)),

d1 - d5*(cos(th3)*sin(th2)*sin(th4) + cos(th4)*sin(th2)*sin(th3)) +

d7*(sin(th6)*(cos(th2)*sin(th5) + cos(th5)*(sin(th2)*sin(th3)*sin(th4) -

cos(th3)*cos(th4)*sin(th2))) - cos(th6)*(cos(th3)*sin(th2)*sin(th4) +

cos(th4)*sin(th2)*sin(th3))) + d3*cos(th2) - a3*cos(th3)*sin(th2)]

[

0,

0,

0,

1]

T07v =

[ sin(q7)*(sin(q5)*(cos(q4)*(sin(q3)*sin(pi/18 + q1) - cos(q2)*cos(q3)*cos(pi/18 + q1)) +

sin(q4)*(cos(q3)*sin(pi/18 + q1) + cos(q2)*cos(pi/18 + q1)*sin(q3))) + cos(q5)*cos(pi/18 +

q1)*sin(q2)) - cos(q7)*(sin(q6)*(cos(q4)*(cos(q3)*sin(pi/18 + q1) + cos(q2)*cos(pi/18 +

q1)*sin(q3)) - sin(q4)*(sin(q3)*sin(pi/18 + q1) - cos(q2)*cos(q3)*cos(pi/18 + q1))) +

cos(q6)*(cos(q5)*(cos(q4)*(sin(q3)*sin(pi/18 + q1) - cos(q2)*cos(q3)*cos(pi/18 + q1)) +

sin(q4)*(cos(q3)*sin(pi/18 + q1) + cos(q2)*cos(pi/18 + q1)*sin(q3))) - cos(pi/18 +

q1)*sin(q2)*sin(q5))), sin(q7)*(sin(q6)*(cos(q4)*(cos(q3)*sin(pi/18 + q1) +

cos(q2)*cos(pi/18 + q1)*sin(q3)) - sin(q4)*(sin(q3)*sin(pi/18 + q1) -

cos(q2)*cos(q3)*cos(pi/18 + q1))) + cos(q6)*(cos(q5)*(cos(q4)*(sin(q3)*sin(pi/18 + q1) -

cos(q2)*cos(q3)*cos(pi/18 + q1)) + sin(q4)*(cos(q3)*sin(pi/18 + q1) + cos(q2)*cos(pi/18 +

q1)*sin(q3))) - cos(pi/18 + q1)*sin(q2)*sin(q5))) +

cos(q7)*(sin(q5)*(cos(q4)*(sin(q3)*sin(pi/18 + q1) - cos(q2)*cos(q3)*cos(pi/18 + q1)) +

sin(q4)*(cos(q3)*sin(pi/18 + q1) + cos(q2)*cos(pi/18 + q1)*sin(q3))) + cos(q5)*cos(pi/18 +

q1)*sin(q2)), cos(q6)*(cos(q4)*(cos(q3)*sin(pi/18 + q1) + cos(q2)*cos(pi/18 + q1)*sin(q3))

- sin(q4)*(sin(q3)*sin(pi/18 + q1) - cos(q2)*cos(q3)*cos(pi/18 + q1))) -

sin(q6)*(cos(q5)*(cos(q4)*(sin(q3)*sin(pi/18 + q1) - cos(q2)*cos(q3)*cos(pi/18 + q1)) +

sin(q4)*(cos(q3)*sin(pi/18 + q1) + cos(q2)*cos(pi/18 + q1)*sin(q3))) - cos(pi/18 +

q1)*sin(q2)*sin(q5)), d5*(cos(q4)*(cos(q3)*sin(pi/18 + q1) + cos(q2)*cos(pi/18 +

q1)*sin(q3)) - sin(q4)*(sin(q3)*sin(pi/18 + q1) - cos(q2)*cos(q3)*cos(pi/18 + q1))) +

a1*cos(pi/18 + q1) + d7*(cos(q6)*(cos(q4)*(cos(q3)*sin(pi/18 + q1) + cos(q2)*cos(pi/18 +

q1)*sin(q3)) - sin(q4)*(sin(q3)*sin(pi/18 + q1) - cos(q2)*cos(q3)*cos(pi/18 + q1))) -

sin(q6)*(cos(q5)*(cos(q4)*(sin(q3)*sin(pi/18 + q1) - cos(q2)*cos(q3)*cos(pi/18 + q1)) +

sin(q4)*(cos(q3)*sin(pi/18 + q1) + cos(q2)*cos(pi/18 + q1)*sin(q3))) - cos(pi/18 +

q1)*sin(q2)*sin(q5))) + d3*cos(pi/18 + q1)*sin(q2) - a3*sin(q3)*sin(pi/18 + q1) +

a3*cos(q2)*cos(q3)*cos(pi/18 + q1)]

[ cos(q7)*(sin(q6)*(cos(q4)*(cos(q3)*cos(pi/18 + q1) - cos(q2)*sin(q3)*sin(pi/18 + q1)) -

sin(q4)*(cos(pi/18 + q1)*sin(q3) + cos(q2)*cos(q3)*sin(pi/18 + q1))) +

cos(q6)*(cos(q5)*(cos(q4)*(cos(pi/18 + q1)*sin(q3) + cos(q2)*cos(q3)*sin(pi/18 + q1)) +

sin(q4)*(cos(q3)*cos(pi/18 + q1) - cos(q2)*sin(q3)*sin(pi/18 + q1))) +

sin(q2)*sin(q5)*sin(pi/18 + q1))) - sin(q7)*(sin(q5)*(cos(q4)*(cos(pi/18 + q1)*sin(q3) +

cos(q2)*cos(q3)*sin(pi/18 + q1)) + sin(q4)*(cos(q3)*cos(pi/18 + q1) -

cos(q2)*sin(q3)*sin(pi/18 + q1))) - cos(q5)*sin(q2)*sin(pi/18 + q1)), -

sin(q7)*(sin(q6)*(cos(q4)*(cos(q3)*cos(pi/18 + q1) - cos(q2)*sin(q3)*sin(pi/18 + q1)) -

sin(q4)*(cos(pi/18 + q1)*sin(q3) + cos(q2)*cos(q3)*sin(pi/18 + q1))) +

cos(q6)*(cos(q5)*(cos(q4)*(cos(pi/18 + q1)*sin(q3) + cos(q2)*cos(q3)*sin(pi/18 + q1)) +

sin(q4)*(cos(q3)*cos(pi/18 + q1) - cos(q2)*sin(q3)*sin(pi/18 + q1))) +

sin(q2)*sin(q5)*sin(pi/18 + q1))) - cos(q7)*(sin(q5)*(cos(q4)*(cos(pi/18 + q1)*sin(q3) +

cos(q2)*cos(q3)*sin(pi/18 + q1)) + sin(q4)*(cos(q3)*cos(pi/18 + q1) -

cos(q2)*sin(q3)*sin(pi/18 + q1))) - cos(q5)*sin(q2)*sin(pi/18 + q1)),

sin(q6)*(cos(q5)*(cos(q4)*(cos(pi/18 + q1)*sin(q3) + cos(q2)*cos(q3)*sin(pi/18 + q1)) +

sin(q4)*(cos(q3)*cos(pi/18 + q1) - cos(q2)*sin(q3)*sin(pi/18 + q1))) +

sin(q2)*sin(q5)*sin(pi/18 + q1)) - cos(q6)*(cos(q4)*(cos(q3)*cos(pi/18 + q1) -

cos(q2)*sin(q3)*sin(pi/18 + q1)) - sin(q4)*(cos(pi/18 + q1)*sin(q3) +

cos(q2)*cos(q3)*sin(pi/18 + q1))), a1*sin(pi/18 + q1) - d5*(cos(q4)*(cos(q3)*cos(pi/18 +

q1) - cos(q2)*sin(q3)*sin(pi/18 + q1)) - sin(q4)*(cos(pi/18 + q1)*sin(q3) +

cos(q2)*cos(q3)*sin(pi/18 + q1))) - d7*(cos(q6)*(cos(q4)*(cos(q3)*cos(pi/18 + q1) -

cos(q2)*sin(q3)*sin(pi/18 + q1)) - sin(q4)*(cos(pi/18 + q1)*sin(q3) +

cos(q2)*cos(q3)*sin(pi/18 + q1))) - sin(q6)*(cos(q5)*(cos(q4)*(cos(pi/18 + q1)*sin(q3) +

cos(q2)*cos(q3)*sin(pi/18 + q1)) + sin(q4)*(cos(q3)*cos(pi/18 + q1) -

cos(q2)*sin(q3)*sin(pi/18 + q1))) + sin(q2)*sin(q5)*sin(pi/18 + q1))) + a3*cos(pi/18 +

q1)*sin(q3) + d3*sin(q2)*sin(pi/18 + q1) + a3*cos(q2)*cos(q3)*sin(pi/18 + q1)]

[

sin(q7)*(cos(q2)*cos(q5) - sin(q5)*(sin(q2)*sin(q3)*sin(q4) - cos(q3)*cos(q4)*sin(q2))) +

cos(q7)*(cos(q6)*(cos(q2)*sin(q5) + cos(q5)*(sin(q2)*sin(q3)*sin(q4) -

cos(q3)*cos(q4)*sin(q2))) + sin(q6)*(cos(q3)*sin(q2)*sin(q4) + cos(q4)*sin(q2)*sin(q3))),

cos(q7)*(cos(q2)*cos(q5) - sin(q5)*(sin(q2)*sin(q3)*sin(q4) - cos(q3)*cos(q4)*sin(q2))) -

sin(q7)*(cos(q6)*(cos(q2)*sin(q5) + cos(q5)*(sin(q2)*sin(q3)*sin(q4) -

cos(q3)*cos(q4)*sin(q2))) + sin(q6)*(cos(q3)*sin(q2)*sin(q4) + cos(q4)*sin(q2)*sin(q3))),

sin(q6)*(cos(q2)*sin(q5) + cos(q5)*(sin(q2)*sin(q3)*sin(q4) - cos(q3)*cos(q4)*sin(q2))) -

cos(q6)*(cos(q3)*sin(q2)*sin(q4) + cos(q4)*sin(q2)*sin(q3)),

d1 - d5*(cos(q3)*sin(q2)*sin(q4) + cos(q4)*sin(q2)*sin(q3)) + d3*cos(q2) +

d7*(sin(q6)*(cos(q2)*sin(q5) + cos(q5)*(sin(q2)*sin(q3)*sin(q4) -

cos(q3)*cos(q4)*sin(q2))) - cos(q6)*(cos(q3)*sin(q2)*sin(q4) + cos(q4)*sin(q2)*sin(q3))) -

a3*cos(q3)*sin(q2)]

[

0,

0,

0,

1]

T07u =

[ S7*(S5*(C4*(S3*sin(pi/18 + q1) - C2*C3*cos(pi/18 + q1)) + S4*(C3*sin(pi/18 + q1) +

C2*S3*cos(pi/18 + q1))) + C5*S2*cos(pi/18 + q1)) - C7*(S6*(C4*(C3*sin(pi/18 + q1) +

C2*S3*cos(pi/18 + q1)) - S4*(S3*sin(pi/18 + q1) - C2*C3*cos(pi/18 + q1))) +

C6*(C5*(C4*(S3*sin(pi/18 + q1) - C2*C3*cos(pi/18 + q1)) + S4*(C3*sin(pi/18 + q1) +

C2*S3*cos(pi/18 + q1))) - S2*S5*cos(pi/18 + q1))), C7*(S5*(C4*(S3*sin(pi/18 + q1) -

C2*C3*cos(pi/18 + q1)) + S4*(C3*sin(pi/18 + q1) + C2*S3*cos(pi/18 + q1))) +

C5*S2*cos(pi/18 + q1)) + S7*(S6*(C4*(C3*sin(pi/18 + q1) + C2*S3*cos(pi/18 + q1)) -

S4*(S3*sin(pi/18 + q1) - C2*C3*cos(pi/18 + q1))) + C6*(C5*(C4*(S3*sin(pi/18 + q1) -

C2*C3*cos(pi/18 + q1)) + S4*(C3*sin(pi/18 + q1) + C2*S3*cos(pi/18 + q1))) -

S2*S5*cos(pi/18 + q1))), C6*(C4*(C3*sin(pi/18 + q1) + C2*S3*cos(pi/18 + q1)) -

S4*(S3*sin(pi/18 + q1) - C2*C3*cos(pi/18 + q1))) - S6*(C5*(C4*(S3*sin(pi/18 + q1) -

C2*C3*cos(pi/18 + q1)) + S4*(C3*sin(pi/18 + q1) + C2*S3*cos(pi/18 + q1))) -

S2*S5*cos(pi/18 + q1)), d5*(C4*(C3*sin(pi/18 + q1) + C2*S3*cos(pi/18 + q1)) -

S4*(S3*sin(pi/18 + q1) - C2*C3*cos(pi/18 + q1))) - d7*(S6*(C5*(C4*(S3*sin(pi/18 + q1) -

C2*C3*cos(pi/18 + q1)) + S4*(C3*sin(pi/18 + q1) + C2*S3*cos(pi/18 + q1))) -

S2*S5*cos(pi/18 + q1)) - C6*(C4*(C3*sin(pi/18 + q1) + C2*S3*cos(pi/18 + q1)) -

S4*(S3*sin(pi/18 + q1) - C2*C3*cos(pi/18 + q1)))) + a1*cos(pi/18 + q1) - S3*a3*sin(pi/18 +

q1) + S2*d3*cos(pi/18 + q1) + C2*C3*a3*cos(pi/18 + q1)]

[ C7*(S6*(C4*(C3*cos(pi/18 + q1) - C2*S3*sin(pi/18 + q1)) - S4*(S3*cos(pi/18 + q1) +

C2*C3*sin(pi/18 + q1))) + C6*(C5*(C4*(S3*cos(pi/18 + q1) + C2*C3*sin(pi/18 + q1)) +

S4*(C3*cos(pi/18 + q1) - C2*S3*sin(pi/18 + q1))) + S2*S5*sin(pi/18 + q1))) -

S7*(S5*(C4*(S3*cos(pi/18 + q1) + C2*C3*sin(pi/18 + q1)) + S4*(C3*cos(pi/18 + q1) -

C2*S3*sin(pi/18 + q1))) - C5*S2*sin(pi/18 + q1)), - S7*(S6*(C4*(C3*cos(pi/18 + q1) -

C2*S3*sin(pi/18 + q1)) - S4*(S3*cos(pi/18 + q1) + C2*C3*sin(pi/18 + q1))) +

C6*(C5*(C4*(S3*cos(pi/18 + q1) + C2*C3*sin(pi/18 + q1)) + S4*(C3*cos(pi/18 + q1) -

C2*S3*sin(pi/18 + q1))) + S2*S5*sin(pi/18 + q1))) - C7*(S5*(C4*(S3*cos(pi/18 + q1) +

C2*C3*sin(pi/18 + q1)) + S4*(C3*cos(pi/18 + q1) - C2*S3*sin(pi/18 + q1))) -

C5*S2*sin(pi/18 + q1)), S6*(C5*(C4*(S3*cos(pi/18 + q1) + C2*C3*sin(pi/18 + q1)) +

S4*(C3*cos(pi/18 + q1) - C2*S3*sin(pi/18 + q1))) + S2*S5*sin(pi/18 + q1)) -

C6*(C4*(C3*cos(pi/18 + q1) - C2*S3*sin(pi/18 + q1)) - S4*(S3*cos(pi/18 + q1) +

C2*C3*sin(pi/18 + q1))), d7*(S6*(C5*(C4*(S3*cos(pi/18 + q1) + C2*C3*sin(pi/18 + q1)) +

S4*(C3*cos(pi/18 + q1) - C2*S3*sin(pi/18 + q1))) + S2*S5*sin(pi/18 + q1)) -

C6*(C4*(C3*cos(pi/18 + q1) - C2*S3*sin(pi/18 + q1)) - S4*(S3*cos(pi/18 + q1) +

C2*C3*sin(pi/18 + q1)))) - d5*(C4*(C3*cos(pi/18 + q1) - C2*S3*sin(pi/18 + q1)) -

S4*(S3*cos(pi/18 + q1) + C2*C3*sin(pi/18 + q1))) + a1*sin(pi/18 + q1) + S2*d3*sin(pi/18 +

q1) + S3*a3*cos(pi/18 + q1) + C2*C3*a3*sin(pi/18 + q1)]

[

S7*(S5*(C3*C4*S2 - S2*S3*S4) + C2*C5) - C7*(C6*(C5*(C3*C4*S2 - S2*S3*S4) - C2*S5) -

S6*(C3*S2*S4 + C4*S2*S3)),

S7*(C6*(C5*(C3*C4*S2 - S2*S3*S4) - C2*S5) - S6*(C3*S2*S4 + C4*S2*S3)) +

C7*(S5*(C3*C4*S2 - S2*S3*S4) + C2*C5),

- C6*(C3*S2*S4 + C4*S2*S3) - S6*(C5*(C3*C4*S2 - S2*S3*S4) - C2*S5),

d1 + C2*d3 - d7*(C6*(C3*S2*S4 + C4*S2*S3) + S6*(C5*(C3*C4*S2 - S2*S3*S4) - C2*S5)) -

d5*(C3*S2*S4 + C4*S2*S3) - C3*S2*a3]

[

0,

0,

0,

1]

T0en =

0.9848

0

0.1736

0.6661

0.1736

0

-0.9848

-0.8980

0 1.0000

0

0.6000

0 0

0

1.0000