nn_dynamics 学习笔记二

trajectories.py

生成路径

def make_trajectory(shape, starting_state_NN, x_index, y_index, which_agent):

    curr_x = np.copy(starting_state_NN[x_index])
    curr_y = np.copy(starting_state_NN[y_index])

    my_list = []

    if(shape=="left_turn"):
        if(which_agent==1):
            my_list.append(np.array([curr_x, curr_y]))
            my_list.append(np.array([curr_x+2, curr_y]))
            my_list.append(np.array([curr_x+4, curr_y]))
            my_list.append(np.array([curr_x+6, curr_y]))
            my_list.append(np.array([curr_x+6, curr_y+2]))
            my_list.append(np.array([curr_x+6, curr_y+3]))
            my_list.append(np.array([curr_x+6, curr_y+4]))
            my_list.append(np.array([curr_x+6, curr_y+5]))
            my_list.append(np.array([curr_x+6, curr_y+6]))
            my_list.append(np.array([curr_x+6, curr_y+7]))
        else:
            my_list.append(np.array([curr_x, curr_y]))
            my_list.append(np.array([curr_x+1, curr_y]))
            my_list.append(np.array([curr_x+2, curr_y]))
            my_list.append(np.array([curr_x+3, curr_y]))
            my_list.append(np.array([curr_x+4, curr_y+1]))
            my_list.append(np.array([curr_x+4, curr_y+2]))
            my_list.append(np.array([curr_x+4, curr_y+3]))
            my_list.append(np.array([curr_x+4, curr_y+4]))

    if(shape=="right_turn"):
        if(which_agent==1):
            my_list.append(np.array([curr_x, curr_y]))
            my_list.append(np.array([curr_x, curr_y+1]))
            my_list.append(np.array([curr_x, curr_y+2]))
            my_list.append(np.array([curr_x, curr_y+3]))
            my_list.append(np.array([curr_x, curr_y+4]))
            my_list.append(np.array([curr_x+2, curr_y+4]))
            my_list.append(np.array([curr_x+3, curr_y+4]))
            my_list.append(np.array([curr_x+4, curr_y+4]))
            my_list.append(np.array([curr_x+6, curr_y+4]))
            my_list.append(np.array([curr_x+7, curr_y+4]))
        else:
            my_list.append(np.array([curr_x, curr_y]))
            my_list.append(np.array([curr_x, curr_y+1]))
            my_list.append(np.array([curr_x, curr_y+2]))
            my_list.append(np.array([curr_x+2, curr_y+3]))
            my_list.append(np.array([curr_x+3, curr_y+3]))
            my_list.append(np.array([curr_x+4, curr_y+3]))
            my_list.append(np.array([curr_x+5, curr_y+3]))
            my_list.append(np.array([curr_x+6, curr_y+3]))
            my_list.append(np.array([curr_x+7, curr_y+3]))
            my_list.append(np.array([curr_x+8, curr_y+3]))

    if(shape=="u_turn"):
        my_list.append(np.array([curr_x, curr_y]))
        my_list.append(np.array([curr_x+2, curr_y]))
        my_list.append(np.array([curr_x+4, curr_y]))
        my_list.append(np.array([curr_x+4, curr_y+1]))
        my_list.append(np.array([curr_x+4, curr_y+2]))
        my_list.append(np.array([curr_x+2, curr_y+2]))
        my_list.append(np.array([curr_x+1, curr_y+2]))
        my_list.append(np.array([curr_x, curr_y+2]))

    if(shape=="straight"):
        i=0
        num_pts = 40
        while(i < num_pts):
            my_list.append(np.array([curr_x+i, curr_y]))
            i+=0.7

    if(shape=="backward"):
        i=0
        num_pts = 40
        while(i < num_pts):
            my_list.append(np.array([curr_x-i, curr_y]))
            i+=0.5

    if(shape=="forward_backward"):
        my_list.append(np.array([curr_x, curr_y]))
        my_list.append(np.array([curr_x+1, curr_y]))
        my_list.append(np.array([curr_x+2, curr_y]))
        my_list.append(np.array([curr_x+3, curr_y]))
        my_list.append(np.array([curr_x+2, curr_y]))
        my_list.append(np.array([curr_x+1, curr_y]))
        my_list.append(np.array([curr_x+0, curr_y]))
        my_list.append(np.array([curr_x-1, curr_y]))
        my_list.append(np.array([curr_x-2, curr_y]))

    if(shape=="circle"):
        num_pts = 10
        radius=2.0
        speed=-np.pi/8.0
        for i in range(num_pts):
            curr_x= radius*np.cos(speed*i)-radius
            curr_y= radius*np.sin(speed*i)
            my_list.append(np.array([curr_x, curr_y]))

    return np.array(my_list)