repair some bugs and path planning 0.2.0 version

.gitignore

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

agent/agent.py

@@ -6,6 +6,7 @@ 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
+from agent.path_planner import PathPlanner
 
 
 logger = logging.getLogger()
@@ -49,9 +50,6 @@ class Agent:
         }
     }
 
-    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.
@@ -63,10 +61,7 @@ class Agent:
 
         self.agent: Base_Agent = agent
         self.is_getting_up: bool = False
-        self.is_kicking: bool = False
+        self.path_planner = PathPlanner(agent.world)
-        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:
         """
@@ -92,19 +87,11 @@ class Agent:
             self.is_getting_up = not self.agent.skills_manager.execute(skill_name="GetUp")
 
         elif self.agent.world.playmode is PlayModeEnum.PLAY_ON:
-            active_player_number = self.get_active_player_number()
+            self.carry_ball()
-            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.execute_support_behavior()
+            self.carry_ball()
-            self.agent.skills_manager.execute("Neutral")
 
         self.agent.robot.commit_motor_targets_pd()
 
@@ -112,21 +99,19 @@ class Agent:
         """
         Basic example of a behavior: moves the robot toward the goal while handling the ball.
         """
-        target_point = self.target_point
+        their_goal_pos = np.array(self.agent.world.field.get_their_goal_position()[:2])
         ball_pos = self.agent.world.ball_pos[:2]
         my_pos = self.agent.world.global_position[:2]
+        next_target = None
 
-        if self.kick_ball(target_point=target_point):
+        ball_to_goal = their_goal_pos - ball_pos
+        bg_norm = np.linalg.norm(ball_to_goal)
+        if bg_norm == 0:
             return
-
+        ball_to_goal_dir = ball_to_goal / bg_norm
-        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
 
         dist_from_ball_to_start_carrying = 0.30
-        carry_ball_pos = ball_pos - ball_to_target_dir * dist_from_ball_to_start_carrying
+        carry_ball_pos = ball_pos - ball_to_goal_dir * dist_from_ball_to_start_carrying
 
         my_to_ball = ball_pos - my_pos
         my_to_ball_norm = np.linalg.norm(my_to_ball)
@@ -135,17 +120,22 @@ 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)
 
         ANGLE_TOL = np.deg2rad(7.5)
         aligned = (my_to_ball_norm > 1e-6) and (angle_diff <= ANGLE_TOL)
 
-        behind_ball = np.dot(my_pos - ball_pos, ball_to_target_dir) < 0
+        behind_ball = np.dot(my_pos - ball_pos, ball_to_goal_dir) < 0
-        desired_orientation = MathOps.vector_angle(ball_to_target)
+        desired_orientation = MathOps.vector_angle(ball_to_goal)
 
         if not aligned or not behind_ball:
+            self.path_planner.set_target(carry_ball_pos)
+            current_time = self.agent.world.server_time
+            next_target = self.path_planner.update(my_pos, current_time=current_time)
+            if next_target is None:
+                next_target = carry_ball_pos
             self.agent.skills_manager.execute(
                 "Walk",
                 target_2d=carry_ball_pos,
@@ -153,142 +143,15 @@ class Agent:
                 orientation=None if np.linalg.norm(my_pos - carry_ball_pos) > 2 else desired_orientation
             )
         else:
+            self.path_planner.set_target(their_goal_pos)
+            current_time = self.agent.world.server_time
+            next_target = self.path_planner.update(my_pos, current_time=current_time)
+            if next_target is None:
+                next_target = their_goal_pos
             self.agent.skills_manager.execute(
                 "Walk",
-                target_2d=target_point,
+                target_2d=their_goal_pos,
                 is_target_absolute=True,
                 orientation=desired_orientation
             )
 
-    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,
-        )
-
-        close_to_ball = my_to_ball_dist <= 0.24
-        good_orientation = orientation_error <= 14.0
-        good_lateral_offset = lateral_error_to_shot_line <= 0.16
-        in_shooting_range = ball_to_target_dist <= 12.0
-
-        can_shoot = (
-            close_to_ball
-            and behind_ball
-            and good_orientation
-            and good_lateral_offset
-            and in_shooting_range
-        )
-
-        if not can_shoot:
-            prepare_offset = 0.06
-            prepare_pos = ball_pos - ball_to_target_dir * prepare_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,
-            is_target_absolute=True,
-            orientation=support_orientation,
-        )

agent/path_planner.py

@@ -0,0 +1,520 @@
+import numpy as np
+from heapq import heappush, heappop
+from typing import List, Tuple, Optional
+from world.world import World  # 假设你的世界类
+from world.commons.field_landmarks import FieldLandmarks  # 假设你的地标类
+import time
+
+
+class PathPlanner:
+    """
+    足球机器人全局路径规划器（A* + 动态避障）
+    - 静态地图：边界硬墙，球门内部可通行，门柱硬墙
+    - 动态障碍物：对手球员（扩大半径 + 缓冲代价）
+    - 支持多目标点队列，自动切换
+    - 不预测对手运动，仅使用当前帧位置
+    """
+
+    def __init__(self, world, grid_resolution: float = 0.2):
+        """
+        :param world: 世界对象，需包含 field_landmarks 和 global_position 属性
+        :param grid_resolution: 栅格分辨率（米/格）
+        """
+        self.world = world
+        self.res = grid_resolution
+
+        # 球场参数（基于 Sim3D7vs7SoccerField）
+        self.field_half_len = 27.5      # 球场半长（不含球门深度）
+        self.field_half_width = 18.0    # 球场半宽
+        self.goal_width = 3.66          # 球门宽度
+        self.goal_depth = 1.0           # 球门深度
+        self.goal_half_width = self.goal_width / 2.0
+        self.post_radius = 0.05         # 门柱半径
+
+        # 机器人物理参数
+        self.robot_radius = 0.2         # 机器人半径（米）
+        self.safety_margin = 0.2        # 避障安全余量（代替预测）
+
+        # 栅格尺寸
+        # x 范围扩展以包含球门内部：[-field_half_len - goal_depth, field_half_len + goal_depth]
+        self.x_min = -self.field_half_len - self.goal_depth
+        self.x_max = self.field_half_len + self.goal_depth
+        self.y_min = -self.field_half_width
+        self.y_max = self.field_half_width
+        self.nx = int((self.x_max - self.x_min) / self.res) + 1
+        self.ny = int((self.y_max - self.y_min) / self.res) + 1
+
+        # 静态代价地图（初始化后不再改变）
+        self.static_cost_map = np.zeros((self.nx, self.ny), dtype=np.float32)
+        self._init_static_map()
+
+        # 多目标点管理
+        self.waypoints: List[np.ndarray] = []   # 目标点列表
+        self.current_wp_idx: int = 0
+        self.current_path: List[np.ndarray] = []   # 当前规划的完整路径（世界坐标）
+        self.replan_interval: float = 1.0          # 重新规划频率（Hz）
+        self.last_replan_time: float = 0.0
+        self.arrival_threshold: float = 0.3        # 到达目标的距离阈值（米）
+
+    # ---------- 坐标转换辅助 ----------
+    def _world_to_grid(self, x: float, y: float) -> Tuple[int, int]:
+        """世界坐标 -> 栅格坐标（边界裁剪）"""
+        ix = int((x - self.x_min) / self.res)
+        iy = int((y - self.y_min) / self.res)
+        ix = max(0, min(ix, self.nx - 1))
+        iy = max(0, min(iy, self.ny - 1))
+        return ix, iy
+
+    def _grid_to_world(self, ix: int, iy: int) -> Tuple[float, float]:
+        """栅格坐标 -> 世界坐标（中心点）"""
+        x = ix * self.res + self.x_min
+        y = iy * self.res + self.y_min
+        return x, y
+
+    def _world_to_grid_x(self, x: float) -> int:
+        return int((x - self.x_min) / self.res)
+
+    def _world_to_grid_y(self, y: float) -> int:
+        return int((y - self.y_min) / self.res)
+
+    def _grid_to_world_x(self, ix: int) -> float:
+        return ix * self.res + self.x_min
+
+    def _grid_to_world_y(self, iy: int) -> float:
+        return iy * self.res + self.y_min
+
+    # ---------- 静态地图生成 ----------
+    def _init_static_map(self):
+        """根据 Sim3D7vs7SoccerField 参数生成静态代价地图"""
+        # 球场参数
+        field_half_len = 27.5      # 球场半长（不含球门深度）
+        field_half_width = 18.0
+        goal_width = 3.66
+        goal_depth = 1.0
+        goal_half_width = goal_width / 2.0
+        post_radius = 0.05
+
+        # 1. 初始化全地图为 0（自由空间）
+        self.static_cost_map.fill(0.0)
+
+        # 2. 边界硬墙
+        # 左侧边界：x < -field_half_len 的区域，但保留球门开口（|y| <= goal_half_width 时球门内部可通行）
+        for i in range(self._world_to_grid_x(-field_half_len - 0.001), -1, -1):
+            for j in range(self.ny):
+                y = self._grid_to_world_y(j)
+                if abs(y) > goal_half_width:
+                    self.static_cost_map[i, j] = -3
+        # 右侧边界：x > field_half_len 的区域，保留球门开口
+        for i in range(self._world_to_grid_x(field_half_len + 0.001), self.nx):
+            for j in range(self.ny):
+                y = self._grid_to_world_y(j)
+                if abs(y) > goal_half_width:
+                    self.static_cost_map[i, j] = -3
+        # 上下边界
+        for i in range(self.nx):
+            for j in [0, self.ny-1]:
+                self.static_cost_map[i, j] = -3
+        # 可选：如果需要在边界内留出线宽，可额外处理
+
+        # 4. 门柱（作为硬墙）
+        goal_post_positions = [
+            
+            (field_half_len,  goal_half_width),  # 右侧上柱
+            (field_half_len,  -goal_half_width),  # 右侧下柱
+            (-field_half_len,  goal_half_width),  # 左侧上柱
+            (-field_half_len,  -goal_half_width),  # 左侧下柱
+        ]
+        for px, py in goal_post_positions:
+            ix, iy = self._world_to_grid(px, py)
+            rad_cells = int(post_radius / self.res) + 1
+            for dx in range(-rad_cells, rad_cells+1):
+                for dy in range(-rad_cells, rad_cells+1):
+                    nx, ny = ix + dx, iy + dy
+                    if not (0 <= nx < self.nx and 0 <= ny < self.ny):
+                        continue
+                    dist = np.hypot(dx * self.res, dy * self.res)
+                    if dist <= post_radius:
+                        self.static_cost_map[nx, ny] = -3
+
+    # ---------- 获取动态障碍物（对手球员） ----------
+    def _get_opponent_positions(self) -> List[np.ndarray]:
+        """从 FieldLandmarks 获取所有对手球员的全局位置"""
+        opponents = []
+        for player in self.world.their_team_players:
+            if player.last_seen_time is not None and (self.world.server_time - player.last_seen_time) < 1.0:
+                opponents.append(player.position[:2])  # 只使用 x, y
+            else:
+                # 长时间未看到的球员不添加到避障列表中
+                continue  # 跳过未看到的球员
+        return opponents
+
+    def _get_teammate_positions(self) -> List[np.ndarray]:
+        """从 FieldLandmarks 获取所有队友球员的全局位置"""
+        teammates = []
+        for player in self.world.our_team_players:
+            if player.last_seen_time is not None and (self.world.server_time - player.last_seen_time) < 1.0:
+                teammates.append(player.position[:2])  # 只使用 x, y
+            else:
+                # 长时间未看到的球员不添加到避障列表中
+                continue  # 跳过未看到的球员
+        return teammates
+    # ---------- 动态障碍物添加到代价地图 ----------
+    def _apply_opponents_to_map(self, cost_map: np.ndarray, opponents: List[np.ndarray]):
+        """
+        在动态代价地图上添加对手障碍物：
+        - 硬半径内：-3（不可通行）
+        - 缓冲区内：正代价（鼓励远离）
+        """
+        effective_radius = self.robot_radius + self.safety_margin
+        radius_cells = int(effective_radius / self.res) + 1
+        buffer_width = 0.2   # 缓冲区宽度（米）
+
+        for opp in opponents:
+            if opp is None:
+                continue
+            ox, oy = opp[0], opp[1]
+            ix, iy = self._world_to_grid(ox, oy)
+            for dx in range(-radius_cells, radius_cells + 1):
+                for dy in range(-radius_cells, radius_cells + 1):
+                    nx, ny = ix + dx, iy + dy
+                    if not (0 <= nx < self.nx and 0 <= ny < self.ny):
+                        continue
+                    dist = np.hypot(dx * self.res, dy * self.res)
+                    if dist <= effective_radius:
+                        cost_map[nx, ny] = -3   # 硬墙
+                    elif dist <= effective_radius + buffer_width:
+                        # 缓冲区内增加代价（线性衰减）
+                        if cost_map[nx, ny] >= 0:
+                            cost_map[nx, ny] += 10.0 * (1.0 - (dist - effective_radius) / buffer_width)
+
+    def _apply_teammate_to_map(self, cost_map, teammates):
+        soft_radius = self.robot_radius + 0.2   # 允许稍微接近
+        for tm in teammates:
+            ix, iy = self._world_to_grid(tm[0], tm[1])
+            rad_cells = int(soft_radius / self.res) + 1
+            for dx in range(-rad_cells, rad_cells+1):
+                for dy in range(-rad_cells, rad_cells+1):
+                    nx, ny = ix+dx, iy+dy
+                    if not (0 <= nx < self.nx and 0 <= ny < self.ny):
+                        continue
+                    dist = np.hypot(dx*self.res, dy*self.res)
+                    if dist <= soft_radius:
+                        # 不设为硬墙，只加较小代价
+                        if cost_map[nx, ny] >= 0:
+                            cost_map[nx, ny] += 5.0   # 代价比对手小
+
+    # ---------- 启发函数 ----------
+    def _heuristic(self, ix1: int, iy1: int, ix2: int, iy2: int) -> float:
+        """对角线距离（允许8方向移动）"""
+        dx = abs(ix1 - ix2)
+        dy = abs(iy1 - iy2)
+        return (dx + dy) - 0.585786 * min(dx, dy)   # sqrt(2)-1 ≈ 0.414
+
+    # ---------- 直线检测 ----------
+    def _line_is_free(self, start: np.ndarray, end: np.ndarray, opponents: List[np.ndarray]) -> bool:
+        """检查线段是否与任何对手（扩大后）相交"""
+        effective_radius = self.robot_radius + self.safety_margin
+        ax, ay = start
+        bx, by = end
+        abx = bx - ax
+        aby = by - ay
+        len_sq = abx * abx + aby * aby
+        if len_sq == 0:
+            return True
+
+        for opp in opponents:
+            if opp is None:
+                continue
+            ox, oy = opp[0], opp[1]
+            # 计算投影参数 t
+            t = ((ox - ax) * abx + (oy - ay) * aby) / len_sq
+            if t < 0:
+                t = 0
+            elif t > 1:
+                t = 1
+            closest_x = ax + t * abx
+            closest_y = ay + t * aby
+            dist = np.hypot(closest_x - ox, closest_y - oy)
+            if dist <= effective_radius:
+                return False
+        return True
+
+    # ---------- 路径重构 ----------
+    def _reconstruct_path(self, node: Tuple[int, int], parent: dict, start: np.ndarray) -> List[np.ndarray]:
+        """从父字典重构路径（世界坐标）"""
+        path = []
+        cur = node
+        while cur in parent:
+            x, y = self._grid_to_world(cur[0], cur[1])
+            path.append(np.array([x, y]))
+            cur = parent[cur]
+        path.append(start)
+        path.reverse()
+        return path
+
+    # ---------- A* 规划 ----------
+    def plan(self, start: np.ndarray, target: Optional[np.ndarray] = None,
+         go_to_goal: bool = False, timeout_ms: float = 10.0) -> List[np.ndarray]:
+        """
+        A* 路径规划
+        :param start: 起点 (x, y)
+        :param target: 目标点（当 go_to_goal=False 时使用）
+        :param go_to_goal: 是否前往对方球门
+        :param timeout_ms: 超时时间（毫秒）
+        :return: 路径点列表（世界坐标），若失败返回空列表
+        """
+        # 1. 获取队友并构建动态代价地图
+        teammates = self._get_teammate_positions()
+        cost_map = self.static_cost_map.copy()
+        self._apply_teammate_to_map(cost_map, teammates)
+        opponents = self._get_opponent_positions()
+        self._apply_opponents_to_map(cost_map, opponents)
+
+        # 2. 转换坐标
+        sx, sy = self._world_to_grid(start[0], start[1])
+        if go_to_goal:
+            # 目标点为球门线上 y=0 附近的格子
+            goal_x = self.field_half_len
+            goal_y = 0.0
+            tx, ty = self._world_to_grid(goal_x, goal_y)
+        else:
+            if target is None:
+                raise ValueError("target must be provided when go_to_goal=False")
+            tx, ty = self._world_to_grid(target[0], target[1])
+
+        # 3. 【关键修改】强制将目标点格子设为 -1（覆盖障碍物）
+        if go_to_goal:
+            # 球门线上所有格子都设为 -1（增加容错）
+            goal_line_cell = self._world_to_grid_x(self.field_half_len)
+            y_min_cell = self._world_to_grid_y(-self.goal_half_width)
+            y_max_cell = self._world_to_grid_y(self.goal_half_width)
+            for j in range(y_min_cell, y_max_cell + 1):
+                if 0 <= goal_line_cell < self.nx and 0 <= j < self.ny:
+                    cost_map[goal_line_cell, j] = -1
+        else:
+            if 0 <= tx < self.nx and 0 <= ty < self.ny:
+                cost_map[tx, ty] = -1
+
+        # 4. 快速直线检测（可选，可提高效率）
+        if go_to_goal:
+            end_point = np.array([goal_x, goal_y])
+        else:
+            end_point = target
+        if self._line_is_free(start, end_point, opponents):
+            # 直线无碰撞，直接返回
+            return [start, end_point]
+
+        # 5. A* 初始化
+        open_set = []
+        closed = np.zeros((self.nx, self.ny), dtype=bool)
+        g = np.full((self.nx, self.ny), np.inf)
+        f = np.full((self.nx, self.ny), np.inf)
+        parent = {}   # (ix, iy) -> (pix, piy)
+
+        g[sx, sy] = 0.0
+        f[sx, sy] = self._heuristic(sx, sy, tx, ty)
+        heappush(open_set, (f[sx, sy], sx, sy))
+
+        # 记录最佳节点（用于超时/不可达回退）
+        best_node = (sx, sy)
+        best_h = self._heuristic(sx, sy, tx, ty)
+
+        start_time = time.time()
+        iterations = 0
+
+        # 邻居方向（8方向）及其移动代价
+        dirs = [(-1, -1, 1.414), (-1, 0, 1.0), (-1, 1, 1.414),
+                (0, -1, 1.0),                 (0, 1, 1.0),
+                (1, -1, 1.414),  (1, 0, 1.0), (1, 1, 1.414)]
+
+        while open_set:
+            iterations += 1
+            # 超时检查
+            if iterations % 32 == 0 and (time.time() - start_time) * 1000 > timeout_ms:
+                # 超时，返回最佳节点路径（如果有）
+                if best_node != (sx, sy):
+                    return self._reconstruct_path(best_node, parent, start)
+                else:
+                    return []
+
+            _, cx, cy = heappop(open_set)
+            if closed[cx, cy]:
+                continue
+            closed[cx, cy] = True
+
+            # 更新最佳节点（基于启发式距离）
+            h = self._heuristic(cx, cy, tx, ty)
+            if h < best_h:
+                best_h = h
+                best_node = (cx, cy)
+
+            # 到达目标
+            if (cx, cy) == (tx, ty) or (go_to_goal and cost_map[cx, cy] == -1):
+                return self._reconstruct_path((cx, cy), parent, start)
+
+            # 扩展邻居
+            for dx, dy, step_cost in dirs:
+                nx, ny = cx + dx, cy + dy
+                if not (0 <= nx < self.nx and 0 <= ny < self.ny):
+                    continue
+                if closed[nx, ny]:
+                    continue
+
+                cell_cost = cost_map[nx, ny]
+                if cell_cost == -3:   # 硬墙
+                    continue
+
+                move_cost = step_cost + max(0.0, cell_cost)
+                tentative_g = g[cx, cy] + move_cost
+
+                if tentative_g < g[nx, ny]:
+                    parent[(nx, ny)] = (cx, cy)
+                    g[nx, ny] = tentative_g
+                    f[nx, ny] = tentative_g + self._heuristic(nx, ny, tx, ty)
+                    heappush(open_set, (f[nx, ny], nx, ny))
+
+        # open 集为空，不可达
+        if best_node != (sx, sy):
+            return self._reconstruct_path(best_node, parent, start)
+        else:
+            return []
+
+    # ---------- 多目标点管理 ----------
+    def set_waypoints(self, waypoints: List[np.ndarray]):
+        """
+        设置目标点序列（世界坐标）。如果某个元素为 None，表示前往对方球门。
+        """
+        self.waypoints = [np.array(wp) if wp is not None else None for wp in waypoints]
+        self.current_wp_idx = 0
+        self.current_path = []
+        self.last_replan_time = 0.0
+
+    def get_next_target(self) -> Optional[np.ndarray]:
+        """返回当前需要前往的目标点（世界坐标）"""
+        if self.current_wp_idx >= len(self.waypoints):
+            return None
+        wp = self.waypoints[self.current_wp_idx]
+        if wp is None:
+            # 前往球门
+            return np.array([self.field_half_len, 0.0])
+        return wp
+
+    def advance_to_next_target(self):
+        """标记当前目标已完成，切换到下一个"""
+        if self.current_wp_idx < len(self.waypoints):
+            self.current_wp_idx += 1
+            self.current_path = []   # 清空旧路径
+
+    def update(self, current_pos: np.ndarray, current_time: float) -> Optional[np.ndarray]:
+        """
+        更新路径规划，返回下一个需要前往的路径点。
+        :param current_pos: 机器人当前世界坐标 (x, y)
+        :param current_time: 当前时间（秒），用于可选的重规划频率控制
+        :return: 下一个路径点（世界坐标），若无有效目标则返回 None
+        """
+        # 1. 获取当前需要前往的目标点
+        target = self.get_next_target()
+        if target is None:
+            # 没有剩余目标，停止移动
+            return None
+
+        # 2. 到达检测：如果已有路径且路径终点距离机器人很近，则认为已到达当前目标
+        if len(self.current_path) >= 2:
+            last_point = self.current_path[-1]
+            if np.linalg.norm(last_point - current_pos) < self.arrival_threshold:
+                # 当前目标已完成，切换到下一个目标
+                self.advance_to_next_target()
+                target = self.get_next_target()
+                if target is None:
+                    return None
+                # 清空旧路径，强制下次重规划到新目标
+                self.current_path = []
+
+        # 3. 路径有效性检查（仅当存在有效路径时）
+        path_valid = (len(self.current_path) >= 2) and self._is_path_still_valid(current_pos)
+
+        # 4. 判断是否需要重新规划
+        #   条件1：当前路径为空（包括刚切换目标后）
+        #   条件2：当前路径被障碍物阻塞
+        need_replan = (len(self.current_path) < 2) or not path_valid
+
+        if need_replan:
+            # 重新规划到当前目标
+            new_path = self.plan(current_pos,
+                                target=target if target is not None else None,
+                                go_to_goal=(target is None),
+                                timeout_ms=10.0)
+            if new_path and len(new_path) > 1:
+                self.current_path = new_path
+                self.last_replan_time = current_time
+            else:
+                # 当前目标不可达（规划失败），跳过它，尝试下一个
+                self.advance_to_next_target()
+                # 递归调用，继续处理下一个目标（避免深度过大，但目标数量有限）
+                return self.update(current_pos, current_time)
+
+        # 5. 返回下一个路径点（路径的第二个点，第一个点为机器人当前位置的近似）
+        if len(self.current_path) >= 2:
+            return self.current_path[1]
+        else:
+            return None
+        
+    def _is_path_still_valid(self, current_pos: np.ndarray, lookahead_dist: float = 3.0) -> bool:
+        """
+        检查从机器人当前位置开始的剩余路径是否仍无碰撞（仅检查前方 lookahead_dist 米内）。
+        :param current_pos: 机器人当前世界坐标 (x, y)
+        :param lookahead_dist: 检查的前向距离（米），默认 3.0
+        :return: 如果路径在前方范围内无碰撞返回 True，否则 False
+        """
+        if len(self.current_path) < 2:
+            return False
+
+        # 获取对手最新位置
+        opponents = self._get_opponent_positions()
+
+        # 累积距离
+        accumulated_dist = 0.0
+        # 从机器人当前位置到路径第二个点的第一段
+        start = current_pos
+        end = self.current_path[1]
+        seg_dist = np.linalg.norm(end - start)
+        if not self._line_is_free(start, end, opponents):
+            return False
+        accumulated_dist += seg_dist
+        # 如果第一段就已经超过阈值，直接返回 True（已检查第一段无碰撞）
+        if accumulated_dist >= lookahead_dist:
+            return True
+
+        # 继续检查后续路径段，直到累积距离超过阈值
+        for i in range(1, len(self.current_path) - 1):
+            start = self.current_path[i]
+            end = self.current_path[i+1]
+            seg_dist = np.linalg.norm(end - start)
+            if not self._line_is_free(start, end, opponents):
+                return False
+            accumulated_dist += seg_dist
+            if accumulated_dist >= lookahead_dist:
+                break
+
+        return True
+    
+    def set_target(self, target: np.ndarray, force: bool = False):
+        """
+        设置单目标点（世界坐标）。
+        :param target: 新目标点
+        :param force: 是否强制更新（即使目标相同或距离很近）
+        """
+        # 获取当前有效目标（如果存在）
+        current_target = self.get_next_target()
+        if current_target is not None and not force:
+            # 计算新目标与当前目标的欧氏距离
+            dist = np.linalg.norm(target - current_target)
+            if dist < 0.2:   # 阈值可调（例如 0.2 米）
+                # 目标没有显著变化，不更新
+                return
+
+        # 目标变化显著，或强制更新
+        self.waypoints = [target]
+        self.current_wp_idx = 0
+        self.current_path = []          # 清空旧路径，触发重规划
+        self.last_replan_time = 0.0

behaviors/behavior_manager.py

@@ -1,5 +1,4 @@
 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
@@ -23,7 +22,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, GetUp]
 
         # instantiate each Skill and store in the skills dictionary
         self.skills = {cls.__name__: cls(agent=self.agent) for cls in classes}

behaviors/custom/keyframe/get_up/get_up.py

@@ -9,7 +9,7 @@ logger = logging.getLogger()
 
 class GetUp(Behavior):
     STABILITY_THRESHOLD_CYCLES: int = 3
-    NEUTRAL_EXECUTION_TIME: float = 1.0
+    NEUTRAL_EXECUTION_TIME: float = 1.5
     def __init__(self, agent):
         super().__init__(agent)
         self.get_up_front = KeyframeSkill(

behaviors/custom/keyframe/get_up/get_up_back.yaml

@@ -2,33 +2,6 @@ symmetry: true
 kp: 75
 kd: 1
 keyframes:
-  - delta: 1
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Shoulder_Pitch: 0
-      Shoulder_Roll: 0
-      Elbow_Pitch: 0
-      Elbow_Yaw: 0
-      Waist: 0.000
-      Hip_Pitch: 0
-      Hip_Roll: 0
-      Hip_Yaw: 0
-      Knee_Pitch: 0
-      Ankle_Pitch: 0
-      Ankle_Roll: 0
-    kp: 20
-    kd: 1
-    p_gains:
-      Hip_Pitch: 20
-      Hip_Roll: 20
-      Hip_Yaw: 20
-      Knee_Pitch: 20
-    d_gains:
-      Hip_Pitch: 1
-      Hip_Roll: 1
-      Hip_Yaw: 1
-      Knee_Pitch: 1
 
   - delta: 1
     motor_positions:
@@ -36,15 +9,15 @@ keyframes:
       Head_pitch: 0.000
       Shoulder_Pitch: -114.592
       Shoulder_Roll: -71.619
-      Elbow_Pitch: -100.944
+      Elbow_Pitch: -85.944
-      Elbow_Yaw: -171.000
+      Elbow_Yaw: -171.887
       Waist: 0.000
-      Hip_Pitch: -35.000
+      Hip_Pitch: -45.836
-      Hip_Roll: 24.000
+      Hip_Roll: 179.908
-      Hip_Yaw: 8.000
+      Hip_Yaw: 179.908
-      Knee_Pitch: -100.000
+      Knee_Pitch: -179.908
-      Ankle_Pitch: 70.000
+      Ankle_Pitch: 89.954
-      Ankle_Roll: 10.000
+      Ankle_Roll: 89.954
     kp: 250
     kd: 1
     p_gains:
@@ -58,91 +31,13 @@ keyframes:
       Hip_Yaw: 1
       Knee_Pitch: 1
 
-  - delta: 0.45
+  - delta: 0.8125
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Shoulder_Pitch: -114.592
-      Shoulder_Roll: -80.000
-      Elbow_Pitch: -140.944
-      Elbow_Yaw: -171.000
-      Waist: 0.000
-      Hip_Pitch: -25.000
-      Hip_Roll: 20.000
-      Hip_Yaw: 8.000
-      Knee_Pitch: -120.000
-      Ankle_Pitch: 60.000
-      Ankle_Roll: 8.000
-    kp: 220
-    kd: 1.5
-    p_gains:
-      Hip_Pitch: 20
-      Hip_Roll: 20
-      Hip_Yaw: 20
-      Knee_Pitch: 20
-    d_gains:
-      Hip_Pitch: 1
-      Hip_Roll: 1
-      Hip_Yaw: 1
-      Knee_Pitch: 1
-
-  - delta: 0.35
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Shoulder_Pitch: -114.000
-      Shoulder_Roll: -86.000
-      Elbow_Pitch: -140.944
-      Elbow_Yaw: -171.000
-      Waist: 0.000
-      Hip_Pitch: -30.000
-      Hip_Roll: 160.000
-      Hip_Yaw: 160.000
-      Knee_Pitch: -150.000
-      Ankle_Pitch: 60.000
-      Ankle_Roll: 60.000
-    kp: 50
-    kd: 2
-    p_gains:
-      Hip_Pitch: 120
-      Hip_Roll: 120
-      Hip_Yaw: 120
-      Knee_Pitch: 180
-      Ankle_Pitch: 150
-      Ankle_Roll: 150
-
-  - delta: 0.35
     motor_positions:
       Head_yaw: 0.000
       Head_pitch: 0.000
       Shoulder_Pitch: -114.592
       Shoulder_Roll: -89.954
-      Elbow_Pitch: -90.944
+      Elbow_Pitch: -85.944
-      Elbow_Yaw: 0.000
-      Waist: 0.000
-      Hip_Pitch: -30.000
-      Hip_Roll: 160.000
-      Hip_Yaw: 160.000
-      Knee_Pitch: -150.000
-      Ankle_Pitch: 60.000
-      Ankle_Roll: 60.000
-    kp: 50
-    kd: 2
-    p_gains:
-      Hip_Pitch: 120
-      Hip_Roll: 120
-      Hip_Yaw: 120
-      Knee_Pitch: 180
-      Ankle_Pitch: 150
-      Ankle_Roll: 150
-  
-  - delta: 0.1
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Shoulder_Pitch: -114.592
-      Shoulder_Roll: -89.954
-      Elbow_Pitch: -140.944
       Elbow_Yaw: 0.000
       Waist: 0.000
       Hip_Pitch: -45.836
@@ -151,38 +46,15 @@ keyframes:
       Knee_Pitch: -179.908
       Ankle_Pitch: 63.026
       Ankle_Roll: 45.836
-    kp: 50
-    kd: 2
     p_gains:
-      Hip_Pitch: 120
+      Hip_Pitch: 25
-      Hip_Roll: 120
+      Hip_Roll: 25
-      Hip_Yaw: 120
+      Hip_Yaw: 25
-      Knee_Pitch: 180
+      Knee_Pitch: 25
-      Ankle_Pitch: 150
-      Ankle_Roll: 150
 
-  - delta: 0.15
-    motor_positions:
-      Head_yaw: 0.000
-      Head_pitch: 0.000
-      Shoulder_Pitch: -114.000
-      Shoulder_Roll: -89.954
-      Elbow_Pitch: -45.000
-      Elbow_Yaw: 0.000
-      Waist: 0.000
-      Hip_Pitch: 10.000
-      Hip_Roll: 16.000
-      Hip_Yaw: 4.000
-      Knee_Pitch: -80.000
-      Ankle_Pitch: 30.000
-      Ankle_Roll: 6.000
-    kp: 50
-    kd: 2
-    p_gains:
-      Ankle_Pitch: 80
-      Ankle_Roll: 80
 
-  - delta: 0.15
+
+  - delta: 0.6
     motor_positions:
       Head_yaw: 0.000
       Head_pitch: 0.000
@@ -197,8 +69,8 @@ keyframes:
       Knee_Pitch: -42.971
       Ankle_Pitch: 57.296
       Ankle_Roll: 0.000
-    kp: 50
+    kp: 40
-    kd: 2
+    kd: 4
     p_gains:
        Ankle_Pitch: 100
        Ankle_Roll: 100

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: -18.000
-      Left_Hip_Roll: 8.000
-      Left_Hip_Yaw: 0.000
-      Left_Knee_Pitch: 42.000
-      Left_Ankle_Pitch: 10.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: -30.000
-      Left_Hip_Roll: 10.000
-      Left_Hip_Yaw: 1.000
-      Left_Knee_Pitch: 54.000
-      Left_Ankle_Pitch: 10.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: -46.000
-      Left_Hip_Roll: 12.000
-      Left_Hip_Yaw: 2.000
-      Left_Knee_Pitch: 28.000
-      Left_Ankle_Pitch: 10.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: -58.000
-      Left_Hip_Roll: 13.000
-      Left_Hip_Yaw: 3.000
-      Left_Knee_Pitch: -16.000
-      Left_Ankle_Pitch: 8.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: -62.000
-      Left_Hip_Roll: 13.000
-      Left_Hip_Yaw: 3.000
-      Left_Knee_Pitch: 10.000
-      Left_Ankle_Pitch: 6.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: -16.000
-      Left_Hip_Roll: 9.000
-      Left_Hip_Yaw: 1.000
-      Left_Knee_Pitch: 38.000
-      Left_Ankle_Pitch: 9.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: -10.000
-      Left_Hip_Roll: 9.000
-      Left_Hip_Yaw: 1.000
-      Left_Knee_Pitch: 34.000
-      Left_Ankle_Pitch: 9.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

world/world.py

@@ -34,7 +34,7 @@ class World:
         self.playmode_group: PlayModeGroupEnum = PlayModeGroupEnum.NOT_INITIALIZED
         self.is_left_team: bool = None
         self.game_time: float = None
-        self.server_time: float = None
+        self.server_time: float = None  # 服务器时间，单位：秒
         self.score_left: int = None
         self.score_right: int = None
         self.their_team_name: str = None