-
Notifications
You must be signed in to change notification settings - Fork 22
/
Copy pathinitialize.m
103 lines (89 loc) · 3.86 KB
/
initialize.m
1
2
3
4
5
6
7
8
9
10
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
100
101
102
103
%% initialization script
% common pre-run set up for simulation and experiment
%% Setup path and ROS
setPath; % include current path
setROS; % rosinit
%% Application and if get new Forces solver
application = 'basic';
% application = 'chance';
fprintf('[%s] Application: %s \n',datestr(now,'HH:MM:SS'), application);
getNewSolver = 0;
quad_ID = [4, 3, 2, 1, 5:100]; % ID of the real drones used for experiments
%% Load problem setup
nQuad = 4; % number of quadrotors
nDynObs = 2; % number of moving obstacles
% three global variables are defined: pr, model, index
if strcmp(application, 'basic')
basic_setup; % basic multi-mav collision avoidance, slack is used
elseif strcmp(application, 'chance')
chance_setup; % chance constrained collision avoidance
else
error('No application is spercified!');
end
%% Load initial scenario
% including quad initial positions and goal
[quadStartPos, quadStartVel, quadEndPos] = scn_circle(model.nQuad, 3.2);
% [quadStartPos, quadStartVel, quadEndPos] = scn_random(model.nQuad, [-3.2, 3.2], [-3.2, 3.2], [0.6, 2.1]);
%% Runnning configuration, changble when running, should not be global variables
% running mode
cfg.application = application;
cfg.modeSim = 1; % 0 - experiment
% 1 - simple simulation mode
cfg.modeCoor = 0; % -1 - sequential prioritized planning
% 0 - sequential planning (centralized)
% 1 - with path communication (distributed)
% 2 - path prediction based on constant v (decentralized)
% environment boundary, [xmax, ymax, zmax]
% cfg.ws = [3.1; 1.4; 2.5]; % DCSC Lab, m
cfg.ws = [4.8; 4.8; 2.4];% simulation, m
% quad goal
cfg.quad.goal = quadEndPos;
% drone size, collision avoidance paramters
cfg.quad.size = [0.3; 0.3; 0.4]; % [a, b, c], m
cfg.quad.coll = [10; 1.2; 0.03]; % lambda, buffer, delta
% stage weights
wS.wp = 0.0; % w_wp
wS.input = 0.1; % w_input
wS.coll = 1.0; % w_coll
wS.slack = 1E4; % w_slack
cfg.weightStage = [wS.wp; wS.input; wS.coll; wS.slack];
% terminal weights
wN.wp = 8;
wN.input = 0.0;
wN.coll = 1.2;
wN.slack = 1E4;
cfg.weightN = [wN.wp; wN.input; wN.coll; wN.slack];
% moving obstalce
cfg.obs.size = [0.4; 0.4; 0.9]; % [a, b, c], m
cfg.obs.coll = [10; 1.4; 0.03]; % lambda, buffer, delta
% communication with gui
cfg.setParaGui = 1;
cfg.ifShowQuadHead = 1;
cfg.ifShowQuadSize = 1;
cfg.ifShowQuadGoal = 0;
cfg.ifShowQuadPath = 1;
cfg.ifShowQuadCov = 0;
cfg.ifShowQuadPathCov = 0;
%% Extra running configuration for chance constrained collision avoidance
% collision chance threshold
cfg.quad.coll(3) = 0.03;
cfg.obs.coll(3) = 0.03;
cfg.quad.deltaAux = erfinv(1-2.0*cfg.quad.coll(3));
cfg.obs.deltaAux = erfinv(1-2.0*cfg.obs.coll(3));
cfg.quad.Mahalanobis = sqrt(chi2inv(1-cfg.quad.coll(3),3));
cfg.obs.Mahalanobis = sqrt(chi2inv(1-cfg.obs.coll(3),3));
% default added noise to the quad
% quad
cfg.addQuadStateNoise = 0;
if strcmp(cfg.application, 'chance')
cfg.addQuadStateNoise = 1;
end
cfg.quad.noise.pos = diag([0.04, 0.04, 0.04].^2);
cfg.quad.noise.vel = diag([0.01, 0.01, 0.01].^2);
cfg.quad.noise.euler = diag(deg2rad([0.1, 0.1, 0.0]).^2);
% obs
cfg.addObsStateNoise = 0;
cfg.obs.noise.pos = diag([0.04, 0.04, 0.04].^2);
cfg.obs.noise.vel = diag([0.01, 0.01, 0.01].^2);
% for extra visualization
cfg.quadPathCovShowNum = 5;