Discrete (Grid-based) planners

Planner

Plans in

Discrete/Continuous

Obstacle avoidance

Bug2

\(\mathbb{R}^2\)

discrete

yes

DistanceTransformPlanner

\(\mathbb{R}^2\)

discrete

yes

DstarPlanner

\(\mathbb{R}^2\)

discrete

yes

LatticePlanner

\(\SE{2}\)

discrete

yes

Distance transform planner

class DistanceTransformPlanner(occgrid=None, metric='euclidean', *, distance=None, **kwargs)[source]

Bases: PlannerBase

Distance transform path planner

Parameters:

  • occgrid (BinaryOccupancyGrid | ndarray | None) – occupancy grid

  • metric (str) – distance metric, one of: “euclidean” [default], “manhattan” (“cityblock” is a synonym)

  • distance (str | None) – deprecated, use metric

  • kwargs (Any) – common planner options, see PlannerBase

Raises:

ValueError – metric is not recognised, or metric and distance are both given and are different

Deprecated since version 1.5.0: distance is deprecated, use metric. It was never an argument (it was silently ignored), but code printed in the book uses it.

Feature

Capability

Plan

\(\mathbb{R}^2\), discrete

Obstacle avoidance

Yes, occupancy grid

Curvature

Discontinuous

Motion

Omnidirectional

Creates a planner that finds the path between two points in the 2D grid using omnidirectional motion and avoiding occupied cells. The path comprises a set of 4- or 8-way connected points in adjacent cells. Also known as the wavefront, grassfire or brushfire planning algorithm.

The map is described by a 2D occupancy occgrid whose elements are zero if traversable or nonzero if untraversable, ie. an obstacle.

Positions, paths and distances are in world units. Crossing a cell horizontally or vertically travels one cellsize; a diagonal travels \(\sqrt{2}\) times that distance. Endpoints are rounded to cell centres.

Example:

>>> from roboticstoolbox import DistanceTransformPlanner
>>> import numpy as np
>>> simplegrid = np.zeros((6, 6));
>>> simplegrid[2:5, 3:5] = 1
>>> dx = DistanceTransformPlanner(simplegrid, goal=(1, 1), metric="manhattan");
>>> dx.plan()
>>> path = dx.query(start=(5, 4))
>>> print(path.T)
[[5 5 5 4 3 2 1]
 [4 3 2 1 1 1 1]]

Warning

The distance planner is iterative and implemented in Python, will be slow for very large occupancy grids.

Author:

Peter Corke

See also

plan() query() PlannerBase

property metric: str

Get the distance metric

Returns:

the metric, either “euclidean” or “manhattan”

property distancemap: ndarray | None

Get the distance map

Returns:

distance map

Return type:

ndarray(h,w)

The 2D array, the same size as the passed occupancy grid, has elements equal to nan if they contain an obstacle, otherwise the minimum obstacle-free distance to the goal using the particular distance metric.

__str__()[source]

Compact representation of the planner

Return type:

str

Returns:

the base class description, the metric and whether a distance map has been computed

plan(goal: List[float] | Tuple[float, float] | ndarray[Tuple[Literal[2]], dtype[floating]] | None = None, animate: bool | None = None, summary: bool = False) → None[source]

Plan path using distance transform

Parameters:
Raises:

ValueError – no goal is given here or to the constructor, or there is no occupancy grid

Return type:

None

Compute the distance transform for all non-obstacle cells, which is the minimum obstacle-free distance to the goal using the particular distance metric.

See also

query()

query(start: List[float] | Tuple[float, float] | ndarray[Tuple[Literal[2]], dtype[floating]] | None = None, goal: List[float] | Tuple[float, float] | ndarray[Tuple[Literal[2]], dtype[floating]] | None = None, dtype: Any = None, animate: bool | None = None, movie: str | tuple[str, float] | None = None, *, next: bool | None = None) → ndarray[source]

Find a path from start to goal using the distance map

Parameters:
Raises:
Returns:

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

Return type:

ndarray

Repeatedly steps to the neighbouring cell with the lowest distance until the goal is reached.

Deprecated since version 1.5.0: next was accepted but had no effect.

See also

plan()

next(position: List[float] | Tuple[float, float] | ndarray[Tuple[Literal[2]], dtype[floating]]) → ndarray | None[source]

Find next point on the path

Parameters:

position (Union[List[float], Tuple[float, float], ndarray[Tuple[Literal[2]], dtype[floating]]]) – current robot position in world coordinates

Raises:
Returns:

next robot position, None if the next point is the goal

Return type:

ndarray | None

Return the robot position that is one step closer to the goal. Called by query() to find a path from start to goal.

See also

plan() query()

plot_3d(path: ndarray | None = None, path_style: dict[str, Any] | None = None, block: bool | None = True, *, ls: dict[str, Any] | None = None) → Any[source]

Plot path on 3D cost surface

Parameters:

  • path (ndarray | None) – robot path, one point per row, defaults to None

  • path_style (dict[str, Any] | None) – dictionary of Matplotlib line options for the path, defaults to None

  • 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 True

  • ls (dict[str, Any] | None) – deprecated, use path_style

Raises:

ValueError – no plan has been computed

Returns:

Matplotlib 3D axes

Return type:

Any

Creates a 3D plot showing distance from the goal as a cost surface. Overlays the path if given.

Deprecated since version 1.5.0: ls is deprecated, use path_style.

Warning

The visualization is poor because of Matplotlib’s poor hidden line/surface handling.

__repr__()

Compact representation of the planner, same as __str__()

Return type:

str

property animate: bool

Get or set animation of the planner’s operation

Getter:

True if planners that can animate will do so

Setter:

Set the default for methods that take an animate argument

A movie file given to the constructor also implies animation.

See also

animation_start()

animation_end() → None

End an animation, closing the movie file if there is one

See also

animation_start()

Return type:

None

animation_frame(pause: float = 0.05) → None

Show the current state of the animation

Parameters:

pause (float) – time to pause for the display to update, defaults to 0.05 s

Return type:

None

Updates the display and adds a frame to the movie, if there is one.

See also

animation_start()

animation_start(animate: bool | None = None, movie: str | tuple[str, float] | None = None, draw: Callable[[], Any] | None = None) → bool

Begin an animation

Parameters:

  • animate (bool | None) – animate this call, defaults to the planner’s animate

  • movie (str | tuple[str, float] | None) – movie file for this call, defaults to the planner’s movie

  • draw (Callable[[], Any] | None) – function that draws the static background of the animation

Return type:

bool

Returns:

True if the caller should animate

The planner decides what is drawn and when, this helper owns the pacing and recording. If animating, draw is called, then the movie file if any is opened on the current figure. The caller then calls animation_frame() after updating its artists, and animation_end() when done.

A movie implies animation.

See also

animation_frame() animation_end()

property goal: ndarray[Tuple[Literal[2]], dtype[floating]] | ndarray[Tuple[Literal[3]], dtype[floating]] | None

Set/get goal point or configuration (superclass)

Getter:

Return goal point or configuration, None if not set

Setter:

Set goal point or configuration, None is ignored

Raises:

ValueError – (setter) not of length ndims, or inside an obstacle

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

See also

start()

Setter accepts:

Union[List[float], Tuple[float, float], ndarray[Tuple[Literal[2]], dtype[floating]], Tuple[float, float, float], ndarray[Tuple[Literal[3]], dtype[floating]], None]

isoccupied(p: List[float] | Tuple[float, float] | ndarray[Tuple[Literal[2]], dtype[floating]] | Tuple[float, float, float] | ndarray[Tuple[Literal[3]], dtype[floating]]) → bool

Test if point is occupied (superclass)

Parameters:

p (Union[List[float], Tuple[float, float], ndarray[Tuple[Literal[2]], dtype[floating]], Tuple[float, float, float], ndarray[Tuple[Literal[3]], dtype[floating]]]) – world coordinate (x, y), or configuration (x, y, :math:` heta`)

Return type:

bool

Returns:

occupancy status of corresponding grid cell

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

See also

occgrid() validate_endpoint() isoccupied

property movie: str | tuple[str, float] | None

Get or set the movie file for the animation

Getter:

filename, or tuple (filename, frame interval), None for no movie

Setter:

Set the default movie file, None for no movie

property occgrid: BinaryOccupancyGrid | None

Occupancy grid

Returns:

occupancy grid used for planning, None if there is none

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

See also

validate_endpoint() isoccupied()

plot(path: NDArray | None = None, path_style: dict[str, Any] | Sequence[dict[str, Any]] | None = None, path_reverse_style: dict[str, Any] | Sequence[dict[str, Any]] | None = None, configspace: bool = False, unwrap: bool = True, direction: ArrayLike | None = None, background: bool = True, path_marker: dict[str, Any] | Sequence[dict[str, Any]] | None = None, path_marker_reverse: dict[str, Any] | Sequence[dict[str, Any]] | None = None, start_style: dict[str, Any] | None = None, goal_style: dict[str, Any] | None = None, start_vehicle_style: dict[str, Any] | None = None, goal_vehicle_style: dict[str, Any] | None = None, start: ArrayLike2 | ArrayLike3 | None = None, goal: ArrayLike2 | ArrayLike3 | None = None, ax: Axes | None = None, block: bool | None = None, bgargs: dict[str, Any] | None = None, *, line: dict[str, Any] | Sequence[dict[str, Any]] | None = None, line_r: dict[str, Any] | Sequence[dict[str, Any]] | None = None, start_marker: dict[str, Any] | None = None, goal_marker: dict[str, Any] | None = None, start_vehicle: dict[str, Any] | None = None, goal_vehicle: dict[str, Any] | None = None, labels: bool = False, label_style: dict[str, Any] | None = None, labelvalues: bool = False, inflated: bool | None = None, colorbar: bool | dict[str, Any] | None = None, colorbar_style: dict[str, Any] | None = None, cmap: str | None = None, **unused: Any) → Axes

Plot vehicle path (superclass)

Parameters:
Raises:

ValueError – configspace is True but the path is not in configuration space

Return type:

Axes

Returns:

the axes plotted into

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_style and goal_style, which are dicts using Matplotlib keywords, for example:

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

If the start and goal have length 3, planning in \(\SE{2}\), and configspace is False, then the start and goal configuration are displayed as a vehicle, using start_vehicle_style and goal_vehicle_style.

If configspace is True then a 3D plot is created and the start and goal are indicated by Matplotlib markers specified by the dicts start_style and goal_style.

Default values are provided for all styles:

  • 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 the layers of path_style while reverse motion segments are drawn using path_reverse_style. These are each one dict, or a sequence of dicts, of Matplotlib plot options which can draw an arbitrary number of lines on top of each other. The default:

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

will draw a black line of width 4 with a dashed yellow line of width 3 plotted on top, giving a line of alternating black and yellow dashes. A style given by the caller replaces these layers.

Deprecated since version 1.5.0: The arguments line, line_r, path_marker, path_marker_reverse, start_marker, goal_marker, start_vehicle and goal_vehicle are deprecated, use path_style, path_reverse_style, start_style, goal_style, start_vehicle_style and goal_vehicle_style. path_marker and path_marker_reverse were accepted but had no effect.

See also

plot_bg() plot_poly

Note

Unknown keyword arguments are ignored, with a DeprecationWarning, as they were silently by RTB 1.4.4 and earlier.

Deprecated since version 1.5.0: labelvalues is deprecated, use labels. It is the name used in the book, where it had no effect.

Changed in version 1.5.0: block=False shows the figure without waiting, as documented. It used to do nothing, the same as None.

plot_bg(distance: NDArray | None = None, cmap: str = 'gray', ax: Axes | None = None, inflated: bool = True, colorbar: bool | dict[str, Any] = True, block: bool | None = None, colorbar_style: dict[str, Any] | None = None, labels: bool = False, label_style: dict[str, Any] | None = None, labelvalues: bool = False, **unused: Any) → None

Plot background (superclass)

Parameters:

  • distance (ndarray | None) – override distance field, defaults to None. The array is not modified.

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

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

  • inflated (bool) – show the inflated obstacles, defaults to True

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

  • 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

  • colorbar_style (dict[str, Any] | None) – options for the colorbar, passed to matplotlib.figure.Figure.colorbar(), defaults to None. The label defaults to “Distance”.

  • labels (bool) – write the value in each cell of the distance field, defaults to False

  • labelvalues (bool) – deprecated, use labels

  • label_style (dict[str, Any] | None) – Matplotlib text options for the values, merged over red, centered fontsize 8 text, as in the book, with a thin white outline (path_effects) so that it is legible on any gray. The axes are set to show every cell whole.

Return type:

None

Deprecated since version 1.5.0: Giving a dict as colorbar is deprecated, use colorbar=True with colorbar_style.

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 distance field will be used as the background and obstacle cells (actual or inflated) will be shown in red. A colorbar is added.

Note

Unknown keyword arguments are ignored, with a DeprecationWarning, as they were silently by RTB 1.4.4 and earlier.

Deprecated since version 1.5.0: labelvalues is deprecated, use labels. It is the name used in the book, where it had no effect.

property start: ndarray[Tuple[Literal[2]], dtype[floating]] | ndarray[Tuple[Literal[3]], dtype[floating]] | None

Set/get start point or configuration (superclass)

Getter:

Return start point or configuration, None if not set

Setter:

Set start point or configuration, None is ignored

Raises:

ValueError – (setter) not of length ndims, or inside an obstacle

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

See also

goal()

Setter accepts:

Union[List[float], Tuple[float, float], ndarray[Tuple[Literal[2]], dtype[floating]], Tuple[float, float, float], ndarray[Tuple[Literal[3]], dtype[floating]], None]

validate_endpoint(p: List[float] | Tuple[float, float] | ndarray[Tuple[Literal[2]], dtype[floating]] | Tuple[float, float, float] | ndarray[Tuple[Literal[3]], dtype[floating]] | None, dtype: Any = None) → ndarray[Tuple[Literal[2]], dtype[floating]] | ndarray[Tuple[Literal[3]], dtype[floating]] | None

Validate start or goal point

Parameters:
Raises:

ValueError – point is of the wrong length, or is inside an obstacle

Return type:

ndarray[Tuple[Literal[2]], dtype[floating]] | ndarray[Tuple[Literal[3]], dtype[floating]] | None

Returns:

point as a NumPy array of specified dtype, None if p is None

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

See also

isoccupied() occgrid()

validate_start_goal(start: List[float] | Tuple[float, float] | ndarray[Tuple[Literal[2]], dtype[floating]] | Tuple[float, float, float] | ndarray[Tuple[Literal[3]], dtype[floating]] | None = None, goal: List[float] | Tuple[float, float] | ndarray[Tuple[Literal[2]], dtype[floating]] | Tuple[float, float, float] | ndarray[Tuple[Literal[3]], dtype[floating]] | None = None, dtype: Any = None) → None

Validate and store the start and goal

Parameters:
Raises:

ValueError – start or goal is of the wrong length or is inside an obstacle

Return type:

None

Each given value is tested against the planner’s dimensionality and the occupancy grid, and then stored as start or goal. A value of None leaves the existing value unchanged. If either is invalid neither is stored.

See also

validate_endpoint() start goal

property verbose: bool

Get verbosity

Returns:

verbosity

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

D* planner

class DstarPlanner(costmap=None, **kwargs)[source]

Bases: PlannerBase

D* path planner

Parameters:

Feature

Capability

Plan

\(\mathbb{R}^2\), discrete

Obstacle avoidance

Yes, occupancy grid

Curvature

Discontinuous

Motion

Omnidirectional

Creates a planner that finds the minimum-cost path between two points in the plane using omnidirectional motion. The path comprises a set of 8-way connected points in adjacent cells.

The map is described by a 2D costmap whose elements indicate the cost per world unit. The cost of a step is the mean of the two cell costs multiplied by the distance between their centres. An infinite cost indicates an untraversable cell or obstacle. Endpoints and returned paths are in world coordinates; endpoints are rounded to cell centres.

Example:

>>> from roboticstoolbox import DstarPlanner
>>> import numpy as np
>>> costmap = np.ones((6, 6));
>>> costmap[2:5, 3:5] = 10
>>> ds = DstarPlanner(costmap, goal=(1, 1));
>>> ds.plan()
>>> path, status = ds.query(start=(5, 4))
>>> print(path.T)
[[5. 5. 5. 4. 3. 2. 1.]
 [4. 3. 2. 1. 1. 1. 1.]]
>>> print(status)
DstarStatus(cost=np.float64(6.414213562373095))
Thanks:

based on D* grid planning included from Python Robotics

See also

PlannerBase

plan(goal: List[float] | Tuple[float, float] | ndarray[Tuple[Literal[2]], dtype[floating]] | None = None, animate: bool | None = None, progress: bool = True, summary: bool = False) → None[source]

Plan D* path

Parameters:
Return type:

None

Deprecated since version 1.5.0: animate and progress were accepted but had no effect.

Compute the minimum-cost obstacle-free distance to the goal from all points in the grid.

property nexpand: int

Number of node expansions

Returns:

number of expansions

This number will increase during initial planning, and also if replanning is invoked during the query().

query(start: List[float] | Tuple[float, float] | ndarray[Tuple[Literal[2]], dtype[floating]], sensor: Callable[[...], Any] | None = None, animate: bool | None = None, verbose: bool = False, movie: str | tuple[str, float] | None = None) → tuple[ndarray, DstarStatus][source]

Find path with replanning

Parameters:
Returns:

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

Return type:

tuple[ndarray, DstarStatus]

If sensor is None then the plan determined by the plan phase is used unaltered.

If sensor is not None it must be callable, and is called at each step of the path with the current robot coordinates in world units:

sensor((x, y))

and mimics the behaviour of a simple sensor onboard the robot which can dynamically change the costmap. The function return a list (0 or more) of 3-tuples (x, y, newcost) which are world coordinates of cells and their cost per world unit. If the cost has changed this will trigger D* incremental replanning. In this case the value returned by nexpand() will increase, according to the severity of the replanning.

See also

plan()

__repr__()

Compact representation of the planner, same as __str__()

Return type:

str

__str__()

Compact representation of the planner

Returns:

pretty printed representation

Return type:

str

property animate: bool

Get or set animation of the planner’s operation

Getter:

True if planners that can animate will do so

Setter:

Set the default for methods that take an animate argument

A movie file given to the constructor also implies animation.

See also

animation_start()

animation_end() → None

End an animation, closing the movie file if there is one

See also

animation_start()

Return type:

None

animation_frame(pause: float = 0.05) → None

Show the current state of the animation

Parameters:

pause (float) – time to pause for the display to update, defaults to 0.05 s

Return type:

None

Updates the display and adds a frame to the movie, if there is one.

See also

animation_start()

animation_start(animate: bool | None = None, movie: str | tuple[str, float] | None = None, draw: Callable[[], Any] | None = None) → bool

Begin an animation

Parameters:

  • animate (bool | None) – animate this call, defaults to the planner’s animate

  • movie (str | tuple[str, float] | None) – movie file for this call, defaults to the planner’s movie

  • draw (Callable[[], Any] | None) – function that draws the static background of the animation

Return type:

bool

Returns:

True if the caller should animate

The planner decides what is drawn and when, this helper owns the pacing and recording. If animating, draw is called, then the movie file if any is opened on the current figure. The caller then calls animation_frame() after updating its artists, and animation_end() when done.

A movie implies animation.

See also

animation_frame() animation_end()

property goal: ndarray[Tuple[Literal[2]], dtype[floating]] | ndarray[Tuple[Literal[3]], dtype[floating]] | None

Set/get goal point or configuration (superclass)

Getter:

Return goal point or configuration, None if not set

Setter:

Set goal point or configuration, None is ignored

Raises:

ValueError – (setter) not of length ndims, or inside an obstacle

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

See also

start()

Setter accepts:

Union[List[float], Tuple[float, float], ndarray[Tuple[Literal[2]], dtype[floating]], Tuple[float, float, float], ndarray[Tuple[Literal[3]], dtype[floating]], None]

property movie: str | tuple[str, float] | None

Get or set the movie file for the animation

Getter:

filename, or tuple (filename, frame interval), None for no movie

Setter:

Set the default movie file, None for no movie

property occgrid: BinaryOccupancyGrid | None

Occupancy grid

Returns:

occupancy grid used for planning, None if there is none

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

See also

validate_endpoint() isoccupied()

plot(path: NDArray | None = None, path_style: dict[str, Any] | Sequence[dict[str, Any]] | None = None, path_reverse_style: dict[str, Any] | Sequence[dict[str, Any]] | None = None, configspace: bool = False, unwrap: bool = True, direction: ArrayLike | None = None, background: bool = True, path_marker: dict[str, Any] | Sequence[dict[str, Any]] | None = None, path_marker_reverse: dict[str, Any] | Sequence[dict[str, Any]] | None = None, start_style: dict[str, Any] | None = None, goal_style: dict[str, Any] | None = None, start_vehicle_style: dict[str, Any] | None = None, goal_vehicle_style: dict[str, Any] | None = None, start: ArrayLike2 | ArrayLike3 | None = None, goal: ArrayLike2 | ArrayLike3 | None = None, ax: Axes | None = None, block: bool | None = None, bgargs: dict[str, Any] | None = None, *, line: dict[str, Any] | Sequence[dict[str, Any]] | None = None, line_r: dict[str, Any] | Sequence[dict[str, Any]] | None = None, start_marker: dict[str, Any] | None = None, goal_marker: dict[str, Any] | None = None, start_vehicle: dict[str, Any] | None = None, goal_vehicle: dict[str, Any] | None = None, labels: bool = False, label_style: dict[str, Any] | None = None, labelvalues: bool = False, inflated: bool | None = None, colorbar: bool | dict[str, Any] | None = None, colorbar_style: dict[str, Any] | None = None, cmap: str | None = None, **unused: Any) → Axes

Plot vehicle path (superclass)

Parameters:
Raises:

ValueError – configspace is True but the path is not in configuration space

Return type:

Axes

Returns:

the axes plotted into

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_style and goal_style, which are dicts using Matplotlib keywords, for example:

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

If the start and goal have length 3, planning in \(\SE{2}\), and configspace is False, then the start and goal configuration are displayed as a vehicle, using start_vehicle_style and goal_vehicle_style.

If configspace is True then a 3D plot is created and the start and goal are indicated by Matplotlib markers specified by the dicts start_style and goal_style.

Default values are provided for all styles:

  • 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 the layers of path_style while reverse motion segments are drawn using path_reverse_style. These are each one dict, or a sequence of dicts, of Matplotlib plot options which can draw an arbitrary number of lines on top of each other. The default:

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

will draw a black line of width 4 with a dashed yellow line of width 3 plotted on top, giving a line of alternating black and yellow dashes. A style given by the caller replaces these layers.

Deprecated since version 1.5.0: The arguments line, line_r, path_marker, path_marker_reverse, start_marker, goal_marker, start_vehicle and goal_vehicle are deprecated, use path_style, path_reverse_style, start_style, goal_style, start_vehicle_style and goal_vehicle_style. path_marker and path_marker_reverse were accepted but had no effect.

See also

plot_bg() plot_poly

Note

Unknown keyword arguments are ignored, with a DeprecationWarning, as they were silently by RTB 1.4.4 and earlier.

Deprecated since version 1.5.0: labelvalues is deprecated, use labels. It is the name used in the book, where it had no effect.

Changed in version 1.5.0: block=False shows the figure without waiting, as documented. It used to do nothing, the same as None.

plot_bg(distance: NDArray | None = None, cmap: str = 'gray', ax: Axes | None = None, inflated: bool = True, colorbar: bool | dict[str, Any] = True, block: bool | None = None, colorbar_style: dict[str, Any] | None = None, labels: bool = False, label_style: dict[str, Any] | None = None, labelvalues: bool = False, **unused: Any) → None

Plot background (superclass)

Parameters:

  • distance (ndarray | None) – override distance field, defaults to None. The array is not modified.

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

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

  • inflated (bool) – show the inflated obstacles, defaults to True

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

  • 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

  • colorbar_style (dict[str, Any] | None) – options for the colorbar, passed to matplotlib.figure.Figure.colorbar(), defaults to None. The label defaults to “Distance”.

  • labels (bool) – write the value in each cell of the distance field, defaults to False

  • labelvalues (bool) – deprecated, use labels

  • label_style (dict[str, Any] | None) – Matplotlib text options for the values, merged over red, centered fontsize 8 text, as in the book, with a thin white outline (path_effects) so that it is legible on any gray. The axes are set to show every cell whole.

Return type:

None

Deprecated since version 1.5.0: Giving a dict as colorbar is deprecated, use colorbar=True with colorbar_style.

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 distance field will be used as the background and obstacle cells (actual or inflated) will be shown in red. A colorbar is added.

Note

Unknown keyword arguments are ignored, with a DeprecationWarning, as they were silently by RTB 1.4.4 and earlier.

Deprecated since version 1.5.0: labelvalues is deprecated, use labels. It is the name used in the book, where it had no effect.

property start: ndarray[Tuple[Literal[2]], dtype[floating]] | ndarray[Tuple[Literal[3]], dtype[floating]] | None

Set/get start point or configuration (superclass)

Getter:

Return start point or configuration, None if not set

Setter:

Set start point or configuration, None is ignored

Raises:

ValueError – (setter) not of length ndims, or inside an obstacle

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

See also

goal()

Setter accepts:

Union[List[float], Tuple[float, float], ndarray[Tuple[Literal[2]], dtype[floating]], Tuple[float, float, float], ndarray[Tuple[Literal[3]], dtype[floating]], None]

validate_endpoint(p: List[float] | Tuple[float, float] | ndarray[Tuple[Literal[2]], dtype[floating]] | Tuple[float, float, float] | ndarray[Tuple[Literal[3]], dtype[floating]] | None, dtype: Any = None) → ndarray[Tuple[Literal[2]], dtype[floating]] | ndarray[Tuple[Literal[3]], dtype[floating]] | None

Validate start or goal point

Parameters:
Raises:

ValueError – point is of the wrong length, or is inside an obstacle

Return type:

ndarray[Tuple[Literal[2]], dtype[floating]] | ndarray[Tuple[Literal[3]], dtype[floating]] | None

Returns:

point as a NumPy array of specified dtype, None if p is None

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

See also

isoccupied() occgrid()

validate_start_goal(start: List[float] | Tuple[float, float] | ndarray[Tuple[Literal[2]], dtype[floating]] | Tuple[float, float, float] | ndarray[Tuple[Literal[3]], dtype[floating]] | None = None, goal: List[float] | Tuple[float, float] | ndarray[Tuple[Literal[2]], dtype[floating]] | Tuple[float, float, float] | ndarray[Tuple[Literal[3]], dtype[floating]] | None = None, dtype: Any = None) → None

Validate and store the start and goal

Parameters:
Raises:

ValueError – start or goal is of the wrong length or is inside an obstacle

Return type:

None

Each given value is tested against the planner’s dimensionality and the occupancy grid, and then stored as start or goal. A value of None leaves the existing value unchanged. If either is invalid neither is stored.

See also

validate_endpoint() start goal

property verbose: bool

Get verbosity

Returns:

verbosity

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

Lattice planner

class LatticePlanner(costs=None, root=(0, 0, 0), **kwargs)[source]

Bases: PlannerBase

Lattice planner

Parameters:

Feature

Capability

Plan

\(\SE{2}\)

Obstacle avoidance

Yes, occupancy grid

Curvature

Discontinuous

Motion

Forwards only

The lattice planner incrementally builds a graph from the root vertex, at each iteration adding three edges to the graph:

code

direction

‘S’

straight ahead

‘L’

turn left

‘R’

turn right

If the configuration is already in the graph, the edge connects to that existing vertex. The vertex is named after the sequence of moves required to reach it from the root. This means, that any configuration, ie. \((x, y, \theta)\) can be reached by multiple paths and potentially have multiple names. The first name assigned to a vertex is permanent and is not overridden.

If an occupancy grid exists and the configuration is an obstacle, then the vertex is not added.

The path through the lattice is found using A* graph search, and costs changes the weighting for path costs at query time.

Example:

>>> from roboticstoolbox import LatticePlanner
>>> import numpy as np
>>> lattice = LatticePlanner();
>>> lattice.plan(iterations=6)
>>> path, status = lattice.query(start=(0, 0, 0), goal=(1, 2, np.pi/2))
>>> print(path.T)
[[0.     1.     1.    ]
 [0.     1.     2.    ]
 [0.     1.5708 1.5708]]
>>> print(status)
LatticeStatus(cost=2.5707963267948966, segments=['L', 'S'], edges=[LatticeEdge{[0] -> [0L], cost=1.571}, LatticeEdge{[0L] -> [0LS], cost=1}])

See also

plan() query() PlannerBase

plan(iterations: int | None = None, verbose: bool = False, summary: bool = False) → None[source]

Create a lattice plan

Parameters:

  • iterations (int | None) – number of iterations, defaults to None

  • verbose (bool) – show frontier and added vertices/edges at each iteration, defaults to False

Return type:

None

If an occupancy grid exists the if iterations is None the area of the grid will be completely filled.

Note

A spatial index can only be built after the last iteration: adding a vertex invalidates it. Automatic indexing is deferred because pgraph 1.0.0 can reject lattice angles just below zero: wrapping them rounds to the upper boundary of its periodic k-d tree. The unindexed nearest-vertex search remains valid. See examples/benchmark_roadmap.py for a comparison on completed roadmaps, including index-construction failures.

See also

query()

query(start: List[float] | Tuple[float, float, float] | ndarray[Tuple[Literal[3]], dtype[floating]], goal: List[float] | Tuple[float, float, float] | ndarray[Tuple[Literal[3]], dtype[floating]]) → tuple[ndarray, LatticeStatus][source]

Find a path through the lattice

Parameters:
Returns:

path and status

Return type:

tuple[ndarray, LatticeStatus]

The returned status value has elements:

Element

Description

cost

path cost

segments

a list containing the type of each path segment as a single letter code: either “L”, “R” or “S” for left turn, right turn or straight line respectively.

edges

successive edges of the graph LatticeEdge type

See also

plan()

plot(path: ndarray | None = None, **kwargs: Any) → None[source]

Plot vehicle path (superclass)

Parameters:

  • path (ndarray | None) – path, one point per row, defaults to None

  • path_style – style for forward motion, one dict of Matplotlib line options or a sequence of them drawn on top of each other, defaults to PATH_STYLE, a dashed yellow line on black

  • path_reverse_style – style for reverse motion, as path_style, defaults to PATH_REVERSE_STYLE, a dashed red line on black

  • configspace – plot path in configuration space, defaults to False

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

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

  • background – plot occupancy grid if present, default True

  • start_style – Matplotlib options for the start point, merged over START_STYLE

  • goal_style – Matplotlib options for the goal point, merged over GOAL_STYLE

  • start_vehicle_style – options for the vehicle drawn at the start configuration, merged over START_VEHICLE_STYLE

  • goal_vehicle_style – options for the vehicle drawn at the goal configuration, merged over GOAL_VEHICLE_STYLE

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

  • goal – goal position \((x, y)\) or configuration \((x, y, \theta)\), defaults to value previously set. The planner’s own goal is not changed.

  • ax – axes to plot into, defaults to a new axes

  • block – 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

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

  • labels – write the distance value in each cell of the distance field, defaults to False. Useful for small grids, see plot_bg().

  • label_style – Matplotlib text options for labels

  • labelvalues – deprecated, use labels

  • inflated – show the inflated obstacles, passed to plot_bg()

  • colorbar – show a colorbar for the distance field, passed to plot_bg()

  • colorbar_style – options for the colorbar, passed to plot_bg()

  • cmap – colormap for the distance field, passed to plot_bg()

  • path_marker – deprecated, use path_style

  • path_marker_reverse – deprecated, use path_reverse_style

  • line – deprecated, use path_style

  • line_r – deprecated, use path_reverse_style

  • start_marker – deprecated, use start_style

  • goal_marker – deprecated, use goal_style

  • start_vehicle – deprecated, use start_vehicle_style

  • goal_vehicle – deprecated, use goal_vehicle_style

Raises:

ValueError – configspace is True but the path is not in configuration space

Return type:

None

Returns:

the axes plotted into

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_style and goal_style, which are dicts using Matplotlib keywords, for example:

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

If the start and goal have length 3, planning in \(\SE{2}\), and configspace is False, then the start and goal configuration are displayed as a vehicle, using start_vehicle_style and goal_vehicle_style.

If configspace is True then a 3D plot is created and the start and goal are indicated by Matplotlib markers specified by the dicts start_style and goal_style.

Default values are provided for all styles:

  • 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 the layers of path_style while reverse motion segments are drawn using path_reverse_style. These are each one dict, or a sequence of dicts, of Matplotlib plot options which can draw an arbitrary number of lines on top of each other. The default:

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

will draw a black line of width 4 with a dashed yellow line of width 3 plotted on top, giving a line of alternating black and yellow dashes. A style given by the caller replaces these layers.

Deprecated since version 1.5.0: The arguments line, line_r, path_marker, path_marker_reverse, start_marker, goal_marker, start_vehicle and goal_vehicle are deprecated, use path_style, path_reverse_style, start_style, goal_style, start_vehicle_style and goal_vehicle_style. path_marker and path_marker_reverse were accepted but had no effect.

See also

plot_bg() plot_poly

Note

Unknown keyword arguments are ignored, with a DeprecationWarning, as they were silently by RTB 1.4.4 and earlier.

Deprecated since version 1.5.0: labelvalues is deprecated, use labels. It is the name used in the book, where it had no effect.

Changed in version 1.5.0: block=False shows the figure without waiting, as documented. It used to do nothing, the same as None.

__repr__()

Compact representation of the planner, same as __str__()

Return type:

str

property animate: bool

Get or set animation of the planner’s operation

Getter:

True if planners that can animate will do so

Setter:

Set the default for methods that take an animate argument

A movie file given to the constructor also implies animation.

See also

animation_start()

animation_end() → None

End an animation, closing the movie file if there is one

See also

animation_start()

Return type:

None

animation_frame(pause: float = 0.05) → None

Show the current state of the animation

Parameters:

pause (float) – time to pause for the display to update, defaults to 0.05 s

Return type:

None

Updates the display and adds a frame to the movie, if there is one.

See also

animation_start()

animation_start(animate: bool | None = None, movie: str | tuple[str, float] | None = None, draw: Callable[[], Any] | None = None) → bool

Begin an animation

Parameters:

  • animate (bool | None) – animate this call, defaults to the planner’s animate

  • movie (str | tuple[str, float] | None) – movie file for this call, defaults to the planner’s movie

  • draw (Callable[[], Any] | None) – function that draws the static background of the animation

Return type:

bool

Returns:

True if the caller should animate

The planner decides what is drawn and when, this helper owns the pacing and recording. If animating, draw is called, then the movie file if any is opened on the current figure. The caller then calls animation_frame() after updating its artists, and animation_end() when done.

A movie implies animation.

See also

animation_frame() animation_end()

property goal: ndarray[Tuple[Literal[2]], dtype[floating]] | ndarray[Tuple[Literal[3]], dtype[floating]] | None

Set/get goal point or configuration (superclass)

Getter:

Return goal point or configuration, None if not set

Setter:

Set goal point or configuration, None is ignored

Raises:

ValueError – (setter) not of length ndims, or inside an obstacle

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

See also

start()

Setter accepts:

Union[List[float], Tuple[float, float], ndarray[Tuple[Literal[2]], dtype[floating]], Tuple[float, float, float], ndarray[Tuple[Literal[3]], dtype[floating]], None]

property movie: str | tuple[str, float] | None

Get or set the movie file for the animation

Getter:

filename, or tuple (filename, frame interval), None for no movie

Setter:

Set the default movie file, None for no movie

property occgrid: BinaryOccupancyGrid | None

Occupancy grid

Returns:

occupancy grid used for planning, None if there is none

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

See also

validate_endpoint() isoccupied()

property start: ndarray[Tuple[Literal[2]], dtype[floating]] | ndarray[Tuple[Literal[3]], dtype[floating]] | None

Set/get start point or configuration (superclass)

Getter:

Return start point or configuration, None if not set

Setter:

Set start point or configuration, None is ignored

Raises:

ValueError – (setter) not of length ndims, or inside an obstacle

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

See also

goal()

Setter accepts:

Union[List[float], Tuple[float, float], ndarray[Tuple[Literal[2]], dtype[floating]], Tuple[float, float, float], ndarray[Tuple[Literal[3]], dtype[floating]], None]

validate_endpoint(p: List[float] | Tuple[float, float] | ndarray[Tuple[Literal[2]], dtype[floating]] | Tuple[float, float, float] | ndarray[Tuple[Literal[3]], dtype[floating]] | None, dtype: Any = None) → ndarray[Tuple[Literal[2]], dtype[floating]] | ndarray[Tuple[Literal[3]], dtype[floating]] | None

Validate start or goal point

Parameters:
Raises:

ValueError – point is of the wrong length, or is inside an obstacle

Return type:

ndarray[Tuple[Literal[2]], dtype[floating]] | ndarray[Tuple[Literal[3]], dtype[floating]] | None

Returns:

point as a NumPy array of specified dtype, None if p is None

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

See also

isoccupied() occgrid()

validate_start_goal(start: List[float] | Tuple[float, float] | ndarray[Tuple[Literal[2]], dtype[floating]] | Tuple[float, float, float] | ndarray[Tuple[Literal[3]], dtype[floating]] | None = None, goal: List[float] | Tuple[float, float] | ndarray[Tuple[Literal[2]], dtype[floating]] | Tuple[float, float, float] | ndarray[Tuple[Literal[3]], dtype[floating]] | None = None, dtype: Any = None) → None

Validate and store the start and goal

Parameters:
Raises:

ValueError – start or goal is of the wrong length or is inside an obstacle

Return type:

None

Each given value is tested against the planner’s dimensionality and the occupancy grid, and then stored as start or goal. A value of None leaves the existing value unchanged. If either is invalid neither is stored.

See also

validate_endpoint() start goal

property verbose: bool

Get verbosity

Returns:

verbosity

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