Commit 216b4438 authored by Farid Alijani's avatar Farid Alijani
Browse files

First draft updated

parent adc5f987
......@@ -414,7 +414,7 @@ grid on
% Trajectory
%
% figure;
% plot(Pose_Y_data_1,Pose_X_data_1,'b',ref_Y_data_1,ref_X_data_1,'k*');
% hold on
......@@ -422,7 +422,7 @@ grid on
%
% 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))
......@@ -433,23 +433,35 @@ grid on
% 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);
%
% Vx = gradient(Pose_X_data_1);
% Vy = gradient(Pose_Y_data_1);
% figure;
% plot(y_arrow,x_arrow,'b--','LineWidth',.5);
% % hold on
% % quiver(y_arrow,x_arrow,Vy,Vx,'r--','LineWidth',1.5);
%
% plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',3.5);
% hold on
% quiver(Pose_Y_data_1,Pose_X_data_1,Vy,Vx,'r--','LineWidth',.1);
figure;
subplot(1,3,1);
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',.1);
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',.1);
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',.1);
hold on
plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
hold on
quiver(Pose_Y_data_4,Pose_X_data_4,gradient(Pose_Y_data_4),gradient(Pose_X_data_4),'b--','LineWidth',.1);
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);
......
......@@ -18,14 +18,19 @@ clc;
% grid on
% % axis([-1 3 -1 2.5])
%
x = linspace(0,2*pi,15);
x_prime = gradient(x,.2);
x = linspace(0,2*pi,5);
y = sin(x);
y_prime = gradient(y,.2);
%
% x = linspace(1,10,10);
% y = zeros(1,size(x,2));
x_prime = gradient(x);
y_prime = gradient(y);
figure;
plot(x,y,'r--','LineWidth',1.8)
plot(x,y,'r--','LineWidth',3.8)
hold on
quiver(x,y,x_prime,y_prime,'g','LineWidth',2.2)
grid on
%
% R
......
clc;
close all;
clear all;
upper_angle = 135; % deg.
lower_angle = -135; % deg.
dist_to_obst = .1; % m.
%% docked robot for 5 different experiences...
S_mat = csvread('Scan_Data_5_exp.csv');
indx = S_mat(:,1);
s_exp_1 = S_mat(:,2);
s_exp_2 = S_mat(:,3);
s_exp_3 = S_mat(:,4);
s_exp_4 = S_mat(:,5);
s_exp_5 = S_mat(:,6);
points = size(indx,1); % to calculate the angle...
laser_beam = points -1;
angle = zeros(points,1);
X_exp_1 = zeros(points,1);
X_exp_2 = zeros(points,1);
X_exp_3 = zeros(points,1);
X_exp_4 = zeros(points,1);
X_exp_5 = zeros(points,1);
X_obst = zeros(points,1);
Y_exp_1 = zeros(points,1);
Y_exp_2 = zeros(points,1);
Y_exp_3 = zeros(points,1);
Y_exp_4 = zeros(points,1);
Y_exp_5 = zeros(points,1);
Y_obst = zeros(points,1);
for i = 1:points
angle(i) = (lower_angle + (indx(i)* ((upper_angle - lower_angle)/(laser_beam))))* pi/180;
X_exp_1(i) = s_exp_1(i) * cos(angle(i));
X_exp_2(i) = s_exp_2(i) * cos(angle(i));
X_exp_3(i) = s_exp_3(i) * cos(angle(i));
X_exp_4(i) = s_exp_4(i) * cos(angle(i));
X_exp_5(i) = s_exp_5(i) * cos(angle(i));
X_obst(i) = dist_to_obst * cos(angle(i));
Y_exp_1(i) = s_exp_1(i) * sin(angle(i));
Y_exp_2(i) = s_exp_2(i) * sin(angle(i));
Y_exp_3(i) = s_exp_3(i) * sin(angle(i));
Y_exp_4(i) = s_exp_4(i) * sin(angle(i));
Y_exp_5(i) = s_exp_5(i) * sin(angle(i));
Y_obst(i) = dist_to_obst * sin(angle(i));
end
% theta vs. scanner_value & reference
figure;
subplot(1,2,1);
% plot(angle,s_exp_1,'m--','LineWidth',1.1);
% hold on;
plot(angle,s_exp_2,'m--','LineWidth',1.9);
hold on;
plot(angle,s_exp_3,'b--','LineWidth',1.9);
hold on;
plot(angle,s_exp_4,'c--','LineWidth',1.5);
hold on;
% plot(angle,s_exp_5,'b--','LineWidth',.8);
% hold on;
plot(angle,dist_to_obst,'rx','LineWidth',.05);
title('');
xlabel('${Angle}$ [rad]','interpreter','latex');
ylabel('${Obstacle Distance}$ [m]','interpreter','latex');
axis([-.09 .42 .08 .2]);
% legend('Experiment 1','Experiment 2','Experiment 3','Safety Curve');
subplot(1,2,2);
% plot(X_exp_1,Y_exp_1,'m--','LineWidth',.9);
% hold on
plot(X_exp_2,Y_exp_2,'m--','LineWidth',1.9);
hold on
plot(X_exp_3,Y_exp_3,'b--','LineWidth',1.9);
hold on
plot(X_exp_4,Y_exp_4,'c--','LineWidth',1.5);
hold on
% plot(X_exp_5,Y_exp_5,'b--','LineWidth',.1);
% hold on
plot(X_obst,Y_obst,'r--','LineWidth',.05);
title('');
xlabel('${X_{rob}}$ [m]','interpreter','latex');
ylabel('${Y_{rob}}$ [m]','interpreter','latex');
axis([-.1 .16 -.18 .18]);
l = legend('Experiment 1','Experiment 2','Experiment 3','Safety Curve');
set(l,'interpreter','latex','FontSize',12);
%% docked robot with marker vs. docked robot without marker!
S_Mat_DockedwithMar = csvread('scanner_data_Docked_WITH_Marker.txt',0,0);
S_Mat_DockedwithNOmar = csvread('scanner_data_no_cylinder.txt',0,0);
indx_DockedwithMar = S_Mat_DockedwithMar(:,1);
s_val_DockedwithMar = S_Mat_DockedwithMar(:,2);
indx_DockedwithNOmar = S_Mat_DockedwithNOmar(:,1);
s_val_DockedwithNOmar = S_Mat_DockedwithNOmar(:,2);
points_DockedwithMar = size(indx_DockedwithMar,1);
points_DockedwithNOmar = size(indx_DockedwithNOmar,1);
laser_beam_DockedwithMar = points_DockedwithMar - 1;
laser_beam_DockedwithNOmar = points_DockedwithNOmar - 1;
s_angle_DockedwithMar = zeros(points_DockedwithMar,1);
s_angle_DockedwithNOmar = zeros(points_DockedwithNOmar,1);
X_DockedwithMar = zeros(points_DockedwithMar,1); Y_DockedwithMar = zeros(points_DockedwithMar,1);
X_DockedwithNOmar = zeros(points_DockedwithNOmar,1); Y_DockedwithNOmar = zeros(points_DockedwithNOmar,1);
X_obst_DockedwithMar = zeros(points_DockedwithMar,1); Y_obst_DockedwithMar = zeros(points_DockedwithMar,1);
X_obst_DockedwithNOmar = zeros(points_DockedwithNOmar,1); Y_obst_DockedwithNOmar = zeros(points_DockedwithNOmar,1);
for i = 1:points_DockedwithMar
s_angle_DockedwithMar(i) = (lower_angle + (indx_DockedwithMar(i)* ((upper_angle - lower_angle)/(laser_beam_DockedwithMar))))* pi/180;
X_DockedwithMar(i) = s_val_DockedwithMar(i) * cos(s_angle_DockedwithMar(i));
Y_DockedwithMar(i) = s_val_DockedwithMar(i) * sin(s_angle_DockedwithMar(i));
X_obst_DockedwithMar(i) = dist_to_obst * cos(s_angle_DockedwithMar(i));
Y_obst_DockedwithMar(i)= dist_to_obst * sin(s_angle_DockedwithMar(i));
end
for i = 1:points_DockedwithNOmar
s_angle_DockedwithNOmar(i) = (lower_angle + (indx_DockedwithNOmar(i)* ((upper_angle - lower_angle)/(laser_beam_DockedwithNOmar))))* pi/180;
X_DockedwithNOmar(i) = s_val_DockedwithNOmar(i) * cos(s_angle_DockedwithNOmar(i));
Y_DockedwithNOmar(i) = s_val_DockedwithNOmar(i) * sin(s_angle_DockedwithNOmar(i));
X_obst_DockedwithNOmar(i) = dist_to_obst * cos(s_angle_DockedwithNOmar(i));
Y_obst_DockedwithNOmar(i)= dist_to_obst * sin(s_angle_DockedwithNOmar(i));
end
% checking accuracy in robot coordinate system
figure;
subplot(3,1,1);
plot(X_DockedwithNOmar,Y_DockedwithNOmar);
hold on;
plot(X_obst_DockedwithNOmar,Y_obst_DockedwithNOmar,'r','LineWidth',3);
xlabel('X_r [m]');
ylabel('Y_r [m]');
title('Scanner Analysis robot coordinate system WITHOUT a marker');
legend('Obstacles','Laser scanner safety curve (r = 10 cm)');
grid on
axis([-2.5 4 -4 1]);
subplot(3,1,2);
plot(X_DockedwithMar,Y_DockedwithMar,'b');
hold on;
plot(X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',3);
xlabel('X_r [m]');
ylabel('Y_r [m]');
title('Scanner Analysis robot coordinate system WITH a marker');
grid on
axis([-2.5 4 -4 1]);
subplot(3,1,3);
plot(X_DockedwithMar,Y_DockedwithMar,'b');
hold on;
plot(X_obst_DockedwithMar,Y_obst_DockedwithMar,'r','LineWidth',.3);
xlabel('X_r [m]');
ylabel('Y_r [m]');
grid on
axis([.04 .16 -.12 .12]);
%
figure;
subplot(3,1,1);
plot(s_angle_DockedwithNOmar,s_val_DockedwithNOmar);
hold on
plot(s_angle_DockedwithNOmar,dist_to_obst,'r','LineWidth',.2);
xlabel('Angle [rad]');
ylabel('Distance to Obstacle [m]');
title('Scanner Analysis; docked robot WITHOUT a marker');
legend('Surrounding objects','Laser scanner safety line (r = 10 cm)');
axis([-2.2 2.2 -.5 5]);
subplot(3,1,2);
plot(s_angle_DockedwithMar,s_val_DockedwithMar);
hold on
plot(s_angle_DockedwithMar,dist_to_obst,'r','LineWidth',.2);
xlabel('Angle [rad]');
ylabel('Distance to objects [m]');
title('Scanner Analysis; docked robot WITH a marker');
axis([-2.2 2.2 -.5 5]);
subplot(3,1,3);
plot(s_angle_DockedwithMar,s_val_DockedwithMar);
hold on
plot(s_angle_DockedwithMar,dist_to_obst,'r','LineWidth',10);
xlabel('Angle [rad]');
ylabel('Distance to Obstacle [m]');
axis([-.15 .35 .08 .15]);
%% The robot is moving Towards docking...!
Scanner_Matrix_Moving = csvread('scanner_data_Moving_Towards_Docking_WITH_Marker.txt',0,0);
index_Moving = Scanner_Matrix_Moving(:,1);
scanner_value_Moving = Scanner_Matrix_Moving(:,2);
index_Moving_Final = Scanner_Matrix_Moving(size(Scanner_Matrix_Moving,1),1);
scanner_value_Moving_Final = Scanner_Matrix_Moving(size(Scanner_Matrix_Moving,1),2);
% figure;
% plot(index_Moving,scanner_value_Moving,'m',index_Moving_Final,scanner_value_Moving_Final,'k*');
% legend('Distance to obstacle','Reference Position');
% grid on;
% xlabel('Index');
% ylabel('Distance to Obstacle [m]');
% title('Scanner Analysis for a moving robot!');
% axis([0 540 0 31]);
%% The robot is in the docking area, while NOT docked!
S_Mat_Out = csvread('scanner_data_outside.txt',0,0);
indx_out = S_Mat_Out(:,1);
s_val_out = S_Mat_Out(:,2);
points_out = size(indx_out,1);
laser_beam_out = points_out - 1;
s_angle_out = zeros(points_out,1);
X_out = zeros(points_out,1); Y_out = zeros(points_out,1);
X_obst_out = zeros(points_out,1); Y_obst_out = zeros(points_out,1);
for i = 1:points_out
s_angle_out(i) = (lower_angle + (indx_out(i)* ((upper_angle - lower_angle)/(laser_beam_out))))* pi/180;
X_out(i) = s_val_out(i) * cos(s_angle_out(i));
Y_out(i) = s_val_out(i) * sin(s_angle_out(i));
X_obst_out(i) = dist_to_obst * cos(s_angle_out(i));
Y_obst_out(i)= dist_to_obst * sin(s_angle_out(i));
end
figure;
subplot(1,2,1);
plot(s_angle_out,s_val_out,'b','LineWidth',2);
hold on
plot(s_angle_out,dist_to_obst,'r','LineWidth',10);
xlabel('$Angle [rad]$','interpreter','latex','FontSize',14);
ylabel('$Obstacle$','interpreter','latex','FontSize',14);
grid on;
axis([-2.23 2.23 -1 10]);
subplot(1,2,2)
plot(X_out,Y_out,'b','LineWidth',2);
hold on
plot(X_obst_out,Y_obst_out,'r-','LineWidth',4)
xlabel('$ X [m] $','interpreter','latex','FontSize',14);
ylabel('$Obstacle$','interpreter','latex','FontSize',14);
title('Scanner Analysis Top view ( X - Y )');
legend('Obstacles','Laser scanner safety margin (r = 10 cm)');
grid on;
axis([-1.2 5 -3.5 1.5]);
%% The robot is totally docked and a box is besides of laser scanner!
S_Mat_DockedwithBOX = csvread('scanner_data_box.txt',0,0);
indx_DockedwithBOX = S_Mat_DockedwithBOX(:,1);
s_val_DockedwithBOX = S_Mat_DockedwithBOX(:,2);
points_DockedwithBOX = size(indx_DockedwithBOX,1);
laser_beam_DockedwithBOX = points_DockedwithBOX - 1;
s_angle_DockedwithBOX = zeros(points_DockedwithBOX,1);
X_DockedwithBOX = zeros(points_DockedwithBOX,1); Y_DockedwithBOX = zeros(points_DockedwithBOX,1);
X_obst_DockedwithBOX = zeros(points_DockedwithBOX,1); Y_obst_DockedwithBOX = zeros(points_DockedwithBOX,1);
for i = 1:points_DockedwithBOX
s_angle_DockedwithBOX(i) = (lower_angle + (indx_DockedwithBOX(i)* ((upper_angle - lower_angle)/(laser_beam_DockedwithBOX))))* pi/180;
X_DockedwithBOX(i) = s_val_DockedwithBOX(i) * cos(s_angle_DockedwithBOX(i));
Y_DockedwithBOX(i) = s_val_DockedwithBOX(i) * sin(s_angle_DockedwithBOX(i));
X_obst_DockedwithBOX(i) = dist_to_obst * cos(s_angle_DockedwithBOX(i));
Y_obst_DockedwithBOX(i)= dist_to_obst * sin(s_angle_DockedwithBOX(i));
end
figure;
subplot(2,1,1);
plot(s_angle_DockedwithBOX,s_val_DockedwithBOX,'b','LineWidth',.01);
hold on
plot(s_angle_DockedwithBOX,dist_to_obst, 'r','LineWidth',5);
xlabel('Angle [rad]');
ylabel('Distance to Obstacle [m]');
title('Scanner Analysis; docked robot with a BOX marker');
legend('Surrounding objects','Laser scanner safety line (r = 10 cm)');
axis([-2.2 2.2 0 5]);
subplot(2,1,2);
plot(s_angle_DockedwithBOX,s_val_DockedwithBOX,'b','LineWidth',.01);
hold on
plot(s_angle_DockedwithBOX,dist_to_obst, 'r','LineWidth',5);
xlabel('Angle [rad]');
ylabel('Distance to Obstacle [m]');
axis([-1.9 0 .08 .22]);
figure;
plot(X_DockedwithBOX,Y_DockedwithBOX);
\ No newline at end of file
Supports Markdown
0% or .
You are about to add 0 people to the discussion. Proceed with caution.
Finish editing this message first!
Please register or to comment