Continuous configuration-space planners

Planner

Plans in

Discrete/Continuous

Obstacle avoidance

PRMPlanner

\(\mathbb{R}^2\)

continuous

yes

DubinsPlanner

\(\SE{2}\)

continuous

no

ReedsSheppPlanner

\(\SE{2}\)

continuous

no

CurvaturePolyPlanner

\(\SE{2}\)

continuous

no

QuinticPolyPlanner

\(\SE{2}\)

continuous

no

RRTPlanner

\(\SE{2}\)

continuous

yes

The PRM and RRT planners plan around obstacles. The others do not, but allow for the start and goal configuration \((x, y, \theta)\) to be specified.

PRM planner

class PRMPlanner(occgrid=None, npoints=100, dist_thresh=None, collision_step=None, **kwargs)[source]

Bases: PlannerBase

Probabilistic roadmap path planner

Parameters:

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

  • npoints (int) – number of random points, defaults to 100

  • dist_thresh (float | None) – distance threshold, an edge is only added to the roadmap between two vertices that are closer than this distance, defaults to 0.3 times the largest dimension of the occupancy grid

  • collision_step (float | None) – spacing in cells of the points tested along a candidate edge, defaults to None for the approximate test described below

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

Raises:

ValueError – there is no occupancy grid, or collision_step is not positive

Feature

Capability

Plan

Cartesian space

Obstacle avoidance

Yes, occupancy grid

Curvature

Discontinuous

Motion

Omnidirectional

Creates a planner that finds the path between two points in the plane using omnidirectional motion. The path comprises a set of way points.

A candidate edge is added to the roadmap if it is shorter than dist_thresh and a test finds it free of obstacles. By default the test samples the edge once per world unit, with only the endpoints tested for edges shorter than about 2.5 units, so a wall one cell thick can be stepped over. If collision_step is given the edge is instead sampled at intervals of at most collision_step cells, and both endpoints are always tested. A value of 0.5 or less cannot step over a wall one cell thick, and smaller values give no gain for the increased cost. Larger values are allowed, and are the user’s responsibility.

Example:

>>> from roboticstoolbox import PRMPlanner
>>> import numpy as np
>>> simplegrid = np.zeros((6, 6));
>>> simplegrid[2:5, 3:5] = 1
>>> prm = PRMPlanner(simplegrid);
>>> prm.plan()
>>> path = prm.query(start=(5, 4), goal=(1,1))
>>> print(path.T)
[[5.     4.5252 3.1674 2.1524 2.0585 1.1368 0.9117 1.    ]
 [4.     4.1603 4.9378 3.5443 3.1939 1.9798 1.4833 1.    ]]
Author:

Peter Corke

See also

PlannerBase

__str__()[source]

Compact representation of the planner

Return type:

str

Returns:

the base class description, and the roadmap if there is one

property npoints: int

Number of points in the roadmap

Returns:

Number of points

property collision_step: float | None

Spacing of the points tested along an edge

Returns:

spacing in cells, None for the default approximate test

property dist_thresh: float

Distance threshold

Returns:

distance threshold

Edges are created between points if the distance between them is less than this value.

property graph: UGraph | None

Roadmap graph

Returns:

roadmap as an undirected graph

Return type:

UGraph instance

plan(npoints: int | None = None, dist_thresh: float | None = None, animate: bool | None = None) → None[source]

Plan PRM path

Parameters:

  • npoints (int | None) – number of random points, defaults to npoints given to constructor

  • dist_thresh (float | None) – distance threshold, defaults to dist_thresh given to constructor

  • animate (bool | None) – deprecated, animating the planning is not implemented

Return type:

None

Create a probabilistic roadmap. This is a graph connecting points randomly selected from the free space of the occupancy grid. Edges are created between points if the distance between them is less than dist_thresh.

The roadmap is a UGraph. A k-d tree is built after the roadmap is complete to accelerate the nearest-vertex lookups in query(). The index does not accelerate roadmap construction or A* search.

Note

Adding or removing a vertex, or assigning vertex.coord, invalidates the index. Queries then use a linear scan until planner.graph.build_kdtree() is called again. Coordinate edits in place are not detected: assign the whole coordinate instead.

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

See also

query() graph()

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) → ndarray[source]

Find a path from start to goal using planner

Parameters:
Returns:

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

Return type:

ndarray

Raises:

The path is a sparse sequence of waypoints, with variable distance between them.

If several roadmap vertices are exactly equidistant from an endpoint, the spatial index may select a different vertex than a linear scan, and hence a different path.

Warning

Waypoints 1 to N-2 are part of the roadmap, while waypoints 0 and N-1 are the start and goal respectively. The first and last motion segment is not guaranteed to be obstacle free.

plot(*args: Any, vertex_style: dict[str, Any] | None = None, edge_style: dict[str, Any] | None = None, vertex: dict[str, Any] | None = None, edge: dict[str, Any] | None = None, **kwargs: Any) → Axes[source]

Plot PRM path

Parameters:
Raises:

RuntimeError – plan() has not been called

Return type:

Axes

Returns:

the axes plotted into

Displays:

  • the planner background (obstacles)

  • the roadmap graph

  • the path

Deprecated since version 1.5.0: vertex and edge are deprecated, use vertex_style and edge_style.

See also

PlannerBase.plot() plot

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

Dubins path planner

class DubinsPlanner(curvature=1, stepsize=0.1, **kwargs)[source]

Bases: PlannerBase

Dubins path planner

Parameters:

  • curvature (float) – maximum path curvature, defaults to 1.0

  • stepsize (float) – spacing between points on the path, defaults to 0.1

Returns:

Dubins path planner

Feature

Capability

Plan

\(\SE{2}\)

Obstacle avoidance

No

Curvature

Discontinuous

Motion

Forwards only

Creates a planner that finds the path between two configurations in the plane using forward motion only. The path comprises upto 3 segments that are straight lines, or arcs with curvature of \(\pm\) curvature.

Example:

>>> from roboticstoolbox import DubinsPlanner
>>> from math import pi
>>> dubins = DubinsPlanner(curvature=1.0)
>>> path, status = dubins.query(start=(0, 0, pi/2), goal=(1, 0, pi/2))
>>> print(path[:5,:])
[[ 0.      0.      1.5708]
 [-0.005   0.0998  1.6708]
 [-0.0199  0.1987  1.7708]
 [-0.0447  0.2955  1.8708]
 [-0.0789  0.3894  1.9708]]
>>> print(status)
DubinsStatus(segments=['L', 'S', 'L'], length=7.283185307179586, seglengths=[4.71238898038469, 1.0, 1.5707963267948966])
Reference:

On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents, Dubins, L.E. (July 1957), American Journal of Mathematics. 79(3): 497–516.

Thanks:

based on Dubins path planning from Python Robotics

See also

ReedsSheppPlanner PlannerBase

property curvature: float
property stepsize: float
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, DubinsStatus][source]

Find a path between two configurations

Parameters:
Returns:

path and status

Return type:

tuple[ndarray, DubinsStatus]

The path comprises points equally spaced at a distance of stepsize.

The returned status value has elements:

Element

Description

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.

length

total path length

lengths

the length of each path segment. The sign of the length indicates the direction of travel.
__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()

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.

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.

Reeds-Shepp path planner

class ReedsSheppPlanner(curvature=1, stepsize=0.1, **kwargs)[source]

Bases: PlannerBase

Reeds-Shepp path planner

Parameters:

  • curvature (float) – maximum path curvature, defaults to 1.0

  • stepsize (float) – spacing between points on the path, defaults to 0.1

  • Planner (ReedsSheppPlanner instance) – Reeds-Shepp path planner

Feature

Capability

Plan

\(\SE{2}\)

Obstacle avoidance

No

Curvature

Discontinuous

Motion

Bidirectional

Creates a planner that finds the path between two configurations in the plane using forward and backward motion. The path comprises upto 3 segments that are straight lines, or arcs with curvature of \(\pm\) curvature.

Example:

>>> from roboticstoolbox import ReedsSheppPlanner
>>> from math import pi
>>> reedshepp = ReedsSheppPlanner(curvature=1.0)
>>> path, status = reedshepp.query(start=(0, 0, pi/2), goal=(1, 0, pi/2))
>>> print(path[:5,:])
[[0.     0.     1.5708]
 [0.005  0.0998 1.4708]
 [0.0199 0.1987 1.3708]
 [0.0447 0.2955 1.2708]
 [0.0789 0.3894 1.1708]]
>>> print(status)
ReedsSheppStatus(segments=['R', 'L', 'R'], length=6.283185307179586, seglengths=[4.459708725242611, -0.5053605102841573, 1.3181160716528177], direction=[1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, -1, -1, -1, -1, -1, -1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1, 1])
Reference:

Optimal paths for a car that goes both forwards and backwards, Reeds, J.A. and L.A. Shepp, Pacific J. Math., 145 (1990), pp. 367–393.

Thanks:

based on Reeds-Shepp path planning from Python Robotics

See also

DubinsPlanner PlannerBase

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, ReedsSheppStatus][source]

Find a path between two configurations

Parameters:
Returns:

path and status

Return type:

tuple[ndarray, ReedsSheppStatus]

The path comprises points equally spaced at a distance of stepsize.

The returned status value has elements:

Element

Description

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.

length

total path length

lengths

the length of each path segment. The sign of the length indicates the direction of travel.

direction

the direction of motion at each point on the path

Note

The direction of turning is reversed when travelling backwards.

__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()

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.

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.

Curvature-polynomial planner

class CurvaturePolyPlanner(curvature=None)[source]

Bases: PlannerBase

Continuous curvature path planner

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, CurvaturePolyStatus][source]

Find a path between two configurations

Parameters:
Returns:

path and status

Return type:

tuple[ndarray, CurvaturePolyStatus]

The curvature of the path is a cubic polynomial in the distance along the path, whose coefficients and the path length are found by numerical optimization. The path is integrated in steps of at most 0.01.

The returned status value has elements:

Element

Description

length

total path length

maxcurvature

maximum curvature on path

poly

curvature polynomial coefficients
__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.

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.

Quintic-polynomial planner

class QuinticPolyPlanner(dt=0.1, start_vel=0, start_acc=0, goal_vel=0, goal_acc=0, max_acc=1, max_jerk=0.5, min_t=5, max_t=100)[source]

Bases: PlannerBase

Quintic polynomial path planner

Parameters:

  • dt (float) – time step, defaults to 0.1

  • start_vel (float) – initial velocity, defaults to 0

  • start_acc (float) – initial acceleration, defaults to 0

  • goal_vel (float) – goal velocity, defaults to 0

  • goal_acc (float) – goal acceleration, defaults to 0

  • max_acc (float) – maximum acceleration, defaults to 1

  • max_jerk (float) – maximum jerk, defaults to 0.5

  • min_t (float) – minimum path time, defaults to 5

  • max_t (float) – maximum path time, defaults to 100

Feature

Capability

Plan

\(\SE{2}\)

Obstacle avoidance

No

Curvature

Continuous

Motion

Forwards only

Creates a planner that finds the path between two configurations in the plane using forward motion only. The path is a continuous quintic polynomial for x and y

\[\begin{split}x(t) &= a_0 + a_1 t + a_2 t^2 + a_3 t^3 + a_4 t^4 + a_5 t^5 \\ y(t) &= b_0 + b_1 t + b_2 t^2 + b_3 t^3 + b_4 t^4 + b_5 t^5\end{split}\]

that meets the given constraints. Trajectory time is given as a range.

Reference:

“Local Path Planning And Motion Control For AGV In Positioning”, Takahashi, T. Hongo, Y. Ninomiya and G. Sugimoto; Proceedings. IEEE/RSJ International Workshop on Intelligent Robots and Systems (IROS ‘89) doi: 10.1109/IROS.1989.637936

Note

The path time is searched in the interval [min_t, max_t] in steps of min_t.

Example:

>>> from roboticstoolbox import QuinticPolyPlanner
>>> import numpy as np
>>> start = (10, 10, np.deg2rad(10.0))
>>> goal = (30, -10, np.deg2rad(20.0))
>>> quintic = QuinticPolyPlanner(start_vel=1)
>>> path, status = quintic.query(start, goal)
>>> print(path[:5,:])
[[10.     10.      0.1745]
 [10.0985 10.0173  0.1725]
 [10.1972 10.0342  0.1665]
 [10.2963 10.0504  0.1568]
 [10.3959 10.0655  0.1436]]
Thanks:

based on quintic polynomial planning from Python Robotics

See also

PlannerBase CurvaturePolyPlanner

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, QuinticPolyStatus][source]

Find a quintic polynomial path

Parameters:
Raises:

NoPathError – no path time in the range min_t to max_t meets the acceleration and jerk limits

Returns:

path and status

Return type:

tuple[ndarray, QuinticPolyStatus]

The returned status value has elements:

Element

Description

t

time to execute the path

vel

velocity profile along the path

acc

acceleration profile along the path

jerk

jerk profile along the path
__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.

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.

RRT planner

class RRTPlanner(map, vehicle, curvature=1.0, stepsize=0.2, npoints=50, **kwargs)[source]

Bases: PlannerBase

Rapidly exploring tree planner

Parameters:

  • map (PolygonMap) – occupancy grid

  • vehicle (VehicleBase | None) – vehicle kinematic model

  • curvature (float | None) – maximum path curvature, defaults to 1.0

  • stepsize (float) – spacing between points on the path, defaults to 0.2

  • npoints (int) – number of vertices in random tree, defaults to 50

Feature

Capability

Plan

\(\SE{2}\)

Obstacle avoidance

Yes, polygons

Curvature

Discontinuous

Motion

Bidirectional

Creates a planner that finds the obstacle-free path between two configurations in the plane using forward and backward motion. The path comprises multiple Dubins curves comprising straight lines, or arcs with curvature of \(\pm\) curvature. Motion along the segments may be in the forward or backward direction.

Polygons are used for obstacle avoidance:

  • the environment is defined by a set of polygons represented by a PolygonMap

  • the vehicle is defined by a single polygon specified by the polygon argument to its constructor

Example:

from roboticstoolbox import RRTPlanner
from spatialmath import Polygon2
from math import pi

# create polygonal obstacles
map = PolygonMap(workspace=[0, 10])
map.add([(5, 50), (5, 6), (6, 6), (6, 50)])
map.add([(5, 4), (5, -50), (6, -50), (6, 4)])

# create outline polygon for vehicle
l, w = 3, 1.5
vpolygon = Polygon2([(-l/2, w/2), (-l/2, -w/2), (l/2, -w/2), (l/2, w/2)])

# create vehicle model
vehicle = Bicycle(steer_max=1, L=2, polygon=vpolygon)

# create planner
rrt = RRTPlanner(map=map, vehicle=vehicle, npoints=50, seed=0)
# start and goal configuration
qs = (2, 8, -pi/2)
qg = (8, 2, -pi/2)

# plan path
rrt.plan(goal=qg)
path, status = rrt.query(start=qs)
print(path[:5,:])
print(status)

See also

DubinsPlanner Vehicle PlannerBase

plan(goal: List[float] | Tuple[float, float, float] | ndarray[Tuple[Literal[3]], dtype[floating]], showsamples: bool = True, showvalid: bool = True, animate: bool | None = None, block: bool | None = None, movie: str | tuple[str, float] | None = None) → None[source]

Plan paths to goal using RRT

Parameters:

  • goal (Union[List[float], Tuple[float, float, float], ndarray[Tuple[Literal[3]], dtype[floating]]]) – goal pose \((x, y, \theta)\), defaults to previously set value

  • showsamples (bool) – display position part of configurations overlaid on the map, defaults to True

  • showvalid (bool) – display valid configurations as vehicle polygons overlaid on the map, defaults to False

  • animate (bool | None) – update the display as configurations are tested, defaults to the planner’s animate

  • 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 (only when not animating); ignored by notebook backends, defaults to None

  • movie (str | tuple[str, float] | None) – save the animation as a movie file, filename or tuple (filename, frame interval), defaults to the planner’s movie. Implies animate.

Return type:

None

Compute a rapidly exploring random tree with its root at the goal. The tree will have npoints vertices spread uniformly randomly over the workspace which is an attribute of the map.

For every new point added, a Dubins path is computed to the nearest vertex already in the graph. Each configuration on that path, with spacing of stepsize, is tested for obstacle intersection.

The configurations tested are displayed (translation only) if showsamples is True. The valid configurations are displayed as vehicle polygones if showvalid is True. If animate is True these points are displayed during the search process, otherwise a single figure is displayed at the end.

See also

query()

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

Find a path from start configuration

Parameters:

start (Union[List[float], Tuple[float, float, float], ndarray[Tuple[Literal[3]], dtype[floating]]]) – start configuration \((x, y, \theta)\)

Returns:

path and status

Return type:

tuple[ndarray, RRTStatus]

The path comprises points equally spaced at a distance of stepsize.

The returned status value has elements:

Element

Description

length | total path length

initial_d | distance from start to first vertex in graph

vertices

sequence of vertices in the graph
qrandom() → ndarray[source]

Random configuration

Returns:

random configuration \((x, y, \theta)\)

Return type:

ndarray

Returns a random configuration where position \((x, y)\) lies within the bounds of the map associated with this planner.

See also

qrandom_free()

qrandom_free() → ndarray[source]

Random obstacle free configuration

Returns:

random configuration \((x, y, \theta)\)

Return type:

ndarray

Returns a random obstacle free configuration where position \((x, y)\) lies within the bounds of the map associated with this planner. Iterates on qrandom()

See also

qrandom() iscollision()

iscollision(q: List[float] | Tuple[float, float, float] | ndarray[Tuple[Literal[3]], dtype[floating]]) → bool[source]

Test if configuration is collision

Parameters:

q (Union[List[float], Tuple[float, float, float], ndarray[Tuple[Literal[3]], dtype[floating]]]) – vehicle configuration \((x, y, \theta)\)

Return type:

bool

Returns:

collision status

Transforms the vehicle polygon and tests for intersection against the polygonal obstacle map.

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

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.