% grasping example:
%                               f1
%     f1   f2       _____________|____
%      | /\ |       |            |   |
%      v/  \v       |            v   |
%      /    \       __________________
%      \    /       |                |
%  z   ^\  /^    z  |            ^   |
%  ^   | \/ |    ^  _____________|____
%  |  f3   f4    |               |
%  |             |              f3
%   -->y          -->x
%
% For a given attrition coefficient mu, find the maximum torque around the
% x-axis for which there is a stable grasp.
% Includes a mass (const. force in z) and constraints on max. of each force.
%
% Solves for a range of mu, plots max. torque as fct. of mu.


list_mu=[];
list_K=[];
list_f=[];
list_F1=[];
list_F2=[];
list_F3=[];
list_F4=[];

for mu=0.50:0.02:2.00,

% problem:  (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]';

Fa=[0; 0; 0];
Fb=[0; 0; -9.8];	% 1 Kg mass
Ta=[1; 0; 0];		% find maximum torque around x-axis
Tb=[0; 0; 0];

fmax=10;

grasp_polyhedron;

list_mu=[list_mu mu];
list_K=[list_K K];
list_f=[list_f f];
list_F1=[list_F1 F(:,1)];
list_F2=[list_F2 F(:,2)];
list_F3=[list_F3 F(:,3)];
list_F4=[list_F4 F(:,4)];

disp([mu K]);

end	% of for loop

	% plot results

figure(1);
plot(list_F1(1,:),list_F1(2,:),'-');
grid on;
xlabel('F1x');
ylabel('F1y');

figure(2);
plot(list_F3(1,:),list_F3(2,:),'-');
grid on;
xlabel('F3x');
ylabel('F3y');

figure(3);
plot(list_mu(:),list_f([1 3],:));
grid on;
xlabel('mu');
ylabel('f1,f3');

figure(4);
i=length(list_mu);
plot(list_mu,list_K,'-');
grid on;
xlabel('mu');
ylabel('K');
