% invezní kinematika robotu Staubli TX 200
clear all
global T16 DH i
% konstanty
l1=642; %[mm]
l2=250; %[mm]
l3=950; %[mm]
l4=800; %[mm]
l5=194; %[mm]
% tabulka konst. parametrù v D-H metodì
DH=[l1 l2 pi/2
    0  l3 0
    0  0  pi/2
    l4 0  -pi/2
    0  0  pi/2
    l5 0  0];
%naètení požadované hodnoty
load T16;
% poèáteèní odhad neznámých
fi0=[]
fi0=[0.1;pi*3/4;0;0.2;-pi/4;0.3];
fi=[];
%øešení (rovnice jsou sestaveny ve funkci rce.m)
for i=1:size(T16,3)
    r=fsolve('rce',fi0(:,i));
    fi=[fi r];
    fi0=[fi0 r];
end 
% vykreslení
plot(fi(1,:),'b')
hold on
plot(fi(2,:),'g')
plot(fi(3,:),'r')
plot(fi(4,:),'k')
plot(fi(5,:),'c')
plot(fi(6,:),'m')
legend('fi1','fi2','fi3','fi4','fi5','fi6')
xlabel('t[s]');ylabel('fi[rad]')
