clear all;
close all;
clc;

% DH TABLE:
ThetaOffset  = [ pi/2, pi/2, 0, 0, 0, 0 ];
Alpha        = [ pi/2, 0, -pi/2, pi/2, -pi/2, pi ];
R            = [ 100, 250, 0, 0, 0, 0 ];
D            = [ 320, 0, 0, -250, 0, -65];
JointsLimits = [ -170*pi/180, 170*pi/180; -160*pi/180, 65*pi/180; -51*pi/180, 225*pi/180; -200*pi/180, 200*pi/180; -135*pi/180, 135*pi/180; -360*pi/180, 360*pi/180];
Theta        = [ 0, 0, 0, 0, 0, 0];

% % CREATING JOINTS [THETA(OFFSET),ALPHA,R,D]:
for i=1:1:6
    L(i) = Revolute ('offset', ThetaOffset(i), 'alpha',  Alpha(i), 'a', R(i), 'd', D(i),'qlim',[JointsLimits(i,1) JointsLimits(i,2)]); 
end
EPSON_C3 = SerialLink (L, 'name', 'EPSON');
%EPSON_C3.teach()

%% HOMOGENEOUS TRANSFORMATION MATRIX:
syms theta alpha r d q1 q2 q3 q4 q5 q6
HTM_sym = [cos(theta), -sin(theta)*cos(alpha),  sin(theta)*sin(alpha), r*cos(theta) ;
           sin(theta),  cos(theta)*cos(alpha), -cos(theta)*sin(alpha), r*sin(theta) ;
                    0,             sin(alpha),             cos(alpha),            d ;
                    0,                      0,                      0,            1];
                
HTM_0_6_numeric = eye(4);
HTM_0_6_symbolic = eye(4);
Theta_symbolic = [q1, q2, q3, q4, q5, q6];
Theta = Theta*(pi/180);
for i=1:1:6
 HTM_numeric(:,:,i) = double(subs(HTM_sym, [theta, alpha, r, d], [Theta(i)+ThetaOffset(i), Alpha(i), R(i), D(i)]));
 HTM_symbolic(:,:,i) = subs(HTM_sym, [theta, alpha, r, d], [Theta_symbolic(i)+ThetaOffset(i), Alpha(i), R(i), D(i)]);
 HTM_0_6_numeric  = HTM_0_6_numeric  * HTM_numeric(:,:,i);
 HTM_0_6_symbolic = HTM_0_6_symbolic * HTM_symbolic(:,:,i);
end

H0_symbolic_(:,:,1) = sym(eye(4));
H0_symbolic_(:,:,2) = HTM_symbolic(:,:,1);
H0_symbolic_(:,:,3) = HTM_symbolic(:,:,1)* HTM_symbolic(:,:,2);
H0_symbolic_(:,:,4) = HTM_symbolic(:,:,1)* HTM_symbolic(:,:,2)* HTM_symbolic(:,:,3);
H0_symbolic_(:,:,5) = HTM_symbolic(:,:,1)* HTM_symbolic(:,:,2)* HTM_symbolic(:,:,3)* HTM_symbolic(:,:,4);
H0_symbolic_(:,:,6) = HTM_symbolic(:,:,1)* HTM_symbolic(:,:,2)* HTM_symbolic(:,:,3)* HTM_symbolic(:,:,4)* HTM_symbolic(:,:,5);
H0_symbolic_(:,:,7) = HTM_symbolic(:,:,1)* HTM_symbolic(:,:,2)* HTM_symbolic(:,:,3)* HTM_symbolic(:,:,4)* HTM_symbolic(:,:,5)* HTM_symbolic(:,:,6);

% HOMOGENEOUS MATRIXES FROM ZERO FRAME TO EACH FRAME NUMERIC:
H0_numeric_(:,:,1) = eye(4);
H0_numeric_(:,:,2) = HTM_numeric(:,:,1);
H0_numeric_(:,:,3) = HTM_numeric(:,:,1)*HTM_numeric(:,:,2);  
H0_numeric_(:,:,4) = HTM_numeric(:,:,1)*HTM_numeric(:,:,2)*HTM_numeric(:,:,3); 
H0_numeric_(:,:,5) = HTM_numeric(:,:,1)*HTM_numeric(:,:,2)*HTM_numeric(:,:,3)*HTM_numeric(:,:,4); 
H0_numeric_(:,:,6) = HTM_numeric(:,:,1)*HTM_numeric(:,:,2)*HTM_numeric(:,:,3)*HTM_numeric(:,:,4)*HTM_numeric(:,:,5); 
H0_numeric_(:,:,7) = HTM_numeric(:,:,1)*HTM_numeric(:,:,2)*HTM_numeric(:,:,3)*HTM_numeric(:,:,4)*HTM_numeric(:,:,5)*HTM_numeric(:,:,6); 

% JACOBIAN:
for j=2:1:7
    J(1:3,j-1) = cross(H0_numeric_(1:3,1:3,j-1)*[0;0;1],H0_numeric_(1:3,4,7)-H0_numeric_(1:3,4,j-1));
    J(4:6,j-1) = H0_numeric_(1:3,1:3,j-1)*[0;0;1];
    
    J_symbolic(1:3,j-1) = cross( H0_symbolic_(1:3,3,j-1),(H0_symbolic_(1:3,4,7) - H0_symbolic_(1:3,4,j-1)) );
    J_symbolic(4:6,j-1) = H0_symbolic_(1:3,3,j-1);
end

J=simplify(J_symbolic);
detJ=simplify(det(J))

figure(1)
vzorka = 20;
Q2inter = linspace(-160,65,vzorka);
Q3inter = linspace(-51,225,vzorka);
[Q2, Q3] = meshgrid(Q2inter,Q3inter);
Z = 5*cos(Q2*pi/180+Q3*pi/180) - 5*sin(Q2*pi/180)+2;
surf(Q2,Q3,Z)
title('Third factor of Jacobian determinant')
set(gcf,'color','w');
xlabel('Q2');ylabel('Q3'),zlabel('Determinant')
hold on

i=1;
range = 0.15
for q2 = -160:1:65
    for q3 = -51:1:225
        q2rad = q2*pi/180;
        q3rad = q3*pi/180;
        det = 5*cos(q2rad+q3rad)-5*sin(q2rad)+2;
        if ((det>-range)&&(det<range))
            table(i,:) = [ det, q2, q3 ]
            plot3(q2,q3,det,'.','color','r')
            hold on
            xlabel('Q2 (°)');ylabel('Q3 (°)'),zlabel('Determinant');
            i=i+1
        end    
    end
end

figure(2)

for q2 = -160:1:65
    for q3 = -51:1:225
        q2rad = q2*pi/180;
        q3rad = q3*pi/180;
        det = 5*cos(q2rad+q3rad)-5*sin(q2rad)+2;
        if ((det>-range)&&(det<range))
            table(i,:) = [ det, q2, q3 ]
            plot3(q2,q3,det,'.','color','r')
            title('Third factor of Jacobian determinant. Top view')
            view(0,90);
            set(gcf,'color','w');
            hold on
            xlabel('Q2 (°)');ylabel('Q3 (°)'),zlabel('Determinant');
            i=i+1
        end    
    end
end