Code covered by the BSD License  

Highlights from
Inverse Optimal Functions for Motoman HP-3 Tip Precision

image thumbnail
from Inverse Optimal Functions for Motoman HP-3 Tip Precision by Alan Jennings
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

Parameters

Problem Parameters

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

L_1=0.29;L_2=norm([.085,0.300]);L_3=0.09+4*0.0254; %link lengths
% 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)};

FuncGrad={@(theta) -2*[L_1*L_3*sin(theta(1,:)+theta(2,:))+L_1*L_2*sin(theta(1,:));L_1*L_3*sin(theta(1,:)+theta(2,:))+L_2*L_3*sin(theta(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,:));...
    L_1^2*L_2*L_3*sin(2*theta(1,:)+theta(2,:))-L_1^2*L_2*L_3*sin(theta(2,:))+L_2^2*L_3^2*sin(2*theta(2,:))+2*L_1^2*L_3^2*sin(2*theta(1,:)+2*theta(2,:))+2*L_1*L_2*L_3^2*sin(theta(1,:)+2*theta(2,:))]}; %note gradients are columns

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...
% GammaCost,MinGrad_Cost,Gamma_Out,MinGrad_Out,...
% 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);
J_Grad=@(X) FuncGrad{J_fun_num}(X);
Y_Grad=@(X) FuncGrad{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

YY_GradMag=sqrt(sum(Y_Grad([X1(:),X2(:)].').^2,1));
JJ_GradMag=sqrt(sum(J_Grad([X1(:),X2(:)].').^2,1));
%
figure(31)
subplot(211)
hist(YY_GradMag,60);
title('Output Gradient Magnitude Histogram');
ylabel('# occurances');
subplot(212)
hist(JJ_GradMag,60);
title('Cost Gradient Magnitude Histogram');
ylabel('# occurances');
xlabel('These Gradients should be on the same order')

test for gradient accuracy

GradTestPoints=repmat((X_range(:,2)-X_range(:,1)),1,100).*...
    rand(size(X_range,1),100)+repmat(X_range(:,1),1,100);
GradTestPoints_diff_Y=zeros(size(GradTestPoints));
GradTestPoints_func_Y=zeros(size(GradTestPoints));
GradTestPoints_diff_J=zeros(size(GradTestPoints));
GradTestPoints_func_J=zeros(size(GradTestPoints));
TestStepMag=0.001;
for lexi=1:size(GradTestPoints,2)
    GradTestPoints_diff_Y(:,lexi)=[...
        (Y(GradTestPoints(:,lexi)+TestStepMag*[1;0])-Y(GradTestPoints(:,lexi)))/TestStepMag;...
        (Y(GradTestPoints(:,lexi)+TestStepMag*[0;1])-Y(GradTestPoints(:,lexi)))/TestStepMag];
    GradTestPoints_diff_J(:,lexi)=[...
        (J(GradTestPoints(:,lexi)+TestStepMag*[1;0])-J(GradTestPoints(:,lexi)))/TestStepMag;...
        (J(GradTestPoints(:,lexi)+TestStepMag*[0;1])-J(GradTestPoints(:,lexi)))/TestStepMag];
    GradTestPoints_func_Y(:,lexi)=Y_Grad(GradTestPoints(:,lexi));
    GradTestPoints_func_J(:,lexi)=J_Grad(GradTestPoints(:,lexi));
end
figure(17);
subplot(2,2,1);plot(GradTestPoints_diff_Y(1,:),GradTestPoints_diff_Y(1,:)-GradTestPoints_func_Y(1,:),'.'); %daspect([1,1,1])
title('Difference of finite step gradient and analyitical gradient');
subplot(2,2,3);plot(GradTestPoints_diff_Y(2,:),GradTestPoints_diff_Y(2,:)-GradTestPoints_func_Y(2,:),'.'); %daspect([1,1,1])
subplot(2,2,2);plot(GradTestPoints_diff_J(1,:),GradTestPoints_diff_J(1,:)-GradTestPoints_func_J(1,:),'.'); %daspect([1,1,1])
subplot(2,2,4);plot(GradTestPoints_diff_J(2,:),GradTestPoints_diff_J(2,:)-GradTestPoints_func_J(2,:),'.'); %daspect([1,1,1])
%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
MinGrad_Cost=FunOptzgParameters(J_fun_num,2);
% when the cost gradient is less than this MinGrad_Cost,
% 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
% gradients
MinGrad_Final=FunOptzgParameters(J_fun_num,7);

MinGrad_Out=FunOptzgParameters(Y_fun_num,4);
% when the output gradient is less than this MinGrad_Out,
% 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);
MinBoundGrad=FunOptzgParameters(Y_fun_num,8);
%this saturates the normalization for the correction of boundary violations

Clustering parameters

MaxTestSteps=FunCustngParameters(J_fun_num,1);
MaxGradientSize=FunCustngParameters(J_fun_num,2);
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.Grad_J=J_Grad;
CostStepOpt.Grad_Y=Y_Grad;
CostStepOpt.MinGrad_Cost=MinGrad_Cost;
CostStepOpt.Gamma_Cost=Gamma_Cost;
CostStepOpt.MinGrad_Out=MinGrad_Out;
CostStepOpt.Gamma_Out=Gamma_Out;
CostStepOpt.MinGrad_Final=MinGrad_Final;
CostStepOpt.MinGrad_Bnd=MinBoundGrad;
CostStepOpt.X_range=X_range;
CostStepOpt.BoundaryViolationAllowance=BoundaryViolationAllowance;
CostStepOpt.MinGrad_Cost=MinGrad_Cost;
CostStepOpt.Gamma_Cost=Gamma_Cost;
CostStepOpt.MinGrad_Out=MinGrad_Out;
CostStepOpt.Gamma_Out=Gamma_Out;
CostStepOpt.MinGrad_Final=MinGrad_Final;
CostStepOpt.StepDiscount=StepDecayRate;
CostStepOpt.MaxDecay=40;
CostStepOpt.ArmijoK=0.4;

% for cluster forming
ClusterFrmOpt.Y=Y;
ClusterFrmOpt.J=J;
ClusterFrmOpt.Grad_J=J_Grad;
ClusterFrmOpt.Grad_Y=Y_Grad;
ClusterFrmOpt.CostStepOpt=CostStepOpt;
ClusterFrmOpt.MinGrad_Cost=MinGrad_Cost;
ClusterFrmOpt.Gamma_Cost=Gamma_Cost;
ClusterFrmOpt.MinGrad_Out=MinGrad_Out;
ClusterFrmOpt.Gamma_Out=Gamma_Out;
ClusterFrmOpt.MinGrad_Final=MinGrad_Final;
ClusterFrmOpt.MinGrad_Bnd=MinBoundGrad;
ClusterFrmOpt.MaxTestSteps=MaxTestSteps;
ClusterFrmOpt.MaxGradientSize=MaxGradientSize;
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;
    Particles_adj=false(1,size(Particles_new,2));

    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
    Adj_Delta_X=any(Particles_new<repmat(X_range(:,1),1,NumParticles),1)|...
        any(Particles_new>repmat(X_range(:,2),1,NumParticles),1);
    Particles_adj(Adj_Delta_X)=true;
    for gabby=find(Adj_Delta_X)
        [Delta_X(:,gabby),Particles_new(:,gabby)]=PullStepInside3(Particles_new(:,gabby),Particles_J(gabby),...
            J_Grad(Particles_old(:,gabby)),Delta_X(:,gabby),J,J_Grad,Y_Grad,CostStepOpt);
        % 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).
    for gabby=find(~Adj_Delta_X)
    Particles_J_new=J(Particles_new(:,gabby)); %NOT an vector
    [Particles_new(:,gabby),Particles_J(gabby),Delta_X(:,gabby),...
        Particles_adj(gabby)]=ArmijoStep_1(...
        Particles_old(:,gabby),Particles_new(:,gabby),...
        Particles_J(gabby),J_Grad(Particles_old(:,gabby)),CostStepOpt);
    % will not cross boundary, so no need to check
    end

hold on;plot(Particles_old(1,Particles_adj),Particles_old(2,Particles_adj),'ys');hold off;drawnow;


if max(ParticleView)>NumParticles
    ParticleView(ParticleView>NumParticles)=[];  %#ok<SAGROW>
end
for etsuko=ParticleView;
    figure(ParticleViewBaseFig+etsuko);
    Grad_Cost1=J_Grad(Particles_old(:,etsuko));
    Grad_Cost2=Grad_Cost1./max(sum(Grad_Cost1.^2,1).^.5,CostStepOpt.MinGrad_Cost);
    Grad_Out1=Y_Grad(Particles_old(:,etsuko));
    Grad_Out2=Grad_Out1./max(sum(Grad_Out1.^2,1).^.5,CostStepOpt.MinGrad_Out);
    Delta_X1=-Grad_Cost2+(Grad_Out2.'*Grad_Cost2)*Grad_Out2;
    Delta_X2=CostStepOpt.Gamma_Cost*Delta_X1./max(sum(Delta_X1.^2,1).^.5,CostStepOpt.MinGrad_Final);

    plot(Particles_old(1,etsuko),Particles_old(2,etsuko),'bo',...
        cumsum([Particles_old(1,etsuko),Delta_X1(1)/CostStepOpt.MinGrad_Final],2).',cumsum([Particles_old(2,etsuko),Delta_X1(2)/CostStepOpt.MinGrad_Final],2).','r-+',...
        cumsum([Particles_old(1,etsuko),-Grad_Cost1(1)/CostStepOpt.MinGrad_Final],2).',cumsum([Particles_old(2,etsuko),-Grad_Cost1(2)/CostStepOpt.MinGrad_Final],2).','m-x',...
        cumsum([Particles_old(1,etsuko),Grad_Out1(1)/CostStepOpt.MinGrad_Final],2).',cumsum([Particles_old(2,etsuko),Grad_Out1(2)/CostStepOpt.MinGrad_Final],2).','g-^');
    title(num2str(round(Particles_old(:,etsuko).'*100)/100));
    legend({'Location','Actual Step','-Cost Grad','Output Grad'},'location','best')
    daspect([1,1,1])
    figure(ParticleViewBaseFig+100+etsuko);hold on;
    if Particles_adj(etsuko)
        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)));
    Adj_Delta_X=any(Particles_new<repmat(X_range(:,1),1,NumParticles),1)|...
        any(Particles_new>repmat(X_range(:,2),1,NumParticles),1);
    for gabby=find(Adj_Delta_X)
        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
            % Need to add check if last cluster added made this one obsolete
            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}=ClusterGenerate_4(Particles_new(:,amy),J,J_Grad,Y_Grad,ClusterFrmOpt); %#ok<SAGROW>


        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
        xlabel('Output');ylabel('Radial');zlabel('Tangential');
        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);
title('Output Function'); xlabel('\theta_1 (rad)'); ylabel('\theta_2 (rad)');% colorbar;
daspect([1,1,1]);
subplot(1,3,2); contour(X1,X2,JJ);
title('Cost Function'); xlabel('\theta_1 (rad)'); ylabel('\theta_2 (rad)');% colorbar;
daspect([1,1,1]);
subplot(1,3,3); cla; hold on; contour(X1,X2,YY); contour(X1,X2,JJ); hold off;
title('Both Functions Overlayed'); xlabel('\theta_1 (rad)'); ylabel('\theta_2 (rad)');% colorbar;
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');
xlabel('\theta_1 (rad)'); ylabel('\theta_2 (rad)');% colorbar;
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')


Published with MATLAB® 7.8

Contact us