def test_vec2d_norm():
node_a = (1, 2)
node_b = (1, -2)
res_1 = Vec2d.get_norm(node_a)
res_2 = Vec2d.get_norm(node_b)
assert np.sqrt(1 * 1 + 2 * 2) == res_1
assert np.sqrt(1 * 1 + (-2) * (-2)) == res_2
def test_vec2d_scale():
node_a = (1, 2)
node_b = (1, -2)
res_1 = Vec2d.scale(node_a, 2)
res_2 = Vec2d.scale(node_b, -2.5)
assert res_1 == (2, 4)
assert res_2 == (-2.5, 5)
def test_vec2d_normalize():
node_a = (1, 2)
node_b = (1, -2)
res_1 = Vec2d.normalize(node_a)
res_2 = Vec2d.normalize(node_b)
assert np.isclose(1.0, Vec2d.get_norm(res_1))
assert np.isclose(1.0, Vec2d.get_norm(res_2))
def test_vec2d_round():
node_a = (-1.95, -2.2)
node_b = (1.95, 2.2)
res_1 = Vec2d.round(node_a)
res_2 = Vec2d.round(node_b)
assert res_1 == (-2, -2)
assert res_2 == (2, 2)
def test_vec2d_add():
node_a = (1, 2)
node_b = (2, 3)
res_1 = Vec2d.add(node_a, node_b)
res_2 = Vec2d.add(node_b, node_a)
assert res_1 == res_2
assert res_1 == (3, 5)
def test_vec2d_bound():
node_a = (-1.95, -2.2)
node_b = (1.95, 2.2)
res_1 = Vec2d.bound(node_a, -1, 0)
res_2 = Vec2d.bound(node_b, 2, 2.2)
assert res_1 == (-1, -1)
assert res_2 == (2, 2.2)
def test_vec2d_floor():
node_a = (-1.95, -2.2)
node_b = (1.95, 2.2)
res_1 = Vec2d.floor(node_a)
res_2 = Vec2d.floor(node_b)
assert res_1 == (-2, -3)
assert res_2 == (1, 2)
def test_vec2d_ceil():
node_a = (-1.95, -2.2)
node_b = (1.95, 2.2)
res_1 = Vec2d.ceil(node_a)
res_2 = Vec2d.ceil(node_b)
assert res_1 == (-1, -2)
assert res_2 == (2, 3)
def _create_action_plan_for_agent(self, agent_id, train_run) -> ActionPlan:
action_plan = []
agent = self.env.agents[agent_id]
minimum_cell_time = int(np.ceil(1.0 / agent.speed_data['speed']))
for path_loop, train_run_way_point in enumerate(train_run):
train_run_way_point: TrainRunWayPoint = train_run_way_point
position = train_run_way_point.way_point.position
if Vec2d.is_equal(agent.target, position):
break
next_train_run_way_point: TrainRunWayPoint = train_run[path_loop +
1]
next_position = next_train_run_way_point.way_point.position
if path_loop == 0:
self._add_action_plan_elements_for_first_path_element_of_agent(
action_plan, train_run_way_point, next_train_run_way_point)
continue
just_before_target = Vec2d.is_equal(agent.target, next_position)
self._add_action_plan_elements_for_current_path_element(
action_plan, minimum_cell_time, train_run_way_point,
next_train_run_way_point)
# add a final element
if just_before_target:
self._add_action_plan_elements_for_target_at_path_element_just_before_target(
action_plan, minimum_cell_time, train_run_way_point,
next_train_run_way_point)
return action_plan
def test_vec2d_subtract():
node_a = (1, 2)
node_b = (2, 4)
res_1 = Vec2d.subtract(node_a, node_b)
res_2 = Vec2d.subtract(node_b, node_a)
assert res_1 != res_2
assert res_1 == (-1, -2)
assert res_2 == (1, 2)
def test_vec2d_is_equal():
node_a = (1, 2)
node_b = (2, 4)
node_c = (1, 2)
res_1 = Vec2d.is_equal(node_a, node_b)
res_2 = Vec2d.is_equal(node_a, node_c)
assert not res_1
assert res_2
def test_vec2d_chebyshev_distance():
node_a = (3, -7)
node_0 = (0, 0)
assert Vec2d.get_chebyshev_distance(node_a, node_0) == 7
node_b = (-3, 7)
node_0 = (0, 0)
assert Vec2d.get_chebyshev_distance(node_b, node_0) == 7
node_c = (3, 7)
node_0 = (0, 0)
assert Vec2d.get_chebyshev_distance(node_c, node_0) == 7
def check_path_exists(self, start: IntVector2DArray, direction: int,
end: IntVector2DArray):
"""
Breath first search for a possible path from one node with a certain orientation to a target node.
:param start: Start cell rom where we want to check the path
:param direction: Start direction for the path we are testing
:param end: Cell that we try to reach from the start cell
:return: True if a path exists, False otherwise
"""
visited = OrderedSet()
stack = [(start, direction)]
while stack:
node = stack.pop()
node_position = node[0]
node_direction = node[1]
if Vec2d.is_equal(node_position, end):
return True
if node not in visited:
visited.add(node)
moves = self.get_transitions(node_position[0],
node_position[1], node_direction)
for move_index in range(4):
if moves[move_index]:
stack.append(
(get_new_position(node_position,
move_index), move_index))
return False
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 _closest_neighbour_in_grid4_directions(
current_city_idx: int,
city_positions: IntVector2DArray) -> List[int]:
"""
Finds the closest city in each direction of the current city
Parameters
----------
current_city_idx: int
Index of current city
city_positions: IntVector2DArray
Vector containing the coordinates of all cities
Returns
-------
Returns indices of closest neighbour in every direction NESW
"""
city_distances = []
closest_neighbour: List[int] = [None for i in range(4)]
# compute distance to all other cities
for city_idx in range(len(city_positions)):
city_distances.append(
Vec2dOperations.get_manhattan_distance(
city_positions[current_city_idx],
city_positions[city_idx]))
sorted_neighbours = np.argsort(city_distances)
for neighbour in sorted_neighbours[1:]: # do not include city itself
direction_to_neighbour = direction_to_point(
city_positions[current_city_idx], city_positions[neighbour])
if closest_neighbour[direction_to_neighbour] is None:
closest_neighbour[direction_to_neighbour] = neighbour
# early return once all 4 directions have a closest neighbour
if None not in closest_neighbour:
return closest_neighbour
return closest_neighbour
def _generate_city_connection_points(
city_positions: IntVector2DArray,
city_radius: int,
vector_field: IntVector2DArray,
rails_between_cities: int,
rails_in_city: int = 2,
np_random: RandomState = None
) -> (List[List[List[IntVector2D]]], List[List[List[IntVector2D]]], List[
np.ndarray], List[Grid4TransitionsEnum]):
"""
Generate the city connection points. Internal connection points are used to generate the parallel paths
within the city.
External connection points are used to connect different cities together
Parameters
----------
city_positions: IntVector2DArray
Vector that contains all the positions of the cities
city_radius: int
Radius of each city. Cities are squares with edge length 2 * city_radius + 1
vector_field: IntVector2DArray
Vectorfield of the size of the environment. It is used to generate preferred orienations for each cell.
Each cell contains the prefered orientation of cells. If no prefered orientation is present it is set to -1
rails_between_cities: int
Number of rails that connect out from the city
rails_in_city: int
Number of rails within the city
Returns
-------
inner_connection_points: List of List of length number of cities
Contains all the inner connection points for each boarder of each city.
[North_Points, East_Poinst, South_Points, West_Points]
outer_connection_points: List of List of length number of cities
Contains all the outer connection points for each boarder of the city.
[North_Points, East_Poinst, South_Points, West_Points]
city_orientations: List of length number of cities
Contains all the orientations of cities. This is then used to orient agents according to the rails
city_cells: List
List containing the coordinates of all the cells that belong to a city. This is used by other algorithms
to avoid drawing inter-city-rails through cities.
"""
inner_connection_points: List[List[List[IntVector2D]]] = []
outer_connection_points: List[List[List[IntVector2D]]] = []
city_orientations: List[Grid4TransitionsEnum] = []
city_cells: IntVector2DArray = []
for city_position in city_positions:
# Chose the directions where close cities are situated
neighb_dist = []
for neighbour_city in city_positions:
neighb_dist.append(
Vec2dOperations.get_manhattan_distance(
city_position, neighbour_city))
closest_neighb_idx = argsort(neighb_dist)
# Store the directions to these neighbours and orient city to face closest neighbour
connection_sides_idx = []
idx = 1
if grid_mode:
current_closest_direction = np_random.randint(4)
else:
current_closest_direction = direction_to_point(
city_position, city_positions[closest_neighb_idx[idx]])
connection_sides_idx.append(current_closest_direction)
connection_sides_idx.append((current_closest_direction + 2) % 4)
city_orientations.append(current_closest_direction)
city_cells.extend(
_get_cells_in_city(city_position, city_radius,
city_orientations[-1], vector_field))
# set the number of tracks within a city, at least 2 tracks per city
connections_per_direction = np.zeros(4, dtype=int)
nr_of_connection_points = np_random.randint(2, rails_in_city + 1)
for idx in connection_sides_idx:
connections_per_direction[idx] = nr_of_connection_points
connection_points_coordinates_inner: List[List[IntVector2D]] = [
[] for i in range(4)
]
connection_points_coordinates_outer: List[List[IntVector2D]] = [
[] for i in range(4)
]
number_of_out_rails = np_random.randint(
1,
min(rails_between_cities, nr_of_connection_points) + 1)
start_idx = int(
(nr_of_connection_points - number_of_out_rails) / 2)
for direction in range(4):
connection_slots = np.arange(
nr_of_connection_points) - start_idx
# Offset the rails away from the center of the city
offset_distances = np.arange(nr_of_connection_points) - int(
nr_of_connection_points / 2)
# The clipping helps ofsetting one side more than the other to avoid switches at same locations
# The magic number plus one is added such that all points have at least one offset
inner_point_offset = np.abs(offset_distances) + np.clip(
offset_distances, 0, 1) + 1
for connection_idx in range(
connections_per_direction[direction]):
if direction == 0:
tmp_coordinates = (city_position[0] - city_radius +
inner_point_offset[connection_idx],
city_position[1] +
connection_slots[connection_idx])
out_tmp_coordinates = (
city_position[0] - city_radius, city_position[1] +
connection_slots[connection_idx])
if direction == 1:
tmp_coordinates = (city_position[0] +
connection_slots[connection_idx],
city_position[1] + city_radius -
inner_point_offset[connection_idx])
out_tmp_coordinates = (
city_position[0] +
connection_slots[connection_idx],
city_position[1] + city_radius)
if direction == 2:
tmp_coordinates = (city_position[0] + city_radius -
inner_point_offset[connection_idx],
city_position[1] +
connection_slots[connection_idx])
out_tmp_coordinates = (
city_position[0] + city_radius, city_position[1] +
connection_slots[connection_idx])
if direction == 3:
tmp_coordinates = (city_position[0] +
connection_slots[connection_idx],
city_position[1] - city_radius +
inner_point_offset[connection_idx])
out_tmp_coordinates = (
city_position[0] +
connection_slots[connection_idx],
city_position[1] - city_radius)
connection_points_coordinates_inner[direction].append(
tmp_coordinates)
if connection_idx in range(start_idx, start_idx +
number_of_out_rails):
connection_points_coordinates_outer[direction].append(
out_tmp_coordinates)
inner_connection_points.append(connection_points_coordinates_inner)
outer_connection_points.append(connection_points_coordinates_outer)
return inner_connection_points, outer_connection_points, city_orientations, city_cells
def _connect_cities(
city_positions: IntVector2DArray,
connection_points: List[List[List[IntVector2D]]],
city_cells: IntVector2DArray, rail_trans: RailEnvTransitions,
grid_map: RailEnvTransitions) -> List[IntVector2DArray]:
"""
Connects cities together through rails. Each city connects from its outgoing connection points to the closest
cities. This guarantees that all connection points are used.
Parameters
----------
city_positions: IntVector2DArray
All coordinates of the cities
connection_points: List[List[List[IntVector2D]]]
List of coordinates of all outer connection points
city_cells: IntVector2DArray
Coordinates of all the cells contained in any city. This is used to avoid drawing rails through existing
cities.
rail_trans: RailEnvTransitions
Railway transition objects
grid_map: RailEnvTransitions
The grid map containing the rails. Used to draw new rails
Returns
-------
Returns a list of all the cells (Coordinates) that belong to a rail path. This can be used to access railway
cells later.
"""
all_paths: List[IntVector2DArray] = []
connect_cities = []
connected_points = []
grid4_directions = [
Grid4TransitionsEnum.NORTH, Grid4TransitionsEnum.EAST,
Grid4TransitionsEnum.SOUTH, Grid4TransitionsEnum.WEST
]
for current_city_idx in np.arange(len(city_positions)):
closest_neighbours = _closest_neighbour_in_grid4_directions(
current_city_idx, city_positions)
for out_direction in grid4_directions:
neighbour_idx = get_closest_neighbour_for_direction(
closest_neighbours, out_direction)
if set((current_city_idx, neighbour_idx)) in connect_cities:
continue
for city_out_connection_point in connection_points[
current_city_idx][out_direction]:
city_out_connection_dir = out_direction
min_connection_dist = np.inf
for direction in grid4_directions:
current_points = connection_points[neighbour_idx][
direction]
for tmp_in_connection_point in current_points:
tmp_dist = Vec2dOperations.get_manhattan_distance(
city_out_connection_point,
tmp_in_connection_point)
if tmp_dist < min_connection_dist:
min_connection_dist = tmp_dist
neighbour_connection_point = tmp_in_connection_point
neighbour_connection_dir = direction
if set((*city_out_connection_point,
*neighbour_connection_point)) in connected_points:
continue
lines = _align_start_end(city_out_connection_point, neighbour_connection_point,\
city_out_connection_dir, neighbour_connection_dir, grid_map, rail_trans, city_cells)
if len(city_positions) == 2:
new_line = connect_points(lines, grid_map, rail_trans)
else:
new_line = connect_rail_in_grid_map(
grid_map,
city_out_connection_point,
neighbour_connection_point,
rail_trans,
flip_start_node_trans=False,
flip_end_node_trans=False,
respect_transition_validity=False,
avoid_rail=True,
forbidden_cells=city_cells)
all_paths.extend(new_line)
connect_cities.append(
set((current_city_idx, neighbour_idx)))
connected_points.append(
set((*city_out_connection_point,
*neighbour_connection_point)))
return all_paths
def a_star(grid_map: GridTransitionMap,
start: IntVector2D,
end: IntVector2D,
a_star_distance_function: IntVector2DDistance = Vec2d.
get_manhattan_distance,
avoid_rails=False,
respect_transition_validity=True,
forbidden_cells: IntVector2DArray = None) -> IntVector2DArray:
"""
:param avoid_rails:
:param grid_map: Grid Map where the path is found in
:param start: Start positions as (row,column)
:param end: End position as (row,column)
:param a_star_distance_function: Define the distance function to use as heuristc:
-get_euclidean_distance
-get_manhattan_distance
-get_chebyshev_distance
:param respect_transition_validity: Whether or not a-star respect allowed transitions on the grid map.
- True: Respects the validity of transition. This generates valid paths, of no path if it cannot be found
- False: This always finds a path, but the path might be illegal and thus needs to be fixed afterwards
:param forbidden_cells: List of cells where the path cannot pass through. Used to avoid certain areas of Grid map
:return: IF a path is found a ordered list of al cells in path is returned
"""
"""
Returns a list of tuples as a path from the given start to end.
If no path is found, returns path to closest point to end.
"""
rail_shape = grid_map.grid.shape
start_node = AStarNode(start, None)
end_node = AStarNode(end, None)
open_nodes = OrderedSet()
closed_nodes = OrderedSet()
open_nodes.add(start_node)
while len(open_nodes) > 0:
# get node with current shortest est. path (lowest f)
current_node = None
for item in open_nodes:
if current_node is None:
current_node = item
continue
if item.f < current_node.f:
current_node = item
# pop current off open list, add to closed list
open_nodes.remove(current_node)
closed_nodes.add(current_node)
# found the goal
if current_node == end_node:
path = []
current = current_node
while current is not None:
path.append(current.pos)
current = current.parent
# return reversed path
return path[::-1]
# generate children
children = []
if current_node.parent is not None:
prev_pos = current_node.parent.pos
else:
prev_pos = None
for new_pos in [(0, -1), (0, 1), (-1, 0), (1, 0)]:
# update the "current" pos
node_pos: IntVector2D = Vec2d.add(current_node.pos, new_pos)
# is node_pos inside the grid?
if node_pos[0] >= rail_shape[0] or node_pos[0] < 0 or node_pos[
1] >= rail_shape[1] or node_pos[1] < 0:
continue
# validate positions
#
if not grid_map.validate_new_transition(
prev_pos, current_node.pos, node_pos,
end_node.pos) and respect_transition_validity:
continue
# create new node
new_node = AStarNode(node_pos, current_node)
# Skip paths through forbidden regions if they are provided
if forbidden_cells is not None:
if node_pos in forbidden_cells and new_node != start_node and new_node != end_node:
continue
children.append(new_node)
# loop through children
for child in children:
# already in closed list?
if child in closed_nodes:
continue
# create the f, g, and h values
child.g = current_node.g + 1.0
# this heuristic avoids diagonal paths
if avoid_rails:
child.h = a_star_distance_function(
child.pos, end_node.pos) + np.clip(
grid_map.grid[child.pos], 0, 1)
else:
child.h = a_star_distance_function(child.pos, end_node.pos)
child.f = child.g + child.h
# already in the open list?
if child in open_nodes:
continue
# add the child to the open list
open_nodes.add(child)
# no full path found
if len(open_nodes) == 0:
return []
def test_vec2d_rotate():
node_a = (-1.95, -2.2)
res_1 = Vec2d.rotate(node_a, -90.0)
res_2 = Vec2d.rotate(node_a, 0.0)
res_3 = Vec2d.rotate(node_a, 90.0)
res_4 = Vec2d.rotate(node_a, 180.0)
res_5 = Vec2d.rotate(node_a, 270.0)
res_6 = Vec2d.rotate(node_a, 30.0)
res_1 = (Vec2d.get_norm(Vec2d.subtract(res_1, (-2.2, 1.95))))
res_2 = (Vec2d.get_norm(Vec2d.subtract(res_2, (-1.95, -2.2))))
res_3 = (Vec2d.get_norm(Vec2d.subtract(res_3, (2.2, -1.95))))
res_4 = (Vec2d.get_norm(Vec2d.subtract(res_4, (1.95, 2.2))))
res_5 = (Vec2d.get_norm(Vec2d.subtract(res_5, (-2.2, 1.95))))
res_6 = (Vec2d.get_norm(
Vec2d.subtract(res_6, (-0.5887495373796556, -2.880255888325765))))
assert np.isclose(0, res_1)
assert np.isclose(0, res_2)
assert np.isclose(0, res_3)
assert np.isclose(0, res_4)
assert np.isclose(0, res_5)
assert np.isclose(0, res_6)
def test_vec2d_make_orthogonal():
node_a = (1, 2)
res_1 = Vec2d.make_orthogonal(node_a)
assert res_1 == (2, -1)
def test_vec2d_euclidean_distance():
node_a = (3, -7)
node_0 = (0, 0)
assert Vec2d.get_euclidean_distance(node_a,
node_0) == Vec2d.get_norm(node_a)
def test_vec2d_manhattan_distance():
node_a = (3, -7)
node_0 = (0, 0)
assert Vec2d.get_manhattan_distance(node_a, node_0) == 3 + 7
