forked from UW-ACL/SCPToolbox.jl
-
Notifications
You must be signed in to change notification settings - Fork 0
/
tests.jl
232 lines (177 loc) · 5.65 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
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
#= Tests for spacecraft rendezvous with discrete logic.
Sequential convex programming algorithms for trajectory optimization.
Copyright (C) 2021 Autonomous Controls Laboratory (University of Washington)
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 ptr(trials::Int, hom_trials::Int)::Nothing
# Problem definition
N = 25
mdl = RendezvousProblem()
pbm = TrajectoryProblem(mdl)
define_problem!(pbm, :ptr, N)
# PTR algorithm parameters
Nsub = 10
iter_max = 30
disc_method = IMPULSE
wvc = 1e4
wtr = 5e0
ε_abs = -Inf
ε_rel = 1e-3 / 100
feas_tol = 5e-3
q_tr = Inf
q_exit = Inf
solver = ECOS
solver_options = Dict("verbose" => 0, "maxit" => 1000)
pars = PTR.Parameters(
N,
Nsub,
iter_max,
disc_method,
wvc,
wtr,
ε_abs,
ε_rel,
feas_tol,
q_tr,
q_exit,
solver,
solver_options,
)
test_single(mdl, pbm, pars)
test_runtime(mdl, pbm, pars; num_trials = trials)
test_homotopy_update(mdl, pbm, pars; resol = hom_trials)
return nothing
end
"""
test_single(pbm, pars)
Compute a single trajectory.
# Arguments
- `mdl`: the rendezvous problem definition.
- `pbm`: the rendezvous trajectory problem.
- `pars`: the rendezvous trajectory problem.
# Returns
- `sol`: the trajectory solution.
- `history`: the iterate history.
"""
function test_single(
mdl::RendezvousProblem,
pbm::TrajectoryProblem,
pars::PTR.Parameters,
)::Tuple{SCPSolution,SCPHistory}
test_heading("PTR", "Single trajectory")
# Create problem
ptr = PTR.create(pars, pbm)
reset_homotopy(pbm)
# Solve problem
sol, history = PTR.solve(ptr)
@test sol.status == @sprintf("%s", SCP_SOLVED)
# Make plots
plot_trajectory_2d(mdl, sol)
plot_trajectory_2d(mdl, sol; attitude = true)
plot_state_timeseries(mdl, sol)
plot_inputs(mdl, sol, history)
plot_inputs(mdl, sol, history; quad = "D")
plot_cost_evolution(mdl, history)
return sol, history
end
"""
test_single(mdl, pbm, pars[; num_trials])
Run the algorithm several times and plot runtime statistics.
# Arguments
- `mdl`: the rendezvous problem definition.
- `pbm`: the rendezvous trajectory problem.
- `pars`: the rendezvous trajectory problem.
- `num_trials`: number of trials. All trials will give the same solution, but we need
many to plot statistically meaningful timing results
# Returns
- `history_list`: vector of iterate histories for each trial.
"""
function test_runtime(
mdl::RendezvousProblem,
pbm::TrajectoryProblem,
pars::PTR.Parameters;
num_trials::Int = 20,
)::Vector{SCPHistory}
test_heading("PTR", "Runtime statistics")
history_list = Vector{SCPHistory}(undef, num_trials)
for trial = 1:num_trials
# Create new problem
ptr_pbm = PTR.create(pars, pbm)
reset_homotopy(pbm)
@printf("Trial %d/%d\n", trial, num_trials)
# Suppress output
real_stdout = stdout
(rd, wr) = redirect_stdout()
# Run algorithm
sol, history_list[trial] = PTR.solve(ptr_pbm)
redirect_stdout(real_stdout) # Revert to normal output
@test sol.status == @sprintf("%s", SCP_SOLVED)
end
plot_convergence(
history_list,
"rendezvous_3d",
options = fig_opts,
xlabel = "\$\\ell\$",
horizontal = true,
)
return history_list
end
"""
test_homotopy_update(pbm, pars)
Test a sweep of homotopy update thresholds.
# Arguments
- `mdl`: the rendezvous problem definition.
- `pbm`: the rendezvous trajectory problem.
- `pars`: the rendezvous trajectory problem.
- `resol`: the number of threshold parameters to test.
# Returns
- `β_sweep`: vector of homotopy update thresholds that were tested.
- `sol_list`: vector of trajectory solutions that were obtained for each
setting.
"""
function test_homotopy_update(
mdl::RendezvousProblem,
pbm::TrajectoryProblem,
pars::PTR.Parameters;
resol::Int = 20,
)::Tuple{Vector{Float64},Vector{SCPSolution}}
test_heading("PTR", "Homotopy update sweep")
β_sweep = collect(LinRange(0.1, 30, resol)) / 100
sol_list = Vector{SCPSolution}(undef, resol)
for i = 1:resol
mdl.traj.β = β_sweep[i]
# Create new problem
ptr_pbm = PTR.create(pars, pbm)
reset_homotopy(pbm)
@printf("(%d/%d) β = %.2e\n", i, resol, mdl.traj.β)
# Suppress output
real_stdout = stdout
(rd, wr) = redirect_stdout()
# Run algorithm
sol_list[i], _ = PTR.solve(ptr_pbm)
redirect_stdout(real_stdout) # Revert to normal output
@test sol_list[i].status == @sprintf("%s", SCP_SOLVED)
end
plot_homotopy_threshold_sweep(mdl, β_sweep, sol_list)
return β_sweep, sol_list
end
"""
reset_homotopy(pbm)
Reset the homotopy value back to the initial one.
# Arguments
- `pbm`: the rendezvous trajectory problem.
"""
function reset_homotopy(pbm::TrajectoryProblem)::Nothing
pbm.mdl.traj.hom = pbm.mdl.traj.hom_grid[1] # Reset homotopy
return nothing
end