%Script iksym.m			INVERSE KINEMATICS
%

disp('This will help you do the symbolic inverse kinematics of the robot selected')
disp('It will start in the form T=A1*A2*A3*A4*A5')
disp(' and every time you multiply by the inverse A matrix')
disp(' it will become     A1^-1*T=A2*A3*A4*A5 and so on..')

%create symbolic transform
eval(['T' num2str(1) num2str(dhrow) '=sym(''nx ox ax px;ny oy ay py;nz oz az pz;0 0 0 1'')'])
eval(['A' num2str(1) num2str(dhrow)])

%reset loop
check=1;
i=1;
while check
	menu3=menu('Inverse Kinematics','Multiply by inverse A matrix','MAIN MENU');
	if (menu3==1)&(i>=dhrow)
		disp('Cannot bring over any more A matricies')
		disp(' Exit and reenter if you wish to repeat process')
	end
	%MULTIPLY BY NEXT INVERSE A MATRIX
	if (menu3==1)&(i<dhrow)
		%T25=simple(summul(inverse(A1),T15)
		eval(['T' num2str(i+1) num2str(dhrow) '=simple(symmul(inverse(A' num2str(i) '),T' num2str(i) num2str(dhrow) '))'])
		%A25
		eval(['A' num2str(i+1) num2str(dhrow)])
		%increment i which is the number of the A matrix to bring over
		i=i+1;
	end
	
	%MAIN MENU
	if menu3==2
		check=0;
	end
end


