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))