function metric = form_closure(alpha)
% Form closure test for planar example in Grasping Chapter of Robotics
% Handbook.
%
% usage: d = form_closure(alpha)
%
% alpha is the angle of the normal at contact four in radians
% If d > 0, then the grasp has form closure. If alpha is in the
% range [1.05. 1.59], then the grasp has form closure.
nc = 4;
l = 1;
Gn =[0 0 1 -cos(alpha);
1 1 0 -sin(alpha);
-l 0 7*l/4 0];
% We know that the rank of Gn is three for all values of alpha,
% so we can move on.
% Set up the linear program named LP1 in the Grasping chapter
% The unknown vector is of length 5. It contains [lambda_1n ...
% ... lambda_2n lambda_3n lambda_4n d].
f = [zeros(nc,1);
-1 ]; % linprog assumes minimize. Change sign to maximize d
% A and b define the inequality constrants:
A = [ [-eye(4,4) ones(4,1)]; % lambda_n \ge 0
[ ones(1,4) 0 ]; % make LP1 bounded
[zeros(1,4) -1 ] ]; % d \ge 0
b = [zeros(4,1) % lambda_n \ge 0
nc % make LP1 bounded
0 ]; % d \ge 0
% Here is the equality constraint to force lambda_n into the
% null space of Gn.
Aeq = [Gn zeros(3,1)];
beq = [zeros(3,1)];
% Make calling this m-file from form_cl_plot behave quietly
options = optimset('Display','off');
[X,FVAL,EXITFLAG,OUTPUT,LAMBDA] = linprog(f,A,b,Aeq,beq,[],[],[],options);
if FVAL < 0
% fprintf('Form closure exists!!!\n');
% fprintf('Closure measure = %0.5g \n', -FVAL);
metric = -FVAL;
else
% fprintf('Form closure DOES NOT exist.\n');
metric = 0;
end
return;
end