def test_find_no_solution_where_none_exists(my_map, starts, goals):
depth = 5
a1, a2 = 0, 1
a1_mdd = mdd.MDD(my_map, a1, starts[a1], goals[a1], depth)
a2_mdd = mdd.MDD(my_map, a2, starts[a2], goals[a2], depth)
solution_path = mdd.find_solution_in_joint_mdd([a1_mdd, a2_mdd],
dummy_tracker)
assert solution_path == None, "test_find_no_solution_where_none_exists Failed: Finds a solution where none exists"
print("test_find_no_solution_where_none_exists Passed")
def test_three_agent_joint_mdd_search(my_map, starts, goals):
depth = 4
a1, a2, a3 = 0, 1, 2
a1_mdd = mdd.MDD(my_map, a1, starts[a1], goals[a1], depth)
a2_mdd = mdd.MDD(my_map, a2, starts[a2], goals[a2], depth)
a3_mdd = mdd.MDD(my_map, a3, starts[a3], goals[a3], depth)
found_path = mdd.is_solution_in_joint_mdd([a1_mdd, a2_mdd, a3_mdd],
dummy_tracker)
assert found_path == True, "test_three_agent_joint_mdd_search Failed: Finds no solution with 3 agents when one exists"
print("test_three_agent_joint_mdd_search Passed")
def test_bootstrap_bfs_tree(my_map, starts, goals):
agent = 0
depth = 4
mdd_4 = mdd.MDD(my_map, 0, starts[agent], goals[agent], depth)
mdd_5 = mdd.MDD(my_map, 0, starts[agent], goals[agent], depth + 1, mdd_4)
bfs_tree = mdd_5.bfs_tree['tree']
for node in bfs_tree.keys():
for val in bfs_tree[node]:
loc, t = val
assert not my_map[loc[0]][loc[
1]], "test_bootstrap_bfs_tree Failed: Bootstrap BFS explores invalid cells"
assert t <= depth + 1, "test_bootstrap_bfs_tree Failed: Bootstrap BFS explores to depth greater than " + str(
depth + 1)
print("test_bootstrap_bfs_tree Passed")
def test_mdd_generation(my_map, starts, goals):
agent = 0
depth = 4
new_mdd = mdd.MDD(my_map, 0, starts[agent], goals[agent], depth)
# Hardcoded test for a simple case below
assert set(new_mdd.mdd[((1, 1), 0)]) == set(
[((2, 1), 1), ((1, 2), 1)]
), "test_mdd_generation Failed: test case ((1, 1), 0) -> ((2, 1), 1), ((1, 2), 1) failed"
assert set(new_mdd.mdd[((1, 2), 1)]) == set(
[((2, 2), 2), ((1, 3), 2)]
), "test_mdd_generation Failed: test case ((1, 2), 1) -> ((2, 2), 2), ((1, 3), 2) failed"
assert set(new_mdd.mdd[((2, 1), 1)]) == set(
[((2, 2), 2)]
), "test_mdd_generation Failed: test case ((2, 1), 1) -> ((2, 2), 2) failed"
assert set(new_mdd.mdd[((1, 3), 2)]) == set(
[((2, 3), 3)]
), "test_mdd_generation Failed: test case (((1, 3), 2) -> ((2, 3), 3) failed"
assert set(new_mdd.mdd[((2, 2), 2)]) == set(
[((3, 2), 3), ((2, 3), 3)]
), "test_mdd_generation Failed: test case ((2, 2), 2) -> ((3, 2), 3), ((2, 3), 3) failed"
assert set(new_mdd.mdd[((2, 3), 3)]) == set([
((3, 3), 4)
]), "test_mdd_generation Failed: test case ((2,3),3) -> ((3,3),4) failed"
assert set(new_mdd.mdd[((3, 2), 3)]) == set([
((3, 3), 4)
]), "test_mdd_generation Failed: test case ((3,2),3) -> ((3,3),4) failed"
print("test_mdd_generation Passed")
def test_find_solution_in_joint_mdd(my_map, starts, goals):
depth = 4
a1, a2, a3 = 0, 1, 2
a1_mdd = mdd.MDD(my_map, a1, starts[a1], goals[a1], depth)
a2_mdd = mdd.MDD(my_map, a2, starts[a2], goals[a2], depth)
a3_mdd = mdd.MDD(my_map, a3, starts[a3], goals[a3], depth)
solution_path = mdd.find_solution_in_joint_mdd([a1_mdd, a2_mdd, a3_mdd],
dummy_tracker)
for i in range(len(solution_path[0]) - 1):
this_locs = [solution_path[j][i] for j in range(len(solution_path))]
next_locs = [
solution_path[j][i + 1] for j in range(len(solution_path))
]
assert all_moves_valid(
this_locs, next_locs
), "test_find_solution_in_joint_mdd Failed: Finds a solution with invalid steps"
print("test_find_solution_in_joint_mdd Passed")
def test_mdd_level_i(my_map, starts, goals):
agent = 0
depth = 4
new_mdd = mdd.MDD(my_map, agent, starts[agent], goals[agent], depth)
assert {starts[agent]} == new_mdd.get_level(
0), "test_mdd_level_i Failed: Level 0 isn't the start node"
assert set([(2, 1), (1, 2)]) == set(
new_mdd.get_level(1)
), "test_mdd_level_i Failed: Level 1 isn't [((2, 1), 1), ((1, 2), 1)]"
print("test_mdd_level_i Passed")
def test_simple_construction(my_map, starts, goals):
agent = 0
depth = 7 # Arbitrary depth
new_mdd = mdd.MDD(my_map, 0, starts[agent], goals[agent], depth, False)
assert new_mdd.agent == 0, "test_simple_construction Failed: Agent is improperly set"
assert new_mdd.depth == depth, "test_simple_construction Failed: Depth is improperly set"
assert new_mdd.start == starts[
agent], "test_simple_construction Failed: Start is improperly set"
assert new_mdd.goal == goals[
agent], "test_simple_construction Failed: Goal is improperly set"
print("test_simple_construction Passed")
def test_depth_d_bfs_tree(my_map, starts, goals):
agent = 0
depth = 4
new_mdd = mdd.MDD(my_map, 0, starts[agent], goals[agent], depth)
bfs_tree_dict = new_mdd.get_depth_d_bfs_tree(my_map, starts[agent], depth)
bfs_tree = bfs_tree_dict['tree']
for node in bfs_tree.keys():
for val in bfs_tree[node]:
loc, t = val
assert not my_map[loc[0]][loc[
1]], "test_depth_d_bfs_tree Failed: BFS explores invalid cells"
assert t <= depth, "test_depth_d_bfs_tree Failed: BFS explores to depth greater than " + str(
depth)
print("test_depth_d_bfs_tree Passed")
def test_two_agent_joint_mdd_search(my_map, starts, goals):
a1, a2 = 0, 1
depth = 2 # No solution
a1_mdd = mdd.MDD(my_map, a1, starts[a1], goals[a1], depth)
a2_mdd = mdd.MDD(my_map, a2, starts[a2], goals[a2], depth)
found_path = mdd.is_solution_in_joint_mdd([a1_mdd, a2_mdd], dummy_tracker)
assert found_path == False, "test_two_agent_joint_mdd_search Failed: Finds a solution when none exists"
depth = 3 # Solution
a1_mdd = mdd.MDD(my_map, a1, starts[a1], goals[a1], depth)
a2_mdd = mdd.MDD(my_map, a2, starts[a2], goals[a2], depth)
found_path = mdd.is_solution_in_joint_mdd([a1_mdd, a2_mdd], dummy_tracker)
assert found_path == True, "test_two_agent_joint_mdd_search Failed: Finds no solution when one exists"
depth1 = 3
depth2 = 2 # Mismatched solution depths
a1_mdd = mdd.MDD(my_map, a1, starts[a1], goals[a1], depth1)
a2_mdd = mdd.MDD(my_map, a2, starts[a2], goals[a2], depth2)
found_path = mdd.is_solution_in_joint_mdd([a1_mdd, a2_mdd], dummy_tracker)
assert found_path == True, "test_two_agent_joint_mdd_search Failed: Finds no solution when the depth of the input mdds mismatch"
def find_solution(self, disjoint=True, heuristic=None):
""" Finds paths for all agents from their start locations to their goal locations
disjoint - use disjoint splitting or not
"""
self.start_time = timer.time()
# Generate the root node
# constraints - list of constraints
# paths - list of paths, one for each agent
# [[(x11, y11), (x12, y12), ...], [(x21, y21), (x22, y22), ...], ...]
# collisions - list of collisions in paths
root = {'cost': 0, 'constraints': [], 'paths': [], 'collisions': []}
for i in range(self.num_of_agents): # Find initial path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, root['constraints'])
if path is None:
raise BaseException('No solutions')
root['paths'].append(path)
root['cost'] = get_sum_of_cost(root['paths'])
root['collisions'] = detect_collisions(root['paths'])
root['h_val'] = 0
self.push_node(root)
# Task 3.1: Testing
#print(root['collisions'])
# Task 3.2: Testing
#for collision in root['collisions']:
# print(standard_splitting(collision))
##############################
# Task 3.3: High-Level Search
# Repeat the following as long as the open list is not empty:
# 1. Get the next node from the open list (you can use self.pop_node()
# 2. If this node has no collision, return solution
# 3. Otherwise, choose the first collision and convert to a list of constraints (using your
# standard_splitting function). Add a new child node to your open list for each constraint
# Ensure to create a copy of any objects that your child nodes might inherit
while len(self.open_list) > 0:
p = self.pop_node()
if len(p['collisions']) == 0:
self.sum_of_costs = get_sum_of_cost(p['paths'])
return p['paths']
collision = p['collisions'][0]
constraints = standard_splitting(
collision) #disjoint_splitting(collision)
for constraint in constraints:
q = {
'cost': 0,
'constraints':
copy.deepcopy(p['constraints'] + [constraint]),
'paths': copy.deepcopy(p['paths']),
'collisions': []
}
""" Task 4
if constraint['positive'] == True:
if paths_violate_constraint(constraint, q['paths']):
continue
"""
agent = constraint['agent']
path = a_star(self.my_map, self.starts[agent],
self.goals[agent], self.heuristics[agent], agent,
q['constraints'])
if path != None:
q['paths'][agent] = path
q['collisions'] = detect_collisions(q['paths'])
q['cost'] = get_sum_of_cost(q['paths'])
if self.CT_heuristic == None:
q['h_val'] = 0
else:
MDD_list = [mdd.MDD(self.my_map, self.starts[i], self.goals[i],\
compute_heuristics(self.my_map, self.goals[i]), i, q['constraints'])\
for i in range(len(self.starts))]
if self.CT_heuristic == "CG":
graph_inst = mdd.graph_from_MDD(MDD_list)
mvc_inst = mvc.BruteForceMVC(graph_inst)
q['h_val'] = mvc_inst.getMVC()
elif self.CT_heuristic == "DG":
graph_inst = joint_mdd.graph_from_MDD(MDD_list)
mvc_inst = mvc.BruteForceMVC(graph_inst)
q['h_val'] = mvc_inst.getMVC()
elif self.CT_heuristic == "WDG":
graph_inst = joint_mdd.weighted_graph_from_MDD(
MDD_list, self.my_map, self.starts, self.goals)
ewmvc_inst = mvc.BruteForceEWMVC(graph_inst)
q['h_val'] = ewmvc_inst.getEWMVC()
self.push_node(q)
if path is None:
raise BaseException('No solutions')
self.print_results(root)
return root['paths']
