Discrete (Grid-based) planners

Planner

Plans in

Discrete/Continuous

Obstacle avoidance

Bug2

\(\mathbb{R}^2\)

discrete

yes

DistanceTransformPlanner

\(\mathbb{R}^2\)

discrete

yes

DstarPlanner

\(\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 grid

  • metric (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:

plan() query() PlannerBase

property metric

Get the distance metric

Returns:

Get the metric, either “euclidean” or “manhattan”

Return type:

str

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 obstacle-free 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 non-obstacle cells, which is the minimum obstacle-free distance to the goal using the particular distance metric.

Seealso:

query()

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.

Seealso:

plan() query()

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:

goal()

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:

bool

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:

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

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 True

  • animate (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 to goal 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:

  • Validate start and goal

  • If animate, visualize the environment using plot()

  • Iterate on the class’s next() method until the goal is achieved, and if animate plot points.

Seealso:

next() plan()

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.

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 minimum-cost path between two points in the plane using omnidirectional motion. The path comprises a set of 8-way 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:

PlannerBase

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

Plan D* path

Parameters:

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

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

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

Compute the minimum-cost obstacle-free distance to the goal from all points in the grid.

property nexpand

Number of node expansions

Returns:

number of expansions

Return type:

int

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:

  • start (array_like(2)) – start position \((x,y)\)

  • sensor (callable, optional) – sensor function, defaults to None

  • animate (bool, optional) – animate the motion of the robot, defaults to False

  • verbose (bool, optional) – display detailed diagnostic information about D* operations, defaults to False

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 the plan 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 3-tuples (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:

plan()

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

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:

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.

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, left-turn, right-turn, defaults to \((1, \pi/2, \pi/2)\)

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

  • kwargs – arguments passed to 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() query() PlannerBase

plan(iterations=None, verbose=False, summary=False)[source]

Create a lattice plan

Parameters:

  • iterations (int, optional) – number of iterations, defaults to None

  • verbose (bool, optional) – show frontier and added vertices/edges at each iteration, defaults to False

If an occupancy grid exists the if iterations is None the area of the grid will be completely filled.

Seealso:

query()

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:

plan()

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

Continuous configuration-space planners

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

Dubins path planner

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

Bases: 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.