%%
clc
clear all
%load('jacobiano_mat_aprox1_manipulator.mat');
%Q=[pi/3 0 0 pi/3 0 pi/4];
Q=[0 0 0 pi/4 0 0];
teta1= Q(1);
teta2= Q(2);
teta3= Q(3);
teta4= Q(4);
teta5= Q(5);
teta6= Q(6);

Jcolunas1_3=[ (33*sin(teta1)*sin(teta2))/100 - sin(teta1)/20 + (7*cos(teta2)*sin(teta1)*sin(teta3))/200 + (7*cos(teta3)*sin(teta1)*sin(teta2))/200 + (2*cos(teta1)*sin(teta4)*sin(teta5))/25 + (67*sin(teta1)*sin(teta2)*sin(teta3))/200 - (67*cos(teta2)*cos(teta3)*sin(teta1))/200 - (2*cos(teta2)*cos(teta3)*cos(teta5)*sin(teta1))/25 + (2*cos(teta5)*sin(teta1)*sin(teta2)*sin(teta3))/25 + (2*cos(teta2)*cos(teta4)*sin(teta1)*sin(teta3)*sin(teta5))/25 + (2*cos(teta3)*cos(teta4)*sin(teta1)*sin(teta2)*sin(teta5))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   -(cos(teta1)*(66*cos(teta2) + 7*cos(teta2)*cos(teta3) + 67*cos(teta2)*sin(teta3) + 67*cos(teta3)*sin(teta2) - 7*sin(teta2)*sin(teta3) + 16*cos(teta2)*cos(teta5)*sin(teta3) + 16*cos(teta3)*cos(teta5)*sin(teta2) + 16*cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - 16*cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/200,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   -(cos(teta1)*(7*cos(teta2)*cos(teta3) + 67*cos(teta2)*sin(teta3) + 67*cos(teta3)*sin(teta2) - 7*sin(teta2)*sin(teta3) + 16*cos(teta2)*cos(teta5)*sin(teta3) + 16*cos(teta3)*cos(teta5)*sin(teta2) + 16*cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - 16*cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/200
 cos(teta1)/20 - (33*cos(teta1)*sin(teta2))/100 - (67*cos(teta1)*sin(teta2)*sin(teta3))/200 + (2*sin(teta1)*sin(teta4)*sin(teta5))/25 + (67*cos(teta1)*cos(teta2)*cos(teta3))/200 - (7*cos(teta1)*cos(teta2)*sin(teta3))/200 - (7*cos(teta1)*cos(teta3)*sin(teta2))/200 + (2*cos(teta1)*cos(teta2)*cos(teta3)*cos(teta5))/25 - (2*cos(teta1)*cos(teta5)*sin(teta2)*sin(teta3))/25 - (2*cos(teta1)*cos(teta2)*cos(teta4)*sin(teta3)*sin(teta5))/25 - (2*cos(teta1)*cos(teta3)*cos(teta4)*sin(teta2)*sin(teta5))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   -(sin(teta1)*(66*cos(teta2) + 7*cos(teta2)*cos(teta3) + 67*cos(teta2)*sin(teta3) + 67*cos(teta3)*sin(teta2) - 7*sin(teta2)*sin(teta3) + 16*cos(teta2)*cos(teta5)*sin(teta3) + 16*cos(teta3)*cos(teta5)*sin(teta2) + 16*cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - 16*cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/200,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   -(sin(teta1)*(7*cos(teta2)*cos(teta3) + 67*cos(teta2)*sin(teta3) + 67*cos(teta3)*sin(teta2) - 7*sin(teta2)*sin(teta3) + 16*cos(teta2)*cos(teta5)*sin(teta3) + 16*cos(teta3)*cos(teta5)*sin(teta2) + 16*cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - 16*cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/200
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                         (67*cos(teta2)*cos(teta3))/200 - (33*sin(teta2))/100 - (7*cos(teta2)*sin(teta3))/200 - (7*cos(teta3)*sin(teta2))/200 - (67*sin(teta2)*sin(teta3))/200 - (2*cos(teta5)*sin(teta2)*sin(teta3))/25 + (2*cos(teta2)*cos(teta3)*cos(teta5))/25 - (2*cos(teta2)*cos(teta4)*sin(teta3)*sin(teta5))/25 - (2*cos(teta3)*cos(teta4)*sin(teta2)*sin(teta5))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               (67*cos(teta2)*cos(teta3))/200 - (7*cos(teta2)*sin(teta3))/200 - (7*cos(teta3)*sin(teta2))/200 - (67*sin(teta2)*sin(teta3))/200 - (2*cos(teta5)*sin(teta2)*sin(teta3))/25 + (2*cos(teta2)*cos(teta3)*cos(teta5))/25 - (2*cos(teta2)*cos(teta4)*sin(teta3)*sin(teta5))/25 - (2*cos(teta3)*cos(teta4)*sin(teta2)*sin(teta5))/25
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                1, ((cos(teta4)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta4))*(sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta2 + teta3)*cos(teta1)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2*(cos(teta2)*cos(teta3)*sin(teta4)*sin(teta6) + cos(teta2)*cos(teta6)*sin(teta3)*sin(teta5) + cos(teta3)*cos(teta6)*sin(teta2)*sin(teta5) - sin(teta2)*sin(teta3)*sin(teta4)*sin(teta6) - cos(teta2)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6) + cos(teta4)*cos(teta5)*cos(teta6)*sin(teta2)*sin(teta3)))/(((sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta2 + teta3)*sin(teta1)*sin(teta5) + cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2 + (sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta2 + teta3)*cos(teta1)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2)*(cos(teta4)*sin(teta1)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta1)*sin(teta4) - cos(teta1)*cos(teta2)*cos(teta3)*cos(teta6)*sin(teta5) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta6)*sin(teta2)*sin(teta3)*sin(teta5) - cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta3) - cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta2))^2), ((cos(teta4)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta4))*(sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta2 + teta3)*cos(teta1)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2*(cos(teta2)*cos(teta3)*sin(teta4)*sin(teta6) + cos(teta2)*cos(teta6)*sin(teta3)*sin(teta5) + cos(teta3)*cos(teta6)*sin(teta2)*sin(teta5) - sin(teta2)*sin(teta3)*sin(teta4)*sin(teta6) - cos(teta2)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6) + cos(teta4)*cos(teta5)*cos(teta6)*sin(teta2)*sin(teta3)))/(((sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta2 + teta3)*sin(teta1)*sin(teta5) + cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2 + (sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta2 + teta3)*cos(teta1)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2)*(cos(teta4)*sin(teta1)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta1)*sin(teta4) - cos(teta1)*cos(teta2)*cos(teta3)*cos(teta6)*sin(teta5) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta6)*sin(teta2)*sin(teta3)*sin(teta5) - cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta3) - cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta2))^2)
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            (cos(teta2 + teta3)*cos(teta6)*sin(teta5) - sin(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(1/2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            (cos(teta2 + teta3)*cos(teta6)*sin(teta5) - sin(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(1/2)
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              ((sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2*(cos(teta4)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta4)))/(((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)*(cos(teta2)*cos(teta5)*sin(teta3) + cos(teta3)*cos(teta5)*sin(teta2) + cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5))^2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              ((sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2*(cos(teta4)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta4)))/(((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)*(cos(teta2)*cos(teta5)*sin(teta3) + cos(teta3)*cos(teta5)*sin(teta2) + cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5))^2)];
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            
Jcolunas4_6= [                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    (2*sin(teta5)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   - (2*cos(teta5)*(cos(teta1)*cos(teta2)*cos(teta4)*sin(teta3) - sin(teta1)*sin(teta4) + cos(teta1)*cos(teta3)*cos(teta4)*sin(teta2)))/25 - (2*cos(teta2 + teta3)*cos(teta1)*sin(teta5))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        0
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                    (2*sin(teta5)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                   - (2*cos(teta5)*(cos(teta1)*sin(teta4) + cos(teta2)*cos(teta4)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*sin(teta1)*sin(teta2)))/25 - (2*cos(teta2 + teta3)*sin(teta1)*sin(teta5))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        0
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                         -(2*cos(teta2 + teta3)*sin(teta4)*sin(teta5))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      (2*cos(teta2 + teta3)*cos(teta4)*cos(teta5))/25 - (2*sin(teta2 + teta3)*sin(teta5))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        0
 ((sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta2 + teta3)*cos(teta1)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2*(cos(teta2)*sin(teta3) + cos(teta3)*sin(teta2) - cos(teta2)*cos(teta6)^2*sin(teta3) - cos(teta3)*cos(teta6)^2*sin(teta2) + cos(teta2)*cos(teta5)^2*cos(teta6)^2*sin(teta3) + cos(teta3)*cos(teta5)^2*cos(teta6)^2*sin(teta2) - cos(teta2)*cos(teta3)*cos(teta6)*sin(teta4)*sin(teta5)*sin(teta6) + cos(teta6)*sin(teta2)*sin(teta3)*sin(teta4)*sin(teta5)*sin(teta6) + cos(teta2)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6)^2*sin(teta5) - cos(teta4)*cos(teta5)*cos(teta6)^2*sin(teta2)*sin(teta3)*sin(teta5)))/(((sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta2 + teta3)*sin(teta1)*sin(teta5) + cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2 + (sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta2 + teta3)*cos(teta1)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2)*(cos(teta4)*sin(teta1)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta1)*sin(teta4) - cos(teta1)*cos(teta2)*cos(teta3)*cos(teta6)*sin(teta5) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta6)*sin(teta2)*sin(teta3)*sin(teta5) - cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta3) - cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta2))^2), (cos(teta6)*(sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta2 + teta3)*cos(teta1)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2*(cos(teta6)*sin(teta2)*sin(teta3)*sin(teta4) - cos(teta2)*cos(teta3)*cos(teta6)*sin(teta4) + cos(teta2)*sin(teta3)*sin(teta5)*sin(teta6) + cos(teta3)*sin(teta2)*sin(teta5)*sin(teta6) - cos(teta2)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta6) + cos(teta4)*cos(teta5)*sin(teta2)*sin(teta3)*sin(teta6)))/(((sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta2 + teta3)*sin(teta1)*sin(teta5) + cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2 + (sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta2 + teta3)*cos(teta1)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2)*(cos(teta4)*sin(teta1)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta1)*sin(teta4) - cos(teta1)*cos(teta2)*cos(teta3)*cos(teta6)*sin(teta5) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta6)*sin(teta2)*sin(teta3)*sin(teta5) - cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta3) - cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta2))^2), ((sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta2 + teta3)*cos(teta1)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2*(cos(teta2)*cos(teta5)*sin(teta3) + cos(teta3)*cos(teta5)*sin(teta2) + cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/(((sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta2 + teta3)*sin(teta1)*sin(teta5) + cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2 + (sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta2 + teta3)*cos(teta1)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2)*(cos(teta4)*sin(teta1)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta1)*sin(teta4) - cos(teta1)*cos(teta2)*cos(teta3)*cos(teta6)*sin(teta5) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta6)*sin(teta2)*sin(teta3)*sin(teta5) - cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta3) - cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta2))^2)
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     ((cos(teta2 + teta3)*(cos(teta4)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta4)))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(1/2) - ((2*cos(teta2 + teta3)*(cos(teta4)*cos(teta6) - cos(teta5)*sin(teta4)*sin(teta6))*(cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6)) - 2*cos(teta2 + teta3)*sin(teta4)*sin(teta5)*(sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5)))*(cos(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta6)*sin(teta5) - cos(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6)))/(2*((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(3/2)))*((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  ((cos(teta6)*(sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5)))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(1/2) + (cos(teta6)*(sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))*(cos(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta6)*sin(teta5) - cos(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6))^2)/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(3/2))*((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     ((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(1/2) + ((cos(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta6)*sin(teta5) - cos(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6))^2*(cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6)))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(3/2))*((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                         -(cos(teta2 + teta3)*(cos(teta2 + teta3)*cos(teta6)*sin(teta5) - sin(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6)))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                          ((sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2*(sin(teta6) - ((sin(teta2 + teta3)*sin(teta5) - cos(teta2 + teta3)*cos(teta4)*cos(teta5))*(cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6)))/(sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  ((sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))*(cos(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta6)*sin(teta5) - cos(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6)))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)];
 

J2=[Jcolunas1_3 Jcolunas4_6];

%% abordagem2
clc
clear all

Q=[0 0 0 pi/3 0 0];
teta1= Q(1);
teta2= Q(2);
teta3= Q(3);
teta4= Q(4);
teta5= Q(5);
teta6= Q(6);

Jcolunas1_3=[ cos(teta1)/20 - (33*cos(teta1)*sin(teta2))/100 - (67*cos(teta1)*sin(teta2)*sin(teta3))/200 + (2*sin(teta1)*sin(teta4)*sin(teta5))/25 + (67*cos(teta1)*cos(teta2)*cos(teta3))/200 - (7*cos(teta1)*cos(teta2)*sin(teta3))/200 - (7*cos(teta1)*cos(teta3)*sin(teta2))/200 + (2*cos(teta1)*cos(teta2)*cos(teta3)*cos(teta5))/25 - (2*cos(teta1)*cos(teta5)*sin(teta2)*sin(teta3))/25 - (2*cos(teta1)*cos(teta2)*cos(teta4)*sin(teta3)*sin(teta5))/25 - (2*cos(teta1)*cos(teta3)*cos(teta4)*sin(teta2)*sin(teta5))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      -(sin(teta1)*(66*cos(teta2) + 7*cos(teta2)*cos(teta3) + 67*cos(teta2)*sin(teta3) + 67*cos(teta3)*sin(teta2) - 7*sin(teta2)*sin(teta3) + 16*cos(teta2)*cos(teta5)*sin(teta3) + 16*cos(teta3)*cos(teta5)*sin(teta2) + 16*cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - 16*cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/200,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      -(sin(teta1)*(7*cos(teta2)*cos(teta3) + 67*cos(teta2)*sin(teta3) + 67*cos(teta3)*sin(teta2) - 7*sin(teta2)*sin(teta3) + 16*cos(teta2)*cos(teta5)*sin(teta3) + 16*cos(teta3)*cos(teta5)*sin(teta2) + 16*cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - 16*cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/200
 sin(teta1)/20 - (33*sin(teta1)*sin(teta2))/100 - (7*cos(teta2)*sin(teta1)*sin(teta3))/200 - (7*cos(teta3)*sin(teta1)*sin(teta2))/200 - (2*cos(teta1)*sin(teta4)*sin(teta5))/25 - (67*sin(teta1)*sin(teta2)*sin(teta3))/200 + (67*cos(teta2)*cos(teta3)*sin(teta1))/200 + (2*cos(teta2)*cos(teta3)*cos(teta5)*sin(teta1))/25 - (2*cos(teta5)*sin(teta1)*sin(teta2)*sin(teta3))/25 - (2*cos(teta2)*cos(teta4)*sin(teta1)*sin(teta3)*sin(teta5))/25 - (2*cos(teta3)*cos(teta4)*sin(teta1)*sin(teta2)*sin(teta5))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       (cos(teta1)*(66*cos(teta2) + 7*cos(teta2)*cos(teta3) + 67*cos(teta2)*sin(teta3) + 67*cos(teta3)*sin(teta2) - 7*sin(teta2)*sin(teta3) + 16*cos(teta2)*cos(teta5)*sin(teta3) + 16*cos(teta3)*cos(teta5)*sin(teta2) + 16*cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - 16*cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/200,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       (cos(teta1)*(7*cos(teta2)*cos(teta3) + 67*cos(teta2)*sin(teta3) + 67*cos(teta3)*sin(teta2) - 7*sin(teta2)*sin(teta3) + 16*cos(teta2)*cos(teta5)*sin(teta3) + 16*cos(teta3)*cos(teta5)*sin(teta2) + 16*cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - 16*cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/200
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            (67*cos(teta2)*cos(teta3))/200 - (33*sin(teta2))/100 - (7*cos(teta2)*sin(teta3))/200 - (7*cos(teta3)*sin(teta2))/200 - (67*sin(teta2)*sin(teta3))/200 - (2*cos(teta5)*sin(teta2)*sin(teta3))/25 + (2*cos(teta2)*cos(teta3)*cos(teta5))/25 - (2*cos(teta2)*cos(teta4)*sin(teta3)*sin(teta5))/25 - (2*cos(teta3)*cos(teta4)*sin(teta2)*sin(teta5))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  (67*cos(teta2)*cos(teta3))/200 - (7*cos(teta2)*sin(teta3))/200 - (7*cos(teta3)*sin(teta2))/200 - (67*sin(teta2)*sin(teta3))/200 - (2*cos(teta5)*sin(teta2)*sin(teta3))/25 + (2*cos(teta2)*cos(teta3)*cos(teta5))/25 - (2*cos(teta2)*cos(teta4)*sin(teta3)*sin(teta5))/25 - (2*cos(teta3)*cos(teta4)*sin(teta2)*sin(teta5))/25
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                1, ((cos(teta4)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta4))*(sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta3)*sin(teta1)*sin(teta5) - sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2*(cos(teta2)*cos(teta3)*sin(teta4)*sin(teta6) + cos(teta2)*cos(teta6)*sin(teta3)*sin(teta5) + cos(teta3)*cos(teta6)*sin(teta2)*sin(teta5) - sin(teta2)*sin(teta3)*sin(teta4)*sin(teta6) - cos(teta2)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6) + cos(teta4)*cos(teta5)*cos(teta6)*sin(teta2)*sin(teta3)))/(((sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta3)*sin(teta1)*sin(teta5) - sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2 + (sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta2)*cos(teta3)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) - cos(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2)*(cos(teta1)*cos(teta4)*sin(teta6) + cos(teta1)*cos(teta5)*cos(teta6)*sin(teta4) + cos(teta2)*cos(teta3)*cos(teta6)*sin(teta1)*sin(teta5) - cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4)*sin(teta6) - cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)*sin(teta6) - cos(teta6)*sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta1)*sin(teta2))^2), ((cos(teta4)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta4))*(sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta3)*sin(teta1)*sin(teta5) - sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2*(cos(teta2)*cos(teta3)*sin(teta4)*sin(teta6) + cos(teta2)*cos(teta6)*sin(teta3)*sin(teta5) + cos(teta3)*cos(teta6)*sin(teta2)*sin(teta5) - sin(teta2)*sin(teta3)*sin(teta4)*sin(teta6) - cos(teta2)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6) + cos(teta4)*cos(teta5)*cos(teta6)*sin(teta2)*sin(teta3)))/(((sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta3)*sin(teta1)*sin(teta5) - sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2 + (sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta2)*cos(teta3)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) - cos(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2)*(cos(teta1)*cos(teta4)*sin(teta6) + cos(teta1)*cos(teta5)*cos(teta6)*sin(teta4) + cos(teta2)*cos(teta3)*cos(teta6)*sin(teta1)*sin(teta5) - cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4)*sin(teta6) - cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)*sin(teta6) - cos(teta6)*sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta1)*sin(teta2))^2)
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               (cos(teta2 + teta3)*cos(teta6)*sin(teta5) - sin(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(1/2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               (cos(teta2 + teta3)*cos(teta6)*sin(teta5) - sin(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(1/2)
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 ((sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2*(cos(teta4)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta4)))/(((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)*(cos(teta2)*cos(teta5)*sin(teta3) + cos(teta3)*cos(teta5)*sin(teta2) + cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5))^2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 ((sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2*(cos(teta4)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta4)))/(((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)*(cos(teta2)*cos(teta5)*sin(teta3) + cos(teta3)*cos(teta5)*sin(teta2) + cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5))^2)];
 

Jcolunas4_6=[   
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       (2*sin(teta5)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              (2*sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5))/25 - (2*cos(teta2)*cos(teta3)*sin(teta1)*sin(teta5))/25 - (2*cos(teta1)*cos(teta5)*sin(teta4))/25 - (2*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3))/25 - (2*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                           0
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      -(2*sin(teta5)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              (2*cos(teta1)*cos(teta2)*cos(teta3)*sin(teta5))/25 - (2*cos(teta5)*sin(teta1)*sin(teta4))/25 - (2*cos(teta1)*sin(teta2)*sin(teta3)*sin(teta5))/25 + (2*cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3))/25 + (2*cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                           0
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            -(2*cos(teta2 + teta3)*sin(teta4)*sin(teta5))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                         (2*cos(teta2 + teta3)*cos(teta4)*cos(teta5))/25 - (2*sin(teta2 + teta3)*sin(teta5))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                           0
 ((sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta3)*sin(teta1)*sin(teta5) - sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2*(cos(teta2)*sin(teta3) + cos(teta3)*sin(teta2) - cos(teta2)*cos(teta6)^2*sin(teta3) - cos(teta3)*cos(teta6)^2*sin(teta2) + cos(teta2)*cos(teta5)^2*cos(teta6)^2*sin(teta3) + cos(teta3)*cos(teta5)^2*cos(teta6)^2*sin(teta2) - cos(teta2)*cos(teta3)*cos(teta6)*sin(teta4)*sin(teta5)*sin(teta6) + cos(teta6)*sin(teta2)*sin(teta3)*sin(teta4)*sin(teta5)*sin(teta6) + cos(teta2)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6)^2*sin(teta5) - cos(teta4)*cos(teta5)*cos(teta6)^2*sin(teta2)*sin(teta3)*sin(teta5)))/(((sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta3)*sin(teta1)*sin(teta5) - sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2 + (sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta2)*cos(teta3)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) - cos(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2)*(cos(teta1)*cos(teta4)*sin(teta6) + cos(teta1)*cos(teta5)*cos(teta6)*sin(teta4) + cos(teta2)*cos(teta3)*cos(teta6)*sin(teta1)*sin(teta5) - cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4)*sin(teta6) - cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)*sin(teta6) - cos(teta6)*sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta1)*sin(teta2))^2), (cos(teta6)*(sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta3)*sin(teta1)*sin(teta5) - sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2*(cos(teta6)*sin(teta2)*sin(teta3)*sin(teta4) - cos(teta2)*cos(teta3)*cos(teta6)*sin(teta4) + cos(teta2)*sin(teta3)*sin(teta5)*sin(teta6) + cos(teta3)*sin(teta2)*sin(teta5)*sin(teta6) - cos(teta2)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta6) + cos(teta4)*cos(teta5)*sin(teta2)*sin(teta3)*sin(teta6)))/(((sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta3)*sin(teta1)*sin(teta5) - sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2 + (sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta2)*cos(teta3)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) - cos(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2)*(cos(teta1)*cos(teta4)*sin(teta6) + cos(teta1)*cos(teta5)*cos(teta6)*sin(teta4) + cos(teta2)*cos(teta3)*cos(teta6)*sin(teta1)*sin(teta5) - cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4)*sin(teta6) - cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)*sin(teta6) - cos(teta6)*sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta1)*sin(teta2))^2), ((sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta3)*sin(teta1)*sin(teta5) - sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2*(cos(teta2)*cos(teta5)*sin(teta3) + cos(teta3)*cos(teta5)*sin(teta2) + cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/(((sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta3)*sin(teta1)*sin(teta5) - sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2 + (sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta2)*cos(teta3)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) - cos(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2)*(cos(teta1)*cos(teta4)*sin(teta6) + cos(teta1)*cos(teta5)*cos(teta6)*sin(teta4) + cos(teta2)*cos(teta3)*cos(teta6)*sin(teta1)*sin(teta5) - cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4)*sin(teta6) - cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)*sin(teta6) - cos(teta6)*sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta1)*sin(teta2))^2)
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        ((cos(teta2 + teta3)*(cos(teta4)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta4)))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(1/2) - ((2*cos(teta2 + teta3)*(cos(teta4)*cos(teta6) - cos(teta5)*sin(teta4)*sin(teta6))*(cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6)) - 2*cos(teta2 + teta3)*sin(teta4)*sin(teta5)*(sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5)))*(cos(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta6)*sin(teta5) - cos(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6)))/(2*((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(3/2)))*((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     ((cos(teta6)*(sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5)))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(1/2) + (cos(teta6)*(sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))*(cos(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta6)*sin(teta5) - cos(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6))^2)/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(3/2))*((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        ((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(1/2) + ((cos(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta6)*sin(teta5) - cos(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6))^2*(cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6)))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(3/2))*((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            -(cos(teta2 + teta3)*(cos(teta2 + teta3)*cos(teta6)*sin(teta5) - sin(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6)))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                             ((sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2*(sin(teta6) - ((sin(teta2 + teta3)*sin(teta5) - cos(teta2 + teta3)*cos(teta4)*cos(teta5))*(cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6)))/(sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     ((sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))*(cos(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta6)*sin(teta5) - cos(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6)))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)];
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                           
J=[Jcolunas1_3 Jcolunas4_6]

%% abordagem 2 _v2

clc
clear all

Q=[0 0 0 pi/3 0 0];
teta1= Q(1);
teta2= Q(2);
teta3= Q(3);
teta4= Q(4);
teta5= Q(5);
teta6= Q(6);

Jcolunas1_3= [(33*sin(teta1)*sin(teta2))/100 - sin(teta1)/20 + (7*cos(teta2)*sin(teta1)*sin(teta3))/200 + (7*cos(teta3)*sin(teta1)*sin(teta2))/200 + (2*cos(teta1)*sin(teta4)*sin(teta5))/25 + (67*sin(teta1)*sin(teta2)*sin(teta3))/200 - (67*cos(teta2)*cos(teta3)*sin(teta1))/200 - (2*cos(teta2)*cos(teta3)*cos(teta5)*sin(teta1))/25 + (2*cos(teta5)*sin(teta1)*sin(teta2)*sin(teta3))/25 + (2*cos(teta2)*cos(teta4)*sin(teta1)*sin(teta3)*sin(teta5))/25 + (2*cos(teta3)*cos(teta4)*sin(teta1)*sin(teta2)*sin(teta5))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      -(cos(teta1)*(66*cos(teta2) + 7*cos(teta2)*cos(teta3) + 67*cos(teta2)*sin(teta3) + 67*cos(teta3)*sin(teta2) - 7*sin(teta2)*sin(teta3) + 16*cos(teta2)*cos(teta5)*sin(teta3) + 16*cos(teta3)*cos(teta5)*sin(teta2) + 16*cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - 16*cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/200,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      -(cos(teta1)*(7*cos(teta2)*cos(teta3) + 67*cos(teta2)*sin(teta3) + 67*cos(teta3)*sin(teta2) - 7*sin(teta2)*sin(teta3) + 16*cos(teta2)*cos(teta5)*sin(teta3) + 16*cos(teta3)*cos(teta5)*sin(teta2) + 16*cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - 16*cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/200
 cos(teta1)/20 - (33*cos(teta1)*sin(teta2))/100 - (67*cos(teta1)*sin(teta2)*sin(teta3))/200 + (2*sin(teta1)*sin(teta4)*sin(teta5))/25 + (67*cos(teta1)*cos(teta2)*cos(teta3))/200 - (7*cos(teta1)*cos(teta2)*sin(teta3))/200 - (7*cos(teta1)*cos(teta3)*sin(teta2))/200 + (2*cos(teta1)*cos(teta2)*cos(teta3)*cos(teta5))/25 - (2*cos(teta1)*cos(teta5)*sin(teta2)*sin(teta3))/25 - (2*cos(teta1)*cos(teta2)*cos(teta4)*sin(teta3)*sin(teta5))/25 - (2*cos(teta1)*cos(teta3)*cos(teta4)*sin(teta2)*sin(teta5))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      -(sin(teta1)*(66*cos(teta2) + 7*cos(teta2)*cos(teta3) + 67*cos(teta2)*sin(teta3) + 67*cos(teta3)*sin(teta2) - 7*sin(teta2)*sin(teta3) + 16*cos(teta2)*cos(teta5)*sin(teta3) + 16*cos(teta3)*cos(teta5)*sin(teta2) + 16*cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - 16*cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/200,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                      -(sin(teta1)*(7*cos(teta2)*cos(teta3) + 67*cos(teta2)*sin(teta3) + 67*cos(teta3)*sin(teta2) - 7*sin(teta2)*sin(teta3) + 16*cos(teta2)*cos(teta5)*sin(teta3) + 16*cos(teta3)*cos(teta5)*sin(teta2) + 16*cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - 16*cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/200
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            (67*cos(teta2)*cos(teta3))/200 - (33*sin(teta2))/100 - (7*cos(teta2)*sin(teta3))/200 - (7*cos(teta3)*sin(teta2))/200 - (67*sin(teta2)*sin(teta3))/200 - (2*cos(teta5)*sin(teta2)*sin(teta3))/25 + (2*cos(teta2)*cos(teta3)*cos(teta5))/25 - (2*cos(teta2)*cos(teta4)*sin(teta3)*sin(teta5))/25 - (2*cos(teta3)*cos(teta4)*sin(teta2)*sin(teta5))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                  (67*cos(teta2)*cos(teta3))/200 - (7*cos(teta2)*sin(teta3))/200 - (7*cos(teta3)*sin(teta2))/200 - (67*sin(teta2)*sin(teta3))/200 - (2*cos(teta5)*sin(teta2)*sin(teta3))/25 + (2*cos(teta2)*cos(teta3)*cos(teta5))/25 - (2*cos(teta2)*cos(teta4)*sin(teta3)*sin(teta5))/25 - (2*cos(teta3)*cos(teta4)*sin(teta2)*sin(teta5))/25
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                1, ((cos(teta4)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta4))*(sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta2)*cos(teta3)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) - cos(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2*(cos(teta2)*cos(teta3)*sin(teta4)*sin(teta6) + cos(teta2)*cos(teta6)*sin(teta3)*sin(teta5) + cos(teta3)*cos(teta6)*sin(teta2)*sin(teta5) - sin(teta2)*sin(teta3)*sin(teta4)*sin(teta6) - cos(teta2)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6) + cos(teta4)*cos(teta5)*cos(teta6)*sin(teta2)*sin(teta3)))/(((sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta3)*sin(teta1)*sin(teta5) - sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2 + (sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta2)*cos(teta3)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) - cos(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2)*(cos(teta4)*sin(teta1)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta1)*sin(teta4) - cos(teta1)*cos(teta2)*cos(teta3)*cos(teta6)*sin(teta5) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta6)*sin(teta2)*sin(teta3)*sin(teta5) - cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta3) - cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta2))^2), ((cos(teta4)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta4))*(sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta2)*cos(teta3)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) - cos(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2*(cos(teta2)*cos(teta3)*sin(teta4)*sin(teta6) + cos(teta2)*cos(teta6)*sin(teta3)*sin(teta5) + cos(teta3)*cos(teta6)*sin(teta2)*sin(teta5) - sin(teta2)*sin(teta3)*sin(teta4)*sin(teta6) - cos(teta2)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6) + cos(teta4)*cos(teta5)*cos(teta6)*sin(teta2)*sin(teta3)))/(((sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta3)*sin(teta1)*sin(teta5) - sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2 + (sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta2)*cos(teta3)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) - cos(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2)*(cos(teta4)*sin(teta1)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta1)*sin(teta4) - cos(teta1)*cos(teta2)*cos(teta3)*cos(teta6)*sin(teta5) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta6)*sin(teta2)*sin(teta3)*sin(teta5) - cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta3) - cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta2))^2)
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               (cos(teta2 + teta3)*cos(teta6)*sin(teta5) - sin(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(1/2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                               (cos(teta2 + teta3)*cos(teta6)*sin(teta5) - sin(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(1/2)
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                0,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 ((sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2*(cos(teta4)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta4)))/(((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)*(cos(teta2)*cos(teta5)*sin(teta3) + cos(teta3)*cos(teta5)*sin(teta2) + cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5))^2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                 ((sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2*(cos(teta4)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta4)))/(((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)*(cos(teta2)*cos(teta5)*sin(teta3) + cos(teta3)*cos(teta5)*sin(teta2) + cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5))^2)];
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            
Jcolunas4_6=[                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       (2*sin(teta5)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              (2*cos(teta5)*sin(teta1)*sin(teta4))/25 - (2*cos(teta1)*cos(teta2)*cos(teta3)*sin(teta5))/25 + (2*cos(teta1)*sin(teta2)*sin(teta3)*sin(teta5))/25 - (2*cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3))/25 - (2*cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                           0
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                       (2*sin(teta5)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                              (2*sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5))/25 - (2*cos(teta2)*cos(teta3)*sin(teta1)*sin(teta5))/25 - (2*cos(teta1)*cos(teta5)*sin(teta4))/25 - (2*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3))/25 - (2*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                           0
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            -(2*cos(teta2 + teta3)*sin(teta4)*sin(teta5))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                         (2*cos(teta2 + teta3)*cos(teta4)*cos(teta5))/25 - (2*sin(teta2 + teta3)*sin(teta5))/25,                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                           0
 ((sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta2)*cos(teta3)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) - cos(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2*(cos(teta2)*sin(teta3) + cos(teta3)*sin(teta2) - cos(teta2)*cos(teta6)^2*sin(teta3) - cos(teta3)*cos(teta6)^2*sin(teta2) + cos(teta2)*cos(teta5)^2*cos(teta6)^2*sin(teta3) + cos(teta3)*cos(teta5)^2*cos(teta6)^2*sin(teta2) - cos(teta2)*cos(teta3)*cos(teta6)*sin(teta4)*sin(teta5)*sin(teta6) + cos(teta6)*sin(teta2)*sin(teta3)*sin(teta4)*sin(teta5)*sin(teta6) + cos(teta2)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6)^2*sin(teta5) - cos(teta4)*cos(teta5)*cos(teta6)^2*sin(teta2)*sin(teta3)*sin(teta5)))/(((sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta3)*sin(teta1)*sin(teta5) - sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2 + (sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta2)*cos(teta3)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) - cos(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2)*(cos(teta4)*sin(teta1)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta1)*sin(teta4) - cos(teta1)*cos(teta2)*cos(teta3)*cos(teta6)*sin(teta5) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta6)*sin(teta2)*sin(teta3)*sin(teta5) - cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta3) - cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta2))^2), (cos(teta6)*(sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta2)*cos(teta3)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) - cos(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2*(cos(teta6)*sin(teta2)*sin(teta3)*sin(teta4) - cos(teta2)*cos(teta3)*cos(teta6)*sin(teta4) + cos(teta2)*sin(teta3)*sin(teta5)*sin(teta6) + cos(teta3)*sin(teta2)*sin(teta5)*sin(teta6) - cos(teta2)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta6) + cos(teta4)*cos(teta5)*sin(teta2)*sin(teta3)*sin(teta6)))/(((sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta3)*sin(teta1)*sin(teta5) - sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2 + (sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta2)*cos(teta3)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) - cos(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2)*(cos(teta4)*sin(teta1)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta1)*sin(teta4) - cos(teta1)*cos(teta2)*cos(teta3)*cos(teta6)*sin(teta5) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta6)*sin(teta2)*sin(teta3)*sin(teta5) - cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta3) - cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta2))^2), ((sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta2)*cos(teta3)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) - cos(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2*(cos(teta2)*cos(teta5)*sin(teta3) + cos(teta3)*cos(teta5)*sin(teta2) + cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/(((sin(teta6)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta5)*sin(teta4) + cos(teta2)*cos(teta3)*sin(teta1)*sin(teta5) - sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3) + cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2)))^2 + (sin(teta6)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)) - cos(teta6)*(cos(teta1)*cos(teta2)*cos(teta3)*sin(teta5) - cos(teta5)*sin(teta1)*sin(teta4) - cos(teta1)*sin(teta2)*sin(teta3)*sin(teta5) + cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3) + cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2)))^2)*(cos(teta4)*sin(teta1)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta1)*sin(teta4) - cos(teta1)*cos(teta2)*cos(teta3)*cos(teta6)*sin(teta5) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)*sin(teta6) + cos(teta1)*cos(teta6)*sin(teta2)*sin(teta3)*sin(teta5) - cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta3) - cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*cos(teta6)*sin(teta2))^2)
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        ((cos(teta2 + teta3)*(cos(teta4)*sin(teta6) + cos(teta5)*cos(teta6)*sin(teta4)))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(1/2) - ((2*cos(teta2 + teta3)*(cos(teta4)*cos(teta6) - cos(teta5)*sin(teta4)*sin(teta6))*(cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6)) - 2*cos(teta2 + teta3)*sin(teta4)*sin(teta5)*(sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5)))*(cos(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta6)*sin(teta5) - cos(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6)))/(2*((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(3/2)))*((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     ((cos(teta6)*(sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5)))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(1/2) + (cos(teta6)*(sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))*(cos(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta6)*sin(teta5) - cos(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6))^2)/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(3/2))*((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        ((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(1/2) + ((cos(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta6)*sin(teta5) - cos(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6))^2*(cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6)))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)^(3/2))*((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                            -(cos(teta2 + teta3)*(cos(teta2 + teta3)*cos(teta6)*sin(teta5) - sin(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6)))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                             ((sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2*(sin(teta6) - ((sin(teta2 + teta3)*sin(teta5) - cos(teta2 + teta3)*cos(teta4)*cos(teta5))*(cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6)))/(sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2),                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                     ((sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))*(cos(teta2 + teta3)*sin(teta4)*sin(teta6) + sin(teta2 + teta3)*cos(teta6)*sin(teta5) - cos(teta2 + teta3)*cos(teta4)*cos(teta5)*cos(teta6)))/((cos(teta2 + teta3)*cos(teta6)*sin(teta4) - sin(teta2 + teta3)*sin(teta5)*sin(teta6) + cos(teta2 + teta3)*cos(teta4)*cos(teta5)*sin(teta6))^2 + (sin(teta2 + teta3)*cos(teta5) + cos(teta2 + teta3)*cos(teta4)*sin(teta5))^2)];         
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                        
J=[Jcolunas1_3 Jcolunas4_6]   


%%
clc
clear all

Q=[0 0 0 pi/4 0 0];
teta1= Q(1);
teta2= Q(2);
teta3= Q(3);
teta4= Q(4);
teta5= Q(5);
teta6= Q(6);




J =[ 
    cos(teta1)/20 - (33*cos(teta1)*sin(teta2))/100 - (67*cos(teta1)*sin(teta2)*sin(teta3))/200 + (2*sin(teta1)*sin(teta4)*sin(teta5))/25 + (67*cos(teta1)*cos(teta2)*cos(teta3))/200 - (7*cos(teta1)*cos(teta2)*sin(teta3))/200 - (7*cos(teta1)*cos(teta3)*sin(teta2))/200 + (2*cos(teta1)*cos(teta2)*cos(teta3)*cos(teta5))/25 - (2*cos(teta1)*cos(teta5)*sin(teta2)*sin(teta3))/25 - (2*cos(teta1)*cos(teta2)*cos(teta4)*sin(teta3)*sin(teta5))/25 - (2*cos(teta1)*cos(teta3)*cos(teta4)*sin(teta2)*sin(teta5))/25,                           -(sin(teta1)*(66*cos(teta2) + 7*cos(teta2)*cos(teta3) + 67*cos(teta2)*sin(teta3) + 67*cos(teta3)*sin(teta2) - 7*sin(teta2)*sin(teta3) + 16*cos(teta2)*cos(teta5)*sin(teta3) + 16*cos(teta3)*cos(teta5)*sin(teta2) + 16*cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - 16*cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/200,                     -(sin(teta1)*(7*cos(teta2)*cos(teta3) + 67*cos(teta2)*sin(teta3) + 67*cos(teta3)*sin(teta2) - 7*sin(teta2)*sin(teta3) + 16*cos(teta2)*cos(teta5)*sin(teta3) + 16*cos(teta3)*cos(teta5)*sin(teta2) + 16*cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - 16*cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/200,  (2*sin(teta5)*(cos(teta2)*sin(teta1)*sin(teta3)*sin(teta4) - cos(teta1)*cos(teta4) + cos(teta3)*sin(teta1)*sin(teta2)*sin(teta4)))/25, (2*sin(teta1)*sin(teta2)*sin(teta3)*sin(teta5))/25 - (2*cos(teta2)*cos(teta3)*sin(teta1)*sin(teta5))/25 - (2*cos(teta1)*cos(teta5)*sin(teta4))/25 - (2*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta3))/25 - (2*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta1)*sin(teta2))/25,                                                                                                                                                                                                                                                                       0
 sin(teta1)/20 - (33*sin(teta1)*sin(teta2))/100 - (7*cos(teta2)*sin(teta1)*sin(teta3))/200 - (7*cos(teta3)*sin(teta1)*sin(teta2))/200 - (2*cos(teta1)*sin(teta4)*sin(teta5))/25 - (67*sin(teta1)*sin(teta2)*sin(teta3))/200 + (67*cos(teta2)*cos(teta3)*sin(teta1))/200 + (2*cos(teta2)*cos(teta3)*cos(teta5)*sin(teta1))/25 - (2*cos(teta5)*sin(teta1)*sin(teta2)*sin(teta3))/25 - (2*cos(teta2)*cos(teta4)*sin(teta1)*sin(teta3)*sin(teta5))/25 - (2*cos(teta3)*cos(teta4)*sin(teta1)*sin(teta2)*sin(teta5))/25,                            (cos(teta1)*(66*cos(teta2) + 7*cos(teta2)*cos(teta3) + 67*cos(teta2)*sin(teta3) + 67*cos(teta3)*sin(teta2) - 7*sin(teta2)*sin(teta3) + 16*cos(teta2)*cos(teta5)*sin(teta3) + 16*cos(teta3)*cos(teta5)*sin(teta2) + 16*cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - 16*cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/200,                      (cos(teta1)*(7*cos(teta2)*cos(teta3) + 67*cos(teta2)*sin(teta3) + 67*cos(teta3)*sin(teta2) - 7*sin(teta2)*sin(teta3) + 16*cos(teta2)*cos(teta5)*sin(teta3) + 16*cos(teta3)*cos(teta5)*sin(teta2) + 16*cos(teta2)*cos(teta3)*cos(teta4)*sin(teta5) - 16*cos(teta4)*sin(teta2)*sin(teta3)*sin(teta5)))/200, -(2*sin(teta5)*(cos(teta4)*sin(teta1) + cos(teta1)*cos(teta2)*sin(teta3)*sin(teta4) + cos(teta1)*cos(teta3)*sin(teta2)*sin(teta4)))/25, (2*cos(teta1)*cos(teta2)*cos(teta3)*sin(teta5))/25 - (2*cos(teta5)*sin(teta1)*sin(teta4))/25 - (2*cos(teta1)*sin(teta2)*sin(teta3)*sin(teta5))/25 + (2*cos(teta1)*cos(teta2)*cos(teta4)*cos(teta5)*sin(teta3))/25 + (2*cos(teta1)*cos(teta3)*cos(teta4)*cos(teta5)*sin(teta2))/25,                                                                                                                                                                                                                                                                       0
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                0, (67*cos(teta2)*cos(teta3))/200 - (33*sin(teta2))/100 - (7*cos(teta2)*sin(teta3))/200 - (7*cos(teta3)*sin(teta2))/200 - (67*sin(teta2)*sin(teta3))/200 - (2*cos(teta5)*sin(teta2)*sin(teta3))/25 + (2*cos(teta2)*cos(teta3)*cos(teta5))/25 - (2*cos(teta2)*cos(teta4)*sin(teta3)*sin(teta5))/25 - (2*cos(teta3)*cos(teta4)*sin(teta2)*sin(teta5))/25, (67*cos(teta2)*cos(teta3))/200 - (7*cos(teta2)*sin(teta3))/200 - (7*cos(teta3)*sin(teta2))/200 - (67*sin(teta2)*sin(teta3))/200 - (2*cos(teta5)*sin(teta2)*sin(teta3))/25 + (2*cos(teta2)*cos(teta3)*cos(teta5))/25 - (2*cos(teta2)*cos(teta4)*sin(teta3)*sin(teta5))/25 - (2*cos(teta3)*cos(teta4)*sin(teta2)*sin(teta5))/25,                                                                                       -(2*cos(teta2 + teta3)*sin(teta4)*sin(teta5))/25,                                                                                                                                                                                            (2*cos(teta2 + teta3)*cos(teta4)*cos(teta5))/25 - (2*sin(teta2 + teta3)*sin(teta5))/25,                                                                                                                                                                                                                                                                       0
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                0,                                                                                                                                                                                                                                                                                                                                   sin(teta1 - pi/2),                                                                                                                                                                                                                                                                                                             sin(teta1 - pi/2),                                        cos(teta3)*cos(teta1 - pi/2)*sin(teta2 + pi/2) + cos(teta1 - pi/2)*cos(teta2 + pi/2)*sin(teta3),                                                                                                                                       cos(teta4)*sin(teta1 - pi/2) - sin(teta4)*(cos(teta3)*cos(teta1 - pi/2)*cos(teta2 + pi/2) - cos(teta1 - pi/2)*sin(teta3)*sin(teta2 + pi/2)), sin(teta5)*(sin(teta4)*sin(teta1 - pi/2) + cos(teta4)*(cos(teta3)*cos(teta1 - pi/2)*cos(teta2 + pi/2) - cos(teta1 - pi/2)*sin(teta3)*sin(teta2 + pi/2))) + cos(teta5)*(cos(teta3)*cos(teta1 - pi/2)*sin(teta2 + pi/2) + cos(teta1 - pi/2)*cos(teta2 + pi/2)*sin(teta3))
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                0,                                                                                                                                                                                                                                                                                                                                  -cos(teta1 - pi/2),                                                                                                                                                                                                                                                                                                            -cos(teta1 - pi/2),                                        cos(teta3)*sin(teta1 - pi/2)*sin(teta2 + pi/2) + cos(teta2 + pi/2)*sin(teta3)*sin(teta1 - pi/2),                                                                                                                                     - cos(teta4)*cos(teta1 - pi/2) - sin(teta4)*(cos(teta3)*cos(teta2 + pi/2)*sin(teta1 - pi/2) - sin(teta3)*sin(teta1 - pi/2)*sin(teta2 + pi/2)), cos(teta5)*(cos(teta3)*sin(teta1 - pi/2)*sin(teta2 + pi/2) + cos(teta2 + pi/2)*sin(teta3)*sin(teta1 - pi/2)) - sin(teta5)*(cos(teta1 - pi/2)*sin(teta4) - cos(teta4)*(cos(teta3)*cos(teta2 + pi/2)*sin(teta1 - pi/2) - sin(teta3)*sin(teta1 - pi/2)*sin(teta2 + pi/2)))
                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                                1,                                                                                                                                                                                                                                                                                                                                                   0,                                                                                                                                                                                                                                                                                                                             0,                                                                            sin(teta3)*sin(teta2 + pi/2) - cos(teta3)*cos(teta2 + pi/2),                                                                                                                                                                                                         -sin(teta4)*(cos(teta3)*sin(teta2 + pi/2) + cos(teta2 + pi/2)*sin(teta3)),                                                                                                          cos(teta4)*sin(teta5)*(cos(teta3)*sin(teta2 + pi/2) + cos(teta2 + pi/2)*sin(teta3)) - cos(teta5)*(cos(teta3)*cos(teta2 + pi/2) - sin(teta3)*sin(teta2 + pi/2))];
%% LAST ONE

addpath ./lib
addpath ./urdf
clc
clear all
%close all


%----------Matriz DH Manipulador FANUC--------------------
% posição incial e final de juntas
 %tp d_p 1 2 3 4 5 6
Qi=[0 0 0 0 0 0]';
Qf=[pi 0 0 0 0 0]';
N=400;
MQ=LinspaceVect(Qi, Qf, N); %pontos de passagem das juntas

teta_p=0;
d_p=0;
teta1=0;
teta2=0;
teta3=0;
teta4=0;
teta5=0;
teta6=0;
%distancia á origem = 0.330;
a1=0.050;
a2=0.330;
a3=0.035;
d4=0.335;
d6=0.080;



z_platform= 0; %altura da platform

%matriz DH
DH=[teta1-pi/2 a1 0 pi/2
    teta2+pi/2 a2 0 0
    teta3 a3 0 pi/2
    teta4 0 d4 -pi/2
    teta5 0 0 pi/2
    teta6 0 d6 0];


%posição inicial do ee
MDH=GenerateMultiDH(DH,MQ);
AAA=CalculateRobotMotion(MDH);

AA=AAA(:,:,:,1);
%AA = Tlinks(DH);
T_ee=eye(4);
for i=1:size(AA,3)
    T_ee=T_ee*AA(:,:,i);
end
X_ee_i=T_ee(1,4);
Y_ee_i=T_ee(2,4);
Z_ee_i=T_ee(3,4);
[phi_ee_i,teta_ee_i,psi_ee_i]=invRPY(T_ee);

%plot3(X_ee_i,Y_ee_i,Z_ee_i,'*g')


% Representação do sistema
% robot_fanuc=importrobot('lrmate200id_macro.xacro');
% axes = show(robot_fanuc); hold on
% axes.CameraPositionMode= 'auto';

% [Ps,Fs]=seixos3(0.3);
% h1=patch('Vertices',Ps(1:3,:)','Faces',Fs,'FaceColor','k'); %%desenha origem

figure(2)
axis equal, axis(0.8*[-1 4 -1 4 -1 2])
hold on, grid on
xlabel('x'), ylabel('y'), zlabel('z');


AA=AAA(:,:,:,1);


Org=LinkOrigins(AA);
[P,F]=seixos3(0.1);

h=DrawLinks(Org);
c=['r' 'g' 'b' 'y' 'm' 'r' 'g' 'b'];
H=DrawFrames(AA,P,F,c);

% Teste do jacobiano
dq=(Qf-Qi)/N;
Dr=zeros(6,N);
Dr(1,1)=(X_ee_i);
Dr(2,1)=(Y_ee_i);
Dr(3,1)=(z_platform+Z_ee_i);

Q=Qi; %instante incial
for n=2:N
    J=Jacobian_manipulator(Q);
    dr=J*dq;
    Q=Q+dq;
    Dr(:,n)=Dr(:,n-1) + dr; %incromentar
end
%plot dos pontos calculados pelos jacobiano
plot3(Dr(1,:),Dr(2,:),Dr(3,:),'*g'), hold on

sd=0.1; %tempo
%while 1
AnimateRobot(H,AAA,P,h,sd); 