diff --git a/agent/agent.py b/agent/agent.py index 6966aa4..d4052c6 100644 --- a/agent/agent.py +++ b/agent/agent.py @@ -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"): diff --git a/readme.md b/readme.md index d155647..d639abd 100644 --- a/readme.md +++ b/readme.md @@ -41,7 +41,14 @@ CLI parameter (a usage help is also available): - `--port ` to specify the agent port (default: 60000) - `-n ` Player number (1–11) (default: 1) - `-t ` Team name (default: 'Default') +- `-f ` 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') @@ -85,4 +94,4 @@ This project was developed and contributed by: - **Pedro Rabelo** - **Melissa Damasceno** -Contributions, bug reports, and feature requests are welcome via pull requests. \ No newline at end of file +Contributions, bug reports, and feature requests are welcome via pull requests. diff --git a/run_player.py b/run_player.py index fc14737..bfea17b 100755 --- a/run_player.py +++ b/run_player.py @@ -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() diff --git a/start_7v7.sh b/start_7v7.sh index d95827b..ba7e11d 100755 --- a/start_7v7.sh +++ b/start_7v7.sh @@ -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 diff --git a/utils/math_ops.py b/utils/math_ops.py index f142e34..1272f76 100644 --- a/utils/math_ops.py +++ b/utils/math_ops.py @@ -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) ''' diff --git a/world/commons/field.py b/world/commons/field.py index 26b3e33..43f4b9e 100644 --- a/world/commons/field.py +++ b/world/commons/field.py @@ -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), + } diff --git a/world/commons/field_landmarks.py b/world/commons/field_landmarks.py index f686ef6..46979b4 100644 --- a/world/commons/field_landmarks.py +++ b/world/commons/field_landmarks.py @@ -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) \ No newline at end of file + 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)