# Continuous configuration-space planners

Planner

Plans in

Discrete/Continuous

Obstacle avoidance

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

These planners do not support planning around obstacles, but allow for the start and goal configuration $$(x, y, \theta)$$ to be specified.

## PRM planner

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

Bases: PlannerBase

Distance transform path planner

Parameters:
• occgrid (BinaryOccGrid or ndarray(h,w)) – occupancy grid

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

• dist_thresh (float, optional) – distance threshold, a new point is only added to the roadmap if it is closer than this distance to an existing vertex, defaults to None

• Planner – probabilistic roadmap path planner

• kwargs – common planner options, see PlannerBase

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.

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.9748 4.6733 4.5757 3.1849 2.1734 1.0059 1.    ]
[4.     4.0343 3.0184 2.0005 1.351  1.2122 1.0357 1.    ]]

Author:

Peter Corke

Seealso:

PlannerBase

property npoints

Number of points in the roadmap

Returns:

Number of points

Return type:

int

property dist_thresh

Distance threshold

Returns:

distance threshold

Return type:

float

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

property graph

Returns:

Return type:

pgraph.UGraph instance

plan(npoints=None, dist_thresh=None, animate=None)[source]

Plan PRM path

Parameters:
• npoints (int, optional) – number of random points, defaults to npoints given to constructor

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

• animate (bool, optional) – animate the planning algorithm iterations, defaults to False

Create a probablistic 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 pgraph UGraph UGraph UGraph

Seealso:
query(start, goal, **kwargs)[source]

Find a path from start to goal using planner

Parameters:
• start (array_like(), optional) – start position $$(x, y)$$, defaults to previously set value

• goal (array_like(), optional) – goal position $$(x, y)$$, defaults to previously set value

• kwargs – options passed to PlannerBase.query()

Returns:

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

Return type:

ndarray(N,2)

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

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, vertex={}, edge={}, **kwargs)[source]

Plot PRM path

Parameters:
• vertex (dict, optional) – vertex style, defaults to {}

• edge (dict, optional) – edge style, defaults to {}

Displays:

• the planner background (obstacles)

• the path

Seealso:

UGraph.plot()

property goal

Set/get goal point or configuration (superclass)

Getter:

Return goal pointor configuration

Return type:

ndarray(2) or ndarray(3)

Setter:

Set goal point or configuration

Param:

array_like(2) or array_like(3)

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

Seealso:

goal()

property occgrid

Occupancy grid

Returns:

occupancy grid used for planning

Return type:

OccGrid or subclass or None

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

Seealso:

validate_endpoint() isoccupied()

property start

Set/get start point or configuration (superclass)

Getter:

Return start point or configuration

Return type:

ndarray(2) or ndarray(3)

Setter:

Set start point or configuration

Param:

array_like(2) or array_like(3)

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

Seealso:

goal()

validate_endpoint(p, dtype=None)

Validate start or goal point

Parameters:
• p (array_like(2)) – the point

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

Raises:

ValueError – point is inside obstacle

Returns:

point as a NumPy array of specified dtype

Return type:

ndarray(2)

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

Seealso:

isoccupied() occgrid()

property verbose

Get verbosity

Returns:

verbosity

Return type:

bool

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

## Dubins path planner

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

Bases: PlannerBase

Dubins path planner

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

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

Returns:

Dubins path planner

Return type:

DubinsPlanner instance

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

Seealso:
property curvature
property stepsize
query(start, goal, **kwargs)[source]

Find a path between two configurations

Parameters:
• start (array_like(3), optional) – start configuration $$(x, y, \theta)$$

• goal (array_like(3), optional) – goal configuration $$(x, y, \theta)$$

Returns:

path and status

Return type:

ndarray(N,3), namedtuple

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.
property goal

Set/get goal point or configuration (superclass)

Getter:

Return goal pointor configuration

Return type:

ndarray(2) or ndarray(3)

Setter:

Set goal point or configuration

Param:

array_like(2) or array_like(3)

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

Seealso:

goal()

property occgrid

Occupancy grid

Returns:

occupancy grid used for planning

Return type:

OccGrid or subclass or None

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

Seealso:

validate_endpoint() isoccupied()

plan()

Plan path (superclass)

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

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

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

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

Plot vehicle path (superclass)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

• ax (matplotlib axes) – axes to plot into

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

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

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

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


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

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

Default values are provided for all markers:

• the start point is a circle

• the goal point is a star

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

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

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

• the occupancy grid

• the distance field of the planner

Additional arguments bgargs can be passed through to plot_bg()

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

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

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

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

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

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


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

Seealso:

plot_bg() base.plot_poly()

property start

Set/get start point or configuration (superclass)

Getter:

Return start point or configuration

Return type:

ndarray(2) or ndarray(3)

Setter:

Set start point or configuration

Param:

array_like(2) or array_like(3)

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

Seealso:

goal()

validate_endpoint(p, dtype=None)

Validate start or goal point

Parameters:
• p (array_like(2)) – the point

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

Raises:

ValueError – point is inside obstacle

Returns:

point as a NumPy array of specified dtype

Return type:

ndarray(2)

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

Seealso:

isoccupied() occgrid()

property verbose

Get verbosity

Returns:

verbosity

Return type:

bool

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

## Reeds-Shepp path planner

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

Bases: PlannerBase

Reeds-Shepp path planner

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

• stepsize (float, optional) – 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])

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

Seealso:
query(start, goal, **kwargs)[source]

Find a path between two configurations

Parameters:
• start (array_like(3), optional) – start configuration $$(x, y, \theta)$$

• goal (array_like(3), optional) – goal configuration $$(x, y, \theta)$$

Returns:

path and status

Return type:

ndarray(N,3), namedtuple

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.

property goal

Set/get goal point or configuration (superclass)

Getter:

Return goal pointor configuration

Return type:

ndarray(2) or ndarray(3)

Setter:

Set goal point or configuration

Param:

array_like(2) or array_like(3)

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

Seealso:

goal()

property occgrid

Occupancy grid

Returns:

occupancy grid used for planning

Return type:

OccGrid or subclass or None

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

Seealso:

validate_endpoint() isoccupied()

plan()

Plan path (superclass)

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

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

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

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

Plot vehicle path (superclass)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

• ax (matplotlib axes) – axes to plot into

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

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

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

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


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

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

Default values are provided for all markers:

• the start point is a circle

• the goal point is a star

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

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

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

• the occupancy grid

• the distance field of the planner

Additional arguments bgargs can be passed through to plot_bg()

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

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

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

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

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

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


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

Seealso:

plot_bg() base.plot_poly()

property start

Set/get start point or configuration (superclass)

Getter:

Return start point or configuration

Return type:

ndarray(2) or ndarray(3)

Setter:

Set start point or configuration

Param:

array_like(2) or array_like(3)

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

Seealso:

goal()

validate_endpoint(p, dtype=None)

Validate start or goal point

Parameters:
• p (array_like(2)) – the point

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

Raises:

ValueError – point is inside obstacle

Returns:

point as a NumPy array of specified dtype

Return type:

ndarray(2)

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

Seealso:

isoccupied() occgrid()

property verbose

Get verbosity

Returns:

verbosity

Return type:

bool

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

## Curvature-polynomial planner

class roboticstoolbox.mobile.CurvaturePolyPlanner(curvature=None)[source]

Bases: PlannerBase

query(start, goal)[source]

Find a path betwee two configurations

Parameters:
• start (array_like(3), optional) – start configuration $$(x, y, \theta)$$

• goal (array_like(3), optional) – goal configuration $$(x, y, \theta)$$

Returns:

path and status

Return type:

ndarray(N,3), namedtuple

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

The returned status value has elements:

 Element Description length total path length maxcurvature maximum curvature on path poly curvature polynomial coefficients
__str__()

Compact representation of the planner

Returns:

pretty printed representation

Return type:

str

property goal

Set/get goal point or configuration (superclass)

Getter:

Return goal pointor configuration

Return type:

ndarray(2) or ndarray(3)

Setter:

Set goal point or configuration

Param:

array_like(2) or array_like(3)

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

Seealso:

goal()

property occgrid

Occupancy grid

Returns:

occupancy grid used for planning

Return type:

OccGrid or subclass or None

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

Seealso:

validate_endpoint() isoccupied()

plan()

Plan path (superclass)

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

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

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

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

Plot vehicle path (superclass)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

• ax (matplotlib axes) – axes to plot into

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

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

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

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


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

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

Default values are provided for all markers:

• the start point is a circle

• the goal point is a star

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

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

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

• the occupancy grid

• the distance field of the planner

Additional arguments bgargs can be passed through to plot_bg()

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

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

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

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

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

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


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

Seealso:

plot_bg() base.plot_poly()

property start

Set/get start point or configuration (superclass)

Getter:

Return start point or configuration

Return type:

ndarray(2) or ndarray(3)

Setter:

Set start point or configuration

Param:

array_like(2) or array_like(3)

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

Seealso:

goal()

validate_endpoint(p, dtype=None)

Validate start or goal point

Parameters:
• p (array_like(2)) – the point

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

Raises:

ValueError – point is inside obstacle

Returns:

point as a NumPy array of specified dtype

Return type:

ndarray(2)

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

Seealso:

isoccupied() occgrid()

property verbose

Get verbosity

Returns:

verbosity

Return type:

bool

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

## Quintic-polynomial planner

class roboticstoolbox.mobile.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, optional) – time step, defaults to 0.1

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

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

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

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

• max_acc (int, optional) – maximum acceleration, defaults to 1

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

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

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

Returns:

Quintic polynomial path planner

Return type:

QuinticPolyPlanner instance

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)
find path!!
>>> print(path[:5,:])
[[10.     10.      0.1745]
[10.0985 10.0173  0.1724]
[10.1971 10.0342  0.1662]
[10.2959 10.0503  0.156 ]
[10.3949 10.0652  0.142 ]]

Thanks:

based on quintic polynomial planning from Python Robotics

Seealso:

Planner

query(start, goal)[source]

Find a quintic polynomial path

Parameters:
• start (array_like(3), optional) – start configuration $$(x, y, \theta)$$

• goal (array_like(3), optional) – goal configuration $$(x, y, \theta)$$

Returns:

path and status

Return type:

ndarray(N,3), namedtuple

The returned status value has elements:

Element

Description

t

time to execute the path

vel

velocity profile along the path

accel

acceleration profile along the path

jerk

jerk profile along the path

Seealso:

Planner.query()

__str__()

Compact representation of the planner

Returns:

pretty printed representation

Return type:

str

property goal

Set/get goal point or configuration (superclass)

Getter:

Return goal pointor configuration

Return type:

ndarray(2) or ndarray(3)

Setter:

Set goal point or configuration

Param:

array_like(2) or array_like(3)

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

Seealso:

goal()

property occgrid

Occupancy grid

Returns:

occupancy grid used for planning

Return type:

OccGrid or subclass or None

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

Seealso:

validate_endpoint() isoccupied()

plan()

Plan path (superclass)

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

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

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

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

Plot vehicle path (superclass)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

• ax (matplotlib axes) – axes to plot into

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

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

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

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


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

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

Default values are provided for all markers:

• the start point is a circle

• the goal point is a star

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

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

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

• the occupancy grid

• the distance field of the planner

Additional arguments bgargs can be passed through to plot_bg()

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

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

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

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

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

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


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

Seealso:

plot_bg() base.plot_poly()

property start

Set/get start point or configuration (superclass)

Getter:

Return start point or configuration

Return type:

ndarray(2) or ndarray(3)

Setter:

Set start point or configuration

Param:

array_like(2) or array_like(3)

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

Seealso:

goal()

validate_endpoint(p, dtype=None)

Validate start or goal point

Parameters:
• p (array_like(2)) – the point

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

Raises:

ValueError – point is inside obstacle

Returns:

point as a NumPy array of specified dtype

Return type:

ndarray(2)

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

Seealso:

isoccupied() occgrid()

property verbose

Get verbosity

Returns:

verbosity

Return type:

bool

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

## RRT planner

class roboticstoolbox.mobile.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 subclass) – vehicle kinematic model

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

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

• npoints (int, optional) – 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)

Seealso:
plan(goal, showsamples=True, showvalid=True, animate=False)[source]

Plan paths to goal using RRT

Parameters:
• goal (array_like(3), optional) – goal pose $$(x, y, \theta)$$, defaults to previously set value

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

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

• animate (bool, optional) – update the display as configurations are tested, defaults to False

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.

Seealso:

query()

query(start)[source]

Find a path from start configuration

Parameters:

start (array_like(3), optional) – start configuration $$(x, y, \theta)$$

Returns:

path and status

Return type:

ndarray(N,3), namedtuple

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

Random configuration

Returns:

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

Return type:

ndarray(3)

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

Seealso:

qrandom_free()

qrandom_free()[source]

Random obstacle free configuration

Returns:

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

Return type:

ndarray(3)

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

Seealso:
iscollision(q)[source]

Test if configuration is collision

Parameters:

q (array_like(3)) – vehicle configuration $$(x, y, \theta)$$

Returns:

collision status

Return type:

bool

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

__str__()

Compact representation of the planner

Returns:

pretty printed representation

Return type:

str

property goal

Set/get goal point or configuration (superclass)

Getter:

Return goal pointor configuration

Return type:

ndarray(2) or ndarray(3)

Setter:

Set goal point or configuration

Param:

array_like(2) or array_like(3)

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

Seealso:

goal()

property occgrid

Occupancy grid

Returns:

occupancy grid used for planning

Return type:

OccGrid or subclass or None

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

Seealso:

validate_endpoint() isoccupied()

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

Plot vehicle path (superclass)

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

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

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

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

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

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

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

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

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

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

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

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

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

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

• ax (matplotlib axes) – axes to plot into

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

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

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

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


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

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

Default values are provided for all markers:

• the start point is a circle

• the goal point is a star

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

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

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

• the occupancy grid

• the distance field of the planner

Additional arguments bgargs can be passed through to plot_bg()

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

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

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

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

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

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


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

Seealso:

plot_bg() base.plot_poly()

property start

Set/get start point or configuration (superclass)

Getter:

Return start point or configuration

Return type:

ndarray(2) or ndarray(3)

Setter:

Set start point or configuration

Param:

array_like(2) or array_like(3)

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

Seealso:

goal()

validate_endpoint(p, dtype=None)

Validate start or goal point

Parameters:
• p (array_like(2)) – the point

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

Raises:

ValueError – point is inside obstacle

Returns:

point as a NumPy array of specified dtype

Return type:

ndarray(2)

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

Seealso:

isoccupied() occgrid()

property verbose

Get verbosity

Returns:

verbosity

Return type:

bool

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