%--------------------------------------------------------------------------------------------
%                             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  CINDIR - Calcula a cinemática directa das pernas em função da sua descrição
%                  cinemática DH e de uma matriz Q de coordenadas generalizadas das juntas;
%                  para as pernas com n juntas,  Q é uma matriz n×m (ou m×n)
%                  que descreve uma trajectória de m pontos.


function [RTG,Pf] = CINDIR(IR52c,Q)


RTG = eye(4);          

x=0;
y=0;
z=20;

n = length(IR52c(:,1));

for i = 1:n
    
    [A] = ELOTRANS(IR52c,Q);     % Cálculo das matrizes transformação.
    
    RTG = RTG*A(:,:,i);          % Cálculo da matriz final.

    x = [x RTG(1,4)];            % Concatenação dos valores de x.
    y = [y RTG(2,4)];            % Concatenação dos valores de y.
    z = [z RTG(3,4)];            % Concatenação dos valores de z.
          
end

Pf=[x;y;z];                       % Matriz com os pontos das juntas.