def test_did_not_move(controller): """ Robot did not move. """ pos, angle = controller.odometry(10, 10, Vector2(0, 0), 0) assert pos == Vector2(0, 0) assert angle == 0
def test_negation(): """ Test negation. """ vec = Vector2(42, -10) assert Vector2(-42, 10) == -vec
def test_multiplication(): """ Test multiplication. """ vec = Vector2(42, -10) assert Vector2(84, -20) == vec * 2
def test_division(): """ Test division. """ vec = Vector2(42, -10) assert Vector2(21, -5) == vec / 2
def test_move_straight(configuration): """ Robot moved in a straight line. """ pos, angle = odometry_arc(10, 10, Vector2(0, 0), 0, configuration) assert pos == Vector2(10, 0) assert angle == 0
def test_did_not_move(configuration): """ Robot did not move an inch, with a non zero initial position. """ pos, angle = odometry_arc(0, 0, Vector2(123, 321), 456, configuration) assert pos == Vector2(123, 321) assert angle == 456
def test_addition(): """ Test +. """ vec1 = Vector2(42, 0) vec2 = Vector2(-40, 12) assert Vector2(2, 12) == (vec1 + vec2)
def test_subtraction(): """ Test -. """ vec1 = Vector2(10, -10) vec2 = Vector2(20, -20) assert Vector2(-10, 10) == (vec1 - vec2)
def test_move_straight_at_angle(configuration): """ Robot moved in a straight line but with a non zero initial angle. """ pos, angle = odometry_arc(10, 10, Vector2(0, 0), math.pi / 2, configuration) assert pos == Vector2(0, 10) assert angle == math.pi / 2
def test_rotate_without_translating(configuration): """ Robot rotated without any linear movement. """ angle_rotated = math.pi / 2 distance = angle_rotated * DISTANCE_BETWEEN_WHEELS / 2 pos, angle = odometry_arc(-distance, distance, Vector2(1, 2), 3, configuration) assert pos == Vector2(1, 2) assert angle == math.pi / 2 + 3
def test_odometry_position(localization_controller, odometry_controller_mock): """ Test the functions related to odometry pos/angle. """ odometry_controller_mock.odometry = MagicMock( return_value=(Vector2(1, 2), 3)) localization_controller.update_odometry_position(10, 10) assert localization_controller.get_position() == Vector2(1, 2) assert localization_controller.get_angle() == 3
def odometry_controller(configuration): """ Odometry controller. """ controller = OdometryController(configuration=configuration) # Set initial position. pos, angle = controller.odometry(10, 10, Vector2(0, 0), 0) assert pos == Vector2(0, 0) assert angle == 0 return controller
def test_move_straight(controller): """ Robot moved in a straight line. """ pos, angle = controller.odometry(20, 20, Vector2(0, 0), 0) assert pos == Vector2( 2 * math.pi * WHEEL_RADIUS * 10 / TICK_PER_REVOLUTION, 0, ) assert angle == 0 # Move backward in a straight line. pos, angle = controller.odometry(10, 10, Vector2(0, 0), math.pi / 2) assert pos.x < 1e-10 assert pos.y == -2 * math.pi * WHEEL_RADIUS * 10 / TICK_PER_REVOLUTION assert angle == math.pi / 2
def set_detection( self, seen_polar: Tuple[Tuple[Radian, Millimeter], ...]) -> None: """ Set the detected obstacles to the desired value in the lidar controller. """ self.seen_polar = seen_polar self.seen_cartesian = tuple( Vector2(radius * cos(angle), radius * sin(angle)) for radius, angle in seen_polar)
def test_rotate_and_move_left(controller): """ Robot rotated AND translated. Right wheel travelled more than left wheel. """ pos, angle = controller.odometry(10, 11, Vector2(0, 0), 0) assert pos.x > 0 # Moved forward. assert pos.y > 0 # Went a bit up. assert angle > 0 # Turned left.
def test_rotate_and_move_right(controller): """ Robot rotated AND translated. Left wheel travelled more than right wheel. """ pos, angle = controller.odometry(11, 10, Vector2(0, 0), 0) assert pos.x > 0 # Moved forward. assert pos.y < 0 # Went a bit down. assert angle < 0 # Turned right.
def test_rotate_and_move_left(configuration): """ Robot rotated AND translated. Right wheel travelled more than left wheel. """ pos, angle = odometry_arc(0, 1, Vector2(0, 0), 0, configuration) assert pos.x > 0 # Moved forward. assert pos.y > 0 # Went a bit up. assert angle > 0 # Turned left.
def configuration_stub(configuration_test: Configuration) -> Configuration: """ Configuration for tests. """ return dataclasses.replace(configuration_test, initial_angle=0, initial_position=Vector2(0, 0), wheel_radius=1 / (2 * math.pi))
def test_properties(): """ Test X and Y properties. """ vec = Vector2(10, -42) assert vec.x == 10 assert vec.y == -42
def symmetries_position(self, position: Vector2) -> Vector2: """ Symmetries a position if necessary """ if self.configuration.color == Color.YELLOW: symmetric_position = Vector2(-position.x, position.y) return symmetric_position return position
def right(angle: Radian) -> Vector2: """ Get the right direction relative to the direction. """ return Vector2( math.cos(angle - math.pi / 2), math.sin(angle - math.pi / 2), )
def forward(angle: Radian) -> Vector2: """ Get the front direction relative to the direction. """ return Vector2( math.cos(angle), math.sin(angle), )
def test_rotate_and_move_right(configuration): """ Robot rotated AND translated. Left wheel travelled more than right wheel. """ pos, angle = odometry_arc(1, 0, Vector2(0, 0), 0, configuration) assert pos.x > 0 # Moved forward. assert pos.y < 0 # Went a bit down. assert angle < 0 # Turned right.
def test_json_encoder_vector2(): """ Happy path for Vector2. """ to_encode = {'a': Vector2(2, 3)} dump = json.dumps(to_encode, cls=RobotJSONEncoder, indent=None) assert dump == '{"a": {"x": 2.0, "y": 3.0}}'
def test_vector_sym2(): """ Test vector symmetry """ vec = Vector2(-167, 124) symmetry_controller = get_symmetry_controller(color=Color.BLUE) sym_vec = symmetry_controller.symmetries_position(vec) assert sym_vec.x == -167 assert sym_vec.y == 124
def test_vector_sym1(): """ Test vector symmetry """ vec = Vector2(-148, 29) symmetry_controller = get_symmetry_controller(color=Color.YELLOW) sym_vec = symmetry_controller.symmetries_position(vec) assert sym_vec.x == 148 assert sym_vec.y == 29
def _odometry(self, left_tick: int, right_tick: int, pos: Vector2, angle: Radian) -> \ Tuple[Vector2, Radian]: if not self.intialized: # Cannot calculate the delta of the ticks. self.intialized = True return pos, angle d_left = self._tick_to_millimeter(left_tick - self.previous_left_tick) d_right = self._tick_to_millimeter(right_tick - self.previous_right_tick) if d_left == d_right == 0: # Robot did not move. return pos, angle d_distance = (d_right + d_left) / 2 if d_distance == 0: # Robot did not translate, rotate only. wheel_dist = self.configuration.distance_between_wheels / 2 d_theta = d_right / wheel_dist return pos, angle + d_theta if d_right == d_left: # Trajectory is straight, translate only. return pos + Vector2( math.cos(angle) * d_distance, math.sin(angle) * d_distance), angle # Trajectory is both translation and rotation. # 1. Calculate the radius of curvature. wheel_dist = self.configuration.distance_between_wheels curvature_radius = (d_right + d_left) / \ (d_right - d_left) * wheel_dist / 2 # 2. Calculate the angle delta. d_theta = d_distance / curvature_radius # 3. Calculate the center of curvature. center_pos = pos + Vector2(-curvature_radius * math.sin(angle), curvature_radius * math.cos(angle)) # 4. Calculate the "new" position, approximating the trajectory to a circular arc. return center_pos + Vector2( curvature_radius * math.sin(angle + d_theta), -curvature_radius * math.cos(angle + d_theta)), angle + d_theta
async def run(self) -> None: """ Run the strategy. """ for vec, reverse in PATH: LOGGER.get().info("move robot", destination=vec) await self.motion_controller.move_to(Vector2(vec.x, 2000 - vec.y), reverse) LOGGER.get().info("Strategy algorithm finished running") # lol
def ray_segment_intersection( ray: Ray, segment: Segment) -> Tuple[Optional[Vector2], Optional[float]]: """ Get the intersection point between a ray and a segment. More precisely, will find the intersection point between [a,b) and ]c,d[ (thus a 0 length segment will never intersect). Returns None if they do not intersect. Return the intersection coordinates and the distance from the origin of the ray otherwise. """ origin = numpy.array([ray.origin.x, ray.origin.y]) direction = numpy.array([ray.direction.x, ray.direction.y]) start = numpy.array([segment.start.x, segment.start.y]) end = numpy.array([segment.end.x, segment.end.y]) # Find intersection point: # origin + direction * x = start + (end - start) * y # origin ^ (end-start) + direction ^ (end-start) * x = start ^ (end-start) # direction ^ (end-start) * x = start ^ (end-start) - origin ^ (end-start) # direction ^ (end-start) * x = (start - origin) ^ (end-start) # A * x = B # x = B / A segment_length2 = (end - start).dot(end - start) if segment_length2 == 0: return None, None # Segment is 0 length, will never be hit by a ray. coef_a = numpy.cross(direction, (end - start)) if coef_a == 0: return None, None # Collinear, will never intersect. coef_b = numpy.cross(start - origin, end - start) v_x = coef_b / coef_a if v_x < 0: return None, None # Ray hit behind. # Then... # intersection_point = origin + direction * x intersection_point = origin + direction * v_x # To find Y: # intersection_point = start + (end - start) * y # intersection_point - start = (end - start) * y # (intersection_point - start) . (end - start) = (end - start) . (end - start) * y # (intersection_point - start) . (end - start) = || end - start || ** 2 * y # y = (intersection_point - start) . (end - start) / || end - start || ** 2 # y = (intersection_point - start) . (end - start) / || end - start || ** 2 coef_a = (intersection_point - start).dot(end - start) v_y = coef_a / segment_length2 if v_y <= 0 or v_y >= 1: return None, None # Ray did not hit segment. return Vector2(*intersection_point), v_x
def odometry_arc(delta_left: Millimeter, delta_right: Millimeter, current_position: Vector2, current_angle: Radian, configuration: Configuration) -> Tuple[Vector2, Radian]: """ Computes the current position and angle of the robot using the movements of its wheels. Uses a curvature radius to compute the new position. """ if math.isclose(delta_right, 0) and math.isclose(delta_left, 0): # The robot did not move. return current_position, current_angle delta_distance = (delta_left + delta_right) / 2 if math.isclose(delta_distance, 0): # Robot did not translate, rotation only. wheel_dist = configuration.distance_between_wheels / 2 d_theta = delta_right / wheel_dist return current_position, current_angle + d_theta if math.isclose(delta_left, delta_right): # Linear Translation. return current_position + Vector2( math.cos(current_angle) * delta_distance, math.sin(current_angle) * delta_distance), current_angle # Trajectory is both translation and rotation. # 1. Calculate the radius of curvature. wheel_dist = configuration.distance_between_wheels curvature_radius = (delta_right + delta_left) / \ (delta_right - delta_left) * wheel_dist / 2 # 2. Calculate the angle delta. d_theta = delta_distance / curvature_radius # 3. Calculate the center of curvature. center_pos = current_position + Vector2( -curvature_radius * math.sin(current_angle), curvature_radius * math.cos(current_angle)) # 4. Calculate the "new" position, approximating the trajectory to a circular arc. return center_pos + Vector2( curvature_radius * math.sin(current_angle + d_theta), -curvature_radius * math.cos(current_angle + d_theta)), current_angle + d_theta