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.

```function Clusters=ClusterGenerate_4(TestPoint,J,J_Grad,Y_Grad,Options)
%% Expand intervals as possible

% Set up outputs
X_Tips=[TestPoint,TestPoint];
Y_Tips=Options.Y([TestPoint,TestPoint]);
J_Tips=Options.J([TestPoint,TestPoint]);
Clusters.x=TestPoint;
Clusters.y=Options.Y(TestPoint);
Clusters.j=Options.J(TestPoint);
if isfield(Options,'Verbose')
PrintOut=Options.Verbose;
else
PrintOut=false;
end
OutputStr=repmat(' %g,',1,size(TestPoint,1));
OutputStr(end)=[];

% Check for lower extensions
Finished=false;
indy=0;

if PrintOut
end
while ~Finished
indy=indy+1;
% No need to perserve step direction exactly, so use simple method.
% This does artifically reduce step length, corrected
% This doesn't step to border and then along border, so isn't most efficient
TestPoint_n=TestPoint+Delta_X;
%         Delta_X=PullStepInside(TestPoint_n,Delta_X,Options.X_range);
Delta_X((TestPoint_n<Options.X_range(:,1))|...
(TestPoint_n>Options.X_range(:,2)))=0;
end
TestPoint=TestPoint+Delta_X;
if PrintOut
fprintf(['New Test Point:',OutputStr,'\n'],TestPoint);
end
TestPoint=max(min(TestPoint,Options.X_range(:,2)),Options.X_range(:,1));
Settled=false;
Options2=Options;
for kim=1:Options.MaxTestSteps
Delta_X=PSO_OptFun_CostDescent_Armijo_1(TestPoint,Options2.CostStepOpt);
TestPoint_n=TestPoint+Delta_X;
%         disp(TestPoint_n.')
any(TestPoint_n>Options.X_range(:,2));
else
Particles_J_old=J(TestPoint); %NOT an vector
[TestPoint,Particles_J,Delta_X]=ArmijoStep_1(...
TestPoint,TestPoint+Delta_X,...
% will not cross boundary, so no need to check
end
% %         disp(Delta_X.')
%         TestPoint=TestPoint+Delta_X;
% %         disp(TestPoint.')
Settled=true;
break
end
end
if Settled
MiddleOutput=(Options.Y(TestPoint)-Y_Tips(1))<0;
NearPoints=(norm(TestPoint-X_Tips(:,1))<Options.MaxClusterDist);
TooNearPoints=(norm(TestPoint-X_Tips(:,1))<Options.MinClusterDist);
OutOfRange=~(all(TestPoint>=Options.X_range(:,1)-...
Options.BoundaryViolationAllowance)&&...
all(TestPoint<=Options.X_range(:,2)+...
Options.BoundaryViolationAllowance));
if TooNearPoints||OutOfRange
Finished=true;
if PrintOut
if TooNearPoints
fprintf(['Point settled too near last point: Current',OutputStr,' Last',OutputStr,'\n'],...
TestPoint, X_Tips(:,1));
else
fprintf(['Point settled out of range: Current',OutputStr,' Previous step',OutputStr,' Last Point',OutputStr,'\n'],...
TestPoint, TestPoint-Delta_X, X_Tips(:,1));
end
end
elseif MiddleOutput&&NearPoints
Clusters.x=[...
TestPoint,Clusters.x];
Clusters.y=[...
Options.Y(TestPoint),Clusters.y];
Clusters.j=[...
Options.J(TestPoint),Clusters.j];
Y_Tips(1)=Clusters.y(1);
X_Tips(:,1)=Clusters.x(:,1);
J_Tips(1)=Clusters.j(1);
if PrintOut
fprintf(['New Cluster Point',OutputStr,'\n'],...
Clusters.x(:,1));
end
else
Finished=true;
if PrintOut
if ~NearPoints
fprintf(['Point settled too far from last point: Current',OutputStr,' Last',OutputStr,'\n'],...
TestPoint, X_Tips(:,1));
else
fprintf('Output no longer decreasing: Current %g; Previous %g\n',...
Options.Y(TestPoint),Y_Tips(:,1));
end
end
end
else
Finished=true;
if PrintOut
fprintf(['Point did not settle within max itterations for lower bound at',OutputStr,'\n'],TestPoint);
Delta_X);
end
end
end
% Check for upper extensions
Finished=false;
indy=0;
TestPoint=X_Tips(:,2);
while ~Finished
indy=indy+1;
% Find step direction in output direction
% No need to perserve step direction exactly, so use simple method.
% This does artifically reduce step length, corrected
% This doesn't step to border and then along border, so isn't most efficient
TestPoint_n=TestPoint+Delta_X;
%         Delta_X=PullStepInside(TestPoint_n,Delta_X,Options.X_range);
Delta_X((TestPoint_n<Options.X_range(:,1))|...
(TestPoint_n>Options.X_range(:,2)))=0;
% % % NOTE: this doesn't step right to edge % % %
end
TestPoint=TestPoint+Delta_X;
if PrintOut
fprintf(['New Test Point:',OutputStr,'\n'],TestPoint);
end
TestPoint=max(min(TestPoint,Options.X_range(:,2)),Options.X_range(:,1));
Settled=false;
Options2=Options;
for kim=1:Options.MaxTestSteps
Delta_X=PSO_OptFun_CostDescent_Armijo_1(TestPoint,Options2.CostStepOpt);
TestPoint_n=TestPoint+Delta_X;
%         if PrintOut
%             fprintf(['Test Point with step:',OutputStr,'\n'],TestPoint_n);
%         end
any(TestPoint_n>Options.X_range(:,2));
temp_nan=Delta_X;
if any(isnan(Delta_X))
fprintf(['Faulty TestPoint:',OutputStr,'\n'],TestPoint_n);
fprintf(['Faulty Delta_X:',OutputStr,'\n'],temp_nan);
fprintf(['Resultant Delta_X:',OutputStr,'\n'],Delta_X);
return
end
else
Particles_J_old=J(TestPoint); %NOT an vector
[TestPoint,Particles_J,Delta_X]=ArmijoStep_1(...
TestPoint,TestPoint+Delta_X,...
% will not cross boundary, so no need to check
end
%         if PrintOut
%             fprintf(['Adj Test Point step :',OutputStr,'\n'],TestPoint);
%         end
Settled=true;
break
end
end
if Settled
MiddleOutput=...
(Options.Y(TestPoint)-Y_Tips(:,2))>0;
NearPoints=(norm(TestPoint-X_Tips(:,2))<Options.MaxClusterDist);
TooNearPoints=(norm(TestPoint-X_Tips(:,2))<Options.MinClusterDist);
OutOfRange=~(all(TestPoint>=Options.X_range(:,1)-...
Options.BoundaryViolationAllowance)&&...
all(TestPoint<=Options.X_range(:,2)+...
Options.BoundaryViolationAllowance));
if TooNearPoints||OutOfRange
Finished=true;
if PrintOut
if TooNearPoints
fprintf(['Point settled too near last point: Current',OutputStr,' Last',OutputStr,'\n'],...
TestPoint, X_Tips(:,2));
else
fprintf(['Point settled out of range: Current',OutputStr,' Previous step',OutputStr,' Last Point',OutputStr,'\n'],...
TestPoint, TestPoint-Delta_X, X_Tips(:,2));
end
end
elseif MiddleOutput&&NearPoints
Clusters.x=[...
Clusters.x,TestPoint];
Clusters.y=[...
Clusters.y,Options.Y(TestPoint)];
Clusters.j=[...
Clusters.j,Options.J(TestPoint)];
Y_Tips(2)=Clusters.y(end);
X_Tips(:,2)=Clusters.x(:,end);
J_Tips(2)=Clusters.j(end);
if PrintOut
fprintf(['New Cluster Point',OutputStr,'\n'],...
Clusters.x(:,end));
end
else
Finished=true;
if PrintOut
if ~NearPoints
fprintf(['Point settled too far from last point: Current',OutputStr,' Last',OutputStr,'\n'],...
TestPoint, X_Tips(:,2));
else
fprintf('Output no longer decreasing: Current %g; Previous %g\n',...
Options.Y(TestPoint),Y_Tips(:,2));
end
end
end
else
Finished=true;
if PrintOut
fprintf(['Point did not settle within max itterations for lower bound at',OutputStr,'\n'],TestPoint);