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.

% this function pulls a point inside a domain defined by X_range (min, max)
% assuming that the point isn't outside already
TestPointA=TestPoint-Delta_X; %location before step
TestPointA0=TestPoint-Delta_X; %used for final Delta_X calc
TestPoint0=TestPoint;
Delta_X0=Delta_X;

for jini=1:size(TestPoint,1) %possibly repeating through every dimension
tempW2=TestPoint0-min(max(TestPoint0,Options.X_range(:,1)),Options.X_range(:,2));
% tempW2 is the vector of the step after it passes the boundary
% No need for abs, since both are in the same direction
% Will fix exits along step (tempW3>1), but not perpindicular to it
% max reduction scale factors
tempW3=max(tempW2./(Delta_X0+((abs(Delta_X0)<1e-8).*-sign(tempW2)))); % Fraction of step that violates a constraint
% corrected to handle case where entries in Delta_X0 are zero
Delta_X1=-tempW3*Delta_X0; %That fraction is subtracted from the step
NewPoint1=TestPoint0+Delta_X1; %Point moved numerically exactly on nearest edge
% test to see if desent condition is violated

New_J=Fun_J(NewPoint1);
if (New_J-TestPointJ-ArmijoRequirement)>0
% decrease the step size and then exit
[NewPoint]=ArmijoStep_1(TestPointA,NewPoint1,...
Delta_X=NewPoint-TestPointA0;
% point meets optimality conditions, but should return true values
% the next run should identify the point as stationary
return;
end
% initial step had sufficient descent, so give another step along boundary
Violations=(NewPoint1<=Options.X_range(:,1))|(NewPoint1>=Options.X_range(:,2));
% 'Violations' is used to identify components to remove from the step direction

% Give additional step along boundary

% New step along edge
% disp(norm(Delta_X0))

TestPoint0=NewPoint1+Delta_X0;
TestPointA=NewPoint1;
TestPointJ=New_J;

if ~any(((TestPoint0-Options.X_range(:,1))<-Options.BoundaryViolationAllowance)|...
((TestPoint0-Options.X_range(:,2))>Options.BoundaryViolationAllowance))
% No boundary violations, therefor no need to continue
break
end
% Violations0=Violations;
end

Delta_X=TestPoint0-TestPointA0;
NewPoint=TestPoint0;