docking_different_angle.m 8.6 KB
Newer Older
1
2
3
4
5
clc;
close all;
clear all;
%% Import data and time conversion

Farid Alijani's avatar
Farid Alijani committed
6
% data 1: 
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29

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

% -----------------------------------------------------------------------------------------------------
Farid Alijani's avatar
Farid Alijani committed
30
% data 2: 
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54

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

% -----------------------------------------------------------------------------------------------------
Farid Alijani's avatar
Farid Alijani committed
55
% data 3: 
56
57
58
Pose_Matrix_data_3 = csvread('Pose_right.txt',1,0);
Vel_Matrix_data_3 = csvread('Velocity_right.txt',1,0);

Farid Alijani's avatar
Farid Alijani committed
59

60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
% 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
85
Theta_data_1 = Pose_Matrix_data_1(:,7);
86
87
88
89
90
91
92
93
94
95
96
97


% 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
98
Theta_data_2 = Pose_Matrix_data_2(:,7);
99
100
101
102
103
104
105
106
107
108
109
110


% 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
111
Theta_data_3 = Pose_Matrix_data_3(:,7);
112
113
114
115
116
117
118
119

% 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 
Farid Alijani's avatar
Farid Alijani committed
120
thresh_X = .0013;
121
122
theta = 0:.001:2*pi;
% needs to be adjusted manually if docking platform is replaced!
Farid Alijani's avatar
Farid Alijani committed
123
124
ref_x = -.2025;
ref_y = -.0185;
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
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);
Farid Alijani's avatar
Farid Alijani committed
143
Vel_Y_data_3 = abs(Vel_Matrix_data_3(:,3));
144
145
146
147
148
149
Omega_Z_data_3 = Vel_Matrix_data_3(:,7);

%% Plots

% marker position
figure;
Farid Alijani's avatar
Farid Alijani committed
150
151
set(gcf,'color','white');

152
153
154
155
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);
Farid Alijani's avatar
Farid Alijani committed
156
157
% hold on
% plot(t_P_sec_data_3,Pose_X_data_3,'g','LineWidth',1.6);
158

Farid Alijani's avatar
Farid Alijani committed
159
title('Camera Pose Estimation');
160
ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',14);
Farid Alijani's avatar
Farid Alijani committed
161

Farid Alijani's avatar
Farid Alijani committed
162
l2 = legend('Left', 'Right');
163
set(l2,'interpreter','latex','FontSize',11);
Farid Alijani's avatar
Farid Alijani committed
164
% legend('boxoff');
165
166
167
168
169

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);
Farid Alijani's avatar
Farid Alijani committed
170
171
hold on
plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',1.6);
172
ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',14);
173
174
175
176
177

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);
Farid Alijani's avatar
Farid Alijani committed
178
179
hold on
plot(t_P_sec_data_3,Theta_data_3,'g','LineWidth',1.6);
180

Farid Alijani's avatar
Farid Alijani committed
181
xlabel('$ time [s]$','interpreter','latex','FontSize',14);
182
ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',14);
183
184
185

% control signals
figure;
Farid Alijani's avatar
Farid Alijani committed
186
187
set(gcf,'color','white');

Farid Alijani's avatar
Farid Alijani committed
188
subplot(311);
189
190
191
192

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);
Farid Alijani's avatar
Farid Alijani committed
193
194
hold on
plot(t_V_sec_data_3,Vel_X_data_3,'g','LineWidth',1.6);
195

Farid Alijani's avatar
Farid Alijani committed
196
title('Control Signals');
197
ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);
Farid Alijani's avatar
Farid Alijani committed
198
l2 = legend('${Left}$', '${Right}$');
199
set(l2,'interpreter','latex','FontSize',11);
Farid Alijani's avatar
Farid Alijani committed
200
legend('boxoff')
201
202
203
204
205

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);
Farid Alijani's avatar
Farid Alijani committed
206
207
% hold on
% plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',1.6);
208
ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',14);
Farid Alijani's avatar
Farid Alijani committed
209

210
211
212
213
214

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);
Farid Alijani's avatar
Farid Alijani committed
215
216
217
hold on
plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.6);
xlabel('$ time [s]$','interpreter','latex','FontSize',14);
218
ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',14);
219
220
221
222

% Trajectory 

figure;
Farid Alijani's avatar
Farid Alijani committed
223
224
set(gcf,'color','white');

225
226
subplot(1,3,1);
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
227
228
229
% hold on
% quiver(Pose_Y_data_1,Pose_X_data_1,gradient(Pose_Y_data_1),gradient(Pose_X_data_1),'r--','LineWidth',.01);

230
231
hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
232
233
% hold on
% quiver(Pose_Y_data_2,Pose_X_data_2,gradient(Pose_Y_data_2),gradient(Pose_X_data_2),'m--','LineWidth',.01);
Farid Alijani's avatar
Farid Alijani committed
234
235
236
% 
hold on
plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
237
238
239
% hold on
% quiver(Pose_Y_data_3,Pose_X_data_3,gradient(Pose_Y_data_3),gradient(Pose_X_data_3),'k--','LineWidth',.01);

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

Farid Alijani's avatar
Farid Alijani committed
243
title('Approach zone + SM zone + Target area');
244
ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',14);
Farid Alijani's avatar
Farid Alijani committed
245
% axis([-.4 .4 .18 1.4]);
246
247
248
249
250

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);
Farid Alijani's avatar
Farid Alijani committed
251
252
hold on
plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',1.6);
253
254
255
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);

Farid Alijani's avatar
Farid Alijani committed
256
title('SM zone + Target area');
257
xlabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',14);
Farid Alijani's avatar
Farid Alijani committed
258
axis([-.03 0.03 -.29 -.191]);
259
260
261
262
263

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);
Farid Alijani's avatar
Farid Alijani committed
264
265
% hold on
% plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
266
267
268
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);

Farid Alijani's avatar
Farid Alijani committed
269
title('Target area');
Farid Alijani's avatar
Farid Alijani committed
270
271
axis([-.0205 -.0168 -.2054 -.199]);
l = legend('${Left}$', '${Right}$','Target');
272
273
274
275
276
277
278
set(l,'interpreter','latex','FontSize',11);



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)