%	CREATES SYMOLIC A MATRIX FROM DH PARAMETER MATRIX
%
% Usage:
%	A = symdh_a(dh,n)
% returns a symbolic A matrix for link n using the parameters stored in dh matrix
%

function A=symbh_a(dh,n)

%b=alpha, t=theta in degrees

%create symbolic A matrix
Asym=sym('cos(t) -sin(t)*cos(b) sin(t)*sin(b) a*cos(t);sin(t) cos(t)*cos(b) -cos(t)*sin(b) a*sin(t);0 sin(b) cos(b) d;0 0 0 1');
var=sym('a1a2a3a4a5a6t1t2t3t4t5t6d1d2d3d4d5d6');

A=subs(Asym,dh(n,1)*pi/180,'b');			%subs in numerical twist angle of link	
		
if dh(n,2)==0									%checks for existence of link length a
	A=subs(A,0,'a');							%subs in zero for link length
else
	A=subs(A,var(2*n-1:2*n),'a');			%subs in variable link length
end

if dh(n,5)==0									%checks whether link is revolute(0) or prismatic(non-0)
	A=subs(A,var(2*n+11:2*n+12),'t');	%subs in variable joint angle if revolute
	if dh(n,4)==0
		A=subs(A,0,'d');						%subs in zero for joint offset length
	else
		A=subs(A,var(2*n+23:2*n+24),'d');		%subs in variable joint offset length
	end
else
	A=subs(A,var(2*n+23:2*n+24),'d');	%subs in variable joint offset length if prismatic
	A=subs(A,dh(n,3)*pi/180,'t');			%subs in the numerical joint angle
end

A=simplify(A);
