def find_solution(self): """ Finds paths for all agents from their start locations to their goal locations.""" start_time = timer.time() result = [] constraints = [] # constraints = [{'agent': 0, 'loc': [(1,5)], 'timestep': 4, 'type': 'vertex'}] # For Task 1,2 # constraints = [{'agent': 0, 'loc': [(1,5)], 'timestep': 4}] # For Task 1,2 # constraints = [{'agent': 1, 'loc': [(1,2), (1,3)], 'timestep': 1, 'type': 'edge'}] # For Task 1,3 # constraints = [{'agent': 1, 'loc': [(1,2), (1,3)], 'timestep': 1}] # For Task 1,3 # constraints = [{'agent': 0, 'loc': [(1,5)], 'timestep': 6, 'type': 'vertex'}, # {'agent': 0, 'loc': [(1,5)], 'timestep': 15, 'type': 'vertex'}, # {'agent': 0, 'loc': [(1,5)], 'timestep': 9, 'type': 'vertex'}] # For Task 1.4 # constraints = [{'agent':1, 'loc': [(1,3), (1,4)], 'timestep': 2}, # {'agent':1, 'loc': [(1,3)], 'timestep': 2}, # {'agent':1, 'loc': [(1,2)], 'timestep': 2}, # {'agent': 0, 'loc': [(1,5)], 'timestep': 6, 'type': 'vertex'}] # For Task 1.5 # agent_priorities = [1,0] # Bad ordering for exp2_1.txt agent_priorities = range(self.num_of_agents) # Linear ordering for ind, i in enumerate(agent_priorities): # Find path for each agent path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, constraints) if path is None: raise BaseException('No solutions') result.append(path) for j in range(ind + 1, len(agent_priorities)): agent = agent_priorities[j] for timestep in range(len(path) - 1): curr_loc = path[timestep] next_loc = path[timestep + 1] constraints.append({ 'agent': agent, 'loc': [curr_loc], 'timestep': timestep, 'type': 'vertex' }) constraints.append({ 'agent': agent, 'loc': [next_loc, curr_loc], 'timestep': timestep + 1, 'type': 'edge' }) constraints.append({ 'agent': agent, 'loc': path[-1], 'timestep': len(path) - 1, 'type': 'inf' }) self.CPU_time = timer.time() - start_time print("\n Found a solution! \n") print("CPU time (s): {:.2f}".format(self.CPU_time)) print("Sum of costs: {}".format(get_sum_of_cost(result))) print(result) return result
def find_most_optimal_paths(self): optimal_paths = [] for agent in range(self.num_of_agents): optimal_paths.append( a_star(self.my_map, self.starts[agent], self.goals[agent], self.heuristics[agent], agent, [])) return optimal_paths
def find_solution(self): """ Finds paths for all agents from their start locations to their goal locations.""" start_time = timer.time() result = [] ############################## # Task 1: Understand the following code (see the lab description for some hints) for i in range(self.num_of_agents): # Find path for each agent path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, [], []) if path is None: raise BaseException('No solutions') result.append(path) ############################## self.CPU_time = timer.time() - start_time print("\n Found a solution! \n") print("CPU time (s): {:.2f}".format(self.CPU_time)) print("Sum of costs: {}".format(get_sum_of_cost(result))) return result
def find_solution(self): """ Finds paths for all agents from their start locations to their goal locations.""" start_time = timer.time() result = [] constraints = [] for i in range(self.num_of_agents): # Find path for each agent path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, constraints) if path is None: raise BaseException('No solutions') result.append(path) ############################## # Task 2: Add constraints here # Useful variables: # * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)] # * self.num_of_agents has the number of total agents # * constraints: array of constraints to consider for future A* searches ############################## self.CPU_time = timer.time() - start_time print("\n Found a solution! \n") print("CPU time (s): {:.2f}".format(self.CPU_time)) print("Sum of costs: {}".format(get_sum_of_cost(result))) print(result) return result
def find_solution(self): """ Finds paths for all agents from their start locations to their goal locations.""" start_time = timer.time() result = [] constraints = [] max_path_len = 0 map_size = sum(x.count(False) for x in self.my_map) upperbound = max_path_len + map_size # constraints.append({'agent': 0, 'loc': [(1, 5)], 'timestep': 4}) # 1.2 # constraints.append({'agent': 1, 'loc': [(1, 2), (1,3)], 'timestep': 1}) # 1.3 # constraints.append({'agent': 0, 'loc': [(1, 5)], 'timestep': 10}) # 1.4 # 1.5 # constraints.append({'agent': 1, 'loc': [(1, 2)], 'timestep': 2}) # constraints.append({'agent': 1, 'loc': [(1, 3)], 'timestep': 2}) # constraints.append({'agent': 1, 'loc': [(1, 4)], 'timestep': 2}) for i in range(self.num_of_agents): # Find path for each agent path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, constraints) if path is None: raise BaseException('No solutions') # Task 2.4 if len(path) > upperbound: print("Reach upper bound {} for agent {} with path length {}".format(upperbound, i,len(path))) raise BaseException('No solutions') # Calculate an upper bound on the path length max_path_len = max(max_path_len, len(path)) upperbound = max_path_len + map_size result.append(path) ############################## # Task 2: Add constraints here # Useful variables: # * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)] # * self.num_of_agents has the number of total agents # * constraints: array of constraints to consider for future A* searches for agent in range(i + 1, self.num_of_agents + 1): # future agents for t in range(len(path)): constraints.append({'agent': agent, 'loc': [path[t]], 'timestep': t}) # vertex if t > 0: constraints.append({'agent': agent, 'loc': [path[t], path[t-1]], 'timestep': t}) # edge constraints.append({'agent': agent, 'loc': [path[t-1], path[t]], 'timestep': t}) # edge # Task 2.3 Additional Constraints (can also implemented by modify is_constrained function in single_agent_planner.py,then need to comment out it when choosing CBS) for t_future in range(len(path), upperbound): constraints.append({'agent': agent, 'loc': [path[len(path) - 1]], 'timestep': t_future}) ############################## self.CPU_time = timer.time() - start_time print("\n Found a solution! \n") print("CPU time (s): {:.2f}".format(self.CPU_time)) print("Sum of costs: {}".format(get_sum_of_cost(result))) print(result) return result
def find_solution(self): """ Finds paths for all agents from their start locations to their goal locations.""" start_time = timer.time() result = [] constraints = [] for i in range(self.num_of_agents): # Find path for each agent path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, constraints) if path is None: raise BaseException('No solutions') result.append(path) ############################## # Task 2: Add constraints here # Useful variables: # * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)] # * self.num_of_agents has the number of total agents # * constraints: array of constraints to consider for future A* searches for timestep, loc in enumerate(path): for agent in range(i + 1, self.num_of_agents): constraints.append({ 'agent': agent, 'loc': [loc], 'timestep': timestep, 'positive': False }) if timestep + 1 < len(path): next_loc = path[timestep + 1] constraints.append({ 'agent': agent, 'loc': [next_loc, loc], 'timestep': timestep + 1, 'positive': False }) # Code to handle Task 2.3 (Adding Additional Constraints) else: # At goal = Negative timestep = Constrain for all future timesteps too constraints.append({ 'agent': agent, 'loc': [loc], 'timestep': -(timestep + 1), 'positive': False }) ############################## self.CPU_time = timer.time() - start_time print("\n Found a solution! \n") print("CPU time (s): {:.2f}".format(self.CPU_time)) print("Sum of costs: {}".format(get_sum_of_cost(result))) print(result) return result
def find_solution(self): """ Finds paths for all agents from their start locations to their goal locations.""" start_time = timer.time() result = [] blocks = [] constraints = [] for i in range(self.num_of_agents): # Find path for each agent path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, constraints) if path is None: raise BaseException('No solutions') result.append(path) for count in range(len(path)): blocks.append({'loc': [path[count]], 'timestep': count}) if count == len(path) - 1: blocks.append({ 'loc': [path[count]], 'timestep': '>' + str(count) }) if count == 0: continue else: blocks.append({ 'loc': [path[count], path[count - 1]], 'timestep': count }) for j in range(i + 1, self.num_of_agents): for block in blocks: constraints.append({ 'agent': j, 'loc': block['loc'], 'timestep': block['timestep'] }) ############################## # Task 2: Add constraints here # Useful variables: # * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)] # * self.num_of_agents has the number of total agents # * constraints: array of constraints to consider for future A* searches ############################## self.CPU_time = timer.time() - start_time print("\n Found a solution! \n") print("CPU time (s): {:.2f}".format(self.CPU_time)) print("Sum of costs: {}".format(get_sum_of_cost(result))) print(result) return result
def find_solution(self): """ Finds paths for all agents from their start locations to their goal locations.""" start_time = timer.time() result = [] constraints = [] for i in range(self.num_of_agents): # Find path for each agent path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, constraints, []) if path is None: raise BaseException('No solutions') result.append(path) ############################## # Task 2: Add constraints here # Useful variables: # * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)] # * self.num_of_agents has the total number of agents # * constraints: array of constraints to consider for future A* searches ############################## for sid, place in enumerate(path): for agent in range(i, self.num_of_agents): constraints.append({ 'agent': agent + 1, 'loc': [place], 'timestep': sid, 'positive': False }) #constraints.append({'agent' : agent + 1, 'loc': [place], 'timestep': sid+1, 'positive': False}) if sid < len(path) - 1: constraints.append({ 'agent': agent + 1, 'loc': [path[sid], path[sid + 1]], 'timestep': sid + 1, 'positive': False }) constraints.append({ 'agent': agent + 1, 'loc': [path[sid + 1], path[sid]], 'timestep': sid + 1, 'positive': False }) self.CPU_time = timer.time() - start_time print("\n Found a solution! \n") print("CPU time (s): {:.2f}".format(self.CPU_time)) print("Sum of costs: {}".format(get_sum_of_cost(result))) print(result) return result
def compute_paths(self, agents_need_update, child, group_idx=None): if len(agents_need_update) == 1: agent = agents_need_update[0] path = a_star(self.my_map, self.starts[agent], self.goals[agent], self.heuristics[agent], agent, flatten_constraints(child['constraints'])) if path is None: return False child['paths'][agent] = path else: t_starts = tuple(self.starts[i] for i in agents_need_update) t_goals = tuple(self.goals[i] for i in agents_need_update) agents_mapping = {} for agent in agents_need_update: agents_mapping[agent] = len(agents_mapping) new_agents = [agents_mapping[a] for a in agents_need_update] meta_constraints = [] for constraint in child['constraints']: if constraint['agent'] == agents_need_update: meta_constraints.append(copy.copy(constraint)) meta_constraints[len(meta_constraints) - 1]['agent'] = new_agents if len(meta_constraints) != 0: t_constraints = convert_cons(meta_constraints) # print("agents: " + str(agents_need_update) + " cons: " + str(t_constraints)) else: t_constraints = (tuple(agents_need_update), ()) key = (t_starts, t_constraints) if key in self.cache: path = self.cache[key] else: planner = constrained_od_mstar.Constrained_Od_Mstar( self.my_map, t_starts, t_goals, t_constraints, epeastar=epeastar, astar=astar) path = planner.find_path_time_pad(t_starts) self.cache[key] = path if path is None: return False path = convert_path(path) for (idx, m) in enumerate(agents_need_update): child['paths'][m] = path[idx] return True
def find_solution(self, disjoint=True): """ 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']) 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 self.print_results(root) return root['paths']
def compute_paths(map, starts, goals, heuristics, agents_need_update, child, group_idx=None): if isinstance(agents_need_update, int) or len(agents_need_update) == 1: if isinstance(agents_need_update, list): agent = agents_need_update[0] path = a_star(map, starts[agent], goals[agent], heuristics[agent], agent, flatten_constraints(child['constraints'])) if path is None: return False child['paths'][agent] = path else: cbs = CBSSolver(map, [starts[m] for m in agents_need_update], [goals[m] for m in agents_need_update]) agents_mapping = {} for agent in agents_need_update: agents_mapping[agent] = len(agents_mapping) meta_constraints = [] for constraint in child['constraints']: if constraint['group'] == group_idx: for agent in constraint['agent']: meta_constraints.append(copy.copy(constraint)) # print('agents:', constraint['agent']) # print('mapping:', agents_mapping) meta_constraints[len(meta_constraints) - 1]['agent'] = agents_mapping[agent] result = cbs.find_solution(meta_constraints=meta_constraints) if isinstance(result, tuple): path = result[0] elif result is None: return False for (idx, m) in enumerate(agents_need_update): child['paths'][m] = path[idx] # print("path", path) return True
def find_solution(self): """ Finds paths for all agents from their start locations to their goal locations.""" start_time = timer.time() result = [] constraints = [] # Constraint for Task 1.2 # constraints = [{'agent': 0,'loc': [(1,5)],'timestep': 4}] # Constraint for Task 1.3 # constraints = [{'agent': 1,'loc': [(1,2),(1,3)],'timestep': 1}] # Constraints for Task 1.4 # constraints = [{'agent': 0,'loc': [(1,5)],'timestep': 10}, # {'agent': 0,'loc': [(1,3),(1,4)],'timestep': 5}, # {'agent': 0,'loc': [(1,3),(1,4)],'timestep': 7}, # {'agent': 0,'loc': [(1,2),(1,3)],'timestep': 2}] # Constraints for Task 1.5 # constraints = [{'agent': 1,'loc': [(1,4)],'timestep': 2}, # {'agent': 1,'loc': [(1,3),(1,2)],'timestep': 2}] for i in range(self.num_of_agents): # Find path for each agent path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, constraints) if path is None: raise BaseException('No solutions') result.append(path) ############################## # Task 2: Add constraints here # Useful variables: # * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)] # * self.num_of_agents has the number of total agents # * constraints: array of constraints to consider for future A* searches ############################## # if len(constraint_table.keys()) != 0: # if next_time > max(constraint_table.keys()): # next_time = max(constraint_table.keys()) # print(path) for next_agent in range(self.num_of_agents): for i in range(0, 10): # # Task 2.3 Adding goal constraints and max timestep allowed was 10 constraints.append({ 'agent': next_agent, 'loc': [path[len(path) - 1]], 'timestep': len(path) + i - 1 }) for nPath in range(len(path)): # print(path[len(path) - 1]) # constraints.append({'agent' : next_agent, 'loc' : [path[len(path) - 1]], 'timestep' : nPath}) # constraints.append({'agent' : next_agent, 'loc' : [path[len(path) - 1]], 'timestep' : nPath+len(path)}) # Task 2.1 Vertex Constraints if next_agent != i: constraints.append({ 'agent': next_agent, 'loc': [path[nPath]], 'timestep': nPath }) # Task 2.2 Edge Constraints if nPath > 0: constraints.append({ 'agent': next_agent, 'loc': [path[nPath], path[nPath - 1]], 'timestep': nPath }) self.CPU_time = timer.time() - start_time # print(constraints) print("\n Found a solution! \n") print("CPU time (s): {:.2f}".format(self.CPU_time)) print("Sum of costs: {}".format(get_sum_of_cost(result))) print(result) return result
def find_solution(self, disjoint=True, mode=None): """ Finds paths for all agents from their start locations to their goal locations disjoint - use disjoint splitting or not """ if mode is None: self.mode = CBSSolver.NORMAL else: self.mode = mode 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']) self.push_node(root) ############################## # 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 self.open_list: node = self.pop_node() if self.mode == CBSSolver.DIAGNOSTIC: self.print_node(node) collisions = detect_collisions(node['paths']) if not collisions: self.print_results(node) return node['paths'] first_collision = collisions[0] constraints = standard_splitting(first_collision) for constraint in constraints: child = { 'cost': 0, 'constraints': [], 'paths': [], 'collisions': [] } child['constraints'] = node['constraints'].copy() child['constraints'].append(constraint) child['paths'] = node['paths'].copy() agent_id = constraint['agent'] new_path = a_star(self.my_map, self.starts[agent_id], self.goals[agent_id], self.heuristics[agent_id], agent_id, child['constraints']) if new_path is not None: child['paths'][agent_id] = new_path child['collisions'] = detect_collisions(child['paths']) child['cost'] = get_sum_of_cost(child['paths']) self.push_node(child) print('\n No Solution Found! \n') CPU_time = timer.time() - self.start_time print("CPU time (s): {:.2f}".format(CPU_time)) return None
def find_solution(self, disjoint=True): """ 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']) 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']) < 1: self.print_results(P) return P['paths'] # select first collision collision = P['collisions'][0] constraints = standard_splitting(collision) if disjoint is False else disjoint_splitting(collision) for c_idx, constraint in enumerate(constraints): Q = {'cost': 0, 'constraints': [], 'paths': [], 'collisions': []} Q['constraints'] = copy.copy(P['constraints']) self.add_constraint(constraint, Q['constraints']) Q['paths'] = copy.copy(P['paths']) agent = constraint['agent'] new_path = a_star(self.my_map, self.starts[agent], self.goals[agent], self.heuristics[agent], agent, Q['constraints']) # find the violated agent given the new path with positive constraints (valid when disjoint == True) if disjoint and 'positive' in constraint: violate_agents = paths_violate_constraint(constraint, Q['paths']) for c_neg_idx, neg_constraint in enumerate(constraints): if 'positive' not in neg_constraint: self.add_constraint(neg_constraint, Q['constraints']) else: violate_agents = [] # update other agent path given new negative constraints (valid when disjoint == True) violate_paths = {} num_new_paths = 0 for violate_agent in violate_agents: new_vio_path = a_star(self.my_map, self.starts[violate_agent], self.goals[violate_agent], self.heuristics[violate_agent], violate_agent, Q['constraints']) violate_paths[violate_agent] = new_vio_path if new_vio_path is not None and len(new_vio_path) > 0: num_new_paths += 1 # add new child node if new path of current agent and new paths from other agents exsit. if len(new_path) > 0 and num_new_paths == len(violate_agents): # update pathes Q['paths'][agent] = new_path for violate_agent in violate_agents: Q['paths'][violate_agent] = violate_paths[violate_agent] # update collison and costs new_collison = detect_collisions(Q['paths']) Q['collisions'] = new_collison Q['cost'] = get_sum_of_cost(Q['paths']) self.push_node(Q) self.print_results(root) return root['paths']
def find_solution(self, disjoint=True): """ 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 print("Time through init loop -", i+1) 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']) self.push_node(root) # Task 3.1: Testing print(root['collisions']) # Task 3.2: Testing for collision in root['collisions']: print(disjoint_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: curr = self.pop_node() if len(curr['collisions']) == 0: # paths found print("PATH REACHED:") for i in curr['paths']: print(i) return curr['paths'] collision = curr['collisions'].pop(0) constraints = standard_splitting(collision) for constraint in constraints: child = {'cost': 0, 'constraints': [], 'paths': [], 'collisions': []} child['constraints'] = curr['constraints'] child['constraints'].append(constraint) child['paths'] = curr['paths'] agents = paths_violate_constraint(curr['paths'], constraint, self.num_of_agents) # disjoint splitting agent = constraint['agent'] path = a_star(self.my_map, self.starts[agent], self.goals[agent], self.heuristics[agent], agent, child['constraints']) if path != None and len(path) > 0: child['paths'][agent] = path child['collisions'] = detect_collisions(child['paths']) child['cost'] = get_sum_of_cost(child['paths']) self.push_node(child) # end of if statement # end of for loop # end of while loop self.print_results(root) return root['paths']
def find_solution(self): """ Finds paths for all agents from their start locations to their goal locations.""" start_time = timer.time() result = [] constraints = [{ 'agent': 1, 'loc': [(1, 3), (1, 2)], 'time_step': 2 }, { 'agent': 1, 'loc': [(1, 3), (1, 3)], 'time_step': 2 }, { 'agent': 1, 'loc': [(1, 3), (1, 4)], 'time_step': 2 }] # added these constraints for 1.5 maxPathLength = 0 for i in range(self.num_of_agents): # Find path for each agent path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, constraints) if path is None: raise BaseException('No solutions') result.append(path) ############################## # Task 2: Add constraints here # Useful variables: # * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)] # * self.num_of_agents has the number of total agents # * constraints: array of constraints to consider for future A* searches maxPathLength = max(maxPathLength, len(path)) upperBound = len(self.my_map[0]) * len(self.my_map) + maxPathLength # Task 2.1 adding vertex constraints: need to update this for the upperbound for time in range(len( path)): # will have to update this line for 2.4 and 2.5 for nextAgent in range( i + 1, self.num_of_agents ): # add vertex constraints for all future agents (i = current agent from top for loop) constraints.append( { 'agent': nextAgent, 'loc': [path[time]], 'time_step': time } ) # add vertex constraints # will have to update this line for 2.4 and 2.5 # Task 2.2 adding edge constraints: for time in range(1, len(path)): for nextAgent in range(i + 1, self.num_of_agents): constraints.append({ 'agent': nextAgent, 'loc': [path[time], path[time - 1]], 'time_step': time }) #Task 2.3 adding additional constraints: for time in range(len(path), upperBound + 1): lastLoc = path[len(path) - 1] for nextAgent in range(i + 1, self.num_of_agents): constraints.append({ 'agent': nextAgent, 'loc': [lastLoc], 'time_step': time }) ############################## self.CPU_time = timer.time() - start_time print("\n Found a solution! \n") print("CPU time (s): {:.2f}".format(self.CPU_time)) print("Sum of costs: {}".format(get_sum_of_cost(result))) print(result) return result
def find_solution(self): """ Finds paths for all agents from their start locations to their goal locations.""" start_time = timer.time() result = [] constraints = [] for i in range(self.num_of_agents): # Find path for each agent path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, constraints) if path is None: raise BaseException('No solutions') result.append(path) ############################## # Task 2: Add constraints here # Useful variables: # * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)] # * self.num_of_agents has the number of total agents # * constraints: array of constraints to consider for future A* searches for node_index in range(len(path)): for agent_index in range(self.num_of_agents): if agent_index != i: # Adding Vertex Constraints vertex_constraint = {'agent': agent_index, 'loc':[path[node_index]], 'timestep': node_index} constraints.append(vertex_constraint) # Adding Edge Constraints for dir in range(4): curr_pos = move(path[node_index], dir) if self.my_map[curr_pos[0]][curr_pos[1]]: continue edge_constraint = {'agent': agent_index, 'loc':[curr_pos, path[node_index]], 'timestep':node_index+1} constraints.append(edge_constraint) # Calculate the edge_constraints for goal state nodes path_upperbound = 0 for p in result: path_upperbound += len(p) path_upperbound += len(self.my_map)+len(self.my_map[0]) ### Look for something more specific to find upper bound for index, route in enumerate(result): for i in range(path_upperbound-len(route)): goal_state = route[len(route)-1] time = len(route) + i - 1 # create constraints for every agent for a in range(self.num_of_agents): if a == index: continue for dir in range(4): curr_pos = move(goal_state, dir) if self.my_map[curr_pos[0]][curr_pos[1]]: continue edge_constraint = {'agent': a, 'loc':[curr_pos, goal_state], 'timestep':time+1} constraints.append(edge_constraint) ############################## self.CPU_time = timer.time() - start_time print("\n Found a solution! \n") print("CPU time (s): {:.2f}".format(self.CPU_time)) print("Sum of costs: {}".format(get_sum_of_cost(result))) print(result) return result
def find_solution(self, disjoint): """ 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 for agent ' + str(i)) root['paths'].append(path) root['cost'] = get_sum_of_cost(root['paths']) root['collisions'] = detect_collisions(root['paths']) 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: curr = self.pop_node() if len(curr['collisions']) == 0: self.print_results(curr) return curr['paths'] #collision = curr['collisions'][0] collision = ICBS(curr['collisions'], curr['paths'], curr['constraints'], self) if disjoint: constraints = disjoint_splitting(collision) else: constraints = standard_splitting(collision) for constraint in constraints: Q = { 'cost': 0, 'constraints': [], 'paths': [], 'collisions': [] } Q['constraints'] = copy.deepcopy(curr['constraints']) Q['constraints'].append(constraint) Q['paths'] = copy.deepcopy(curr['paths']) if disjoint: if constraint['positive'] == 0: ai = constraint['agent'] path = a_star(self.my_map, self.starts[ai], self.goals[ai], self.heuristics[ai], ai, Q['constraints']) if path is None: continue Q['paths'][ai] = path else: newc = new_constraints(constraint, self.num_of_agents) for c in newc: Q['constraints'].append(c) agentsId = paths_violate_constraint( curr['paths'], constraint) ai = constraint['agent'] path = a_star(self.my_map, self.starts[ai], self.goals[ai], self.heuristics[ai], ai, Q['constraints']) if path is None: continue Q['paths'][ai] = path no_path = 0 for agent in agentsId: path = a_star(self.my_map, self.starts[agent], self.goals[agent], self.heuristics[agent], agent, Q['constraints']) if path is None: no_path = 1 break Q['paths'][agent] = path if no_path == 1: continue else: ai = constraint['agent'] path = a_star(self.my_map, self.starts[ai], self.goals[ai], self.heuristics[ai], ai, Q['constraints']) if path is None: continue Q['paths'][ai] = path Q['collisions'] = detect_collisions(Q['paths']) Q['cost'] = get_sum_of_cost(Q['paths']) self.push_node(Q) raise BaseException('No solutions')
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']
def find_solution(self): """ Finds paths for all agents from their start locations to their goal locations.""" start_time = timer.time() result = [] # 1.2 # constraints = [ # { # 'agent': 0, # 'loc': [(1, 5)], # 'timestep': 4 # } # ] # 1.3 # constraints = [ # { # 'agent': 1, # 'loc': [(1,2), (1, 3)], # 'timestep': 1 # } # ] # 1.4 # constraints = [ # { # 'agent': 0, # 'loc': [(1, 5)], # 'timestep': 10 # } # ] # 1.5 # constraints = [ # { # 'agent': 1, # 'loc': [(1,3), (1,4)], # 'timestep': 2 # }, # { # 'agent': 1, # 'loc': [(1,2)], # 'timestep': 2 # }, # { # 'agent': 1, # 'loc': [(1,3)], # 'timestep': 2 # } # ] constraints = [] mymap_size = len(max(self.my_map)) * len(self.my_map) print("mymap_size: ", mymap_size) for i in range(self.num_of_agents): # Find path for each agent path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, constraints) if path is None: raise BaseException('No solutions') ############################## # Task 2: Add constraints here # Useful variables: # * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)] # * self.num_of_agents has the number of total agents # * constraints: array of constraints to consider for future A* searches for j in range(self.num_of_agents): if (j == i): continue timestep = 0 for cell in path: constraints.append({ 'agent': j, 'loc': [cell], 'timestep': timestep }) timestep += 1 if (timestep < len(path)): constraints.append({ 'agent': j, 'loc': [cell, path[timestep]], 'timestep': timestep }) # goal_timestep = timestep # add constraint once it reaches goal to the upper bound timestep while (timestep <= mymap_size): constraints.append({ 'agent': j, 'loc': [path[-1]], 'timestep': timestep }) timestep += 1 result.append(path) ############################## self.CPU_time = timer.time() - start_time print("\n Found a solution! \n") print("CPU time (s): {:.2f}".format(self.CPU_time)) print("Sum of costs: {}".format(get_sum_of_cost(result))) print(result) return result
def find_solution(self, disjoint=True): """ Finds paths for all agents from their start locations to their goal locations disjoint - use disjoint splitting or not """ self.start_time = timer.time() 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']) self.push_node(root) ############################## # 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: curr = self.pop_node( ) # firstly sorted with cost then sorted with #constraints # no collision return solution if len(curr['collisions']) == 0: self.print_results(curr) print(curr) return curr['paths'] first_collision = curr['collisions'][0] constraints = [] # standard CBS if disjoint == False: constraints = standard_splitting(first_collision) # CBS with disjoint splitting else: constraints = disjoint_splitting(first_collision) # for each constraint add a new child node for constraint in constraints: # deep copy parent attributes to child child_constarints = copy_parent(curr['constraints']) if constraint not in child_constarints: child_constarints.append(constraint) child_paths = copy_parent(curr['paths']) constrainted_agent = constraint['agent'] child = { 'cost': 0, 'constraints': child_constarints, 'paths': child_paths, 'collisions': [] } if constraint['positive'] == 0: path = a_star(self.my_map, self.starts[constrainted_agent], self.goals[constrainted_agent], self.heuristics[constrainted_agent], constrainted_agent, child['constraints']) path = eliminate_duplicates(path) if path is not None: # replace the constrainted agent path by new path in parent paths child['paths'][constrainted_agent] = path child['collisions'] = detect_collisions(child['paths']) child['cost'] = get_sum_of_cost(child['paths']) self.push_node(child) else: Add = True # update all agents' path violate_IDs = paths_violate_constraint( constraint, curr['paths']) # replace the constrainted agent path by new path in parent paths for each_agent in violate_IDs: path = a_star(self.my_map, self.starts[each_agent], self.goals[each_agent], self.heuristics[each_agent], each_agent, child['constraints']) path = eliminate_duplicates(path) if path is not None: child['paths'][each_agent] = path else: Add = False break #update the constraint agent path = a_star(self.my_map, self.starts[constrainted_agent], self.goals[constrainted_agent], self.heuristics[constrainted_agent], constrainted_agent, child['constraints']) path = eliminate_duplicates(path) if path is not None: child['paths'][constrainted_agent] = path child['collisions'] = detect_collisions(child['paths']) child['cost'] = get_sum_of_cost(child['paths']) else: Add = False if Add: self.push_node(child) raise BaseException('No solutions')
def find_solution(self): """ Finds paths for all agents from their start locations to their goal locations.""" start_time = timer.time() result = [] constraints = [] # for testing Task 1.2 Handling Vertex Constraints # constraints.append({'agent': 0, 'loc': [(1, 5)], 'timestep': 4}) # for testing Task 1.3 Handling Edge Constraints # constraints.append({'agent': 1, 'loc': [(1, 2), (1, 3)], 'timestep': 1}) # for testing Task 1.4 Handling Goal Constraints # constraints.append({'agent': 0, 'loc': [(1, 5)], 'timestep': 10}) # for testing Task 1.5 Designing Constraints # constraints = [ # {'agent': 1, 'loc': [(1, 1)], 'timestep': 2}, # {'agent': 1, 'loc': [(1, 2)], 'timestep': 2}, # {'agent': 1, 'loc': [(1, 3)], 'timestep': 2}, # {'agent': 1, 'loc': [(1, 4)], 'timestep': 2} # ] for i in range(self.num_of_agents): # Find path for each agent path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, constraints) if path is None: raise BaseException('No solutions') result.append(path) ############################## # Task 2: Add constraints here # Useful variables: # * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)] # * self.num_of_agents has the number of total agents # * constraints: array of constraints to consider for future A* searches # iterate over the path of the current agent for path_idx in range(len(path)): for agent in range(i + 1, self.num_of_agents): # add vertex constraints for all future agents constraints.append({ 'agent': agent, 'loc': [path[path_idx]], 'timestep': path_idx }) # add edge constraints for all future agents if path_idx > 0: constraints.append({ 'agent': agent, 'loc': [path[path_idx], path[path_idx - 1]], 'timestep': path_idx }) ############################## self.CPU_time = timer.time() - start_time print("\n Found a solution! \n") print("CPU time (s): {:.2f}".format(self.CPU_time)) print("Sum of costs: {}".format(get_sum_of_cost(result))) print(result) return result
def find_solution(self, disjoint=True): """ 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']) 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 this node has no collision, return solution if len(P['collisions']) == 0: self.print_results(P) return P['paths'] # else convert the first collision to a list of constraints collision = P['collisions'][0] # constraints = standard_splitting(collision) constraints = disjoint_splitting(collision) for constraint in constraints: Q = { 'cost': 0, 'constraints': P['constraints'].copy(), 'paths': [], 'collisions': [] } # shallow copy constraints of P and add the new constraint Q['constraints'].append(constraint) # shallow copy paths of P Q['paths'] = P['paths'].copy() agent_idx = constraint['agent'] path = a_star(self.my_map, self.starts[agent_idx], self.goals[agent_idx], self.heuristics[agent_idx], agent_idx, Q['constraints']) if path is not None: Q['paths'][agent_idx] = path flag = False if constraint['positive']: violate_agents = paths_violate_constraint( constraint, Q['paths']) for violate_agent_idx in violate_agents: Q['constraints'].append({ 'agent': violate_agent_idx, 'loc': constraint['loc'], 'timestep': constraint['timestep'], 'positive': False }) temp_path = a_star( self.my_map, self.starts[violate_agent_idx], self.goals[violate_agent_idx], self.heuristics[violate_agent_idx], violate_agent_idx, Q['constraints']) if temp_path is not None: Q['paths'][violate_agent_idx] = temp_path else: flag = True break if not flag: Q['collisions'] = detect_collisions(Q['paths']) Q['cost'] = get_sum_of_cost(Q['paths']) self.push_node(Q) # self.print_results(root) # return root['paths'] raise BaseException('No solutions')
def find_solution( self, disjoint=True, meta_constraints=None ): # add constraint as param to support low lever search for meta agent cbs """ 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': []} if meta_constraints != None: root['constraints'] = copy.copy(meta_constraints) 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']) 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 # expanded_nodes = [] while len(self.open_list) > 0: if meta_constraints is None: if timer.time() - self.start_time > 100: raise Exception( 'timeout|' + str(self.num_of_expanded) + '|' + str(self.num_of_generated) + '|' + str(round(timer.time() - self.start_time, 2))) else: if timer.time() - self.start_time > 20: return None # print("Open list: {}".format(self.open_list)) curr = self.pop_node() # expanded_nodes.append(curr) if len(curr['collisions']) == 0: # print("Expanded nodes list: {}".format(str(expanded_nodes))) # self.print_results(curr) CPU_time = timer.time() - self.start_time return (curr['paths'], "{:.03f}".format(CPU_time), self.num_of_expanded, self.num_of_generated) else: collision = curr['collisions'][0] # print("In parent collisions, pick one collision {}".format(collision)) # constraints = standard_splitting(collision) constraints = disjoint_splitting(collision) for constraint in constraints: child_contraints = list(curr['constraints']) child_contraints.append(constraint) child_paths = list(curr['paths']) agents_need_update = [] if constraint['positive']: agents_need_update = paths_violate_constraint( constraint, child_paths) else: agents_need_update.append(constraint['agent']) keep = True for j in agents_need_update: child_paths[j] = a_star(self.my_map, self.starts[j], self.goals[j], self.heuristics[j], j, child_contraints) if child_paths[j] is None: keep = False break if keep: child = { 'cost': get_sum_of_cost(child_paths), 'constraints': child_contraints, 'paths': child_paths, 'collisions': detect_collisions(child_paths) } self.push_node(child) # print("Expanded nodes list: {}".format(str(expanded_nodes))) if meta_constraints != None: return None else: raise Exception('no solutions|' + str(self.num_of_expanded) + '|' + str(self.num_of_generated) + '|' + str(round(timer.time() - self.start_time, 2)))
def find_solution(self, disjoint=True, time_limit=60): """ Finds paths for all agents from their start locations to their goal locations disjoint - use disjoint splitting or not time_limit - maximum amount of execution time allowed """ 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), ...], ...] # conflicts - list of conflicts in paths root = { 'cost': 0, 'constraints': [], 'paths': [], 'conflicts': [], 'parent': None } 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'], root['paths']) if path is None: raise BaseException('No solutions') root['paths'].append(path) root['cost'] = get_sum_of_cost(root['paths']) root['conflicts'] = detect_conflicts(root['paths']) self.push_node(root) # Task 3.1: Testing print(root['conflicts']) # Task 3.2: Testing for conflict in root['conflicts']: print(standard_splitting(conflict)) ############################## # 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 conflict, return solution # 3. Otherwise, choose the first conflict 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: node = self.pop_node() if node['conflicts'] == []: self.print_results(node) return node['paths'] conflict = node['conflicts'][0] print( "Choose a conflict between {} and {} at location {} at timestep {}" .format(conflict['a1'], conflict['a2'], conflict['loc'], conflict['timestep'])) #new_constraints = standard_splitting(conflict) new_constraints = disjoint_splitting(conflict) for constraint in new_constraints: #i = constraint['agent'] print( "Negative constraint on agent {} at location {} at timestep {}" .format(constraint['agent'], constraint['loc'], constraint['timestep'])) constraints = list(node['constraints']) constraints.append(constraint) paths = list(node['paths']) #paths[i] = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], # i, constraints, paths) replan = [] if constraint['positive']: replan = paths_violate_constraint(constraint, paths) else: replan.append(constraint['agent']) #child_node = {'cost': get_sum_of_cost(paths), # 'constraints':constraints, # 'paths': paths, # 'conflicts': detect_conflicts(paths), # 'parent': node} #self.push_node(child_node) prune = False for i in replan: paths[i] = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, constraints, paths) if paths[i] is None: prune = True break if not prune: child_node = { 'cost': get_sum_of_cost(paths), 'constraints': constraints, 'paths': paths, 'conflicts': detect_conflicts(paths), 'parent': node } self.push_node(child_node)
def find_solution(self, disjoint=True): """ 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']) 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 # loop while open_list is not empty while len(self.open_list) > 0: # pop node from the open list node = self.pop_node() print("Expanded Nodes: " + str(self.num_of_expanded)) # check if there are any collisions if len(node['collisions']) == 0: return node['paths'] # choose the first collision collision = node['collisions'][0] # use standard_splitting to create a list of constraints # constraints = standard_splitting(collision) # use disjoint_splitting to create a list of constraints constraints = disjoint_splitting(collision) for constraint in constraints: # Q is new node -> constraints, paths, collisions, cost Q = { 'constraints': [], 'paths': [], 'collisions': [], 'cost': 0 } # was overwriting all ['constraints'] by not creating a deep copy lmao Q['constraints'] = copy.deepcopy(node['constraints']) Q['constraints'].append(constraint) Q['paths'] = copy.deepcopy(node['paths']) agent = constraint['agent'] path = a_star(self.my_map, self.starts[agent], self.goals[agent], self.heuristics[agent], agent, Q['constraints']) # print("Path: " + str(path)) if path is not None: Q['paths'][agent] = path # use paths_violate_constraint to create a new path list of the paths that violate constraint if constraint['positive'] == True: constraintPath = paths_violate_constraint( constraint, Q['paths']) # print("here") # print(constraintPath) for disjointAgent in constraintPath: constraint = { 'agent': disjointAgent, 'loc': constraint['loc'], 'timestep': constraint['timestep'] } Q['constraints'].append(constraint) disjointPath = a_star( self.my_map, self.starts[disjointAgent], self.goals[disjointAgent], self.heuristics[disjointAgent], disjointAgent, Q['constraints']) # Condition to not add constraint if no such path exists # break and push the new node Q if disjointPath is None: break # keep same Q['collisions'] = detect_collisions(Q['paths']) Q['cost'] = get_sum_of_cost(Q['paths']) self.push_node(Q) raise BaseException('No Solutions') self.print_results(root) return root['paths']
def find_solution(self, disjoint=True): """ 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']) 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 standard = False disjoint = True while len(self.open_list) > 0: new_node = self.pop_node() if len(new_node['collisions']) == 0: print("No collision") return new_node['paths'] collision = new_node['collisions'][0] if standard == True: constraints = standard_splitting(collision) if disjoint == True: constraints = disjoint_splitting(collision) for constraint in constraints: Q = { 'constraints': [], 'paths': [], 'cost': [], 'collisions': [] } for i in new_node['constraints']: Q['constraints'].append(i) Q['constraints'].append(constraint) for i in new_node['paths']: Q['paths'].append(i) agent = constraint['agent'] path = a_star(self.my_map, self.starts[agent], self.goals[agent], self.heuristics[agent], agent, Q['constraints']) if path is not None: Q['paths'][agent] = path paths_violate = list() if constraint['positive'] == True: paths_violate = paths_violate_constraint( constraint, Q['paths']) for new_agent in paths_violate: new_constraint = { 'agent': new_agent, 'loc': constraint['loc'], 'timestep': constraint['timestep'], 'positive': False } Q['constraints'].append(new_constraint) new_path = a_star(self.my_map, self.starts[new_agent], self.goals[new_agent], self.heuristics[new_agent], new_agent, Q['constraints']) if new_path is None: break Q['paths'][new_agent] = new_path Q['cost'] = get_sum_of_cost(Q['paths']) Q['collisions'] = detect_collisions(Q['paths']) self.push_node(Q) raise BaseException('No solutions')
def find_solution(self, disjoint=True): """ 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']) self.push_node(root) ############################## # 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 split_func = disjoint_splitting if disjoint else standard_splitting while self.open_list: p = self.pop_node() # print(p['paths']) # print(p['constraints']) # print(p['collisions']) if not p['collisions']: solve_time = self.print_results(p) return p['paths'], solve_time collision = p['collisions'][0] constraints = split_func(collision) # print(constraints) for constraint in constraints: q = { # 'cost': 0, 'constraints': p['constraints'] + [constraint] if constraint not in p['constraints'] else list(p['constraints']), 'paths': list(p['paths']), # 'collisions': [] } if constraint['positive']: agents = paths_violate_constraint(constraint, q['paths']) paths_exist = True for i in agents: path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, q['constraints']) if not path: paths_exist = False break q['paths'][i] = path if paths_exist: q['collisions'] = detect_collisions(q['paths']) q['cost'] = get_sum_of_cost(q['paths']) self.push_node(q) else: i = constraint['agent'] path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, q['constraints']) if path: q['paths'][i] = path q['collisions'] = detect_collisions(q['paths']) q['cost'] = get_sum_of_cost(q['paths']) self.push_node(q) raise BaseException('No solutions')
def find_solution(self): """ 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']) self.push_node(root) while len(self.open_list) > 0: next_node = self.pop_node() if len(next_node['collisions']) == 0: self.print_results(next_node) return next_node['paths'] collision = next_node['collisions'][0] constraints = standard_splitting(collision) for constraint in constraints: new_child_node = dict() new_child_node['constraints'] = [constraint] + next_node['constraints'] new_child_node['paths'] = [] + next_node['paths'] agent_in_constraint = constraint['agent'] path = a_star(self.my_map, self.starts[agent_in_constraint], self.goals[agent_in_constraint], self.heuristics[agent_in_constraint], agent_in_constraint, new_child_node['constraints']) if path is not None and len(path) > 0: new_child_node['paths'][agent_in_constraint] = [] + path new_child_node['collisions'] = detect_collisions(new_child_node['paths']) new_child_node['cost'] = get_sum_of_cost(new_child_node['paths']) self.push_node(new_child_node) self.print_results(root) return None
def find_solution(self): """ Finds paths for all agents from their start locations to their goal locations.""" start_time = timer.time() result = [] constraints = [] # Test Task 1.2, add constraint for agent #constraints.append({'agent':0, 'loc':[(1,5)], 'timestep':4}) #constraints.append({'agent':1, 'loc':[(1,2), (1,3)], 'timestep':1}) # Test Task 1.5 """ constraints.append({'agent':1, 'loc':[(1,3), (1,2)], 'timestep':2}) constraints.append({'agent':1, 'loc':[(1,3), (1,3)], 'timestep':2}) constraints.append({'agent':1, 'loc':[(1,3), (1,4)], 'timestep':2}) constraints.append({'agent':1, 'loc':[(1,2)], 'timestep':1}) """ for i in range(self.num_of_agents): # Find path for each agent path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i], i, constraints) if path is None: print(result) raise BaseException('No solutions') result.append(path) ############################## # Task 2: Add constraints here # Useful variables: # * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)] # * self.num_of_agents has the number of total agents # * constraints: array of constraints to consider for future A* searches # add constraints for all after agents current_path = result[i] # add vertex constraints timestep = 0 for position in current_path: # each agent need to add constraint relate to current path for each_agent in range(i + 1, self.num_of_agents): need_to_append = { 'agent': each_agent, 'loc': [position], 'timestep': timestep, 'positive': 0 } if need_to_append not in constraints: constraints.append(need_to_append) timestep += 1 # add edge constraints timestep = 1 for j in range(len(current_path) - 1): for each_agent in range(i + 1, self.num_of_agents): need_to_append = { 'agent': each_agent, 'loc': [current_path[j + 1], current_path[j]], 'timestep': timestep, 'positive': 0 } if need_to_append not in constraints: constraints.append(need_to_append) timestep += 1 # add additional constraints # -1 represent all future time for each_agent in range(i + 1, self.num_of_agents): need_to_append = { 'agent': each_agent, 'loc': [current_path[len(current_path) - 1]], 'timestep': (-1, len(current_path) - 1), 'positive': 0 } if need_to_append not in constraints: constraints.append(need_to_append) ############################## self.CPU_time = timer.time() - start_time print("\n Found a solution! \n") print("CPU time (s): {:.2f}".format(self.CPU_time)) print("Sum of costs: {}".format(get_sum_of_cost(result))) print(result) return result