Skip to content
Snippets Groups Projects
Commit 0f662a83 authored by Farid Alijani's avatar Farid Alijani
Browse files

Report caption added

parent dcb003a6
No related branches found
No related tags found
No related merge requests found
Show whitespace changes
Inline Side-by-side
Files_4_thesis/Different_angle/docking_different_angle.asv deleted 100644 → 0
+ 0
− 487
View file @ dcb003a6
clc;
close all;
clear all;
%% Import data and time conversion
% data 1: 556 ros messages for geometry_msgs/Twist
% 557 ros messages for geometry_msgs/PoseStamped
% y-axis gains:
% (010816_1718) best!!!
% P = .4; I = 0; D = 0;
% y-axis gains:
% (280716_2024) better!!!
% P = .44; I = 0; D = 0;
% theta-axis gains:
% P = .08; I = 0; D = 0;
% y-axis gains:
% (280716_2002)
% P = .46; I = 0; D = 0;
% theta-axis gains:
% P = .08; I = 0; D = 0;
Pose_Matrix_data_1 = csvread('Pose_negative_angle.txt',1,0);
Vel_Matrix_data_1 = csvread('Velocity_negative_angle.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: 588 ros messages for geometry_msgs/Twist
% 589 ros messages for geometry_msgs/PoseStamped
% y-axis gains:
% P = .5; I = 0.001; D = 0.03;
% theta-axis gains:
% P = .28; I = 0.1; D = 0.15;
Pose_Matrix_data_2 = csvread('Pose_zero_angle.txt',1,0);
Vel_Matrix_data_2 = csvread('Velocity_zero_angle.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: 538 ros messages for geometry_msgs/Twist
% 539 ros messages for geometry_msgs/PoseStamped
% y-axis gains:
% P = .44; I = .0005; D = 0.15;
% theta-axis gains:
% P = .08; I = 0; D = 0;
Pose_Matrix_data_3 = csvread('Pose_positive_angle.txt',1,0);
Vel_Matrix_data_3 = csvread('Velocity_positive_angle.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(:,5);
% when using robot odometry ,,,,
% Pose_X = Pose_Matrix(:,2);
% Pose_Y = Pose_Matrix(:,3);
% Theta = Pose_Matrix(:,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(:,5);
% when using robot odometry ,,,,
% Pose_X = Pose_Matrix(:,2);
% Pose_Y = Pose_Matrix(:,3);
% Theta = Pose_Matrix(:,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(:,5);
% when using robot odometry ,,,,
% Pose_X = Pose_Matrix(:,2);
% Pose_Y = Pose_Matrix(:,3);
% Theta = Pose_Matrix(:,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 = .001;
theta = 0:.001:2*pi;
% needs to be adjusted manually if docking platform is replaced!
ref_x = .20095;
ref_y = .0081;
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 = Vel_Matrix_data_3(:,3);
Omega_Z_data_3 = Vel_Matrix_data_3(:,7);
%% Plots
% for the presenation all pose estimation(x,y,theta) in one plot
% figure;
% plot(t_P_sec,Pose_X,'b',t_P_sec,Pose_Y,'r',t_P_sec,Theta,'g');
%
% title('Marker Pose');
% xlabel('Time [sec]');
% ylabel('Pose');
% legend('X [m]','Y [m]','\theta [rad]');
% grid on
% marker position
figure;
subplot(3,1,1);
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',2.3);
title('Marker Pose Along X_{mar} - Axis');
% set(t,'interpreter','latex');
% xlabel('$ time [sec]$','interpreter','latex');
ylabel('${X_{mar}}$ [m]','interpreter','latex','FontSize',12);
grid on
% axis([0 45 -.01 .2]);
l2 = legend('${P_y = 0.86, I_y = 0, D_y = 0.002}$', '${P_y = 0.66, I_y = 0, D_y = 0.1}$','${P_y = 0.86, I_y = 0, D_y = 0.1}$','${P_y = 0.51, I_y = 0.0005, D_y = 0.05}$');
set(l2,'interpreter','latex','FontSize',11);
subplot(3,1,2);
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',2.3);
title('Marker Pose Along Y_{mar} - Axis');
% xlabel('$ time [sec]$','interpreter','latex');
ylabel('${Y_{mar}}$ [m]','interpreter','latex','FontSize',12);
grid on
% axis([0 46 -.2 .1]);
subplot(3,1,3);
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',2.3);
title('Marker Pose Along \theta_{mar} - Axis');
xlabel('$ time [sec]$','interpreter','latex','FontSize',12);
ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',12);
grid on
% axis([0 46 -.1 .15]);
% control signals
figure;
subplot(3,1,1);
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',2.3);
title('Control Signal Along X_{rob} - Axis');
% xlabel('$ time [sec]$','interpreter','latex');
ylabel('$\dot{X_{rob}}$ [m/s]','interpreter','latex','FontSize',12);
grid on
axis([0 45 -.01 .2]);
l2 = legend('${P_y = 0.86, I_y = 0, D_y = 0.002}$', '${P_y = 0.66, I_y = 0, D_y = 0.1}$','${P_y = 0.86, I_y = 0, D_y = 0.1}$','${P_y = 0.51, I_y = 0.0005, D_y = 0.05}$');
set(l2,'interpreter','latex','FontSize',11);
subplot(3,1,2);
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',2.3);
title('Control Signal Along Y_{rob} - Axis');
% xlabel('$ time [sec]$','interpreter','latex');
ylabel('$\dot{Y_{rob}}$ [m/s]','interpreter','latex','FontSize',12);
grid on
% axis([0 46 -.2 .1]);
subplot(3,1,3);
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',2.3);
title('Control Signal Along \theta_{rob} - Axis');
xlabel('$ time [sec]$','interpreter','latex','FontSize',12);
ylabel('$\dot{\theta_{rob}}$ [rad/s]','interpreter','latex','FontSize',12);
grid on
% axis([0 46 -.1 .15]);
% Trajectory
% figure;
% plot(Pose_Y_data_1,Pose_X_data_1,'b',ref_Y_data_1,ref_X_data_1,'k*');
% hold on
% plot(Pose_Y_data_2,Pose_X_data_2,'r',ref_Y_data_2,ref_X_data_2,'k*');
%
% hold on
% plot(Pose_Y_data_3,Pose_X_data_3,'g',ref_Y_data_3,ref_X_data_3,'k*');
%
%
% for k = Pose_X_data_1(1):Pose_X_data_1(size(Pose_X_data_1,1))
% x_arrow(k) =
%
% end
%
% x_arrow = linspace(Pose_X_data_1(1),Pose_X_data_1(size(Pose_X_data_1,1)),150);
% y_arrow = linspace(Pose_Y_data_1(1),Pose_Y_data_1(size(Pose_Y_data_1,1)),150);
%
% Vx = gradient(x_arrow,.01);
% Vy = gradient(y_arrow,.01);
%
% figure;
% plot(y_arrow,x_arrow,'b--','LineWidth',.5);
% % hold on
% % quiver(y_arrow,x_arrow,Vy,Vx,'r--','LineWidth',1.5);
%
figure;
subplot(1,3,1);
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(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);
title('Entire Area');
xlabel('${Y}$ [m]','interpreter','latex','FontSize',11);
ylabel('${X}$ [m]','interpreter','latex','FontSize',11);
axis([-.1 .4 .18 1.4]);
grid on
subplot(1,3,2);
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(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',1.6);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);
title('SM zone');
xlabel('${Y}$ [m]','interpreter','latex','FontSize',11);
% ylabel('${X}$ [m]','interpreter','latex','FontSize',11);
axis([-.005 0.045 .195 .32]);
grid on
subplot(1,3,3);
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(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);
title('Target');
xlabel('${Y}$ [m]','interpreter','latex','FontSize',11);
% ylabel('${X}$ [m]','interpreter','latex','FontSize',11);
axis([0.0065 .0095 .1995 .2055]);
grid on
l = legend('${P_y = 0.86, I_y = 0, D_y = 0.002}$', '${P_y = 0.66, I_y = 0, D_y = 0.1}$','${P_y = 0.86, I_y = 0, D_y = 0.1}$','${P_y = 0.51, I_y = 0.0005, D_y = 0.05}$' ,'Target');
set(l,'interpreter','latex','FontSize',11);
% sampling time
figure;
subplot(2,2,1);
plot(diff(TimeP_ros_data_3)/1e9,'r','LineWidth',1.4)
hold on
plot (diff(t_P_sec_data_3),'k--','LineWidth',1.8);
% hold on
title({'USB Camera';'Position'});
ylabel('Sampling time [sec]','interpreter','latex','FontSize',11);
axis([0 450 .068 .11]);
l3 = legend('${Time_{ROS}}$','${Time_{Real}} [sec]$','Orientation','horizontal');
set(l3,'interpreter','latex','FontSize',13);
subplot(2,2,2);
plot(diff(TimeP_ros_data_5)/1e9,'g','LineWidth',1.4)
hold on;
plot (diff(t_P_sec_data_5),'b--','LineWidth',1.8);
title({'Android Camera';'Position'});
% ylabel('Sampling time [sec]','interpreter','latex','FontSize',11);
axis([0 234 .068 .3]);
l4 = legend('${Time_{ROS}}$','${Time_{Real}} [sec]$','Orientation','horizontal');
set(l4,'interpreter','latex','FontSize',13);
subplot(2,2,3);
plot(diff(TimeV_ros_data_3)/1e9,'r','LineWidth',1.4);
hold on;
plot (diff(t_V_sec_data_3),'k--','LineWidth',1.8);
% hold on;
title('Velocity');
xlabel('samples','interpreter','latex','FontSize',13);
ylabel('Sampling time [sec]','interpreter','latex','FontSize',13);
axis([0 450 .068 .11]);
subplot(2,2,4);
plot(diff(TimeV_ros_data_5)/1e9,'g','LineWidth',1.4);
hold on;
plot (diff(t_V_sec_data_5),'b--','LineWidth',1.8);
title('Velocity');
xlabel('samples','interpreter','latex','FontSize',13);
% ylabel('Sampling time [sec]','interpreter','latex','FontSize',11);
axis([0 234 .068 .3]);
figure;
subplot(2,1,1);
pa1 = plot(diff(TimeP_ros_data_3)/1e9,'r','LineWidth',2.1);
hold on
pa2 = plot (diff(t_P_sec_data_3),'k--','LineWidth',1.5);
hold on
pa3 = plot(diff(TimeP_ros_data_5)/1e9,'g','LineWidth',1.4);
hold on;
pa4 = plot (diff(t_P_sec_data_5),'b--','LineWidth',1.8);
hold off;
axis([0 234 0 .3]);
title('Position');
ylabel('Sampling time [sec]','interpreter','latex','FontSize',13);
l5 = legend([pa1 pa2],'$USB Camera {Time_{ROS}}$','$ USB Camera {Time_{Real}} [sec]$','Orientation','horizontal');
set(l5,'interpreter','latex','FontSize',12);
% legend('boxoff');
subplot(2,1,2);
p1 = plot(diff(TimeV_ros_data_3)/1e9,'r','LineWidth',2.1);
hold on;
p2 = plot (diff(t_V_sec_data_3),'k--','LineWidth',1.5);
hold on;
p3 = plot(diff(TimeV_ros_data_5)/1e9,'g','LineWidth',1.4);
hold on;
p4 = plot (diff(t_V_sec_data_5),'b--','LineWidth',1.8);
hold off;
axis([0 234 0 .3]);
title('Velocity');
ylabel('Sampling time [sec]','interpreter','latex','FontSize',13);
xlabel('Sample','interpreter','latex','FontSize',13);
l6 = legend([p3 p4],'$ Android Camera {Time_{ROS}}$','$ Android Camera {Time_{Real}} [sec]$','Orientation','horizontal');
set(l6,'interpreter','latex','FontSize',12);
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)
MaxOvershoot_4 = -ref_y + min(Pose_Y_data_4)
% Pose diff
% figure;
% subplot(3,1,1);
% plot(diff(Pose_X_data_1));
% title('X-Pose diff');
% xlabel('samples');
% ylabel('X difference [m]');
% grid on
%
% subplot(3,1,2);
% plot(diff(Pose_X_data_1));
% title('Y-Pose diff');
% xlabel('samples');
% ylabel('Y difference [m]');
% grid on
%
% subplot(3,1,3);
% plot(diff(Theta_data_1));
% title('\theta-Pose diff');
% xlabel('samples');
% ylabel('\theta difference [rad]');
% grid on
\ No newline at end of file
Files_4_thesis/Report.docx
+ 0
− 0
View file @ 0f662a83
No preview for this file type
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Please register or to comment