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)
