# plannerRRTStar

Create an optimal RRT path planner (RRT*)

Since R2019b

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

### Syntax

``planner = plannerRRTStar(stateSpace,stateVal)``
``planner = plannerRRTStar(___,Name=Value)``

### Description

````planner = plannerRRTStar(stateSpace,stateVal)` creates an RRT* planner from a state space object, `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.```

example

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

## Properties

expand all

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.

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

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}\left(n\right)}{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 ={2}^{d}\left(1+\frac{1}{d}\right)\left(\frac{{V}_{Free}}{{V}_{Ball}}\right)$`

where:

• VFree — Approximate free volume in search-space

• VBall — Volume of unit ball in 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`

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`

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

Example: `MaxNumTreeNodes=2500`

Data Types: `single` | `double`

Maximum number of iterations, specified as a positive integer.

Example: `MaxIterations=2500`

Data Types: `single` | `double`

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

Example: `MaxConnectionDistance=0.3`

Data Types: `single` | `double`

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`

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`

## Object Functions

 `plan` Plan path between two states `copy` Create copy of planner object

## Examples

collapse all

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

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

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

## Version History

Introduced in R2019b