def detect_collisions(paths):
##############################
# Task 3.1: Return a list of first collisions between all robot pairs.
# A collision can be represented as dictionary that contains the id of the two robots, the vertex or edge
# causing the collision, and the timestep at which the collision occurred.
# You should use your detect_collision function to find a collision between two robots.
collisions = []
for i in range(len(paths)):
for j in range(i + 1, len(paths)):
timestep, collision_type = detect_collision(paths[i], paths[j])
if timestep:
if collision_type == 'v':
collision = {
'a1': i,
'a2': j,
'loc': [get_location(paths[i], timestep)],
'timestep': timestep
}
collisions.append(collision)
elif collision_type == 'e':
collision = {
'a1':
i,
'a2':
j,
'loc': [
get_location(paths[i], timestep - 1),
get_location(paths[i], timestep)
],
'timestep':
timestep
}
collisions.append(collision)
return collisions
def vertex_collision(t):
# Returns vertex collision, otherwise returns None
agent1_curr_loc = get_location(path1, t)
agent2_curr_loc = get_location(path2, t)
if agent1_curr_loc == agent2_curr_loc:
return {'loc': [agent1_curr_loc], 'timestep': t}
return None
def edge_collision(t):
# Returns edge collision (in a way that blames Agent 1 for
# the collision), otherwise returns None
agent1_prev_loc = get_location(path1, t - 1)
agent2_prev_loc = get_location(path2, t - 1)
agent1_curr_loc = get_location(path1, t)
agent2_curr_loc = get_location(path2, t)
if agent1_prev_loc == agent2_curr_loc and agent2_prev_loc == agent1_curr_loc:
return {'loc': [agent1_prev_loc, agent1_curr_loc], 'timestep': t}
return None
def detect_collision(path1, path2):
##############################
# Task 3.1: Return the first collision that occurs between two robot paths (or None if there is no collision)
# There are two types of collisions: vertex collision and edge collision.
# A vertex collision occurs if both robots occupy the same location at the same timestep
# An edge collision occurs if the robots swap their location at the same timestep.
# You should use "get_location(path, t)" to get the location of a robot at time t.
# if get_location(path1, 0) == get_location(path2, 0):
# return [path1[0]], 0
# edges = zip(pairwise(path1), pairwise(path2))
# for timestep, (edges1, edges2) in enumerate(edges, 1):
# if edges1[1] == edges2[1]:
# return [edges1[1]], timestep
# if edges1 == list(reversed(edges2)):
# return list(edges1), timestep
for timestep in range(max(len(path1), len(path2))):
if get_location(path1, timestep) == get_location(path2, timestep):
return [get_location(path1, timestep)], timestep
if timestep + 1 < min(len(path1), len(path2)):
if [
get_location(path1, timestep),
get_location(path1, timestep + 1)
] == [
get_location(path2, timestep + 1),
get_location(path2, timestep)
]:
return [
get_location(path1, timestep),
get_location(path1, timestep + 1)
], timestep + 1
return None
def detect_collision(path1, path2):
##############################
# Task 3.1: Return the first collision that occurs between two robot paths (or None if there is no collision)
# There are two types of collisions: vertex collision and edge collision.
# A vertex collision occurs if both robots occupy the same location at the same timestep
# An edge collision occurs if the robots swap their location at the same timestep.
# You should use "get_location(path, t)" to get the location of a robot at time t.
max_time = max(len(path1), len(path2))
collision_list = []
for time in range(max_time):
if get_location(path1, time) == get_location(path2, time):
collision_list.append({
'loc': [get_location(path1, time)],
'timestep': time
})
if time == 0:
continue
else:
if [get_location(path1, time - 1),
get_location(path1, time)] == [
get_location(path2, time),
get_location(path2, time - 1)
]:
collision_list.append({
'loc':
[get_location(path1, time - 1),
get_location(path1, time)],
'timestep':
time
})
return collision_list
def detect_collision(path1, path2):
##############################
# Task 3.1: Return the first collision that occurs between two robot paths (or None if there is no collision)
# There are two types of collisions: vertex collision and edge collision.
# A vertex collision occurs if both robots occupy the same location at the same timestep
# An edge collision occurs if the robots swap their location at the same timestep.
# You should use "get_location(path, t)" to get the location of a robot at time t.
# Vertex Collision
if len(path1) > len(path2):
goal_state = path2[len(path2) - 1]
diff = len(path1) - len(path2)
path2 = path2 + [goal_state] * diff
else:
goal_state = path1[len(path1) - 1]
diff = len(path2) - len(path1)
path1 = path1 + [goal_state] * diff
for t in range(len(path1) + 1):
if get_location(path1, t) == get_location(path2, t):
vertex_collision = ([get_location(path1, t)], t)
return vertex_collision
# Edge Collision
for t in range(len(path1)):
if get_location(path1, t) == get_location(
path2, t + 1) and get_location(path1, t + 1) == get_location(
path2, t):
edge_collision = ([
get_location(path1, t),
get_location(path1, t + 1)
], t + 1)
return edge_collision
return None
def detect_collision(path1, path2):
##############################
# Task 3.1: Return the first collision that occurs between two robot paths (or None if there is no collision)
# There are two types of collisions: vertex collision and edge collision.
# A vertex collision occurs if both robots occupy the same location at the same timestep
# An edge collision occurs if the robots swap their location at the same timestep.
# You should use "get_location(path, t)" to get the location of a robot at time t.
maxTimestep = max(len(path1), len(path2))
# run through each timestep in the paths -> length of the longest path
for timestep in range(maxTimestep):
# if the two locations at a time are the same, vertex collision -> and return in a list
# check vertex edge cases
if get_location(path1, timestep) == get_location(path2, timestep):
return {'loc': get_location(path1, timestep), 'timestep': timestep}
# check negative edge cases
# looking at exp2_1.txt
# path1: [1,3] at 3 and path2:[1,4] at 3 -> path2 cannot be at [1,4] at timestep 2 otherwise path1 (agent 1) will not have a collision free path
# then
elif get_location(path1, timestep) == get_location(
path2, timestep - 1) and get_location(
path1, timestep - 1) == get_location(path2, timestep):
# print(get_location(path2, timestep - 1))
# print(get_location(path2, timestep))
return {
'loc': [
get_location(path2, timestep),
get_location(path2, timestep - 1)
],
'timestep':
timestep
}
return {}
def detect_collision(path1, path2):
##############################
# Task 3.1: Return the first collision that occurs between two robot paths (or None if there is no collision)
# There are two types of collisions: vertex collision and edge collision.
# A vertex collision occurs if both robots occupy the same location at the same timestep
# An edge collision occurs if the robots swap their location at the same timestep.
# You should use "get_location(path, t)" to get the location of a robot at time t.
prev_pos1 = None
prev_pos2 = None
longest_path = None # iterate through longest path
if len(path1) >= len(path2):
longest_path = path1
else:
longest_path = path2
for i in range(len(longest_path)):
if get_location(path1, i) == get_location(path2, i): # vertex collision
return [get_location(path1, i), i]
elif get_location(path1, i) == prev_pos2 and get_location(path2, i) == prev_pos1: # edge collision
return [get_location(path1, i), get_location(path2, i), i+1]
prev_pos1 = get_location(path1, i)
prev_pos2 = get_location(path2, i)
return None
def detect_collision(path1, path2):
##############################
# Task 3.1: Return the first collision that occurs between two robot paths (or None if there is no collision)
# There are two types of collisions: vertex collision and edge collision.
# A vertex collision occurs if both robots occupy the same location at the same timestep
# An edge collision occurs if the robots swap their location at the same timestep.
# You should use "get_location(path, t)" to get the location of a robot at time t.
timestepTotal = max(len(path1), len(path2))
collisions = dict()
for i in range(0, timestepTotal):
#for vertex collision
if get_location(path2, i) == get_location(path1, i):
collision = {'loc': [get_location(path1, i)], 'timestep': i}
#for edge collision
if 1 != 0 and get_location(path2, i) == get_location(
path1, i - 1) and get_location(path2, t - 1) == get_location(
path1, t):
collision = {
'loc': [get_location(path2, i - 1),
get_location(path2, i)],
'timestep': i
}
return collisions
def paths_violate_constraint(constraint, paths):
assert constraint['positive'] is True
rst = []
for i in range(len(paths)):
if i == constraint['agent']:
continue
curr = get_location(paths[i], constraint['timestep'])
prev = get_location(paths[i], constraint['timestep'] - 1)
if len(constraint['loc']) == 1: # vertex constraint
if constraint['loc'][0] == curr:
rst.append(i)
else: # edge constraint
if constraint['loc'][0] == prev or constraint['loc'][1] == curr \
or constraint['loc'] == [curr, prev]:
rst.append(i)
return rst
def paths_violate_constraint(paths, constraint):
t = constraint['timestep']
agentsId = []
for i in range(len(paths)):
if i == constraint['agent']:
continue
else:
if constraint['loc'] == [get_location(paths[i], t)]:
agentsId.append(i)
continue
if t == 0:
continue
if constraint['loc'] == [
get_location(paths[i], t),
get_location(paths[i], t - 1)
]:
agentsId.append(i)
return agentsId
def detect_conflict(path1, path2):
##############################
# Task 3.1: Return the first conflict that occurs between two robot paths (or None if there is no conflict)
# There are two types of conflicts: vertex conflict and edge conflict.
# A vertex conflict occurs if both robots occupy the same location at the same timestep
# An edge conflict occurs if the robots swap their location at the same timestep.
# You should use "get_location(path, t)" to get the location of a robot at time t.
for t in range(max(len(path1), len(path2))):
c1 = get_location(path1, t)
c2 = get_location(path2, t)
if c1 == c2:
return [c1, t]
else:
p1 = get_location(path1, t - 1)
p2 = get_location(path2, t - 1)
if c1 == p2 and c2 == p1:
return [p1, c1, t]
return None
def detect_collision(path1, path2):
##############################
# Task 3.1: Return the first collision that occurs between two robot paths (or None if there is no collision)
# There are two types of collisions: vertex collision and edge collision.
# A vertex collision occurs if both robots occupy the same location at the same timestep
# An edge collision occurs if the robots swap their location at the same timestep.
# You should use "get_location(path, t)" to get the location of a robot at time t.
max_timestep = max(len(path1), len(path2))
for t in range(max_timestep):
loc1 = get_location(path1, t)
loc2 = get_location(path2, t)
if loc1 == loc2: # vertex
return {'loc': [loc1], 'timestep': t}
else: # edge
past_loc1 = get_location(path1, t - 1)
past_loc2 = get_location(path2, t - 1)
if past_loc1 == loc2 and past_loc2 == loc1:
return {'loc': [past_loc1, loc1], 'timestep': t}
return None
def detect_collision(path1, path2):
##############################
# Task 3.1: Return the first collision that occurs between two robot paths (or None if there is no collision)
# There are two types of collisions: vertex collision and edge collision.
# A vertex collision occurs if both robots occupy the same location at the same timestep
# An edge collision occurs if the robots swap their location at the same timestep.
# You should use "get_location(path, t)" to get the location of a robot at time t.
collisions = {}
collisions['vertex'] = []
collisions['edge'] = []
# vertex collision
max_time = max(len(path1), len(path2))
for t in range(max_time):
loc_p1 = get_location(path1, t)
loc_p2 = get_location(path2, t)
if loc_equal(loc_p1, loc_p2):
collisions['vertex'].append((loc_p1, t))
# edge collision
for t in range(max_time - 1):
pre_loc_p1 = get_location(path1, t)
cur_loc_p1 = get_location(path1, t+1)
pre_loc_p2 = get_location(path2, t)
cur_loc_p2 = get_location(path2, t+1)
# check the edge swap
if loc_equal(pre_loc_p1, cur_loc_p2) and loc_equal(pre_loc_p2, cur_loc_p1):
collisions['edge'].append((cur_loc_p1, cur_loc_p2, t + 1))
return collisions
def detect_collision(path1, path2):
##############################
# Task 3.1: Return the first collision that occurs between two robot paths (or None if there is no collision)
# There are two types of collisions: vertex collision and edge collision.
# A vertex collision occurs if both robots occupy the same location at the same timestep
# An edge collision occurs if the robots swap their location at the same timestep.
# You should use "get_location(path, t)" to get the location of a robot at time t.
timestep = 0
while timestep < len(path1) or timestep < len(path2):
# detect vertex collision first
loc1 = get_location(path1, timestep)
loc2 = get_location(path2, timestep)
if loc1 == loc2:
return [loc1], timestep
# detect edge collision
if timestep > 0:
loc1_prev = get_location(path1, timestep - 1)
loc2_prev = get_location(path2, timestep - 1)
if loc1 == loc2_prev and loc2 == loc1_prev:
return [loc1_prev, loc2_prev], timestep
timestep += 1
def detect_collision(path1, path2):
##############################
# Task 3.1: Return the first collision that occurs between two robot paths (or None if there is no collision)
# There are two types of collisions: vertex collision and edge collision.
# A vertex collision occurs if both robots occupy the same location at the same timestep
# An edge collision occurs if the robots swap their location at the same timestep.
# You should use "get_location(path, t)" to get the location of a robot at time t.
l1, l2 = len(path1), len(path2)
max_path = max(l1, l2)
# deep copy of path1 and path2
temp_path1, temp_path2 = [], []
for pos in path1:
temp_path1.append(pos)
for pos in path2:
temp_path2.append(pos)
if l1 > l2:
for i in range(l1 - l2):
temp_path2.append(temp_path2[l2 - 1])
else:
for i in range(l2 - l1):
temp_path1.append(temp_path1[l1 - 1])
for i in range(max_path):
# detect vertex collision
if get_location(temp_path1, i) == get_location(temp_path2, i):
return [[get_location(temp_path1, i)], i]
# detect edge collisions
if i < max_path - 1:
if get_location(temp_path1, i) == get_location(
temp_path2, i + 1) and get_location(
temp_path2, i) == get_location(temp_path1, i + 1):
return [[
get_location(temp_path1, i),
get_location(temp_path1, i + 1)
], i + 1]
# no collision between two paths
return None
def detect_collision(path1, path2):
##############################
# Task 3.1: Return the first collision that occurs between two robot paths (or None if there is no collision)
# There are two types of collisions: vertex collision and edge collision.
# A vertex collision occurs if both robots occupy the same location at the same timestep
# An edge collision occurs if the robots swap their location at the same timestep.
# You should use "get_location(path, t)" to get the location of a robot at time t.
for t in range(max(len(path1), len(path2))):
if get_location(path1, t) == get_location(path2, t):
return {"loc": [get_location(path1, t)], "timestep": t}
elif t > 0 and get_location(path1, t - 1) == get_location(
path2, t) and get_location(path1, t) == get_location(
path2, t - 1):
return {
"loc": [get_location(path2, t - 1),
get_location(path2, t)],
"timestep": t
}
return {}
def detect_collision(path1, path2):
##############################
# Task 3.1: Return the first collision that occurs between two robot paths (or None if there is no collision)
# There are two types of collisions: vertex collision and edge collision.
# A vertex collision occurs if both robots occupy the same location at the same timestep
# An edge collision occurs if the robots swap their location at the same timestep.
# You should use "get_location(path, t)" to get the location of a robot at time t.
time = 0
while (time < max(len(path1), len(path2))):
if (get_location(path1, time) == get_location(path2, time)):
# vertex collision
return {'loc': [get_location(path1, time)], 'timestep': time}
if (time + 1 < min(len(path1), len(path2))):
if (get_location(path1, time) == get_location(path2, time + 1)
and get_location(path1, time + 1) == get_location(
path2, time)):
return {
'loc':
[get_location(path1, time),
get_location(path2, time)],
'timestep': time + 1
}
time += 1
return None
def detect_collision(path1, path2):
longest_path_length = max(len(path1), len(path2))
# check for vertex collisions
for index in range(0, longest_path_length):
if get_location(path1, index) == get_location(path2, index):
return {'loc': [get_location(path1, index)], 'timestep': index}
# check for edge collisions
for index in range(0, longest_path_length - 1):
edge1 = [get_location(path1, index), get_location(path1, index + 1)]
edge2 = [get_location(path2, index), get_location(path2, index + 1)]
if edge1 == edge2[::-1]:
return {'loc': edge1, 'timestep': index + 1}
return None
def detect_collision(path1, path2):
##############################
# Task 3.1: Return the first collision that occurs between two robot paths (or None if there is no collision)
# There are two types of collisions: vertex collision and edge collision.
# A vertex collision occurs if both robots occupy the same location at the same timestep
# An edge collision occurs if the robots swap their location at the same timestep.
# You should use "get_location(path, t)" to get the location of a robot at time t.
for i in range(max(len(path1), len(path2))):
if get_location(path1, i) == get_location(path2, i):
return i, 'v'
elif get_location(path1, i) == get_location(
path2, i + 1) and get_location(path1, i + 1) == get_location(
path2, i):
return i + 1, 'e'
return None, None
def detect_collision(path1, path2):
##############################
# Task 3.1: Return the first collision that occurs between two robot paths (or None if there is no collision)
# There are two types of collisions: vertex collision and edge collision.
# A vertex collision occurs if both robots occupy the same location at the same timestep
# An edge collision occurs if the robots swap their location at the same timestep.
# You should use "get_location(path, t)" to get the location of a robot at time t.
for time in range(max(len(path1), len(path2))):
if get_location(path1, time) == get_location(path2,
time): # Vertex collision
return {'loc': [get_location(path1, time)], 'time_step': time}
for time in range(max(len(path1), len(path2)) - 1):
if get_location(path1,
time) == get_location(path2,
time + 1): # Edge collision
return {
'loc':
[get_location(path1, time),
get_location(path2, time + 1)],
'time_step': time
}
if get_location(path1,
time + 1) == get_location(path2,
time): # Edge collision
return {
'loc':
[get_location(path2, time),
get_location(path1, time + 1)],
'time_step': time
}
return None
