Apollo3D/Apollo3D_SE
3
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

对齐场地几何与运行模式基础设施

Browse Source
This commit is contained in:
jjh
2026-04-02 11:50:35 +08:00
parent 4b9d5afbb6
commit 3d1fc285d3
7 changed files with 243 additions and 91 deletions
Download Patch File Download Diff File Expand all files Collapse all files

38
agent/agent.py
View File

@@ -1,10 +1,7 @@
from dataclasses import Field
import logging
from typing import Mapping
import numpy as np
from utils.math_ops import MathOps
from world.commons.field import FIFAField, HLAdultField, Soccer7vs7Field
from world.commons.play_mode import PlayModeEnum, PlayModeGroupEnum
@@ -19,36 +16,6 @@ class Agent:
based on the current state of the world and game conditions.
"""
BEAM_POSES: Mapping[type[Field], Mapping[int, tuple[float, float, float]]] ={
FIFAField: {
1: (2.1, 0, 0),
2: (22.0, 12.0, 0),
3: (22.0, 4.0, 0),
4: (22.0, -4.0, 0),
5: (22.0, -12.0, 0),
6: (15.0, 0.0, 0),
7: (4.0, 16.0, 0),
8: (11.0, 6.0, 0),
9: (11.0, -6.0, 0),
10: (4.0, -16.0, 0),
11: (7.0, 0.0, 0),
},
HLAdultField: {
1: (7.0, 0.0, 0),
2: (2.0, -1.5, 0),
3: (2.0, 1.5, 0),
},
Soccer7vs7Field: {
1: (2.1, 0, 0),
2: (22.0, 12.0, 0),
3: (22.0, 4.0, 0),
4: (22.0, -4.0, 0),
5: (22.0, -12.0, 0),
6: (15.0, 0.0, 0),
7: (4.0, 16.0, 0)
}
}
def __init__(self, agent):
"""
Creates a new DecisionMaker linked to the given agent.
@@ -76,9 +43,10 @@ class Agent:
PlayModeGroupEnum.ACTIVE_BEAM,
PlayModeGroupEnum.PASSIVE_BEAM,
):
beam_pose = self.agent.world.field.get_beam_pose(self.agent.world.number)
self.agent.server.commit_beam(
pos2d=self.BEAM_POSES[type(self.agent.world.field)][self.agent.world.number][:2],
rotation=self.BEAM_POSES[type(self.agent.world.field)][self.agent.world.number][2],
pos2d=beam_pose[:2],
rotation=beam_pose[2],
)
if self.is_getting_up or self.agent.skills_manager.is_ready(skill_name="GetUp"):

9
readme.md
View File

@@ -41,7 +41,14 @@ CLI parameter (a usage help is also available):
- `--port <port>` to specify the agent port (default: 60000)
- `-n <number>` Player number (1–11) (default: 1)
- `-t <team_name>` Team name (default: 'Default')
- `-f <field>` Field profile (default: `fifa`)
### Field profiles
There are two supported ways to run Apollo3D:
- Official rules test: use the server with `--rules ssim`, and run agents with `-f fifa`. This matches the current `rcssservermj` default field for the SSIM rule book.
- Apollo custom 7v7: run agents with `-f sim3d_7vs7`. This profile is kept for Apollo's custom small-field setup and should not be treated as the official SSIM geometry baseline.
### Run a team
You can also use a shell script to start the entire team, optionally specifying host and port:
@@ -55,6 +62,8 @@ Using **Poetry**:
poetry run ./start_7v7.sh [host] [port]
```
`start_7v7.sh` now launches agents explicitly with `-f sim3d_7vs7`.
CLI parameter:
- `[host]` Server IP address (default: 'localhost')

2
run_player.py
View File

@@ -16,7 +16,7 @@ parser.add_argument("-t", "--team", type=str, default="Default", help="Team name
parser.add_argument("-n", "--number", type=int, default=1, help="Player number")
parser.add_argument("--host", type=str, default="127.0.0.1", help="Server host")
parser.add_argument("--port", type=int, default=60000, help="Server port")
parser.add_argument("-f", "--field", type=str, default='sim3d_7vs7', help="Field to be played")
parser.add_argument("-f", "--field", type=str, default='fifa', help="Field to be played")
args = parser.parse_args()

2
start_7v7.sh
View File

@@ -5,5 +5,5 @@ host=${1:-localhost}
port=${2:-60000}
for i in {1..7}; do
python3 run_player.py --host $host --port $port -n $i -t SE &
python3 run_player.py --host $host --port $port -n $i -t SE -f sim3d_7vs7 &
done

1
utils/math_ops.py
View File

@@ -51,6 +51,7 @@ class MathOps():
if size == 0: return vec
return vec / size
@staticmethod
def rel_to_global_3d(local_pos_3d: np.ndarray, global_pos_3d: np.ndarray,
global_orientation_quat: np.ndarray) -> np.ndarray:
''' Converts a local 3d position to a global 3d position given the global position and orientation (quaternion) '''

251
world/commons/field.py
View File

@@ -1,62 +1,229 @@
from abc import ABC, abstractmethod
from typing_extensions import override
from __future__ import annotations
from abc import ABC
from typing import Literal
import numpy as np
from world.commons.field_landmarks import FieldLandmarks
GoalSide = Literal["our", "their", "left", "right"]
Bounds2D = tuple[float, float, float, float]
class Field(ABC):
FIELD_DIM: tuple[float, float, float]
LINE_WIDTH: float
GOAL_DIM: tuple[float, float, float]
GOALIE_AREA_DIM: tuple[float, float]
PENALTY_AREA_DIM: tuple[float, float] | None
PENALTY_SPOT_DISTANCE: float
CENTER_CIRCLE_RADIUS: float
DEFAULT_BEAM_POSES: dict[int, tuple[float, float, float]]
def __init__(self, world):
from world.world import World # type hinting
self.world: World = world
self.field_landmarks: FieldLandmarks = FieldLandmarks(world=self.world)
self.field_landmarks: FieldLandmarks = FieldLandmarks(field=self)
def get_our_goal_position(self):
return (-self.get_length() / 2, 0)
def _resolve_side(self, side: GoalSide) -> Literal["left", "right"]:
if side in ("our", "left"):
return "left"
if side in ("their", "right"):
return "right"
raise ValueError(f"Unknown field side: {side}")
def get_their_goal_position(self):
return (self.get_length() / 2, 0)
def get_width(self) -> float:
return self.FIELD_DIM[1]
@abstractmethod
def get_width(self):
raise NotImplementedError()
def get_length(self) -> float:
return self.FIELD_DIM[0]
@abstractmethod
def get_length(self):
raise NotImplementedError()
def get_goal_dim(self) -> tuple[float, float, float]:
return self.GOAL_DIM
def get_goal_half_width(self) -> float:
return self.GOAL_DIM[1] / 2.0
def get_center_circle_radius(self) -> float:
return self.CENTER_CIRCLE_RADIUS
def get_goal_position(self, side: GoalSide = "our") -> tuple[float, float]:
resolved_side = self._resolve_side(side)
x = -self.get_length() / 2.0 if resolved_side == "left" else self.get_length() / 2.0
return (x, 0.0)
def get_our_goal_position(self) -> tuple[float, float]:
return self.get_goal_position("our")
def get_their_goal_position(self) -> tuple[float, float]:
return self.get_goal_position("their")
def _build_box_bounds(self, depth: float, width: float, side: GoalSide) -> Bounds2D:
resolved_side = self._resolve_side(side)
field_half_x = self.get_length() / 2.0
half_width = width / 2.0
if resolved_side == "left":
return (-field_half_x, -field_half_x + depth, -half_width, half_width)
return (field_half_x - depth, field_half_x, -half_width, half_width)
def get_goalie_area_bounds(self, side: GoalSide = "our") -> Bounds2D:
return self._build_box_bounds(
depth=self.GOALIE_AREA_DIM[0],
width=self.GOALIE_AREA_DIM[1],
side=side,
)
def get_penalty_area_bounds(self, side: GoalSide = "our") -> Bounds2D:
if self.PENALTY_AREA_DIM is None:
raise ValueError(f"{type(self).__name__} does not define a penalty area")
return self._build_box_bounds(
depth=self.PENALTY_AREA_DIM[0],
width=self.PENALTY_AREA_DIM[1],
side=side,
)
def get_penalty_spot(self, side: GoalSide = "our") -> tuple[float, float]:
resolved_side = self._resolve_side(side)
x = (self.get_length() / 2.0) - self.PENALTY_SPOT_DISTANCE
return (-x, 0.0) if resolved_side == "left" else (x, 0.0)
def _is_inside_bounds(self, pos2d: np.ndarray | tuple[float, float], bounds: Bounds2D) -> bool:
x, y = float(pos2d[0]), float(pos2d[1])
min_x, max_x, min_y, max_y = bounds
return min_x <= x <= max_x and min_y <= y <= max_y
def is_inside_goalie_area(
self, pos2d: np.ndarray | tuple[float, float], side: GoalSide = "our"
) -> bool:
return self._is_inside_bounds(pos2d, self.get_goalie_area_bounds(side))
def is_inside_penalty_area(
self, pos2d: np.ndarray | tuple[float, float], side: GoalSide = "our"
) -> bool:
return self._is_inside_bounds(pos2d, self.get_penalty_area_bounds(side))
def is_inside_field(self, pos2d: np.ndarray | tuple[float, float]) -> bool:
field_half_x = self.get_length() / 2.0
field_half_y = self.get_width() / 2.0
return self._is_inside_bounds(pos2d, (-field_half_x, field_half_x, -field_half_y, field_half_y))
def get_beam_pose(self, number: int) -> tuple[float, float, float]:
try:
return self.DEFAULT_BEAM_POSES[number]
except KeyError as exc:
raise KeyError(f"No beam pose configured for player {number} on {type(self).__name__}") from exc
def get_default_beam_poses(self) -> dict[int, tuple[float, float, float]]:
return dict(self.DEFAULT_BEAM_POSES)
def get_canonical_landmarks(self) -> dict[str, np.ndarray]:
field_half_x = self.get_length() / 2.0
field_half_y = self.get_width() / 2.0
goal_half_y = self.get_goal_half_width()
penalty_marker_x = field_half_x - self.PENALTY_SPOT_DISTANCE
goalie_area_x = field_half_x - self.GOALIE_AREA_DIM[0]
goalie_marker_y = self.GOALIE_AREA_DIM[1] / 2.0
landmarks = {
"l_luf": np.array([-field_half_x, field_half_y, 0.0]),
"l_llf": np.array([-field_half_x, -field_half_y, 0.0]),
"l_ruf": np.array([field_half_x, field_half_y, 0.0]),
"l_rlf": np.array([field_half_x, -field_half_y, 0.0]),
"t_cuf": np.array([0.0, field_half_y, 0.0]),
"t_clf": np.array([0.0, -field_half_y, 0.0]),
"x_cuc": np.array([0.0, self.CENTER_CIRCLE_RADIUS, 0.0]),
"x_clc": np.array([0.0, -self.CENTER_CIRCLE_RADIUS, 0.0]),
"p_lpm": np.array([-penalty_marker_x, 0.0, 0.0]),
"p_rpm": np.array([penalty_marker_x, 0.0, 0.0]),
"g_lup": np.array([-field_half_x, goal_half_y, self.GOAL_DIM[2]]),
"g_llp": np.array([-field_half_x, -goal_half_y, self.GOAL_DIM[2]]),
"g_rup": np.array([field_half_x, goal_half_y, self.GOAL_DIM[2]]),
"g_rlp": np.array([field_half_x, -goal_half_y, self.GOAL_DIM[2]]),
"l_luga": np.array([-goalie_area_x, goalie_marker_y, 0.0]),
"l_llga": np.array([-goalie_area_x, -goalie_marker_y, 0.0]),
"l_ruga": np.array([goalie_area_x, goalie_marker_y, 0.0]),
"l_rlga": np.array([goalie_area_x, -goalie_marker_y, 0.0]),
"t_luga": np.array([-field_half_x, goalie_marker_y, 0.0]),
"t_llga": np.array([-field_half_x, -goalie_marker_y, 0.0]),
"t_ruga": np.array([field_half_x, goalie_marker_y, 0.0]),
"t_rlga": np.array([field_half_x, -goalie_marker_y, 0.0]),
}
if self.PENALTY_AREA_DIM is not None:
penalty_area_x = field_half_x - self.PENALTY_AREA_DIM[0]
penalty_marker_y = self.PENALTY_AREA_DIM[1] / 2.0
landmarks.update(
{
"l_lupa": np.array([-penalty_area_x, penalty_marker_y, 0.0]),
"l_llpa": np.array([-penalty_area_x, -penalty_marker_y, 0.0]),
"l_rupa": np.array([penalty_area_x, penalty_marker_y, 0.0]),
"l_rlpa": np.array([penalty_area_x, -penalty_marker_y, 0.0]),
"t_lupa": np.array([-field_half_x, penalty_marker_y, 0.0]),
"t_llpa": np.array([-field_half_x, -penalty_marker_y, 0.0]),
"t_rupa": np.array([field_half_x, penalty_marker_y, 0.0]),
"t_rlpa": np.array([field_half_x, -penalty_marker_y, 0.0]),
}
)
return landmarks
class FIFAField(Field):
def __init__(self, world):
super().__init__(world)
@override
def get_width(self):
return 68
@override
def get_length(self):
return 105
FIELD_DIM = (105.0, 68.0, 40.0)
LINE_WIDTH = 0.1
GOAL_DIM = (1.6, 7.32, 2.44)
GOALIE_AREA_DIM = (5.5, 18.32)
PENALTY_AREA_DIM = (16.5, 40.32)
PENALTY_SPOT_DISTANCE = 11.0
CENTER_CIRCLE_RADIUS = 9.15
DEFAULT_BEAM_POSES = {
1: (2.1, 0.0, 0.0),
2: (22.0, 12.0, 0.0),
3: (22.0, 4.0, 0.0),
4: (22.0, -4.0, 0.0),
5: (22.0, -12.0, 0.0),
6: (15.0, 0.0, 0.0),
7: (4.0, 16.0, 0.0),
8: (11.0, 6.0, 0.0),
9: (11.0, -6.0, 0.0),
10: (4.0, -16.0, 0.0),
11: (7.0, 0.0, 0.0),
}
class HLAdultField(Field):
def __init__(self, world):
super().__init__(world)
FIELD_DIM = (14.0, 9.0, 40.0)
LINE_WIDTH = 0.05
GOAL_DIM = (0.6, 2.6, 1.8)
GOALIE_AREA_DIM = (1.0, 4.0)
PENALTY_AREA_DIM = (3.0, 6.0)
PENALTY_SPOT_DISTANCE = 2.1
CENTER_CIRCLE_RADIUS = 1.5
DEFAULT_BEAM_POSES = {
1: (5.5, 0.0, 0.0),
2: (2.0, -1.5, 0.0),
3: (2.0, 1.5, 0.0),
}
@override
def get_width(self):
return 9
@override
def get_length(self):
return 14
class Soccer7vs7Field(Field):
def __init__(self, world):
super().__init__(world)
@override
def get_width(self):
return 36
@override
def get_length(self):
return 55
FIELD_DIM = (55.0, 36.0, 40.0)
LINE_WIDTH = 0.1
GOAL_DIM = (0.84, 3.9, 2.44)
GOALIE_AREA_DIM = (2.9, 9.6)
PENALTY_AREA_DIM = (8.6, 21.3)
PENALTY_SPOT_DISTANCE = 5.8
CENTER_CIRCLE_RADIUS = 4.79
DEFAULT_BEAM_POSES = {
1: (2.0, 0.0, 0.0),
2: (12.0, 8.0, 0.0),
3: (13.5, 0.0, 0.0),
4: (12.0, -8.0, 0.0),
5: (7.0, 9.5, 0.0),
6: (4.5, 0.0, 0.0),
7: (7.0, -9.5, 0.0),
}

29
world/commons/field_landmarks.py
View File

@@ -1,14 +1,16 @@
from __future__ import annotations
import numpy as np
from utils.math_ops import MathOps
class FieldLandmarks:
def __init__(self, world):
from world.world import World # type hinting
self.world: World = world
self.landmarks: dict = {}
def __init__(self, field):
self.field = field
self.world = field.world
self.landmarks: dict[str, np.ndarray] = {}
self.canonical_landmarks: dict[str, np.ndarray] = field.get_canonical_landmarks()
def update_from_perception(self, landmark_id: str, landmark_pos: np.ndarray) -> None:
"""
@@ -21,14 +23,19 @@ class FieldLandmarks:
global_pos_3d = MathOps.rel_to_global_3d(
local_pos_3d=local_cart_3d,
global_pos_3d=world.global_position,
global_orientation_quat=world.agent.robot.global_orientation_quat
global_orientation_quat=world.agent.robot.global_orientation_quat,
)
self.landmarks[landmark_id] = global_pos_3d
def get_landmark_position(self, landmark_id: str) -> np.ndarray | None:
def get_landmark_position(
self, landmark_id: str, use_canonical: bool = False
) -> np.ndarray | None:
"""
Returns the calculated 2d global position for a given landmark ID.
Returns None if the landmark is not currently visible or processed.
Returns the current perceived or canonical global position for a landmark.
"""
return self.global_positions.get(landmark_id)
source = self.canonical_landmarks if use_canonical else self.landmarks
return source.get(landmark_id)
def get_canonical_landmark_position(self, landmark_id: str) -> np.ndarray | None:
return self.canonical_landmarks.get(landmark_id)