clear
%
%link lengths
syms a2
syms a3
assume(a2,'real')
assume(a3,'real')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% INVERSE KINEMATICS
% Input: pe
syms pex
syms pey
syms pez
assume(pex,'real')
assume(pey,'real')
assume(pez,'real')

pw=[pex,pey,pez]'

c3 = (pw'*pw-a2^2-a3^2)/2*a2*a3
s3plus = sqrt(1-c3^2)
s3minus = -sqrt(1-c3^2)
theta3_I = atan2(s3plus,c3)
theta3_II = atan2(s3minus,c3)

c2_I   = ( sqrt(pw(1)^2+pw(2)^2)*(a2+a3*c3)+pw(3)*a3*s3plus )/(a2^2+a3^2+2*a2*a3*c3)
c2_II  = (-sqrt(pw(1)^2+pw(2)^2)*(a2+a3*c3)+pw(3)*a3*s3plus)/(a2^2+a3^2+2*a2*a3*c3)
c2_III = ( sqrt(pw(1)^2+pw(2)^2)*(a2+a3*c3)+pw(3)*a3*s3minus )/(a2^2+a3^2+2*a2*a3*c3)
c2_IV  = (-sqrt(pw(1)^2+pw(2)^2)*(a2+a3*c3)+pw(3)*a3*s3minus)/(a2^2+a3^2+2*a2*a3*c3)
s2_I   = (pw(3)*(a2+a3*c3) - sqrt(pw(1)^2+pw(2)^2)*a3*s3plus)/(a2^2+a3^2+2*a2*a3*c3)
s2_II  = (pw(3)*(a2+a3*c3) + sqrt(pw(1)^2+pw(2)^2)*a3*s3plus)/(a2^2+a3^2+2*a2*a3*c3)
s2_III = (pw(3)*(a2+a3*c3) - sqrt(pw(1)^2+pw(2)^2)*a3*s3minus)/(a2^2+a3^2+2*a2*a3*c3)
s2_IV  = (pw(3)*(a2+a3*c3) + sqrt(pw(1)^2+pw(2)^2)*a3*s3minus)/(a2^2+a3^2+2*a2*a3*c3)

theta2_I = atan2(s2_I,c2_I)
theta2_II = atan2(s2_II,c2_II)
theta2_III = atan2(s2_III,c2_III)
theta2_IV = atan2(s2_IV,c2_IV)

theta1_I = atan2(pw(2),pw(1))
theta1_II = atan2(-pw(2),-pw(1))

solspos(1,:) = [theta1_I, theta2_I, theta3_I]
solspos(2,:) = [theta1_I, theta2_III, theta3_II]
solspos(3,:) = [theta1_II, theta2_II, theta3_I]
solspos(4,:) = [theta1_II, theta2_IV, theta3_II]

% Particularize to unit link lengths (a2=a3=1) 
va2=1.0;
va3=1.0;
vsolspos = subs(solspos,{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)

vpe = T1.t
jointspos(1,:) = double(subs(vsolspos(1,:),{pex pey pez},{vpe(1) vpe(2) vpe(3)}))
jointspos(2,:) = double(subs(vsolspos(2,:),{pex pey pez},{vpe(1) vpe(2) vpe(3)}))
jointspos(3,:) = double(subs(vsolspos(3,:),{pex pey pez},{vpe(1) vpe(2) vpe(3)}))
jointspos(4,:) = double(subs(vsolspos(4,:),{pex pey pez},{vpe(1) vpe(2) vpe(3)}))

vpe'
rob.fkine(jointspos(1,:)).transl
rob.fkine(jointspos(2,:)).transl
rob.fkine(jointspos(3,:)).transl
rob.fkine(jointspos(4,:)).transl