AUV-Coop-Assembly
Master Thesis for Robotics Engineering. Cooperative peg-in-hole assembly with two underwater vehicles guided by vision
plotTransformMatricesSlide.m
1 function plotTransformMatricesSlide(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 fileName = "wTt.txt";
12 wTt = importMatrices(strcat(rootPath, coordName, fileName));
13 fileNamegoal = "wTgoal.txt";
14 wTgoal = importMatrices(strcat(rootPath, coordName, fileNamegoal));
15 nStep = min(size(wTgoal, 3), size(wTt, 3));
16 
17 %w_yLimPos = wTt(1:3,1:3,1) * [0; holeDim/2; 0]
18 %w_yLimNeg = wTt(1:3,1:3,1) * [0; -holeDim/2; 0]
19 %w_zLimPos = wTt(1:3,1:3,1) * [0; 0; holeDim/2]
20 %w_zLimNeg = wTt(1:3,1:3,1) * [0; 0; -holeDim/2]
21 
22 
23 global sControlLoop
24 totSecondPassed = sControlLoop*(nStep-1);
25 seconds = 0:sControlLoop:totSecondPassed;
26 
27 global secInsertion
28 
29 toolLin = wTt(1:3, 4, :);
30 toolLinSqueezed = squeeze(toolLin(1:3,:,:));
31 toolAng = zeros(3, nStep);
32 for i = 1:nStep
33  toolAng(1:3,i) = rotm2eul( wTt(1:3,1:3,i), 'XYZ');
34 end
35 
36 goalLin = wTgoal(1:3, 4, :);
37 goalLinSqueezed = squeeze(goalLin(1:3,:,:));
38 goalAng = zeros(3, nStep);
39 for i = 1:nStep
40  goalAng(1:3,i) = rotm2eul( wTgoal(1:3,1:3,i), 'XYZ');
41 end
42 
43 %%
44 figure('Renderer', 'painters', 'Position', [0 0 2000 450])
45 %% plot X lin
46 subplot(1,3,1);
47 plot(seconds, toolLinSqueezed(1,:));
48 hold on;
49 plot(seconds, goalLinSqueezed(1,:));
50 plot([seconds(1), seconds(end)], [wLinGReal(1), wLinGReal(1)]);
51 %vertical line to show where insertion begin
52 plot([secInsertion; secInsertion], [0.96, 1.04]', '--m');
53 text([secInsertion+2], [1.035], {'\rightarrow Inside the hole'}, 'Color','magenta', 'FontSize',14);
54 hold off;
55 xlab1 = xlabel('time [s]');
56 ylab1 = ylabel('Vectors (X) [m]');
57 ylim([0.96, 1.04]);
58 xlim([0, 200]);
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(1,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], [-9.55], {'\rightarrow Inside the hole'}, 'Color','magenta', 'FontSize',14);
73 hold off;
74 xlab2 = xlabel('time [s]');
75 ylab2 = ylabel('Vectors (Y) [m]');
76 xlim([0, 200]);
77 tq2 = title("Y Position of tool, goal and goalReal");
78 leg2 = legend('$y_{tool}$', '$y_{goal}$', '$y_{goalReal}$');
79 
80 %% plot z lin
81 subplot(1,3,3);
82 plot(seconds, toolLinSqueezed(3,:));
83 hold on;
84 plot(seconds, goalLinSqueezed(3,:));
85 plot([seconds(1), 200], [wLinGReal(3), wLinGReal(3)]);
86 %vertical line to show where insertion begin
87 plot([secInsertion; secInsertion], [ylim]', '--m');
88 text([secInsertion+2], [8.397], {'\rightarrow Inside the hole'}, 'Color','magenta', 'FontSize',14);
89 hold off;
90 xlab3 = xlabel('time [s]');
91 ylab3 = ylabel('Vectors (Z) [m]');
92 ylim([8.365, 8.4]);
93 xlim([0, 200]);
94 tq3 = title("Z Position of tool, goal and goalReal");
95 leg3 = legend('$z_{tool}$', '$z_{goal}$', '$z_{goalReal}$');
96 
97 
98 set(ylab1, 'Interpreter', 'latex', 'FontSize' , ylabFontSize);
99 set (tq1, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
100 set(ylab2, 'Interpreter', 'latex', 'FontSize' , ylabFontSize);
101 set (tq2, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
102 set(ylab3, 'Interpreter', 'latex', 'FontSize' , ylabFontSize);
103 set (tq3, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
104 set(leg1, 'Interpreter', 'latex', 'FontSize' , legFontSize);
105 set(leg2, 'Interpreter', 'latex', 'FontSize' , legFontSize);
106 set(leg3, 'Interpreter', 'latex', 'FontSize' , legFontSize);
107 set(xlab1, 'Interpreter', 'latex', 'FontSize' , xlabFontSize);
108 set(xlab2, 'Interpreter', 'latex', 'FontSize' , xlabFontSize);
109 set(xlab3, 'Interpreter', 'latex', 'FontSize' , xlabFontSize);
110 
111 
112 
113 
114 
115 
116 %% old plot ang all togheter
117 % subplot(2,3,[4,5,6]);
118 % plot(seconds, toolAng);
119 % hold on
120 % plot(seconds, goalAng);
121 % hold off;
122 % xlab = xlabel('time [s]');
123 % ylab2 = ylabel('tool and goal angular position [rad]');
124 % tq2 = title("Angular position of tool and goal");
125 % leg2 = legend('$roll_tool$','$pitch_tool$', '$yaw_tool$', '$roll_goal$', '$pitch_goal$', '$yaw_goal$');
126 %
127 %
128 % set(ylab1, 'Interpreter', 'none', 'FontSize' , ylabFontSize);
129 % set (tq1, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
130 % set(ylab2, 'Interpreter', 'none', 'FontSize' , ylabFontSize);
131 % set (tq2, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
132 % set(leg1, 'Interpreter', 'latex', 'FontSize' , legFontSize);
133 % set(leg2, 'Interpreter', 'latex', 'FontSize' , legFontSize);
134 %
135 % hold off
136 %
137 %
Generated by   doxygen 1.8.11