%Função para extrair a origem de cada elo do robô da hipermatriz AA

function PZ=PZeros(SHM)

for j=1:(size(SHM,3))
    for i=1:(size(SHM,4))
        pontos(i,:,j)=([ SHM(1,4,j,i) SHM(2,4,j,i) SHM(3,4,j,i) ]);
    end
end

PZ=pontos;