function [ x ] = fkin( L1, L2, L3, L4, L5, q )

% funkcia pre doprednú kinematiku TRTT manipulátoru

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

end

