% 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');