# plannerRRTStar

## Description

The `plannerRRTStar`

object creates an asymptotically-optimal RRT
planner, RRT*. The RRT* algorithm converges to an optimal solution in terms of the state space
distance. Also, its runtime is a constant factor of the runtime of the RRT algorithm. RRT* is
used to solve geometric planning problems. A geometric planning problem requires that any two
random states drawn from the state space can be connected.

## Creation

### Description

creates an RRT* planner from a state space object, `planner`

= plannerRRTStar(`stateSpace`

,`stateVal`

)`stateSpace`

, and a
state validator object, `stateVal`

. The state space of
`stateVal`

must be the same as `stateSpace`

.
`stateSpace`

and `stateVal`

also sets the
StateSpace and
StateValidator
properties of the `planner`

object.

sets properties using one or more name-value arguments in addition to the input arguments
in the previous syntax. You can specify the StateSampler,
BallRadiusConstant, ContinueAfterGoalReached, MaxNumTreeNodes,
MaxIterations,
MaxConnectionDistance, GoalReachedFcn,
and GoalBias
properties as name-value arguments.`planner`

= plannerRRTStar(___,`Name=Value`

)

## Properties

`StateSpace`

— State space for planner

state space object

State space for the planner, specified as a state space object. You can use state
space objects such as `stateSpaceSE2`

,
`stateSpaceDubins`

, `stateSpaceReedsShepp`

, and `stateSpaceSE3`

.
You can also customize a state space object using the `nav.StateSpace`

object.

`StateValidator`

— State validator for planner

state validator object

State validator for the planner, specified as a state validator object. You can use
state validator objects such as `validatorOccupancyMap`

, `validatorVehicleCostmap`

, and `validatorOccupancyMap3D`

.

`StateSampler`

— State space sampler for sampling input space

`stateSamplerUniform`

object (default) | `stateSamplerGaussian`

object | `stateSamplerMPNET`

object | `nav.StateSampler`

object

*Since R2023b*

State space sampler used for finding state samples in the input space, specified as
a `stateSamplerUniform`

object, `stateSamplerGaussian`

object, `stateSamplerMPNET`

object, or `nav.StateSampler`

object. By default, the `plannerRRTStar`

uses uniform state
sampling.

`BallRadiusConstant`

— Constant used to estimate the near neighbors search radius

`100`

(default) | positive scalar

Constant used to estimate the near neighbors search radius, specified as a positive scalar. The radius is estimated as following:

$$r=\mathrm{min}\left(\gamma {\left(\frac{\mathrm{log}(n)}{n}\right)}^{\frac{1}{d}},\eta \right)$$

where:

*γ*— The value of the`BallRadiusConstant`

property*n*— Current number of nodes in the tree*d*— Dimension of the state space*η*— The value of the`MaxConnectionDistance`

property

*γ* is defined as following:

$$\gamma ={{\displaystyle 2}}^{d}\left(1+\frac{1}{d}\right)\left(\frac{{{\displaystyle V}}_{Free}}{{{\displaystyle V}}_{Ball}}\right)$$

where:

*V*— Approximate free volume in search-space_{Free}*V*— Volume of unit ball in_{Ball}*d*dimensions

The formulae above define a `BallRadiusConstant`

of "appropriate"
size for a given space, meaning that as the number of nodes filling the space grows and
the radius shrinks, the expected number of neighbors grows logarithmically. Higher
values will result in a higher average number of neighbors within the
*d*-ball per iteration, leading to more rewire candidates. However,
values below this suggested minimum could lead to a single nearby neighbor, which fails
to produce asymptotically optimal results.

**Example: **`BallRadiusConstant=80`

**Data Types: **`single`

| `double`

`ContinueAfterGoalReached`

— Continue to optimize after goal is reached

`false`

(default) | `true`

Decide if the planner continues to optimize after the goal is reached, specified as
`false`

or `true`

. The planner also terminates
regardless of the value of this property if the maximum number of iterations or maximum
number of tree nodes is reached.

**Example: **`ContinueAfterGoalReached=true`

**Data Types: **`logical`

`MaxNumTreeNodes`

— Maximum number of nodes in the search tree

`1e4`

(default) | positive integer

Maximum number of nodes in the search tree (excluding the root node), specified as a positive integer.

**Example: **`MaxNumTreeNodes=2500`

**Data Types: **`single`

| `double`

`MaxIterations`

— Maximum number of iterations

`1e4`

(default) | positive integer

Maximum number of iterations, specified as a positive integer.

**Example: **`MaxIterations=2500`

**Data Types: **`single`

| `double`

`MaxConnectionDistance`

— Maximum length of motion

`0.1`

(default) | positive scalar

Maximum length of a motion allowed in the tree, specified as a scalar.

**Example: **`MaxConnectionDistance=0.3`

**Data Types: **`single`

| `double`

`GoalReachedFcn`

— Callback function to determine whether goal is reached

`@nav.algs.checkIfGoalIsReached`

| function handle

Callback function to determine whether the goal is reached, specified as a function handle. You can create your own goal reached function. The function must follow this syntax:

function isReached = myGoalReachedFcn(planner,currentState,goalState)

where:

`planner`

— The created planner object, specified as`plannerRRTStar`

object.`currentState`

— The current state, specified as a three element real vector.`goalState`

— The goal state, specified as a three element real vector.`isReached`

— A boolean variable to indicate whether the current state has reached the goal state, returned as`true`

or`false`

.

To use custom `GoalReachedFcn`

in code generation workflow, this
property must be set to a custom function handle before calling the plan function and it
cannot be changed after initialization.

**Data Types: **`function handle`

`GoalBias`

— Probability of choosing goal state during state sampling

`0.05`

(default) | real scalar in range [0,1]

Probability of choosing the goal state during state sampling, specified as a real
scalar in range [0,1]. The property defines the probability of choosing the actual goal
state during the process of randomly selecting states from the state space. You can
start by setting the probability to a small value such as
`0.05`

.

**Example: **`GoalBias=0.1`

**Data Types: **`single`

| `double`

## Examples

### Plan Optimal Path Between Two States

Create a state space.

ss = stateSpaceSE2;

Create an `occupancyMap`

-based state validator using the created state space.

sv = validatorOccupancyMap(ss);

Create an occupancy map from an example map and set map resolution as 10 cells/meter.

```
load exampleMaps.mat
map = occupancyMap(simpleMap,10);
sv.Map = map;
```

Set validation distance for the validator.

sv.ValidationDistance = 0.01;

Update state space bounds to be the same as map limits.

ss.StateBounds = [map.XWorldLimits; map.YWorldLimits; [-pi pi]];

Create RRT* path planner and allow further optimization after goal is reached. Reduce the maximum iterations and increase the maximum connection distance.

planner = plannerRRTStar(ss,sv, ... ContinueAfterGoalReached=true, ... MaxIterations=2500, ... MaxConnectionDistance=0.3);

Set the start and goal states.

start = [0.5 0.5 0]; goal = [2.5 0.2 0];

Plan a path with default settings.

rng(100,'twister') % repeatable result [pthObj,solnInfo] = plan(planner,start,goal);

Visualize the results.

map.show hold on % Tree expansion plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'.-') % Draw path plot(pthObj.States(:,1),pthObj.States(:,2),'r-','LineWidth',2)

### Plan Path Through 3-D Occupancy Map Using RRT Star Planner

Load a 3-D occupancy map of a city block into the workspace. Specify the threshold to consider cells as obstacle-free.

```
mapData = load("dMapCityBlock.mat");
omap = mapData.omap;
omap.FreeThreshold = 0.5;
```

Inflate the occupancy map to add a buffer zone for safe operation around the obstacles.

inflate(omap,1)

Create an SE(3) state space object with bounds for state variables.

ss = stateSpaceSE3([0 220;0 220;0 100;inf inf;inf inf;inf inf;inf inf]);

Create a 3-D occupancy map state validator using the created state space. Assign the occupancy map to the state validator object. Specify the sampling distance interval.

sv = validatorOccupancyMap3D(ss, ... Map = omap, ... ValidationDistance = 0.1);

Create a RRT star path planner with increased maximum connection distance and reduced maximum number of iterations. Specify a custom goal function that determines that a path reaches the goal if the Euclidean distance to the target is below a threshold of 1 meter.

planner = plannerRRTStar(ss,sv, ... MaxConnectionDistance = 50, ... MaxIterations = 1000, ... GoalReachedFcn = @(~,s,g)(norm(s(1:3)-g(1:3))<1), ... GoalBias = 0.1);

Specify start and goal poses.

start = [40 180 25 0.7 0.2 0 0.1]; goal = [150 33 35 0.3 0 0.1 0.6];

Configure the random number generator for repeatable result.

`rng(1,"twister");`

Plan the path.

[pthObj,solnInfo] = plan(planner,start,goal);

Visualize the planned path.

show(omap) axis equal view([-10 55]) hold on % Start state scatter3(start(1,1),start(1,2),start(1,3),"g","filled") % Goal state scatter3(goal(1,1),goal(1,2),goal(1,3),"r","filled") % Path plot3(pthObj.States(:,1),pthObj.States(:,2),pthObj.States(:,3), ... "r-",LineWidth=2)

## More About

### Ball Radius Constant

The major difference between RRT and RRT* is the
*rewiring* behavior, which guarantees asymptotic optimality for the
RRT* algorithm. When RRT-based planners generate a new node, the planner finds the nearest
node in the tree. If the path between nodes is collision free and otherwise valid, the RRT
algorithm connects the nodes, but the RRT* algorithm performs additional steps to optimize
the tree after connecting the nodes. First, RRT* finds all nodes in the tree within some
distance to the new node, then RRT* finds the node that provides the new node with the
shortest valid path back to the starting node, and adds an edge between the node and the new
node. Lastly, the planner performs a rewire operation, which checks whether the new node can
provide each of the nearest nodes with a shorter route back to the starting node. In the
case that there is a shorter path, the node is disconnected from the current parent and
reparented to the closer node.

The radius at which the rewiring occurs, is called the ball radius constant. Selecting an appropriate ball radius constant is important as the goal of RRT* is to guarantee asymptotic optimality while limiting any additional overhead computation. If the ball radius constant is too large, the runtime of RRT* increases. If the ball radius constant is too small, then the algorithm may fail to converge on an optimal result.

`plannerRRTStar`

uses this distance formula, adapted from [1], to find the nearest
neighbors:

$$r=\mathrm{min}\left(\gamma {\left(\frac{\mathrm{log}(n)}{n}\right)}^{\frac{1}{d}},\eta \right)$$

where:

*n*— Number of nodes in tree*d*— Number of dimensions of the state space*η*— Maximum connection distance*γ*— Ball radius constant, defined as:$$\gamma ={{\displaystyle 2}}^{d}\left(1+\frac{1}{d}\right)\left(\frac{{{\displaystyle V}}_{Free}}{{{\displaystyle V}}_{Ball}}\right)$$

where:

*V*— Lebesgue Measure, the approximate free volume in search space_{Free}*V*— Volume of unit ball in_{Ball}*d*dimensions

**Meaning and Intuition of Ball Radius Constant**

The formulae for *r* an *γ* define a radius of
appropriate size for a given space and sampling density. And as the number of nodes
filling the space grows linearly, the radius must shrink and the number of neighbors
inside the shrinking ball grows logarithmically.

This intuition is from the expectation that all the newly sampled points in the tree
have been uniformly and independently sampled from a free portion of the configuration
space. By sampling points in this way, you can say they were generated using a homogeneous
Poisson point process. This means that in each iteration of RRT*, *n*
points have been uniformly sampled in the free space, so there should be an average
density of points per unit volume, *λ*. For spaces of arbitrary
dimensions, there is an intensity of points per unit measure.

$$\lambda =\frac{N}{{V}_{Free}}$$

Therefore, the number of points you can expect to see in any portion of the planning
space is the volume of that portion, multiplied by the density. For RRT*, the focus is in
the number of points within a ball of *d* dimensions of radius
*r*:

$${n}_{1,d}={V}_{Ball}\xb7\lambda =\frac{{V}_{Ball}}{{V}_{Free}}\xb7n$$

$${\text{n}}_{1,d}={\text{n}}_{1,d}\xb7{r}^{d}$$

where,

*n*— Expected number of points inside unit ball of_{1,d}*d*dimensions*n*— Expected number of points inside ball of_{r,d}*d*dimensions with a radius*r*

And recalling that the goal for the number of neighbors is to grow logarithmically as
*n* approaches infinity, you can set
*n _{r,d}=log(n)* and solve for

*r*:

$$\text{r=}{\left(\frac{{V}_{Free}}{{V}_{Ball}}\xb7\frac{\mathrm{log}(n)}{n}\right)}^{\frac{1}{d}}$$

The remaining coefficients from formula 2 are derived from the
convergence proof in [1]. However with
*n* removed you can see that the ball radius constant is a ratio of the
free space in the sample region vs the measure of the unit ball multiplied by a
dimension-specific constant.

## References

[1] Karaman, S. and E. Frazzoli.
"Sampling-Based Algorithms for Optimal Motion Planning." *The International Journal
of Robotics Research.* Vol. 30, Number 7, 2011, pp 846 – 894.

## Extended Capabilities

### C/C++ Code Generation

Generate C and C++ code using MATLAB® Coder™.

Usage notes and limitations:

To use custom GoalReachedFcn in code generation workflow, this property must be set to a custom function handle before calling the plan function and it cannot be changed after initialization.

## Version History

**Introduced in R2019b**

### R2023b: Specify sampling approach for path planning

You can now specify uniform sampling, Gaussian sampling, MPNet sampling, or a custom
sampling approach to generate samples for path planning. Use the name, value argument
`StateSampler`

to specify the sampling approach.

## See Also

### Objects

`plannerRRT`

|`plannerBiRRT`

|`stateSpaceReedsShepp`

|`stateSpaceDubins`

|`stateSpaceSE2`

|`stateSpaceSE3`

|`validatorOccupancyMap`

|`validatorVehicleCostmap`

|`validatorOccupancyMap3D`

|`stateSamplerUniform`

### Functions

## Open Example

You have a modified version of this example. Do you want to open this example with your edits?

## MATLAB Command

You clicked a link that corresponds to this MATLAB command:

Run the command by entering it in the MATLAB Command Window. Web browsers do not support MATLAB commands.

Select a Web Site

Choose a web site to get translated content where available and see local events and offers. Based on your location, we recommend that you select: .

You can also select a web site from the following list:

## How to Get Best Site Performance

Select the China site (in Chinese or English) for best site performance. Other MathWorks country sites are not optimized for visits from your location.

### Americas

- América Latina (Español)
- Canada (English)
- United States (English)

### Europe

- Belgium (English)
- Denmark (English)
- Deutschland (Deutsch)
- España (Español)
- Finland (English)
- France (Français)
- Ireland (English)
- Italia (Italiano)
- Luxembourg (English)

- Netherlands (English)
- Norway (English)
- Österreich (Deutsch)
- Portugal (English)
- Sweden (English)
- Switzerland
- United Kingdom (English)