docking_with_P_controller.m 11.2 KB
Newer Older
Farid Alijani's avatar
Farid Alijani committed
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
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;

Farid Alijani's avatar
Farid Alijani committed
26
27
28
29
30
% Pose_Matrix_data_1 = csvread('Pose01_08_16_1718.txt',1,0);
% Vel_Matrix_data_1 = csvread('Velocity01_08_16_1718.txt',1,0);

Pose_Matrix_data_1 = csvread('CAM_x_dot_.1.txt',1,0);
Vel_Matrix_data_1 = csvread('x_dot_.1.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50

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

% -----------------------------------------------------------------------------------------------------
Farid Alijani's avatar
Farid Alijani committed
51
52
53
54
% data 2: 
% py = .25;     ptheta = .25;
% iy = 0;       itheta = 0;
% dy = 0;       dtheta = 0;
Farid Alijani's avatar
Farid Alijani committed
55

Farid Alijani's avatar
Farid Alijani committed
56
57
% Pose_Matrix_data_2 = csvread('Pose01_08_16_1749.txt',1,0);
% Vel_Matrix_data_2 = csvread('Velocity01_08_16_1749.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
58

Farid Alijani's avatar
Farid Alijani committed
59
60
61

Pose_Matrix_data_2 = csvread('CAM_x_dot_.15.txt',1,0);
Vel_Matrix_data_2 = csvread('x_dot_.15.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81

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

% -----------------------------------------------------------------------------------------------------
Farid Alijani's avatar
Farid Alijani committed
82
83
84
% data 3: 
% Pose_Matrix_data_3 = csvread('Pose01_08_16_1831.txt',1,0);
% Vel_Matrix_data_3 = csvread('Velocity01_08_16_1831.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
85
86


Farid Alijani's avatar
Farid Alijani committed
87
88
Pose_Matrix_data_3 = csvread('CAM_x_dot_.16.txt',1,0);
Vel_Matrix_data_3 = csvread('x_dot_.16.txt',1,0);
Farid Alijani's avatar
Farid Alijani committed
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114

% 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);
Farid Alijani's avatar
Farid Alijani committed
115
Theta_data_1 = Pose_Matrix_data_1(:,7);
Farid Alijani's avatar
Farid Alijani committed
116
117
118
119
120
121
122
123
124
125
126
127


% 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
128
Theta_data_2 = Pose_Matrix_data_2(:,7);
Farid Alijani's avatar
Farid Alijani committed
129
130
131
132
133
134
135
136
137
138
139

% 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
140
Theta_data_3 = Pose_Matrix_data_3(:,7);
Farid Alijani's avatar
Farid Alijani committed
141
142
143
144
145
146
147
148
149
150
151
152


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

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
153
154
ref_x = -.203;
ref_y = -.019;
Farid Alijani's avatar
Farid Alijani committed
155
156
157
158
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
185
186
187
188
189

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


% marker position
figure;
subplot(3,1,1);
plot(t_P_sec_data_1,Pose_X_data_1);
hold on
plot(t_P_sec_data_2,Pose_X_data_2,'r');
hold on
plot(t_P_sec_data_3,Pose_X_data_3,'g');

190
title('Marker X-axis','Color','r');
Farid Alijani's avatar
Farid Alijani committed
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
% xlabel('Time [sec]');
ylabel('X [m]');
grid on

subplot(3,1,2);
plot(t_P_sec_data_1,Pose_Y_data_1);
hold on
plot(t_P_sec_data_2,Pose_Y_data_2,'r');
hold on
plot(t_P_sec_data_3,Pose_Y_data_3,'g');

title('Marker Y-axis');
% xlabel('Time [sec]');
ylabel('Y [m]');
grid on

subplot(3,1,3);
plot(t_P_sec_data_1,Theta_data_1);
hold on
plot(t_P_sec_data_2,Theta_data_2,'r');
hold on
plot(t_P_sec_data_3,Theta_data_3,'g');

title('Marker \theta-axis');
Farid Alijani's avatar
Farid Alijani committed
215
xlabel('Time [s]');
Farid Alijani's avatar
Farid Alijani committed
216
217
218
ylabel('\theta [rad]');
grid on

Farid Alijani's avatar
Farid Alijani committed
219
220


Farid Alijani's avatar
Farid Alijani committed
221
222
% control signals
figure;
Farid Alijani's avatar
Farid Alijani committed
223
224
set(gcf,'color','white');

Farid Alijani's avatar
Farid Alijani committed
225
subplot(3,1,1);
226
plot(t_V_sec_data_1,Vel_X_data_1,'LineWidth',2);
Farid Alijani's avatar
Farid Alijani committed
227
hold on
Farid Alijani's avatar
Farid Alijani committed
228
plot(t_V_sec_data_2,Vel_X_data_2,'r','LineWidth',1.6);
Farid Alijani's avatar
Farid Alijani committed
229
hold on
230
plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',2);
Farid Alijani's avatar
Farid Alijani committed
231

Farid Alijani's avatar
Farid Alijani committed
232
title('Control Signals');
Farid Alijani's avatar
Farid Alijani committed
233
% xlabel('$ time [sec]$','interpreter','latex','FontSize',14);
Farid Alijani's avatar
Farid Alijani committed
234
235
ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);
% grid on
Farid Alijani's avatar
Farid Alijani committed
236
237
axis([0 35 -.01 .2]);
l = legend('$\dot{x}_{Rob}$ = 0.1 [m/s]','$\dot{x}_{Rob}$ = 0.15[m/s]','$\dot{x}_{Rob}$ = 0.16[m/s]');
Farid Alijani's avatar
Farid Alijani committed
238
239
set(l,'interpreter','latex','FontSize',12);
legend('boxoff');
Farid Alijani's avatar
Farid Alijani committed
240
241

subplot(3,1,2);
242
plot(t_V_sec_data_1,Vel_Y_data_1,'LineWidth',2.5);
Farid Alijani's avatar
Farid Alijani committed
243
hold on
Farid Alijani's avatar
Farid Alijani committed
244
plot(t_V_sec_data_2,Vel_Y_data_2,'r','LineWidth',1.5);
Farid Alijani's avatar
Farid Alijani committed
245
hold on
246
plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',2.1);
Farid Alijani's avatar
Farid Alijani committed
247

Farid Alijani's avatar
Farid Alijani committed
248
249
250
% title('Control Signal  y_{Rob} - axis');
ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);
% grid on
Farid Alijani's avatar
Farid Alijani committed
251
252
253
% axis([0 46 -.2 .1]);

subplot(3,1,3);
254
plot(t_V_sec_data_1,Omega_Z_data_1,'LineWidth',2.5);
Farid Alijani's avatar
Farid Alijani committed
255
hold on
Farid Alijani's avatar
Farid Alijani committed
256
plot(t_V_sec_data_2,Omega_Z_data_2,'r','LineWidth',1.45);
Farid Alijani's avatar
Farid Alijani committed
257
hold on
258
plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',2.1);
Farid Alijani's avatar
Farid Alijani committed
259

Farid Alijani's avatar
Farid Alijani committed
260
% title('Control Signal  \theta_{Rob} - axis');
Farid Alijani's avatar
Farid Alijani committed
261
xlabel('$ time [s]$','interpreter','latex','FontSize',14);
Farid Alijani's avatar
Farid Alijani committed
262
263
ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14);
% grid on
264

Farid Alijani's avatar
Farid Alijani committed
265
266
% axis([0 46 -.1 .15]);

Farid Alijani's avatar
Farid Alijani committed
267
268


Farid Alijani's avatar
Farid Alijani committed
269
% Trajectory 
Farid Alijani's avatar
Farid Alijani committed
270

Farid Alijani's avatar
Farid Alijani committed
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
% 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;
Farid Alijani's avatar
Farid Alijani committed
299
300
set(gcf,'color','white');

Farid Alijani's avatar
Farid Alijani committed
301
subplot(131);
Farid Alijani's avatar
Farid Alijani committed
302
303
304
305
306
307
308
309
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)

Farid Alijani's avatar
Farid Alijani committed
310
title('Approach zone + SM zone + Target area');
Farid Alijani's avatar
Farid Alijani committed
311
312
313
% xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14);
ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',16);
axis([-.65 .01 -1.47 -.185]);
Farid Alijani's avatar
Farid Alijani committed
314
% grid on
Farid Alijani's avatar
Farid Alijani committed
315
316
l2 = legend('$\dot{x}_{Rob}$ = 0.1[m/s]','$\dot{x}_{Rob}$ = 0.15[m/s]','$\dot{x}_{Rob}$ = 0.16[m/s]','Target','Location','northwest');
set(l2,'interpreter','latex','FontSize',14);
Farid Alijani's avatar
Farid Alijani committed
317
legend('boxoff');
Farid Alijani's avatar
Farid Alijani committed
318
319
320
321
322
323
324
325
326

subplot(1,3,2);
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);
Farid Alijani's avatar
Farid Alijani committed
327
title('SM zone + Target area');
Farid Alijani's avatar
Farid Alijani committed
328
xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',16);
329
% ylabel('${X}$ [m]','interpreter','latex');
Farid Alijani's avatar
Farid Alijani committed
330
axis([-.033 -.01 -.26 -.195]);
Farid Alijani's avatar
Farid Alijani committed
331
% grid on
Farid Alijani's avatar
Farid Alijani committed
332

Farid Alijani's avatar
Farid Alijani committed
333
subplot(133);
Farid Alijani's avatar
Farid Alijani committed
334
335
336
337
338
339
340
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);
Farid Alijani's avatar
Farid Alijani committed
341
title('Target area');
Farid Alijani's avatar
Farid Alijani committed
342
% xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',14);
343
% ylabel('${X}$ [m]','interpreter','latex');
Farid Alijani's avatar
Farid Alijani committed
344
axis([-.0205 -.0175 -.206 -.20]);
Farid Alijani's avatar
Farid Alijani committed
345
% grid on
346
% l2 = legend('x_dot = 0.15','x_dot = 0.1','x_dot = 0.16','Reference Position');
Farid Alijani's avatar
Farid Alijani committed
347
348

figure;
Farid Alijani's avatar
Farid Alijani committed
349
350
set(gcf,'color','white');

Farid Alijani's avatar
Farid Alijani committed
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
subplot(2,1,1);
plot(diff(TimeP_ros_data_1)/1e9,'g')
hold on
plot (diff(t_P_sec_data_1),'k--')
title('Position sampling time');
ylabel('Sampling time');
legend('ROS calc.','Real time calc. [sec]','Orientation','horizontal');
% grid on
% axis([0 428 .05 .15]);

subplot(2,1,2);
plot(diff(TimeV_ros_data_1)/1e9,'g');
hold on
plot (diff(t_V_sec_data_1),'k--')
title('Velocity sampling time');
xlabel('samples');
ylabel('Sampling time');
% legend('ROS calc.','Ordinary calc.','Orientation','horizontal');
% grid on
% axis([0 428 -.005 .025]);
% 
% subplot(3,1,3);
% plot(diff(TimeV_ros_data_1)/1e9,'g')
% hold on
% plot (diff(t_V_sec_data_1),'k--')
% xlabel('samples');
% ylabel('time step');
% axis([150 250 0 .02]);
Farid Alijani's avatar
Farid Alijani committed
379
380
381
382
383
384
385
386
387
388
389
390
391
392
393
394
395
396
397
398
399
400
401
% figure;
% set(gcf,'color','white');
% 
% 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