www.pudn.com > delta_robot_code-master.zip > plotWorkspace.m, change:2015-12-15,size:6279b

```%% Plot robot workspace
function plotWorkspace(alpha, f, e, rf, re)
E = [0; -300; 0]; % User desired pose of TCP (Tool Center Point)

% Solve IK
[q1, F1, J1, E1] = IK(E, alpha(1), f, e, rf, re);
[q2, F2, J2, E2] = IK(E, alpha(2), f, e, rf, re);
[q3, F3, J3, E3] = IK(E, alpha(3), f, e, rf, re);

rad_m = e/2 * tan(30*pi/180); % radius from joint E to Ei
rad_f = f/2 * tan(30*pi/180); % radius from joint O to Fi
theta = 60*pi/180;
angle = linspace(0, 2*pi, 360);

u = [                 0             rad_f 0;

s = [                 0             rad_m 0;

new_centerpts = u-s;

% center pt of joint Ji
x1=J1(1); y1=J1(2); z1=J1(3);
x2=J2(1); y2=J2(2); z2=J2(3);
x3=J3(1); y3=J3(2); z3=J3(3);
centerptsJi(1,:) = [x1 z1];
centerptsJi(2,:) = [x2 z2];
centerptsJi(3,:) = [x3 z3];

% % center pt of joint Fi
% x1=F1(1); y1=F1(2); z1=F1(3);
% x2=F2(1); y2=F2(2); z2=F2(3);
% x3=F3(1); y3=F3(2); z3=F3(3);
% centerptsFi(1,:) = [x1 z1];
% centerptsFi(2,:) = [x2 z2];
% centerptsFi(3,:) = [x3 z3];
%
% % center pt of joint Fi
% x1=E1(1); y1=E1(2); z1=E1(3);
% x2=E2(1); y2=E2(2); z2=E2(3);
% x3=E3(1); y3=E3(2); z3=E3(3);
% centerptsEi(1,:) = [x1 z1];
% centerptsEi(2,:) = [x2 z2];
% centerptsEi(3,:) = [x3 z3];

figure
hold on;
title('Robot workspace circles and shifted center points as red *')
xlabel('X (mm)');
ylabel('Z (mm)');
% unshifted circle centers
% plot(centerptsFi(1,1), centerptsFi(1,2), '*')
% plot(centerptsFi(2,1), centerptsFi(2,2), '*')
% plot(centerptsFi(3,1), centerptsFi(3,2), '*')
for k = 1:3
y = 0; % max is the radius of the circle. this is the height level of the sphere
% Joint Fi
xcenter = new_centerpts(k,1);
zcenter = new_centerpts(k,2);

% shifted circle centers
plot(new_centerpts(1,1), new_centerpts(1,2), '*r')
plot(new_centerpts(2,1), new_centerpts(2,2), '*r')
plot(new_centerpts(3,1), new_centerpts(3,2), '*r')

plot(x_re,z_re, 'b');
plot(x_rf,z_rf, 'r');
end

xlim = 300; % the min and max values for x-axis
zlim = 300; % the min and max values for z-axis
ylim = 200; % the min and max values for y-axis which is for height
points = [];
points_alt = [];

for x = -xlim:1:xlim % check all x values at increments of 5mm
for z = -zlim:1:zlim % then check all z values at increments of 5mm
if norm( [new_centerpts(1,1), new_centerpts(1,2)] - [x, z] ) <= radius_rf && ...
norm( [new_centerpts(2,1), new_centerpts(2,2)] - [x, z] ) <= radius_rf && ...
norm( [new_centerpts(3,1), new_centerpts(3,2)] - [x, z] ) <= radius_rf
points_alt = [points_alt; [x,y-J1(3),z]];  % add points to total list

end
end
end

figure;
set(gcf,'units','normalized','outerposition',[0 0 1 1])
sub2 = subplot(2,2,2);
plot(points_alt(:,1), points_alt(:,3), '.', 'MarkerSize', 3) % plot the generated points
grid(sub2, 'on');
axis(sub2, [-300 300 -300 300 -600 0])
axis(sub2, 'square')
xlabel(sub2, 'X (mm)')
zlabel(sub2, 'Y (mm)')
ylabel(sub2, 'Z (mm)')
title(sub2, 'Workspace plot filled in with generated points', 'FontSize', 18)
view(0,90);

for x = -xlim:10:xlim % check all x values at increments of 5mm
for z = -zlim:10:zlim % then check all z values at increments of 5mm
for y = -ylim:10:ylim % Then check for the range of heights
if norm( [new_centerpts(1,1), new_centerpts(1,2)] - [x, z] ) <= radius_rf && ...
norm( [new_centerpts(2,1), new_centerpts(2,2)] - [x, z] ) <= radius_rf && ...
norm( [new_centerpts(3,1), new_centerpts(3,2)] - [x, z] ) <= radius_rf
points = [points; [x,y-J1(3),z]];  % add points to total list
end
end
end
end

% figure;
sub1 = subplot(2,2,1);
scatter3(points(:,1), points(:,3),points(:,2),20,points(:,2),'filled');
colormap(prism);
grid(sub1, 'on');
axis(sub1, [-300 300 -300 300 -600 0])
axis(sub1, 'square')
xlabel(sub1, 'X (mm)')
zlabel(sub1, 'Y (mm)')
ylabel(sub1, 'Z (mm)')
title(sub1, 'Workspace plot filled in with generated points', 'FontSize', 18)
view(-38,30);

% sub2 = subplot(2,2,2)
% scatter3(points(:,1), points(:,3),points(:,2),20,points(:,2),'filled');
% colormap(prism);
% grid(sub2, 'on');
% axis(sub2, [-300 300 -300 300 -600 0])
% axis(sub2, 'square')
% xlabel(sub2, 'X (mm)')
% zlabel(sub2, 'Y (mm)')
% ylabel(sub2, 'Z (mm)')
% title(sub2, 'Workspace plot filled in with generated points', 'FontSize', 18)
% view(0,90);

sub3 = subplot(2,2,3);
scatter3(points(:,1), points(:,3),points(:,2),20,points(:,2),'filled');
colormap(prism);
grid(sub3, 'on');
axis(sub3, [-300 300 -300 300 -600 0])
axis(sub3, 'square')
xlabel(sub3, 'X (mm)')
zlabel(sub3, 'Y (mm)')
ylabel(sub3, 'Z (mm)')
title(sub3, 'Workspace plot filled in with generated points', 'FontSize', 18)
view(-90,0);

sub4 = subplot(2,2,4);
scatter3(points(:,1), points(:,3),points(:,2),20,points(:,2),'filled');
colormap(prism);
grid(sub4, 'on');
axis(sub4, [-300 300 -300 300 -600 0])
axis(sub4, 'square')
xlabel(sub4, 'X (mm)')
zlabel(sub4, 'Y (mm)')
ylabel(sub4, 'Z (mm)')
title(sub4, 'Workspace plot filled in with generated points', 'FontSize', 18)
view(-45,0);
end```