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

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 *

python_motion_planning/traj_optimizer/curve_generator/pose_based/bezier.py

@@ -0,0 +1,121 @@
+"""
+@file: bezier_curve.py
+@author: Yang Haodong, Wu Maojia
+@update: 2026.4.12
+"""
+from typing import List, Tuple, Dict, Any
+
+import numpy as np
+from scipy.special import comb
+
+from python_motion_planning.traj_optimizer.base_curve_generator import BaseCurveGenerator
+
+
+class Bezier(BaseCurveGenerator):
+    """
+    Class for Bezier curve generator.
+
+    Args:
+        *args: see the parent class.
+        offset: The offset of control points (larger value yields sharper curvature).
+        *kwargs: see the parent class.
+
+    References:
+        [1] https://en.wikipedia.org/wiki/B%C3%A9zier_curve
+
+    Examples:
+        >>> import math
+        >>> generator = Bezier(step=0.1, offset=3.0)
+        >>> points = [(0.0, 0.0, 0.0), (10.0, 10.0, -math.pi/2), (20.0, 5.0, math.pi/3)]
+        >>> path, curve_info = generator.generate(points)
+        >>> print(curve_info['success'])
+        True
+    """
+    def __init__(self, *args,
+                 offset: float = 3.0,
+                 **kwargs) -> None:
+        super().__init__(*args, **kwargs)
+        self.offset = offset
+
+    def __str__(self) -> str:
+        return "Bezier Curve"
+
+    def generate(self, points: List[Tuple[float, float, float]]) -> Tuple[List[Tuple[float, float]], Dict[str, Any]]:
+        """
+        Generate a concatenated Bezier curve through a list of poses.
+
+        Args:
+            points: A list of poses (x, y, yaw) in world frame.
+
+        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}
+
+        path: List[Tuple[float, float]] = []
+        for i in range(len(points) - 1):
+            segment, _ = self._generate_segment(points[i], points[i + 1])
+            path.extend([(float(pt[0]), float(pt[1])) for pt in segment])
+
+        return path, {"success": True, "length": self.length(path)}
+
+    def _generate_segment(self, start_pose: Tuple[float, float, float],
+                          goal_pose: Tuple[float, float, float]
+                          ) -> Tuple[List[np.ndarray], List[Tuple[float, float]]]:
+        """
+        Generate a single Bezier curve segment between two poses.
+
+        Args:
+            start_pose: Initial pose (x, y, yaw).
+            goal_pose: Target pose (x, y, yaw).
+
+        Returns:
+            segment: A list of points sampled from the Bezier curve.
+            control_points: The control points of the Bezier curve.
+        """
+        sx, sy, _ = start_pose
+        gx, gy, _ = goal_pose
+        n_points = max(int(np.hypot(sx - gx, sy - gy) / self.step), 2)
+        control_points = self._get_control_points(start_pose, goal_pose)
+
+        segment = [self._bezier(t, control_points) for t in np.linspace(0, 1, n_points)]
+        return segment, control_points
+
+    def _bezier(self, t: float, control_points: List[Tuple[float, float]]) -> np.ndarray:
+        """
+        Calculate the Bezier curve point.
+
+        Args:
+            t: Scale factor in [0, 1].
+            control_points: Control points of the Bezier curve.
+
+        Returns:
+            point: Point on the Bezier curve at the given t.
+        """
+        n = len(control_points) - 1
+        control_points = np.array(control_points)
+        return np.sum([comb(n, i) * t ** i * (1 - t) ** (n - i) * control_points[i]
+                       for i in range(n + 1)], axis=0)
+
+    def _get_control_points(self, start_pose: Tuple[float, float, float],
+                            goal_pose: Tuple[float, float, float]) -> List[Tuple[float, float]]:
+        """
+        Calculate the control points heuristically from start and goal poses.
+
+        Args:
+            start_pose: Initial pose (x, y, yaw).
+            goal_pose: Target pose (x, y, yaw).
+
+        Returns:
+            control_points: Control points of the Bezier curve.
+        """
+        sx, sy, syaw = start_pose
+        gx, gy, gyaw = goal_pose
+
+        dist = np.hypot(sx - gx, sy - gy) / self.offset
+        return [(sx, sy),
+                (sx + dist * np.cos(syaw), sy + dist * np.sin(syaw)),
+                (gx - dist * np.cos(gyaw), gy - dist * np.sin(gyaw)),
+                (gx, gy)]