Ai_for_robotics/Motion Planning/first_search.py

# ----------
# User Instructions:
#
# Define a function, search() that returns a list
# in the form of [optimal path length, row, col]. For
# the grid shown below, your function should output
# [11, 4, 5].
#
# If there is no valid path from the start point
# to the goal, your function should return the string
# 'fail'
# ----------

# Grid format:
#   0 = Navigable space
#   1 = Occupied space

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([7, 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:
    if all(goal >= np.array([0, 0])) and all(goal <= np.array([len(grid)-1, len(grid[0])-1])):
        if not grid[goal[0], goal[1]]:
            return True
    return False


def search(grid: np.ndarray, init: list, goal: list, cost: list):
    # ----------------------------------------
    # insert code here
    # ----------------------------------------
    next_check = deque()
    next_check.append(init)
    already_checked = dict()
    already_checked[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])]) - 1
    expand_now = 0
    expand_map[init[0], init[1]] = expand_now

    while next_check:
        checking = next_check.popleft()
        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]]+cost
                    already_checked[tuple(next)] = cost_now
                    cost_map[next[0], next[1]] = cost_now
                    expand_map[next[0], next[1]] = expand_now
                    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"


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))