FIR filters

README.md

@@ -40,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
 

pixi.toml

@@ -22,6 +22,8 @@ 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]

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)

utama_core/run/refiners/position.py → utama_core/data_processing/refiners/position.py

@@ -6,13 +6,16 @@
 import numpy as np
 
 from utama_core.config.settings import BALL_MERGE_THRESHOLD
+from utama_core.data_processing.refiners.base_refiner import BaseRefiner
+from utama_core.data_processing.refiners.filters.kalman import (
+    KalmanFilter,
+    KalmanFilterBall,
+)
 from utama_core.entities.data.raw_vision import RawBallData, RawRobotData, RawVisionData
 from utama_core.entities.data.vector import Vector2D, Vector3D
 from utama_core.entities.data.vision import VisionBallData, VisionData, VisionRobotData
 from utama_core.entities.game import Ball, FieldBounds, GameFrame, Robot
 from utama_core.global_utils.mapping_utils import map_friendly_enemy_to_colors
-from utama_core.run.refiners.base_refiner import BaseRefiner
-from utama_core.run.refiners.kalman import KalmanFilter, KalmanFilterBall
 
 
 class AngleSmoother:

utama_core/run/refiners/referee.py → utama_core/data_processing/refiners/referee.py

@@ -1,10 +1,10 @@
 from typing import Optional
 
+from utama_core.data_processing.refiners.base_refiner import BaseRefiner
 from utama_core.entities.data.referee import RefereeData
 from utama_core.entities.game.team_info import TeamInfo
 from utama_core.entities.referee.referee_command import RefereeCommand
 from utama_core.entities.referee.stage import Stage
-from utama_core.run.refiners.base_refiner import BaseRefiner
 
 
 class RefereeRefiner(BaseRefiner):

utama_core/run/refiners/robot_info.py → utama_core/data_processing/refiners/robot_info.py

@@ -2,9 +2,9 @@
 from dataclasses import replace
 from typing import List
 
+from utama_core.data_processing.refiners.base_refiner import BaseRefiner
 from utama_core.entities.data.command import RobotResponse
 from utama_core.entities.game.game_frame import GameFrame
-from utama_core.run.refiners.base_refiner import BaseRefiner
 
 # TODO: current doesn't handle has_ball for enemy robots. In future, implement using vision data
 

utama_core/run/refiners/velocity.py → utama_core/data_processing/refiners/velocity.py

@@ -4,6 +4,7 @@
 
 import numpy as np  # Import NumPy
 
+from utama_core.data_processing.refiners.base_refiner import BaseRefiner
 from utama_core.entities.data.object import ObjectKey, TeamType
 from utama_core.entities.data.vector import Vector2D, Vector3D
 from utama_core.entities.game import GameFrame, Robot
@@ -12,7 +13,6 @@
     GameHistory,
     get_structured_object_key,
 )
-from utama_core.run.refiners.base_refiner import BaseRefiner
 
 logger = logging.getLogger(__name__)
 

utama_core/entities/data/vision.py

@@ -1,29 +1,29 @@
-from dataclasses import dataclass
-from typing import List
-
-# position data: meters
-# orientation: radians
-
-
-@dataclass
-class VisionBallData:
-    x: float
-    y: float
-    z: float
-    confidence: float
-
-
-@dataclass
-class VisionRobotData:
-    id: int
-    x: float
-    y: float
-    orientation: float
-
-
-@dataclass
-class VisionData:
-    ts: float
-    yellow_robots: List[VisionRobotData]
-    blue_robots: List[VisionRobotData]
-    balls: List[VisionBallData]
+from dataclasses import dataclass
+from typing import List
+
+# position data: meters
+# orientation: radians
+
+
+@dataclass
+class VisionBallData:
+    x: float
+    y: float
+    z: float
+    confidence: float
+
+
+@dataclass
+class VisionRobotData:
+    id: int
+    x: float
+    y: float
+    orientation: float
+
+
+@dataclass
+class VisionData:
+    ts: float
+    yellow_robots: List[VisionRobotData]
+    blue_robots: List[VisionRobotData]
+    balls: List[VisionBallData]

utama_core/run/game_gater.py

@@ -1,10 +1,10 @@
 import time
 from typing import Deque, List, Optional, Tuple
 
+from utama_core.data_processing.refiners import PositionRefiner
 from utama_core.entities.data.raw_vision import RawVisionData
 from utama_core.entities.game.game_frame import GameFrame
 from utama_core.rsoccer_simulator.src.ssl.ssl_gym_base import SSLBaseEnv
-from utama_core.run.refiners import PositionRefiner
 
 
 class GameGater:

utama_core/run/strategy_runner.py

@@ -19,6 +19,12 @@
     MAX_GAME_HISTORY,
     TIMESTEP,
 )
+from utama_core.data_processing.receivers import VisionReceiver
+from utama_core.data_processing.refiners import (
+    PositionRefiner,
+    RobotInfoRefiner,
+    VelocityRefiner,
+)
 from utama_core.entities.data.command import RobotCommand
 from utama_core.entities.data.raw_vision import RawVisionData
 from utama_core.entities.game import Game, GameHistory
@@ -33,8 +39,6 @@
 from utama_core.rsoccer_simulator.src.ssl.envs import SSLStandardEnv
 from utama_core.rsoccer_simulator.src.Utils.gaussian_noise import RsimGaussianNoise
 from utama_core.run import GameGater
-from utama_core.run.receivers import VisionReceiver
-from utama_core.run.refiners import PositionRefiner, RobotInfoRefiner, VelocityRefiner
 from utama_core.strategy.common.abstract_strategy import AbstractStrategy
 from utama_core.team_controller.src.controllers import (
     AbstractSimController,

utama_core/skills/src/defend_parameter.py

@@ -3,11 +3,11 @@
 
 import numpy as np
 
+from utama_core.data_processing.predictors.position import predict_ball_pos_at_x
 from utama_core.entities.data.vector import Vector2D
 from utama_core.entities.game import Game
 from utama_core.motion_planning.src.common.motion_controller import MotionController
 from utama_core.rsoccer_simulator.src.ssl.envs.standard_ssl import SSLStandardEnv
-from utama_core.run.predictors.position import predict_ball_pos_at_x
 from utama_core.skills.src.go_to_point import go_to_point
 
 
@@ -22,10 +22,20 @@ def defend_parameter(
     if len(game.friendly_robots) > 2:
         if game.ball.p.y >= -0.5 and robot_id == 1:
             target_pos = [3.0 if game.my_team_is_right else -3.0, -1.2]
-            return go_to_point(game, motion_controller, robot_id, Vector2D(target_pos[0], target_pos[1]))
+            return go_to_point(
+                game,
+                motion_controller,
+                robot_id,
+                Vector2D(target_pos[0], target_pos[1]),
+            )
         elif game.ball.p.y < -0.5 and robot_id == 2:
             target_pos = [3.0 if game.my_team_is_right else -3.0, 1.2]
-            return go_to_point(game, motion_controller, robot_id, Vector2D(target_pos[0], target_pos[1]))
+            return go_to_point(
+                game,
+                motion_controller,
+                robot_id,
+                Vector2D(target_pos[0], target_pos[1]),
+            )
     if robot_id == 1:
         goal_frame = 0.5
     else:
@@ -65,12 +75,9 @@ def distance(x1, y1, x2, y2):
     if (
         vel[0] ** 2 + vel[1] ** 2 > 0.05
         and (ball_y_at_baseline is not None and ball_y_at_baseline[1] < 0.5 and ball_y_at_baseline[1] > -0.5)
-        and (
-            ball_y_at_robo is not None
-            and abs(ball_y_at_robo[1] - defenseing_friendly.p.y) > 0.1
-        )
+        and (ball_y_at_robo is not None and abs(ball_y_at_robo[1] - defenseing_friendly.p.y) > 0.1)
     ):
-        x2, y2 = 4.5 if game.my_team_is_right else -4.5, goal_frame + 0.2 if robot_id == 1 else goal_frame - 0.2
+        x2, y2 = 4.5 if game.my_team_is_right else -4.5, (goal_frame + 0.2 if robot_id == 1 else goal_frame - 0.2)
         x3, y3, x4, y4 = positions_to_defend_parameter(x2, y2)
         target_pos = np.array([x4, y4])
 
@@ -92,4 +99,10 @@ def distance(x1, y1, x2, y2):
             vec_dir = np.array([0.0, 0.0])
         target_pos = np.array([x4, y4]) - vec_dir * robot_rad
 
-    return go_to_point(game, motion_controller, robot_id, Vector2D(target_pos[0], target_pos[1]), dribbling=True)
+    return go_to_point(
+        game,
+        motion_controller,
+        robot_id,
+        Vector2D(target_pos[0], target_pos[1]),
+        dribbling=True,
+    )