close all; clear all;

n=6;
alpha=[0 -90 0 -90 90 -90];
a=[0 0 17 0 0 0];
d=[0 0 5.5 17.05 0 0];
dE=[0;0;2.2];
xmin=-20; xmax=20; ymin=-20; ymax=20; zmin=-20; zmax=20;

thi=[12.8005 8.1514 72.2595 167.8824 -20.2323 -155.1012];

thf=[22.2777 22.0360 25.9568 147.9668 -52.7567 124.7032];
T=3;
step=90;
incr=T/step;
i=1
for t=0:incr:T
th(i,:)= thi+(((thf-thi)/T)*(t-(T/(2*pi))*sin(2*pi*t/T)));
i=i+1;
end
for j=1:91

Ttemp=eye(4);
for i=1:n

T=[cosd(th(j,i)) -sind(th(j,i)) 0 a(i);

sind(th(j,i))*cosd(alpha(i)) cosd(th(j,i))*cosd(alpha(i)) -
sind(alpha(i)) -d(i)*sind(alpha(i));
sind(th(j,i))*sind(alpha(i)) cosd(th(j,i))*sind(alpha(i))
cosd(alpha(i)) d(i)*cosd(alpha(i));
0 0 0 1];
To(:,:,i)=Ttemp*T;
Ttemp=To(:,:,i);
Qo(1:3,i)=To(1:3,4,i);
end
pe=Qo(:,6)+To(1:3,1:3,6)*dE;

x=[Qo(1,:),pe(1)];
y=[Qo(2,:),pe(2)];
z=[Qo(3,:),pe(3)];
plot3(x,y,z,'linewidth',2)
axis([xmin xmax ymin ymax zmin zmax]);
grid on;
pause(0.1)

end