def validate_new_transition(self, prev_pos: IntVector2D,
current_pos: IntVector2D, new_pos: IntVector2D,
end_pos: IntVector2D):
"""
Utility function to test that a path drawn by a-start algorithm uses valid transition objects.
We us this to quide a-star as there are many transition elements that are not allowed in RailEnv
:param prev_pos: The previous position we were checking
:param current_pos: The current position we are checking
:param new_pos: Possible child position we move into
:param end_pos: End cell of path we are drawing
:return: True if the transition is valid, False if transition element is illegal
"""
# start by getting direction used to get to current node
# and direction from current node to possible child node
new_dir = get_direction(current_pos, new_pos)
if prev_pos is not None:
current_dir = get_direction(prev_pos, current_pos)
else:
current_dir = new_dir
# create new transition that would go to child
new_trans = self.grid[current_pos]
if prev_pos is None:
if new_trans == 0:
# need to flip direction because of how end points are defined
new_trans = self.transitions.set_transition(
new_trans, mirror(current_dir), new_dir, 1)
else:
# check if matches existing layout
new_trans = self.transitions.set_transition(
new_trans, current_dir, new_dir, 1)
else:
# set the forward path
new_trans = self.transitions.set_transition(
new_trans, current_dir, new_dir, 1)
# set the backwards path
new_trans = self.transitions.set_transition(
new_trans, mirror(new_dir), mirror(current_dir), 1)
if Vec2d.is_equal(new_pos, end_pos):
# need to validate end pos setup as well
new_trans_e = self.grid[end_pos]
if new_trans_e == 0:
# need to flip direction because of how end points are defined
new_trans_e = self.transitions.set_transition(
new_trans_e, new_dir, mirror(new_dir), 1)
else:
# check if matches existing layout
new_trans_e = self.transitions.set_transition(
new_trans_e, new_dir, new_dir, 1)
if not self.transitions.is_valid(new_trans_e):
return False
# is transition is valid?
return self.transitions.is_valid(new_trans)
def connect_straight_line_in_grid_map_(
grid_map: GridTransitionMap, path: IntVector2DArray,
rail_trans: RailEnvTransitions) -> IntVector2DArray:
"""
Generates a straight rail line from start cell to end cell.
Diagonal lines are not allowed
:param rail_trans:
:param grid_map:
:param start: Cell coordinates for start of line
:param end: Cell coordinates for end of line
:return: A list of all cells in the path
"""
plen = len(path)
to_return = path.copy()
if plen < 2:
return []
if path[0] == path[1]:
path = path[1:]
to_return.remove(path[0])
current_dir = get_direction(path[0], path[1])
end_pos = path[-1]
for index in range(len(path) - 1):
current_pos = path[index]
new_pos = path[index + 1]
if current_pos == new_pos:
to_return.remove(current_pos)
continue
new_dir = get_direction(current_pos, new_pos)
try:
new_trans = grid_map.grid[current_pos]
except:
import pdb
pdb.set_trace()
new_trans = rail_trans.set_transition(new_trans, current_dir, new_dir,
1)
new_trans = rail_trans.set_transition(new_trans, mirror(new_dir),
mirror(current_dir), 1)
grid_map.grid[current_pos] = new_trans
if new_pos == end_pos:
new_trans_e = grid_map.grid[end_pos]
new_trans_e = rail_trans.set_transition(new_trans_e, new_dir,
new_dir, 1)
grid_map.grid[end_pos] = new_trans_e
current_dir = new_dir
return to_return
def find_alternative(env, possible_transitions, agent_pos, agent_dir,
prediction):
# Approccio naive - se non mi trovo su un fork mi blocco
# altrimenti si potrebbe far ricalcolare uno shortestpath che non consideri il binario su cui si trova il treno che confligge
possible_directions = []
neighbours = []
for j, branch_direction in enumerate([(agent_dir + j) % 4
for j in range(-1, 3)]):
if possible_transitions[branch_direction]:
possible_directions.append(branch_direction)
for direction in possible_directions:
neighbour_cell = get_new_position(agent_pos, direction)
new_direction = get_direction(pos1=agent_pos, pos2=neighbour_cell)
neighbours.append((neighbour_cell, new_direction))
# Compute all possible moves except the ones of the shortest path
next_cell = (prediction[0][0], prediction[0][1])
neighbours = [n for n in neighbours if next_cell not in n]
actions = [
get_action_for_move(agent_pos, agent_dir, n[0], n[1], env.rail)
for n in neighbours
]
return actions
def test_get_direction():
assert get_direction((0, 0), (0, 1)) == Grid4TransitionsEnum.EAST
assert get_direction((0, 0), (0, 2)) == Grid4TransitionsEnum.EAST
assert get_direction((0, 0), (1, 0)) == Grid4TransitionsEnum.SOUTH
assert get_direction((1, 0), (0, 0)) == Grid4TransitionsEnum.NORTH
assert get_direction((1, 0), (0, 0)) == Grid4TransitionsEnum.NORTH
with pytest.raises(Exception, match="Could not determine direction"):
get_direction((0, 0), (0, 0))
def connect_rail_in_grid_map(grid_map: GridTransitionMap, start: IntVector2D, end: IntVector2D,
rail_trans: RailEnvTransitions,
a_star_distance_function: IntVector2DDistance = Vec2d.get_manhattan_distance,
flip_start_node_trans: bool = False, flip_end_node_trans: bool = False,
respect_transition_validity: bool = True, forbidden_cells: IntVector2DArray = None,
avoid_rail=False) -> IntVector2DArray:
"""
Creates a new path [start,end] in `grid_map.grid`, based on rail_trans, and
returns the path created as a list of positions.
:param avoid_rail:
:param rail_trans: basic rail transition object
:param grid_map: grid map
:param start: start position of rail
:param end: end position of rail
:param flip_start_node_trans: make valid start position by adding dead-end, empty start if False
:param flip_end_node_trans: make valid end position by adding dead-end, empty end if False
:param respect_transition_validity: Only draw rail maps if legal rail elements can be use, False, draw line without
respecting rail transitions.
:param a_star_distance_function: Define what distance function a-star should use
:param forbidden_cells: cells to avoid when drawing rail. Rail cannot go through this list of cells
:return: List of cells in the path
"""
path: IntVector2DArray = a_star(grid_map, start, end, a_star_distance_function, avoid_rail,
respect_transition_validity,
forbidden_cells)
if len(path) < 2:
return []
current_dir = get_direction(path[0], path[1])
end_pos = path[-1]
for index in range(len(path) - 1):
current_pos = path[index]
new_pos = path[index + 1]
new_dir = get_direction(current_pos, new_pos)
new_trans = grid_map.grid[current_pos]
if index == 0:
if new_trans == 0:
# end-point
if flip_start_node_trans:
# need to flip direction because of how end points are defined
new_trans = rail_trans.set_transition(new_trans, mirror(current_dir), new_dir, 1)
else:
new_trans = 0
else:
# into existing rail
new_trans = rail_trans.set_transition(new_trans, current_dir, new_dir, 1)
else:
# set the forward path
new_trans = rail_trans.set_transition(new_trans, current_dir, new_dir, 1)
# set the backwards path
new_trans = rail_trans.set_transition(new_trans, mirror(new_dir), mirror(current_dir), 1)
grid_map.grid[current_pos] = new_trans
if new_pos == end_pos:
# setup end pos setup
new_trans_e = grid_map.grid[end_pos]
if new_trans_e == 0:
# end-point
if flip_end_node_trans:
new_trans_e = rail_trans.set_transition(new_trans_e, new_dir, mirror(new_dir), 1)
else:
new_trans_e = 0
else:
# into existing rail
new_trans_e = rail_trans.set_transition(new_trans_e, new_dir, new_dir, 1)
grid_map.grid[end_pos] = new_trans_e
current_dir = new_dir
return path
def generator(width: int,
height: int,
num_agents: int,
num_resets: int = 0,
np_random: RandomState = None) -> RailGenerator:
if num_agents > nr_start_goal:
num_agents = nr_start_goal
print(
"complex_rail_generator: num_agents > nr_start_goal, changing num_agents"
)
grid_map = GridTransitionMap(width=width,
height=height,
transitions=RailEnvTransitions())
rail_array = grid_map.grid
rail_array.fill(0)
# generate rail array
# step 1:
# - generate a start and goal position
# - validate min/max distance allowed
# - validate that start/goals are not placed too close to other start/goals
# - draw a rail from [start,goal]
# - if rail crosses existing rail then validate new connection
# - possibility that this fails to create a path to goal
# - on failure generate new start/goal
#
# step 2:
# - add more rails to map randomly between cells that have rails
# - validate all new rails, on failure don't add new rails
#
# step 3:
# - return transition map + list of [start_pos, start_dir, goal_pos] points
#
rail_trans = grid_map.transitions
start_goal = []
start_dir = []
nr_created = 0
created_sanity = 0
sanity_max = 9000
while nr_created < nr_start_goal and created_sanity < sanity_max:
all_ok = False
for _ in range(sanity_max):
start = (np_random.randint(0, height),
np_random.randint(0, width))
goal = (np_random.randint(0,
height), np_random.randint(0, width))
# check to make sure start,goal pos is empty?
if rail_array[goal] != 0 or rail_array[start] != 0:
continue
# check min/max distance
dist_sg = distance_on_rail(start, goal)
if dist_sg < min_dist:
continue
if dist_sg > max_dist:
continue
# check distance to existing points
sg_new = [start, goal]
def check_all_dist(sg_new):
"""
Function to check the distance betweens start and goal
:param sg_new: start and goal tuple
:return: True if distance is larger than 2, False otherwise
"""
for sg in start_goal:
for i in range(2):
for j in range(2):
dist = distance_on_rail(sg_new[i], sg[j])
if dist < 2:
return False
return True
if check_all_dist(sg_new):
all_ok = True
break
if not all_ok:
# we might as well give up at this point
break
new_path = connect_rail_in_grid_map(
grid_map,
start,
goal,
rail_trans,
Vec2d.get_chebyshev_distance,
flip_start_node_trans=True,
flip_end_node_trans=True,
respect_transition_validity=True,
forbidden_cells=None)
if len(new_path) >= 2:
nr_created += 1
start_goal.append([start, goal])
start_dir.append(
mirror(get_direction(new_path[0], new_path[1])))
else:
# after too many failures we will give up
created_sanity += 1
# add extra connections between existing rail
created_sanity = 0
nr_created = 0
while nr_created < nr_extra and created_sanity < sanity_max:
all_ok = False
for _ in range(sanity_max):
start = (np_random.randint(0, height),
np_random.randint(0, width))
goal = (np_random.randint(0,
height), np_random.randint(0, width))
# check to make sure start,goal pos are not empty
if rail_array[goal] == 0 or rail_array[start] == 0:
continue
else:
all_ok = True
break
if not all_ok:
break
new_path = connect_rail_in_grid_map(
grid_map,
start,
goal,
rail_trans,
Vec2d.get_chebyshev_distance,
flip_start_node_trans=True,
flip_end_node_trans=True,
respect_transition_validity=True,
forbidden_cells=None)
if len(new_path) >= 2:
nr_created += 1
else:
# after too many failures we will give up
created_sanity += 1
return grid_map, {
'agents_hints': {
'start_goal': start_goal,
'start_dir': start_dir
}
}
def generator() -> RailGenerator:
"""
Arguments are ignored and taken directly from the curriculum except the np_random: RandomState which is the last
argument (args[-1])
"""
if curriculum.get("n_agents") > curriculum.get("n_cities"):
raise Exception("complex_rail_generator: n_agents > n_cities!")
grid_map = GridTransitionMap(width=curriculum.get("x_dim"),
height=curriculum.get("y_size"),
transitions=RailEnvTransitions())
rail_array = grid_map.grid
rail_array.fill(0)
# generate rail array
# step 1:
# - generate a start and goal position
# - validate min/max distance allowed
# - validate that start/goals are not placed too close to other start/goals
# - draw a rail from [start,goal]
# - if rail crosses existing rail then validate new connection
# - possibility that this fails to create a path to goal
# - on failure generate new start/goal
#
# step 2:
# - add more rails to map randomly between cells that have rails
# - validate all new rails, on failure don't add new rails
#
# step 3:
# - return transition map + list of [start_pos, start_dir, goal_pos] points
#
rail_trans = grid_map.transitions
start_goal = []
start_dir = []
nr_created = 0
created_sanity = 0
sanity_max = 9000
free_cells = set([(r, c) for r, row in enumerate(rail_array)
for c, col in enumerate(row) if col == 0])
while nr_created < curriculum.get(
"n_cities") and created_sanity < sanity_max:
all_ok = False
if len(free_cells) == 0:
break
for _ in range(sanity_max):
start = random.sample(free_cells, 1)[0]
goal = random.sample(free_cells, 1)[0]
# check min/max distance
dist_sg = distance_on_rail(start, goal)
if dist_sg < curriculum.get("min_dist"):
continue
if dist_sg > curriculum.get("max_dist"):
continue
# check distance to existing points
sg_new = [start, goal]
def check_all_dist():
"""
Function to check the distance betweens start and goal
:param sg_new: start and goal tuple
:return: True if distance is larger than 2, False otherwise
"""
for sg in start_goal:
for i in range(2):
for j in range(2):
dist = distance_on_rail(sg_new[i], sg[j])
if dist < 2:
return False
return True
if check_all_dist():
all_ok = True
free_cells.remove(start)
free_cells.remove(goal)
break
if not all_ok:
# we might as well give up at this point
break
new_path = connect_rail_in_grid_map(
grid_map,
start,
goal,
rail_trans,
Vec2d.get_chebyshev_distance,
flip_start_node_trans=True,
flip_end_node_trans=True,
respect_transition_validity=True,
forbidden_cells=None)
if len(new_path) >= 2:
nr_created += 1
start_goal.append([start, goal])
start_dir.append(
mirror(get_direction(new_path[0], new_path[1])))
else:
# after too many failures we will give up
created_sanity += 1
# add extra connections between existing rail
created_sanity = 0
nr_created = 0
while nr_created < curriculum.get(
"n_extra") and created_sanity < sanity_max:
if len(free_cells) == 0:
break
for _ in range(sanity_max):
start = random.sample(free_cells, 1)[0]
goal = random.sample(free_cells, 1)[0]
new_path = connect_rail_in_grid_map(
grid_map,
start,
goal,
rail_trans,
Vec2d.get_chebyshev_distance,
flip_start_node_trans=True,
flip_end_node_trans=True,
respect_transition_validity=True,
forbidden_cells=None)
if len(new_path) >= 2:
nr_created += 1
else:
# after too many failures we will give up
created_sanity += 1
return grid_map, {
'agents_hints': {
'start_goal': start_goal,
'start_dir': start_dir
}
}