def __init__(self, world, start, goal): self.world = world self.start = start self.goal = goal self.h = RRAstar(world, start, goal) self.path = [] self.conflicts = SortedListWithKey(key=lambda c: c.time) self.priorities = [] self.stable_prio = [] self._actual_prio = set() self.old_conflicts = set()
def __init__(self, world, start, goal, weights=Weights(1, 3), caching=True): self.world = world self.start = start self.goal = goal self.h = RRAstar(world, start, goal) self.path = [] self.path_cache = {} self.conflicts = SortedListWithKey(key=lambda conflict: conflict.time) self.resolved_conflicts = [] self.current_conflict = None self.higher_prio = frozenset() self.weights = weights self.caching = caching
def __init__(self, starts, goals, w): self.starts = starts self.goals = goals self.paths = [] self.heur = {} for i in range(len(starts)): self.heur[goals[i]] = RRAstar(w, starts[i], goals[i])
def astar(world, start, goal, window): closed_set = set() open_set = [] came_from = {} g = {start: 0} heapq.heappush(open_set, (0, 0, start)) h = RRAstar(world, start, goal) while open_set: f, time_step, cur = heapq.heappop(open_set) print('evaluating', f, time_step, cur) pprint(open_set) if time_step > window: return reverse_path((time_step, cur), came_from) closed_set.add(cur) for successor in (world.neighbours(cur) + [cur]): if successor in closed_set and cur != successor: continue if time_step == window: score = g[cur] + h.dist(successor) elif cur == goal and successor == goal: score = g[cur] else: score = g[cur] + 1 # Ignore a path if it is a longer variant if successor in g and score >= g[successor] and successor != cur: continue came_from[time_step + 1, successor] = (time_step, cur) g[successor] = score if time_step == window: heapq.heappush(open_set, (score, time_step + 1, successor)) else: heapq.heappush( open_set, (score + h.dist(successor), time_step + 1, successor)) raise util.NoPathsFoundException()
def standard_algorithm(agents, world, starts, goals, start_time=None, max_time=None): starts = tuple(starts) goals = tuple(goals) closed_set = set() open_set = [] came_from = {} g = {starts: 0} count = 0 heapq.heappush(open_set, (0, count, starts)) # Set up heuristics heur = {} for i in range(agents): heur[goals[i]] = RRAstar(world, starts[i], goals[i]) # Display predicted cost pred_cost = heur_dist(heur, goals, starts) print(f'predicted cost: {pred_cost}') while open_set: time = timeit.default_timer() if start_time != None and (time - start_time) > max_time: raise TimeExceeded() f, _, current = heapq.heappop(open_set) #print(f'f: {f:4}, current: {current}') if current == goals: return reconstruct_path(came_from, current) closed_set.add(current) for cost, neighbour in successor_states(world, current, goals, start_time, max_time): if neighbour in closed_set: continue score = g[current] + cost # We found a longer path, ignore it if neighbour in g and score >= g[neighbour]: continue came_from[neighbour] = current g[neighbour] = score count += 1 heapq.heappush(open_set, (score + heur_dist(heur, goals, current), count, neighbour)) return None
def __init__(self, world, start, goal): self.world = world self.start = start self.goal = goal self.h = RRAstar(world, start, goal) self.path = []
class Agent: def __init__(self, world, start, goal): self.world = world self.start = start self.goal = goal self.h = RRAstar(world, start, goal) self.path = [] def plan(self, global_plan, start_time, max_time): self.path = self._astar(global_plan, start_time, max_time) def _astar(self, global_plan, start_time, max_time): closed_set = set() open_set = [] came_from = {} g = {self.start: 0} heapq.heappush(open_set, (0, 0, self.start)) while open_set: time = timeit.default_timer() if start_time != None and (time - start_time) > max_time: raise TimeExceeded() _, time_step, cur = heapq.heappop(open_set) if cur == self.goal: return self._reverse_path((time_step, cur), came_from) closed_set.add(cur) for successor in self._successors(cur, time_step, global_plan, start_time, max_time): # Skip successor in closed list if successor in closed_set and successor != cur: continue score = g[cur] + 1 # Ignore path if it is a longer variant if successor in g and score >= g[successor] \ and successor != cur: continue came_from[time_step + 1, successor] = (time_step, cur) g[successor] = score heapq.heappush(open_set, (score + self.h.dist(successor), time_step + 1, successor)) raise util.NoPathsFoundException() def _successors(self, pos, time, global_plan, start_time, max_time): successors = [pos] + self.world.neighbours(pos) filtered = [] for successor in successors: for other_path in global_plan: cur_time = timeit.default_timer() if start_time != None and (cur_time - start_time) > max_time: raise TimeExceeded() if len(other_path[time:]) >= 2: if util.moves_conflict(other_path[time:time + 2], (pos, successor)): break elif util.moves_conflict((other_path[-1], other_path[-1]), (pos, successor)): break else: filtered.append(successor) return filtered def _reverse_path(self, state, came_from): path = [state[1]] while state in came_from: state = came_from[state] path.append(state[1]) path.reverse() return path def __repr__(self): return f'{self.start}-{self.goal}'
class Agent: def __init__(self, world, start, goal, weights=Weights(1, 3), caching=True): self.world = world self.start = start self.goal = goal self.h = RRAstar(world, start, goal) self.path = [] self.path_cache = {} self.conflicts = SortedListWithKey(key=lambda conflict: conflict.time) self.resolved_conflicts = [] self.current_conflict = None self.higher_prio = frozenset() self.weights = weights self.caching = caching def plan(self, start_time, max_time): #print(f'Planning for agent {self}') self.old_path = self.path self.construct_higher_prio() # Check if there is a path in the cache for this prio set up if self.caching and self.higher_prio in self.path_cache: # Check if the cached path conflicts with those of higher prio paths = [agent.path for agent in self.higher_prio] conflicts = util.paths_conflict(paths) if not conflicts: #print('Using cached path') self.path = self.path_cache[self.higher_prio] return self.path = self._astar(start_time, max_time) # Update the cache self.path_cache[self.higher_prio] = self.path def construct_higher_prio(self): prio = [] # Construct priorities from the conflict solutions for conflict in self.resolved_conflicts: for i in range(conflict.solution['level'] + 1): if conflict.solution[i] == self: continue prio.append(conflict.solution[i]) # Update with the current proposed solution if self.current_conflict != None: for i in range(self.current_conflict.proposal['level'] + 1): if self.current_conflict.proposal[i] == self: continue prio.append(self.current_conflict.proposal[i]) self.higher_prio = frozenset(prio) #print(f' Agent {self} final prio: {self.higher_prio}') def propose(self, conflict): # If there are no proposals yet, propose to go first if len(conflict.proposals) == 0: proposal = {'score': None, 'level': 0, 0: self} return proposal def resolved_conflict(self, conflict): self.resolved_conflicts.append(conflict) def evaluate(self, conflicts): # Current conflict solved if self.current_conflict in conflicts.values(): #print('Conflict not solved') raise ConflictNotSolved() score = 0 # Change in path length score += (len(self.old_path) - len(self.path)) * self.weights.path_len # Change in conflicts filtered = list(filter(lambda c: self in c.agents, conflicts.values())) #print(f'{self} {len(self.conflicts)} {len(filtered)}') score += (len(self.conflicts) - len(filtered)) * \ self.weights.conflict_count #print(f'Agent score {self}: {score}') return score def _astar(self, start_time, max_time): closed_set = set() open_set = [] came_from = {} g = {self.start: 0} heapq.heappush(open_set, (0, 0, self.start)) while open_set: time = timeit.default_timer() if start_time != None and (time - start_time) > max_time: raise TimeExceeded() _, time_step, cur = heapq.heappop(open_set) if cur == self.goal: return self._reverse_path((time_step, cur), came_from) closed_set.add(cur) for successor in self._successors(cur, time_step, start_time, max_time): # Skip successor in closed list if successor in closed_set and successor != cur: continue score = g[cur] + 1 # Ignore a path if it is a longer variant if successor in g and score >= g[successor] \ and successor != cur: continue came_from[time_step + 1, successor] = (time_step, cur) g[successor] = score heapq.heappush(open_set, (score + self.h.dist(successor), time_step + 1, successor)) raise util.NoPathsFoundException() def _successors(self, pos, time, start_time, max_time): successors = [pos] + self.world.neighbours(pos) filtered = [] for successor in successors: for other_agent in self.higher_prio: cur_time = timeit.default_timer() if start_time != None and (cur_time - start_time) > max_time: raise TimeExceeded() path = other_agent.path if len(path[time:]) >= 2: if util.moves_conflict(path[time:time + 2], (pos, successor)): break else: if util.moves_conflict((path[-1], path[-1]), (pos, successor)): break else: filtered.append(successor) return filtered def _reverse_path(self, state, came_from): path = [state[1]] while state in came_from: state = came_from[state] path.append(state[1]) path.reverse() return path def __repr__(self): return f'{self.start}-{self.goal}'
def od(agents, w, starts, goals): start_state = tuple(State(s) for s in starts) count = 0 closed_set = set() open_set = [] came_from = {} g = {start_state: 0} heapq.heappush(open_set, (0, count, 0, start_state)) # Set up heuristics heur = {} for i in range(agents): heur[goals[i]] = RRAstar(w, starts[i], goals[i]) # Display predicted cost pred_cost = heur_dist(heur, goals, starts) print('predicted cost:', pred_cost) while open_set: f, _, agent, current = heapq.heappop(open_set) #print(f'f: {f:4}, agent: {agent}, current: {current}') # Check if we've reached the goal if agent == 0: state = tuple(s.pos for s in current) if state == goals: print('open set size: ', len(open_set)) print('closed set size:', len(closed_set)) print('final cost: ', f) return reverse_paths(state, came_from) # If we haven't then add the standard state to the closed set closed_set.add(current) # Add all possible actions successors = next_states(current, w, agent) for new_state in successors: count += 1 # If the agent is in its goal position and the action is wait # then there should be no cost if new_state[agent].action == Actions.wait and \ new_state[agent].pos == goals[agent]: score = g[current] else: score = g[current] + 1 # Create a standard state if necessary if agent == agents - 1: new_state = tuple(State(s.new_pos()) for s in new_state) # Don't add it if it already exists if new_state in closed_set: continue if new_state in g and score >= g[new_state]: continue # Check if the standard state is already in the open set if new_state not in g: # No duplicate found, add this one simple_state = tuple(s.new_pos() for s in new_state) h = heur_dist(heur, goals, simple_state) heapq.heappush(open_set, (score + h, count, 0, new_state)) came_from[simple_state] = tuple(s.pos for s in current) # Create intermediate state else: # If we found a longer path, ignore it if new_state in g and score >= g[new_state]: continue h = 0 for i in range(agent + 1): h += heur[goals[i]].dist(new_state[i].new_pos()) for i in range(agent + 1, agents): h += heur[goals[i]].dist(new_state[i].pos) heapq.heappush(open_set, (score + h, count, agent + 1, new_state)) g[new_state] = score return None
class Agent: def __init__(self, world, start, goal): self.world = world self.start = start self.goal = goal self.h = RRAstar(world, start, goal) self.path = [] self.conflicts = SortedListWithKey(key=lambda c: c.time) self.priorities = [] self.stable_prio = [] self._actual_prio = set() self.old_conflicts = set() def plan(self, start_time=None, max_time=None): self.path = self._astar(start_time, max_time) def _astar(self, start_time, max_time): self._actual_prio = set(self.stable_prio + self.priorities) closed_set = set() open_set = [] came_from = {} g = {self.start: 0} heapq.heappush(open_set, (0, 0, self.start)) while open_set: time = timeit.default_timer() if start_time != None and (time - start_time) > max_time: raise TimeExceeded() _, time_step, cur = heapq.heappop(open_set) if cur == self.goal: return self._reverse_path((time_step, cur), came_from) closed_set.add(cur) for successor in self._successors(cur, time_step, start_time, max_time): # Skip successor in closed list if successor in closed_set and successor != cur: continue score = g[cur] + 1 # Ignore a path if it is longer if successor in g and score >= g[successor] \ and successor != cur: continue came_from[time_step + 1, successor] = (time_step, cur) g[successor] = score heapq.heappush( open_set, (score + self.h.dist(successor), time_step + 1, successor)) raise util.NoPathsFoundException() def _successors(self, pos, time, start_time=None, max_time=None): successors = [pos] + self.world.neighbours(pos) filtered = [] for successor in successors: for other_agent in self._actual_prio: cur_time = timeit.default_timer() if start_time != None and (cur_time - start_time) > max_time: raise TimeExceeded() path = other_agent.path if len(path[time:]) >= 2: paths = [path[time:time + 2], (pos, successor)] else: paths = [[path[-1]], (pos, successor)] if util.paths_conflict(paths): break else: filtered.append(successor) return filtered def _reverse_path(self, state, came_from): path = [state[1]] while state in came_from: state = came_from[state] path.append(state[1]) path.reverse() return path def __repr__(self): return f'{self.start}-{self.goal}'