Search for a path on a grid avoiding obstacles. >>> grid = [[0, 1, 0, 0, 0, 0], ... [0, 1, 0, 0, 0, 0], ... [0, 1, 0, 0, 0, 0], ... [0, 1, 0, 0, 1, 0], ... [0, 0, 0, 0, 1, 0]] >>> init = [0, 0] >>> goal = [len(grid) - 1, len(grid[0]) -
(
grid: list[list[int]],
init: list[int],
goal: list[int],
cost: int,
heuristic: list[list[int]],
)
| 10 | |
| 11 | # function to search the path |
| 12 | def search( |
| 13 | grid: list[list[int]], |
| 14 | init: list[int], |
| 15 | goal: list[int], |
| 16 | cost: int, |
| 17 | heuristic: list[list[int]], |
| 18 | ) -> tuple[list[list[int]], list[list[int]]]: |
| 19 | """ |
| 20 | Search for a path on a grid avoiding obstacles. |
| 21 | >>> grid = [[0, 1, 0, 0, 0, 0], |
| 22 | ... [0, 1, 0, 0, 0, 0], |
| 23 | ... [0, 1, 0, 0, 0, 0], |
| 24 | ... [0, 1, 0, 0, 1, 0], |
| 25 | ... [0, 0, 0, 0, 1, 0]] |
| 26 | >>> init = [0, 0] |
| 27 | >>> goal = [len(grid) - 1, len(grid[0]) - 1] |
| 28 | >>> cost = 1 |
| 29 | >>> heuristic = [[0] * len(grid[0]) for _ in range(len(grid))] |
| 30 | >>> heuristic = [[0 for row in range(len(grid[0]))] for col in range(len(grid))] |
| 31 | >>> for i in range(len(grid)): |
| 32 | ... for j in range(len(grid[0])): |
| 33 | ... heuristic[i][j] = abs(i - goal[0]) + abs(j - goal[1]) |
| 34 | ... if grid[i][j] == 1: |
| 35 | ... heuristic[i][j] = 99 |
| 36 | >>> path, action = search(grid, init, goal, cost, heuristic) |
| 37 | >>> path # doctest: +NORMALIZE_WHITESPACE |
| 38 | [[0, 0], [1, 0], [2, 0], [3, 0], [4, 0], [4, 1], [4, 2], [4, 3], [3, 3], |
| 39 | [2, 3], [2, 4], [2, 5], [3, 5], [4, 5]] |
| 40 | >>> action # doctest: +NORMALIZE_WHITESPACE |
| 41 | [[0, 0, 0, 0, 0, 0], [2, 0, 0, 0, 0, 0], [2, 0, 0, 0, 3, 3], |
| 42 | [2, 0, 0, 0, 0, 2], [2, 3, 3, 3, 0, 2]] |
| 43 | """ |
| 44 | closed = [ |
| 45 | [0 for col in range(len(grid[0]))] for row in range(len(grid)) |
| 46 | ] # the reference grid |
| 47 | closed[init[0]][init[1]] = 1 |
| 48 | action = [ |
| 49 | [0 for col in range(len(grid[0]))] for row in range(len(grid)) |
| 50 | ] # the action grid |
| 51 | |
| 52 | x = init[0] |
| 53 | y = init[1] |
| 54 | g = 0 |
| 55 | f = g + heuristic[x][y] # cost from starting cell to destination cell |
| 56 | cell = [[f, g, x, y]] |
| 57 | |
| 58 | found = False # flag that is set when search is complete |
| 59 | resign = False # flag set if we can't find expand |
| 60 | |
| 61 | while not found and not resign: |
| 62 | if len(cell) == 0: |
| 63 | raise ValueError("Algorithm is unable to find solution") |
| 64 | else: # to choose the least costliest action so as to move closer to the goal |
| 65 | cell.sort() |
| 66 | cell.reverse() |
| 67 | next_cell = cell.pop() |
| 68 | x = next_cell[2] |
| 69 | y = next_cell[3] |