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. 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})

4. 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