Skip to content

Commit a151968

Browse files
li9ili9i
committed
Why not commit?
1 parent d8c010c commit a151968

17 files changed

+1201
-286
lines changed

‎3D/disturbances:OFF/vault/(2,1)/snapshots_trajectory.m

+67
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,67 @@
1+
close all
2+
clear all
3+
load('xX_1.mat')
4+
load('xX_2.mat')
5+
load('tT_1.mat')
6+
load('tT_2.mat')
7+
load('uU_1.mat')
8+
load('uU_2.mat')
9+
10+
figure(1)
11+
hold on
12+
grid
13+
axis([-8 8 -2 8])
14+
axis equal
15+
16+
% for i=1:size(tT_1,1)-1
17+
%
18+
% i
19+
%
20+
% plot(des_1(1), des_1(2), 'X', 'Color', 'b')
21+
% plot(des_2(1), des_2(2), 'X', 'Color', 'r')
22+
% plot(des_3(1), des_3(2), 'X', 'Color', 'g')
23+
%
24+
% viscircles([xX_1(i,1) + des_1(1), xX_1(i,2) + des_1(2)], r(1), 'EdgeColor', 'b')
25+
% viscircles([xX_2(i,1) + des_2(1), xX_2(i,2) + des_2(2)], r(2), 'EdgeColor', 'r')
26+
% viscircles([xX_3(i,1) + des_3(1), xX_3(i,2) + des_3(2)], r(3), 'EdgeColor', 'g')
27+
%
28+
% viscircles([obs(1,1), obs(1,2)], obs(1,3), 'EdgeColor', 'k')
29+
%
30+
% pause()
31+
% drawnow
32+
% cla
33+
% end
34+
35+
frames = [1,5,6,7,8,10,14,20];
36+
37+
counter = 1;
38+
39+
for i = 1:size(frames,2)
40+
41+
subplot(4,2,counter)
42+
hold on
43+
axis([-9 9 0 6])
44+
axis equal
45+
46+
if i < size(frames,2)
47+
plot(des_1(1), des_1(2), 'X', 'Color', [0.00000,0.44700,0.74100])
48+
plot(des_2(1), des_2(2), 'X', 'Color', [0.85000,0.32500,0.09800])
49+
centers_connector_x = [xX_1(frames(i),1) + des_1(1), xX_2(frames(i),1) + des_2(1)];
50+
centers_connector_y = [xX_1(frames(i),2) + des_1(2), xX_2(frames(i),2) + des_2(2)];
51+
plot(centers_connector_x, centers_connector_y, 'Color', [0.4660 0.6740 0.1880])
52+
end
53+
54+
if i==size(frames,2)
55+
plot(des_1(1), des_1(2), 'O', 'Color', [0.00000,0.44700,0.74100])
56+
plot(des_2(1), des_2(2), 'O', 'Color', [0.85000,0.32500,0.09800])
57+
end
58+
59+
viscircles([xX_1(frames(i),1) + des_1(1), xX_1(frames(i),2) + des_1(2)], r(1), 'EdgeColor', [0.00000,0.44700,0.74100])
60+
viscircles([xX_2(frames(i),1) + des_2(1), xX_2(frames(i),2) + des_2(2)], r(2), 'EdgeColor', [0.85000,0.32500,0.09800])
61+
viscircles([obs(1,1), obs(1,2)], obs(1,3), 'EdgeColor', 'k')
62+
63+
counter = counter + 1;
64+
end
65+
66+
67+
% matlab2tikz('trajectory_d_OFF_2_1.tex')

‎3D/disturbances:OFF/vault/(2,1)/traj.m

+61-35
Original file line numberDiff line numberDiff line change
@@ -7,52 +7,64 @@
77
load('xX_1.mat')
88
load('xX_2.mat')
99

10-
figure(1)
11-
hold on
12-
grid
13-
axis([-10 10 0 5])
14-
axis equal
15-
filename = 'trajectories.gif';
16-
for i=1:size(tT_1,1)-1
17-
18-
plot(des_1(1), des_1(2), 'X', 'Color', 'b')
19-
plot(des_2(1), des_2(2), 'X', 'Color', 'r')
20-
21-
viscircles([xX_1(i,1) + des_1(1), xX_1(i,2) + des_1(2)], r(1), 'EdgeColor', 'b')
22-
viscircles([xX_2(i,1) + des_2(1), xX_2(i,2) + des_2(2)], r(2), 'EdgeColor', 'r')
23-
viscircles([obs(1,1), obs(1,2)], obs(1,3), 'EdgeColor', 'k')
24-
25-
pause()
26-
27-
drawnow
28-
frame = getframe(1);
29-
im = frame2im(frame);
30-
[imind,cm] = rgb2ind(im,256);
31-
if i == 1;
32-
imwrite(imind,cm,filename,'gif', 'Loopcount',inf);
33-
else
34-
imwrite(imind,cm,filename,'gif','WriteMode','append');
35-
end
36-
cla
37-
end
10+
% figure(1)
11+
% hold on
12+
% grid
13+
% axis([-10 10 0 5])
14+
% axis equal
15+
% filename = 'trajectories.gif';
16+
% for i=1:size(tT_1,1)-1
17+
%
18+
% plot(des_1(1), des_1(2), 'X', 'Color', 'b')
19+
% plot(des_2(1), des_2(2), 'X', 'Color', 'r')
20+
%
21+
% viscircles([xX_1(i,1) + des_1(1), xX_1(i,2) + des_1(2)], r(1), 'EdgeColor', 'b')
22+
% viscircles([xX_2(i,1) + des_2(1), xX_2(i,2) + des_2(2)], r(2), 'EdgeColor', 'r')
23+
% viscircles([obs(1,1), obs(1,2)], obs(1,3), 'EdgeColor', 'k')
24+
%
25+
% pause()
26+
%
27+
% drawnow
28+
% frame = getframe(1);
29+
% im = frame2im(frame);
30+
% [imind,cm] = rgb2ind(im,256);
31+
% if i == 1;
32+
% imwrite(imind,cm,filename,'gif', 'Loopcount',inf);
33+
% else
34+
% imwrite(imind,cm,filename,'gif','WriteMode','append');
35+
% end
36+
% cla
37+
% end
3838

3939

4040
% Distances of agent 1 and agent 2 to the obstacle
4141
figure
42-
plot(sqrt((xX_1(:,1)+(des_1(1)-obs(1,1))*ones(size(tT_1))).^2 + (xX_1(:,2)+(des_1(2)-obs(1,2))*ones(size(tT_1))).^2));
42+
plot(sqrt((xX_1(:,1)+(des_1(1)-obs(1,1))*ones(size(tT_1))).^2 + (xX_1(:,2)+(des_1(2)-obs(1,2))*ones(size(tT_1))).^2), 'Color', [ 0 0.4470 0.7410]);
4343
hold on
44-
plot(sqrt((xX_2(:,1)+(des_2(1)-obs(1,1))*ones(size(tT_2))).^2 + (xX_2(:,2)+(des_2(2)-obs(1,2))*ones(size(tT_2))).^2));
44+
plot(sqrt((xX_2(:,1)+(des_2(1)-obs(1,1))*ones(size(tT_2))).^2 + (xX_2(:,2)+(des_2(2)-obs(1,2))*ones(size(tT_2))).^2), 'Color', [ 0.8500 0.3250 0.0980]);
45+
constr_x = [0, 30];
46+
constr_y = [1.6, 1.6];
47+
plot(constr_x, constr_y, 'Color', 'c')
4548
grid
49+
axis([0 30 1.2 7])
4650

4751
% Distance of agent 1 to agent 2
4852
figure
4953
hold on
54+
constr_x = [0, 30];
55+
constr_y = [1.1, 1.1];
56+
plot(constr_x, constr_y, 'Color', 'c')
57+
constr_x = [0, 30];
58+
constr_y = [2.1, 2.1];
59+
plot(constr_x, constr_y, 'Color', 'c')
60+
axis([0 30 0.8 2.2])
61+
5062
grid
5163
plot(...
5264
sqrt(...
5365
(xX_1(:,1) - xX_2(:,1)+ (des_1(1) - des_2(1))*ones(size(tT_1))).^2 + ...
5466
(xX_1(:,2) - xX_2(:,2)+ (des_1(2) - des_2(2))*ones(size(tT_2))).^2 ...
55-
) ...
67+
), 'Color', 'b' ...
5668
);
5769

5870
% errors
@@ -73,12 +85,26 @@
7385
% inputs
7486
figure
7587
hold on
76-
plot(uU_1_proper(1,:))
77-
plot(uU_1_proper(2,:))
88+
constr_x = [0, 30];
89+
constr_y = [-10, -10];
90+
plot(constr_x, constr_y, 'Color', 'c')
91+
constr_x = [0, 30];
92+
constr_y = [10, 10];
93+
plot(constr_x, constr_y, 'Color', 'c')
94+
plot(uU_1_proper(1,:), 'Color', [0 0.4470 0.7410])
95+
plot(uU_1_proper(2,:), 'Color', [0.8500 0.3250 0.0980])
7896
grid
97+
axis([0 30 -11 11])
7998

8099
figure
81100
hold on
82-
plot(uU_2_proper(1,:))
83-
plot(uU_2_proper(2,:))
101+
constr_x = [0, 30];
102+
constr_y = [-10, -10];
103+
plot(constr_x, constr_y, 'Color', 'c')
104+
constr_x = [0, 30];
105+
constr_y = [10, 10];
106+
plot(constr_x, constr_y, 'Color', 'c')
107+
plot(uU_2_proper(1,:), 'Color', [0 0.4470 0.7410])
108+
plot(uU_2_proper(2,:), 'Color', [ 0.8500 0.3250 0.0980])
84109
grid
110+
axis([0 30 -11 11])

‎3D/disturbances:OFF/vault/(2,2)/snapshots_trajectory.m

+68
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,68 @@
1+
close all
2+
clear all
3+
load('xX_1.mat')
4+
load('xX_2.mat')
5+
load('tT_1.mat')
6+
load('tT_2.mat')
7+
load('uU_1.mat')
8+
load('uU_2.mat')
9+
10+
% figure(1)
11+
% hold on
12+
% grid
13+
% axis([-8 8 -2 8])
14+
% axis equal
15+
16+
% for i=1:size(tT_1,1)-1
17+
%
18+
% i
19+
%
20+
% plot(des_1(1), des_1(2), 'X', 'Color', 'b')
21+
% plot(des_2(1), des_2(2), 'X', 'Color', 'r')
22+
% plot(des_3(1), des_3(2), 'X', 'Color', 'g')
23+
%
24+
% viscircles([xX_1(i,1) + des_1(1), xX_1(i,2) + des_1(2)], r(1), 'EdgeColor', 'b')
25+
% viscircles([xX_2(i,1) + des_2(1), xX_2(i,2) + des_2(2)], r(2), 'EdgeColor', 'r')
26+
% viscircles([xX_3(i,1) + des_3(1), xX_3(i,2) + des_3(2)], r(3), 'EdgeColor', 'g')
27+
%
28+
% viscircles([obs(1,1), obs(1,2)], obs(1,3), 'EdgeColor', 'k')
29+
%
30+
% pause()
31+
% drawnow
32+
% cla
33+
% end
34+
35+
frames = [1,3,6,7,8,9,12,20];
36+
37+
counter = 1;
38+
39+
for i = 1:size(frames,2)
40+
41+
subplot(4,2,counter)
42+
hold on
43+
axis([-9 9 0 6])
44+
axis equal
45+
46+
if i < size(frames,2)
47+
plot(des_1(1), des_1(2), 'X', 'Color', [0.00000,0.44700,0.74100])
48+
plot(des_2(1), des_2(2), 'X', 'Color', [0.85000,0.32500,0.09800])
49+
centers_connector_x = [xX_1(frames(i),1) + des_1(1), xX_2(frames(i),1) + des_2(1)];
50+
centers_connector_y = [xX_1(frames(i),2) + des_1(2), xX_2(frames(i),2) + des_2(2)];
51+
plot(centers_connector_x, centers_connector_y, 'Color', [0.4660 0.6740 0.1880])
52+
end
53+
54+
if i==size(frames,2)
55+
plot(des_1(1), des_1(2), 'O', 'Color', [0.00000,0.44700,0.74100])
56+
plot(des_2(1), des_2(2), 'O', 'Color', [0.85000,0.32500,0.09800])
57+
end
58+
59+
viscircles([xX_1(frames(i),1) + des_1(1), xX_1(frames(i),2) + des_1(2)], r(1), 'EdgeColor', [0.00000,0.44700,0.74100])
60+
viscircles([xX_2(frames(i),1) + des_2(1), xX_2(frames(i),2) + des_2(2)], r(2), 'EdgeColor', [0.85000,0.32500,0.09800])
61+
viscircles([obs(1,1), obs(1,2)], obs(1,3), 'EdgeColor', 'k')
62+
viscircles([obs(2,1), obs(2,2)], obs(1,3), 'EdgeColor', 'k')
63+
64+
counter = counter + 1;
65+
end
66+
67+
68+
% matlab2tikz('trajectory_d_OFF_2_2.tex')

0 commit comments

Comments
 (0)