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

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)
    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])]:
            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 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(
        ([heuristic(init, goal), 0], 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])]) - 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)
        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:
                    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
                    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))