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 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_do_update_observation():
env = NavigationGoal()
env._pose = Pose(Point(0, 8.5), 0.78539816339)
env._goal = Point(1.5, 8.5)
env._do_update_observation()
expected_observation = [
2.12132034356, 1.5, 2.12132034356, 10, 14.1421356237, 1.5,
0.78539816339
]
assert np.allclose(env._observation, expected_observation, atol=0.06)
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_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 get_intersection(self, other: Line) -> Point:
"""Get the intersection point between two lines.
Raise an error if it does not exist.
"""
if self.slope == other.slope: # Parallel lines
raise NoIntersectionError
if self.start.x_coordinate == self.end.x_coordinate:
x_coordinate = self.start.x_coordinate
y_coordinate = other.slope * x_coordinate + other.y_intercept
elif other.start.x_coordinate == other.end.x_coordinate:
x_coordinate = other.start.x_coordinate
y_coordinate = self.slope * x_coordinate + self.y_intercept
elif self.start.y_coordinate == self.end.y_coordinate:
y_coordinate = self.start.y_coordinate
x_coordinate = (y_coordinate - other.y_intercept) / other.slope
elif other.start.y_coordinate == other.end.y_coordinate:
y_coordinate = other.start.y_coordinate
x_coordinate = (y_coordinate - self.y_intercept) / self.slope
else:
x_coordinate = ((self.y_intercept - other.y_intercept)
/ (other.slope - self.slope))
y_coordinate = self.slope * x_coordinate + self.y_intercept
intersection = Point(x_coordinate, y_coordinate)
if self.contains(intersection) and other.contains(intersection):
return intersection
raise NoIntersectionError
def test_forward_action():
env = NavigationTrack()
env._pose = Pose(Point(0, 8.5), 0)
env._do_perform_action(env._FORWARD)
assert math.isclose(env._pose.position.x_coordinate, 0)
assert math.isclose(env._pose.position.y_coordinate, 8.7, abs_tol=0.06)
assert math.isclose(env._pose.yaw, 0, abs_tol=0.06)
def test_do_update_observation():
env = NavigationTrack()
env._pose = Pose(Point(0, 8.5), 0.78539816339)
env._do_update_observation()
assert np.allclose(env._observation,
[2.12132034356, 1.5, 2.12132034356, 10, 2.12132034356],
atol=0.06)
def test_yaw_right_action():
env = NavigationTrack()
env._pose = Pose(Point(0, 8.5), 0)
env._do_perform_action(env._YAW_RIGHT)
assert math.isclose(env._pose.position.x_coordinate, 0)
assert math.isclose(env._pose.position.y_coordinate, 8.54, abs_tol=0.06)
assert math.isclose(env._pose.yaw, 0.2, abs_tol=0.06)
def calculate_angle_difference(self, target: Point) -> float:
"""Calculate the angle difference from a point.
This is the angle and the direction (+ or -) that the object
needs to rotate in order to face the target point.
"""
vector1 = Point(target.x_coordinate - self.position.x_coordinate,
target.y_coordinate - self.position.y_coordinate)
pose2 = Pose(
Point(self.position.x_coordinate, self.position.y_coordinate),
self.yaw)
pose2.move(1)
vector2 = Point(
pose2.position.x_coordinate - self.position.x_coordinate,
pose2.position.y_coordinate - self.position.y_coordinate)
angle_difference = math.atan2(
vector1.x_coordinate * vector2.y_coordinate -
vector1.y_coordinate * vector2.x_coordinate,
vector1.x_coordinate * vector2.x_coordinate +
vector1.y_coordinate * vector2.y_coordinate)
return angle_difference
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_move_with_yaw_zero():
pose = Pose(Point(1, 2), 0)
pose.move(1)
assert pose == Pose(Point(1, 3), 0)
def test_valid_yaw_less_than_negative_pi():
pose = Pose(Point(1, 2), -3 * math.pi / 2)
assert pose.yaw == math.pi / 2
def test_valid_yaw_greater_than_pi():
pose = Pose(Point(1, 2), 3 * math.pi / 2)
assert pose.yaw == -math.pi / 2
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_move_with_negative_yaw():
pose = Pose(Point(1, 2), -math.pi / 4)
pose.move(math.sqrt(2))
assert pose == Pose(Point(0, 3), -math.pi / 4)
def test_not_contains():
line = Line(Point(2, 0), Point(7, 5))
assert not line.contains(Point(0, -2))
def test_shift():
pose = Pose(Point(1, 2), math.pi / 4)
pose.shift(math.sqrt(2), math.pi / 4)
assert pose == Pose(Point(2, 3), math.pi / 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
def test_move_with_yaw_pi():
pose = Pose(Point(1, 2), math.pi)
pose.move(1)
assert pose == Pose(Point(1, 1), math.pi)
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_move_with_positive_yaw():
pose = Pose(Point(1, 2), math.pi / 4)
pose.move(math.sqrt(2))
assert pose == Pose(Point(2, 3), math.pi / 4)
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_pose_equality():
pose1 = Pose(Point(1, 2), math.pi / 4)
pose2 = Pose(Point(1, 2), math.pi / 4)
assert pose1 == pose2
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')
def test_rotate():
pose = Pose(Point(1, 2), math.pi / 4)
pose.rotate(math.pi / 4)
assert pose == Pose(Point(1, 2), math.pi / 2)
def test_contains():
line = Line(Point(2, 0), Point(7, 5))
assert line.contains(Point(4, 2))
