How do I stop 2 robots from colliding? (Python, A* search) -

i new python , wondering if me figure out how solve problem have code. supposed use a* search robots find goals.

my 2 robots r1 , r2 appear on grid , go respective goals, g1 , g2, collide , not @ clear in graph. have created 3 different maps them. if can fix 1 of them, fixed.

can tell me doing wrong here? sorry posting such huge chunk of code.

implementation.py

class simplegraph:     def __init__(self):         self.edges = {}      def neighbors(self, id):         return self.edges[id]  example_graph = simplegraph() example_graph.edges = {     'a': ['b'],     'b': ['a', 'c', 'd'],     'c': ['a'],     'd': ['e', 'a'],     'e': ['b'] }  import collections  class queue:     def __init__(self):         self.elements = collections.deque()      def empty(self):         return len(self.elements) == 0      def put(self, x):         self.elements.append(x)      def get(self):         return self.elements.popleft()  # utility functions dealing square grids def from_id_width(id, width):     return (id % width, id // width)  def draw_tile(graph, id, style, width):     r = "-"     if 'number' in style , id in style['number']: r = "%d" % style['number'][id]     if 'path' in style , id in style['path']: r = "@"     if 'number2' in style , id in style['number2']: r = "%d" % style['number2'][id]     if 'path2' in style , id in style['path2']: r = "*"     if 'start' in style , id == style['start']: r = "r1"     if 'goal' in style , id == style['goal']: r = "g1"     if 'start2' in style , id == style['start2']: r = "r2"     if 'goal2' in style , id == style['goal2']: r = "g2"     if id in graph.walls: r = "#" * (width - 1)     return r  def draw_grid(graph, width=2, **style):     y in range(graph.height):         x in range(graph.width):             print("%%-%ds" % width % draw_tile(graph, (x, y), style, width), end="")         print()  # data main article diagram1_walls = [from_id_width(id, width=30) id in [21,22,51,52,81,82,93,94,111,112,123,124,133,134,141,142,153,154,                                                          163,164,171,172,173,174,175,183,184,193,194,201,202,203,204,205,                                                          213,214,223,224,243,244,253,254,273,274,283,284,303,304,313,314,                                                          333,334,343,344,373,374,403,404,433,434]]  class squaregrid:     def __init__(self, width, height):         self.width = width         self.height = height         self.walls = []      def in_bounds(self, id):         (x, y) = id         return 0 <= x < self.width , 0 <= y < self.height      def passable(self, id):         return id not in self.walls      def neighbors(self, id):         (x, y) = id         results = [(x+1, y), (x, y-1), (x-1, y), (x, y+1)]         if (x + y) % 2 == 0: results.reverse() # aesthetics         results = filter(self.in_bounds, results)         results = filter(self.passable, results)         return results  class gridwithweights(squaregrid):     def __init__(self, width, height):         super().__init__(width, height)         self.weights = {}      def cost(self, from_node, to_node):         return self.weights.get(to_node, 1) #problem 1 diagram4 = gridwithweights(8,5) #problem 3 diagram2 = gridwithweights(8,5) #problem 2 diagram3 = gridwithweights(8,5)  #walls placed diagram4.walls = [(0, 2), (3, 1), (4, 1), (3, 3), (2, 4), (3, 4), (5, 3), (7, 3)] diagram2.walls = [(0, 1), (2, 1), (2, 2), (3, 3), (4, 1), (4, 3), (5, 3), (6, 3)] diagram3.walls = [(0, 2), (3, 1), (4, 1), (3, 3), (2, 4), (3, 4), (5, 3), (7, 3)]  #trap set diagram4.weights = {loc: 5 loc in [(3, 2)]} diagram2.weights = {loc: 5 loc in [(3, 2)]} diagram3.weights = {loc: 5 loc in [(3, 2)]}  import heapq  class priorityqueue:     def __init__(self):         self.elements = []      def empty(self):         return len(self.elements) == 0      def put(self, item, priority):         heapq.heappush(self.elements, (priority, item))      def get(self):         return heapq.heappop(self.elements)[1]  def dijkstra_search(graph, start, goal):     frontier = priorityqueue()     frontier.put(start, 0)     came_from = {}     cost_so_far = {}     came_from[start] = none     cost_so_far[start] = 0      while not frontier.empty():         current = frontier.get()          if current == goal:             break          next in graph.neighbors(current):             new_cost = cost_so_far[current] + graph.cost(current, next)             if next not in cost_so_far or new_cost < cost_so_far[next]:                 cost_so_far[next] = new_cost                 priority = new_cost                 frontier.put(next, priority)                 came_from[next] = current      return came_from, cost_so_far  def reconstruct_path(came_from, start, goal):     current = goal     path = [current]     while current != start:         current = came_from[current]         path.append(current)     path.append(start) # optional     path.reverse() # optional     return path def reconstruct_path2(came_from2, start2, goal2):     current = goal2     path2 = [current]     while current != start2:         current = came_from2[current]         path2.append(current)     path2.append(start2) # optional     path2.reverse() # optional     return path2  def heuristic(a, b):     (x1, y1) =     (x2, y2) = b     return abs(x1 - x2) + abs(y1 - y2)    # manhattan distance     #return math.sqrt((x1 - x2)*(x1 - x2) + (y1 - y2)*(y1 - y2))  # euclidian distance     #return max(abs(x1-x2), abs(y1-y2)) # chebyshev distance  def a_star_search(graph, start, goal):     frontier = priorityqueue()     frontier.put(start, 0)     came_from = {}     cost_so_far = {}     came_from[start] = none     cost_so_far[start] = 0      while not frontier.empty():         current = frontier.get()          if current == goal:             break          next in graph.neighbors(current):             new_cost = cost_so_far[current] + graph.cost(current, next)             if next not in cost_so_far or new_cost < cost_so_far[next]:                 cost_so_far[next] = new_cost                 priority = new_cost + heuristic(goal, next)                 frontier.put(next, priority)                 came_from[next] = current      return came_from, cost_so_far  def a_star_search2(graph, start2, goal2):     frontier = priorityqueue()     frontier.put(start2, 0)     came_from2 = {}     cost_so_far2 = {}     came_from2[start2] = none     cost_so_far2[start2] = 0      while not frontier.empty():         current = frontier.get()          if current == goal2:             break          next in graph.neighbors(current):             new_cost = cost_so_far2[current] + graph.cost(current, next)             if next not in cost_so_far2 or new_cost < cost_so_far2[next]:                 cost_so_far2[next] = new_cost                 priority = new_cost + heuristic(goal2, next)                 frontier.put(next, priority)                 came_from2[next] = current      return came_from2, cost_so_far2 

and here main file:

a_star.py

from implementation import * #robots start , finish points problem 1 & 3. r1s = (0, 0) r1f = (5, 2) r2s = (6, 4) r2f = (2, 0) #robots start , finish points problem 2. r1s1 = (0, 4) r1f1 = (5, 2) r2s1 = (4, 3) r2f1 = (2, 0)  came_from, cost_so_far = a_star_search(diagram4, r1s, r1f) came_from2, cost_so_far2 = a_star_search2(diagram4, r2s, r2f) print() print(" problem 1 grid path shown") draw_grid(diagram4, width=3, path=reconstruct_path (came_from, start=r1s, goal=r1f), start=r1s, goal=r1f,                              path2=reconstruct_path2 (came_from2, start2=r2s, goal2=r2f), start2=r2s, goal2=r2f) print() print("problem 1 grid weights shown") draw_grid(diagram4, width=3, number=cost_so_far, start=r1s, goal=r1f, number2=cost_so_far2, start2=r2s, goal2=r2f) print() print ("robot 1 start point: %s" %str(r1s)) print ("robot 1 finish point: %s" %str(r1f)) print ("robot 2 start point: %s" %str(r2s)) print ("robot 2 finish point: %s" %str(r2f))  print ("......................")  came_from, cost_so_far = a_star_search(diagram3, r1s1, r1f1) came_from2, cost_so_far2 = a_star_search2(diagram3, r2s1, r2f1) print() print(" problem 2 grid path shown") draw_grid(diagram3, width=3, path=reconstruct_path (came_from, start=r1s1, goal=r1f1), start=r1s1, goal=r1f1,                              path2=reconstruct_path2 (came_from2, start2=r2s1, goal2=r2f1), start2=r2s1, goal2=r2f1) print("problem 2 grid weights shown") draw_grid(diagram3, width=3, number=cost_so_far, start=r1s1, goal=r1f1, number2=cost_so_far2, start2=r2s1, goal2=r2f1) print() print ("robot 1 start point: %s" %str(r1s1)) print ("robot 1 finish point: %s" %str(r1f1)) print ("robot 2 start point: %s" %str(r2s1)) print ("robot 2 finish point: %s" %str(r2f1))  print ("......................")  came_from, cost_so_far = a_star_search(diagram2, r1s, r1f) came_from2, cost_so_far2 = a_star_search2(diagram2, r2s, r2f) print() print(" problem 3 grid path shown") draw_grid(diagram2, width=3, path=reconstruct_path (came_from, start=r1s, goal=r1f), start=r1s, goal=r1f,                              path2=reconstruct_path2 (came_from2, start2=r2s, goal2=r2f), start2=r2s, goal2=r2f) print() print("problem 3 grid weights shown") draw_grid(diagram2, width=3, number=cost_so_far, start=r1s, goal=r1f, number2=cost_so_far2, start2=r2s, goal2=r2f) print() print ("robot 1 start point: %s" %str(r1s)) print ("robot 1 finish point: %s" %str(r1f)) print ("robot 2 start point: %s" %str(r2s)) print ("robot 2 finish point: %s" %str(r2f)) 

here output:

 problem 1 grid path shown r1 @  g2 *  *  *  -  -   -  -  -  ## ## *  -  -   ## -  -  -  -  g1 *  -   -  -  -  ## -  ## *  ##  -  -  ## ## -  -  r2 -    problem 1 grid weights shown r1 1  g2 7  6  5  6  -   1  2  3  ## ## 4  3  -   ## 3  4  9  4  g1 2  3   -  4  5  ## 3  ## 1  ##  -  -  ## ## 2  1  r2 1    robot 1 start point: (0, 0) robot 1 finish point: (5, 2) robot 2 start point: (6, 4) robot 2 finish point: (2, 0) ......................   problem 2 grid path shown -  -  g2 *  *  *  -  -   -  -  -  ## ## *  -  -   ## @  @  @  *  g1 -  -   @  @  -  ## r2 ## -  ##  r1 -  ## ## -  -  -  -   problem 2 grid weights shown 6  5  g2 6  5  4  5  -   5  4  5  ## ## 3  4  -   ## 3  4  6  1  g1 3  -   1  2  3  ## r2 ## -  ##  r1 1  ## ## 1  2  -  -    robot 1 start point: (0, 4) robot 1 finish point: (5, 2) robot 2 start point: (4, 3) robot 2 finish point: (2, 0) ......................   problem 3 grid path shown r1 *  g2 @  @  @  -  -   ## *  ## -  ## @  -  -   -  *  ## -  -  g1 -  -   -  *  *  ## ## ## ## -   -  -  *  *  *  *  r2 -    problem 3 grid weights shown r1 9  g2 3  4  5  6  -   ## 8  ## 4  ## 6  7  -   8  7  ## 9  -  g1 -  -   7  6  5  ## ## ## ## -   6  5  4  3  2  1  r2 1    robot 1 start point: (0, 0) robot 1 finish point: (5, 2) robot 2 start point: (6, 4) robot 2 finish point: (2, 0)