Skip to content

Commit f0a4496

Browse files
energy-in-jolesCopilot
energy-in-joles
and
Copilot
authored
Parameterisable Full Field Dimensions (#112)
* parameterisable field * more fixes * fix out of bounds issue by moving game creation to run/run_test * remove use of asserts * Update utama_core/run/strategy_runner.py Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> * more fixes * changed it so that formations are now dynamically allocated based on field bounds * add TODO comment about fitting algo * fix rsim hardcoded ball pos. Extensive testing * Update utama_core/strategy/examples/startup_strategy.py Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> * add FieldBounds overlay and rendered rsim field based on FieldDimensions * fix bug with send robot stop command * small fixes * ensure clear left and right team separation * correct logic * fix goalkeep constants * fixes * relax vision bounding for position refiner * improve documentation for position refiner --------- Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com>
1 parent a7ad1b6 commit f0a4496

47 files changed

Lines changed: 1560 additions & 602 deletions

Some content is hidden

Large Commits have some content hidden by default. Use the searchbox below for content that may be hidden.

‎main.py‎

Lines changed: 4 additions & 1 deletion
Original file line numberDiff line numberDiff line change
@@ -1,3 +1,4 @@
1+
from utama_core.config.field_params import GREAT_EXHIBITION_FIELD_DIMS
12
from utama_core.entities.game.field import FieldBounds
23
from utama_core.replay import ReplayWriterConfig
34
from utama_core.rsoccer_simulator.src.Utils.gaussian_noise import RsimGaussianNoise
@@ -15,7 +16,7 @@
1516
def main():
1617
# Setup for real testing
1718
# Custom field size based setup in real
18-
custom_bounds = FieldBounds(top_left=(2.25, 1.5), bottom_right=(4.5, -1.5))
19+
custom_bounds = FieldBounds(top_left=(-2, 1.5), bottom_right=(1, -1.5))
1920

2021
runner = StrategyRunner(
2122
strategy=RandomMovementStrategy(n_robots=2, field_bounds=custom_bounds, endpoint_tolerance=0.1, seed=42),
@@ -25,6 +26,8 @@ def main():
2526
exp_friendly=2,
2627
exp_enemy=0,
2728
replay_writer_config=ReplayWriterConfig(replay_name="test_replay", overwrite_existing=True),
29+
field_bounds=custom_bounds,
30+
full_field_dims=GREAT_EXHIBITION_FIELD_DIMS,
2831
print_real_fps=True,
2932
profiler_name=None,
3033
)

‎utama_core/config/field_params.py‎

Lines changed: 148 additions & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -0,0 +1,148 @@
1+
from dataclasses import dataclass
2+
from functools import cached_property
3+
4+
import numpy as np
5+
6+
7+
@dataclass(frozen=True)
8+
class FieldBounds:
9+
top_left: tuple[float, float]
10+
bottom_right: tuple[float, float]
11+
12+
@property
13+
def center(self) -> tuple[float, float]:
14+
"""Calculates the geometric center of the field bounds."""
15+
cx = (self.top_left[0] + self.bottom_right[0]) / 2.0
16+
cy = (self.top_left[1] + self.bottom_right[1]) / 2.0
17+
return (cx, cy)
18+
19+
20+
@dataclass(frozen=True)
21+
class FieldDimensions:
22+
"""Holds field dimensions and derives all geometric shapes."""
23+
24+
full_field_half_length: float
25+
full_field_half_width: float
26+
half_defense_area_depth: float
27+
half_defense_area_width: float
28+
half_goal_width: float
29+
30+
# --- Bounds ---
31+
32+
@cached_property
33+
def full_field_bounds(self):
34+
return FieldBounds(
35+
top_left=(-self.full_field_half_length, self.full_field_half_width),
36+
bottom_right=(self.full_field_half_length, -self.full_field_half_width),
37+
)
38+
39+
# --- Full field polygon ---
40+
41+
@cached_property
42+
def full_field(self) -> np.ndarray:
43+
L = self.full_field_half_length
44+
W = self.full_field_half_width
45+
return np.array(
46+
[
47+
(L, W),
48+
(L, -W),
49+
(-L, -W),
50+
(-L, W),
51+
]
52+
)
53+
54+
# --- Goal lines ---
55+
56+
@cached_property
57+
def right_goal_line(self) -> np.ndarray:
58+
L = self.full_field_half_length
59+
G = self.half_goal_width
60+
return np.array(
61+
[
62+
(L, G),
63+
(L, -G),
64+
]
65+
)
66+
67+
@cached_property
68+
def left_goal_line(self) -> np.ndarray:
69+
L = self.full_field_half_length
70+
G = self.half_goal_width
71+
return np.array(
72+
[
73+
(-L, G),
74+
(-L, -G),
75+
]
76+
)
77+
78+
# --- Defense areas ---
79+
80+
@cached_property
81+
def right_defense_area(self) -> np.ndarray:
82+
L = self.full_field_half_length
83+
D = self.half_defense_area_depth
84+
W = self.half_defense_area_width
85+
return np.array(
86+
[
87+
(L, W),
88+
(L - 2 * D, W),
89+
(L - 2 * D, -W),
90+
(L, -W),
91+
]
92+
)
93+
94+
@cached_property
95+
def left_defense_area(self) -> np.ndarray:
96+
L = self.full_field_half_length
97+
D = self.half_defense_area_depth
98+
W = self.half_defense_area_width
99+
return np.array(
100+
[
101+
(-L, W),
102+
(-L + 2 * D, W),
103+
(-L + 2 * D, -W),
104+
(-L, -W),
105+
]
106+
)
107+
108+
def __post_init__(self):
109+
L = self.full_field_half_length
110+
W = self.full_field_half_width
111+
D = self.half_defense_area_depth
112+
DW = self.half_defense_area_width
113+
G = self.half_goal_width
114+
115+
# --- Positivity ---
116+
if not (L > 0 and W > 0):
117+
raise ValueError("Field length/width must be positive")
118+
if not (D > 0 and DW > 0 and G > 0):
119+
raise ValueError("Goal/defense measurements must be positive")
120+
121+
# --- Fit constraints ---
122+
if 2 * D > L:
123+
raise ValueError(f"Defense depth {2*D} exceeds field length {L}")
124+
if DW > W:
125+
raise ValueError(f"Defense width {DW} exceeds field width {W}")
126+
if G > W:
127+
raise ValueError(f"Goal width {G} exceeds field width {W}")
128+
129+
# --- Optional semantic constraint ---
130+
if G > DW:
131+
raise ValueError(f"Goal width {G} should not exceed defense width {DW}")
132+
133+
134+
STANDARD_FIELD_DIMS = FieldDimensions(
135+
full_field_half_length=4.5,
136+
full_field_half_width=3.0,
137+
half_defense_area_depth=0.5,
138+
half_defense_area_width=1,
139+
half_goal_width=0.5,
140+
)
141+
142+
GREAT_EXHIBITION_FIELD_DIMS = FieldDimensions(
143+
full_field_half_length=2.0,
144+
full_field_half_width=1.5,
145+
half_defense_area_depth=0.4,
146+
half_defense_area_width=0.8,
147+
half_goal_width=0.5,
148+
)

‎utama_core/config/formations.py‎

Lines changed: 174 additions & 18 deletions
Original file line numberDiff line numberDiff line change
@@ -1,21 +1,177 @@
1+
import math
2+
from enum import Enum
3+
from typing import NamedTuple
4+
15
import numpy as np
26

7+
from utama_core.config.field_params import FieldBounds, FieldDimensions
8+
from utama_core.config.physical_constants import MAX_ROBOTS, ROBOT_RADIUS
9+
from utama_core.global_utils.math_utils import normalise_heading
10+
11+
12+
class FormationEntry(NamedTuple):
13+
x: float
14+
y: float
15+
theta: float
16+
17+
318
# Starting positions for right team
4-
RIGHT_START_ONE = [
5-
(4.2000, 0.0, np.pi),
6-
(3.4000, -0.2000, np.pi),
7-
(3.4000, 0.2000, np.pi),
8-
(0.7000, 0.0, np.pi),
9-
(0.7000, 2.2500, np.pi),
10-
(0.7000, -2.2500, np.pi),
11-
]
12-
13-
# Starting positions for left team
14-
LEFT_START_ONE = [
15-
(-4.2000, 0.0, 0),
16-
(-3.4000, 0.2000, 0),
17-
(-3.4000, -0.2000, 0),
18-
(-0.7000, 0.0, 0),
19-
(-0.7000, -2.2500, 0),
20-
(-0.7000, 2.2500, 0),
21-
]
19+
20+
#################### INSERT FORMATIONS HERE ######################
21+
# Normalised right team formation that will be scaled,
22+
# then mirrored for left.
23+
24+
# Anisotropic normalised scaling to field half-length, half-width
25+
# e.g
26+
# 0.75 = 3.375 (actual scale of x-coord) / 4.5 (field half-length)
27+
# -0.06 = -0.18 (actual scale of y-coord) / 3.0 (field half-width)
28+
##################################################################
29+
30+
31+
class FormationType(Enum):
32+
START_ONE = "START_ONE"
33+
34+
35+
FORMATIONS = {
36+
FormationType.START_ONE: [
37+
FormationEntry(0.9, 0.0, np.pi),
38+
FormationEntry(0.75, -0.2, np.pi),
39+
FormationEntry(0.75, 0.2, np.pi),
40+
FormationEntry(0.16, 0.0, np.pi),
41+
FormationEntry(0.16, 0.75, np.pi),
42+
FormationEntry(0.16, -0.75, np.pi),
43+
],
44+
}
45+
46+
################## END OF FORMATIONS ##########################
47+
48+
49+
def _mirror(formation: list[FormationEntry], bounds: FieldBounds) -> list[FormationEntry]:
50+
cx, _ = bounds.center
51+
52+
return [
53+
FormationEntry(
54+
2 * cx - entry.x,
55+
entry.y,
56+
normalise_heading(np.pi - entry.theta),
57+
)
58+
for entry in formation
59+
]
60+
61+
62+
def _scale(norm_formation, bounds: FieldBounds) -> list[FormationEntry]:
63+
x_min = bounds.top_left[0]
64+
x_max = bounds.bottom_right[0]
65+
y_max = bounds.top_left[1]
66+
y_min = bounds.bottom_right[1]
67+
68+
L = (x_max - x_min) / 2
69+
W = (y_max - y_min) / 2
70+
71+
cx, cy = bounds.center
72+
73+
return [FormationEntry(cx + x * L, cy + y * W, theta) for x, y, theta in norm_formation]
74+
75+
76+
def _validate_bounds_and_intra_team_collision(
77+
formation: list[FormationEntry],
78+
bounds: FieldBounds,
79+
) -> None:
80+
"""
81+
Validate that the formation fits inside the bounding box and that robots do not overlap.
82+
The robot center may touch the edges.
83+
84+
Raises ValueError if the formation cannot fit.
85+
86+
Args:
87+
formation: List of (x, y, theta) tuples for robots.
88+
bounds: FieldBounds object with top_left and bottom_right coordinates.
89+
"""
90+
# --- Bounding box edges (center can touch boundary) ---
91+
x_min = bounds.top_left[0]
92+
x_max = bounds.bottom_right[0]
93+
y_max = bounds.top_left[1]
94+
y_min = bounds.bottom_right[1]
95+
96+
# --- Check bounds for each robot ---
97+
for i, (x, y, _) in enumerate(formation):
98+
if not (x_min <= x <= x_max):
99+
raise ValueError(f"Robot {i} x-position out of bounds: {x}, allowed: [{x_min}, {x_max}]")
100+
if not (y_min <= y <= y_max):
101+
raise ValueError(f"Robot {i} y-position out of bounds: {y}, allowed: [{y_min}, {y_max}]")
102+
103+
# --- Check pairwise collisions ---
104+
n = len(formation)
105+
for i in range(n):
106+
x1, y1, _ = formation[i]
107+
for j in range(i + 1, n):
108+
x2, y2, _ = formation[j]
109+
dist = math.hypot(x1 - x2, y1 - y2)
110+
if dist < 2 * ROBOT_RADIUS:
111+
raise ValueError(
112+
f"Could not fit all robots in provided FieldBounds/FieldDimensions. Robots {i} and {j} overlap (distance={dist:.3f})"
113+
)
114+
115+
116+
def _validate_team_separation(left, right):
117+
"""
118+
Validate that the left and right teams are sufficiently separated to avoid collisions.
119+
"""
120+
max_left_x = -np.inf
121+
min_right_x = np.inf
122+
if left:
123+
max_left_x = max(x for x, _, _ in left)
124+
if right:
125+
min_right_x = min(x for x, _, _ in right)
126+
127+
gap = min_right_x - max_left_x
128+
required = 2 * ROBOT_RADIUS
129+
130+
if gap < required:
131+
raise ValueError(f"Teams not sufficiently separated: gap={gap:.3f}, required={required:.3f}")
132+
133+
134+
# TODO: can consider a fitting algorithm that can optimise robot placement so that the chance of running out of space is reduced.
135+
136+
137+
def get_formations(
138+
bounds: FieldBounds,
139+
n_left: int,
140+
n_right: int,
141+
formation_type: FormationType,
142+
) -> tuple[list[FormationEntry], list[FormationEntry]]:
143+
"""
144+
Returns the starting formations for both teams based on the provided field dimensions.
145+
The formations are defined as lists of FormationEntry objects, which contain the x and y coordinates
146+
147+
Args:
148+
bounds: FieldBounds object defining the top-left and bottom-right corners of the field.
149+
n_left: Number of robots on the left team.
150+
n_right: Number of robots on the right team.
151+
formation_type: The type of formation to generate (e.g., START_ONE).
152+
153+
Returns:
154+
tuple[list[FormationEntry], list[FormationEntry]]: A tuple containing two lists of FormationEntry objects.
155+
left and right team formations respectively.
156+
"""
157+
if n_left > MAX_ROBOTS or n_right > MAX_ROBOTS:
158+
raise ValueError(
159+
f"Number of robots per team cannot exceed {MAX_ROBOTS}. Got n_left={n_left}, n_right={n_right}."
160+
)
161+
if formation_type not in FORMATIONS:
162+
raise ValueError(f"Formation '{formation_type.value}' not found. Available: {list(FORMATIONS.keys())}")
163+
164+
base = FORMATIONS[formation_type]
165+
if n_left > len(base) or n_right > len(base):
166+
raise ValueError(
167+
f"Formation '{formation_type.value}' only defines {len(base)} positions, but got n_left={n_left}, n_right={n_right}."
168+
)
169+
170+
left = _mirror(_scale(base[:n_left], bounds), bounds)
171+
right = _scale(base[:n_right], bounds)
172+
173+
_validate_bounds_and_intra_team_collision(left, bounds)
174+
_validate_bounds_and_intra_team_collision(right, bounds)
175+
_validate_team_separation(left, right)
176+
177+
return left, right

‎utama_core/config/physical_constants.py‎

Lines changed: 1 addition & 0 deletions
Original file line numberDiff line numberDiff line change
@@ -1,2 +1,3 @@
11
ROBOT_RADIUS = 0.09
22
MAX_ROBOTS = 6
3+
BALL_RADIUS = 0.0215

0 commit comments

Comments
 (0)