%---------------cinematica diferencial integrada ROBONUC-----------------
%Tiago De Almeida Tavares 77001
%com graus de liberdade em x y e teta + manipulador (aprox2)
addpath ./lib
addpath ./urdf
clc
clear all
%close all


%----------Matriz DH R+P+Manipulador FANUC--------------------
% posição incial e final de juntas
 %tp d_p 1 2 3 4 5 6
Qi=[0 0 0 0 0 pi/4 0 0]';
Qf=[pi/2 1 0 0 0 0 pi/2 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=[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];


%posição inicial do ee
t=[0 1 0 0 0 0 0 0]';
MDH=GenerateMultiDH(DH,MQ,t);
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);

%% Move parametrizado
tic
%posicao incial e final do ee
Pos_ee_i=[X_ee_i Y_ee_i Z_ee_i phi_ee_i teta_ee_i psi_ee_i]';

dt=5/N;


Q=Qi;
MQ=zeros(8,N);

for n=1:N
    
    manipulabilidade_var=Manipulabilidade(2,Q);
    manipulabilidade(:,n)=[n;manipulabilidade_var];
    
    if imag(manipulabilidade_var)~= 0
        manipulabilidade_var=0;
    end
    
    manipulabilidade_withoutCOmplex(:,n)=[n;manipulabilidade_var];
    MQ(:,n)=Q;
    Jinv=JacobianInv_robonuc_aprox2(Q);
    
    %Posição: dx dy dz
    dx=0;
    dy=1*dt;
    dz=(-(32*sin(16*n*dt))/25)*dt;
    
    %Orientação: da, db, dc
    da=0;
    db=0;
    dc=0;
    
    
    dr=[dx,dy,dz,da,db,dc]';
    
    dq=Jinv*dr;
    Q=Q+dq;
    AllDq(:,n)=dq; 
end

MDH=GenerateMultiDH(DH,MQ,t);
AAA=CalculateRobotMotion(MDH);

% qff=MQ(:,end);
% jacobiano_qf=Jacobian_robonuc_aprox2(qff);
% jacobiano_qf_t=jacobiano_qf.';
% Manipulabilidade=sqrt(det(jacobiano_qf*jacobiano_qf_t))
sd=0.1; %tempo
%while 1
AnimateRobot(H,AAA,P,h,sd); 
%end

toc
figure(4)
axis([0 n+5, 0 1.6])
hold on, grid on
xlabel('Pontos'), ylabel('Manipulabilidade')
plot(manipulabilidade(1,:),manipulabilidade(2,:),'*')


figure(5)
axis([0 n+5, 0 1.6])
hold on, grid on
xlabel('Pontos'), ylabel('Manipulabilidade')
plot(manipulabilidade_withoutCOmplex(1,:),manipulabilidade_withoutCOmplex(2,:),'*')


%% Qi e Qf obtido atraves do Jinv (MOVE LINEAR)
tic
%posicao incial e final do ee
Pos_ee_i=[X_ee_i Y_ee_i Z_ee_i phi_ee_i teta_ee_i psi_ee_i]';
Pos_ee_f=[X_ee_i+2 Y_ee_i+2 Z_ee_i+0.3 phi_ee_i teta_ee_i psi_ee_i]';

%incemento usado
dr=(Pos_ee_f-Pos_ee_i)/N;


Q=Qi;
MQ=zeros(8,N);

for n=1:N
    manipulabilidade(:,n)=[n;Manipulabilidade(2,Q)];
    q_manipulador=Q(3:end);
    manipulabilidade_manipulador(:,n)=[n;Manipulabilidade_manipulador(1,q_manipulador)];
    MQ(:,n)=Q;
    Jinv=JacobianInv_robonuc_aprox2(Q);
    dq=Jinv*dr;
    Q=Q+dq;
    AllDq(:,n)=dq; 
end

MDH=GenerateMultiDH(DH,MQ,t);
AAA=CalculateRobotMotion(MDH);

% qff=MQ(:,end);
% jacobiano_qf=Jacobian_robonuc_aprox2(qff);
% jacobiano_qf_t=jacobiano_qf.';
% Manipulabilidade=sqrt(det(jacobiano_qf*jacobiano_qf_t))
sd=0.1; %tempo
%while 1
AnimateRobot(H,AAA,P,h,sd); 
%end

toc
figure(4)
axis([0 n+5, 0 0.1])
hold on, grid on
xlabel('Pontos'), ylabel('Manipulabilidade_manipulador')
plot(manipulabilidade_manipulador(1,:),manipulabilidade_manipulador(2,:),'*')


figure(5)
axis([0 n+5, 0 1.6])
hold on, grid on
xlabel('Pontos'), ylabel('Manipulabilidade')
plot(manipulabilidade(1,:),manipulabilidade(2,:),'*')

save('linear_1_ab2')
%% 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_robonuc_aprox2(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

%% Animação do conjunto

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