Example #1
0
def build_up_graph(grid, save_path):
    max_vel = 5

    # velocity dimension
    vel_list = []
    for i_vel in range(-max_vel+1, max_vel):
        for j_vel in range(-max_vel+1, max_vel):
            vel_list.append([i_vel, j_vel])

    # position dimension
    x_idx, y_idx = np.where(grid == FREE)
    coord = np.stack([x_idx, y_idx], axis=1)
    for p_idx in range(coord.shape[0]):
        pnt = coord[p_idx]
        for vel in vel_list:
            state = Node(pnt[0], pnt[1], vel[0], vel[1])
            state.connect_to_graph(grid)
            graph[state.key] = state

    for pnt in START_LINE:
        state = Node(pnt[0], pnt[1], 0, 0)
        state.connect_to_graph(grid)
        graph[state.key] = state

    for pnt in FINISH_LINE:
        state = Node(pnt[0], pnt[1], 0, 0)
        state.is_goal = True
        graph[state.key] = state

    output = open(save_path, 'wb')
    pickle.dump(graph, output)
def build_up_graph(grid, save_path):
    max_vel = 5

    # velocity dimension
    vel_list = []
    for i_vel in range(-max_vel + 1, max_vel):
        for j_vel in range(-max_vel + 1, max_vel):
            vel_list.append([i_vel, j_vel])  # 速度的一个范围 ?

    # position dimension
    x_idx, y_idx = np.where(grid == FREE)  # 记录所有为0的坐标
    coord = np.stack([x_idx, y_idx], axis=1)  # 每一行都是一个坐标
    for p_idx in range(coord.shape[0]):
        pnt = coord[p_idx]
        for vel in vel_list:  # 每个点都按速度循环????这个有什么意义
            state = Node(pnt[0], pnt[1], vel[0], vel[1])
            m_dist = np.abs(
                np.asarray(FINISH_LINE) -
                np.array([state.px, state.py]))  # 前一项的每一个元素都会和后面的相减

            # IMPORTANT-1 Heuristic Function design here
            # TO BE IMPLEMENTED

            # Note: Both the two heuristics are not the best
            # example-1
            #heuristic = np.linalg.norm(m_dist, axis=1)  # Euclidean distance
            # example-2
            #heuristic = m_dist[:, 0] + m_dist[:, 1]  # Mahalonobis distance

            # diagonal heuristic

            heuristic = m_dist[:, 0] + m_dist[:, 1] + (2**0.5 - 2) * np.min(
                m_dist, axis=1)[:, np.newaxis]

            state.g_value = np.min(heuristic)  # 取最小的
            print(state.g_value)
            state.connect_to_graph(grid)
            graph[state.key] = state

    for pnt in START_LINE:
        state = Node(pnt[0], pnt[1], 0, 0)
        heuristic = np.linalg.norm(np.asarray(FINISH_LINE) -
                                   np.array([state.px, state.py]),
                                   axis=1)
        state.g_value = np.min(heuristic)
        state.connect_to_graph(grid)
        graph[state.key] = state

    for pnt in FINISH_LINE:
        state = Node(pnt[0], pnt[1], 0, 0)
        state.is_goal = True
        graph[state.key] = state

    output = open(save_path, 'wb')
    pickle.dump(graph, output)
Example #3
0
def build_up_graph(grid, save_path):
    max_vel = 5

    # velocity dimension
    vel_list = []
    for i_vel in range(-max_vel + 1, max_vel):
        for j_vel in range(-max_vel + 1, max_vel):
            vel_list.append([i_vel, j_vel])

    # position dimension
    x_idx, y_idx = np.where(grid == FREE)
    coord = np.stack([x_idx, y_idx], axis=1)
    for p_idx in range(coord.shape[0]):
        pnt = coord[p_idx]
        for vel in vel_list:
            state = Node(pnt[0], pnt[1], vel[0], vel[1])
            m_dist = np.abs(
                np.asarray(FINISH_LINE) - np.array([state.px, state.py]))

            # IMPORTANT-1 Heuristic Function design here
            # TO BE IMPLEMENTED
            # heuristic = np.linalg.norm(m_dist, axis=1)
            L1 = m_dist[:, 0] + m_dist[:, 1]
            L2 = np.linalg.norm(m_dist, axis=1)

            heuristic = 0.5 * L1 + 0.5 * L2

            # Note: Both the two heuristics are not the best
            # example-1 heuristic = np.linalg.norm(m_dist, axis=1)  # Euclidean distance
            # example-2 heuristic = m_dist[:, 0] + m_dist[:, 1]  # Mahalonobis distance

            state.g_value = np.min(heuristic)
            state.connect_to_graph(grid)
            graph[state.key] = state

    for pnt in START_LINE:
        state = Node(pnt[0], pnt[1], 0, 0)
        heuristic = np.linalg.norm(np.asarray(FINISH_LINE) -
                                   np.array([state.px, state.py]),
                                   axis=1)
        state.g_value = np.min(heuristic)
        state.connect_to_graph(grid)
        graph[state.key] = state

    for pnt in FINISH_LINE:
        state = Node(pnt[0], pnt[1], 0, 0)
        state.is_goal = True
        graph[state.key] = state

    output = open(save_path, 'wb')
    pickle.dump(graph, output)