Fix/rsim teleport bug (#109)

* fix teleport bug

* add teleport position test

* clean up movement tests

* default filter to false

* remove conflict in teleport positions

* remove print artefacts from teleport_position_accuracy_test.py

* update docstring for strategy_runner.py to reflect filtering false default

* move bounds check earlier to prevent mismatch of kalman filter and gameframe mismatch

* added comment acknowledge the double initialization of Game in run_test

Author: energy-in-joles

utama_core/data_processing/refiners/position.py

@@ -1,5 +1,5 @@
 from collections import defaultdict
-from dataclasses import replace
+from dataclasses import dataclass, replace
 from functools import partial
 from typing import Dict, List, Optional, Tuple
 
@@ -31,6 +31,14 @@ def smooth(self, old_angle: float, new_angle: float) -> float:
         return smoothed_angle
 
 
+@dataclass
+class VisionBounds:
+    x_min: float
+    x_max: float
+    y_min: float
+    y_max: float
+
+
 class PositionRefiner(BaseRefiner):
     def __init__(
         self,
@@ -40,11 +48,12 @@ def __init__(
     ):
         # alpha=0 means no change in angle (inf smoothing), alpha=1 means no smoothing
         self.angle_smoother = AngleSmoother(alpha=1)
-        self.x_min = field_bounds.top_left[0] - bounds_buffer  # expand left
-        self.x_max = field_bounds.bottom_right[0] + bounds_buffer  # expand right
-        self.y_min = field_bounds.bottom_right[1] - bounds_buffer  # expand bottom
-        self.y_max = field_bounds.top_left[1] + bounds_buffer  # expand top
-        self.BOUNDS_BUFFER = bounds_buffer
+        self.vision_bounds = VisionBounds(
+            x_min=field_bounds.top_left[0] - bounds_buffer,  # expand left
+            x_max=field_bounds.bottom_right[0] + bounds_buffer,  # expand right
+            y_min=field_bounds.bottom_right[1] - bounds_buffer,  # expand bottom
+            y_max=field_bounds.top_left[1] + bounds_buffer,  # expand top
+        )
 
         # For Kalman filtering and imputing vanished values.
         self.filtering = filtering
@@ -68,7 +77,7 @@ def refine(self, game_frame: GameFrame, data: List[RawVisionData]) -> GameFrame:
 
         # class VisionData: ts: float; yellow_robots: List[VisionRobotData]; blue_robots: List[VisionRobotData]; balls: List[VisionBallData]
         # class VisionRobotData: id: int; x: float; y: float; orientation: float
-        combined_vision_data: VisionData = CameraCombiner().combine_cameras(frames)
+        combined_vision_data: VisionData = CameraCombiner().combine_cameras(frames, bounds=self.vision_bounds)
 
         time_elapsed = combined_vision_data.ts - game_frame.ts
 
@@ -87,7 +96,9 @@ def refine(self, game_frame: GameFrame, data: List[RawVisionData]) -> GameFrame:
             for y_rbt_id, vision_y_rbt in vision_yellow.items():
                 if y_rbt_id not in self.kalman_filters_yellow:
                     self.kalman_filters_yellow[y_rbt_id] = KalmanFilter(id=y_rbt_id)
-
+                    if y_rbt_id not in yellow_rbt_last_frame:
+                        filtered_yellow_robots.append(vision_y_rbt)
+                        continue
                 filtered_robot = self.kalman_filters_yellow[y_rbt_id].filter_data(
                     vision_y_rbt,  # new measurement
                     yellow_rbt_last_frame[y_rbt_id],  # last frame
@@ -100,6 +111,9 @@ def refine(self, game_frame: GameFrame, data: List[RawVisionData]) -> GameFrame:
             for b_rbt_id, vision_b_rbt in vision_blue.items():
                 if b_rbt_id not in self.kalman_filters_blue:
                     self.kalman_filters_blue[b_rbt_id] = KalmanFilter(id=b_rbt_id)
+                    if b_rbt_id not in blue_rbt_last_frame:
+                        filtered_blue_robots.append(vision_b_rbt)
+                        continue
 
                 filtered_robot = self.kalman_filters_blue[b_rbt_id].filter_data(
                     vision_b_rbt,  # new measurement
@@ -264,12 +278,6 @@ def _combine_single_team_positions(
         friendly: bool,
     ) -> Dict[int, Robot]:
         for robot in vision_robots:
-            new_x, new_y = robot.x, robot.y
-
-            if not (self.x_min <= new_x <= self.x_max and self.y_min <= new_y <= self.y_max):
-                # Out of bounds so ignore this robot
-                continue
-
             if robot.id not in new_game_robots:
                 # At the start of the game, we haven't seen anything yet, so just create a new robot
                 new_game_robots[robot.id] = PositionRefiner._robot_from_vision(robot, is_friendly=friendly)
@@ -312,9 +320,16 @@ def filter_running(self) -> bool:
 
 
 class CameraCombiner:
-    def combine_cameras(self, frames: List[RawVisionData]) -> VisionData:
-        # Now we have access to the game we can do more sophisticated things
-        # Such as ignoring outlier cameras etc
+    def combine_cameras(self, frames: List[RawVisionData], bounds: VisionBounds) -> VisionData:
+        """
+        Combines the vision data from multiple cameras into a single coherent VisionData object.
+        Also, removes any robot detections that are out of the specified bounds.
+        Args:
+            frames (List[RawVisionData]): A list of RawVisionData objects from different cameras.
+            bounds (VisionBounds): The bounds within which to consider vision data for combination.
+        Returns:
+            VisionData: A combined VisionData object containing averaged robot positions and the most confident ball position.
+        """
 
         ts = []
         # maps robot id to list of frames seen for that robot
@@ -325,13 +340,16 @@ def combine_cameras(self, frames: List[RawVisionData]) -> VisionData:
         # Each frame is from a different camera
         for frame_ind, frame in enumerate(frames):
             for yr in frame.yellow_robots:
-                yellow_captured[yr.id].append(yr)
+                if self._bounds_check(yr.x, yr.y, bounds):
+                    yellow_captured[yr.id].append(yr)
 
             for br in frame.blue_robots:
-                blue_captured[br.id].append(br)
+                if self._bounds_check(br.x, br.y, bounds):
+                    blue_captured[br.id].append(br)
 
             for b in frame.balls:
-                balls_captured[frame_ind].append(b)
+                if self._bounds_check(b.x, b.y, bounds):
+                    balls_captured[frame_ind].append(b)
             ts.append(frame.ts)
 
         avg_yellows = list(map(self._avg_robots, yellow_captured.values()))
@@ -380,6 +398,9 @@ def _combine_balls_by_proximity(self, bs: Dict[int, List[RawBallData]]) -> List[
                     combined_balls.append(b)
         return combined_balls
 
+    def _bounds_check(self, x: float, y: float, bounds: VisionBounds) -> bool:
+        return bounds.x_min <= x <= bounds.x_max and bounds.y_min <= y <= bounds.y_max
+
     @staticmethod
     def ball_merge_predicate(b1: RawBallData, b2: RawBallData) -> bool:
         return abs(b1.x - b2.x) + abs(b1.y - b2.y) < BALL_MERGE_THRESHOLD

utama_core/run/game_gater.py

@@ -31,6 +31,7 @@ def wait_until_game_valid(
         def _add_frame(my_game_frame: GameFrame, opp_game_frame: GameFrame) -> Tuple[GameFrame, Optional[GameFrame]]:
             if rsim_env:
                 vision_frames = [rsim_env._frame_to_observations()[0]]
+                rsim_env.steps += 1  # Increment the step count to simulate time passing in the environment
             else:
                 vision_frames = [buffer.popleft() if buffer else None for buffer in vision_buffers]
             my_game_frame = position_refiner.refine(my_game_frame, vision_frames)

utama_core/run/strategy_runner.py

@@ -111,7 +111,7 @@ class StrategyRunner:
             Defaults to 0 for each.
         rsim_vanishing (float, optional): When running in rsim, cause robots and ball to vanish with the given probability.
             Defaults to 0.
-        filtering (bool, optional): Turn on Kalman filtering. Defaults to true.
+        filtering (bool, optional): Turn on Kalman filtering. Defaults to false.
     """
 
     def __init__(
@@ -131,7 +131,7 @@ def __init__(
         profiler_name: Optional[str] = None,
         rsim_noise: RsimGaussianNoise = RsimGaussianNoise(),
         rsim_vanishing: float = 0,
-        filtering: bool = True,
+        filtering: bool = False,
     ):
         self.logger = logging.getLogger(__name__)
 

utama_core/tests/common/abstract_test_manager.py

@@ -49,9 +49,14 @@ def reset_field(self, sim_controller: AbstractSimController, game: Game):
         """
         Method is called at start of each test episode in strategyRunner.run_test().
         Use this to reset position of robots and ball for the next episode.
+
         Args:
             sim_controller (AbstractSimController): The simulation controller to manipulate robot positions.
             game (Game): The current game state.
+
+        Note:
+            This causes run_test to initialise Game twice on the first episode.
+            However, this is necessary to give the test manager access to the initial Game object before the first episode setup.
         """
         ...
 

utama_core/tests/motion_planning/multiple_robots_test.py

@@ -45,50 +45,13 @@ def __init__(self, scenario: MultiRobotScenario):
 
     def reset_field(self, sim_controller: AbstractSimController, game: Game):
         """Reset field with all robots at their starting positions."""
-        # Teleport friendly robots to starting positions
         for i, (x, y) in enumerate(self.scenario.friendly_positions):
-            if i < MAX_ROBOTS:  # Max MAX_ROBOTS robots per team
-                sim_controller.teleport_robot(
-                    game.my_team_is_yellow,
-                    i,
-                    x,
-                    y,
-                    0.0,
-                )
-
-        # Teleport remaining friendly robots far away
-        for i in range(len(self.scenario.friendly_positions), MAX_ROBOTS):
-            sim_controller.teleport_robot(
-                game.my_team_is_yellow,
-                i,
-                -10.0,
-                -10.0,
-                0.0,
-            )
-
-        # Teleport enemy robots to starting positions
+            sim_controller.teleport_robot(game.my_team_is_yellow, i, x, y, 0.0)
+
         for i, (x, y) in enumerate(self.scenario.enemy_positions):
-            if i < MAX_ROBOTS:
-                sim_controller.teleport_robot(
-                    not game.my_team_is_yellow,
-                    i,
-                    x,
-                    y,
-                    0.0,
-                )
-
-        # Teleport remaining enemy robots far away
-        for i in range(len(self.scenario.enemy_positions), MAX_ROBOTS):
-            sim_controller.teleport_robot(
-                not game.my_team_is_yellow,
-                i,
-                -10.0,
-                -10.0,
-                0.0,
-            )
-
-        # Place ball out of the way
-        sim_controller.teleport_ball(-10.0, -10.0)
+            sim_controller.teleport_robot(not game.my_team_is_yellow, i, x, y, 0.0)
+
+        sim_controller.teleport_ball(0.0, 0.0)
 
         self._reset_metrics()
 

utama_core/tests/motion_planning/random_movement_test.py

@@ -52,41 +52,16 @@ def reset_field(self, sim_controller: AbstractSimController, game: Game):
         """Reset field with robots in random starting positions within bounds."""
         (min_x, max_x), (min_y, max_y) = self.scenario.field_bounds
 
-        # Place robots at random positions within bounds
+        # Use a fixed seed for reproducibility across test runs
+        rng = random.Random(42 + self.current_episode_number)
+
         for i in range(self.scenario.n_robots):
-            x = random.uniform(min_x + 0.5, max_x - 0.5)
-            y = random.uniform(min_y + 0.5, max_y - 0.5)
-            sim_controller.teleport_robot(
-                game.my_team_is_yellow,
-                i,
-                x,
-                y,
-                0.0,
-            )
+            x = rng.uniform(min_x + 0.5, max_x - 0.5)
+            y = rng.uniform(min_y + 0.5, max_y - 0.5)
+            sim_controller.teleport_robot(game.my_team_is_yellow, i, x, y, 0.0)
             self.targets_reached_count[i] = 0
 
-        # Teleport remaining robots far away
-        for i in range(self.scenario.n_robots, 6):
-            sim_controller.teleport_robot(
-                game.my_team_is_yellow,
-                i,
-                -10.0,
-                -10.0,
-                0.0,
-            )
-
-        # Remove enemy robots
-        for i in range(6):
-            sim_controller.teleport_robot(
-                not game.my_team_is_yellow,
-                i,
-                -10.0,
-                -10.0,
-                0.0,
-            )
-
-        # Place ball out of the way
-        sim_controller.teleport_ball(-10.0, -10.0)
+        sim_controller.teleport_ball(0.0, 0.0)
 
         self._reset_metrics()
 

utama_core/tests/motion_planning/single_robot_moving_obstacle_test.py

@@ -57,47 +57,22 @@ def __init__(self, scenario: MovingObstacleScenario, robot_id: int):
 
     def reset_field(self, sim_controller: AbstractSimController, game: Game):
         """Reset field with robot at start position and moving obstacles."""
-        # Teleport ALL friendly robots off-field
-        for i in range(6):
-            if i == self.robot_id:
-                # Place the test robot at start position
-                sim_controller.teleport_robot(
-                    game.my_team_is_yellow,
-                    i,
-                    self.scenario.start_position[0],
-                    self.scenario.start_position[1],
-                    0.0,
-                )
-            else:
-                # Move other friendly robots far away
-                sim_controller.teleport_robot(
-                    game.my_team_is_yellow,
-                    i,
-                    -10.0,
-                    -10.0,
-                    0.0,
-                )
-
-        # Place enemy robots at their center positions (they will start moving via their strategy)
-        for i in range(6):
-            if i < len(self.scenario.moving_obstacles):
-                obstacle_config = self.scenario.moving_obstacles[i]
-                sim_controller.teleport_robot(
-                    not game.my_team_is_yellow,
-                    i,
-                    obstacle_config.center_position[0],
-                    obstacle_config.center_position[1],
-                    0.0,
-                )
-            else:
-                # Move extra enemy robots far away
-                sim_controller.teleport_robot(
-                    not game.my_team_is_yellow,
-                    i,
-                    -10.0,
-                    -10.0,
-                    0.0,
-                )
+        sim_controller.teleport_robot(
+            game.my_team_is_yellow,
+            self.robot_id,
+            self.scenario.start_position[0],
+            self.scenario.start_position[1],
+            0.0,
+        )
+
+        for i, obstacle_config in enumerate(self.scenario.moving_obstacles):
+            sim_controller.teleport_robot(
+                not game.my_team_is_yellow,
+                i,
+                obstacle_config.center_position[0],
+                obstacle_config.center_position[1],
+                0.0,
+            )
 
         # Place ball at target (for visual reference)
         sim_controller.teleport_ball(