docking_different_angle.m

clc;
close all;
clear all;
%% Import data and time conversion

% data 1: 

Pose_Matrix_data_1 = csvread('Pose_left.txt',1,0);
Vel_Matrix_data_1 = csvread('Velocity_left.txt',1,0);

% time in position
TimeP_ros_data_1 = Pose_Matrix_data_1(:,1); % ros time, needs to be converted to sec...

% duriation =         end_time        -     start_time
  durationP_data_1 = (TimeP_ros_data_1(size(TimeP_ros_data_1,1),:) - TimeP_ros_data_1(1,:))*10^(-9); 
  
t_P_sec_data_1 = 0:durationP_data_1/size(TimeP_ros_data_1,1):durationP_data_1;
t_P_sec_data_1(:,size(t_P_sec_data_1,2)) = [];

% time in vel
TimeV_ros_data_1 = Vel_Matrix_data_1(:,1); % ros time, needs to be converted to sec...

% duriation =         end_time        -     start_time
  durationV_data_1 = (TimeV_ros_data_1(size(TimeV_ros_data_1,1),:) - TimeV_ros_data_1(1,:))*10^(-9);

t_V_sec_data_1 = 0:durationV_data_1/size(TimeV_ros_data_1,1):durationV_data_1;
t_V_sec_data_1(:,size(t_V_sec_data_1,2)) = [];

% -----------------------------------------------------------------------------------------------------
% data 2: 

Pose_Matrix_data_2 = csvread('Pose_center.txt',1,0);
Vel_Matrix_data_2 = csvread('Velocity_center.txt',1,0);


% time in position
TimeP_ros_data_2 = Pose_Matrix_data_2(:,1); % ros time, needs to be converted to sec...

% duriation =         end_time        -     start_time
  durationP_data_2 = (TimeP_ros_data_2(size(TimeP_ros_data_2,1),:) - TimeP_ros_data_2(1,:))*10^(-9); 
  
t_P_sec_data_2 = 0:durationP_data_2/size(TimeP_ros_data_2,1):durationP_data_2;
t_P_sec_data_2(:,size(t_P_sec_data_2,2)) = [];

% time in vel
TimeV_ros_data_2 = Vel_Matrix_data_2(:,1); % ros time, needs to be converted to sec...

% duriation =         end_time        -     start_time
  durationV_data_2 = (TimeV_ros_data_2(size(TimeV_ros_data_2,1),:) - TimeV_ros_data_2(1,:))*10^(-9);

t_V_sec_data_2 = 0:durationV_data_2/size(TimeV_ros_data_2,1):durationV_data_2;
t_V_sec_data_2(:,size(t_V_sec_data_2,2)) = [];

% -----------------------------------------------------------------------------------------------------
% data 3: 
Pose_Matrix_data_3 = csvread('Pose_right.txt',1,0);
Vel_Matrix_data_3 = csvread('Velocity_right.txt',1,0);


% time in position
TimeP_ros_data_3 = Pose_Matrix_data_3(:,1); % ros time, needs to be converted to sec...

% duriation =         end_time        -     start_time
  durationP_data_3 = (TimeP_ros_data_3(size(TimeP_ros_data_3,1),:) - TimeP_ros_data_3(1,:))*10^(-9); 
  
t_P_sec_data_3 = 0:durationP_data_3/size(TimeP_ros_data_3,1):durationP_data_3;
t_P_sec_data_3(:,size(t_P_sec_data_3,2)) = [];

% time in vel
TimeV_ros_data_3 = Vel_Matrix_data_3(:,1); % ros time, needs to be converted to sec...

% duriation =         end_time        -     start_time
  durationV_data_3 = (TimeV_ros_data_3(size(TimeV_ros_data_3,1),:) - TimeV_ros_data_3(1,:))*10^(-9);

t_V_sec_data_3 = 0:durationV_data_3/size(TimeV_ros_data_3,1):durationV_data_3;
t_V_sec_data_3(:,size(t_V_sec_data_3,2)) = [];


%% Pose estimation

%data 1:
% when using marker pose ,,,,
Pose_X_data_1 = Pose_Matrix_data_1(:,4);
Pose_Y_data_1 = Pose_Matrix_data_1(:,3);
Theta_data_1 = Pose_Matrix_data_1(:,7);


% Extracting reference values when the robot is manually docked!
ref_X_data_1 = Pose_X_data_1(size(Pose_X_data_1,1));
ref_Y_data_1 = Pose_Y_data_1(size(Pose_Y_data_1,1));
ref_Theta_data_1 = Theta_data_1(size(Theta_data_1,1));
ref_Pose_data_1 =[ref_X_data_1;ref_Y_data_1];

%data 2:
% when using marker pose ,,,,
Pose_X_data_2 = Pose_Matrix_data_2(:,4);
Pose_Y_data_2 = Pose_Matrix_data_2(:,3);
Theta_data_2 = Pose_Matrix_data_2(:,7);


% Extracting reference values when the robot is manually docked!
ref_X_data_2 = Pose_X_data_2(size(Pose_X_data_2,1));
ref_Y_data_2 = Pose_Y_data_2(size(Pose_Y_data_2,1));
ref_Theta_data_2 = Theta_data_2(size(Theta_data_2,1));
ref_Pose_data_2 =[ref_X_data_2;ref_Y_data_2];

%data 3:
% when using marker pose ,,,,
Pose_X_data_3 = Pose_Matrix_data_3(:,4);
Pose_Y_data_3 = Pose_Matrix_data_3(:,3);
Theta_data_3 = Pose_Matrix_data_3(:,7);

% Extracting reference values when the robot is manually docked!
ref_X_data_3 = Pose_X_data_3(size(Pose_X_data_3,1));
ref_Y_data_3 = Pose_Y_data_3(size(Pose_Y_data_3,1));
ref_Theta_data_3 = Theta_data_3(size(Theta_data_3,1));
ref_Pose_data_3 =[ref_X_data_3;ref_Y_data_3];

% Reference Circle 
thresh_X = .0013;
theta = 0:.001:2*pi;
% needs to be adjusted manually if docking platform is replaced!
ref_x = -.2025;
ref_y = -.0185;
x_circle = thresh_X*cos(theta) + ref_x; % ref_X needs to be recorded
y_circle = thresh_X*sin(theta) + ref_y; % ref_Y needs to be recorded


%% Velocity estimation

% data 1:
Vel_X_data_1 = Vel_Matrix_data_1(:,2);
Vel_Y_data_1 = Vel_Matrix_data_1(:,3);
Omega_Z_data_1 = Vel_Matrix_data_1(:,7);

% data 2:
Vel_X_data_2 = Vel_Matrix_data_2(:,2);
Vel_Y_data_2 = Vel_Matrix_data_2(:,3);
Omega_Z_data_2 = Vel_Matrix_data_2(:,7);

% data 3:
Vel_X_data_3 = Vel_Matrix_data_3(:,2);
Vel_Y_data_3 = abs(Vel_Matrix_data_3(:,3));
Omega_Z_data_3 = Vel_Matrix_data_3(:,7);

%% Plots

% marker position
figure;
set(gcf,'color','white');

subplot(321);
plot(t_P_sec_data_1,Pose_X_data_1,'LineWidth',3);
hold on
plot(t_P_sec_data_2,Pose_X_data_2,'r','LineWidth',1.7);
% hold on
% plot(t_P_sec_data_3,Pose_X_data_3,'g','LineWidth',1.6);
% ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',20);
% title('a) ${x_{mar}}$','interpreter','latex','FontSize',20);
% set(gca,'fontsize',20);

subplot(323);
plot(t_P_sec_data_1,Pose_Y_data_1,'LineWidth',3);
hold on
plot(t_P_sec_data_2,Pose_Y_data_2,'r','LineWidth',1.7);
% hold on
% plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',1.6);
% ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',20);
% title('b) ${y_{mar}}$','interpreter','latex','FontSize',20);
% set(gca,'fontsize',20);

subplot(325);
plot(t_P_sec_data_1,Theta_data_1,'LineWidth',3);
hold on
plot(t_P_sec_data_2,Theta_data_2,'r','LineWidth',1.7);
% hold on
% plot(t_P_sec_data_3,Theta_data_3,'g','LineWidth',1.6);

% title('c) ${\theta_{mar}}$','interpreter','latex','FontSize',20);
% ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',20);
% xlabel('$ time$ [s]','interpreter','latex','FontSize',20);
% set(gca,'fontsize',20);

% % control signals
% figure;
% set(gcf,'color','white');

subplot(322);

plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',3);
hold on
plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.7);
% hold on
% plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',1.6);
% 
% title('d) $\dot{x}_{Rob}$','interpreter','latex','FontSize',20);
% ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);

l2 = legend('${Left}$', '${Right}$');
set(l2,'interpreter','latex','FontSize',14);
legend('boxoff')
axis ([0 40 -.01 .18]);
% set(gca,'fontsize',20);

subplot(324);
plot(t_V_sec_data_1,Vel_Y_data_1,'LineWidth',3);
hold on
plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.7);
% hold on
% plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',1.6);
% ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
% title('e) $\dot{y}_{Rob}$','interpreter','latex','FontSize',20);
% set(gca,'fontsize',20);

subplot(326);
plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',3);
hold on
plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.7);
% hold on
% plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.6);
% xlabel('$ time$ [s]','interpreter','latex','FontSize',20);
% ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',20);
% title('f) $\dot{\theta}_{Rob}$','interpreter','latex','FontSize',20);
% set(gca,'fontsize',20);

% Trajectory 

figure;
set(gcf,'color','white');

subplot(131);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
% hold on
% quiver(Pose_Y_data_1,Pose_X_data_1,gradient(Pose_Y_data_1),gradient(Pose_X_data_1),'r--','LineWidth',.01);

hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
% hold on
% quiver(Pose_Y_data_2,Pose_X_data_2,gradient(Pose_Y_data_2),gradient(Pose_X_data_2),'m--','LineWidth',.01);
% % 
% hold on
% plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
% hold on
% quiver(Pose_Y_data_3,Pose_X_data_3,gradient(Pose_Y_data_3),gradient(Pose_X_data_3),'k--','LineWidth',.01);

hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);

% axis([-.4 .4 .18 1.4]);
% title('${a) Approach  + SM  + Target}$','interpreter','latex','FontSize',20);
% ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',20);
% set(gca,'fontsize',20);

subplot(132);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',1.6);
hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',1.6);
% hold on
% plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',1.6);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);

% title('$ {b) SM + Target}$','interpreter','latex','FontSize',20);
% xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',20);
axis([-.03 0.03 -.29 -.191]);
% set(gca,'fontsize',20);

subplot(133);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
% hold on
% plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);

% title('$ {c) Target}$','interpreter','latex','FontSize',20);
axis([-.0202 -.017 -.2054 -.199]);
% set(gca,'fontsize',20);

l = legend('${Left}$', '${Right}$','Target');
set(l,'interpreter','latex','FontSize',14);
legend('boxoff');


MaxOvershoot_1 = -ref_y + min(Pose_Y_data_1)
MaxOvershoot_2 = -ref_y + min(Pose_Y_data_2)
MaxOvershoot_3 = -ref_y + min(Pose_Y_data_3)