Parameterisable Full Field Dimensions

main.py

@@ -1,3 +1,4 @@
+from utama_core.config.field_params import GREAT_EXHIBITION_FIELD_DIMS
 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
@@ -15,7 +16,7 @@
 def main():
     # Setup for real testing
     # Custom field size based setup in real
-    custom_bounds = FieldBounds(top_left=(2.25, 1.5), bottom_right=(4.5, -1.5))
+    custom_bounds = FieldBounds(top_left=(-2, 1.5), bottom_right=(1, -1.5))
 
     runner = StrategyRunner(
         strategy=RandomMovementStrategy(n_robots=2, field_bounds=custom_bounds, endpoint_tolerance=0.1, seed=42),
@@ -25,6 +26,8 @@ def main():
         exp_friendly=2,
         exp_enemy=0,
         replay_writer_config=ReplayWriterConfig(replay_name="test_replay", overwrite_existing=True),
+        field_bounds=custom_bounds,
+        full_field_dims=GREAT_EXHIBITION_FIELD_DIMS,
         print_real_fps=True,
         profiler_name=None,
     )

utama_core/config/field_params.py

@@ -0,0 +1,148 @@
+from dataclasses import dataclass
+from functools import cached_property
+
+import numpy as np
+
+
+@dataclass(frozen=True)
+class FieldBounds:
+    top_left: tuple[float, float]
+    bottom_right: tuple[float, float]
+
+    @property
+    def center(self) -> tuple[float, float]:
+        """Calculates the geometric center of the field bounds."""
+        cx = (self.top_left[0] + self.bottom_right[0]) / 2.0
+        cy = (self.top_left[1] + self.bottom_right[1]) / 2.0
+        return (cx, cy)
+
+
+@dataclass(frozen=True)
+class FieldDimensions:
+    """Holds field dimensions and derives all geometric shapes."""
+
+    full_field_half_length: float
+    full_field_half_width: float
+    half_defense_area_depth: float
+    half_defense_area_width: float
+    half_goal_width: float
+
+    # --- Bounds ---
+
+    @cached_property
+    def full_field_bounds(self):
+        return FieldBounds(
+            top_left=(-self.full_field_half_length, self.full_field_half_width),
+            bottom_right=(self.full_field_half_length, -self.full_field_half_width),
+        )
+
+    # --- Full field polygon ---
+
+    @cached_property
+    def full_field(self) -> np.ndarray:
+        L = self.full_field_half_length
+        W = self.full_field_half_width
+        return np.array(
+            [
+                (L, W),
+                (L, -W),
+                (-L, -W),
+                (-L, W),
+            ]
+        )
+
+    # --- Goal lines ---
+
+    @cached_property
+    def right_goal_line(self) -> np.ndarray:
+        L = self.full_field_half_length
+        G = self.half_goal_width
+        return np.array(
+            [
+                (L, G),
+                (L, -G),
+            ]
+        )
+
+    @cached_property
+    def left_goal_line(self) -> np.ndarray:
+        L = self.full_field_half_length
+        G = self.half_goal_width
+        return np.array(
+            [
+                (-L, G),
+                (-L, -G),
+            ]
+        )
+
+    # --- Defense areas ---
+
+    @cached_property
+    def right_defense_area(self) -> np.ndarray:
+        L = self.full_field_half_length
+        D = self.half_defense_area_depth
+        W = self.half_defense_area_width
+        return np.array(
+            [
+                (L, W),
+                (L - 2 * D, W),
+                (L - 2 * D, -W),
+                (L, -W),
+            ]
+        )
+
+    @cached_property
+    def left_defense_area(self) -> np.ndarray:
+        L = self.full_field_half_length
+        D = self.half_defense_area_depth
+        W = self.half_defense_area_width
+        return np.array(
+            [
+                (-L, W),
+                (-L + 2 * D, W),
+                (-L + 2 * D, -W),
+                (-L, -W),
+            ]
+        )
+
+    def __post_init__(self):
+        L = self.full_field_half_length
+        W = self.full_field_half_width
+        D = self.half_defense_area_depth
+        DW = self.half_defense_area_width
+        G = self.half_goal_width
+
+        # --- Positivity ---
+        if not (L > 0 and W > 0):
+            raise ValueError("Field length/width must be positive")
+        if not (D > 0 and DW > 0 and G > 0):
+            raise ValueError("Goal/defense measurements must be positive")
+
+        # --- Fit constraints ---
+        if 2 * D > L:
+            raise ValueError(f"Defense depth {2*D} exceeds field length {L}")
+        if DW > W:
+            raise ValueError(f"Defense width {DW} exceeds field width {W}")
+        if G > W:
+            raise ValueError(f"Goal width {G} exceeds field width {W}")
+
+        # --- Optional semantic constraint ---
+        if G > DW:
+            raise ValueError(f"Goal width {G} should not exceed defense width {DW}")
+
+
+STANDARD_FIELD_DIMS = FieldDimensions(
+    full_field_half_length=4.5,
+    full_field_half_width=3.0,
+    half_defense_area_depth=0.5,
+    half_defense_area_width=1,
+    half_goal_width=0.5,
+)
+
+GREAT_EXHIBITION_FIELD_DIMS = FieldDimensions(
+    full_field_half_length=2.0,
+    full_field_half_width=1.5,
+    half_defense_area_depth=0.4,
+    half_defense_area_width=0.8,
+    half_goal_width=0.5,
+)

utama_core/config/formations.py

@@ -1,21 +1,177 @@
+import math
+from enum import Enum
+from typing import NamedTuple
+
 import numpy as np
 
+from utama_core.config.field_params import FieldBounds, FieldDimensions
+from utama_core.config.physical_constants import MAX_ROBOTS, ROBOT_RADIUS
+from utama_core.global_utils.math_utils import normalise_heading
+
+
+class FormationEntry(NamedTuple):
+    x: float
+    y: float
+    theta: float
+
+
 # Starting positions for right team
-RIGHT_START_ONE = [
-    (4.2000, 0.0, np.pi),
-    (3.4000, -0.2000, np.pi),
-    (3.4000, 0.2000, np.pi),
-    (0.7000, 0.0, np.pi),
-    (0.7000, 2.2500, np.pi),
-    (0.7000, -2.2500, np.pi),
-]
-
-# Starting positions for left team
-LEFT_START_ONE = [
-    (-4.2000, 0.0, 0),
-    (-3.4000, 0.2000, 0),
-    (-3.4000, -0.2000, 0),
-    (-0.7000, 0.0, 0),
-    (-0.7000, -2.2500, 0),
-    (-0.7000, 2.2500, 0),
-]
+
+#################### INSERT FORMATIONS HERE ######################
+# Normalised right team formation that will be scaled,
+# then mirrored for left.
+
+# Anisotropic normalised scaling to field half-length, half-width
+# e.g
+# 0.75 = 3.375 (actual scale of x-coord) / 4.5 (field half-length)
+# -0.06 = -0.18 (actual scale of y-coord) / 3.0 (field half-width)
+##################################################################
+
+
+class FormationType(Enum):
+    START_ONE = "START_ONE"
+
+
+FORMATIONS = {
+    FormationType.START_ONE: [
+        FormationEntry(0.9, 0.0, np.pi),
+        FormationEntry(0.75, -0.2, np.pi),
+        FormationEntry(0.75, 0.2, np.pi),
+        FormationEntry(0.16, 0.0, np.pi),
+        FormationEntry(0.16, 0.75, np.pi),
+        FormationEntry(0.16, -0.75, np.pi),
+    ],
+}
+
+################## END OF FORMATIONS ##########################
+
+
+def _mirror(formation: list[FormationEntry], bounds: FieldBounds) -> list[FormationEntry]:
+    cx, _ = bounds.center
+
+    return [
+        FormationEntry(
+            2 * cx - entry.x,
+            entry.y,
+            normalise_heading(np.pi - entry.theta),
+        )
+        for entry in formation
+    ]
+
+
+def _scale(norm_formation, bounds: FieldBounds) -> list[FormationEntry]:
+    x_min = bounds.top_left[0]
+    x_max = bounds.bottom_right[0]
+    y_max = bounds.top_left[1]
+    y_min = bounds.bottom_right[1]
+
+    L = (x_max - x_min) / 2
+    W = (y_max - y_min) / 2
+
+    cx, cy = bounds.center
+
+    return [FormationEntry(cx + x * L, cy + y * W, theta) for x, y, theta in norm_formation]
+
+
+def _validate_bounds_and_intra_team_collision(
+    formation: list[FormationEntry],
+    bounds: FieldBounds,
+) -> None:
+    """
+    Validate that the formation fits inside the bounding box and that robots do not overlap.
+    The robot center may touch the edges.
+
+    Raises ValueError if the formation cannot fit.
+
+    Args:
+        formation: List of (x, y, theta) tuples for robots.
+        bounds: FieldBounds object with top_left and bottom_right coordinates.
+    """
+    # --- Bounding box edges (center can touch boundary) ---
+    x_min = bounds.top_left[0]
+    x_max = bounds.bottom_right[0]
+    y_max = bounds.top_left[1]
+    y_min = bounds.bottom_right[1]
+
+    # --- Check bounds for each robot ---
+    for i, (x, y, _) in enumerate(formation):
+        if not (x_min <= x <= x_max):
+            raise ValueError(f"Robot {i} x-position out of bounds: {x}, allowed: [{x_min}, {x_max}]")
+        if not (y_min <= y <= y_max):
+            raise ValueError(f"Robot {i} y-position out of bounds: {y}, allowed: [{y_min}, {y_max}]")
+
+    # --- Check pairwise collisions ---
+    n = len(formation)
+    for i in range(n):
+        x1, y1, _ = formation[i]
+        for j in range(i + 1, n):
+            x2, y2, _ = formation[j]
+            dist = math.hypot(x1 - x2, y1 - y2)
+            if dist < 2 * ROBOT_RADIUS:
+                raise ValueError(
+                    f"Could not fit all robots in provided FieldBounds/FieldDimensions. Robots {i} and {j} overlap (distance={dist:.3f})"
+                )
+
+
+def _validate_team_separation(left, right):
+    """
+    Validate that the left and right teams are sufficiently separated to avoid collisions.
+    """
+    max_left_x = -np.inf
+    min_right_x = np.inf
+    if left:
+        max_left_x = max(x for x, _, _ in left)
+    if right:
+        min_right_x = min(x for x, _, _ in right)
+
+    gap = min_right_x - max_left_x
+    required = 2 * ROBOT_RADIUS
+
+    if gap < required:
+        raise ValueError(f"Teams not sufficiently separated: gap={gap:.3f}, required={required:.3f}")
+
+
+# TODO: can consider a fitting algorithm that can optimise robot placement so that the chance of running out of space is reduced.
+
+
+def get_formations(
+    bounds: FieldBounds,
+    n_left: int,
+    n_right: int,
+    formation_type: FormationType,
+) -> tuple[list[FormationEntry], list[FormationEntry]]:
+    """
+    Returns the starting formations for both teams based on the provided field dimensions.
+    The formations are defined as lists of FormationEntry objects, which contain the x and y coordinates
+
+    Args:
+        bounds: FieldBounds object defining the top-left and bottom-right corners of the field.
+        n_left: Number of robots on the left team.
+        n_right: Number of robots on the right team.
+        formation_type: The type of formation to generate (e.g., START_ONE).
+
+    Returns:
+        tuple[list[FormationEntry], list[FormationEntry]]: A tuple containing two lists of FormationEntry objects.
+                                                            left and right team formations respectively.
+    """
+    if n_left > MAX_ROBOTS or n_right > MAX_ROBOTS:
+        raise ValueError(
+            f"Number of robots per team cannot exceed {MAX_ROBOTS}. Got n_left={n_left}, n_right={n_right}."
+        )
+    if formation_type not in FORMATIONS:
+        raise ValueError(f"Formation '{formation_type.value}' not found. Available: {list(FORMATIONS.keys())}")
+
+    base = FORMATIONS[formation_type]
+    if n_left > len(base) or n_right > len(base):
+        raise ValueError(
+            f"Formation '{formation_type.value}' only defines {len(base)} positions, but got n_left={n_left}, n_right={n_right}."
+        )
+
+    left = _mirror(_scale(base[:n_left], bounds), bounds)
+    right = _scale(base[:n_right], bounds)
+
+    _validate_bounds_and_intra_team_collision(left, bounds)
+    _validate_bounds_and_intra_team_collision(right, bounds)
+    _validate_team_separation(left, right)
+
+    return left, right

utama_core/config/physical_constants.py

@@ -1,2 +1,3 @@
 ROBOT_RADIUS = 0.09
 MAX_ROBOTS = 6
+BALL_RADIUS = 0.0215