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

头部追踪：
- 球可见时 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

.gitignore

@@ -15,6 +15,7 @@ dist/
 *.npz
 *.xml
 *.json
+*.yaml
 *.iml
 *.pyc
 .TXT

agent/agent.py

@@ -1,10 +1,8 @@
-from dataclasses import Field
 import logging
-from typing import Mapping
+import math
 
 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,38 +17,37 @@ class Agent:
     based on the current state of the world and game conditions.
     """
 
-    BEAM_POSES: Mapping[type[Field], Mapping[int, tuple[float, float, float]]] ={
+    GOALIE_MODE_RECOVER = "RECOVER"
-        FIFAField: {
+    GOALIE_MODE_HOME = "HOME"
-            1: (2.1, 0, 0),
+    GOALIE_MODE_INTERCEPT = "INTERCEPT"
-            2: (22.0, 12.0, 0),
+    GOALIE_MODE_CLEAR = "CLEAR"
-            3: (22.0, 4.0, 0),
+    GOALIE_MODE_PENALTY_READY = "PENALTY_READY"
-            4: (22.0, -4.0, 0),
+    GOALIE_MODE_CATCH_ATTEMPT = "CATCH_ATTEMPT"
-            5: (22.0, -12.0, 0),
+    GOALIE_MODE_CATCH_HOLD = "CATCH_HOLD"
-            6: (15.0, 0.0, 0),
+    GOALIE_MODE_CATCH_COOLDOWN = "CATCH_COOLDOWN"
-            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)
-        }
-    }
 
-    TEAMMATE_INFO_MAX_AGE: float = 0.8
+    GOALIE_ACTION_NONE = "NONE"
-    SUPPORT_DISTANCE_FROM_BALL: float = 1.2
+    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):
         """
@@ -63,10 +60,20 @@ class Agent:
 
         self.agent: Base_Agent = agent
         self.is_getting_up: bool = False
-        self.is_kicking: bool = False
+        self.goalie_mode: str = self.GOALIE_MODE_HOME
-        self.current_shot_target_point: np.ndarray | None = None
+        self.goalie_mode_since: float = 0.0
-        self.target_point: np.ndarray = np.array([0.0, 0.0])
+        self.goalie_action_mode: str = self.GOALIE_ACTION_NONE
-        self.my_pos: np.ndarray = self.agent.world.global_position[:2]
+        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:
         """
@@ -83,50 +90,413 @@ class Agent:
             PlayModeGroupEnum.ACTIVE_BEAM,
             PlayModeGroupEnum.PASSIVE_BEAM,
         ):
+            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=self.BEAM_POSES[type(self.agent.world.field)][self.agent.world.number][:2],
+                pos2d=beam_pose[:2],
-                rotation=self.BEAM_POSES[type(self.agent.world.field)][self.agent.world.number][2],
+                rotation=beam_pose[2],
             )
 
         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:
-            active_player_number = self.get_active_player_number()
-            if active_player_number == self.agent.world.number:
-                # self.carry_ball()
-                self.target_point = self.my_pos + np.array([4.0, 0.0])
-                self.kick_ball()
-            else:
-                # self.execute_support_behavior()
-                self.agent.skills_manager.execute("Neutral")
         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.execute_support_behavior()
+            self._outfield_decide()
-            self.kick_ball()
 
+        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:
         """
-        Basic example of a behavior: moves the robot toward the goal while handling the ball.
+        Minimal catch-ball behavior.
+
+        All players share the same logic:
+        1. approach a point behind the ball
+        2. reposition with a lateral offset if they are close but not yet behind it
+        3. push the ball forward once they are aligned
         """
-        target_point = self.target_point
         ball_pos = self.agent.world.ball_pos[:2]
         my_pos = self.agent.world.global_position[:2]
 
-        if self.kick_ball(target_point=target_point):
+        ball_to_goal = drive_target_2d - ball_pos
-            return
+        bg_norm = np.linalg.norm(ball_to_goal)
+        if bg_norm <= 1e-6:
+            ball_to_goal_dir = np.array([1.0, 0.0])
+        else:
+            ball_to_goal_dir = ball_to_goal / bg_norm
 
-        ball_to_target = target_point - ball_pos
+        lateral_dir = np.array([-ball_to_goal_dir[1], ball_to_goal_dir[0]])
-        bt_norm = np.linalg.norm(ball_to_target)
-        if bt_norm == 0:
-            return
-        ball_to_target_dir = ball_to_target / bt_norm
 
-        dist_from_ball_to_start_carrying = 0.30
+        back_offset = 0.40
-        carry_ball_pos = ball_pos - ball_to_target_dir * dist_from_ball_to_start_carrying
+        side_offset = 0.35
+        push_distance = 0.80
+        approach_distance = 0.90
+        push_start_distance = 0.55
+        behind_margin = 0.08
+        angle_tolerance = np.deg2rad(20.0)
+
+        behind_point = ball_pos - ball_to_goal_dir * back_offset
+        push_target = ball_pos + ball_to_goal_dir * push_distance
 
         my_to_ball = ball_pos - my_pos
         my_to_ball_norm = np.linalg.norm(my_to_ball)
@@ -135,160 +505,281 @@ class Agent:
         else:
             my_to_ball_dir = my_to_ball / my_to_ball_norm
 
-        cosang = np.dot(my_to_ball_dir, ball_to_target_dir)
+        cosang = np.dot(my_to_ball_dir, ball_to_goal_dir)
         cosang = np.clip(cosang, -1.0, 1.0)
         angle_diff = np.arccos(cosang)
+        aligned = (my_to_ball_norm > 1e-6) and (angle_diff <= angle_tolerance)
 
-        ANGLE_TOL = np.deg2rad(7.5)
+        behind_ball = np.dot(my_pos - ball_pos, ball_to_goal_dir) < -behind_margin
-        aligned = (my_to_ball_norm > 1e-6) and (angle_diff <= ANGLE_TOL)
+        desired_orientation = MathOps.vector_angle(ball_to_goal)
 
-        behind_ball = np.dot(my_pos - ball_pos, ball_to_target_dir) < 0
+        lateral_sign = np.sign(np.cross(ball_to_goal_dir, my_to_ball_dir))
-        desired_orientation = MathOps.vector_angle(ball_to_target)
+        if lateral_sign == 0:
+            lateral_sign = 1.0 if (my_pos[1] - ball_pos[1]) >= 0 else -1.0
 
-        if not aligned or not behind_ball:
+        reposition_point = behind_point + lateral_dir * lateral_sign * side_offset
-            self.agent.skills_manager.execute(
+
-                "Walk",
+        if my_to_ball_norm > approach_distance:
-                target_2d=carry_ball_pos,
+            target_2d = behind_point
-                is_target_absolute=True,
+            orientation = None
-                orientation=None if np.linalg.norm(my_pos - carry_ball_pos) > 2 else desired_orientation
+        elif not behind_ball:
-            )
+            target_2d = reposition_point
+            orientation = None if np.linalg.norm(my_pos - reposition_point) > 0.8 else desired_orientation
+        elif not aligned and my_to_ball_norm > push_start_distance:
+            target_2d = behind_point
+            orientation = desired_orientation
         else:
-            self.agent.skills_manager.execute(
+            target_2d = push_target
-                "Walk",
+            orientation = desired_orientation
-                target_2d=target_point,
-                is_target_absolute=True,
-                orientation=desired_orientation
-            )
 
-    def kick_ball(self, target_point=None):
+        if np.linalg.norm(target_2d - my_pos) <= 1e-4:
-        if target_point is None:
+            target_2d = my_pos + ball_to_goal_dir * 0.30
-            target_point = self.target_point
-
-        target_point = np.asarray(target_point, dtype=float)[:2]
-        ball_pos = self.agent.world.ball_pos[:2]
-        my_pos = self.agent.world.global_position[:2]
-
-        ball_to_target = target_point - ball_pos
-        ball_to_target_dist = np.linalg.norm(ball_to_target)
-        if ball_to_target_dist <= 1e-6:
-            return False
-        if self.is_kicking:
-            if self.current_shot_target_point is None:
-                self.current_shot_target_point = target_point
-
-            active_target = self.current_shot_target_point
-            active_distance = np.linalg.norm(active_target - ball_pos)
-            self.is_kicking = not self.agent.skills_manager.execute(
-                "Kick", distance=active_distance
-            )
-            if not self.is_kicking:
-                self.current_shot_target_point = None
-            return True
-
-        my_to_ball = ball_pos - my_pos
-        my_to_ball_dist = np.linalg.norm(my_to_ball)
-
-        ball_to_target_dir = ball_to_target / ball_to_target_dist
-        behind_ball = np.dot(my_pos - ball_pos, ball_to_target_dir) < 0
-        me_to_ball_dir = ball_pos - my_pos
-        me_to_ball_dir_norm = np.linalg.norm(me_to_ball_dir)
-
-        desired_orientation = MathOps.vector_angle(me_to_ball_dir)
-        current_orientation = self.agent.robot.global_orientation_euler[2]
-        orientation_error = abs(
-            MathOps.normalize_deg(desired_orientation - current_orientation)
-        )
-
-        lateral_error_to_shot_line, _ = MathOps.distance_point_to_line(
-            p=my_pos,
-            a=ball_pos,
-            b=target_point,
-        )
-
-        kick_offset = MathOps.rotate_2d_vec(np.array([0.0, -0.1]), desired_orientation)
-
-        close_to_ball = my_to_ball_dist <= 0.24
-        good_orientation = orientation_error <= 8.0
-        good_lateral_offset = lateral_error_to_shot_line <= 0.16
-
-        can_shoot = (
-            close_to_ball
-            and behind_ball
-            and good_orientation
-            and good_lateral_offset
-        )
-
-        if not can_shoot:
-            prepare_offset = 0.1
-            prepare_pos = ball_pos - ball_to_target_dir * prepare_offset + kick_offset
-
-            self.agent.skills_manager.execute(
-                "Walk",
-                target_2d=prepare_pos,
-                is_target_absolute=True,
-                orientation=desired_orientation,
-            )
-            return True
-
-        self.current_shot_target_point = target_point
-        self.is_kicking = not self.agent.skills_manager.execute(
-            "Kick", distance=ball_to_target_dist
-        )
-        if not self.is_kicking:
-            self.current_shot_target_point = None
-        return True
-
-    def get_active_player_number(self) -> int:
-        world = self.agent.world
-        ball_pos = world.ball_pos[:2]
-        server_time = world.server_time
-
-        my_num = world.number
-        my_pos = world.global_position[:2]
-        best_player_number = my_num
-        best_distance = np.linalg.norm(my_pos - ball_pos)
-
-        for teammate_number, teammate in enumerate(world.our_team_players, start=1):
-            if teammate_number == my_num:
-                continue
-
-            if teammate.last_seen_time is None or server_time is None:
-                continue
-
-            info_age = server_time - teammate.last_seen_time
-            if info_age > self.TEAMMATE_INFO_MAX_AGE:
-                continue
-
-            teammate_pos = teammate.position[:2]
-            teammate_distance = np.linalg.norm(teammate_pos - ball_pos)
-
-            if teammate_distance + 1e-6 < best_distance:
-                best_distance = teammate_distance
-                best_player_number = teammate_number
-            elif abs(teammate_distance - best_distance) <= 1e-6:
-                best_player_number = min(best_player_number, teammate_number)
-
-        return best_player_number
-
-    def execute_support_behavior(self) -> None:
-        world = self.agent.world
-        my_pos = world.global_position[:2]
-        ball_pos = world.ball_pos[:2]
-        target_point = self.target_point
-
-        ball_to_target = target_point - ball_pos
-        bt_norm = np.linalg.norm(ball_to_target)
-        if bt_norm <= 1e-6:
-            ball_to_target_dir = np.array([1.0, 0.0])
-        else:
-            ball_to_target_dir = ball_to_target / bt_norm
-
-        support_pos = ball_pos - ball_to_target_dir * self.SUPPORT_DISTANCE_FROM_BALL
-        support_orientation = MathOps.vector_angle(ball_pos - my_pos)
 
         self.agent.skills_manager.execute(
             "Walk",
-            target_2d=support_pos,
+            target_2d=target_2d,
             is_target_absolute=True,
-            orientation=support_orientation,
+            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)

agent/base_agent.py

@@ -5,6 +5,7 @@ from world.robot import T1, Robot
 from behaviors.behavior_manager import BehaviorManager
 from world.world import World
 from communication.server import Server
+from communication.monitor_client import MonitorClient
 from communication.world_parser import WorldParser
 
 logger = logging.getLogger(__file__)
@@ -35,6 +36,7 @@ class Base_Agent:
         self.server: Server = Server(
             host=host, port=port, world_parser=self.world_parser
         )
+        self.monitor: MonitorClient = MonitorClient(host=host, port=port + 1)
         self.robot: Robot = T1(agent=self)
         self.skills_manager: BehaviorManager = BehaviorManager(agent=self)
         self.decision_maker: Agent = Agent(agent=self)
@@ -53,6 +55,7 @@ class Base_Agent:
              - Sends the next set of commands to the server.
         """
         self.server.connect()
+        self.monitor.connect()
 
         self.server.send_immediate(
             f"(init {self.robot.name} {self.world.team_name} {self.world.number})"
@@ -78,4 +81,5 @@ class Base_Agent:
         Logs a shutdown message and closes the server connection.
         """
         logger.info("Shutting down.")
-        self.server.shutdown()
+        self.monitor.close()
+        self.server.shutdown()

behaviors/behavior_manager.py

@@ -1,6 +1,8 @@
 from behaviors.custom.keyframe.get_up.get_up import GetUp
-from behaviors.custom.keyframe.kick.kick import Kick
 from behaviors.custom.keyframe.keyframe import KeyframeSkill
+from behaviors.custom.keyframe.poses.goalie_set.goalie_set import GoalieSet
+from behaviors.custom.keyframe.poses.low_block_left.low_block_left import LowBlockLeft
+from behaviors.custom.keyframe.poses.low_block_right.low_block_right import LowBlockRight
 from behaviors.custom.keyframe.poses.neutral.neutral import Neutral
 from behaviors.behavior import Behavior
 from behaviors.custom.reinforcement.walk.walk import Walk
@@ -23,7 +25,7 @@ class BehaviorManager:
         Each skill is indexed by its class name.
         """
 
-        classes: list[type[Behavior]] = [Walk, Neutral, GetUp, Kick]
+        classes: list[type[Behavior]] = [Walk, Neutral, GoalieSet, LowBlockLeft, LowBlockRight, GetUp]
 
         # instantiate each Skill and store in the skills dictionary
         self.skills = {cls.__name__: cls(agent=self.agent) for cls in classes}
@@ -34,6 +36,9 @@ class BehaviorManager:
             raise KeyError(f"No skill found with the name '{name}'")
         return self.skills[name]
 
+    def has_skill(self, name: str) -> bool:
+        return name in self.skills
+
     def execute(self, skill_name: str, *args, **kwargs) -> bool:
         """
         Executes one step of the specified skill.

behaviors/custom/keyframe/kick/kick.py

@@ -1,91 +0,0 @@
-import os
-from numbers import Real
-
-from behaviors.behavior import Behavior
-from behaviors.custom.keyframe.keyframe import KeyframeSkill
-
-
-class Kick(Behavior):
-    SHORT_MAX_DISTANCE: float = 1.8
-    MID_MAX_DISTANCE: float = 4.5
-    TEN_METER_MIN_DISTANCE: float = 8.0
-    DEFAULT_DISTANCE: float = 3.0
-
-    def __init__(self, agent):
-        super().__init__(agent)
-
-        base_dir = os.path.dirname(__file__)
-        self.short_kick = KeyframeSkill(
-            agent=agent,
-            file=os.path.join(base_dir, "kick_short.yaml"),
-        )
-        self.mid_kick = KeyframeSkill(
-            agent=agent,
-            file=os.path.join(base_dir, "kick_mid.yaml"),
-        )
-        self.long_kick = KeyframeSkill(
-            agent=agent,
-            file=os.path.join(base_dir, "kick_long.yaml"),
-        )
-        self.ten_meter_kick = KeyframeSkill(
-            agent=agent,
-            file=os.path.join(base_dir, "kick_10m.yaml"),
-        )
-
-        self.current_kick: KeyframeSkill | None = None
-        self.should_reset_kick: bool = True
-        self.should_execute_neutral: bool = True
-
-    def execute(self, reset: bool, *args, **kwargs) -> bool:
-        if reset:
-            kick_distance = self._extract_kick_distance(*args, **kwargs)
-            self.current_kick = self._choose_kick_from_distance(kick_distance)
-            self.should_reset_kick = True
-            self.should_execute_neutral = True
-
-        if self.current_kick is None:
-            self.current_kick = self.mid_kick
-
-        if self.should_execute_neutral:
-            neutral_finished = self.agent.skills_manager.execute(
-                "Neutral",
-            )
-            self.should_reset_kick = False
-            if not neutral_finished:
-                return False
-            self.should_execute_neutral = False
-            self.should_reset_kick = True
-
-        has_finished = self.current_kick.execute(reset=self.should_reset_kick)
-        self.should_reset_kick = False
-        return has_finished
-
-    def _extract_kick_distance(self, *args, **kwargs) -> float:
-        distance_candidates = (
-            kwargs.get("distance"),
-            kwargs.get("kick_distance"),
-            kwargs.get("target_distance"),
-            kwargs.get("ball_to_target_distance"),
-            args[0] if args else None,
-        )
-
-        for candidate in distance_candidates:
-            if isinstance(candidate, Real):
-                return float(max(0.0, candidate))
-
-        return self.DEFAULT_DISTANCE
-
-    def _choose_kick_from_distance(self, distance: float) -> KeyframeSkill:
-        if distance <= self.SHORT_MAX_DISTANCE:
-            return self.short_kick
-
-        if distance <= self.MID_MAX_DISTANCE:
-            return self.mid_kick
-
-        if distance >= self.TEN_METER_MIN_DISTANCE:
-            return self.ten_meter_kick
-
-        return self.long_kick
-
-    def is_ready(self, *args) -> bool:
-        return True

behaviors/custom/keyframe/kick/kick_10m.yaml

@@ -1,187 +0,0 @@
-symmetry: false
-kp: 175
-kd: 1.4
-keyframes:
-  - delta: 0.00
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 68.000
-      Left_Shoulder_Roll: -76.000
-      Left_Elbow_Pitch: -62.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 52.000
-      Right_Elbow_Pitch: -72.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -26.000
-      Left_Hip_Roll: 8.000
-      Left_Hip_Yaw: 0.000
-      Left_Knee_Pitch: 62.000
-      Left_Ankle_Pitch: 12.000
-      Left_Ankle_Roll: 3.000
-      Right_Hip_Pitch: 14.000
-      Right_Hip_Roll: -10.000
-      Right_Hip_Yaw: 0.000
-      Right_Knee_Pitch: -10.000
-      Right_Ankle_Pitch: 6.000
-      Right_Ankle_Roll: -5.000
-
-  - delta: 0.23
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 70.000
-      Left_Shoulder_Roll: -80.000
-      Left_Elbow_Pitch: -70.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 50.000
-      Right_Elbow_Pitch: -72.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -40.000
-      Left_Hip_Roll: 10.000
-      Left_Hip_Yaw: 2.500
-      Left_Knee_Pitch: 64.000
-      Left_Ankle_Pitch: 10.000
-      Left_Ankle_Roll: 4.500
-      Right_Hip_Pitch: 14.000
-      Right_Hip_Roll: -10.000
-      Right_Hip_Yaw: -3.000
-      Right_Knee_Pitch: -10.000
-      Right_Ankle_Pitch: 6.000
-      Right_Ankle_Roll: -6.000
-
-  - delta: 0.26
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 72.000
-      Left_Shoulder_Roll: -82.000
-      Left_Elbow_Pitch: -74.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 48.000
-      Right_Elbow_Pitch: -68.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -56.000
-      Left_Hip_Roll: 12.000
-      Left_Hip_Yaw: 5.000
-      Left_Knee_Pitch: 34.000
-      Left_Ankle_Pitch: 6.000
-      Left_Ankle_Roll: 5.000
-      Right_Hip_Pitch: 14.000
-      Right_Hip_Roll: -10.000
-      Right_Hip_Yaw: -6.000
-      Right_Knee_Pitch: -8.000
-      Right_Ankle_Pitch: 5.000
-      Right_Ankle_Roll: -7.000
-
-  - delta: 0.10
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 74.000
-      Left_Shoulder_Roll: -86.000
-      Left_Elbow_Pitch: -76.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 44.000
-      Right_Elbow_Pitch: -64.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -66.000
-      Left_Hip_Roll: 13.000
-      Left_Hip_Yaw: 5.500
-      Left_Knee_Pitch: -12.000
-      Left_Ankle_Pitch: 4.000
-      Left_Ankle_Roll: 5.000
-      Right_Hip_Pitch: 12.000
-      Right_Hip_Roll: -9.000
-      Right_Hip_Yaw: -2.500
-      Right_Knee_Pitch: -6.000
-      Right_Ankle_Pitch: 4.000
-      Right_Ankle_Roll: -6.000
-    kp: 210
-
-  - delta: 0.23
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 72.000
-      Left_Shoulder_Roll: -82.000
-      Left_Elbow_Pitch: -72.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 46.000
-      Right_Elbow_Pitch: -66.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -24.000
-      Left_Hip_Roll: 10.000
-      Left_Hip_Yaw: 3.000
-      Left_Knee_Pitch: 46.000
-      Left_Ankle_Pitch: 12.500
-      Left_Ankle_Roll: 4.500
-      Right_Hip_Pitch: 14.000
-      Right_Hip_Roll: -8.000
-      Right_Hip_Yaw: 0.000
-      Right_Knee_Pitch: -10.000
-      Right_Ankle_Pitch: 6.000
-      Right_Ankle_Roll: -4.000
-
-  - delta: 0.23
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 70.000
-      Left_Shoulder_Roll: -80.000
-      Left_Elbow_Pitch: -68.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 48.000
-      Right_Elbow_Pitch: -68.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -18.000
-      Left_Hip_Roll: 10.000
-      Left_Hip_Yaw: 1.000
-      Left_Knee_Pitch: 38.000
-      Left_Ankle_Pitch: 10.000
-      Left_Ankle_Roll: 3.000
-      Right_Hip_Pitch: 12.000
-      Right_Hip_Roll: -8.000
-      Right_Hip_Yaw: 0.000
-      Right_Knee_Pitch: -6.000
-      Right_Ankle_Pitch: 4.000
-      Right_Ankle_Roll: -4.000
-
-  - delta: 0.34
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 64.000
-      Left_Shoulder_Roll: -72.000
-      Left_Elbow_Pitch: -58.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 68.000
-      Right_Shoulder_Roll: 45.000
-      Right_Elbow_Pitch: -64.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: 0.000
-      Left_Hip_Roll: 0.000
-      Left_Hip_Yaw: 0.000
-      Left_Knee_Pitch: 0.000
-      Left_Ankle_Pitch: 0.000
-      Left_Ankle_Roll: 0.000
-      Right_Hip_Pitch: 0.000
-      Right_Hip_Roll: 0.000
-      Right_Hip_Yaw: 0.000
-      Right_Knee_Pitch: 0.000
-      Right_Ankle_Pitch: 0.000
-      Right_Ankle_Roll: 0.000
-    kp: 120

behaviors/custom/keyframe/kick/kick_long.yaml

@@ -1,213 +0,0 @@
-symmetry: false
-kp: 160
-kd: 1.3
-keyframes:
-  - delta: 0.00
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 68.000
-      Left_Shoulder_Roll: -76.000
-      Left_Elbow_Pitch: -62.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 52.000
-      Right_Elbow_Pitch: -72.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -18.000
-      Left_Hip_Roll: 8.000
-      Left_Hip_Yaw: 0.000
-      Left_Knee_Pitch: 38.000
-      Left_Ankle_Pitch: 12.000
-      Left_Ankle_Roll: 3.000
-      Right_Hip_Pitch: 24.000
-      Right_Hip_Roll: -10.000
-      Right_Hip_Yaw: 0.000
-      Right_Knee_Pitch: -38.000
-      Right_Ankle_Pitch: 10.000
-      Right_Ankle_Roll: -5.000
-
-  - delta: 0.20
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 70.000
-      Left_Shoulder_Roll: -80.000
-      Left_Elbow_Pitch: -70.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 50.000
-      Right_Elbow_Pitch: -72.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -30.000
-      Left_Hip_Roll: 10.000
-      Left_Hip_Yaw: 2.000
-      Left_Knee_Pitch: 24.000
-      Left_Ankle_Pitch: 10.000
-      Left_Ankle_Roll: 4.500
-      Right_Hip_Pitch: 26.000
-      Right_Hip_Roll: -10.000
-      Right_Hip_Yaw: -3.000
-      Right_Knee_Pitch: -40.000
-      Right_Ankle_Pitch: 10.000
-      Right_Ankle_Roll: -6.000
-
-  - delta: 0.21
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 72.000
-      Left_Shoulder_Roll: -82.000
-      Left_Elbow_Pitch: -74.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 48.000
-      Right_Elbow_Pitch: -68.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -48.000
-      Left_Hip_Roll: 12.000
-      Left_Hip_Yaw: 3.000
-      Left_Knee_Pitch: 4.000
-      Left_Ankle_Pitch: 4.000
-      Left_Ankle_Roll: 5.000
-      Right_Hip_Pitch: 24.000
-      Right_Hip_Roll: -10.000
-      Right_Hip_Yaw: -4.000
-      Right_Knee_Pitch: -36.000
-      Right_Ankle_Pitch: 8.000
-      Right_Ankle_Roll: -7.000
-
-  - delta: 0.12
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 74.000
-      Left_Shoulder_Roll: -86.000
-      Left_Elbow_Pitch: -76.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 44.000
-      Right_Elbow_Pitch: -64.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -56.000
-      Left_Hip_Roll: 13.000
-      Left_Hip_Yaw: 4.000
-      Left_Knee_Pitch: -4.000
-      Left_Ankle_Pitch: 2.000
-      Left_Ankle_Roll: 5.000
-      Right_Hip_Pitch: 22.000
-      Right_Hip_Roll: -9.000
-      Right_Hip_Yaw: -1.500
-      Right_Knee_Pitch: -28.000
-      Right_Ankle_Pitch: 6.000
-      Right_Ankle_Roll: -6.000
-
-  - delta: 0.12
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 76.000
-      Left_Shoulder_Roll: -88.000
-      Left_Elbow_Pitch: -78.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 42.000
-      Right_Elbow_Pitch: -62.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -60.000
-      Left_Hip_Roll: 14.000
-      Left_Hip_Yaw: 5.000
-      Left_Knee_Pitch: -8.000
-      Left_Ankle_Pitch: 0.000
-      Left_Ankle_Roll: 5.000
-      Right_Hip_Pitch: 20.000
-      Right_Hip_Roll: -8.000
-      Right_Hip_Yaw: -2.000
-      Right_Knee_Pitch: -24.000
-      Right_Ankle_Pitch: 4.000
-      Right_Ankle_Roll: -5.000
-    kp: 190
-
-  - delta: 0.21
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 72.000
-      Left_Shoulder_Roll: -82.000
-      Left_Elbow_Pitch: -72.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 46.000
-      Right_Elbow_Pitch: -66.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -22.000
-      Left_Hip_Roll: 10.000
-      Left_Hip_Yaw: 3.000
-      Left_Knee_Pitch: 16.000
-      Left_Ankle_Pitch: 11.500
-      Left_Ankle_Roll: 4.500
-      Right_Hip_Pitch: 24.000
-      Right_Hip_Roll: -8.000
-      Right_Hip_Yaw: 0.000
-      Right_Knee_Pitch: -34.000
-      Right_Ankle_Pitch: 8.000
-      Right_Ankle_Roll: -4.000
-
-  - delta: 0.21
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 70.000
-      Left_Shoulder_Roll: -80.000
-      Left_Elbow_Pitch: -68.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 48.000
-      Right_Elbow_Pitch: -68.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -18.000
-      Left_Hip_Roll: 10.000
-      Left_Hip_Yaw: 1.000
-      Left_Knee_Pitch: 12.000
-      Left_Ankle_Pitch: 10.000
-      Left_Ankle_Roll: 3.000
-      Right_Hip_Pitch: 24.000
-      Right_Hip_Roll: -8.000
-      Right_Hip_Yaw: 0.000
-      Right_Knee_Pitch: -34.000
-      Right_Ankle_Pitch: 8.000
-      Right_Ankle_Roll: -4.000
-
-  - delta: 0.31
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 64.000
-      Left_Shoulder_Roll: -72.000
-      Left_Elbow_Pitch: -58.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 68.000
-      Right_Shoulder_Roll: 45.000
-      Right_Elbow_Pitch: -64.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: 0.000
-      Left_Hip_Roll: 0.000
-      Left_Hip_Yaw: 0.000
-      Left_Knee_Pitch: 0.000
-      Left_Ankle_Pitch: 0.000
-      Left_Ankle_Roll: 0.000
-      Right_Hip_Pitch: 20.000
-      Right_Hip_Roll: -6.000
-      Right_Hip_Yaw: 0.000
-      Right_Knee_Pitch: -26.000
-      Right_Ankle_Pitch: 6.000
-      Right_Ankle_Roll: 0.000
-    kp: 120

behaviors/custom/keyframe/kick/kick_mid.yaml

@@ -1,213 +0,0 @@
-symmetry: false
-kp: 150
-kd: 1.2
-keyframes:
-  - delta: 0.00
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 68.000
-      Left_Shoulder_Roll: -76.000
-      Left_Elbow_Pitch: -62.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 52.000
-      Right_Elbow_Pitch: -72.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -16.000
-      Left_Hip_Roll: 8.000
-      Left_Hip_Yaw: 0.000
-      Left_Knee_Pitch: 36.000
-      Left_Ankle_Pitch: 12.000
-      Left_Ankle_Roll: 4.000
-      Right_Hip_Pitch: 24.000
-      Right_Hip_Roll: -10.000
-      Right_Hip_Yaw: 0.000
-      Right_Knee_Pitch: -36.000
-      Right_Ankle_Pitch: 9.000
-      Right_Ankle_Roll: -5.000
-
-  - delta: 0.17
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 70.000
-      Left_Shoulder_Roll: -80.000
-      Left_Elbow_Pitch: -70.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 50.000
-      Right_Elbow_Pitch: -72.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -28.000
-      Left_Hip_Roll: 10.000
-      Left_Hip_Yaw: 1.500
-      Left_Knee_Pitch: 24.000
-      Left_Ankle_Pitch: 10.000
-      Left_Ankle_Roll: 4.500
-      Right_Hip_Pitch: 26.000
-      Right_Hip_Roll: -10.000
-      Right_Hip_Yaw: -2.000
-      Right_Knee_Pitch: -38.000
-      Right_Ankle_Pitch: 10.000
-      Right_Ankle_Roll: -6.000
-
-  - delta: 0.18
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 72.000
-      Left_Shoulder_Roll: -82.000
-      Left_Elbow_Pitch: -74.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 48.000
-      Right_Elbow_Pitch: -68.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -44.000
-      Left_Hip_Roll: 12.000
-      Left_Hip_Yaw: 3.000
-      Left_Knee_Pitch: 6.000
-      Left_Ankle_Pitch: 5.000
-      Left_Ankle_Roll: 5.000
-      Right_Hip_Pitch: 24.000
-      Right_Hip_Roll: -10.000
-      Right_Hip_Yaw: -4.000
-      Right_Knee_Pitch: -36.000
-      Right_Ankle_Pitch: 8.000
-      Right_Ankle_Roll: -7.000
-
-  - delta: 0.12
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 74.000
-      Left_Shoulder_Roll: -84.000
-      Left_Elbow_Pitch: -76.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 46.000
-      Right_Elbow_Pitch: -66.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -52.000
-      Left_Hip_Roll: 13.000
-      Left_Hip_Yaw: 3.500
-      Left_Knee_Pitch: -2.000
-      Left_Ankle_Pitch: 2.000
-      Left_Ankle_Roll: 5.000
-      Right_Hip_Pitch: 24.000
-      Right_Hip_Roll: -9.000
-      Right_Hip_Yaw: -1.000
-      Right_Knee_Pitch: -30.000
-      Right_Ankle_Pitch: 6.000
-      Right_Ankle_Roll: -5.000
-
-  - delta: 0.12
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 76.000
-      Left_Shoulder_Roll: -86.000
-      Left_Elbow_Pitch: -78.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 44.000
-      Right_Elbow_Pitch: -64.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -58.000
-      Left_Hip_Roll: 14.000
-      Left_Hip_Yaw: 4.000
-      Left_Knee_Pitch: -6.000
-      Left_Ankle_Pitch: 0.000
-      Left_Ankle_Roll: 5.000
-      Right_Hip_Pitch: 22.000
-      Right_Hip_Roll: -9.000
-      Right_Hip_Yaw: -1.000
-      Right_Knee_Pitch: -26.000
-      Right_Ankle_Pitch: 4.000
-      Right_Ankle_Roll: -6.000
-    kp: 180
-
-  - delta: 0.18
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 72.000
-      Left_Shoulder_Roll: -82.000
-      Left_Elbow_Pitch: -72.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 46.000
-      Right_Elbow_Pitch: -66.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -20.000
-      Left_Hip_Roll: 10.500
-      Left_Hip_Yaw: 2.000
-      Left_Knee_Pitch: 20.000
-      Left_Ankle_Pitch: 11.000
-      Left_Ankle_Roll: 4.500
-      Right_Hip_Pitch: 24.000
-      Right_Hip_Roll: -8.000
-      Right_Hip_Yaw: 0.000
-      Right_Knee_Pitch: -34.000
-      Right_Ankle_Pitch: 8.000
-      Right_Ankle_Roll: -4.000
-
-  - delta: 0.18
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 70.000
-      Left_Shoulder_Roll: -80.000
-      Left_Elbow_Pitch: -68.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 72.000
-      Right_Shoulder_Roll: 48.000
-      Right_Elbow_Pitch: -68.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -16.000
-      Left_Hip_Roll: 10.000
-      Left_Hip_Yaw: 1.000
-      Left_Knee_Pitch: 14.000
-      Left_Ankle_Pitch: 10.000
-      Left_Ankle_Roll: 3.000
-      Right_Hip_Pitch: 24.000
-      Right_Hip_Roll: -8.000
-      Right_Hip_Yaw: 0.000
-      Right_Knee_Pitch: -34.000
-      Right_Ankle_Pitch: 8.000
-      Right_Ankle_Roll: -4.000
-
-  - delta: 0.26
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 64.000
-      Left_Shoulder_Roll: -72.000
-      Left_Elbow_Pitch: -58.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: 68.000
-      Right_Shoulder_Roll: 45.000
-      Right_Elbow_Pitch: -64.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: 0.000
-      Left_Hip_Roll: 0.000
-      Left_Hip_Yaw: 0.000
-      Left_Knee_Pitch: 0.000
-      Left_Ankle_Pitch: 0.000
-      Left_Ankle_Roll: 0.000
-      Right_Hip_Pitch: 20.000
-      Right_Hip_Roll: -6.000
-      Right_Hip_Yaw: 0.000
-      Right_Knee_Pitch: -26.000
-      Right_Ankle_Pitch: 6.000
-      Right_Ankle_Roll: 0.000
-    kp: 120

behaviors/custom/keyframe/kick/kick_short.yaml

@@ -1,213 +0,0 @@
-symmetry: false
-kp: 140
-kd: 1.2
-keyframes:
-  - delta: 0.00
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 68.000
-      Left_Shoulder_Roll: -76.000
-      Left_Elbow_Pitch: -62.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: -72.000
-      Right_Shoulder_Roll: 52.000
-      Right_Elbow_Pitch: -70.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -12.000
-      Left_Hip_Roll: 8.000
-      Left_Hip_Yaw: 0.000
-      Left_Knee_Pitch: 35.000
-      Left_Ankle_Pitch: -18.000
-      Left_Ankle_Roll: 3.000
-      Right_Hip_Pitch: 14.000
-      Right_Hip_Roll: -9.000
-      Right_Hip_Yaw: 0.000
-      Right_Knee_Pitch: -10.000
-      Right_Ankle_Pitch: 6.000
-      Right_Ankle_Roll: -5.000
-
-  - delta: 0.16
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 70.000
-      Left_Shoulder_Roll: -80.000
-      Left_Elbow_Pitch: -70.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: -72.000
-      Right_Shoulder_Roll: 50.000
-      Right_Elbow_Pitch: -70.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -25.000
-      Left_Hip_Roll: 10.000
-      Left_Hip_Yaw: 1.000
-      Left_Knee_Pitch: 45.000
-      Left_Ankle_Pitch: -20.000
-      Left_Ankle_Roll: 4.500
-      Right_Hip_Pitch: 14.000
-      Right_Hip_Roll: -9.000
-      Right_Hip_Yaw: -1.000
-      Right_Knee_Pitch: -10.000
-      Right_Ankle_Pitch: 6.000
-      Right_Ankle_Roll: -5.500
-
-  - delta: 0.17
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 72.000
-      Left_Shoulder_Roll: -82.000
-      Left_Elbow_Pitch: -74.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: -72.000
-      Right_Shoulder_Roll: 48.000
-      Right_Elbow_Pitch: -68.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -40.000
-      Left_Hip_Roll: 12.000
-      Left_Hip_Yaw: 2.000
-      Left_Knee_Pitch: 18.000
-      Left_Ankle_Pitch: -24.000
-      Left_Ankle_Roll: 5.000
-      Right_Hip_Pitch: 14.000
-      Right_Hip_Roll: -8.000
-      Right_Hip_Yaw: -2.000
-      Right_Knee_Pitch: -8.000
-      Right_Ankle_Pitch: 5.000
-      Right_Ankle_Roll: -7.000
-
-  - delta: 0.16
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 73.500
-      Left_Shoulder_Roll: -84.000
-      Left_Elbow_Pitch: -76.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: -72.000
-      Right_Shoulder_Roll: 46.000
-      Right_Elbow_Pitch: -66.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -48.000
-      Left_Hip_Roll: 13.000
-      Left_Hip_Yaw: 3.000
-      Left_Knee_Pitch: -8.000
-      Left_Ankle_Pitch: -18.000
-      Left_Ankle_Roll: 5.000
-      Right_Hip_Pitch: 12.000
-      Right_Hip_Roll: -8.000
-      Right_Hip_Yaw: -1.000
-      Right_Knee_Pitch: -6.000
-      Right_Ankle_Pitch: 4.000
-      Right_Ankle_Roll: -5.000
-
-  - delta: 0.16
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 75.000
-      Left_Shoulder_Roll: -86.000
-      Left_Elbow_Pitch: -72.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: -70.000
-      Right_Shoulder_Roll: 44.000
-      Right_Elbow_Pitch: -64.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -50.000
-      Left_Hip_Roll: 13.000
-      Left_Hip_Yaw: 3.000
-      Left_Knee_Pitch: 5.000
-      Left_Ankle_Pitch: -22.000
-      Left_Ankle_Roll: 5.000
-      Right_Hip_Pitch: 12.000
-      Right_Hip_Roll: -8.000
-      Right_Hip_Yaw: 0.000
-      Right_Knee_Pitch: -6.000
-      Right_Ankle_Pitch: 4.000
-      Right_Ankle_Roll: -5.000
-    kp: 170
-
-  - delta: 0.18
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 73.500
-      Left_Shoulder_Roll: -82.000
-      Left_Elbow_Pitch: -69.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: -72.000
-      Right_Shoulder_Roll: 48.000
-      Right_Elbow_Pitch: -68.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -8.000
-      Left_Hip_Roll: 9.000
-      Left_Hip_Yaw: 1.000
-      Left_Knee_Pitch: 28.000
-      Left_Ankle_Pitch: -22.000
-      Left_Ankle_Roll: 3.000
-      Right_Hip_Pitch: 8.000
-      Right_Hip_Roll: -5.000
-      Right_Hip_Yaw: 0.000
-      Right_Knee_Pitch: -8.000
-      Right_Ankle_Pitch: 5.000
-      Right_Ankle_Roll: -3.000
-
-  - delta: 0.18
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 72.000
-      Left_Shoulder_Roll: -82.000
-      Left_Elbow_Pitch: -68.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: -68.000
-      Right_Shoulder_Roll: 48.000
-      Right_Elbow_Pitch: -62.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: -2.000
-      Left_Hip_Roll: 9.000
-      Left_Hip_Yaw: 1.000
-      Left_Knee_Pitch: 28.000
-      Left_Ankle_Pitch: -18.000
-      Left_Ankle_Roll: 3.000
-      Right_Hip_Pitch: 8.000
-      Right_Hip_Roll: -5.000
-      Right_Hip_Yaw: 0.000
-      Right_Knee_Pitch: -6.000
-      Right_Ankle_Pitch: 4.000
-      Right_Ankle_Roll: -3.000
-
-  - delta: 0.24
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Left_Shoulder_Pitch: 65.000
-      Left_Shoulder_Roll: -75.000
-      Left_Elbow_Pitch: -60.000
-      Left_Elbow_Yaw: 0.000
-      Right_Shoulder_Pitch: -65.000
-      Right_Shoulder_Roll: 45.000
-      Right_Elbow_Pitch: -60.000
-      Right_Elbow_Yaw: 0.000
-      Waist: 0.000
-      Left_Hip_Pitch: 0.000
-      Left_Hip_Roll: 0.000
-      Left_Hip_Yaw: 0.000
-      Left_Knee_Pitch: 0.000
-      Left_Ankle_Pitch: 0.000
-      Left_Ankle_Roll: 0.000
-      Right_Hip_Pitch: 0.000
-      Right_Hip_Roll: 0.000
-      Right_Hip_Yaw: 0.000
-      Right_Knee_Pitch: 0.000
-      Right_Ankle_Pitch: 0.000
-      Right_Ankle_Roll: 0.000
-    kp: 120

behaviors/custom/keyframe/poses/goalie_set/goalie_set.py

@@ -0,0 +1,8 @@
+import os
+
+from behaviors.custom.keyframe.keyframe import KeyframeSkill
+
+
+class GoalieSet(KeyframeSkill):
+    def __init__(self, agent):
+        super().__init__(agent, os.path.join(os.path.dirname(__file__), "goalie_set.yaml"))

behaviors/custom/keyframe/poses/low_block_left/low_block_left.py

@@ -0,0 +1,8 @@
+import os
+
+from behaviors.custom.keyframe.keyframe import KeyframeSkill
+
+
+class LowBlockLeft(KeyframeSkill):
+    def __init__(self, agent):
+        super().__init__(agent, os.path.join(os.path.dirname(__file__), "low_block_left.yaml"))

behaviors/custom/keyframe/poses/low_block_right/low_block_right.py

@@ -0,0 +1,8 @@
+import os
+
+from behaviors.custom.keyframe.keyframe import KeyframeSkill
+
+
+class LowBlockRight(KeyframeSkill):
+    def __init__(self, agent):
+        super().__init__(agent, os.path.join(os.path.dirname(__file__), "low_block_right.yaml"))

behaviors/custom/reinforcement/goalie_block/goalie_block.py

@@ -0,0 +1,32 @@
+import logging
+
+from behaviors.behavior import Behavior
+
+
+logger = logging.getLogger(__name__)
+
+
+class GoalieBlock(Behavior):
+    """
+    Placeholder interface for a future RL-based goalkeeper blocking skill.
+
+    The production code does not register or call this skill yet.
+    It exists to lock down the expected execution shape for later training work.
+    """
+
+    def __init__(self, agent):
+        super().__init__(agent)
+        self.last_side: str | None = None
+
+    def execute(self, reset: bool, side: str, *args, **kwargs) -> bool:
+        if reset:
+            self.last_side = side
+
+        logger.warning(
+            "GoalieBlock.execute(side=%s) was called before an RL model was integrated.",
+            side,
+        )
+        return True
+
+    def is_ready(self, *args) -> bool:
+        return False

behaviors/custom/reinforcement/walk/walk.py

@@ -143,5 +143,7 @@ class Walk(Behavior):
                 robot.ROBOT_MOTORS[idx], target*180/math.pi, kp=25, kd=0.6
             )
 
+        return False
+
     def is_ready(self, *args, **kwargs) -> bool:
         return True

communication/monitor_client.py

@@ -0,0 +1,70 @@
+import logging
+import socket
+
+
+logger = logging.getLogger(__name__)
+
+
+class MonitorClient:
+    """
+    TCP client for the RCSSServerMJ monitor port.
+
+    Sends monitor commands via the length-prefixed S-expression protocol.
+    """
+
+    def __init__(self, host: str = "localhost", port: int = 60001):
+        self._host = host
+        self._port = port
+        self._socket: socket.socket | None = None
+
+    def connect(self) -> None:
+        self._socket = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+        self._socket.setsockopt(socket.IPPROTO_TCP, socket.TCP_NODELAY, 1)
+        self._socket.connect((self._host, self._port))
+        logger.info("Monitor connection established to %s:%d.", self._host, self._port)
+
+    def close(self) -> None:
+        if self._socket is not None:
+            self._socket.close()
+            self._socket = None
+
+    def send(self, msg: str) -> None:
+        data = msg.encode()
+        self._socket.send(len(data).to_bytes(4, byteorder="big") + data)
+
+    def place_ball(
+        self,
+        pos: tuple[float, float, float],
+        vel: tuple[float, float, float] | None = None,
+    ) -> None:
+        msg = f"(ball (pos {pos[0]} {pos[1]} {pos[2]})"
+        if vel is not None:
+            msg += f" (vel {vel[0]} {vel[1]} {vel[2]})"
+        msg += ")"
+        self.send(msg)
+
+    def drop_ball(self) -> None:
+        self.send("(dropBall)")
+
+    def kick_off(self, side: str = "Left") -> None:
+        self.send(f"(kickOff {side})")
+
+    def set_play_mode(self, mode: str) -> None:
+        self.send(f"(playMode {mode})")
+
+    def place_player(
+        self,
+        unum: int,
+        team_name: str,
+        pos: tuple[float, float, float],
+        yaw_deg: float | None = None,
+    ) -> None:
+        if yaw_deg is None:
+            command = f"(agent (unum {unum}) (team {team_name}) (pos {pos[0]} {pos[1]} {pos[2]}))"
+        else:
+            command = (
+                f"(agent (unum {unum}) (team {team_name}) "
+                f"(move {pos[0]} {pos[1]} {pos[2]} {yaw_deg}))"
+            )
+
+        self.send(command)

communication/server.py

@@ -1,5 +1,6 @@
 import logging
 import socket
+import time
 from select import select
 from communication.world_parser import WorldParser
 
@@ -27,12 +28,16 @@ class Server:
                 logger.error(
                     "Connection refused. Make sure the server is running and listening on {self.__host}:{self.__port}."
                 )
+                time.sleep(1.0)
 
         logger.info(f"Server connection established to {self.__host}:{self.__port}.")
 
     def shutdown(self) -> None:
+        try:
+            self.__socket.shutdown(socket.SHUT_RDWR)
+        except OSError:
+            pass
         self.__socket.close()
-        self.__socket.shutdown(socket.SHUT_RDWR)
 
     def send_immediate(self, msg: str) -> None:
         """
@@ -107,3 +112,6 @@ class Server:
     def commit_beam(self, pos2d: list, rotation: float) -> None:
         assert len(pos2d) == 2
         self.commit(f"(beam {pos2d[0]} {pos2d[1]} {rotation})")
+
+    def commit_catch(self, angle_deg: float) -> None:
+        logger.debug("commit_catch(%s) is a no-op on the current server backend.", angle_deg)

communication/world_parser.py

@@ -127,7 +127,10 @@ class WorldParser:
             if not world.is_left_team:
                 world._global_cheat_position[:2] = -world._global_cheat_position[:2]
 
-                global_rotation = R.from_quat(robot.global_orientation_quat)
+                # Rotate the current raw server pose into Apollo's unified
+                # left-team view. Using the previous frame orientation here
+                # makes right-side players drift and face the wrong direction.
+                global_rotation = R.from_quat(robot._global_cheat_orientation)
                 yaw180 = R.from_euler('z', 180, degrees=True)
                 fixed_rotation = yaw180 * global_rotation
                 robot._global_cheat_orientation = fixed_rotation.as_quat()
@@ -138,7 +141,7 @@ class WorldParser:
                 [MathOps.normalize_deg(axis_angle) for axis_angle in euler_angles_deg])
             robot.global_orientation_quat = robot._global_cheat_orientation
             world.global_position = world._global_cheat_position
-        except:
+        except Exception:
             logger.exception(f'Failed to rotate orientation and position considering team side')
 
         robot.gyroscope = np.array(perception_dict["GYR"]["rt"])
@@ -157,12 +160,33 @@ class WorldParser:
 
                     polar_coords = np.array(seen_object['pol'])
                     local_cartesian_3d = MathOps.deg_sph2cart(polar_coords)
+                    previous_ball_pos = np.copy(world.ball_pos)
+                    previous_ball_time = world.ball_last_update_time
 
                     world.ball_pos = MathOps.rel_to_global_3d(
                         local_pos_3d=local_cartesian_3d,
                         global_pos_3d=world.global_position,
                         global_orientation_quat=robot.global_orientation_quat
                     )
+                    world.ball_last_pos = previous_ball_pos
+                    world.ball_last_update_time = world.server_time
+
+                    if previous_ball_time is not None:
+                        dt = world.server_time - previous_ball_time
+                        if dt > 1e-6:
+                            raw_vel = (
+                                world.ball_pos[:2] - previous_ball_pos[:2]
+                            ) / dt
+                            alpha = 0.4
+                            world.ball_velocity_2d = (
+                                alpha * raw_vel
+                                + (1.0 - alpha) * world.ball_velocity_2d
+                            )
+                            world.ball_speed = float(np.linalg.norm(world.ball_velocity_2d))
+                    else:
+                        world.ball_velocity_2d = np.zeros(2)
+                        world.ball_speed = 0.0
+
                     world.is_ball_pos_updated = True
 
                 elif obj_type == "P":

kick_test.sh

@@ -1,7 +0,0 @@
-#!/bin/bash
-export OMP_NUM_THREADS=1
-
-host=${1:-localhost}
-port=${2:-60000}
-
-python3 run_player.py --host $host --port $port -n 1 -t SE &

readme.md

@@ -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')
@@ -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.
+Contributions, bug reports, and feature requests are welcome via pull requests.

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()
 

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

utils/math_ops.py

@@ -5,7 +5,7 @@ import sys
 
 try:
     GLOBAL_DIR = sys._MEIPASS  # temporary folder with libs & data files
-except:
+except AttributeError:
     GLOBAL_DIR = "."
 
 
@@ -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) '''
@@ -285,22 +286,10 @@ class MathOps():
     @staticmethod
     def intersection_segment_opp_goal(a: np.ndarray, b: np.ndarray):
         ''' Computes the intersection point of 2d segment 'ab' and the opponents' goal (front line) '''
-        vec_x = b[0] - a[0]
+        raise NotImplementedError(
-
+            "intersection_segment_opp_goal uses hardcoded x=15 / y=±1.01. "
-        # Collinear intersections are not accepted
+            "Refactor to accept field dimensions before use."
-        if vec_x == 0: return None
+        )
-
-        k = (15.01 - a[0]) / vec_x
-
-        # No collision
-        if k < 0 or k > 1: return None
-
-        intersection_pt = a + (b - a) * k
-
-        if -1.01 <= intersection_pt[1] <= 1.01:
-            return intersection_pt
-        else:
-            return None
 
     @staticmethod
     def intersection_circle_opp_goal(p: np.ndarray, r):
@@ -308,34 +297,18 @@ class MathOps():
         Computes the intersection segment of circle (center p, radius r) and the opponents' goal (front line)
         Only the y coordinates are returned since the x coordinates are always equal to 15
         '''
-
+        raise NotImplementedError(
-        x_dev = abs(15 - p[0])
+            "intersection_circle_opp_goal uses hardcoded x=15 / y=±1.01. "
-
+            "Refactor to accept field dimensions before use."
-        if x_dev > r:
+        )
-            return None  # no intersection with x=15
-
-        y_dev = sqrt(r * r - x_dev * x_dev)
-
-        p1 = max(p[1] - y_dev, -1.01)
-        p2 = min(p[1] + y_dev, 1.01)
-
-        if p1 == p2:
-            return p1  # return the y coordinate of a single intersection point
-        elif p2 < p1:
-            return None  # no intersection
-        else:
-            return p1, p2  # return the y coordinates of the intersection segment
 
     @staticmethod
     def distance_point_to_opp_goal(p: np.ndarray):
         ''' Distance between point 'p' and the opponents' goal (front line) '''
-
+        raise NotImplementedError(
-        if p[1] < -1.01:
+            "distance_point_to_opp_goal uses hardcoded x=15 / y=±1.01. "
-            return np.linalg.norm(p - (15, -1.01))
+            "Refactor to accept field dimensions before use."
-        elif p[1] > 1.01:
+        )
-            return np.linalg.norm(p - (15, 1.01))
-        else:
-            return abs(15 - p[0])
 
     @staticmethod
     def circle_line_segment_intersection(circle_center, circle_radius, pt1, pt2, full_line=True, tangent_tol=1e-9):

world/commons/field.py

@@ -1,62 +1,237 @@
-from abc import ABC, abstractmethod
+from __future__ import annotations
-from typing_extensions import override
+
+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):
+    def _resolve_side(self, side: GoalSide) -> Literal["left", "right"]:
-        return (-self.get_length() / 2, 0)
+        if side == "left":
+            return "left"
+        if side == "right":
+            return "right"
+        is_left = self.world.is_left_team
+        if side == "our":
+            return "left" if is_left else "right"
+        if side == "their":
+            return "right" if is_left else "left"
+        raise ValueError(f"Unknown field side: {side}")
 
-    def get_their_goal_position(self):
+    def get_width(self) -> float:
-        return (self.get_length() / 2, 0)
+        return self.FIELD_DIM[1]
 
-    @abstractmethod
+    def get_length(self) -> float:
-    def get_width(self):
+        return self.FIELD_DIM[0]
-        raise NotImplementedError()
 
-    @abstractmethod
+    def get_goal_dim(self) -> tuple[float, float, float]:
-    def get_length(self):
+        return self.GOAL_DIM
-        raise NotImplementedError()
+
+    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, is_left_team: bool = True) -> tuple[float, float, float]:
+        try:
+            pose = 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
+
+        if is_left_team:
+            return pose
+
+        x, y, rotation = pose
+        mirrored_rotation = ((rotation + 180.0 + 180.0) % 360.0) - 180.0
+        return (-x, -y, mirrored_rotation)
+
+    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):
+    FIELD_DIM = (105.0, 68.0, 40.0)
-        super().__init__(world)
+    LINE_WIDTH = 0.1
-
+    GOAL_DIM = (1.6, 7.32, 2.44)
-    @override
+    GOALIE_AREA_DIM = (5.5, 18.32)
-    def get_width(self):
+    PENALTY_AREA_DIM = (16.5, 40.32)
-        return 68
+    PENALTY_SPOT_DISTANCE = 11.0
-
+    CENTER_CIRCLE_RADIUS = 9.15
-    @override
+    DEFAULT_BEAM_POSES = {
-    def get_length(self):
+        1: (-50.7, 0.0, 0.0),
-        return 105
+        2: (-38.9, 10.9, 0.0),
+        3: (-36.8, 4.8, 0.0),
+        4: (-36.8, -4.8, 0.0),
+        5: (-38.9, -10.9, 0.0),
+        6: (-10.2, 0.0, 0.0),
+        7: (-18.9, 0.0, 0.0),
+    }
 
 
 class HLAdultField(Field):
-    def __init__(self, world):
+    FIELD_DIM = (14.0, 9.0, 40.0)
-        super().__init__(world)
+    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):
+    FIELD_DIM = (55.0, 36.0, 40.0)
-        super().__init__(world)
+    LINE_WIDTH = 0.1
-
+    GOAL_DIM = (0.84, 3.9, 2.44)
-    @override
+    GOALIE_AREA_DIM = (2.9, 9.6)
-    def get_width(self):
+    PENALTY_AREA_DIM = (8.6, 21.3)
-        return 36
+    PENALTY_SPOT_DISTANCE = 5.8
-
+    CENTER_CIRCLE_RADIUS = 4.79
-    @override
+    DEFAULT_BEAM_POSES = {
-    def get_length(self):
+        1: (-25.7, 0.0, 0.0),
-        return 55
+        2: (-15.7, 5.8, 0.0),
+        3: (-13.8, 2.5, 0.0),
+        4: (-13.8, -2.5, 0.0),
+        5: (-15.7, -5.8, 0.0),
+        6: (-5.8, 0.0, 0.0),
+        7: (-9.9, 0.0, 0.0),
+    }

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):
+    def __init__(self, field):
-        from world.world import World  # type hinting
+        self.field = field
-
+        self.world = field.world
-        self.world: World = world
+        self.landmarks: dict[str, np.ndarray] = {}
-
+        self.canonical_landmarks: dict[str, np.ndarray] = field.get_canonical_landmarks()
-        self.landmarks: dict = {}
 
     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 the current perceived or canonical global position for a landmark.
-        Returns None if the landmark is not currently visible or processed.
         """
-        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)

world/world.py

@@ -1,4 +1,4 @@
-from dataclasses import Field
+from world.commons.field import Field
 import numpy as np
 from world.commons.other_robot import OtherRobot
 from world.commons.field import FIFAField, HLAdultField, Soccer7vs7Field
@@ -38,10 +38,14 @@ class World:
         self.score_left: int = None
         self.score_right: int = None
         self.their_team_name: str = None
-        self.last_server_time: str = None
+        self.last_server_time: float = None
         self._global_cheat_position: np.ndarray = np.zeros(3)
         self.global_position: np.ndarray = np.zeros(3)
         self.ball_pos: np.ndarray = np.zeros(3)
+        self.ball_last_pos: np.ndarray = np.zeros(3)
+        self.ball_last_update_time: float = None
+        self.ball_velocity_2d: np.ndarray = np.zeros(2)
+        self.ball_speed: float = 0.0
         self.is_ball_pos_updated: bool = False
         self.our_team_players: list[OtherRobot] = [OtherRobot() for _ in range(self.MAX_PLAYERS_PER_TEAM)]
         self.their_team_players: list[OtherRobot] = [OtherRobot(is_teammate=False) for _ in
@@ -56,6 +60,19 @@ class World:
             playmode=self.playmode, is_left_team=self.is_left_team
         )
 
+    # ------------------------------------------------------------------
+    # Ball freshness
+    # ------------------------------------------------------------------
+    BALL_FRESH_THRESHOLD: float = 0.6    # < 0.6s: reliable
+    BALL_STALE_THRESHOLD: float = 3.0    # > 3.0s: completely lost
+
+    @property
+    def ball_age(self) -> float:
+        """Seconds since the ball position was last updated by vision."""
+        if self.ball_last_update_time is None or self.server_time is None:
+            return float("inf")
+        return self.server_time - self.ball_last_update_time
+
     def is_fallen(self) -> bool:
         return self.global_position[2] < 0.3
 
@@ -65,4 +82,4 @@ class World:
         elif field_name in ('sim3d_7vs7'):
             return Soccer7vs7Field(world=self)
         else:
-            return FIFAField(world=self)
+            return FIFAField(world=self)