|
|
@@ -21,33 +21,25 @@ from collections import deque
|
|
|
import numpy as np
|
|
|
from icecream import ic
|
|
|
|
|
|
-grid = np.array([[0, 0, 0, 1, 0, 0, 0, 0, 0, 0, 0],
|
|
|
- [1, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
|
|
|
- [0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0],
|
|
|
- [0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0],
|
|
|
- [0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0],
|
|
|
- [0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0],
|
|
|
- [0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0],
|
|
|
- [0, 0, 0, 0, 0, 0, 1, 0, 1, 0, 0],
|
|
|
- [0, 0, 0, 0, 1, 0, 0, 1, 0, 0, 0],
|
|
|
- [0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0]])
|
|
|
-init = np.array([0, 0])
|
|
|
-# goal = np.array([len(grid)-1, len(grid[0])-1])
|
|
|
-goal = np.array([0, 10])
|
|
|
-cost = 1
|
|
|
-
|
|
|
-delta = np.array([[-1, 0], # go up
|
|
|
- [0, -1], # go left
|
|
|
- [1, 0], # go down
|
|
|
- [0, 1]]) # go right
|
|
|
-
|
|
|
-delta_name = ['^', '<', 'v', '>']
|
|
|
-
|
|
|
|
|
|
def check_navigable(grid, goal) -> bool:
|
|
|
- goal = np.array(goal)
|
|
|
+ """Check if the grid is navigable
|
|
|
+
|
|
|
+ Parameters
|
|
|
+ ----------
|
|
|
+ grid : np.ndarray
|
|
|
+ The gird map of the environment
|
|
|
+ goal : np.ndarray
|
|
|
+ Goal position the function needs to reach
|
|
|
+
|
|
|
+ Returns
|
|
|
+ -------
|
|
|
+ bool
|
|
|
+ It's navigable(True) or not(False)
|
|
|
+ """
|
|
|
+ goal = np.array(goal).astype(int)
|
|
|
if all(goal >= np.array([0, 0])) and all(goal <= np.array([len(grid)-1, len(grid[0])-1])):
|
|
|
- if not grid[int(goal[0]), int(goal[1])]:
|
|
|
+ if not grid[goal[0], goal[1]]:
|
|
|
return True
|
|
|
return False
|
|
|
|
|
|
@@ -71,49 +63,44 @@ def heuristic(now_pos, goal) -> float:
|
|
|
now_pos = np.array(now_pos)
|
|
|
if type(goal) is not np.ndarray:
|
|
|
goal = np.array(goal)
|
|
|
+ # return 0
|
|
|
return np.linalg.norm(now_pos-goal)
|
|
|
|
|
|
|
|
|
def search(grid: np.ndarray, init: list, goal: list, cost: list):
|
|
|
- # ----------------------------------------
|
|
|
- # insert code here
|
|
|
- # ----------------------------------------
|
|
|
- next_check = []
|
|
|
- heapq.heappush(next_check, np.concatenate(
|
|
|
+ open_list = []
|
|
|
+ heapq.heappush(open_list, np.concatenate(
|
|
|
([heuristic(init, goal), 0], init)))
|
|
|
- already_checked = dict()
|
|
|
- already_checked[tuple(init)] = 0
|
|
|
+ closed_list = dict()
|
|
|
+ closed_list[tuple(init)] = 0
|
|
|
cost_map = np.zeros([len(grid), len(grid[0])]) + float('inf')
|
|
|
cost_map[init[0], init[1]] = 0
|
|
|
expand_map = np.zeros([len(grid), len(grid[0])]) - float('inf')
|
|
|
current_expand_num = 0
|
|
|
expand_map[init[0], init[1]] = current_expand_num
|
|
|
|
|
|
- while next_check:
|
|
|
- current_center_grid = heapq.heappop(next_check)
|
|
|
+ while open_list:
|
|
|
+ current_center_grid = heapq.heappop(open_list)
|
|
|
current_center_grid = current_center_grid[2:4] # type: list[int]
|
|
|
if check_navigable(grid, current_center_grid):
|
|
|
for move_num in range(len(delta)):
|
|
|
- next = current_center_grid+delta[move_num] # type: np.ndarray
|
|
|
- if check_navigable(grid, next) and tuple(next) not in already_checked:
|
|
|
+ next = current_center_grid+delta[move_num]
|
|
|
+ ic(type(next), next)
|
|
|
+ next = next.astype('int') # type: np.ndarray[int]
|
|
|
+ if check_navigable(grid, next) and tuple(next) not in closed_list:
|
|
|
current_expand_num += 1
|
|
|
- gg = [heuristic(next, goal),
|
|
|
- current_expand_num] + list(next)
|
|
|
- heapq.heappush(next_check, gg)
|
|
|
- # next_check.append(next)
|
|
|
- current_cost = already_checked[current_center_grid[0],
|
|
|
- current_center_grid[1]]+cost
|
|
|
- already_checked[tuple(next)] = current_cost
|
|
|
- cost_map[int(next[0]), int(next[1])] = current_cost
|
|
|
- expand_map[int(next[0]), int(next[1])] = current_expand_num
|
|
|
+ current_cost = closed_list[current_center_grid[0],
|
|
|
+ current_center_grid[1]]+cost
|
|
|
+ expand_grid = [heuristic(next, goal)+current_expand_num,
|
|
|
+ current_expand_num] + list(next)
|
|
|
+ heapq.heappush(open_list, expand_grid)
|
|
|
+ closed_list[tuple(next)] = current_cost
|
|
|
+ cost_map[next[0], next[1]] = current_cost
|
|
|
+ expand_map[next[0], next[1]] = current_expand_num
|
|
|
if all(next == goal):
|
|
|
ic(cost_map)
|
|
|
ic(expand_map)
|
|
|
- # return [cost_now, next[0], next[1]]
|
|
|
return cost_map
|
|
|
-
|
|
|
- # ic(cost_map)
|
|
|
- # ic(expand_map)
|
|
|
return "fail"
|
|
|
|
|
|
|
|
|
@@ -123,7 +110,7 @@ def show_path(grid, init, goal, cost):
|
|
|
for y in range(len(grid))])
|
|
|
motion_map[goal[0], goal[1]] = '*'
|
|
|
if type(cost_map) == np.ndarray:
|
|
|
- print('success')
|
|
|
+ print('Success')
|
|
|
now_position = goal
|
|
|
while any(now_position != init):
|
|
|
dlt_opst = deque(['v', '>', '^', '<'])
|
|
|
@@ -136,8 +123,31 @@ def show_path(grid, init, goal, cost):
|
|
|
motion_map[now_position[0], now_position[1]] = next_move
|
|
|
return motion_map
|
|
|
elif cost_map == 'fail':
|
|
|
- print('Fail to generate a feasible path.')
|
|
|
+ print('Failed to generate a feasible path.')
|
|
|
return motion_map
|
|
|
|
|
|
|
|
|
-ic(show_path(grid, init, goal, cost))
|
|
|
+if __name__ == '__main__':
|
|
|
+ inf = float('inf')
|
|
|
+ grid = np.array([[0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
|
|
|
+ [0, 0, 1, 0, 0, 0, 0, 0, 0, 0, 0],
|
|
|
+ [0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0],
|
|
|
+ [0, 0, 1, 0, 1, 0, 0, 0, 0, 0, 0],
|
|
|
+ [0, 0, 1, 0, 1, 0, 0, 0, 1, 0, 0],
|
|
|
+ [0, 0, 1, 0, 1, 0, 1, 0, 0, 0, 0],
|
|
|
+ [0, 0, 0, 0, 1, 0, 1, 0, 0, 0, 0],
|
|
|
+ [0, 0, 0, 0, 1, 0, 0, 0, 1, 0, 0],
|
|
|
+ [0, 0, 1, 0, 1, 0, 0, 1, 0, 0, 0],
|
|
|
+ [0, 0, 0, 1, 0, 0, 0, 1, 0, 0, 0]])
|
|
|
+ init = np.array([0, 0])
|
|
|
+ # goal = np.array([len(grid)-1, len(grid[0])-1])
|
|
|
+ goal = np.array([3, 10])
|
|
|
+ cost = 1
|
|
|
+
|
|
|
+ delta = np.array([[-1, 0], # type: np.ndarray[int]
|
|
|
+ [0, -1], # go left
|
|
|
+ [1, 0], # go down
|
|
|
+ [0, 1]]) # go right
|
|
|
+
|
|
|
+ delta_name = ['^', '<', 'v', '>']
|
|
|
+ ic(show_path(grid, init, goal, cost))
|
Zhilong Li