add kick test script

.gitignore

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

agent/agent.py

@@ -1,7 +1,10 @@
+from dataclasses import Field
 import logging
+from typing import Mapping
 
 import numpy as np
 from utils.math_ops import MathOps
+from world.commons.field import FIFAField, HLAdultField, Soccer7vs7Field
 from world.commons.play_mode import PlayModeEnum, PlayModeGroupEnum
 
 
@@ -16,6 +19,39 @@ class Agent:
     based on the current state of the world and game conditions.
     """
 
+    BEAM_POSES: Mapping[type[Field], Mapping[int, tuple[float, float, float]]] ={
+        FIFAField: {
+            1: (2.1, 0, 0),
+            2: (22.0, 12.0, 0),
+            3: (22.0, 4.0, 0),
+            4: (22.0, -4.0, 0),
+            5: (22.0, -12.0, 0),
+            6: (15.0, 0.0, 0),
+            7: (4.0, 16.0, 0),
+            8: (11.0, 6.0, 0),
+            9: (11.0, -6.0, 0),
+            10: (4.0, -16.0, 0),
+            11: (7.0, 0.0, 0),
+        },
+        HLAdultField: {
+            1: (7.0, 0.0, 0),
+            2: (2.0, -1.5, 0),
+            3: (2.0, 1.5, 0),
+        },
+        Soccer7vs7Field: {
+            1: (2.1, 0, 0),
+            2: (22.0, 12.0, 0),
+            3: (22.0, 4.0, 0),
+            4: (22.0, -4.0, 0),
+            5: (22.0, -12.0, 0),
+            6: (15.0, 0.0, 0),
+            7: (4.0, 16.0, 0)
+        }
+    }
+
+    TEAMMATE_INFO_MAX_AGE: float = 0.8
+    SUPPORT_DISTANCE_FROM_BALL: float = 1.2
+
     def __init__(self, agent):
         """
         Creates a new DecisionMaker linked to the given agent.
@@ -27,6 +63,10 @@ class Agent:
 
         self.agent: Base_Agent = agent
         self.is_getting_up: bool = False
+        self.is_kicking: bool = False
+        self.current_shot_target_point: np.ndarray | None = None
+        self.target_point: np.ndarray = np.array([0.0, 0.0])
+        self.my_pos: np.ndarray = self.agent.world.global_position[:2]
 
     def update_current_behavior(self) -> None:
         """
@@ -43,56 +83,50 @@ class Agent:
             PlayModeGroupEnum.ACTIVE_BEAM,
             PlayModeGroupEnum.PASSIVE_BEAM,
         ):
-            beam_pose = self.agent.world.field.get_beam_pose(self.agent.world.number)
             self.agent.server.commit_beam(
-                pos2d=beam_pose[:2],
-                rotation=beam_pose[2],
+                pos2d=self.BEAM_POSES[type(self.agent.world.field)][self.agent.world.number][:2],
+                rotation=self.BEAM_POSES[type(self.agent.world.field)][self.agent.world.number][2],
             )
 
         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")
 
         elif self.agent.world.playmode is PlayModeEnum.PLAY_ON:
-            self.carry_ball()
+            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")
         else:
-            self.carry_ball()
+            # self.execute_support_behavior()
+            self.kick_ball()
 
         self.agent.robot.commit_motor_targets_pd()
 
     def carry_ball(self):
         """
-        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
+        Basic example of a behavior: moves the robot toward the goal while handling the ball.
         """
-        their_goal_pos = self.agent.world.field.get_their_goal_position()[:2]
+        target_point = self.target_point
         ball_pos = self.agent.world.ball_pos[:2]
         my_pos = self.agent.world.global_position[:2]
 
-        ball_to_goal = their_goal_pos - ball_pos
-        bg_norm = np.linalg.norm(ball_to_goal)
-        if bg_norm <= 1e-6:
-            ball_to_goal_dir = np.array([1.0, 0.0])
-        else:
-            ball_to_goal_dir = ball_to_goal / bg_norm
+        if self.kick_ball(target_point=target_point):
+            return
 
-        lateral_dir = np.array([-ball_to_goal_dir[1], ball_to_goal_dir[0]])
+        ball_to_target = target_point - ball_pos
+        bt_norm = np.linalg.norm(ball_to_target)
+        if bt_norm == 0:
+            return
+        ball_to_target_dir = ball_to_target / bt_norm
 
-        back_offset = 0.40
-        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
+        dist_from_ball_to_start_carrying = 0.30
+        carry_ball_pos = ball_pos - ball_to_target_dir * dist_from_ball_to_start_carrying
 
         my_to_ball = ball_pos - my_pos
         my_to_ball_norm = np.linalg.norm(my_to_ball)
@@ -101,39 +135,160 @@ class Agent:
         else:
             my_to_ball_dir = my_to_ball / my_to_ball_norm
 
-        cosang = np.dot(my_to_ball_dir, ball_to_goal_dir)
+        cosang = np.dot(my_to_ball_dir, ball_to_target_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)
 
-        behind_ball = np.dot(my_pos - ball_pos, ball_to_goal_dir) < -behind_margin
-        desired_orientation = MathOps.vector_angle(ball_to_goal)
+        ANGLE_TOL = np.deg2rad(7.5)
+        aligned = (my_to_ball_norm > 1e-6) and (angle_diff <= ANGLE_TOL)
 
-        lateral_sign = np.sign(np.cross(ball_to_goal_dir, my_to_ball_dir))
-        if lateral_sign == 0:
-            lateral_sign = 1.0 if (my_pos[1] - ball_pos[1]) >= 0 else -1.0
+        behind_ball = np.dot(my_pos - ball_pos, ball_to_target_dir) < 0
+        desired_orientation = MathOps.vector_angle(ball_to_target)
 
-        reposition_point = behind_point + lateral_dir * lateral_sign * side_offset
-
-        if my_to_ball_norm > approach_distance:
-            target_2d = behind_point
-            orientation = None
-        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
+        if not aligned or not behind_ball:
+            self.agent.skills_manager.execute(
+                "Walk",
+                target_2d=carry_ball_pos,
+                is_target_absolute=True,
+                orientation=None if np.linalg.norm(my_pos - carry_ball_pos) > 2 else desired_orientation
+            )
         else:
-            target_2d = push_target
-            orientation = desired_orientation
+            self.agent.skills_manager.execute(
+                "Walk",
+                target_2d=target_point,
+                is_target_absolute=True,
+                orientation=desired_orientation
+            )
 
-        if np.linalg.norm(target_2d - my_pos) <= 1e-4:
-            target_2d = my_pos + ball_to_goal_dir * 0.30
+    def kick_ball(self, target_point=None):
+        if target_point is None:
+            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=target_2d,
+            target_2d=support_pos,
             is_target_absolute=True,
-            orientation=orientation,
+            orientation=support_orientation,
         )

agent/base_agent.py

@@ -5,7 +5,6 @@ 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__)
@@ -36,7 +35,6 @@ 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)
@@ -55,7 +53,6 @@ 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})"
@@ -81,5 +78,4 @@ class Base_Agent:
         Logs a shutdown message and closes the server connection.
         """
         logger.info("Shutting down.")
-        self.monitor.close()
-        self.server.shutdown()
+        self.server.shutdown()

behaviors/behavior_manager.py

@@ -1,4 +1,5 @@
 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.neutral.neutral import Neutral
 from behaviors.behavior import Behavior
@@ -22,7 +23,7 @@ class BehaviorManager:
         Each skill is indexed by its class name.
         """
 
-        classes: list[type[Behavior]] = [Walk, Neutral, GetUp]
+        classes: list[type[Behavior]] = [Walk, Neutral, GetUp, Kick]
 
         # instantiate each Skill and store in the skills dictionary
         self.skills = {cls.__name__: cls(agent=self.agent) for cls in classes}

behaviors/custom/keyframe/kick/kick.py

@@ -0,0 +1,91 @@
+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

@@ -0,0 +1,187 @@
+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

@@ -0,0 +1,213 @@
+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

@@ -0,0 +1,213 @@
+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

@@ -0,0 +1,213 @@
+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

communication/monitor_client.py

@@ -1,63 +0,0 @@
-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],
-    ) -> None:
-        self.send(
-            f"(agent (unum {unum}) (team {team_name}) (pos {pos[0]} {pos[1]} {pos[2]}))"
-        )

kick_test.sh

@@ -0,0 +1,7 @@
+#!/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,14 +41,7 @@ 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:
@@ -62,8 +55,6 @@ 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')
@@ -94,4 +85,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='fifa', help="Field to be played")
+parser.add_argument("-f", "--field", type=str, default='sim3d_7vs7', 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 -f sim3d_7vs7 &
+  python3 run_player.py --host $host --port $port -n $i -t SE &
 done

utils/math_ops.py

@@ -51,7 +51,6 @@ 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) '''

world/commons/field.py

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

world/commons/field_landmarks.py

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