Code covered by the BSD License  

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

image thumbnail

Inverse Optimal Functions for Motoman HP-3 Tip Precision

by

 

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

[Delta_X, NewPoint]=PullStepInside3(TestPoint,TestPointJ,TestPointGradJ,...
function [Delta_X, NewPoint]=PullStepInside3(TestPoint,TestPointJ,TestPointGradJ,...
    Delta_X,Fun_J,Grad_J,Grad_y,Options)
% 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);
ArmijoRequirement=Options.ArmijoK*(TestPointGradJ(:).')*(NewPoint1-TestPointA);
if (New_J-TestPointJ-ArmijoRequirement)>0
    % decrease the step size and then exit
[NewPoint]=ArmijoStep_1(TestPointA,NewPoint1,...
    TestPointJ,TestPointGradJ,Options);
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
Grad_Cost1a=Grad_J(NewPoint1);
Grad_Cost1b=Grad_Cost1a;
Grad_Cost1b(Violations)=0;
Grad_Cost2=Grad_Cost1b./max(sum(Grad_Cost1b.^2,1).^.5,Options.MinGrad_Cost);

Grad_Out1a=Grad_y(NewPoint1);
Grad_Out1b=Grad_Out1a;
Grad_Out1b(Violations)=0;
Grad_Out2=Grad_Out1b./max(sum(Grad_Out1b.^2,1).^.5,Options.MinGrad_Out);
% New step along edge
Delta_X0=-Grad_Cost2+Grad_Out2.'*Grad_Cost2*Grad_Out2;
% disp(norm(Delta_X0))
Delta_X0=Options.Gamma_Cost*Delta_X0./max(norm(Delta_X0),Options.MinGrad_Final);

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

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;

Contact us