From 8733d80e759ac266ce5503852931e7b362d23fe3 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Fri, 14 Nov 2025 18:02:27 +0000 Subject: [PATCH 01/48] dribbling --- .../src/ssl/envs/standard_ssl.py | 117 +++++++++++++++++- 1 file changed, 115 insertions(+), 2 deletions(-) diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index 5eefaad2..2021b00b 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -1,4 +1,5 @@ import logging +import math import random from typing import Tuple @@ -101,16 +102,128 @@ def __init__( self.blue_formation = LEFT_START_ONE if not blue_starting_formation else blue_starting_formation self.yellow_formation = RIGHT_START_ONE if not yellow_starting_formation else yellow_starting_formation + # Track dribbler state across steps so we can model ball release when + # the dribbler turns off in a way that depends on robot speed. + self.prev_dribbler_blue = [False] * self.n_robots_blue + self.prev_dribbler_yellow = [False] * self.n_robots_yellow + logger.info(f"{n_robots_blue}v{n_robots_yellow} SSL Environment Initialized") def reset(self, *, seed=None, options=None): self.reward_shaping_total = None + # Reset dribbler tracking state + self.prev_dribbler_blue = [False] * self.n_robots_blue + self.prev_dribbler_yellow = [False] * self.n_robots_yellow return super().reset(seed=seed, options=options) def step(self, action): - observation, reward, terminated, truncated, _ = super().step(action) + """ + Advance the simulation by one step while modelling ball release when the + dribbler turns off. The ball's release speed is proportional to the + robot's current speed and capped to a realistic maximum. + """ + # Increment step counter and build low-level simulator commands + self.steps += 1 + commands = self._get_commands(action) + + # Send command to simulator + self.rsim.send_commands(commands) + self.sent_commands = commands + + # Get Frame from simulator + prev_prev_frame = self.last_frame + prev_frame = self.frame + self.last_frame = self.frame + self.frame = self.rsim.get_frame() + + # Derive current dribbler commands from actions (blue first, then yellow) + n_blue = self.n_robots_blue + curr_dribbler_blue = [cmd.dribbler for cmd in commands[:n_blue]] + curr_dribbler_yellow = [cmd.dribbler for cmd in commands[n_blue:]] + + # When the dribbler falls from True -> False while the robot had the ball + # in the previous frame, give the ball a small velocity in the direction + # of the robot's motion. This approximates the ball "rolling away" after + # release, as in grSim. + if prev_frame is not None and prev_prev_frame is not None: + applied = False + dt = self.time_step if self.time_step > 0 else 1.0 + + # Check yellow robots first + for i in range(self.n_robots_yellow): + if self.prev_dribbler_yellow[i] and not curr_dribbler_yellow[i]: + # Use motion between the two previous frames (when the + # dribbler was still on) to estimate release speed. + prev_robot_prev = prev_prev_frame.robots_yellow[i] + prev_robot_curr = prev_frame.robots_yellow[i] + if getattr(prev_robot_curr, "infrared", False): + applied = self._apply_ball_release(prev_robot_prev, prev_robot_curr, dt) + if applied: + break + + # If no yellow release was applied, check blue robots + if not applied: + for i in range(self.n_robots_blue): + if self.prev_dribbler_blue[i] and not curr_dribbler_blue[i]: + prev_robot_prev = prev_prev_frame.robots_blue[i] + prev_robot_curr = prev_frame.robots_blue[i] + if getattr(prev_robot_curr, "infrared", False): + applied = self._apply_ball_release(prev_robot_prev, prev_robot_curr, dt) + if applied: + break + + # Update dribbler history for the next step + self.prev_dribbler_blue = curr_dribbler_blue + self.prev_dribbler_yellow = curr_dribbler_yellow + + # Calculate environment observation, reward and done condition + observation = self._frame_to_observations() + reward, done = self._calculate_reward_and_done() + if self.render_mode == "human": + self.render() + + # Expose reward shaping totals via the info dict (for compatibility + # with existing callers of SSLStandardEnv) + return observation, reward, done, False, self.reward_shaping_total + + def _apply_ball_release(self, prev_robot: Robot, curr_robot: Robot, dt: float) -> bool: + """ + Apply a release impulse to the ball based on the robot's linear speed. + + The ball's speed is proportional to the robot speed and capped to a + reasonable maximum so that it behaves realistically in the simulator. + """ + if dt <= 0: + return False + + # Estimate robot linear velocity in simulator coordinates (m/s) + vx = (curr_robot.x - prev_robot.x) / dt + vy = (curr_robot.y - prev_robot.y) / dt + speed = math.hypot(vx, vy) + + # Ignore tiny movements to avoid numerical noise causing releases + MIN_RELEASE_SPEED = 0.1 # m/s + if speed < MIN_RELEASE_SPEED: + return False + + # Map robot speed to ball release speed. We use a gain < 1 so the ball + # moves slightly slower than the robot, and cap to a realistic upper + # bound to prevent extreme velocities. + RELEASE_GAIN = 0.8 + MAX_BALL_SPEED = 3.0 # m/s + + ball_speed = min(RELEASE_GAIN * speed, MAX_BALL_SPEED) + ux = vx / speed + uy = vy / speed + + ball = self.frame.ball + ball.v_x = ux * ball_speed + ball.v_y = uy * ball_speed + self.frame.ball = ball - return observation, reward, terminated, truncated, self.reward_shaping_total + # Commit the new ball velocity to the underlying simulator state + self.rsim.reset(self.frame) + return True def _frame_to_observations( self, From 306e651e2617865557523a33773430874368eaed Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Mon, 17 Nov 2025 01:42:54 +0000 Subject: [PATCH 02/48] improved logic --- .../src/ssl/envs/standard_ssl.py | 118 +++++++----------- 1 file changed, 42 insertions(+), 76 deletions(-) diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index 2021b00b..8cbb623a 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -131,8 +131,6 @@ def step(self, action): self.sent_commands = commands # Get Frame from simulator - prev_prev_frame = self.last_frame - prev_frame = self.frame self.last_frame = self.frame self.frame = self.rsim.get_frame() @@ -141,37 +139,6 @@ def step(self, action): curr_dribbler_blue = [cmd.dribbler for cmd in commands[:n_blue]] curr_dribbler_yellow = [cmd.dribbler for cmd in commands[n_blue:]] - # When the dribbler falls from True -> False while the robot had the ball - # in the previous frame, give the ball a small velocity in the direction - # of the robot's motion. This approximates the ball "rolling away" after - # release, as in grSim. - if prev_frame is not None and prev_prev_frame is not None: - applied = False - dt = self.time_step if self.time_step > 0 else 1.0 - - # Check yellow robots first - for i in range(self.n_robots_yellow): - if self.prev_dribbler_yellow[i] and not curr_dribbler_yellow[i]: - # Use motion between the two previous frames (when the - # dribbler was still on) to estimate release speed. - prev_robot_prev = prev_prev_frame.robots_yellow[i] - prev_robot_curr = prev_frame.robots_yellow[i] - if getattr(prev_robot_curr, "infrared", False): - applied = self._apply_ball_release(prev_robot_prev, prev_robot_curr, dt) - if applied: - break - - # If no yellow release was applied, check blue robots - if not applied: - for i in range(self.n_robots_blue): - if self.prev_dribbler_blue[i] and not curr_dribbler_blue[i]: - prev_robot_prev = prev_prev_frame.robots_blue[i] - prev_robot_curr = prev_frame.robots_blue[i] - if getattr(prev_robot_curr, "infrared", False): - applied = self._apply_ball_release(prev_robot_prev, prev_robot_curr, dt) - if applied: - break - # Update dribbler history for the next step self.prev_dribbler_blue = curr_dribbler_blue self.prev_dribbler_yellow = curr_dribbler_yellow @@ -186,45 +153,6 @@ def step(self, action): # with existing callers of SSLStandardEnv) return observation, reward, done, False, self.reward_shaping_total - def _apply_ball_release(self, prev_robot: Robot, curr_robot: Robot, dt: float) -> bool: - """ - Apply a release impulse to the ball based on the robot's linear speed. - - The ball's speed is proportional to the robot speed and capped to a - reasonable maximum so that it behaves realistically in the simulator. - """ - if dt <= 0: - return False - - # Estimate robot linear velocity in simulator coordinates (m/s) - vx = (curr_robot.x - prev_robot.x) / dt - vy = (curr_robot.y - prev_robot.y) / dt - speed = math.hypot(vx, vy) - - # Ignore tiny movements to avoid numerical noise causing releases - MIN_RELEASE_SPEED = 0.1 # m/s - if speed < MIN_RELEASE_SPEED: - return False - - # Map robot speed to ball release speed. We use a gain < 1 so the ball - # moves slightly slower than the robot, and cap to a realistic upper - # bound to prevent extreme velocities. - RELEASE_GAIN = 0.8 - MAX_BALL_SPEED = 3.0 # m/s - - ball_speed = min(RELEASE_GAIN * speed, MAX_BALL_SPEED) - ux = vx / speed - uy = vy / speed - - ball = self.frame.ball - ball.v_x = ux * ball_speed - ball.v_y = uy * ball_speed - self.frame.ball = ball - - # Commit the new ball velocity to the underlying simulator state - self.rsim.reset(self.frame) - return True - def _frame_to_observations( self, ) -> Tuple[RawVisionData, RobotResponse, RobotResponse]: @@ -274,33 +202,71 @@ def _get_robot_observation(self, robot): def _get_commands(self, actions) -> list[Robot]: commands = [] + # Use current frame information (robot velocities and infrared) to + # model a ball release when the dribbler turns off on this step. + frame = self.frame + + # Constants for mapping robot speed to ball release speed + MIN_RELEASE_SPEED = 0.1 # m/s + RELEASE_GAIN = 0.8 + MAX_BALL_SPEED = 3.0 # m/s + + # Blue robots for i in range(self.n_robots_blue): v_x = actions["team_blue"][i][0] v_y = actions["team_blue"][i][1] v_theta = actions["team_blue"][i][2] + + dribbler = actions["team_blue"][i][4] > 0 + base_kick_v_x = KICK_SPD if actions["team_blue"][i][3] > 0 else 0.0 + release_kick_v_x = 0.0 + + # If the dribbler is turning off this step and the robot had the + # ball in the previous frame, approximate a release by sending a + # kick whose speed depends on the robot's linear speed. + if frame is not None and self.prev_dribbler_blue[i] and not dribbler: + robot_state = frame.robots_blue[i] + if getattr(robot_state, "infrared", False): + speed = math.hypot(robot_state.v_x, robot_state.v_y) + if speed >= MIN_RELEASE_SPEED: + release_kick_v_x = min(RELEASE_GAIN * speed, MAX_BALL_SPEED) + cmd = Robot( yellow=False, # Blue team id=i, # ID of the robot v_x=v_x, v_y=v_y, v_theta=v_theta, - kick_v_x=KICK_SPD if actions["team_blue"][i][3] > 0 else 0.0, - dribbler=True if actions["team_blue"][i][4] > 0 else False, + kick_v_x=max(base_kick_v_x, release_kick_v_x), + dribbler=dribbler, ) commands.append(cmd) + # Yellow robots for i in range(self.n_robots_yellow): v_x = actions["team_yellow"][i][0] v_y = actions["team_yellow"][i][1] v_theta = actions["team_yellow"][i][2] + + dribbler = actions["team_yellow"][i][4] > 0 + base_kick_v_x = KICK_SPD if actions["team_yellow"][i][3] > 0 else 0.0 + release_kick_v_x = 0.0 + + if frame is not None and self.prev_dribbler_yellow[i] and not dribbler: + robot_state = frame.robots_yellow[i] + if getattr(robot_state, "infrared", False): + speed = math.hypot(robot_state.v_x, robot_state.v_y) + if speed >= MIN_RELEASE_SPEED: + release_kick_v_x = min(RELEASE_GAIN * speed, MAX_BALL_SPEED) + cmd = Robot( yellow=True, # Yellow team id=i, # ID of the robot v_x=v_x, v_y=v_y, v_theta=v_theta, - kick_v_x=KICK_SPD if actions["team_yellow"][i][3] > 0 else 0.0, - dribbler=True if actions["team_yellow"][i][4] > 0 else False, + kick_v_x=max(base_kick_v_x, release_kick_v_x), + dribbler=dribbler, ) commands.append(cmd) From f7aeeb1faa3a0e8af3e22cb978f74ac687ca5e48 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Mon, 17 Nov 2025 02:10:11 +0000 Subject: [PATCH 03/48] bug fix --- utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index 8cbb623a..4df16454 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -227,7 +227,7 @@ def _get_commands(self, actions) -> list[Robot]: if frame is not None and self.prev_dribbler_blue[i] and not dribbler: robot_state = frame.robots_blue[i] if getattr(robot_state, "infrared", False): - speed = math.hypot(robot_state.v_x, robot_state.v_y) + speed = math.hypot(v_x, v_y) if speed >= MIN_RELEASE_SPEED: release_kick_v_x = min(RELEASE_GAIN * speed, MAX_BALL_SPEED) @@ -255,7 +255,7 @@ def _get_commands(self, actions) -> list[Robot]: if frame is not None and self.prev_dribbler_yellow[i] and not dribbler: robot_state = frame.robots_yellow[i] if getattr(robot_state, "infrared", False): - speed = math.hypot(robot_state.v_x, robot_state.v_y) + speed = math.hypot(v_x, v_y) if speed >= MIN_RELEASE_SPEED: release_kick_v_x = min(RELEASE_GAIN * speed, MAX_BALL_SPEED) From c535696ca9553e5a0c9b0cd441a5289c35fb0475 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Mon, 17 Nov 2025 02:50:45 +0000 Subject: [PATCH 04/48] hot fix --- .../rsoccer_simulator/src/ssl/envs/standard_ssl.py | 11 +++++++---- 1 file changed, 7 insertions(+), 4 deletions(-) diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index 4df16454..691abd64 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -143,6 +143,9 @@ def step(self, action): self.prev_dribbler_blue = curr_dribbler_blue self.prev_dribbler_yellow = curr_dribbler_yellow + self.prev_speed_blue = [math.hypot(cmd.v_x, cmd.v_y) for cmd in commands[:n_blue]] + self.prev_speed_yellow = [math.hypot(cmd.v_x, cmd.v_y) for cmd in commands[n_blue:]] + # Calculate environment observation, reward and done condition observation = self._frame_to_observations() reward, done = self._calculate_reward_and_done() @@ -208,7 +211,7 @@ def _get_commands(self, actions) -> list[Robot]: # Constants for mapping robot speed to ball release speed MIN_RELEASE_SPEED = 0.1 # m/s - RELEASE_GAIN = 0.8 + RELEASE_GAIN = 0.7 MAX_BALL_SPEED = 3.0 # m/s # Blue robots @@ -227,7 +230,7 @@ def _get_commands(self, actions) -> list[Robot]: if frame is not None and self.prev_dribbler_blue[i] and not dribbler: robot_state = frame.robots_blue[i] if getattr(robot_state, "infrared", False): - speed = math.hypot(v_x, v_y) + speed = self.prev_speed_blue[i] if speed >= MIN_RELEASE_SPEED: release_kick_v_x = min(RELEASE_GAIN * speed, MAX_BALL_SPEED) @@ -255,12 +258,12 @@ def _get_commands(self, actions) -> list[Robot]: if frame is not None and self.prev_dribbler_yellow[i] and not dribbler: robot_state = frame.robots_yellow[i] if getattr(robot_state, "infrared", False): - speed = math.hypot(v_x, v_y) + speed = self.prev_speed_yellow[i] if speed >= MIN_RELEASE_SPEED: release_kick_v_x = min(RELEASE_GAIN * speed, MAX_BALL_SPEED) cmd = Robot( - yellow=True, # Yellow team + yellow=True, # Yellow team, id=i, # ID of the robot v_x=v_x, v_y=v_y, From 960a66c8d1522df4d0df4f07c3804ab0f9b379fc Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Mon, 17 Nov 2025 03:13:29 +0000 Subject: [PATCH 05/48] updates --- utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index 691abd64..a4da25e7 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -211,7 +211,7 @@ def _get_commands(self, actions) -> list[Robot]: # Constants for mapping robot speed to ball release speed MIN_RELEASE_SPEED = 0.1 # m/s - RELEASE_GAIN = 0.7 + RELEASE_GAIN = 0.1 MAX_BALL_SPEED = 3.0 # m/s # Blue robots From 1443eb8441ded0fb2be97cc3a9dcc5f5a2204dd9 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Thu, 20 Nov 2025 20:25:31 +0000 Subject: [PATCH 06/48] improvements --- utama_core/config/settings.py | 8 ++ utama_core/motion_planning/src/pid/pid.py | 28 +---- .../src/ssl/envs/standard_ssl.py | 116 +++++++++++------- .../controllers/real/real_robot_controller.py | 58 ++++----- 4 files changed, 110 insertions(+), 100 deletions(-) diff --git a/utama_core/config/settings.py b/utama_core/config/settings.py index b99f9a11..30e20dd1 100644 --- a/utama_core/config/settings.py +++ b/utama_core/config/settings.py @@ -22,11 +22,19 @@ REMOVAL_Y_COORD = -10 TELEPORT_X_COORDS = [0.4, 0.8, 1.2, 1.6, 2, 2.4] +### RSIM Dribbler Settings ### +MIN_RELEASE_SPEED = 0.1 # m/s +RELEASE_GAIN = 0.1 +MAX_BALL_SPEED = 3.0 # m/s + ### REAL CONTROLLER SETTINGS ### BAUD_RATE = 115200 PORT = "/dev/ttyACM0" TIMEOUT = 0.1 +CMD_MAX_VELOCITY = 4.0 # m/s +CMD_MAX_ANGULAR_VELOCITY = 8 # rad/s + MAX_GAME_HISTORY = 20 REPLAY_BASE_PATH = Path.cwd() / "replays" diff --git a/utama_core/motion_planning/src/pid/pid.py b/utama_core/motion_planning/src/pid/pid.py index a5535794..85d49907 100644 --- a/utama_core/motion_planning/src/pid/pid.py +++ b/utama_core/motion_planning/src/pid/pid.py @@ -80,8 +80,6 @@ def __init__( self.integral_min = config.integral_min self.integral_max = config.integral_max - self.prev_times = {i: 0.0 for i in range(6)} - self.first_pass = {i: True for i in range(6)} def calculate( @@ -94,25 +92,17 @@ def calculate( The delay is compensated by predicting the current value using the derivative. """ - call_func_time = time.time() # Compute the basic (instantaneous) error raw_error = target - current # For angular measurements adjust error error = normalise_heading(raw_error) # For very small errors, return zero if abs(error) < 0.001: - self.prev_times[robot_id] = call_func_time return 0.0 - # Compute time difference - dt = self.dt # Default - if self.prev_times[robot_id] != 0: - measured_dt = call_func_time - self.prev_times[robot_id] - dt = measured_dt if measured_dt > 0 else self.dt - # Compute derivative term using the previous stored error if not self.first_pass[robot_id]: - derivative = (error - self.pre_errors[robot_id]) / dt + derivative = (error - self.pre_errors[robot_id]) / self.dt else: derivative = 0.0 self.first_pass[robot_id] = False @@ -157,7 +147,6 @@ def calculate( # Store the error and update the time for the next iteration self.pre_errors[robot_id] = error - self.prev_times[robot_id] = call_func_time # print(f"oren PID: {robot_id}, current:{current}, target: {target}, error: {error}, output: {output}") return output @@ -194,32 +183,20 @@ def __init__( self.integral_min = config.integral_min self.integral_max = config.integral_max - self.prev_times = {i: 0.0 for i in range(6)} - self.first_pass = {i: True for i in range(6)} def calculate(self, target: Vector2D, current: Vector2D, robot_id: int) -> Vector2D: - call_func_time = time.time() - dx = target[0] - current[0] dy = target[1] - current[1] error = math.hypot(dx, dy) if abs(error) < 3 / 1000: - self.prev_times[robot_id] = call_func_time return Vector2D(0.0, 0.0) - # Compute time difference - dt = self.dt - if self.prev_times[robot_id] != 0: - measured_dt = call_func_time - self.prev_times[robot_id] - # Use the measured dt if nonzero; otherwise fall back to self.dt. - dt = measured_dt if measured_dt > 0 else self.dt - # Compute derivative term using the previous stored error if not self.first_pass[robot_id]: - derivative = (error - self.pre_errors[robot_id]) / dt + derivative = (error - self.pre_errors[robot_id]) / self.dt else: derivative = 0.0 self.first_pass[robot_id] = False @@ -255,7 +232,6 @@ def calculate(self, target: Vector2D, current: Vector2D, robot_id: int) -> Vecto # Store the error and update the time for the next iteration self.pre_errors[robot_id] = error - self.prev_times[robot_id] = call_func_time # print(f"x-y PID: {robot_id}, current:{current}, target: {target}, error: {error}, output: {output}") if error == 0.0: diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index a4da25e7..c1a2d66d 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -8,7 +8,12 @@ from utama_core.config.formations import LEFT_START_ONE, RIGHT_START_ONE from utama_core.config.robot_params.rsim import KICK_SPD -from utama_core.config.settings import TIMESTEP +from utama_core.config.settings import ( + MAX_BALL_SPEED, + MIN_RELEASE_SPEED, + RELEASE_GAIN, + TIMESTEP, +) from utama_core.entities.data.command import RobotResponse from utama_core.entities.data.raw_vision import RawBallData, RawRobotData, RawVisionData from utama_core.global_utils.math_utils import deg_to_rad, rad_to_deg @@ -106,6 +111,8 @@ def __init__( # the dribbler turns off in a way that depends on robot speed. self.prev_dribbler_blue = [False] * self.n_robots_blue self.prev_dribbler_yellow = [False] * self.n_robots_yellow + self.prev_speed_blue = [0.0] * self.n_robots_blue + self.prev_speed_yellow = [0.0] * self.n_robots_yellow logger.info(f"{n_robots_blue}v{n_robots_yellow} SSL Environment Initialized") @@ -114,6 +121,8 @@ def reset(self, *, seed=None, options=None): # Reset dribbler tracking state self.prev_dribbler_blue = [False] * self.n_robots_blue self.prev_dribbler_yellow = [False] * self.n_robots_yellow + self.prev_speed_blue = [0.0] * self.n_robots_blue + self.prev_speed_yellow = [0.0] * self.n_robots_yellow return super().reset(seed=seed, options=options) def step(self, action): @@ -126,25 +135,17 @@ def step(self, action): self.steps += 1 commands = self._get_commands(action) + # Apply any implicit kicks caused by dribbler state transitions. + self._apply_dribbler_release_kicks(commands) + # Send command to simulator self.rsim.send_commands(commands) self.sent_commands = commands - # Get Frame from simulator + # Get Frame from simulator and update dribbler/speed history self.last_frame = self.frame self.frame = self.rsim.get_frame() - - # Derive current dribbler commands from actions (blue first, then yellow) - n_blue = self.n_robots_blue - curr_dribbler_blue = [cmd.dribbler for cmd in commands[:n_blue]] - curr_dribbler_yellow = [cmd.dribbler for cmd in commands[n_blue:]] - - # Update dribbler history for the next step - self.prev_dribbler_blue = curr_dribbler_blue - self.prev_dribbler_yellow = curr_dribbler_yellow - - self.prev_speed_blue = [math.hypot(cmd.v_x, cmd.v_y) for cmd in commands[:n_blue]] - self.prev_speed_yellow = [math.hypot(cmd.v_x, cmd.v_y) for cmd in commands[n_blue:]] + self._update_dribbler_history(commands) # Calculate environment observation, reward and done condition observation = self._frame_to_observations() @@ -205,15 +206,6 @@ def _get_robot_observation(self, robot): def _get_commands(self, actions) -> list[Robot]: commands = [] - # Use current frame information (robot velocities and infrared) to - # model a ball release when the dribbler turns off on this step. - frame = self.frame - - # Constants for mapping robot speed to ball release speed - MIN_RELEASE_SPEED = 0.1 # m/s - RELEASE_GAIN = 0.1 - MAX_BALL_SPEED = 3.0 # m/s - # Blue robots for i in range(self.n_robots_blue): v_x = actions["team_blue"][i][0] @@ -221,18 +213,7 @@ def _get_commands(self, actions) -> list[Robot]: v_theta = actions["team_blue"][i][2] dribbler = actions["team_blue"][i][4] > 0 - base_kick_v_x = KICK_SPD if actions["team_blue"][i][3] > 0 else 0.0 - release_kick_v_x = 0.0 - - # If the dribbler is turning off this step and the robot had the - # ball in the previous frame, approximate a release by sending a - # kick whose speed depends on the robot's linear speed. - if frame is not None and self.prev_dribbler_blue[i] and not dribbler: - robot_state = frame.robots_blue[i] - if getattr(robot_state, "infrared", False): - speed = self.prev_speed_blue[i] - if speed >= MIN_RELEASE_SPEED: - release_kick_v_x = min(RELEASE_GAIN * speed, MAX_BALL_SPEED) + kick_v_x = KICK_SPD if actions["team_blue"][i][3] > 0 else 0.0 cmd = Robot( yellow=False, # Blue team @@ -240,7 +221,7 @@ def _get_commands(self, actions) -> list[Robot]: v_x=v_x, v_y=v_y, v_theta=v_theta, - kick_v_x=max(base_kick_v_x, release_kick_v_x), + kick_v_x=kick_v_x, dribbler=dribbler, ) commands.append(cmd) @@ -252,15 +233,7 @@ def _get_commands(self, actions) -> list[Robot]: v_theta = actions["team_yellow"][i][2] dribbler = actions["team_yellow"][i][4] > 0 - base_kick_v_x = KICK_SPD if actions["team_yellow"][i][3] > 0 else 0.0 - release_kick_v_x = 0.0 - - if frame is not None and self.prev_dribbler_yellow[i] and not dribbler: - robot_state = frame.robots_yellow[i] - if getattr(robot_state, "infrared", False): - speed = self.prev_speed_yellow[i] - if speed >= MIN_RELEASE_SPEED: - release_kick_v_x = min(RELEASE_GAIN * speed, MAX_BALL_SPEED) + kick_v_x = KICK_SPD if actions["team_yellow"][i][3] > 0 else 0.0 cmd = Robot( yellow=True, # Yellow team, @@ -268,13 +241,64 @@ def _get_commands(self, actions) -> list[Robot]: v_x=v_x, v_y=v_y, v_theta=v_theta, - kick_v_x=max(base_kick_v_x, release_kick_v_x), + kick_v_x=kick_v_x, dribbler=dribbler, ) commands.append(cmd) return commands + def _apply_dribbler_release_kicks(self, commands: list[Robot]) -> None: + """Approximate kicks when dribblers turn off for robots that had possession. + + This mutates the provided ``commands`` list in-place, potentially + increasing ``kick_v_x`` for robots whose dribbler transitioned from + on to off while they were moving with the ball. + """ + prior_frame = self.frame + blue_states = prior_frame.robots_blue if prior_frame is not None else None + yellow_states = prior_frame.robots_yellow if prior_frame is not None else None + + n_blue = self.n_robots_blue + for i in range(n_blue): + release = self._dribbler_release_kick( + blue_states, self.prev_dribbler_blue, self.prev_speed_blue, i, commands[i].dribbler + ) + if release > 0.0: + commands[i].kick_v_x = max(commands[i].kick_v_x, release) + + for j in range(self.n_robots_yellow): + cmd_idx = n_blue + j + release = self._dribbler_release_kick( + yellow_states, self.prev_dribbler_yellow, self.prev_speed_yellow, j, commands[cmd_idx].dribbler + ) + if release > 0.0: + commands[cmd_idx].kick_v_x = max(commands[cmd_idx].kick_v_x, release) + + def _update_dribbler_history(self, commands: list[Robot]) -> None: + """Update previous dribbler and speed history for all robots.""" + n_blue = self.n_robots_blue + self.prev_dribbler_blue = [cmd.dribbler for cmd in commands[:n_blue]] + self.prev_dribbler_yellow = [cmd.dribbler for cmd in commands[n_blue:]] + + self.prev_speed_blue = [math.hypot(cmd.v_x, cmd.v_y) for cmd in commands[:n_blue]] + self.prev_speed_yellow = [math.hypot(cmd.v_x, cmd.v_y) for cmd in commands[n_blue:]] + + def _dribbler_release_kick(self, robot_states, prev_dribbler, prev_speed, index, dribbler): + """Estimate the kick needed to release the ball when the dribbler turns off.""" + if robot_states is None or not prev_dribbler[index] or dribbler: + return 0.0 + + robot_state = robot_states[index] + if not getattr(robot_state, "infrared", False): + return 0.0 + + speed = prev_speed[index] + if speed < MIN_RELEASE_SPEED: + return 0.0 + + return min(RELEASE_GAIN * speed, MAX_BALL_SPEED) + def _calculate_reward_and_done(self): if self.reward_shaping_total is None: # Initialize reward shaping dictionary (info) diff --git a/utama_core/team_controller/src/controllers/real/real_robot_controller.py b/utama_core/team_controller/src/controllers/real/real_robot_controller.py index 71c79d2e..69a63a3e 100644 --- a/utama_core/team_controller/src/controllers/real/real_robot_controller.py +++ b/utama_core/team_controller/src/controllers/real/real_robot_controller.py @@ -5,8 +5,14 @@ import numpy as np from serial import EIGHTBITS, PARITY_EVEN, STOPBITS_TWO, Serial -from utama_core.config.robot_params.real import MAX_ANGULAR_VEL, MAX_VEL -from utama_core.config.settings import BAUD_RATE, PORT, TIMEOUT +# from utama_core.config.robot_params.real import MAX_ANGULAR_VEL, MAX_VEL +from utama_core.config.settings import ( + BAUD_RATE, + CMD_MAX_ANGULAR_VELOCITY, + CMD_MAX_VELOCITY, + PORT, + TIMEOUT, +) from utama_core.entities.data.command import ( RobotCommand, RobotPacketCommand, @@ -72,7 +78,6 @@ def _add_robot_command(self, command: RobotCommand, robot_id: int) -> None: """ c_command = self._convert_uint16_command(robot_id, command) command_buffer = self._generate_command_buffer(robot_id, c_command) - print(command_buffer) start_idx = robot_id * self._rbt_cmd_size + 1 # account for the start frame byte self._out_packet[start_idx : start_idx + self._rbt_cmd_size] = ( command_buffer # +1 to account for start frame byte @@ -146,31 +151,29 @@ def _convert_uint16_command(self, robot_id, command: RobotCommand) -> RobotPacke Also converts angular velocity to degrees per second. """ - print(command) angular_vel = command.angular_vel local_forward_vel = command.local_forward_vel local_left_vel = command.local_left_vel - if abs(command.angular_vel) > MAX_ANGULAR_VEL: + if abs(command.angular_vel) > CMD_MAX_ANGULAR_VELOCITY: warnings.warn( - f"Angular velocity for robot {robot_id} is greater than the maximum angular velocity. Clipping to {MAX_ANGULAR_VEL}." + f"Angular velocity for robot {robot_id} is greater than the maximum angular velocity. Clipping to {CMD_MAX_ANGULAR_VELOCITY}." ) - angular_vel = MAX_ANGULAR_VEL if command.angular_vel > 0 else -MAX_ANGULAR_VEL - if abs(command.local_forward_vel) > MAX_VEL: + angular_vel = CMD_MAX_ANGULAR_VELOCITY if command.angular_vel > 0 else -CMD_MAX_ANGULAR_VELOCITY + if abs(command.local_forward_vel) > CMD_MAX_VELOCITY: warnings.warn( - f"Local forward velocity for robot {robot_id} is greater than the maximum velocity. Clipping to {MAX_VEL}." + f"Local forward velocity for robot {robot_id} is greater than the maximum velocity. Clipping to {CMD_MAX_VELOCITY}." ) - local_forward_vel = MAX_VEL if command.local_forward_vel > 0 else -MAX_VEL - - if abs(command.local_left_vel) > MAX_VEL: + local_forward_vel = CMD_MAX_VELOCITY if command.local_forward_vel > 0 else -CMD_MAX_VELOCITY + if abs(command.local_left_vel) > CMD_MAX_VELOCITY: warnings.warn( - f"Local left velocity for robot {robot_id} is greater than the maximum velocity. Clipping to {MAX_VEL}." + f"Local left velocity for robot {robot_id} is greater than the maximum velocity. Clipping to {CMD_MAX_VELOCITY}." ) - local_left_vel = MAX_VEL if command.local_left_vel > 0 else -MAX_VEL + local_left_vel = CMD_MAX_VELOCITY if command.local_left_vel > 0 else -CMD_MAX_VELOCITY - local_forward_vel = self._encode_signed_to_u16(local_forward_vel, MAX_VEL) - local_left_vel = self._encode_signed_to_u16(local_left_vel, MAX_VEL) - angular_vel = self._encode_signed_to_u16(angular_vel, MAX_ANGULAR_VEL) + local_forward_vel = self._encode_signed_to_u16(local_forward_vel, CMD_MAX_VELOCITY) + local_left_vel = self._encode_signed_to_u16(local_left_vel, CMD_MAX_VELOCITY) + angular_vel = self._encode_signed_to_u16(angular_vel, CMD_MAX_ANGULAR_VELOCITY) command = RobotPacketCommand( local_forward_vel=self._uint16_rep(local_forward_vel), @@ -192,24 +195,23 @@ def _convert_float_command(self, robot_id, command: RobotCommand) -> RobotComman local_forward_vel = command.local_forward_vel local_left_vel = command.local_left_vel - if abs(command.angular_vel) > MAX_ANGULAR_VEL: + if abs(command.angular_vel) > CMD_MAX_ANGULAR_VELOCITY: warnings.warn( - f"Angular velocity for robot {robot_id} is greater than the maximum angular velocity. Clipping to {MAX_ANGULAR_VEL}." + f"Angular velocity for robot {robot_id} is greater than the maximum angular velocity. Clipping to {CMD_MAX_ANGULAR_VELOCITY}." ) - angular_vel = MAX_ANGULAR_VEL if command.angular_vel > 0 else -MAX_ANGULAR_VEL - # TODO put back to max_vel - if abs(command.local_forward_vel) > 0.8: + angular_vel = CMD_MAX_ANGULAR_VELOCITY if command.angular_vel > 0 else -CMD_MAX_ANGULAR_VELOCITY + + if abs(command.local_forward_vel) > CMD_MAX_VELOCITY: warnings.warn( - f"Local forward velocity for robot {robot_id} is greater than the maximum velocity. Clipping to {MAX_VEL}." + f"Local forward velocity for robot {robot_id} is greater than the maximum velocity. Clipping to {CMD_MAX_VELOCITY}." ) - local_forward_vel = MAX_VEL if command.local_forward_vel > 0 else -MAX_VEL + local_forward_vel = CMD_MAX_VELOCITY if command.local_forward_vel > 0 else -CMD_MAX_VELOCITY - if abs(command.local_left_vel) > MAX_VEL: + if abs(command.local_left_vel) > CMD_MAX_VELOCITY: warnings.warn( - f"Local left velocity for robot {robot_id} is greater than the maximum velocity. Clipping to {MAX_VEL}." + f"Local left velocity for robot {robot_id} is greater than the maximum velocity. Clipping to {CMD_MAX_VELOCITY}." ) - local_left_vel = MAX_VEL if command.local_left_vel > 0 else -MAX_VEL - + local_left_vel = CMD_MAX_VELOCITY if command.local_left_vel > 0 else -CMD_MAX_VELOCITY command = RobotCommand( local_forward_vel=self._float16_rep(local_forward_vel), local_left_vel=self._float16_rep(local_left_vel), From 6992fff97775809792ecb202ae1714af42d77d82 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Thu, 20 Nov 2025 20:33:16 +0000 Subject: [PATCH 07/48] typing --- .../rsoccer_simulator/src/ssl/envs/standard_ssl.py | 13 ++++++++++--- 1 file changed, 10 insertions(+), 3 deletions(-) diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index c1a2d66d..33321e34 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -1,7 +1,7 @@ import logging import math import random -from typing import Tuple +from typing import Dict, List, Optional, Tuple import gymnasium as gym import numpy as np @@ -236,7 +236,7 @@ def _get_commands(self, actions) -> list[Robot]: kick_v_x = KICK_SPD if actions["team_yellow"][i][3] > 0 else 0.0 cmd = Robot( - yellow=True, # Yellow team, + yellow=True, # Yellow team id=i, # ID of the robot v_x=v_x, v_y=v_y, @@ -284,7 +284,14 @@ def _update_dribbler_history(self, commands: list[Robot]) -> None: self.prev_speed_blue = [math.hypot(cmd.v_x, cmd.v_y) for cmd in commands[:n_blue]] self.prev_speed_yellow = [math.hypot(cmd.v_x, cmd.v_y) for cmd in commands[n_blue:]] - def _dribbler_release_kick(self, robot_states, prev_dribbler, prev_speed, index, dribbler): + def _dribbler_release_kick( + self, + robot_states: Optional[Dict[int, Robot]], + prev_dribbler: List[bool], + prev_speed: List[float], + index: int, + dribbler: bool, + ) -> float: """Estimate the kick needed to release the ball when the dribbler turns off.""" if robot_states is None or not prev_dribbler[index] or dribbler: return 0.0 From c41dcb5281724fbfe7b7f900592cdf067cad9cff Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Thu, 20 Nov 2025 20:39:06 +0000 Subject: [PATCH 08/48] optimizations to rsim --- .../src/ssl/envs/standard_ssl.py | 115 +----------------- 1 file changed, 1 insertion(+), 114 deletions(-) diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index 33321e34..d7caa969 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -98,11 +98,6 @@ def __init__( } ) - # Set scales for rewards - self.ball_dist_scale = np.linalg.norm([self.field.width, self.field.length / 2]) - self.ball_grad_scale = np.linalg.norm([self.field.width / 2, self.field.length / 2]) / 4 - self.energy_scale = (160 * 4) * 1000 # max wheel speed (rad/s) * 4 wheels * steps - # set starting formation style for self.blue_formation = LEFT_START_ONE if not blue_starting_formation else blue_starting_formation self.yellow_formation = RIGHT_START_ONE if not yellow_starting_formation else yellow_starting_formation @@ -307,104 +302,7 @@ def _dribbler_release_kick( return min(RELEASE_GAIN * speed, MAX_BALL_SPEED) def _calculate_reward_and_done(self): - if self.reward_shaping_total is None: - # Initialize reward shaping dictionary (info) - self.reward_shaping_total = { - "blue_team": { - "goal": 0, - "rbt_in_gk_area": 0, - "done_ball_out": 0, - "done_ball_out_right": 0, - "done_rbt_out": 0, - "energy": 0, - }, - "yellow_team": { - "conceded_goal": 0, - "rbt_in_gk_area": 0, - "done_ball_out": 0, - "done_ball_out_right": 0, - "done_rbt_out": 0, - "energy": 0, - }, - } - - # reward_blue = 0 - # reward_yellow = 0 - done = False - - # Field parameters - half_len = self.field.length / 2 - half_wid = self.field.width / 2 - pen_len = self.field.penalty_length - half_pen_wid = self.field.penalty_width / 2 - half_goal_wid = self.field.goal_width / 2 - - ball = self.frame.ball - - def robot_in_gk_area(rbt): - return abs(rbt.x) > half_len - pen_len and abs(rbt.y) < half_pen_wid - - # Check if any robot on the blue team exited field or violated rules (for info) - for (_, robot_b), (_, robot_y) in zip(self.frame.robots_blue.items(), self.frame.robots_yellow.items()): - if abs(robot_y.y) > half_wid or abs(robot_y.x) > half_len: - done = True - self.reward_shaping_total["blue_team"]["done_rbt_out"] += 1 - elif abs(robot_y.y) > half_wid or abs(robot_y.x) > half_len: - done = True - self.reward_shaping_total["yellow_team"]["done_rbt_out"] += 1 - elif robot_in_gk_area(robot_b): - done = True - self.reward_shaping_total["blue_team"]["rbt_in_gk_area"] += 1 - elif robot_in_gk_area(robot_y): - done = True - self.reward_shaping_total["yellow_team"]["rbt_in_gk_area"] += 1 - - # Check if ball exited field or a goal was made (if blue was attacking) - # TODO: Add reward shaping for yellow team (obtaining possession of the ball) - if abs(ball.y) > half_wid or abs(ball.x) > half_len: - done = True - self.reward_shaping_total["blue_team"]["done_ball_out"] += 1 - # if the ball is outside the attacking half for blue team (right half of the field) - elif ball.x > half_len: - done = True - # if the ball is inside the goal area otherwise it is a ball out from goalie line - if abs(ball.y) < half_goal_wid: - # reward_blue = 5 - # reward_yellow = -5 - self.reward_shaping_total["blue_team"]["goal"] += 1 - self.reward_shaping_total["yellow_team"]["conceded_goal"] += 1 - else: - # reward = 0 - self.reward_shaping_total["team_blue"]["done_ball_out_right"] += 1 - # elif self.last_frame is not None: - - # Example: Energy penalty for all blue robots - # total_energy_rw_b = 0 - # total_energy_rw_y = 0 - # for (_, robot_b), (_, robot_y) in zip( - # self.frame.robots_blue.items(), self.frame.robots_yellow.items() - # ): - # total_energy_rw_b += self.__energy_pen(robot_b) - # total_energy_rw_y += self.__energy_pen(robot_y) - - # avg_energy_rw_b = total_energy_rw_b / len(self.frame.robots_blue) - # avg_energy_rw_y = total_energy_rw_y / len(self.frame.robots_yellow) - - # energy_rw_b = -(avg_energy_rw_b / self.energy_scale) - # energy_rw_y = -(avg_energy_rw_y / self.energy_scale) - - # self.reward_shaping_total["blue_team"]["energy"] += energy_rw_b - # self.reward_shaping_total["yellow_team"]["energy"] += energy_rw_y - - # # Total reward (Scoring reward + Energy penalty v ) - # reward_blue = reward_blue + energy_rw_b - # reward_yellow = reward_yellow + energy_rw_y - - # reward = {"blue_team": reward_blue, "yellow_team": reward_yellow} - - reward = 0 # NB: We are not using reward for now - - return reward, done + return 0, False def _get_initial_positions_frame(self): """Returns the position of each robot and ball for the initial frame (random placement)""" @@ -471,14 +369,3 @@ def in_gk_area(obj): pos_frame.robots_yellow[i] = Robot(id=i, x=pos[0], y=pos[1], theta=theta()) return pos_frame - - # def __energy_pen(self, robot): - # # Sum of abs each wheel speed sent - # energy = ( - # abs(robot.v_wheel0) - # + abs(robot.v_wheel1) - # + abs(robot.v_wheel2) - # + abs(robot.v_wheel3) - # ) - - # return energy From a3af538ba067bd8bd6f5bdebe172a96a885273fd Mon Sep 17 00:00:00 2001 From: Fred Huang <60901360+fred-huang122@users.noreply.github.com> Date: Thu, 20 Nov 2025 21:00:48 +0000 Subject: [PATCH 09/48] Update utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py | 2 ++ 1 file changed, 2 insertions(+) diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index d7caa969..78e10bc9 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -291,6 +291,8 @@ def _dribbler_release_kick( if robot_states is None or not prev_dribbler[index] or dribbler: return 0.0 + if index not in robot_states: + return 0.0 robot_state = robot_states[index] if not getattr(robot_state, "infrared", False): return 0.0 From 838fff733b1c7448716763662368291fa26bc565 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Sun, 23 Nov 2025 00:51:50 +0000 Subject: [PATCH 10/48] bug fixes + updates --- utama_core/motion_planning/src/pid/pid.py | 9 +++------ utama_core/motion_planning/src/pid/pid_abstract.py | 1 - .../rsoccer_simulator/src/ssl/envs/standard_ssl.py | 7 +++---- 3 files changed, 6 insertions(+), 11 deletions(-) diff --git a/utama_core/motion_planning/src/pid/pid.py b/utama_core/motion_planning/src/pid/pid.py index aab2c613..c6ec431b 100644 --- a/utama_core/motion_planning/src/pid/pid.py +++ b/utama_core/motion_planning/src/pid/pid.py @@ -38,16 +38,13 @@ def __init__( self.max_output = config.max_output self.min_output = config.min_output - def calculate( + def _calculate( self, target: float, current: float, robot_id: int, ) -> float: - """Compute the PID output to move a robot towards a target with delay compensation. - - The delay is compensated by predicting the current value using the derivative. - """ + """Compute the PID output to move a robot towards a target with delay compensation.""" # Compute the basic (instantaneous) error raw_error = target - current # For angular measurements adjust error @@ -119,7 +116,7 @@ def __init__( super().__init__(config) self.max_velocity = config.max_velocity - def calculate(self, target: Vector2D, current: Vector2D, robot_id: int) -> Vector2D: + def _calculate(self, target: Vector2D, current: Vector2D, robot_id: int) -> Vector2D: dx = target[0] - current[0] dy = target[1] - current[1] diff --git a/utama_core/motion_planning/src/pid/pid_abstract.py b/utama_core/motion_planning/src/pid/pid_abstract.py index af30ed1e..e1b784af 100644 --- a/utama_core/motion_planning/src/pid/pid_abstract.py +++ b/utama_core/motion_planning/src/pid/pid_abstract.py @@ -29,7 +29,6 @@ def __init__(self, config: OrientationPIDConfigs | TranslationPIDConfigs): # Error tracking for 6 robots self.pre_errors = {i: 0.0 for i in range(6)} self.integrals = {i: 0.0 for i in range(6)} - self.prev_times = {i: 0.0 for i in range(6)} self.first_pass = {i: True for i in range(6)} # Anti-windup diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index 8f5266c0..78af5c40 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -7,14 +7,13 @@ import numpy as np from utama_core.config.formations import LEFT_START_ONE, RIGHT_START_ONE +from utama_core.config.robot_params import RSIM_PARAMS from utama_core.config.settings import ( MAX_BALL_SPEED, MIN_RELEASE_SPEED, RELEASE_GAIN, TIMESTEP, ) -from utama_core.config.robot_params import RSIM_PARAMS -from utama_core.config.settings import TIMESTEP from utama_core.entities.data.command import RobotResponse from utama_core.entities.data.raw_vision import RawBallData, RawRobotData, RawVisionData from utama_core.global_utils.math_utils import deg_to_rad, rad_to_deg @@ -199,7 +198,7 @@ def _get_robot_observation(self, robot): robot_info = RobotResponse(robot.id, robot.infrared) return robot_pos, robot_info - def _get_commands(self, actions) -> list[Robot] + def _get_commands(self, actions) -> list[Robot]: commands = [] # Blue robots @@ -209,7 +208,7 @@ def _get_commands(self, actions) -> list[Robot] v_theta = actions["team_blue"][i][2] dribbler = actions["team_blue"][i][4] > 0 - kick_v_x = RSIM_PARAMS.KICK_SPD if actions["team_blue"][i][3] > 0 else 0.0 + kick_v_x = RSIM_PARAMS.KICK_SPD if actions["team_blue"][i][3] > 0 else 0.0 cmd = Robot( yellow=False, # Blue team From 582d27bf8ac434e2898730d9cc4bfaa8dadf9e18 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Sun, 23 Nov 2025 01:44:37 +0000 Subject: [PATCH 11/48] linting --- .../src/controllers/real/real_robot_controller.py | 4 ++-- 1 file changed, 2 insertions(+), 2 deletions(-) diff --git a/utama_core/team_controller/src/controllers/real/real_robot_controller.py b/utama_core/team_controller/src/controllers/real/real_robot_controller.py index 859c8fe5..bf582251 100644 --- a/utama_core/team_controller/src/controllers/real/real_robot_controller.py +++ b/utama_core/team_controller/src/controllers/real/real_robot_controller.py @@ -5,6 +5,8 @@ import numpy as np from serial import EIGHTBITS, PARITY_EVEN, STOPBITS_TWO, Serial +from utama_core.config.robot_params import REAL_PARAMS + # from utama_core.config.robot_params.real import MAX_ANGULAR_VEL, MAX_VEL from utama_core.config.settings import ( BAUD_RATE, @@ -13,8 +15,6 @@ PORT, TIMEOUT, ) -from utama_core.config.robot_params import REAL_PARAMS -from utama_core.config.settings import BAUD_RATE, PORT, TIMEOUT from utama_core.entities.data.command import ( RobotCommand, RobotPacketCommand, From 82850be9f4df2bf646eaf01b863294c0281bded8 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Sun, 23 Nov 2025 01:49:09 +0000 Subject: [PATCH 12/48] test bug fix --- utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py | 5 +++-- 1 file changed, 3 insertions(+), 2 deletions(-) diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index 78af5c40..6258253e 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -297,14 +297,15 @@ def _dribbler_release_kick( if not getattr(robot_state, "infrared", False): return 0.0 - speed = prev_speed[index] + # Use actual simulated velocity, not last commanded speed, for release + speed = math.hypot(robot_state.v_x, robot_state.v_y) if speed < MIN_RELEASE_SPEED: return 0.0 return min(RELEASE_GAIN * speed, MAX_BALL_SPEED) def _calculate_reward_and_done(self): - return 0, False + return 1, False def _get_initial_positions_frame(self): """Returns the position of each robot and ball for the initial frame (random placement)""" From b011603718d176e14b4d71e055ff4ce7d9431ebc Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Sun, 23 Nov 2025 01:55:25 +0000 Subject: [PATCH 13/48] test_fix --- utama_core/config/robot_params.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utama_core/config/robot_params.py b/utama_core/config/robot_params.py index 184bef9f..a06d5a63 100644 --- a/utama_core/config/robot_params.py +++ b/utama_core/config/robot_params.py @@ -26,7 +26,7 @@ class RobotParams: RSIM_PARAMS = RobotParams( MAX_VEL=2, MAX_ANGULAR_VEL=4, - MAX_ACCELERATION=8, + MAX_ACCELERATION=2, MAX_ANGULAR_ACCELERATION=50, KICK_SPD=5, DRIBBLE_SPD=3, From 0c36171a5da6b23d96af43ba65775ba1d3367ff0 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Sun, 23 Nov 2025 01:57:16 +0000 Subject: [PATCH 14/48] undo --- utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py | 5 ++--- 1 file changed, 2 insertions(+), 3 deletions(-) diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index 6258253e..39daadb9 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -293,12 +293,11 @@ def _dribbler_release_kick( if index not in robot_states: return 0.0 - robot_state = robot_states[index] + robot_state = math.hypot(robot_states[index].v_x, robot_states[index].v_y) if not getattr(robot_state, "infrared", False): return 0.0 - # Use actual simulated velocity, not last commanded speed, for release - speed = math.hypot(robot_state.v_x, robot_state.v_y) + speed = prev_speed[index] if speed < MIN_RELEASE_SPEED: return 0.0 From e1f16acf58b63428d4e77b7e688d06460f14866c Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Sun, 23 Nov 2025 02:00:38 +0000 Subject: [PATCH 15/48] undo --- utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index 39daadb9..471679fc 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -293,7 +293,7 @@ def _dribbler_release_kick( if index not in robot_states: return 0.0 - robot_state = math.hypot(robot_states[index].v_x, robot_states[index].v_y) + robot_state = robot_states[index] if not getattr(robot_state, "infrared", False): return 0.0 From d0b8210eedf029cad19487b4ae19b9aa3bb0ac2c Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Sun, 23 Nov 2025 02:06:48 +0000 Subject: [PATCH 16/48] reverted params --- utama_core/config/robot_params.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utama_core/config/robot_params.py b/utama_core/config/robot_params.py index a06d5a63..184bef9f 100644 --- a/utama_core/config/robot_params.py +++ b/utama_core/config/robot_params.py @@ -26,7 +26,7 @@ class RobotParams: RSIM_PARAMS = RobotParams( MAX_VEL=2, MAX_ANGULAR_VEL=4, - MAX_ACCELERATION=2, + MAX_ACCELERATION=8, MAX_ANGULAR_ACCELERATION=50, KICK_SPD=5, DRIBBLE_SPD=3, From 9b33ef8fbb49765cd5c806d090fd794c7329616f Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Sun, 23 Nov 2025 02:19:17 +0000 Subject: [PATCH 17/48] bug fixes --- utama_core/config/robot_params.py | 2 +- utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py | 8 +++++--- 2 files changed, 6 insertions(+), 4 deletions(-) diff --git a/utama_core/config/robot_params.py b/utama_core/config/robot_params.py index 184bef9f..5d4b1c16 100644 --- a/utama_core/config/robot_params.py +++ b/utama_core/config/robot_params.py @@ -26,7 +26,7 @@ class RobotParams: RSIM_PARAMS = RobotParams( MAX_VEL=2, MAX_ANGULAR_VEL=4, - MAX_ACCELERATION=8, + MAX_ACCELERATION=4, MAX_ANGULAR_ACCELERATION=50, KICK_SPD=5, DRIBBLE_SPD=3, diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index 471679fc..71f29592 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -297,11 +297,13 @@ def _dribbler_release_kick( if not getattr(robot_state, "infrared", False): return 0.0 - speed = prev_speed[index] - if speed < MIN_RELEASE_SPEED: + # Only release if the robot is moving forward relative to its heading + heading_rad = math.radians(robot_state.theta) + forward_speed = robot_state.v_x * math.cos(heading_rad) + robot_state.v_y * math.sin(heading_rad) + if forward_speed < MIN_RELEASE_SPEED: return 0.0 - return min(RELEASE_GAIN * speed, MAX_BALL_SPEED) + return min(RELEASE_GAIN * forward_speed, MAX_BALL_SPEED) def _calculate_reward_and_done(self): return 1, False From dddafec4a9fe1f6d2d9d40375b4ca83a1a09edd0 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Sun, 23 Nov 2025 02:40:53 +0000 Subject: [PATCH 18/48] improved dribbler logic --- .../src/ssl/envs/standard_ssl.py | 42 +++++++++++++++---- 1 file changed, 35 insertions(+), 7 deletions(-) diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index 71f29592..79e98181 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -108,6 +108,8 @@ def __init__( self.prev_dribbler_yellow = [False] * self.n_robots_yellow self.prev_speed_blue = [0.0] * self.n_robots_blue self.prev_speed_yellow = [0.0] * self.n_robots_yellow + self.prev_forward_blue = [0.0] * self.n_robots_blue + self.prev_forward_yellow = [0.0] * self.n_robots_yellow logger.info(f"{n_robots_blue}v{n_robots_yellow} SSL Environment Initialized") @@ -118,6 +120,8 @@ def reset(self, *, seed=None, options=None): self.prev_dribbler_yellow = [False] * self.n_robots_yellow self.prev_speed_blue = [0.0] * self.n_robots_blue self.prev_speed_yellow = [0.0] * self.n_robots_yellow + self.prev_forward_blue = [0.0] * self.n_robots_blue + self.prev_forward_yellow = [0.0] * self.n_robots_yellow return super().reset(seed=seed, options=options) def step(self, action): @@ -257,7 +261,12 @@ def _apply_dribbler_release_kicks(self, commands: list[Robot]) -> None: n_blue = self.n_robots_blue for i in range(n_blue): release = self._dribbler_release_kick( - blue_states, self.prev_dribbler_blue, self.prev_speed_blue, i, commands[i].dribbler + blue_states, + self.prev_dribbler_blue, + self.prev_speed_blue, + self.prev_forward_blue, + i, + commands[i].dribbler, ) if release > 0.0: commands[i].kick_v_x = max(commands[i].kick_v_x, release) @@ -265,7 +274,12 @@ def _apply_dribbler_release_kicks(self, commands: list[Robot]) -> None: for j in range(self.n_robots_yellow): cmd_idx = n_blue + j release = self._dribbler_release_kick( - yellow_states, self.prev_dribbler_yellow, self.prev_speed_yellow, j, commands[cmd_idx].dribbler + yellow_states, + self.prev_dribbler_yellow, + self.prev_speed_yellow, + self.prev_forward_yellow, + j, + commands[cmd_idx].dribbler, ) if release > 0.0: commands[cmd_idx].kick_v_x = max(commands[cmd_idx].kick_v_x, release) @@ -279,11 +293,22 @@ def _update_dribbler_history(self, commands: list[Robot]) -> None: self.prev_speed_blue = [math.hypot(cmd.v_x, cmd.v_y) for cmd in commands[:n_blue]] self.prev_speed_yellow = [math.hypot(cmd.v_x, cmd.v_y) for cmd in commands[n_blue:]] + # Store commanded forward components relative to the current robot headings + def forward_component(robot, cmd): + heading_rad = math.radians(robot.theta) + return cmd.v_x * math.cos(heading_rad) + cmd.v_y * math.sin(heading_rad) + + self.prev_forward_blue = [forward_component(self.frame.robots_blue[i], commands[i]) for i in range(n_blue)] + self.prev_forward_yellow = [ + forward_component(self.frame.robots_yellow[j], commands[n_blue + j]) for j in range(self.n_robots_yellow) + ] + def _dribbler_release_kick( self, robot_states: Optional[Dict[int, Robot]], prev_dribbler: List[bool], prev_speed: List[float], + prev_forward: List[float], index: int, dribbler: bool, ) -> float: @@ -297,13 +322,16 @@ def _dribbler_release_kick( if not getattr(robot_state, "infrared", False): return 0.0 - # Only release if the robot is moving forward relative to its heading - heading_rad = math.radians(robot_state.theta) - forward_speed = robot_state.v_x * math.cos(heading_rad) + robot_state.v_y * math.sin(heading_rad) - if forward_speed < MIN_RELEASE_SPEED: + # Require forward motion relative to heading to avoid releasing while backing up + forward = prev_forward[index] + if forward <= 0.0: + return 0.0 + + speed = prev_speed[index] + if speed < MIN_RELEASE_SPEED: return 0.0 - return min(RELEASE_GAIN * forward_speed, MAX_BALL_SPEED) + return min(RELEASE_GAIN * speed, MAX_BALL_SPEED) def _calculate_reward_and_done(self): return 1, False From bbb402393ab0d578382eab379a5f9f19b8f82be2 Mon Sep 17 00:00:00 2001 From: Fred Huang <60901360+fred-huang122@users.noreply.github.com> Date: Mon, 24 Nov 2025 00:16:45 +0000 Subject: [PATCH 19/48] Update utama_core/team_controller/src/controllers/real/real_robot_controller.py Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- .../src/controllers/real/real_robot_controller.py | 1 - 1 file changed, 1 deletion(-) diff --git a/utama_core/team_controller/src/controllers/real/real_robot_controller.py b/utama_core/team_controller/src/controllers/real/real_robot_controller.py index bf582251..7af791c2 100644 --- a/utama_core/team_controller/src/controllers/real/real_robot_controller.py +++ b/utama_core/team_controller/src/controllers/real/real_robot_controller.py @@ -7,7 +7,6 @@ from utama_core.config.robot_params import REAL_PARAMS -# from utama_core.config.robot_params.real import MAX_ANGULAR_VEL, MAX_VEL from utama_core.config.settings import ( BAUD_RATE, CMD_MAX_ANGULAR_VELOCITY, From 4ed3e4aea356a0f220fa2c75d5f97bd7a8f2bc5a Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Wed, 26 Nov 2025 15:44:55 +0000 Subject: [PATCH 20/48] updates --- utama_core/skills/src/block.py | 2 +- utama_core/skills/src/defend_parameter.py | 2 +- 2 files changed, 2 insertions(+), 2 deletions(-) diff --git a/utama_core/skills/src/block.py b/utama_core/skills/src/block.py index 7824d660..7a0c9142 100644 --- a/utama_core/skills/src/block.py +++ b/utama_core/skills/src/block.py @@ -30,7 +30,7 @@ def block_attacker( # ========== Prioritize blocking the shot line ========== ax, ay = attacker.p.x, attacker.p.y - gx, gy = -game.field.enemy_goal_line.coords[0][0], 0 + gx, gy = -game.field.enemy_goal_line[0][0], 0 agx, agy = (gx - ax), (gy - ay) dist_ag = math.hypot(agx, agy) diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py index 66e97fbb..81d76f3d 100644 --- a/utama_core/skills/src/defend_parameter.py +++ b/utama_core/skills/src/defend_parameter.py @@ -47,7 +47,7 @@ def defend_parameter( dribbling=True, ) - gp = (game.field.my_goal_line.coords[0][0], 0) + gp = (game.field.my_goal_line.coords, 0) if env: env.draw_line( [gp, (target_tracking_coord[0], target_tracking_coord[1])], From 174dc74c54042a3cc2137ec142cb84cbad8e0dfb Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Wed, 26 Nov 2025 15:48:23 +0000 Subject: [PATCH 21/48] update fix --- utama_core/skills/src/defend_parameter.py | 2 +- utama_core/skills/src/utils/defense_utils.py | 6 +++--- 2 files changed, 4 insertions(+), 4 deletions(-) diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py index 81d76f3d..dcf4621c 100644 --- a/utama_core/skills/src/defend_parameter.py +++ b/utama_core/skills/src/defend_parameter.py @@ -47,7 +47,7 @@ def defend_parameter( dribbling=True, ) - gp = (game.field.my_goal_line.coords, 0) + gp = (game.field.my_goal_line, 0) if env: env.draw_line( [gp, (target_tracking_coord[0], target_tracking_coord[1])], diff --git a/utama_core/skills/src/utils/defense_utils.py b/utama_core/skills/src/utils/defense_utils.py index 4ee77cb9..c9d654a0 100644 --- a/utama_core/skills/src/utils/defense_utils.py +++ b/utama_core/skills/src/utils/defense_utils.py @@ -25,7 +25,7 @@ def align_defenders( defender_pos = calculate_defense_area(game, defender_parametric_pos) # logger.debug(f"DEFENDER {dx} {dy}") - goal_centre_x = game.field.my_goal_line.coords[0][0] + goal_centre_x = game.field.my_goal_line[0][0] if attacker_orientation is None or attacker_orientation == 0: # In case there is no ball velocity or attackers, use centre of goal @@ -92,7 +92,7 @@ def calculate_defense_area(game: Game, t: float) -> Vector2D: ) # This slows everything down sooo much that everything breaks (ask Fred if still confused) - # goal_centre_x, _ = game.field.my_goal_line.coords[0] + # goal_centre_x, _ = game.field.my_goal_line[0] if game.my_team_is_right: goal_centre_x = 4.5 @@ -111,7 +111,7 @@ def make_relative_to_goal_centre(goal_centre_x: float, p: Vector2D) -> Vector2D: def predict_goal_y_location(game: Game, shooter_position: Vector2D, orientation: float) -> float: dx, dy = np.cos(orientation), np.sin(orientation) - gx, _ = game.field.my_goal_line.coords[0] + gx, _ = game.field.my_goal_line[0] if dx == 0: return float("inf") t = (gx - shooter_position[0]) / dx From d4678ab43a28d2a5f94262cdbff1e30ac7c77957 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Wed, 26 Nov 2025 15:49:58 +0000 Subject: [PATCH 22/48] updates --- utama_core/config/settings.py | 3 - .../controllers/real/real_robot_controller.py | 77 ++++--------------- 2 files changed, 14 insertions(+), 66 deletions(-) diff --git a/utama_core/config/settings.py b/utama_core/config/settings.py index 2e07a7b9..41d5fe68 100644 --- a/utama_core/config/settings.py +++ b/utama_core/config/settings.py @@ -32,9 +32,6 @@ PORT = "/dev/ttyACM0" TIMEOUT = 0.1 -CMD_MAX_VELOCITY = 4.0 # m/s -CMD_MAX_ANGULAR_VELOCITY = 8 # rad/s - MAX_GAME_HISTORY = 20 REPLAY_BASE_PATH = Path.cwd() / "replays" diff --git a/utama_core/team_controller/src/controllers/real/real_robot_controller.py b/utama_core/team_controller/src/controllers/real/real_robot_controller.py index a09a924d..a6574ef4 100644 --- a/utama_core/team_controller/src/controllers/real/real_robot_controller.py +++ b/utama_core/team_controller/src/controllers/real/real_robot_controller.py @@ -6,19 +6,8 @@ from serial import EIGHTBITS, PARITY_EVEN, STOPBITS_TWO, Serial from utama_core.config.robot_params import REAL_PARAMS - -from utama_core.config.settings import ( - BAUD_RATE, - CMD_MAX_ANGULAR_VELOCITY, - CMD_MAX_VELOCITY, - PORT, - TIMEOUT, -) -from utama_core.entities.data.command import ( - RobotCommand, - RobotPacketCommand, - RobotResponse, -) +from utama_core.config.settings import BAUD_RATE, PORT, TIMEOUT +from utama_core.entities.data.command import RobotCommand, RobotResponse from utama_core.team_controller.src.controllers.common.robot_controller_abstract import ( AbstractRobotController, ) @@ -147,46 +136,7 @@ def _generate_command_buffer(self, robot_id: int, c_command: RobotCommand) -> by return packet - def _convert_uint16_command(self, robot_id, command: RobotCommand) -> RobotPacketCommand: - """Prepares the float values in the command to be formatted to binary in the buffer. - - Also converts angular velocity to degrees per second. - """ - angular_vel = command.angular_vel - local_forward_vel = command.local_forward_vel - local_left_vel = command.local_left_vel - - if abs(command.angular_vel) > CMD_MAX_ANGULAR_VELOCITY: - warnings.warn( - f"Angular velocity for robot {robot_id} is greater than the maximum angular velocity. Clipping to {CMD_MAX_ANGULAR_VELOCITY}." - ) - angular_vel = CMD_MAX_ANGULAR_VELOCITY if command.angular_vel > 0 else -CMD_MAX_ANGULAR_VELOCITY - if abs(command.local_forward_vel) > CMD_MAX_VELOCITY: - warnings.warn( - f"Local forward velocity for robot {robot_id} is greater than the maximum velocity. Clipping to {CMD_MAX_VELOCITY}." - ) - local_forward_vel = CMD_MAX_VELOCITY if command.local_forward_vel > 0 else -CMD_MAX_VELOCITY - if abs(command.local_left_vel) > CMD_MAX_VELOCITY: - warnings.warn( - f"Local left velocity for robot {robot_id} is greater than the maximum velocity. Clipping to {CMD_MAX_VELOCITY}." - ) - local_left_vel = CMD_MAX_VELOCITY if command.local_left_vel > 0 else -CMD_MAX_VELOCITY - - local_forward_vel = self._encode_signed_to_u16(local_forward_vel, CMD_MAX_VELOCITY) - local_left_vel = self._encode_signed_to_u16(local_left_vel, CMD_MAX_VELOCITY) - angular_vel = self._encode_signed_to_u16(angular_vel, CMD_MAX_ANGULAR_VELOCITY) - - command = RobotPacketCommand( - local_forward_vel=self._uint16_rep(local_forward_vel), - local_left_vel=self._uint16_rep(local_left_vel), - angular_vel=self._uint16_rep(angular_vel), - kick=command.kick, - chip=command.chip, - dribble=command.dribble, - ) - return command - - def _convert_float_command(self, robot_id, command: RobotCommand) -> RobotCommand: + def _convert_float16_command(self, robot_id, command: RobotCommand) -> RobotCommand: """Prepares the float values in the command to be formatted to binary in the buffer. Also converts angular velocity to degrees per second. @@ -196,23 +146,24 @@ def _convert_float_command(self, robot_id, command: RobotCommand) -> RobotComman local_forward_vel = command.local_forward_vel local_left_vel = command.local_left_vel - if abs(command.angular_vel) > CMD_MAX_ANGULAR_VELOCITY: + if abs(command.angular_vel) > MAX_ANGULAR_VEL: warnings.warn( - f"Angular velocity for robot {robot_id} is greater than the maximum angular velocity. Clipping to {CMD_MAX_ANGULAR_VELOCITY}." + f"Angular velocity for robot {robot_id} is greater than the maximum angular velocity. Clipping to {MAX_ANGULAR_VEL}." ) - angular_vel = CMD_MAX_ANGULAR_VELOCITY if command.angular_vel > 0 else -CMD_MAX_ANGULAR_VELOCITY - - if abs(command.local_forward_vel) > CMD_MAX_VELOCITY: + angular_vel = MAX_ANGULAR_VEL if command.angular_vel > 0 else -MAX_ANGULAR_VEL + # TODO put back to max_vel + if abs(command.local_forward_vel) > 0.8: warnings.warn( - f"Local forward velocity for robot {robot_id} is greater than the maximum velocity. Clipping to {CMD_MAX_VELOCITY}." + f"Local forward velocity for robot {robot_id} is greater than the maximum velocity. Clipping to {MAX_VEL}." ) - local_forward_vel = CMD_MAX_VELOCITY if command.local_forward_vel > 0 else -CMD_MAX_VELOCITY + local_forward_vel = MAX_VEL if command.local_forward_vel > 0 else -MAX_VEL - if abs(command.local_left_vel) > CMD_MAX_VELOCITY: + if abs(command.local_left_vel) > MAX_VEL: warnings.warn( - f"Local left velocity for robot {robot_id} is greater than the maximum velocity. Clipping to {CMD_MAX_VELOCITY}." + f"Local left velocity for robot {robot_id} is greater than the maximum velocity. Clipping to {MAX_VEL}." ) - local_left_vel = CMD_MAX_VELOCITY if command.local_left_vel > 0 else -CMD_MAX_VELOCITY + local_left_vel = MAX_VEL if command.local_left_vel > 0 else -MAX_VEL + command = RobotCommand( local_forward_vel=self._float16_rep(local_forward_vel), local_left_vel=self._float16_rep(local_left_vel), From 30faaa84e4ea8921554dde785b8176a21d18facf Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Wed, 26 Nov 2025 16:34:20 +0000 Subject: [PATCH 23/48] updates --- .../src/ssl/envs/standard_ssl.py | 25 +++++++++++-------- 1 file changed, 14 insertions(+), 11 deletions(-) diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index 79e98181..641986b9 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -290,18 +290,21 @@ def _update_dribbler_history(self, commands: list[Robot]) -> None: self.prev_dribbler_blue = [cmd.dribbler for cmd in commands[:n_blue]] self.prev_dribbler_yellow = [cmd.dribbler for cmd in commands[n_blue:]] - self.prev_speed_blue = [math.hypot(cmd.v_x, cmd.v_y) for cmd in commands[:n_blue]] - self.prev_speed_yellow = [math.hypot(cmd.v_x, cmd.v_y) for cmd in commands[n_blue:]] + # Use measured velocities from the simulator frame to avoid drift from commanded speeds. + self.prev_speed_blue = [ + math.hypot(self.frame.robots_blue[i].v_x, self.frame.robots_blue[i].v_y) for i in range(n_blue) + ] + self.prev_speed_yellow = [ + math.hypot(self.frame.robots_yellow[j].v_x, self.frame.robots_yellow[j].v_y) + for j in range(self.n_robots_yellow) + ] - # Store commanded forward components relative to the current robot headings - def forward_component(robot, cmd): - heading_rad = math.radians(robot.theta) - return cmd.v_x * math.cos(heading_rad) + cmd.v_y * math.sin(heading_rad) + # Store forward components relative to the current robot headings using measured velocity + def forward_component(robot): + return robot.v_x > 0.0 - self.prev_forward_blue = [forward_component(self.frame.robots_blue[i], commands[i]) for i in range(n_blue)] - self.prev_forward_yellow = [ - forward_component(self.frame.robots_yellow[j], commands[n_blue + j]) for j in range(self.n_robots_yellow) - ] + self.prev_forward_blue = [forward_component(self.frame.robots_blue[i]) for i in range(n_blue)] + self.prev_forward_yellow = [forward_component(self.frame.robots_yellow[j]) for j in range(self.n_robots_yellow)] def _dribbler_release_kick( self, @@ -324,7 +327,7 @@ def _dribbler_release_kick( # Require forward motion relative to heading to avoid releasing while backing up forward = prev_forward[index] - if forward <= 0.0: + if not forward: return 0.0 speed = prev_speed[index] From a28a158df2b410c1df51821fbefee9642fb45fbe Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Wed, 26 Nov 2025 16:42:25 +0000 Subject: [PATCH 24/48] fixes --- .../src/ssl/envs/standard_ssl.py | 32 ++++++------------- 1 file changed, 9 insertions(+), 23 deletions(-) diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index 641986b9..c8fd2afd 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -254,14 +254,9 @@ def _apply_dribbler_release_kicks(self, commands: list[Robot]) -> None: increasing ``kick_v_x`` for robots whose dribbler transitioned from on to off while they were moving with the ball. """ - prior_frame = self.frame - blue_states = prior_frame.robots_blue if prior_frame is not None else None - yellow_states = prior_frame.robots_yellow if prior_frame is not None else None - n_blue = self.n_robots_blue for i in range(n_blue): release = self._dribbler_release_kick( - blue_states, self.prev_dribbler_blue, self.prev_speed_blue, self.prev_forward_blue, @@ -274,7 +269,6 @@ def _apply_dribbler_release_kicks(self, commands: list[Robot]) -> None: for j in range(self.n_robots_yellow): cmd_idx = n_blue + j release = self._dribbler_release_kick( - yellow_states, self.prev_dribbler_yellow, self.prev_speed_yellow, self.prev_forward_yellow, @@ -291,24 +285,20 @@ def _update_dribbler_history(self, commands: list[Robot]) -> None: self.prev_dribbler_yellow = [cmd.dribbler for cmd in commands[n_blue:]] # Use measured velocities from the simulator frame to avoid drift from commanded speeds. - self.prev_speed_blue = [ - math.hypot(self.frame.robots_blue[i].v_x, self.frame.robots_blue[i].v_y) for i in range(n_blue) - ] + self.prev_speed_blue = [math.hypot(commands[i].v_x, commands[i].v_y) for i in range(n_blue)] self.prev_speed_yellow = [ - math.hypot(self.frame.robots_yellow[j].v_x, self.frame.robots_yellow[j].v_y) - for j in range(self.n_robots_yellow) + math.hypot(commands[n_blue + j].v_x, commands[n_blue + j].v_y) for j in range(self.n_robots_yellow) ] # Store forward components relative to the current robot headings using measured velocity - def forward_component(robot): - return robot.v_x > 0.0 + def forward_component(cmd): + return cmd.v_x > 0.0 - self.prev_forward_blue = [forward_component(self.frame.robots_blue[i]) for i in range(n_blue)] - self.prev_forward_yellow = [forward_component(self.frame.robots_yellow[j]) for j in range(self.n_robots_yellow)] + self.prev_forward_blue = [forward_component(commands[i]) for i in range(n_blue)] + self.prev_forward_yellow = [forward_component(commands[n_blue + j]) for j in range(self.n_robots_yellow)] def _dribbler_release_kick( self, - robot_states: Optional[Dict[int, Robot]], prev_dribbler: List[bool], prev_speed: List[float], prev_forward: List[float], @@ -316,21 +306,17 @@ def _dribbler_release_kick( dribbler: bool, ) -> float: """Estimate the kick needed to release the ball when the dribbler turns off.""" - if robot_states is None or not prev_dribbler[index] or dribbler: - return 0.0 - - if index not in robot_states: - return 0.0 - robot_state = robot_states[index] - if not getattr(robot_state, "infrared", False): + if not prev_dribbler[index] or dribbler: return 0.0 # Require forward motion relative to heading to avoid releasing while backing up forward = prev_forward[index] + print(forward) if not forward: return 0.0 speed = prev_speed[index] + print(speed) if speed < MIN_RELEASE_SPEED: return 0.0 From d5339ca7f51cc1af31b321f1d40ee00fd23d0bde Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Wed, 26 Nov 2025 16:43:10 +0000 Subject: [PATCH 25/48] removed prints --- utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py | 2 -- 1 file changed, 2 deletions(-) diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index c8fd2afd..45abe429 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -311,12 +311,10 @@ def _dribbler_release_kick( # Require forward motion relative to heading to avoid releasing while backing up forward = prev_forward[index] - print(forward) if not forward: return 0.0 speed = prev_speed[index] - print(speed) if speed < MIN_RELEASE_SPEED: return 0.0 From b547a90c9e54fec093f01aa86ed33e778f0663b7 Mon Sep 17 00:00:00 2001 From: Yangping Li Date: Wed, 26 Nov 2025 17:02:01 +0000 Subject: [PATCH 26/48] feat: change goalkeep --- utama_core/skills/src/goalkeep.py | 24 ++++++++++++++++++++---- 1 file changed, 20 insertions(+), 4 deletions(-) diff --git a/utama_core/skills/src/goalkeep.py b/utama_core/skills/src/goalkeep.py index c57ae3ac..00c6f5b5 100644 --- a/utama_core/skills/src/goalkeep.py +++ b/utama_core/skills/src/goalkeep.py @@ -11,20 +11,36 @@ from utama_core.skills.src.utils.move_utils import face_ball, move -def goalkeep( +def mykeep( game: Game, motion_controller: MotionController, robot_id: int, env: Optional[SSLStandardEnv] = None, ): - goalie_obj = game.friendly_robots[robot_id] - if goalie_obj.has_ball: + shooting_enemy = game.enemy_robots[robot_id] + defenseing_friendly = game.friendly_robots[robot_id] + if shooting_enemy.has_ball: + robot_rad = 0.09 # radius of robot in meters + # Calculate the perpendicular projection point from the robot to the goal line + x1 = shooting_enemy.x + y1 = shooting_enemy.y + x2 = 4.5 + y2 = -0.5 + x3 = defenseing_friendly.x + y3 = defenseing_friendly.y + t = (x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1) + t /= (x2 - x1) ** 2 + (y2 - y1) ** 2 + x4 = x1 + t * (x2 - x1) + y4 = y1 + t * (y2 - y1) + target_pos = Vector2D(x4, y4) - Vector2D(x4 - x3, y4 - y3).normalized() * robot_rad + target_oren = Vector2D(x3 - x1, y3 - y1).angle() + 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_pos, target_oren, True, ) From 368d9d7f79322093d305c1e431e445fcbfcf2c2d Mon Sep 17 00:00:00 2001 From: Yangping Li Date: Wed, 26 Nov 2025 17:20:28 +0000 Subject: [PATCH 27/48] feat: fix bug --- utama_core/skills/src/goalkeep.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utama_core/skills/src/goalkeep.py b/utama_core/skills/src/goalkeep.py index 00c6f5b5..0b0081ff 100644 --- a/utama_core/skills/src/goalkeep.py +++ b/utama_core/skills/src/goalkeep.py @@ -11,7 +11,7 @@ from utama_core.skills.src.utils.move_utils import face_ball, move -def mykeep( +def goalkeep( game: Game, motion_controller: MotionController, robot_id: int, From 0370dfcfe49b260835b3d64474a4facedda3cdac Mon Sep 17 00:00:00 2001 From: Yangping Li Date: Wed, 14 Jan 2026 17:33:53 +0000 Subject: [PATCH 28/48] add change --- utama_core/skills/src/defend_parameter.py | 82 +++++++++++------------ utama_core/skills/src/goal_keep.py | 58 ++++++++++++++++ 2 files changed, 98 insertions(+), 42 deletions(-) create mode 100644 utama_core/skills/src/goal_keep.py diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py index dcf4621c..ac8c016e 100644 --- a/utama_core/skills/src/defend_parameter.py +++ b/utama_core/skills/src/defend_parameter.py @@ -1,58 +1,56 @@ 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.run.predictors.position import predict_ball_pos_at_x 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, -) +from utama_core.skills.src.utils.move_utils import face_ball, move 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 - 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( +): + shooting_enemy = game.enemy_robots[robot_id] + defenseing_friendly = game.friendly_robots[robot_id] + + robot_rad = 0.09 # radius of robot in meters + # Calculate the perpendicular projection point from the robot to the goal line + x1 = shooting_enemy.p.x + y1 = shooting_enemy.p.y + x2 = 4.5 + y2 = -0.5 + x3 = defenseing_friendly.p.x + y3 = defenseing_friendly.p.y + t = (x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1) + t /= (x2 - x1) ** 2 + (y2 - y1) ** 2 + x4 = x1 + t * (x2 - x1) + y4 = y1 + t * (y2 - y1) + + def normalize_vector(v: Vector2D) -> Vector2D: + norm = np.sqrt(v.x**2 + v.y**2) + if norm == 0: + return Vector2D(0, 0) + return Vector2D(v.x / norm, v.y / norm) + + def vector_angle(v: Vector2D) -> float: + return np.arctan2(v.y, v.x) + + target_pos = Vector2D(x4, y4) - normalize_vector(Vector2D(x4 - x3, y4 - y3)) * robot_rad + target_oren = vector_angle(Vector2D(x3 - x1, y3 - y1)) + + target_oren = np.pi if game.my_team_is_right else 0 + return move( game, motion_controller, - defender_id, - target, - dribbling=True, + robot_id, + target_pos, + target_oren, + True, ) - - gp = (game.field.my_goal_line, 0) - if env: - env.draw_line( - [gp, (target_tracking_coord[0], target_tracking_coord[1])], - width=5, - color="RED" if tracking_ball else "PINK", - ) - - return cmd diff --git a/utama_core/skills/src/goal_keep.py b/utama_core/skills/src/goal_keep.py new file mode 100644 index 00000000..ca060e98 --- /dev/null +++ b/utama_core/skills/src/goal_keep.py @@ -0,0 +1,58 @@ +from typing import Optional + +from utama_core.entities.data.command import RobotCommand +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 goal_keep( + game: Game, + motion_controller: MotionController, + defender_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 + 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) + if env: + env.draw_line( + [gp, (target_tracking_coord[0], target_tracking_coord[1])], + width=5, + color="RED" if tracking_ball else "PINK", + ) + + return cmd From 7cf62de69baaad2fe32fcaf5f83d164eaa89e428 Mon Sep 17 00:00:00 2001 From: Ningchaun <1311856651@qq.com> Date: Sat, 17 Jan 2026 16:58:05 +0000 Subject: [PATCH 29/48] Fix: fix the bug run toward enemy --- utama_core/skills/src/defend_parameter.py | 62 ++++++++++++++--------- 1 file changed, 39 insertions(+), 23 deletions(-) diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py index ac8c016e..35fb0948 100644 --- a/utama_core/skills/src/defend_parameter.py +++ b/utama_core/skills/src/defend_parameter.py @@ -17,40 +17,56 @@ def defend_parameter( robot_id: int, env: Optional[SSLStandardEnv] = None, ): - shooting_enemy = game.enemy_robots[robot_id] defenseing_friendly = game.friendly_robots[robot_id] + vel = game.ball.v.to_2d() + if vel[0]**2 + vel[1]**2 > 0.05: + x1, y1 = game.ball.p.x, game.ball.p.y + x2, y2 = 4.5, 0.5 - robot_rad = 0.09 # radius of robot in meters - # Calculate the perpendicular projection point from the robot to the goal line - x1 = shooting_enemy.p.x - y1 = shooting_enemy.p.y - x2 = 4.5 - y2 = -0.5 - x3 = defenseing_friendly.p.x - y3 = defenseing_friendly.p.y - t = (x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1) - t /= (x2 - x1) ** 2 + (y2 - y1) ** 2 + 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) + + target_pos = np.array([x4, y4]) + return go_to_point( + game, + motion_controller, + robot_id, + Vector2D(target_pos[0], target_pos[1]), + dribbling=True, + ) + + shooting_enemy = game.enemy_robots[0] + + robot_rad = 0.09 + + x1, y1 = game.ball.p.x, game.ball.p.y + x2, y2 = 4.5, -0.5 + + 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 normalize_vector(v: Vector2D) -> Vector2D: - norm = np.sqrt(v.x**2 + v.y**2) - if norm == 0: - return Vector2D(0, 0) - return Vector2D(v.x / norm, v.y / norm) + vec_to_target = np.array([x4 - x3, y4 - y3]) + dist_to_target = np.linalg.norm(vec_to_target) - def vector_angle(v: Vector2D) -> float: - return np.arctan2(v.y, v.x) + if dist_to_target > 0: + vec_dir = vec_to_target / dist_to_target + else: + vec_dir = np.array([0.0, 0.0]) - target_pos = Vector2D(x4, y4) - normalize_vector(Vector2D(x4 - x3, y4 - y3)) * robot_rad - target_oren = vector_angle(Vector2D(x3 - x1, y3 - y1)) + target_pos = np.array([x4, y4]) - vec_dir * robot_rad target_oren = np.pi if game.my_team_is_right else 0 return move( game, motion_controller, robot_id, - target_pos, + Vector2D(target_pos[0], target_pos[1]), target_oren, - True, - ) + True + ) \ No newline at end of file From 60d37cd9ddea129ce147e67241fe81f26130bdb2 Mon Sep 17 00:00:00 2001 From: Ningchaun <1311856651@qq.com> Date: Sat, 17 Jan 2026 17:15:12 +0000 Subject: [PATCH 30/48] Fix: add a barrier to the forbiden square --- utama_core/skills/src/defend_parameter.py | 24 +++++++++++++++++++++-- 1 file changed, 22 insertions(+), 2 deletions(-) diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py index 35fb0948..46686958 100644 --- a/utama_core/skills/src/defend_parameter.py +++ b/utama_core/skills/src/defend_parameter.py @@ -30,6 +30,17 @@ def defend_parameter( y4 = y1 + t * (y2 - y1) target_pos = np.array([x4, y4]) + if target_pos[0] > 4.0: + if target_pos[1] < 1.0 and target_pos[1] >= 0: + if 4.5 - target_pos[0] < target_pos[1] - 1.0: + target_pos[0] = 4.0 + else: + target_pos[1] = 1.0 + elif target_pos[1] > -1.0 and target_pos[1] < 0: + if 4.5 - target_pos[0] < -1.0 - target_pos[1]: + target_pos[0] = 4.0 + else: + target_pos[1] = -1.0 return go_to_point( game, motion_controller, @@ -37,8 +48,6 @@ def defend_parameter( Vector2D(target_pos[0], target_pos[1]), dribbling=True, ) - - shooting_enemy = game.enemy_robots[0] robot_rad = 0.09 @@ -62,6 +71,17 @@ def defend_parameter( target_pos = np.array([x4, y4]) - vec_dir * robot_rad target_oren = np.pi if game.my_team_is_right else 0 + if target_pos[0] > 4.0: + if target_pos[1] < 1.0 and target_pos[1] >= 0: + if 4.5 - target_pos[0] < target_pos[1] - 1.0: + target_pos[0] = 4.0 + else: + target_pos[1] = 1.0 + elif target_pos[1] > -1.0 and target_pos[1] < 0: + if 4.5 - target_pos[0] < -1.0 - target_pos[1]: + target_pos[0] = 4.0 + else: + target_pos[1] = -1.0 return move( game, motion_controller, From 232cce15d859154ef0a6fb00ac5146b2e8859aae Mon Sep 17 00:00:00 2001 From: Ningchaun <1311856651@qq.com> Date: Sat, 17 Jan 2026 17:45:12 +0000 Subject: [PATCH 31/48] Fix: goal keep --- utama_core/skills/src/goalkeep.py | 28 ---------------------------- 1 file changed, 28 deletions(-) diff --git a/utama_core/skills/src/goalkeep.py b/utama_core/skills/src/goalkeep.py index 0b0081ff..54323eb7 100644 --- a/utama_core/skills/src/goalkeep.py +++ b/utama_core/skills/src/goalkeep.py @@ -17,34 +17,6 @@ def goalkeep( robot_id: int, env: Optional[SSLStandardEnv] = None, ): - shooting_enemy = game.enemy_robots[robot_id] - defenseing_friendly = game.friendly_robots[robot_id] - if shooting_enemy.has_ball: - robot_rad = 0.09 # radius of robot in meters - # Calculate the perpendicular projection point from the robot to the goal line - x1 = shooting_enemy.x - y1 = shooting_enemy.y - x2 = 4.5 - y2 = -0.5 - x3 = defenseing_friendly.x - y3 = defenseing_friendly.y - t = (x3 - x1) * (y2 - y1) - (y3 - y1) * (x2 - x1) - t /= (x2 - x1) ** 2 + (y2 - y1) ** 2 - x4 = x1 + t * (x2 - x1) - y4 = y1 + t * (y2 - y1) - target_pos = Vector2D(x4, y4) - Vector2D(x4 - x3, y4 - y3).normalized() * robot_rad - target_oren = Vector2D(x3 - x1, y3 - y1).angle() - - target_oren = np.pi if game.my_team_is_right else 0 - return move( - game, - motion_controller, - robot_id, - target_pos, - target_oren, - True, - ) - if game.my_team_is_right: target = predict_ball_pos_at_x(game, 4.5) else: From b7d6afad02ee14ff57ddd347d07f6cb0b92f962a Mon Sep 17 00:00:00 2001 From: Yangping Li Date: Sun, 18 Jan 2026 16:37:23 +0000 Subject: [PATCH 32/48] simplification of similar codes --- utama_core/skills/src/defend_parameter.py | 74 +++++++++-------------- 1 file changed, 27 insertions(+), 47 deletions(-) diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py index 46686958..795d9da6 100644 --- a/utama_core/skills/src/defend_parameter.py +++ b/utama_core/skills/src/defend_parameter.py @@ -19,17 +19,16 @@ def defend_parameter( ): defenseing_friendly = game.friendly_robots[robot_id] vel = game.ball.v.to_2d() - if vel[0]**2 + vel[1]**2 > 0.05: - x1, y1 = game.ball.p.x, game.ball.p.y - x2, y2 = 4.5, 0.5 + def positions_to_defend_parameter(x1, y1): + x2, y2 = 4.5, 0.5 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) + 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) + return x3, y3, x4, y4 - target_pos = np.array([x4, y4]) + def modified_pos(target_pos): if target_pos[0] > 4.0: if target_pos[1] < 1.0 and target_pos[1] >= 0: if 4.5 - target_pos[0] < target_pos[1] - 1.0: @@ -41,52 +40,33 @@ def defend_parameter( target_pos[0] = 4.0 else: target_pos[1] = -1.0 - return go_to_point( - game, - motion_controller, - robot_id, - Vector2D(target_pos[0], target_pos[1]), - dribbling=True, - ) + return target_pos - robot_rad = 0.09 + x3, y3, x4, y4 = positions_to_defend_parameter(game.ball.p.x, game.ball.p.y) - x1, y1 = game.ball.p.x, game.ball.p.y - x2, y2 = 4.5, -0.5 + if vel[0] ** 2 + vel[1] ** 2 > 0.05: + target_pos = modified_pos(np.array([x4, y4])) - x3, y3 = defenseing_friendly.p.x, defenseing_friendly.p.y + return go_to_point( + game, + motion_controller, + robot_id, + Vector2D(target_pos[0], target_pos[1]), + dribbling=True, + ) - 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) + else: + robot_rad = 0.09 - vec_to_target = np.array([x4 - x3, y4 - y3]) - dist_to_target = np.linalg.norm(vec_to_target) + 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]) + 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 + target_pos = modified_pos(np.array([x4, y4]) - vec_dir * robot_rad) + target_oren = np.pi if game.my_team_is_right else 0 - target_oren = np.pi if game.my_team_is_right else 0 - if target_pos[0] > 4.0: - if target_pos[1] < 1.0 and target_pos[1] >= 0: - if 4.5 - target_pos[0] < target_pos[1] - 1.0: - target_pos[0] = 4.0 - else: - target_pos[1] = 1.0 - elif target_pos[1] > -1.0 and target_pos[1] < 0: - if 4.5 - target_pos[0] < -1.0 - target_pos[1]: - target_pos[0] = 4.0 - else: - target_pos[1] = -1.0 - return move( - game, - motion_controller, - robot_id, - Vector2D(target_pos[0], target_pos[1]), - target_oren, - True - ) \ No newline at end of file + return move(game, motion_controller, robot_id, Vector2D(target_pos[0], target_pos[1]), target_oren, True) From 8a5938ad06d9aed3138206f0e2d83f0bfd1ed085 Mon Sep 17 00:00:00 2001 From: Yangping Li Date: Wed, 21 Jan 2026 14:38:37 +0000 Subject: [PATCH 33/48] simplification --- utama_core/skills/src/defend_parameter.py | 12 +----------- 1 file changed, 1 insertion(+), 11 deletions(-) diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py index 795d9da6..0e069e3f 100644 --- a/utama_core/skills/src/defend_parameter.py +++ b/utama_core/skills/src/defend_parameter.py @@ -47,14 +47,6 @@ def modified_pos(target_pos): if vel[0] ** 2 + vel[1] ** 2 > 0.05: target_pos = modified_pos(np.array([x4, y4])) - return go_to_point( - game, - motion_controller, - robot_id, - Vector2D(target_pos[0], target_pos[1]), - dribbling=True, - ) - else: robot_rad = 0.09 @@ -65,8 +57,6 @@ def modified_pos(target_pos): vec_dir = vec_to_target / dist_to_target else: vec_dir = np.array([0.0, 0.0]) - target_pos = modified_pos(np.array([x4, y4]) - vec_dir * robot_rad) - target_oren = np.pi if game.my_team_is_right else 0 - return move(game, motion_controller, robot_id, Vector2D(target_pos[0], target_pos[1]), target_oren, True) + return go_to_point(game, motion_controller, robot_id, Vector2D(target_pos[0], target_pos[1]), dribbling=True) From c055a87e33cf6c5be6032db9bcc7122105c1d909 Mon Sep 17 00:00:00 2001 From: Yangping Li Date: Wed, 21 Jan 2026 16:45:16 +0000 Subject: [PATCH 34/48] minor: improve in algo --- utama_core/skills/src/defend_parameter.py | 73 ++++++++++++++++------- 1 file changed, 52 insertions(+), 21 deletions(-) diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py index 0e069e3f..a7210c40 100644 --- a/utama_core/skills/src/defend_parameter.py +++ b/utama_core/skills/src/defend_parameter.py @@ -1,3 +1,4 @@ +import math from typing import Optional import numpy as np @@ -20,43 +21,73 @@ def defend_parameter( defenseing_friendly = game.friendly_robots[robot_id] vel = game.ball.v.to_2d() - def positions_to_defend_parameter(x1, y1): - x2, y2 = 4.5, 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 or abs(y2 - y4) < 2: + 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 - def modified_pos(target_pos): - if target_pos[0] > 4.0: - if target_pos[1] < 1.0 and target_pos[1] >= 0: - if 4.5 - target_pos[0] < target_pos[1] - 1.0: - target_pos[0] = 4.0 - else: - target_pos[1] = 1.0 - elif target_pos[1] > -1.0 and target_pos[1] < 0: - if 4.5 - target_pos[0] < -1.0 - target_pos[1]: - target_pos[0] = 4.0 - else: - target_pos[1] = -1.0 - return target_pos - - x3, y3, x4, y4 = positions_to_defend_parameter(game.ball.p.x, game.ball.p.y) + # def modified_pos(target_pos): + # if target_pos[0] > 4.0: + # if target_pos[1] < 1.0 and target_pos[1] >= 0: + # if 4.5 - target_pos[0] < target_pos[1] - 1.0: + # target_pos[0] = 4.0 + # else: + # target_pos[1] = 1.0 + # elif target_pos[1] > -1.0 and target_pos[1] < 0: + # if 4.5 - target_pos[0] < -1.0 - target_pos[1]: + # target_pos[0] = 4.0 + # else: + # target_pos[1] = -1.0 + # return target_pos if vel[0] ** 2 + vel[1] ** 2 > 0.05: - target_pos = modified_pos(np.array([x4, y4])) + x2, y2 = 4.5, 0.5 + # target_pos = modified_pos(np.array([positions_to_defend_parameter(x2, y2)[2], positions_to_defend_parameter(x2, y2)[3]])) + target_pos = np.array([positions_to_defend_parameter(x2, y2)[2], positions_to_defend_parameter(x2, y2)[3]]) else: robot_rad = 0.09 - - vec_to_target = np.array([x4 - x3, y4 - y3]) + x2, y2 = 4.5, -0.5 + vec_to_target = np.array( + [ + positions_to_defend_parameter(x2, y2)[2] - positions_to_defend_parameter(x2, y2)[0], + positions_to_defend_parameter(x2, y2)[3] - positions_to_defend_parameter(x2, y2)[1], + ] + ) 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 = modified_pos(np.array([x4, y4]) - vec_dir * robot_rad) + # target_pos = modified_pos(np.array([positions_to_defend_parameter(x2, y2)[2], positions_to_defend_parameter(x2, y2)[3]]) - vec_dir * robot_rad) + target_pos = ( + np.array([positions_to_defend_parameter(x2, y2)[2], positions_to_defend_parameter(x2, y2)[3]]) + - vec_dir * robot_rad + ) return go_to_point(game, motion_controller, robot_id, Vector2D(target_pos[0], target_pos[1]), dribbling=True) From 527a3ef40d346922db58359a9bf95c124307b646 Mon Sep 17 00:00:00 2001 From: Yangping Li Date: Wed, 21 Jan 2026 16:50:33 +0000 Subject: [PATCH 35/48] minor: improve in algo --- utama_core/skills/src/defend_parameter.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py index a7210c40..09cefc5a 100644 --- a/utama_core/skills/src/defend_parameter.py +++ b/utama_core/skills/src/defend_parameter.py @@ -29,7 +29,7 @@ def positions_to_defend_parameter(x2, y2): y4 = y1 + t * (y2 - y1) def cal_xy5(xa, ya, xb, yb, w, x4, y4): - x5 = xa + w + x5 = xa - w dx = xb - xa if abs(dx) < 1e-12: return x4, y4 From d4970ec949824a3a4fbe7ea643a1171ac704fe1b Mon Sep 17 00:00:00 2001 From: Yangping Li Date: Wed, 21 Jan 2026 17:17:46 +0000 Subject: [PATCH 36/48] fix: algorithm problem --- utama_core/skills/src/defend_parameter.py | 30 ++++++----------------- 1 file changed, 7 insertions(+), 23 deletions(-) diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py index 09cefc5a..1467828f 100644 --- a/utama_core/skills/src/defend_parameter.py +++ b/utama_core/skills/src/defend_parameter.py @@ -40,7 +40,7 @@ def cal_xy5(xa, ya, xb, yb, w, x4, y4): def distance(x1, y1, x2, y2): return math.sqrt((x2 - x1) ** 2 + (y2 - y1) ** 2) - if abs(x2 - x4) < 1 or abs(y2 - y4) < 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): @@ -50,32 +50,19 @@ def distance(x1, y1, x2, y2): return x3, y3, x4, y4 - # def modified_pos(target_pos): - # if target_pos[0] > 4.0: - # if target_pos[1] < 1.0 and target_pos[1] >= 0: - # if 4.5 - target_pos[0] < target_pos[1] - 1.0: - # target_pos[0] = 4.0 - # else: - # target_pos[1] = 1.0 - # elif target_pos[1] > -1.0 and target_pos[1] < 0: - # if 4.5 - target_pos[0] < -1.0 - target_pos[1]: - # target_pos[0] = 4.0 - # else: - # target_pos[1] = -1.0 - # return target_pos - if vel[0] ** 2 + vel[1] ** 2 > 0.05: x2, y2 = 4.5, 0.5 - # target_pos = modified_pos(np.array([positions_to_defend_parameter(x2, y2)[2], positions_to_defend_parameter(x2, y2)[3]])) - target_pos = np.array([positions_to_defend_parameter(x2, y2)[2], positions_to_defend_parameter(x2, y2)[3]]) + x3, y3, x4, y4 = positions_to_defend_parameter(x2, y2) + target_pos = np.array([x4, y4]) else: robot_rad = 0.09 x2, y2 = 4.5, -0.5 + x3, y3, x4, y4 = positions_to_defend_parameter(x2, y2) vec_to_target = np.array( [ - positions_to_defend_parameter(x2, y2)[2] - positions_to_defend_parameter(x2, y2)[0], - positions_to_defend_parameter(x2, y2)[3] - positions_to_defend_parameter(x2, y2)[1], + x4 - x3, + y4 - y3, ] ) dist_to_target = np.linalg.norm(vec_to_target) @@ -85,9 +72,6 @@ def distance(x1, y1, x2, y2): else: vec_dir = np.array([0.0, 0.0]) # target_pos = modified_pos(np.array([positions_to_defend_parameter(x2, y2)[2], positions_to_defend_parameter(x2, y2)[3]]) - vec_dir * robot_rad) - target_pos = ( - np.array([positions_to_defend_parameter(x2, y2)[2], positions_to_defend_parameter(x2, y2)[3]]) - - vec_dir * robot_rad - ) + target_pos = np.array([x4, y4]) - vec_dir * robot_rad return go_to_point(game, motion_controller, robot_id, Vector2D(target_pos[0], target_pos[1]), dribbling=True) From c0249399e9e4491609d270e3f9516b229b8bd763 Mon Sep 17 00:00:00 2001 From: Ningchaun <1311856651@qq.com> Date: Sat, 24 Jan 2026 14:40:47 +0000 Subject: [PATCH 37/48] Fix: write the algorithm that let goalkeeper go between predict line and other robo --- utama_core/skills/src/goalkeep.py | 18 +++++++++++++++++- 1 file changed, 17 insertions(+), 1 deletion(-) diff --git a/utama_core/skills/src/goalkeep.py b/utama_core/skills/src/goalkeep.py index 54323eb7..2e2fd9a2 100644 --- a/utama_core/skills/src/goalkeep.py +++ b/utama_core/skills/src/goalkeep.py @@ -22,8 +22,24 @@ def goalkeep( 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) + return (x_line, y_intersect) + + x = game.friendly_robots[robot_id] + if len(game.friendly_robots) == 2: + _, 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 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) From 3a307b73320e7dd568e94a2f23c2fdcc302627a0 Mon Sep 17 00:00:00 2001 From: Ningchaun <1311856651@qq.com> Date: Sat, 24 Jan 2026 15:56:58 +0000 Subject: [PATCH 38/48] Feat: add the third defender --- utama_core/skills/src/defend_parameter.py | 25 +++++++++++++++++++---- utama_core/skills/src/goalkeep.py | 8 ++++++++ 2 files changed, 29 insertions(+), 4 deletions(-) diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py index 1467828f..afc1e198 100644 --- a/utama_core/skills/src/defend_parameter.py +++ b/utama_core/skills/src/defend_parameter.py @@ -20,7 +20,25 @@ def defend_parameter( ): 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 and robot_id == 1: + return go_to_point( + game, + motion_controller, + robot_id, + Vector2D(2, -2) + ) + elif game.ball.p.y < 0 and robot_id == 2: + return go_to_point( + game, + motion_controller, + robot_id, + Vector2D(2, 2) + ) + 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 @@ -51,13 +69,13 @@ def distance(x1, y1, x2, y2): return x3, y3, x4, y4 if vel[0] ** 2 + vel[1] ** 2 > 0.05: - x2, y2 = 4.5, 0.5 + x2, y2 = 4.5, goal_frame x3, y3, x4, y4 = positions_to_defend_parameter(x2, y2) target_pos = np.array([x4, y4]) else: robot_rad = 0.09 - x2, y2 = 4.5, -0.5 + x2, y2 = 4.5, -goal_frame x3, y3, x4, y4 = positions_to_defend_parameter(x2, y2) vec_to_target = np.array( [ @@ -71,7 +89,6 @@ def distance(x1, y1, x2, y2): vec_dir = vec_to_target / dist_to_target else: vec_dir = np.array([0.0, 0.0]) - # target_pos = modified_pos(np.array([positions_to_defend_parameter(x2, y2)[2], positions_to_defend_parameter(x2, y2)[3]]) - vec_dir * robot_rad) target_pos = np.array([x4, y4]) - vec_dir * robot_rad 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 2e2fd9a2..bb57fac1 100644 --- a/utama_core/skills/src/goalkeep.py +++ b/utama_core/skills/src/goalkeep.py @@ -32,12 +32,20 @@ def intersection_with_vertical_line(a, b, x_line=4.5): 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) x = game.friendly_robots[robot_id] if len(game.friendly_robots) == 2: _, 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 + if len(game.friendly_robots) > 2: + _, 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 if not target or abs(target[1]) > 0.5: target = Vector2D(4.5 if game.my_team_is_right else -4.5, stop_y) From fbf65d6d9e8c19a8f707aba6993d67786be18d29 Mon Sep 17 00:00:00 2001 From: Ningchaun <1311856651@qq.com> Date: Sat, 24 Jan 2026 16:09:51 +0000 Subject: [PATCH 39/48] Fix: better tribble defender --- utama_core/skills/src/defend_parameter.py | 12 +++++++----- utama_core/skills/src/goalkeep.py | 2 +- 2 files changed, 8 insertions(+), 6 deletions(-) diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py index afc1e198..3c5f3052 100644 --- a/utama_core/skills/src/defend_parameter.py +++ b/utama_core/skills/src/defend_parameter.py @@ -21,19 +21,21 @@ def defend_parameter( 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 and robot_id == 1: + if game.ball.p.y >= -0.5 and robot_id == 1: + target_pos = [3.0, -1.2] return go_to_point( game, motion_controller, robot_id, - Vector2D(2, -2) + Vector2D(target_pos[0], target_pos[1]) ) - elif game.ball.p.y < 0 and robot_id == 2: + elif game.ball.p.y < -0.5 and robot_id == 2: + target_pos = [3.0, 1.2] return go_to_point( game, motion_controller, robot_id, - Vector2D(2, 2) + Vector2D(target_pos[0], target_pos[1]) ) if robot_id == 1: goal_frame = 0.5 @@ -69,7 +71,7 @@ def distance(x1, y1, x2, y2): return x3, y3, x4, y4 if vel[0] ** 2 + vel[1] ** 2 > 0.05: - x2, y2 = 4.5, goal_frame + x2, y2 = 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]) diff --git a/utama_core/skills/src/goalkeep.py b/utama_core/skills/src/goalkeep.py index bb57fac1..68ab80e1 100644 --- a/utama_core/skills/src/goalkeep.py +++ b/utama_core/skills/src/goalkeep.py @@ -42,7 +42,7 @@ def intersection_with_vertical_line(a, b, x_line=4.5): if len(game.friendly_robots) == 2: _, 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 - if len(game.friendly_robots) > 2: + if len(game.friendly_robots) == 3: _, 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 From 93567000e573164af6fbcf28185a9ca1b4320136 Mon Sep 17 00:00:00 2001 From: Fred Huang Date: Sat, 24 Jan 2026 16:28:46 +0000 Subject: [PATCH 40/48] chore: linting --- utama_core/skills/src/defend_parameter.py | 15 +++------------ utama_core/skills/src/goalkeep.py | 15 ++++++++++----- 2 files changed, 13 insertions(+), 17 deletions(-) diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py index 3c5f3052..b78ec7f8 100644 --- a/utama_core/skills/src/defend_parameter.py +++ b/utama_core/skills/src/defend_parameter.py @@ -23,24 +23,15 @@ def defend_parameter( if len(game.friendly_robots) > 2: if game.ball.p.y >= -0.5 and robot_id == 1: target_pos = [3.0, -1.2] - return go_to_point( - game, - motion_controller, - robot_id, - Vector2D(target_pos[0], target_pos[1]) - ) + 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, 1.2] - return go_to_point( - game, - motion_controller, - robot_id, - Vector2D(target_pos[0], target_pos[1]) - ) + 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 diff --git a/utama_core/skills/src/goalkeep.py b/utama_core/skills/src/goalkeep.py index 68ab80e1..c62c6b01 100644 --- a/utama_core/skills/src/goalkeep.py +++ b/utama_core/skills/src/goalkeep.py @@ -37,14 +37,19 @@ def intersection_with_vertical_line(a, b, x_line=4.5): elif y_intersect > 0.5: return (x_line, 0.5) return (x_line, y_intersect) - - x = game.friendly_robots[robot_id] + if len(game.friendly_robots) == 2: - _, 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)) + _, 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 if len(game.friendly_robots) == 3: - _, 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)) + _, 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 if not target or abs(target[1]) > 0.5: target = Vector2D(4.5 if game.my_team_is_right else -4.5, stop_y) From 0dae44edb48c5e0b22405c428930ea0ce01659ee Mon Sep 17 00:00:00 2001 From: Ningchuan Wang <1311856651@qq.com> Date: Sat, 24 Jan 2026 16:44:19 +0000 Subject: [PATCH 41/48] Update utama_core/skills/src/goalkeep.py Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- utama_core/skills/src/goalkeep.py | 30 +++++++++++++++++++----------- 1 file changed, 19 insertions(+), 11 deletions(-) diff --git a/utama_core/skills/src/goalkeep.py b/utama_core/skills/src/goalkeep.py index c62c6b01..36d6a606 100644 --- a/utama_core/skills/src/goalkeep.py +++ b/utama_core/skills/src/goalkeep.py @@ -39,18 +39,26 @@ def intersection_with_vertical_line(a, b, x_line=4.5): return (x_line, y_intersect) if len(game.friendly_robots) == 2: - _, 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 + 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 if len(game.friendly_robots) == 3: - _, 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 + 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, stop_y) From eb0a3d0ea0fface2c41b4127c682cf1222b1ea8a Mon Sep 17 00:00:00 2001 From: Ningchuan Wang <1311856651@qq.com> Date: Sat, 24 Jan 2026 16:46:20 +0000 Subject: [PATCH 42/48] Update utama_core/skills/src/defend_parameter.py Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- utama_core/skills/src/defend_parameter.py | 6 ++++-- 1 file changed, 4 insertions(+), 2 deletions(-) diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py index b78ec7f8..f68a3264 100644 --- a/utama_core/skills/src/defend_parameter.py +++ b/utama_core/skills/src/defend_parameter.py @@ -22,10 +22,12 @@ def defend_parameter( 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, -1.2] + side_multiplier = 1.0 if getattr(game, "my_team_is_right", True) else -1.0 + target_pos = [3.0 * side_multiplier, -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, 1.2] + side_multiplier = 1.0 if getattr(game, "my_team_is_right", True) else -1.0 + target_pos = [3.0 * side_multiplier, 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 From 44e91ad0ecb505d7eb26ad235015305535028cff Mon Sep 17 00:00:00 2001 From: Ningchuan Wang <1311856651@qq.com> Date: Sat, 24 Jan 2026 16:47:02 +0000 Subject: [PATCH 43/48] Update utama_core/skills/src/defend_parameter.py Co-authored-by: Copilot <175728472+Copilot@users.noreply.github.com> --- utama_core/skills/src/defend_parameter.py | 1 - 1 file changed, 1 deletion(-) diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py index f68a3264..4e6c26e3 100644 --- a/utama_core/skills/src/defend_parameter.py +++ b/utama_core/skills/src/defend_parameter.py @@ -7,7 +7,6 @@ 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.run.predictors.position import predict_ball_pos_at_x from utama_core.skills.src.go_to_point import go_to_point from utama_core.skills.src.utils.move_utils import face_ball, move From ac6d7ca2b1b5e41260ea3f7b96e34443c2c58b7d Mon Sep 17 00:00:00 2001 From: Copilot <198982749+Copilot@users.noreply.github.com> Date: Sat, 24 Jan 2026 16:52:19 +0000 Subject: [PATCH 44/48] Remove unused imports from defend_parameter.py (#100) * Initial plan * Remove unused imports face_ball and move Co-authored-by: NingchuanIC <212761386+NingchuanIC@users.noreply.github.com> --------- Co-authored-by: copilot-swe-agent[bot] <198982749+Copilot@users.noreply.github.com> Co-authored-by: NingchuanIC <212761386+NingchuanIC@users.noreply.github.com> --- utama_core/skills/src/defend_parameter.py | 1 - 1 file changed, 1 deletion(-) diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py index 4e6c26e3..a5396f14 100644 --- a/utama_core/skills/src/defend_parameter.py +++ b/utama_core/skills/src/defend_parameter.py @@ -8,7 +8,6 @@ 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.move_utils import face_ball, move def defend_parameter( From 6ad963bb42bae2aa2a42bb88dda72ab45fc024bd Mon Sep 17 00:00:00 2001 From: Copilot <198982749+Copilot@users.noreply.github.com> Date: Sat, 24 Jan 2026 16:52:34 +0000 Subject: [PATCH 45/48] Fix goalkeep to handle teams with more than 3 robots (#99) * Initial plan * Fix: handle cases with more than 3 friendly robots in goalkeep Co-authored-by: NingchuanIC <212761386+NingchuanIC@users.noreply.github.com> --------- Co-authored-by: copilot-swe-agent[bot] <198982749+Copilot@users.noreply.github.com> Co-authored-by: NingchuanIC <212761386+NingchuanIC@users.noreply.github.com> --- utama_core/skills/src/goalkeep.py | 2 +- 1 file changed, 1 insertion(+), 1 deletion(-) diff --git a/utama_core/skills/src/goalkeep.py b/utama_core/skills/src/goalkeep.py index 36d6a606..65f692d2 100644 --- a/utama_core/skills/src/goalkeep.py +++ b/utama_core/skills/src/goalkeep.py @@ -47,7 +47,7 @@ def intersection_with_vertical_line(a, b, x_line=4.5): except (IndexError, KeyError): # If robot with ID 1 is not available, keep default stop_y pass - if len(game.friendly_robots) == 3: + 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) From 82d7be6b29c94c12f991a8b18bb3695c9067d130 Mon Sep 17 00:00:00 2001 From: Copilot <198982749+Copilot@users.noreply.github.com> Date: Sat, 24 Jan 2026 16:52:55 +0000 Subject: [PATCH 46/48] Remove dead code file goal_keep.py (#98) * Initial plan * Remove dead code file goal_keep.py Co-authored-by: NingchuanIC <212761386+NingchuanIC@users.noreply.github.com> --------- Co-authored-by: copilot-swe-agent[bot] <198982749+Copilot@users.noreply.github.com> Co-authored-by: NingchuanIC <212761386+NingchuanIC@users.noreply.github.com> --- utama_core/skills/src/goal_keep.py | 58 ------------------------------ 1 file changed, 58 deletions(-) delete mode 100644 utama_core/skills/src/goal_keep.py diff --git a/utama_core/skills/src/goal_keep.py b/utama_core/skills/src/goal_keep.py deleted file mode 100644 index ca060e98..00000000 --- a/utama_core/skills/src/goal_keep.py +++ /dev/null @@ -1,58 +0,0 @@ -from typing import Optional - -from utama_core.entities.data.command import RobotCommand -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 goal_keep( - game: Game, - motion_controller: MotionController, - defender_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 - 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) - if env: - env.draw_line( - [gp, (target_tracking_coord[0], target_tracking_coord[1])], - width=5, - color="RED" if tracking_ball else "PINK", - ) - - return cmd From 4930ce2a3b5f37df29835be3ea93abcd12872709 Mon Sep 17 00:00:00 2001 From: Copilot <198982749+Copilot@users.noreply.github.com> Date: Sat, 24 Jan 2026 16:53:38 +0000 Subject: [PATCH 47/48] Remove unused math import from standard_ssl.py (#97) * Initial plan * Remove unused math import from standard_ssl.py Co-authored-by: NingchuanIC <212761386+NingchuanIC@users.noreply.github.com> --------- Co-authored-by: copilot-swe-agent[bot] <198982749+Copilot@users.noreply.github.com> Co-authored-by: NingchuanIC <212761386+NingchuanIC@users.noreply.github.com> --- utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py | 1 - 1 file changed, 1 deletion(-) diff --git a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py index 8aba5e05..bfd9b1de 100644 --- a/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py +++ b/utama_core/rsoccer_simulator/src/ssl/envs/standard_ssl.py @@ -1,5 +1,4 @@ import logging -import math import random from typing import List, Tuple From 892ef5e55c859df221a7556fdb6175521f9bf165 Mon Sep 17 00:00:00 2001 From: Copilot <198982749+Copilot@users.noreply.github.com> Date: Sat, 24 Jan 2026 16:55:50 +0000 Subject: [PATCH 48/48] Fix hardcoded goal coordinates in defend_parameter.py for bidirectional play (#96) * Initial plan * Fix hardcoded goal x-coordinates in defend_parameter.py Co-authored-by: NingchuanIC <212761386+NingchuanIC@users.noreply.github.com> --------- Co-authored-by: copilot-swe-agent[bot] <198982749+Copilot@users.noreply.github.com> Co-authored-by: NingchuanIC <212761386+NingchuanIC@users.noreply.github.com> Co-authored-by: Fred Huang <60901360+fred-huang122@users.noreply.github.com> --- utama_core/skills/src/defend_parameter.py | 10 ++++------ 1 file changed, 4 insertions(+), 6 deletions(-) diff --git a/utama_core/skills/src/defend_parameter.py b/utama_core/skills/src/defend_parameter.py index a5396f14..6bb58296 100644 --- a/utama_core/skills/src/defend_parameter.py +++ b/utama_core/skills/src/defend_parameter.py @@ -20,12 +20,10 @@ def defend_parameter( vel = game.ball.v.to_2d() if len(game.friendly_robots) > 2: if game.ball.p.y >= -0.5 and robot_id == 1: - side_multiplier = 1.0 if getattr(game, "my_team_is_right", True) else -1.0 - target_pos = [3.0 * side_multiplier, -1.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])) elif game.ball.p.y < -0.5 and robot_id == 2: - side_multiplier = 1.0 if getattr(game, "my_team_is_right", True) else -1.0 - target_pos = [3.0 * side_multiplier, 1.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 @@ -62,13 +60,13 @@ def distance(x1, y1, x2, y2): return x3, y3, x4, y4 if vel[0] ** 2 + vel[1] ** 2 > 0.05: - x2, y2 = 4.5, goal_frame + 0.2 if robot_id == 1 else goal_frame - 0.2 + 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: robot_rad = 0.09 - x2, y2 = 4.5, -goal_frame + 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( [