def profile_by_seq_len(nloop=100):
for _l in [3, 7, 10, 30, 60, 120]:
drone, targets = pmu.make_random_scenario(_l)
_start = time.perf_counter()
for i in range(nloop):
pm.intercept_sequence_copy(drone, targets)
_end = time.perf_counter()
dt = _end - _start
ips = nloop / dt
print(f'{_l}: {dt:.1f} s {ips:.0f} seq/s')
def test2(filename='../../data/scenario_60_6.yaml', ntest=100):
scen = pmu.Scenario(filename=filename)
s = pm_cpp_ext.Solver()
s.init(scen.drone, scen.targets)
for i in range(ntest):
seq = np.random.permutation(scen.targets).tolist()
_seq = [_s.name - 1 for _s in seq]
#print(_seq)
c_dur = s.run_sequence(_seq)
c_psis = s.debug()
py_drone, py_dur = pm.intercept_sequence_copy(scen.drone, seq)
#pdb.set_trace()
c_psis1 = [pmu.norm_angles_mpi_pi(_psi) for _psi in c_psis]
p_psis1 = [pmu.norm_angles_mpi_pi(_psi) for _psi in py_drone.psis]
#passed1 = np.allclose(c_psis1, p_psis1, rtol=1e-02, atol=1e-03) # rtol=1e-05, atol=1e-08
passed1 = np.allclose(c_psis1, p_psis1, rtol=1e-03,
atol=1e-08) # rtol=1e-05, atol=1e-08
#passed1 = np.allclose(c_psis, py_drone.psis, rtol=1e-05, atol=1e-06) # rtol=1e-05, atol=1e-08
passed2 = np.allclose(c_dur, py_dur, rtol=1e-05, atol=1e-06)
print(f'Test passed: {passed1} {passed2}')
if not passed1:
#print(f'{c_psis} \n{py_drone.psis}')
for i in range(nb_tg):
if abs(c_psis[i] - py_drone.psis[i]) > 1e-5:
print(f'failed idx {i} {c_psis1[i]} {p_psis1[i]}')
print([_s.name for _s in seq])
if not passed2:
print(f'durations (c/py): {c_dur}, {py_dur}')
def plot_solution(fig, ax, scen, sol_name):
seq = scen.solution_by_name(sol_name)[2]
drone, dur = pm.intercept_sequence_copy(scen.drone, seq)
#print(f'recomputed {format_seq(seq)} {dur:.2f} ')
drone_poss = np.asarray(drone.Xs)
ax.plot(drone_poss[:, 0], drone_poss[:, 1], '-X')
for _target, _t in zip(seq, drone.ts[1:]):
plot_actor(ax, _target, dt=_t)
decorate(ax, f'{scen.name}/{sol_name} ({dur:.2f} s)')
ax.axis('equal')
def anim_scens(show_opt=True, show_heu=True, show_hist=True, _fs=3.84):
_scens = [make_or_load_scenario(_i) for _i in [0, 1, 2, 3]]
#_scens = [make_or_load_scenario(_i) for _i in [4, 5, 6, 7]]
#_scens = [make_or_load_scenario(_i) for _i in [8, 9]]
#_scens = [make_or_load_scenario(_i) for _i in [10, 11]]
_sols = [_3 for _1, _2, _3 in _scens]
drones, targets = [], []
if show_opt:
for _d, _t, _s in _scens:
best_dur, best_seq = pmu.sol_by_name(_s, 'optimal')
if best_seq is not None:
best_drone, best_dur = pm.intercept_sequence_copy(_d, best_seq)
else:
best_drone, best_seq = pm.search_exhaustive(_d, _t)
drones.append(best_drone), targets.append(best_seq)
if show_heu:
for _d, _t, _s in _scens:
test_dur, test_seq = pmu.sol_by_name(_s, 'heuristic')
if test_seq is not None:
test_drone, test_dur = pm.intercept_sequence_copy(_d, test_seq)
else:
test_drone, test_seq = search_heuristic_closest(_d, _t)
drones.append(test_drone), targets.append(test_seq)
_nr, _nc = np.sum([show_opt, show_heu, show_hist]), len(_scens)
titles = [f'scenario {_i}_opt' for _i in range(_nc)
] + [f'scenario {_i}_heur' for _i in range(_nc)]
fig = plt.figure(tight_layout=True, figsize=[_fs * _nc, _fs * _nr])
axes = fig.subplots(_nr, _nc) #, sharex=True)
if show_hist:
_titles = [f'scenario {_i} histogram' for _i in range(_nc)]
for (_d, _t, _s), _title, _ax in zip(_scens, _titles, axes[2, :]):
plot_histogram(fig, _ax, _d, _t, _title)
return pma.animate_multi(fig,
axes.flatten(),
drones,
targets,
titles,
xlim=(-150, 150),
ylim=(-150, 150))
def compare_with_optimal(scen):
try:
name, opt_dur, opt_seq = scen.solution_by_name('optimal')
opt_drone, _ = pm.intercept_sequence_copy(scen.drone, opt_seq)
except KeyError:
opt_drone, opt_seq = pm.search_exhaustive(scen.drone, scen.targets)
opt_dur = opt_drone.flight_duration()
try:
name, heur_dur, heur_seq = scen.solution_by_name('heuristic')
heur_drone, _ = pm.intercept_sequence_copy(scen.drone, heur_seq)
except KeyError:
heur_drone, heur_seq = pm_t3.search_heuristic_closest(
scen.drone, scen.targets)
heur_dur = heur_drone.flight_duration()
print(f'optimal {opt_dur:.1f} s heuristic {heur_dur:.1f} s')
fig = plt.figure(tight_layout=True, figsize=[10.24, 5.12])
ax1, ax2 = fig.subplots(1, 2, sharex=True)
return pma.animate_multi(fig, [ax1, ax2], [opt_drone, heur_drone],
[opt_seq, heur_seq], ['Optimal', 'Heuristic'])
def animate_solutions(scen, names, tf=1., window_title=None, _size=3.84):
sols = [scen.solution_by_name(name) for name in names]
seqs = [_seq for _1, _2, _seq in sols]
drones = [pm.intercept_sequence_copy(scen.drone, _seq)[0] for _seq in seqs]
_n = len(names)
fig = plt.figure(tight_layout=True, figsize=[_size * _n, _size])
if window_title is not None: fig.canvas.set_window_title(window_title)
axes = fig.subplots(1, _n) #, sharex=True)
if _n == 1: axes = [axes]
titles = [f'{window_title}/{_n}' for _n in names]
return animate_fig(fig, axes, drones, seqs, titles, tf=tf)
def search_exhaustive(drone, targets, keep_all=False, display=False):
perms = set(itertools.permutations(targets))
if display:
print(
f'exhaustive search for {len(targets)} targets ({len(perms)} sequences)'
)
best_dur, best_drone, best_targets, all_drones, all_targets = float(
'inf'), None, None, [], []
for _seq in perms:
_drone, _dur = pm.intercept_sequence_copy(drone, _seq)
if _dur < best_dur:
best_dur, best_drone, best_targets = _dur, _drone, _seq
if keep_all:
all_drones.append(_drone)
all_targets.append(_seq)
if display:
print(f'optimal seq {best_dur:.02f}s {format_seq(best_targets)}')
return (all_drones, all_targets) if keep_all else (best_drone,
best_targets)
def test22(filename='../../data/scenario_60_6.yaml'):
#seq = [30, 47, 12, 20, 39, 36, 28, 29, 18, 2, 41, 24, 35, 54, 58, 23, 53, 11, 4, 15, 10, 9, 34, 43, 44, 45, 42, 14, 37, 17, 19, 52, 8, 33, 48, 27, 56, 5, 55, 21, 59, 26, 40, 50, 49, 7, 22, 31, 38, 25, 57, 32, 3, 60, 6, 13, 51, 46, 16, 1]
seq = [
16, 15, 14, 13, 12, 11, 10, 9, 8, 7, 6, 5, 4, 3, 2, 1, 60, 59, 58, 57,
56, 55, 54, 53, 52, 51, 50, 49, 48, 47, 46, 45, 44, 43, 42, 41, 40, 39,
38, 37, 36, 35, 34, 33, 32, 31, 30, 29, 28, 27, 26, 25, 24, 23, 22, 21,
20, 19, 18, 17
]
scen = pmu.Scenario(filename=filename)
s = pm_cpp_ext.Solver()
s.init(scen.drone, scen.targets)
py_seq = [scen.targets[_n - 1] for _n in seq]
py_drone, py_dur = pm.intercept_sequence_copy(scen.drone, py_seq)
print(f'{py_dur}')
c_drone, c_dur = s.intercept_sequence_copy(scen.drone, py_seq)
print(f'{c_dur}')
pdb.set_trace()
def search(drone,
targets,
start_seq=None,
epochs=1000,
debug=False,
Tf=None,
display=0,
T0=2.,
use_native=False):
solver = pm_cpp_ext.Solver(drone, targets) if use_native else pm
if display > 0:
print(
f'running simulated annealing with {len(targets)} targets for {epochs:.1e} epochs'
)
print(
f' ({epochs/np.math.factorial(len(targets)):.2e} search space coverage)'
)
print(' custom temperature control'
if Tf is not None else f' default linear temperature (T0={T0})')
if start_seq is None: start_seq = np.random.permutation(targets).tolist()
start_drone, start_dur = pm.intercept_sequence_copy(drone, start_seq)
best_drone, cur_drone = start_drone, start_drone
best_seq = cur_seq = start_seq
best_dur = cur_dur = start_dur
if display > 0:
print(f' start solution')
_print_sol(0, T0, best_dur, cur_dur, cur_seq)
if display > 1: last_display = time.perf_counter()
if Tf is None: Tf = lambda i: _f1(T0, 0, 1e-2, 0.8 * epochs, i)
if debug:
all_durs, cur_durs, Paccept, max_durs = (np.zeros(epochs)
for i in range(4))
for i in range(epochs):
T = Tf(i)
_r = np.random.uniform(low=0, high=1.)
max_dur = cur_dur + T * np.log(_r)
_s2 = _mutate(cur_seq)
# BROKEN!! _d2, _dur = solver.intercept_sequence_copy_threshold(drone, _s2, max_dur)
_d2, _dur = solver.intercept_sequence_copy(drone, _s2)
acc_prob = np.exp(
-(_dur - cur_dur) / T
) if _dur > cur_dur else 0. # warning 1, but 0 looks nicer on plot
if debug:
max_durs[i] = max_dur
all_durs[i] = _dur
Paccept[i] = acc_prob
if _dur < best_dur:
best_dur, best_drone, best_seq = _dur, _d2, _s2
if _dur < cur_dur or _r <= acc_prob:
cur_dur, cur_drone, cur_seq = _dur, _d2, _s2
if display > 1 and time.perf_counter() - last_display > 1.:
_print_sol(i, T, best_dur, cur_dur, cur_seq)
last_display = time.perf_counter()
if debug: cur_durs[i] = cur_dur
if display > 0:
print(f' best solution')
_print_sol(i, T, best_dur, cur_dur, cur_seq)
if debug:
return best_drone, best_seq, all_durs, cur_durs, Paccept, max_durs
else:
return best_drone, best_seq
def main(filename,
method='sa3',
max_epoch=10000,
sol_name=None,
save_filename=None,
overwrite=False,
show=False,
T0=2.,
start_sol_name=None):
scen = pm_sc.Scenario(filename=filename)
_start = time.perf_counter()
_neval = max_epoch
if method == 'ex': # exhaustive
_neval = np.math.factorial(len(scen.targets))
_drone, _seq = pmu.search_exhaustive(scen.drone,
scen.targets,
keep_all=False,
display=True)
elif method == 'ex2': # exhaustive native
_neval = np.math.factorial(len(scen.targets))
_drone, _seq = pm_nc.search_exhaustive(scen.drone, scen.targets)
elif method == 'he': # heuristic_closest
_drone, _seq = pmu.search_heuristic_closest(scen.drone, scen.targets)
elif method == 'sa': # simulated annealing
start_seq = scen.solution_by_name(
start_sol_name)[2] if start_sol_name is not None else None
_drone, _seq = pm_sa.search(scen.drone,
scen.targets,
epochs=max_epoch,
display=2,
T0=T0,
use_native=False,
start_seq=start_seq)
elif method == 'sa2': # simulated annealing hybrid native
start_seq = scen.solution_by_name(
start_sol_name)[2] if start_sol_name is not None else None
_drone, _seq = pm_sa.search(scen.drone,
scen.targets,
epochs=max_epoch,
display=2,
T0=T0,
use_native=True,
start_seq=start_seq)
elif method == 'sa3': # simulated annealing full native
start_seq = scen.solution_by_name(
start_sol_name)[2] if start_sol_name is not None else None
_drone, _seq = pm_nc.search_sa(scen.drone, scen.targets, start_seq,
max_epoch, T0)
else:
print('unknown search method')
_end = time.perf_counter()
cpu_dur = _end - _start
eval_per_sec = _neval / cpu_dur
print(
f'{sol_name}: {_drone.flight_duration():.2f}s (computed in {datetime.timedelta(seconds=cpu_dur)} h:m:s, {eval_per_sec:.0f} ev/s)'
)
check = False # FIXME: move this to unit test
if check:
_drone2, _dur2 = pm.intercept_sequence_copy(scen.drone, _seq)
if _dur2 != _drone2.flight_duration() or\
not np.allclose([_dur2], [_drone.flight_duration()]): # python and C did not recompute same duration for sequence
print('#### search.py: check failed FIXME ####')
print(f'{_dur2} {_drone.flight_duration()}')
pdb.set_trace()
scen.add_solution(sol_name, _drone.flight_duration(), _seq)
if save_filename is not None:
scen.save(save_filename)
if overwrite:
scen.save(filename)
if show:
pmu.plot_solutions(scen, [sol_name], filename)
