% Vyjadrenie polohy, rýchlosti a zrýchlenia koncového bodu

close all
clc

% Parametre: kedže nie je známa regulácia rýchlosti polohovacieho zariadenia,
%            je nutné vyjadrit vstupné parametre ako funkciu casu.

L1 = k*t; % vzdialenost
L2 = k*t; % vzdialenost
L3 = k*t; % vzdialenost
L4 = k*t; % vzdialenost
L5 = k*t  % vzdialenost

q1 = 206*t; % posuv 1 
q2 = (pi/2)*t; % natocenie 2
q3 = 26*t; % posuv 3
q4 = 52*t; % posuv 4

% V prvom rade je nutné vyjadrit polohu koncového bodu. Následne sa derváciou
% matice T získajú výpocetné vztahy pre rýchlost a zrýchlenie.

T10_T = [ 1 0 0 0;                     % translacná matica
          0 1 0 0;
          0 0 1 L1;
          0 0 0 1];
      
T10_TT = [ 1 0 0 q(1);                  % translacná matica
           0 1 0 0;
           0 0 1 0;
           0 0 0 1];
       
T10 = T10_T * T10_TT;

T21_T = [ 1 0 0 0;                      % translacná matica
          0 1 0 0;
          0 0 1 L2;
          0 0 0 1 ];
     
T21_R = [ cos(q(2)) -sin(q(2)) 0 0;     % rotacná matica
          sin(q(2))  cos(q(2)) 0 0;
          0 0 1 0;
          0 0 0 1];
            
T21 = T21_T * T21_R

T32_T = [ 1 0 0 0;                      % translacná matica
          0 1 0 0;
          0 0 1 L3;
          0 0 0 1];
      
T32_TT = [ 1 0 0 q(3);                     % translacná matica
           0 1 0 0;
           0 0 1 0;
           0 0 0 1];
            
T32 = T32_T * T32_TT

T43_T = [ 1 0 0 0;                      % translacná matica
          0 1 0 0;
          0 0 1 L4;
          0 0 0 1];

T43_TT = [ 1 0 0 0;                     % translacná matica
           0 1 0 0;
           0 0 1 q(4);
           0 0 0 1];

T43 = T43_T * T43_TT

T54_T = [ 1 0 0 L5;                      % translacná matica
          0 1 0 0;
          0 0 1 0;
          0 0 0 1];
     
      
r5E = [0;0;0;1];  % súradnice koncového efektoru E v súradnicovom systéme 5

T = T10 * T21 * T32 * T43 *T54_T

r0E = T * r5E;   % súradnice koncového efektoru E v súradnicovom systéme 0

x = r0E(1:3);



% Pre rýchlost bodu platí

% Pre rýchlost bodu v priestore v0 je nutné vyjadrit vektor okamžitej uhlovej rýchlosti telesa, ktorý je
% zapásaný formou antisymetrickej matice Omega. 

 Omega  = [  0      -omega_z   omega_y;                    
           omega_z     0       -omega_x;
          -omega_y   omega_x      0];
  
r0 = [x;                                 % vektor polohy bodu v súradnicovom systéme 0
      y;
      z];
  
v0 = Omega * r0    

% Vzájomným porovnaním a použitím vztahov môžeme písat výsledný vztah

v0E = diff (T)* r5D =  T * Omega_dev * r5E   

Omega_dev = diff (T10) * Omega * diff (T21_T) * diff (T32) * diff (T43) * diff (T54_T)



% Pre zrýchlenie bodu platí

% Vztah pre zrýchlenie bodu pri priestorovom pohybe získame obdobne ako
% rýchlost. Je nutné vyjadrit vektor okamžitého uhlového zrýchlenia telesa, ktorý je
% zapásaný formou antisymetrickej matice Epsilon.  

a0D = diff (v0D) = diff(diff(T)) * r5E + diff (T)* diff (r5E)  % diff (r5E) = 0 

Epsilon = [   0       -epsilon_z   epsilon_y;                      
           epsilon_z      0        -epsilon_x;
          -epsilon_y   epsilon_x       0];


diff(diff(T)) = T * ( Omega^2 + Epsilon )

Epsilon = diff (Omega_dev)

% Vzájomným porovnaním a použitím vztahov môžeme písat výsledný vztah

a0E = diff(diff(T)) * r5E  

















