%% sym -> GET JACOBIAN of integrated system (HH_aprox1)
addpath ./lib
clc
clear all
close all

%------------------------------Manipulator--------------------
%distancia á origem = 0.330;
a1=0.050;
a2=0.330;
a3=0.035;
d4=0.335;
d6=0.080;

syms teta1 teta2 teta3 teta4 teta5 teta6 real

% teta1= sym('teta1');
% teta2= sym('teta2');
% teta3= sym('teta3');
% teta4= sym('teta4');
% teta5= sym('teta5');
% teta6= sym('teta6');

DH=[teta1 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];


[AA] = Tlinks(DH);
T_ee=eye(4);
for i=1:size(AA,3)
    T_ee=T_ee*AA(:,:,i);
end

%Transformada plat_base to end effector
pb_T_ee=T_ee;

X_b1=pb_T_ee(1,4);%postions..
Y_b1=pb_T_ee(2,4);
Z_b1=pb_T_ee(3,4);
%angles.. phi, teta, psi 
[A_b1, B_b1, C_b1]=invRPY(pb_T_ee);



%-----------------------------------------------------------
%=====================Sistema total=========================
%posição do end effector
X_ee=X_b1;
Y_ee= Y_b1;
Z_ee= Z_b1;
A_ee=A_b1;
B_ee=B_b1;
C_ee=C_b1;

%----------------jacobiano------------------
J= jacobian([X_ee; Y_ee; Z_ee; A_ee; B_ee; C_ee], [teta1 teta2 teta3 teta4 teta5 teta6]);

J=simplify(J);

save('jacobiano_mat_aprox1_manipulator.mat','J');

%% GET jacobian only manipulator abordagem 2

addpath ./lib
clc
clear all
close all

%------------------------------Manipulator--------------------
%distancia á origem = 0.330;
a1=0.050;
a2=0.330;
a3=0.035;
d4=0.335;
d6=0.080;

syms teta1 teta2 teta3 teta4 teta5 teta6 real

% teta1= sym('teta1');
% teta2= sym('teta2');
% teta3= sym('teta3');
% teta4= sym('teta4');
% teta5= sym('teta5');
% teta6= sym('teta6');
% 
% teta_p= sym('teta_p');
% d_p= sym('d_p');

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];


[AA] = Tlinks(DH);
T_ee=eye(4);
for i=1:size(AA,3)
    T_ee=T_ee*AA(:,:,i);
end

%Transformada plat_base to end effector
pb_T_ee=T_ee;

X_b1=pb_T_ee(1,4);%postions..
Y_b1=pb_T_ee(2,4);
Z_b1=pb_T_ee(3,4);
%angles.. phi, teta, psi 
[A_b1, B_b1, C_b1]=invRPY(pb_T_ee);



%-----------------------------------------------------------
%=====================Sistema total=========================

X_ee= X_b1;
Y_ee= Y_b1;
Z_ee= Z_b1;
A_ee=A_b1;
B_ee=B_b1;
C_ee=C_b1;

%jacobiano
J= jacobian([X_ee; Y_ee; Z_ee; A_ee; B_ee; C_ee], [teta1 teta2 teta3 teta4 teta5 teta6]);
J=simplify(J);

save('jacobiano_mat_aprox2_manipulador.mat','J');



%g = matlabFunction(J,'vars',{'X_p','Y_p','teta_p','teta1','teta2','teta3','teta4','teta5','teta6'});


%% sym -> GET JACOBIAN of manipulator (ab2. tentativa2)
addpath ./lib
clc
clear all
close all

%------------------------------Manipulator--------------------
%distancia á origem = 0.330;
a1=0.050;
a2=0.330;
a3=0.035;
d4=0.335;
d6=0.080;

syms teta1 teta2 teta3 teta4 teta5 teta6 real

teta_p=0
d_p=0
% teta1= sym('teta1');
% teta2= sym('teta2');
% teta3= sym('teta3');
% teta4= sym('teta4');
% teta5= sym('teta5');
% teta6= sym('teta6');
% 
% teta_p= sym('teta_p');
% d_p= sym('d_p');

DH=[teta_p+pi/2 0 0 pi/2
     0    0  d_p -pi/2
    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];


[AA] = Tlinks(DH);
T_ee=eye(4);
for i=1:size(AA,3)
    T_ee=T_ee*AA(:,:,i);
end

%Transformada plat_base to end effector
pb_T_ee=T_ee;

X_b1=pb_T_ee(1,4);%postions..
Y_b1=pb_T_ee(2,4);
Z_b1=pb_T_ee(3,4);
%angles.. phi, teta, psi 
[A_b1, B_b1, C_b1]=invRPY(pb_T_ee);



%-----------------------------------------------------------
%=====================Sistema total=========================

X_ee= X_b1;
Y_ee= Y_b1;
Z_ee= Z_b1;
A_ee=A_b1;
B_ee=B_b1;
C_ee=C_b1;

%jacobiano
J= jacobian([X_ee; Y_ee; Z_ee; A_ee; B_ee; C_ee], [teta1 teta2 teta3 teta4 teta5 teta6]);
J=simplify(J);

%save('jacobiano_mat_aprox2_manipulator_v2.mat','J');



%g = matlabFunction(J,'vars',{'X_p','Y_p','teta_p','teta1','teta2','teta3','teta4','teta5','teta6'});




%% Get jacobiano (ab2 tentativa invicta_porto)


addpath ./lib
clc
clear all
close all

%------------------------------Manipulator--------------------
%distancia á origem = 0.330;
a1=0.050;
a2=0.330;
a3=0.035;
d4=0.335;
d6=0.080;

syms teta1 teta2 teta3 teta4 teta5 teta6 real

% teta1= sym('teta1');
% teta2= sym('teta2');
% teta3= sym('teta3');
% teta4= sym('teta4');
% teta5= sym('teta5');
% teta6= sym('teta6');
% 
% teta_p= sym('teta_p');
% d_p= sym('d_p');

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];


[AA] = Tlinks(DH);
T_ee=eye(4);
for i=1:size(AA,3)
    T_ee=T_ee*AA(:,:,i);
end

%Transformada plat_base to end effector
pb_T_ee=T_ee;

X_b1=pb_T_ee(1,4);%postions..
Y_b1=pb_T_ee(2,4);
Z_b1=pb_T_ee(3,4);

%angles.. phi, teta, psi 
%[A_b1, B_b1, C_b1]=invRPY(pb_T_ee);



%-----------------------------------------------------------
%=====================Sistema total=========================

X_ee= X_b1;
Y_ee= Y_b1;
Z_ee= Z_b1;
% A_ee=A_b1;
% B_ee=B_b1;
% C_ee=C_b1;

%jacobiano
Jv= jacobian([X_ee; Y_ee; Z_ee], [teta1 teta2 teta3 teta4 teta5 teta6]);
Jv=simplify(Jv);

k=[0 0 1]';
z0=k;
z1=AA(1:3,1:3,1)*k;
z2=AA(1:3,1:3,1)*AA(1:3,1:3,2)*k;
z3=AA(1:3,1:3,1)*AA(1:3,1:3,2)*AA(1:3,1:3,3)*k;
z4=AA(1:3,1:3,1)*AA(1:3,1:3,2)*AA(1:3,1:3,3)*AA(1:3,1:3,4)*k;
z5=AA(1:3,1:3,1)*AA(1:3,1:3,2)*AA(1:3,1:3,3)*AA(1:3,1:3,4)*AA(1:3,1:3,5)*k;

Jw=[z0 z1 z2 z3 z4 z5];

J=[Jv; Jw]