Supporting classes

Planner superclass

class roboticstoolbox.mobile.PlannerBase(occgrid=None, inflate=0, ndims=None, start=None, goal=None, verbose=False, msgcolor='yellow', progress=True, marker=None, seed=None, **unused)[source]

Bases: ABC

Mobile robot motion planner (superclass)

Parameters:

  • occgrid (OccGrid instance of ndarray(N,M), optional) – occupancy grid, defaults to None

  • start (array_like(2) or array_like(3), optional) – start position \((x, y)\) or configuration \((x, y, \theta)\), defaults to None

  • goal (array_like(2) or array_like(3), optional) – goal position \((x, y)\) or configuration \((x, y, \theta)\), defaults to None

  • inflate (float, optional) – obstacle inflation, defaults to 0

  • ndims – dimensionality of the planning, either 2 for \(\mathbb{R}^2\) or 3 for \(\SE{2}\)

  • ndims – int, optional

  • verbose (bool, optional) – verbosity, defaults to False

  • msgcolor (str, defaults to yellow) – color for message channel printing

  • seed (int, optional) – seed provided to private random number generator, defaults to None

Superclass for all mobile robot motion planners. Key functionality includes:

  • encapsulates an occupancy grid and optionally inflates it

  • validates start and goal if given

  • encapsulates a private random number generator with specifiable seed

  • encapsulates state such as start, goal, and the plan

  • provides a message channel for diagnostic output

The start and goal can be specifed in various ways:

  • at constructor time by the arguments start or goal

  • by assigning the attributes start or goal

  • at planning time by specifying goal to plan()

  • at query time by specifying start to query()

Seealso:

OccGrid

__str__()[source]

Compact representation of the planner

Returns:

pretty printed representation

Return type:

str

property start

Set/get start point or configuration (superclass)

Getter:

Return start point or configuration

Return type:

ndarray(2) or ndarray(3)

Setter:

Set start point or configuration

Param:

array_like(2) or array_like(3)

The start is either a point \((x, y)\) or a configuration \((x, y, \theta)\).

Seealso:

goal()

property goal

Set/get goal point or configuration (superclass)

Getter:

Return goal pointor configuration

Return type:

ndarray(2) or ndarray(3)

Setter:

Set goal point or configuration

Param:

array_like(2) or array_like(3)

The goal is either a point \((x, y)\) or a configuration \((x, y, \theta)\).

Seealso:

goal()

property verbose

Get verbosity

Returns:

verbosity

Return type:

bool

If verbosity print more diagnostic messages to the planner’s message channel.

property random

Get private random number generator

Returns:

NumPy random number generator

Return type:

numpy.random.Generator

Has methods including:

The generator is initialized with the seed provided at constructor time.

Seealso:

numpy.random.default_rng()

random_init(seed=None)[source]

Initialize private random number generator

Parameters:

seed (int) – random number seed, defaults to value given to constructor

The private random number generator is initialized. The seed is seed or the value given to the constructor. If None, the generator will be randomly seeded using a seed from the operating system.

plan()[source]

Plan path (superclass)

Parameters:

  • start (array_like(2) or array_like(3), optional) – start position \((x, y)\) or configuration \((x, y, \theta)\), defaults to value passed to constructor

  • goal (array_like(2) or array_like(3), optional) – goal position \((x, y)\) or configuration \((x, y, \theta)\), defaults to value passed to constructor

The implementation depends on the particular planner. Some may have no planning phase. The plan may also depend on just the start or goal.

property occgrid

Occupancy grid

Returns:

occupancy grid used for planning

Return type:

OccGrid or subclass or None

Returns the occupancy grid that was optionally inflated at constructor time.

Seealso:

validate_endpoint() isoccupied()

isoccupied(p)[source]

Test if point is occupied (superclass)

Parameters:

p (array_like(2)) – world coordinate (x, y)

Returns:

occupancy status of corresponding grid cell

Return type:

bool

The world coordinate is transformed and the status of the occupancy grid cell is returned. If the point lies outside the bounds of the occupancy grid return True (obstacle)

If there is no occupancy grid this function always returns False (free).

Seealso:

occgrid() validate_endpoint() BinaryOccGrid.isoccupied()

validate_endpoint(p, dtype=None)[source]

Validate start or goal point

Parameters:

  • p (array_like(2)) – the point

  • dtype (str, optional) – data type for point coordinates, defaults to None

Raises:

ValueError – point is inside obstacle

Returns:

point as a NumPy array of specified dtype

Return type:

ndarray(2)

The coordinate is tested to be a free cell in the occupancy grid.

Seealso:

isoccupied() occgrid()

progress_start(n)[source]

Initialize a progress bar (superclass)

Parameters:

n (int) – Number of iterations in the operation

Create a progress bar for an operation that has n steps, for example:

planner.progress_start(100)
for i in range(100):
    # ...
    planner.progress_next()
planner.progress_end()
Seealso:

progress_next() progress_end()

progress_next()[source]

Increment a progress bar (superclass)

Create a progress bar for an operation that has n steps, for example:

planner.progress_start(100)
for i in range(100):
    # ...
    planner.progress_next()
planner.progress_end()
Seealso:

progress_start() progress_end()

progress_end()[source]

Finalize a progress bar (superclass)

Remove/cleanip a progress bar, for example:

planner.progress_start(100)
for i in range(100):
    # ...
    planner.progress_next()
planner.progress_end()
Seealso:

progress_start() progress_next()

query(start=None, goal=None, dtype=None, next=True, animate=False, movie=None)[source]

Find a path from start to goal using planner (superclass)

Parameters:

  • start (array_like(), optional) – start position \((x, y)\) or configuration \((x, y, \theta)\), defaults to value specified to constructor

  • goal (array_like(), optional) – goal position \((x, y)\) or configuration \((x, y, \theta)\), defaults to value specified to constructor

  • dtype (str, optional) – data type for point coordinates, defaults to None

  • next (bool, optional) – invoke next() method of class, defaults to True

  • animate (bool, optional) – show the vehicle path, defaults to False

Returns:

path from start to goal, one point \((x, y)\) or configuration \((x, y, \theta)\) per row

Return type:

ndarray(N,2) or ndarray(N,3)

Find a path from start to goal using a previously computed plan.

This is a generic method that works for any planner (\(\mathbb{R}^2\) or \(\SE{2}\)) that can incrementally determine the next point on the path. The method performs the following steps:

  • Validate start and goal

  • If animate, visualize the environment using plot()

  • Iterate on the class’s next() method until the goal is achieved, and if animate plot points.

Seealso:

next() plan()

plot(path=None, line=None, line_r=None, configspace=False, unwrap=True, direction=None, background=True, path_marker=None, path_marker_reverse=None, start_marker=None, goal_marker=None, start_vehicle=None, goal_vehicle=None, start=None, goal=None, ax=None, block=None, bgargs={}, **unused)[source]

Plot vehicle path (superclass)

Parameters:

  • path ((N, 2) or ndarray(N, 3)) – path, defaults to None

  • direction (array_like(N), optional) – travel direction associated with each point on path, is either >0 or <0, defaults to None

  • line (sequence of dict of arguments for plot) – line style for forward motion, default is striped yellow on black

  • line_r (sequence of dict of arguments for plot) – line style for reverse motion, default is striped red on black

  • configspace (bool, optional) – plot the path in 3D configuration space, input must be 3xN. Start and goal style will be given by qstart_marker and qgoal_marker, defaults to False

  • unwrap (bool, optional) – for configuration space plot unwrap \(\theta\) so there are no discontinuities at \(\pm \pi\), defaults to True

  • background (bool, optional) – plot occupancy grid if present, default True

  • start_marker (dict, optional) – style for marking start point

  • goal_marker (dict, optional) – style for marking goal point

  • start_vehicle (dict) – style for vehicle animation object at start configuration

  • goal_vehicle (dict) – style for vehicle animation object at goal configuration

  • start (array_like(2) or array_like(3), optional) – start position \((x, y)\) or configuration \((x, y, \theta)\), defaults to value previously set

  • goal (array_like(2) or array_like(3), optional) – goal position \((x, y)\) or configuration \((x, y, \theta)\), defaults to value previously set

  • bgargs (dict, optional) – arguments passed to plot_bg(), defaults to None

  • ax (matplotlib axes) – axes to plot into

  • block (bool, optional) – block after displaying the plot

Plots the start and goal location/pose if they are specified by start or goal or were set by the object constructor or plan or query method.

If the start and goal have length 2, planning in \(\mathbb{R}^2\), then markers are drawn using styles specified by start_marker and end_marker which are dicts using Matplotlib keywords, for example:

planner.plot(path, start=dict(marker='s', color='b'))

If the start and goal have length 3, planning in \(\SE{2}\), and configspace is False, then direction-indicating markers are used to display start and goal configuration. These are also given as dicts but have two items: 'shape' which is the shape of the polygonal marker and is either 'triangle' or 'car'. The second item 'args' is passed to base.plot_poly() and Matplotlib and could have values such as filled=True or color.

If configspace is False then a 3D plot is created and the start and goal are indicated by Matplotlib markers specified by the dicts start_marker and end_marker

Default values are provided for all markers:

  • the start point is a circle

  • the goal point is a star

  • the start vehicle style is a VehiclePolygon(shape='car') as an unfilled outline

  • the goal vehicle style is a VehiclePolygon(shape='car') as a transparent filled shape

If background is True then the background of the plot is either or both of:

  • the occupancy grid

  • the distance field of the planner

Additional arguments bgargs can be passed through to plot_bg()

If path is specified it has one column per point and either 2 or 3 rows:

  • 2 rows describes motion in the \(xy\)-plane and a 2D plot is created

  • 3 rows describes motion in the \(xy\theta\)-configuration space. By default only the \(xy\)-plane is plotted unless configspace is True in which case motion in \(xy\theta\)-configuration space is shown as a 3D plot.

If the planner supports bi-directional motion then the direction option gives the direction for each point on the path.

Forward motion segments are drawn using style information from line while reverse motion segments are drawn using style information from line_r. These are each a sequence of dicts of Matplotlib plot options which can draw an arbitrary number of lines on top of each other. The default:

line = (
        {color:'black', linewidth:4},
        {color:'yellow', linewidth:3, dashes:(5,5)}
    )

will draw a blackline of width 4 with a dashed yellow line of width 3 plotted on top, giving a line of alternating black and yellow dashes.

Seealso:

plot_bg() base.plot_poly()

plot_bg(distance=None, cmap='gray', ax=None, inflated=True, colorbar=True, block=None, **unused)[source]

Plot background (superclass)

Parameters:

  • distance (ndarray(N,M), optional) – override distance field, defaults to None

  • cmap (str or Colormap, optional) – Specify a colormap for the distance field, defaults to ‘gray’

Displays the background which is either the occupancy grid or a distance field. The distance field encodes the distance of a point from the goal, small distance is dark, a large distance is bright.

If the planner has an occupancy grid then that will be displayed with:

  • free cells in white

  • occupied cells in red

  • inflated occupied cells in pink

If distance is provided, or the planner has a distancemap attribute the the distance field will be used as the background and obstacle cells (actual or inflated) will be shown in red. A colorbar is added.

message(s, color=None)[source]

Print message to message channel

Parameters:

  • s (str) – message to print

  • color (str, optional) – color to print it, defaults to color specified at constructor time.

Occupancy grid base classes

class roboticstoolbox.mobile.OccGrid.BaseMap(workspace=None, name=None, **unused)[source]

Bases: ABC

class roboticstoolbox.mobile.OccGrid.BaseOccupancyGrid(grid=None, origin=(0, 0), value=0, cellsize=1, **kwargs)[source]

Bases: BaseMap

copy()[source]

Copy an occupancy grid (superclass)

Returns:

copy of the ocupancy grid

Return type:

OccGrid

__str__()[source]

Compact string description of occupancy grid (superclass)

Returns:

summary of occupancy grid characteristics

Return type:

str

property grid

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.

property xmin

Minimum x-coordinate of this grid (superclass)

Returns:

minimum world x-coordinate

Return type:

float

property xmax

Maximum x-coordinate of this grid (superclass)

Returns:

maximum world x-coordinate

Return type:

float

property ymin

Minimum y-coordinate of this grid (superclass)

Returns:

minimum world y-coordinate

Return type:

float

property ymax

Maximum y-coordinate of this grid (superclass)

Returns:

maximum world y-coordinate

Return type:

float

property shape

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.

property maxdim

Maximum dimension of grid in world coordinates (superclass)

Returns:

maximum side length of the occupancy grid

Return type:

float

property workspace

Bounds of the occupancy grid in world coordinates (superclass)

Returns:

workspace bounds [xmin, xmax, ymin, ymax]

Return type:

ndarray(4)

Returns the bounds of the occupancy grid in world coordinates.

property name

Occupancy grid name (superclass)

Returns:

name of the occupancy grid

Return type:

str

set(region, value)[source]

Set region of map (superclass)

Parameters:

  • region (array_like(4)) – The region [xmin, ymin, xmax, ymax]

  • value (int, bool, float) – value to set cells to

g2w(p)[source]

Convert grid coordinate to world coordinate (superclass)

Parameters:

p (array_like(2)) – grid coordinate (column, row)

Returns:

world coordinate (x, y)

Return type:

ndarray(2)

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

w2g(p)[source]

Convert world coordinate to grid coordinate (superclass)

Parameters:

p (array_like(2)) – world coordinate (x, y)

Returns:

grid coordinate (column, row)

Return type:

ndarray(2)

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.

plot(map=None, ax=None, block=None, **kwargs)[source]

Plot the occupancy grid (superclass)

Parameters:

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

  • ax (Axes2D, optional) – matplotlib axes to plot into, defaults to None

  • block (bool, optional) – block until plot is dismissed, defaults to None

  • kwargs – arguments passed to imshow

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

The grid is a NumPy boolean array which has values 0 (false=unoccupied) and 1 (true=occupied). Passing a cmap option to imshow can be used to control the displayed color of free space and obstacles.

line_w(p1, p2)[source]

Get index of cells along a line segment (superclass)

Parameters:

  • p1 (array_like(2)) – start

  • p2 (array_like(2)) – end

Returns:

index into grid

Return type:

ndarray(N)

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.

Seealso:

ravel() w2g()

property ravel

Ravel the grid (superclass)

Returns:

1D view of the occupancy grid

Return type:

ndarray(N)