%% This Matlab code for drawing the dexterity index for 3-RRR Planar Parallel Manipulator (PPM)
clear
clc
global a1x a1y a2x a2y a3x a3y r1 r2 r3 l1 l2 l3
r1=0.40;
r2=0.40;
r3=0.40;
l1=0.40;
l2=0.40;
l3=0.40;
%% Plot the workspace area which is the intersected area of the 3 circle
% Position of the 1st active joint a_1
a1x=-0.349;
a1y=-0.202;
plot(a1x,a1y,'bo')
hold on
% Position of the 2nd active joint a_2
a2x=0.349;
a2y=-0.202;
plot(a2x,a2y,'bo')
hold on
% Position of the 3rd active joint a_3
a3x=0;
a3y=0.404;
plot(a3x,a3y,'bo')
hold on
% The length of the links of the active link ri and the passive one li [ where i =1,2,3 ] ]
ri=0.40;
li=0.40;
%% Plot circles (if you want to observe the intersected area came from)
% for circle 1
%C1=circleplot(a1x,a1y,(ri+li))
% for circle 2
%C2=circleplot(a2x,a2y,(ri+li))
% for circle 3
%C3=circleplot(a3x,a3y,(ri+li))
%hold on
%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% The intersection points of the 3 circle
[x1out,y1out] = circcirc(a1x,a1y,(ri+li),a2x,a2y,(ri+li));
[x2out,y2out] = circcirc(a3x,a3y,(ri+li),a2x,a2y,(ri+li));
[x3out,y3out] = circcirc(a1x,a1y,(ri+li),a3x,a3y,(ri+li));
%%%%%%%%%%%%%%%%%%%
%% Plot the curve of the intersection area between a1 , a2 and a3 which give us the workspace area.
d = sqrt((x3out(2)-x1out(1))^2+(y3out(2)-y1out(1))^2); % Distance between points
a = atan2(x3out(2)-x1out(1),-(y3out(2)-y1out(1))); % Perpendicular bisector angle
b = asin(d/2/(ri+li)); % Half arc angle
c = linspace(a-b,a+b); % Arc angle range
e = sqrt((ri+li)^2-d^2/4); % Distance, center to midpoint
xcurve = (x1out(1)+x3out(2))/2-e*cos(a)+(ri+li)*cos(c); % Cartesian coords. of arc
ycurve = (y1out(1)+y3out(2))/2-e*sin(a)+(ri+li)*sin(c);
plot(xcurve,ycurve,'k.')
hold on
d = sqrt((x2out(2)-x1out(1))^2+(y2out(2)-y1out(1))^2); % Distance between points
a = atan2(-(x2out(2)-x1out(1)),y2out(2)-y1out(1)); % Perpendicular bisector angle
b = asin(d/2/(ri+li)); % Half arc angle
c = linspace(a-b,a+b); % Arc angle range
e = sqrt((ri+li)^2-d^2/4); % Distance, center to midpoint
xcurve = (x1out(1)+x2out(2))/2-e*cos(a)+(ri+li)*cos(c); % Cartesian coords. of arc
ycurve = (y1out(1)+y2out(2))/2-e*sin(a)+(ri+li)*sin(c);
plot(xcurve,ycurve,'k.')
hold on
d = sqrt((x3out(2)-x2out(2))^2+(y3out(2)-y2out(2))^2); % Distance between points
a = atan2(-(x3out(2)-x2out(2)),y3out(2)-y2out(2)); % Perpendicular bisector angle
b = asin(d/2/(ri+li)); % Half arc angle
c = linspace(a-b,a+b); % Arc angle range
e = sqrt((ri+li)^2-d^2/4); % Distance, center to midpoint
xcurve = (x2out(2)+x3out(2))/2-e*cos(a)+(ri+li)*cos(c); % Cartesian coords. of arc
ycurve = (y2out(2)+y3out(2))/2-e*sin(a)+(ri+li)*sin(c);
plot(xcurve,ycurve,'k.')
hold on
xlabel('X [m]') % x-axis label
ylabel('Y [m]') % y-axis label
xlim([-0.5 0.55]) % xlim([-0.3 0.9])
set(gca,'xtick',-0.55:0.1:0.55)
ylim([-0.5 0.55]) % ylim([-0.3 0.9])
set(gca,'ytick',-0.55:0.1:0.55)
set(gca,'FontSize',15)
%% Test for area from 0 to 0.5 meter in x and y if the (x,y) locted at the workspace of the robot or not
i=0;
j=0;
k=0;
cx=-1.2:0.005:1.2;
cy=-1.2:0.005:1.2;
[CX,CY] = meshgrid(cx,cy);
for i = 1:length(cx)
for j = 1:length(cy)
%% Test condition if the postion of end effector in the workspace or not.
if( ((cx(i)-a1x)^2+(cy(j)-a1y)^2<=(r1+l1)^2) && ((cx(i)-a2x)^2+(cy(j)-a2y)^2<=(r2+l2)^2) && ((cx(i)-a3x)^2+(cy(j)-a3y)^2<=(r3+l3)^2))
disp('Value within specified range.')
% Passive joints Beta
beta=passivejoint(cx(i),cy(j));
alpha=ikm(cx(i),cy(j));
Jx=[cos(beta(1)) sin(beta(1));
cos(beta(2)) sin(beta(2));
cos(beta(3)) sin(beta(3))];
Jq=[r1*sin(beta(1)-alpha(1)) 0 0;
0 r2*sin(beta(2)-alpha(2)) 0;
0 0 r3*sin(beta(3)-alpha(3))];
% Jacobian matrix and dexterity index derivation
J=Jq\Jx;
s=svd(J);
sigmamin=min(s);
sigmamax=max(s);
eta=sigmamin/sigmamax;
Eta(j,i)=eta;
plot(j,i)
else
disp('Value out of specified range.')
Eta(j,i)=0;
end
end
end
%% Plotting
axis square
grid
hold on
[z,h]= contour(CX,CY,Eta,'ShowText', 'on');
colormap(jet)
% Change the width of the lines in the contour map
h.LineWidth = 1.5;
colorbar
代码比较混乱
d = sqrt((x3out(2)-x1out(1))^2+(y3out(2)-y1out(1))^2); % Distance between points
a = atan2(x3out(2)-x1out(1),-(y3out(2)-y1out(1))); % Perpendicular bisector angle
b = asin(d/2/(ri+li)); % Half arc angle
c = linspace(a-b,a+b); % Arc angle range
e = sqrt((ri+li)^2-d^2/4); % Distance, center to midpoint
这些都是三角函数算坐标和角度的