Main Content

plan

Plan path from start to goal state

    Description

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

    example

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

    Examples

    collapse all

    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

    Figure contains an axes object. The axes object with title Occupancy Grid contains 3 objects of type image, line.

    Input Arguments

    collapse all

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

    Initial state of the path, specified as an s-element vector. s is the number of state variables in the state space. For example, a robot in the SE(2) space has a state vector of [x y theta].

    Example: [1 1 pi/6]

    Data Types: single | double

    Goal state of the path, specified as an s-element vector. s is the number of state variables in the state space. For example, a robot in the SE(2) space has a state vector of [x y theta].

    Example: [2 2 pi/3]

    Data Types: single | double

    Output Arguments

    collapse all

    Planned path information, returned as a navPathControl object.

    Solution Information, returned as a structure. The structure contains these fields:

    FieldDescription
    IsPathFoundIndicates whether a path is found. It returns as 1 if a path is found. Otherwise, it returns as 0.
    ExitFlag

    Indicates the terminate status of the planner, returned as one of these options:

    • 1 — Goal successfully reached

    • 2 — Exceeded maximum number of iterations

    • 3 — Exceeded maximum number of nodes

    • 4 — Exceeded maximum planning time

    NumTreeNodeNumber of nodes in the search tree when the planner terminates excluding the root node.
    NumIterationsNumber of extension routines executed.
    PlanningTimeElapsed time while planning, returned as a scalar in seconds.
    TreeInfoCollection of explored states that reflects the status of the search tree when the planner terminates. Note that the planner inserts NaN values as delimiters to separate each individual edge.

    Data Types: structure

    Introduced in R2021b