Grid¶

`src.python_motion_planning.common.env.map.grid.Grid` ¶

Bases: BaseMap

Class for Grid Map. The shape of each dimension of the grid map is determined by the base world and resolution. For each dimension, the conversion equation is: shape_grid = shape_world * resolution + 1 For example, if the base world is (30, 40) and the resolution is 0.5, the grid map will be (30 * 0.5 + 1, 40 * 0.5 + 1) = (61, 81).

Parameters:

Name	Type	Description	Default
`bounds`	`Iterable`	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.	`[[0, 30], [0, 40]]`
`resolution`	`float`	resolution of the grid map	`1.0`
`type_map`	`Union[GridTypeMap, ndarray]`	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)	`None`
`inflation_radius`	`float`	radius of the inflation	`0.0`

Examples:

Python Console Session

>>> grid_map = Grid(bounds=[[0, 51], [0, 31]], resolution=0.5)
>>> grid_map
Grid(bounds=[[ 0. 51.]
 [ 0. 31.]], resolution=0.5)

Python Console Session

>>> grid_map.bounds    # bounds of the base world
array([[ 0., 51.],
       [ 0., 31.]])

Python Console Session

>>> grid_map.dim
2

Python Console Session

>>> grid_map.resolution
0.5

Python Console Session

>>> grid_map.shape   # shape of the grid map
(102, 62)

Python Console Session

>>> grid_map.dtype
dtype('int8')

Python Console Session

>>> grid_map.type_map
GridTypeMap(data=
[[0 0 0 ... 0 0 0]
 [0 0 0 ... 0 0 0]
 [0 0 0 ... 0 0 0]
 ...
 [0 0 0 ... 0 0 0]
 [0 0 0 ... 0 0 0]
 [0 0 0 ... 0 0 0]]
, shape=(102, 62), dtype=int8)

Python Console Session

>>> grid_map.map_to_world((1, 2))
(0.75, 1.25)

Python Console Session

>>> grid_map.world_to_map((0.5, 1.0))
(0, 2)

Python Console Session

>>> grid_map.get_neighbors(Node((1, 2)))
[Node((0, 1), (1, 2), 0, 0), Node((0, 2), (1, 2), 0, 0), Node((0, 3), (1, 2), 0, 0), Node((1, 1), (1, 2), 0, 0), Node((1, 3), (1, 2), 0, 0), Node((2, 1), (1, 2), 0, 0), Node((2, 2), (1, 2), 0, 0), Node((2, 3), (1, 2), 0, 0)]

Python Console Session

>>> 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)]

Python Console Session

>>> 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, 1), (0, 0), 0, 0)]

Python Console Session

>>> 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)]

Python Console Session

>>> grid_map.line_of_sight((1, 2), (3, 6))
[(1, 2), (1, 3), (2, 4), (2, 5), (3, 6)]

Python Console Session

>>> grid_map.line_of_sight((1, 2), (1, 2))
[(1, 2)]

Python Console Session

>>> grid_map.in_collision((1, 2), (3, 6))
False

Python Console Session

>>> grid_map[1, 3] = TYPES.OBSTACLE
>>> grid_map.update_esdf()
>>> grid_map.in_collision((1, 2), (3, 6))
True

Source code in src\python_motion_planning\common\env\map\grid.py

Python

class Grid(BaseMap):
    """
    Class for Grid Map.
    The shape of each dimension of the grid map is determined by the base world and resolution.
    For each dimension, the conversion equation is: shape_grid = shape_world * resolution + 1
    For example, if the base world is (30, 40) and the resolution is 0.5, the grid map will be (30 * 0.5 + 1, 40 * 0.5 + 1) = (61, 81).

    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.
        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)
        inflation_radius: radius of the inflation

    Examples:
        >>> grid_map = Grid(bounds=[[0, 51], [0, 31]], resolution=0.5)
        >>> grid_map
        Grid(bounds=[[ 0. 51.]
         [ 0. 31.]], resolution=0.5)

        >>> grid_map.bounds    # bounds of the base world
        array([[ 0., 51.],
               [ 0., 31.]])

        >>> grid_map.dim
        2

        >>> grid_map.resolution
        0.5

        >>> grid_map.shape   # shape of the grid map
        (102, 62)

        >>> grid_map.dtype
        dtype('int8')

        >>> grid_map.type_map
        GridTypeMap(data=
        [[0 0 0 ... 0 0 0]
         [0 0 0 ... 0 0 0]
         [0 0 0 ... 0 0 0]
         ...
         [0 0 0 ... 0 0 0]
         [0 0 0 ... 0 0 0]
         [0 0 0 ... 0 0 0]]
        , shape=(102, 62), dtype=int8)

        >>> grid_map.map_to_world((1, 2))
        (0.75, 1.25)

        >>> grid_map.world_to_map((0.5, 1.0))
        (0, 2)

        >>> grid_map.get_neighbors(Node((1, 2)))
        [Node((0, 1), (1, 2), 0, 0), Node((0, 2), (1, 2), 0, 0), Node((0, 3), (1, 2), 0, 0), Node((1, 1), (1, 2), 0, 0), Node((1, 3), (1, 2), 0, 0), Node((2, 1), (1, 2), 0, 0), Node((2, 2), (1, 2), 0, 0), Node((2, 3), (1, 2), 0, 0)]

        >>> 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[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, 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)]

        >>> grid_map.line_of_sight((1, 2), (3, 6))
        [(1, 2), (1, 3), (2, 4), (2, 5), (3, 6)]

        >>> grid_map.line_of_sight((1, 2), (1, 2))
        [(1, 2)]

        >>> grid_map.in_collision((1, 2), (3, 6))
        False

        >>> grid_map[1, 3] = TYPES.OBSTACLE
        >>> grid_map.update_esdf()
        >>> grid_map.in_collision((1, 2), (3, 6))
        True
    """
    def __init__(self, 
                bounds: Iterable = [[0, 30], [0, 40]], 
                resolution: float = 1.0, 
                type_map: Union[GridTypeMap, np.ndarray] = None,
                inflation_radius: float = 0.0,
                ) -> None:
        super().__init__(bounds)

        self._resolution = resolution
        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(shape, dtype=np.int8))
        else:
            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
            elif isinstance(type_map, np.ndarray):
                self.type_map = GridTypeMap(type_map)        
            else:
                raise ValueError("Type map must be GridTypeMap or numpy.ndarray instead of {}".format(type(type_map)))

        self._shape_array = np.asarray(self.shape, dtype=np.int64)
        self._precompute_offsets()

        self._esdf = np.zeros(self.shape, dtype=np.float32)
        # self.update_esdf()    # updated in self.inflate_obstacles()

        self.inflation_radius = inflation_radius
        if self.inflation_radius >= 1:
            self.inflate_obstacles(self.inflation_radius)

    def __str__(self) -> str:
        return "Grid(bounds={}, resolution={})".format(self.bounds, self.resolution)

    def __repr__(self) -> str:
        return self.__str__()

    @property
    def resolution(self) -> float:
        return self._resolution

    @property
    def shape(self) -> tuple:
        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

    @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 _type_map_flat(self) -> np.ndarray:
        return np.ravel(self.type_map.data)

    def _esdf_flat(self) -> np.ndarray:
        return np.ravel(self._esdf)

    def map_to_world(self, point: tuple) -> Tuple[float, ...]:
        """
        Convert map coordinates to world coordinates.

        Args:
            point: Point in map coordinates.

        Returns:
            point: Point in world coordinates.
        """
        if len(point) != self.dim:
            raise ValueError("Point dimension does not match map dimension.")

        point_world = _grid_map_to_world(np.asarray(point, dtype=np.float64), self.bounds, self.resolution)
        return tuple(float(x) for x in point_world)

    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.
        """
        if len(point) != self.dim:
            raise ValueError("Point dimension does not match map dimension.")

        point_array = np.asarray(point, dtype=np.float64)
        if discrete:
            point_map = _grid_world_to_map_int(point_array, self.bounds, self.resolution, self._shape_array)
            return tuple(int(x) for x in point_map)
        else:
            point_map = _grid_world_to_map_float(point_array, self.bounds, self.resolution)
            return tuple(float(x) for x in point_map)

    def get_distance(self, p1: Tuple[int, int], p2: Tuple[int, int]) -> float:
        """
        Get the distance between two points.

        Args:
            p1: Start point.
            p2: Goal point.

        Returns:
            dist: Distance between two points.
        """
        if len(p1) != len(p2):
            raise ValueError("Dimension mismatch")
        return _grid_distance(np.asarray(p1, dtype=np.float64), np.asarray(p2, dtype=np.float64))

    def within_bounds(self, point: Tuple[int, ...]) -> bool:
        """
        Check if a point is within the bounds of the grid map.

        Args:
            point: Point to check.

        Returns:
            bool: True if the point is within the bounds of the map, False otherwise.
        """
        # if point.dim != self.dim:
        #     raise ValueError("Point dimension does not match map dimension.")

        # return all(0 <= point[i] < self.shape[i] for i in range(self.dim))
        return _grid_within_bounds(np.asarray(point, dtype=np.int64), self._shape_array)

    def is_expandable(self, point: Tuple[int, ...], src_point: Tuple[int, ...] = None) -> bool:
        """
        Check if a point is expandable.

        Args:
            point: Point to check.
            src_point: Source point.

        Returns:
            expandable: True if the point is expandable, False otherwise.
        """
        point_array = np.asarray(point, dtype=np.int64)
        has_src_point = src_point is not None
        src_array = point_array if src_point is None else np.asarray(src_point, dtype=np.int64)

        return _grid_is_expandable(
            point_array,
            src_array,
            has_src_point,
            self._shape_array,
            self._type_map_flat(),
            self._esdf_flat(),
            TYPES.OBSTACLE,
            TYPES.INFLATION,
        )

    def get_neighbors(self, 
                    node: Node, 
                    diagonal: bool = True
                    ) -> list:
        """
        Get neighbor nodes of a given node.

        Args:
            node: Node to get neighbor nodes.
            diagonal: Whether to include diagonal neighbors.

        Returns:
            nodes: List of neighbor nodes.
        """
        if node.dim != self.dim:
            raise ValueError("Node dimension does not match map dimension.")

        offsets = self._diagonal_offsets_array if diagonal else self._orthogonal_offsets_array
        positions, mask = _grid_neighbor_positions_and_mask(
            np.asarray(node.current, dtype=np.int64),
            offsets,
            self._shape_array,
            self._type_map_flat(),
            self._esdf_flat(),
            TYPES.OBSTACLE,
            TYPES.INFLATION,
        )

        return [
            Node(tuple(int(x) for x in positions[i]), node.current, node.g, node.h)
            for i in range(positions.shape[0])
            if mask[i]
        ]

    def line_of_sight(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> List[Tuple[int, ...]]:
        """
        N-dimensional line of sight (Bresenham's line algorithm)

        Args:
            p1: Start point of the line.
            p2: End point of the line.

        Returns:
            points: List of point on the line of sight.
        """
        p1_array = np.asarray(p1, dtype=np.int64)
        p2_array = np.asarray(p2, dtype=np.int64)
        if p1_array.shape != p2_array.shape:
            p2_array - p1_array

        points = _grid_line_of_sight(p1_array, p2_array)
        return [tuple(int(x) for x in points[i]) for i in range(points.shape[0])]

    def in_collision(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> bool:
        """
        Check if the line of sight between two points is in collision.

        Args:
            p1: Start point of the line.
            p2: End point of the line.

        Returns:
            in_collision: True if the line of sight is in collision, False otherwise.
        """
        p1_array = np.asarray(p1, dtype=np.int64)
        p2_array = np.asarray(p2, dtype=np.int64)
        if p1_array.shape != p2_array.shape:
            p2_array - p1_array

        return _grid_in_collision(
            p1_array,
            p2_array,
            self._shape_array,
            self._type_map_flat(),
            self._esdf_flat(),
            TYPES.OBSTACLE,
            TYPES.INFLATION,
        )

    def fill_boundary_with_obstacles(self) -> None:
        """
        Fill the boundary of the map with obstacles.
        """
        for d in range(self.dim):
            # Create a tuple of slice objects to select boundary elements in current dimension
            # First boundary (start index)
            slices_start = [slice(None)] * self.dim
            slices_start[d] = 0
            self.type_map[tuple(slices_start)] = TYPES.OBSTACLE

            # Last boundary (end index)
            slices_end = [slice(None)] * self.dim
            slices_end[d] = -1
            self.type_map[tuple(slices_end)] = TYPES.OBSTACLE

    def inflate_obstacles(self, radius: float = 1.0) -> None:
        """
        Inflate the obstacles in the map.

        Args:
            radius: Radius of the inflation.
        """
        self.update_esdf()
        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, ...], Node]) -> None:
        """
        Fill the expands in the map.

        Args:
            expands: List of expands.
        """
        for expand in expands.keys():
            if self.type_map[expand] != TYPES.FREE:
                continue
            self.type_map[expand] = TYPES.EXPAND

    def update_esdf(self) -> None:
        """
        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 di/stance to the nearest obstacle
        """
        obstacle_mask = (self.type_map.data == TYPES.OBSTACLE)
        free_mask = ~obstacle_mask

        # distance to obstacles
        dist_outside = ndimage.distance_transform_edt(free_mask, sampling=self.resolution)
        # distance to free space (internal distance of obstacles)
        dist_inside = ndimage.distance_transform_edt(obstacle_mask, sampling=self.resolution)

        self._esdf = dist_outside.astype(np.float32)
        self._esdf[obstacle_mask] = -dist_inside[obstacle_mask]

    def path_map_to_world(self, path: List[tuple]) -> List[Tuple[float, ...]]:
        """
        Convert path from map coordinates to world coordinates

        Args:
            path: a list of map coordinates

        Returns:
            path: a list of world coordinates
        """
        path = list(path)
        if not path:
            return []

        points = np.asarray(path, dtype=np.float64)
        if points.ndim != 2 or points.shape[1] != self.dim:
            raise ValueError("Point dimension does not match map dimension.")

        path_world = _grid_path_map_to_world(points, self.bounds, self.resolution)
        return [tuple(float(x) for x in path_world[i]) for i in range(path_world.shape[0])]

    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
        """
        path = list(path)
        if not path:
            return []

        points = np.asarray(path, dtype=np.float64)
        if points.ndim != 2 or points.shape[1] != self.dim:
            raise ValueError("Point dimension does not match map dimension.")

        if discrete:
            path_map = _grid_path_world_to_map_int(points, self.bounds, self.resolution, self._shape_array)
            return [tuple(int(x) for x in path_map[i]) for i in range(path_map.shape[0])]
        else:
            path_map = _grid_path_world_to_map_float(points, self.bounds, self.resolution)
            return [tuple(float(x) for x in path_map[i]) for i in range(path_map.shape[0])]

    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 = _grid_point_float_to_int(np.asarray(point, dtype=np.float64), self._shape_array)
        return tuple(int(x) for x in point_int)

    def _precompute_offsets(self):
        # Generate all possible offsets (-1, 0, +1) in each dimension
        self._diagonal_offsets_array = np.array(np.meshgrid(*[[-1, 0, 1]]*self.dim), dtype=np.int64).T.reshape(-1, self.dim)
        # Remove the zero offset (current node itself)
        self._diagonal_offsets_array = self._diagonal_offsets_array[np.any(self._diagonal_offsets_array != 0, axis=1)]
        # self._diagonal_offsets = [Node((offset.tolist(), dtype=self.dtype)) for offset in self._diagonal_offsets]
        self._diagonal_offsets = [Node(tuple(offset.tolist())) for offset in self._diagonal_offsets_array]

        # Generate only orthogonal offsets (one dimension changes by ±1)
        self._orthogonal_offsets_array = np.zeros((2*self.dim, self.dim), dtype=np.int64)
        for d in range(self.dim):
            self._orthogonal_offsets_array[2*d, d] = 1
            self._orthogonal_offsets_array[2*d+1, d] = -1
        # self._orthogonal_offsets = [Node((offset.tolist(), dtype=self.dtype)) for offset in self._orthogonal_offsets]
        self._orthogonal_offsets = [Node(tuple(offset.tolist())) for offset in self._orthogonal_offsets_array]

`fill_boundary_with_obstacles()` ¶

Fill the boundary of the map with obstacles.

Source code in src\python_motion_planning\common\env\map\grid.py

Python

def fill_boundary_with_obstacles(self) -> None:
    """
    Fill the boundary of the map with obstacles.
    """
    for d in range(self.dim):
        # Create a tuple of slice objects to select boundary elements in current dimension
        # First boundary (start index)
        slices_start = [slice(None)] * self.dim
        slices_start[d] = 0
        self.type_map[tuple(slices_start)] = TYPES.OBSTACLE

        # Last boundary (end index)
        slices_end = [slice(None)] * self.dim
        slices_end[d] = -1
        self.type_map[tuple(slices_end)] = TYPES.OBSTACLE

`fill_expands(expands)` ¶

Fill the expands in the map.

Parameters:

Name	Type	Description	Default
`expands`	`Dict[Tuple[int, ...], Node]`	List of expands.	required

Source code in src\python_motion_planning\common\env\map\grid.py

Python

def fill_expands(self, expands: Dict[Tuple[int, ...], Node]) -> None:
    """
    Fill the expands in the map.

    Args:
        expands: List of expands.
    """
    for expand in expands.keys():
        if self.type_map[expand] != TYPES.FREE:
            continue
        self.type_map[expand] = TYPES.EXPAND

`get_distance(p1, p2)` ¶

Get the distance between two points.

Parameters:

Name	Type	Description	Default
`p1`	`Tuple[int, int]`	Start point.	required
`p2`	`Tuple[int, int]`	Goal point.	required

Returns:

Name	Type	Description
`dist`	`float`	Distance between two points.

Source code in src\python_motion_planning\common\env\map\grid.py

Python

def get_distance(self, p1: Tuple[int, int], p2: Tuple[int, int]) -> float:
    """
    Get the distance between two points.

    Args:
        p1: Start point.
        p2: Goal point.

    Returns:
        dist: Distance between two points.
    """
    if len(p1) != len(p2):
        raise ValueError("Dimension mismatch")
    return _grid_distance(np.asarray(p1, dtype=np.float64), np.asarray(p2, dtype=np.float64))

`get_neighbors(node, diagonal=True)` ¶

Get neighbor nodes of a given node.

Parameters:

Name	Type	Description	Default
`node`	`Node`	Node to get neighbor nodes.	required
`diagonal`	`bool`	Whether to include diagonal neighbors.	`True`

Returns:

Name	Type	Description
`nodes`	`list`	List of neighbor nodes.

Source code in src\python_motion_planning\common\env\map\grid.py

Python

def get_neighbors(self, 
                node: Node, 
                diagonal: bool = True
                ) -> list:
    """
    Get neighbor nodes of a given node.

    Args:
        node: Node to get neighbor nodes.
        diagonal: Whether to include diagonal neighbors.

    Returns:
        nodes: List of neighbor nodes.
    """
    if node.dim != self.dim:
        raise ValueError("Node dimension does not match map dimension.")

    offsets = self._diagonal_offsets_array if diagonal else self._orthogonal_offsets_array
    positions, mask = _grid_neighbor_positions_and_mask(
        np.asarray(node.current, dtype=np.int64),
        offsets,
        self._shape_array,
        self._type_map_flat(),
        self._esdf_flat(),
        TYPES.OBSTACLE,
        TYPES.INFLATION,
    )

    return [
        Node(tuple(int(x) for x in positions[i]), node.current, node.g, node.h)
        for i in range(positions.shape[0])
        if mask[i]
    ]

`in_collision(p1, p2)` ¶

Check if the line of sight between two points is in collision.

Parameters:

Name	Type	Description	Default
`p1`	`Tuple[int, ...]`	Start point of the line.	required
`p2`	`Tuple[int, ...]`	End point of the line.	required

Returns:

Name	Type	Description
`in_collision`	`bool`	True if the line of sight is in collision, False otherwise.

Source code in src\python_motion_planning\common\env\map\grid.py

Python

def in_collision(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> bool:
    """
    Check if the line of sight between two points is in collision.

    Args:
        p1: Start point of the line.
        p2: End point of the line.

    Returns:
        in_collision: True if the line of sight is in collision, False otherwise.
    """
    p1_array = np.asarray(p1, dtype=np.int64)
    p2_array = np.asarray(p2, dtype=np.int64)
    if p1_array.shape != p2_array.shape:
        p2_array - p1_array

    return _grid_in_collision(
        p1_array,
        p2_array,
        self._shape_array,
        self._type_map_flat(),
        self._esdf_flat(),
        TYPES.OBSTACLE,
        TYPES.INFLATION,
    )

`inflate_obstacles(radius=1.0)` ¶

Inflate the obstacles in the map.

Parameters:

Name	Type	Description	Default
`radius`	`float`	Radius of the inflation.	`1.0`

Source code in src\python_motion_planning\common\env\map\grid.py

Python

def inflate_obstacles(self, radius: float = 1.0) -> None:
    """
    Inflate the obstacles in the map.

    Args:
        radius: Radius of the inflation.
    """
    self.update_esdf()
    mask = (self.esdf <= radius) & (self.type_map.data == TYPES.FREE)
    self.type_map[mask] = TYPES.INFLATION
    self.inflation_radius = radius

`is_expandable(point, src_point=None)` ¶

Check if a point is expandable.

Parameters:

Name	Type	Description	Default
`point`	`Tuple[int, ...]`	Point to check.	required
`src_point`	`Tuple[int, ...]`	Source point.	`None`

Returns:

Name	Type	Description
`expandable`	`bool`	True if the point is expandable, False otherwise.

Source code in src\python_motion_planning\common\env\map\grid.py

Python

def is_expandable(self, point: Tuple[int, ...], src_point: Tuple[int, ...] = None) -> bool:
    """
    Check if a point is expandable.

    Args:
        point: Point to check.
        src_point: Source point.

    Returns:
        expandable: True if the point is expandable, False otherwise.
    """
    point_array = np.asarray(point, dtype=np.int64)
    has_src_point = src_point is not None
    src_array = point_array if src_point is None else np.asarray(src_point, dtype=np.int64)

    return _grid_is_expandable(
        point_array,
        src_array,
        has_src_point,
        self._shape_array,
        self._type_map_flat(),
        self._esdf_flat(),
        TYPES.OBSTACLE,
        TYPES.INFLATION,
    )

`line_of_sight(p1, p2)` ¶

N-dimensional line of sight (Bresenham's line algorithm)

Parameters:

Name	Type	Description	Default
`p1`	`Tuple[int, ...]`	Start point of the line.	required
`p2`	`Tuple[int, ...]`	End point of the line.	required

Returns:

Name	Type	Description
`points`	`List[Tuple[int, ...]]`	List of point on the line of sight.

Source code in src\python_motion_planning\common\env\map\grid.py

Python

def line_of_sight(self, p1: Tuple[int, ...], p2: Tuple[int, ...]) -> List[Tuple[int, ...]]:
    """
    N-dimensional line of sight (Bresenham's line algorithm)

    Args:
        p1: Start point of the line.
        p2: End point of the line.

    Returns:
        points: List of point on the line of sight.
    """
    p1_array = np.asarray(p1, dtype=np.int64)
    p2_array = np.asarray(p2, dtype=np.int64)
    if p1_array.shape != p2_array.shape:
        p2_array - p1_array

    points = _grid_line_of_sight(p1_array, p2_array)
    return [tuple(int(x) for x in points[i]) for i in range(points.shape[0])]

`map_to_world(point)` ¶

Convert map coordinates to world coordinates.

Parameters:

Name	Type	Description	Default
`point`	`tuple`	Point in map coordinates.	required

Returns:

Name	Type	Description
`point`	`Tuple[float, ...]`	Point in world coordinates.

Source code in src\python_motion_planning\common\env\map\grid.py

Python

def map_to_world(self, point: tuple) -> Tuple[float, ...]:
    """
    Convert map coordinates to world coordinates.

    Args:
        point: Point in map coordinates.

    Returns:
        point: Point in world coordinates.
    """
    if len(point) != self.dim:
        raise ValueError("Point dimension does not match map dimension.")

    point_world = _grid_map_to_world(np.asarray(point, dtype=np.float64), self.bounds, self.resolution)
    return tuple(float(x) for x in point_world)

`path_map_to_world(path)` ¶

Convert path from map coordinates to world coordinates

Parameters:

Name	Type	Description	Default
`path`	`List[tuple]`	a list of map coordinates	required

Returns:

Name	Type	Description
`path`	`List[Tuple[float, ...]]`	a list of world coordinates

Source code in src\python_motion_planning\common\env\map\grid.py

Python

def path_map_to_world(self, path: List[tuple]) -> List[Tuple[float, ...]]:
    """
    Convert path from map coordinates to world coordinates

    Args:
        path: a list of map coordinates

    Returns:
        path: a list of world coordinates
    """
    path = list(path)
    if not path:
        return []

    points = np.asarray(path, dtype=np.float64)
    if points.ndim != 2 or points.shape[1] != self.dim:
        raise ValueError("Point dimension does not match map dimension.")

    path_world = _grid_path_map_to_world(points, self.bounds, self.resolution)
    return [tuple(float(x) for x in path_world[i]) for i in range(path_world.shape[0])]

`path_world_to_map(path, discrete=True)` ¶

Convert path from world coordinates to map coordinates

Parameters:

Name	Type	Description	Default
`path`	`List[Tuple[float, ...]]`	a list of world coordinates	required
`discrete`	`bool`	whether to round the coordinates to the nearest integer	`True`

Returns:

Name	Type	Description
`path`	`List[tuple]`	a list of map coordinates

Source code in src\python_motion_planning\common\env\map\grid.py

Python

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
    """
    path = list(path)
    if not path:
        return []

    points = np.asarray(path, dtype=np.float64)
    if points.ndim != 2 or points.shape[1] != self.dim:
        raise ValueError("Point dimension does not match map dimension.")

    if discrete:
        path_map = _grid_path_world_to_map_int(points, self.bounds, self.resolution, self._shape_array)
        return [tuple(int(x) for x in path_map[i]) for i in range(path_map.shape[0])]
    else:
        path_map = _grid_path_world_to_map_float(points, self.bounds, self.resolution)
        return [tuple(float(x) for x in path_map[i]) for i in range(path_map.shape[0])]

`point_float_to_int(point)` ¶

Convert a point from float to integer coordinates.

Parameters:

Name	Type	Description	Default
`point`	`Tuple[float, ...]`	a point in float coordinates	required

Returns:

Name	Type	Description
`point`	`Tuple[int, ...]`	a point in integer coordinates

Source code in src\python_motion_planning\common\env\map\grid.py

Python

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 = _grid_point_float_to_int(np.asarray(point, dtype=np.float64), self._shape_array)
    return tuple(int(x) for x in point_int)

`update_esdf()` ¶

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 di/stance to the nearest obstacle

Source code in src\python_motion_planning\common\env\map\grid.py

Python

def update_esdf(self) -> None:
    """
    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 di/stance to the nearest obstacle
    """
    obstacle_mask = (self.type_map.data == TYPES.OBSTACLE)
    free_mask = ~obstacle_mask

    # distance to obstacles
    dist_outside = ndimage.distance_transform_edt(free_mask, sampling=self.resolution)
    # distance to free space (internal distance of obstacles)
    dist_inside = ndimage.distance_transform_edt(obstacle_mask, sampling=self.resolution)

    self._esdf = dist_outside.astype(np.float32)
    self._esdf[obstacle_mask] = -dist_inside[obstacle_mask]

`within_bounds(point)` ¶

Check if a point is within the bounds of the grid map.

Parameters:

Name	Type	Description	Default
`point`	`Tuple[int, ...]`	Point to check.	required

Returns:

Name	Type	Description
`bool`	`bool`	True if the point is within the bounds of the map, False otherwise.

Source code in src\python_motion_planning\common\env\map\grid.py

Python

def within_bounds(self, point: Tuple[int, ...]) -> bool:
    """
    Check if a point is within the bounds of the grid map.

    Args:
        point: Point to check.

    Returns:
        bool: True if the point is within the bounds of the map, False otherwise.
    """
    # if point.dim != self.dim:
    #     raise ValueError("Point dimension does not match map dimension.")

    # return all(0 <= point[i] < self.shape[i] for i in range(self.dim))
    return _grid_within_bounds(np.asarray(point, dtype=np.int64), self._shape_array)

`world_to_map(point, discrete=True)` ¶

Convert world coordinates to map coordinates.

Parameters:

Name	Type	Description	Default
`point`	`Tuple[float, ...]`	Point in world coordinates.	required
`discrete`	`bool`	Whether to round the coordinates to the nearest integer.	`True`

Returns:

Name	Type	Description
`point`	`tuple`	Point in map coordinates.

Source code in src\python_motion_planning\common\env\map\grid.py

Python

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.
    """
    if len(point) != self.dim:
        raise ValueError("Point dimension does not match map dimension.")

    point_array = np.asarray(point, dtype=np.float64)
    if discrete:
        point_map = _grid_world_to_map_int(point_array, self.bounds, self.resolution, self._shape_array)
        return tuple(int(x) for x in point_map)
    else:
        point_map = _grid_world_to_map_float(point_array, self.bounds, self.resolution)
        return tuple(float(x) for x in point_map)

Grid¶

src.python_motion_planning.common.env.map.grid.Grid ¶

fill_boundary_with_obstacles() ¶

fill_expands(expands) ¶

get_distance(p1, p2) ¶

get_neighbors(node, diagonal=True) ¶

in_collision(p1, p2) ¶

inflate_obstacles(radius=1.0) ¶

is_expandable(point, src_point=None) ¶

line_of_sight(p1, p2) ¶

map_to_world(point) ¶

path_map_to_world(path) ¶

path_world_to_map(path, discrete=True) ¶

point_float_to_int(point) ¶

update_esdf() ¶

within_bounds(point) ¶

world_to_map(point, discrete=True) ¶

`src.python_motion_planning.common.env.map.grid.Grid` ¶

`fill_boundary_with_obstacles()` ¶

`fill_expands(expands)` ¶

`get_distance(p1, p2)` ¶

`get_neighbors(node, diagonal=True)` ¶

`in_collision(p1, p2)` ¶

`inflate_obstacles(radius=1.0)` ¶

`is_expandable(point, src_point=None)` ¶

`line_of_sight(p1, p2)` ¶

`map_to_world(point)` ¶

`path_map_to_world(path)` ¶

`path_world_to_map(path, discrete=True)` ¶

`point_float_to_int(point)` ¶

`update_esdf()` ¶

`within_bounds(point)` ¶

`world_to_map(point, discrete=True)` ¶