%--------------------------------------------------------------------------------------------
%                             Projecto final
% 
%                    Luís Alberto Soares Rêgo nº. 20008 
%              Renato Francisco dos Santos Barbosa n.º 18382
%                         Engenharia Mecânica
%                       
%                Cinematica directa das pernas do robot
%--------------------------------------------------------------------------------------------
%
% Função ELOTRANS - Permite o cálculo da matriz transformação genérica Ai 
%                   (associada ao elo i) com base na descrição cinemática DH 
%                   e no vector das juntas Q.


function [A] = ELOTRANS(IR52c,Q)


% MATRIZ TRANSFORMAÇAO GEOMETRICA GENERICA:

A0 = RotX(pi)*RotY(pi/2);

for i = 1:length(IR52c(:,1))
    
        A(:,:,i) = RotZ((Q(i) + IR52c(i,2))) * Trans(IR52c(i,4),0,0) * Trans(0,0,IR52c(i,5)) * RotX(IR52c(i,3));
        
end

A(:,:,1) = A0*A(:,:,1);