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

架构改进：头部追踪 + 球过时检测 + 球员独占 + 定位球处理

Browse Source 
头部追踪：
- 球可见时 he1/he2 追踪球方位，球不可见时左右扫描
- 在技能执行后、commit_motor_targets_pd 前覆盖头部电机目标

球位置过时检测：
- world.py 新增 ball_age property 和阈值常量
- 球消失 ≥3s 时外场球员原地旋转搜索，门将不再基于过期信息拦截

最近球员独占：
- 每个 agent 自行判断是否离球最近（视觉队友位置 + 0.5m 死区防抖）
- 最近球员执行 carry_ball，其他球员走到编号对应的支援站位

定位球基础处理：
- THEIR_KICK 时外场球员退到中圈半径外防守位，面朝球
- OUR_KICK 时最近球员上前踢球，其他人进攻支援位

决策层重构：
- update_current_behavior 外场分支拆分为 playmode_group 判断
- 新增 _outfield_decide / _am_i_nearest_to_ball / support_position / _defensive_set_piece
This commit is contained in:
jjh
2026-04-02 21:38:28 +08:00
parent a70557d2cc
commit 34f74c3005
2 changed files with 672 additions and 9 deletions
Download Patch File Download Diff File Expand all files Collapse all files

658
agent/agent.py
View File

@@ -1,4 +1,5 @@
import logging
import math
import numpy as np
from utils.math_ops import MathOps
@@ -16,6 +17,38 @@ class Agent:
based on the current state of the world and game conditions.
"""
GOALIE_MODE_RECOVER = "RECOVER"
GOALIE_MODE_HOME = "HOME"
GOALIE_MODE_INTERCEPT = "INTERCEPT"
GOALIE_MODE_CLEAR = "CLEAR"
GOALIE_MODE_PENALTY_READY = "PENALTY_READY"
GOALIE_MODE_CATCH_ATTEMPT = "CATCH_ATTEMPT"
GOALIE_MODE_CATCH_HOLD = "CATCH_HOLD"
GOALIE_MODE_CATCH_COOLDOWN = "CATCH_COOLDOWN"
GOALIE_ACTION_NONE = "NONE"
GOALIE_ACTION_SET = "SET"
GOALIE_ACTION_LOW_BLOCK_LEFT = "LOW_BLOCK_LEFT"
GOALIE_ACTION_LOW_BLOCK_RIGHT = "LOW_BLOCK_RIGHT"
GOALIE_INTENT_NONE = "NONE"
GOALIE_INTENT_SET = "SET"
GOALIE_INTENT_BLOCK_LEFT = "BLOCK_LEFT"
GOALIE_INTENT_BLOCK_RIGHT = "BLOCK_RIGHT"
GOALIE_INTENT_CATCH_CANDIDATE = "CATCH_CANDIDATE"
GOALIE_SET_BALL_SPEED_MIN = 0.2
GOALIE_SET_BALL_SPEED_MAX = 2.2
GOALIE_SET_BALL_HEIGHT_MAX = 0.22
GOALIE_SET_DISTANCE_MAX = 2.2
GOALIE_SET_LATERAL_MAX = 1.4
GOALIE_LOW_BLOCK_DISTANCE_MAX = 1.1
GOALIE_LOW_BLOCK_LATERAL_MAX = 0.7
GOALIE_SET_HOLD_TIME = 0.3
GOALIE_PENALTY_SET_DISTANCE_MAX = 0.9
GOALIE_CATCH_HOLD_TIME = 5.5
GOALIE_CATCH_COOLDOWN_TIME = 3.0
def __init__(self, agent):
"""
Creates a new DecisionMaker linked to the given agent.
@@ -27,6 +60,20 @@ class Agent:
self.agent: Base_Agent = agent
self.is_getting_up: bool = False
self.goalie_mode: str = self.GOALIE_MODE_HOME
self.goalie_mode_since: float = 0.0
self.goalie_action_mode: str = self.GOALIE_ACTION_NONE
self.goalie_action_since: float = 0.0
self.goalie_intent: str = self.GOALIE_INTENT_NONE
self.goalie_intent_since: float = 0.0
self.last_patrol_switch_time: float = 0.0
self.last_home_anchor: np.ndarray | None = None
self.last_home_anchor_change_time: float = 0.0
self.home_patrol_offset: float = 0.0
self.catch_ready_time: float = 0.0
self.catch_hold_started_at: float = -1.0
self.catch_cooldown_until: float = 0.0
self._head_scan_direction: float = 1.0
def update_current_behavior(self) -> None:
"""
@@ -43,7 +90,11 @@ class Agent:
PlayModeGroupEnum.ACTIVE_BEAM,
PlayModeGroupEnum.PASSIVE_BEAM,
):
beam_pose = self.agent.world.field.get_beam_pose(self.agent.world.number)
is_left_team = True if self.agent.world.is_left_team is None else self.agent.world.is_left_team
beam_pose = self.agent.world.field.get_beam_pose(
self.agent.world.number,
is_left_team=is_left_team,
)
self.agent.server.commit_beam(
pos2d=beam_pose[:2],
rotation=beam_pose[2],
@@ -51,17 +102,371 @@ class Agent:
if self.is_getting_up or self.agent.skills_manager.is_ready(skill_name="GetUp"):
self.is_getting_up = not self.agent.skills_manager.execute(skill_name="GetUp")
if self.agent.world.number == 1:
self._set_goalie_mode(self.GOALIE_MODE_RECOVER)
self._set_goalie_action_mode(self.GOALIE_ACTION_NONE)
self._set_goalie_intent(self.GOALIE_INTENT_NONE)
elif self.agent.world.playmode is PlayModeEnum.PLAY_ON:
self.carry_ball()
elif self.agent.world.playmode in (PlayModeEnum.BEFORE_KICK_OFF, PlayModeEnum.THEIR_GOAL, PlayModeEnum.OUR_GOAL):
self.agent.skills_manager.execute("Neutral")
if self.agent.world.number == 1:
self._set_goalie_mode(self.GOALIE_MODE_HOME)
self._set_goalie_action_mode(self.GOALIE_ACTION_NONE)
self._set_goalie_intent(self.GOALIE_INTENT_NONE)
elif self.agent.world.number == 1:
self.goalkeeper()
elif self.agent.world.playmode_group == PlayModeGroupEnum.THEIR_KICK:
self._defensive_set_piece()
elif self.agent.world.playmode_group == PlayModeGroupEnum.OUR_KICK:
if self._am_i_nearest_to_ball():
self.carry_ball()
else:
self.support_position()
else:
self.carry_ball()
self._outfield_decide()
self._apply_head_tracking()
self.agent.robot.commit_motor_targets_pd()
# ------------------------------------------------------------------
# Ball search
# ------------------------------------------------------------------
BALL_STALE_THRESHOLD = 3.0
def _search_for_ball(self) -> None:
world = self.agent.world
my_pos = world.global_position[:2]
yaw = self.agent.robot.global_orientation_euler[2] if hasattr(self.agent.robot, "global_orientation_euler") else 0.0
self.agent.skills_manager.execute(
"Walk",
target_2d=my_pos,
is_target_absolute=True,
orientation=yaw + 60.0,
is_orientation_absolute=True,
)
# ------------------------------------------------------------------
# Head tracking
# ------------------------------------------------------------------
HEAD_YAW_MIN = -90.0
HEAD_YAW_MAX = 90.0
HEAD_PITCH_MIN = -20.0
HEAD_PITCH_MAX = 70.0
HEAD_SCAN_RANGE = 70.0
HEAD_SCAN_SPEED = 80.0 # deg per second
HEAD_SCAN_PITCH = 15.0
HEAD_TRACK_KP = 8.0
HEAD_TRACK_KD = 0.4
HEAD_HEIGHT = 0.56 # approximate head height above ground
def _apply_head_tracking(self) -> None:
world = self.agent.world
robot = self.agent.robot
if not hasattr(robot, "set_motor_target_position"):
return
ball_age = float("inf")
if world.ball_last_update_time is not None and world.server_time is not None:
ball_age = world.server_time - world.ball_last_update_time
if ball_age < 0.6:
ball_rel = world.ball_pos[:2] - world.global_position[:2]
yaw_deg = robot.global_orientation_euler[2]
local_2d = MathOps.rotate_2d_vec(ball_rel, -yaw_deg, is_rad=False)
target_yaw = math.degrees(math.atan2(local_2d[1], local_2d[0]))
horiz_dist = max(np.linalg.norm(local_2d), 0.1)
dz = world.ball_pos[2] - self.HEAD_HEIGHT
target_pitch = -math.degrees(math.atan2(dz, horiz_dist))
target_yaw = np.clip(target_yaw, self.HEAD_YAW_MIN, self.HEAD_YAW_MAX)
target_pitch = np.clip(target_pitch, self.HEAD_PITCH_MIN, self.HEAD_PITCH_MAX)
else:
dt = 0.02
if world.server_time and world.last_server_time:
dt = max(world.server_time - world.last_server_time, 0.005)
current_yaw = robot.motor_positions.get("he1", 0.0)
step = self.HEAD_SCAN_SPEED * dt * self._head_scan_direction
target_yaw = current_yaw + step
if target_yaw >= self.HEAD_SCAN_RANGE:
target_yaw = self.HEAD_SCAN_RANGE
self._head_scan_direction = -1.0
elif target_yaw <= -self.HEAD_SCAN_RANGE:
target_yaw = -self.HEAD_SCAN_RANGE
self._head_scan_direction = 1.0
target_pitch = self.HEAD_SCAN_PITCH
robot.set_motor_target_position("he1", target_yaw, kp=self.HEAD_TRACK_KP, kd=self.HEAD_TRACK_KD)
robot.set_motor_target_position("he2", target_pitch, kp=self.HEAD_TRACK_KP, kd=self.HEAD_TRACK_KD)
def _ball_age_safe(self) -> float:
world = self.agent.world
if hasattr(world, "ball_age"):
return world.ball_age
if world.ball_last_update_time is not None and world.server_time is not None:
return world.server_time - world.ball_last_update_time
return float("inf")
# ------------------------------------------------------------------
# Set piece handling
# ------------------------------------------------------------------
DEFENSIVE_OFFSETS: dict[int, tuple[float, float]] = {
2: (-14.0, 8.0),
3: (-10.0, 3.0),
4: (-10.0, -3.0),
5: (-14.0, -8.0),
6: (-6.0, 0.0),
7: (-18.0, 0.0),
}
def _defensive_set_piece(self) -> None:
world = self.agent.world
ball_2d = world.ball_pos[:2]
offset = self.DEFENSIVE_OFFSETS.get(world.number, (-14.0, 0.0))
target = ball_2d + np.array(offset, dtype=float)
half_x = world.field.get_length() / 2.0
half_y = world.field.get_width() / 2.0
target[0] = np.clip(target[0], -half_x + 1.0, half_x - 1.0)
target[1] = np.clip(target[1], -half_y + 1.0, half_y + 1.0)
dist_to_ball = float(np.linalg.norm(target - ball_2d))
min_dist = world.field.get_center_circle_radius()
if dist_to_ball < min_dist:
away_dir = target - ball_2d
norm = np.linalg.norm(away_dir)
if norm > 1e-6:
away_dir = away_dir / norm
else:
away_dir = np.array([-1.0, 0.0])
target = ball_2d + away_dir * min_dist
orientation = MathOps.target_abs_angle(
world.global_position[:2], ball_2d
)
self.agent.skills_manager.execute(
"Walk",
target_2d=target,
is_target_absolute=True,
orientation=orientation,
)
# ------------------------------------------------------------------
# Outfield coordination
# ------------------------------------------------------------------
NEAREST_HYSTERESIS = 0.5 # meters dead-zone to prevent flickering
TEAMMATE_FRESH_TIME = 2.0 # seconds — ignore teammates not seen recently
SUPPORT_OFFSETS: dict[int, tuple[float, float]] = {
2: (-8.0, 6.0),
3: (-5.0, 2.0),
4: (-5.0, -2.0),
5: (-8.0, -6.0),
6: (-2.0, 0.0),
7: (-12.0, 0.0),
}
def _am_i_nearest_to_ball(self) -> bool:
world = self.agent.world
if not hasattr(world, "our_team_players"):
return True
ball_2d = world.ball_pos[:2]
my_dist = float(np.linalg.norm(world.global_position[:2] - ball_2d))
now = world.server_time if world.server_time is not None else 0.0
for idx, teammate in enumerate(world.our_team_players):
unum = idx + 1
if unum == world.number or unum == 1:
continue
if teammate.last_seen_time is None:
continue
if now - teammate.last_seen_time > self.TEAMMATE_FRESH_TIME:
continue
mate_dist = float(np.linalg.norm(teammate.position[:2] - ball_2d))
if mate_dist < my_dist - self.NEAREST_HYSTERESIS:
return False
return True
def support_position(self) -> None:
world = self.agent.world
ball_2d = world.ball_pos[:2]
offset = self.SUPPORT_OFFSETS.get(world.number, (-8.0, 0.0))
target = ball_2d + np.array(offset, dtype=float)
half_x = world.field.get_length() / 2.0
half_y = world.field.get_width() / 2.0
target[0] = np.clip(target[0], -half_x + 1.0, half_x - 1.0)
target[1] = np.clip(target[1], -half_y + 1.0, half_y + 1.0)
orientation = MathOps.target_abs_angle(
world.global_position[:2], ball_2d
)
self.agent.skills_manager.execute(
"Walk",
target_2d=target,
is_target_absolute=True,
orientation=orientation,
)
def _outfield_decide(self) -> None:
if self._ball_age_safe() >= self.BALL_STALE_THRESHOLD:
self._search_for_ball()
elif self._am_i_nearest_to_ball():
self.carry_ball()
else:
self.support_position()
def carry_ball(self):
their_goal_pos = np.array(self.agent.world.field.get_their_goal_position()[:2], dtype=float)
self._drive_ball_towards(their_goal_pos)
def goalkeeper(self) -> None:
world = self.agent.world
field = world.field
goal_x, _ = field.get_our_goal_position()
my_pos = world.global_position[:2]
now = self._current_time()
self._update_goalie_catch_state()
if world.playmode in (
PlayModeEnum.THEIR_PENALTY_KICK,
PlayModeEnum.THEIR_PENALTY_SHOOT,
):
self._set_goalie_mode(self.GOALIE_MODE_PENALTY_READY)
target_2d = self.compute_goalie_penalty_target()
self._set_goalie_intent(self.compute_goalie_intent())
if self.goalie_intent in (
self.GOALIE_INTENT_BLOCK_LEFT,
self.GOALIE_INTENT_BLOCK_RIGHT,
self.GOALIE_INTENT_CATCH_CANDIDATE,
):
self._execute_goalie_set()
return
if self.goalie_intent == self.GOALIE_INTENT_SET:
if np.linalg.norm(target_2d - my_pos) <= self.GOALIE_PENALTY_SET_DISTANCE_MAX:
self._execute_goalie_set()
else:
self._set_goalie_action_mode(self.GOALIE_ACTION_NONE)
orientation = self.compute_goalie_orientation(target_2d)
self.agent.skills_manager.execute(
"Walk",
target_2d=target_2d,
is_target_absolute=True,
orientation=orientation,
)
return
self._set_goalie_action_mode(self.GOALIE_ACTION_NONE)
orientation = self.compute_goalie_orientation(target_2d)
self.agent.skills_manager.execute(
"Walk",
target_2d=target_2d,
is_target_absolute=True,
orientation=orientation,
)
return
if world.playmode is PlayModeEnum.OUR_GOAL_KICK:
self._set_goalie_mode(self.GOALIE_MODE_CLEAR)
self._set_goalie_action_mode(self.GOALIE_ACTION_NONE)
self._set_goalie_intent(self.GOALIE_INTENT_NONE)
self.goalie_clear_ball()
return
self._set_goalie_intent(self.compute_goalie_intent())
if (
self.goalie_action_mode == self.GOALIE_ACTION_SET
and now - self.goalie_action_since < self.GOALIE_SET_HOLD_TIME
and self.goalie_intent in (
self.GOALIE_INTENT_SET,
self.GOALIE_INTENT_BLOCK_LEFT,
self.GOALIE_INTENT_BLOCK_RIGHT,
self.GOALIE_INTENT_CATCH_CANDIDATE,
)
and not self.should_goalie_intercept()
):
self._execute_goalie_set()
return
if self.should_goalie_intercept():
if self.goalie_intent in (
self.GOALIE_INTENT_BLOCK_LEFT,
self.GOALIE_INTENT_BLOCK_RIGHT,
):
logger.debug("Goalie block intent fallback: using INTERCEPT/CLEAR instead of low-block keyframe.")
ball_pos = world.ball_pos[:2]
ball_in_penalty_area = world.field.is_inside_penalty_area(ball_pos, side="our")
ball_distance = np.linalg.norm(ball_pos - my_pos)
if self.goalie_mode == self.GOALIE_MODE_CLEAR:
if ball_in_penalty_area or ball_distance <= 1.2:
self._set_goalie_action_mode(self.GOALIE_ACTION_NONE)
self.goalie_clear_ball()
return
if ball_distance <= 0.65:
self._set_goalie_mode(self.GOALIE_MODE_CLEAR)
self._set_goalie_action_mode(self.GOALIE_ACTION_NONE)
self.goalie_clear_ball()
return
self._set_goalie_mode(self.GOALIE_MODE_INTERCEPT)
self._set_goalie_action_mode(self.GOALIE_ACTION_NONE)
target_2d = self.compute_goalie_intercept_target()
orientation = self.compute_goalie_orientation(target_2d)
self.agent.skills_manager.execute(
"Walk",
target_2d=target_2d,
is_target_absolute=True,
orientation=orientation,
)
return
if self.goalie_intent in (
self.GOALIE_INTENT_SET,
self.GOALIE_INTENT_BLOCK_LEFT,
self.GOALIE_INTENT_BLOCK_RIGHT,
self.GOALIE_INTENT_CATCH_CANDIDATE,
):
target_2d = self.compute_goalie_home_target()
if np.linalg.norm(target_2d - my_pos) <= 0.45:
self._execute_goalie_set()
else:
self._set_goalie_action_mode(self.GOALIE_ACTION_NONE)
orientation = self.compute_goalie_orientation(target_2d)
self.agent.skills_manager.execute(
"Walk",
target_2d=target_2d,
is_target_absolute=True,
orientation=orientation,
)
return
self._set_goalie_mode(self.GOALIE_MODE_HOME)
self._set_goalie_action_mode(self.GOALIE_ACTION_NONE)
target_2d = self.compute_goalie_home_target()
orientation = self.compute_goalie_orientation(target_2d)
# When the keeper is already close to the goal line target, force a
# stable forward-facing posture. This avoids repeated twist-fall cycles
# caused by tiny sideways home-line corrections.
if abs(my_pos[0] - goal_x) <= 2.0 and np.linalg.norm(target_2d - my_pos) <= 0.9:
orientation = 0.0
self.agent.skills_manager.execute(
"Walk",
target_2d=target_2d,
is_target_absolute=True,
orientation=orientation,
)
def _drive_ball_towards(self, drive_target_2d: np.ndarray) -> None:
"""
Minimal catch-ball behavior.
@@ -70,11 +475,10 @@ class Agent:
2. reposition with a lateral offset if they are close but not yet behind it
3. push the ball forward once they are aligned
"""
their_goal_pos = self.agent.world.field.get_their_goal_position()[:2]
ball_pos = self.agent.world.ball_pos[:2]
my_pos = self.agent.world.global_position[:2]
ball_to_goal = their_goal_pos - ball_pos
ball_to_goal = drive_target_2d - ball_pos
bg_norm = np.linalg.norm(ball_to_goal)
if bg_norm <= 1e-6:
ball_to_goal_dir = np.array([1.0, 0.0])
@@ -137,3 +541,245 @@ class Agent:
is_target_absolute=True,
orientation=orientation,
)
def _current_time(self) -> float:
server_time = self.agent.world.server_time
return 0.0 if server_time is None else float(server_time)
def _set_goalie_mode(self, mode: str) -> None:
if self.goalie_mode == mode:
return
self.goalie_mode = mode
self.goalie_mode_since = self._current_time()
def _set_goalie_action_mode(self, mode: str) -> None:
if self.goalie_action_mode == mode:
return
self.goalie_action_mode = mode
self.goalie_action_since = self._current_time()
def _set_goalie_intent(self, intent: str) -> None:
if self.goalie_intent == intent:
return
self.goalie_intent = intent
self.goalie_intent_since = self._current_time()
def _orientation_to_ball(self, from_pos_2d: np.ndarray) -> float:
return MathOps.vector_angle(self.agent.world.ball_pos[:2] - from_pos_2d)
def compute_goalie_orientation(self, from_pos_2d: np.ndarray) -> float:
ball_pos = self.agent.world.ball_pos[:2]
ball_vec = ball_pos - from_pos_2d
if np.linalg.norm(ball_vec) < 1e-6:
return 0.0
orientation = MathOps.vector_angle(ball_vec)
return float(np.clip(orientation, -90.0, 90.0))
def compute_goalie_home_target(self) -> np.ndarray:
field = self.agent.world.field
ball_pos = self.agent.world.ball_pos[:2]
goal_x, _ = field.get_our_goal_position()
goal_half_width = field.get_goal_half_width()
raw_target = np.array(
[
goal_x + 0.8,
np.clip(
ball_pos[1] * 0.35,
-(goal_half_width - 0.35),
goal_half_width - 0.35,
),
],
dtype=float,
)
now = self._current_time()
if self.last_home_anchor is None or np.linalg.norm(raw_target - self.last_home_anchor) > 0.05:
self.last_home_anchor = raw_target
self.last_home_anchor_change_time = now
self.last_patrol_switch_time = now
self.home_patrol_offset = 0.0
elif now - self.last_home_anchor_change_time >= 8.0 and now - self.last_patrol_switch_time >= 2.0:
self.home_patrol_offset = -0.15 if self.home_patrol_offset > 0 else 0.15
self.last_patrol_switch_time = now
target = np.copy(self.last_home_anchor)
target[1] = np.clip(
target[1] + self.home_patrol_offset,
-(goal_half_width - 0.20),
goal_half_width - 0.20,
)
return target
def compute_goalie_penalty_target(self) -> np.ndarray:
field = self.agent.world.field
ball_pos = self.agent.world.ball_pos[:2]
goal_x, _ = field.get_our_goal_position()
return np.array(
[
goal_x + 0.15,
np.clip(ball_pos[1] * 0.15, -1.3, 1.3),
],
dtype=float,
)
def _update_goalie_catch_state(self) -> None:
now = self._current_time()
if (
self.goalie_mode == self.GOALIE_MODE_CATCH_HOLD
and self.catch_hold_started_at >= 0.0
and now - self.catch_hold_started_at >= self.GOALIE_CATCH_HOLD_TIME
):
self._set_goalie_mode(self.GOALIE_MODE_CATCH_COOLDOWN)
self.catch_cooldown_until = now + self.GOALIE_CATCH_COOLDOWN_TIME
self.catch_hold_started_at = -1.0
if (
self.goalie_mode == self.GOALIE_MODE_CATCH_COOLDOWN
and now >= self.catch_cooldown_until
):
self._set_goalie_mode(self.GOALIE_MODE_HOME)
def can_attempt_goalie_catch(self) -> bool:
return False
def compute_goalie_intent(self) -> str:
if self.can_attempt_goalie_catch():
return self.GOALIE_INTENT_CATCH_CANDIDATE
if self.should_goalie_block_intent():
return self.select_goalie_block_intent()
if self.should_goalie_set():
return self.GOALIE_INTENT_SET
return self.GOALIE_INTENT_NONE
def should_goalie_set(self) -> bool:
world = self.agent.world
ball_pos = world.ball_pos
my_pos = world.global_position[:2]
if not world.field.is_inside_penalty_area(ball_pos[:2], side="our"):
return False
if ball_pos[2] > self.GOALIE_SET_BALL_HEIGHT_MAX:
return False
if world.ball_speed < self.GOALIE_SET_BALL_SPEED_MIN or world.ball_speed > self.GOALIE_SET_BALL_SPEED_MAX:
return False
if np.linalg.norm(ball_pos[:2] - my_pos) > self.GOALIE_SET_DISTANCE_MAX:
return False
if abs(ball_pos[1] - my_pos[1]) > self.GOALIE_SET_LATERAL_MAX:
return False
return True
def should_goalie_block_intent(self) -> bool:
world = self.agent.world
ball_pos = world.ball_pos
my_pos = world.global_position[:2]
if not world.field.is_inside_penalty_area(ball_pos[:2], side="our"):
return False
if ball_pos[2] > self.GOALIE_SET_BALL_HEIGHT_MAX:
return False
if world.ball_speed < self.GOALIE_SET_BALL_SPEED_MIN or world.ball_speed > self.GOALIE_SET_BALL_SPEED_MAX:
return False
if np.linalg.norm(ball_pos[:2] - my_pos) > self.GOALIE_LOW_BLOCK_DISTANCE_MAX:
return False
if abs(ball_pos[1] - my_pos[1]) > self.GOALIE_LOW_BLOCK_LATERAL_MAX:
return False
return True
def select_goalie_block_side(self) -> str:
ball_y = self.agent.world.ball_pos[1]
my_y = self.agent.world.global_position[1]
return "left" if (ball_y - my_y) >= 0.0 else "right"
def select_goalie_block_intent(self) -> str:
return (
self.GOALIE_INTENT_BLOCK_LEFT
if self.select_goalie_block_side() == "left"
else self.GOALIE_INTENT_BLOCK_RIGHT
)
def _execute_goalie_set(self) -> None:
self._set_goalie_action_mode(self.GOALIE_ACTION_SET)
skill_name = "GoalieSet" if self.agent.skills_manager.has_skill("GoalieSet") else "Neutral"
self.agent.skills_manager.execute(skill_name)
def predict_ball_goal_intersection(self, horizon_s: float = 1.2) -> np.ndarray | None:
field = self.agent.world.field
ball_pos = self.agent.world.ball_pos[:2]
ball_velocity = self.agent.world.ball_velocity_2d
goal_x, _ = field.get_our_goal_position()
if ball_velocity[0] >= -1e-6:
return None
time_to_goal_line = (goal_x - ball_pos[0]) / ball_velocity[0]
if time_to_goal_line <= 0.0 or time_to_goal_line > horizon_s:
return None
y_at_goal_line = ball_pos[1] + ball_velocity[1] * time_to_goal_line
if abs(y_at_goal_line) > field.get_goal_half_width() + 0.2:
return None
return np.array([goal_x, y_at_goal_line], dtype=float)
def should_goalie_intercept(self) -> bool:
if self._ball_age_safe() >= self.BALL_STALE_THRESHOLD:
return False
field = self.agent.world.field
ball_pos = self.agent.world.ball_pos[:2]
my_pos = self.agent.world.global_position[:2]
ball_distance = np.linalg.norm(ball_pos - my_pos)
ball_in_penalty_area = field.is_inside_penalty_area(ball_pos, side="our")
if field.is_inside_goalie_area(ball_pos, side="our"):
return True
if ball_in_penalty_area and self.predict_ball_goal_intersection() is not None:
return True
if ball_in_penalty_area and ball_distance < 1.6:
return True
return False
def compute_goalie_intercept_target(self) -> np.ndarray:
field = self.agent.world.field
ball_pos = self.agent.world.ball_pos[:2]
our_goal_pos = np.array(field.get_our_goal_position()[:2], dtype=float)
goal_to_ball = ball_pos - our_goal_pos
goal_to_ball_norm = np.linalg.norm(goal_to_ball)
if goal_to_ball_norm <= 1e-6:
goal_to_ball_dir = np.array([1.0, 0.0], dtype=float)
else:
goal_to_ball_dir = goal_to_ball / goal_to_ball_norm
intercept_target = ball_pos - goal_to_ball_dir * 0.30
min_x, max_x, min_y, max_y = field.get_penalty_area_bounds(side="our")
intercept_target[0] = np.clip(intercept_target[0], min_x + 0.15, max_x - 0.15)
intercept_target[1] = np.clip(intercept_target[1], min_y + 0.15, max_y - 0.15)
return intercept_target
def goalie_clear_ball(self) -> None:
field = self.agent.world.field
ball_pos = self.agent.world.ball_pos[:2]
my_pos = self.agent.world.global_position[:2]
field_half_width = field.get_width() / 2.0
side_reference = ball_pos[1] if abs(ball_pos[1]) > 1e-3 else my_pos[1]
side_sign = 1.0 if side_reference >= 0 else -1.0
clear_y = side_sign * min(field_half_width - 1.0, abs(ball_pos[1]) + 4.0)
clear_x = min(0.0, ball_pos[0] + 3.0)
clear_target = np.array([clear_x, clear_y], dtype=float)
self._set_goalie_mode(self.GOALIE_MODE_CLEAR)
self._set_goalie_action_mode(self.GOALIE_ACTION_NONE)
self._drive_ball_towards(clear_target)