AUV-Coop-Assembly
Master Thesis for Robotics Engineering. Cooperative peg-in-hole assembly with two underwater vehicles guided by vision
plotTransformMatrices.m
1 function plotTransformMatrices(rootPath, coordName)
2 
3 legFontSize = 19;
4 titleFontSize = 18;
5 ylabFontSize = 17;
6 xlabFontSize = 17;
7 
8 
9 wLinGReal = [1.02545; -10.1984; 8.37884];
10 
11 wAngGReal = [0; 0; -1.443185307179587];
12 
13 fileName = "wTt.txt";
14 wTt = importMatrices(strcat(rootPath, coordName, fileName));
15 fileNamegoal = "wTgoal.txt";
16 wTgoal = importMatrices(strcat(rootPath, coordName, fileNamegoal));
17 nStep = min(size(wTgoal, 3), size(wTt, 3));
18 
19 %w_yLimPos = wTt(1:3,1:3,1) * [0; holeDim/2; 0]
20 %w_yLimNeg = wTt(1:3,1:3,1) * [0; -holeDim/2; 0]
21 %w_zLimPos = wTt(1:3,1:3,1) * [0; 0; holeDim/2]
22 %w_zLimNeg = wTt(1:3,1:3,1) * [0; 0; -holeDim/2]
23 
24 
25 global sControlLoop
26 totSecondPassed = sControlLoop*(nStep-1);
27 seconds = 0:sControlLoop:totSecondPassed;
28 
29 global secInsertion
30 
31 toolLin = wTt(1:3, 4, :);
32 toolLinSqueezed = squeeze(toolLin(1:3,:,:));
33 toolAng = zeros(3, nStep);
34 for i = 1:nStep
35  toolAng(1:3,i) = rotm2eul( wTt(1:3,1:3,i), 'XYZ');
36 end
37 
38 goalLin = wTgoal(1:3, 4, :);
39 goalLinSqueezed = squeeze(goalLin(1:3,:,:));
40 goalAng = zeros(3, nStep);
41 for i = 1:nStep
42  goalAng(1:3,i) = rotm2eul( wTgoal(1:3,1:3,i), 'XYZ');
43 end
44 
45 %%
46 figure('Renderer', 'painters', 'Position', [0 0 2000 900])
47 %% plot X lin
48 subplot(2,3,1);
49 plot(seconds, toolLinSqueezed(1,:));
50 hold on;
51 plot(seconds, goalLinSqueezed(1,:));
52 plot([seconds(1), seconds(end)], [wLinGReal(1), wLinGReal(1)]);
53 %vertical line to show where insertion begin
54 plot([secInsertion; secInsertion], [ylim]', '--m');
55 text([secInsertion+2], [1.07], {'\rightarrow Inside the hole'}, 'Color','magenta', 'FontSize',14);
56 hold off;
57 xlab1 = xlabel('time [s]');
58 ylab1 = ylabel('Vectors (X) [m]');
59 tq1 = title("X Position of tool, goal and goalReal");
60 leg1 = legend('$x_{tool}$', '$x_{goal}$', '$x_{goalReal}$');
61 
62 
63 %% plot y lin
64 subplot(2,3,2);
65 plot(seconds, toolLinSqueezed(2,:));
66 hold on;
67 plot(seconds, goalLinSqueezed(2,:));
68 plot([seconds(1), seconds(end)], [wLinGReal(2), wLinGReal(2)]);
69 plot([secInsertion; secInsertion], [ylim]', '--k');
70 %vertical line to show where insertion begin
71 plot([secInsertion; secInsertion], [ylim]', '--m');
72 text([secInsertion+2], [-5.32], {'\rightarrow Inside the hole'}, 'Color','magenta', 'FontSize',14);
73 hold off;
74 xlab2 = xlabel('time [s]');
75 ylab2 = ylabel('Vectors (Y) [m]');
76 tq2 = title("Y Position of tool, goal and goalReal");
77 leg2 = legend('$y_{tool}$', '$y_{goal}$', '$y_{goalReal}$');
78 
79 %% plot z lin
80 subplot(2,3,3);
81 plot(seconds, toolLinSqueezed(3,:));
82 hold on;
83 plot(seconds, goalLinSqueezed(3,:));
84 plot([seconds(1), seconds(end)], [wLinGReal(3), wLinGReal(3)]);
85 %vertical line to show where insertion begin
86 plot([secInsertion; secInsertion], [ylim]', '--m');
87 text([secInsertion+2], [8.465], {'\rightarrow Inside the hole'}, 'Color','magenta', 'FontSize',14);
88 hold off;
89 xlab3 = xlabel('time [s]');
90 ylab3 = ylabel('Vectors (Z) [m]');
91 tq3 = title("Z Position of tool, goal and goalReal");
92 leg3 = legend('$z_{tool}$', '$z_{goal}$', '$z_{goalReal}$');
93 
94 %% plot x ang
95 subplot(2, 3, 4);
96 plot(seconds, toolAng(1,:));
97 hold on
98 plot(seconds, goalAng(1,:));
99 plot([seconds(1), seconds(end)], [wAngGReal(1), wAngGReal(1)]);
100 %vertical line to show where insertion begin
101 plot([secInsertion; secInsertion], [ylim]', '--m');
102 text([secInsertion+2], [0.0003], {'\rightarrow Inside the hole'}, 'Color','magenta', 'FontSize',14);
103 hold off;
104 xlab4 = xlabel('time [s]');
105 ylab4 = ylabel('Vectors (roll) [rad]');
106 tq4 = title("Roll of tool, goal and goalReal");
107 leg4 = legend('$roll_{tool}$', '$roll_{goal}$', '$roll_{goalReal}$');
108 
109 %% plot y ang
110 subplot(2, 3, 5);
111 plot(seconds, toolAng(2,:));
112 hold on
113 plot(seconds, goalAng(2,:));
114 plot([seconds(1), seconds(end)], [wAngGReal(2), wAngGReal(2)]);
115 %vertical line to show where insertion begin
116 plot([secInsertion; secInsertion], [ylim]', '--m');
117 text([secInsertion+2], [0.00012], {'\rightarrow Inside the hole'}, 'Color','magenta', 'FontSize',14);
118 hold off;
119 xlab5 = xlabel('time [s]');
120 ylab5 = ylabel('Vectors (pitch) [rad]');
121 tq5 = title("Pitch of tool, goal and goalReal");
122 leg5 = legend('$pitch_{tool}$', '$pitch_{goal}$', '$pitch_{goalReal}$');
123 
124 %% plot z ang
125 subplot(2, 3, 6);
126 plot(seconds, toolAng(3,:));
127 hold on
128 plot(seconds, goalAng(3,:));
129 plot([seconds(1), seconds(end)], [wAngGReal(3), wAngGReal(3)]);
130 %vertical line to show where insertion begin
131 plot([secInsertion; secInsertion], [ylim]', '--m');
132 text(secInsertion+2, -1.4475, '\rightarrow Inside the hole', 'Color','magenta', 'FontSize',14);
133 hold off;
134 ylim([-1.48, -1.44]);
135 xlab6 = xlabel('time [s]');
136 ylab6 = ylabel('Vectors (yaw) [rad]');
137 tq6 = title("Yaw of tool, goal and goalReal");
138 leg6 = legend('$yaw_{tool}$', '$yaw_{goal}$', '$yaw_{goalReal}$');
139 
140 set(ylab1, 'Interpreter', 'latex', 'FontSize' , ylabFontSize);
141 set (tq1, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
142 set(ylab2, 'Interpreter', 'latex', 'FontSize' , ylabFontSize);
143 set (tq2, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
144 set(ylab3, 'Interpreter', 'latex', 'FontSize' , ylabFontSize);
145 set (tq3, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
146 set(ylab4, 'Interpreter', 'latex', 'FontSize' , ylabFontSize);
147 set (tq4, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
148 set(ylab5, 'Interpreter', 'latex', 'FontSize' , ylabFontSize);
149 set (tq5, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
150 set(ylab6, 'Interpreter', 'latex', 'FontSize' , ylabFontSize);
151 set (tq6, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
152 set(leg1, 'Interpreter', 'latex', 'FontSize' , legFontSize);
153 set(leg2, 'Interpreter', 'latex', 'FontSize' , legFontSize);
154 set(leg3, 'Interpreter', 'latex', 'FontSize' , legFontSize);
155 set(leg4, 'Interpreter', 'latex', 'FontSize' , legFontSize);
156 set(leg5, 'Interpreter', 'latex', 'FontSize' , legFontSize);
157 set(leg6, 'Interpreter', 'latex', 'FontSize' , legFontSize);
158 set(xlab1, 'Interpreter', 'latex', 'FontSize' , xlabFontSize);
159 set(xlab2, 'Interpreter', 'latex', 'FontSize' , xlabFontSize);
160 set(xlab3, 'Interpreter', 'latex', 'FontSize' , xlabFontSize);
161 set(xlab4, 'Interpreter', 'latex', 'FontSize' , xlabFontSize);
162 set(xlab5, 'Interpreter', 'latex', 'FontSize' , xlabFontSize);
163 set(xlab6, 'Interpreter', 'latex', 'FontSize' , xlabFontSize);
164 
165 
166 
167 
168 
169 
170 %% old plot ang all togheter
171 % subplot(2,3,[4,5,6]);
172 % plot(seconds, toolAng);
173 % hold on
174 % plot(seconds, goalAng);
175 % hold off;
176 % xlab = xlabel('time [s]');
177 % ylab2 = ylabel('tool and goal angular position [rad]');
178 % tq2 = title("Angular position of tool and goal");
179 % leg2 = legend('$roll_tool$','$pitch_tool$', '$yaw_tool$', '$roll_goal$', '$pitch_goal$', '$yaw_goal$');
180 %
181 %
182 % set(ylab1, 'Interpreter', 'none', 'FontSize' , ylabFontSize);
183 % set (tq1, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
184 % set(ylab2, 'Interpreter', 'none', 'FontSize' , ylabFontSize);
185 % set (tq2, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
186 % set(leg1, 'Interpreter', 'latex', 'FontSize' , legFontSize);
187 % set(leg2, 'Interpreter', 'latex', 'FontSize' , legFontSize);
188 %
189 % hold off
190 %
191 %
Generated by   doxygen 1.8.11