%			SYMBOLIC ROBOTIC TOOLBOX
%
%  by Christian Paquette, Chris Harper, Roger Toogood
%     Department of Mechanical Engineering
%     University of Alberta
%     April 1996
%
% This toolbox was written to perform the symbolic operations involved in
% robot kinematics.  This includes specification of Denavit-Hartenberg parameters,
% setup of link A-matrices, forward kinematics, portions of the inverse
% kinematics, and evaluation of the manipulator Jacobian (relative to world).
%
% These are not generally simple tasks involving algebra and trigonometry, but 
% there is a good chance that doing this manually will result in disaster!  
% MATLAB symbolic operations are used (including simplification of terms and
% identification of trigonometric identities) to reduce this chance of error
% and allow a larger number of robot configurations to be studied.
%
% This is a list of all the files involved in this Toolbox:
%    amatrix	script	 creates symbolic A matricies (9 maximum)
%    symdh_a	function  returns an A matrix from its dh parameters
%    fksym	   script    creates transform from A matricies (forward kinematics)
%    atest	   script    evaluates A matricies and Transforms
%    iksym	   script    assists user in doing inverse kinematics by multiplying
%			                T by inverse A matricies
%    viewele	script    allows user to view elements of matricies created
%    viewsym	function  returns symbolic element of matrix at certain row and column
%    rlsym	   script    calculates the dynamics of the robot using recursive
%			                lagrangian technique
%    jacsym	   script    creates jacobian matrices
%    jtest	   script    evaluates jacobian as well as tip and joint velocities
%
% This is a list of all major variables used in this Toolbox:
%	menu1		tracks answers to Main menu
%	menu2		tracks answers to Create A Matrix menu
%	menu3		tracks answers to Inverse Kinematics menu
%	menu4		tracks answers to View Element menu
%	menu5		tracks answers to View Element of A Matrix menu
%	menu6		tracks answers to View Element of Transform menu
%	menu7		tracks answers to Which Transform menu
%	mnu		tracks answers to Jacobian menu
%	menu8		tracks answers to Evaluate and test jacobian menu
%	dh			array of dh parameters [alpha a theta d sigma]
%	dhrow		number of rows in dh/links of robot
%	An			A is symbolic A matrix and n is link number
%	Anm		transform between link n and link m
%	link_num	link number for evaluating A matricies
%	trans_num	transform number for evaluating transforms
%	jac		current jacobian
%	jactest	current jacobian in 6 x 6 form
%

disp('================================================================')
disp('')
disp('Welcome to the Symbolic Robotics Toolbox')
disp('')
disp('User input and control is via the keyboard, and menus such as the')
disp('one above.  Keep your eye on both the MATLAB window and the menu')
disp('window for prompts for input.')
disp('')
disp('================================================================')
disp('Clearing memory.')
clear
dhrow = 0;
fkindone = 0;
whitebg('blue')
close

menu1 = menu('Would you like a MATLAB diary file?','Yes - file robsym.out will be used','No');
if menu1 == 1
   diary robsym.out
end

while 1
	menu1=0;
	disp('At MAIN MENU')
	menu1=menu('SYMBOLIC ROBOTIC TOOLBOX',...
   	'Create A Matrices',...
   	'Forward Kinematics',...
      'Evaluate Matrices',...
   	'Inverse Kinematics',...
      'List Current Variables',...
      'View Elements of Previous Matrices',...
   	'Create or Evaluate Jacobian',...
      'QUIT');

	if menu1==1
      disp('Creating A Matrices')
		amatrix
	end	
	
	if menu1==2
      if dhrow ~= 0
         disp('Forward Kinematics')
	   	fksym
         fkindone = 1;
      else
         disp('Cannot perform FORWARD KINEMATICS.')
         disp('You must first specify the DH parameters and A matrices.')
      end
	end	
	
	if menu1==3
      if dhrow ~= 0
         disp('Evaluating A Matrices')
	   	atest
      else
         disp('Cannot EVALUATE MATRICES.')
         disp('You must first specify the DH parameters and A matrices.')
      end

	end

	if menu1==4		
      if dhrow ~= 0
         if ~fkindone
            disp('Forward Kinematics')
	      	fksym
            fkindone = 1;
         end
         disp('Matrices for Inverse Kinematics')
         iksym	
      else
         disp('Cannot perform INVERSE KINEMATICS.')
         disp('You must first specify the DH parameters and A matrices.')
      end
   end

	if menu1==5
      who
   end

	if menu1==6
      if dhrow ~= 0
         disp('Viewing Matrices')
	   	viewele
      else
         disp('Cannot VIEW MATRICES.')
         disp('You must first specify the DH parameters and A matrices.')
      end
	end
	
	if menu1==7
      if dhrow ~= 0
         disp('Setting up the Jacobian')
         jacsym	
      else
         disp('Cannot evaluate JACOBIAN.')
         disp('You must first specify the DH parameters and A matrices.')
      end

   end	

	if menu1==8
      disp('PROGRAM EXIT')
		fprintf('\n Thank You for Using Our Program \n')
		fprintf('\n Please wipe your feet on the way out! \n')
		break
	end
end
