python-motion-planning 2.0.dev1__py3-none-any.whl → 2.0.dev2__py3-none-any.whl

python_motion_planning/common/env/map/base_map.py

@@ -1,7 +1,7 @@
 """
 @file: map.py
 @author: Wu Maojia
-@update: 2025.10.3
+@update: 2025.11.25
 """
 from typing import Iterable, Union
 from abc import ABC, abstractmethod
@@ -17,12 +17,10 @@ class BaseMap(ABC):
 
     Args:
         bounds: The size of map in the world (shape: (n, 2) (n>=2)). bounds[i, 0] means the lower bound of the world in the i-th dimension. bounds[i, 1] means the upper bound of the world in the i-th dimension.  
-        dtype: data type of coordinates
     """
-    def __init__(self, bounds: Iterable, dtype: np.dtype) -> None:
+    def __init__(self, bounds: Iterable) -> None:
         super().__init__()
         self._bounds = np.asarray(bounds, dtype=float)
-        self._dtype = dtype
 
         if len(self._bounds.shape) != 2 or self._bounds.shape[0] <= 1 or self._bounds.shape[1] != 2:
             raise ValueError(f"The shape of bounds must be (n, 2) (n>=2) instead of {self._bounds.shape}")
@@ -39,10 +37,6 @@ class BaseMap(ABC):
     def dim(self) -> int:
         return self._bounds.shape[0]
 
-    @property
-    def dtype(self) -> np.dtype:
-        return self._dtype
-
     @abstractmethod
     def map_to_world(self, point: tuple) -> tuple:
         """

python_motion_planning/common/env/map/grid.py

@@ -1,7 +1,7 @@
 """
 @file: grid.py
 @author: Wu Maojia
-@update: 2025.10.3
+@update: 2025.12.20
 """
 from itertools import product
 from typing import Iterable, Union, Tuple, Callable, List, Dict
@@ -26,13 +26,13 @@ class GridTypeMap:
         >>> type_map = np.array([[0, 0, 0], [0, 1, 0], [0, 0, 0]], dtype=np.int8)
         >>> grid_type_map = GridTypeMap(type_map)
         >>> grid_type_map
-        GridTypeMap(array(
+        GridTypeMap(data=
         [[0 0 0]
          [0 1 0]
          [0 0 0]]
-        ), shape=(3, 3), dtype=int8)
+        , shape=(3, 3), dtype=int8)
 
-        >>> grid_type_map.array
+        >>> grid_type_map.data
         array([[0, 0, 0],
                [0, 1, 0],
                [0, 0, 0]], dtype=int8)
@@ -42,42 +42,31 @@ class GridTypeMap:
 
         >>> grid_type_map.dtype
         dtype('int8')
-
-        >>> new_array = np.array([[1, 1, 1], [0, 0, 0], [0, 0, 0]], dtype=np.int8)
-
-        >>> grid_type_map.update(new_array)
-
-        >>> grid_type_map
-        GridTypeMap(array(
-        [[1 1 1]
-         [0 0 0]
-         [0 0 0]]
-        ), shape=(3, 3), dtype=int8)
     """
     def __init__(self, type_map: np.ndarray):
-        self._array = np.asarray(type_map)
-        self._shape = self._array.shape
-        self._dtype = self._array.dtype
+        self._data = np.asarray(type_map)
+        self._shape = self._data.shape
+        self._dtype = self._data.dtype
         
         self._dtype_options = [np.int8, np.int16, np.int32, np.int64]
         if self._dtype not in self._dtype_options:
-            raise ValueError("Dtype must be one of {} instead of {}".format(self._dtype_options, self._dtype))
+            raise ValueError("Dtype must be one of {} instead of {}. If you are not sure, set it to `np.int8`.".format(self._dtype_options, self._dtype))
 
     def __str__(self) -> str:
-        return "GridTypeMap(array(\n{}\n), shape={}, dtype={})".format(self._array, self._shape, self._dtype)
+        return "GridTypeMap(data=\n{}\n, shape={}, dtype={})".format(self._data, self._shape, self._dtype)
 
     def __repr__(self) -> str:
         return self.__str__()
 
     def __getitem__(self, idx):
-        return self._array[idx]
+        return self._data[idx]
 
     def __setitem__(self, idx, value):
-        self._array[idx] = value
+        self._data[idx] = value
 
     @property
-    def array(self) -> np.ndarray:
-        return self._array.view()
+    def data(self) -> np.ndarray:
+        return self._data.view()
 
     @property
     def shape(self) -> Tuple:
@@ -87,14 +76,6 @@ class GridTypeMap:
     def dtype(self) -> np.dtype:
         return self._dtype
 
-    def update(self, new_array):
-        new_array = np.asarray(new_array)
-        if new_array.shape != self._shape:
-            raise ValueError(f"Shape must be {self._shape}")
-        if new_array.dtype != self.dtype:
-            raise ValueError(f"New values dtype must be {self.dtype}")
-        np.copyto(self._array, new_array)
-
 
 class Grid(BaseMap):
     """
@@ -107,7 +88,6 @@ class Grid(BaseMap):
         bounds: The size of map in the world (shape: (n, 2) (n>=2)). bounds[i, 0] means the lower bound of the world in the i-th dimension. bounds[i, 1] means the upper bound of the world in the i-th dimension.
         resolution: resolution of the grid map
         type_map: initial type map of the grid map (its shape must be the same as the converted grid map shape, and its dtype must be int)
-        dtype: data type of coordinates (must be int)
         inflation_radius: radius of the inflation
 
     Examples:
@@ -130,10 +110,10 @@ class Grid(BaseMap):
         (102, 62)
 
         >>> grid_map.dtype
-        <class 'numpy.int32'>
+        dtype('int8')
 
         >>> grid_map.type_map
-        GridTypeMap(array(
+        GridTypeMap(data=
         [[0 0 0 ... 0 0 0]
          [0 0 0 ... 0 0 0]
          [0 0 0 ... 0 0 0]
@@ -141,7 +121,7 @@ class Grid(BaseMap):
          [0 0 0 ... 0 0 0]
          [0 0 0 ... 0 0 0]
          [0 0 0 ... 0 0 0]]
-        ), shape=(102, 62), dtype=int8)
+        , shape=(102, 62), dtype=int8)
 
         >>> grid_map.map_to_world((1, 2))
         (0.75, 1.25)
@@ -155,9 +135,9 @@ class Grid(BaseMap):
         >>> grid_map.get_neighbors(Node((1, 2)), diagonal=False)
         [Node((2, 2), (1, 2), 0, 0), Node((0, 2), (1, 2), 0, 0), Node((1, 3), (1, 2), 0, 0), Node((1, 1), (1, 2), 0, 0)]
 
-        >>> grid_map.type_map[1, 0] = TYPES.OBSTACLE     # place an obstacle
+        >>> grid_map[1, 0] = TYPES.OBSTACLE     # place an obstacle
         >>> grid_map.get_neighbors(Node((0, 0)))    # limited within the bounds
-        [Node((0, 1), (0, 0), 0, 0), Node((1, 0), (0, 0), 0, 0), Node((1, 1), (0, 0), 0, 0)]
+        [Node((0, 1), (0, 0), 0, 0), Node((1, 1), (0, 0), 0, 0)]
 
         >>> grid_map.get_neighbors(Node((grid_map.shape[0] - 1, grid_map.shape[1] - 1)), diagonal=False)  # limited within the boundss
         [Node((100, 61), (101, 61), 0, 0), Node((101, 60), (101, 61), 0, 0)]
@@ -171,7 +151,7 @@ class Grid(BaseMap):
         >>> grid_map.in_collision((1, 2), (3, 6))
         False
 
-        >>> grid_map.type_map[1, 3] = TYPES.OBSTACLE
+        >>> grid_map[1, 3] = TYPES.OBSTACLE
         >>> grid_map.update_esdf()
         >>> grid_map.in_collision((1, 2), (3, 6))
         True
@@ -179,26 +159,19 @@ class Grid(BaseMap):
     def __init__(self, 
                 bounds: Iterable = [[0, 30], [0, 40]], 
                 resolution: float = 1.0, 
-                type_map: Union[GridTypeMap, np.ndarray] = None, 
-                dtype: np.dtype = np.int32,
+                type_map: Union[GridTypeMap, np.ndarray] = None,
                 inflation_radius: float = 0.0,
                 ) -> None:
-        super().__init__(bounds, dtype)
-
-        self._dtype_options = [np.int8, np.int16, np.int32, np.int64]
-        if self._dtype not in self._dtype_options:
-            raise ValueError("Dtype must be one of {} instead of {}".format(self._dtype_options, self._dtype))
+        super().__init__(bounds)
 
         self._resolution = resolution
-        self._shape = tuple([int((self.bounds[i, 1] - self.bounds[i, 0]) / self.resolution) for i in range(self.dim)])
+        shape = tuple([int((self.bounds[i, 1] - self.bounds[i, 0]) / self.resolution) for i in range(self.dim)])
 
         if type_map is None:
-            self.type_map = GridTypeMap(np.zeros(self._shape, dtype=np.int8))
+            self.type_map = GridTypeMap(np.zeros(shape, dtype=np.int8))
         else:
-            if type_map.shape != self._shape:
-                raise ValueError("Shape must be {} instead of {}".format(self._shape, type_map.shape))
-            if type_map.dtype not in self._dtype_options:
-                raise ValueError("Dtype must be one of {} instead of {}".format(self._dtype_options, type_map.dtype))
+            if type_map.shape != shape:
+                raise ValueError("Shape must be {} instead of {} with given bounds={} and resolution={}".format(shape, type_map.shape, self.bounds, self.resolution))
 
             if isinstance(type_map, GridTypeMap):
                 self.type_map = type_map
@@ -209,7 +182,7 @@ class Grid(BaseMap):
 
         self._precompute_offsets()
         
-        self._esdf = np.zeros(self._shape, dtype=np.float32)
+        self._esdf = np.zeros(self.shape, dtype=np.float32)
         # self.update_esdf()    # updated in self.inflate_obstacles()
 
         self.inflation_radius = inflation_radius
@@ -228,13 +201,27 @@ class Grid(BaseMap):
     
     @property
     def shape(self) -> tuple:
-        return self._shape
+        return self.type_map.shape
+    
+    @property
+    def dtype(self) -> np.dtype:
+        return self.type_map.dtype
     
     @property
     def esdf(self) -> np.ndarray:
         return self._esdf
     
-    def map_to_world(self, point: Tuple[int, ...]) -> tuple:
+    @property
+    def data(self) -> np.ndarray:
+        return self.type_map.data
+    
+    def __getitem__(self, idx):
+        return self.type_map[idx]
+
+    def __setitem__(self, idx, value):
+        self.type_map[idx] = value
+
+    def map_to_world(self, point: tuple) -> Tuple[float, ...]:
         """
         Convert map coordinates to world coordinates.
         
@@ -249,12 +236,13 @@ class Grid(BaseMap):
 
         return tuple((x + 0.5) * self.resolution + float(self.bounds[i, 0]) for i, x in enumerate(point))
 
-    def world_to_map(self, point: Tuple[float, ...]) -> tuple:
+    def world_to_map(self, point: Tuple[float, ...], discrete: bool = True) -> tuple:
         """
         Convert world coordinates to map coordinates.
         
         Args:
             point: Point in world coordinates.
+            discrete: Whether to round the coordinates to the nearest integer.
         
         Returns:
             point: Point in map coordinates.
@@ -262,7 +250,10 @@ class Grid(BaseMap):
         if len(point) != self.dim:
             raise ValueError("Point dimension does not match map dimension.")
         
-        return tuple(round((x - float(self.bounds[i, 0])) * (1.0 / self.resolution) - 0.5) for i, x in enumerate(point))
+        point_map = tuple((x - float(self.bounds[i, 0])) * (1.0 / self.resolution) - 0.5 for i, x in enumerate(point))
+        if discrete:
+            point_map = self.point_float_to_int(point_map)
+        return point_map
 
     def get_distance(self, p1: Tuple[int, int], p2: Tuple[int, int]) -> float:
         """
@@ -313,7 +304,7 @@ class Grid(BaseMap):
         if not self.within_bounds(point):
             return False
         if src_point is not None:
-            if self._esdf[point] >= self._esdf[src_point]:
+            if self.type_map[src_point] == TYPES.INFLATION and self._esdf[point] >= self._esdf[src_point]:
                 return True
                 
         return not self.type_map[point] == TYPES.OBSTACLE and not self.type_map[point] == TYPES.INFLATION
@@ -334,10 +325,6 @@ class Grid(BaseMap):
         """
         if node.dim != self.dim:
             raise ValueError("Node dimension does not match map dimension.")
-
-        # current_point = node.current.astype(self.dtype)
-        # current_pos = current_point.numpy()
-        # neighbors = []
         
         offsets = self._diagonal_offsets if diagonal else self._orthogonal_offsets
         
@@ -346,20 +333,9 @@ class Grid(BaseMap):
         neighbors = [node + offset for offset in offsets]
         filtered_neighbors = []
 
-        # print(neighbors)
-
-        # Filter out positions outside map bounds
-        # for pos in neighbor_positions:
-        #     point = (pos, dtype=self.dtype)
-        #     if self.within_bounds(point):
-        #         if self.type_map[tuple(point)] != TYPES.OBSTACLE:
-        #             neighbor_node = Node(point, parent=current_point)
-        #             neighbors.append(neighbor_node)
         for neighbor in neighbors:
             if self.is_expandable(neighbor.current, node.current):
                 filtered_neighbors.append(neighbor)
-
-        # print(filtered_neighbors)
         
         return filtered_neighbors
 
@@ -386,7 +362,7 @@ class Grid(BaseMap):
         primary_step = 1 if delta[primary_axis] > 0 else -1
         
         # Initialize the error variable
-        error = np.zeros(dim, dtype=self.dtype)
+        error = np.zeros(dim, dtype=int)
         delta2 = 2 * abs_delta
         
         # Calculate the number of steps and initialize the current point
@@ -495,11 +471,11 @@ class Grid(BaseMap):
             radius: Radius of the inflation.
         """
         self.update_esdf()
-        mask = (self.esdf <= radius) & (self.type_map.array == TYPES.FREE)
+        mask = (self.esdf <= radius) & (self.type_map.data == TYPES.FREE)
         self.type_map[mask] = TYPES.INFLATION
         self.inflation_radius = radius
 
-    def fill_expands(self, expands: Dict[Tuple[int, int], Node]) -> None:
+    def fill_expands(self, expands: Dict[Tuple[int, ...], Node]) -> None:
         """
         Fill the expands in the map.
         
@@ -515,9 +491,9 @@ class Grid(BaseMap):
         """
         Update the ESDF (signed Euclidean Distance Field) based on the obstacles in the map.
         - Obstacle grid ESDF = 0
-        - Free grid ESDF > 0. The value is the distance to the nearest obstacle
+        - Free grid ESDF > 0. The value is the di/stance to the nearest obstacle
         """
-        obstacle_mask = (self.type_map.array == TYPES.OBSTACLE)
+        obstacle_mask = (self.type_map.data == TYPES.OBSTACLE)
         free_mask = ~obstacle_mask
 
         # distance to obstacles
@@ -528,7 +504,7 @@ class Grid(BaseMap):
         self._esdf = dist_outside.astype(np.float32)
         self._esdf[obstacle_mask] = -dist_inside[obstacle_mask]
 
-    def path_map_to_world(self, path: List[Tuple[int, int]]) -> List[Tuple[float, float]]:
+    def path_map_to_world(self, path: List[tuple]) -> List[Tuple[float, ...]]:
         """
         Convert path from map coordinates to world coordinates
 
@@ -540,17 +516,34 @@ class Grid(BaseMap):
         """
         return [self.map_to_world(p) for p in path]
 
-    def path_world_to_map(self, path: List[Tuple[float, float]]) -> List[Tuple[int, int]]:
+    def path_world_to_map(self, path: List[Tuple[float, ...]], discrete: bool = True) -> List[tuple]:
         """
         Convert path from world coordinates to map coordinates
 
         Args:
             path: a list of world coordinates
+            discrete: whether to round the coordinates to the nearest integer
         
         Returns:
             path: a list of map coordinates
         """
-        return [self.world_to_map(p) for p in path]
+        return [self.world_to_map(p, discrete) for p in path]
+
+    def point_float_to_int(self, point: Tuple[float, ...]) -> Tuple[int, ...]:
+        """
+        Convert a point from float to integer coordinates.
+
+        Args:
+            point: a point in float coordinates
+        
+        Returns:
+            point: a point in integer coordinates
+        """
+        point_int = []
+        for d in range(self.dim):
+            point_int.append(max(0, min(self.shape[d] - 1, int(round(point[d])))))
+        point_int = tuple(point_int)
+        return point_int
 
     def _precompute_offsets(self):
         # Generate all possible offsets (-1, 0, +1) in each dimension

python_motion_planning/common/utils/__init__.py

@@ -1,2 +1,3 @@
 from .geometry import Geometry
-from .frame_transformer import FrameTransformer
+from .frame_transformer import FrameTransformer
+from .child_tree import ChildTree

python_motion_planning/common/utils/child_tree.py

@@ -0,0 +1,22 @@
+"""
+@file: frame_transformer.py
+@author: Wu Maojia
+@update: 2025.12.19
+"""
+from typing import Union
+
+class ChildTree:
+    def __init__(self):
+        self.tree = {}
+
+    def __getitem__(self, point: tuple) -> Union[set, None]:
+        return self.tree.get(point)
+
+    def add(self, point: tuple, child_point: tuple) -> None:
+        if self.tree.get(point) is None:
+            self.tree[point] = set()
+        self.tree[point].add(child_point)
+
+    def remove(self, point: tuple, child_point: tuple) -> None:
+        if self.tree.get(point) is not None:
+            self.tree[point].discard(child_point)

python_motion_planning/common/visualizer/__init__.py

@@ -1 +1,3 @@
-from .visualizer import *
+from .base_visualizer import *
+from .visualizer_2d import *
+from .visualizer_3d import *

python_motion_planning/common/visualizer/base_visualizer.py

@@ -0,0 +1,165 @@
+"""
+@file: base_visualizer.py
+@author: Wu Maojia
+@update: 2025.12.20
+"""
+from typing import Union, Dict, List, Tuple, Any
+from abc import ABC, abstractmethod
+
+import numpy as np
+
+from python_motion_planning.common.utils import Geometry
+
+class BaseVisualizer(ABC):
+    """
+    Base visualizer for motion planning.
+    """
+    def __init__(self):
+        self.dim = None
+        self.trajs = {}
+
+    def __del__(self):
+        self.close()
+
+    @abstractmethod
+    def show(self):
+        """
+        Show plot.
+        """
+        pass
+    
+    @abstractmethod
+    def close(self):
+        """
+        Close plot.
+        """
+        pass
+
+    def get_traj_info(self, 
+            rid: int, 
+            ref_path: List[Tuple[float, ...]], 
+            goal_pose: np.ndarray, 
+            goal_dist_tol: float, 
+            goal_orient_tol: float
+        ) -> Dict[str, Any]:
+        """
+        Get trajectory information.
+
+        Args:
+            rid: Robot ID.
+            ref_path: Reference pose path (world frame).
+            goal_pose: Goal pose.
+            goal_dist_tol: Distance tolerance for goal.
+            goal_orient_tol: Orientation tolerance for goal.
+        """
+        traj = self.trajs[rid]
+
+        info = {
+            "traj_length": 0.0,
+            "navigation_error": None,
+            "DTW": None,
+            "nDTW": None,
+            "success": False,
+            "dist_success": False, 
+            "oracle_success": False,
+            "oracle_dist_success": False,
+            "success_time": None,
+            "dist_success_time": None,
+            "oracle_success_time": None,
+            "oracle_dist_success_time": None,
+        }
+
+        goal_pos = goal_pose[:self.dim]
+        goal_orient = goal_pose[self.dim:]
+
+        for i in range(len(traj["poses"])):
+            pose = traj["poses"][i]
+            time = traj["time"][i]
+            
+            pos = pose[:self.dim]
+            orient = pose[self.dim:]
+
+            if i > 0:
+                info["traj_length"] += np.linalg.norm(pos - traj["poses"][i-1][:self.dim])
+
+            if np.linalg.norm(pos - goal_pos) <= goal_dist_tol:
+                if not info["oracle_dist_success"]:
+                    info["oracle_dist_success"] = True
+                    info["oracle_dist_success_time"] = time
+
+                if not info["dist_success"]:
+                    info["dist_success"] = True
+                    info["dist_success_time"] = time
+
+                if np.abs(Geometry.regularize_orient(orient - goal_orient)) <= goal_orient_tol:
+                    if not info["oracle_success"]:
+                        info["oracle_success"] = True
+                        info["oracle_success_time"] = time  
+
+                    if not info["success"]:
+                        info["success"] = True
+                        info["success_time"] = time
+                    
+                else:
+                    info["success"] = False
+                    info["success_time"] = None
+                
+            else:
+                info["success"] = False
+                info["success_time"] = None
+                info["dist_success"] = False
+                info["dist_success_time"] = None
+
+        info["navigation_error"] = float(np.linalg.norm(traj["poses"][-1][:self.dim] - goal_pos))
+        info["traj_length"] = float(info["traj_length"])
+        info["DTW"], info["nDTW"] = self.calc_dtw_ndtw(np.array(traj["poses"])[:, :self.dim], np.array(ref_path)[:, :self.dim])
+        return info
+
+    def calc_dtw_ndtw(self, path1: np.ndarray, path2: np.ndarray) -> Tuple[float, float]:
+        """
+        Compute the Dynamic Time Warping (DTW) and normalized DTW (nDTW)
+        between two N-dimensional paths.
+
+        Args:
+            path1 (np.ndarray): Path 1, shape (N, D)
+            path2 (np.ndarray): Path 2, shape (M, D)
+
+        Returns:
+            dtw: accumulated dynamic time warping distance
+            ndtw: normalized DTW in [0, 1], higher means more similar
+        
+        Reference:
+            [1] General Evaluation for Instruction Conditioned Navigation using Dynamic Time Warping
+        """
+        # Input validation
+        if path1.ndim != 2 or path2.ndim != 2:
+            raise ValueError("Both paths must be 2D arrays with shape (T, D).")
+        if path1.shape[1] != path2.shape[1]:
+            raise ValueError("Paths must have the same dimensionality.")
+
+        N, M = len(path1), len(path2)
+
+        # Initialize DTW cost matrix
+        dtw_matrix = np.full((N + 1, M + 1), np.inf)
+        dtw_matrix[0, 0] = 0.0
+
+        # Fill the DTW matrix
+        for i in range(1, N + 1):
+            for j in range(1, M + 1):
+                # Euclidean distance between points
+                cost = np.linalg.norm(path1[i - 1] - path2[j - 1])
+                # Accumulate cost with the best previous step
+                dtw_matrix[i, j] = cost + min(
+                    dtw_matrix[i - 1, j],      # insertion
+                    dtw_matrix[i, j - 1],      # deletion
+                    dtw_matrix[i - 1, j - 1]   # match
+                )
+
+        # Final DTW distance
+        dtw = dtw_matrix[N, M]
+
+        # Normalized DTW: exponential decay with respect to path length
+        max_len = max(N, M)
+        ndtw = np.exp(-dtw / (max_len + 1e-8))  # nDTW ∈ (0, 1]
+
+        return float(dtw), float(ndtw)