diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py index 4988caa4..6bb58296 100644 --- a/utama_core/skills/src/defend_parameter.py +++ b/utama_core/skills/src/defend_parameter.py @@ -1,58 +1,85 @@ +import math from typing import Optional -from utama_core.entities.data.command import RobotCommand +import numpy as np + from utama_core.entities.data.vector import Vector2D from utama_core.entities.game import Game from utama_core.motion_planning.src.common.motion_controller import MotionController from utama_core.rsoccer_simulator.src.ssl.envs.standard_ssl import SSLStandardEnv from utama_core.skills.src.go_to_point import go_to_point -from utama_core.skills.src.utils.defense_utils import ( - align_defenders, - find_likely_enemy_shooter, - to_defense_parametric, - velocity_to_orientation, -) def defend_parameter( game: Game, motion_controller: MotionController, - defender_id: int, + robot_id: int, env: Optional[SSLStandardEnv] = None, -) -> RobotCommand: - _, enemy, ball = game.friendly_robots, game.enemy_robots, game.ball - shooters_data = find_likely_enemy_shooter(enemy, ball) - orientation = None - tracking_ball = False - - if not shooters_data: - target_tracking_coord = ball.p.to_2d() - if ball.v is not None and None not in ball.v: - orientation = velocity_to_orientation(ball.v.to_2d()) - tracking_ball = True +): + defenseing_friendly = game.friendly_robots[robot_id] + vel = game.ball.v.to_2d() + if len(game.friendly_robots) > 2: + if game.ball.p.y >= -0.5 and robot_id == 1: + target_pos = [3.0 if game.my_team_is_right else -3.0, -1.2] + return go_to_point(game, motion_controller, robot_id, Vector2D(target_pos[0], target_pos[1])) + elif game.ball.p.y < -0.5 and robot_id == 2: + target_pos = [3.0 if game.my_team_is_right else -3.0, 1.2] + return go_to_point(game, motion_controller, robot_id, Vector2D(target_pos[0], target_pos[1])) + if robot_id == 1: + goal_frame = 0.5 + else: + goal_frame = -0.5 + + def positions_to_defend_parameter(x2, y2): + x1, y1 = game.ball.p.x, game.ball.p.y + x3, y3 = defenseing_friendly.p.x, defenseing_friendly.p.y + t = ((x3 - x1) * (x2 - x1) + (y3 - y1) * (y2 - y1)) / ((x2 - x1) ** 2 + (y2 - y1) ** 2) + x4 = x1 + t * (x2 - x1) + y4 = y1 + t * (y2 - y1) + + def cal_xy5(xa, ya, xb, yb, w, x4, y4): + x5 = xa - w + dx = xb - xa + if abs(dx) < 1e-12: + return x4, y4 + t = (x5 - xa) / dx + y5 = ya + t * (yb - ya) + return x5, y5 + + def distance(x1, y1, x2, y2): + return math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) + + if abs(x2 - x4) < 1 and -1 < y4 < 1: + hori_x, hori_y = cal_xy5(x2, y2, x1, y1, 1.0, x4, y4) + ver_x, ver_y = cal_xy5(y2, x2, y1, x1, 2.0, x4, y4) + if distance(x4, y4, hori_x, hori_y) < distance(x4, y4, ver_x, ver_y): + x4, y4 = hori_x, hori_y + else: + x4, y4 = ver_y, ver_x + + return x3, y3, x4, y4 + + if vel[0] ** 2 + vel[1] ** 2 > 0.05: + x2, y2 = 4.5 if game.my_team_is_right else -4.5, goal_frame + 0.2 if robot_id == 1 else goal_frame - 0.2 + x3, y3, x4, y4 = positions_to_defend_parameter(x2, y2) + target_pos = np.array([x4, y4]) + else: - # TODO (deploy more defenders, or find closest shooter?) - sd = shooters_data[0] - target_tracking_coord = Vector2D(sd.p.x, sd.p.y) - orientation = sd.orientation - - real_def_pos = game.friendly_robots[defender_id].p - current_def_parametric = to_defense_parametric(game, real_def_pos) - target = align_defenders(game, current_def_parametric, target_tracking_coord, orientation, env) - cmd = go_to_point( - game, - motion_controller, - defender_id, - target, - dribbling=True, - ) - - gp = (game.field.my_goal_line[0][0], 0) - if env: - env.draw_line( - [gp, (target_tracking_coord[0], target_tracking_coord[1])], - width=5, - color="RED" if tracking_ball else "PINK", + robot_rad = 0.09 + x2, y2 = 4.5 if game.my_team_is_right else -4.5, -goal_frame + x3, y3, x4, y4 = positions_to_defend_parameter(x2, y2) + vec_to_target = np.array( + [ + x4 - x3, + y4 - y3, + ] ) + dist_to_target = np.linalg.norm(vec_to_target) + + if dist_to_target > 0: + vec_dir = vec_to_target / dist_to_target + else: + vec_dir = np.array([0.0, 0.0]) + target_pos = np.array([x4, y4]) - vec_dir * robot_rad - return cmd + return go_to_point(game, motion_controller, robot_id, Vector2D(target_pos[0], target_pos[1]), dribbling=True) diff --git a/utama_core/skills/src/goalkeep.py b/utama_core/skills/src/goalkeep.py index c57ae3ac..65f692d2 100644 --- a/utama_core/skills/src/goalkeep.py +++ b/utama_core/skills/src/goalkeep.py @@ -17,25 +17,50 @@ def goalkeep( robot_id: int, env: Optional[SSLStandardEnv] = None, ): - goalie_obj = game.friendly_robots[robot_id] - if goalie_obj.has_ball: - target_oren = np.pi if game.my_team_is_right else 0 - return move( - game, - motion_controller, - robot_id, - Vector2D((4 if game.my_team_is_right else -4), 0), - target_oren, - True, - ) - if game.my_team_is_right: target = predict_ball_pos_at_x(game, 4.5) else: target = predict_ball_pos_at_x(game, -4.5) + stop_y = 0.0 + + def intersection_with_vertical_line(a, b, x_line=4.5): + xa, ya = a + xb, yb = b + if xb < xa: + return a + + k = (yb - ya) / (xb - xa) + y_intersect = ya + k * (x_line - xa) + if y_intersect < -0.5: + return (x_line, -0.5) + elif y_intersect > 0.5: + return (x_line, 0.5) + return (x_line, y_intersect) + + if len(game.friendly_robots) == 2: + try: + _, yy = intersection_with_vertical_line( + (game.ball.p.x, game.ball.p.y), (game.friendly_robots[1].p.x, game.friendly_robots[1].p.y + 0.1) + ) + stop_y = (yy + 0.5) / 2 + except (IndexError, KeyError): + # If robot with ID 1 is not available, keep default stop_y + pass + elif len(game.friendly_robots) >= 3: + try: + _, yy1 = intersection_with_vertical_line( + (game.ball.p.x, game.ball.p.y), (game.friendly_robots[1].p.x, game.friendly_robots[1].p.y + 0.1) + ) + _, yy2 = intersection_with_vertical_line( + (game.ball.p.x, game.ball.p.y), (game.friendly_robots[2].p.x, game.friendly_robots[2].p.y - 0.1) + ) + stop_y = (yy1 + yy2) / 2 + except (IndexError, KeyError): + # If robots with IDs 1 or 2 are not available, keep existing stop_y + pass if not target or abs(target[1]) > 0.5: - target = Vector2D(4.5 if game.my_team_is_right else -4.5, 0) + target = Vector2D(4.5 if game.my_team_is_right else -4.5, stop_y) # shooters_data = find_likely_enemy_shooter(game.enemy_robots, game.ball)