% PØÍMÁ KINEMATIKA ROBOTU STAUBLI TX 200 POMOCÍ D-H PAREMETRÙ
% slouží k generování poøadované polohy koncového bodu pro inverzní
% kinematiku (inv_kin)
clear all;
% 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];
% promìnné-poèáteèní hodnota
fi0=[0.1;pi/4;-pi/2;0.2;-pi/4;0.3]; %[rad]
%úprava poèáteèních hodnot, aby odpovídaly zákl. poloze mechanismu
fikor=[0;pi/2;pi/2;0;0;0]; %[rad]
fi0=fi0+fikor;
% promìnné-úhel natoèení v prùbìhu pohybu
fit=[2*pi;0;0;pi*2;0;0];  %[rad]
% poèet výpoètových bodù
n=100;
% úhly natoèení jednotl kloubù
for i=1:6
    fi(i,:)=linspace(fi0(i),fi0(i)+fit(i),n);
end
% poloha koncového bodu v systému 6
r6=[0;0;0;1];
r0c2=[];
%výpoèet transformaèních matic a kreslení robotu
for j=1:n
    rk=[];
    rk=[0;0;0;1];
    for i=1:6
        T(:,:,i)=Tfiz(fi(i,j))*Tz(DH(i,1))*Tx(DH(i,2))*Tfix(DH(i,3));
    end
    T16(:,:,j)=T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5)*T(:,:,6);
    r0c2=[r0c2 T16(:,:,j)*r6];
    rk=[T(:,:,1)*r6 rk];
    rk=[T(:,:,1)*T(:,:,2)*r6 rk];
    rk=[T(:,:,1)*T(:,:,2)*T(:,:,3)*r6 rk];
    rk=[T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*r6 rk];
    rk=[T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5)*r6 rk];
    rk=[T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5)*T(:,:,6)*r6 rk];
    figure(1)
    grid on
    li=plot3(rk(1,:),rk(2,:),rk(3,:));
    axis equal
    xlabel('x [mm]');ylabel('y [mm]');zlabel('z [mm]');
    set(li,'LineWidth',2);
end
grid on
figure(2)
plot3(r0c2(1,:),r0c2(2,:),r0c2(3,:))
title('Trajektorie koncového bodu robotu');
xlabel('x [mm]');ylabel('y [mm]');zlabel('z [mm]');
axis equal
grid on
savefile = 'T16.mat';
save(savefile, 'T16');