theta = [0 0 0 0 0 0]; %input theta values to verfiy
theta = theta.*(pi/180); %converting from degrees to radians
dh=[
    theta(1)      290  0   -pi/2;
    -pi/2         0    0    0;
    theta(2)      0    270  0;
    theta(3)      0    70  -pi/2;
    theta(4)      302  0    pi/2;
    theta(5)      0    0   -pi/2;
    theta(6)      72   0    0;
    ]; %inputing dh table values into matrix form


A=cell(1,length(dh)); %indexing values from array for different n values
for n=1:length(dh)
    A{n} = [
        cos(dh(n,1))    -cos(dh(n,4))*sin(dh(n,1))      sin(dh(n,4))*sin(dh(n,1))   dh(n,3)*cos(dh(n,1));
        sin(dh(n,1))     cos(dh(n,4))*cos(dh(n,1))     -sin(dh(n,4))*cos(dh(n,1))   dh(n,3)*sin(dh(n,1));
        0                sin(dh(n,4))                   cos(dh(n,4))                dh(n,2);
        0                0                              0                           1];
end


%Transformation Matrices
% A1=A{1}
% A2=A{2}
% A3=A{3}
% A4=A{4}
% A5=A{5}
% A6=A{6}
% A7=A{7}


%Final Transformation Matrix
A_t = A{1}*A{2}*A{3}*A{4}*A{5}*A{6}*A{7}
%A_t (A_t<0.001 & A_t>-0.001) = 0;


%Quaternion Calculations
Q1=0.5*sqrt(A_t(1,1)+A_t(2,2)+A_t(3,3)+1)
Q2=0.5*sqrt(A_t(1,1)-A_t(2,2)-A_t(3,3)+1)*sign(A_t(3,2)-A_t(2,3))
Q3=0.5*sqrt(-A_t(1,1)+A_t(2,2)-A_t(3,3)+1)*sign(A_t(1,3)-A_t(3,1))
Q4=0.5*sqrt(-A_t(1,1)-A_t(2,2)+A_t(3,3)+1)*sign(A_t(2,1)-A_t(1,2))