重写 carry_ball 为三段式 catch_ball 拱球逻辑

agent/agent.py

@@ -95,7 +95,12 @@ class Agent:
 
     def carry_ball(self):
         """
-        Basic example of a behavior: moves the robot toward the goal while handling the ball.
+        Minimal catch-ball behavior.
+
+        All players share the same logic:
+        1. approach a point behind the ball
+        2. reposition with a lateral offset if they are close but not yet behind it
+        3. push the ball forward once they are aligned
         """
         their_goal_pos = self.agent.world.field.get_their_goal_position()[:2]
         ball_pos = self.agent.world.ball_pos[:2]
@@ -103,12 +108,23 @@ class Agent:
 
         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
+        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
 
-        dist_from_ball_to_start_carrying = 0.30
-        carry_ball_pos = ball_pos - ball_to_goal_dir * dist_from_ball_to_start_carrying
+        lateral_dir = np.array([-ball_to_goal_dir[1], ball_to_goal_dir[0]])
+
+        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
 
         my_to_ball = ball_pos - my_pos
         my_to_ball_norm = np.linalg.norm(my_to_ball)
@@ -120,25 +136,36 @@ class Agent:
         cosang = np.dot(my_to_ball_dir, ball_to_goal_dir)
         cosang = np.clip(cosang, -1.0, 1.0)
         angle_diff = np.arccos(cosang)
+        aligned = (my_to_ball_norm > 1e-6) and (angle_diff <= angle_tolerance)
 
-        ANGLE_TOL = np.deg2rad(7.5)
-        aligned = (my_to_ball_norm > 1e-6) and (angle_diff <= ANGLE_TOL)
-
-        behind_ball = np.dot(my_pos - ball_pos, ball_to_goal_dir) < 0
+        behind_ball = np.dot(my_pos - ball_pos, ball_to_goal_dir) < -behind_margin
         desired_orientation = MathOps.vector_angle(ball_to_goal)
 
-        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:
-            self.agent.skills_manager.execute(
-                "Walk",
-                target_2d=their_goal_pos,
-                is_target_absolute=True,
-                orientation=desired_orientation
-            )
+        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
 
+        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
+        else:
+            target_2d = push_target
+            orientation = desired_orientation
+
+        if np.linalg.norm(target_2d - my_pos) <= 1e-4:
+            target_2d = my_pos + ball_to_goal_dir * 0.30
+
+        self.agent.skills_manager.execute(
+            "Walk",
+            target_2d=target_2d,
+            is_target_absolute=True,
+            orientation=orientation,
+        )