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 search_exhaustive(drone, targets):
s = pm_cpp_ext.Solver(drone, targets)
best_dur, _best_seq = s.search_exhaustive()
drone = copy.deepcopy(drone)
drone.ts.append(best_dur) # Warning: we only update flight time
best_seq = [targets[_i] for _i in _best_seq]
return drone, best_seq
def test1(filename='../../data/scenario_60_6.yaml'):
scen = pmu.Scenario(filename=filename)
s = pm_cpp_ext.Solver()
s.init(scen.drone, scen.targets)
seq = [scen.targets[17]]
#nb_t = len(scen.targets)
#seq = np.random.permutation(scen.targets).tolist()
_seq = [_s.name for _s in seq]
print(_seq)
s.run_sequence(_seq)
c_psis = s.debug()
#print(psis)
# if 0:
# psi, dt = pm.intercept_1(scen.drone, scen.targets[0])
# print(f'intercept seq py {psi}, {dt}')
# scen.drone.add_leg(dt, psi)
# psi, dt = pm.intercept_1(scen.drone, scen.targets[1])
# print(f'intercept seq py {psi}, {dt}')
# scen.drone.add_leg(dt, psi)
# else:
pm.intercept_sequence(scen.drone, scen.targets[:nb_t])
#print(f'dur: {scen.drone.flight_duration()}')
#print(f'{scen.drone.psis}')
#passed = np.allclose(psis, scen.drone.psis, rtol=1e-05, atol=1e-08)
passed = np.allclose(c_psis, scen.drone.psis, rtol=1e-05, atol=1e-06)
if not passed:
print(f'{s.debug()} \n{scen.drone.psis}')
#pdb.set_trace()
print(f'Test passed: {passed}')
def search_sa(drone, targets, start_seq, nepoch, T0=2., display=1):
s = pm_cpp_ext.Solver(drone, targets)
if start_seq is None: start_seq = np.random.permutation(targets).tolist()
best_dur, _best_seq = s.search_sa([_t.name - 1 for _t in start_seq],
nepoch, T0, display)
drone = copy.deepcopy(drone)
drone.ts.append(best_dur) # Warning: we only update flight time
best_seq = [targets[_i] for _i in _best_seq]
return drone, best_seq
def test5(filename='../../data/scen_small/scenario_9_6.yaml', nepoch=int(10)):
scen = pmu.Scenario(filename=filename)
s = pm_cpp_ext.Solver(scen.drone, scen.targets)
start_seq = np.random.permutation(scen.targets).tolist()
_start_seq = [_s.name - 1 for _s in start_seq]
_start = time.perf_counter()
best_dur, best_seq = s.search_sa(_start_seq, nepoch, T0=1., display=2)
_end = time.perf_counter()
dt = _end - _start
print(f'search took {dt:.0f}s best dur {best_dur:.3f}')
def test11(filename='../../data/scen_120/1.yaml'
): #'../../data/scenario_7820_2_2.yaml'):
scen = pmu.Scenario(filename=filename)
s = pm_cpp_ext.Solver(scen.drone, scen.targets)
for i in range(1):
#seq = np.random.permutation(scen.targets).tolist()#[:nb_tg]
seq = scen.targets
_seq = [_s.name - 1 for _s in seq]
c_dur = s.run_sequence(_seq)
c_psis = s.debug()
print(f'intercepted {len(c_psis)} targets ({c_dur} s)')
def test4(filename='../../data/scenario_9_6.yaml'):
scen = pmu.Scenario(filename=filename)
s = pm_cpp_ext.Solver()
s.init(scen.drone, scen.targets)
n_targ = len(scen.targets)
n_seq = np.math.factorial(n_targ)
print(f'searching in all {n_targ} targets permutations ({n_seq:.2e})')
_start = time.perf_counter()
best_cost, best_seq = s.run_exhaustive()
_end = time.perf_counter()
dt = _end - _start
ips = n_seq / dt
print(f'best cost: {best_cost}')
print(best_seq)
print(f'search in C took {dt:.3f}s ({ips:.2e} cost_evals/s)')
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 test3(filename='../../data/scen_120/9.yaml',
ntest=1000): # scen_7820/9.yaml
scen = pmu.Scenario(filename=filename)
s = pm_cpp_ext.Solver(scen.drone, scen.targets)
seq = [_s.name - 1 for _s in scen.targets]
#pdb.set_trace()
_start1 = time.perf_counter()
for i in range(ntest):
s.run_sequence(seq)
_end1 = time.perf_counter()
_dt1 = _end1 - _start1
print(f'{ntest} evaluations in C took {_dt1:.3f} s')
_start2 = time.perf_counter()
for i in range(ntest):
pm.intercept_sequence(scen.drone, scen.targets)
scen.drone.clear_traj()
_end2 = time.perf_counter()
_dt2 = _end2 - _start2
print(f'{ntest} evaluations in Python took {_dt2:.3f} s')
print(f'improvement {_dt2/_dt1:.1f}')
def sample_solutions(filenames, nb_samples=int(1e5), force_recompute=False, use_native=True, show_2d=True):
_scens = [pmu.Scenario(filename=_f) for _f in filenames]
_durs = []
for _scen in _scens:
cache_filename = f'/tmp/samples__{_scen.name}__{nb_samples}.npz'
if force_recompute or not os.path.exists(cache_filename):
print(f'sampling and storing to {cache_filename}')
solver = pm_cpp_ext.Solver(_scen.drone, _scen.targets) if use_native else pm
_durs.append([solver.intercept_sequence_copy(_scen.drone, np.random.permutation(_scen.targets).tolist())[1] for _i in range(nb_samples)])
np.savez(cache_filename, durs=_durs[-1])
else:
print(f'loading samples from {cache_filename}')
_durs.append(np.load(cache_filename)['durs'])
nb_scen = len(filenames)
nr, nc = nb_scen, 2 if show_2d else 1
fig = plt.figure(tight_layout=True, figsize=[6.40*nc, 2.56*nr]);
fig.canvas.set_window_title("Random Sampling of Solutions")
gs = fig.add_gridspec(nr, 3)
axes_2d = [fig.add_subplot(gs[_i,:1]) for _i in range(nb_scen)]
axes_histo = [fig.add_subplot(gs[_i,1:]) for _i in range(nb_scen)]
np.set_printoptions(precision=3)
for ax, _d, _scen in zip(axes_histo, _durs, _scens):
_dens, _, _ = ax.hist(_d, label=f'random sampling ({nb_samples:.0e} samples)', density=True)
for sol_name in ['best', 'hc', 'optimal']:
try:
_name, _dur, _seq = _scen.solution_by_name(sol_name)
#ax.annotate(f'{sol_name}', xy=(_dur, 1))#, xytext=(3, 1.5), arrowprops=dict(facecolor='black', shrink=0.05),)
ax.plot([_dur, _dur],[0, np.max(_dens)*0.8], label=f'{sol_name}: {_dur:.3f}s', alpha=0.6, linewidth=2)
except KeyError: pass
pmu.decorate(ax, _fmt_mmmm2(_d), xlab='time in s', legend=True)
for ax, _scen in zip(axes_2d, _scens):
pmu.plot_scenario(_scen, title=f'scenario: {_scen.name}', annotate=False, fig=fig, ax=ax)
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