Feature/fastpathplanning

.gitignore

@@ -215,3 +215,6 @@ outputs/
 wandb/
 mappo_*/
 *.gif
+
+# macOS
+.DS_Store 

conftest.py

@@ -59,14 +59,14 @@ def pytest_generate_tests(metafunc):
 
 
 # temporarily excludes the motion planning tests until the motion planning algorithms are working
-def pytest_collection_modifyitems(config, items):
-    if config.getoption("--include-motion-planning"):
-        return
+# def pytest_collection_modifyitems(config, items):
+#     if config.getoption("--include-motion-planning"):
+#         return
 
-    skip_mp = pytest.mark.skip(reason="motion planning not ready")
-    for item in items:
-        if "motion_planning" in item.keywords:
-            item.add_marker(skip_mp)
+#     skip_mp = pytest.mark.skip(reason="motion planning not ready")
+#     for item in items:
+#         if "motion_planning" in item.keywords:
+#             item.add_marker(skip_mp)
 
 
 @pytest.fixture

utama_core/global_utils/math_utils.py

@@ -5,6 +5,8 @@
 from utama_core.entities.data.vector import Vector2D
 from utama_core.entities.game.field import Field, FieldBounds
 
+EPS = 1e-9
+
 
 def rotate_vector(vx_global: float, vy_global: float, theta: float) -> Tuple[float, float]:
     """Rotates a 2D vector from global coordinates to local coordinates based on a given angle.
@@ -170,3 +172,209 @@ def assert_contains(outer: FieldBounds, inner: FieldBounds):
         raise ValueError(f"Outer right {ox1} does not contain inner right {ix1}")
     if oy1 > iy1:
         raise ValueError(f"Outer bottom {oy1} does not contain inner bottom {iy1}")
+
+
+def distance_between_line_segments(
+    seg1_start: np.ndarray,
+    seg1_end: np.ndarray,
+    seg2_start: np.ndarray,
+    seg2_end: np.ndarray,
+) -> float:
+    """Calculate the minimum distance between two line segments in 2D space.
+
+    Args:
+        seg1_start (tuple): A tuple representing the start of the first line segment (x1, y1).
+        seg1_end (tuple): A tuple representing the end of the first line segment (x2, y2).
+        seg2_start (tuple): A tuple representing the start of the second line segment (x3, y3).
+        seg2_end (tuple): A tuple representing the end of the second line segment (x4, y4).
+    Returns:
+        float: The minimum distance between the two line segments.
+    """
+    if segments_intersect(seg1_start, seg1_end, seg2_start, seg2_end):
+        return 0.0
+
+    return min(
+        distance_point_to_segment(seg1_start, seg2_start, seg2_end),
+        distance_point_to_segment(seg1_end, seg2_start, seg2_end),
+        distance_point_to_segment(seg2_start, seg1_start, seg1_end),
+        distance_point_to_segment(seg2_end, seg1_start, seg1_end),
+    )
+
+
+def distance_point_to_segment(point: np.ndarray, seg_start: np.ndarray, seg_end: np.ndarray) -> float:
+    """Calculate the minimum distance from a point to a line segment in 2D space.
+
+    Args:
+        point (tuple): A tuple representing the point (px, py).
+        seg_start (tuple): A tuple representing the start of the segment (x1, y1).
+        seg_end (tuple): A tuple representing the end of the segment (x2, y2).
+    Returns:
+        float: The minimum distance from the point to the line segment.
+    """
+    point = np.asarray(point)
+    seg_start = np.asarray(seg_start)
+    seg_end = np.asarray(seg_end)
+
+    seg_vec = seg_end - seg_start
+    pt_vec = point - seg_start
+
+    seg_len_sq = np.dot(seg_vec, seg_vec)
+
+    if seg_len_sq < EPS:
+        return np.linalg.norm(point - seg_start)
+
+    t = np.dot(pt_vec, seg_vec) / seg_len_sq
+
+    if t < 0:
+        closest = seg_start
+    elif t > 1:
+        closest = seg_end
+    else:
+        closest = seg_start + t * seg_vec
+
+    return np.linalg.norm(point - closest)
+
+
+def closest_point_on_segment(point, seg_start, seg_end):
+    """Calculate the point on a segment closest to another point.
+
+    Args:
+        point (tuple): A tuple representing the point (px, py).
+        seg_start (tuple): A tuple representing the start of the segment (x1, y1).
+        seg_end (tuple): A tuple representing the end of the segment (x2, y2).
+    Returns:
+        np.ndarray: An np array representing the closest point on the segment.
+    """
+    point = np.asarray(point)
+    seg_start = np.asarray(seg_start)
+    seg_end = np.asarray(seg_end)
+
+    seg_vec = seg_end - seg_start
+    pt_vec = point - seg_start
+
+    seg_len_sq = np.dot(seg_vec, seg_vec)
+
+    if seg_len_sq < EPS:
+        return seg_start
+
+    t = np.dot(pt_vec, seg_vec) / seg_len_sq
+
+    if t < 0:
+        return seg_start
+    elif t > 1:
+        return seg_end
+    else:
+        return seg_start + t * seg_vec
+
+
+def segments_intersect(
+    seg1_start: np.ndarray,
+    seg1_end: np.ndarray,
+    seg2_start: np.ndarray,
+    seg2_end: np.ndarray,
+):
+    """Check if two line segments intersect.
+
+    Args:
+        seg1_start (tuple): ((x1, y1), (x2, y2))
+        seg1_end (tuple): ((x3, y3), (x4, y4))
+        seg2_start (tuple): ((x5, y5), (x6, y6))
+        seg2_end (tuple): ((x7, y7), (x8, y8))
+    Returns:
+        bool: True if the segments intersect, False otherwise.
+    """
+    p1 = np.asarray(seg1_start)
+    q1 = np.asarray(seg1_end)
+    p2 = np.asarray(seg2_start)
+    q2 = np.asarray(seg2_end)
+
+    o1 = point_orientation(p1, q1, p2)
+    o2 = point_orientation(p1, q1, q2)
+    o3 = point_orientation(p2, q2, p1)
+    o4 = point_orientation(p2, q2, q1)
+
+    if o1 != o2 and o3 != o4:
+        return True
+
+    if o1 == 0 and on_segment(p1, p2, q1):
+        return True
+    if o2 == 0 and on_segment(p1, q2, q1):
+        return True
+    if o3 == 0 and on_segment(p2, p1, q2):
+        return True
+    if o4 == 0 and on_segment(p2, q1, q2):
+        return True
+
+    return False
+
+
+def point_orientation(p_1: np.ndarray, p_2: np.ndarray, p_3: np.ndarray) -> int:
+    """Calculate the orientation of 3 points (e.g. on a line or in a triangle).
+
+    Args:
+        p_1 (np.ndarray): First point as (x, y).
+        p_2 (np.ndarray): Second point as (x, y).
+        p_3 (np.ndarray): Third point as (x, y).
+
+    Returns:
+        int: 0 if collinear, 1 if clockwise, 2 if counterclockwise.
+    """
+    p1 = np.asarray(p_1)
+    p2 = np.asarray(p_2)
+    p3 = np.asarray(p_3)
+
+    val = (p2[0] - p1[0]) * (p3[1] - p1[1]) - (p2[1] - p1[1]) * (p3[0] - p1[0])
+
+    if abs(val) < EPS:
+        return 0
+
+    return 1 if val < 0 else 2
+
+
+def on_segment(p: np.ndarray, q: np.ndarray, r: np.ndarray) -> bool:
+    """Check if point q lies on line segment 'pr'.
+
+    Args:
+        p (np.ndarray): Start point of segment as (x, y).
+        q (np.ndarray): Point to check as (x, y).
+        r (np.ndarray): End point of segment as (x, y).
+
+    Returns:
+        bool: True if q lies on segment pr, False otherwise.
+    """
+    p = np.asarray(p)
+    q = np.asarray(q)
+    r = np.asarray(r)
+
+    return (
+        min(p[0], r[0]) - EPS <= q[0] <= max(p[0], r[0]) + EPS
+        and min(p[1], r[1]) - EPS <= q[1] <= max(p[1], r[1]) + EPS
+    )
+
+
+def find_intersection(line1, line2):
+    """
+    Find the intersection point of two line segments.
+
+    Args:
+        line1: tuple of two np.arrays (start, end) -> (A, B)
+        line2: tuple of two np.arrays (start, end) -> (C, D)
+
+    Returns:
+        np.array of intersection point (x, y), or None if no intersection.
+    """
+    A, B = np.asarray(line1[0]), np.asarray(line1[1])
+    C, D = np.asarray(line2[0]), np.asarray(line2[1])
+
+    denom = (B[0] - A[0]) * (D[1] - C[1]) - (B[1] - A[1]) * (D[0] - C[0])
+
+    if abs(denom) < EPS:
+        return None
+
+    t = ((C[0] - A[0]) * (D[1] - C[1]) - (C[1] - A[1]) * (D[0] - C[0])) / denom
+    u = ((C[0] - A[0]) * (B[1] - A[1]) - (C[1] - A[1]) * (B[0] - A[0])) / denom
+
+    if -EPS <= t <= 1 + EPS and -EPS <= u <= 1 + EPS:
+        return A + t * (B - A)
+
+    return None

utama_core/motion_planning/src/common/control_schemes.py

@@ -1,9 +1,13 @@
 from typing import Type
 
 from utama_core.motion_planning.src.common.motion_controller import MotionController
-from utama_core.motion_planning.src.controllers import DWAController, PIDController
+from utama_core.motion_planning.src.controllers import (
+    DWAController,
+    FastPathPlanningController,
+    PIDController,
+)
 
-CONTROL_SCHEME_MAP = {"pid": PIDController, "dwa": DWAController}
+CONTROL_SCHEME_MAP = {"pid": PIDController, "dwa": DWAController, "fpp": FastPathPlanningController}
 
 
 def get_control_scheme(scheme_name: str) -> Type[MotionController]:

utama_core/motion_planning/src/controllers/__init__.py

@@ -1,2 +1,5 @@
 from utama_core.motion_planning.src.controllers.dwa_controller import DWAController
+from utama_core.motion_planning.src.controllers.fastpathplanning import (
+    FastPathPlanningController,
+)
 from utama_core.motion_planning.src.controllers.pid_controller import PIDController

utama_core/motion_planning/src/controllers/fastpathplanning.py

@@ -0,0 +1,44 @@
+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
+from utama_core.rsoccer_simulator.src.ssl.envs import SSLStandardEnv
+
+
+class FastPathPlanningController(MotionController):
+    def __init__(self, mode: Mode, rsim_env: SSLStandardEnv | None = None):
+        self.mode = mode
+        self.rsim_env: SSLStandardEnv | None = rsim_env
+        self.pid_oren, self.pid_trans = get_pids(mode)
+        self.fpp = FastPathPlanner(env=self.rsim_env)
+
+    def calculate(
+        self,
+        game: Game,
+        robot_id: int,
+        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)
+        vel = self.pid_trans.calculate(pos, robot.p, robot_id)
+
+        return vel, oren
+
+    def reset(self, robot_id):
+        self.pid_oren.reset(robot_id)
+        self.pid_trans.reset(robot_id)

utama_core/motion_planning/src/fastpathplanning/config.py

@@ -0,0 +1,18 @@
+from utama_core.config.physical_constants import ROBOT_RADIUS
+
+
+class fastpathplanningconfig:
+    ROBOT_DIAMETER = 2 * ROBOT_RADIUS
+
+    # how fat is the danger zone around obstacles, in multiples of robot diameter
+    CLEARANCE_MULTIPLIER = 1.5
+    OBSTACLE_CLEARANCE = ROBOT_DIAMETER * CLEARANCE_MULTIPLIER
+
+    # How far outside the danger zone shold the waypoint be
+    SUBGOAL_MULTIPLIER = 1.2
+    SUBGOAL_DISTANCE = OBSTACLE_CLEARANCE * SUBGOAL_MULTIPLIER
+
+    LOOK_AHEAD_RANGE = 3
+    MAXRECURSION_LENGTH = 3
+    PROJECTEDFRAMES = 20
+    PROJECTION_DISTANCE = 1