%   EVALUATES AND TESTS JACOBIAN
% produced using jacsym.m


disp('Evaluation/Testing of Jacobian')
jactst=jactest; % assign jacobian

% menu system
menu8=menu(' Evaluate and/or Test Jacobian Matrix',...
   'Calculate Current Jacobian',...
   'Calculate Tip Speed from Joint Rates',...
   'Calculate Joint Rates from Tip Speed',...
   'Return to Jacobian Menu');

% 			EVALUATE JACOBIAN
if menu8==1
    disp('Evaluating Jacobian.')
    for i =1:nrow
		if dh(i,5)==0    % get numerical value of joint angle
			eval(['t=input(''Enter Joint Angle, theta' num2str(i) ' in degrees: '');'])
			t=t*pi/180;
			%sub joint angle, t, into jacobian
			eval(['jactst=subs(jactst,t,''t' num2str(i) ''');'])
		end

      if dh(i,4) ~= 0 | dh(i,5) == 1   %get numerical value of joint offset
		   eval(['d=input(''Enter Joint Offset, d' num2str(i) ' in meters: '');'])
   		%sub joint offset, d, into jacobian
   		eval(['jactst=subs(jactst,d,''d' num2str(i) ''');'])
		end

      if dh(i,2) ~= 0   %get numerical value of link length
		   eval(['a=input(''Enter Link Length, a' num2str(i) ' (meters): '');'])
   		%sub link length, a, into jacobian			
	   	eval(['jactst=subs(jactst,a,''a' num2str(i) ''');'])
		end

	end

   % convert to numeric matrix and cut off excess columns
	jacnum=numeric(jactst);
	jacnum=jacnum(1:3,1:nrow);
   disp('Numerical value of Jacobian is:')
   disp(jacnum)
   isjacnum = 1;
   [jr,jc] = size(jacnum);
   if jr == jc
      fprintf('Determinant of Jacobian is: %8.2e \n',det(jacnum))
   end

end

%		Test using joint rates

if menu8==2
   disp('Computing tip speed using given joint rates.')
   if isjacnum ~= 1  % is the numerical Jacobian available?
      disp('You need to calculate the Jacobian first.')
      break
   end
   rate=[];

   %	Ask for joint rates
   for i=1:nrow
       eval(['rate(i)=input(''Enter Joint Rate, For Joint' num2str(i) ' (rad/s or m/s): '');'])
   end

   rate=rate';
   x=jacnum*rate; % calculate tip velocities
   % print out results
   disp('Using the most recently evaluated Jacobian,')  
   fprintf('\n Tip Velocities are: %5g   %5g   %5g  Meters/s \n',x(1),x(2),x(3))
end

%		Test using tip velocities
if menu8==3
   disp('Computing joint rates given tip speed.')
   if isjacnum ~= 1  % is the numerical Jacobian available?
      disp('You need to calculate the Jacobian first.')
      break
   end

   x=[];

   % the next lines input tip speeds 
   x(1)=input('enter x tip speed (m/s) ');
   x(2)=input('enter y tip speed (m/s) ');
   x(3)=input('enter z tip speed (m/s) ');
   x=x';

   % joint rates are calculated
   rate=jacnum\x;
   disp('Using the most recently evaluated Jacobian,')  
   fprintf('\n Joint rates are: %5g   %5g   %5g  (rad/s or m/s) \n',rate(1),rate(2),rate(3))

end

