
%% Testing the functions of spherical manipulator %%
p_xyz = forward_kinematics(45, 45, 1.5); % Joint angle 1 = 45*, Joint angle 2 = 45*, d_3 = 1.5
q = inverse_kinematics(1.25, 1.25, 2.7678); 

%% Testing if forward-kinematics and inverse-kinematics yields the same answer. %%
% Choose any arbitrary joint angles for theta 1, theta 2 and d3
p_xyz = forward_kinematics(30, 45, 1.5); 

p_end_effector = p_xyz(1:end,4); 
x = p_end_effector(1);
y = p_end_effector(2);
z = p_end_effector(3);

q_new = inverse_kinematics(x,y,z);

% Checking if the solutions from q_new gives us the same p_xyz
p_xyz_new = forward_kinematics(q_new(2,1), q_new(2,2), q_new(2,3))
p_end_effector_new = p_xyz_new(1:end,4);

disp(p_end_effector);      % MATCH
disp(p_end_effector_new);  % MATCH
disp(q_new);
%% Function - Forward-kinematics for spherical Manipulator %%
function end_effector = forward_kinematics(Theta_1, Theta_2, d_3)
    t1=deg2rad(Theta_1); 
    t2=deg2rad(Theta_2); 
    d3=d_3;
    
    L1 = 1; 
    L2 = 1;
    A_1 = [cos(t1) 0 sin(t1) 0;
           sin(t1) 0 -cos(t1) 0;
           0 1 0 L1;
           0 0 0 1];

    A_2 = [cos(t2+pi/2) 0 sin(t2+pi/2) 0;
           sin(t2+pi/2) 0 -cos(t2+pi/2) 0;
           0  1 0 0;
           0 0 0 1;];

    A_3 = [1 0 0 0;
           0 1 0 0;
           0 0 1 d3+L2;
           0 0 0 1;];

    T_01 = (A_1);
    T_02 = ((A_1*A_2));
    T_03 = ((A_1*A_2)*A_3);
    end_effector = T_03;
end

%% Function - Inverse-kinematics for spherical Manipulator %%
function solutions = inverse_kinematics(x,y,z)
    L1 = 1; % Link length 1
    L2 = 1; % Link length 1
    
    % Supporting Variables
    s = z-L1;
    r = sqrt(x.^2+y.^2); 
    
    % First solution 
    Theta_1 = atan2(y,x);
    Theta_2 = atan2(s,r);
    d_3 = sqrt(r.^2+s.^2)-L2;
    
    % Second solution
    Theta_1_rotated = atan2(y,x)+ pi;
    Theta_2_rotated = atan2(s,-r);
    d_3 = sqrt(r.^2+s.^2)-L2;
    
    % Appending both of the solutions 
    solution1 = [rad2deg(Theta_1) rad2deg(Theta_2) d_3];
    solution2 = [rad2deg(Theta_1_rotated) rad2deg(Theta_2_rotated) d_3];
    solutions = [solution1; solution2;]
end
