def test_get_vertices():
g = ext.get_graph_00()
V, n = ext.get_set_vertices(g)
assert V == [1, 2, 3, 4, 5, 6, 7]
assert n == 7
V, n = ext.get_set_vertices(n)
assert V == [1, 2, 3, 4, 5, 6, 7]
assert n == 7
V_idx, n = ext.get_idx_vertices(g)
assert V_idx == [0, 1, 2, 3, 4, 5, 6]
assert n == 7
V_idx, n = ext.get_idx_vertices(n)
assert V_idx == [0, 1, 2, 3, 4, 5, 6]
assert n == 7
V_ext = ext.get_set_vertices_u_0(g)
assert V_ext == [0, 1, 2, 3, 4, 5, 6, 7]
V_ext = ext.get_set_vertices_u_0(n)
assert V_ext == [0, 1, 2, 3, 4, 5, 6, 7]
v_list = [1, 2]
v_left = ext.get_v_left(g, v_list)
v_left2 = ext.get_v_left(n, v_list)
assert v_left == [3, 4, 5, 6, 7]
assert v_left2 == [3, 4, 5, 6, 7]
def plot_target_belief(g, folder_name, my_layout, b_target: dict, t: int):
"""Plot target belief"""
V, n = ext.get_set_vertices(g)
rgba_color = (255, 0, 0, 1)
g.vs["color"] = "white"
for v in V:
v_idx = ext.get_python_idx(v)
b = b_target[v, t]
c = b / 1
if 0.001 < c < 0.7:
c = 0.7
my_color = get_color_belief(rgba_color, c)
if my_color[3] < 0.001:
my_color = "white"
g.vs[v_idx]["color"] = my_color
name_file = folder_name + "/" + g["name"] + "_tgt_t" + str(t) + ".png"
plot(g,
name_file,
layout=my_layout,
figsize=(3, 3),
bbox=(400, 400),
margin=15,
dpi=400)
return name_file
def plot_searchers_position(g, folder_name, my_layout, x_searchers: dict,
t: int):
"""plot results of searchers position"""
m = ext.get_m_from_tuple(x_searchers)
V, n = ext.get_set_vertices(g)
S = ext.get_set_searchers(m)[0]
g.vs["color"] = "white"
for s in S:
for v in V:
my_value = x_searchers.get((s, v, t))
if my_value == 1:
v_idx = ext.get_python_idx(v)
g.vs[v_idx]["color"] = "blue"
name_file = folder_name + "/" + g["name"] + "_t" + str(t) + ".png"
plot(g,
name_file,
layout=my_layout,
figsize=(3, 3),
bbox=(400, 400),
margin=15,
dpi=400)
return name_file
def get_vertices_and_steps(G, deadline, searchers):
"""Extract information from the user provided graph and information on searchers
For each time step, find which vertices each searcher is allowed to be"""
start = ext.get_searchers_positions(searchers)
# S_ and Tau
S, m = ext.get_set_searchers(start)
Tau = ext.get_set_time(deadline)
# get vertices
V, n = ext.get_set_vertices(G)
# shortest path lengths
spl = ext.get_length_short_paths(G)
# find V^tau(t) = {v in V} --> possible v(t) in optimal solution
vertices_t = {}
# find V^tau(v) = {t in Tau} --> possible t(v) in optimal solution
times_v = {}
start_idx = ext.get_python_idx(start)
for s in S:
s_idx = ext.get_python_idx(s)
# initial position
vertices_t[s, 0] = [start[s_idx]]
# starting at t = 1 (idx = 1), it begins to move
for t in Tau:
vertices_t[s, t] = []
# find the nodes that are within t of distance (thus can be reached at t)
for v in V:
v_idx = ext.get_python_idx(v)
dummy_var = spl[start_idx[s_idx]][v_idx]
if dummy_var <= t:
vertices_t[s, t].append(v)
# find times allowed for each vertex
for v in V:
times_v[s, v] = []
if v == start[s_idx]:
times_v[s, v] = [0]
for t in Tau:
if v in vertices_t[s, t]:
times_v[s, v].append(t)
# find dummy goal vertex and T + 1
v_g = ext.get_label_dummy_goal(V)
t_g = ext.get_last_t(Tau)
# append dummy goal
vertices_t[s,
t_g] = [v_g] # at T + 1, searcher is at dummy goal vertex
times_v[s, v_g] = [
t_g
] # the only time allowed for the dummy goal vertex is T + 1
return start, vertices_t, times_v
def plot_target_belief2(g,
folder_path,
my_layout,
b_target: dict,
t: int,
true_pos=None):
"""Plot target belief"""
V, n = ext.get_set_vertices(g)
rgba_color = (255, 0, 0, 1)
g.vs["color"] = "white"
for v in V:
v_idx = ext.get_python_idx(v)
b = b_target[v]
if b <= 0.005:
my_color = "white"
else:
if 0.005 < b <= 0.1:
c = 0.1
elif 0.1 < b <= 0.2:
c = 0.2
elif 0.2 < b <= 0.3:
c = 0.3
elif 0.3 < b <= 0.4:
c = 0.4
elif 0.4 < b <= 0.5:
c = 0.5
elif 0.5 < b <= 0.6:
c = 0.6
elif 0.6 < b <= 0.7:
c = 0.7
elif 0.7 < b <= 0.8:
c = 0.8
else:
c = b / 1
my_color = get_color_belief(rgba_color, c)
if true_pos is not None:
if v == true_pos:
my_color = "red"
g.vs[v_idx]["color"] = my_color
name_file = folder_path + "/" + g["name"] + "_tgt_t" + str(t) + ".png"
plot(g,
name_file,
layout=my_layout,
figsize=(3, 3),
bbox=(400, 400),
margin=15,
dpi=400)
plt.close()
return name_file
def add_capture_constraints(md, g, my_vars: dict, searchers: dict, vertices_t,
b0: list, M: list, deadline: int):
"""Define constraints about belief and capture events
:param vertices_t:
:param md:
:param g:
:param my_vars:
:param searchers:
:param b0
:param deadline
"""
# capture-related variables
beta = get_var(my_vars, 'beta')
alpha = get_var(my_vars, 'alpha')
psi = get_var(my_vars, 'psi')
false_neg, zeta = cm.check_false_negatives(searchers)
# if false negative model, there will exist a delta
if false_neg:
delta = get_var(my_vars, 'delta')
beta_s = get_var(my_vars, 'beta_s')
else:
delta = {}
beta_s = {}
# searchers position
X = get_var(my_vars, 'x')
# sets
V = ext.get_set_vertices(g)[0]
S, m = ext.get_set_searchers(searchers)
Tau = ext.get_set_time(deadline)
V_ext = ext.get_set_vertices_u_0(g)
# initial belief (user input), t = 0 (Eq. 13)
for i in V_ext:
md.addConstr(beta[i, 0] == b0[i])
for t in Tau:
# this is a dictionary
my_vertices = cm.get_current_vertices(t, vertices_t, S)
for v in V:
# v_idx = ext.get_python_idx(v)
# take Markovian model into account (Eq. 14)
# NOTE M matrix is accessed by python indexing
md.addConstr(alpha[v,
t] == quicksum(M[u - 1][v - 1] * beta[u, t - 1]
for u in V))
# find searchers position that could capture the target while it is in v
list_u_capture = cm.get_u_for_capture(searchers, V, v)
if list_u_capture and my_vertices:
if false_neg:
for s in S:
md.addConstr(
quicksum(
X[s, u, t]
for u in filter(lambda x: x in my_vertices[
s], list_u_capture)) >= psi[s, v, t])
md.addConstr(
quicksum(
X[s, u, t]
for u in filter(lambda x: x in my_vertices[
s], list_u_capture)) <= psi[s, v, t])
else:
md.addConstr(
quicksum(
quicksum(X[s, u, t] for u in filter(
lambda x: x in my_vertices[s], list_u_capture))
for s in S) >= psi[v, t])
md.addConstr(
quicksum(
quicksum(X[s, u, t] for u in filter(
lambda x: x in my_vertices[s], list_u_capture))
for s in S) <= m * psi[v, t])
if false_neg:
for s in S:
# first searcher
if s == S[0]:
md.addConstr(beta_s[0, v, t] == alpha[v, t])
# Eq. (38)
md.addConstr(delta[s, v, t] <= 1 - psi[s, v, t])
# Eq. (39)
md.addConstr(delta[s, v, t] <= beta_s[s - 1, v, t])
# Eq. (40)
md.addConstr(
delta[s, v, t] >= beta_s[s - 1, v, t] - psi[s, v, t])
# Eq. (37)
md.addConstr(beta_s[s, v,
t] == ((1 - zeta) * delta[s, v, t]) +
(zeta * beta_s[s - 1, v, t]))
# last searcher
md.addConstr(beta[v, t] == beta_s[S[-1], v, t])
else:
# (15)
md.addConstr(beta[v, t] <= 1 - psi[v, t])
# (16)
md.addConstr(beta[v, t] <= alpha[v, t])
# (17)
md.addConstr(beta[v, t] >= alpha[v, t] - psi[v, t])
# probability of being intercepted == what is left
md.addConstr(beta[0, t] == 1 - quicksum(beta[v, t] for v in V))
def add_target_variables(md, g, deadline: int, searchers=None):
"""Add variables related to the target and capture events:
belief variable, B
interception-related variables: belief vector composition, beta
belief evolution, alpha
capture event, zeta and psi
"""
# TODO change this to allow for different zetas (and unit-test it)
V = ext.get_set_vertices(g)[0]
T_ext = ext.get_set_time_u_0(deadline)
T = ext.get_set_time(deadline)
V_ext = ext.get_set_vertices_u_0(g)
var_for_test = {}
list_beta_name = []
list_alpha_name = []
list_delta_name = []
[beta, beta_s, alpha, psi, delta] = ext.init_dict_variables(5)
if searchers is not None:
false_neg, zeta = cm.check_false_negatives(searchers)
S = ext.get_set_searchers(searchers)[0]
else:
false_neg = False
S = None
# alpha and psi: only exist from 1, 2.., T
for t in T:
for v in V:
dummy_a_name = "[%d,%d]" % (v, t)
alpha[v, t] = md.addVar(vtype="CONTINUOUS",
lb=0.0,
ub=1.0,
name="alpha" + dummy_a_name)
list_alpha_name.append(dummy_a_name)
if false_neg:
for s in S:
dummy_delta_name = "[%d,%d,%d]" % (s, v, t)
psi[s, v, t] = md.addVar(vtype="BINARY",
name="psi" + dummy_delta_name)
delta[s, v, t] = md.addVar(vtype="CONTINUOUS",
lb=0.0,
ub=1.0,
name="delta" + dummy_delta_name)
list_delta_name.append(dummy_delta_name)
else:
psi[v, t] = md.addVar(vtype="BINARY",
name="psi" + dummy_a_name)
for t in T_ext:
for v in V_ext:
dummy_b_name = "[%d,%d]" % (v, t)
list_beta_name.append(dummy_b_name)
beta[v, t] = md.addVar(vtype="CONTINUOUS",
lb=0.0,
ub=1.0,
name="beta" + dummy_b_name)
if false_neg:
# include 0 for searcher s = 1, s-1 = 0
for s_ in [0] + S:
dummy_bs_name = "[%d,%d,%d]" % (s_, v, t)
beta_s[s_, v, t] = md.addVar(vtype="CONTINUOUS",
lb=0.0,
ub=1.0,
name="beta_s" + dummy_bs_name)
var_for_test.update({'beta': list_beta_name})
var_for_test.update({'alpha': list_alpha_name})
if false_neg:
my_vars = {
'beta': beta,
'beta_s': beta_s,
'alpha': alpha,
'psi': psi,
'delta': delta
}
var_for_test.update({'delta': list_delta_name})
else:
my_vars = {'beta': beta, 'alpha': alpha, 'psi': psi}
return my_vars, var_for_test
def test_time_consistency():
# GET parameters for the simulation, according to sim_param
horizon, theta, deadline, solver_type = get_solver_param()
g, v0_target, v0_searchers, target_motion, belief_distribution = parameters_sim()
gamma = 0.99
# get sets for easy iteration
V, n = ext.get_set_vertices(g)
# ________________________________________________________________________________________________________________
# INITIALIZE
# initialize parameters according to inputs
b_0 = cp.set_initial_belief(g, v0_target, belief_distribution)
M = cp.set_motion_matrix(g, target_motion)
searchers = cp.create_dict_searchers(g, v0_searchers)
solver_data = MySolverData(horizon, deadline, theta, g, solver_type)
belief = MyBelief(b_0)
target = MyTarget(v0_target, M)
# initialize time: actual sim time, t = 0, 1, .... T and time relative to the planning, t_idx = 0, 1, ... H
t, t_plan = 0, 0
# FIRST ITERATION
# call for model solver wrapper according to centralized or decentralized solver and return the solver data
obj_fun, time_sol, gap, x_searchers, b_target, threads = pln.run_solver(g, horizon, searchers, belief.new, M,
solver_type, gamma)
# save the new data
solver_data.store_new_data(obj_fun, time_sol, gap, threads, x_searchers, b_target, horizon)
# get position of each searcher at each time-step based on x[s, v, t] variable
searchers, path = pln.update_plan(searchers, x_searchers)
# reset time-steps of planning
t_plan = 1
path_next_t = pln.next_from_path(path, t_plan)
# evolve searcher position
searchers = pln.searchers_evolve(searchers, path_next_t)
# update belief
belief.update(searchers, path_next_t, M, n)
# update target
target = sf.evolve_target(target, belief.new)
# next time-step
t, t_plan = t + 1, t_plan + 1
assert t == 1
assert t_plan == 2
# get next time and vertex (after evolving position)
t_t, v_t = ext.get_last_info(target.stored_v_true)
assert target.current_pos == v_t
t_s, v_s = ext.get_last_info(searchers[1].path_taken)
assert t_t == t_s
assert t_t == t
# high level
specs = my_specs()
belief1, searchers1, solver_data1, target1 = pln.init_wrapper(specs)
deadline1, horizon1, theta1, solver_type1, gamma1 = solver_data1.unpack()
M1 = target1.unpack()
assert deadline1 == deadline
assert horizon1 == horizon
assert theta1 == theta
assert solver_type1 == solver_type
assert gamma1 == gamma
assert M1 == M
# initialize time: actual sim time, t = 0, 1, .... T and time relative to the planning, t_idx = 0, 1, ... H
t1, t_plan1 = 0, 0
# FIRST ITERATION
# call for model solver wrapper according to centralized or decentralized solver and return the solver data
obj_fun1, time_sol1, gap1, x_searchers1, b_target1, threads1 = pln.run_solver(g, horizon1, searchers1, belief1.new,
M1, solver_type1, gamma1)
assert obj_fun == obj_fun1
assert round(time_sol, 2) == round(time_sol1, 2)
assert gap == gap1
assert x_searchers == x_searchers1
assert b_target == b_target1
assert threads == threads1
# save the new data
solver_data1.store_new_data(obj_fun1, time_sol1, gap1, threads1, x_searchers1, b_target1, horizon1)
# get position of each searcher at each time-step based on x[s, v, t] variable
searchers1, path1 = pln.update_plan(searchers1, x_searchers1)
assert path == path1
# reset time-steps of planning
t_plan1 = 1
path_next_t1 = pln.next_from_path(path1, t_plan1)
assert path_next_t == path_next_t1
# evolve searcher position
searchers1 = pln.searchers_evolve(searchers1, path_next_t1)
# update belief
belief1.update(searchers1, path_next_t1, M1, n)
# update target
target1 = sf.evolve_target(target1, belief1.new)
# next time-step
t1, t_plan1 = t1 + 1, t_plan1 + 1
assert t1 == 1
assert t_plan1 == 2
assert target1.start_possible == target.start_possible
# get next time and vertex (after evolving position)
t_t1, v_t1 = ext.get_last_info(target1.stored_v_true)
t_s1, v_s1 = ext.get_last_info(searchers1[1].path_taken)
assert t_t1 == t_s1
assert t_t1 == t1
assert t_t1 == t_t
assert t_s1 == t_s
assert v_s1 == v_s
def test_get_graph():
g0 = ext.get_graph_00()
g1 = ext.get_graph_01()
g2 = ext.get_graph_02()
g3 = ext.get_graph_03()
g4 = ext.get_graph_04()
g5 = ext.get_graph_05()
g6 = ext.get_graph_06()
g7 = ext.get_graph_07()
assert g0['name'] == 'G7V_test'
assert g1['name'] == 'G60V'
assert g2['name'] == 'G100V_grid'
assert g3['name'] == 'G256V_grid'
assert g4['name'] == 'G9V_grid'
assert g5['name'] == 'G20_home'
assert g6['name'] == 'G25_home'
assert g7['name'] == 'G70V'
V0, n0 = ext.get_set_vertices(g0)
V1, n1 = ext.get_set_vertices(g1)
V2, n2 = ext.get_set_vertices(g2)
V3, n3 = ext.get_set_vertices(g3)
V4, n4 = ext.get_set_vertices(g4)
V5, n5 = ext.get_set_vertices(g5)
V6, n6 = ext.get_set_vertices(g6)
V7, n7 = ext.get_set_vertices(g7)
assert n0 == 7
assert n1 == 60
assert n2 == 100
assert n3 == 256
assert n4 == 9
assert n5 == 20
assert n6 == 25
assert n7 == 70
g0 = ext.get_graph('G7V_test')
g1 = ext.get_graph('G60V')
g2 = ext.get_graph('G100V_grid')
g3 = ext.get_graph('G256V_grid')
g4 = ext.get_graph('G9V_grid')
g5 = ext.get_graph('G20_home')
g6 = ext.get_graph('G25_home')
g7 = ext.get_graph('G70V')
assert g0['name'] == 'G7V_test'
assert g1['name'] == 'G60V'
assert g2['name'] == 'G100V_grid'
assert g3['name'] == 'G256V_grid'
assert g4['name'] == 'G9V_grid'
assert g5['name'] == 'G20_home'
assert g6['name'] == 'G25_home'
assert g7['name'] == 'G70V'
V0, n0 = ext.get_set_vertices(g0)
V1, n1 = ext.get_set_vertices(g1)
V2, n2 = ext.get_set_vertices(g2)
V3, n3 = ext.get_set_vertices(g3)
V4, n4 = ext.get_set_vertices(g4)
V5, n5 = ext.get_set_vertices(g5)
V6, n6 = ext.get_set_vertices(g6)
V7, n7 = ext.get_set_vertices(g7)
assert n0 == 7
assert n1 == 60
assert n2 == 100
assert n3 == 256
assert n4 == 9
assert n5 == 20
assert n6 == 25
assert n7 == 70
def get_vertices_and_steps_distributed(G, deadline, searchers, temp_s_path):
"""Extract information from the user provided graph and information on searchers
For each time step, find which vertices each searcher is allowed to be
Since this is the distributed version, use info on temporary searchers path (temp_s_path)"""
start = ext.get_searchers_positions(searchers)
# S_ and Tau
S, m = ext.get_set_searchers(start)
Tau = ext.get_set_time(deadline)
# get vertices
V, n = ext.get_set_vertices(G)
# shortest path lengths
spl = ext.get_length_short_paths(G)
# find V^tau(t) = {v in V} --> possible v(t) in optimal solution
vertices_t = {}
# find V^tau(v) = {t in Tau} --> possible t(v) in optimal solution
times_v = {}
for s in S:
# initial position
vertices_t[s, 0] = [temp_s_path[(s, 0)]]
st_idx = ext.get_python_idx(vertices_t.get((s, 0))[0])
# starting at t = 1 (idx = 1), it begins to move
for t in Tau:
vertices_t[s, t] = []
if s == temp_s_path['current_searcher']:
# if it's planning for this searcher, consider all possibilities
# find the nodes that are within t of distance (thus can be reached at t)
for v in V:
v_idx = ext.get_python_idx(v)
dummy_var = spl[st_idx][v_idx]
if dummy_var <= t:
vertices_t[s, t].append(v)
else:
# if is not the planning searcher, just use the info on the temporary path
# either the start vertex of the pre-computed path
v = temp_s_path[s, t]
vertices_t[s, t].append(v)
# find times allowed for each vertex
for v in V:
times_v[s, v] = []
if v == vertices_t[s, 0][0]:
times_v[s, v] = [0]
for t in Tau:
if v in vertices_t[s, t]:
times_v[s, v].append(t)
# find dummy goal vertex and T + 1
v_g = ext.get_label_dummy_goal(V)
t_g = ext.get_last_t(Tau)
# append dummy goal
vertices_t[s,
t_g] = [v_g] # at T + 1, searcher is at dummy goal vertex
times_v[s, v_g] = [
t_g
] # the only time allowed for the dummy goal vertex is T + 1
return start, vertices_t, times_v
