Map classes

Planner endpoints, returned paths and plot axes use world coordinates (x, y), or (x, y, theta) with angles in radians. Distances and obstacle inflation radii are in world units; parameters explicitly expressed in cells, such as PRM’s collision_step, retain that meaning.

An occupancy grid’s origin is the world position of cell (column=0, row=0), and cellsize is the side length of a cell. Arrays are indexed as [row, column]. Use world_to_grid (w2g) and grid_to_world (g2w) to convert coordinates. A raw array has unit cells and origin (0, 0), as in the book examples. DistanceTransformPlanner, DstarPlanner and Bug2 round endpoints to the nearest cell and return cell centres in world coordinates; continuous planners retain continuous world coordinates. Distance-transform fields are in world units, while the standalone distancexform array algorithm measures distances in cells. D* costs are per world unit, and its sensor receives and returns positions in world coordinates.

A map draws its own background. An occupancy grid’s plot method accepts an optional field with the same shape as the grid, plus colorbar, colorbar_style, labels, label_style, cmap and an inflated obstacle mask. The existing map argument replaces the image without an obstacle overlay. A polygon map draws its polygons in world coordinates.

Fields, obstacles and labels share the grid’s coordinate conversion and the supplied axes. Image edges extend half a cell beyond the outer cell centres; labels sit at cell centres, and enabling them shows edge cells in full. Plotting preserves the supplied field and style dictionaries.

Occupancy grid classes

Binary Occupancy grid

class BinaryOccupancyGrid(grid=None, origin=(0, 0), value=0, cellsize=1, workspace=None, name=None)[source]

Bases: BaseOccupancyGrid

Binary occupancy grid

__init__(grid=None, origin=(0, 0), value=0, cellsize=1, workspace=None, name=None)[source]

Create a binary occupancy grid instance

Parameters:
Raises:

ValueError – grid is not an array or an occupancy grid, or neither grid nor workspace is given

The array is kept internally as a bool array. Cells are set to True (occupied) corresponding to input values > 0.

This object supports a user-defined coordinate system and grid size. World coordinates are converted to grid coordinates to lookup the occupancy status.

Example:

>>> from roboticstoolbox import BinaryOccupancyGrid
>>> import numpy as np
>>> og = BinaryOccupancyGrid(np.zeros((5,5)))
>>> print(og)
BinaryOccupancyGrid: 5 x 5, cell size=1, x = [0.0, 4.0], y = [0.0, 4.0], 0.0% occupied
>>> og = BinaryOccupancyGrid(workspace=[-5,5], cellsize=0.1, value=0)
>>> print(og)
BinaryOccupancyGrid: 101 x 101, cell size=0.1, x = [-5.0, 5.0], y = [-5.0, 5.0], 0.0% occupied

See also

OccupancyGrid

__str__()[source]

Compact string description of the occupancy grid, including the percentage of cells that are occupied

Return type:

str

isoccupied(p)[source]

Test if coordinate is occupied

Parameters:

p (Union[List[float], Tuple[float, float], ndarray[Tuple[Literal[2]], dtype[floating]]]) – world coordinate (x, y)

Return type:

bool

Returns:

occupancy status of corresponding grid cell

The grid cell size and offset are used to convert p to an occupancy grid coordinate. The grid coordinate is rounded and cast to integer value. If the coordinate is outside the bounds of the occupancy grid it is considered to be occupied.

See also

w2g()

inflate(radius)[source]

Inflate obstacles

Parameters:

radius (float) – radius of circular structuring element in world units

Return type:

None

A circular structuring element is created and used to dilate the stored occupancy grid.

Successive calls to inflate will compound the inflation.

See also

scipy.ndimage.binary_dilation()

__repr__()

Compact string description of occupancy grid, same as __str__()

Return type:

str

property cellsize: float

Cell size in world units (superclass)

Returns:

side length of a grid cell

copy()

Copy an occupancy grid (superclass)

Return type:

Self

Returns:

copy of the occupancy grid

g2w(p)

Convert grid coordinate to world coordinate (superclass)

Parameters:

p (Union[List[float], Tuple[float, float], ndarray[Tuple[Literal[2]], dtype[floating]]]) – grid coordinate (column, row)

Return type:

ndarray[Tuple[Literal[2]], dtype[floating]]

Returns:

world coordinate (x, y)

The grid cell size and offset are used to convert occupancy grid coordinate p to a world coordinate.

g2w is a shorthand for this method.

property grid: ndarray

Occupancy grid as a NumPy array (superclass)

Returns:

binary occupancy grid

Return type:

ndarray(N,M) of bool

If inflate has been called, this will return the inflated occupancy grid.

grid_to_world(p)

Convert grid coordinate to world coordinate (superclass)

Parameters:

p (Union[List[float], Tuple[float, float], ndarray[Tuple[Literal[2]], dtype[floating]]]) – grid coordinate (column, row)

Return type:

ndarray[Tuple[Literal[2]], dtype[floating]]

Returns:

world coordinate (x, y)

The grid cell size and offset are used to convert occupancy grid coordinate p to a world coordinate.

g2w is a shorthand for this method.

property height: float | None

Height of the workspace in world units

Returns:

y-extent of the workspace, None if there is no workspace

line_w(p1, p2)

Get index of cells along a line segment (superclass)

Parameters:
Returns:

index into the raveled grid

Return type:

ndarray

Get the indices of cells along a line segment defined by the end points given in world coordinates.

The returned indices can be applied to a raveled view of the grid.

line_w is a shorthand for this method.

See also

ravel() world_to_grid()

line_world(p1, p2)

Get index of cells along a line segment (superclass)

Parameters:
Returns:

index into the raveled grid

Return type:

ndarray

Get the indices of cells along a line segment defined by the end points given in world coordinates.

The returned indices can be applied to a raveled view of the grid.

line_w is a shorthand for this method.

See also

ravel() world_to_grid()

property maxdim: float

Maximum dimension of grid in world coordinates (superclass)

Returns:

maximum side length of the occupancy grid

Return type:

float

property name: str | None

Occupancy grid name (superclass)

Returns:

name of the occupancy grid

Return type:

str

plot(map=None, ax=None, block=None, *, field=None, colorbar=True, colorbar_style=None, labels=False, label_style=None, cmap=None, inflated=None, **kwargs)

Plot the occupancy grid (superclass)

Parameters:

  • map (ndarray | None) – array which is plotted instead of the grid, must be same size as the occupancy grid, defaults to None

  • ax (Axes | None) – Matplotlib axes to plot into, defaults to None (the current axes)

  • block (bool | None) – None leaves showing the figure to the environment (no plt.show call), False shows it and returns at once, True shows it and waits until it is closed; ignored by notebook backends, defaults to None

  • field (ndarray | None) – optional scalar field, with the same shape as the grid; obstacles are drawn over it, defaults to None

  • colorbar (bool | dict[str, Any]) – show a colorbar for the field, defaults to True

  • colorbar_style (dict[str, Any] | None) – colorbar options, with label “Distance” by default

  • labels (bool) – label finite field values at cell centres, defaults to False

  • label_style (dict[str, Any] | None) – Matplotlib text options, merged over red centred text with a white outline

  • cmap (str | Colormap | None) – colormap for the field or replacement map. Fields default to gray; replacement images use Matplotlib’s default colormap

  • inflated (ndarray | None) – inflated obstacle mask, with the same shape as the grid; additional occupied cells are drawn in pink, defaults to None

  • kwargs (Any) – arguments passed to imshow

Raises:

ValueError – an array has the wrong shape, or both map and field are supplied

Return type:

None

The grid is plotted as an image but with axes in world coordinates.

Binary grids show free space in white, obstacles in red and inflated obstacles in pink. A field is drawn beneath the obstacles, with unreachable (infinite) values in blue. General grids and replacement images are drawn using their numeric values. Passing cmap without a field overrides the grid’s colormap.

Cell centres use the grid’s origin and cell size; image edges extend half a cell beyond the outer centres. labels shows the edge cells whole. The field and supplied style dictionaries are not modified.

See also

world_to_grid() grid_to_world()

property ravel: ndarray

Ravel the grid (superclass)

Returns:

1D view of the occupancy grid

Return type:

ndarray(N)

set(region, value)

Set region of map (superclass)

Parameters:
Return type:

None

The cells that contain the corners of the region, and all those between them, are set.

property shape: tuple[int, ...]

Shape of the occupancy grid array (superclass)

Returns:

shape of the occupancy grid array

Return type:

2-tuple

This is the shape of the NumPy array that holds the occupancy grid.

w2g(p)

Convert world coordinate to grid coordinate (superclass)

Parameters:

p (Union[List[float], Tuple[float, float], ndarray[Tuple[Literal[2]], dtype[floating]]]) – world coordinate (x, y)

Return type:

ndarray[Tuple[Literal[2]], dtype[floating]]

Returns:

grid coordinate (column, row)

The grid cell size and offset are used to convert p to an occupancy grid coordinate. The grid coordinate is rounded and cast to integer value. No check is made on the validity of the coordinate.

w2g is a shorthand for this method.

property width: float | None

Width of the workspace in world units

Returns:

x-extent of the workspace, None if there is no workspace

property workspace: ndarray[Tuple[Literal[4]], dtype[floating]]

Bounds of the occupancy grid in world coordinates (superclass)

Returns:

workspace bounds [xmin, xmax, ymin, ymax]

Returns the bounds of the occupancy grid in world coordinates.

world_to_grid(p)

Convert world coordinate to grid coordinate (superclass)

Parameters:

p (Union[List[float], Tuple[float, float], ndarray[Tuple[Literal[2]], dtype[floating]]]) – world coordinate (x, y)

Return type:

ndarray[Tuple[Literal[2]], dtype[floating]]

Returns:

grid coordinate (column, row)

The grid cell size and offset are used to convert p to an occupancy grid coordinate. The grid coordinate is rounded and cast to integer value. No check is made on the validity of the coordinate.

w2g is a shorthand for this method.

property xmax: float

Maximum x-coordinate of this grid (superclass)

Returns:

maximum world x-coordinate

Return type:

float

property xmin: float

Minimum x-coordinate of this grid (superclass)

Returns:

minimum world x-coordinate

Return type:

float

property ymax: float

Maximum y-coordinate of this grid (superclass)

Returns:

maximum world y-coordinate

Return type:

float

property ymin: float

Minimum y-coordinate of this grid (superclass)

Returns:

minimum world y-coordinate

Return type:

float

Occupancy grid

class OccupancyGrid(grid=None, origin=(0, 0), value=0, cellsize=1, workspace=None, name=None)[source]

Bases: BaseOccupancyGrid

General occupancy grid

The elements of the array are floats and can represent occupancy probability or traversal cost.

Example:

>>> from roboticstoolbox import OccupancyGrid
>>> import numpy as np
>>> og = OccupancyGrid(np.zeros((5,5)))
>>> print(og)
OccupancyGrid: 5 x 5, cell size=1, x = [0.0, 4.0], y = [0.0, 4.0], dtype float64, min 0.0, max 0.0, mean 0.0
>>> og = OccupancyGrid(workspace=[-5,5], cellsize=0.1, value=0.5)
>>> print(og)
OccupancyGrid: 101 x 101, cell size=0.1, x = [-5.0, 5.0], y = [-5.0, 5.0], dtype float64, min 0.5, max 0.5, mean 0.5

See also

BinaryOccupancyGrid

__str__()[source]

Compact string description of the occupancy grid, including the data type and the minimum, maximum and mean of the cell values

Return type:

str

__init__(grid=None, origin=(0, 0), value=0, cellsize=1, workspace=None, name=None)

Occupancy grid (superclass)

Parameters:
Raises:

ValueError – neither grid nor workspace is given

This object supports a user-defined coordinate system and grid size. World coordinates are converted to grid coordinates to lookup the occupancy status.

The grid can be initialized by:

  • a 2D NumPy array, grid, with origin and cellsize

  • specifying workspace, cellsize and value arguments

__repr__()

Compact string description of occupancy grid, same as __str__()

Return type:

str

property cellsize: float

Cell size in world units (superclass)

Returns:

side length of a grid cell

copy()

Copy an occupancy grid (superclass)

Return type:

Self

Returns:

copy of the occupancy grid

g2w(p)

Convert grid coordinate to world coordinate (superclass)

Parameters:

p (Union[List[float], Tuple[float, float], ndarray[Tuple[Literal[2]], dtype[floating]]]) – grid coordinate (column, row)

Return type:

ndarray[Tuple[Literal[2]], dtype[floating]]

Returns:

world coordinate (x, y)

The grid cell size and offset are used to convert occupancy grid coordinate p to a world coordinate.

g2w is a shorthand for this method.

property grid: ndarray

Occupancy grid as a NumPy array (superclass)

Returns:

binary occupancy grid

Return type:

ndarray(N,M) of bool

If inflate has been called, this will return the inflated occupancy grid.

grid_to_world(p)

Convert grid coordinate to world coordinate (superclass)

Parameters:

p (Union[List[float], Tuple[float, float], ndarray[Tuple[Literal[2]], dtype[floating]]]) – grid coordinate (column, row)

Return type:

ndarray[Tuple[Literal[2]], dtype[floating]]

Returns:

world coordinate (x, y)

The grid cell size and offset are used to convert occupancy grid coordinate p to a world coordinate.

g2w is a shorthand for this method.

property height: float | None

Height of the workspace in world units

Returns:

y-extent of the workspace, None if there is no workspace

line_w(p1, p2)

Get index of cells along a line segment (superclass)

Parameters:
Returns:

index into the raveled grid

Return type:

ndarray

Get the indices of cells along a line segment defined by the end points given in world coordinates.

The returned indices can be applied to a raveled view of the grid.

line_w is a shorthand for this method.

See also

ravel() world_to_grid()

line_world(p1, p2)

Get index of cells along a line segment (superclass)

Parameters:
Returns:

index into the raveled grid

Return type:

ndarray

Get the indices of cells along a line segment defined by the end points given in world coordinates.

The returned indices can be applied to a raveled view of the grid.

line_w is a shorthand for this method.

See also

ravel() world_to_grid()

property maxdim: float

Maximum dimension of grid in world coordinates (superclass)

Returns:

maximum side length of the occupancy grid

Return type:

float

property name: str | None

Occupancy grid name (superclass)

Returns:

name of the occupancy grid

Return type:

str

plot(map=None, ax=None, block=None, *, field=None, colorbar=True, colorbar_style=None, labels=False, label_style=None, cmap=None, inflated=None, **kwargs)

Plot the occupancy grid (superclass)

Parameters:

  • map (ndarray | None) – array which is plotted instead of the grid, must be same size as the occupancy grid, defaults to None

  • ax (Axes | None) – Matplotlib axes to plot into, defaults to None (the current axes)

  • block (bool | None) – None leaves showing the figure to the environment (no plt.show call), False shows it and returns at once, True shows it and waits until it is closed; ignored by notebook backends, defaults to None

  • field (ndarray | None) – optional scalar field, with the same shape as the grid; obstacles are drawn over it, defaults to None

  • colorbar (bool | dict[str, Any]) – show a colorbar for the field, defaults to True

  • colorbar_style (dict[str, Any] | None) – colorbar options, with label “Distance” by default

  • labels (bool) – label finite field values at cell centres, defaults to False

  • label_style (dict[str, Any] | None) – Matplotlib text options, merged over red centred text with a white outline

  • cmap (str | Colormap | None) – colormap for the field or replacement map. Fields default to gray; replacement images use Matplotlib’s default colormap

  • inflated (ndarray | None) – inflated obstacle mask, with the same shape as the grid; additional occupied cells are drawn in pink, defaults to None

  • kwargs (Any) – arguments passed to imshow

Raises:

ValueError – an array has the wrong shape, or both map and field are supplied

Return type:

None

The grid is plotted as an image but with axes in world coordinates.

Binary grids show free space in white, obstacles in red and inflated obstacles in pink. A field is drawn beneath the obstacles, with unreachable (infinite) values in blue. General grids and replacement images are drawn using their numeric values. Passing cmap without a field overrides the grid’s colormap.

Cell centres use the grid’s origin and cell size; image edges extend half a cell beyond the outer centres. labels shows the edge cells whole. The field and supplied style dictionaries are not modified.

See also

world_to_grid() grid_to_world()

property ravel: ndarray

Ravel the grid (superclass)

Returns:

1D view of the occupancy grid

Return type:

ndarray(N)

set(region, value)

Set region of map (superclass)

Parameters:
Return type:

None

The cells that contain the corners of the region, and all those between them, are set.

property shape: tuple[int, ...]

Shape of the occupancy grid array (superclass)

Returns:

shape of the occupancy grid array

Return type:

2-tuple

This is the shape of the NumPy array that holds the occupancy grid.

w2g(p)

Convert world coordinate to grid coordinate (superclass)

Parameters:

p (Union[List[float], Tuple[float, float], ndarray[Tuple[Literal[2]], dtype[floating]]]) – world coordinate (x, y)

Return type:

ndarray[Tuple[Literal[2]], dtype[floating]]

Returns:

grid coordinate (column, row)

The grid cell size and offset are used to convert p to an occupancy grid coordinate. The grid coordinate is rounded and cast to integer value. No check is made on the validity of the coordinate.

w2g is a shorthand for this method.

property width: float | None

Width of the workspace in world units

Returns:

x-extent of the workspace, None if there is no workspace

property workspace: ndarray[Tuple[Literal[4]], dtype[floating]]

Bounds of the occupancy grid in world coordinates (superclass)

Returns:

workspace bounds [xmin, xmax, ymin, ymax]

Returns the bounds of the occupancy grid in world coordinates.

world_to_grid(p)

Convert world coordinate to grid coordinate (superclass)

Parameters:

p (Union[List[float], Tuple[float, float], ndarray[Tuple[Literal[2]], dtype[floating]]]) – world coordinate (x, y)

Return type:

ndarray[Tuple[Literal[2]], dtype[floating]]

Returns:

grid coordinate (column, row)

The grid cell size and offset are used to convert p to an occupancy grid coordinate. The grid coordinate is rounded and cast to integer value. No check is made on the validity of the coordinate.

w2g is a shorthand for this method.

property xmax: float

Maximum x-coordinate of this grid (superclass)

Returns:

maximum world x-coordinate

Return type:

float

property xmin: float

Minimum x-coordinate of this grid (superclass)

Returns:

minimum world x-coordinate

Return type:

float

property ymax: float

Maximum y-coordinate of this grid (superclass)

Returns:

maximum world y-coordinate

Return type:

float

property ymin: float

Minimum y-coordinate of this grid (superclass)

Returns:

minimum world y-coordinate

Return type:

float

Polygon map

class PolygonMap(workspace=None, polygons=None)[source]

Bases: BaseMap

Polygonal obstacle map

__init__(workspace=None, polygons=None)[source]

Polygonal obstacle map

Parameters:

The obstacle polygons are specified as instances of Polygon2. Use add() to add a polygon given as an ndarray(2,N).

The workspace can be specified in several ways:

workspace

x-range

y-range

A (scalar)

-A:A

-A:A

[A, B]

A:B

A:B

[A, B, C, D]

A:B

C:D

Workspace is used only to set plot bounds.

add(polygon)[source]

Add a polygon to map

Parameters:

polygon (Polygon2 | ndarray) – a polygon, or its vertices

Return type:

None

property height: float | None

Height of the workspace in world units

Returns:

y-extent of the workspace, None if there is no workspace

property width: float | None

Width of the workspace in world units

Returns:

x-extent of the workspace, None if there is no workspace

iscollision(polygon)[source]

Test for collision

Parameters:

polygon (Polygon2 | ndarray) – a polygon, or its vertices

Return type:

bool

Returns:

collision

The polygon is tested against polygons in the map, and returns True on the first collision.

See also

add() Polygon2

plot(block=None)[source]

Plot the polygonal obstacles

Parameters:

block (bool | None) – None leaves showing the figure to the environment (no plt.show call), False shows it and returns at once, True shows it and waits until it is closed; ignored by notebook backends, defaults to None

Return type:

None

The obstacles are plotted in red, in axes that have the limits of the workspace.

isoccupied(p)[source]

Test if point lies inside an obstacle

Parameters:

p (Union[List[float], Tuple[float, float], ndarray[Tuple[Literal[2]], dtype[floating]]]) – a 2D point

Return type:

bool

Returns:

True if the point lies inside any obstacle

The point is tested for enclosure by polygons in the map, and returns True on the first enclosure.

property workspace: ndarray | None

Bounds of the map

Returns:

workspace bounds [xmin, xmax, ymin, ymax], None if not set

Return type:

ndarray(4) or None

Returns the bounds of the workspace given to the constructor, which is used only to set the limits of plots.