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

1. Treść programu:

% 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 d2 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,d2,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

T03=A1*A2*A3

% podstawienie nowych zmiennych za obrotowe zmienne złączowe

% do upraszczania zapisu macierzy przekształcenia jednorodnego

% zmienne thi lub ali sa zamieniane na qi

T03v=subs(T03,{th1 th2 th3},{q1 q2 q3})

% wskazanie zmiennych złączowych

% zmienne: th1 th2 th3 wskazuj? ‘1’-ki

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

% uproszczona postać macierzy przekształcenia jednorodnego

% dla celów interpretacji

T03u=zam(zmie,T03v)

% 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 th2 th3 th4 th5 th6 th7 d1 d2 d3 d5 d7 a1 a3},{0 0 0 0 0 0 0 0.4 0.25 0.13 0.38 0.32 0.3 0.25})

1. Wyliczone macierze:

T03 =

[ cos(th2)*cos(th3)*cos(pi/18 + th1) - sin(th3)*sin(pi/18 + th1), - cos(th3)*sin(pi/18 + th1) - cos(th2)*cos(pi/18 + th1)*sin(th3), cos(pi/18 + th1)*sin(th2), a1*cos(pi/18 + th1) - d2*sin(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(pi/18 + th1)*sin(th3) + cos(th2)*cos(th3)*sin(pi/18 + th1), cos(th3)*cos(pi/18 + th1) - cos(th2)*sin(th3)*sin(pi/18 + th1), sin(th2)*sin(pi/18 + th1), d2*cos(pi/18 + th1) + a1*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)]

[ -cos(th3)*sin(th2), sin(th2)*sin(th3), cos(th2), d1 + d3*cos(th2) - a3*cos(th3)*sin(th2)]

[ 0, 0, 0, 1]

T03v =

[ cos(q2)*cos(q3)*cos(pi/18 + q1) - sin(q3)*sin(pi/18 + q1), - cos(q3)*sin(pi/18 + q1) - cos(q2)*cos(pi/18 + q1)*sin(q3), cos(pi/18 + q1)*sin(q2), a1*cos(pi/18 + q1) - d2*sin(pi/18 + q1) + d3*cos(pi/18 + q1)*sin(q2) - a3*sin(q3)*sin(pi/18 + q1) + a3*cos(q2)*cos(q3)*cos(pi/18 + q1)]

[ cos(pi/18 + q1)*sin(q3) + cos(q2)*cos(q3)*sin(pi/18 + q1), cos(q3)*cos(pi/18 + q1) - cos(q2)*sin(q3)*sin(pi/18 + q1), sin(q2)*sin(pi/18 + q1), d2*cos(pi/18 + q1) + a1*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)]

[ -cos(q3)*sin(q2), sin(q2)*sin(q3), cos(q2), d1 + d3*cos(q2) - a3*cos(q3)*sin(q2)]

[ 0, 0, 0, 1]

T03u =

[ C2*C3*cos(pi/18 + q1) - S3*sin(pi/18 + q1), - C3*sin(pi/18 + q1) - C2*S3*cos(pi/18 + q1), S2*cos(pi/18 + q1), a1*cos(pi/18 + q1) - d2*sin(pi/18 + q1) - S3*a3*sin(pi/18 + q1) + S2*d3*cos(pi/18 + q1) + C2*C3*a3*cos(pi/18 + q1)]

[ S3*cos(pi/18 + q1) + C2*C3*sin(pi/18 + q1), C3*cos(pi/18 + q1) - C2*S3*sin(pi/18 + q1), S2*sin(pi/18 + q1), d2*cos(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)]

[ -C3*S2, S2*S3, C2, d1 + C2*d3 - C3*S2*a3]

[ 0, 0, 0, 1]

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) - d2*sin(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)))) + d2*cos(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) - d2*sin(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))), d2*cos(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))) + a1*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) - d2*sin(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))) + d2*cos(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.6198

0.1736 0 -0.9848 -0.3477

0 1.0000 0 0.5300

0 0 0 1.0000