%% Forward-kinematics of the Spherical Wrist%%
syms L1 L2 L3 d3
syms t1 t2 t3

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 L3+d3+L2;
       0 0 0 1;];

T_01 = simplify(A_1);
T_02 = simplify((A_1*A_2));
T_03 = simplify((A_1*A_2)*A_3);

disp(T_01);
disp(T_02);
disp(T_03);

%% Jacobian of the spherical robot 
jacobian_matrix = Jacobian_spherical_manipulator();
disp(jacobian_matrix);

%% Determinant of the spherical robot
det_J = determinant_jacobian_spherical_manipulator(jacobian_matrix);
disp(det_J);

%% Testing the Joint velocities
q = [0 0 3];
q_dot = [0.1; 0.05; 0;];

% Getting the jacobian linear-velocity components
jv = jacobian_matrix(1:3,1:3,1);

% Substituting the variables onto the jv-matrix
jv = subs(jv, t1, q(1));
jv = subs(jv, t2, q(2));
jv = subs(jv, d3, q(3));
jv = subs(jv, L2, 1);
disp(jv);

% Calculating the linear-velocity
v = jv*q_dot;
disp(double(v));

%% Jacobian_matrix %%
function jacobian_matrix = Jacobian_spherical_manipulator()
    syms t1 t2 d3
    syms L1 L2 L3
    z_0 = [0 0 1;];
    z_1 = [sin(t1) -cos(t1) 0;];
    z_2 = [cos(t1)*cos(t2) cos(t2)*sin(t1) sin(t2);];

    O3_0 = [cos(t1)*cos(t2)*(L2+d3) cos(t2)*sin(t1)*(L2+d3) L1+sin(t2)*(d3)];
    O3_1 = [cos(t1)*cos(t2)*(L2+d3) cos(t2)*sin(t1)*(L2+d3) sin(t2)*(d3);];

    jv_1 = cross(z_0, O3_0);
    jv_2 = cross(z_1, O3_1);
    jv_3 = z_2;

    jw_1 = z_0; 
    jw_2 = z_1;
    jw_3 = [0 0 0;];

    Jv = simplify([jv_1; jv_2; jv_3;]);
    Jw = simplify([jw_1; jw_2; jw_3;]);    

    jacobian_matrix = [Jv.'; Jw.';];
   
end
%% Determinant of Jacobian matrix %%
function jacobian_det = determinant_jacobian_spherical_manipulator(J)
    a = J(1,1);
    b = J(1,2);
    c = J(1,3);
    d = J(2,1);
    e = J(2,2);
    f = J(2,3);
    g = J(3,1);
    h = J(3,2);
    i = J(3,3);
    det_J = a*(e*i-f*h) - b*(d*i-f*g) + c*(d*h-e*g);
    jacobian_det = simplify(det_J);
end
