forked from sisl/Multilane.jl
-
Notifications
You must be signed in to change notification settings - Fork 0
/
Copy pathgap.jl
258 lines (230 loc) · 9.64 KB
/
gap.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
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
using Multilane
using MCTS
using POMDPToolbox
using POMDPs
# using POMCP
using Missings
using DataFrames
using CSV
using POMCPOW
using ARDESPOT
@everywhere using Missings
@everywhere using Multilane
@everywhere using POMDPToolbox
@show N = 5000
@show n_iters = 1000
@show max_time = Inf
@show max_depth = 40
@show val = SimpleSolver()
alldata = DataFrame()
dpws = DPWSolver(depth=max_depth,
n_iterations=n_iters,
max_time=max_time,
exploration_constant=8.0,
k_state=4.5,
alpha_state=1/10.0,
enable_action_pw=false,
check_repeat_state=false,
estimate_value=RolloutEstimator(val)
# estimate_value=val
)
dpws_x10 = deepcopy(dpws)
dpws_x10.n_iterations *= 10
solvers = Dict{String, Solver}(
# "baseline" => SingleBehaviorSolver(dpws, Multilane.NORMAL),
# "omniscient" => dpws,
# "omniscient-x10" => dpws_x10,
# "mlmpc" => MLMPCSolver(dpws),
# "meanmpc" => MeanMPCSolver(dpws),
# "qmdp" => QBSolver(dpws),
# "pftdpw" => begin
# m = 10
# wup = WeightUpdateParams(smoothing=0.0, wrong_lane_factor=0.5)
# rng = MersenneTwister(123)
# up = AggressivenessUpdater(nothing, m, 0.1, 0.1, wup, rng)
# ABMDPSolver(dpws, up)
# end,
# "pomcpow" => POMCPOWSolver(tree_queries=n_iters,
# criterion=MaxUCB(8.0),
# max_depth=max_depth,
# max_time=max_time,
# enable_action_pw=false,
# k_observation=4.5,
# alpha_observation=1/10.0,
# estimate_value=FORollout(val),
# # estimate_value=val,
# check_repeat_obs=false,
# # node_sr_belief_updater=AggressivenessPOWFilter(wup)
# ),
# "outcome" => OutcomeSolver(dpws)
# "despot" => begin
# rng = MersenneTwister(13)
# b = IndependentBounds(DefaultPolicyLB(val), 1.02, check_terminal=true)
# sol = DESPOTSolver(lambda=0.01,
# K=100,
# D=max_depth,
# max_trials=1_000_000,
# T_max=max_time,
# bounds=b,
# random_source=MemorizingSource(500, max_depth, rng, min_reserve=25),
# default_action=ReportWhenUsed(MLAction(0.0, 0.0)),
# rng=rng)
# end,
# "simple" => SimpleSolver()
)
function make_updater(cor, problem, k, rng_seed)
wup = WeightUpdateParams(smoothing=0.0, wrong_lane_factor=0.05)
if cor >= 1.0 || k == "meanmpc"
return AggressivenessUpdater(problem, 2000, 0.05, 0.1, wup, MersenneTwister(rng_seed+50000))
else
return BehaviorParticleUpdater(problem, 5000, 0.0, 0.0, wup, MersenneTwister(rng_seed+50000))
end
end
pow_updater(up::AggressivenessUpdater) = AggressivenessPOWFilter(up.params)
pow_updater(up::BehaviorParticleUpdater) = BehaviorPOWFilter(up.params)
# for cor in [false, 0.75, true]
for cor in [true, false, 0.75]
for lambda in 2.0.^(-1:3)
# for lambda in [1.0]
@show cor
@show lambda
behaviors = standard_uniform(correlation=cor)
pp = PhysicalParam(4, lane_length=100.0)
dmodel = NoCrashIDMMOBILModel(10, pp,
behaviors=behaviors,
p_appear=1.0,
lane_terminate=true,
max_dist=1000.0
)
rmodel = SuccessReward(lambda=lambda)
pomdp = NoCrashPOMDP{typeof(rmodel), typeof(behaviors)}(dmodel, rmodel, 0.95, false)
mdp = NoCrashMDP{typeof(rmodel), typeof(behaviors)}(dmodel, rmodel, 0.95, false)
problems = Dict{String, Any}(
"baseline"=>mdp,
"omniscient"=>mdp,
"omniscient-x10"=>mdp,
"outcome"=>mdp
)
solver_problems = Dict{String, Any}(
"qmdp"=>mdp
)
for (k, solver) in solvers
@show k
p = get(problems, k, pomdp)
sp = get(solver_problems, k, p)
sim_problem = deepcopy(p)
sim_problem.throw=true
sims = []
for i in 1:N
rng_seed = i+40000
rng = MersenneTwister(rng_seed)
is = initial_state(sim_problem, rng)
ips = MLPhysicalState(is)
metadata = Dict(:rng_seed=>rng_seed,
:lambda=>lambda,
:solver=>k,
:dt=>pp.dt,
:cor=>cor
)
hr = HistoryRecorder(max_steps=100, rng=rng, capture_exception=false)
if p isa POMDP
up = make_updater(cor, sim_problem, k, rng_seed)
if k == "pomcpow"
solver.node_sr_belief_updater = pow_updater(up)
end
planner = deepcopy(solve(solver, sp))
srand(planner, rng_seed+60000)
push!(sims, Sim(sim_problem, planner, up, ips, is,
simulator=hr,
metadata=metadata
))
else
planner = deepcopy(solve(solver, sp))
srand(planner, rng_seed+60000)
push!(sims, Sim(sim_problem, planner, is,
simulator=hr,
metadata=metadata
))
end
@assert problem(last(sims)).throw
end
# data = run(sims) do sim, hist
data = run_parallel(sims) do sim, hist
if nothing === (exception(hist))
p = problem(sim)
steps_in_lane = 0
steps_to_lane = missing
nb_brakes = 0
crashed = false
min_speed = Inf
min_ego_speed = Inf
for (k,(s,sp)) in enumerate(eachstep(hist, "s,sp"))
nb_brakes += detect_braking(p, s, sp)
if sp.cars[1].y == p.rmodel.target_lane
steps_in_lane += 1
end
if sp.cars[1].y == p.rmodel.target_lane
if ismissing(steps_to_lane)
steps_to_lane = k
end
end
if is_crash(p, s, sp)
crashed = true
end
min_speed = min(minimum(c.vel for c in sp.cars), min_speed)
min_ego_speed = min(min_ego_speed, sp.cars[1].vel)
end
time_to_lane = steps_to_lane*p.dmodel.phys_param.dt
distance = last(state_hist(hist)).x
ais = eachstep(hist, "ai")
return [:n_steps=>n_steps(hist),
:mean_iterations=>mean(ai isa Dict ? ai[:tree_queries] : missing for ai in ais),
:mean_search_time=>1e-6*mean(ai isa Dict ? ai[:search_time_us] : missing for ai in ais),
:reward=>discounted_reward(hist),
:crashed=>crashed,
:steps_to_lane=>steps_to_lane,
:steps_in_lane=>steps_in_lane,
:nb_brakes=>nb_brakes,
:exception=>false,
:distance=>distance,
:mean_ego_speed=>distance/(n_steps(hist)*p.dmodel.phys_param.dt),
:min_speed=>min_speed,
:min_ego_speed=>min_ego_speed,
:terminal=>string(last(state_hist(hist).terminal, missing))
]
else
warn("Error in Simulation")
showerror(STDERR, exception(hist))
# show(STDERR, MIME("text/plain"), stacktrace(backtrace(hist)))
return [:exception=>true,
:ex_type=>string(typeof(exception(hist)))
]
end
end
success = 100.0*sum(data[:terminal].=="lane")/N
brakes = 100.0*sum(data[:nb_brakes].>=1)/N
@printf("%% reaching:%5.1f; %% braking:%5.1f\n", success, brakes)
@show extrema(data[:distance])
@show mean(data[:mean_iterations])
@show mean(data[:mean_search_time])
@show mean(data[:reward])
if minimum(data[:min_speed]) < 15.0
@show minimum(data[:min_speed])
end
if isempty(alldata)
alldata = data
else
alldata = vcat(alldata, data)
end
datestring = Dates.format(now(), "E_d_u_HH_MM")
filename = joinpath("/tmp", "gap_checkpoint_"*datestring*".csv")
println("Writing data to $filename")
CSV.write(filename, alldata)
end
end
end
# @show alldata
datestring = Dates.format(now(), "E_d_u_HH_MM")
filename = Pkg.dir("Multilane", "data", "gap_"*datestring*".csv")
println("Writing data to $filename")
CSV.write(filename, alldata)