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

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 wrist%%
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_o0 = [cos(t1)*cos(t2)*(L2+d3) cos(t2)*sin(t1)*(L2+d3) L1+sin(t2)*(d3+L2);];
o3_o1 = [cos(t1)*cos(t2)*(L2+d3) cos(t2)*sin(t1)*(L2+d3) sin(t2)*(d3+L2);];

jv_1 = cross(z_0, o3_o0);
jv_2 = cross(z_1, o3_o1);
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;]);

J = [jv.'; jw.';];

disp(J)

%% Determinant of the spherical wrist%%
%% First solution of the determinant%%
det_J = simplify(det(jv));
disp(det_J)

%% Second solution of the determinant %%
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_second = (a*(e*i-f*h))-(b*(d*i-f*g))+c*((d*h-e*g));
det_J_second = simplify(det_J_second);
disp(det_J_second)
%% Linear Velocity test %%
q = [pi/4 pi/4 1.5];
q_dot = [0.5; 0.05; 0.05];

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

% Calculating the linear Velocity , v = J(q)*q_dot%
v = jv*q_dot;
disp(double(v))

%%