-
Notifications
You must be signed in to change notification settings - Fork 55
/
tests.jl
202 lines (179 loc) · 4.66 KB
/
tests.jl
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
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
"""
Tests for quadrotor obstacle avoidance.
Sequential convex programming algorithms for trajectory optimization.
Copyright (C) 2021 Autonomous Controls Laboratory (University of Washington),
and Autonomous Systems Laboratory (Stanford University)
This program is free software: you can redistribute it and/or modify it under
the terms of the GNU General Public License as published by the Free Software
Foundation, either version 3 of the License, or (at your option) any later
version.
This program is distributed in the hope that it will be useful, but WITHOUT ANY
WARRANTY; without even the implied warranty of MERCHANTABILITY or FITNESS FOR A
PARTICULAR PURPOSE. See the GNU General Public License for more details.
You should have received a copy of the GNU General Public License along with
this program. If not, see <https://www.gnu.org/licenses/>.
"""
using ECOS
using Printf
using Test
function scvx(trials::Int)::Nothing
# Problem definition
mdl = QuadrotorProblem()
pbm = TrajectoryProblem(mdl)
define_problem!(pbm, :scvx)
# SCvx algorithm parameters
N = 30
Nsub = 15
iter_max = 15
disc_method = FOH
λ = 30.0
ρ_0 = 0.0
ρ_1 = 0.1
ρ_2 = 0.7
β_sh = 2.0
β_gr = 2.0
η_init = 1.0
η_lb = 1e-3
η_ub = 10.0
ε_abs = 0#1e-5
ε_rel = 0#0.01/100
feas_tol = 1e-3
q_tr = Inf
q_exit = Inf
solver = ECOS
solver_options = Dict("verbose" => 0)
pars = SCvx.Parameters(
N,
Nsub,
iter_max,
disc_method,
λ,
ρ_0,
ρ_1,
ρ_2,
β_sh,
β_gr,
η_init,
η_lb,
η_ub,
ε_abs,
ε_rel,
feas_tol,
q_tr,
q_exit,
solver,
solver_options,
)
# Solve multiple times to gather statistics
run_trials(mdl, pbm, pars, SCvx; num_trials = trials)
end
function gusto(trials::Int)::Nothing
# Problem definition
mdl = QuadrotorProblem()
pbm = TrajectoryProblem(mdl)
define_problem!(pbm, :gusto)
# SCvx algorithm parameters
N = 30
Nsub = 15
iter_max = 15
disc_method = FOH
λ_init = 1e4
λ_max = 1e9
ρ_0 = 0.1
ρ_1 = 0.9
β_sh = 2.0
β_gr = 2.0
γ_fail = 5.0
η_init = 10.0
η_lb = 1e-3
η_ub = 10.0
μ = 0.8
iter_μ = 6
ε_abs = 0#1e-5
ε_rel = 0#0.01/100
feas_tol = 1e-3
pen = :quad
hom = 100.0
q_tr = Inf
q_exit = Inf
solver = ECOS
solver_options = Dict("verbose" => 0)
pars = GuSTO.Parameters(
N,
Nsub,
iter_max,
disc_method,
λ_init,
λ_max,
ρ_0,
ρ_1,
β_sh,
β_gr,
γ_fail,
η_init,
η_lb,
η_ub,
μ,
iter_μ,
ε_abs,
ε_rel,
feas_tol,
pen,
hom,
q_tr,
q_exit,
solver,
solver_options,
)
# Solve multiple times to gather statistics
run_trials(mdl, pbm, pars, GuSTO; num_trials = trials)
end
"""
run_trials(mdl, traj, pars, solver[; num_trials])
Solves the same problem multiple times in order to gather realiable runtime statistics.
# Parameters
- `mdl`: problem-specific data.
- `traj`: the trajectory problem.
- `pars`: solution algorithm parameters.
- `solver`: the solver algorithm's module.
- `num_trials`: number of trials. All trials will give the same solution, but we need
many to plot statistically meaningful timing results
"""
function run_trials(
mdl::QuadrotorProblem,
traj::TrajectoryProblem,
pars::T,
solver::Module;
num_trials::Int = 100,
)::Nothing where {T<:SCPParameters}
sol_list = Vector{SCPSolution}(undef, num_trials)
history_list = Vector{SCPHistory}(undef, num_trials)
for trial = 1:num_trials
local pbm = solver.create(pars, traj)
@printf("Trial %d/%d\n", trial, num_trials)
if trial > 1
# Suppress output
real_stdout = stdout
(rd, wr) = redirect_stdout()
end
sol_list[trial], history_list[trial] = solver.solve(pbm)
@test sol_list[trial].status == @sprintf("%s", SCP_SOLVED)
if trial > 1
redirect_stdout(real_stdout)
end
end
# Save one solution instance - for plotting a single trial
sol = sol_list[end]
history = history_list[end]
# Make plots
try
plot_trajectory_history(mdl, history)
plot_final_trajectory(mdl, sol)
plot_input_norm(mdl, sol)
plot_tilt_angle(mdl, sol)
plot_convergence(history_list, "quadrotor")
catch e
showerror(stdout, e)
end
return nothing
end