% SCRIPT:
% find maximum K such that all forces  Fext=K*Fa(i)+Fb
% and all torques  Text=K*Ta(i)+Tb
% allow stable grasp
%
% requires pre-definition of:
% p, u, v, Fa, Fb, Ta, Tb, fmax
%    p, u, v \in \reals^{3\times n}
%	description of contact points:
%		p -- position relative to center of mass
%		u -- direction of force
%		v -- inward normal to the surface
%    Fa, Ta \in \reals^{3\times m}
%    Fb, Tb \in \reals^3
%    fmax \in \reals  (upper bound on forces)
% where:
%  n is the number of contact points
%  m is the number of vertices in the force/torque polyhedron
%
% outputs:
% K, f, F, list_gap, list_dev
%   K \in \reals
%   f \in \reals^n
%   F \in \reals^{3\times nm}
%   list_gap
%
% temp. vars.: m, n, t, AF, Af, AK, A, b, C, d, X, y, x, z, info, hist, time,
%	maxgap, maxiter


% get and check dimensions
[t,n]=size(p);
[t,m]=size(Fa);

% variable is
% h=[F(1,1)' ... F(1,n)' ... F(m,1)' ... F(m,n) f(1) ... f(n) K]'
% length(h) = m*3*n+n+1 = number of columns of X
% equality constraint will be X*h+y=0

X=[];
y=[];

for j=1:m	% polyhedron vertex loop

% constrain friction force to be in surface plane
%  $(F^i)^T v^i=0$
AF=zeros(n,3*n);
for i=1:n
	AF(i,3*i-2:3*i)=v(:,i)';
end
Af=zeros(n,n);
AK=zeros(n,1);
b=zeros(n,1);
% add to linear system (n rows of X, y)
X=[X; zeros(n,3*n*(j-1)) AF zeros(n,3*n*(m-j)) Af AK];
y=[y; b];

% match forces, for Fext=k*Fa+Fb
%  $\sum_{i} (f_i u^i+F^i)+F^{\rm ext}=0$
AF=zeros(3,3*n);
for i=1:n
	AF(:,3*i-2:3*i)=eye(3);
end
Af=u;
AK=Fa(:,j);
b=Fb;
% add to linear system (3 rows of X, y)
X=[X; zeros(3,3*n*(j-1)) AF zeros(3,3*n*(m-j)) Af AK];
y=[y; b];

% match torques, for Text=K*Ta+Tb
%   $\sum_{i} p^i\times(f_i u^i+F^i)+T^{\rm ext}=0$
% compute outer products:
for i=1:n
	AF(1,3*i-2:3*i)=p(2,i)*[0 0 1]-p(3,i)*[0 1 0];
	Af(1,i)=p(2,i)*u(3,1)-p(3,i)*u(2,i);
end
for i=1:n
	AF(2,3*i-2:3*i)=p(3,i)*[1 0 0]-p(1,i)*[0 0 1];
	Af(2,i)=p(3,i)*u(1,1)-p(1,i)*u(3,i);
end
for i=1:n
	AF(3,3*i-2:3*i)=p(1,i)*[0 1 0]-p(2,i)*[1 0 0];
	Af(3,i)=p(1,i)*u(2,1)-p(2,i)*u(1,i);
end
AK=Ta(:,j);
b=Tb;
% add to linear system (3 rows of X, y)
X=[X; zeros(3,3*n*(j-1)) AF zeros(3,3*n*(m-j)) Af AK];
y=[y; b];

end	% of polyhedron vertex loop

% find linear transformation to reduce problem size  h=T*x-s
%  requires  X*T=0  and  X*s=y
[n1,n2]=size(X);
[U,S,V]=svd(X);
T=V(:,n1+1:n2);		% base for the nullspace of X
s=X\y;		% ...or s=V*S\(U'*y);

% define SOCP
f=T'*[zeros(m*3*n+n,1); -1];	  % minimize -K
N=[3*ones(m*n,1); zeros(2*n,1)];  % m*n SOC constrs., 2*n lin. constrs. on f_i
% friction cones: $\|F^{i,j}\| \leq \mu f_i (u^i)^T v^i$
A=[];
b=[];
C=[];
d=[];
for j=1:m
for i=1:n
  A=[	A;
	zeros(3,(j-1)*3*n+(i-1)*3) eye(3) zeros(3,(n-i)*3+(m-j)*3*n+n+1) ];
  C=[	C;
	zeros(1,m*3*n+(i-1)) mu*u(:,i)'*v(:,i) zeros(1,(n-i)+1) ];
  b=[b; zeros(3,1)];
  d=[d; 0];
end
end
% $f \geq 0$
C=[C; zeros(n,m*3*n) eye(n) zeros(n,1)];
d=[d; zeros(n,1)];
% $f \leq fmax$
C=[C; zeros(n,m*3*n) -eye(n) zeros(n,1)];;
d=[d; fmax*ones(n,1)];

% apply transformation
b=b-A*s;
d=d-C*s;
A=A*T;
C=C*T;

% solve SOCP
maxgap=1e-4;
maxiter=200;
[x,info,z,w,hist,time]=...
	socp(f,A,b,C,d,N,[],[],[],maxgap,0,0,maxiter,[]);

% output
h=T*x-s;
K=h(m*3*n+n+1);
f=h(m*3*n+1:m*3*n+n);
h=h(1:m*3*n); F=zeros(3,m*n); F(:)=h;

list_gap=hist(1,:);
