Add motion map

.gitignore

@@ -1,5 +1,6 @@
 # VSCODE
 /.vscode
+/.ipynb_checkpoints
 # ---> Python
 # Byte-compiled / optimized / DLL files
 __pycache__/

Motion Planning/first_search.py

@@ -18,16 +18,22 @@
 from collections import deque
 
 import numpy as np
+from icecream import ic
 
-grid = np.array([[0, 0, 1, 0, 0, 0, 0],
-                 [0, 0, 1, 0, 1, 0, 1],
-                 [0, 0, 1, 0, 0, 0, 0],
-                 [0, 0, 1, 1, 1, 0, 0],
-                 [0, 0, 0, 0, 1, 0, 1]])
+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([3, 6])
-cost = np.array([1.0, 1.0, 1.0, 1.0])
+goal = np.array([7, 10])
+cost = 1
 
 delta = np.array([[-1, 0],  # go up
                   [0, -1],  # go left
@@ -37,9 +43,8 @@ delta = np.array([[-1, 0],  # go up
 delta_name = ['^', '<', 'v', '>']
 
 
-def check_navigable(goal) -> bool:
+def check_navigable(grid, goal) -> bool:
     if all(goal >= np.array([0, 0])) and all(goal <= np.array([len(grid)-1, len(grid[0])-1])):
-        # print("G: ",goal)
         if not grid[goal[0], goal[1]]:
             return True
     return False
@@ -49,36 +54,60 @@ def search(grid: np.ndarray, init: list, goal: list, cost: list):
     # ----------------------------------------
     # insert code here
     # ----------------------------------------
-    path = list()
     next_check = deque()
     next_check.append(init)
     already_checked = dict()
     already_checked[tuple(init)] = 0
-    cost_map = np.zeros([len(grid), len(grid[0])])
-    extend_map = np.zeros([len(grid), len(grid[0])]) - 1
-    extend_now = 0
-    extend_map[0,0] = extend_now
+    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])]) - 1
+    expand_now = 0
+    expand_map[init[0], init[1]] = expand_now
 
     while next_check:
         checking = next_check.popleft()
-        if check_navigable(checking):
-            for move in delta:
-                next = checking+move  # type: np.ndarray
-                if check_navigable(next) and tuple(next) not in already_checked:
-                    extend_now += 1
+        if check_navigable(grid, checking):
+            for move_num in range(len(delta)):
+                next = checking+delta[move_num]  # type: np.ndarray
+                if check_navigable(grid, next) and tuple(next) not in already_checked:
+                    expand_now += 1
                     next_check.append(next)
-                    cost_now = already_checked[checking[0], checking[1]]+1
+                    cost_now = already_checked[checking[0], checking[1]]+cost
                     already_checked[tuple(next)] = cost_now
                     cost_map[next[0], next[1]] = cost_now
-                    extend_map[next[0], next[1]] = extend_now
+                    expand_map[next[0], next[1]] = expand_now
                     if all(next == goal):
-                        print(cost_map)
-                        print(extend_map)
-                        return [cost_now, next[0], next[1]]
+                        ic(cost_map)
+                        ic(expand_map)
+                        # return [cost_now, next[0], next[1]]
+                        return cost_map
 
-    print(cost_map)
-    print(extend_map)
+    # ic(cost_map)
+    # ic(expand_map)
     return "fail"
 
 
-print(search(grid, init, goal, cost))
+def show_path(grid, init, goal, cost):
+    cost_map = search(grid, init, goal, cost)
+    motion_map = np.array([[' ' for x in range(len(grid[0]))]
+                          for y in range(len(grid))])
+    motion_map[goal[0], goal[1]] = '*'
+    if type(cost_map) == np.ndarray:
+        print('success')
+        now_position = goal
+        while any(now_position != init):
+            dlt_opst = deque(['v', '>', '^', '<'])
+            neighbor_grid = np.array([now_position for _ in range(4)])+delta
+            neighbor_grid = map(lambda pos: pos if check_navigable(
+                grid, pos) else False, neighbor_grid)
+            neighbor_cost = {cost_map[pos[0], pos[1]] if type(pos) == np.ndarray else float(
+                'INF'): [dlt_opst.popleft(), pos] for pos in neighbor_grid}
+            next_move, now_position = neighbor_cost[min(neighbor_cost.keys())]
+            motion_map[now_position[0], now_position[1]] = next_move
+        return motion_map
+    elif cost_map == 'fail':
+        print('Fail to generate a feasible path.')
+        return motion_map
+
+
+ic(show_path(grid, init, goal, cost))