%GET images for pdf
%Tiago De Almeida Tavares 77001
%---------------cinematica diferencial integrada ROBONUC-----------------
%% ------------------------ ABORDAGEM 1 posicao inicial----------
% com graus de liberdade em x y e teta + manipulador (Aprox1)

addpath ./lib
addpath ./urdf
clc
clear all
close all
% posição incial e final de juntas
   %x y t 1 2 3 4 5 6
Qi=[0 0 0 0 0 0 0 0 0]';
Qf=[1 1 0 0 0 0 0 0 0]';
N=15;

%----------------
Qi_fanuc=Qi(4:9);
Qf_fanuc=Qf(4:9);
MQ_manipulator=LinspaceVect(Qi_fanuc, Qf_fanuc, N); %pontos de passagem das juntas

Qi_p=Qi(1:3);
Qf_p=Qf(1:3);
MQ_platform=LinspaceVect(Qi_p,Qf_p,N);


%----------Matriz DH Manipulador FANUC--------------------
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 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_manipulator);
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);

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

MDH=GenerateMultiDH(DH,MQ_manipulator);
AAA=CalculateRobotMotion(MDH);

% [Ps,Fs]=seixos3(0.3);
% h1=patch('Vertices',Ps(1:3,:)','Faces',Fs,'FaceColor','k'); %%desenha origem
axis equal, axis(1*[-0.7 0.7 -0.3 0.3 -0.05 0.5])
hold on, grid on
xlabel('x'), ylabel('y'), zlabel('z');

%desenhar o robot na primeira posicao
x_p_i=MQ_platform(1,1);
y_p_i=MQ_platform(2,1);
teta_p_i=MQ_platform(3,1);
w_T_basePlat=[cos(teta_p_i) -sin(teta_p_i) 0  x_p_i
              sin(teta_p_i) cos(teta_p_i)  0  y_p_i
                    0            0         1  z_platform

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

for ww=1%:size(AAA,3)
    AA(:,:,ww)=w_T_basePlat*AA(:,:,ww);
end

comprimento=0.75;
largura=0.5;
rectangle('Position', [x_p_i-comprimento*0.8 y_p_i-largura/2 comprimento largura]);

view(23,18)
Org=LinkOrigins(AA);
%Org(:,1)=[x_p_i y_p_i z_platform]';

[P,F]=seixos3(0.05);

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

%% ------------------------ ABORDAGEM 2 posicao inicial----------
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 0 0 0]';
Qf=[0 0 0 0 0 0 0 0]';
N=15;
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(1*[-0.7 0.7 -0.3 0.3 -0.05 0.5])
hold on, grid on
xlabel('x'), ylabel('y'), zlabel('z');


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

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

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

x_p_i=0;
y_p_i=0;

comprimento=0.75;
largura=0.5;
rectangle('Position', [x_p_i-comprimento*0.8 y_p_i-largura/2 comprimento largura]);
view(23,18)
