Code covered by the BSD License

# Inverse Optimal Functions for Motoman HP-3 Tip Precision

### Alan Jennings (view profile)

A population based optimization increases pointing precision for a planar robotic arm.

PSO_Function_ArmijoRobot_3

## Contents

% Based on the problem as suggested by Anthony A. Maciejewski of Colorado
% State University http://www.engr.colostate.edu/~aam/
% use a redundent robot to use configurations that decrease sensitivity to
% joint angles to increase repeatability


## Problem Parameters

X_range=pi/180*[-100,162;-125,125]; %
J_fun_num=2;
Y_fun_num=1;

% robot functions for visualization
P_1=@(theta) L_1*[cos(theta(1));sin(theta(1))];
P_2=@(theta) P_1(theta(1))+L_2*[cos(sum(theta(1:2)));sin(sum(theta(1:2)))];
P_3=@(theta) P_2(theta(1:2))+L_3*[cos(sum(theta(1:3)));sin(sum(theta(1:3)))];
Theta_o=@(P_3_pnt) -atan2(P_3_pnt(2),P_3_pnt(1));

FuncUsed={@(theta) L_1^2+L_2^2+L_3^2+2*cos(theta(1,:))*L_1*L_2+2*cos(theta(1,:)+theta(2,:))*L_1*L_3+2*cos(theta(2,:))*L_2*L_3 ;... % radial distance to tip
@(theta) 10*(L_1^2*(L_2*sin(theta(1,:))+L_3*sin(theta(1,:)+theta(2,:))).^2+...
L_3^2*(L_2*sin(theta(2,:))+L_1*sin(theta(1,:)+theta(2,:))).^2)};

@(theta) 10*[L_1*L_2*L_3^2*sin(theta(1,:)+2*theta(2,:))-L_1*L_2*L_3^2*sin(theta(1,:))+L_1^2*L_2^2*sin(2*theta(1,:))+2*L_1^2*L_3^2*sin(2*theta(1,:)+2*theta(2,:))+2*L_1^2*L_2*L_3*sin(2*theta(1,:)+theta(2,:));...

FunOptzgParameters=[...
0.025, 0.025, 0.08, 0.05, 0.025*[8, 2e-2], .5, 1e-1;...
0.025, 0.025, 0.08, 0.05, 0.025*[8, 2e-2], .5, 1e-1];...
% First row for first function; Second row, second function...
% KillDist (Cost), SettledStepSize (Cost), MinGrad_Final
% From test functions
%     0.025, 0.025, 0.08, 0.05, 0.025*[3, 2e-5], .5, 1e-1;...

FunCustngParameters=[...
250, FunOptzgParameters(1,6),...
max(FunOptzgParameters(Y_fun_num,3),FunOptzgParameters(J_fun_num,1))*[10, 0.00015];...
250, FunOptzgParameters(2,6),...
max(FunOptzgParameters(Y_fun_num,3),FunOptzgParameters(J_fun_num,1))*[10, 0.00015]];
% MaxTestSteps, MaxGradientSize (measure of particle is settled),
% MaxClusterDist, MinClusterDist

CustngTestParameters=[...
20, 6, 0.5, 0.3;...
20, 6, 0.5, 0.3];
% NumTestPerClust, NumTestPerPoint, PervertScale, addition to PervertScale

J=@(X) FuncUsed{J_fun_num}(X);
Y=@(X) FuncUsed{Y_fun_num}(X);

XSamples=[28,35];
[X1,X2]=meshgrid(linspace(X_range(1,1),X_range(1,2),XSamples(1)),...
linspace(X_range(2,1),X_range(2,2),XSamples(2)));
YY=reshape(Y([X1(:),X2(:)].'),size(X1));
JJ=reshape(J([X1(:),X2(:)].'),size(X1));

figure(10); clf;
subplot(2,2,1); contour(X1,X2,YY);
title('Output Function'); xlabel('x_1'); ylabel('x_2'); colorbar;
axis equal
subplot(2,2,3); contour(X1,X2,JJ);
title('Cost Function'); xlabel('x_1'); ylabel('x_2'); colorbar;
axis equal
subplot(1,2,2); cla; hold on; contour(X1,X2,YY); contour(X1,X2,JJ); hold off;
title('Both Functions Overlayed'); xlabel('x_1'); ylabel('x_2');
axis equal;axis tight

%
figure(31)
subplot(211)
ylabel('# occurances');
subplot(212)
ylabel('# occurances');
xlabel('These Gradients should be on the same order')


GradTestPoints=repmat((X_range(:,2)-X_range(:,1)),1,100).*...
rand(size(X_range,1),100)+repmat(X_range(:,1),1,100);
TestStepMag=0.001;
end
figure(17);
%looks good, error is proportional to TestStepMag


## Optimization Parameters

NumParticles=36; %initial number of agents
ParticleView=1;%[1:3];%which particles to observe as they take steps
ParticleViewBaseFig=1000;
for etsuko=ParticleView
figure(ParticleViewBaseFig+100+etsuko);clf;
end
MaxSteps=300; %maximum number of steps
StepDecayRate=0.12;
PauseLength=0.005;
BoundaryViolationAllowance=1e-8;

Gamma_Cost=FunOptzgParameters(J_fun_num,1); % maximum step size
% the cost gradient will be reduced from the maximum step size
% Note that the maximum step is also reduced by (1-cos) of the angle between

% the Output gradient will be reduced from unity resulting in a component
% in the output gradient direction to be left in the step
% KillDist=2*Gamma_Cost;
KillDist=FunOptzgParameters(J_fun_num,5);
% SettledStepSize=0.02*Gamma_Cost;
SettledStepSize=FunOptzgParameters(J_fun_num,6);
%this saturates the normalization for the correction of boundary violations


## Clustering parameters

MaxTestSteps=FunCustngParameters(J_fun_num,1);
MaxClusterDist=FunCustngParameters(J_fun_num,3);
MinClusterDist=FunCustngParameters(J_fun_num,4);

Gamma_Out=FunOptzgParameters(Y_fun_num,3);
% maximum step size for extending the clusters


## Testing parameters

NumTestPerClust=CustngTestParameters(J_fun_num,1);
NumTestPerPoint=CustngTestParameters(J_fun_num,2); % purtibations to ideal points
PervertScale=CustngTestParameters(J_fun_num,3); % scaling of total range to apply for purtibations
PervertConstant=CustngTestParameters(J_fun_num,4); % Added to scale to ensure some variation
TestBaseFigNum=2000;


## initialize particles

% Might as well do uniform random distrubution
Particles_intial=repmat((X_range(:,2)-X_range(:,1)),1,NumParticles).*...
rand(size(X_range,1),NumParticles)+repmat(X_range(:,1),1,NumParticles);
% Particles_intial=[-0.7407;1.135];NumParticles=1; %to test a specific point


## Run Optimization

Particles_new=Particles_intial;
Particles_J=zeros(1,size(Particles_new,2));
for deedee=1:size(Particles_new,2);
Particles_J(deedee)=J(Particles_new(:,deedee));
end
Particles_J_last=Particles_J;
StationaryPoints=zeros(nargin(Y),0);
NumClusters=0;
clear Clusters

% Set up for function form
CostStepOpt.J=J;
CostStepOpt.Gamma_Cost=Gamma_Cost;
CostStepOpt.Gamma_Out=Gamma_Out;
CostStepOpt.X_range=X_range;
CostStepOpt.BoundaryViolationAllowance=BoundaryViolationAllowance;
CostStepOpt.Gamma_Cost=Gamma_Cost;
CostStepOpt.Gamma_Out=Gamma_Out;
CostStepOpt.StepDiscount=StepDecayRate;
CostStepOpt.MaxDecay=40;
CostStepOpt.ArmijoK=0.4;

% for cluster forming
ClusterFrmOpt.Y=Y;
ClusterFrmOpt.J=J;
ClusterFrmOpt.CostStepOpt=CostStepOpt;
ClusterFrmOpt.Gamma_Cost=Gamma_Cost;
ClusterFrmOpt.Gamma_Out=Gamma_Out;
ClusterFrmOpt.MaxTestSteps=MaxTestSteps;
ClusterFrmOpt.MaxClusterDist=MaxClusterDist;
ClusterFrmOpt.MinClusterDist=MinClusterDist;
ClusterFrmOpt.X_range=X_range;
ClusterFrmOpt.BoundaryViolationAllowance=BoundaryViolationAllowance;
ClusterFrmOpt.Verbose=false;
ClusterFrmOpt.StepsBeginDecay=floor(0.8*ClusterFrmOpt.MaxTestSteps);
ClusterFrmOpt.StepDiscount=CostStepOpt.StepDiscount;
ClusterFrmOpt.MaxDecay=CostStepOpt.MaxDecay;
ClusterFrmOpt.ArmijoK=CostStepOpt.ArmijoK;

tic;
for becky=1:MaxSteps
NumParticles=size(Particles_new,2);
Particles_old=Particles_new;
Delta_X=PSO_OptFun_CostDescent_Armijo_1(Particles_old,CostStepOpt);
Particles_new=Particles_old+Delta_X;

figure(12); clf; hold on;
contour(X1,X2,YY); contour(X1,X2,JJ);
plot(Particles_old(1,:),Particles_old(2,:),'r.')
if ~isempty(StationaryPoints)
plot(StationaryPoints(1,:),StationaryPoints(2,:),'go','markerface','g')
end
plot(cumsum([Particles_old(1,:);Delta_X(1,:)],1),...
cumsum([Particles_old(2,:);Delta_X(2,:)],1),'r-')
daspect([1,1,1]);
hold off;
title(['Swarm, Step: ' num2str(becky),...
' Number of particles: ', num2str(NumParticles),...
' Number of clusters: ', num2str(NumClusters)]);
xlabel('x_1'); ylabel('x_2');
drawnow;

% Move particles to within the joint angle limits
any(Particles_new>repmat(X_range(:,2),1,NumParticles),1);
[Delta_X(:,gabby),Particles_new(:,gabby)]=PullStepInside3(Particles_new(:,gabby),Particles_J(gabby),...
% note cost is based on Particles_old, but this isn't passed in
Particles_J(gabby)=J(Particles_new(:,gabby));
end

% test sufficient descent, testing the boundary ones can mess it up, so
% exclude those points (two or more steps may not meet the condition
% for one step).
Particles_J_new=J(Particles_new(:,gabby)); %NOT an vector
[Particles_new(:,gabby),Particles_J(gabby),Delta_X(:,gabby),...
Particles_old(:,gabby),Particles_new(:,gabby),...
% will not cross boundary, so no need to check
end

if max(ParticleView)>NumParticles
ParticleView(ParticleView>NumParticles)=[];  %#ok<SAGROW>
end
for etsuko=ParticleView;
figure(ParticleViewBaseFig+etsuko);

plot(Particles_old(1,etsuko),Particles_old(2,etsuko),'bo',...
title(num2str(round(Particles_old(:,etsuko).'*100)/100));
daspect([1,1,1])
figure(ParticleViewBaseFig+100+etsuko);hold on;
if (Particles_J_last(etsuko)-Particles_J(etsuko))>0
plot(becky,log10(Particles_J_last(etsuko)-Particles_J(etsuko)),'b.');
else
plot(becky,log10(-(Particles_J_last(etsuko)-Particles_J(etsuko))),'bx');
end
else
if (Particles_J_last(etsuko)-Particles_J(etsuko))>0
plot(becky,log10(Particles_J_last(etsuko)-Particles_J(etsuko)),'r.');
else
plot(becky,log10(Particles_J_last(etsuko)-Particles_J(etsuko)),'rx');
end
end
title('Log of the change in cost (blue increase, red decrease)')
end
% pause;

if any(any(Particles_new<repmat(X_range(:,1)-BoundaryViolationAllowance,1,NumParticles),1)|...
any(Particles_new>repmat(X_range(:,2)+BoundaryViolationAllowance,1,NumParticles),1))
disp(find(any(Particles_new<repmat(X_range(:,1),1,NumParticles),1)|...
any(Particles_new>repmat(X_range(:,2),1,NumParticles),1)));
any(Particles_new>repmat(X_range(:,2),1,NumParticles),1);
tempW1=Particles_new(:,gabby);
fprintf('Initial Location:  %.20g, %.20g\n',Particles_old(:,gabby))
fprintf('Initial motion:    %.20g, %.20g\n',Delta_X(:,gabby))
fprintf('Next Location:     %.20g, %.20g\n',tempW1)
%find difference
tempW2=tempW1-min(max(tempW1,X_range(:,1)),X_range(:,2));
fprintf('Boundary Violation: %.20g, %.20g\n',tempW2)
tempW3=max(tempW2./Delta_X(:,gabby));
fprintf('Violation Scale:    %.20g, %.20g\n',tempW2./Delta_X(:,gabby))
Delta_X_temp=(1-tempW3)*Delta_X(:,gabby);
fprintf('Final motion:       %.20g, %.20g\n',Delta_X_temp)
fprintf('Final Location:     %.20g, %.20g\n\n',Particles_new(:,gabby))
end
fprintf('Did not get particle inside bound')
end
clear Adj_Delta_X gabby tempW1 tempW2 tempW3

% tempA and tempB are arrays of the particles in two different
% directions
ParticlesAll=[Particles_new, StationaryPoints];
tempA=repmat(reshape(ParticlesAll.',1,size(ParticlesAll,2),[]),size(Particles_new,2),1);
tempB=repmat(reshape(Particles_new.',size(Particles_new,2),1,[]),1,size(ParticlesAll,2));
% The relative direction of any particle with respect to any other
Direction_all=tempA-tempB;
% Find the distance, the lower diag inf makes it so only the lower
% indexed particle is labeled as violating the distance, prevents
% double deletion
Dist=sum(Direction_all.^2,3).^.5+...
[tril(inf+zeros(size(Particles_new,2))), ...
zeros(size(Particles_new,2),size(ParticlesAll,2)-size(Particles_new,2))];
% NOTE: there are computer science ways of doing this more efficiently,
% but isn't needed at this scale for my purposes
Dist_min=min(Dist,[],2);
Particles_new(:,Dist_min<KillDist)=[];
Particles_old(:,Dist_min<KillDist)=[];
Particles_J(:,Dist_min<KillDist)=[];
Delta_X(:,Dist_min<KillDist)=[];
clear ParticlesAll tempA tempB Direction_all Dist Dist_min

Delta_X_Sz=(sum(Delta_X.^2,1)).^.5;
temp42=find(Delta_X_Sz<=SettledStepSize);
for amy=temp42 %over the settled points
if NumClusters>0 %checking on first cluster can cause indexing problems
Dist=sum((StationaryPoints-repmat(Particles_new(:,amy),...
1,size(StationaryPoints,2))).^2,1).^.5;
Dist_min=min(Dist,[],2);
% fprintf('\nMinimum Distance From Stationary Point: %g',Dist_min);
if Dist_min<KillDist
continue %removed this particle and move to next
end
end
NumClusters=NumClusters+1;
temp_t=toc;

Clusters{NumClusters}.genStartTime=temp_t; %#ok<SAGROW>
StationaryPoints=...
[StationaryPoints,Clusters{NumClusters}.x]; %#ok<AGROW>
Clusters{NumClusters}.genElapsedTime=toc-Clusters{NumClusters}.genStartTime; %#ok<SAGROW>
Clusters{NumClusters}.genStep=becky;%#ok<SAGROW>
fprintf(['Generating Cluster %g, at itteration %g and ',...
'elapsed time %g min; Generation took %g min and contains %g points\n'],...
NumClusters,becky,toc/60,(Clusters{NumClusters}.genElapsedTime)/60,...
size(Clusters{NumClusters}.x,2));
if numel(Clusters{NumClusters}.y)>1
figure(20+NumClusters);clf;
for jax=1:size(Clusters{NumClusters}.x,2)
theta_temp=[Theta_o(P_3([0;Clusters{NumClusters}.x(:,jax)]));Clusters{NumClusters}.x(:,jax)];
plot3(Clusters{NumClusters}.y(jax)*[1,1],[0,[1,0]*P_1(theta_temp)],[0,[0,1]*P_1(theta_temp)],'b-o',...
Clusters{NumClusters}.y(jax)*[1,1],[[1,0]*P_1(theta_temp),[1,0]*P_2(theta_temp)],[[0,1]*P_1(theta_temp),[0,1]*P_2(theta_temp)],'r-o',...
Clusters{NumClusters}.y(jax)*[1,1],[[1,0]*P_2(theta_temp),[1,0]*P_3(theta_temp)],[[0,1]*P_2(theta_temp),[0,1]*P_3(theta_temp)],'g-o');
hold on;
end
view([26,18]);hold off;daspect([1,1,1]);
end
end
Particles_new(:,temp42)=[];
Particles_J(:,temp42)=[];

clear Delta_X_Sz temp42 amy Delta_X

if size(Particles_new,2)<1
break;
end

Particles_J_last=Particles_J;
pause(PauseLength);
end
% xlabel('All Done Now, you can go back to what you were doing')
fprintf(['Finished at itteration %g and ',...
'elapsed time %g min; Generated %g Clusters\n'],...
becky,toc/60,NumClusters);
figure(12); clf; hold on;
contour(X1,X2,YY); contour(X1,X2,JJ);
plot(StationaryPoints(1,:),StationaryPoints(2,:),'go','markerface','g')
daspect([1,1,1]);
hold off;
title(['Swarm finished, Step: ' num2str(becky),...
' Number of particles: ', num2str(size(Particles_new,2)),...
' Number of clusters: ', num2str(NumClusters)]);
xlabel('x_1'); ylabel('x_2');
drawnow;

clear becky

Generating Cluster 1, at itteration 6 and elapsed time 0.0665969 min; Generation took 0.00408574 min and contains 52 points
Generating Cluster 2, at itteration 6 and elapsed time 0.0746527 min; Generation took 0.00305104 min and contains 19 points
Generating Cluster 3, at itteration 11 and elapsed time 0.136372 min; Generation took 0.00432103 min and contains 11 points
Generating Cluster 4, at itteration 12 and elapsed time 0.15531 min; Generation took 0.00475001 min and contains 57 points
Generating Cluster 5, at itteration 32 and elapsed time 0.372232 min; Generation took 0.00433275 min and contains 45 points
Generating Cluster 6, at itteration 96 and elapsed time 1.04032 min; Generation took 0.00812108 min and contains 15 points
Finished at itteration 265 and elapsed time 2.82389 min; Generated 6 Clusters


## Find output ranges for clusters and sort points by output

Y_Tips=zeros(2,size(Clusters,2));
X_Tips=zeros(size(Particles_intial,1),size(Clusters,2),2);
J_Tips=zeros(2,size(Clusters,2));
for marsha=1:length(Clusters)
[Clusters{marsha}.y, temp3]=sort(Clusters{marsha}.y); %#ok<SAGROW>
Clusters{marsha}.x=Clusters{marsha}.x(:,temp3);%#ok<SAGROW>
Clusters{marsha}.j=Clusters{marsha}.j(:,temp3);%#ok<SAGROW>
Y_Tips(:,marsha)=[Clusters{marsha}.y(1),Clusters{marsha}.y(end)].';
X_Tips(:,marsha,1)=Clusters{marsha}.x(:,1);
X_Tips(:,marsha,2)=Clusters{marsha}.x(:,end);
J_Tips(:,marsha)=[Clusters{marsha}.j(:,1),Clusters{marsha}.j(:,end)].';
end
clear marsha temp3


## Remove solitary clusters, I'm not going to use them

RemoveClust=false(size(Clusters));
for marsha=1:length(Clusters)
if size(Clusters{marsha}.x,2)<=1
RemoveClust(marsha)=true;
end
end
Y_Tips(:,RemoveClust)=[];
X_Tips(:,RemoveClust,:)=[];
J_Tips(:,RemoveClust)=[];
% Clusters=Clusters{~RemoveClust};% Doesn't work, darn cell structure
ClustersNew=cell(1,sum(~RemoveClust));
jones=find(~RemoveClust);
for indy=1:length(ClustersNew)
ClustersNew{indy}=Clusters{jones(indy)};
end
Clusters=ClustersNew;

clear marsha indy jones RemoveClusters ClustersNew


## Plot Clusters

figure(3); clf;
subplot(1,3,1); contour(X1,X2,YY);
daspect([1,1,1]);
subplot(1,3,2); contour(X1,X2,JJ);
daspect([1,1,1]);
subplot(1,3,3); cla; hold on; contour(X1,X2,YY); contour(X1,X2,JJ); hold off;
daspect([1,1,1]);

% plot in X space to verify
figure(2223);
ColorSet=colormap(hsv(max(length(Clusters))));
Markers='so+x^';
clf; hold on;
contour(X1,X2,YY); contour(X1,X2,JJ);
for marsha=1:length(Clusters)
if size(Clusters{marsha}.x,2)>=1
plot(Clusters{marsha}.x(1,:),Clusters{marsha}.x(2,:),['-', Markers(mod(marsha-1,length(Markers))+1)],...
'color',ColorSet(marsha,:));
end
end
clear marsha ColorSet
axis equal
hold off;
title('Optimal Inverse Functions');
axis([X_range(1,:),X_range(2,:)]);

% plot in Y space vs X and J
figure(2323); clf;
for uri=1:size(Particles_intial,1);
subplot(size(Particles_intial,1)+1,1,uri);hold on;grid on;
ylabel(['\theta_', num2str(uri)]);
set(gca,'XTickLabel',[]);
end
subplot(size(Particles_intial,1)+1,1,1);hold on;grid on;
title(['Optimal Inverse Functions, h_i' char(39), 's'])
subplot(size(Particles_intial,1)+1,1,size(Particles_intial,1)+1);hold on;grid on;
xlabel('y_d');ylabel('J');
ColorSet=colormap(hsv(max(length(Clusters))));
y_plot_min=inf;y_plot_max=-inf;
j_plot_min=inf;j_plot_max=-inf;
for marsha=1:length(Clusters)
if size(Clusters{marsha}.x,2)>=1
for uri=1:size(Particles_intial,1)
subplot(size(Particles_intial,1)+1,1,uri);
plot(Clusters{marsha}.y,Clusters{marsha}.x(uri,:),['-', Markers(mod(marsha-1,length(Markers))+1)],...
'color',ColorSet(marsha,:));
end
subplot(size(Particles_intial,1)+1,1,size(Particles_intial,1)+1);
plot(Clusters{marsha}.y,Clusters{marsha}.j,['-', Markers(mod(marsha-1,length(Markers))+1)],...
'color',ColorSet(marsha,:));
if min(Clusters{marsha}.y)<y_plot_min
y_plot_min=min(Clusters{marsha}.y);
end
if max(Clusters{marsha}.y)>y_plot_max
y_plot_max=max(Clusters{marsha}.y);
end
if min(Clusters{marsha}.j)<j_plot_min
j_plot_min=min(Clusters{marsha}.j);
end
if max(Clusters{marsha}.j)>j_plot_max
j_plot_max=max(Clusters{marsha}.j);
end
end
end
for uri=1:size(Particles_intial,1)
subplot(size(Particles_intial,1)+1,1,uri);
axis([y_plot_min,y_plot_max,X_range(uri,1),X_range(uri,2)])
end
subplot(size(Particles_intial,1)+1,1,size(Particles_intial,1)+1);
axis([y_plot_min,y_plot_max,j_plot_min,j_plot_max])

clear marsha ColorSet


## Test results

for marsha=1:length(Clusters)
TestY_d=rand(1,NumTestPerClust)*...
(Clusters{marsha}.y(end)-Clusters{marsha}.y(1))+Clusters{marsha}.y(1);
TestX=interp1(Clusters{marsha}.y.',Clusters{marsha}.x.',TestY_d,...
'pchip','extrap').';
PervertsX=zeros(size(Clusters{marsha}.x,1),NumTestPerClust*NumTestPerPoint);
for alice=1:NumTestPerClust
PervertsX(:,NumTestPerPoint*(alice-1)+(1:NumTestPerPoint))=...
PervertScale*repmat(max(Clusters{marsha}.x,[],2)-...
min(Clusters{marsha}.x,[],2),1,NumTestPerPoint).*...
(rand(size(Clusters{marsha}.x,1),NumTestPerPoint)-.5)+...
PervertConstant*(rand(size(Clusters{marsha}.x,1),NumTestPerPoint)-.5)+...
repmat(TestX(:,alice),1,NumTestPerPoint);
end
PervertsX=max(min(PervertsX,repmat(X_range(:,2),1,size(PervertsX,2))),...
repmat(X_range(:,1),1,size(PervertsX,2)));
TestY_a=Y(TestX);
TestJ_a=J(TestX);
TestJ_d=interp1(Clusters{marsha}.y.',Clusters{marsha}.j.',TestY_a,...
'pchip','extrap');
PervertsY_a=Y(PervertsX);
PervertsJ_a=J(PervertsX);
PervertsJ_d=interp1(Clusters{marsha}.y.',Clusters{marsha}.j.',PervertsY_a,...
'pchip','extrap');

Y_error=TestY_a-TestY_d;

figure(TestBaseFigNum+marsha);clf;
subplot(241)
plot([min([TestY_d(:);TestY_a(:)]),max([TestY_d(:);TestY_a(:)])],...
[min([TestY_d(:);TestY_a(:)]),max([TestY_d(:);TestY_a(:)])],'r-',...
TestY_d,TestY_a,'b.')
title('Output vs Desired')
ylabel('y_a')
xlabel('y_d')
axis equal
subplot(245)
plot(TestY_d,Y_error,'b.')
title('Error in output')
ylabel('y_a-y_d')
xlabel('y_d')

subplot(242)
plot(PervertsY_a,PervertsJ_a,'g.',TestY_a,TestJ_a,'b.')
title('Cost of outputs')
ylabel('J')
xlabel('y')
subplot(246)
plot(PervertsY_a,PervertsJ_a-PervertsJ_d,'g.',TestY_a,TestJ_a-TestJ_d,'b.')
title('Cost from expected Optimal')
ylabel('J-J(y)')
xlabel('y')

subplot(122);hold on;
contour(X1,X2,YY); contour(X1,X2,JJ);
plot(PervertsX(1,:),PervertsX(2,:),'g.',TestX(1,:),TestX(2,:),'b.',...
Clusters{marsha}.x(1,:),Clusters{marsha}.x(2,:),'r.')
daspect([1,1,1]);axis tight
hold off;
xlabel('\theta_1'); ylabel('\theta_2');
end
% clear marsha TestY_d TestX PervertsX alice TestY_a TestJ_a PervertsY_a
% clear PervertsJ_a Y_error


## Conclude

save('PSO_Function_ArmijoTest_20120405_3.mat')