Program obliczający macierz przekształcenia jednorodnego:
% 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 d1 th2 d2 a2 th3 a3 th4 d4 q2 q3 q4
% wyznaczenie symbolicznej postaci macierzy - funkcja mA
A1=mA(0,d1,0,0)
A2=mA(th2,d2,a2,sym(pi/2))
A3=mA(th3,0,a3,0)
A4=mA(th4,0,0,sym(-pi/2))
A5=mA(0,d4,0,0)
% mnożenie macierzy
T03=A1*A2*A3*A4*A5
% podstawienie nowych zmiennych za obrotowe zmienne złączowe
% do upraszczania zapisu macierzy przekształcenia jednorodnego
% zmienne thi lub ali są zamieniane na qi
T03v=subs(T03,{th2,th3,th4},{q2,q3,q4})
% wskazanie zmiennych złączowych
% zmienne: th2,th3,th4 i d1 wskazuj? ‘1’-ki
zmie=[[0,1,0,0];[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)
Macierze opisujące środki i orientację lokalnych układów współrzędnych:
A1 = [ 1, 0, 0, 0]
[ 0, 1, 0, 0]
[ 0, 0, 1, d1]
[ 0, 0, 0, 1]
A2 = [ cos(th2), 0, sin(th2), cos(th2)*a2]
[ sin(th2), 0, -cos(th2), sin(th2)*a2]
[ 0, 1, 0, d2]
[ 0, 0, 0, 1]
A3 = [ cos(th3), -sin(th3), 0, cos(th3)*a3]
[ sin(th3), cos(th3), 0, sin(th3)*a3]
[ 0, 0, 1, 0]
[ 0, 0, 0, 1]
A4 = [ cos(th4), 0, -sin(th4), 0]
[ sin(th4), 0, cos(th4), 0]
[ 0, -1, 0, 0]
[ 0, 0, 0, 1]
A5 = [ 1, 0, 0, 0]
[ 0, 1, 0, 0]
[ 0, 0, 1, d4]
[ 0, 0, 0, 1]
Otrzymane macierze:
T03 =
[ cos(th2)*cos(th3)*cos(th4)-cos(th2)*sin(th3)*sin(th4), -sin(th2), -cos(th2)*cos(th3)*sin(th4)-cos(th2)*sin(th3)*cos(th4), (-cos(th2)*cos(th3)*sin(th4)-cos(th2)*sin(th3)*cos(th4))*d4+cos(th2)*cos(th3)*a3+cos(th2)*a2]
[ sin(th2)*cos(th3)*cos(th4)-sin(th2)*sin(th3)*sin(th4), cos(th2), -sin(th2)*cos(th3)*sin(th4)-sin(th2)*sin(th3)*cos(th4), (-sin(th2)*cos(th3)*sin(th4)-sin(th2)*sin(th3)*cos(th4))*d4+sin(th2)*cos(th3)*a3+sin(th2)*a2]
[ sin(th3)*cos(th4)+cos(th3)*sin(th4), 0, -sin(th3)*sin(th4)+cos(th3)*cos(th4), (-sin(th3)*sin(th4)+cos(th3)*cos(th4))*d4+sin(th3)*a3+d2+d1]
[ 0, 0, 0, 1]
T03v =
[ cos(q2)*cos(q3)*cos(q4)-cos(q2)*sin(q3)*sin(q4), -sin(q2), -cos(q2)*cos(q3)*sin(q4)-cos(q2)*sin(q3)*cos(q4), (-cos(q2)*cos(q3)*sin(q4)-cos(q2)*sin(q3)*cos(q4))*d4+cos(q2)*cos(q3)*a3+cos(q2)*a2]
[ sin(q2)*cos(q3)*cos(q4)-sin(q2)*sin(q3)*sin(q4), cos(q2), -sin(q2)*cos(q3)*sin(q4)-sin(q2)*sin(q3)*cos(q4), (-sin(q2)*cos(q3)*sin(q4)-sin(q2)*sin(q3)*cos(q4))*d4+sin(q2)*cos(q3)*a3+sin(q2)*a2]
[ sin(q3)*cos(q4)+cos(q3)*sin(q4), 0, -sin(q3)*sin(q4)+cos(q3)*cos(q4), (-sin(q3)*sin(q4)+cos(q3)*cos(q4))*d4+sin(q3)*a3+d2+d1]
[ 0, 0, 0, 1]
T03u =
[ 1/2*cos(-q4+q2-q3)+1/2*cos(q4+q2+q3), -S2, 1/2*sin(-q4+q2-q3)-1/2*sin(q4+q2+q3), 1/2*d4*sin(-q4+q2-q3)-1/2*d4*sin(q4+q2+q3)+1/2*a3*C2_3+1/2*a3*C23+C2*a2]
[ 1/2*sin(q4+q2+q3)+1/2*sin(-q4+q2-q3), C2, 1/2*cos(q4+q2+q3)-1/2*cos(-q4+q2-q3), 1/2*d4*cos(q4+q2+q3)-1/2*d4*cos(-q4+q2-q3)+1/2*a3*S23+1/2*a3*S2_3+S2*a2]
[ S34, 0, C34, C34*d4+S3*a3+d2+d1]
[ 0, 0, 0, 1]