%     CREATES JACOBIAN MATRICIES
%
% use a previous transform or enter px, py, and pz

disp('*** NOTE:')
disp('This Jacobian routine computes only the derivatives of')
disp(' px, py, and pz.  Up to 6 joints can be handled.')

mnu=1;
while mnu ~= 5
% Menu system
  disp('At Jacobian menu.')
  mnu=menu('Symbolic Jacobian Generation',...
    'Use Equations for px, py, and pz',...
    'Use px, py, and pz of Previous Transform',...
    'Display elements of Jacobian',...
    'Evaluate or Test Jacobian',...
	 'Return to Main Menu');




if mnu==1 | mnu==2
   % v and var are used to determine variable elements
   v=sym('t1,t2,t3,t4,t5,t6');
   var=sym('d1d2d3d4d5d6');

   % This section is used if you want to enter px, py, and pz
   if mnu==1

      % Enter px py and pz equations for the robot
   	px= input('enter px equation: ','s')
   	py= input('enter py equation: ','s')
   	pz= input('enter pz equation: ','s')
      % Enter the number of variables
   	nrow=input('enter number of variables: ')

      % the pris variable is used to compensate for
      % prismatic joints
   	pris=input('Enter the joint number of prismatic joints (e.g. [1 3] for joints 1 and 3 or [0] for none')
   	k=length(pris);

      % the loop substitutes d variables from var into
      % v to compensate for prismatic joints

	if pris(1)~=0
            temp = v;
            for l=1:k
 	           n=pris(l)				%checks whether link is revolute(0) or prismatic(non-0)
	           temp=subs(temp,var(2*n-1:2*n),v(2*n+n-1:2*n+n));
     	    end	
	    v = temp;	
	end
   end   % end of enter equations loop

   % The next section is used if you want to use a previous transform

   if mnu==2
      if ~fkindone
         disp('Forward Kinematics being evaluated')
	     	fksym
         fkindone = 1;
      end
%      disp('Current variables are:')
      who

     	trans_num=[0,0]; % reset transform number
	
      % Enter the desired transform and make sure it exists 
	   while (trans_num(1)<1) | (trans_num(1)>dhrow-1) | (trans_num(2)~=dhrow)
		   trans_num=input('Enter Transform number (e.g., for A1*A2*A3 enter [1,3]): ');
	   end
	
      %create sym matrix t from Anm
	   eval(['t=A' num2str(trans_num(1)) num2str(trans_num(2)) ';']);

      % determine size of new transform	
      [n,m]=size(t);


      % the loop determines which joints are prismatic from 
      % the dh parameters ( dhrow ) and adjusts v acordingly
      temp = v;
      for n=1:dhrow
 	 if dh(n,5)==1		%checks whether link is revolute(0) or prismatic(non-0)
   	     temp=subs(temp,var((2*n-1):(2*n)),v((2*n+n-1):(2*n+n)));
         end	
      end
      v = temp;	


	   joint=dh(:,5); % needed for jtest
      % a b and c are the first three rows of the transform
   	a=t(1,:);
	   b=t(2,:);
   	c=t(3,:);

      % the next part looks for commas in the first three rows of the transform
	   num1=findstr(a,',');
	   num2=findstr(b,',');
	   num3=findstr(c,',');

      % then determine between which places px, py and pz occur
	   l=num1(1,3)+1;
	   l2=num2(1,3)+1;
	   l3=num3(1,3)+1;

      % assign px py and pz from the original transform
	   px=a(l:m-1);
   	py=b(l2:m-1);
   	pz=c(l3:m-1);

	   nrow= dhrow; % number of variables

   end  % end of use previous transform section, mnu=2

   % determine the three rows of the jacobian and simplify
   jacx=simple(jacobian(px,v));
   jacy=simple(jacobian(py,v));
   jacz=simple(jacobian(pz,v));

   % the program automaticly determines jacobian for six DOF
   % so the next two sections cut off the unnesasary lines

   num1=findstr(jacx,',');
   num2=findstr(jacy,',');
   num3=findstr(jacz,',');

   jacox=jacx(2:num1(nrow)-1);
   jacoy=jacy(2:num2(nrow)-1);
   jacoz=jacz(2:num3(nrow)-1);

   % convert jacobian rows to one matrix
   
   jac=str2mat(jacox,jacoy,jacoz);


   % used for testing jacobin in JTEST.M 
   jactest=str2mat(jacx,jacy,jacz);
end

%if mnu == 3
if mnu < 4
   disp('The Jacobian is:')
   disp('Elements by column. Press any key for next element')
   for c=1:dhrow
	   for r=1:3
		   eval(['element=viewsym(jactest,r,c);'])
   		disp(['jacobian(' num2str(r) ',' num2str(c) ')='])
	   	pretty(element)
		   pause
   	end
   end
end

% calls JTEST.M to evaluate jacobian
if mnu == 4
   jtest
end

end % while mnu~=5

 
