% DOPØEDNÁ KINEMATIKA ROBOTU STAUBLI TX 200
clear all;close 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
    0  l3 0 0
    0  0  pi/2 0
    l4 0  -pi/2 0
    0  0  pi/2 0
    l5 0  0 0];
% promìnné-poèáteèní hodnota
fi0=[pi/4;0;pi/4;0;pi/2;0]; %[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 (použití pro jinou než ètvrtou osu
% vyžaduje úravu porovnání výsledkù)
fit=[0;0;0;pi*2;0;0];  
% poèet výpoètových bodù +1
n=51;
% ú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];
r0c=[];
r0ct=[];
%výpoèet transformaèních matic a kreslení robotu
for j=1:n-1
    for i=1:6
        T(:,:,i)=Tfiz(fi(i,j))*Tz(DH(i,1))*Tx(DH(i,2))*Tfix(DH(i,3))*Tfiy(DH(i,4));
    end
    rk=[];
    rk=[0;0;0;1];
    T16(:,:,j)=T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5)*T(:,:,6);
    r0c=[r0c 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)
    li=plot3(rk(1,:),rk(2,:),rk(3,:));
    axis([min(rk(1,:))-5 max(rk(1,:))+5 min(rk(2,:))-5 max(rk(2,:))+5 min(rk(3,:))-5 max(rk(3,:))+5])
    axis equal
    xlabel('x [mm]');ylabel('y [mm]');zlabel('z [mm]');
    set(li,'LineWidth',2);
end
figure(2)
plot3(r0c(1,:),r0c(2,:),r0c(3,:),'b')
hold on
title('Trajektorie koncového bodu robotu');
xlabel('x [mm]');ylabel('y [mm]');zlabel('z [mm]');
axis equal
%chyba mìøení polohy konc. bodu laser trackerem
lt=0.1
r0cer=randn(3,n-1)*lt;
for i=1:3
    um(i,:)=r0c(i,:)+r0cer(i,:);
end;
plot3(um(1,:),um(2,:),um(3,:),'.k');
legend('trajektorie robotu','namìøené body')
grid on
% zápis skuteèné polohy osy rotace pro porovnání
To=T(:,:,1)*T(:,:,2)*T(:,:,3);
To(:,3)'
Tb=T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4);
OS=[To(:,3)'
    Tb(:,4)']
savefile = 'sim_1osa.mat';
save(savefile, 'um', 'OS');
