%GRIPPER    Gripper
% Draws gripper
%
% Syntax:  gripper(T, theta)
%
% Inputs:
%    T - 4x4 homogeneous rotation matrix
%    theta - angle of links 
%            0 open; pi/4 closed
%
% Other m-files required: closedCylinder.m

% Author: Travis Hydzik
% Last revision: 19 October 2004

function gripper(T, theta)

    warning off all;

	% gripper link lengths
	d = 0.3;
	
	s30 = sin(pi/6); % sin(30)
	c30 = cos(pi/6); % cos(30)
	
	[X, Y, Z] = closedCylinder(T*[0 0 0 1]', T*[ c30*d*cos(theta) s30*d*cos(theta) d*sin(theta) 1]', 0.05);
	surfl(X,Y,Z);
	[X, Y, Z] = closedCylinder(T*[0 0 0 1]', T*[-c30*d*cos(theta) s30*d*cos(theta) d*sin(theta) 1]', 0.05);
	surfl(X,Y,Z);
	[X, Y, Z] = closedCylinder(T*[0 0 0 1]', T*[ 0 -d*cos(theta) d*sin(theta) 1]', 0.05);
	surfl(X,Y,Z);
	
	beta = (pi/2) - theta; % angle between links of gripper
	
	[X, Y, Z] = closedCylinder(T*[ c30*d*cos(theta) s30*d*cos(theta) d*sin(theta) 1]', T*[c30*d*(cos(theta) - cos(beta)) s30*d*(cos(theta) - cos(beta)) d*(sin(theta) + sin(beta)) 1]', 0.05);
	surfl(X,Y,Z);
	[X, Y, Z] = closedCylinder(T*[-c30*d*cos(theta) s30*d*cos(theta) d*sin(theta) 1]', T*[-c30*d*(cos(theta) - cos(beta)) s30*d*(cos(theta) - cos(beta)) d*(sin(theta) + sin(beta)) 1]', 0.05);
	surfl(X,Y,Z);
	[X, Y, Z] = closedCylinder(T*[ 0 -d*cos(theta) d*sin(theta) 1]', T*[ 0 -d*(cos(theta) - cos(beta)) d*(sin(theta) + sin(beta)) 1]', 0.05);
	surfl(X,Y,Z);