% animation routine 
% gomi@cslab  21/July/2000

figure(3)
el_x = L1*cos(th_h(1,:)); 
el_y = L1*sin(th_h(1,:)); 
ha_x = L1*cos(th_h(1,:))+L2*cos(th_h(1,:)+th_h(2,:));
ha_y = L1*sin(th_h(1,:))+L2*sin(th_h(1,:)+th_h(2,:));

% Freeze Axes limits

set(gca,'PlotBoxAspectRatio',[1,1,1])
set(gca,'ylim',[-0.4,0.6]);  set(gca,'xlim',[-0.4,0.6]); 
axis manual
set(gca,'nextplot','replacechildren');

step_d = 50

tic
for i=1:step_d:size(el_x,2)
 plot([0,el_x(i),ha_x(i)],[0,el_y(i),ha_y(i)],'LineWidth',2,'color','k')
 set(gca,'nextplot','add');
 set(gca,'nextplot','replacechildren');
 pause(0.1)

end
toc

hold on
plot(pos_x,pos_y)