diff --git a/utama_core/motion_planning/src/controllers/fastpathplanning.py b/utama_core/motion_planning/src/controllers/fastpathplanning.py index d4f07342..7a7703fb 100644 --- a/utama_core/motion_planning/src/controllers/fastpathplanning.py +++ b/utama_core/motion_planning/src/controllers/fastpathplanning.py @@ -1,6 +1,12 @@ +from abc import ABC, abstractmethod + +import numpy as np + from utama_core.config.enums import Mode +from utama_core.config.physical_constants import ROBOT_RADIUS from utama_core.entities.data.vector import Vector2D from utama_core.entities.game import Game +from utama_core.entities.game.field import Field from utama_core.motion_planning.src.common.motion_controller import MotionController from utama_core.motion_planning.src.fastpathplanning.planner import FastPathPlanner from utama_core.motion_planning.src.pid.pid import get_pids @@ -9,9 +15,10 @@ class FastPathPlanningController(MotionController): def __init__(self, mode: Mode, rsim_env: SSLStandardEnv | None = None): - super().__init__(mode, rsim_env) + self.mode = mode + self.rsim_env: SSLStandardEnv | None = rsim_env self.pid_oren, self.pid_trans = get_pids(mode) - self.fpp = FastPathPlanner(env=rsim_env) + self.fpp = FastPathPlanner(env=self.rsim_env) def calculate( self, @@ -20,14 +27,12 @@ def calculate( target_pos: Vector2D, target_oren: float, ) -> tuple[Vector2D, float]: - field = game.field - field_bounds = field.field_bounds robot = game.friendly_robots[robot_id] oren = self.pid_oren.calculate(target_oren, robot.orientation, robot_id) - pos = self.fpp._path_to(game, robot_id, target_pos, field_bounds) + pos = self.fpp._path_to(game, robot_id, target_pos) vel = self.pid_trans.calculate(pos, robot.p, robot_id) return vel, oren diff --git a/utama_core/motion_planning/src/fastpathplanning/config.py b/utama_core/motion_planning/src/fastpathplanning/config.py index 93d21229..285e3ca1 100644 --- a/utama_core/motion_planning/src/fastpathplanning/config.py +++ b/utama_core/motion_planning/src/fastpathplanning/config.py @@ -5,14 +5,14 @@ class fastpathplanningconfig: ROBOT_DIAMETER = 2 * ROBOT_RADIUS # how fat is the danger zone around obstacles, in multiples of robot diameter - CLEARANCE_MULTIPLIER = 1.5 + CLEARANCE_MULTIPLIER = 1.1 OBSTACLE_CLEARANCE = ROBOT_DIAMETER * CLEARANCE_MULTIPLIER # How far outside the danger zone shold the waypoint be - SUBGOAL_MULTIPLIER = 1.2 + SUBGOAL_MULTIPLIER = 1.1 SUBGOAL_DISTANCE = OBSTACLE_CLEARANCE * SUBGOAL_MULTIPLIER LOOK_AHEAD_RANGE = 3 MAXRECURSION_LENGTH = 3 PROJECTEDFRAMES = 20 - PROJECTION_DISTANCE = 1 + PROJECTION_DISTANCE = 2 diff --git a/utama_core/motion_planning/src/fastpathplanning/planner.py b/utama_core/motion_planning/src/fastpathplanning/planner.py index 0ec7424b..0be036ed 100644 --- a/utama_core/motion_planning/src/fastpathplanning/planner.py +++ b/utama_core/motion_planning/src/fastpathplanning/planner.py @@ -5,7 +5,7 @@ from utama_core.config.settings import CONTROL_FREQUENCY from utama_core.entities.game import Game -from utama_core.entities.game.field import FieldBounds +from utama_core.entities.game.field import Field, FieldBounds from utama_core.global_utils.math_utils import ( closest_point_on_segment, distance, @@ -42,8 +42,42 @@ def is_point_in_field(self, point, field_bounds: FieldBounds) -> bool: max_y = max(field_bounds.top_left[1], field_bounds.bottom_right[1]) return min_x <= x <= max_x and min_y <= y <= max_y + def is_point_in_defense_area( + self, + point: np.ndarray | Tuple[float, float], + defense_area: np.ndarray, + ) -> bool: + """Return True if a point lies inside the rectangular defense area.""" + x, y = float(point[0]), float(point[1]) + min_x = float(np.min(defense_area[:, 0])) + max_x = float(np.max(defense_area[:, 0])) + min_y = float(np.min(defense_area[:, 1])) + max_y = float(np.max(defense_area[:, 1])) + return min_x <= x <= max_x and min_y <= y <= max_y + + def _add_opponent_defense_area_obstacles( + self, + robot_pos: np.ndarray, + defense_area: np.ndarray, + obstacle_list: List[Tuple[np.ndarray, np.ndarray]], + ) -> List[Tuple[np.ndarray, np.ndarray]]: + """Add opponent defense area boundary segments to the obstacle list.""" + if defense_area.shape[0] < 4: + return obstacle_list + + tl, tr, br, bl = defense_area + if distance_point_to_segment(robot_pos, tl, tr) < self.LOOK_AHEAD_RANGE: + obstacle_list.append((tl, tr)) + if distance_point_to_segment(robot_pos, tr, br) < self.LOOK_AHEAD_RANGE: + obstacle_list.append((tr, br)) + if distance_point_to_segment(robot_pos, br, bl) < self.LOOK_AHEAD_RANGE: + obstacle_list.append((br, bl)) + if distance_point_to_segment(robot_pos, bl, tl) < self.LOOK_AHEAD_RANGE: + obstacle_list.append((bl, tl)) + return obstacle_list + def _get_obstacles( - self, game: Game, robot_id: int, our_pos: np.ndarray, field_bounds: FieldBounds + self, game: Game, robot_id: int, our_pos: np.ndarray, field_bounds: FieldBounds, defense_area: np.ndarray ) -> List[Tuple[np.ndarray, np.ndarray]]: """ Compiles obstacles and draws projected velocity lines in Red. @@ -62,16 +96,23 @@ def _get_obstacles( obstacle_list.append(obstacle_segment) - # DRAWING: Show the projected velocity line in Red when an RSim renderer is available. - if self._env is not None: - self._env.draw_line(obstacle_segment, color="Red") - # Field bounds as obstacles (static, usually not drawn to keep screen clean) tl, br = np.array(field_bounds.top_left), np.array(field_bounds.bottom_right) tr = np.array([field_bounds.bottom_right[0], field_bounds.top_left[1]]) bl = np.array([field_bounds.top_left[0], field_bounds.bottom_right[1]]) - obstacle_list.extend([(tl, tr), (tr, br), (br, bl), (bl, tl)]) + if distance_point_to_segment(our_pos, tl, tr) < self.LOOK_AHEAD_RANGE: + obstacle_list.append((tl, tr)) + if distance_point_to_segment(our_pos, tr, br) < self.LOOK_AHEAD_RANGE: + obstacle_list.append((tr, br)) + if distance_point_to_segment(our_pos, br, bl) < self.LOOK_AHEAD_RANGE: + obstacle_list.append((br, bl)) + if distance_point_to_segment(our_pos, bl, tl) < self.LOOK_AHEAD_RANGE: + obstacle_list.append((bl, tl)) + + # Add opponent defense area boundaries as static obstacles. + obstacle_list = self._add_opponent_defense_area_obstacles(our_pos, defense_area, obstacle_list) + return obstacle_list def _find_subgoal( @@ -156,6 +197,7 @@ def check_segment( recursion_length: int, target: np.ndarray, field_bounds: FieldBounds, + defense_area: np.ndarray, ) -> Tuple[List[Tuple[np.ndarray, np.ndarray]], float]: """ Recursively checks a segment for collisions and generates subgoals with @@ -174,39 +216,46 @@ def check_segment( left_valid = self.is_point_in_field(subgoal_left, field_bounds) right_valid = self.is_point_in_field(subgoal_right, field_bounds) - + left_valid_defense_area = not self.is_point_in_defense_area(subgoal_left, defense_area) + right_valid_defense_area = not self.is_point_in_defense_area(subgoal_right, defense_area) best_subgoal = None - # Heuristic: Pick the valid subgoal closest to the ultimate destination - if left_valid and right_valid: - if distance(subgoal_left, target) < distance(subgoal_right, target): - best_subgoal = subgoal_left - else: - best_subgoal = subgoal_right - elif left_valid: + if left_valid and left_valid_defense_area and right_valid and right_valid_defense_area: + pass + elif left_valid and left_valid_defense_area: best_subgoal = subgoal_left - elif right_valid: + elif right_valid and right_valid_defense_area: best_subgoal = subgoal_right else: return [segment], segment_length + if best_subgoal is not None: + seg1, len1 = self.check_segment( + (segment[0], best_subgoal), obstacles, recursion_length + 1, target, field_bounds, defense_area + ) + seg2, len2 = self.check_segment( + (best_subgoal, segment[1]), obstacles, recursion_length + 1, target, field_bounds, defense_area + ) + return seg1 + seg2, len1 + len2 - # Recursively check the two halves of the selected detour - seg1, len1 = self.check_segment( - (segment[0], best_subgoal), - obstacles, - recursion_length + 1, - target, - field_bounds, - ) - seg2, len2 = self.check_segment( - (best_subgoal, segment[1]), - obstacles, - recursion_length + 1, - target, - field_bounds, - ) - - return seg1 + seg2, len1 + len2 + else: + left_seg1, left_len1 = self.check_segment( + (segment[0], subgoal_left), obstacles, recursion_length + 1, target, field_bounds, defense_area + ) + left_seg2, left_len2 = self.check_segment( + (subgoal_left, segment[1]), obstacles, recursion_length + 1, target, field_bounds, defense_area + ) + right_seg1, right_len1 = self.check_segment( + (segment[0], subgoal_right), obstacles, recursion_length + 1, target, field_bounds, defense_area + ) + right_seg2, right_len2 = self.check_segment( + (subgoal_right, segment[1]), obstacles, recursion_length + 1, target, field_bounds, defense_area + ) + left_len = left_len1 + left_len2 + right_len = right_len1 + right_len2 + if left_len < right_len: + return left_seg1 + left_seg2, left_len + else: + return right_seg1 + right_seg2, right_len def smooth_path(self, trajectory, target, robot_position) -> np.ndarray: if len(trajectory) == 1: @@ -215,11 +264,10 @@ def smooth_path(self, trajectory, target, robot_position) -> np.ndarray: direction = trajectory[0][1] - robot_position unit_vec = direction / np.linalg.norm(direction) new_target = robot_position + unit_vec * self.PROJECTION_DISTANCE - + return new_target # Removed redundant math ops by caching distance calls here too dist_new_target = distance(new_target, robot_position) dist_trajectory = distance(robot_position, trajectory[0][1]) - if dist_new_target < dist_trajectory: return trajectory[0][1] else: @@ -240,7 +288,7 @@ def sanitize_target(self, target: np.ndarray, obstacles: List, robot_pos: np.nda if np.linalg.norm(push_dir) == 0: push_dir = robot_pos - closest_pt unit_push = push_dir / np.linalg.norm(push_dir) - safe_target = closest_pt + unit_push * (self.OBSTACLE_CLEARANCE * 1.05) + safe_target = closest_pt - unit_push * (self.OBSTACLE_CLEARANCE * 1.05) collision_found = True if not collision_found: break @@ -251,7 +299,6 @@ def _path_to( game: Game, robot_id: int, target: Tuple[float, float], - field_bounds: FieldBounds, ): """ Main entry point. Clears cache, sanitizes target, and plans path. @@ -261,15 +308,19 @@ def _path_to( robot = game.friendly_robots[robot_id] our_pos = np.array([robot.p.x, robot.p.y]) raw_target = np.array(target) + field_bounds = game.field.field_bounds + defense_area = game.field.enemy_defense_area # 1. Get obstacles and draw Red velocity lines - obstacles = self._get_obstacles(game, robot_id, our_pos, field_bounds) + obstacles = self._get_obstacles(game, robot_id, our_pos, field_bounds, defense_area) # 3. Sanitize target (Critical for velocity obstacles) safe_target = self.sanitize_target(raw_target, obstacles, our_pos) # 4. Plan geometric path - final_trajectory, _ = self.check_segment((our_pos, safe_target), obstacles, 0, safe_target, field_bounds) + final_trajectory, _ = self.check_segment( + (our_pos, safe_target), obstacles, 0, safe_target, field_bounds, defense_area + ) # 5. Draw the resulting safe path segments when an RSim renderer is available. if self._env is not None: