Feature/kalman

.gitignore

@@ -113,6 +113,7 @@ target/
 
 # Jupyter Notebook
 .ipynb_checkpoints
+.jupyter/
 
 # IPython
 profile_default/

main.py

@@ -5,6 +5,7 @@
 from utama_core.strategy.examples import (
     DefenceStrategy,
     GoToBallExampleStrategy,
+    PointCycleStrategy,
     RobotPlacementStrategy,
     StartupStrategy,
     TwoRobotPlacementStrategy,
@@ -17,15 +18,15 @@ def main():
     custom_bounds = FieldBounds(top_left=(2.25, 1.5), bottom_right=(4.5, -1.5))
 
     runner = StrategyRunner(
-        strategy=TwoRobotPlacementStrategy(first_robot_id=0, second_robot_id=1, field_bounds=custom_bounds),
+        strategy=PointCycleStrategy(n_robots=2, field_bounds=custom_bounds, endpoint_tolerance=0.1, seed=42),
         my_team_is_yellow=True,
         my_team_is_right=True,
         mode="rsim",
         exp_friendly=2,
         exp_enemy=0,
         replay_writer_config=ReplayWriterConfig(replay_name="test_replay", overwrite_existing=True),
         print_real_fps=True,
-        profiler_name=None
+        profiler_name=None,
     )
     runner.my_strategy.render()
     runner.run()

pyproject.toml

@@ -10,7 +10,11 @@ authors = [
     { name = "Hamish Starling", email = "hamish.starling22@imperial.ac.uk" },
     { name = "Luke Moran", email = "luke.moran22@imperial.ac.uk" },
     { name = "Sadig Sadikhzada", email = "sadig.sadikhzada23@imperial.ac.uk" },
-    { name = "Li Han", email = "han.li124@imperial.ac.uk" }
+    { name = "Li Han", email = "han.li124@imperial.ac.uk" },
+    { name = "Valentin Bruhl", email = "valentin.bruhl24@imperial.ac.uk" },
+    { name = "Sarthak Agarwal", email = "sarthak.agarwal25@imperial.ac.uk" },
+    { name = "Low Sze Yoong", email = "szeyoong.low25@imperial.ac.uk" },
+    { name = "Jiaming Liu", email = "jiaming.liu25@imperial.ac.uk" },
 ]
 requires-python = ">=3.11"
 

utama_core/global_utils/math_utils.py

@@ -56,7 +56,7 @@ def normalise_heading_deg(angle):
         The normalized angle in the range [-180, 180] degrees.
     """
     half_rev = 180
-    
+
     return (angle + half_rev) % (2 * half_rev) - half_rev
 
 

utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py

@@ -1,8 +1,9 @@
 import logging
-from numpy.random import normal
 import random
 from typing import List, Tuple
 
+from numpy.random import normal
+
 from utama_core.config.formations import LEFT_START_ONE, RIGHT_START_ONE
 from utama_core.config.robot_params import RSIM_PARAMS
 from utama_core.config.settings import (
@@ -13,11 +14,15 @@
 )
 from utama_core.entities.data.command import RobotResponse
 from utama_core.entities.data.raw_vision import RawBallData, RawRobotData, RawVisionData
-from utama_core.global_utils.math_utils import deg_to_rad, rad_to_deg, normalise_heading_deg
+from utama_core.global_utils.math_utils import (
+    deg_to_rad,
+    normalise_heading_deg,
+    rad_to_deg,
+)
 from utama_core.rsoccer_simulator.src.Entities import Ball, Frame, Robot
 from utama_core.rsoccer_simulator.src.ssl.ssl_gym_base import SSLBaseEnv
-from utama_core.rsoccer_simulator.src.Utils.gaussian_noise import RsimGaussianNoise
 from utama_core.rsoccer_simulator.src.Utils import KDTree
+from utama_core.rsoccer_simulator.src.Utils.gaussian_noise import RsimGaussianNoise
 
 logger = logging.getLogger(__name__)
 
@@ -38,19 +43,19 @@ class SSLStandardEnv(SSLBaseEnv):
         Description:
             list of (x, y, theta) coords for each robot to spawn in (in meters and radians).
             See the default BLUE_START_ONE/YELLOW_START_ONE for reference.
-        
+
         gaussian_noise
         Type: RsimGaussianNoise
         Description:
             When running in rsim, add Gaussian noise to ball and robots with the given standard deviations.
             Mutates the Robot object in place. The 3 parameters are for x (in m), y (in m), and orientation (in degrees)
             respectively. Defaults to 0 for each.
-            
+
         vanishing
         Type: float
         Description:
             When running in rsim, cause robots and ball to vanish with the given probability. Defaults to 0.
-            
+
     Observation:
         Type: Tuple[FrameData, List[RobotInfo], List[RobotInfo]]
         Num     Item
@@ -78,7 +83,7 @@ def __init__(
         blue_starting_formation: list[tuple] = None,
         yellow_starting_formation: list[tuple] = None,
         gaussian_noise: RsimGaussianNoise = RsimGaussianNoise(),
-        vanishing: float = 0
+        vanishing: float = 0,
     ):
         super().__init__(
             field_type=field_type,
@@ -108,11 +113,11 @@ def __init__(
         self.latest_observation = (-1, None)
 
         logger.info(f"{n_robots_blue}v{n_robots_yellow} SSL Environment Initialized")
-        
+
         # Adding Gaussian noise and vanishing. Refer to StrategyRunner
         self.gaussian_noise = gaussian_noise
-        
-        assert vanishing >= 0
+
+        assert vanishing >= 0, "Negative vanishing probability not allowed"
         self.vanishing = vanishing
 
     def reset(self, *, seed=None, options=None):
@@ -172,7 +177,7 @@ def _frame_to_observations(
         yellow_robots_info: feedback from individual yellow robots that returns a List[RobotInfo]
         blue_robots_info: feedback from individual blue robots that returns a List[RobotInfo]
         """
-        
+
         if self.latest_observation[0] == self.steps:
             return self.latest_observation[1]
 
@@ -189,7 +194,7 @@ def _frame_to_observations(
         for i in range(len(self.frame.robots_blue)):
             if self._vanishing():
                 continue
-            
+
             robot = self.frame.robots_blue[i]
             robot_pos, robot_info = self._get_robot_observation(robot)
             blue_obs.append(robot_pos)
@@ -200,7 +205,7 @@ def _frame_to_observations(
         for i in range(len(self.frame.robots_yellow)):
             if self._vanishing():
                 continue
-                
+
             robot = self.frame.robots_yellow[i]
             robot_pos, robot_info = self._get_robot_observation(robot)
             yellow_obs.append(robot_pos)
@@ -221,7 +226,7 @@ def _frame_to_observations(
 
     def _get_robot_observation(self, robot):
         SSLStandardEnv._add_gaussian_noise_robot(robot, self.gaussian_noise)
-        
+
         robot_pos = RawRobotData(robot.id, robot.x, -robot.y, -float(deg_to_rad(robot.theta)), 1)
         robot_info = RobotResponse(robot.id, robot.infrared)
         return robot_pos, robot_info
@@ -415,47 +420,45 @@ def in_gk_area(obj):
 
         return pos_frame
 
-
     def _vanishing(self) -> bool:
         """Determines whether a frame should vanish. Only runs after Game Gater is passed"""
         return self.steps > 0 and self.vanishing and (random.random() < self.vanishing)
 
-
     @staticmethod
     def _add_gaussian_noise_ball(ball: Ball, noise: RsimGaussianNoise):
         """
         When running in rsim, add Gaussian noise to ball with the given standard deviations.
         Mutates the Robot object in place.
-        
+
         Args:
             noise (RsimGaussianNoise): The 3 parameters are for x (in m), y (in m), and orientation (in degrees) respectively.
                 Defaults to 0 for each.
         """
-        
+
         if noise.x_stddev:
             ball.x += normal(scale=noise.x_stddev)
-            
+
         if noise.y_stddev:
             ball.y += normal(scale=noise.y_stddev)
-            
+
         # No noise addition for z, since rSim is 2-D
-        
+
     @staticmethod
     def _add_gaussian_noise_robot(robot: Robot, noise: RsimGaussianNoise):
         """
         When running in rsim, add Gaussian noise to robot with the given standard deviations.
         Mutates the Robot object in place.
-        
+
         Args:
             noise (RsimGaussianNoise): The 3 parameters are for x (in m), y (in m), and orientation (in degrees) respectively.
                 Defaults to 0 for each.
         """
-        
+
         if noise.x_stddev:
             robot.x += normal(scale=noise.x_stddev)
-            
+
         if noise.y_stddev:
             robot.y += normal(scale=noise.y_stddev)
-            
+
         if noise.th_stddev_deg:
-            robot.theta = normalise_heading_deg(robot.theta + normal(scale=noise.th_stddev_deg))
+            robot.theta = normalise_heading_deg(robot.theta + normal(scale=noise.th_stddev_deg))