def check_for_capture(searchers, target):
"""Check if the next position of any searcher
allow it to capture the target"""
# get next time and vertex (after evolving position)
next_time, v_target = ext.get_last_info(target.stored_v_true)
for s_id in searchers.keys():
s = searchers[s_id]
zeta = s.zeta
# get capture matrix for that vertex
C = s.capture_matrices.get(s.current_pos)
# just check for capture
if cm.check_capture(C, v_target):
# no false negatives
if zeta is None:
zeta = 0
chance_capture = random.random()
# flip a coin
if chance_capture <= 1 - zeta:
target.update_status(True)
searchers[s_id].update_status(True)
break
return searchers, target
def plot_sim_results(belief, target, searchers, sim_data, folder_name):
"""Get information of the simulation from the classes
assemble into frames for each time step"""
g, my_layout = sim_data.retrieve_graph()
# data folder path
folder_path = ext.get_whole_path(folder_name)
# time info
max_time = ext.get_last_info(target.stored_v_true)[0]
tau_ext = ext.get_set_time_u_0(max_time)
# capture info
capture_time = target.capture_time
captor = ext.find_captor(searchers)
vertex_cap = target.capture_v
# pack
capture_info = [capture_time, vertex_cap, captor]
print("Starting plots")
for t in tau_ext:
# get belief at that time
b_target = belief.stored[t]
# get POC(t)
b0 = b_target[0]
# plot belief
tgt_file = plot_target_belief2(g, folder_path, my_layout, b_target, t)
# plot searchers and true target position
s_file = plot_searchers_and_target(g, folder_path, my_layout, target,
searchers, t)
# assemble it nicely
mount_sim_frame(s_file, tgt_file, folder_path, b0, t, capture_info, 1)
delete_frames(folder_path, 'G')
return
def test_get_positions_searchers():
horizon, theta, deadline, solver_type = get_solver_param()
g, v0_target, v0_searchers, target_motion, belief_distribution = parameters_sim()
# ________________________________________________________________________________________________________________
# 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)
specs = my_specs()
target = cp.create_target(specs)
obj_fun, time_sol, gap, x_searchers, b_target, threads = pln.run_solver(g, horizon, searchers, b_0, M)
# get position of each searcher at each time-step based on x[s, v, t] variable
searchers, s_pos = pln.update_plan(searchers, x_searchers)
assert s_pos[1, 0] == 1
assert s_pos[1, 1] == 3
assert s_pos[1, 2] == 5
assert s_pos[1, 3] == 6
assert s_pos[2, 0] == 2
assert s_pos[2, 1] == 5
assert s_pos[2, 2] == 6
assert s_pos[2, 3] == 7
assert searchers[1].path_planned[0] == [1, 3, 5, 6]
assert searchers[2].path_planned[0] == [2, 5, 6, 7]
new_pos = pln.next_from_path(s_pos, 1)
searchers = pln.searchers_evolve(searchers, new_pos)
assert searchers[1].path_taken[1] == 3
assert searchers[2].path_taken[1] == 5
assert searchers[1].current_pos == 3
assert searchers[2].current_pos == 5
# get next time and vertex (after evolving position)
next_time, v_target = ext.get_last_info(target.stored_v_true)
# evolve searcher position
searchers[1].current_pos = v_target
searchers, target = sf.check_for_capture(searchers, target)
assert target.is_captured is True
def evolve_possible_position(self, updated_belief: list):
"""Use belief vector to store which vertices the target might be in (probability > 0)"""
# get current time and vertex
current_time = ext.get_last_info(self.stored_v_possible)[0]
# next time step
next_time = current_time + 1
new_vertices = []
n = len(updated_belief)
# get vertices where probability is higher than zero
for i in range(1, n):
if updated_belief[i] > 1e-4:
new_vertices.append(i)
# update the planning information
self.stored_v_possible[next_time] = new_vertices
def update(self, searchers: dict, pi_next_t: dict, Mtarget: list, n: int):
"""Use Eq. (2) to update current belief
store belief vector based on new belief"""
# get last time and belief
current_time, current_belief = ext.get_last_info(self.stored)
next_time = current_time + 1
# find the product of the capture matrices, s = 1...m
prod_C = cm.product_capture_matrix(searchers, pi_next_t, n)
# assemble the matrix for multiplication
big_M = cm.assemble_big_matrix(n, Mtarget)
new_belief = cm.belief_update_equation(current_belief, big_M, prod_C)
self.stored[next_time] = new_belief
self.new = new_belief
def plot_and_show_sim_results(belief, target, searchers, sim_data,
folder_name):
"""Get information of the simulation from the classes
turn into short video"""
# graph file and layout
g, my_layout = sim_data.retrieve_graph()
# data folder path
folder_path = ext.get_whole_path(folder_name)
# time info
max_time = ext.get_last_info(target.stored_v_true)[0]
tau_ext = ext.get_set_time_u_0(max_time)
# capture info
capture_time = target.capture_time
captor = ext.find_captor(searchers)
vertex_cap = target.capture_v
# pack
capture_info = [capture_time, vertex_cap, captor]
print("Starting plots")
for t in tau_ext:
# get belief at that time
b_target = belief.stored[t]
# get POC(t)
b0 = b_target[0]
# plot belief
tgt_file = plot_target_belief2(g, folder_path, my_layout, b_target, t)
# plot searchers and true target position
s_file = plot_searchers_and_target(g, folder_path, my_layout, target,
searchers, t)
# assemble it nicely and make copies
mount_sim_frame(s_file, tgt_file, folder_path, b0, t, capture_info)
# get until multiRobotTargetSearch/data/name_folder
# compose short video
compose_video(folder_path)
delete_frames(folder_path)
return
def evolve_true_position(self):
"""evolve the target position based on the probability given by the true motion matrix
which vertex is now, sample with probability into neighboring vertices
To be called each time step of the simulation"""
# get motion matrix
M = self.motion_matrix_true
# get current time and vertex
current_time, current_vertex = ext.get_last_info(self.stored_v_true)
# next time step
next_time = current_time + 1
# get moving probabilities for current vertex
my_vertices, prob_move = cm.probability_move(M, current_vertex)
# sample 1 vertex with probability weight according to prob_move
new_vertex = ext.sample_vertex(my_vertices, prob_move)
# update true position
self.stored_v_true[next_time] = new_vertex
self.current_pos = new_vertex
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 distributed_wrapper(g,
horizon,
searchers,
b0,
M_target,
gamma,
timeout=5,
n_inter=1,
pre_solver=-1):
"""Distributed version of the algorithm """
# parameter to stop iterations
# number of full loops s= 1..m
n_it = n_inter
# iterative parameters
total_time_sol = 0
previous_obj_fun = 0
my_counter = 0
# temporary path for the searchers
temp_pi = init_temp_path(searchers, horizon)
# get last searcher number [m]
m = ext.get_last_info(searchers)[0]
obj_fun_list = {}
time_sol_list = {}
gap_list = {}
while True:
for s_id in searchers.keys():
# create model
md = mf.create_model()
temp_pi['current_searcher'] = s_id
start, vertices_t, times_v = cm.get_vertices_and_steps_distributed(
g, horizon, searchers, temp_pi)
# add variables
my_vars = mf.add_variables(md, g, horizon, start, vertices_t,
searchers)
mf.add_constraints(md, g, my_vars, searchers, vertices_t, horizon,
b0, M_target)
# objective function
mf.set_solver_parameters(md, gamma, horizon, my_vars, timeout,
pre_solver)
# solve and save results
obj_fun, time_sol, gap, x_searchers, b_target, threads = mf.solve_model(
md)
if md.SolCount == 0:
# problem was infeasible or other error (no solution found)
print('Error, no solution found!')
obj_fun, time_sol, gap, threads = -1, -1, -1, -1
# keep previous belief
b_target = {}
v = 0
for el in b0:
b_target[(v, 0)] = el
v += 1
x_searchers = keep_all_still(temp_pi)
# clean things
md.reset()
md.terminate()
del md
# ------------------------------------------------------
# append to the list
obj_fun_list[s_id] = obj_fun
time_sol_list[s_id] = time_sol
gap_list[s_id] = gap
# save the current searcher's path
temp_pi = update_temp_path(x_searchers, temp_pi, s_id)
total_time_sol = total_time_sol + time_sol_list[s_id]
# end of a loop through searchers
if s_id == m:
# compute difference between previous and current objective functions
delta_obj = abs(previous_obj_fun - obj_fun)
# iterate
previous_obj_fun = obj_fun
my_counter = my_counter + 1
# check for stoppers: either the objective function converged or iterated as much as I wanted
if (delta_obj < 1e-4) or (my_counter >= n_it):
time_sol_list['total'] = total_time_sol
# clean and delete
disposeDefaultEnv()
return obj_fun_list, time_sol_list, gap_list, x_searchers, b_target, threads
def update_status(self, status=False):
time, vertex = ext.get_last_info(self.stored_v_true)
self.is_captured = status
self.capture_time = time
self.capture_v = vertex