function [Jacobiano] = Jacobian_robonuc_aprox2(Q)
%UNTITLED6 Summary of this function goes here
%   Detailed explanation goes here
teta_p= Q(1);
d_p=Q(2);
teta1= Q(3);
teta2= Q(4);
teta3= Q(5);
teta4= Q(6);
teta5= Q(7);
teta6= Q(8);

%load de ficheiro
load('jacobiano_mat_aprox2.mat');
g = matlabFunction(J,'vars',{'teta_p','d_p','teta1','teta2','teta3','teta4','teta5','teta6'});
Jacobiano=g(teta_p,d_p,teta1,teta2,teta3,teta4,teta5,teta6);


% J_1_1to4=[ (2*sin(teta5)*(sin(teta4)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) + cos(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))))/25 - (cos(teta1 - pi/2)*sin(teta_p + pi/2))/20 - (cos(teta_p + pi/2)*sin(teta1 - pi/2))/20 - (33*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/100 - (2*cos(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2))))/25 + d_p*cos(teta_p + pi/2) - (67*cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/200 - (67*cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/200 + (7*sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/200 - (7*cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/200, sin(teta_p + pi/2), (2*sin(teta5)*(sin(teta4)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) + cos(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))))/25 - (cos(teta1 - pi/2)*sin(teta_p + pi/2))/20 - (cos(teta_p + pi/2)*sin(teta1 - pi/2))/20 - (33*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/100 - (2*cos(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2))))/25 - (67*cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/200 - (67*cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/200 + (7*sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/200 - (7*cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/200, (67*cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/200 - (2*cos(teta5)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2))))/25 - (7*cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/200 - (7*cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/200 - (67*sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/200 - (2*cos(teta4)*sin(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2))))/25 - (33*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/100];
% 
% J_1_5_8=[ (67*cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/200 - (7*cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/200 - (7*cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/200 - (67*sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/200 - (2*cos(teta4)*sin(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2))))/25 - (2*cos(teta5)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2))))/25, (2*sin(teta5)*(cos(teta4)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) + sin(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))))/25, (2*cos(teta5)*(sin(teta4)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))))/25 - (2*sin(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2))))/25, 0];
% 
% J_2_1to4=[ (33*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/100 + (cos(teta1 - pi/2)*cos(teta_p + pi/2))/20 - (sin(teta1 - pi/2)*sin(teta_p + pi/2))/20 + (2*sin(teta5)*(sin(teta4)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))))/25 + (2*cos(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2))))/25 + d_p*sin(teta_p + pi/2) + (67*cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/200 + (67*cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/200 - (7*sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/200 + (7*cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/200, -cos(teta_p + pi/2), (33*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/100 + (cos(teta1 - pi/2)*cos(teta_p + pi/2))/20 - (sin(teta1 - pi/2)*sin(teta_p + pi/2))/20 + (2*sin(teta5)*(sin(teta4)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))))/25 + (2*cos(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2))))/25 + (67*cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/200 + (67*cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/200 - (7*sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/200 + (7*cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))/200, (67*cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/200 - (2*cos(teta5)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2))))/25 - (7*cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/200 - (7*cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/200 - (67*sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/200 - (2*cos(teta4)*sin(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2))))/25 - (33*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/100];
% 
% J_2_5to8=[ (67*cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/200 - (7*cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/200 - (7*cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/200 - (67*sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))/200 - (2*cos(teta4)*sin(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2))))/25 - (2*cos(teta5)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2))))/25, -(2*sin(teta5)*(cos(teta4)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - sin(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))))/25, - (2*cos(teta5)*(sin(teta4)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) + cos(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))))/25 - (2*sin(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2))))/25, 0];
% 
% J_3_1to4=[ 0, 0, 0, (33*cos(teta2 + pi/2))/100 + (7*cos(teta3)*cos(teta2 + pi/2))/200 + (67*cos(teta3)*sin(teta2 + pi/2))/200 + (67*cos(teta2 + pi/2)*sin(teta3))/200 - (7*sin(teta3)*sin(teta2 + pi/2))/200 + (2*cos(teta5)*(cos(teta3)*sin(teta2 + pi/2) + cos(teta2 + pi/2)*sin(teta3)))/25 + (2*cos(teta4)*sin(teta5)*(cos(teta3)*cos(teta2 + pi/2) - sin(teta3)*sin(teta2 + pi/2)))/25];
% 
% J_3_5to8=[ (7*cos(teta3)*cos(teta2 + pi/2))/200 + (67*cos(teta3)*sin(teta2 + pi/2))/200 + (67*cos(teta2 + pi/2)*sin(teta3))/200 - (7*sin(teta3)*sin(teta2 + pi/2))/200 + (2*cos(teta5)*(cos(teta3)*sin(teta2 + pi/2) + cos(teta2 + pi/2)*sin(teta3)))/25 + (2*cos(teta4)*sin(teta5)*(cos(teta3)*cos(teta2 + pi/2) - sin(teta3)*sin(teta2 + pi/2)))/25, -(2*sin(teta4)*sin(teta5)*(cos(teta3)*sin(teta2 + pi/2) + cos(teta2 + pi/2)*sin(teta3)))/25, (2*sin(teta5)*(cos(teta3)*cos(teta2 + pi/2) - sin(teta3)*sin(teta2 + pi/2)))/25 + (2*cos(teta4)*cos(teta5)*(cos(teta3)*sin(teta2 + pi/2) + cos(teta2 + pi/2)*sin(teta3)))/25, 0];
% 
% 
% J_4_1to2=[ (((sin(teta6)*(cos(teta4)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - sin(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))) + cos(teta6)*(cos(teta5)*(sin(teta4)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) + cos(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))) + sin(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))))^2/(sin(teta6)*(cos(teta4)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) + sin(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))) + cos(teta6)*(cos(teta5)*(sin(teta4)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))) - sin(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))))^2 + 1)*(sin(teta6)*(cos(teta4)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) + sin(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))) + cos(teta6)*(cos(teta5)*(sin(teta4)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))) - sin(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))))^2)/((sin(teta6)*(cos(teta4)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - sin(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))) + cos(teta6)*(cos(teta5)*(sin(teta4)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) + cos(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))) + sin(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))))^2 + (sin(teta6)*(cos(teta4)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) + sin(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))) + cos(teta6)*(cos(teta5)*(sin(teta4)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))) - sin(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))))^2), 0];
% 
% J_4_3=(((sin(teta6)*(cos(teta4)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - sin(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))) + cos(teta6)*(cos(teta5)*(sin(teta4)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) + cos(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))) + sin(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))))^2/(sin(teta6)*(cos(teta4)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) + sin(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))) + cos(teta6)*(cos(teta5)*(sin(teta4)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))) - sin(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))))^2 + 1)*(sin(teta6)*(cos(teta4)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) + sin(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))) + cos(teta6)*(cos(teta5)*(sin(teta4)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))) - sin(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))))^2)/((sin(teta6)*(cos(teta4)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - sin(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))) + cos(teta6)*(cos(teta5)*(sin(teta4)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) + cos(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))) + sin(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)))))^2 + (sin(teta6)*(cos(teta4)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) + sin(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))) + cos(teta6)*(cos(teta5)*(sin(teta4)*(cos(teta1 - pi/2)*sin(teta_p + pi/2) + cos(teta_p + pi/2)*sin(teta1 - pi/2)) - cos(teta4)*(sin(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) - cos(teta3)*cos(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))) - sin(teta5)*(cos(teta3)*sin(teta2 + pi/2)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)) + cos(teta2 + pi/2)*sin(teta3)*(cos(teta1 - pi/2)*cos(teta_p + pi/2) - sin(teta1 - pi/2)*sin(teta_p + pi/2)))))^2);
% 
% J_4_5to8=
% J_5_1to4=
% J_5_5to8=
% J_6_1to4=
% J_6_5to8=

end

