Main Content

plan

Plan path between start and goal states on roadmap

Since R2022a

collapse all in page

    Syntax

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

    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.

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

    example

    Examples

    collapse all

    Open Live Script

    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, xlabel X [meters], ylabel Y [meters] contains 103 objects of type image, line.

    Open Live Script

    Configure the random number generator for repeatable result.

    rng(1,"twister");

    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];

    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)

    Figure contains an axes object. The axes object with title Occupancy Map, xlabel X [meters], ylabel Y [meters] contains 4 objects of type patch, scatter, line.

    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