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

function PZ=PZeros(SHM)

MSHM=ones(size(SHM,1),size(SHM,2),size(SHM,3),size(SHM,4));
for i=1:1:size(SHM,3)
    MSHM(:,:,i,1)=SHM(:,:,i,1);
end
for i=1:1:size(SHM,3)
    for j=2:1:size(SHM,4)
        MSHM(:,:,i,j)=MSHM(:,:,i,j-1)*SHM(:,:,i,j);
    end
end
%%%%%%%%%
for k=1:1:size(MSHM,3)
    for j=1:1:size(MSHM,4)
        for i=1:1:3
            pontos(j,i,k)=MSHM(i,4,k,j);
        end
    end
end

PZ=pontos;