Now can generate motion policy

Motion Planning/dynamic_prog.py

@@ -16,13 +16,13 @@ from icecream import ic
 
 from first_search import check_navigable
 
-# grid = [[0, 1, 0, 0, 0, 0],
-#         [0, 1, 0, 0, 0, 0],
-#         [0, 1, 0, 0, 0, 0],
-#         [0, 1, 0, 0, 0, 0],
-#         [0, 0, 0, 0, 1, 0]]
-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],
+grid = [[0, 1, 0, 0, 0, 0],
+        [0, 1, 0, 0, 0, 0],
+        [0, 1, 0, 0, 0, 0],
+        [0, 1, 0, 0, 0, 0],
+        [0, 0, 0, 0, 1, 0]]
+grid = np.array([[0, 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, 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],
@@ -30,7 +30,7 @@ grid = np.array([[0, 0, 1, 0, 0, 0, 0, 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]])
+                [0, 0, 0, 1, 1, 0, 0, 1, 0, 0, 0]])
 
 # init = [0,0]
 goal = [len(grid)-1, len(grid[0])-1]
@@ -43,9 +43,11 @@ delta = [[-1, 0],  # go up
 
 delta_name = ['^', '<', 'v', '>']
 
+
 def heuristic(*args):
     return 0
 
+
 def compute_value(grid, goal, cost):
     """Compute the reversed cost map. The goal point will have zero cost and
     each grid has a cost value representing the cost from here to the goal.
@@ -97,7 +99,51 @@ def compute_value(grid, goal, cost):
                     expand_map[next[0], next[1]] = current_expand_num
     return cost_map
 
-def generate_policy(cost_map):
-    pass
 
-ic(compute_value(grid, goal, cost))
+def generate_policy(cost_map, goal):
+
+    def check_navigable(cost_map, pos):
+        goal = np.array(pos).astype(int)
+        if all(goal >= np.array([0, 0])) and all(goal <= np.array([len(cost_map)-1, len(cost_map[0])-1])):
+            if cost_map[goal[0], goal[1]] != float('inf'):
+                return True
+        return False
+
+    delta = np.array([[-1, 0],  # type: np.ndarray[int]
+                      [0, -1],  # go left
+                      [1, 0],  # go down
+                      [0, 1]])  # go right
+    cost_map = np.array(cost_map)
+    policy_map = np.array([['+' for _ in range(len(cost_map[0]))]
+                          for _ in range(len(cost_map))])
+    policy_map[goal[0], goal[1]] = '*'
+    open_list = deque()
+    checked = set([tuple(goal)])
+    neighbor_grids = np.array([goal for _ in range(4)]) + delta
+    neighbor_grids = map(lambda pos: pos if check_navigable(
+        cost_map, pos) else False, neighbor_grids)
+    #  print(list(neighbor_grids))
+    for pos in neighbor_grids:
+        if type(pos) == np.ndarray:
+            open_list.append(pos)
+
+    while open_list:
+        current_grid = open_list.popleft()
+        checked.add(tuple(current_grid))
+        neighbor_grids = np.array([current_grid for _ in range(4)]) + delta
+        neighbor_costs = list(map(lambda pos: cost_map[pos[0], pos[1]] if check_navigable(
+            cost_map, pos) else float('inf'), neighbor_grids))
+        # expand the open list
+        for i in range(4):
+            if neighbor_costs[i] != float('inf') and tuple(neighbor_grids[i]) not in checked:
+                open_list.append(neighbor_grids[i])
+        minimum_cost_grid_index = np.argmin(neighbor_costs)
+        dlt_opst = ['^', '<', 'v', '>']
+        policy_map[current_grid[0], current_grid[1]
+                   ] = dlt_opst[minimum_cost_grid_index]
+
+    return policy_map
+
+
+cost_map = compute_value(grid, goal, cost)
+ic(generate_policy(cost_map, goal))

Motion Planning/first_search.py

@@ -114,11 +114,11 @@ def show_path(grid, init, goal, cost):
         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_grids = np.array([now_position for _ in range(4)])+delta
+            neighbor_grids = map(lambda pos: pos if check_navigable(
+                grid, pos) else False, neighbor_grids)
             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}
+                'INF'): [dlt_opst.popleft(), pos] for pos in neighbor_grids}
             next_move, now_position = neighbor_cost[min(neighbor_cost.keys())]
             motion_map[now_position[0], now_position[1]] = next_move
         return motion_map