% Mec.E. 555 assignment #3 - Kinematics of 3R robot

% symbolic operations with the Jacobian



disp('Pretty-print of Forward kinematics:')

w = sym('[cos(t1)*(a2*cos(t2)+a3*cos(t2+t3));sin(t1)*(a2*cos(t2)+a3*cos(t2+t3));d1+a2*sin(t2)+a3*sin(t2+t3)]');

%pretty(w)
%disp(w)
X=w



% define variables

disp('Defined variables:')

v = sym('t1,t2,t3');



% evaluate the Jacobian symbolically

disp('Pretty-print of the Jacobian:')

J = jacobian(w,v);

%pretty(J)
%disp(J);
J



% get the determinant

d = determ(J);

d2 = simplify(d);

d3 = simple(d2);

% make a substitution for an identity MATLAB can't recognize

d4 = subs(d3,'-sin(t3)','cos(t2+t3)*sin(t2)-sin(t2+t3)*cos(t2)');

disp('Pretty-print of det J :')

%pretty(d4)
%disp(d4)
detJ=d4



% get the inverse of the Jacobian

invJ = inverse(J);

inv1 = simple(invJ);

% make a substitution for an identity MATLAB can't recognize

inv2 = subs(inv1,'-sin(t3)','cos(t2+t3)*sin(t2)-sin(t2+t3)*cos(t2)');

disp('Pretty-print the inverse of the Jacobian:')

%pretty(inv2)
disp(inv2)
invJ=inv2
