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

python_motion_planning/traj_optimizer/curve_generator/pose_based/dubins.py

@@ -0,0 +1,355 @@
+"""
+@file: dubins_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 scipy.spatial.transform import Rotation as Rot
+
+from python_motion_planning.traj_optimizer.base_curve_generator import BaseCurveGenerator
+from python_motion_planning.common.utils.geometry import Geometry
+
+
+class Dubins(BaseCurveGenerator):
+    """
+    Class for Dubins curve generator.
+
+    Args:
+        *args: see the parent class.
+        max_curv: The maximum curvature of the curve.
+        *kwargs: see the parent class.
+
+    References:
+        [1] On curves of minimal length with a constraint on average curvature, and with prescribed initial and terminal positions and tangents
+
+    Examples:
+        >>> import math
+        >>> generator = Dubins(step=0.1, max_curv=1.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, max_curv: float = 1.0, **kwargs) -> None:
+        super().__init__(*args, **kwargs)
+        self.max_curv = max_curv
+
+    def __str__(self) -> str:
+        return "Dubins Curve"
+
+    def generate(self, points: List[Tuple[float, float, float]]) -> Tuple[List[Tuple[float, float, float]], Dict[str, Any]]:
+        """
+        Generate a concatenated Dubins 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, yaw) 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, float]] = []
+        total_cost = 0.0
+        for i in range(len(points) - 1):
+            best_cost, _, x_list, y_list, yaw_list = self._generate_segment(
+                points[i], points[i + 1])
+            if best_cost is None:
+                return [], {"success": False, "length": 0.0}
+            total_cost += best_cost / self.max_curv
+
+            start = 1 if i > 0 else 0
+            for x, y, yaw in zip(x_list[start:], y_list[start:], yaw_list[start:]):
+                path.append((float(x), float(y), float(yaw)))
+
+        total_cost = float(total_cost)
+
+        return path, {"success": True, "length": total_cost}
+
+    def _lsl(self, alpha: float, beta: float, dist: float):
+        """
+        Left-Straight-Left generation mode.
+
+        Args:
+            alpha: Initial heading of pose (0, 0, alpha).
+            beta: Goal heading of pose (dist, 0, beta).
+            dist: The distance between the initial and goal poses.
+
+        Returns:
+            t, p, q: Moving length of segments.
+            mode: Motion mode.
+        """
+        sin_a, sin_b, cos_a, cos_b, _, cos_a_b = self.trigonometric(alpha, beta)
+
+        p_lsl = 2 + dist ** 2 - 2 * cos_a_b + 2 * dist * (sin_a - sin_b)
+        if p_lsl < 0:
+            return None, None, None, ["L", "S", "L"]
+        p_lsl = math.sqrt(p_lsl)
+
+        t_lsl = Geometry.mod_to_2pi(-alpha + math.atan2(cos_b - cos_a, dist + sin_a - sin_b))
+        q_lsl = Geometry.mod_to_2pi(beta - math.atan2(cos_b - cos_a, dist + sin_a - sin_b))
+        return t_lsl, p_lsl, q_lsl, ["L", "S", "L"]
+
+    def _rsr(self, alpha: float, beta: float, dist: float):
+        """
+        Right-Straight-Right generation mode.
+
+        Args:
+            alpha: Initial heading of pose (0, 0, alpha).
+            beta: Goal heading of pose (dist, 0, beta).
+            dist: The distance between the initial and goal poses.
+
+        Returns:
+            t, p, q: Moving length of segments.
+            mode: Motion mode.
+        """
+        sin_a, sin_b, cos_a, cos_b, _, cos_a_b = self.trigonometric(alpha, beta)
+
+        p_rsr = 2 + dist ** 2 - 2 * cos_a_b + 2 * dist * (sin_b - sin_a)
+        if p_rsr < 0:
+            return None, None, None, ["R", "S", "R"]
+        p_rsr = math.sqrt(p_rsr)
+
+        t_rsr = Geometry.mod_to_2pi(alpha - math.atan2(cos_a - cos_b, dist - sin_a + sin_b))
+        q_rsr = Geometry.mod_to_2pi(-beta + math.atan2(cos_a - cos_b, dist - sin_a + sin_b))
+        return t_rsr, p_rsr, q_rsr, ["R", "S", "R"]
+
+    def _lsr(self, alpha: float, beta: float, dist: float):
+        """
+        Left-Straight-Right generation mode.
+
+        Args:
+            alpha: Initial heading of pose (0, 0, alpha).
+            beta: Goal heading of pose (dist, 0, beta).
+            dist: The distance between the initial and goal poses.
+
+        Returns:
+            t, p, q: Moving length of segments.
+            mode: Motion mode.
+        """
+        sin_a, sin_b, cos_a, cos_b, _, cos_a_b = self.trigonometric(alpha, beta)
+
+        p_lsr = -2 + dist ** 2 + 2 * cos_a_b + 2 * dist * (sin_a + sin_b)
+        if p_lsr < 0:
+            return None, None, None, ["L", "S", "R"]
+        p_lsr = math.sqrt(p_lsr)
+
+        t_lsr = Geometry.mod_to_2pi(-alpha + math.atan2(-cos_a - cos_b, dist + sin_a + sin_b) - math.atan2(-2.0, p_lsr))
+        q_lsr = Geometry.mod_to_2pi(-beta + math.atan2(-cos_a - cos_b, dist + sin_a + sin_b) - math.atan2(-2.0, p_lsr))
+        return t_lsr, p_lsr, q_lsr, ["L", "S", "R"]
+
+    def _rsl(self, alpha: float, beta: float, dist: float):
+        """
+        Right-Straight-Left generation mode.
+
+        Args:
+            alpha: Initial heading of pose (0, 0, alpha).
+            beta: Goal heading of pose (dist, 0, beta).
+            dist: The distance between the initial and goal poses.
+
+        Returns:
+            t, p, q: Moving length of segments.
+            mode: Motion mode.
+        """
+        sin_a, sin_b, cos_a, cos_b, _, cos_a_b = self.trigonometric(alpha, beta)
+
+        p_rsl = -2 + dist ** 2 + 2 * cos_a_b - 2 * dist * (sin_a + sin_b)
+        if p_rsl < 0:
+            return None, None, None, ["R", "S", "L"]
+        p_rsl = math.sqrt(p_rsl)
+
+        t_rsl = Geometry.mod_to_2pi(alpha - math.atan2(cos_a + cos_b, dist - sin_a - sin_b) + math.atan2(2.0, p_rsl))
+        q_rsl = Geometry.mod_to_2pi(beta - math.atan2(cos_a + cos_b, dist - sin_a - sin_b) + math.atan2(2.0, p_rsl))
+        return t_rsl, p_rsl, q_rsl, ["R", "S", "L"]
+
+    def _rlr(self, alpha: float, beta: float, dist: float):
+        """
+        Right-Left-Right generation mode.
+
+        Args:
+            alpha: Initial heading of pose (0, 0, alpha).
+            beta: Goal heading of pose (dist, 0, beta).
+            dist: The distance between the initial and goal poses.
+
+        Returns:
+            t, p, q: Moving length of segments.
+            mode: Motion mode.
+        """
+        sin_a, sin_b, cos_a, cos_b, _, cos_a_b = self.trigonometric(alpha, beta)
+
+        p_rlr = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_a - sin_b)) / 8.0
+        if abs(p_rlr) > 1.0:
+            return None, None, None, ["R", "L", "R"]
+        p_rlr = Geometry.mod_to_2pi(2 * math.pi - math.acos(p_rlr))
+
+        t_rlr = Geometry.mod_to_2pi(alpha - math.atan2(cos_a - cos_b, dist - sin_a + sin_b) + p_rlr / 2.0)
+        q_rlr = Geometry.mod_to_2pi(alpha - beta - t_rlr + p_rlr)
+        return t_rlr, p_rlr, q_rlr, ["R", "L", "R"]
+
+    def _lrl(self, alpha: float, beta: float, dist: float):
+        """
+        Left-Right-Left generation mode.
+
+        Args:
+            alpha: Initial heading of pose (0, 0, alpha).
+            beta: Goal heading of pose (dist, 0, beta).
+            dist: The distance between the initial and goal poses.
+
+        Returns:
+            t, p, q: Moving length of segments.
+            mode: Motion mode.
+        """
+        sin_a, sin_b, cos_a, cos_b, _, cos_a_b = self.trigonometric(alpha, beta)
+
+        p_lrl = (6.0 - dist ** 2 + 2.0 * cos_a_b + 2.0 * dist * (sin_b - sin_a)) / 8.0
+        if abs(p_lrl) > 1.0:
+            return None, None, None, ["L", "R", "L"]
+        p_lrl = Geometry.mod_to_2pi(2 * math.pi - math.acos(p_lrl)  )
+
+        t_lrl = Geometry.mod_to_2pi(-alpha + math.atan2(-cos_a + cos_b, dist + sin_a - sin_b) + p_lrl / 2.0)
+        q_lrl = Geometry.mod_to_2pi(beta - alpha - t_lrl + p_lrl)
+        return t_lrl, p_lrl, q_lrl, ["L", "R", "L"]
+
+    def _interpolate(self, mode: str, length: float,
+                     init_pose: Tuple[float, float, float]) -> Tuple[float, float, float]:
+        """
+        Planning path interpolation.
+
+        Args:
+            mode: Motion type, one of {"L", "S", "R"}.
+            length: Single step motion path length.
+            init_pose: Initial pose (x, y, yaw).
+
+        Returns:
+            new_pose: New pose (new_x, new_y, new_yaw) after moving.
+        """
+        x, y, yaw = init_pose
+
+        if mode == "S":
+            new_x = x + length / self.max_curv * math.cos(yaw)
+            new_y = y + length / self.max_curv * math.sin(yaw)
+            new_yaw = yaw
+        elif mode == "L":
+            new_x = x + (math.sin(yaw + length) - math.sin(yaw)) / self.max_curv
+            new_y = y - (math.cos(yaw + length) - math.cos(yaw)) / self.max_curv
+            new_yaw = yaw + length
+        elif mode == "R":
+            new_x = x - (math.sin(yaw - length) - math.sin(yaw)) / self.max_curv
+            new_y = y + (math.cos(yaw - length) - math.cos(yaw)) / self.max_curv
+            new_yaw = yaw - length
+        else:
+            raise NotImplementedError
+
+        return new_x, new_y, new_yaw
+
+    def _generate_segment(self, start_pose: Tuple[float, float, float],
+                          goal_pose: Tuple[float, float, float]):
+        """
+        Generate a single Dubins curve segment between two poses.
+
+        Args:
+            start_pose: Initial pose (x, y, yaw).
+            goal_pose: Target pose (x, y, yaw).
+
+        Returns:
+            best_cost: Best planning path length.
+            best_mode: Best motion modes.
+            x_list: Trajectory of x.
+            y_list: Trajectory of y.
+            yaw_list: Trajectory of yaw.
+        """
+        sx, sy, syaw = start_pose
+        gx, gy, gyaw = goal_pose
+
+        dx = gx - sx
+        dy = gy - sy
+        
+        cos_s = math.cos(syaw)
+        sin_s = math.sin(syaw)
+        local_gx = dx * cos_s + dy * sin_s
+        local_gy = -dx * sin_s + dy * cos_s
+        local_gyaw = gyaw - syaw
+        
+        dist = math.hypot(local_gx, local_gy) * self.max_curv
+        theta = Geometry.mod_to_2pi(math.atan2(local_gy, local_gx))
+        
+        alpha = Geometry.mod_to_2pi(-theta)
+        beta = Geometry.mod_to_2pi(local_gyaw - theta)
+
+        planners = [self._lsl, self._rsr, self._lsr, self._rsl, self._rlr, self._lrl]
+        best_t, best_p, best_q, best_mode, best_cost = None, None, None, None, float("inf")
+
+        for planner in planners:
+            t, p, q, mode = planner(alpha, beta, dist)
+            if t is None:
+                continue
+            cost = abs(t) + abs(p) + abs(q)
+            if best_cost > cost:
+                best_t, best_p, best_q, best_mode, best_cost = t, p, q, mode, cost
+
+        if best_mode is None:
+            return None, None, [], [], []
+
+        segments = [best_t, best_p, best_q]
+        points_num = int(sum(segments) / self.step) + len(segments) + 3
+        x_list = [0.0 for _ in range(points_num)]
+        y_list = [0.0 for _ in range(points_num)]
+        yaw_list = [0.0 for _ in range(points_num)]
+
+        idx = 0
+        for mode_, seg_length in zip(best_mode, segments):
+            d_length = self.step if seg_length > 0.0 else -self.step
+            current_x, current_y, current_yaw = x_list[idx], y_list[idx], yaw_list[idx]
+            length = d_length
+            while abs(length) <= abs(seg_length):
+                idx += 1
+                current_x, current_y, current_yaw = self._interpolate(
+                    mode_, d_length, (current_x, current_y, current_yaw)
+                )
+                x_list[idx], y_list[idx], yaw_list[idx] = current_x, current_y, current_yaw
+                length += d_length
+            
+            idx += 1
+            remainder = seg_length - (length - d_length)
+            x_list[idx], y_list[idx], yaw_list[idx] = self._interpolate(
+                mode_, remainder, (x_list[idx-1], y_list[idx-1], y_list[idx-1])
+            )
+
+        x_list = x_list[:idx + 1]
+        y_list = y_list[:idx + 1]
+        yaw_list = yaw_list[:idx + 1]
+
+        if len(x_list) <= 1:
+            return None, None, [], [], []
+
+        rot = Rot.from_euler('z', syaw).as_matrix()[0:2, 0:2]
+        converted_xy = rot @ np.stack([x_list, y_list])
+        x_list = (converted_xy[0, :] + sx).tolist()
+        y_list = (converted_xy[1, :] + sy).tolist()
+        yaw_list = [Geometry.regularize_orient(i_yaw + syaw) for i_yaw in yaw_list]
+
+        return best_cost, best_mode, x_list, y_list, yaw_list
+    
+    def trigonometric(self, alpha: float, beta: float) -> Tuple[float, float, float, float, float, float]:
+        """
+        Calculate trigonometric values for alpha and beta.
+        
+        Args:
+            alpha: Initial heading angle.
+            beta: Goal heading angle.
+            
+        Returns:
+            sin_a, sin_b, cos_a, cos_b, cos_ab, cos_a_b: Trigonometric values.
+        """
+        sin_a = math.sin(alpha)
+        sin_b = math.sin(beta)
+        cos_a = math.cos(alpha)
+        cos_b = math.cos(beta)
+        cos_ab = math.cos(alpha - beta)
+        cos_a_b = math.cos(alpha + beta) if hasattr(self, '_use_sum') else cos_ab
+        return sin_a, sin_b, cos_a, cos_b, cos_ab, cos_ab

python_motion_planning/traj_optimizer/curve_generator/pose_based/polynomial.py

@@ -0,0 +1,197 @@
+"""
+@file: polynomial_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 Polynomial(BaseCurveGenerator):
+    """
+    Class for quintic polynomial curve generator.
+
+    Args:
+        *args: see the parent class.
+        max_acc: Maximum allowed acceleration magnitude.
+        max_jerk: Maximum allowed jerk magnitude.
+        *args: see the parent class.
+
+    References:
+        [1] https://en.wikipedia.org/wiki/Polynomial_trajectory
+
+    Examples:
+        >>> import math
+        >>> generator = Polynomial(step=0.1, max_acc=2.82, max_jerk=10.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, 
+                 max_acc: float = 2.82, 
+                 max_jerk: float = 10.0,
+                 **kwargs) -> None:
+        super().__init__(*args, **kwargs)
+        self.max_acc = max_acc
+        self.max_jerk = max_jerk
+        self.dt = 0.1
+        self.t_min = 1
+        self.t_max = 30
+
+    def __str__(self) -> str:
+        return "Quintic Polynomial Curve"
+
+    class _Poly:
+        """
+        Quintic polynomial solver for a single dimension.
+        """
+        def __init__(self, state0: tuple, state1: tuple, t: float) -> None:
+            x0, v0, a0 = state0
+            xt, vt, at = state1
+
+            A = np.array([[t ** 3, t ** 4, t ** 5],
+                          [3 * t ** 2, 4 * t ** 3, 5 * t ** 4],
+                          [6 * t, 12 * t ** 2, 20 * t ** 3]])
+            b = np.array([xt - x0 - v0 * t - a0 * t ** 2 / 2,
+                          vt - v0 - a0 * t,
+                          at - a0])
+            X = np.linalg.solve(A, b)
+
+            self.p0 = x0
+            self.p1 = v0
+            self.p2 = a0 / 2.0
+            self.p3 = X[0]
+            self.p4 = X[1]
+            self.p5 = X[2]
+
+        def x(self, t: float) -> float:
+            return (self.p0 + self.p1 * t + self.p2 * t ** 2
+                    + self.p3 * t ** 3 + self.p4 * t ** 4 + self.p5 * t ** 5)
+
+        def dx(self, t: float) -> float:
+            return (self.p1 + 2 * self.p2 * t + 3 * self.p3 * t ** 2
+                    + 4 * self.p4 * t ** 3 + 5 * self.p5 * t ** 4)
+
+        def ddx(self, t: float) -> float:
+            return 2 * self.p2 + 6 * self.p3 * t + 12 * self.p4 * t ** 2 + 20 * self.p5 * t ** 3
+
+        def dddx(self, t: float) -> float:
+            return 6 * self.p3 + 24 * self.p4 * t + 60 * self.p5 * t ** 2
+
+    class _Trajectory:
+        """
+        Container for a polynomial trajectory.
+        """
+        def __init__(self):
+            self.clear()
+
+        def clear(self):
+            self.time = []
+            self.x = []
+            self.y = []
+            self.yaw = []
+            self.v = []
+            self.a = []
+            self.jerk = []
+
+        @property
+        def size(self) -> int:
+            assert (len(self.time) == len(self.x) == len(self.y) == len(self.yaw)
+                    == len(self.v) == len(self.a) == len(self.jerk)), \
+                "Unequal dimensions of each attribute, this should not happen."
+            return len(self.time)
+
+    def generate(self, points: List[Tuple[float, ...]]) -> Tuple[List[Tuple[float, float, float]], Dict[str, Any]]:
+        """
+        Generate a concatenated quintic polynomial curve through a list of poses.
+
+        Args:
+            points: A list of poses (x, y, yaw) in world frame. Optionally the
+                points may also carry velocity and acceleration, i.e.
+                (x, y, yaw, v, a). Missing values are filled heuristically.
+
+        Returns:
+            path: A list of (x, y, yaw) 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}
+
+        states: List[Tuple[float, float, float, float, float]] = []
+        for i, pt in enumerate(points):
+            if len(pt) >= 5:
+                states.append((float(pt[0]), float(pt[1]), float(pt[2]), float(pt[3]), float(pt[4])))
+            elif len(pt) == 3:
+                v = 0.0 if (i == 0 or i == len(points) - 1) else 1.0
+                states.append((float(pt[0]), float(pt[1]), float(pt[2]), v, 0.0))
+            else:
+                raise ValueError("Points must be (x, y, yaw) or (x, y, yaw, v, a).")
+
+        path: List[Tuple[float, float, float]] = []
+        for i in range(len(states) - 1):
+            traj = self._generate_segment(states[i], states[i + 1])
+            for j in range(traj.size):
+                path.append((float(traj.x[j]), float(traj.y[j]), float(traj.yaw[j])))
+
+        success = len(path) > 0
+        return path, {"success": success, "length": self.length(path) if success else 0.0}
+
+    def _generate_segment(self, start_state: Tuple[float, float, float, float, float],
+                          goal_state: Tuple[float, float, float, float, float]) -> "_Trajectory":
+        """
+        Generate a single quintic polynomial segment between two states.
+
+        Args:
+            start_state: Initial state (x, y, yaw, v, a).
+            goal_state: Target state (x, y, yaw, v, a).
+
+        Returns:
+            traj: The first trajectory that satisfies the acceleration and jerk constraints.
+        """
+        sx, sy, syaw, sv, sa = start_state
+        gx, gy, gyaw, gv, ga = goal_state
+
+        sv_x, sv_y = sv * math.cos(syaw), sv * math.sin(syaw)
+        gv_x, gv_y = gv * math.cos(gyaw), gv * math.sin(gyaw)
+
+        sa_x, sa_y = sa * math.cos(syaw), sa * math.sin(syaw)
+        ga_x, ga_y = ga * math.cos(gyaw), ga * math.sin(gyaw)
+
+        traj = self._Trajectory()
+
+        for T in np.arange(self.t_min, self.t_max, self.step):
+            x_psolver = self._Poly((sx, sv_x, sa_x), (gx, gv_x, ga_x), T)
+            y_psolver = self._Poly((sy, sv_y, sa_y), (gy, gv_y, ga_y), T)
+
+            for t in np.arange(0.0, T + self.dt, self.dt):
+                traj.time.append(t)
+                traj.x.append(x_psolver.x(t))
+                traj.y.append(y_psolver.x(t))
+
+                vx, vy = x_psolver.dx(t), y_psolver.dx(t)
+                traj.v.append(math.hypot(vx, vy))
+                traj.yaw.append(math.atan2(vy, vx))
+
+                ax, ay = x_psolver.ddx(t), y_psolver.ddx(t)
+                a = math.hypot(ax, ay)
+                if len(traj.v) >= 2 and traj.v[-1] - traj.v[-2] < 0.0:
+                    a *= -1
+                traj.a.append(a)
+
+                jx, jy = x_psolver.dddx(t), y_psolver.dddx(t)
+                j = math.hypot(jx, jy)
+                if len(traj.a) >= 2 and traj.a[-1] - traj.a[-2] < 0.0:
+                    j *= -1
+                traj.jerk.append(j)
+
+            if (max(np.abs(traj.a)) <= self.max_acc
+                    and max(np.abs(traj.jerk)) <= self.max_jerk):
+                return traj
+            traj.clear()
+
+        return traj