% DOPØEDNÁ KINEMATIKA ROBOTU STAUBLI TX 200+SIMULACE MÌØENÍ
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ì
DHt=[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];
% chyby DH parametrù
DHer=randn(6,4).*[0.1 0.1 pi/180 0
                  0 0.1 pi/180 pi/180
                  0.1 0.1 pi/180 0
                  0.1 0.1 pi/180 0
                  0.1 0.1 pi/180 0
                  0.1 0 pi/180 pi/180]
%zajištìní nezápornosti parametru a
DHer(:,2)=abs(DHer(:,2));
DH=DHt+DHer;              
% promìnné úhly natoèení-poèáteèní hodnota
fi0=[0;0;0;0;pi/3;0]; %[rad]
%úprava úhlù natoèení aby odpovídaly základní 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 
fitt=[2*pi pi/2 2*pi 2*pi 2*pi 0]; %definice úhlù pohybu pro jednotlivé osy
fit=diag(fitt);  %[rad]
% poèet bodù mìøení na jedné ose +1
n=81;
lt=0.01; %velikost smìrodatné odchylky mìøení laser trackerem
for k=1:5
% úhly natoèení jednotl kloubù
for i=1:6
    fi(i,:)=linspace(fi0(i),fi0(i)+fit(k,i),n);
end
% poloha koncového bodu v systému 6
r6=[0;0;0;1];
% pomocná poloha pro kreslení robotu
r0=[0;0;0;1];
r0c=[];
%výpoèet transformaèních matic a kreslení robotu
for j=1:n-1
    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))*Tfiy(DH(i,4)));
    end
    T16(:,:,j)=T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5)*T(:,:,6);
    r0c=[r0c T16(:,:,j)*r6];
    rk=[T(:,:,1)*r0 rk];
    rk=[T(:,:,1)*T(:,:,2)*r0 rk];
    rk=[T(:,:,1)*T(:,:,2)*T(:,:,3)*r0 rk];
    rk=[T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*r0 rk];
    rk=[T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5)*r0 rk];
    rk=[T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5)*T(:,:,6)*r0 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(1+k)
plot3(r0c(1,:),r0c(2,:),r0c(3,:))
xlabel('x [mm]');ylabel('y [mm]');zlabel('z [mm]');
%zahrnutí chyby mìøení laser trackerem
r0cer=randn(3,n-1)*lt;
for i=1:3
    um(i,:,k)=r0c(i,:)+r0cer(i,:);
end;
umi(:,:,k)=r0c(1:3,:);
hold on
plot3(um(1,:,k),um(2,:,k),um(3,:,k),'k.');
legend('trajektorie koncového bodu robotu','namìøené body');
axis equal
end
% promìnné-úhel natoèení v prùbìhu pohybu pøíruby
fit=[0;0;0;0;0;2*pi];  %[rad]
% úhly natoèení jednotl kloubù
for i=1:6
    fi(i,:)=linspace(fi0(i),fi0(i)+fit(i),n);
end
%poèet mìøených bodù na pøírubì +1
m=7;
alfa=linspace(0,2*pi,m);
for k=1:(m-1)
% urèení poloh koutového odražeèe na pøírubì
r6=[50*cos(alfa(k));50*sin(alfa(k));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)*r0 rk];
    rk=[T(:,:,1)*T(:,:,2)*r0 rk];
    rk=[T(:,:,1)*T(:,:,2)*T(:,:,3)*r0 rk];
    rk=[T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*r0 rk];
    rk=[T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5)*r0 rk];
    rk=[T(:,:,1)*T(:,:,2)*T(:,:,3)*T(:,:,4)*T(:,:,5)*T(:,:,6)*r0 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(7)
plot3(r0c(1,:),r0c(2,:),r0c(3,:))
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 (max chyba 0.03)
r0cer=randn(3,n-1)*lt;
for i=1:3
    um(i,:,k+5)=r0c(i,:)+r0cer(i,:);
end;
umi(:,:,k+5)=r0c(1:3,:);
plot3(um(1,1,k+5),um(2,1,k+5),um(3,1,k+5),'.r');
legend('rùzné polohy odražeèe','získané body')
grid on
end
% uložení namìøených dat
savefile = 'sim_mereni.mat';
save(savefile,'um','umi','DH','fi0');