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

python_motion_planning/controller/base_controller.py

@@ -1,13 +1,14 @@
 """
 @file: base_controller.py
 @author: Wu Maojia
-@update: 2025.10.3
+@update: 2025.10.16
 """
 from typing import List, Tuple
 
 import numpy as np
 from gymnasium import spaces
 
+from python_motion_planning.common.env import Grid, TYPES, BaseRobot
 from python_motion_planning.common.utils.geometry import Geometry
 
 class BaseController:
@@ -19,17 +20,20 @@ class BaseController:
         observation_space: observation space ([pos, orient, lin_vel, ang_vel])
         action_space: action space ([lin_acc, ang_acc])
         dt: time step for control
-        path: pose path to follow
+        path: pose path to follow (world frame)
         max_lin_speed: maximum linear speed of the robot
         max_ang_speed: maximum angular speed of the robot
         goal_dist_tol: goal distance tolerance
         goal_orient_tol: goal orient tolerance
+        robot_model: robot model for kinematic parameters
+        obstacle_grid: occupancy grid map for collision checking
         eps: epsilon (numerical precision)
     """
     def __init__(self, observation_space: spaces.Space, action_space: spaces.Box,
                  dt: float, path: List[Tuple[float, ...]] = [], 
                  max_lin_speed: float = np.inf, max_ang_speed: float = np.inf,
                  goal_dist_tol: float = 0.5, goal_orient_tol: float = np.deg2rad(5),
+                 robot_model: BaseRobot = None, obstacle_grid: Grid = None,
                  eps: float = 1e-8):
         self.observation_space = observation_space
         self.action_space = action_space
@@ -57,9 +61,20 @@ class BaseController:
             self.goal = self.path[-1]
         else:
             self.goal = None
-            self.path = False 
+            self.path = False
 
         self.pred_traj = np.array([])
+        
+        if robot_model and robot_model.dim != self.dim:
+            raise ValueError("Dimension of robot model and controller must be the same")
+        self.robot_model = robot_model
+
+        if obstacle_grid and obstacle_grid.dim != self.dim:
+            raise ValueError("Dimension of obstacle grid and controller must be the same")
+        self.obstacle_grid = obstacle_grid
+
+    def __str__(self) -> str:
+        return "Base Controller"
 
     def reset(self):
         """
@@ -155,4 +170,22 @@ class BaseController:
             if np.abs(Geometry.regularize_orient(orient - goal_orient)) < self.goal_orient_tol:
                 desired_vel[self.dim:] = 0.0
 
-        return desired_vel
+        return desired_vel
+
+    def _get_dist_to_nearest_obstacle(self, pos: np.ndarray) -> float:
+        """
+        Get distance to nearest obstacle.
+        
+        Args:
+            pos: position (length = self.dim)
+        
+        Returns:
+            dist: distance to nearest obstacle
+        """
+        grid_pt = self.obstacle_grid.world_to_map(tuple(pos))
+        if not self.obstacle_grid.within_bounds(grid_pt) or self.obstacle_grid.type_map[grid_pt] == TYPES.OBSTACLE:
+            dist = 0.0
+        else:
+            dist = self.obstacle_grid.esdf[grid_pt] * self.obstacle_grid.resolution # using ESDF to compute distance to nearest obstacle
+
+        return dist

python_motion_planning/controller/path_tracker/__init__.py

@@ -2,4 +2,5 @@ from .path_tracker import PathTracker
 from .pure_pursuit import PurePursuit
 from .pid import PID
 from .dwa import DWA
-from .apf import APF
+from .apf import APF
+from .rpp import RPP

python_motion_planning/controller/path_tracker/apf.py

@@ -1,7 +1,7 @@
 """
 @file: apf.py
 @author: Wu Maojia, Yang Haodong 
-@update: 2025.10.3
+@update: 2025.10.16
 """
 from typing import List, Tuple
 import math
@@ -18,39 +18,41 @@ from .path_tracker import PathTracker
 
 class APF(PathTracker):
     """
-    Artificial Potential Field (APF) path-tracking controller.
+    Artificial Potential Field (APF) path-tracking controller. `robot_model` and `obstacle_grid` must be provided.
+`
 
     Args:
         *args: see the parent class.
-        robot_model: robot model for kinematic parameters
-        obstacle_grid: occupancy grid map for collision checking
         attr_weight: weight factor for attractive potential
         rep_weight: weight factor for repulsive potential
         rep_range: influence range for repulsive potential
         **kwargs: see the parent class.
+
+    References:
+        [1] Real-time obstacle avoidance for manipulators and mobile robots
     """
     def __init__(self,
                  *args,
-                 robot_model: BaseRobot,
-                 obstacle_grid: Grid = None,
                  attr_weight: float = 1.0,
                  rep_weight: float = 1.0,
                  rep_range: float = None,
                  **kwargs):
         super().__init__(*args, **kwargs)
-        if robot_model.dim != self.dim:
-            raise ValueError("Dimension of robot model and controller must be the same")
-        self.robot_model = robot_model
 
-        if obstacle_grid and obstacle_grid.dim != self.dim:
-            raise ValueError("Dimension of obstacle grid and controller must be the same")
-        self.obstacle_grid = obstacle_grid
+        if self.robot_model is None:
+            raise ValueError("Robot model is required.")
+        
+        if self.obstacle_grid is None:
+            raise ValueError("Obstacle grid is required.")
 
         # APF parameters
         self.attr_weight = attr_weight  # Attractive potential weight
         self.rep_weight = rep_weight    # Repulsive potential weight
         self.rep_range = rep_range if rep_range is not None else self.robot_model.radius * 2.0  # Repulsive influence range
 
+    def __str__(self) -> str:
+        return "APF"
+
     def get_action(self, obs: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
         """
         Get action from observation using Artificial Potential Field method.
@@ -125,22 +127,19 @@ class APF(PathTracker):
         if self.obstacle_grid is None:
             return np.zeros(self.dim)
 
-        # Convert world position to grid coordinates
-        grid_pt = self.obstacle_grid.world_to_map(tuple(current_pos[:2]))
-        grid_x, grid_y = grid_pt
-
-        # Check if position is out of bounds or in an obstacle
-        if not self.obstacle_grid.within_bounds(grid_pt) or self.obstacle_grid.type_map[grid_pt] == TYPES.OBSTACLE:
-            # Large repulsive force if in collision
-            return np.full(self.dim, self.rep_weight * self.rep_range)
-
-        # Get distance to nearest obstacle from ESDF (in world units)
-        dist_to_obstacle = self.obstacle_grid.esdf[grid_pt] * self.obstacle_grid.resolution
+        dist_to_obstacle = self._get_dist_to_nearest_obstacle(current_pos[:self.dim])
 
         # No repulsive force if outside influence range
         if dist_to_obstacle >= self.rep_range:
             return np.zeros(self.dim)
 
+        # avoid division by zero
+        dist_to_obstacle = max(dist_to_obstacle, self.eps)
+        
+        # Convert world position to grid coordinates
+        grid_pt = self.obstacle_grid.world_to_map(tuple(current_pos[:2]))
+        grid_x, grid_y = grid_pt
+
         # Calculate gradient of repulsive potential using numpy's gradient function
         # Extract a small window around current grid point to compute gradient
         window_size = 3  # 3x3 window for gradient calculation

python_motion_planning/controller/path_tracker/dwa.py

@@ -1,7 +1,7 @@
 """
 @file: dwa.py
 @author: Wu Maojia
-@update: 2025.10.3
+@update: 2025.10.16
 """
 from typing import List, Tuple, Optional
 import math
@@ -18,23 +18,22 @@ from .path_tracker import PathTracker
 
 class DWA(PathTracker):
     """
-    Dynamic Window Approach (DWA) path-tracking controller.
+    Dynamic Window Approach (DWA) path-tracking controller. `robot_model` and `obstacle_grid` must be provided.
 
     Args:
         *args: see the parent class.
-        robot_model: robot model for kinematic simulation
-        obstacle_grid: occupancy grid map for collision checking
         vel_reso: resolution of velocity sampling
         predict_time: forward simulation time horizon
         heading_weight: weight for heading term
         velocity_weight: weight for velocity term
         clearance_weight: weight for obstacle clearance term
         **kwargs: see the parent class.
+
+    References:
+        [1] The Dynamic Window Approach to Collision Avoidance
     """
     def __init__(self,
                  *args,
-                 robot_model: BaseRobot,
-                 obstacle_grid: Grid = None,
                  vel_reso: float = np.array([0.2, 0.2, np.deg2rad(15)]),
                  predict_time: float = None,
                  heading_weight: float = 0.5,
@@ -42,13 +41,12 @@ class DWA(PathTracker):
                  clearance_weight: float = 0.3,
                  **kwargs):
         super().__init__(*args, **kwargs)
-        if robot_model.dim != self.dim:
-            raise ValueError("Dimension of robot model and controller must be the same")
-        self.robot_model = robot_model
 
-        if obstacle_grid.dim != self.dim:
-            raise ValueError("Dimension of obstacle grid and controller must be the same")
-        self.obstacle_grid = obstacle_grid
+        if self.robot_model is None:
+            raise ValueError("Robot model is required.")
+        
+        if self.obstacle_grid is None:
+            raise ValueError("Obstacle grid is required.")
 
         self.vel_reso = vel_reso
         self.predict_time = predict_time if predict_time is not None else self.lookahead_distance / self.max_lin_speed
@@ -56,6 +54,9 @@ class DWA(PathTracker):
         self.velocity_weight = velocity_weight
         self.clearance_weight = clearance_weight
 
+    def __str__(self) -> str:
+        return "DWA"
+
     def get_action(self, obs: np.ndarray) -> Tuple[np.ndarray, np.ndarray]:
         """
         Get action from observation using Dynamic Window Approach.
@@ -238,11 +239,7 @@ class DWA(PathTracker):
 
         min_dist = float("inf") 
         for p in traj:
-            grid_pt = self.obstacle_grid.world_to_map(tuple(p[:2]))
-            if not self.obstacle_grid.within_bounds(grid_pt) or self.obstacle_grid.type_map[grid_pt] == TYPES.OBSTACLE:
-                return -float("inf")     # collision
-            # update min distance (Euclidean to occupied cells could be added here)
-            dist = self.obstacle_grid.esdf[grid_pt] * self.obstacle_grid.resolution # using ESDF to compute distance to nearest obstacle
+            dist = self._get_dist_to_nearest_obstacle(p[:self.dim])
             min_dist = min(min_dist, dist)
             
         normalized_min_dist = min_dist / self.robot_model.radius

python_motion_planning/controller/path_tracker/path_tracker.py

@@ -1,7 +1,7 @@
 """
 @file: path_tracker.py
 @author: Wu Maojia
-@update: 2025.10.3
+@update: 2025.10.16
 """
 from typing import List, Tuple
 import math
@@ -35,6 +35,9 @@ class PathTracker(BaseController):
         self.pose_interp = pose_interp
         self.current_target_index = 0
 
+    def __str__(self) -> str:
+        return "Path Tracker"
+
     def reset(self):
         """
         Reset the controller to initial state.

python_motion_planning/controller/path_tracker/pid.py

@@ -1,7 +1,7 @@
 """
 @file: pid.py
 @author: Wu Maojia
-@update: 2025.10.3
+@update: 2025.10.16
 """
 from typing import List, Tuple
 import math
@@ -22,6 +22,9 @@ class PID(PathTracker):
         Ki: integral gain
         Kd: derivative gain
         **kwargs: see the parent class.
+
+    References:
+        [1] Directional stability of automatically steered bodies
     """
     def __init__(self,
                  *args,
@@ -38,6 +41,9 @@ class PID(PathTracker):
         self.integral_error = np.zeros(self.action_space.shape[0])
         self.prev_error = np.zeros(self.action_space.shape[0])
 
+    def __str__(self) -> str:
+        return "PID"
+
     def reset(self):
         """
         Reset the controller to initial state.

python_motion_planning/controller/path_tracker/pure_pursuit.py

@@ -1,7 +1,7 @@
 """
 @file: pure_pursuit.py
 @author: Wu Maojia
-@update: 2025.10.3
+@update: 2025.10.16
 """
 from typing import List, Tuple
 import math
@@ -19,12 +19,18 @@ class PurePursuit(PathTracker):
     Args:
         *args: see the parent class.
         **kwargs: see the parent class.
+
+    References:
+        [1] Implementation of the Pure Pursuit Path Tracking Algorithm.
     """
     def __init__(self,
                  *args,
                  **kwargs):
         super().__init__(*args, **kwargs)
 
+    def __str__(self) -> str:
+        return "Pure Pursuit"
+
     def _get_desired_vel(self, target_pose: np.ndarray, cur_pose: np.ndarray) -> np.ndarray:
         """
         Calculate the desired velocity in robot frame using pure pursuit.

python_motion_planning/controller/path_tracker/rpp.py

@@ -0,0 +1,111 @@
+"""
+@file: rpp.py
+@author: Wu Maojia
+@update: 2025.10.16
+"""
+from typing import List, Tuple
+import math
+
+import numpy as np
+
+from python_motion_planning.common.utils.geometry import Geometry
+from python_motion_planning.common.utils.frame_transformer import FrameTransformer
+from .pure_pursuit import PurePursuit
+
+
+class RPP(PurePursuit):
+    """
+    Regulated Pure Pursuit (RPP) path-tracking controller. `obstacle_grid` must be provided.
+
+    Args:
+        *args: see the parent class.
+        curvature_thresh (float): minimum curvature threshold T_kappa to trigger speed scaling.
+        r_min (float): minimum radius (used in curvature heuristic).
+        d_prox (float): proximity distance threshold to start slowing down near obstacles.
+        alpha (float): proximity scaling gain (0 < alpha <= 1).
+        lookahead_gain (float): lookahead time gain for adaptive lookahead.
+        **kwargs: see the parent class.
+    """
+
+    def __init__(self,
+                 *args,
+                 curvature_thresh: float = 0.1,
+                 r_min: float = 1.0,
+                 d_prox: float = None,
+                 alpha: float = 0.8,
+                 lookahead_gain: float = 1.0,
+                 **kwargs):
+        super().__init__(*args, **kwargs)
+        
+        if self.obstacle_grid is None:
+            raise ValueError("Obstacle grid is required.")
+
+        self.curvature_thresh = curvature_thresh
+        self.r_min = r_min
+        self.d_prox = d_prox if d_prox is not None else self.obstacle_grid.inflation_radius
+        self.alpha = alpha
+        self.lookahead_gain = lookahead_gain
+
+    def __str__(self) -> str:
+        return "RPP"
+
+    def _get_desired_vel(self, target_pose: np.ndarray, cur_pose: np.ndarray) -> np.ndarray:
+        """
+        Calculate the desired velocity in robot frame using Regulated Pure Pursuit.
+
+        Args:
+            target_pose: target pose in world frame (from lookahead)
+            cur_pose: current pose in world frame
+            obs_dist: distance to nearest obstacle (if available), defaults to infinity
+
+        Returns:
+            desired_vel: desired velocity in robot frame [lin_x, lin_y, ang_z]
+        """
+        # transform target pose to robot frame
+        rel_pose = FrameTransformer.pose_world_to_robot(self.dim, target_pose, cur_pose)
+        x = rel_pose[0]
+        y = rel_pose[1]
+        L = math.hypot(x, y)
+
+        # if lookahead distance is (nearly) zero, no movement
+        if L < self.eps:
+            desired_vel = np.zeros(self.action_space.shape[0])
+            return self.clip_velocity(desired_vel)
+            
+        # Pure Pursuit curvature: kappa = 2*y / L^2
+        # Note: y is lateral offset in robot frame (positive left). For our coordinate,
+        # forward x, lateral y. Angular velocity = kappa * v.
+        kappa = (2.0 * y) / (L * L)
+
+        # Base linear speed (before regulation)
+        v_t = self.max_lin_speed
+
+        # Curvature heuristic
+        # Slow down in sharp turns: Eq. (5) in the paper
+        if abs(kappa) > self.curvature_thresh:
+            v_curv = v_t / (self.r_min * max(abs(kappa), self.eps))
+        else:
+            v_curv = v_t
+
+        # Proximity heuristic
+        # Slow down when close to obstacles: Eq. (6) in the paper
+        obs_dist = self._get_dist_to_nearest_obstacle(cur_pose[:self.dim])
+        if obs_dist <= self.d_prox:
+            v_prox = v_t * self.alpha * obs_dist / max(self.d_prox, self.eps)
+        else:
+            v_prox = v_t
+        v_prox = v_t
+
+        # Take minimum regulated velocity (for safety)
+        v_reg = min(v_curv, v_prox)
+
+        # Compute angular velocity using regulated linear speed
+        omega = np.clip(v_reg * kappa, -self.max_ang_speed, self.max_ang_speed)
+
+        # Assemble desired velocity vector
+        desired_lin_vel = np.array([v_reg * (1.0 if x >= 0 else -1.0), 0.0])
+        desired_ang_vel = np.array([omega])
+        desired_vel = np.concatenate([desired_lin_vel, desired_ang_vel])
+
+        desired_vel = self.clip_velocity(desired_vel)
+        return desired_vel

python_motion_planning/path_planner/__init__.py

@@ -1,3 +1,4 @@
 from .base_path_planner import *
 from .graph_search import *
-from .sample_search import *
+from .sample_search import *
+from .hybrid_search import *

python_motion_planning/path_planner/base_path_planner.py

@@ -1,9 +1,9 @@
 """
-@file: planner.py
+@file: base_path_planner.py
 @author: Wu Maojia
-@update: 2025.10.3
+@update: 2025.12.19
 """
-from typing import Union
+from typing import Union, List, Tuple, Dict, Any, Iterable
 from abc import ABC, abstractmethod
  
 from python_motion_planning.common import BaseMap
@@ -25,8 +25,31 @@ class BasePathPlanner(ABC):
         self.goal = goal
         self.failed_info = [], {"success": False, "start": None, "goal": None, "length": 0, "cost": 0, "expand": {}}
 
+    def __str__(self) -> str:
+        return "Base Path Planner"
+
+    @property
+    def dim(self) -> int:
+        """
+        Get the dimension of the map.
+
+        Returns:
+            dim (int): The dimension of the map.
+        """
+        return self.map_.dim
+
+    @property
+    def bounds(self) -> Iterable:
+        """
+        Get the bounds of the map.
+
+        Returns:
+            bounds (Iterable): The bounds of the map.
+        """
+        return self.map_.bounds
+
     @abstractmethod
-    def plan(self) -> Union[list, dict]:
+    def plan(self) -> Union[List[Tuple[float, ...]], Dict[str, Any]]:
         """
         Interface for planning.
 
@@ -63,26 +86,37 @@ class BasePathPlanner(ABC):
         return self.get_cost(point, self.goal)
 
     
-    def extract_path(self, closed_list: dict) -> tuple:
+    def extract_path(self, closed_list: dict, start: tuple = None, goal: tuple = None) -> Tuple[List[Tuple[float, ...]], float, float]:
         """
         Extract the path based on the CLOSED list.
 
         Args:
-            closed_list (dict): CLOSED list
+            closed_list: CLOSED list
+            start: Start point. (default: self.start)
+            goal: Goal point. (default: self.goal)
 
         Returns:
-            cost (float): the cost of planned path
-            path (list): the planning path
+            path: A list containing the path waypoints
+            length: Length of the path
+            cost: Cost of the path
         """
         length = 0
         cost = 0
-        node = closed_list[self.goal]
+
+        if start is None:
+            start = self.start
+        if goal is None:
+            goal = self.goal
+
+        node = closed_list.get(goal)
         path = [node.current]
-        while node.current != self.start:
-            node_parent = closed_list[node.parent]
+
+        while node.current != start:
+            node_parent = closed_list.get(node.parent)
             length += self.map_.get_distance(node.current, node_parent.current)
             cost += self.get_cost(node.current, node_parent.current)
             node = node_parent
             path.append(node.current)
         path = path[::-1]   # make the order: start -> goal
+        
         return path, length, cost

python_motion_planning/path_planner/graph_search/__init__.py

@@ -1,3 +1,6 @@
 from .dijkstra import *
+from .gbfs import *
 from .a_star import *
-from .theta_star import *
+from .jps import *
+from .theta_star import *
+from .lazy_theta_star import *

python_motion_planning/path_planner/graph_search/a_star.py

@@ -1,9 +1,9 @@
 """
 @file: a_star.py
 @author: Wu Maojia
-@update: 2025.10.3
+@update: 2025.10.6
 """
-from typing import Union
+from typing import Union, List, Tuple, Dict, Any
 import heapq
  
 from python_motion_planning.common import BaseMap, Grid, Node, TYPES
@@ -23,12 +23,14 @@ class AStar(Dijkstra):
     Examples:
         >>> map_ = Grid(bounds=[[0, 15], [0, 15]])
         >>> planner = AStar(map_=map_, start=(5, 5), goal=(10, 10))
-        >>> planner.plan()
-        ([(5, 5), (6, 6), (7, 7), (8, 8), (9, 9), (10, 10)], {'success': True, 'start': (5, 5), 'goal': (10, 10), 'length': 7.0710678118654755, 'cost': 7.0710678118654755, 'expand': {(5, 5): Node((5, 5), None, 0, 7.0710678118654755), (6, 6): Node((6, 6), (5, 5), 1.4142135623730951, 5.656854249492381), (7, 7): Node((7, 7), (6, 6), 2.8284271247461903, 4.242640687119285), (8, 8): Node((8, 8), (7, 7), 4.242640687119286, 2.8284271247461903), (9, 9): Node((9, 9), (8, 8), 5.656854249492381, 1.4142135623730951), (10, 10): Node((10, 10), (9, 9), 7.0710678118654755, 0.0)}})
-
+        >>> path, path_info = planner.plan()
+        >>> print(path_info['success'])
+        True
+        
         >>> planner.map_.type_map[3:10, 6] = TYPES.OBSTACLE
-        >>> planner.plan()
-        ([(5, 5), (6, 6), (7, 7), (8, 8), (9, 9), (10, 10)], {'success': True, 'start': (5, 5), 'goal': (10, 10), 'length': 7.0710678118654755, 'cost': 7.0710678118654755, 'expand': {(5, 5): Node((5, 5), None, 0, 7.0710678118654755), (6, 6): Node((6, 6), (5, 5), 1.4142135623730951, 5.656854249492381), (7, 7): Node((7, 7), (6, 6), 2.8284271247461903, 4.242640687119285), (8, 8): Node((8, 8), (7, 7), 4.242640687119286, 2.8284271247461903), (9, 9): Node((9, 9), (8, 8), 5.656854249492381, 1.4142135623730951), (10, 10): Node((10, 10), (9, 9), 7.0710678118654755, 0.0)}})
+        >>> path, path_info = planner.plan()
+        >>> print(path_info['success'])
+        True
     """
     def __init__(self, *args, **kwargs) -> None:
         super().__init__(*args, **kwargs)
@@ -36,13 +38,13 @@ class AStar(Dijkstra):
     def __str__(self) -> str:
         return "A*"
 
-    def plan(self) -> Union[list, dict]:
+    def plan(self) -> Union[List[Tuple[float, ...]], Dict[str, Any]]:
         """
         Interface for planning.
 
         Returns:
             path: A list containing the path waypoints
-            path_info: A dictionary containing the path information (success, length, cost, expand)
+            path_info: A dictionary containing the path information
         """
         # OPEN list (priority queue) and CLOSED list (hash table)
         OPEN = []
@@ -53,10 +55,6 @@ class AStar(Dijkstra):
         while OPEN:
             node = heapq.heappop(OPEN)
 
-            # obstacle found
-            if not self.map_.is_expandable(node.current, node.parent):
-                continue
-
             # exists in CLOSED list
             if node.current in CLOSED:
                 continue
@@ -74,7 +72,7 @@ class AStar(Dijkstra):
                     "expand": CLOSED
                 }
 
-            for node_n in self.map_.get_neighbors(node): 
+            for node_n in self.map_.get_neighbors(node, diagonal=self.diagonal): 
                 # exists in CLOSED list
                 if node_n.current in CLOSED:
                     continue