def test_immediate_neighbor(self):
for i in range(100):
with self.subTest(i=i):
start_position = np.array(
[np.random.randint(1, 7),
np.random.randint(1, 7)])
p = np.random.randint(0, 1)
goal_position = np.power(-1, np.random.randint(
1, 2)) * np.array([p, 1 - p]) + start_position
#print(goal_position,start_position)
obstacles = []
start_position = global_defs.Point2D(start_position[0],
start_position[1])
goal_position = global_defs.Point2D(goal_position[0],
goal_position[1])
# begin_time = time.time()
a = astar(start_position, goal_position, obstacles, 10, False)
# time_array.append(time.time() - begin_time)
res = a.find_minimumpath()
a.print_path(res)
# pdb.set_trace()
self.assertTrue(
utils.is_neighbor(start_position, goal_position))
self.assertEqual(res[0], 1)
def test_immediate_neighbor(self):
"""
Testing ASTAR when the start and end are immediate neighbors of each other. Edge Case.
:return:
"""
for i in range(100):
with self.subTest(i=i):
start_position = np.array(
[np.random.randint(1, 7),
np.random.randint(1, 7)])
p = np.random.randint(0, 1)
goal_position = np.power(-1, np.random.randint(
1, 2)) * np.array([p, 1 - p]) + start_position
#print(goal_position,start_position)
obstacles = []
start_position = (start_position[0], start_position[1])
goal_position = (goal_position[0], goal_position[1])
# begin_time = time.time()
a = astar(start_position, goal_position, obstacles, 10, False)
# time_array.append(time.time() - begin_time)
res = a.find_minimumpath()
# pdb.set_trace()
start_position = global_defs.Point2D(start_position[0],
start_position[1])
goal_position = global_defs.Point2D(goal_position[0],
goal_position[1])
self.assertTrue(
utils.is_neighbor(start_position, goal_position))
self.assertEqual(res[0], 1)
self.assertTrue(len(res[1]) == 2)
def generate_initial_conditions(n_objects, n_agents):
#First generate objects.
grid_size = global_defs.GRID_SIZE
angfactor = 0.9
radius = int((grid_size // 2) * angfactor)
print(radius)
phase = np.random.random() * (np.pi / 2)
#Generate n_objects-1 offset angles.
#spread_factor = 1/8
spread_factor = 80
offset_angles_deviation = np.random.normal(0, 1 / spread_factor,
(n_objects - 1, 1))
angular_positions = []
angular_positions.append(phase)
curr_angular_position = copy.deepcopy(phase)
for i in range(n_objects - 1):
curr_angular_position = curr_angular_position + (
2 * np.pi / (n_objects)) + offset_angles_deviation[i]
angular_positions.append(copy.deepcopy(curr_angular_position[0]))
#Now convert from angular positions to integer positions.
object_positions = []
for idx in range(n_objects):
ang_pos = angular_positions[idx]
pos = global_defs.Point2D(int(radius * np.cos(ang_pos)),
int(radius * np.sin(ang_pos)))
pos += ((grid_size - 1) // 2, (grid_size - 1) // 2)
object_positions.append(pos)
#Now we need agent positions.
agent_positions = []
agent_ang_pos = np.array([phase + np.pi / 2, phase + np.pi * (3 / 2)])
offset_angles_deviation = np.random.normal(0, spread_factor, 2)
agent_ang_pos += offset_angles_deviation
rfactor = 0.2
rad = radius * rfactor
agent_pos = [
global_defs.Point2D(int(rad * np.cos(angpos)),
int(rad * np.sin(angpos)))
for angpos in agent_ang_pos
]
agent_pos = [
apos + ((grid_size - 1) // 2, (grid_size - 1) // 2)
for apos in agent_pos
]
return (object_positions, agent_pos)
def getDifferenceToPoint(pos_begin, pos_end):
"""
Difference in the 2D coordinates in the toroidal space
:param pos1: defs.Point2d tuple
:param pos2: defs.Point2d tuple
:return: defs.Point2d tuple
"""
delta = defs.Point2D(0, 0)
maxDx = 0.5 * (dim)
maxDy = 0.5 * (dim)
delta.x = pos_end.x - pos_begin.x
delta.y = pos_end.y - pos_begin.y
if delta.x > maxDx:
delta.x -= dim
if delta.x < (-maxDx):
delta.x += dim
if delta.y > maxDy:
delta.y -= dim
if delta.y < (-maxDy):
delta.y += dim
return delta
def test_with_obstacles_noroute(self):
time_array = []
for i in range(100):
with self.subTest(i=i):
start_position = np.array([1, np.random.randint(0, 7)])
goal_position = np.array([7, np.random.randint(0, 7)])
obstacle_array = np.array([[4, i] for i in range(0, 11)])
obstacles = []
for obstacle in obstacle_array:
if np.all(obstacle == start_position) or np.all(
obstacle == goal_position):
pass
else:
obstacles.append(
global_defs.Point2D(obstacle[0], obstacle[1]))
# begin_time = time.time()
a = astar(start_position, goal_position, obstacles, 10, False)
# time_array.append(time.time() - begin_time)
res = a.find_minimumpath()
a.print_path(res)
for node in res[1][:-1]:
self.assertFalse(node.is_obstacle)
self.assertEqual(res[0], 0)
def test_with_obstacles_noroute(self):
"""
Testing with no route to the end.
:return:
"""
time_array = []
for i in range(100):
with self.subTest(i=i):
start_position = np.array([1, np.random.randint(0, 7)])
goal_position = np.array([7, np.random.randint(0, 7)])
obstacle_array = np.array([[4, i] for i in range(0, 10)])
start_position = tuple(start_position)
goal_position = tuple(goal_position)
obstacles = []
for obstacle in obstacle_array:
if np.all(obstacle == start_position) or np.all(
obstacle == goal_position):
pass
else:
obstacles.append((obstacle[0], obstacle[1]))
# begin_time = time.time()
a = astar(start_position, goal_position, obstacles, 10)
# time_array.append(time.time() - begin_time)
res = a.find_minimumpath()
obstacle_set = [
global_defs.Point2D(obstacle[0], obstacle[1])
for obstacle in obstacles
]
self.assertEqual(res[0], 0) #Path not found.
def test_neighborhood_check(self):
"""
Test for is_neighbor function in utils.py
:return:
"""
#positive examples
points = []
for x in range(0, 10):
for y in range(0, 10):
points.append(global_defs.Point2D(x, y))
neighbors = []
for point in points:
curr_neighbors = []
for move in global_defs.MOVES:
curr_neighbors.append(point + move)
neighbors.append(curr_neighbors)
not_neighbors = []
for point in points:
curr_not_neighbors = []
for move in global_defs.MOVES:
curr_not_neighbors.append(point + 2 * (move) + 2)
not_neighbors.append(curr_not_neighbors)
for neighbor_set, point in zip(neighbors, points):
for neighbor in neighbor_set:
self.assertTrue(utils.is_neighbor(neighbor, point))
self.assertTrue(utils.is_neighbor(point, neighbor))
for neighbor_set, point in zip(not_neighbors, points):
for neighbor in neighbor_set:
#pdb.set_trace()
self.assertFalse(utils.is_neighbor(neighbor, point))
self.assertFalse(utils.is_neighbor(point, neighbor))
def test_agent_respond_sanity(self):
"""
Testing that agent probabilites always sum to one and that the agent always takes actions within bounds.
:return:
"""
for i in range(1000):
with self.subTest(i=i):
random_pos_array = np.random.randint(
0, 10, (20, 2)) #Generating 20 random locations
random_pos_list = [
global_defs.Point2D(ele[0], ele[1])
for ele in random_pos_array
]
a = agent_random.agent_random(
global_defs.Point2D(random_pos_list[0][0],
random_pos_list[0][1]))
allPos = random_pos_list
myInd = 0
loadIndices = range(4, 8)
random_observation = global_defs.obs(allPos, myInd,
loadIndices)
(action_probs, action_idx) = a.respond(random_observation)
self.assertTrue(len(action_probs) == 6)
np.testing.assert_approx_equal(np.sum(action_probs), 1)
self.assertTrue(action_idx < 6)
print(action_probs, action_idx)
if action_idx == global_defs.Actions.LOAD:
is_neighbor = False
for loadidx in (loadIndices):
loadPos = allPos[loadidx]
is_neighbor = is_neighbor or utils.is_neighbor(
loadPos, a.pos)
self.assertTrue(is_neighbor)
is_neighbor = False
for loadidx in (loadIndices):
loadPos = allPos[loadidx]
is_neighbor = is_neighbor or utils.is_neighbor(
loadPos, a.pos)
if is_neighbor:
msg_str = "Is neighbor {} {}".format(
a.pos, [allPos[i] for i in range(4, 8)])
self.assertTrue(action_probs[-1] > 0, msg=msg_str)
def test_with_obstacles(self):
"""
Testing that ASTAR should work even when there are obstacles.
The start and end are very far away, so we are not testing edge cases.
:return:
"""
time_array = []
for i in range(100):
with self.subTest(i=i):
start_position = np.array([8, np.random.randint(0, 7)])
goal_position = np.array([np.random.randint(0, 7), 8])
start_position = tuple(start_position)
goal_position = tuple(goal_position)
obstacle_array = np.random.randint(0, 10, (10, 2))
obstacles = []
for obstacle in obstacle_array:
if np.all(obstacle == start_position) or np.all(
obstacle == goal_position):
pass
else:
obstacles.append((obstacle[0], obstacle[1]))
# begin_time = time.time()
#ipdb.set_trace()
a = astar(start_position, goal_position, obstacles, 10)
# time_array.append(time.time() - begin_time)
res = a.find_minimumpath()
obstacle_set = [
global_defs.Point2D(obstacle[0], obstacle[1])
for obstacle in obstacles
]
self.assertEqual(res[0], 1)
#print(obstacle_array)
for node in res[1][:-1]:
if (global_defs.Point2D(node[0], node[1]) in obstacle_set):
print('----' +
str(global_defs.Point2D(node[0], node[1])))
self.assertFalse(
global_defs.Point2D(node[0], node[1]) in obstacle_set)
def test_agent_tp_1_respond(self):
random_pos_array = np.random.randint(
0, 10, (20, 2)) #Generating 20 random locations
random_pos_list = [
global_defs.Point2D(ele[0], ele[1]) for ele in random_pos_array
]
a = agent_lifter.agent_lifter(
global_defs.Point2D(random_pos_list[0][0], random_pos_list[0][1]),
1)
allPos = random_pos_list
myInd = 0
loadIndices = range(4, 8)
random_observation = global_defs.obs(allPos, myInd, loadIndices)
(action_probs, action_idx) = a.respond(random_observation)
self.assertTrue(len(action_probs) == 6)
np.testing.assert_approx_equal(np.sum(action_probs), 1)
self.assertTrue(action_idx < 6)
def __init__(self, prey_loc, agents_list, visualize):
self.agents = []
self.visualize = visualize
self.terminal = False
self.init_add_prey(prey_loc)
self.init_add_agents(agents_list)
if self.visualize:
allPos = [self.prey_loc] + [self.build_agentPositionArray()]
obs = defs.obs(self.build_agentPositionArray(), -1, 0)
self.init_visualization(obs)
self.center_point = defs.Point2D(0, 0)
self.center_point.x = int(config.DIMENSIONS / 2)
self.center_point.y = int(config.DIMENSIONS / 2)
def movePosition(position, movement):
"""
Function to calculate how motion is performed in the 2D space
:param position: defs.Point2d tuple
:param movement: defs.Point2d tuple
:return: defs.Point2d tuple of the final position
"""
newPosition = defs.Point2D(0, 0)
newPosition.x = (position.x + movement.x) % dim
newPosition.y = (position.y + movement.y) % dim
if (newPosition.x < 0):
newPosition.x += dim
if (newPosition.y < 0):
newPosition.y += dim
return newPosition
def test_diffType(self):
n_tests = 0
total_test = 100
for i in range(total_test):
#Agents: Same type.
#Object: Not on edges.
#Distance: target_dist steps.
object_location = np.array(random.sample(range(1, 8), 4)[:2])
#Put agents target_dist positions away.
target_dist = 6
displacement = np.random.randint(-target_dist, target_dist, (2, 2))
displacement[:, 1] = random.choice(
[-1, 1]) * (target_dist - np.abs(displacement[:, 0]))
a1_pos = (object_location[0] + displacement[0][0],
object_location[1] + displacement[0][1])
a2_pos = (object_location[0] + displacement[1][0],
object_location[1] + displacement[1][1])
if not (utils.check_within_boundaries(a1_pos)
and utils.check_within_boundaries(a2_pos)):
continue
else:
n_tests += 1
print("-----------Test Iter: {}-------------".format(i))
with self.subTest(i=i, msg='Experiment {}'.format(i)):
self.assertTrue(
np.sum(np.abs(object_location -
a1_pos)) == target_dist)
self.assertTrue(
np.sum(np.abs(object_location -
a2_pos)) == target_dist)
a1 = agent_lifter.agent_lifter(a1_pos, 1)
a2 = agent_lifter.agent_lifter(a2_pos, 2)
object_location = global_defs.Point2D(
object_location[0], object_location[1])
env = environment.environment(
10, [object_location,
global_defs.Point2D(0, 0)], False)
env.register_agent(a1)
env.register_agent(a2)
n_steps = 0
print("******** Test 2 **************8")
print("Target Distance: {}".format(target_dist))
print("Object location {}".format(object_location))
while (not env.is_terminal and n_steps < 100):
is_terminal, reward = env.step()
n_steps += 1
if is_terminal:
print(n_steps)
print("A1 {}".format(a1))
print("A2 {}".format(a2))
print("Env {}".format(env))
print("Reward {} ".format(reward))
print("******************************")
#self.assertTrue(reward==0)
print("n_tests {} total_tests {}".format(n_tests, total_test))
def _test_sameType_copy_middleStep(self):
n_tests = 0
total_test = 100
for i in range(total_test):
#Agents: Same type.
#Object: Not on edges.
#Distance: target_dist steps.
object_location = np.array(random.sample(range(1, 8), 2)[:2])
#Put agents target_dist positions away.
target_dist = 6
displacement = np.random.randint(-target_dist, target_dist, (2, 2))
displacement[:, 1] = random.choice(
[-1, 1]) * (target_dist - np.abs(displacement[:, 0]))
a1_pos = (object_location[0] + displacement[0][0],
object_location[1] + displacement[0][1])
a2_pos = (object_location[0] + displacement[1][0],
object_location[1] + displacement[1][1])
if not (utils.check_within_boundaries(a1_pos)
and utils.check_within_boundaries(a2_pos)):
continue
else:
n_tests += 1
print("-----------Test Iter: {}-------------".format(i))
with self.subTest(i=i, msg='Experiment {}'.format(i)):
self.assertTrue(
np.sum(np.abs(object_location -
a1_pos)) == target_dist)
self.assertTrue(
np.sum(np.abs(object_location -
a2_pos)) == target_dist)
a1 = agent_lifter.agent_lifter(a1_pos, 1)
a2 = agent_lifter.agent_lifter(a2_pos, 1)
object_location = global_defs.Point2D(
object_location[0], object_location[1])
env = environment.environment(
10, [object_location,
global_defs.Point2D(0, 0)], False)
env.register_agent(a1)
env.register_agent(a2)
n_steps = 0
#print("Target Distance: {}".format(target_dist))
#print("Object location {}".format(object_location))
print("A1 {}".format(a1.pos))
print("A2 {}".format(a2.pos))
_, _ = env.step()
_, _ = env.step()
new_env = env.__copy__()
while (not env.is_terminal and n_steps < 10):
is_terminal, reward = env.step()
is_terminal2, reward2 = new_env.step()
self.assertEqual(is_terminal, is_terminal2)
self.assertEqual(reward, reward2)
s1 = env.__getstate__()
s2 = new_env.__getstate__()
try:
self.assertTrue(utils.compare_env_states(s1, s2))
except:
pdb.set_trace()
n_steps += 1
#print(n_steps)
#print("A1 {}".format(a1.pos))
#print("A2 {}".format(a2.pos))
#if is_terminal:
#print("Reward {}".format(reward))
if is_terminal:
print("Terminal Reward {}".format(reward))
print("N_iters {}".format(n_steps))
def copy_lifter_to_random(a1, a2):
a2.pos = global_defs.Point2D(a1.pos[0], a1.pos[1])
a2.name += ('_copy_' + a1.name)
# Put agents target_dist positions away.
target_dist = 6
displacement = np.random.randint(-target_dist, target_dist, (2, 2))
displacement[:, 1] = random.choice(
[-1, 1]) * (target_dist - np.abs(displacement[:, 0]))
a1_pos = (object_location[0] + displacement[0][0],
object_location[1] + displacement[0][1])
a2_pos = (object_location[0] + displacement[1][0],
object_location[1] + displacement[1][1])
a1 = agent_lifter.agent_lifter(a1_pos, 1)
a2 = agent_lifter.agent_lifter(a2_pos, 1)
object_location = global_defs.Point2D(object_location[0], object_location[1])
env = environment.environment(
10, [object_location, global_defs.Point2D(0, 0)], True)
env.register_agent(a1)
env.register_agent(a2)
n_steps = 0
print("A1 {}".format(a1.pos))
print("A2 {}".format(a2.pos))
while (not env.is_terminal and n_steps < 10):
is_terminal, reward = env.step()
n_steps += 1
def init_add_prey(self, prey_loc):
self.prey_loc = defs.Point2D(prey_loc[0], prey_loc[1])