Create an RRT planner for geometric planning
plannerRRT object creates a rapidly-exploring random tree
(RRT) planner for solving geometric planning problems. RRT is a tree-based motion planner that
builds a search tree incrementally from samples randomly drawn from a given state space. The
tree eventually spans the search space and connects the start state to the goal state. The
general tree growing process is as follows:
The planner samples a random state xrand in the state space.
The planner finds a state xnear that is already in the search tree and is closest (based on the distance definition in the state space) to xrand.
The planner expands from xnear towards xrand, until a state xnew is reached.
Then new state xnew is added to the search tree.
For geometric RRT, the expansion and connection between two states can be found analytically without violating the constraints specified in the state space of the planner object.
creates an RRT planner from a state space object,
planner = plannerRRT(
stateSpace, and a
state validator object,
stateVal. The state space of
stateVal must be the same as
stateVal also sets the
StateValidator properties of
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.
MaxIterations — Maximum number of iterations
1e4 (default) | positive integer
Maximum number of iterations, specified as a positive integer.
MaxConnectionDistance — Maximum length of motion
0.1 (default) | positive scalar
Maximum length of a motion allowed in the tree, specified as a scalar.
GoalReachedFcn — Callback function to evaluate whether goal is reached
@nav.algs.checkIfGoalIsReached | function handle
Callback function to evaluate 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)
planner— The created planner object, specified as
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
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.
GoalBias — Probability of choosing goal state during state sampling
0.05 (default) | real scalar in [0,1]
Probability of choosing the goal state during state sampling, specified as a real
scalar in [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
Plan Path Between Two States
Create a state space.
ss = stateSpaceSE2;
occupanyMap-based state validator using the created state space.
sv = validatorOccupancyMap(ss);
Create an occupany map from an example map and set map resolution as 10 cells/meter.
load exampleMaps 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 the path planner and increase max connection distance.
planner = plannerRRT(ss,sv); planner.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'); % for repeatable result [pthObj,solnInfo] = plan(planner,start,goal);
Visualize the results.
show(map) hold on plot(solnInfo.TreeData(:,1),solnInfo.TreeData(:,2),'.-'); % tree expansion plot(pthObj.States(:,1),pthObj.States(:,2),'r-','LineWidth',2) % draw path
 S.M. Lavalle and J.J. Kuffner. "Randomized Kinodynamic Planning." The International Journal of Robotics Research. Vol. 20, Number 5, 2001, pp. 378 – 400.
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.