Feature/fastpathplanning

.gitignore

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

README.md

@@ -1,3 +1,4 @@
+[![Ask DeepWiki](https://deepwiki.com/badge.svg)](https://deepwiki.com/First-Order-RoboCup-SSL/Utama-Core)
 # Utama Core
 First Order Robotics core software stack for [RoboCup SSL](https://ssl.robocup.org/), an international league where teams build autonomous robotic teams to play football competitively.
 
@@ -39,16 +40,17 @@ First Order Robotics core software stack for [RoboCup SSL](https://ssl.robocup.o
 ### Folder Structure
 
 1. `strategy`: higher level control from above roles to plays and tactics in decision-tree like abstraction
-2. `skills`: lowest level of control for individual robots
-3. `motion_planning`: control algorithms for movement and path planning
-4. `team_controller`: interfacing with vision (including processing) and robots
-5. `run`: The logic for main running loop, including refiners and predictors
-6. `global_utils`: store utility functions that can be shared across all folders
-7. `entities`: store classes for building field, robot, data entities etc.
-8. `rsoccer_simulator`: Lightweight rSoccer simulator for testing
-9. `replay`: replay system for storing played games in a .pkl file that can be reconstructed in rsoccer sim
-10. `tests`: include all unit tests here
-11. `config`: configs for the robots (defaults, settings, roles/tactics enums, etc.)
+1. `skills`: lowest level of control for individual robots
+1. `motion_planning`: control algorithms for movement and path planning
+1. `team_controller`: interfacing with vision (including processing) and robots
+1. `run`: The logic for main running loop
+1. `data_processing`: processors of vision, robot_info and referee raw data
+1. `global_utils`: store utility functions that can be shared across all folders
+1. `entities`: store classes for building field, robot, data entities etc.
+1. `rsoccer_simulator`: Lightweight rSoccer simulator for testing
+1. `replay`: replay system for storing played games in a .pkl file that can be reconstructed in rsoccer sim
+1. `tests`: include all unit tests here
+1. `config`: configs for the robots (defaults, settings, roles/tactics enums, etc.)
 
 ### Code Writing
 

main.py

@@ -1,9 +1,11 @@
 from utama_core.entities.game.field import FieldBounds
 from utama_core.replay import ReplayWriterConfig
+from utama_core.rsoccer_simulator.src.Utils.gaussian_noise import RsimGaussianNoise
 from utama_core.run import StrategyRunner
 from utama_core.strategy.examples import (
     DefenceStrategy,
     GoToBallExampleStrategy,
+    PointCycleStrategy,
     RobotPlacementStrategy,
     StartupStrategy,
     TwoRobotPlacementStrategy,
@@ -16,7 +18,7 @@ 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",

pixi.toml

@@ -3,7 +3,7 @@ name = "Utama-Core"
 channels = ["https://prefix.dev/conda-forge"]
 platforms = ["linux-64", "osx-arm64"]
 preview = ["pixi-build"]
-version = "1.5.19"
+version = "1.8.0"
 
 [pypi-dependencies]
 lenses = ">=1.2.0"
@@ -22,11 +22,13 @@ pre-commit = ">=3.8.0"
 hatch = ">=1.14.1,<2"
 graphviz = ">=13.1.2,<14"
 snakeviz = ">=2.2.2,<3"
+scipy = ">=1.16.3,<2"
+pandas = "==3.0.1"
 rich = ">=14.2.0,<15"
 
 [package]
 name = "Utama-Core"
-version = "1.5.19"
+version = "1.8.0"
 
 [package.build]
 backend = { name = "pixi-build-python", version = "*" }

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/__init__.py

@@ -1 +1 @@
-__version__ = "1.5.19"
+__version__ = "1.8.0"

utama_core/config/settings.py

@@ -31,6 +31,9 @@
 BAUD_RATE = 115200
 PORT = "/dev/ttyUSB0"
 TIMEOUT = 0.1
+KICKER_COOLDOWN_TIME = 10  # in seconds to prevent kicker from being actuated too frequently
+KICKER_COOLDOWN_TIMESTEPS = int(KICKER_COOLDOWN_TIME * CONTROL_FREQUENCY)  # in timesteps
+KICKER_PERSIST_TIMESTEPS = 10  # in timesteps to persist the kick command
 
 MAX_GAME_HISTORY = 20  # number of previous game states to keep in Game
 

utama_core/data_processing/__init__.py

@@ -0,0 +1 @@
+

utama_core/data_processing/receivers/__init__.py

@@ -0,0 +1,2 @@
+from utama_core.data_processing.receivers.referee_receiver import RefereeMessageReceiver
+from utama_core.data_processing.receivers.vision_receiver import VisionReceiver

utama_core/data_processing/refiners/__init__.py

@@ -0,0 +1,4 @@
+from utama_core.data_processing.refiners.position import PositionRefiner
+from utama_core.data_processing.refiners.referee import RefereeRefiner
+from utama_core.data_processing.refiners.robot_info import RobotInfoRefiner
+from utama_core.data_processing.refiners.velocity import VelocityRefiner

utama_core/data_processing/refiners/filters/fir.py

@@ -0,0 +1,119 @@
+from collections import deque
+from typing import Union
+
+import numpy as np
+from scipy.signal import firwin
+
+from utama_core.entities.data.vision import VisionRobotData
+
+
+class FIR_filter:
+    """
+    Finite Impulse Response (FIR) filter for 2D position and orientation.
+    This is essentially a weighted average of the past n data points.
+
+    More about the methodology:
+    https://youtube.com/playlist?list=PLbqhA-NKGP6Afr_KbPUuy_yIBpPR4jzWo&si=7l2BVnsN_jSKq2JL.
+
+    Args:
+        fs (float): Sampling rate (Hz). Defaults to 60.0.
+        taps (array-like or None): FIR taps. If None, a series of taps of length `window_len` is created.
+        window_len (int): Number of taps to be created if `taps` is None. Default 20.
+        cutoff (float): Cutoff frequency of the filter.
+    """
+
+    def __init__(
+        self,
+        fs: float = 60.0,
+        taps: Union[np.array, None] = None,
+        window_len: int = 5,
+        cutoff: Union[float, None] = None,
+    ):
+        self._fs = float(fs)
+        self._N = window_len
+        self._nyquist = 0.4 * self._fs
+
+        if cutoff and cutoff < self._nyquist:
+            self._cutoff = cutoff
+        else:
+            # Sets cutoff frequency according to the maximum acceleration and
+            # velocity of robots, below the limits dictated by Nyquist's theorem.
+
+            a_max = 50
+            v_max = 5
+            fc = a_max / (2 * np.pi * v_max)
+
+            self._cutoff = min(self._nyquist, fc)
+
+        if taps is None:
+            assert window_len >= 1, "window_len must be >= 1"
+            self._taps = firwin(window_len, self._cutoff, fs=fs)
+
+        else:
+            t = np.asarray(taps, dtype=float).ravel()
+            assert t.size >= 1, "taps must have at least 1 element"
+            # Normalize taps to sum to 1 for unity DC gain
+            self._taps = t / np.sum(t)
+            self._N = self._taps.size
+
+        self._buf_x = deque(maxlen=self._N)
+        self._buf_y = deque(maxlen=self._N)
+        self._buf_th = deque(maxlen=self._N)
+
+    def step(self, data: tuple[float]):
+        """
+        A single iteration of the filter
+
+        Args:
+            data (tuple[float]): The new vision data received (x and y coordinates
+                in metres, orientation in radians).
+
+        Returns:
+            tuple[float]: The filtered data
+        """
+
+        x, y, th = map(float, data)
+        # theta = normalise_heading(theta)
+
+        self._buf_x.append(x)
+        self._buf_y.append(y)
+        self._buf_th.append(th)
+
+        # Use only the available samples during warm-up
+        k = len(self._buf_x)  # same as len(buf_y) and len(buf_th)
+        taps_eff = self._taps[-k:]
+        taps_eff = taps_eff / np.sum(taps_eff)  # renormalize
+
+        # Position FIR
+        x_arr = np.asarray(self._buf_x, dtype=float)
+        y_arr = np.asarray(self._buf_y, dtype=float)
+        x_f = float(np.dot(taps_eff, x_arr))
+        y_f = float(np.dot(taps_eff, y_arr))
+
+        # Orientation FIR via circular averaging - currently disabled
+        # th_arr = np.asarray(self._buf_th, dtype=float)
+        # s = np.dot(taps_eff, np.sin(th_arr))
+        # c = np.dot(taps_eff, np.cos(th_arr))
+        # th_f = float(np.arctan2(s, c))  # already wrapped to (-pi, pi]
+
+        return x_f, y_f, th
+
+    @staticmethod
+    def filter_robot(filter, data: VisionRobotData) -> VisionRobotData:
+        """
+        Externally callable function for Position Refiner to pass data
+        to be filtered.
+
+        Args:
+            filter (FIR_filter): The filter associated with a robot
+            data (VisionRobotData): The new vision data received (x and y
+                coordinates in metres, orientation in radians).
+
+        Returns:
+            VisionRobotData: The filtered vision data.
+        """
+
+        # class VisionRobotData: id: int; x: float; y: float; orientation: float
+        (x_f, y_f, th_f) = filter.step((data.x, data.y, data.orientation))
+
+        return VisionRobotData(id=data.id, x=x_f, y=y_f, orientation=th_f)