plan

Plan path between two states

Syntax

path = plan(planner,startState,goalState)

[path,solutionInfo] = plan(planner,startState,goalState)

Description

path = plan(planner,startState,goalState) returns a path from the start state to the goal state.

[path,solutionInfo] = plan(planner,startState,goalState) also returns solInfo that contains the solution information of the path planning.

Examples

Plan Path Between Two States

Create a state space.

ss = stateSpaceSE2;

Create an 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

Input Arguments

planner — Path planner

plannerRRT object | plannerRRTStar object

Path planner, specified as a plannerRRT object or a plannerRRTStar object.

startState — Start state of the path

N-element real-valued vector

Start state of the path, specified as an N-element real-valued vector. N is the dimension of the state space.

Example: [1 1 pi/6]

Data Types: single | double

goalState — Goal state of the path

N-element real-valued vector

Goal state of the path, specified as an N-element real-valued vector. N is the dimension of the state space.

Example: [2 2 pi/3]

Data Types: single | double

Output Arguments

path — Object that holds planned path information

navPath object

An object that holds the planned path information, returned as a navPath object.

solutionInfo — Solution Information

structure

Solution Information, returned as a structure. The fields of the structure are:

Fields of solutionInfo

Fields	Description
IsPathFound	Indicates whether a path is found. It returns as 1 if a path is found. Otherwise, it returns 0.
ExitFlag	Indicates the terminate status of the planner, returned as 1 — if the goal is reached 2 — if the maximum number of iterations is reached 3 — if the maximum number of nodes is reached
NumNodes	Number of nodes in the search tree when the planner terminates (excluding the root node).
NumIterations	Number of "extend" routines executed.
TreeData	A collection of explored states that reflects the status of the search tree when planner terminates. Note that NaN values are inserted as delimiters to separate each individual edge.
PathCosts	Contains the cost of the path at each iteration. Value for iterations when path has not reached the goal is denoted by a NaN. Size of the array is NumIterations-by-1. Last element contains the cost of the final path. Note This field is applicable only for plannerRRTStar object.

Data Types: structure

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2019b