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() 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()
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_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)
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)
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)
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)
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 import matplotlib.pyplot as plt fig = plt.figure() ax = fig.add_subplot(1, 1, 1) obstacle_map.render(ax) ax.plot(sample_start[0], sample_start[1], 'r.') ax.plot(sample_goal[0], sample_goal[1], 'r*') plt.show() break new_test_name = sampled_test + str(i) random_episodes_config[new_test_name] = episodes_config[sampled_test] random_episodes_config[new_test_name]['robot_start_goal'] = \ str([sample_start_list, sample_goal_list]) # format and print generated episode config randomeps_filename = 'randomeps_N' + str(num_episodes_sample) + '_R' + str( seed) + '.cfg' with open(randomeps_filename, 'w') as configfile: random_episodes_config.write(configfile)