docking_different_angle.m 9.25 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');

Farid Alijani's avatar
Farid Alijani committed
152
subplot(321);
153
154
155
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);
Farid Alijani's avatar
Farid Alijani committed
158
159
% ylabel('${x_{mar}}$ [m]','interpreter','latex','FontSize',20);
% title('a) ${x_{mar}}$','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
160
% set(gca,'fontsize',20);
161

Farid Alijani's avatar
Farid Alijani committed
162
subplot(323);
163
164
165
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
166
167
% hold on
% plot(t_P_sec_data_3,Pose_Y_data_3,'g','LineWidth',1.6);
Farid Alijani's avatar
Farid Alijani committed
168
169
% ylabel('${y_{mar}}$ [m]','interpreter','latex','FontSize',20);
% title('b) ${y_{mar}}$','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
170
% set(gca,'fontsize',20);
171

Farid Alijani's avatar
Farid Alijani committed
172
subplot(325);
173
174
175
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
176
177
% hold on
% plot(t_P_sec_data_3,Theta_data_3,'g','LineWidth',1.6);
178

Farid Alijani's avatar
Farid Alijani committed
179
180
181
% title('c) ${\theta_{mar}}$','interpreter','latex','FontSize',20);
% ylabel('${\theta_{mar}}$ [rad]','interpreter','latex','FontSize',20);
% xlabel('$ time$ [s]','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
182
% set(gca,'fontsize',20);
183

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

Farid Alijani's avatar
Farid Alijani committed
188
subplot(322);
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);
Farid Alijani's avatar
Farid Alijani committed
195
196
197
% 
% title('d) $\dot{x}_{Rob}$','interpreter','latex','FontSize',20);
% ylabel('$\dot{x}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
198

Farid Alijani's avatar
Farid Alijani committed
199
l2 = legend('${Left}$', '${Right}$');
Farid Alijani's avatar
Farid Alijani committed
200
set(l2,'interpreter','latex','FontSize',14);
Farid Alijani's avatar
Farid Alijani committed
201
legend('boxoff')
Farid Alijani's avatar
Farid Alijani committed
202
axis ([0 40 -.01 .18]);
Farid Alijani's avatar
Farid Alijani committed
203
% set(gca,'fontsize',20);
204

Farid Alijani's avatar
Farid Alijani committed
205
subplot(324);
206
207
208
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
209
210
% hold on
% plot(t_V_sec_data_3,Vel_Y_data_3,'g','LineWidth',1.6);
Farid Alijani's avatar
Farid Alijani committed
211
212
% ylabel('$\dot{y}_{Rob}$ [m/s]','interpreter','latex','FontSize',20);
% title('e) $\dot{y}_{Rob}$','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
213
% set(gca,'fontsize',20);
214

Farid Alijani's avatar
Farid Alijani committed
215
subplot(326);
216
217
218
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
219
220
% hold on
% plot(t_V_sec_data_3,Omega_Z_data_3,'g','LineWidth',1.6);
Farid Alijani's avatar
Farid Alijani committed
221
222
223
% xlabel('$ time$ [s]','interpreter','latex','FontSize',20);
% ylabel('$\dot{\theta}_{Rob}$ [rad/s]','interpreter','latex','FontSize',20);
% title('f) $\dot{\theta}_{Rob}$','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
224
% set(gca,'fontsize',20);
225
226
227
228

% Trajectory 

figure;
Farid Alijani's avatar
Farid Alijani committed
229
230
set(gcf,'color','white');

Farid Alijani's avatar
Farid Alijani committed
231
subplot(131);
232
plot(Pose_Y_data_1,Pose_X_data_1,'b','LineWidth',2);
233
234
235
% hold on
% quiver(Pose_Y_data_1,Pose_X_data_1,gradient(Pose_Y_data_1),gradient(Pose_X_data_1),'r--','LineWidth',.01);

236
237
hold on
plot(Pose_Y_data_2,Pose_X_data_2,'r','LineWidth',2);
238
239
% 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
240
241
242
% % 
% hold on
% plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
243
244
245
% hold on
% quiver(Pose_Y_data_3,Pose_X_data_3,gradient(Pose_Y_data_3),gradient(Pose_X_data_3),'k--','LineWidth',.01);

246
247
248
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);

Farid Alijani's avatar
Farid Alijani committed
249
% axis([-.4 .4 .18 1.4]);
Farid Alijani's avatar
Farid Alijani committed
250
251
% title('${a) Approach  + SM  + Target}$','interpreter','latex','FontSize',20);
% ylabel('${x}_{mar}$ [m]','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
252
% set(gca,'fontsize',20);
253

Farid Alijani's avatar
Farid Alijani committed
254
subplot(132);
255
256
257
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
258
259
% hold on
% plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',1.6);
260
261
262
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);

Farid Alijani's avatar
Farid Alijani committed
263
264
% title('$ {b) SM + Target}$','interpreter','latex','FontSize',20);
% xlabel('${y}_{mar}$ [m]','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
265
axis([-.03 0.03 -.29 -.191]);
Farid Alijani's avatar
Farid Alijani committed
266
% set(gca,'fontsize',20);
267

Farid Alijani's avatar
Farid Alijani committed
268
subplot(133);
269
270
271
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
272
273
% hold on
% plot(Pose_Y_data_3,Pose_X_data_3,'g','LineWidth',2);
274
275
276
hold on
plot(y_circle,x_circle,'k--','LineWidth',3.4);

Farid Alijani's avatar
Farid Alijani committed
277
% title('$ {c) Target}$','interpreter','latex','FontSize',20);
Farid Alijani's avatar
Farid Alijani committed
278
axis([-.0202 -.017 -.2054 -.199]);
Farid Alijani's avatar
Farid Alijani committed
279
% set(gca,'fontsize',20);
280

Farid Alijani's avatar
Farid Alijani committed
281
282
283
l = legend('${Left}$', '${Right}$','Target');
set(l,'interpreter','latex','FontSize',14);
legend('boxoff');
284
285
286
287
288


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)