function [Org] = LinkOrigins(AA)
%Devolve uma matriz c/ as origens dos diversos sistemas de coordenadas de
%um manipulador, dada a hipermatriz de transforma�oes que pode ser obtida
%pela Tlinks().
%Cada coluna de Org � uma origem!
% AA- HiperMatriz de matriz de transf geometrica (uma para cada elo)
tam_A=size(AA,3);
Org=zeros(3,tam_A+1);
BB=AA;
for n=1:tam_A
%     if n==1
%         Org(:,1)=BB(1:3,4,n);
%     end
    
    if n>1 %if n=1 org(:,1)=[0 0 0]'
        BB(:,:,n)=BB(:,:,n-1)*BB(:,:,n);
        
    end
    Org(:,n+1)=BB(1:3,4,n); %4 coluna porque � a q indica a translacao
end

