Main Content

plan

Plan path between start and goal states on roadmap

    Description

    path = plan(planner,startState,goalState) returns an obstacle-free path as a navPath object between the start state and the goal state within a roadmap that contains a network graph of connected nodes.

    example

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

    Examples

    collapse all

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

    map = load("exampleMaps.mat").simpleMap;
    map = occupancyMap(map,10);

    Create a state space and update the state space bounds to be the same as the map limits.

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

    Create a state validator with stateSpaceSE2 using the map and set the validation distance.

    sv = validatorOccupancyMap(ss,Map=map);
    sv.ValidationDistance = 0.01;

    Create a plannerPRM object.

    planner = plannerPRM(ss,sv);

    Retrieve graph as a digraph object.

    graph = graphData(planner);

    Extract nodes and edges from graph.

    edges = table2array(graph.Edges);
    nodes = table2array(graph.Nodes);

    Specify the start and goal states.

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

    Plot map and graph.

    show(sv.Map)
    hold on
    plot(nodes(:,1),nodes(:,2),"*","Color","b","LineWidth",2)
    for i = 1:size(edges,1)
        % Samples states at distance 0.02 meters.
        states = interpolate(ss,nodes(edges(i,1),:), ...
                             nodes(edges(i,2),:),0:0.02:1);
        plot(states(:,1),states(:,2),"Color","b")
    end
    plot(start(1),start(2),"*","Color","g","LineWidth",3)
    plot(goal(1),goal(2),"*","Color","r","LineWidth",3)

    Plan a path with default settings. Set the rng seed for repeatability.

    rng(100,"twister");
    [pthObj, solnInfo] = plan(planner,start,goal);

    Visualize the results.

    if solnInfo.IsPathFound
        interpolate(pthObj,1000);
        plot(pthObj.States(:,1),pthObj.States(:,2), ...
             "Color",[0.85 0.325 0.098],"LineWidth",2)
    else
        disp("Path not found")
    end
    hold off

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

    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 probabilistic roadmap path planner with increased maximum connection distance.

    planner = plannerPRM(ss,sv);

    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)

    Input Arguments

    collapse all

    Path planner, specified as a plannerPRM object.

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

    Example: [1 1 pi/6]

    Data Types: single | double

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

    Example: [2 2 pi/3]

    Data Types: single | double

    Output Arguments

    collapse all

    Planned path information, returned as a navPath object.

    Solution information, returned as a structure. The structure contains the field:

    FieldDescription
    IsPathFound

    Indicates whether a path is found. It returns as 1 if a path is found. Otherwise, it returns 0.

    Data Types: struct

    Extended Capabilities

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

    Version History

    Introduced in R2022a

    See Also

    Objects

    Functions