def test_intersection_exists():
line1 = Line(Point(5, 0), Point(0, 10))
line2 = Line(Point(2, 0), Point(7, 5))
intersection = line1.get_intersection(line2)
assert intersection == Point(4, 2)
def _create_scan_lines(self) -> np.ndarray:
scan_poses = self._create_scan_poses()
scan_lines = np.empty(self._N_MEASUREMENTS, dtype=Line)
for i, scan_pose in enumerate(scan_poses):
scan_pose.move(self._SCAN_RANGE_MAX)
scan_lines[i] = Line(copy.copy(self._pose.position),
scan_pose.position)
return scan_lines
def test_intersection_vertical_line():
line1 = Line(Point(5, 0), Point(0, 10))
line2 = Line(Point(2, 0), Point(2, 10))
intersection1 = line1.get_intersection(line2)
intersection2 = line2.get_intersection(line1)
assert intersection1 == intersection2 == Point(2, 6)
def test_intersection_horizontal_line():
line1 = Line(Point(5, 0), Point(0, 10))
line2 = Line(Point(0, 2), Point(10, 2))
intersection1 = line1.get_intersection(line2)
intersection2 = line2.get_intersection(line1)
assert intersection1 == intersection2 == Point(4, 2)
def _init_obstacles(self) -> None:
self._track = self._TRACKS[self._track_id - 1]
# Don't check for overlapping obstacles
# in order to create strange shapes.
for _ in range(self._N_OBSTACLES):
while True:
area = random.choice(self._spawn_area)
x_coordinate = random.uniform(area[0][0], area[0][1])
y_coordinate = random.uniform(area[1][0], area[1][1])
obstacles_center = Point(x_coordinate, y_coordinate)
distance_from_pose = obstacles_center.calculate_distance(
self._pose.position)
distance_from_goal = obstacles_center.calculate_distance(
self._goal)
if (distance_from_pose > self._MINIMUM_DISTANCE
or distance_from_goal < self._MINIMUM_DISTANCE):
break
point1 = Point(
obstacles_center.x_coordinate - self._OBSTACLES_LENGTH / 2,
obstacles_center.y_coordinate - self._OBSTACLES_LENGTH / 2)
point2 = Point(
obstacles_center.x_coordinate - self._OBSTACLES_LENGTH / 2,
obstacles_center.y_coordinate + self._OBSTACLES_LENGTH / 2)
point3 = Point(
obstacles_center.x_coordinate + self._OBSTACLES_LENGTH / 2,
obstacles_center.y_coordinate + self._OBSTACLES_LENGTH / 2)
point4 = Point(
obstacles_center.x_coordinate + self._OBSTACLES_LENGTH / 2,
obstacles_center.y_coordinate - self._OBSTACLES_LENGTH / 2)
self._track += (Line(point1, point2), )
self._track += (Line(point2, point3), )
self._track += (Line(point3, point4), )
self._track += (Line(point4, point1), )
def test_correct_slope_and_y_intercept_horizontal_line():
line = Line(Point(0, 2), Point(10, 2))
assert line.slope == 0 and line.y_intercept == 2
def test_correct_slope_and_y_intercept_vertical_line():
line = Line(Point(2, 0), Point(2, 10))
assert line.slope == 0 and line.y_intercept is math.inf
def test_line_equality():
line1 = Line(Point(5, 0), Point(0, 10))
line2 = Line(Point(5, 0), Point(0, 10))
assert line1 == line2
def test_correct_slope_and_y_intercept():
line = Line(Point(2, 0), Point(7, 5))
assert line.slope == 1 and line.y_intercept == -2
def test_not_contains():
line = Line(Point(2, 0), Point(7, 5))
assert not line.contains(Point(0, -2))
def test_contains():
line = Line(Point(2, 0), Point(7, 5))
assert line.contains(Point(4, 2))
def test_intersection_parallel_lines():
line1 = Line(Point(2, 0), Point(7, 5))
line2 = Line(Point(1, 4), Point(2, 5))
with pytest.raises(NoIntersectionError):
_ = line1.get_intersection(line2)
def test_intersection_not_in_line():
line1 = Line(Point(5, 0), Point(0, 10))
line2 = Line(Point(0, -2), Point(2, 0))
with pytest.raises(NoIntersectionError):
_ = line1.get_intersection(line2)
def test_line_inequality():
line1 = Line(Point(5, 0), Point(0, 10))
line2 = Line(Point(0, -2), Point(2, 0))
assert line1 != line2
class NavigationGoal(NavigationTrack):
"""The Navigation Goal environment."""
_GOAL_THRESHOLD = 0.4
_MINIMUM_DISTANCE = 3
_ANGLE_STANDARD_DEVIATION = 0.02
_DISTANCE_STANDARD_DEVIATION = 0.02
_TRANSITION_REWARD_FACTOR = 10
_GOAL_REWARD = 200.0
_MAXIMUM_GOAL_DISTANCE = math.inf
_N_OBSERVATIONS = NavigationTrack._N_MEASUREMENTS + 2
_N_OBSTACLES = 20
_OBSTACLES_LENGTH = 1
_TRACK1 = (Line(Point(-10, -10),
Point(-10, 10)), Line(Point(-10, 10), Point(10, 10)),
Line(Point(10, 10),
Point(10, -10)), Line(Point(10, -10), Point(-10, -10)))
_TRACKS = (_TRACK1, )
_SPAWN_AREA1 = (((-9, 9), (-9, 9)), )
_SPAWN_AREAS = (_SPAWN_AREA1, )
_track_id: int
_distance_from_goal: float
_goal: Point
_observation: np.ndarray
def __init__(self, track_id: int = 1) -> None:
super().__init__(track_id)
self._track_id = track_id
high = np.array(self._N_MEASUREMENTS * [self._SCAN_RANGE_MAX] +
[self._MAXIMUM_GOAL_DISTANCE] + [math.pi],
dtype=np.float32)
low = np.array(self._N_MEASUREMENTS * [self._SCAN_RANGE_MIN] + [0.0] +
[-math.pi],
dtype=np.float32)
self.observation_space = spaces.Box(low=low,
high=high,
shape=(self._N_OBSERVATIONS, ),
dtype=np.float32)
def _do_check_if_done(self) -> bool:
return (self._collision_occurred()
or self._distance_from_goal < self._GOAL_THRESHOLD)
def _do_calculate_reward(self, action: int) -> float:
if self._collision_occurred():
reward = self._COLLISION_REWARD
elif self._distance_from_goal < self._GOAL_THRESHOLD:
reward = self._GOAL_REWARD
else:
reward = (self._TRANSITION_REWARD_FACTOR *
(self._observation[-2] - self._distance_from_goal))
return reward
def _do_update_observation(self) -> None:
self._update_scan()
self._distance_from_goal = (
self._DISTANCE_STANDARD_DEVIATION +
self._pose.position.calculate_distance(self._goal))
angle_from_goal = (self._ANGLE_STANDARD_DEVIATION +
self._pose.calculate_angle_difference(self._goal))
self._observation = np.append(
self._ranges, [self._distance_from_goal, angle_from_goal])
def _do_init_environment(self) -> None:
self._init_pose()
self._init_goal()
self._init_obstacles()
def _init_goal(self) -> None:
while True:
area = random.choice(self._spawn_area)
x_coordinate = random.uniform(area[0][0], area[0][1])
y_coordinate = random.uniform(area[1][0], area[1][1])
goal = Point(x_coordinate, y_coordinate)
distance_from_pose = goal.calculate_distance(self._pose.position)
if distance_from_pose > self._MINIMUM_DISTANCE:
break
self._goal = goal
self._distance_from_goal = self._pose.position.calculate_distance(
self._goal)
def _init_obstacles(self) -> None:
self._track = self._TRACKS[self._track_id - 1]
# Don't check for overlapping obstacles
# in order to create strange shapes.
for _ in range(self._N_OBSTACLES):
while True:
area = random.choice(self._spawn_area)
x_coordinate = random.uniform(area[0][0], area[0][1])
y_coordinate = random.uniform(area[1][0], area[1][1])
obstacles_center = Point(x_coordinate, y_coordinate)
distance_from_pose = obstacles_center.calculate_distance(
self._pose.position)
distance_from_goal = obstacles_center.calculate_distance(
self._goal)
if (distance_from_pose > self._MINIMUM_DISTANCE
or distance_from_goal < self._MINIMUM_DISTANCE):
break
point1 = Point(
obstacles_center.x_coordinate - self._OBSTACLES_LENGTH / 2,
obstacles_center.y_coordinate - self._OBSTACLES_LENGTH / 2)
point2 = Point(
obstacles_center.x_coordinate - self._OBSTACLES_LENGTH / 2,
obstacles_center.y_coordinate + self._OBSTACLES_LENGTH / 2)
point3 = Point(
obstacles_center.x_coordinate + self._OBSTACLES_LENGTH / 2,
obstacles_center.y_coordinate + self._OBSTACLES_LENGTH / 2)
point4 = Point(
obstacles_center.x_coordinate + self._OBSTACLES_LENGTH / 2,
obstacles_center.y_coordinate - self._OBSTACLES_LENGTH / 2)
self._track += (Line(point1, point2), )
self._track += (Line(point2, point3), )
self._track += (Line(point3, point4), )
self._track += (Line(point4, point1), )
def _fork_plot(self) -> None:
plt.plot(self._goal.x_coordinate, self._goal.y_coordinate, 'go')
class NavigationTrack(Navigation):
"""The Navigation Track environment."""
metadata = {'render.modes': ['human']}
_N_ACTIONS = 3
_FORWARD = 0
_YAW_RIGHT = 1
_YAW_LEFT = 2
_FORWARD_LINEAR_SHIFT = 0.2 # m
_YAW_LINEAR_SHIFT = 0.04 # m
_YAW_ANGULAR_SHIFT = 0.2 # rad
_SHIFT_STANDARD_DEVIATION = 0.02
_SENSOR_STANDARD_DEVIATION = 0.02
_COLLISION_THRESHOLD = 0.4
_COLLISION_REWARD = -200.0
_FORWARD_REWARD = +5.0
_YAW_REWARD = -0.5
_SCAN_ANGLES = (-math.pi / 2, -math.pi / 4, 0, math.pi / 4, math.pi / 2)
_SCAN_RANGE_MAX = 30.0
_SCAN_RANGE_MIN = 0.2
_N_MEASUREMENTS = len(_SCAN_ANGLES)
_N_OBSERVATIONS = _N_MEASUREMENTS
_Y_LIM = (-12, 12)
_X_LIM = (-12, 12)
_TRACK1: Tuple[Line,
...] = (Line(Point(-10, -10), Point(-10, 10)),
Line(Point(-10, 10), Point(10, 10)),
Line(Point(10, 10), Point(10, -1.5)),
Line(Point(10, -1.5), Point(1.5, -1.5)),
Line(Point(1.5, -1.5), Point(1.5, -10)),
Line(Point(1.5, -10),
Point(-10,
-10)), Line(Point(-7, -7), Point(-7, 7)),
Line(Point(-7, 7),
Point(7, 7)), Line(Point(7, 7), Point(7, 1.5)),
Line(Point(7, 1.5), Point(-1.5, 1.5)),
Line(Point(-1.5, 1.5), Point(-1.5, -7)),
Line(Point(-1.5, -7), Point(-7, -7)))
_TRACKS = (_TRACK1, )
_SPAWN_AREA1: Tuple[Tuple[Tuple[float, float], Tuple[float, float]],
...] = (((-8.5, -8.5), (-8.5, 8.5)), ((-8.5, 8.5),
(8.5, 8.5)),
((8.5, 8.5), (0, 8.5)), ((0, 8.5), (0, 0)),
((0, 0), (-8.5, 0)), ((-8.5, 0), (-8.5, -8.5)))
_SPAWN_AREAS = (_SPAWN_AREA1, )
_track: Tuple[Line, ...]
_spawn_area: Tuple[Tuple[Tuple[float, float], Tuple[float, float]], ...]
_pose: Pose
_ranges: np.ndarray
_observation: np.ndarray
action_space: spaces.Discrete
observation_space: spaces.Box
def __init__(self, track_id: int = 1) -> None:
if track_id in range(1, len(self._TRACKS) + 1):
self._track = self._TRACKS[track_id - 1]
self._spawn_area = self._SPAWN_AREAS[track_id - 1]
else:
raise ValueError(f'Invalid track id {track_id} ({type(track_id)})')
self._ranges = np.empty(self._N_MEASUREMENTS)
self.action_space = spaces.Discrete(self._N_ACTIONS)
self.observation_space = spaces.Box(low=self._SCAN_RANGE_MAX,
high=self._SCAN_RANGE_MIN,
shape=(self._N_OBSERVATIONS, ),
dtype=np.float32)
def _do_perform_action(self, action: int) -> None:
theta = random.gauss(0, self._SHIFT_STANDARD_DEVIATION)
distance = random.gauss(0, self._SHIFT_STANDARD_DEVIATION)
if action == self._FORWARD:
distance += self._FORWARD_LINEAR_SHIFT
elif action == self._YAW_RIGHT:
distance += self._YAW_LINEAR_SHIFT
theta += self._YAW_ANGULAR_SHIFT
else:
distance += self._YAW_LINEAR_SHIFT
theta -= self._YAW_ANGULAR_SHIFT
self._pose.shift(distance, theta)
def _update_scan(self) -> None:
scan_lines = self._create_scan_lines()
for i, scan_line in enumerate(scan_lines):
min_distance = self._SCAN_RANGE_MAX
for wall in self._track:
try:
intersection = scan_line.get_intersection(wall)
except NoIntersectionError:
continue
distance = self._pose.position.calculate_distance(intersection)
if distance < min_distance:
min_distance = distance
sensor_noise = random.gauss(0, self._SENSOR_STANDARD_DEVIATION)
self._ranges[i] = min_distance + sensor_noise
def _create_scan_lines(self) -> np.ndarray:
scan_poses = self._create_scan_poses()
scan_lines = np.empty(self._N_MEASUREMENTS, dtype=Line)
for i, scan_pose in enumerate(scan_poses):
scan_pose.move(self._SCAN_RANGE_MAX)
scan_lines[i] = Line(copy.copy(self._pose.position),
scan_pose.position)
return scan_lines
def _create_scan_poses(self) -> np.ndarray:
scan_poses = np.empty(self._N_MEASUREMENTS, dtype=Pose)
for i, scan_angle in enumerate(self._SCAN_ANGLES):
scan_poses[i] = Pose(copy.copy(self._pose.position),
self._pose.yaw + scan_angle)
return scan_poses
def _do_check_if_done(self) -> bool:
return self._collision_occurred()
def _collision_occurred(self) -> bool:
return bool((self._ranges < self._COLLISION_THRESHOLD).any())
def _do_calculate_reward(self, action: int) -> float:
if self._collision_occurred():
reward = self._COLLISION_REWARD
elif action == self._FORWARD:
reward = self._FORWARD_REWARD
else:
reward = self._YAW_REWARD
return reward
def _do_update_observation(self) -> None:
self._update_scan()
self._observation = self._ranges
def _do_init_environment(self) -> None:
self._init_pose()
def _init_pose(self) -> None:
area = random.choice(self._spawn_area)
x_coordinate = random.uniform(area[0][0], area[0][1])
y_coordinate = random.uniform(area[1][0], area[1][1])
position = Point(x_coordinate, y_coordinate)
yaw = random.uniform(-math.pi, math.pi)
self._pose = Pose(position, yaw)
def _do_plot(self) -> None:
plt.xlim(self._X_LIM)
plt.ylim(self._Y_LIM)
for wall in self._track:
x_range = (wall.start.x_coordinate, wall.end.x_coordinate)
y_range = (wall.start.y_coordinate, wall.end.y_coordinate)
plt.plot(x_range, y_range, 'b')
scan_lines = self._create_scan_lines()
for scan_line in scan_lines:
x_range = (scan_line.start.x_coordinate,
scan_line.end.x_coordinate)
y_range = (scan_line.start.y_coordinate,
scan_line.end.y_coordinate)
plt.plot(x_range, y_range, 'y')
scan_poses = self._create_scan_poses()
for i, scan_pose in enumerate(scan_poses):
scan_pose.move(self._ranges[i])
plt.plot(scan_pose.position.x_coordinate,
scan_pose.position.y_coordinate, 'co')
plt.plot(self._pose.position.x_coordinate,
self._pose.position.y_coordinate, 'ro')
def close(self) -> None:
plt.close()