python-motion-planning 2.0.dev2__py3-none-any.whl → 2.0.1__py3-none-any.whl

python_motion_planning/common/utils/geometry.py

@@ -35,42 +35,31 @@ class Geometry:
         else:
             raise ValueError("Invalid distance type")
 
-    # @staticmethod
-    # def angle(v1: tuple, v2: tuple) -> float:
-    #     """
-    #     Calculate the angle between two vectors
-
-    #     Args:
-    #         v1: First vector
-    #         v2: Second vector
-
-    #     Returns:
-    #         angle_rad: Angle in rad between the two vectors
-    #     """
-    #     if len(v1) != len(v2):
-    #         raise ValueError("Dimension mismatch")
-        
-    #     dot_product = sum(a * b for a, b in zip(v1, v2))
-    #     v1_norm = math.sqrt(sum(a**2 for a in v1))
-    #     v2_norm = math.sqrt(sum(b**2 for b in v2))
-        
-    #     if  v1_norm == 0 or v2_norm == 0:
-    #         raise ValueError("Zero vector cannot calculate angle")
-
-    #     cos_theta = dot_product / (v1_norm * v2_norm)
+    @staticmethod
+    def mod_to_2pi(orient: np.ndarray) -> np.ndarray:
+        """
+        Regularize orientation to be within [0, 2*pi)
 
-    #     cos_theta = min(1.0, max(-1.0, cos_theta))
+        Args:
+            orient: the orientation angle
 
-    #     angle_rad = math.acos(cos_theta)
-        
-    #     return angle_rad
+        Returns:
+            new_orient: modded orientation
+        """
+        return np.mod(orient, 2 * np.pi)
 
     @staticmethod
     def regularize_orient(orient: np.ndarray) -> np.ndarray:
         """
         Regularize orientation to be within (-pi, pi]
+
+        Args:
+            orient: the orientation angle
+
+        Returns:
+            new_orient: regularized orientation
         """
-        return np.mod(orient + np.pi, 2 * np.pi) - np.pi
+        return -np.mod(-orient + np.pi, 2 * np.pi) + np.pi
         
     @staticmethod
     def add_orient_to_2d_path(path: List[Tuple[float, float]]) -> List[Tuple[float, float, float]]:
@@ -102,4 +91,4 @@ class Geometry:
         last_x, last_y = path[-1]
         path_with_orient.append((last_x, last_y, path_with_orient[-1][2]))
         
-        return path_with_orient
+        return path_with_orient

python_motion_planning/path_planner/sample_search/rrt.py

@@ -1,7 +1,7 @@
 """
 @file: rrt.py
 @author: Wu Maojia, Yang Haodong
-@update: 2025.12.19
+@update: 2026.4.12
 """
 import math
 import random
@@ -20,7 +20,7 @@ class RRT(BasePathPlanner):
     Args:
         *args: see the parent class.
         max_dist: Maximum expansion distance for each step.
-        sample_num: Maximum number of samples to generate.
+        max_sample_step: Maximum number of steps of samples to generate.
         goal_sample_rate: Probability of sampling the goal directly.
         discrete: Whether to use discrete or continuous space.
         faiss: Whether to use Faiss to accelerate the search.
@@ -42,14 +42,14 @@ class RRT(BasePathPlanner):
         True
     """
     def __init__(self, *args, 
-                 max_dist: float = 5.0, sample_num: int = 100000, 
+                 max_dist: float = 5.0, max_sample_step: int = 100000, 
                  goal_sample_rate: float = 0.05,
                  discrete: bool = False,
                  use_faiss: bool = True,
                  **kwargs) -> None:
         super().__init__(*args, **kwargs)
         self.max_dist = max_dist
-        self.sample_num = sample_num
+        self.max_sample_step = max_sample_step
         self.goal_sample_rate = goal_sample_rate
         self.discrete = discrete
         self.use_faiss = use_faiss
@@ -77,7 +77,7 @@ class RRT(BasePathPlanner):
             self._faiss_add_node(start_node, faiss_index, faiss_nodes)
 
         # Main sampling loop
-        for _ in range(self.sample_num):
+        for _ in range(self.max_sample_step):
             # Generate random sample node
             node_rand = self._generate_random_node()
             

python_motion_planning/path_planner/sample_search/rrt_connect.py

@@ -1,7 +1,7 @@
 """
 @file: rrt_connect.py
 @author: Wu Maojia
-@update: 2025.12.19
+@update: 2026.4.12
 """
 from typing import Union, Dict, List, Tuple, Any
 
@@ -73,7 +73,7 @@ class RRTConnect(RRT):
             self._faiss_add_node(self.tree_b[self.goal], self.index_b, self.nodes_b)
 
         # Main planning loop
-        for _ in range(self.sample_num):
+        for _ in range(self.max_sample_step):
             # Generate random sample node
             node_rand = self._generate_random_node()
             

python_motion_planning/path_planner/sample_search/rrt_star.py

@@ -1,7 +1,7 @@
 """
 @file: rrt_star.py
 @author: Wu Maojia
-@update: 2025.12.19
+@update: 2026.4.12
 """
 import math
 import random
@@ -27,7 +27,12 @@ class RRTStar(RRT):
         *args: see parent class.
         rewire_radius: Neighborhood radius for rewiring (If None, adaptively calculated with gamma factor).
         gamma: A factor for calculating rewire_radius. For details, see [1]. (Disabled when rewire_radius is not None)
-        stop_until_sample_num: Stop until sample number limitation is reached, otherwise stop when goal is found.
+        stop_func: A callable(cur_step, first_success_step, max_sample_step) -> bool that controls when to stop sampling. Called at the beginning of each iteration.
+            Arguments:
+                - cur_step (int): Number of sampling iterations completed so far (starts from 0, incremented before each sampling, so the first completed sample has cur_step=1).
+                - first_success_step (int or None): The cur_step value at which a feasible path was first found, or None if no path has been found yet.
+                - max_sample_step (int): The planner's max_sample_step attribute, provided for convenience.
+            Returns True to stop, False to continue.
         propagate_cost_to_children: Whether to propagate cost to children. This is a fix for ensuring the correctness of g-value of each node. But it may slow the algorithm a little.
         *kwargs: see parent class.
 
@@ -41,7 +46,7 @@ class RRTStar(RRT):
         >>> print(path_info['success'])
         True
         
-        >>> planner = RRTStar(map_=map_, start=(5, 5), goal=(10, 10), sample_num=1000, stop_until_sample_num=True)
+        >>> planner = RRTStar(map_=map_, start=(5, 5), goal=(10, 10), max_sample_step=100000, stop_func=lambda current_step, first_success_step, max_step: (first_success_step is not None) or (current_step >= max_step))
         >>> path, path_info = planner.plan()
         >>> print(path_info['success'])
         True
@@ -50,13 +55,18 @@ class RRTStar(RRT):
     def __init__(self, *args,
                  rewire_radius: float = None,
                  gamma: float = 50.0,
-                 stop_until_sample_num: int = False,
+                 stop_func: callable = lambda current_step, first_success_step, max_step: (first_success_step is not None) or (current_step >= max_step),
                  propagate_cost_to_children: bool = True,
                  **kwargs) -> None:
         super().__init__(*args, **kwargs)
         self.rewire_radius = rewire_radius
         self.gamma = gamma
-        self.stop_until_sample_num = stop_until_sample_num
+        self.stop_func = stop_func
+
+        self.failed_info[1]["first_success_step"] = None
+        self.failed_info[1]["best_step"] = None
+        self.failed_info[1]["total_step"] = None
+
         self.best_results = self.failed_info
         self.propagate_cost_to_children = propagate_cost_to_children
 
@@ -86,7 +96,14 @@ class RRTStar(RRT):
             faiss_nodes = []
             self._faiss_add_node(start_node, faiss_index, faiss_nodes)
 
-        for i in range(self.sample_num):
+        i = 0
+        first_success_i = None
+        while True:
+            if self.stop_func(i, first_success_i, self.max_sample_step):
+                break
+                
+            i += 1
+
             # Generate random sample
             node_rand = self._generate_random_node()
 
@@ -179,22 +196,25 @@ class RRTStar(RRT):
                                 0
                             )
                         path, length, cost = self.extract_path(self._tree)
+                        
+                        if first_success_i is None:
+                            first_success_i = i
+
                         self.best_results = path, {
                             "success": True,
                             "start": self.start,
                             "goal": self.goal,
                             "length": length,
                             "cost": cost,
+                            "first_success_step": first_success_i,
+                            "best_step": i,
+                            "total_step": i,
                             "expand": self._tree,
                         }
 
-                        if not self.stop_until_sample_num:
-                            return self.best_results
-
-        n = len(self._tree) + 1
-        radius = self.gamma * ((math.log(n) / n) ** (1 / self.dim))
 
         # Planning stopped
+        self.best_results[1]["total_step"] = i
         self.best_results[1]["expand"] = self._tree
         return self.best_results
 

python_motion_planning/traj_optimizer/__init__.py

@@ -0,0 +1,2 @@
+from .base_curve_generator import *
+from .curve_generator import *

python_motion_planning/traj_optimizer/base_curve_generator.py

@@ -0,0 +1,53 @@
+"""
+@file: base_curve_generator.py
+@author: Yang Haodong, Wu Maojia
+@update: 2026.4.12
+"""
+from typing import List, Tuple, Dict, Any
+from abc import ABC, abstractmethod
+import math
+
+
+class BaseCurveGenerator(ABC):
+    """
+    Base class for curve generator (trajectory smoother).
+
+    Args:
+        step: Step size for interpolation or discretization along the curve.
+    """
+    def __init__(self, step: float = 0.01) -> None:
+        super().__init__()
+        self.step = step
+
+    def __str__(self) -> str:
+        return "Base Curve Generator"
+
+    @abstractmethod
+    def generate(self, points: List[Tuple[float, ...]]) -> Tuple[List[Tuple[float, ...]], Dict[str, Any]]:
+        """
+        Interface for curve generation.
+
+        Args:
+            points: A list of waypoints in world frame. The exact format (2D position
+                or 2D pose with orientation) depends on the concrete generator.
+
+        Returns:
+            path: A list containing the smoothed path waypoints in world frame.
+            curve_info: A dictionary containing the curve information (success, length).
+        """
+        raise NotImplementedError
+
+    def length(self, path: List[Tuple[float, ...]]) -> float:
+        """
+        Calculate the length of a path.
+
+        Args:
+            path: A list containing the path waypoints.
+
+        Returns:
+            length: Length of the path.
+        """
+        dist = 0.0
+        for i in range(len(path) - 1):
+            dist += math.hypot(path[i + 1][0] - path[i][0], path[i + 1][1] - path[i][1])
+        return dist

python_motion_planning/traj_optimizer/curve_generator/__init__.py

@@ -0,0 +1,2 @@
+from .point_based import *
+from .pose_based import *

python_motion_planning/traj_optimizer/curve_generator/point_based/__init__.py

@@ -0,0 +1,2 @@
+from .bspline import *
+from .cubic_spline import *

python_motion_planning/traj_optimizer/curve_generator/point_based/bspline.py

@@ -0,0 +1,256 @@
+"""
+@file: bspline_curve.py
+@author: Yang Haodong, Wu Maojia
+@update: 2026.4.12
+"""
+from typing import List, Tuple, Dict, Any
+import math
+
+import numpy as np
+
+from python_motion_planning.traj_optimizer.base_curve_generator import BaseCurveGenerator
+
+
+class BSpline(BaseCurveGenerator):
+    """
+    Class for B-Spline curve generator.
+
+    Args:
+        *args: see the parent class.
+        k: Degree of the B-Spline curve.
+        param_mode: Parameter selection mode, one of {"centripetal", "chord_length", "uniform_spaced"}.
+        spline_mode: B-Spline construction mode, one of {"interpolation", "approximation"}.
+        *kwargs: see the parent class.
+
+    References:
+        [1] https://en.wikipedia.org/wiki/B-spline
+
+    Examples:
+        >>> generator = BSpline(step=0.1, k=3)
+        >>> points = [(0.0, 0.0), (10.0, 10.0), (20.0, 5.0)]
+        >>> path, curve_info = generator.generate(points)
+        >>> print(curve_info['success'])
+        True
+    """
+    def __init__(self, *args,
+                 k: int = 3,
+                 param_mode: str = "centripetal",
+                 spline_mode: str = "interpolation",
+                 **kwargs) -> None:
+        super().__init__(*args, **kwargs)
+        self.k = k
+
+        if param_mode not in ("centripetal", "chord_length", "uniform_spaced"):
+            raise ValueError("Parameter selection mode error!")
+        self.param_mode = param_mode
+
+        if spline_mode not in ("interpolation", "approximation"):
+            raise ValueError("Spline mode selection error!")
+        self.spline_mode = spline_mode
+
+    def __str__(self) -> str:
+        return "B-Spline Curve"
+
+    def generate(self, points: List[Tuple[float, ...]]) -> Tuple[List[Tuple[float, float]], Dict[str, Any]]:
+        """
+        Generate a B-Spline curve through a list of 2D points.
+
+        Args:
+            points: A list of 2D points (x, y) in world frame. If the points contain
+                additional entries (e.g. yaw), only the first two values are used.
+
+        Returns:
+            path: A list of (x, y) waypoints of the generated curve in world frame.
+            curve_info: A dictionary containing the curve information (success, length).
+        """
+        if len(points) < 2:
+            return [], {"success": False, "length": 0.0}
+
+        points_2d = [(float(p[0]), float(p[1])) for p in points]
+
+        t = np.linspace(0, 1, int(1 / self.step))
+        params = self._param_selection(points_2d)
+        knot = self._knot_generation(params, len(points_2d))
+
+        if self.spline_mode == "interpolation":
+            control_pts = self._interpolation(points_2d, params, knot)
+        else:
+            control_pts = self._approximation(points_2d, params, knot)
+            h = len(control_pts)
+            new_points = [(control_pts[i][0], control_pts[i][1]) for i in range(h)]
+            params = self._param_selection(new_points)
+            knot = self._knot_generation(params, h)
+
+        curve = self._generate_curve(t, self.k, knot, control_pts)
+        path = [(float(pt[0]), float(pt[1])) for pt in curve]
+
+        return path, {"success": True, "length": self.length(path)}
+
+    def _base_function(self, i: int, k: int, t: float, knot: List[float]) -> float:
+        """
+        Calculate the base function using the Cox-de Boor recursion.
+
+        Args:
+            i: The index of the base function.
+            k: The degree of the curve.
+            t: Parameter value.
+            knot: The knot vector.
+
+        Returns:
+            Nik_t: The value of the base function Nik(t).
+        """
+        Nik_t = 0.0
+        if k == 0:
+            Nik_t = 1.0 if knot[i] <= t < knot[i + 1] else 0.0
+        else:
+            length1 = knot[i + k] - knot[i]
+            length2 = knot[i + k + 1] - knot[i + 1]
+            if not length1 and not length2:
+                Nik_t = 0.0
+            elif not length1:
+                Nik_t = (knot[i + k + 1] - t) / length2 * self._base_function(i + 1, k - 1, t, knot)
+            elif not length2:
+                Nik_t = (t - knot[i]) / length1 * self._base_function(i, k - 1, t, knot)
+            else:
+                Nik_t = ((t - knot[i]) / length1 * self._base_function(i, k - 1, t, knot)
+                         + (knot[i + k + 1] - t) / length2 * self._base_function(i + 1, k - 1, t, knot))
+        return Nik_t
+
+    def _param_selection(self, points: List[Tuple[float, float]]) -> List[float]:
+        """
+        Calculate the parameters of the given points using the selected method.
+
+        Args:
+            points: A list of 2D points.
+
+        Returns:
+            parameters: The parameters of the given points.
+        """
+        n = len(points)
+        x_list = [pt[0] for pt in points]
+        y_list = [pt[1] for pt in points]
+        dx, dy = np.diff(x_list), np.diff(y_list)
+
+        if self.param_mode == "uniform_spaced":
+            return np.linspace(0, 1, n).tolist()
+
+        if self.param_mode == "chord_length":
+            parameters = np.zeros(n)
+            s = np.cumsum([math.hypot(idx, idy) for (idx, idy) in zip(dx, dy)])
+            for i in range(1, n):
+                parameters[i] = s[i - 1] / s[-1]
+            return parameters.tolist()
+
+        # centripetal
+        alpha = 0.5
+        s = np.cumsum([math.pow(math.hypot(idx, idy), alpha) for (idx, idy) in zip(dx, dy)])
+        parameters = np.zeros(n)
+        for i in range(1, n):
+            parameters[i] = s[i - 1] / s[-1]
+        return parameters.tolist()
+
+    def _knot_generation(self, param: List[float], n: int) -> List[float]:
+        """
+        Generate the knot vector.
+
+        Args:
+            param: The parameters of the given points.
+            n: The number of data points.
+
+        Returns:
+            knot: The knot vector.
+        """
+        m = n + self.k + 1
+        knot = np.zeros(m)
+        for i in range(n, m):
+            knot[i] = 1
+        for i in range(self.k + 1, n):
+            for j in range(i - self.k, i):
+                knot[i] = knot[i] + param[j]
+            knot[i] = knot[i] / self.k
+        return knot.tolist()
+
+    def _interpolation(self, points: List[Tuple[float, float]], param: List[float],
+                       knot: List[float]) -> np.ndarray:
+        """
+        Build a B-Spline curve of degree k defined by N control points that passes
+        through all N data points.
+
+        Args:
+            points: A list of 2D points.
+            param: The parameters of the given points.
+            knot: The knot vector.
+
+        Returns:
+            control_points: The control points of the resulting B-Spline.
+        """
+        n = len(points)
+        N = np.zeros((n, n))
+
+        for i in range(n):
+            for j in range(n):
+                N[i][j] = self._base_function(j, self.k, param[i], knot)
+        N[n - 1][n - 1] = 1
+        N_inv = np.linalg.inv(N)
+
+        D = np.array(points)
+        return N_inv @ D
+
+    def _approximation(self, points: List[Tuple[float, float]], param: List[float],
+                       knot: List[float]) -> np.ndarray:
+        """
+        Build a B-Spline curve of degree k defined by H control points (H < N) that
+        passes through the first and last data points and approximates the rest in
+        the least-square sense.
+
+        Args:
+            points: A list of 2D points.
+            param: The parameters of the given points.
+            knot: The knot vector.
+
+        Returns:
+            control_points: The control points of the resulting B-Spline.
+        """
+        n = len(points)
+        D = np.array(points)
+
+        h = n - 1
+
+        N = np.zeros((n, h))
+        for i in range(n):
+            for j in range(h):
+                N[i][j] = self._base_function(j, self.k, param[i], knot)
+        N_ = N[1: n - 1, 1: h - 1]
+
+        qk = np.zeros((n - 2, 2))
+        for i in range(1, n - 1):
+            qk[i - 1] = D[i, :] - N[i][0] * D[0, :] - N[i][h - 1] * D[-1, :]
+        Q = N_.T @ qk
+
+        P = np.linalg.inv(N_.T @ N_) @ Q
+        P = np.insert(P, 0, D[0, :], axis=0)
+        P = np.insert(P, len(P), D[-1, :], axis=0)
+        return P
+
+    def _generate_curve(self, t: np.ndarray, k: int, knot: List[float],
+                        control_pts: np.ndarray) -> np.ndarray:
+        """
+        Sample points on the B-Spline curve.
+
+        Args:
+            t: The parameter values to sample at.
+            k: The degree of the B-Spline curve.
+            knot: The knot vector.
+            control_pts: The control points.
+
+        Returns:
+            curve: Sampled points along the B-Spline curve.
+        """
+        N = np.zeros((len(t), len(control_pts)))
+
+        for i in range(len(t)):
+            for j in range(len(control_pts)):
+                N[i][j] = self._base_function(j, k, t[i], knot)
+        N[len(t) - 1][len(control_pts) - 1] = 1
+
+        return N @ control_pts

python_motion_planning/traj_optimizer/curve_generator/point_based/cubic_spline.py

@@ -0,0 +1,115 @@
+"""
+@file: cubic_spline.py
+@author: Yang Haodong, Wu Maojia
+@update: 2026.4.12
+"""
+from typing import List, Tuple, Dict, Any
+import math
+import bisect
+
+import numpy as np
+
+from python_motion_planning.traj_optimizer.base_curve_generator import BaseCurveGenerator
+
+
+class CubicSpline(BaseCurveGenerator):
+    """
+    Class for cubic spline curve generator.
+
+    Args:
+        *args: see the parent class.
+        *kwargs: see the parent class.
+
+    References:
+        [1] https://en.wikipedia.org/wiki/Spline_(mathematics)#Algorithm_for_computing_natural_cubic_splines
+
+    Examples:
+        >>> generator = CubicSpline(step=0.1)
+        >>> points = [(0.0, 0.0), (10.0, 10.0), (20.0, 5.0)]
+        >>> path, curve_info = generator.generate(points)
+        >>> print(curve_info['success'])
+        True
+    """
+    def __init__(self, *args, **kwargs) -> None:
+        super().__init__(*args, **kwargs)
+
+    def __str__(self) -> str:
+        return "Cubic Spline"
+
+    def generate(self, points: List[Tuple[float, ...]]) -> Tuple[List[Tuple[float, float]], Dict[str, Any]]:
+        """
+        Generate a cubic spline curve through a list of 2D points.
+
+        Args:
+            points: A list of 2D points (x, y) in world frame. If the points contain
+                additional entries (e.g. yaw), only the first two values are used.
+
+        Returns:
+            path: A list of (x, y) waypoints of the generated curve in world frame.
+            curve_info: A dictionary containing the curve information (success, length).
+        """
+        if len(points) < 2:
+            return [], {"success": False, "length": 0.0}
+
+        x_list = [float(p[0]) for p in points]
+        y_list = [float(p[1]) for p in points]
+
+        dx, dy = np.diff(x_list), np.diff(y_list)
+        ds = [math.hypot(idx, idy) for (idx, idy) in zip(dx, dy)]
+        s = [0.0]
+        s.extend(np.cumsum(ds))
+        t = np.arange(0, s[-1], self.step)
+
+        path_x, _ = self._spline(s, x_list, t)
+        path_y, _ = self._spline(s, y_list, t)
+
+        path = [(float(ix), float(iy)) for ix, iy in zip(path_x, path_y)]
+        return path, {"success": True, "length": self.length(path)}
+
+    def _spline(self, x_list: List[float], y_list: List[float],
+                t: np.ndarray) -> Tuple[List[float], List[float]]:
+        """
+        Build and evaluate a 1D natural cubic spline y = f(x).
+
+        Args:
+            x_list: Monotonically increasing x-coordinates of the control points.
+            y_list: y-coordinates of the control points.
+            t: Values of x at which to evaluate the spline.
+
+        Returns:
+            p: Values of the spline evaluated at t.
+            dp: Values of the spline derivative evaluated at t.
+        """
+        a, b, c, d = y_list, [], [], []
+        h = np.diff(x_list)
+        num = len(x_list)
+
+        A = np.zeros((num, num))
+        for i in range(1, num - 1):
+            A[i, i - 1] = h[i - 1]
+            A[i, i] = 2.0 * (h[i - 1] + h[i])
+            A[i, i + 1] = h[i]
+        A[0, 0] = 1.0
+        A[num - 1, num - 1] = 1.0
+
+        B = np.zeros(num)
+        for i in range(1, num - 1):
+            B[i] = (3.0 * (a[i + 1] - a[i]) / h[i]
+                    - 3.0 * (a[i] - a[i - 1]) / h[i - 1])
+
+        c = np.linalg.solve(A, B)
+        for i in range(num - 1):
+            d.append((c[i + 1] - c[i]) / (3.0 * h[i]))
+            b.append((a[i + 1] - a[i]) / h[i] - h[i] * (c[i + 1] + 2.0 * c[i]) / 3.0)
+
+        p, dp = [], []
+        for it in t:
+            if it < x_list[0] or it > x_list[-1]:
+                continue
+            i = bisect.bisect(x_list, it) - 1
+            i = min(max(i, 0), num - 2)
+            dx = it - x_list[i]
+            p.append(a[i] + b[i] * dx + c[i] * dx ** 2 + d[i] * dx ** 3)
+            dp.append(b[i] + 2.0 * c[i] * dx + 3.0 * d[i] * dx ** 2)
+
+        return p, dp

python_motion_planning/traj_optimizer/curve_generator/pose_based/__init__.py

@@ -0,0 +1,4 @@
+from .bezier import *
+from .dubins import *
+from .polynomial import *
+from .reeds_shepp import *