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']