Discrete (Gridbased) planners
Planner 
Plans in 
Discrete/Continuous 
Obstacle avoidance 

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

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

\(\mathbb{R}^2\) 
discrete 
yes 
Distance transform planner
 class roboticstoolbox.mobile.DistanceTransformPlanner(occgrid=None, metric='euclidean', **kwargs)[source]
Bases:
PlannerBase
Distance transform path planner
 Parameters:
occgrid (
BinaryOccGrid
or ndarray(h,w)) – occupancy gridmetric (str optional) – distane metric, one of: “euclidean” [default], “manhattan”
kwargs – common planner options, see
PlannerBase
Feature
Capability
Plan
\(\mathbb{R}^2\), discrete
Obstacle avoidance
Yes, occupancy grid
Curvature
Discontinuous
Motion
Omnidirectional
Creates a planner that finds the path between two points in the 2D grid using omnidirectional motion and avoiding occupied cells. The path comprises a set of 4 or way connected points in adjacent cells. Also known as the wavefront, grassfire or brushfire planning algorithm.
The map is described by a 2D occupancy
occgrid
whose elements are zero if traversable of nonzero if untraversable, ie. an obstacle.The cells are assumed to be unit squares. Crossing the cell horizontally or vertically is a travel distance of 1, and for the Euclidean distance measure, the crossing the cell diagonally is a distance of \(\sqrt{2}\).
Example:
>>> from roboticstoolbox import DistanceTransformPlanner >>> import numpy as np >>> simplegrid = np.zeros((6, 6)); >>> simplegrid[2:5, 3:5] = 1 >>> dx = DistanceTransformPlanner(simplegrid, goal=(1, 1), distance="manhattan"); >>> dx.plan() >>> path = dx.query(start=(5, 4)) >>> print(path.T) [[5 5 5 4 3 2 1] [4 3 2 1 1 1 1]]
Warning
The distance planner is iterative and implemented in Python, will be slow for very large occupancy grids.
 Author:
Peter Corke
 Seealso:
 property metric
Get the distance metric
 Returns:
Get the metric, either “euclidean” or “manhattan”
 Return type:
 property distancemap
Get the distance map
 Returns:
distance map
 Return type:
ndarray(h,w)
The 2D array, the same size as the passed occupancy grid, has elements equal to nan if they contain an obstacle, otherwise the minimum obstaclefree distance to the goal using the particular distance metric.
 plan(goal=None, animate=False, summary=False)[source]
Plan path using distance transform
 Parameters:
goal (array_like(2), optional) – goal position \((x, y)\), defaults to previously set value
Compute the distance transform for all nonobstacle cells, which is the minimum obstaclefree distance to the goal using the particular distance metric.
 Seealso:
 next(position)[source]
Find next point on the path
 Parameters:
position (array_like(2)) – current robot position
 Raises:
RuntimeError – no plan has been computed
 Returns:
next robot position
 Return type:
ndarray(2)
Return the robot position that is one step closer to the goal. Called by
query()
to find a path from start to goal.
 plot_3d(path=None, ls=None)[source]
Plot path on 3D cost surface
 Parameters:
path (ndarray(N,2), optional) – robot path, defaults to None
ls (dict, optional) – dictionary of Matplotlib linestyle options, defaults to None
 Returns:
Matplotlib 3D axes
 Return type:
Axes
Creates a 3D plot showing distance from the goal as a cost surface. Overlays the path if given.
Warning
The visualization is poor because of Matplotlib’s poor hidden line/surface handling.
 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:
 isoccupied(p)
Test if point is occupied (superclass)
 Parameters:
p (array_like(2)) – world coordinate (x, y)
 Returns:
occupancy status of corresponding grid cell
 Return type:
The world coordinate is transformed and the status of the occupancy grid cell is returned. If the point lies outside the bounds of the occupancy grid return True (obstacle)
If there is no occupancy grid this function always returns False (free).
 Seealso:
occgrid()
validate_endpoint()
BinaryOccGrid.isoccupied()
 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:
 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 blackline_r (sequence of dict of arguments for
plot
) – line style for reverse motion, default is striped red on blackconfigspace (bool, optional) – plot the path in 3D configuration space, input must be 3xN. Start and goal style will be given by
qstart_marker
andqgoal_marker
, defaults to Falseunwrap (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 Noneax (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
orgoal
or were set by the object constructor orplan
orquery
method.If the
start
andgoal
have length 2, planning in \(\mathbb{R}^2\), then markers are drawn using styles specified bystart_marker
andend_marker
which are dicts using Matplotlib keywords, for example:planner.plot(path, start=dict(marker='s', color='b'))
If the
start
andgoal
have length 3, planning in \(\SE{2}\), andconfigspace
is False, then directionindicating 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 tobase.plot_poly()
and Matplotlib and could have values such asfilled=True
orcolor
.If
configspace
is False then a 3D plot is created and the start and goal are indicated by Matplotlib markers specified by the dictsstart_marker
andend_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 outlinethe 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 toplot_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 bidirectional 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 fromline_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()
 plot_bg(distance=None, cmap='gray', ax=None, inflated=True, colorbar=True, block=None, **unused)
Plot background (superclass)
 Parameters:
distance (ndarray(N,M), optional) – override distance field, defaults to None
cmap (str or Colormap, optional) – Specify a colormap for the distance field, defaults to ‘gray’
Displays the background which is either the occupancy grid or a distance field. The distance field encodes the distance of a point from the goal, small distance is dark, a large distance is bright.
 If the planner has an occupancy grid then that will be displayed with:
free cells in white
occupied cells in red
inflated occupied cells in pink
If distance is provided, or the planner has a distancemap attribute the the distance field will be used as the background and obstacle cells (actual or inflated) will be shown in red. A colorbar is added.
 query(start=None, goal=None, dtype=None, next=True, animate=False, movie=None)
Find a path from start to goal using planner (superclass)
 Parameters:
start (array_like(), optional) – start position \((x, y)\) or configuration \((x, y, \theta)\), defaults to value specified to constructor
goal (array_like(), optional) – goal position \((x, y)\) or configuration \((x, y, \theta)\), defaults to value specified to constructor
dtype (str, optional) – data type for point coordinates, defaults to None
next (bool, optional) – invoke
next()
method of class, defaults to Trueanimate (bool, optional) – show the vehicle path, defaults to False
 Returns:
path from start to goal, one point \((x, y)\) or configuration \((x, y, \theta)\) per row
 Return type:
ndarray(N,2) or ndarray(N,3)
Find a path from
start
togoal
using a previously computed plan.This is a generic method that works for any planner (\(\mathbb{R}^2\) or \(\SE{2}\)) that can incrementally determine the next point on the path. The method performs the following steps:
 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:
 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:
D* planner
 class roboticstoolbox.mobile.DstarPlanner(costmap=None, **kwargs)[source]
Bases:
PlannerBase
D* path planner
 Parameters:
costmap (OccGrid or ndarray(w,h)) – traversability costmap
kwargs – common planner options, see
PlannerBase
Feature
Capability
Plan
\(\mathbb{R}^2\), discrete
Obstacle avoidance
Yes, occupancy grid
Curvature
Discontinuous
Motion
Omnidirectional
Creates a planner that finds the minimumcost path between two points in the plane using omnidirectional motion. The path comprises a set of 8way connected points in adjacent cells.
The map is described by a 2D
costmap
whose elements indicate the cost of traversing that cell. The cost of diagonal traverse is \(\sqrt{2}\) the value of the cell. An infinite cost indicates an untraversable cell or obstacle.Example:
>>> from roboticstoolbox import DstarPlanner >>> import numpy as np >>> costmap = np.ones((6, 6)); >>> costmap[2:5, 3:5] = 10 >>> ds = DstarPlanner(costmap, goal=(1, 1)); >>> ds.plan() >>> path, status = ds.query(start=(5, 4)) >>> print(path.T) [[5 5 5 4 3 2 1] [4 3 2 1 1 1 1]] >>> print(status) _DstarStatus(cost=6.414213562373095)
 Thanks:
based on D* grid planning included from Python Robotics
 Seealso:
 plan(goal=None, animate=False, progress=True, summary=False)[source]
Plan D* path
 Parameters:
Compute the minimumcost obstaclefree distance to the goal from all points in the grid.
 property nexpand
Number of node expansions
 Returns:
number of expansions
 Return type:
This number will increase during initial planning, and also if replanning is invoked during the
query()
.
 query(start, sensor=None, animate=False, verbose=False)[source]
Find path with replanning
 Parameters:
 Returns:
path from start to goal, one point \((x, y)\) per row
 Return type:
ndarray(N,2)
If
sensor
is None then the plan determined by theplan
phase is used unaltered.If
sensor
is not None it must be callable, and is called at each step of the path with the current robot coordintes:sensor((x, y))
and mimics the behaviour of a simple sensor onboard the robot which can dynamically change the costmap. The function return a list (0 or more) of 3tuples (x, y, newcost) which are the coordinates of cells and their cost. If the cost has changed this will trigger D* incremental replanning. In this case the value returned by
nexpand()
will increase, according to the severity of the replanning. Seealso:
 __str__()
Compact representation of the planner
 Returns:
pretty printed representation
 Return type:
 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:
 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 blackline_r (sequence of dict of arguments for
plot
) – line style for reverse motion, default is striped red on blackconfigspace (bool, optional) – plot the path in 3D configuration space, input must be 3xN. Start and goal style will be given by
qstart_marker
andqgoal_marker
, defaults to Falseunwrap (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 Noneax (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
orgoal
or were set by the object constructor orplan
orquery
method.If the
start
andgoal
have length 2, planning in \(\mathbb{R}^2\), then markers are drawn using styles specified bystart_marker
andend_marker
which are dicts using Matplotlib keywords, for example:planner.plot(path, start=dict(marker='s', color='b'))
If the
start
andgoal
have length 3, planning in \(\SE{2}\), andconfigspace
is False, then directionindicating 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 tobase.plot_poly()
and Matplotlib and could have values such asfilled=True
orcolor
.If
configspace
is False then a 3D plot is created and the start and goal are indicated by Matplotlib markers specified by the dictsstart_marker
andend_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 outlinethe 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 toplot_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 bidirectional 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 fromline_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()
 plot_bg(distance=None, cmap='gray', ax=None, inflated=True, colorbar=True, block=None, **unused)
Plot background (superclass)
 Parameters:
distance (ndarray(N,M), optional) – override distance field, defaults to None
cmap (str or Colormap, optional) – Specify a colormap for the distance field, defaults to ‘gray’
Displays the background which is either the occupancy grid or a distance field. The distance field encodes the distance of a point from the goal, small distance is dark, a large distance is bright.
 If the planner has an occupancy grid then that will be displayed with:
free cells in white
occupied cells in red
inflated occupied cells in pink
If distance is provided, or the planner has a distancemap attribute the the distance field will be used as the background and obstacle cells (actual or inflated) will be shown in red. A colorbar is added.
 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:
 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()
Lattice planner
 class roboticstoolbox.mobile.LatticePlanner(costs=None, root=(0, 0, 0), **kwargs)[source]
Bases:
PlannerBase
Lattice planner
 Parameters:
costs (array_like(3), optional) – cost for straight, leftturn, rightturn, 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
PlannerBase
constructor
Feature
Capability
Plan
\(\SE{2}\)
Obstacle avoidance
Yes, occupancy grid
Curvature
Discontinuous
Motion
Forwards only
The lattice planner incrementally builds a graph from the root vertex, at each iteration adding three edges to the graph:
code
direction
‘S’
straight ahead
‘L’
turn left
‘R’
turn right
If the configuration is already in the graph, the edge connects to that existing vertex. The vertex is named after the sequence of moves required to reach it from the root. This means, that any configuration, ie. \((x, y, \theta)\) can be reached by multiple paths and potentially have multiple names. The first name assigned to a vertex is permanent and is not overriden.
If an occupancy grid exists and the configuration is an obstacle, then the vertex is not added.
The path through the lattice is found using A* graph search, and
costs
changes the weighting for path costs at query time.Example:
>>> from roboticstoolbox import LatticePlanner >>> import numpy as np >>> lattice = LatticePlanner(); >>> lattice.plan(iterations=6) >>> path, status = lattice.query(start=(0, 0, 0), goal=(1, 2, np.pi/2)) >>> print(path.T) [[0. 1. 1. ] [0. 1. 2. ] [0. 1.5708 1.5708]] >>> print(status) LatticeStatus(cost=2.5707963267948966, segments=['L', 'S'], edges=[Edge{[0]  [0L], cost=1.571}, Edge{[0L]  [0LS], cost=1}])
 Seealso:
 plan(iterations=None, verbose=False, summary=False)[source]
Create a lattice plan
 Parameters:
If an occupancy grid exists the if
iterations
is None the area of the grid will be completely filled. Seealso:
 query(start, goal)[source]
Find a path through the lattice
 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
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:
 plot(path=None, **kwargs)[source]
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 blackline_r (sequence of dict of arguments for
plot
) – line style for reverse motion, default is striped red on blackconfigspace (bool, optional) – plot the path in 3D configuration space, input must be 3xN. Start and goal style will be given by
qstart_marker
andqgoal_marker
, defaults to Falseunwrap (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 Noneax (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
orgoal
or were set by the object constructor orplan
orquery
method.If the
start
andgoal
have length 2, planning in \(\mathbb{R}^2\), then markers are drawn using styles specified bystart_marker
andend_marker
which are dicts using Matplotlib keywords, for example:planner.plot(path, start=dict(marker='s', color='b'))
If the
start
andgoal
have length 3, planning in \(\SE{2}\), andconfigspace
is False, then directionindicating 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 tobase.plot_poly()
and Matplotlib and could have values such asfilled=True
orcolor
.If
configspace
is False then a 3D plot is created and the start and goal are indicated by Matplotlib markers specified by the dictsstart_marker
andend_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 outlinethe 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 toplot_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 bidirectional 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 fromline_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 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:
 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:
 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()
Continuous configurationspace 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:
PlannerBase
Dubins path planner
 Parameters:
 Returns:
Dubins path planner
 Return type:
DubinsPlanner instance
Feature
Capability
Plan
\(\SE{2}\)
Obstacle avoidance
No
Curvature
Discontinuous
Motion
Forwards only
Creates a planner that finds the path between two configurations in the plane using forward motion only. The path comprises upto 3 segments that are straight lines, or arcs with curvature of \(\pm\)
curvature
.Example:
>>> from roboticstoolbox import DubinsPlanner >>> from math import pi >>> dubins = DubinsPlanner(curvature=1.0) >>> path, status = dubins.query(start=(0, 0, pi/2), goal=(1, 0, pi/2)) >>> print(path[:5,:]) [[ 0. 0. 1.5708] [0.005 0.0998 1.6708] [0.0199 0.1987 1.7708] [0.0447 0.2955 1.8708] [0.0789 0.3894 1.9708]] >>> print(status) DubinsStatus(segments=['L', 'S', 'L'], length=7.283185307179586, seglengths=[4.71238898038469, 1.0, 1.5707963267948966])
 Reference:
On Curves of Minimal Length with a Constraint on Average Curvature, and with Prescribed Initial and Terminal Positions and Tangents, Dubins, L.E. (July 1957), American Journal of Mathematics. 79(3): 497–516.
 Thanks:
based on Dubins path planning from Python Robotics
 Seealso:
 property curvature
 property stepsize
 query(start, goal, **kwargs)[source]
Find a path between two configurations
 Parameters:
start (array_like(3), optional) – start configuration \((x, y, \theta)\)
goal (array_like(3), optional) – goal configuration \((x, y, \theta)\)
 Returns:
path and status
 Return type:
ndarray(N,3), namedtuple
The path comprises points equally spaced at a distance of
stepsize
.The returned status value has elements:
Element
Description
segments
a list containing the type of each path segment as a single letter code: either “L”, “R” or “S” for left turn, right turn or straight line respectively.
length
total path length
lengths
the length of each path segment. The sign of the length indicates the direction of travel.
 property goal
Set/get goal point or configuration (superclass)
 Getter:
Return goal pointor configuration
 Return type:
ndarray(2) or ndarray(3)
 Setter:
Set goal point or configuration
 Param:
array_like(2) or array_like(3)
The goal is either a point \((x, y)\) or a configuration \((x, y, \theta)\).
 Seealso:
 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 blackline_r (sequence of dict of arguments for
plot
) – line style for reverse motion, default is striped red on blackconfigspace (bool, optional) – plot the path in 3D configuration space, input must be 3xN. Start and goal style will be given by
qstart_marker
andqgoal_marker
, defaults to Falseunwrap (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 Noneax (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
orgoal
or were set by the object constructor orplan
orquery
method.If the
start
andgoal
have length 2, planning in \(\mathbb{R}^2\), then markers are drawn using styles specified bystart_marker
andend_marker
which are dicts using Matplotlib keywords, for example:planner.plot(path, start=dict(marker='s', color='b'))
If the
start
andgoal
have length 3, planning in \(\SE{2}\), andconfigspace
is False, then directionindicating 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 tobase.plot_poly()
and Matplotlib and could have values such asfilled=True
orcolor
.If
configspace
is False then a 3D plot is created and the start and goal are indicated by Matplotlib markers specified by the dictsstart_marker
andend_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 outlinethe 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 toplot_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 bidirectional 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 fromline_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:
 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()
ReedsShepp path planner
 class roboticstoolbox.mobile.ReedsSheppPlanner(curvature=1, stepsize=0.1, **kwargs)[source]
Bases:
PlannerBase
ReedsShepp path planner
 Parameters:
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 ReedsShepp path planning from Python Robotics
 Seealso:
 query(start, goal, **kwargs)[source]
Find a path between two configurations
 Parameters:
start (array_like(3), optional) – start configuration \((x, y, \theta)\)
goal (array_like(3), optional) – goal configuration \((x, y, \theta)\)
 Returns:
path and status
 Return type:
ndarray(N,3), namedtuple
The path comprises points equally spaced at a distance of
stepsize
.The returned status value has elements:
Element
Description
segments
a list containing the type of each path segment as a single letter code: either “L”, “R” or “S” for left turn, right turn or straight line respectively.
length
total path length
lengths
the length of each path segment. The sign of the length indicates the direction of travel.
direction
the direction of motion at each point on the path
Note
The direction of turning is reversed when travelling backwards.
 property goal
Set/get goal point or configuration (superclass)
 Getter:
Return goal pointor configuration
 Return type:
ndarray(2) or ndarray(3)
 Setter:
Set goal point or configuration
 Param:
array_like(2) or array_like(3)
The goal is either a point \((x, y)\) or a configuration \((x, y, \theta)\).
 Seealso:
 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 blackline_r (sequence of dict of arguments for
plot
) – line style for reverse motion, default is striped red on blackconfigspace (bool, optional) – plot the path in 3D configuration space, input must be 3xN. Start and goal style will be given by
qstart_marker
andqgoal_marker
, defaults to Falseunwrap (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 Noneax (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
orgoal
or were set by the object constructor orplan
orquery
method.If the
start
andgoal
have length 2, planning in \(\mathbb{R}^2\), then markers are drawn using styles specified bystart_marker
andend_marker
which are dicts using Matplotlib keywords, for example:planner.plot(path, start=dict(marker='s', color='b'))
If the
start
andgoal
have length 3, planning in \(\SE{2}\), andconfigspace
is False, then directionindicating 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 tobase.plot_poly()
and Matplotlib and could have values such asfilled=True
orcolor
.If
configspace
is False then a 3D plot is created and the start and goal are indicated by Matplotlib markers specified by the dictsstart_marker
andend_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 outlinethe 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 toplot_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 bidirectional 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 fromline_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:
 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()
Curvaturepolynomial 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:
 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:
 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 blackline_r (sequence of dict of arguments for
plot
) – line style for reverse motion, default is striped red on blackconfigspace (bool, optional) – plot the path in 3D configuration space, input must be 3xN. Start and goal style will be given by
qstart_marker
andqgoal_marker
, defaults to Falseunwrap (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 Noneax (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
orgoal
or were set by the object constructor orplan
orquery
method.If the
start
andgoal
have length 2, planning in \(\mathbb{R}^2\), then markers are drawn using styles specified bystart_marker
andend_marker
which are dicts using Matplotlib keywords, for example:planner.plot(path, start=dict(marker='s', color='b'))
If the
start
andgoal
have length 3, planning in \(\SE{2}\), andconfigspace
is False, then directionindicating 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 tobase.plot_poly()
and Matplotlib and could have values such asfilled=True
orcolor
.If
configspace
is False then a 3D plot is created and the start and goal are indicated by Matplotlib markers specified by the dictsstart_marker
andend_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 outlinethe 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 toplot_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 bidirectional 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 fromline_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:
 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()
Quinticpolynomial 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 ofmin_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:
 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:
 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 blackline_r (sequence of dict of arguments for
plot
) – line style for reverse motion, default is striped red on blackconfigspace (bool, optional) – plot the path in 3D configuration space, input must be 3xN. Start and goal style will be given by
qstart_marker
andqgoal_marker
, defaults to Falseunwrap (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 Noneax (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
orgoal
or were set by the object constructor orplan
orquery
method.If the
start
andgoal
have length 2, planning in \(\mathbb{R}^2\), then markers are drawn using styles specified bystart_marker
andend_marker
which are dicts using Matplotlib keywords, for example:planner.plot(path, start=dict(marker='s', color='b'))
If the
start
andgoal
have length 3, planning in \(\SE{2}\), andconfigspace
is False, then directionindicating 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 tobase.plot_poly()
and Matplotlib and could have values such asfilled=True
orcolor
.If
configspace
is False then a 3D plot is created and the start and goal are indicated by Matplotlib markers specified by the dictsstart_marker
andend_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 outlinethe 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 toplot_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 bidirectional 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 fromline_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:
 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()
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 gridvehicle (
VehicleBase
subclass) – vehicle kinematic modelcurvature (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 obstaclefree path between two configurations in the plane using forward and backward motion. The path comprises multiple Dubins curves comprising straight lines, or arcs with curvature of \(\pm\)
curvature
. Motion along the segments may be in the forward or backward direction.Polygons are used for obstacle avoidance:
the environment is defined by a set of polygons represented by a
PolygonMap
the vehicle is defined by a single polygon specified by the
polygon
argument to its constructor
Example:
from roboticstoolbox import RRTPlanner from spatialmath import Polygon2 from math import pi # create polygonal obstacles map = PolygonMap(workspace=[0, 10]) map.add([(5, 50), (5, 6), (6, 6), (6, 50)]) map.add([(5, 4), (5, 50), (6, 50), (6, 4)]) # create outline polygon for vehicle l, w = 3, 1.5 vpolygon = Polygon2([(l/2, w/2), (l/2, w/2), (l/2, w/2), (l/2, w/2)]) # create vehicle model vehicle = Bicycle(steer_max=1, L=2, polygon=vpolygon) # create planner rrt = RRTPlanner(map=map, vehicle=vehicle, npoints=50, seed=0) # start and goal configuration qs = (2, 8, pi/2) qg = (8, 2, pi/2) # plan path rrt.plan(goal=qg) path, status = rrt.query(start=qs) print(path[:5,:]) print(status)
 Seealso:
 plan(goal, showsamples=True, showvalid=True, animate=False)[source]
Plan paths to goal using RRT
 Parameters:
goal (array_like(3), optional) – goal pose \((x, y, \theta)\), defaults to previously set value
showsamples (bool, optional) – display position part of configurations overlaid on the map, defaults to True
showvalid (bool, optional) – display valid configurations as vehicle polygons overlaid on the map, defaults to False
animate (bool, optional) – update the display as configurations are tested, defaults to False
Compute a rapidly exploring random tree with its root at the
goal
. The tree will havenpoints
vertices spread uniformly randomly over the workspace which is an attribute of themap
.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 ifshowvalid
is True. Ifanimate
is True these points are displayed during the search process, otherwise a single figure is displayed at the end. Seealso:
 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 lengthinitial_d
 distance from start to first vertex in graphvertices
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()[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 onqrandom()
 Seealso:
 iscollision(q)[source]
Test if configuration is collision
 Parameters:
q (array_like(3)) – vehicle configuration \((x, y, \theta)\)
 Returns:
collision status
 Return type:
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:
 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:
 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 blackline_r (sequence of dict of arguments for
plot
) – line style for reverse motion, default is striped red on blackconfigspace (bool, optional) – plot the path in 3D configuration space, input must be 3xN. Start and goal style will be given by
qstart_marker
andqgoal_marker
, defaults to Falseunwrap (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 Noneax (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
orgoal
or were set by the object constructor orplan
orquery
method.If the
start
andgoal
have length 2, planning in \(\mathbb{R}^2\), then markers are drawn using styles specified bystart_marker
andend_marker
which are dicts using Matplotlib keywords, for example:planner.plot(path, start=dict(marker='s', color='b'))
If the
start
andgoal
have length 3, planning in \(\SE{2}\), andconfigspace
is False, then directionindicating 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 tobase.plot_poly()
and Matplotlib and could have values such asfilled=True
orcolor
.If
configspace
is False then a 3D plot is created and the start and goal are indicated by Matplotlib markers specified by the dictsstart_marker
andend_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 outlinethe 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 toplot_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 bidirectional 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 fromline_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:
 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()