% grasping example:
%                               f1
%     f1   f2       _____________|____
%      | /\ |       |            |   |
%      v/  \v       |            v   |
%      /    \       __________________
%      \    /       |                |
%  z   ^\  /^    z  |            ^   |
%  ^   | \/ |    ^  _____________|____
%  |  f3   f4    |               |
%  |             |              f3
%   -->y          -->x
%
% Find maximally robust stable grasp:  generate a polyhedron of forces with
% random direction and size one.  Find maximum scaling of polyhedron for which
% there is a set of forces that provides a stable grasp for all vertices
% of polyhedron (and therefore for any force inside the scaled polyhedron).
%
% As number of vertices increases, the polyhedron approximates a sphere,
% and the maximum scaling K converges.
%
% Includes a mass (const. force in z) and constraints on max. of each force.

list_m=[];
list_K=[];
list_iter=[];

for m=2:2:60,	% number of vertices in the polyhedron

% problem description:  (data for script grasp_polyhedron.m)

p=[1 -1 1;
   1 1 1;
   1 -1 -1;
   1 1 -1]';
u=[0 0 -1;
   0 0 -1;
   0 0 1;
   0 0 1]';
v=[0 sqrt(2)/2 -sqrt(2)/2;
   0 -sqrt(2)/2 -sqrt(2)/2;
   0 sqrt(2)/2 sqrt(2)/2;
   0 -sqrt(2)/2 sqrt(2)/2]';

fmax=10;
mu=0.5;

% polyhedron with random vertexes (all unit norm)
Fa=random('Normal',0,1,3,m);
Fa=Fa./(ones(3,1)*sqrt(sum(Fa.^2)));	% normalize columns of Fa
Fb=[0;0;-9.8*0.5];	% 0.5 Kg mass
Ta=zeros(3,m);
Tb=[0;0;0];

% alternatively, just use an octahedron  (and disable for m loop)
% m=6;
% Fa=[0 0 -1; 0 0 1; 0 -1 0; 0 1 0; -1 0 0; 1 0 0]';
% Fb=[0; 0; -0.98];
% Ta=[0 0 0; 0 0 0; 0 0 0; 0 0 0; 0 0 0; 0 0 0]';
% Tb=[0; 0; 0];

% solve for polyhedron
grasp_polyhedron;

list_m=[list_m m]
list_K=[list_K K]
list_iter=[list_iter length(list_gap)]

save grasp_robust.mat list_m list_K list_iter f F Fa m
clear all	% a bit of hygiene
load grasp_robust

end	% m loop

if 1,
		% plot projection of the vertices of the polyhedron
	figure(1);
	t=(1:100)/100*2*pi;
	tx=cos(t); ty=sin(t);
	x1=Fa(1,find(Fa(2,:)>=0));
	x2=Fa(1,find(Fa(2,:)<0));
	z1=Fa(3,find(Fa(2,:)>=0));
	z2=Fa(3,find(Fa(2,:)<0));
	plot(	x1,z1,'+', ...	% a + for vertices in front half-sphere
		x2,z2,'o', ...	% a o for vertices in the back
		tx,ty,'.');
	axis('square');
	grid on;

		% show convergence of K
	figure(2);
	plot(list_m, list_K,'o',list_m, list_K,'-');
	xlabel('m');
	ylabel('K');
	axis([0 list_m(length(list_m))+1 min(list_K)-1 max(list_K)+1]);
	grid on;

		% show algorithm performance
	figure(3);
	plot(list_m, list_iter,'o');
	xlabel('m');
	ylabel('iter');
	axis([0 list_m(length(list_m))+1 0 max(list_iter)+5]);
	grid on;
end
