% to create a circular trajectory
n=200; %number of points
r=.15;%radius
%the centre of the circle below
x=.750;
y=.0;
z=.550;
theta = linspace(0,360,n);% the angle of the EE position relative to the cenre of the circle
% you should use equations of the Book for rotation matrices; here it is a
% simplifed trajectory, hence those equations are not used.
for i=1:n
    P(:,i)=[x+r*cosd(theta(i));y+r*sind(theta(i));z]; %EE position
    Q(:,:,i)=[-sind(theta(i)),cosd(theta(i)),0;0,0,-1;-cosd(theta(i)),-sind(theta(i)),0]';%EE orientation
    N_Str(4*i-3:4*i,:)=[P(:,i)';Q(:,:,i)];% to write to the file
end
if (exist('filename','var') ~= 1) filename='Motoman.circle'; end 
[FileName,PathName] = uiputfile('*.*','Do not forget to add the robot name',filename); 
filename=strcat(PathName,FileName);
save(filename,'-ascii','N_Str')
