clear
%
%link lengths
syms ax ay az
syms sx sy sz
syms nx ny nz
assume(nx,'real')
assume(ny,'real')
assume(nz,'real')
assume(sx,'real')
assume(sy,'real')
assume(sz,'real')
assume(ax,'real')
assume(ay,'real')
assume(az,'real')

%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%%
%% INVERSE KINEMATICS
% Input: Re
Re = [nx sx ax;ny sy ay; nz sz az]
n=Re(:,1)
s=Re(:,2)
a=Re(:,3)

theta5_I = atan2(sqrt(a(1)^2+a(2)^2), a(3))
theta5_II = atan2(-sqrt(a(1)^2+a(2)^2), a(3))
theta4_I = atan2(a(2),a(1))
theta4_II = atan2(-a(2),-a(1))
theta6_I = atan2(s(3),-n(3))
theta6_II = atan2(-s(3),n(3))

vsolsori(1,:) = [theta4_I, theta5_I, theta6_I]
vsolsori(2,:) = [theta4_II, theta5_II, theta6_II]

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

% qa = [pi/4,pi/3,pi/2]
qa = [pi/4,pi/3,pi/2]
T1=rob.fkine(qa)

vRe = T1.R
jointspos(1,:) = double(subs(vsolsori(1,:),{nx ny nz sx sy sz ax ay az},...
    {vRe(1,1) vRe(2,1) vRe(3,1) vRe(1,2) vRe(2,2) vRe(3,2) vRe(1,3) vRe(2,3) vRe(3,3)}))
jointspos(2,:) = double(subs(vsolsori(2,:),{nx ny nz sx sy sz ax ay az},...
    {vRe(1,1) vRe(2,1) vRe(3,1) vRe(1,2) vRe(2,2) vRe(3,2) vRe(1,3) vRe(2,3) vRe(3,3)}))

vRe
rob.fkine(jointspos(1,:)).R
rob.fkine(jointspos(2,:)).R