close all;
clear;
ToRad = pi/180;
UX = [1 0 0]';
UY = [0 1 0]';
UZ = [0 0 1]';
d0=165;
a1=225;
a2=175;
d3=95;
a4=0;
%DH = [THETA D A ALPHA SIGMA]
L(1) = Link([0,d0,0,0]);
L(2) = Link([0,0,a1,0]);
L(3) = Link([0,0,a2,pi]);
L(4) = Link([0,0,0,0]);
SCARA = SerialLink([L(1) L(2) L(3) L(4)],'name','SCARA');
SCARA.qlim=[(-3*pi)/4 (3*pi)/4;(-pi/6)*5 (pi/6)*5;0 20;-pi pi];
SCARA.plot([-pi/2,0,0,0])
ToRad = pi/180;
th1=0;
th2=0;
th3=0;
th4=0;
L1=225;
L2=175;
px=0;
py=0;
pz=0;
num =1;
for th1=-135*ToRad:10*ToRad:135*ToRad
for th2=-150*ToRad:10*ToRad:150*ToRad
for th3= 0:1:95
for th4=-180*ToRad:10*ToRad:180*ToRad
px(num)=cos(th1)*225+ 175*cos(th1+th2);
py(num)=225*sin(th1)+175*sin(th1+th2);
pz(num)=th3;
num=num+1;
end
end
end
end
num=num-1;
grid on;
plot3(px,py,pz,'r*');
hold on
SCARA.plot([pi/2,0,0,0])
hold on
axis([-800 800,-800 800,-800,800]);
xlabel('x');
ylabel('y');
zlabel('z');
