clear
%
%joint variables
syms qi
syms q1
syms q2
syms q3
assume(qi,'real')
assume(q1,'real')
assume(q2,'real')
assume(q3,'real')
%link lengths
syms ai
syms a2
syms a3
assume(ai,'real')
assume(a2,'real')
assume(a3,'real')
%link offsets
syms di
assume(di,'real')
%link twists
syms alphai
assume(alphai,'real')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% MODELLING
% A matrices
A01i = [cos(qi) -sin(qi)*cos(alphai)  sin(qi)*sin(alphai) ai*cos(qi);...
       sin(qi)  cos(qi)*cos(alphai) -cos(qi)*sin(alphai) ai*sin(qi);...
          0     sin(alphai)          cos(alphai)         di;...
          0    0     0    1]
    
A01 = subs(A01i,{ai,di,alphai,qi},{  0,    0,  pi/2, q1})
A12 = subs(A01i,{ai,di,alphai,qi},{ a2,    0,     0, q2})
A23 = subs(A01i,{ai,di,alphai,qi},{ a3,    0,     0, q3})

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% FORWARD KINEMATICS
A03 = A01*A12*A23

% Particularize to unit link lengths (a2=d4=1) and EE origin at the wrist (d6=0)
va2=1.0;
va3=1.0;
vA03 = subs(A03,{a2 a3},{va2 va3})

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% VERIFICATION USING THE ROBOTICS TOOLBOX FOR MATLAB
% The manipulator as a SerialLink
L(1) = Link([0   0   0  pi/2]); %[revolute d a alpha]
L(2) = Link([0   0 va2     0]);
L(3) = Link([0   0 va3     0]);
rob = SerialLink(L, 'name', 'anthropomorphic_arm')

% qa = [pi/4,pi/3,pi/2]
qa = [pi/4,pi/3,pi/2]
T1=rob.fkine(qa)
format short
vT03 = double(subs(vA03,{q1,q2,q3},{qa}))
verifyFK = T1.inv*SE3(vT03)