Ejemplo n.º 1
0
def test_sbpd_map(visualize=False):
    np.random.seed(seed=1)
    p = create_params()

    r, dx_cm, traversible = load_building(p)

    trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=p.pos_nk2)

    obstacle_map = SBPDMap(p.obstacle_map_params,
                           renderer=0,
                           res=dx_cm,
                           map_trav=traversible)

    obs_dists_nk = obstacle_map.dist_to_nearest_obs(trajectory.position_nk2())

    assert (np.allclose(obs_dists_nk[0], p.test_obst_map_ans))

    if visualize:
        #occupancy_grid_nn = obstacle_map.create_occupancy_grid(trajectory.position_nk2())

        fig = plt.figure()
        ax = fig.add_subplot(121)
        obstacle_map.render(ax)

        ax = fig.add_subplot(122)
        # ax.imshow(occupancy_grid_nn, cmap='gray', origin='lower')
        ax.set_axis_off()
        # plt.show()
        fig.savefig('./tests/obstacles/test_obstacle_map.png',
                    bbox_inches='tight',
                    pad_inches=0)
def test_sbpd_map(visualize=False):
    np.random.seed(seed=1)

    # Define a set of positions and evaluate objective
    pos_nk2 = tf.constant([[[8., 16.], [8., 12.5], [18., 16.5]]],
                          dtype=tf.float32)
    trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=pos_nk2)

    p = create_params()

    # Create an SBPD Map
    obstacle_map = SBPDMap(p.obstacle_map_params)

    obs_dists_nk = obstacle_map.dist_to_nearest_obs(trajectory.position_nk2())

    assert (np.allclose(obs_dists_nk, [0.59727454, 1.3223624, 0.47055122]))

    if visualize:
        #occupancy_grid_nn = obstacle_map.create_occupancy_grid(trajectory.position_nk2())

        fig = plt.figure()
        ax = fig.add_subplot(121)
        obstacle_map.render(ax)

        ax = fig.add_subplot(122)
        # ax.imshow(occupancy_grid_nn, cmap='gray', origin='lower')
        ax.set_axis_off()
        # plt.show()
        fig.savefig('./tests/obstacles/test_obstacle_map.png',
                    bbox_inches='tight',
                    pad_inches=0)
Ejemplo n.º 3
0
def test_cost_function(plot=False):
    # Create parameters
    p = create_params()

    obstacle_map = SBPDMap(p.obstacle_map_params)
    obstacle_occupancy_grid = obstacle_map.create_occupancy_grid_for_map()
    map_size_2 = obstacle_occupancy_grid.shape[::-1]

    # Define a goal position and compute the corresponding fmm map
    goal_pos_n2 = np.array([[20., 16.5]])
    fmm_map = FmmMap.create_fmm_map_based_on_goal_position(goal_positions_n2=goal_pos_n2,
                                                           map_size_2=map_size_2,
                                                           dx=0.05,
                                                           map_origin_2=[0., 0.],
                                                           mask_grid_mn=obstacle_occupancy_grid)

    # Define the cost function
    objective_function = ObjectiveFunction(p.objective_fn_params)
    objective_function.add_objective(ObstacleAvoidance(params=p.avoid_obstacle_objective, obstacle_map=obstacle_map))
    objective_function.add_objective(GoalDistance(params=p.goal_distance_objective, fmm_map=fmm_map))
    objective_function.add_objective(AngleDistance(params=p.goal_angle_objective, fmm_map=fmm_map))

    # Define each objective separately
    objective1 = ObstacleAvoidance(params=p.avoid_obstacle_objective, obstacle_map=obstacle_map)
    objective2 = GoalDistance(params=p.goal_distance_objective, fmm_map=fmm_map)
    objective3 = AngleDistance(params=p.goal_angle_objective, fmm_map=fmm_map)

    # Define a set of positions and evaluate objective
    pos_nk2 = tf.constant([[[8., 16.], [8., 12.5], [18., 16.5]]], dtype=tf.float32)
    trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=pos_nk2)

    # Compute the objective function
    values_by_objective = objective_function.evaluate_function_by_objective(trajectory)
    overall_objective = objective_function.evaluate_function(trajectory)

    # Expected objective values
    expected_objective1 = objective1.evaluate_objective(trajectory)
    expected_objective2 = objective2.evaluate_objective(trajectory)
    expected_objective3 = objective3.evaluate_objective(trajectory)
    expected_overall_objective = tf.reduce_mean(expected_objective1 + expected_objective2 + expected_objective3, axis=1)

    # assert len(values_by_objective) == 3
    # assert values_by_objective[0][1].shape == (1, 3)
    # assert overall_objective.shape == (1,)
    # assert np.allclose(overall_objective.numpy(), expected_overall_objective.numpy(), atol=1e-2)


    # Optionally visualize the traversable and the points on which
    # we compute the objective function
    if plot:
        import matplotlib.pyplot as plt

        fig = plt.figure()
        ax = fig.add_subplot(111)
        obstacle_map.render(ax)
        ax.plot(pos_nk2[0, :, 0].numpy(), pos_nk2[0, :, 1].numpy(), 'r.')
        ax.plot(goal_pos_n2[0, 0], goal_pos_n2[0, 1], 'k*')
        fig.savefig('./tmp/test_cost_function.png', bbox_inches='tight', pad_inches=0)
Ejemplo n.º 4
0
def test_goal_distance():
    # Create parameters
    p = create_params()

    r, dx_cm, traversible = load_building(p)

    obstacle_map = SBPDMap(p.obstacle_map_params,
                           renderer=0,
                           res=dx_cm,
                           map_trav=traversible)
    # obstacle_map = SBPDMap(p.obstacle_map_params)
    obstacle_occupancy_grid = obstacle_map.create_occupancy_grid_for_map()
    map_size_2 = obstacle_occupancy_grid.shape[::-1]

    # Define a goal position and compute the corresponding fmm map
    goal_pos_n2 = p.goal_pos_n2
    fmm_map = FmmMap.create_fmm_map_based_on_goal_position(
        goal_positions_n2=goal_pos_n2,
        map_size_2=map_size_2,
        dx=0.05,
        map_origin_2=[0., 0.],
        mask_grid_mn=obstacle_occupancy_grid)

    # Define the objective
    objective = GoalDistance(params=p.goal_distance_objective, fmm_map=fmm_map)

    # Define a set of positions and evaluate objective
    pos_nk2 = p.pos_nk2
    trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=pos_nk2)

    # Compute the objective
    objective_values_13 = objective.evaluate_objective(trajectory)
    assert objective_values_13.shape == (1, 3)

    # Expected objective values
    distance_map = fmm_map.fmm_distance_map.voxel_function_mn
    idxs_xy_n2 = pos_nk2[0] / .05
    idxs_yx_n2 = idxs_xy_n2[:, ::-1].astype(np.int32)
    expected_distance = np.array([
        distance_map[idxs_yx_n2[0][0], idxs_yx_n2[0][1]],
        distance_map[idxs_yx_n2[1][0], idxs_yx_n2[1][1]],
        distance_map[idxs_yx_n2[2][0], idxs_yx_n2[2][1]]
    ],
                                 dtype=np.float32)
    cost_at_margin = 25. * p.goal_distance_objective.goal_margin**2
    expected_objective = 25. * expected_distance * expected_distance - cost_at_margin

    # Error in objectives
    # We have to allow a little bit of leeway in this test because the computation of FMM distance is not exact.
    objetive_error = abs(expected_objective -
                         objective_values_13[0]) / (expected_objective + 1e-6)
    assert max(objetive_error) <= 0.1

    numerical_error = max(abs(objective_values_13[0] - p.test_goal_dist_ans))
    assert numerical_error <= .01
def test_goal_angle_distance():
    # Create parameters
    p = create_params()

    r, dx_cm, traversible = load_building(p)

    obstacle_map = SBPDMap(p.obstacle_map_params,
                           renderer=0,
                           res=dx_cm,
                           map_trav=traversible)
    # obstacle_map = SBPDMap(p.obstacle_map_params)
    obstacle_occupancy_grid = obstacle_map.create_occupancy_grid_for_map()
    map_size_2 = obstacle_occupancy_grid.shape[::-1]

    # Define a goal position and compute the corresponding fmm map
    # goal_pos_n2 = np.array([[9., 15.]])
    goal_pos_n2 = p.goal_pos_n2
    fmm_map = FmmMap.create_fmm_map_based_on_goal_position(
        goal_positions_n2=goal_pos_n2,
        map_size_2=map_size_2,
        dx=0.05,
        map_origin_2=[0., 0.],
        mask_grid_mn=obstacle_occupancy_grid)

    # Define the objective
    objective = AngleDistance(params=p.goal_angle_objective, fmm_map=fmm_map)

    # Define a set of positions and evaluate objective
    # pos_nk2 = np.array([[[8., 16.], [8., 12.5], [18., 16.5]]], dtype=np.float32)
    pos_nk2 = p.pos_nk2
    trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=pos_nk2)

    # Compute the objective
    objective_values_13 = objective.evaluate_objective(trajectory)
    assert objective_values_13.shape == (1, 3)

    # Expected objective values
    angle_map = fmm_map.fmm_angle_map.voxel_function_mn
    idxs_xy_n2 = pos_nk2[0] / .05
    idxs_yx_n2 = idxs_xy_n2[:, ::-1].astype(np.int32)
    expected_angles = np.array([
        angle_map[idxs_yx_n2[0][0], idxs_yx_n2[0][1]],
        angle_map[idxs_yx_n2[1][0], idxs_yx_n2[1][1]],
        angle_map[idxs_yx_n2[2][0], idxs_yx_n2[2][1]]
    ],
                               dtype=np.float32)
    expected_objective = 25. * abs(expected_angles)

    assert np.allclose(objective_values_13[0], expected_objective, atol=1e-2)
    # hardcoded results to match the given inputs
    assert np.allclose(objective_values_13[0],
                       p.test_goal_ang_obj_ans,
                       atol=1e-2)
def test_goal_distance():
    # Create parameters
    p = create_params()

    # Create an SBPD Map
    obstacle_map = SBPDMap(p.obstacle_map_params)
    obstacle_occupancy_grid = obstacle_map.create_occupancy_grid_for_map()
    map_size_2 = obstacle_occupancy_grid.shape[::-1]

    # Define a goal position and compute the corresponding fmm map
    goal_pos_n2 = np.array([[20, 16.5]])
    fmm_map = FmmMap.create_fmm_map_based_on_goal_position(
        goal_positions_n2=goal_pos_n2,
        map_size_2=map_size_2,
        dx=0.05,
        map_origin_2=[0., 0.],
        mask_grid_mn=obstacle_occupancy_grid)

    # Define the objective
    objective = GoalDistance(params=p.goal_distance_objective, fmm_map=fmm_map)

    # Define a set of positions and evaluate objective
    pos_nk2 = tf.constant([[[8., 16.], [8., 12.5], [18., 16.5]]],
                          dtype=tf.float32)
    trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=pos_nk2)

    # Compute the objective
    objective_values_13 = objective.evaluate_objective(trajectory)
    assert objective_values_13.shape == (1, 3)

    # Expected objective values
    distance_map = fmm_map.fmm_distance_map.voxel_function_mn
    idxs_xy_n2 = pos_nk2[0] / .05
    idxs_yx_n2 = idxs_xy_n2[:, ::-1].numpy().astype(np.int32)
    expected_distance = np.array([
        distance_map[idxs_yx_n2[0][0], idxs_yx_n2[0][1]],
        distance_map[idxs_yx_n2[1][0], idxs_yx_n2[1][1]],
        distance_map[idxs_yx_n2[2][0], idxs_yx_n2[2][1]]
    ],
                                 dtype=np.float32)
    cost_at_margin = 25. * p.goal_distance_objective.goal_margin**2
    expected_objective = 25. * expected_distance * expected_distance - cost_at_margin

    # Error in objectives
    # We have to allow a little bit of leeway in this test because the computation of FMM distance is not exact.
    objetive_error = abs(expected_objective - objective_values_13.numpy()[0]
                         ) / (expected_objective + 1e-6)
    assert max(objetive_error) <= 0.1

    numerical_error = max(
        abs(objective_values_13[0].numpy() - [3590.4614, 4975.554, 97.15442]))
    assert numerical_error <= .01
Ejemplo n.º 7
0
def test_goal_angle_distance():
    # Create parameters
    p = create_params()

    # Create an SBPD Map
    obstacle_map = SBPDMap(p.obstacle_map_params)
    obstacle_occupancy_grid = obstacle_map.create_occupancy_grid_for_map()
    map_size_2 = obstacle_occupancy_grid.shape[::-1]

    # Define a goal position and compute the corresponding fmm map
    goal_pos_n2 = np.array([[20, 16.5]])
    fmm_map = FmmMap.create_fmm_map_based_on_goal_position(
        goal_positions_n2=goal_pos_n2,
        map_size_2=map_size_2,
        dx=0.05,
        map_origin_2=[0., 0.],
        mask_grid_mn=obstacle_occupancy_grid)

    # Define the objective
    objective = AngleDistance(params=p.goal_angle_objective, fmm_map=fmm_map)

    # Define a set of positions and evaluate objective
    pos_nk2 = tf.constant([[[8., 16.], [8., 12.5], [18., 16.5]]],
                          dtype=tf.float32)
    trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=pos_nk2)

    # Compute the objective
    objective_values_13 = objective.evaluate_objective(trajectory)
    assert objective_values_13.shape == (1, 3)

    # Expected objective values
    angle_map = fmm_map.fmm_angle_map.voxel_function_mn
    idxs_xy_n2 = pos_nk2[0] / .05
    idxs_yx_n2 = idxs_xy_n2[:, ::-1].numpy().astype(np.int32)
    expected_angles = np.array([
        angle_map[idxs_yx_n2[0][0], idxs_yx_n2[0][1]],
        angle_map[idxs_yx_n2[1][0], idxs_yx_n2[1][1]],
        angle_map[idxs_yx_n2[2][0], idxs_yx_n2[2][1]]
    ],
                               dtype=np.float32)
    expected_objective = 25. * abs(expected_angles)

    assert np.allclose(objective_values_13.numpy()[0],
                       expected_objective,
                       atol=1e-2)
    assert np.allclose(objective_values_13.numpy()[0],
                       [1.2449384, 29.137403, 0.],
                       atol=1e-2)
Ejemplo n.º 8
0
def test_avoid_obstacle():
    # Create parameters
    p = create_params()

    # Create an SBPD Map
    obstacle_map = SBPDMap(p.obstacle_map_params)

    # Define the objective
    objective = ObstacleAvoidance(params=p.avoid_obstacle_objective,
                                  obstacle_map=obstacle_map)

    # Define a set of positions and evaluate objective
    pos_nk2 = tf.constant([[[8., 16.], [8., 12.5], [18., 16.5]]],
                          dtype=tf.float32)
    trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=pos_nk2)

    # Compute the objective
    objective_values_13 = objective.evaluate_objective(trajectory)
    assert objective_values_13.shape == (1, 3)

    # Expected objective values
    distance_map = obstacle_map.fmm_map.fmm_distance_map.voxel_function_mn
    idxs_xy_n2 = pos_nk2[0] / .05
    idxs_yx_n2 = idxs_xy_n2[:, ::-1].numpy().astype(np.int32)
    expected_min_dist_to_obs = np.array([
        distance_map[idxs_yx_n2[0][0], idxs_yx_n2[0][1]],
        distance_map[idxs_yx_n2[1][0], idxs_yx_n2[1][1]],
        distance_map[idxs_yx_n2[2][0], idxs_yx_n2[2][1]]
    ],
                                        dtype=np.float32)

    m0 = p.avoid_obstacle_objective.obstacle_margin0
    m1 = p.avoid_obstacle_objective.obstacle_margin1
    expected_infringement = m1 - expected_min_dist_to_obs
    expected_infringement = np.clip(expected_infringement, a_min=0,
                                    a_max=None)  # ReLU
    expected_infringement /= (m1 - m0)
    expected_objective = 25. * expected_infringement * expected_infringement

    assert np.allclose(objective_values_13.numpy()[0],
                       expected_objective,
                       atol=1e-4)
    assert np.allclose(objective_values_13.numpy()[0], [0., 0., 0.54201907],
                       atol=1e-4)
Ejemplo n.º 9
0
def test_avoid_obstacle(visualize=False):
    """
    Run a test on handpicked waypoint values that build a trajectory and are used
    to evaluate the computed distance_map and angle_map from the fmm_*_map.voxel_function_mn
    """
    # Create parameters
    p = create_params()

    # Create an SBPD Map
    r, dx_cm, traversible = load_building(p)

    obstacle_map = SBPDMap(p.obstacle_map_params,
                           renderer=0,
                           res=dx_cm,
                           map_trav=traversible)

    # Define the objective
    objective = ObstacleAvoidance(params=p.avoid_obstacle_objective,
                                  obstacle_map=obstacle_map)

    # Define a set of positions and evaluate objective
    pos_nk2 = p.pos_nk2
    trajectory = Trajectory(dt=0.1, n=1, k=3, position_nk2=pos_nk2)

    # Compute the objective
    objective_values_13 = objective.evaluate_objective(trajectory)
    assert objective_values_13.shape == (1, 3)

    # Expected objective values
    distance_map = obstacle_map.fmm_map.fmm_distance_map.voxel_function_mn
    angle_map = obstacle_map.fmm_map.fmm_angle_map.voxel_function_mn

    idxs_xy_n2 = pos_nk2[0] / .05
    idxs_yx_n2 = idxs_xy_n2[:, ::-1].astype(np.int32)
    expected_min_dist_to_obs = np.array([
        distance_map[idxs_yx_n2[0][0], idxs_yx_n2[0][1]],
        distance_map[idxs_yx_n2[1][0], idxs_yx_n2[1][1]],
        distance_map[idxs_yx_n2[2][0], idxs_yx_n2[2][1]]
    ],
                                        dtype=np.float32)

    m0 = p.avoid_obstacle_objective.obstacle_margin0
    m1 = p.avoid_obstacle_objective.obstacle_margin1
    expected_infringement = m1 - expected_min_dist_to_obs
    expected_infringement = np.clip(expected_infringement, a_min=0,
                                    a_max=None)  # ReLU
    expected_infringement /= (m1 - m0)
    expected_objective = 25. * expected_infringement * expected_infringement

    assert np.allclose(objective_values_13[0], expected_objective, atol=1e-4)
    assert np.allclose(objective_values_13[0], p.test_obs_obj_ans, atol=1e-4)
    if visualize:
        """
        create a 1 x 3 (or 1 x 4) image of the obstacle map itself (as a traversible plot), 
        next to its corresponding angle_map, and distance_map. Optionally plotting the trajectory
        """
        fig = plt.figure()
        ax = fig.add_subplot(1, 3, 1)
        obstacle_map.render(ax)
        ax.plot(pos_nk2[0, :, 0], pos_nk2[0, :, 1], 'r.')
        # ax.plot(objective[0, 0], objective[0, 1], 'k*')
        ax.set_title('obstacle map')

        # Plotting the "angle map"
        ax = fig.add_subplot(1, 3, 2)
        ax.imshow(angle_map, origin='lower')
        ax.set_title('angle map')

        # Plotting the "distance map"
        ax = fig.add_subplot(1, 3, 3)
        ax.imshow(distance_map, origin='lower')
        ax.set_title('distance map')

        # Plotting the trajectory
        #ax = fig.add_subplot(1,4,4)
        # trajectory.render(ax)
        # ax.set_title('trajectory')

        fig.savefig('./tests/obstacles/test_obstacle_objective.png',
                    bbox_inches='tight',
                    pad_inches=0)
Ejemplo n.º 10
0
def test_cost_function(plot=False):
    """
    Creating objective points maually, plotting them in the ObjectiveFunction
    class, and then asserting that combined, their sum adds up to the same
    objective cost as the sum of the individual trajectories
    """
    # Create parameters
    p = create_params()
    r, dx_cm, traversible = load_building(p)

    obstacle_map = SBPDMap(p.obstacle_map_params,
                           renderer=0,
                           res=dx_cm,
                           map_trav=traversible)
    # obstacle_map = SBPDMap(p.obstacle_map_params)
    obstacle_occupancy_grid = obstacle_map.create_occupancy_grid_for_map()
    map_size_2 = obstacle_occupancy_grid.shape[::-1]

    # Define a goal position and compute the corresponding fmm map
    goal_pos_n2 = p.goal_pos_n2
    fmm_map = FmmMap.create_fmm_map_based_on_goal_position(
        goal_positions_n2=goal_pos_n2,
        map_size_2=map_size_2,
        dx=0.05,
        map_origin_2=[0., 0.],
        mask_grid_mn=obstacle_occupancy_grid)
    # Define the cost function
    objective_function = ObjectiveFunction(p.objective_fn_params)
    objective_function.add_objective(
        ObstacleAvoidance(params=p.avoid_obstacle_objective,
                          obstacle_map=obstacle_map))
    objective_function.add_objective(
        GoalDistance(params=p.goal_distance_objective, fmm_map=fmm_map))
    objective_function.add_objective(
        AngleDistance(params=p.goal_angle_objective, fmm_map=fmm_map))

    # Define each objective separately
    objective1 = ObstacleAvoidance(params=p.avoid_obstacle_objective,
                                   obstacle_map=obstacle_map)
    objective2 = GoalDistance(params=p.goal_distance_objective,
                              fmm_map=fmm_map)
    objective3 = AngleDistance(params=p.goal_angle_objective, fmm_map=fmm_map)

    # Define a set of positions and evaluate objective
    pos_nk2 = p.pos_nk2
    heading_nk2 = np.array([[[np.pi / 2.0], [0.1], [0.1]]], dtype=np.float32)
    trajectory = Trajectory(dt=0.1,
                            n=1,
                            k=3,
                            position_nk2=pos_nk2,
                            heading_nk1=heading_nk2)

    # Compute the objective function
    values_by_objective = objective_function.evaluate_function_by_objective(
        trajectory)
    overall_objective = objective_function.evaluate_function(trajectory)

    # Expected objective values
    expected_objective1 = objective1.evaluate_objective(trajectory)
    expected_objective2 = objective2.evaluate_objective(trajectory)
    expected_objective3 = objective3.evaluate_objective(trajectory)
    # expected_overall_objective = tf.reduce_mean(
    #     expected_objective1 + expected_objective2 + expected_objective3, axis=1)
    expected_overall_objective = np.mean(
        expected_objective1 + expected_objective2 + expected_objective3,
        axis=1)
    assert len(values_by_objective) == 3
    assert values_by_objective[0][1].shape == (1, 3)
    assert overall_objective.shape == (1, )
    # assert np.allclose(overall_objective.numpy(), expected_overall_objective.numpy(), atol=1e-2)
    assert np.allclose(overall_objective,
                       expected_overall_objective,
                       atol=1e-2)

    # Optionally visualize the traversable and the points on which
    # we compute the objective function
    if plot:
        fig = plt.figure()
        ax = fig.add_subplot(1, 1, 1)
        obstacle_map.render(ax)
        ax.plot(pos_nk2[0, :, 0].numpy(), pos_nk2[0, :, 1].numpy(), 'r.')
        ax.plot(goal_pos_n2[0, 0], goal_pos_n2[0, 1], 'k*')
        fig.savefig('./tests/cost/test_cost_function.png',
                    bbox_inches='tight',
                    pad_inches=0)
Ejemplo n.º 11
0
def test_personal_cost_function(sim_state: SimState,
                                plot=False,
                                verbose=False):
    """
    Creating objective points maually, plotting them in the ObjectiveFunction
    class, and then asserting that combined, their sum adds up to the same
    objective cost as the sum of the individual trajectories
    """
    # Create parameters
    p = create_params()
    r, dx_cm, traversible = load_building(p)

    obstacle_map = SBPDMap(p.obstacle_map_params,
                           renderer=0,
                           res=dx_cm,
                           map_trav=traversible)
    # obstacle_map = SBPDMap(p.obstacle_map_params)
    obstacle_occupancy_grid = obstacle_map.create_occupancy_grid_for_map()
    map_size_2 = obstacle_occupancy_grid.shape[::-1]

    # Define a goal position and compute the corresponding fmm map
    goal_pos_n2 = np.array([[9., 10]])
    fmm_map = FmmMap.create_fmm_map_based_on_goal_position(
        goal_positions_n2=goal_pos_n2,
        map_size_2=map_size_2,
        dx=0.05,
        map_origin_2=[0., 0.],
        mask_grid_mn=obstacle_occupancy_grid)
    # Define the cost function
    objective_function = ObjectiveFunction(p.objective_fn_params)
    objective_function.add_objective(
        ObstacleAvoidance(params=p.avoid_obstacle_objective,
                          obstacle_map=obstacle_map))
    objective_function.add_objective(
        GoalDistance(params=p.goal_distance_objective, fmm_map=fmm_map))
    objective_function.add_objective(
        AngleDistance(params=p.goal_angle_objective, fmm_map=fmm_map))

    # Define each objective separately
    objective1 = ObstacleAvoidance(params=p.avoid_obstacle_objective,
                                   obstacle_map=obstacle_map)
    objective2 = GoalDistance(params=p.goal_distance_objective,
                              fmm_map=fmm_map)
    objective3 = AngleDistance(params=p.goal_angle_objective, fmm_map=fmm_map)

    # Define cost function for personal state
    objectiveP = PersonalSpaceCost(params=p.personal_space_params)

    # Define a set of positions and evaluate objective
    pos_nk2 = np.array([[[8., 12.5], [8., 16.], [18., 16.5]]],
                       dtype=np.float32)
    heading_nk2 = np.array([[[np.pi / 2.0], [0.1], [0.1]]], dtype=np.float32)
    trajectory = Trajectory(dt=0.1,
                            n=1,
                            k=3,
                            position_nk2=pos_nk2,
                            heading_nk1=heading_nk2)

    # Compute the objective function
    values_by_objective = objective_function.evaluate_function_by_objective(
        trajectory)
    overall_objective = objective_function.evaluate_function(trajectory)

    # Expected objective values
    expected_objective1 = objective1.evaluate_objective(trajectory)
    expected_objective2 = objective2.evaluate_objective(trajectory)
    expected_objective3 = objective3.evaluate_objective(trajectory)
    expected_overall_objective = np.mean(
        expected_objective1 + expected_objective2 + expected_objective3,
        axis=1)
    assert len(values_by_objective) == 3
    assert values_by_objective[0][1].shape == (1, 3)
    assert overall_objective.shape == (1, )
    # assert np.allclose(overall_objective.numpy(), expected_overall_objective.numpy(), atol=1e-2)
    assert np.allclose(overall_objective,
                       expected_overall_objective,
                       atol=1e-2)

    # Use sim_state from main
    sim_state_hist = {}
    sim_state_hist[0] = sim_state
    ps_cost = objectiveP.evaluate_objective(trajectory, sim_state_hist)
    if verbose:
        print("Personal space cost ", ps_cost)
        print("Obstacle avoidance cost", expected_objective1)
        print("Goal distance cost", expected_objective2)
        print("Angle distance cost", expected_objective3)

    # Optionally visualize the traversable and the points on which
    # we compute the objective function
    if plot:
        fig = plt.figure()
        ax = fig.add_subplot(1, 1, 1)
        obstacle_map.render(ax)

        # plot agent start
        ax.plot(pos_nk2[0, :, 0], pos_nk2[0, :, 1], 'r.')
        # plot agent goal
        ax.plot(goal_pos_n2[0, 0], goal_pos_n2[0, 1], 'k*')

        agents = sim_state.get_all_agents()

        for agent_name, agent_vals in agents.items():
            agent_pos3 = agent_vals.get_pos3()  # (x,y,th)
            theta = agent_pos3[2]
            ax.plot(agent_pos3[0], agent_pos3[1], 'g.')

        # plot non ego agents
        fig.savefig('../test_psc_function.png',
                    bbox_inches='tight',
                    pad_inches=0)
Ejemplo n.º 12
0
        # choose among the loaded eps
        sampled_test = np.random.choice(tests)
        sampled_test_dm = create_test_params(test=sampled_test)
        print(sampled_test)
        print(sampled_test_dm)
        print(episodes_config[sampled_test].get('pedestrian_datasets'))
        # get the corresponding map
        map_name = sampled_test_dm.map_name
        # load traversible from map
        resolution, traversible = get_config(socnavdir, map_name)
        obstacle_map_params = DotMap(obstacle_map=SBPDMap,
                                     map_origin_2=[0., 0.],
                                     sampling_thres=2,
                                     plotting_grid_steps=100)
        obstacle_map = SBPDMap(obstacle_map_params,
                               renderer=0,
                               res=resolution,
                               map_trav=traversible)
        # sample freespace start and goal from map
        sample_start = np.squeeze(obstacle_map.sample_point_112(np.random))
        sample_start_orientation = 2 * np.pi * np.random.random()
        sample_start_list = [
            sample_start[0], sample_start[1], sample_start_orientation
        ]

        sample_goal = np.squeeze(obstacle_map.sample_point_112(np.random))
        sample_goal_orientation = 2 * np.pi * np.random.random()
        sample_goal_list = [
            sample_goal[0], sample_goal[1], sample_goal_orientation
        ]

        # if you want to visualize the start and goal