1
- grid = [
2
- [0 , 1 , 0 , 0 , 0 , 0 ],
3
- [0 , 1 , 0 , 0 , 0 , 0 ], # 0 are free path whereas 1's are obstacles
4
- [0 , 1 , 0 , 0 , 0 , 0 ],
5
- [0 , 1 , 0 , 0 , 1 , 0 ],
6
- [0 , 0 , 0 , 0 , 1 , 0 ],
7
- ]
8
-
9
- """
10
- heuristic = [[9, 8, 7, 6, 5, 4],
11
- [8, 7, 6, 5, 4, 3],
12
- [7, 6, 5, 4, 3, 2],
13
- [6, 5, 4, 3, 2, 1],
14
- [5, 4, 3, 2, 1, 0]]"""
15
-
16
- init = [0 , 0 ]
17
- goal = [len (grid ) - 1 , len (grid [0 ]) - 1 ] # all coordinates are given in format [y,x]
18
- cost = 1
19
-
20
- # the cost map which pushes the path closer to the goal
21
- heuristic = [[0 for row in range (len (grid [0 ]))] for col in range (len (grid ))]
22
- for i in range (len (grid )):
23
- for j in range (len (grid [0 ])):
24
- heuristic [i ][j ] = abs (i - goal [0 ]) + abs (j - goal [1 ])
25
- if grid [i ][j ] == 1 :
26
- heuristic [i ][j ] = 99 # added extra penalty in the heuristic map
27
-
1
+ from __future__ import annotations
28
2
29
- # the actions we can take
30
- delta = [[- 1 , 0 ], [0 , - 1 ], [1 , 0 ], [0 , 1 ]] # go up # go left # go down # go right
3
+ DIRECTIONS = [
4
+ [- 1 , 0 ], # left
5
+ [0 , - 1 ], # down
6
+ [1 , 0 ], # right
7
+ [0 , 1 ], # up
8
+ ]
31
9
32
10
33
11
# function to search the path
34
- def search (grid , init , goal , cost , heuristic ):
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 ]]]:
35
19
36
20
closed = [
37
21
[0 for col in range (len (grid [0 ]))] for row in range (len (grid ))
@@ -52,7 +36,7 @@ def search(grid, init, goal, cost, heuristic):
52
36
53
37
while not found and not resign :
54
38
if len (cell ) == 0 :
55
- return "FAIL"
39
+ raise ValueError ( "Algorithm is unable to find solution" )
56
40
else : # to choose the least costliest action so as to move closer to the goal
57
41
cell .sort ()
58
42
cell .reverse ()
@@ -64,9 +48,9 @@ def search(grid, init, goal, cost, heuristic):
64
48
if x == goal [0 ] and y == goal [1 ]:
65
49
found = True
66
50
else :
67
- for i in range (len (delta )): # to try out different valid actions
68
- x2 = x + delta [i ][0 ]
69
- y2 = y + delta [i ][1 ]
51
+ for i in range (len (DIRECTIONS )): # to try out different valid actions
52
+ x2 = x + DIRECTIONS [i ][0 ]
53
+ y2 = y + DIRECTIONS [i ][1 ]
70
54
if x2 >= 0 and x2 < len (grid ) and y2 >= 0 and y2 < len (grid [0 ]):
71
55
if closed [x2 ][y2 ] == 0 and grid [x2 ][y2 ] == 0 :
72
56
g2 = g + cost
@@ -79,22 +63,46 @@ def search(grid, init, goal, cost, heuristic):
79
63
y = goal [1 ]
80
64
invpath .append ([x , y ]) # we get the reverse path from here
81
65
while x != init [0 ] or y != init [1 ]:
82
- x2 = x - delta [action [x ][y ]][0 ]
83
- y2 = y - delta [action [x ][y ]][1 ]
66
+ x2 = x - DIRECTIONS [action [x ][y ]][0 ]
67
+ y2 = y - DIRECTIONS [action [x ][y ]][1 ]
84
68
x = x2
85
69
y = y2
86
70
invpath .append ([x , y ])
87
71
88
72
path = []
89
73
for i in range (len (invpath )):
90
74
path .append (invpath [len (invpath ) - 1 - i ])
75
+ return path , action
76
+
77
+
78
+ if __name__ == "__main__" :
79
+ grid = [
80
+ [0 , 1 , 0 , 0 , 0 , 0 ],
81
+ [0 , 1 , 0 , 0 , 0 , 0 ], # 0 are free path whereas 1's are obstacles
82
+ [0 , 1 , 0 , 0 , 0 , 0 ],
83
+ [0 , 1 , 0 , 0 , 1 , 0 ],
84
+ [0 , 0 , 0 , 0 , 1 , 0 ],
85
+ ]
86
+
87
+ init = [0 , 0 ]
88
+ # all coordinates are given in format [y,x]
89
+ goal = [len (grid ) - 1 , len (grid [0 ]) - 1 ]
90
+ cost = 1
91
+
92
+ # the cost map which pushes the path closer to the goal
93
+ heuristic = [[0 for row in range (len (grid [0 ]))] for col in range (len (grid ))]
94
+ for i in range (len (grid )):
95
+ for j in range (len (grid [0 ])):
96
+ heuristic [i ][j ] = abs (i - goal [0 ]) + abs (j - goal [1 ])
97
+ if grid [i ][j ] == 1 :
98
+ # added extra penalty in the heuristic map
99
+ heuristic [i ][j ] = 99
100
+
101
+ path , action = search (grid , init , goal , cost , heuristic )
102
+
91
103
print ("ACTION MAP" )
92
104
for i in range (len (action )):
93
105
print (action [i ])
94
106
95
- return path
96
-
97
-
98
- a = search (grid , init , goal , cost , heuristic )
99
- for i in range (len (a )):
100
- print (a [i ])
107
+ for i in range (len (path )):
108
+ print (path [i ])
0 commit comments