docking_with_PID_controller.m 21 KB
Newer Older
Farid Alijani's avatar
Farid Alijani committed
1
2
3
clc;
close all;
clear all;
Farid Alijani's avatar
Farid Alijani committed
4
5

yaw_off = pi;
Farid Alijani's avatar
Farid Alijani committed
6
7
8
9
10
11
%% Import data and time conversion

% data 1: 556 ros messages for geometry_msgs/Twist 
%         557 ros messages for geometry_msgs/PoseStamped


Farid Alijani's avatar
Farid Alijani committed
12
13
% Pose_Matrix_data_1 = csvread('POSE_with_Py0.86_Iy0_Dy0.002ANDPt0.08_It0_Dt0.txt',1,0);
% Vel_Matrix_data_1 = csvread('VEL_with_Py0.86_Iy0_Dy0.002ANDPt0.08_It0_Dt0.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
14
15
Pose_Matrix_data_1 = csvread('CAM_PID_.28_.0001_.01.txt',1,0);
Vel_Matrix_data_1 = csvread('vel_PID_.28_.0001_.01.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42

% 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;
Farid Alijani's avatar
Farid Alijani committed
43
% 
Farid Alijani's avatar
Farid Alijani committed
44
45
% Pose_Matrix_data_2 = csvread('POSE_with_Py0.66_Iy0_Dy0.1ANDPt0.08_It0_Dt0.txt',1,0);
% Vel_Matrix_data_2 = csvread('VEL_with_Py0.66_Iy0_Dy0.1ANDPt0.08_It0_Dt0.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
46
47
48
% 
Pose_Matrix_data_2 = csvread('CAM_PID_.29_.00026_.015.txt',1,0);
Vel_Matrix_data_2 = csvread('vel_PID_.29_.00026_.015.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76

% 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;

Farid Alijani's avatar
Farid Alijani committed
77
78
79
% Pose_Matrix_data_3 = csvread('POSE_with_Py0.86_Iy0_Dy0.1ANDPt0.08_It0_Dt0.txt',1,0);
% Vel_Matrix_data_3 = csvread('VEL_with_Py0.86_Iy0_Dy0.1ANDPt0.08_It0_Dt0.txt',1,0);

Farid Alijani's avatar
Farid Alijani committed
80
81
Pose_Matrix_data_3 = csvread('Trans_PID_.28_0_.006.txt',1,0);
Vel_Matrix_data_3 = csvread('vel_PID_.28_0_.006.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100

% 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)) = [];

101
102
103
104
105
106
107
108
% -----------------------------------------------------------------------------------------------------
% data 4: 450 ros messages for geometry_msgs/Twist 
%         451 ros messages for geometry_msgs/PoseStamped
% y-axis gains:
% P = .86; I = 0; D = 0.002;

% theta-axis gains:
% P = .08; I = 0; D = 0;
Farid Alijani's avatar
Farid Alijani committed
109
% 
Farid Alijani's avatar
Farid Alijani committed
110
111
% Pose_Matrix_data_4 = csvread('POSE_with_Py0.51_Iy0.0005_Dy0.05ANDPt0.08_It0_Dt0.txt',1,0);
% Vel_Matrix_data_4 = csvread('VEL_with_Py0.51_Iy0.0005_Dy0.05ANDPt0.08_It0_Dt0.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
112

Farid Alijani's avatar
Farid Alijani committed
113
114
Pose_Matrix_data_4 = csvread('CAM_PID_.266_.0003_.014.txt',1,0);
Vel_Matrix_data_4 = csvread('vel_PID_.266_.0003_.014.txt',1,0);
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133

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

% duriation =         end_time        -     start_time
  durationP_data_4 = (TimeP_ros_data_4(size(TimeP_ros_data_4,1),:) - TimeP_ros_data_4(1,:))*10^(-9); 
  
t_P_sec_data_4 = 0:durationP_data_4/size(TimeP_ros_data_4,1):durationP_data_4;
t_P_sec_data_4(:,size(t_P_sec_data_4,2)) = [];

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

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

t_V_sec_data_4 = 0:durationV_data_4/size(TimeV_ros_data_4,1):durationV_data_4;
t_V_sec_data_4(:,size(t_V_sec_data_4,2)) = [];

134
% -----------------------------------------------------------------------------------------------------
Farid Alijani's avatar
Farid Alijani committed
135
% data 5: ANDROID CAMERA
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158


Pose_Matrix_data_5 = csvread('Pose_with_Android.txt',1,0);
Vel_Matrix_data_5 = csvread('Velocity_with_Android.txt',1,0);

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

% duriation =         end_time        -     start_time
  durationP_data_5 = (TimeP_ros_data_5(size(TimeP_ros_data_5,1),:) - TimeP_ros_data_5(1,:))*10^(-9); 
  
t_P_sec_data_5 = 0:durationP_data_5/size(TimeP_ros_data_5,1):durationP_data_5;
t_P_sec_data_5(:,size(t_P_sec_data_5,2)) = [];

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

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

t_V_sec_data_5 = 0:durationV_data_5/size(TimeV_ros_data_5,1):durationV_data_5;
t_V_sec_data_5(:,size(t_V_sec_data_5,2)) = [];

Farid Alijani's avatar
Farid Alijani committed
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
% -----------------------------------------------------------------------------------------------------
% data 6: with several oscilations


Pose_Matrix_data_6 = csvread('CAM_PID_.39_.0005_.01.txt',1,0);
Vel_Matrix_data_6 = csvread('vel_PID_.39_.0005_.01.txt',1,0);

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

% duriation =         end_time        -     start_time
  durationP_data_6 = (TimeP_ros_data_6(size(TimeP_ros_data_6,1),:) - TimeP_ros_data_6(1,:))*10^(-9); 
  
t_P_sec_data_6 = 0:durationP_data_6/size(TimeP_ros_data_6,1):durationP_data_6;
t_P_sec_data_6(:,size(t_P_sec_data_6,2)) = [];

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

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

t_V_sec_data_6 = 0:durationV_data_6/size(TimeV_ros_data_6,1):durationV_data_6;
t_V_sec_data_6(:,size(t_V_sec_data_6,2)) = [];


Farid Alijani's avatar
Farid Alijani committed
185
186
187
188
189
190
%% 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);
Farid Alijani's avatar
Farid Alijani committed
191
Theta_data_1 = Pose_Matrix_data_1(:,7);
Farid Alijani's avatar
Farid Alijani committed
192

Farid Alijani's avatar
Farid Alijani committed
193
194
195
196
197
198
199
200
% for i = 1:size(Theta_data_1,1)
%     if (Theta_data_1(i) > 0)
%         Theta_data_1(i) = Theta_data_1(i) - yaw_off;
%     else if(Theta_data_1(i) < 0)
%             Theta_data_1(i) = Theta_data_1(i) + yaw_off;
%         end
%     end
% end
Farid Alijani's avatar
Farid Alijani committed
201
202
203
204
205
206
207
208
209
210
% 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);
Farid Alijani's avatar
Farid Alijani committed
211
Theta_data_2 = Pose_Matrix_data_2(:,7);
Farid Alijani's avatar
Farid Alijani committed
212

Farid Alijani's avatar
Farid Alijani committed
213
214
215
216
217
218
219
220
% for i = 1:size(Theta_data_2,1)
%     if (Theta_data_2(i) > 0)
%         Theta_data_2(i) = Theta_data_2(i) - yaw_off;
%     else if(Theta_data_2(i) < 0)
%             Theta_data_2(i) = Theta_data_2(i) + yaw_off;
%         end
%     end
% end
Farid Alijani's avatar
Farid Alijani committed
221
222
223
224
225
226
227
228
229
230
231

% 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);
Farid Alijani's avatar
Farid Alijani committed
232
Theta_data_3 = Pose_Matrix_data_3(:,7);
Farid Alijani's avatar
Farid Alijani committed
233

Farid Alijani's avatar
Farid Alijani committed
234
235
236
237
238
239
240
241
242
% 
% for i = 1:size(Theta_data_3,1)
%     if (Theta_data_3(i) > 0)
%         Theta_data_3(i) = Theta_data_3(i) - yaw_off;
%     else if(Theta_data_3(i) < 0)
%             Theta_data_3(i) = Theta_data_3(i) + yaw_off;
%         end
%     end
% end
Farid Alijani's avatar
Farid Alijani committed
243
244
245
246
247
248
% 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];

249
250
251
%data 4:
% when using marker pose ,,,,
Pose_X_data_4 = Pose_Matrix_data_4(:,4);
Farid Alijani's avatar
Farid Alijani committed
252
253
254
255
256
257
258
259
260
261
262
Pose_Y_data_4 = Pose_Matrix_data_4(:,3);
Theta_data_4 = Pose_Matrix_data_4(:,7);
% 
% for i = 1:size(Theta_data_4,1)
%     if (Theta_data_4(i) > 0)
%         Theta_data_4(i) = Theta_data_4(i) - yaw_off;
%     else if(Theta_data_4(i) < 0)
%             Theta_data_4(i) = Theta_data_4(i) + yaw_off;
%         end
%     end
% end
263
264
265
266
267
268
269
270

% Extracting reference values when the robot is manually docked!
ref_X_data_4 = Pose_X_data_4(size(Pose_X_data_4,1));
ref_Y_data_4 = Pose_Y_data_4(size(Pose_Y_data_4,1));
ref_Theta_data_4 = Theta_data_4(size(Theta_data_4,1));
ref_Pose_data_4 =[ref_X_data_4;ref_Y_data_4];


271
272
273
274
%data 5:
% when using marker pose ,,,,
Pose_X_data_5 = Pose_Matrix_data_5(:,4);
Pose_Y_data_5 = Pose_Matrix_data_5(:,3);
Farid Alijani's avatar
Farid Alijani committed
275
Theta_data_5 = Pose_Matrix_data_5(:,7);
276
277
278
279
280
281
282

% Extracting reference values when the robot is manually docked!
ref_X_data_5 = Pose_X_data_5(size(Pose_X_data_5,1));
ref_Y_data_5 = Pose_Y_data_5(size(Pose_Y_data_5,1));
ref_Theta_data_5 = Theta_data_5(size(Theta_data_5,1));
ref_Pose_data_5 =[ref_X_data_5;ref_Y_data_5];

Farid Alijani's avatar
Farid Alijani committed
283
284
285
286
287
288
289
290
291
292
293
%data 6:
% when using marker pose ,,,,
Pose_X_data_6 = Pose_Matrix_data_6(:,4);
Pose_Y_data_6 = Pose_Matrix_data_6(:,3);
Theta_data_6 = Pose_Matrix_data_6(:,7);

% Extracting reference values when the robot is manually docked!
ref_X_data_6 = Pose_X_data_6(size(Pose_X_data_6,1));
ref_Y_data_6 = Pose_Y_data_6(size(Pose_Y_data_6,1));
ref_Theta_data_6 = Theta_data_6(size(Theta_data_6,1));
ref_Pose_data_6 =[ref_X_data_6;ref_Y_data_6];
294
295

% Reference Circle 
Farid Alijani's avatar
Farid Alijani committed
296
297
298
thresh_X = .001;
theta = 0:.001:2*pi;
% needs to be adjusted manually if docking platform is replaced!
Farid Alijani's avatar
Farid Alijani committed
299
300
ref_x = .2026;
ref_y = .0087;
Farid Alijani's avatar
Farid Alijani committed
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
319
320
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);

321
% data 4:
322
323
324
325
Vel_X_data_4 = Vel_Matrix_data_4(:,2);
Vel_Y_data_4 = Vel_Matrix_data_4(:,3);
Omega_Z_data_4 = Vel_Matrix_data_4(:,7);

326
327
328
329
% data 5:
Vel_X_data_5 = Vel_Matrix_data_5(:,2);
Vel_Y_data_5 = Vel_Matrix_data_5(:,3);
Omega_Z_data_5 = Vel_Matrix_data_5(:,7);
Farid Alijani's avatar
Farid Alijani committed
330

Farid Alijani's avatar
Farid Alijani committed
331
332
333
334
335
% data 6:
Vel_X_data_6 = Vel_Matrix_data_6(:,2);
Vel_Y_data_6 = Vel_Matrix_data_6(:,3);
Omega_Z_data_6 = Vel_Matrix_data_6(:,7);

Farid Alijani's avatar
Farid Alijani committed
336
337
%% Plots

Farid Alijani's avatar
Farid Alijani committed
338
339
% marker position
figure;
Farid Alijani's avatar
Farid Alijani committed
340
plot(t_P_sec_data_6,Pose_Y_data_6,'k','LineWidth',1.6);
Farid Alijani's avatar
Farid Alijani committed
341
342
xlabel('time');
ylabel('y');
Farid Alijani's avatar
Farid Alijani committed
343
344

figure;
Farid Alijani's avatar
Farid Alijani committed
345
plot(t_P_sec_data_6,Theta_data_6,'k','LineWidth',1.6);
Farid Alijani's avatar
Farid Alijani committed
346
347
xlabel('time');
ylabel('\theta');
Farid Alijani's avatar
Farid Alijani committed
348
349
350


figure;
Farid Alijani's avatar
Farid Alijani committed
351
set(gcf,'color','white');
Farid Alijani's avatar
Farid Alijani committed
352
subplot(3,1,1);
Farid Alijani's avatar
Farid Alijani committed
353
plot(t_P_sec_data_1,Pose_X_data_1,'LineWidth',2.1);
Farid Alijani's avatar
Farid Alijani committed
354
hold on
355
plot(t_P_sec_data_2,Pose_X_data_2,'r','LineWidth',1.7);
Farid Alijani's avatar
Farid Alijani committed
356
hold on
Farid Alijani's avatar
Farid Alijani committed
357
358
359
plot(t_P_sec_data_3,Pose_X_data_3,'g','LineWidth',1.8);
hold on
plot(t_P_sec_data_4,Pose_X_data_4,'m','LineWidth',1.5);
Farid Alijani's avatar
Farid Alijani committed
360
361
title('Pose estimation in marker coordinate system');
ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',14);
362

Farid Alijani's avatar
Farid Alijani committed
363
l2 = legend('${P_y = 0.28, I_y = 0.0001, D_y = 0.01}$', '${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$');
364
set(l2,'interpreter','latex','FontSize',11);
Farid Alijani's avatar
Farid Alijani committed
365
legend('boxoff');
Farid Alijani's avatar
Farid Alijani committed
366
367

subplot(3,1,2);
Farid Alijani's avatar
Farid Alijani committed
368
plot(t_P_sec_data_1,Pose_Y_data_1,'LineWidth',2.1);
Farid Alijani's avatar
Farid Alijani committed
369
hold on
370
plot(t_P_sec_data_2,Pose_Y_data_2,'r','LineWidth',1.7);
Farid Alijani's avatar
Farid Alijani committed
371
hold on
Farid Alijani's avatar
Farid Alijani committed
372
373
374
plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',1.8);
hold on
plot(t_P_sec_data_4,Pose_Y_data_4,'m','LineWidth',1.5);
Farid Alijani's avatar
Farid Alijani committed
375
ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',14);
Farid Alijani's avatar
Farid Alijani committed
376

Farid Alijani's avatar
Farid Alijani committed
377
subplot(3,1,3);
Farid Alijani's avatar
Farid Alijani committed
378
plot(t_P_sec_data_1,Theta_data_1,'LineWidth',2.1);
Farid Alijani's avatar
Farid Alijani committed
379
hold on
380
plot(t_P_sec_data_2,Theta_data_2,'r','LineWidth',1.7);
Farid Alijani's avatar
Farid Alijani committed
381
hold on
Farid Alijani's avatar
Farid Alijani committed
382
383
384
385
plot(t_P_sec_data_3,Theta_data_3,'g','LineWidth',1.8);
hold on
plot(t_P_sec_data_4,Theta_data_4,'m','LineWidth',1.5);

Farid Alijani's avatar
Farid Alijani committed
386
387
xlabel('$ time [sec]$','interpreter','latex','FontSize',14);
ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',14);
Farid Alijani's avatar
Farid Alijani committed
388

Farid Alijani's avatar
Farid Alijani committed
389

Farid Alijani's avatar
Farid Alijani committed
390
% control signals
Farid Alijani's avatar
Farid Alijani committed
391
figure;
Farid Alijani's avatar
Farid Alijani committed
392
set(gcf,'color','white');
Farid Alijani's avatar
Farid Alijani committed
393
394
subplot(3,1,1);

Farid Alijani's avatar
Farid Alijani committed
395
plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',2.1);
Farid Alijani's avatar
Farid Alijani committed
396
hold on
397
plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.7);
Farid Alijani's avatar
Farid Alijani committed
398
hold on
Farid Alijani's avatar
Farid Alijani committed
399
400
401
plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',1.8);
hold on
plot(t_V_sec_data_4,Vel_X_data_4,'m','LineWidth',1.5);
Farid Alijani's avatar
Farid Alijani committed
402

Farid Alijani's avatar
Farid Alijani committed
403
404
405
title('Control Signals');
ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);

Farid Alijani's avatar
Farid Alijani committed
406
l2 = legend('${P_y = 0.28, I_y = 0.0001, D_y = 0.01}$', '${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$');
407
set(l2,'interpreter','latex','FontSize',11);
Farid Alijani's avatar
Farid Alijani committed
408
legend('boxoff');
409

Farid Alijani's avatar
Farid Alijani committed
410
411

subplot(3,1,2);
Farid Alijani's avatar
Farid Alijani committed
412
plot(t_V_sec_data_1,Vel_Y_data_1,'LineWidth',2.1);
Farid Alijani's avatar
Farid Alijani committed
413
hold on
414
plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.7);
Farid Alijani's avatar
Farid Alijani committed
415
hold on
Farid Alijani's avatar
Farid Alijani committed
416
417
418
plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',1.8);
hold on
plot(t_V_sec_data_4,Vel_Y_data_4,'m','LineWidth',1.5);
Farid Alijani's avatar
Farid Alijani committed
419
ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);
Farid Alijani's avatar
Farid Alijani committed
420
421

subplot(3,1,3);
Farid Alijani's avatar
Farid Alijani committed
422
plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',2.1);
Farid Alijani's avatar
Farid Alijani committed
423
hold on
424
plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.7);
Farid Alijani's avatar
Farid Alijani committed
425
hold on
Farid Alijani's avatar
Farid Alijani committed
426
427
428
plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.8);
hold on
plot(t_V_sec_data_4,Omega_Z_data_4,'m','LineWidth',1.5);
Farid Alijani's avatar
Farid Alijani committed
429
430
xlabel('$ time [sec]$','interpreter','latex','FontSize',14);
ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14);
431
432


Farid Alijani's avatar
Farid Alijani committed
433
% Trajectory 
434

Farid Alijani's avatar
Farid Alijani committed
435
% 
Farid Alijani's avatar
Farid Alijani committed
436
437
438
439
440
441
442
% 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*');
Farid Alijani's avatar
Farid Alijani committed
443

Farid Alijani's avatar
Farid Alijani committed
444
445
446
447
448
449
450
451
452
453

% 
% 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);
% 
Farid Alijani's avatar
Farid Alijani committed
454
455
456
% Vx = gradient(Pose_X_data_1);
% Vy = gradient(Pose_Y_data_1);

Farid Alijani's avatar
Farid Alijani committed
457
% figure;
Farid Alijani's avatar
Farid Alijani committed
458
459
460
% 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);
Farid Alijani's avatar
Farid Alijani committed
461

Farid Alijani's avatar
Farid Alijani committed
462
figure;
Farid Alijani's avatar
Farid Alijani committed
463
464
plot(Pose_Y_data_6,Pose_X_data_6,'k','LineWidth',1.4);
axis([-.3 .7 .1 1.6]);
Farid Alijani's avatar
Farid Alijani committed
465

Farid Alijani's avatar
Farid Alijani committed
466

Farid Alijani's avatar
Farid Alijani committed
467
figure;
Farid Alijani's avatar
Farid Alijani committed
468
469
subplot(1,3,1);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
Farid Alijani's avatar
Farid Alijani committed
470
471
% hold on
% quiver(abs(Pose_Y_data_1),Pose_X_data_1,gradient(Pose_Y_data_1),gradient(Pose_X_data_1),'r--','LineWidth',.1);
Farid Alijani's avatar
Farid Alijani committed
472

Farid Alijani's avatar
Farid Alijani committed
473
474
hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
Farid Alijani's avatar
Farid Alijani committed
475
476
% hold on
% quiver(abs(Pose_Y_data_2),Pose_X_data_2,gradient(Pose_Y_data_2),gradient(Pose_X_data_2),'m--','LineWidth',.1);
Farid Alijani's avatar
Farid Alijani committed
477

Farid Alijani's avatar
Farid Alijani committed
478
479
hold on
plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
Farid Alijani's avatar
Farid Alijani committed
480
481
% hold on
% quiver(abs(Pose_Y_data_3),Pose_X_data_3,gradient(Pose_Y_data_3),gradient(Pose_X_data_3),'k--','LineWidth',.1);
Farid Alijani's avatar
Farid Alijani committed
482

Farid Alijani's avatar
Farid Alijani committed
483
484
hold on
plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
Farid Alijani's avatar
Farid Alijani committed
485
486
% hold on
% quiver(abs(Pose_Y_data_4),Pose_X_data_4,gradient(Pose_Y_data_4),gradient(Pose_X_data_4),'b--','LineWidth',.1);
Farid Alijani's avatar
Farid Alijani committed
487

488
489
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);
Farid Alijani's avatar
Farid Alijani committed
490

Farid Alijani's avatar
Farid Alijani committed
491
492
title('Approach zone + SM zone + Target area');
ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',14);
Farid Alijani's avatar
Farid Alijani committed
493
axis([-.05 .26 .19 1.52]);
Farid Alijani's avatar
Farid Alijani committed
494
495

subplot(1,3,2);
Farid Alijani's avatar
Farid Alijani committed
496
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
Farid Alijani's avatar
Farid Alijani committed
497
hold on
Farid Alijani's avatar
Farid Alijani committed
498
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
Farid Alijani's avatar
Farid Alijani committed
499
hold on
Farid Alijani's avatar
Farid Alijani committed
500
plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
Farid Alijani's avatar
Farid Alijani committed
501
502
hold on
plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
503
hold on
Farid Alijani's avatar
Farid Alijani committed
504
plot(y_circle,x_circle,'k--','LineWidth',3.4);
505

Farid Alijani's avatar
Farid Alijani committed
506
507
title('SM zone + Target area');
xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14);
Farid Alijani's avatar
Farid Alijani committed
508
axis([-.023 0.02 .19 .29]);
Farid Alijani's avatar
Farid Alijani committed
509
510
511
512
513
514
515

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);
Farid Alijani's avatar
Farid Alijani committed
516
517
hold on
plot(Pose_Y_data_4,Pose_X_data_4,'m','LineWidth',2);
518
hold on
Farid Alijani's avatar
Farid Alijani committed
519
plot(y_circle,x_circle,'k--','LineWidth',3.4);
520

Farid Alijani's avatar
Farid Alijani committed
521
title('Target area');
Farid Alijani's avatar
Farid Alijani committed
522
axis([0.0072 .0101 .20 .2056]);
Farid Alijani's avatar
Farid Alijani committed
523

Farid Alijani's avatar
Farid Alijani committed
524
l = legend('${P_y = 0.28, I_y = 0.0001, D_y = 0.01}$', '${P_y = 0.29, I_y = 0.00026, D_y = 0.015}$','${P_y = 0.28, I_y = 0, D_y = 0.006}$','${P_y = 0.266, I_y = 0.0003, D_y = 0.014}$','${Target}$');
525
set(l,'interpreter','latex','FontSize',11);
Farid Alijani's avatar
Farid Alijani committed
526
% legend('boxoff');
527

Farid Alijani's avatar
Farid Alijani committed
528
529
530
531
532
533
534
535
536

% sampling time
figure;
set(gcf,'color','white');

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);
537
% hold on
Farid Alijani's avatar
Farid Alijani committed
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
553
554
555
556
557
558
559
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'});
axis([0 234 .068 .3]);

l4 = legend('${Time_{ROS}}$','${Time_{Real}} [sec]$','Orientation','horizontal');
set(l4,'interpreter','latex','FontSize',13);
legend('boxoff');

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);
Farid Alijani's avatar
Farid Alijani committed
560
% hold on;
Farid Alijani's avatar
Farid Alijani committed
561
562
563
564
565
566
567
568
569
570
571
572
573
574
575
576
577
578
579
580
581
582
583
584
585
586
587
588
589
590
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;
set(gcf,'color','white');

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');
Farid Alijani's avatar
Farid Alijani committed
591
% ylabel('Sampling time [sec]','interpreter','latex','FontSize',13);
Farid Alijani's avatar
Farid Alijani committed
592
% 
Farid Alijani's avatar
Farid Alijani committed
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
% l5 = legend([pa1 pa2],'$USB Camera {Time_{ROS}}$','$ USB Camera {Time_{Real}} [sec]$','Orientation','horizontal');
% set(l5,'interpreter','latex','FontSize',11);

% leg = 
legend('ROS Time, USB camera','Real Time, USB camera [s]','ROS Time, IP camera','Real Time, IP camera [s]','Location','northwest');
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);
Farid Alijani's avatar
Farid Alijani committed
613
% 
Farid Alijani's avatar
Farid Alijani committed
614
615
616
617
618
619
620
621
% 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 
622
% figure;
Farid Alijani's avatar
Farid Alijani committed
623
624
625
626
627
628
% subplot(3,1,1);
% plot(diff(Pose_X_data_1));
% title('X-Pose diff');
% xlabel('samples');
% ylabel('X difference [m]');
% grid on
629
% 
Farid Alijani's avatar
Farid Alijani committed
630
631
632
633
634
635
% subplot(3,1,2);
% plot(diff(Pose_X_data_1));
% title('Y-Pose diff');
% xlabel('samples');
% ylabel('Y difference [m]');
% grid on
Farid Alijani's avatar
Farid Alijani committed
636
% 
Farid Alijani's avatar
Farid Alijani committed
637
638
639
640
641
642
% subplot(3,1,3);
% plot(diff(Theta_data_1));
% title('\theta-Pose diff');
% xlabel('samples');
% ylabel('\theta difference [rad]');
% grid on