AUV-Coop-Assembly
Master Thesis for Robotics Engineering. Cooperative peg-in-hole assembly with two underwater vehicles guided by vision
plotYDotDivided2.m
1 function plotYDotDivided2(rootPath, robotName, ydotName)
2 
3 legFontSize = 19;
4 titleFontSize = 18;
5 ylabFontSize = 17;
6 xlabFontSize = 17;
7 
8 yDotstr = strcat( '/', ydotName, '.txt');
9 yDot = importMatrices(strcat(rootPath, robotName, yDotstr));
10 if strcmp(ydotName, "yDotFinalWithCollision")
11  yDotNoCol = importMatrices(strcat(rootPath, robotName, "/yDotFinal.txt"));
12  yDot = yDot - yDotNoCol;
13 
14 end
15 nStep = size(yDot, 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 
25 figure('Renderer', 'painters', 'Position', [0 0 750 990])
26 %% plot joint commands
27 subplot(3,2,1);
28 
29 hold on;
30 for i = 1:4
31  a(:) = yDot(i,1,:);
32  plot(seconds, a);
33 end
34 plot([secInsertion; secInsertion], [-0.06,0.03]', '--m');
35 text([secInsertion+2], [0.024], {'\rightarrow Inside the hole'}, 'Color', 'magenta', 'FontSize',14);
36 text(0.35,1.2,'column 1');
37 
38 hold off;
39 xlab = xlabel('time [s]');
40 leg = legend('$\dot{q}_1$','$\dot{q}_2$', '$\dot{q}_3$', '$\dot{q}_4$');
41 ylab = ylabel('Joint velocities [rad/s]');
42 if strcmp(robotName, 'g500_A')
43  tq = title('Joint command for Robot A');
44  text(0.35,1.2,'column 1');
45 else
46  tq = title('Joint command for Robot B');
47 end
48 ylim([-0.06,0.03]);
49 
50 set(leg, 'Interpreter', 'latex', 'FontSize' , legFontSize);
51 set (ylab, 'Interpreter', 'latex', 'FontSize', ylabFontSize);
52 set (xlab, 'Interpreter', 'latex', 'FontSize', xlabFontSize);
53 set (tq, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
54 
55 
56 subplot(3,2,2);
57 hold on;
58 for i = 1:4
59  a(:) = yDot(i,1,:);
60  plot(seconds, a);
61 end
62 plot([secInsertion; secInsertion], [-0.06,0.03]', '--m');
63 text([secInsertion+2], [0.024], {'\rightarrow Inside the hole'}, 'Color', 'magenta', 'FontSize',14);
64 hold off;
65 xlab = xlabel('time [s]');
66 leg = legend('$\dot{q}_1$','$\dot{q}_2$', '$\dot{q}_3$', '$\dot{q}_4$');
67 
68 ylim([-0.06,0.03]);
69 
70 
71 set(leg, 'Interpreter', 'latex', 'FontSize' , legFontSize);
72 set (ylab, 'Interpreter', 'latex', 'FontSize', ylabFontSize);
73 set (xlab, 'Interpreter', 'latex', 'FontSize', xlabFontSize);
74 
75 %% plot vehicle command divided lin
76 
77 subplot(3,2,3);
78 hold on;
79 for i = 5:7
80  a(:) = yDot(i,1,:);
81  plot(seconds, a);
82 end
83 plot([secInsertion; secInsertion], [-0.02, 0.12]', '--m');
84 text([secInsertion+2], [0.11], {'\rightarrow Inside the hole'}, 'Color', 'magenta', 'FontSize',14);
85 hold off;
86 xlab1 = xlabel('time [s]');
87 ylab1 = ylabel('Linear velocity [m/s]');
88 
89 if strcmp(robotName, 'g500_A')
90  tq1 = title('Joint command for Robot A');
91 else
92  tq1 = title('Joint command for Robot B');
93 end
94 leg1 = legend('$\dot{x}$','$\dot{y}$', '$\dot{z}$');
95 set(leg1, 'Interpreter', 'latex', 'FontSize' , legFontSize);
96 set (ylab1, 'Interpreter', 'latex', 'FontSize', ylabFontSize);
97 set (xlab1, 'Interpreter', 'latex', 'FontSize', xlabFontSize);
98 set (tq1, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
99 
100 ylim([-0.02, 0.12]);
101 
102 
103 
104 subplot(3,2,4);
105 hold on;
106 for i = 5:7
107  a(:) = yDot(i,1,:);
108  plot(seconds, a);
109 end
110 plot([secInsertion; secInsertion], [-0.02, 0.12]', '--m');
111 text([secInsertion+2], [0.11], {'\rightarrow Inside the hole'}, 'Color', 'magenta', 'FontSize',14);
112 hold off;
113 xlab1 = xlabel('time [s]');
114 ylim([-0.02, 0.12]);
115 
116 
117 
118 leg1 = legend('$\dot{x}$','$\dot{y}$', '$\dot{z}$');
119 set(leg1, 'Interpreter', 'latex', 'FontSize' , legFontSize);
120 set (ylab1, 'Interpreter', 'latex', 'FontSize', ylabFontSize);
121 set (xlab1, 'Interpreter', 'latex', 'FontSize', xlabFontSize);
122 
123 
124 %% plot vehicle command divided ang
125 
126 subplot(3,2,5);
127 hold on;
128 for i = 8:10
129  a(:) = yDot(i,1,:);
130  plot(seconds, a);
131 end
132 plot([secInsertion; secInsertion], [-0.005, 0.045]', '--m');
133 text([secInsertion+2], [0.041], {'\rightarrow Inside the hole'}, 'Color', 'magenta', 'FontSize',14);
134 hold off;
135 xlab2 = xlabel('time [s]');
136 ylab2 = ylabel('Angular velocity [rad/s]');
137 ylim([-0.005, 0.045])
138 
139 if strcmp(robotName, 'g500_A')
140  tq2 = title('Joint command for Robot A');
141 else
142  tq2 = title('Joint command for Robot B');
143 end
144 
145 leg2 = legend('$w_x$', '$w_y$', '$w_z$');
146 set(leg2, 'Interpreter', 'latex', 'FontSize' , legFontSize);
147 set (ylab2, 'Interpreter', 'latex', 'FontSize', ylabFontSize);
148 set (xlab2, 'Interpreter', 'latex', 'FontSize', xlabFontSize);
149 set (tq2, 'Interpreter', 'latex', 'FontSize' , titleFontSize);
150 
151 subplot(3,2,6);
152 hold on;
153 for i = 8:10
154  a(:) = yDot(i,1,:);
155  plot(seconds, a);
156 end
157 plot([secInsertion; secInsertion], [-0.005, 0.045]', '--m');
158 text([secInsertion+2], [0.041], {'\rightarrow Inside the hole'}, 'Color', 'magenta', 'FontSize',14);
159 hold off;
160 xlab2 = xlabel('time [s]');
161 ylim([-0.005, 0.045])
162 
163 leg2 = legend('$w_x$', '$w_y$', '$w_z$');
164 set(leg2, 'Interpreter', 'latex', 'FontSize' , legFontSize);
165 set (ylab2, 'Interpreter', 'latex', 'FontSize', ylabFontSize);
166 set (xlab2, 'Interpreter', 'latex', 'FontSize', xlabFontSize);
Generated by   doxygen 1.8.11