pozycja_orientacja.m

k=size(theta);
n=k(2);
for i=1:n
    A=matrixA(theta(i), lambda(i), l(i), alpha(i))
    eval(['A' num2str(i) '=A'])
end
T30=A1*A2*A3
disp('wektor pozycji chwytaka względem układu')
p30=T30(1:3,4)
disp('macierz orientacji chwytaka względem układu')
R30=T30(1:3,1:3)

disp('metoda rekurencyjna')
r33=[0 0 0 1]';
r32=A3*r33;
r31=A2*r32;
r30=A1*r31;