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

import heapq
from collections import deque

import numpy as np
from icecream import ic


def check_navigable(grid, goal) -> bool:
    """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[goal[0], goal[1]]:
            return True
    return False


def heuristic(now_pos, goal) -> float:
    """Returns the Euclidian distance between two points

    Parameters
    ----------
    now_pos : np.ndarray
        The current position
    goal : np.ndarray
        The goal position

    Returns
    -------
    float
        Distance
    """
    if type(now_pos) is not np.ndarray:
        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):
    open_list = []
    heapq.heappush(open_list, np.concatenate(
        ([heuristic(init, goal), 0], init)))
    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 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]
                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
                    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_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_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_grids}
            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('Failed to generate a feasible path.')
        return motion_map


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