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

add kick behavior with keyframe, but still need to be improved

Browse Source
This commit is contained in:
徐学颢
2026-04-18 18:32:32 +08:00
parent 9e77e4d6e0
commit 047d3d60c4
8 changed files with 1081 additions and 14 deletions
Download Patch File Download Diff File Expand all files Collapse all files

174
agent/agent.py
View File

@@ -49,6 +49,9 @@ 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.
@@ -60,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:
"""
@@ -85,11 +92,19 @@ class Agent:
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.agent.skills_manager.execute("Neutral")
self.agent.robot.commit_motor_targets_pd()
@@ -97,18 +112,21 @@ class Agent:
"""
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 == 0:
if self.kick_ball(target_point=target_point):
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_goal_dir * dist_from_ball_to_start_carrying
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)
@@ -117,15 +135,15 @@ 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)
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
desired_orientation = MathOps.vector_angle(ball_to_goal)
behind_ball = np.dot(my_pos - ball_pos, ball_to_target_dir) < 0
desired_orientation = MathOps.vector_angle(ball_to_target)
if not aligned or not behind_ball:
self.agent.skills_manager.execute(
@@ -137,8 +155,140 @@ class Agent:
else:
self.agent.skills_manager.execute(
"Walk",
target_2d=their_goal_pos,
target_2d=target_point,
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,
)