function metric = grasp_cube(mu, ng, inscribed, off_set)
%Force closure test for three-point grasp of cube.  
%
%    Usage:  metric = grasp_cube(mu, ng, inscribed, off_set)
%
%  mu = friction coefficient at all contacts
%  ng = number of friction directions per friction cone
%  inscribed = 1 => linear approximation is inscribed in cone
%            = 0 => linear approximation is circumscribed about cone
%  offset = 1 => cone approximation is rotated by half the angle between
%                     facets

%Assume force closure does not exist.
metric = 0;

% Choose contact normals, locations, and models
% I assume the moments are summed about the center of the cube
num_cont = 3;
positions = [1   0     0;
            -1   1     1;
             0  0.5  -0.5];
normals = [0   0   0; 
           1  -1  -1;
           0   0   0];
type1 = 'HF';
type2 = 'SF';
type3 = 'HF';

% for SF contacts
ups = 1;

% inscribed = 1;  % 1 => Inscribed cone (conservative test for force closure)
%                 % 0 => Circumscribed cone (conservative test against form closure)
% off_set = 1;    % Rotate the generators by half the angle of their separation
% ng = ?;         % number facets of Coulomb cone.

R1 = [0  -1  0;
      1   0  0;
      0   0  1];
R2 = [0   1  0;
     -1   0  0;
      0   0  1];
R3 = [0   1  0;
     -1   0  0;
      0   0  1];

Rbar1 = [   R1      zeros(3,3);
         zeros(3,3)    R1];
Rbar2 = [   R2      zeros(3,3);
         zeros(3,3)    R2];
Rbar3 = [   R3      zeros(3,3);
         zeros(3,3)    R3];

S1 = zeros(3,3);
S1(1,[2,3]) = [-positions(3,1)   positions(2,1)];
S1(2,[1,3]) = [ positions(3,1)  -positions(1,1)];
S1(3,[1,2]) = [-positions(2,1)   positions(1,1)];

S2 = zeros(3,3);
S2(1,[2,3]) = [-positions(3,2)   positions(2,2)];
S2(2,[1,3]) = [ positions(3,2)  -positions(1,2)];
S2(3,[1,2]) = [-positions(2,2)   positions(1,2)];

S3 = zeros(3,3);
S3(1,[2,3]) = [-positions(3,3)   positions(2,3)];
S3(2,[1,3]) = [ positions(3,3)  -positions(1,3)];
S3(3,[1,2]) = [-positions(2,3)   positions(1,3)];

P1 = [eye(3,3)  zeros(3,3);
         S1       eye(3,3)];               
P2 = [eye(3,3)  zeros(3,3);
         S2       eye(3,3)];
P3 = [eye(3,3)  zeros(3,3);
         S3       eye(3,3)];
                
tildeG1 = P1 * Rbar1;
tildeG2 = P2 * Rbar2;
tildeG3 = P3 * Rbar3;
tildeG = [tildeG1  tildeG2  tildeG3];     
  
I = eye(6,6);
switch type1
    case('PwoF')
        H1 = I(1,:);
    case('HF')
        H1 = I([1:3],:);
    case('SF')
        H1 = I([1:4],:);
end
switch type2
    case('PwoF')
        H2 = I(1,:);
    case('HF')
        H2 = I([1:3],:);
    case('SF')
        H2 = I([1:4],:);
end
switch type3
    case('PwoF')
        H3 = I(1,:);
    case('HF')
        H3 = I([1:3],:);
    case('SF')
        H3 = I([1:4],:);
end
     
H = blkdiag(H1, H2, H3);

G1 = tildeG1 * H1';
G2 = tildeG2 * H2';
G3 = tildeG2 * H3';
G = tildeG * H';
% Also could use  G = [G1  G2  G3];

% Make simple prismatic fingers with axes 1,2,3 of each aligned with the n,
% t, and o directions at each contact.
tildeJ1 = zeros(6,9);
tildeJ1(1:3,1:3) = eye(3,3);
tildeJ2 = zeros(6,9);
tildeJ2(1:3,4:6) = eye(3,3);
tildeJ3 = zeros(6,9);
tildeJ3(1:3,7:9) = eye(3,3);
tildeJ = [tildeJ1;  tildeJ2;  tildeJ3];

J1 = H1 * tildeJ1;
J2 = H2 * tildeJ2;
J3 = H3 * tildeJ3;
J = H * tildeJ;
% Also could use J = [J1; J2; J3];
 
% Rank tests...
% Here is the test for whether contact locations have been chosen such that
% sufficiently dexterous fingers could move the cube arbitrarily.  If
% rank(G) = 6, then this is true.  If the type of contacts are not all hard
% fingers, then we might no meet this condition.  For example, we need at
% least 6 columns in G.  Thus all hard or soft finger contacts works and so
% do these others: (SF SF PwoF), (SF HF PwoF), (HF HF PwoF).
rnkG = rank(G);
if rnkG < 6
    fprintf('Grasp does not support control over all object twists and wrenches. \n');
    fprintf('Force closure cannot NOT exist.  Exiting. \n');
    return;
else
    fprintf('Grasp geometry and contact types support control over all object twists and wrenches. \n');
end

% Next test rank(G*J) = 6.  If not true, then object cannot control all
% object twists and wrenches.
rnkGJ = rank(G*J);
if rnkGJ < 6
    fprintf('Hand cannot control all object twists and wrenches. \n');
    return;
else
   fprintf('Hand structure is such that all object twists and wrenches can be controlled. \n');
    fprintf('Now checking existence of frictional form closure. \n'); 
end

% Check frictional form closure property by solving linear program LP2
[nu, m] = size(G);

% Linearized Friction Cones
if inscribed == 1
    theta_o_2 = pi / ng;  % half angles between linearizing generators
    mu = mu * cos(theta_o_2);  % make friction coefficients smaller
end
off_set_value = 0;
if off_set == 1
    off_set_value = pi / ng;
end

switch type1
    case 'PwoF'
        F1 = [1];
        e1 = [1];
    case 'HF'
        % Comment out the next two lines for circumscribed cone
        alpha1 = atan2(mu,1);
        beta = 2 * pi / ng;
        beta = beta * [1 : ng]';
        beta = beta + off_set_value;
        F1 = [ones(ng,1) * sin(alpha1)  cos(beta) * cos(alpha1)  sin(beta) * cos(alpha1)]; % For hard finger
        e1 = [1 0 0];
    case 'SF'
        alpha1 = atan2(mu,1);
        beta = 2 * pi / ng;
        beta = beta * [1 : ng]';
        beta = beta + off_set_value;
        eta1 = [ones(ng,1) * sin(alpha1)  cos(beta) * cos(alpha1)  sin(beta) * cos(alpha1)]; % For hard finger
        F1 = [ eta1        zeros(ng,1);
            [ups * mu   0   0  -1] / norm([ups * mu   0   0  -1]);
            [ups * mu   0   0   1] / norm([ups * mu   0   0  -1])];
        e1 = [1 0 0 0];
end
switch type2
    case 'PwoF'
        F2 = [1];
        e2 = [1];
    case 'HF'
        alpha2 = atan2(mu,1);
        beta = 2 * pi / ng;
        beta = beta * [1 : ng]';
        beta = beta + off_set_value;
        F2 = [ones(ng,1) * sin(alpha2)  cos(beta) * cos(alpha2)  sin(beta) * cos(alpha2)]; % For hard finger
        e2 = [1 0 0];
    case 'SF'
        alpha2 = atan2(mu,1);
        beta = 2 * pi / ng;
        beta = beta * [1 : ng]';
        beta = beta + off_set_value;
        eta2 = [ones(ng,1) * sin(alpha2)  cos(beta) * cos(alpha2)  sin(beta) * cos(alpha2)]; % For hard finger
        F2 = [ eta2        zeros(ng,1);
               [ups * mu   0   0  -1] / norm([ups * mu   0   0  -1]);
               [ups * mu   0   0   1] / norm([ups * mu   0   0  -1])];
        e2 = [1 0 0 0];
end
switch type3
    case 'PwoF'
        F3 = [1];
        e3 = [1];
    case 'HF'
        alpha3 = atan2(mu,1);
        beta = 2 * pi / ng;
        beta = beta * [1 : ng]';
        beta = beta + off_set_value;
        F3 = [ones(ng,1) * sin(alpha3)  cos(beta) * cos(alpha3)  sin(beta) * cos(alpha3)]; % For hard finger
        e3 = [1 0 0];
    case 'SF'
        alpha3 = atan2(mu,1);
        beta = 2 * pi / ng;
        beta = beta * [1 : ng]';
        beta = beta + off_set_value;
        eta3 = [ones(ng,1) * sin(alpha3)  cos(beta) * cos(alpha3)  sin(beta) * cos(alpha3)]; % For hard finger
        F3 = [ eta2        zeros(ng,1);
               [ups * mu   0   0  -1] / norm([ups * mu   0   0  -1]);
               [ups * mu   0   0   1] / norm([ups * mu   0   0  -1])];
        e3 = [1 0 0 0];
end
F = blkdiag(F1, F2, F3);
e = [e1  e2  e3];
E = blkdiag(e1, e2, e3);
[Frows,Fcols] = size(F);
[erows,ecols] = size(e);

% Set up LP2
f = [zeros(m,1);
        -1      ];  % This gives us: minimize d
    
A = [  [-F             ones(Frows,1)];  
       [ e                  0       ];  
       [zeros(1,Fcols)     -1       ]]; 

b = [zeros(Frows,1)
     num_cont
         0      ];
      
Aeq = [G   zeros(6,1)];
beq = [zeros(6,1)];

options = optimset('Display','off');

%[X,FVAL,EXITFLAG,OUTPUT,LAMBDA] = linprog(f,A,b,Aeq,beq)
[X,FVAL,EXITFLAG] = linprog(f,A,b,Aeq,beq,[],[],[],options);
if FVAL < 0
    fprintf('Frictional form closure exists!!!\n');
    fprintf('Closure measure = %0.5g \n', -FVAL);
else
    fprintf('Frictional form closure DOES NOT exist.  Exiting. \n');
    metric = 0;
    return;
end

% FVAL < 0 means we have frictional form closure.
% Now test for force closure
% Set up LP3
[mj, nq] = size(J);

A2 =[[ -E   ones(num_cont,1) ];
    [  e       0     ]];

b2 = [zeros(num_cont,1);
    num_cont];

Aeq2 = [G   zeros(nu,1);
    J'  zeros(nq,1)];
beq2 = [zeros(nu+nq,1)];

[X2,FVAL2,EXITFLAG2] = linprog(f,A2,b2,Aeq2,beq2,[],[],[],options);
if FVAL2 > 0
    fprintf('Force closure does not exist. \n');
else
    fprintf('Force closure Exists. \n');
    metric = -FVAL;
end
return;
end