def print_results(self, node):
print("\n Found a solution! \n")
CPU_time = timer.time() - self.start_time
print("CPU time (s): {:.2f}".format(CPU_time))
print("Sum of costs: {}".format(get_sum_of_cost(node['paths'])))
print("Expanded nodes: {}".format(self.num_of_expanded))
print("Generated nodes: {}".format(self.num_of_generated))
result_file = open("results.csv", "a", buffering=1)
cost = get_sum_of_cost(node['paths'])
result_file.write("{},{},{},{}\n".format(cost, CPU_time,
self.num_of_expanded,
self.num_of_generated))
result_file.close()
def find_solution(self):
""" Finds paths for all agents from their start locations to their goal locations."""
start_time = timer.time()
result = []
constraints = []
for i in range(self.num_of_agents): # Find path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, constraints)
if path is None:
raise BaseException('No solutions')
result.append(path)
##############################
# Task 2: Add constraints here
# Useful variables:
# * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)]
# * self.num_of_agents has the number of total agents
# * constraints: array of constraints to consider for future A* searches
##############################
self.CPU_time = timer.time() - start_time
print("\n Found a solution! \n")
print("CPU time (s): {:.2f}".format(self.CPU_time))
print("Sum of costs: {}".format(get_sum_of_cost(result)))
print(result)
return result
def find_solution(self):
""" Finds paths for all agents from their start locations to their goal locations."""
start_time = timer.time()
result = []
constraints = []
# constraints = [{'agent': 0, 'loc': [(1,5)], 'timestep': 4, 'type': 'vertex'}] # For Task 1,2
# constraints = [{'agent': 0, 'loc': [(1,5)], 'timestep': 4}] # For Task 1,2
# constraints = [{'agent': 1, 'loc': [(1,2), (1,3)], 'timestep': 1, 'type': 'edge'}] # For Task 1,3
# constraints = [{'agent': 1, 'loc': [(1,2), (1,3)], 'timestep': 1}] # For Task 1,3
# constraints = [{'agent': 0, 'loc': [(1,5)], 'timestep': 6, 'type': 'vertex'},
# {'agent': 0, 'loc': [(1,5)], 'timestep': 15, 'type': 'vertex'},
# {'agent': 0, 'loc': [(1,5)], 'timestep': 9, 'type': 'vertex'}] # For Task 1.4
# constraints = [{'agent':1, 'loc': [(1,3), (1,4)], 'timestep': 2},
# {'agent':1, 'loc': [(1,3)], 'timestep': 2},
# {'agent':1, 'loc': [(1,2)], 'timestep': 2},
# {'agent': 0, 'loc': [(1,5)], 'timestep': 6, 'type': 'vertex'}] # For Task 1.5
# agent_priorities = [1,0] # Bad ordering for exp2_1.txt
agent_priorities = range(self.num_of_agents) # Linear ordering
for ind, i in enumerate(agent_priorities): # Find path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, constraints)
if path is None:
raise BaseException('No solutions')
result.append(path)
for j in range(ind + 1, len(agent_priorities)):
agent = agent_priorities[j]
for timestep in range(len(path) - 1):
curr_loc = path[timestep]
next_loc = path[timestep + 1]
constraints.append({
'agent': agent,
'loc': [curr_loc],
'timestep': timestep,
'type': 'vertex'
})
constraints.append({
'agent': agent,
'loc': [next_loc, curr_loc],
'timestep': timestep + 1,
'type': 'edge'
})
constraints.append({
'agent': agent,
'loc': path[-1],
'timestep': len(path) - 1,
'type': 'inf'
})
self.CPU_time = timer.time() - start_time
print("\n Found a solution! \n")
print("CPU time (s): {:.2f}".format(self.CPU_time))
print("Sum of costs: {}".format(get_sum_of_cost(result)))
print(result)
return result
def find_solution(self):
""" Finds paths for all agents from their start locations to their goal locations."""
start_time = timer.time()
result = []
##############################
# Task 1: Understand the following code (see the lab description for some hints)
for i in range(self.num_of_agents): # Find path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, [], [])
if path is None:
raise BaseException('No solutions')
result.append(path)
##############################
self.CPU_time = timer.time() - start_time
print("\n Found a solution! \n")
print("CPU time (s): {:.2f}".format(self.CPU_time))
print("Sum of costs: {}".format(get_sum_of_cost(result)))
return result
def print_results(self, node):
print("\n Found a solution! \n")
CPU_time = timer.time() - self.start_time
print("CPU time (s): {:.2f}".format(CPU_time))
print("Sum of costs: {}".format(get_sum_of_cost(node['paths'])))
print("Expanded nodes: {}".format(self.num_of_expanded))
print("Generated nodes: {}".format(self.num_of_generated))
def find_solution(self):
""" Finds paths for all agents from their start locations to their goal locations."""
start_time = timer.time()
result = []
constraints = []
max_path_len = 0
map_size = sum(x.count(False) for x in self.my_map)
upperbound = max_path_len + map_size
# constraints.append({'agent': 0, 'loc': [(1, 5)], 'timestep': 4}) # 1.2
# constraints.append({'agent': 1, 'loc': [(1, 2), (1,3)], 'timestep': 1}) # 1.3
# constraints.append({'agent': 0, 'loc': [(1, 5)], 'timestep': 10}) # 1.4
# 1.5
# constraints.append({'agent': 1, 'loc': [(1, 2)], 'timestep': 2})
# constraints.append({'agent': 1, 'loc': [(1, 3)], 'timestep': 2})
# constraints.append({'agent': 1, 'loc': [(1, 4)], 'timestep': 2})
for i in range(self.num_of_agents): # Find path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i],
i, constraints)
if path is None:
raise BaseException('No solutions')
# Task 2.4
if len(path) > upperbound:
print("Reach upper bound {} for agent {} with path length {}".format(upperbound, i,len(path)))
raise BaseException('No solutions')
# Calculate an upper bound on the path length
max_path_len = max(max_path_len, len(path))
upperbound = max_path_len + map_size
result.append(path)
##############################
# Task 2: Add constraints here
# Useful variables:
# * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)]
# * self.num_of_agents has the number of total agents
# * constraints: array of constraints to consider for future A* searches
for agent in range(i + 1, self.num_of_agents + 1): # future agents
for t in range(len(path)):
constraints.append({'agent': agent, 'loc': [path[t]], 'timestep': t}) # vertex
if t > 0:
constraints.append({'agent': agent, 'loc': [path[t], path[t-1]], 'timestep': t}) # edge
constraints.append({'agent': agent, 'loc': [path[t-1], path[t]], 'timestep': t}) # edge
# Task 2.3 Additional Constraints (can also implemented by modify is_constrained function in single_agent_planner.py,then need to comment out it when choosing CBS)
for t_future in range(len(path), upperbound):
constraints.append({'agent': agent, 'loc': [path[len(path) - 1]], 'timestep': t_future})
##############################
self.CPU_time = timer.time() - start_time
print("\n Found a solution! \n")
print("CPU time (s): {:.2f}".format(self.CPU_time))
print("Sum of costs: {}".format(get_sum_of_cost(result)))
print(result)
return result
def find_solution(self):
""" Finds paths for all agents from their start locations to their goal locations."""
start_time = timer.time()
result = []
constraints = []
for i in range(self.num_of_agents): # Find path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, constraints)
if path is None:
raise BaseException('No solutions')
result.append(path)
##############################
# Task 2: Add constraints here
# Useful variables:
# * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)]
# * self.num_of_agents has the number of total agents
# * constraints: array of constraints to consider for future A* searches
for timestep, loc in enumerate(path):
for agent in range(i + 1, self.num_of_agents):
constraints.append({
'agent': agent,
'loc': [loc],
'timestep': timestep,
'positive': False
})
if timestep + 1 < len(path):
next_loc = path[timestep + 1]
constraints.append({
'agent': agent,
'loc': [next_loc, loc],
'timestep': timestep + 1,
'positive': False
})
# Code to handle Task 2.3 (Adding Additional Constraints)
else: # At goal = Negative timestep = Constrain for all future timesteps too
constraints.append({
'agent': agent,
'loc': [loc],
'timestep': -(timestep + 1),
'positive': False
})
##############################
self.CPU_time = timer.time() - start_time
print("\n Found a solution! \n")
print("CPU time (s): {:.2f}".format(self.CPU_time))
print("Sum of costs: {}".format(get_sum_of_cost(result)))
print(result)
return result
def find_solution(self):
""" Finds paths for all agents from their start locations to their goal locations."""
start_time = timer.time()
result = []
constraints = []
for i in range(self.num_of_agents): # Find path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, constraints, [])
if path is None:
raise BaseException('No solutions')
result.append(path)
##############################
# Task 2: Add constraints here
# Useful variables:
# * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)]
# * self.num_of_agents has the total number of agents
# * constraints: array of constraints to consider for future A* searches
##############################
for sid, place in enumerate(path):
for agent in range(i, self.num_of_agents):
constraints.append({
'agent': agent + 1,
'loc': [place],
'timestep': sid,
'positive': False
})
#constraints.append({'agent' : agent + 1, 'loc': [place], 'timestep': sid+1, 'positive': False})
if sid < len(path) - 1:
constraints.append({
'agent': agent + 1,
'loc': [path[sid], path[sid + 1]],
'timestep': sid + 1,
'positive': False
})
constraints.append({
'agent': agent + 1,
'loc': [path[sid + 1], path[sid]],
'timestep': sid + 1,
'positive': False
})
self.CPU_time = timer.time() - start_time
print("\n Found a solution! \n")
print("CPU time (s): {:.2f}".format(self.CPU_time))
print("Sum of costs: {}".format(get_sum_of_cost(result)))
print(result)
return result
def find_solution(self):
""" Finds paths for all agents from their start locations to their goal locations."""
start_time = timer.time()
result = []
blocks = []
constraints = []
for i in range(self.num_of_agents): # Find path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, constraints)
if path is None:
raise BaseException('No solutions')
result.append(path)
for count in range(len(path)):
blocks.append({'loc': [path[count]], 'timestep': count})
if count == len(path) - 1:
blocks.append({
'loc': [path[count]],
'timestep': '>' + str(count)
})
if count == 0:
continue
else:
blocks.append({
'loc': [path[count], path[count - 1]],
'timestep': count
})
for j in range(i + 1, self.num_of_agents):
for block in blocks:
constraints.append({
'agent': j,
'loc': block['loc'],
'timestep': block['timestep']
})
##############################
# Task 2: Add constraints here
# Useful variables:
# * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)]
# * self.num_of_agents has the number of total agents
# * constraints: array of constraints to consider for future A* searches
##############################
self.CPU_time = timer.time() - start_time
print("\n Found a solution! \n")
print("CPU time (s): {:.2f}".format(self.CPU_time))
print("Sum of costs: {}".format(get_sum_of_cost(result)))
print(result)
return result
def print_results(self, node):
CPU_time = timer.time() - self.start_time
process = psutil.Process(os.getpid())
memory = process.memory_info().rss / 1000000
cost = get_sum_of_cost(node['paths'])
print("\n Found a solution! \n")
print("Memory usage: {} mb".format(memory))
print("CPU time (s): {:.2f}".format(CPU_time))
print("Sum of costs: {}".format(cost))
print("Expanded nodes: {}".format(self.num_of_expanded))
print("Generated nodes: {}".format(self.num_of_generated))
# Write results to file
with open("results.csv", "a", buffering=1) as result_file:
result_file.write("{},{},{},{},{},{}\n".format(self.instance, CPU_time, memory, cost, self.num_of_expanded, self.num_of_generated))
def find_solution(self, disjoint=True):
""" Finds paths for all agents from their start locations to their goal locations
disjoint - use disjoint splitting or not
"""
self.start_time = timer.time()
# Generate the root node
# constraints - list of constraints
# paths - list of paths, one for each agent
# [[(x11, y11), (x12, y12), ...], [(x21, y21), (x22, y22), ...], ...]
# collisions - list of collisions in paths
root = {'cost': 0, 'constraints': [], 'paths': [], 'collisions': []}
for i in range(self.num_of_agents): # Find initial path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, root['constraints'])
if path is None:
raise BaseException('No solutions')
root['paths'].append(path)
root['cost'] = get_sum_of_cost(root['paths'])
root['collisions'] = detect_collisions(root['paths'])
self.push_node(root)
# Task 3.1: Testing
print(root['collisions'])
# Task 3.2: Testing
for collision in root['collisions']:
print(standard_splitting(collision))
##############################
# Task 3.3: High-Level Search
# Repeat the following as long as the open list is not empty:
# 1. Get the next node from the open list (you can use self.pop_node()
# 2. If this node has no collision, return solution
# 3. Otherwise, choose the first collision and convert to a list of constraints (using your
# standard_splitting function). Add a new child node to your open list for each constraint
# Ensure to create a copy of any objects that your child nodes might inherit
self.print_results(root)
return root['paths']
def find_solution(self):
""" Finds paths for all agents from their start locations to their goal locations."""
start_time = timer.time()
result = []
constraints = []
# Test Task 1.2, add constraint for agent
#constraints.append({'agent':0, 'loc':[(1,5)], 'timestep':4})
#constraints.append({'agent':1, 'loc':[(1,2), (1,3)], 'timestep':1})
# Test Task 1.5
"""
constraints.append({'agent':1, 'loc':[(1,3), (1,2)], 'timestep':2})
constraints.append({'agent':1, 'loc':[(1,3), (1,3)], 'timestep':2})
constraints.append({'agent':1, 'loc':[(1,3), (1,4)], 'timestep':2})
constraints.append({'agent':1, 'loc':[(1,2)], 'timestep':1})
"""
for i in range(self.num_of_agents): # Find path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, constraints)
if path is None:
print(result)
raise BaseException('No solutions')
result.append(path)
##############################
# Task 2: Add constraints here
# Useful variables:
# * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)]
# * self.num_of_agents has the number of total agents
# * constraints: array of constraints to consider for future A* searches
# add constraints for all after agents
current_path = result[i]
# add vertex constraints
timestep = 0
for position in current_path:
# each agent need to add constraint relate to current path
for each_agent in range(i + 1, self.num_of_agents):
need_to_append = {
'agent': each_agent,
'loc': [position],
'timestep': timestep,
'positive': 0
}
if need_to_append not in constraints:
constraints.append(need_to_append)
timestep += 1
# add edge constraints
timestep = 1
for j in range(len(current_path) - 1):
for each_agent in range(i + 1, self.num_of_agents):
need_to_append = {
'agent': each_agent,
'loc': [current_path[j + 1], current_path[j]],
'timestep': timestep,
'positive': 0
}
if need_to_append not in constraints:
constraints.append(need_to_append)
timestep += 1
# add additional constraints
# -1 represent all future time
for each_agent in range(i + 1, self.num_of_agents):
need_to_append = {
'agent': each_agent,
'loc': [current_path[len(current_path) - 1]],
'timestep': (-1, len(current_path) - 1),
'positive': 0
}
if need_to_append not in constraints:
constraints.append(need_to_append)
##############################
self.CPU_time = timer.time() - start_time
print("\n Found a solution! \n")
print("CPU time (s): {:.2f}".format(self.CPU_time))
print("Sum of costs: {}".format(get_sum_of_cost(result)))
print(result)
return result
def find_solution(self, disjoint=True):
""" Finds paths for all agents from their start locations to their goal locations
disjoint - use disjoint splitting or not
"""
self.start_time = timer.time()
# Generate the root node
# constraints - list of constraints
# paths - list of paths, one for each agent
# [[(x11, y11), (x12, y12), ...], [(x21, y21), (x22, y22), ...], ...]
# collisions - list of collisions in paths
root = {'cost': 0, 'constraints': [], 'paths': [], 'collisions': []}
for i in range(self.num_of_agents): # Find initial path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, root['constraints'])
if path is None:
raise BaseException('No solutions')
root['paths'].append(path)
root['cost'] = get_sum_of_cost(root['paths'])
root['collisions'] = detect_collisions(root['paths'])
self.push_node(root)
# Task 3.1: Testing
print(root['collisions'])
# Task 3.2: Testing
for collision in root['collisions']:
print(standard_splitting(collision))
##############################
# Task 3.3: High-Level Search
# Repeat the following as long as the open list is not empty:
# 1. Get the next node from the open list (you can use self.pop_node()
# 2. If this node has no collision, return solution
# 3. Otherwise, choose the first collision and convert to a list of constraints (using your
# standard_splitting function). Add a new child node to your open list for each constraint
# Ensure to create a copy of any objects that your child nodes might inherit
standard = False
disjoint = True
while len(self.open_list) > 0:
new_node = self.pop_node()
if len(new_node['collisions']) == 0:
print("No collision")
return new_node['paths']
collision = new_node['collisions'][0]
if standard == True:
constraints = standard_splitting(collision)
if disjoint == True:
constraints = disjoint_splitting(collision)
for constraint in constraints:
Q = {
'constraints': [],
'paths': [],
'cost': [],
'collisions': []
}
for i in new_node['constraints']:
Q['constraints'].append(i)
Q['constraints'].append(constraint)
for i in new_node['paths']:
Q['paths'].append(i)
agent = constraint['agent']
path = a_star(self.my_map, self.starts[agent],
self.goals[agent], self.heuristics[agent], agent,
Q['constraints'])
if path is not None:
Q['paths'][agent] = path
paths_violate = list()
if constraint['positive'] == True:
paths_violate = paths_violate_constraint(
constraint, Q['paths'])
for new_agent in paths_violate:
new_constraint = {
'agent': new_agent,
'loc': constraint['loc'],
'timestep': constraint['timestep'],
'positive': False
}
Q['constraints'].append(new_constraint)
new_path = a_star(self.my_map, self.starts[new_agent],
self.goals[new_agent],
self.heuristics[new_agent],
new_agent, Q['constraints'])
if new_path is None:
break
Q['paths'][new_agent] = new_path
Q['cost'] = get_sum_of_cost(Q['paths'])
Q['collisions'] = detect_collisions(Q['paths'])
self.push_node(Q)
raise BaseException('No solutions')
def find_solution(self, disjoint=True, heuristic=None):
""" Finds paths for all agents from their start locations to their goal locations
disjoint - use disjoint splitting or not
"""
self.start_time = timer.time()
# Generate the root node
# constraints - list of constraints
# paths - list of paths, one for each agent
# [[(x11, y11), (x12, y12), ...], [(x21, y21), (x22, y22), ...], ...]
# collisions - list of collisions in paths
root = {'cost': 0, 'constraints': [], 'paths': [], 'collisions': []}
for i in range(self.num_of_agents): # Find initial path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, root['constraints'])
if path is None:
raise BaseException('No solutions')
root['paths'].append(path)
root['cost'] = get_sum_of_cost(root['paths'])
root['collisions'] = detect_collisions(root['paths'])
root['h_val'] = 0
self.push_node(root)
# Task 3.1: Testing
#print(root['collisions'])
# Task 3.2: Testing
#for collision in root['collisions']:
# print(standard_splitting(collision))
##############################
# Task 3.3: High-Level Search
# Repeat the following as long as the open list is not empty:
# 1. Get the next node from the open list (you can use self.pop_node()
# 2. If this node has no collision, return solution
# 3. Otherwise, choose the first collision and convert to a list of constraints (using your
# standard_splitting function). Add a new child node to your open list for each constraint
# Ensure to create a copy of any objects that your child nodes might inherit
while len(self.open_list) > 0:
p = self.pop_node()
if len(p['collisions']) == 0:
self.sum_of_costs = get_sum_of_cost(p['paths'])
return p['paths']
collision = p['collisions'][0]
constraints = standard_splitting(
collision) #disjoint_splitting(collision)
for constraint in constraints:
q = {
'cost': 0,
'constraints':
copy.deepcopy(p['constraints'] + [constraint]),
'paths': copy.deepcopy(p['paths']),
'collisions': []
}
""" Task 4
if constraint['positive'] == True:
if paths_violate_constraint(constraint, q['paths']):
continue
"""
agent = constraint['agent']
path = a_star(self.my_map, self.starts[agent],
self.goals[agent], self.heuristics[agent], agent,
q['constraints'])
if path != None:
q['paths'][agent] = path
q['collisions'] = detect_collisions(q['paths'])
q['cost'] = get_sum_of_cost(q['paths'])
if self.CT_heuristic == None:
q['h_val'] = 0
else:
MDD_list = [mdd.MDD(self.my_map, self.starts[i], self.goals[i],\
compute_heuristics(self.my_map, self.goals[i]), i, q['constraints'])\
for i in range(len(self.starts))]
if self.CT_heuristic == "CG":
graph_inst = mdd.graph_from_MDD(MDD_list)
mvc_inst = mvc.BruteForceMVC(graph_inst)
q['h_val'] = mvc_inst.getMVC()
elif self.CT_heuristic == "DG":
graph_inst = joint_mdd.graph_from_MDD(MDD_list)
mvc_inst = mvc.BruteForceMVC(graph_inst)
q['h_val'] = mvc_inst.getMVC()
elif self.CT_heuristic == "WDG":
graph_inst = joint_mdd.weighted_graph_from_MDD(
MDD_list, self.my_map, self.starts, self.goals)
ewmvc_inst = mvc.BruteForceEWMVC(graph_inst)
q['h_val'] = ewmvc_inst.getEWMVC()
self.push_node(q)
if path is None:
raise BaseException('No solutions')
self.print_results(root)
return root['paths']
def find_solution(self, disjoint=True, time_limit=60):
""" Finds paths for all agents from their start locations to their goal locations
disjoint - use disjoint splitting or not
time_limit - maximum amount of execution time allowed
"""
start_time = timer.time()
# Generate the root node
# constraints - list of constraints
# paths - list of paths, one for each agent
# [[(x11, y11), (x12, y12), ...], [(x21, y21), (x22, y22), ...], ...]
# conflicts - list of conflicts in paths
root = {
'cost': 0,
'constraints': [],
'paths': [],
'conflicts': [],
'parent': None
}
for i in range(self.num_of_agents): # Find initial path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, root['constraints'],
root['paths'])
if path is None:
raise BaseException('No solutions')
root['paths'].append(path)
root['cost'] = get_sum_of_cost(root['paths'])
root['conflicts'] = detect_conflicts(root['paths'])
self.push_node(root)
# Task 3.1: Testing
print(root['conflicts'])
# Task 3.2: Testing
for conflict in root['conflicts']:
print(standard_splitting(conflict))
##############################
# Task 3.3: High-Level Search
# Repeat the following as long as the open list is not empty:
# 1. Get the next node from the open list (you can use self.pop_node()
# 2. If this node has no conflict, return solution
# 3. Otherwise, choose the first conflict and convert to a list of constraints (using your
# standard_splitting function). Add a new child node to your open list for each constraint
# Ensure to create a copy of any objects that your child nodes might inherit
while len(self.open_list) > 0:
node = self.pop_node()
if node['conflicts'] == []:
self.print_results(node)
return node['paths']
conflict = node['conflicts'][0]
print(
"Choose a conflict between {} and {} at location {} at timestep {}"
.format(conflict['a1'], conflict['a2'], conflict['loc'],
conflict['timestep']))
#new_constraints = standard_splitting(conflict)
new_constraints = disjoint_splitting(conflict)
for constraint in new_constraints:
#i = constraint['agent']
print(
"Negative constraint on agent {} at location {} at timestep {}"
.format(constraint['agent'], constraint['loc'],
constraint['timestep']))
constraints = list(node['constraints'])
constraints.append(constraint)
paths = list(node['paths'])
#paths[i] = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i],
# i, constraints, paths)
replan = []
if constraint['positive']:
replan = paths_violate_constraint(constraint, paths)
else:
replan.append(constraint['agent'])
#child_node = {'cost': get_sum_of_cost(paths),
# 'constraints':constraints,
# 'paths': paths,
# 'conflicts': detect_conflicts(paths),
# 'parent': node}
#self.push_node(child_node)
prune = False
for i in replan:
paths[i] = a_star(self.my_map, self.starts[i],
self.goals[i], self.heuristics[i], i,
constraints, paths)
if paths[i] is None:
prune = True
break
if not prune:
child_node = {
'cost': get_sum_of_cost(paths),
'constraints': constraints,
'paths': paths,
'conflicts': detect_conflicts(paths),
'parent': node
}
self.push_node(child_node)
def find_solution(self):
""" Finds paths for all agents from their start locations to their goal locations."""
start_time = timer.time()
result = []
constraints = []
for i in range(self.num_of_agents): # Find path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i],
i, constraints)
if path is None:
raise BaseException('No solutions')
result.append(path)
##############################
# Task 2: Add constraints here
# Useful variables:
# * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)]
# * self.num_of_agents has the number of total agents
# * constraints: array of constraints to consider for future A* searches
for node_index in range(len(path)):
for agent_index in range(self.num_of_agents):
if agent_index != i:
# Adding Vertex Constraints
vertex_constraint = {'agent': agent_index, 'loc':[path[node_index]], 'timestep': node_index}
constraints.append(vertex_constraint)
# Adding Edge Constraints
for dir in range(4):
curr_pos = move(path[node_index], dir)
if self.my_map[curr_pos[0]][curr_pos[1]]:
continue
edge_constraint = {'agent': agent_index, 'loc':[curr_pos, path[node_index]], 'timestep':node_index+1}
constraints.append(edge_constraint)
# Calculate the edge_constraints for goal state nodes
path_upperbound = 0
for p in result:
path_upperbound += len(p)
path_upperbound += len(self.my_map)+len(self.my_map[0]) ### Look for something more specific to find upper bound
for index, route in enumerate(result):
for i in range(path_upperbound-len(route)):
goal_state = route[len(route)-1]
time = len(route) + i - 1
# create constraints for every agent
for a in range(self.num_of_agents):
if a == index: continue
for dir in range(4):
curr_pos = move(goal_state, dir)
if self.my_map[curr_pos[0]][curr_pos[1]]:
continue
edge_constraint = {'agent': a, 'loc':[curr_pos, goal_state], 'timestep':time+1}
constraints.append(edge_constraint)
##############################
self.CPU_time = timer.time() - start_time
print("\n Found a solution! \n")
print("CPU time (s): {:.2f}".format(self.CPU_time))
print("Sum of costs: {}".format(get_sum_of_cost(result)))
print(result)
return result
def find_solution(self, disjoint=True):
""" Finds paths for all agents from their start locations to their goal locations
disjoint - use disjoint splitting or not
"""
self.start_time = timer.time()
root = {'cost': 0, 'constraints': [], 'paths': [], 'collisions': []}
for i in range(self.num_of_agents): # Find initial path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, root['constraints'])
if path is None:
raise BaseException('No solutions')
root['paths'].append(path)
root['cost'] = get_sum_of_cost(root['paths'])
root['collisions'] = detect_collisions(root['paths'])
self.push_node(root)
##############################
# Task 3.3: High-Level Search
# Repeat the following as long as the open list is not empty:
# 1. Get the next node from the open list (you can use self.pop_node()
# 2. If this node has no collision, return solution
# 3. Otherwise, choose the first collision and convert to a list of constraints (using your
# standard_splitting function). Add a new child node to your open list for each constraint
# Ensure to create a copy of any objects that your child nodes might inherit
while len(self.open_list) > 0:
curr = self.pop_node(
) # firstly sorted with cost then sorted with #constraints
# no collision return solution
if len(curr['collisions']) == 0:
self.print_results(curr)
print(curr)
return curr['paths']
first_collision = curr['collisions'][0]
constraints = []
# standard CBS
if disjoint == False:
constraints = standard_splitting(first_collision)
# CBS with disjoint splitting
else:
constraints = disjoint_splitting(first_collision)
# for each constraint add a new child node
for constraint in constraints:
# deep copy parent attributes to child
child_constarints = copy_parent(curr['constraints'])
if constraint not in child_constarints:
child_constarints.append(constraint)
child_paths = copy_parent(curr['paths'])
constrainted_agent = constraint['agent']
child = {
'cost': 0,
'constraints': child_constarints,
'paths': child_paths,
'collisions': []
}
if constraint['positive'] == 0:
path = a_star(self.my_map, self.starts[constrainted_agent],
self.goals[constrainted_agent],
self.heuristics[constrainted_agent],
constrainted_agent, child['constraints'])
path = eliminate_duplicates(path)
if path is not None:
# replace the constrainted agent path by new path in parent paths
child['paths'][constrainted_agent] = path
child['collisions'] = detect_collisions(child['paths'])
child['cost'] = get_sum_of_cost(child['paths'])
self.push_node(child)
else:
Add = True
# update all agents' path
violate_IDs = paths_violate_constraint(
constraint, curr['paths'])
# replace the constrainted agent path by new path in parent paths
for each_agent in violate_IDs:
path = a_star(self.my_map, self.starts[each_agent],
self.goals[each_agent],
self.heuristics[each_agent], each_agent,
child['constraints'])
path = eliminate_duplicates(path)
if path is not None:
child['paths'][each_agent] = path
else:
Add = False
break
#update the constraint agent
path = a_star(self.my_map, self.starts[constrainted_agent],
self.goals[constrainted_agent],
self.heuristics[constrainted_agent],
constrainted_agent, child['constraints'])
path = eliminate_duplicates(path)
if path is not None:
child['paths'][constrainted_agent] = path
child['collisions'] = detect_collisions(child['paths'])
child['cost'] = get_sum_of_cost(child['paths'])
else:
Add = False
if Add:
self.push_node(child)
raise BaseException('No solutions')
def find_solution(self):
""" Finds paths for all agents from their start locations to their goal locations
disjoint - use disjoint splitting or not
"""
self.start_time = timer.time()
# Generate the root node
# constraints - list of constraints
# paths - list of paths, one for each agent
# [[(x11, y11), (x12, y12), ...], [(x21, y21), (x22, y22), ...], ...]
# collisions - list of collisions in paths
root = {'cost': 0,
'constraints': [],
'paths': [],
'collisions': []}
for i in range(self.num_of_agents): # Find initial path for each agent
path = a_star(self.my_map,
self.starts[i],
self.goals[i],
self.heuristics[i],
i,
root['constraints'])
if path is None:
raise BaseException('No solutions')
root['paths'].append(path)
root['cost'] = get_sum_of_cost(root['paths'])
root['collisions'] = detect_collisions(root['paths'])
self.push_node(root)
while len(self.open_list) > 0:
next_node = self.pop_node()
if len(next_node['collisions']) == 0:
self.print_results(next_node)
return next_node['paths']
collision = next_node['collisions'][0]
constraints = standard_splitting(collision)
for constraint in constraints:
new_child_node = dict()
new_child_node['constraints'] = [constraint] + next_node['constraints']
new_child_node['paths'] = [] + next_node['paths']
agent_in_constraint = constraint['agent']
path = a_star(self.my_map,
self.starts[agent_in_constraint],
self.goals[agent_in_constraint],
self.heuristics[agent_in_constraint],
agent_in_constraint,
new_child_node['constraints'])
if path is not None and len(path) > 0:
new_child_node['paths'][agent_in_constraint] = [] + path
new_child_node['collisions'] = detect_collisions(new_child_node['paths'])
new_child_node['cost'] = get_sum_of_cost(new_child_node['paths'])
self.push_node(new_child_node)
self.print_results(root)
return None
def find_solution(self, disjoint=True):
""" Finds paths for all agents from their start locations to their goal locations
disjoint - use disjoint splitting or not
"""
self.start_time = timer.time()
# Generate the root node
# constraints - list of constraints
# paths - list of paths, one for each agent
# [[(x11, y11), (x12, y12), ...], [(x21, y21), (x22, y22), ...], ...]
# collisions - list of collisions in paths
root = {'cost': 0,
'constraints': [],
'paths': [],
'collisions': []}
for i in range(self.num_of_agents): # Find initial path for each agent
print("Time through init loop -", i+1)
path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i],
i, root['constraints'])
if path is None:
raise BaseException('No solutions')
root['paths'].append(path)
root['cost'] = get_sum_of_cost(root['paths'])
root['collisions'] = detect_collisions(root['paths'])
self.push_node(root)
# Task 3.1: Testing
print(root['collisions'])
# Task 3.2: Testing
for collision in root['collisions']:
print(disjoint_splitting(collision))
##############################
# Task 3.3: High-Level Search
# Repeat the following as long as the open list is not empty:
# 1. Get the next node from the open list (you can use self.pop_node()
# 2. If this node has no collision, return solution
# 3. Otherwise, choose the first collision and convert to a list of constraints (using your
# standard_splitting function). Add a new child node to your open list for each constraint
# Ensure to create a copy of any objects that your child nodes might inherit
while len(self.open_list) > 0:
curr = self.pop_node()
if len(curr['collisions']) == 0: # paths found
print("PATH REACHED:")
for i in curr['paths']:
print(i)
return curr['paths']
collision = curr['collisions'].pop(0)
constraints = standard_splitting(collision)
for constraint in constraints:
child = {'cost': 0,
'constraints': [],
'paths': [],
'collisions': []}
child['constraints'] = curr['constraints']
child['constraints'].append(constraint)
child['paths'] = curr['paths']
agents = paths_violate_constraint(curr['paths'], constraint, self.num_of_agents) # disjoint splitting
agent = constraint['agent']
path = a_star(self.my_map, self.starts[agent], self.goals[agent], self.heuristics[agent], agent, child['constraints'])
if path != None and len(path) > 0:
child['paths'][agent] = path
child['collisions'] = detect_collisions(child['paths'])
child['cost'] = get_sum_of_cost(child['paths'])
self.push_node(child)
# end of if statement
# end of for loop
# end of while loop
self.print_results(root)
return root['paths']
def find_solution(self, disjoint):
""" Finds paths for all agents from their start locations to their goal locations
disjoint - use disjoint splitting or not
"""
self.start_time = timer.time()
# Generate the root node
# constraints - list of constraints
# paths - list of paths, one for each agent
# [[(x11, y11), (x12, y12), ...], [(x21, y21), (x22, y22), ...], ...]
# collisions - list of collisions in paths
root = {'cost': 0, 'constraints': [], 'paths': [], 'collisions': []}
for i in range(self.num_of_agents): # Find initial path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, root['constraints'])
if path is None:
raise BaseException('No solutions for agent ' + str(i))
root['paths'].append(path)
root['cost'] = get_sum_of_cost(root['paths'])
root['collisions'] = detect_collisions(root['paths'])
self.push_node(root)
# Task 3.1: Testing
print(root['collisions'])
# Task 3.2: Testing
for collision in root['collisions']:
print(standard_splitting(collision))
##############################
# Task 3.3: High-Level Search
# Repeat the following as long as the open list is not empty:
# 1. Get the next node from the open list (you can use self.pop_node()
# 2. If this node has no collision, return solution
# 3. Otherwise, choose the first collision and convert to a list of constraints (using your
# standard_splitting function). Add a new child node to your open list for each constraint
# Ensure to create a copy of any objects that your child nodes might inherit
while len(self.open_list) > 0:
curr = self.pop_node()
if len(curr['collisions']) == 0:
self.print_results(curr)
return curr['paths']
#collision = curr['collisions'][0]
collision = ICBS(curr['collisions'], curr['paths'],
curr['constraints'], self)
if disjoint:
constraints = disjoint_splitting(collision)
else:
constraints = standard_splitting(collision)
for constraint in constraints:
Q = {
'cost': 0,
'constraints': [],
'paths': [],
'collisions': []
}
Q['constraints'] = copy.deepcopy(curr['constraints'])
Q['constraints'].append(constraint)
Q['paths'] = copy.deepcopy(curr['paths'])
if disjoint:
if constraint['positive'] == 0:
ai = constraint['agent']
path = a_star(self.my_map, self.starts[ai],
self.goals[ai], self.heuristics[ai], ai,
Q['constraints'])
if path is None:
continue
Q['paths'][ai] = path
else:
newc = new_constraints(constraint, self.num_of_agents)
for c in newc:
Q['constraints'].append(c)
agentsId = paths_violate_constraint(
curr['paths'], constraint)
ai = constraint['agent']
path = a_star(self.my_map, self.starts[ai],
self.goals[ai], self.heuristics[ai], ai,
Q['constraints'])
if path is None:
continue
Q['paths'][ai] = path
no_path = 0
for agent in agentsId:
path = a_star(self.my_map, self.starts[agent],
self.goals[agent],
self.heuristics[agent], agent,
Q['constraints'])
if path is None:
no_path = 1
break
Q['paths'][agent] = path
if no_path == 1:
continue
else:
ai = constraint['agent']
path = a_star(self.my_map, self.starts[ai], self.goals[ai],
self.heuristics[ai], ai, Q['constraints'])
if path is None:
continue
Q['paths'][ai] = path
Q['collisions'] = detect_collisions(Q['paths'])
Q['cost'] = get_sum_of_cost(Q['paths'])
self.push_node(Q)
raise BaseException('No solutions')
def find_solution(
self,
disjoint=True,
meta_constraints=None
): # add constraint as param to support low lever search for meta agent cbs
""" Finds paths for all agents from their start locations to their goal locations
disjoint - use disjoint splitting or not
"""
self.start_time = timer.time()
# Generate the root node
# constraints - list of constraints
# paths - list of paths, one for each agent
# [[(x11, y11), (x12, y12), ...], [(x21, y21), (x22, y22), ...], ...]
# collisions - list of collisions in paths
root = {'cost': 0, 'constraints': [], 'paths': [], 'collisions': []}
if meta_constraints != None:
root['constraints'] = copy.copy(meta_constraints)
for i in range(self.num_of_agents): # Find initial path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, root['constraints'])
if path is None:
raise BaseException('No solutions')
root['paths'].append(path)
root['cost'] = get_sum_of_cost(root['paths'])
root['collisions'] = detect_collisions(root['paths'])
self.push_node(root)
# Task 3.1: Testing
# print(root['collisions'])
# Task 3.2: Testing
# for collision in root['collisions']:
# print(standard_splitting(collision))
##############################
# Task 3.3: High-Level Search
# Repeat the following as long as the open list is not empty:
# 1. Get the next node from the open list (you can use self.pop_node()
# 2. If this node has no collision, return solution
# 3. Otherwise, choose the first collision and convert to a list of constraints (using your
# standard_splitting function). Add a new child node to your open list for each constraint
# Ensure to create a copy of any objects that your child nodes might inherit
# expanded_nodes = []
while len(self.open_list) > 0:
if meta_constraints is None:
if timer.time() - self.start_time > 100:
raise Exception(
'timeout|' + str(self.num_of_expanded) + '|' +
str(self.num_of_generated) + '|' +
str(round(timer.time() - self.start_time, 2)))
else:
if timer.time() - self.start_time > 20:
return None
# print("Open list: {}".format(self.open_list))
curr = self.pop_node()
# expanded_nodes.append(curr)
if len(curr['collisions']) == 0:
# print("Expanded nodes list: {}".format(str(expanded_nodes)))
# self.print_results(curr)
CPU_time = timer.time() - self.start_time
return (curr['paths'], "{:.03f}".format(CPU_time),
self.num_of_expanded, self.num_of_generated)
else:
collision = curr['collisions'][0]
# print("In parent collisions, pick one collision {}".format(collision))
# constraints = standard_splitting(collision)
constraints = disjoint_splitting(collision)
for constraint in constraints:
child_contraints = list(curr['constraints'])
child_contraints.append(constraint)
child_paths = list(curr['paths'])
agents_need_update = []
if constraint['positive']:
agents_need_update = paths_violate_constraint(
constraint, child_paths)
else:
agents_need_update.append(constraint['agent'])
keep = True
for j in agents_need_update:
child_paths[j] = a_star(self.my_map, self.starts[j],
self.goals[j],
self.heuristics[j], j,
child_contraints)
if child_paths[j] is None:
keep = False
break
if keep:
child = {
'cost': get_sum_of_cost(child_paths),
'constraints': child_contraints,
'paths': child_paths,
'collisions': detect_collisions(child_paths)
}
self.push_node(child)
# print("Expanded nodes list: {}".format(str(expanded_nodes)))
if meta_constraints != None:
return None
else:
raise Exception('no solutions|' + str(self.num_of_expanded) + '|' +
str(self.num_of_generated) + '|' +
str(round(timer.time() - self.start_time, 2)))
def find_solution(self):
""" Finds paths for all agents from their start locations to their goal locations."""
start_time = timer.time()
result = []
constraints = []
# Constraint for Task 1.2
# constraints = [{'agent': 0,'loc': [(1,5)],'timestep': 4}]
# Constraint for Task 1.3
# constraints = [{'agent': 1,'loc': [(1,2),(1,3)],'timestep': 1}]
# Constraints for Task 1.4
# constraints = [{'agent': 0,'loc': [(1,5)],'timestep': 10},
# {'agent': 0,'loc': [(1,3),(1,4)],'timestep': 5},
# {'agent': 0,'loc': [(1,3),(1,4)],'timestep': 7},
# {'agent': 0,'loc': [(1,2),(1,3)],'timestep': 2}]
# Constraints for Task 1.5
# constraints = [{'agent': 1,'loc': [(1,4)],'timestep': 2},
# {'agent': 1,'loc': [(1,3),(1,2)],'timestep': 2}]
for i in range(self.num_of_agents): # Find path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, constraints)
if path is None:
raise BaseException('No solutions')
result.append(path)
##############################
# Task 2: Add constraints here
# Useful variables:
# * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)]
# * self.num_of_agents has the number of total agents
# * constraints: array of constraints to consider for future A* searches
##############################
# if len(constraint_table.keys()) != 0:
# if next_time > max(constraint_table.keys()):
# next_time = max(constraint_table.keys())
# print(path)
for next_agent in range(self.num_of_agents):
for i in range(0, 10):
# # Task 2.3 Adding goal constraints and max timestep allowed was 10
constraints.append({
'agent': next_agent,
'loc': [path[len(path) - 1]],
'timestep': len(path) + i - 1
})
for nPath in range(len(path)):
# print(path[len(path) - 1])
# constraints.append({'agent' : next_agent, 'loc' : [path[len(path) - 1]], 'timestep' : nPath})
# constraints.append({'agent' : next_agent, 'loc' : [path[len(path) - 1]], 'timestep' : nPath+len(path)})
# Task 2.1 Vertex Constraints
if next_agent != i:
constraints.append({
'agent': next_agent,
'loc': [path[nPath]],
'timestep': nPath
})
# Task 2.2 Edge Constraints
if nPath > 0:
constraints.append({
'agent':
next_agent,
'loc': [path[nPath], path[nPath - 1]],
'timestep':
nPath
})
self.CPU_time = timer.time() - start_time
# print(constraints)
print("\n Found a solution! \n")
print("CPU time (s): {:.2f}".format(self.CPU_time))
print("Sum of costs: {}".format(get_sum_of_cost(result)))
print(result)
return result
def find_solution(self, disjoint=True):
""" Finds paths for all agents from their start locations to their goal locations
disjoint - use disjoint splitting or not
"""
self.start_time = timer.time()
# Generate the root node
# constraints - list of constraints
# paths - list of paths, one for each agent
# [[(x11, y11), (x12, y12), ...], [(x21, y21), (x22, y22), ...], ...]
# collisions - list of collisions in paths
root = {'cost': 0, 'constraints': [], 'paths': [], 'collisions': []}
for i in range(self.num_of_agents): # Find initial path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, root['constraints'])
if path is None:
raise BaseException('No solutions')
root['paths'].append(path)
root['cost'] = get_sum_of_cost(root['paths'])
root['collisions'] = detect_collisions(root['paths'])
self.push_node(root)
# Task 3.1: Testing
print(root['collisions'])
# Task 3.2: Testing
for collision in root['collisions']:
print(standard_splitting(collision))
##############################
# Task 3.3: High-Level Search
# Repeat the following as long as the open list is not empty:
# 1. Get the next node from the open list (you can use self.pop_node()
# 2. If this node has no collision, return solution
# 3. Otherwise, choose the first collision and convert to a list of constraints (using your
# standard_splitting function). Add a new child node to your open list for each constraint
# Ensure to create a copy of any objects that your child nodes might inherit
while len(self.open_list) > 0:
P = self.pop_node()
# if this node has no collision, return solution
if len(P['collisions']) == 0:
self.print_results(P)
return P['paths']
# else convert the first collision to a list of constraints
collision = P['collisions'][0]
# constraints = standard_splitting(collision)
constraints = disjoint_splitting(collision)
for constraint in constraints:
Q = {
'cost': 0,
'constraints': P['constraints'].copy(),
'paths': [],
'collisions': []
}
# shallow copy constraints of P and add the new constraint
Q['constraints'].append(constraint)
# shallow copy paths of P
Q['paths'] = P['paths'].copy()
agent_idx = constraint['agent']
path = a_star(self.my_map, self.starts[agent_idx],
self.goals[agent_idx],
self.heuristics[agent_idx], agent_idx,
Q['constraints'])
if path is not None:
Q['paths'][agent_idx] = path
flag = False
if constraint['positive']:
violate_agents = paths_violate_constraint(
constraint, Q['paths'])
for violate_agent_idx in violate_agents:
Q['constraints'].append({
'agent':
violate_agent_idx,
'loc':
constraint['loc'],
'timestep':
constraint['timestep'],
'positive':
False
})
temp_path = a_star(
self.my_map, self.starts[violate_agent_idx],
self.goals[violate_agent_idx],
self.heuristics[violate_agent_idx],
violate_agent_idx, Q['constraints'])
if temp_path is not None:
Q['paths'][violate_agent_idx] = temp_path
else:
flag = True
break
if not flag:
Q['collisions'] = detect_collisions(Q['paths'])
Q['cost'] = get_sum_of_cost(Q['paths'])
self.push_node(Q)
# self.print_results(root)
# return root['paths']
raise BaseException('No solutions')
def find_solution(self):
""" Finds paths for all agents from their start locations to their goal locations."""
start_time = timer.time()
result = []
# 1.2
# constraints = [
# {
# 'agent': 0,
# 'loc': [(1, 5)],
# 'timestep': 4
# }
# ]
# 1.3
# constraints = [
# {
# 'agent': 1,
# 'loc': [(1,2), (1, 3)],
# 'timestep': 1
# }
# ]
# 1.4
# constraints = [
# {
# 'agent': 0,
# 'loc': [(1, 5)],
# 'timestep': 10
# }
# ]
# 1.5
# constraints = [
# {
# 'agent': 1,
# 'loc': [(1,3), (1,4)],
# 'timestep': 2
# },
# {
# 'agent': 1,
# 'loc': [(1,2)],
# 'timestep': 2
# },
# {
# 'agent': 1,
# 'loc': [(1,3)],
# 'timestep': 2
# }
# ]
constraints = []
mymap_size = len(max(self.my_map)) * len(self.my_map)
print("mymap_size: ", mymap_size)
for i in range(self.num_of_agents): # Find path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, constraints)
if path is None:
raise BaseException('No solutions')
##############################
# Task 2: Add constraints here
# Useful variables:
# * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)]
# * self.num_of_agents has the number of total agents
# * constraints: array of constraints to consider for future A* searches
for j in range(self.num_of_agents):
if (j == i):
continue
timestep = 0
for cell in path:
constraints.append({
'agent': j,
'loc': [cell],
'timestep': timestep
})
timestep += 1
if (timestep < len(path)):
constraints.append({
'agent': j,
'loc': [cell, path[timestep]],
'timestep': timestep
})
# goal_timestep = timestep
# add constraint once it reaches goal to the upper bound timestep
while (timestep <= mymap_size):
constraints.append({
'agent': j,
'loc': [path[-1]],
'timestep': timestep
})
timestep += 1
result.append(path)
##############################
self.CPU_time = timer.time() - start_time
print("\n Found a solution! \n")
print("CPU time (s): {:.2f}".format(self.CPU_time))
print("Sum of costs: {}".format(get_sum_of_cost(result)))
print(result)
return result
elif args.solver == "Independent":
print("***Run Independent***")
solver = IndependentSolver(my_map, starts, goals)
paths = solver.find_solution()
elif args.solver == "Prioritized":
print("***Run Prioritized***")
solver = PrioritizedPlanningSolver(my_map, starts, goals)
paths = solver.find_solution()
elif args.solver == "LP":
print("***Run LP***")
solver = LPSolver(my_map, starts, goals)
paths, solve_time = solver.find_solution()
total_solve_time += solve_time
elif args.solver == "mdd":
solver = MDDSolver(my_map, starts, goals)
paths = solver.find_solution()
else:
raise RuntimeError("Unknown solver!")
cost = get_sum_of_cost(paths)
result_file.write("{},{}\n".format(file, cost))
if not args.batch:
print("***Test paths on a simulation***")
animation = Animation(my_map, starts, goals, paths)
animation.save("output.webm", 1.0)
animation.show()
result_file.close()
if args.solver in {'LP', 'CBS'}:
print('Total solve time: %f' % total_solve_time)
def find_solution(self, disjoint=True):
""" Finds paths for all agents from their start locations to their goal locations
disjoint - use disjoint splitting or not
"""
self.start_time = timer.time()
# Generate the root node
# constraints - list of constraints
# paths - list of paths, one for each agent
# [[(x11, y11), (x12, y12), ...], [(x21, y21), (x22, y22), ...], ...]
# collisions - list of collisions in paths
root = {'cost': 0,
'constraints': [],
'paths': [],
'collisions': []}
for i in range(self.num_of_agents): # Find initial path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i], self.heuristics[i],
i, root['constraints'])
if path is None:
raise BaseException('No solutions')
root['paths'].append(path)
root['cost'] = get_sum_of_cost(root['paths'])
root['collisions'] = detect_collisions(root['paths'])
self.push_node(root)
# Task 3.1: Testing
print(root['collisions'])
# Task 3.2: Testing
for collision in root['collisions']:
print(standard_splitting(collision))
##############################
# Task 3.3: High-Level Search
# Repeat the following as long as the open list is not empty:
# 1. Get the next node from the open list (you can use self.pop_node()
# 2. If this node has no collision, return solution
# 3. Otherwise, choose the first collision and convert to a list of constraints (using your
# standard_splitting function). Add a new child node to your open list for each constraint
# Ensure to create a copy of any objects that your child nodes might inherit
while len(self.open_list) > 0:
P = self.pop_node()
if len(P['collisions']) < 1:
self.print_results(P)
return P['paths']
# select first collision
collision = P['collisions'][0]
constraints = standard_splitting(collision) if disjoint is False else disjoint_splitting(collision)
for c_idx, constraint in enumerate(constraints):
Q = {'cost': 0,
'constraints': [],
'paths': [],
'collisions': []}
Q['constraints'] = copy.copy(P['constraints'])
self.add_constraint(constraint, Q['constraints'])
Q['paths'] = copy.copy(P['paths'])
agent = constraint['agent']
new_path = a_star(self.my_map, self.starts[agent], self.goals[agent], self.heuristics[agent], agent, Q['constraints'])
# find the violated agent given the new path with positive constraints (valid when disjoint == True)
if disjoint and 'positive' in constraint:
violate_agents = paths_violate_constraint(constraint, Q['paths'])
for c_neg_idx, neg_constraint in enumerate(constraints):
if 'positive' not in neg_constraint:
self.add_constraint(neg_constraint, Q['constraints'])
else:
violate_agents = []
# update other agent path given new negative constraints (valid when disjoint == True)
violate_paths = {}
num_new_paths = 0
for violate_agent in violate_agents:
new_vio_path = a_star(self.my_map, self.starts[violate_agent], self.goals[violate_agent], self.heuristics[violate_agent], violate_agent, Q['constraints'])
violate_paths[violate_agent] = new_vio_path
if new_vio_path is not None and len(new_vio_path) > 0:
num_new_paths += 1
# add new child node if new path of current agent and new paths from other agents exsit.
if len(new_path) > 0 and num_new_paths == len(violate_agents):
# update pathes
Q['paths'][agent] = new_path
for violate_agent in violate_agents:
Q['paths'][violate_agent] = violate_paths[violate_agent]
# update collison and costs
new_collison = detect_collisions(Q['paths'])
Q['collisions'] = new_collison
Q['cost'] = get_sum_of_cost(Q['paths'])
self.push_node(Q)
self.print_results(root)
return root['paths']
def find_solution(self, disjoint=True):
""" Finds paths for all agents from their start locations to their goal locations
disjoint - use disjoint splitting or not
"""
self.start_time = timer.time()
# Generate the root node
# constraints - list of constraints
# paths - list of paths, one for each agent
# [[(x11, y11), (x12, y12), ...], [(x21, y21), (x22, y22), ...], ...]
# collisions - list of collisions in paths
root = {'cost': 0, 'constraints': [], 'paths': [], 'collisions': []}
for i in range(self.num_of_agents): # Find initial path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, root['constraints'])
if path is None:
raise BaseException('No solutions')
root['paths'].append(path)
root['cost'] = get_sum_of_cost(root['paths'])
root['collisions'] = detect_collisions(root['paths'])
self.push_node(root)
##############################
# Task 3.3: High-Level Search
# Repeat the following as long as the open list is not empty:
# 1. Get the next node from the open list (you can use self.pop_node()
# 2. If this node has no collision, return solution
# 3. Otherwise, choose the first collision and convert to a list of constraints (using your
# standard_splitting function). Add a new child node to your open list for each constraint
# Ensure to create a copy of any objects that your child nodes might inherit
split_func = disjoint_splitting if disjoint else standard_splitting
while self.open_list:
p = self.pop_node()
# print(p['paths'])
# print(p['constraints'])
# print(p['collisions'])
if not p['collisions']:
solve_time = self.print_results(p)
return p['paths'], solve_time
collision = p['collisions'][0]
constraints = split_func(collision)
# print(constraints)
for constraint in constraints:
q = { # 'cost': 0,
'constraints':
p['constraints'] + [constraint] if constraint
not in p['constraints'] else list(p['constraints']),
'paths':
list(p['paths']),
# 'collisions': []
}
if constraint['positive']:
agents = paths_violate_constraint(constraint, q['paths'])
paths_exist = True
for i in agents:
path = a_star(self.my_map, self.starts[i],
self.goals[i], self.heuristics[i], i,
q['constraints'])
if not path:
paths_exist = False
break
q['paths'][i] = path
if paths_exist:
q['collisions'] = detect_collisions(q['paths'])
q['cost'] = get_sum_of_cost(q['paths'])
self.push_node(q)
else:
i = constraint['agent']
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, q['constraints'])
if path:
q['paths'][i] = path
q['collisions'] = detect_collisions(q['paths'])
q['cost'] = get_sum_of_cost(q['paths'])
self.push_node(q)
raise BaseException('No solutions')
def find_solution(self, disjoint=True, mode=None):
""" Finds paths for all agents from their start locations to their goal locations
disjoint - use disjoint splitting or not
"""
if mode is None:
self.mode = CBSSolver.NORMAL
else:
self.mode = mode
self.start_time = timer.time()
# Generate the root node
# constraints - list of constraints
# paths - list of paths, one for each agent
# [[(x11, y11), (x12, y12), ...], [(x21, y21), (x22, y22), ...], ...]
# collisions - list of collisions in paths
root = {'cost': 0, 'constraints': [], 'paths': [], 'collisions': []}
for i in range(self.num_of_agents): # Find initial path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, root['constraints'])
if path is None:
raise BaseException('No solutions')
root['paths'].append(path)
root['cost'] = get_sum_of_cost(root['paths'])
root['collisions'] = detect_collisions(root['paths'])
self.push_node(root)
##############################
# Task 3.3: High-Level Search
# Repeat the following as long as the open list is not empty:
# 1. Get the next node from the open list (you can use self.pop_node()
# 2. If this node has no collision, return solution
# 3. Otherwise, choose the first collision and convert to a list of constraints (using your
# standard_splitting function). Add a new child node to your open list for each constraint
# Ensure to create a copy of any objects that your child nodes might inherit
while self.open_list:
node = self.pop_node()
if self.mode == CBSSolver.DIAGNOSTIC:
self.print_node(node)
collisions = detect_collisions(node['paths'])
if not collisions:
self.print_results(node)
return node['paths']
first_collision = collisions[0]
constraints = standard_splitting(first_collision)
for constraint in constraints:
child = {
'cost': 0,
'constraints': [],
'paths': [],
'collisions': []
}
child['constraints'] = node['constraints'].copy()
child['constraints'].append(constraint)
child['paths'] = node['paths'].copy()
agent_id = constraint['agent']
new_path = a_star(self.my_map, self.starts[agent_id],
self.goals[agent_id],
self.heuristics[agent_id], agent_id,
child['constraints'])
if new_path is not None:
child['paths'][agent_id] = new_path
child['collisions'] = detect_collisions(child['paths'])
child['cost'] = get_sum_of_cost(child['paths'])
self.push_node(child)
print('\n No Solution Found! \n')
CPU_time = timer.time() - self.start_time
print("CPU time (s): {:.2f}".format(CPU_time))
return None
def find_solution(self, disjoint=True):
""" Finds paths for all agents from their start locations to their goal locations
disjoint - use disjoint splitting or not
"""
self.start_time = timer.time()
# Generate the root node
# constraints - list of constraints
# paths - list of paths, one for each agent
# [[(x11, y11), (x12, y12), ...], [(x21, y21), (x22, y22), ...], ...]
# collisions - list of collisions in paths
root = {'cost': 0, 'constraints': [], 'paths': [], 'collisions': []}
for i in range(self.num_of_agents): # Find initial path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, root['constraints'])
if path is None:
raise BaseException('No solutions')
root['paths'].append(path)
root['cost'] = get_sum_of_cost(root['paths'])
root['collisions'] = detect_collisions(root['paths'])
self.push_node(root)
# Task 3.1: Testing
print(root['collisions'])
# Task 3.2: Testing
for collision in root['collisions']:
print(standard_splitting(collision))
##############################
# Task 3.3: High-Level Search
# Repeat the following as long as the open list is not empty:
# 1. Get the next node from the open list (you can use self.pop_node()
# 2. If this node has no collision, return solution
# 3. Otherwise, choose the first collision and convert to a list of constraints (using your
# standard_splitting function). Add a new child node to your open list for each constraint
# Ensure to create a copy of any objects that your child nodes might inherit
# loop while open_list is not empty
while len(self.open_list) > 0:
# pop node from the open list
node = self.pop_node()
print("Expanded Nodes: " + str(self.num_of_expanded))
# check if there are any collisions
if len(node['collisions']) == 0:
return node['paths']
# choose the first collision
collision = node['collisions'][0]
# use standard_splitting to create a list of constraints
# constraints = standard_splitting(collision)
# use disjoint_splitting to create a list of constraints
constraints = disjoint_splitting(collision)
for constraint in constraints:
# Q is new node -> constraints, paths, collisions, cost
Q = {
'constraints': [],
'paths': [],
'collisions': [],
'cost': 0
}
# was overwriting all ['constraints'] by not creating a deep copy lmao
Q['constraints'] = copy.deepcopy(node['constraints'])
Q['constraints'].append(constraint)
Q['paths'] = copy.deepcopy(node['paths'])
agent = constraint['agent']
path = a_star(self.my_map, self.starts[agent],
self.goals[agent], self.heuristics[agent], agent,
Q['constraints'])
# print("Path: " + str(path))
if path is not None:
Q['paths'][agent] = path
# use paths_violate_constraint to create a new path list of the paths that violate constraint
if constraint['positive'] == True:
constraintPath = paths_violate_constraint(
constraint, Q['paths'])
# print("here")
# print(constraintPath)
for disjointAgent in constraintPath:
constraint = {
'agent': disjointAgent,
'loc': constraint['loc'],
'timestep': constraint['timestep']
}
Q['constraints'].append(constraint)
disjointPath = a_star(
self.my_map, self.starts[disjointAgent],
self.goals[disjointAgent],
self.heuristics[disjointAgent], disjointAgent,
Q['constraints'])
# Condition to not add constraint if no such path exists
# break and push the new node Q
if disjointPath is None:
break
# keep same
Q['collisions'] = detect_collisions(Q['paths'])
Q['cost'] = get_sum_of_cost(Q['paths'])
self.push_node(Q)
raise BaseException('No Solutions')
self.print_results(root)
return root['paths']
def find_solution(self):
""" Finds paths for all agents from their start locations to their goal locations."""
start_time = timer.time()
result = []
constraints = []
# for testing Task 1.2 Handling Vertex Constraints
# constraints.append({'agent': 0, 'loc': [(1, 5)], 'timestep': 4})
# for testing Task 1.3 Handling Edge Constraints
# constraints.append({'agent': 1, 'loc': [(1, 2), (1, 3)], 'timestep': 1})
# for testing Task 1.4 Handling Goal Constraints
# constraints.append({'agent': 0, 'loc': [(1, 5)], 'timestep': 10})
# for testing Task 1.5 Designing Constraints
# constraints = [
# {'agent': 1, 'loc': [(1, 1)], 'timestep': 2},
# {'agent': 1, 'loc': [(1, 2)], 'timestep': 2},
# {'agent': 1, 'loc': [(1, 3)], 'timestep': 2},
# {'agent': 1, 'loc': [(1, 4)], 'timestep': 2}
# ]
for i in range(self.num_of_agents): # Find path for each agent
path = a_star(self.my_map, self.starts[i], self.goals[i],
self.heuristics[i], i, constraints)
if path is None:
raise BaseException('No solutions')
result.append(path)
##############################
# Task 2: Add constraints here
# Useful variables:
# * path contains the solution path of the current (i'th) agent, e.g., [(1,1),(1,2),(1,3)]
# * self.num_of_agents has the number of total agents
# * constraints: array of constraints to consider for future A* searches
# iterate over the path of the current agent
for path_idx in range(len(path)):
for agent in range(i + 1, self.num_of_agents):
# add vertex constraints for all future agents
constraints.append({
'agent': agent,
'loc': [path[path_idx]],
'timestep': path_idx
})
# add edge constraints for all future agents
if path_idx > 0:
constraints.append({
'agent':
agent,
'loc': [path[path_idx], path[path_idx - 1]],
'timestep':
path_idx
})
##############################
self.CPU_time = timer.time() - start_time
print("\n Found a solution! \n")
print("CPU time (s): {:.2f}".format(self.CPU_time))
print("Sum of costs: {}".format(get_sum_of_cost(result)))
print(result)
return result
