def __init__(self, obs_map, init_pos, goal, constraints, sub_search=None, conn_8=False, inflation=1.0, out_paths=None, sum_of_costs=False): '''initialize variables (obstacle map, positions, ...) Every action except waiting at the goal configuration incurs cost 1. Waiting at the goal configuration incurs zero cost obs_map - binary obstacle map init_pos - start positions goal - goal position constraints - constraints defining where robot cannot go, should consist of a single tuple, with only the appropriate robots conn_8 - 8-connected grid if True; otherwise 4-connected out_paths - paths of other robots to avoid, if possible without incuring extra cost ''' self.obs_map = obs_map self.goal = tuple(goal) self.init_pos = tuple(init_pos) self.constraints = constraints assert(len(cbs.con_get_robots(constraints)) == 1) # Store as tuple, as just need as dictionary key self.rob_id = cbs.con_get_robots(constraints) self.conn_8 = conn_8 # Sub-policies as weighting functions self.sub_search = sub_search # Graph is stored as dictionary indexed by strings of coord and # tuple self.graph = {} # initialize with "normal" indices; is overwritten if other # priority ordering is applied. Need time one greater than that # of the last constraint to ensure happy self.goal_t = cbs.con_get_max_time(self.constraints) + 1 self.max_t = self.goal_t if out_paths is not None: self.max_t = max(self.goal_t, len(out_paths)) self.inflation = inflation self.out_paths = out_paths self.col_check = workspace_graph.Edge_Checker() # Generate policy for each robot if self.sub_search is None: self.sub_search = { self.rob_id: workspace_graph.Astar_Graph(obs_map, goal, self.conn_8)} elif not self.rob_id in self.sub_search: self.sub_search[self.rob_id] = workspace_graph.Astar_Graph( obs_map, goal, self.conn_8)
def test_astar_threshold_neighbors(self): """Tests that cost threshold neighbors work properly for epea*""" graph = workspace_graph.Astar_Graph(self.world_descriptor, (2, 2), connect_8=True) neibs = graph.get_limited_offset_neighbors((4, 2), 0) self.assertTrue(len(neibs) == 3) for t in [(0, (3, 2)), (0, (3, 3)), (0, (3, 1))]: self.assertTrue(t in neibs) neibs = graph.get_limited_offset_neighbors((4, 2), 1) self.assertTrue(len(neibs) == 6) for t in [(0, (3, 2)), (0, (3, 3)), (0, (3, 1)), (1, (4, 3)), (1, (4, 2)), (1, (4, 1))]: self.assertTrue(t in neibs) neibs = graph.get_limited_offset_neighbors((4, 2), 2) self.assertTrue(len(neibs) == 9) # check minimum offset functionality neibs = graph.get_limited_offset_neighbors((4, 2), 1, min_offset=1) self.assertTrue(len(neibs) == 3) for t in [(1, (4, 3)), (1, (4, 2)), (1, (4, 1))]: self.assertTrue(t in neibs) neibs = graph.get_limited_offset_neighbors((4, 2), 2, min_offset=1) self.assertTrue(len(neibs) == 6) for t in [(1, (4, 3)), (1, (4, 2)), (1, (4, 1)), (2, (5, 3)), (2, (5, 2)), (2, (5, 1))]: self.assertTrue(t in neibs)
def test_astar_graph_no_obstacles(self): """Tests that astar graph performs correctly without obstacles""" graph = workspace_graph.Astar_Graph(self.world_descriptor, (5, 5)) self.assertTrue(graph.get_step((5, 5)) == (5, 5)) self.assertTrue(graph.get_cost((5, 5)) == 0) self.assertTrue(graph.get_step((5, 4)) == (5, 5)) self.assertTrue(graph.get_cost((5, 4)) == 1) self.assertTrue(graph.get_cost((0, 0)) == 10)
def test_astar_graph_obstacles(self): """Tests that astar graph performs correctly with simple obstacles""" self.world_descriptor[1][0] = 1 graph = workspace_graph.Astar_Graph(self.world_descriptor, (2, 0)) self.assertTrue(graph.get_step((0, 0)) == (0, 1)) self.assertTrue(graph.get_cost((0, 0)) == 4) self.assertTrue(graph.get_step((1, 1)) == (2, 1)) self.assertTrue(graph.get_cost((1, 1)) == 2)
def test_astar_graph_obstacles_8conn(self): """Tests that 8-conencted astar graph performs correctly with simple obstacles""" self.world_descriptor[1][0] = 1 graph = workspace_graph.Astar_Graph(self.world_descriptor, (2, 0), connect_8=True) self.assertTrue(graph.get_step((0, 0)) == (1, 1)) self.assertTrue(graph.get_cost((0, 0)) == 2) self.assertTrue(graph.get_step((1, 1)) == (2, 0)) self.assertTrue(graph.get_cost((1, 1)) == 1)
def __init__(self, obs_map, goals, sub_search=None, conn_8=False, meta_agents=False, merge_thresh=1, meta_planner='op_decomp', sum_of_costs=False, return_cost=False): """ obs_map - list of lists specifying obstacle positions, as per workspace graph goals - [p1, p2, .., pn] list of goal positions sub_search - specifies individual robot planners conn_8 - whether to use an 8-connected graph instead of a 4-connected graph meta_agents - whether to use meta_agents merge_thresh - number of collisions before merging meta_planner - which coupled planner to use, options are 'od_rmstar' and 'epermstar' sum_of_costs - use the sum of costs formulation return_cost - if True, return both the path and its cost """ self.obs_map = obs_map self.goals = tuple(map(tuple, goals)) self.conn_8 = conn_8 self.meta_agents = meta_agents self.merge_thresh = merge_thresh self.return_cost = return_cost # Cache result paths for reuse/cost calculation # store in the form (cost, path) with the key being the sorted # tuple of constraints (sorted to ensure duplicates are found) self.paths = {} # Catch duplicate nodes, as we never need to revisit these, using a # dictionary, for easy constant time inclusion tests self.closed = {} self.meta_planner = meta_planner self.sum_of_costs = sum_of_costs assert self.meta_planner in [ None, 'od_rmstar', 'op_decomp', 'epea', 'epermstar' ] # Counts the number of collisions for determining when to merge self.col_count = [[0 for i in xrange(len(goals))] for j in xrange(len(goals))] if sub_search == None: self.sub_search = {} for i in xrange(len(self.goals)): self.sub_search[tuple([i])] = workspace_graph.Astar_Graph( obs_map, goals[i], connect_8=conn_8) else: self.sub_search = sub_search # Most of the time is taken by computing constraints. A lot of # checks will be duplicates, so maintain a cache self.solution_validation_results = {}
def test_astar_threshold_neighbors_obstacles(self): """Tests that cost threshold neighbors work properly for epea* when obstacles are present""" self.world_descriptor[2][2] = 1 graph = workspace_graph.Astar_Graph(self.world_descriptor, (1, 2), connect_8=True) neibs = graph.get_limited_offset_neighbors((3, 2), 0) self.assertTrue(len(neibs) == 2) for t in [(0, (2, 3)), (0, (2, 1))]: self.assertTrue(t in neibs) neibs = graph.get_limited_offset_neighbors((3, 2), 1) self.assertTrue(len(neibs) == 5) for t in [(0, (2, 3)), (0, (2, 1)), (1, (3, 3)), (1, (3, 2)), (1, (3, 1))]: self.assertTrue(t in neibs)
def test_astar_graph_no_obstacles_8conn(self): """Tests that 8-connected astar graph performs correctly without obstacles""" graph = workspace_graph.Astar_Graph(self.world_descriptor, (5, 5), connect_8=True) for i in (-1, 0, 1): for j in (-1, 0, 1): coord = (5 + i, 5 + j) if coord == (5, 5): self.assertTrue(graph.get_step(coord) == (5, 5)) self.assertTrue(graph.get_cost(coord) == 0) else: self.assertTrue(graph.get_step(coord) == (5, 5)) self.assertTrue(graph.get_cost(coord) == 1) self.assertTrue(graph.get_step((0, 0)) == (1, 1)) self.assertTrue(graph.get_cost((0, 0)) == 5)
def gen_policy_planners(self, sub_search, world, goals): """Creates the sub-planners and necessary policy keys. This is because pretty much every sub-planner I've made requires adjusting the graph used to create the policies and passing around dummy sub_searches side effects to generate self._sub_search and self.policy_keys """ self.policy_keys = tuple([(i, ) for i in self._rob_id]) self._sub_search = sub_search if self._sub_search is None: self._sub_search = {} # Wrapping the robot number in a tuple so we can use the same # structure for planners for compound robots for dex, key in enumerate(self.policy_keys): self._sub_search[key] = workspace_graph.Astar_Graph( world, goals[dex], connect_8=False, makespan=False)
def test_priority_max_t(self): """Tests that the time limit works as expected""" graph = workspace_graph.Priority_Graph( workspace_graph.Astar_Graph(self.world_descriptor, (5, 5), connect_8=True)) graph.set_max_t(2) for t in [0, 1, 2]: for i in (-1, 0, 1): for j in (-1, 0, 1): coord = (5 + i, 5 + j, t) if coord == (5, 5, t): self.assertTrue(graph.get_step(coord) == (5, 5, min(2, t + 1))) self.assertTrue(graph.get_cost(coord) == 0) else: self.assertTrue(graph.get_step(coord) == (5, 5, min(2, t + 1))) self.assertTrue(graph.get_cost(coord) == 1) self.assertTrue(graph.get_cost((0, 0, 0)) == 5)
def __init__(self, obs_map, init_pos, goal, constraints, out_paths=None, sub_search=None, conn_8=False, inflation=1, prune_paths=True): '''initialize variables (obstacle map, positions, ...) obs_map - binary obstacle map init_pos - (x,y,t) start position and time goal - goal position constraints - constraints defining where robot cannot go, should consist of a single tuple, with only the appropriate robots out_paths - paths of out of group robots to treat as soft constraints conn_8 - 8-connected grid if True; otherwise 4-connected only_init - will only find paths to the original initial position (pruning paths which cannot reach the specified position in time). If false, will not perform pruning ''' self.obs_map = obs_map self.init_pos = tuple(init_pos) self.constraints = constraints self.out_paths = out_paths assert(len(cbs.con_get_robots(constraints)) == 1) # Store as tuple, as just need as dictionary key self.rob_id = cbs.con_get_robots(constraints) self.conn_8 = conn_8 # Sub-policies as weighting functions self.sub_search = sub_search # Graph is stored as dictionary indexed by strings of coord and # tuple self.graph = {} # initialize with "normal" indices; is overwritten if other # priority # ordering is applied. Need time one greater than that of the # last constraint to ensure happy self.max_t = 0 self.con_max_t = 0 if self.constraints is not None: self.max_t = cbs.con_get_max_time(self.constraints) + 1 if self.max_t == 1: # only possible if the constraint is placed at time 0, # which is meaningless, and will only happen if the # planner is passed an empty set of constraints self.max_t = 0 self.con_max_t = self.max_t if self.out_paths is not None: # Set max_t one greater than len(self.out_paths) to make self.max_t = max(self.max_t, len(self.out_paths)) self.inflation = inflation self.col_check = workspace_graph.Edge_Checker() # Generate policy for each robot, for moving towards the initial # configuration self.policy_key = self.rob_id + ('backpolicy', ) if self.sub_search is None: self.sub_search = { self.policy_key: workspace_graph.Back_Priority_Graph( workspace_graph.Astar_Graph( obs_map, self.init_pos[:2], self.conn_8), max_t=self.max_t)} # elif not self.policy_key in self.sub_search: elif not self.policy_key in self.sub_search: # Don't have the individual robot policy here self.sub_search[self.policy_key] = \ workspace_graph.Back_Priority_Graph( workspace_graph.Astar_Graph( obs_map, self.init_pos[:2], self.conn_8)) self.sub_search[self.policy_key].prune_paths = prune_paths assert (self.init_pos[:2] == self.sub_search[self.policy_key].astar_policy.goal) # Need persisitant open list for repeated planning, add the goal # node now, as it will be the initial search posotion # Convert goal to goal in space time self.goal = tuple(goal) + (self.con_max_t, ) goal_node = self.get_node(self.goal) goal_node.cost = (0, 0) goal_node.back_ptr = goal_node self.open_list = SortedCollection.SortedCollection( [goal_node], key=lambda x: [ -x.cost[0] - x.h * self.inflation, -x.cost[1]]) # potentially need to depart the goal at multiple different # times. Including all possible departure times will be a bit # more efficient than forcing exploration prev = goal_node for t in xrange(self.con_max_t + 1, self.max_t + 1): t_coord = tuple(goal) + (t, ) t_node = self.get_node(t_coord) t_node.cost = (0, 0) t_node.back_ptr = t_node prev.back_ptr = t_node prev = t_node self.open_list.insert(t_node)
def test_astar_graph_get_makespan_edge_cost(self): """Tests to ensure that edge cost function works correctly""" graph = workspace_graph.Astar_Graph(self.world_descriptor, (5, 5), makespan=True) self.assertTrue(graph.get_edge_cost((5, 5), (5, 6)) == 1) self.assertTrue(graph.get_edge_cost((5, 5), (5, 5)) == 1)
def gen_coverage_world(size, num_bots, obs_density, connect_8=False): """Generates a random, square world of size cells per side. size - cells per side num_bots - number of robots obs_density - liklihood of given cell being an obstacle connect_8 - whether to be built for 8 connected graph instead of 4 connected returns - [obs_map, init_pos, goals]""" print 'gen_coverage' if obs_density > 1 or obs_density < 0: raise ValueError('Improper obstacle obs_density') def foo(): if random.uniform(0, 1) < obs_density: return 1 return 0 obs_map = [[foo() for i in xrange(size)] for j in xrange(size)] #Generate the initial positions of the robots init_pos = [] goals = [] # Keeps track of how many times tried to generate valid initial, # goal positions counter = 0 #Number of times to try for a given obstacle arrangement max_tries = 100 * num_bots while len(init_pos) < num_bots: #Generate possible initial and goal positions ip = (random.randint(0, size - 1), random.randint(0, size - 1)) g = (random.randint(0, size - 1), random.randint(0, size - 1)) if g == ip or obs_map[g[0]][g[1]] == 1 or obs_map[ip[0]][ip[1]] == 1: # Don't allow the goal and initial positions to be in the # same spot or in an obstacle if counter > max_tries: #obs_map is too hard, try a new one init_pos = [] goals = [] obs_map = [[foo() for i in range(size)] for j in range(size)] counter = 0 continue counter += 1 continue if g in goals or ip in init_pos: #Don't allow duplicate elements if counter > max_tries: #obs_map is too hard, try a new one init_pos = [] goals = [] obs_map = [[foo() for i in range(size)] for j in range(size)] counter = 0 continue counter += 1 continue #Check if we can generate a valid path graph = workspace_graph.Astar_Graph(obs_map, g, connect_8=connect_8) tpath = graph.get_step(ip) if tpath == None or tpath == []: #No individual path, so skip if counter > max_tries: #obs_map is too hard, try a new one init_pos = [] goals = [] obs_map = [[foo() for i in range(size)] for j in range(size)] counter = 0 continue counter += 1 continue #initial position, goal pair is valid init_pos.append(ip) goals.append(g) return obs_map, tuple(init_pos), tuple(goals)
def gen_dragon_age_map(filename, num_bots): """Generates a random set of initial and goal configurations for a dragonage map connect_8 - whether to be built for 8 connected graph instead of 4 connected returns: [obs_map, init_pos, goals] """ obs_map = import_dragon_age_map(filename) sizex = len(obs_map) sizey = len(obs_map[1]) # Generate the initial positions of the robots init_pos = [] goals = [] # Keeps track of how many times tried to generate valid initial, # goal positions counter = 0 # Number of times to try for a given obstacle arrangement max_tries = 100 * num_bots while len(init_pos) < num_bots: # Generate possible initial and goal positions ip = (random.randint(0, sizex - 1), random.randint(0, sizey - 1)) g = (random.randint(0, sizex - 1), random.randint(0, sizey - 1)) if obs_map[g[0]][g[1]] == 1 or obs_map[ip[0]][ip[1]] == 1: # Don't allow the goal and initial positions to be in an # obstacle if counter > max_tries: # obs_map is too hard, try a new one init_pos = [] goals = [] raise ValueError('Couldn\'t Create conditions') counter += 1 continue if g in goals or ip in init_pos: # Don't allow duplicate elements if counter > max_tries: # obs_map is too hard, try a new one init_pos = [] goals = [] obs_map = [[foo() for i in range(sizey)] for j in range(sizex)] counter = 0 continue counter += 1 continue # Check if we can generate a valid path graph = workspace_graph.Astar_Graph(obs_map, g) tpath = graph.get_step(ip) if tpath == None or tpath == []: # No individual path, so skip if counter > max_tries: # obs_map is too hard, try a new one init_pos = [] goals = [] obs_map = [[foo() for i in range(sizey)] for j in range(sizex)] counter = 0 continue counter += 1 continue # initial position, goal pair is valid init_pos.append(ip) goals.append(g) return obs_map, tuple(init_pos), tuple(goals)