function [Robot_end_path, Robot_plat_base_path] = AnimateRobot(H,AAA,P,h,sd,T_plat)
%H- handles gr�ficos dos objetos a animar (do DrawFrames);
%AAA- superhipermatriz que contem a sequencia temporal de hipermatrizes
%calculadas por CalculateRobotMotion
%P- objeto a representar
%h handle dos elos do robo (DrawLinks);
%sd - indicador de novel de pausa, controla velocidade da anima�ao

%end effector
X_ee=[];
Y_ee=[];
Z_ee=[];

%platform base
X_pb=[];
Y_pb=[];
Z_pb=[];

for n=1:size(AAA,4)
    
    if (nargin == 6)
        T=T_plat(:,:,n);
    else
        T=eye(4);
    end
    
    AA=AAA(:,:,:,n);
    Org=LinkOrigins(AA);
    Org=[Org;ones(1,size(Org,2))];
    Org=T*Org;
    
    X=Org(1,:); 
    Y=Org(2,:); 
    Z=Org(3,:);
    set(h,'XData',X,'YData',Y,'ZData',Z);
    
    
    %ajustar os H's todos
    Q=T*P;
    set(H{1},'Vertices',Q(1:3,:)');  
    
    for j=1:size(AAA,3) %numero de elos
        T=T*AAA(:,:,j,n);
        Q=T*P;
        set(H{j+1},'Vertices',Q(1:3,:)');  
    end
    
    X_ee=[X_ee;T(1,4)];
    Y_ee=[Y_ee;T(2,4)];
    Z_ee=[Z_ee;T(3,4)];
    
    plot3(T(1,4),T(2,4),T(3,4),'*r');
    
    X_pb=[X_pb;Org(1,1)];
    Y_pb=[Y_pb;Org(2,1)];
    Z_pb=[Z_pb;Org(3,1)];
    
    plot3(Org(1,1),Org(2,1),Org(3,1),'ob');
    
    if (nargin ~= 6)
        plot3(Org(1,3),Org(2,3),Org(3,3),'ob');
    end
    %legend('base_p','ee_real','2', '3')
 
    pause(sd);
    
end

Robot_end_path= [X_ee Y_ee Z_ee];
Robot_plat_base_path= [X_pb Y_pb Z_pb];

end

