Mobile robot path planning

A set of path planners that all inherit from Planner.

Some planners are based on code from the PathPlanning category of PythonRobotics by Atsushi Sakai.

Planner

Plans in

Obstacle avoidance

Bug2Planner

\(\mathbb{R}^2\)

yes

DistanceTransformPlanner

\(\mathbb{R}^2\)

yes

DstarPlanner

\(\mathbb{R}^2\)

yes

PRMPlanner

\(\mathbb{R}^2\)

yes

LatticePlanner

\(\mathbb{R}^2\)

yes

DubinsPlanner

\(\SE{2}\)

no

ReedsSheppPlanner

\(\SE{2}\)

no

CurvaturePolyPlanner

\(\SE{2}\)

no

QuinticPolyPlanner

\(\SE{2}\)

no

Grid-based planners

Distance transform planner

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

Bases: roboticstoolbox.mobile.PlannerBase.PlannerBase

Distance transform path planner

Parameters
  • occgrid – occupancy grid

  • metric (str optional) – distane metric, one of: “euclidean” [default], “manhattan”

  • Planner (DistanceTransformPlanner instance) – distance transform path planner

Feature

Capability

Plan

Cartesian space

Obstacle avoidance

Yes

Curvature

Discontinuous

Motion

Forwards only

Also known as wavefront, grassfire or brushfire planning algorithm.

Creates a planner that finds the path between two points in the plane using forward motion. The path comprises a set of points in adjacent cells.

Author

Peter Corke_

Seealso

Planner

plan(goal=None, animate=False, verbose=False)[source]

Plan path (abstract superclass)

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

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

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

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

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

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

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

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

Returns

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

Return type

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

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

The method performs the following steps: - Initialize navigation, invoke method N.navigate_init() - Visualize the environment, invoke method N.plot() - Iterate on the next() method of the subclass until the goal is

achieved.

plot(path=None, style='striped', stripe=(('black', 4), ('yellow', 3)), stripe_r=(('black', 4), ('red', 3)), 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=False, **kwargs)

Plot vehicle path

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

  • style (str) – line style: ‘striped’ [default] or ‘line’

  • stripe (tuple of (color, linewidth)) – striped line style for forward motion

  • stripe_r – striped line style for reverse motion

  • line (dict of arguments for plot) – plain line style for forward motion

  • line_r (dict of arguments for plot) – plain line style for forward motion

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

  • 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 used for plan/query

  • goal (array_like(2) or array_like(3), optional) – goal position \((x, y)\) or configuration \((x, y, \theta)\), defaults to value used for plan/query

  • ax (matplotlib axes) – axes to plot into

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

The path has one column per point and either 2 or 3 rows:

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

  • 3 rows describes motion in the \(x-y-\theta\) configuration space. By default only the \(x-y\) plane is plotted unless configspace is True in which case motion in \(x-y-\theta\) configuration space is shown.

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

The default line style is a 'striped' line which comprises a wide first line with a slightly narrow dashed line plotted on top. For example:

(('black', 4), ('yellow', 3))

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

The parameters stripe and stripe_r specify the colors and widths for forward and reverse motion respectively.

The 'line' style is a regular Matplotlib and the parameters line and line_r specify the line and marker styles for forward and reverse motion respectively.

For 2D plots with an \(x-y\) path or 3D plots, the start and goal markers are specified by the dicts start_marker and goal_marker respectively.

For 2D plots with an \(x-y-\theta\) path the vehicle pose is indicated by a vehicle animation object passed to the constructor.

Markers are specified as dicts using Matplotlib keywords, for example:

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

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 configspace is True 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.

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 are passed through to plot_bg()

Seealso

plot_bg() base.plot_poly()

D* planner

class roboticstoolbox.mobile.DstarPlanner(occ_grid=None, reset=False, **kwargs)[source]

Bases: roboticstoolbox.mobile.PlannerBase.PlannerBase

D* path planner

Parameters
  • occgrid – occupancy grid

  • Planner (DstarPlanner instance) – D* path planner

Feature

Capability

Plan

Cartesian space

Obstacle avoidance

Yes

Curvature

Discontinuous

Motion

Forwards only

Also known as wavefront, grassfire or brushfire planning algorithm.

Creates a planner that finds the path between two points in the plane using forward motion. The path comprises a set of points in adjacent cells.

Author

Peter Corke_

Seealso

Planner

plan(goal, animate=False, progress=True)[source]

Plan D* path

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

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

  • progress (bool, optional) – show progress bar, defaults to True

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

query(start, sensor=None, animate=False, verbose=False)[source]

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

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

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

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

Returns

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

Return type

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

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

The method performs the following steps: - Initialize navigation, invoke method N.navigate_init() - Visualize the environment, invoke method N.plot() - Iterate on the next() method of the subclass until the goal is

achieved.

plot(path=None, style='striped', stripe=(('black', 4), ('yellow', 3)), stripe_r=(('black', 4), ('red', 3)), 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=False, **kwargs)

Plot vehicle path

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

  • style (str) – line style: ‘striped’ [default] or ‘line’

  • stripe (tuple of (color, linewidth)) – striped line style for forward motion

  • stripe_r – striped line style for reverse motion

  • line (dict of arguments for plot) – plain line style for forward motion

  • line_r (dict of arguments for plot) – plain line style for forward motion

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

  • 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 used for plan/query

  • goal (array_like(2) or array_like(3), optional) – goal position \((x, y)\) or configuration \((x, y, \theta)\), defaults to value used for plan/query

  • ax (matplotlib axes) – axes to plot into

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

The path has one column per point and either 2 or 3 rows:

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

  • 3 rows describes motion in the \(x-y-\theta\) configuration space. By default only the \(x-y\) plane is plotted unless configspace is True in which case motion in \(x-y-\theta\) configuration space is shown.

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

The default line style is a 'striped' line which comprises a wide first line with a slightly narrow dashed line plotted on top. For example:

(('black', 4), ('yellow', 3))

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

The parameters stripe and stripe_r specify the colors and widths for forward and reverse motion respectively.

The 'line' style is a regular Matplotlib and the parameters line and line_r specify the line and marker styles for forward and reverse motion respectively.

For 2D plots with an \(x-y\) path or 3D plots, the start and goal markers are specified by the dicts start_marker and goal_marker respectively.

For 2D plots with an \(x-y-\theta\) path the vehicle pose is indicated by a vehicle animation object passed to the constructor.

Markers are specified as dicts using Matplotlib keywords, for example:

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

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 configspace is True 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.

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 are passed through to plot_bg()

Seealso

plot_bg() base.plot_poly()

PRM planner

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

Bases: roboticstoolbox.mobile.PlannerBase.PlannerBase

Distance transform path planner

Parameters
  • occgrid – 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 (PRMPlanner instance) – probabilistic roadmap path planner

Feature

Capability

Plan

Cartesian space

Obstacle avoidance

Yes

Curvature

Discontinuous

Motion

Forwards only

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

Author

Peter Corke_

Seealso

Planner

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

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

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

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

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

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

Returns

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

Return type

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

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

The method performs the following steps: - Initialize navigation, invoke method N.navigate_init() - Visualize the environment, invoke method N.plot() - Iterate on the next() method of the subclass until the goal is

achieved.

plot(*args, vertex=None, edge=None, **kwargs)[source]

Plot vehicle path

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

  • style (str) – line style: ‘striped’ [default] or ‘line’

  • stripe (tuple of (color, linewidth)) – striped line style for forward motion

  • stripe_r – striped line style for reverse motion

  • line (dict of arguments for plot) – plain line style for forward motion

  • line_r (dict of arguments for plot) – plain line style for forward motion

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

  • 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 used for plan/query

  • goal (array_like(2) or array_like(3), optional) – goal position \((x, y)\) or configuration \((x, y, \theta)\), defaults to value used for plan/query

  • ax (matplotlib axes) – axes to plot into

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

The path has one column per point and either 2 or 3 rows:

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

  • 3 rows describes motion in the \(x-y-\theta\) configuration space. By default only the \(x-y\) plane is plotted unless configspace is True in which case motion in \(x-y-\theta\) configuration space is shown.

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

The default line style is a 'striped' line which comprises a wide first line with a slightly narrow dashed line plotted on top. For example:

(('black', 4), ('yellow', 3))

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

The parameters stripe and stripe_r specify the colors and widths for forward and reverse motion respectively.

The 'line' style is a regular Matplotlib and the parameters line and line_r specify the line and marker styles for forward and reverse motion respectively.

For 2D plots with an \(x-y\) path or 3D plots, the start and goal markers are specified by the dicts start_marker and goal_marker respectively.

For 2D plots with an \(x-y-\theta\) path the vehicle pose is indicated by a vehicle animation object passed to the constructor.

Markers are specified as dicts using Matplotlib keywords, for example:

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

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 configspace is True 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.

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 are passed through to plot_bg()

Seealso

plot_bg() base.plot_poly()

Lattice planner

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

Bases: roboticstoolbox.mobile.PlannerBase.PlannerBase

Lattice planner

Parameters
  • costs (array_like(3), optional) – cost for straight, left-turn, right-turn, defaults to \((1, \pi/2, \pi/2)\)

  • root (array_like(3), optional) – configuration of root node, defaults to (0,0,0)

  • kwargs – arguments passed to Planner constructor

Feature

Capability

Plan

Configuration space

Obstacle avoidance

Yes

Curvature

Discontinuous

Motion

Forwards only

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

  • straight ahead ‘S’

  • turn left ‘L’

  • turn right ‘R’

If the configuration is already in the graph, the edge connects to that existing vertex.

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

costs changes the weighting for path costs at query time.

icoord(xyt)[source]
query(qs, qg)[source]

Find a path through the lattice

Parameters
  • qs (array_like(3)) – initial configuration

  • qg (array_like(3)) – goal configuration

Returns

path and status

Return type

ndarray(N,3), namedtuple

The returned status value has elements:

Element

Description

cost

path cost

segments

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

``edges ``

successive edges of the graph LatticeEdge type

Seealso

Planner.query()

plot(path=None, **kwargs)[source]

Plot vehicle path

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

  • style (str) – line style: ‘striped’ [default] or ‘line’

  • stripe (tuple of (color, linewidth)) – striped line style for forward motion

  • stripe_r – striped line style for reverse motion

  • line (dict of arguments for plot) – plain line style for forward motion

  • line_r (dict of arguments for plot) – plain line style for forward motion

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

  • 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 used for plan/query

  • goal (array_like(2) or array_like(3), optional) – goal position \((x, y)\) or configuration \((x, y, \theta)\), defaults to value used for plan/query

  • ax (matplotlib axes) – axes to plot into

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

The path has one column per point and either 2 or 3 rows:

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

  • 3 rows describes motion in the \(x-y-\theta\) configuration space. By default only the \(x-y\) plane is plotted unless configspace is True in which case motion in \(x-y-\theta\) configuration space is shown.

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

The default line style is a 'striped' line which comprises a wide first line with a slightly narrow dashed line plotted on top. For example:

(('black', 4), ('yellow', 3))

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

The parameters stripe and stripe_r specify the colors and widths for forward and reverse motion respectively.

The 'line' style is a regular Matplotlib and the parameters line and line_r specify the line and marker styles for forward and reverse motion respectively.

For 2D plots with an \(x-y\) path or 3D plots, the start and goal markers are specified by the dicts start_marker and goal_marker respectively.

For 2D plots with an \(x-y-\theta\) path the vehicle pose is indicated by a vehicle animation object passed to the constructor.

Markers are specified as dicts using Matplotlib keywords, for example:

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

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 configspace is True 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.

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 are passed through to plot_bg()

Seealso

plot_bg() base.plot_poly()

property goal

Goal point or configuration used for planning

Returns

goal point \((x, y)\) or configuration \((x, y, \theta)\)

Return type

ndarray(2) or ndarray(3)

progress_end()
progress_next()
progress_start(n)
randinit()
property start

Start point or configuration used for planning

Returns

start point \((x, y)\) or configuration \((x, y, \theta)\)

Return type

ndarray(2) or ndarray(3)

validate_endpoint(p, dtype=None)
property verbose

Get verbosity

Returns

verbosity

Return type

bool

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

Configuration-space planners

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

Dubins path planner

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

Bases: roboticstoolbox.mobile.PlannerBase.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

Configuration space

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 \(\pm\) curvature.

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.

Author

Atsushi Sakai PythonRobotics

Seealso

Planner

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

Find a Dubins 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

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.

lengths

the length of each path segment. The sign of the

length indicates the direction of travel.

Seealso

Planner.query()

property goal

Goal point or configuration used for planning

Returns

goal point \((x, y)\) or configuration \((x, y, \theta)\)

Return type

ndarray(2) or ndarray(3)

plot(path=None, style='striped', stripe=(('black', 4), ('yellow', 3)), stripe_r=(('black', 4), ('red', 3)), 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=False, **kwargs)

Plot vehicle path

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

  • style (str) – line style: ‘striped’ [default] or ‘line’

  • stripe (tuple of (color, linewidth)) – striped line style for forward motion

  • stripe_r – striped line style for reverse motion

  • line (dict of arguments for plot) – plain line style for forward motion

  • line_r (dict of arguments for plot) – plain line style for forward motion

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

  • 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 used for plan/query

  • goal (array_like(2) or array_like(3), optional) – goal position \((x, y)\) or configuration \((x, y, \theta)\), defaults to value used for plan/query

  • ax (matplotlib axes) – axes to plot into

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

The path has one column per point and either 2 or 3 rows:

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

  • 3 rows describes motion in the \(x-y-\theta\) configuration space. By default only the \(x-y\) plane is plotted unless configspace is True in which case motion in \(x-y-\theta\) configuration space is shown.

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

The default line style is a 'striped' line which comprises a wide first line with a slightly narrow dashed line plotted on top. For example:

(('black', 4), ('yellow', 3))

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

The parameters stripe and stripe_r specify the colors and widths for forward and reverse motion respectively.

The 'line' style is a regular Matplotlib and the parameters line and line_r specify the line and marker styles for forward and reverse motion respectively.

For 2D plots with an \(x-y\) path or 3D plots, the start and goal markers are specified by the dicts start_marker and goal_marker respectively.

For 2D plots with an \(x-y-\theta\) path the vehicle pose is indicated by a vehicle animation object passed to the constructor.

Markers are specified as dicts using Matplotlib keywords, for example:

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

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 configspace is True 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.

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 are passed through to plot_bg()

Seealso

plot_bg() base.plot_poly()

progress_end()
progress_next()
progress_start(n)
randinit()
property start

Start point or configuration used for planning

Returns

start point \((x, y)\) or configuration \((x, y, \theta)\)

Return type

ndarray(2) or ndarray(3)

validate_endpoint(p, dtype=None)
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: roboticstoolbox.mobile.PlannerBase.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

Configuration space

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 \(\pm\) curvature.

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.

Author

Atsushi Sakai PythonRobotics

Seealso

Planner

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

Find Reeds-Shepp 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

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.

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.

Seealso

Planner.query()

property goal

Goal point or configuration used for planning

Returns

goal point \((x, y)\) or configuration \((x, y, \theta)\)

Return type

ndarray(2) or ndarray(3)

plot(path=None, style='striped', stripe=(('black', 4), ('yellow', 3)), stripe_r=(('black', 4), ('red', 3)), 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=False, **kwargs)

Plot vehicle path

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

  • style (str) – line style: ‘striped’ [default] or ‘line’

  • stripe (tuple of (color, linewidth)) – striped line style for forward motion

  • stripe_r – striped line style for reverse motion

  • line (dict of arguments for plot) – plain line style for forward motion

  • line_r (dict of arguments for plot) – plain line style for forward motion

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

  • 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 used for plan/query

  • goal (array_like(2) or array_like(3), optional) – goal position \((x, y)\) or configuration \((x, y, \theta)\), defaults to value used for plan/query

  • ax (matplotlib axes) – axes to plot into

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

The path has one column per point and either 2 or 3 rows:

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

  • 3 rows describes motion in the \(x-y-\theta\) configuration space. By default only the \(x-y\) plane is plotted unless configspace is True in which case motion in \(x-y-\theta\) configuration space is shown.

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

The default line style is a 'striped' line which comprises a wide first line with a slightly narrow dashed line plotted on top. For example:

(('black', 4), ('yellow', 3))

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

The parameters stripe and stripe_r specify the colors and widths for forward and reverse motion respectively.

The 'line' style is a regular Matplotlib and the parameters line and line_r specify the line and marker styles for forward and reverse motion respectively.

For 2D plots with an \(x-y\) path or 3D plots, the start and goal markers are specified by the dicts start_marker and goal_marker respectively.

For 2D plots with an \(x-y-\theta\) path the vehicle pose is indicated by a vehicle animation object passed to the constructor.

Markers are specified as dicts using Matplotlib keywords, for example:

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

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 configspace is True 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.

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 are passed through to plot_bg()

Seealso

plot_bg() base.plot_poly()

progress_end()
progress_next()
progress_start(n)
randinit()
property start

Start point or configuration used for planning

Returns

start point \((x, y)\) or configuration \((x, y, \theta)\)

Return type

ndarray(2) or ndarray(3)

validate_endpoint(p, dtype=None)
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: roboticstoolbox.mobile.PlannerBase.PlannerBase

query(start, goal)[source]

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

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

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

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

Returns

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

Return type

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

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

The method performs the following steps: - Initialize navigation, invoke method N.navigate_init() - Visualize the environment, invoke method N.plot() - Iterate on the next() method of the subclass until the goal is

achieved.

property goal

Goal point or configuration used for planning

Returns

goal point \((x, y)\) or configuration \((x, y, \theta)\)

Return type

ndarray(2) or ndarray(3)

plot(path=None, style='striped', stripe=(('black', 4), ('yellow', 3)), stripe_r=(('black', 4), ('red', 3)), 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=False, **kwargs)

Plot vehicle path

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

  • style (str) – line style: ‘striped’ [default] or ‘line’

  • stripe (tuple of (color, linewidth)) – striped line style for forward motion

  • stripe_r – striped line style for reverse motion

  • line (dict of arguments for plot) – plain line style for forward motion

  • line_r (dict of arguments for plot) – plain line style for forward motion

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

  • 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 used for plan/query

  • goal (array_like(2) or array_like(3), optional) – goal position \((x, y)\) or configuration \((x, y, \theta)\), defaults to value used for plan/query

  • ax (matplotlib axes) – axes to plot into

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

The path has one column per point and either 2 or 3 rows:

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

  • 3 rows describes motion in the \(x-y-\theta\) configuration space. By default only the \(x-y\) plane is plotted unless configspace is True in which case motion in \(x-y-\theta\) configuration space is shown.

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

The default line style is a 'striped' line which comprises a wide first line with a slightly narrow dashed line plotted on top. For example:

(('black', 4), ('yellow', 3))

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

The parameters stripe and stripe_r specify the colors and widths for forward and reverse motion respectively.

The 'line' style is a regular Matplotlib and the parameters line and line_r specify the line and marker styles for forward and reverse motion respectively.

For 2D plots with an \(x-y\) path or 3D plots, the start and goal markers are specified by the dicts start_marker and goal_marker respectively.

For 2D plots with an \(x-y-\theta\) path the vehicle pose is indicated by a vehicle animation object passed to the constructor.

Markers are specified as dicts using Matplotlib keywords, for example:

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

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 configspace is True 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.

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 are passed through to plot_bg()

Seealso

plot_bg() base.plot_poly()

progress_end()
progress_next()
progress_start(n)
randinit()
property start

Start point or configuration used for planning

Returns

start point \((x, y)\) or configuration \((x, y, \theta)\)

Return type

ndarray(2) or ndarray(3)

validate_endpoint(p, dtype=None)
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: roboticstoolbox.mobile.PlannerBase.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) – [description], 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

Configuration space

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}\]
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.

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

property goal

Goal point or configuration used for planning

Returns

goal point \((x, y)\) or configuration \((x, y, \theta)\)

Return type

ndarray(2) or ndarray(3)

plot(path=None, style='striped', stripe=(('black', 4), ('yellow', 3)), stripe_r=(('black', 4), ('red', 3)), 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=False, **kwargs)

Plot vehicle path

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

  • style (str) – line style: ‘striped’ [default] or ‘line’

  • stripe (tuple of (color, linewidth)) – striped line style for forward motion

  • stripe_r – striped line style for reverse motion

  • line (dict of arguments for plot) – plain line style for forward motion

  • line_r (dict of arguments for plot) – plain line style for forward motion

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

  • 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 used for plan/query

  • goal (array_like(2) or array_like(3), optional) – goal position \((x, y)\) or configuration \((x, y, \theta)\), defaults to value used for plan/query

  • ax (matplotlib axes) – axes to plot into

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

The path has one column per point and either 2 or 3 rows:

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

  • 3 rows describes motion in the \(x-y-\theta\) configuration space. By default only the \(x-y\) plane is plotted unless configspace is True in which case motion in \(x-y-\theta\) configuration space is shown.

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

The default line style is a 'striped' line which comprises a wide first line with a slightly narrow dashed line plotted on top. For example:

(('black', 4), ('yellow', 3))

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

The parameters stripe and stripe_r specify the colors and widths for forward and reverse motion respectively.

The 'line' style is a regular Matplotlib and the parameters line and line_r specify the line and marker styles for forward and reverse motion respectively.

For 2D plots with an \(x-y\) path or 3D plots, the start and goal markers are specified by the dicts start_marker and goal_marker respectively.

For 2D plots with an \(x-y-\theta\) path the vehicle pose is indicated by a vehicle animation object passed to the constructor.

Markers are specified as dicts using Matplotlib keywords, for example:

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

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 configspace is True 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.

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 are passed through to plot_bg()

Seealso

plot_bg() base.plot_poly()

progress_end()
progress_next()
progress_start(n)
randinit()
property start

Start point or configuration used for planning

Returns

start point \((x, y)\) or configuration \((x, y, \theta)\)

Return type

ndarray(2) or ndarray(3)

validate_endpoint(p, dtype=None)
property verbose

Get verbosity

Returns

verbosity

Return type

bool

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

Supporting classes

Planner superclass

Occupancy grid class

roboticstoolbox.mobile.OccGrid

alias of <module ‘roboticstoolbox.mobile.OccGrid’ from ‘/opt/hostedtoolcache/Python/3.7.11/x64/lib/python3.7/site-packages/roboticstoolbox/mobile/OccGrid.py’>