%--------------------------------------------------------------------------------------------
%                             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 ELOTRANS2 - 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)


% Transformaçao do referncial global para o 1º referencial das pernas:
A0 = RotX(pi)*RotY(pi/2);

% Calculo das transformaçoes sucessivas dos diferentes referenciais
% existentes:
for i = 1:length(IR52c(:,1))
    if i ~= 6
        A(:,:,i) = RotZ((Q(i) + IR52c(i,2))) * Trans(IR52c(i,4),0,0) * Trans(0,0,IR52c(i,5)) * RotX(IR52c(i,3));
    else
        A(:,:,i) = RotZ((IR52c(i,2))) * Trans(IR52c(i,4),0,0) * Trans(0,0,IR52c(i,5)) * RotX(IR52c(i,3));
    end
end

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