AUV-Coop-Assembly
Master Thesis for Robotics Engineering. Cooperative peg-in-hole assembly with two underwater vehicles guided by vision
plotTask.m
1 function plotTask(rootPath, robotName, taskName)
2 
3 filename = strcat(rootPath, robotName, "/", taskName);
4 legFontSize = 22;
5 titleFontSize = 18;
6 ylabFontSize = 19;
7 xlabFontSize = 19;
8 
9 
10 
11 refs = importMatrices(strcat(filename, '/reference.txt'));
12 act = importMatrices(strcat(filename, '/activation.txt'));
13 
14 nRow = size(refs, 1);
15 nStep = size(refs, 3);
16 
17 %millisecond indicated in missionManager
18 global sControlLoop;
19 totSecondPassed = sControlLoop*(nStep-1);
20 seconds = 0:sControlLoop:totSecondPassed;
21 
22 global secInsertion
23 
24 %% plot refs (vector n*1)
25 % figure('Renderer', 'painters', 'Position', [0 0 650 500])
26 % hold on;
27 % for i = 1:nRow
28 % a(:) = refs(i,1,:);
29 % plot(seconds, a);
30 % xlim([secInsertion;180]);
31 % end
32 % xlab = xlabel('time [s]');
33 %
34 % if strcmp(taskName, 'PIPE_REACHING_GOAL')
35 % leg = legend('$\dot{x}$','$\dot{y}$', '$\dot{z}$', '$w_x$', '$w_y$', '$w_z$');
36 % ylab = ylabel('references [m/s], [rad/s]');
37 % elseif strcmp(taskName, 'JOINT_LIMIT')
38 % leg = legend('$\dot{q}_1$','$\dot{q}_2$', '$\dot{q}_3$', '$\dot{q}_4$');
39 % ylab = ylabel('references [rad/s]');
40 % elseif strcmp(taskName, 'HORIZONTAL_ATTITUDE')
41 % leg = legend('$\dot{\bar{x}}$');
42 % ylab = ylabel('references');
43 % elseif strcmp(taskName, 'ARM_SHAPE')
44 % if nRow == 1 % scalar type (norm)
45 % leg = legend('$\dot{\bar{x}}$');
46 % ylab = ylabel('reference (norm)');
47 % else
48 % leg = legend('$\dot{q}_1$','$\dot{q}_2$', '$\dot{q}_3$', '$\dot{q}_4$');
49 % ylab = ylabel('references [rad/s]');
50 % end
51 % elseif strcmp(taskName, 'FORCE_INSERTION')
52 % if nRow == 1 %only force norm
53 % leg = legend('$\dot{\bar{x}}$');
54 % ylab = ylabel('reference (norm)');
55 %
56 % elseif nRow == 2 %norm of force and torque. BEST METHOD
57 % leg = legend('$\dot{\bar{x}}_f$', '$\dot{\bar{x}}_m$');
58 % ylab = ylabel('reference');
59 % if strcmp(robotName, 'g500_A')
60 % tr = title('Robot A: Force-Torque Task Reference');
61 % else
62 % tr = title('Robot B: Force-Torque Task Reference');
63 % end
64 %
65 % elseif nRow == 3 % only force
66 % leg = legend('$\dot{x}$','$\dot{y}$', '$\dot{z}$');
67 % ylab = ylabel('reference []');
68 % end
69 %
70 % end
71 %
72 % if ~strcmp(taskName, 'FORCE_INSERTION')
73 % tr = title(strcat(robotName, " ", taskName));
74 % end
75 % set(leg, 'Interpreter', 'latex', 'FontSize' , legFontSize );
76 % set (tr, 'Interpreter', 'latex', 'FontSize', titleFontSize);
77 % set (ylab, 'Interpreter', 'latex', 'FontSize', ylabFontSize);
78 % set (xlab, 'Interpreter', 'latex', 'FontSize', xlabFontSize);
79 %
80 %
81 % %% plot errors
82 % % error = importMatrices(strcat(filename, '/error.txt'));
83 % % nRow = size(refs, 1);
84 % % nStep = size(refs, 3);
85 % %
86 % % %millisecond indicated in missionManager
87 % % totSecondPassed = sControlLoop*(nStep-1);
88 % % seconds = 0:sControlLoop:totSecondPassed;
89 % %
90 % % figure
91 % % hold on;
92 % % for i = 1:nRow
93 % % a(:) = error(i,1,:);
94 % % plot(seconds, a);
95 % % end
96 % % xlab = xlabel('time [s]');
97 % %
98 % % if strcmp(taskName, 'PIPE_REACHING_GOAL')
99 % % leg = legend('$x_{err}$','$y_{err}$', '$z_{err}$', '$pitch_{err}$', '$yaw_{err}$');
100 % % ylab = ylabel('error [m],[rad]');
101 % % elseif strcmp(taskName, 'JOINT_LIMIT')
102 % % leg = legend('$q1_{err}$','$q2_{err}$', '$q3_{err}$', '$q4_{err}$');
103 % % ylab = ylabel('error [rad]');
104 % % elseif strcmp(taskName, 'ARM_SHAPE')
105 % % if nRow == 1 % scalar type (norm)
106 % % leg = legend('$norm_{err}$');
107 % % ylab = ylabel('error (norm)');
108 % % else
109 % % leg = legend('$q1_{err}$','$q2_{err}$', '$q3_{err}$', '$q4_{err}$');
110 % % ylab = ylabel('errors [rad]');
111 % % end
112 % % elseif strcmp(taskName, 'FORCE_INSERTION')
113 % % if nRow == 1 %only force norm
114 % % leg = legend('$norm_{err}$');
115 % % ylab = ylabel('error (norm)');
116 % %
117 % % elseif nRow == 2 %norm of force and torque. BEST METHOD
118 % % leg = legend('$err_f$', '$err_m$');
119 % % ylab = ylabel('error (norm)');
120 % %
121 % % elseif nRow == 3 % only force
122 % % leg = legend('$x_{err}$','$y_{err}$', '$z_{err}$');
123 % % ylab = ylabel('error [m]');
124 % % end
125 % % end
126 % %
127 % % te = title(strcat(robotName, " ", taskName));
128 % % set(leg, 'Interpreter', 'latex', 'FontSize' , legFontSize);
129 % % set (te, 'Interpreter', 'latex', 'FontSize', titleFontSize);
130 % % set (ylab, 'Interpreter', 'latex', 'FontSize', ylabFontSize);
131 % %
132 % % hold off;
133 %
134 %
135 % %% plot activations
136 % act = importMatrices(strcat(filename, '/activation.txt'));
137 % nRow = size(refs, 1);
138 % nStep = size(refs, 3);
139 %
140 % %millisecond indicated in missionManager
141 % totSecondPassed = sControlLoop*(nStep-1);
142 % seconds = 0:sControlLoop:totSecondPassed;
143 %
144 % figure('Renderer', 'painters', 'Position', [0 0 650 500])
145 % hold on;
146 % for i = 1:nRow
147 % a(:) = act(i,1,:);
148 % plot(seconds, a);
149 % xlim([secInsertion;180]);
150 %
151 % end
152 % xlab = xlabel('time [s]');
153 %
154 % if strcmp(taskName, 'JOINT_LIMIT')
155 % leg = legend('$A_{11}$','$A_{22}$', '$A_{33}$', '$A_{44}$');
156 % ylab = ylabel('activations');
157 % elseif strcmp(taskName, 'HORIZONTAL_ATTITUDE')
158 % leg = legend('$A$');
159 % ylab = ylabel('activation');
160 % elseif strcmp(taskName, 'ARM_SHAPE')
161 % if nRow == 1 % scalar type (norm)
162 % leg = legend('$A$');
163 % ylab = ylabel('activation');
164 % else
165 % leg = legend('$A_{11}$','$A_{22}$', '$A_{33}$', '$A_{44}$');
166 % ylab = ylabel('activations');
167 % end
168 % elseif strcmp(taskName, 'FORCE_INSERTION')
169 % if nRow == 1 % only norm force
170 % leg = legend('$A$');
171 % ylab = ylabel('activation');
172 % error (norm)
173 %
174 % elseif nRow == 2 %norm of force and torque. BEST METHOD
175 % leg = legend('$a_f$', '$a_m$');
176 % ylab = ylabel('activations');
177 % if strcmp(robotName, 'g500_A')
178 % ta = title('Robot A: Force-Torque Task Activation');
179 % else
180 % ta = title('Robot B: Force-Torque Task Activation');
181 % end
182 %
183 % elseif nRow == 3 % only force
184 % leg = legend('$A_{11}$','$A_{22}$', '$A_{33}$');
185 % ylab = ylabel('activations');
186 % end
187 % end
188 %
189 % if ~strcmp(taskName, 'FORCE_INSERTION')
190 % ta = title(strcat(robotName, " " ,taskName));
191 % end
192 % set(leg, 'Interpreter', 'latex', 'FontSize' , legFontSize);
193 % set (ta, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
194 % set (ylab, 'Interpreter', 'latex', 'FontSize', ylabFontSize);
195 % set (xlab, 'Interpreter', 'latex', 'FontSize', xlabFontSize);
196 
197 
198 %% plot activation and ref for force torque divided
199 if strcmp(taskName, 'FORCE_INSERTION')
200  actSqueez = squeeze(act);
201 
202  figure('Renderer', 'painters', 'Position', [0 0 710 550])
203 
204  subplot(2,1,1)
205  plot(seconds, actSqueez(1,:));
206  xlaba = xlabel('time [s]');
207  ylaba= ylabel('Activation');
208  if strcmp(robotName, 'g500_A')
209  ta = title('Robot A: Force-Torque Task Activation - Force part');
210  else
211  ta = title('Robot B: Force-Torque Task Activation - Force part');
212  end
213  xlim([secInsertion;180]);
214 
215  set (ta, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
216  set (ylaba, 'Interpreter', 'latex', 'FontSize', ylabFontSize);
217  set (xlaba, 'Interpreter', 'latex', 'FontSize', xlabFontSize);
218 
219  subplot(2,1,2)
220  plot(seconds, actSqueez(2,:));
221  xlaba = xlabel('time [s]');
222  ylaba= ylabel('Activation');
223  if strcmp(robotName, 'g500_A')
224  ta = title('Robot A: Force-Torque Task Activation - Torque part');
225  else
226  ta = title('Robot B: Force-Torque Task Activation - Torque part');
227  end
228  xlim([secInsertion;180]);
229 
230  set (ta, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
231  set (ylaba, 'Interpreter', 'latex', 'FontSize', ylabFontSize);
232  set (xlaba, 'Interpreter', 'latex', 'FontSize', xlabFontSize);
233 
234  refSqueez = squeeze(refs);
235  figure('Renderer', 'painters', 'Position', [0 0 710 550])
236 
237  subplot(2,1,1)
238  plot(seconds, refSqueez(1,:));
239  xlaba = xlabel('time [s]');
240  ylaba= ylabel('Reference');
241  if strcmp(robotName, 'g500_A')
242  ta = title('Robot A: Force-Torque Task Reference - Force part');
243  else
244  ta = title('Robot B: Force-Torque Task Reference - Force part');
245  end
246  xlim([secInsertion;180]);
247 
248  set (ta, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
249  set (ylaba, 'Interpreter', 'latex', 'FontSize', ylabFontSize);
250  set (xlaba, 'Interpreter', 'latex', 'FontSize', xlabFontSize);
251 
252  subplot(2,1,2)
253  plot(seconds, refSqueez(2,:));
254  xlaba = xlabel('time [s]');
255  ylaba= ylabel('Reference');
256  if strcmp(robotName, 'g500_A')
257  ta = title('Robot A: Force-Torque Task Reference - Torque part');
258  else
259  ta = title('Robot B: Force-Torque Task Reference - Torque part');
260  end
261  xlim([secInsertion;180]);
262 
263  set (ta, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
264  set (ylaba, 'Interpreter', 'latex', 'FontSize', ylabFontSize);
265  set (xlaba, 'Interpreter', 'latex', 'FontSize', xlabFontSize);
266 
267 
268 end
269 
270 %% plot jacob-1*ref (for ydot generated by the single task) only for force insertion for now
271 legFontSize = 19;
272 titleFontSize = 18;
273 ylabFontSize = 17;
274 xlabFontSize = 17;
275 
276 if strcmp(taskName, 'FORCE_INSERTION')
277  jacobs = importMatrices(strcat(rootPath, robotName, '/forceJacob.txt'));
278  yDotSingle = zeros(10,nStep);
279 
280  for i=1:nStep
281  yDotSingle(:,i) = pinv(jacobs(:,:,i)) * refs(:,:,i);
282  end
283 
284  figure('Renderer', 'painters', 'Position', [0 0 750 990])
285  subplot(3,1,1)
286  plot (seconds, yDotSingle(1:4,:))
287  xlab1 = xlabel('time [s]');
288  leg1 = legend('$\dot{q}_1$','$\dot{q}_2$', '$\dot{q}_3$', '$\dot{q}_4$');
289  ylab1 = ylabel('Joint velocities [rad/s]');
290  if strcmp(robotName, 'g500_A')
291  tq1 = title('Joint command for Robot A');
292  else
293  tq1 = title('Joint command for Robot B');
294  end
295  xlim([secInsertion;180]);
296 
297  set(leg1, 'Interpreter', 'latex', 'FontSize' , legFontSize);
298  set (tq1, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
299  set (ylab1, 'Interpreter', 'latex', 'FontSize', ylabFontSize);
300  set (xlab1, 'Interpreter', 'latex', 'FontSize', xlabFontSize);
301 
302  subplot(3,1,2);
303  plot (seconds, yDotSingle(5:7,:))
304  xlab2 = xlabel('time [s]');
305  leg2 = legend('$\dot{x}$','$\dot{y}$', '$\dot{z}$');
306  ylab2 = ylabel('Linear velocity [m/s]');
307  if strcmp(robotName, 'g500_A')
308  tq2 = title('Vehicle linear velocity command for Robot A');
309  else
310  tq2 = title('Vehicle linear velocity command for Robot B');
311  end
312  xlim([secInsertion;180]);
313 
314 
315  set(leg2, 'Interpreter', 'latex', 'FontSize' , legFontSize);
316  set (tq2, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
317  set (ylab2, 'Interpreter', 'latex', 'FontSize', ylabFontSize);
318  set (xlab2, 'Interpreter', 'latex', 'FontSize', xlabFontSize);
319 
320 
321  subplot(3,1,3);
322  plot (seconds, yDotSingle(8:10,:))
323  xlab3 = xlabel('time [s]');
324  leg3 = legend('$w_x$', '$w_y$', '$w_z$');
325  ylab3 = ylabel('Angular velocity [rad/s]');
326  if strcmp(robotName, 'g500_A')
327  tq3 = title('Vehicle angular velocity command for Robot A');
328  else
329  tq3 = title('Vehicle angular velocity command for Robot B');
330  end
331  xlim([secInsertion;180]);
332 
333 
334  set(leg3, 'Interpreter', 'latex', 'FontSize' , legFontSize);
335  set (tq3, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
336  set (ylab3, 'Interpreter', 'latex', 'FontSize', ylabFontSize);
337  set (xlab3, 'Interpreter', 'latex', 'FontSize', xlabFontSize);
338 
339 end
340 
341 
342 
Task
ABSTRACT class task. Each task is a derived class of this class. It contains all the variable that th...
Definition: task.h:17
Generated by   doxygen 1.8.11