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

Roadmap graph

Returns:: roadmap as an undirected graph
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() graph()

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 roadmap graph
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:: ReedsSheppPlanner PlannerBase

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:: DubinsPlanner PlannerBase

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:: DubinsPlanner Vehicle PlannerBase

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:: qrandom() iscollision()

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.