def test_fmm_map():
from utils.fmm_map import FmmMap
# Create a grid and a function within that grid
scale = 0.5
grid_size = np.array([10, 10])
goal_position_n2 = np.array([[2.5, 2.5]])
fmm_map = FmmMap.create_fmm_map_based_on_goal_position(goal_positions_n2=goal_position_n2,
map_size_2=grid_size,
dx=scale
)
# Let's have a bunch of points to test the fmm distance and angle map
test_positions = tf.constant([[[2.0, 3.0], [2.5, 3.0], [3.0, 3.0], [3.0, 2.0], [2.5, 2.0], [2.0, 2.0]]],
dtype=tf.float32)
# Predicted distance and angles
distances = fmm_map.fmm_distance_map.compute_voxel_function(test_positions, invalid_value=-100.)
angles = fmm_map.fmm_angle_map.compute_voxel_function(test_positions, invalid_value=-100.)
# The expected distance is dist1 as defined below. However, due to the numerical issues, the actual computed
# distance turns out to be a larger (0.6 in this case).
dist1 = scale * np.sqrt(2.) - 0.5*scale*np.cos(np.pi*45./180.)
dist1 = 0.60
expected_distances = np.array([dist1, 0.25, dist1, dist1, 0.25, dist1])
expected_angles = (np.pi/180) * np.array([-45., -90., -135., 135., 90., 45.])
assert np.sum(abs(expected_distances - distances) <= 0.01) == 6
assert np.sum(abs(expected_angles - angles) <= 0.01) == 6
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_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
def _init_fmm_map(self, goal_pos_n2=None):
p = self.params
self.obstacle_occupancy_grid = self.obstacle_map.create_occupancy_grid_for_map()
if goal_pos_n2 is None:
goal_pos_n2 = self.goal_config.position_nk2()[0]
return FmmMap.create_fmm_map_based_on_goal_position(
goal_positions_n2=goal_pos_n2,
map_size_2=np.array(p.obstacle_map_params.map_size_2),
dx=p.obstacle_map_params.dx,
map_origin_2=p.obstacle_map_params.map_origin_2,
mask_grid_mn=self.obstacle_occupancy_grid)
def _initialize_fmm_map(self):
"""
Initialize an FMM Map where 0 level set encodes the obstacle
positions.
"""
p = self.p
occupied_xy_m2 = np.array(np.where(self.occupancy_grid_map)).T
occupied_xy_m2 = occupied_xy_m2[:, ::-1]
occupied_xy_m2_world = self._map_to_point(occupied_xy_m2)
self.fmm_map = FmmMap.create_fmm_map_based_on_goal_position(
goal_positions_n2=occupied_xy_m2_world,
map_size_2=p.map_size_2,
dx=p.dx,
map_origin_2=p.map_origin_2,
mask_grid_mn=None)
def _init_fmm_map(self, goal_pos_n2=None, params=None):
if(params is None):
params = self.params
obstacle_map = self.obstacle_map
obstacle_occupancy_grid = obstacle_map.create_occupancy_grid_for_map()
if goal_pos_n2 is None:
goal_pos_n2 = self.goal_config.position_nk2()[0]
self.fmm_map = \
FmmMap.create_fmm_map_based_on_goal_position(goal_positions_n2=goal_pos_n2,
map_size_2=np.array(
self.obstacle_map.get_map_size_2()),
dx=self.obstacle_map.get_dx(),
map_origin_2=self.obstacle_map.get_map_origin_2(),
mask_grid_mn=obstacle_occupancy_grid)
Agent._update_fmm_map(self)
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)
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)