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