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])