plan

Plan path between start and goal states on roadmap

Since R2022a

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.

Examples

Plan Obstacle-Free Path Using Probabilistic Roadmap Path Planner

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

Plan Path Through 3-D Occupancy Map Using Probabilistic Roadmap Planner

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)

Input Arguments

planner — Path planner

plannerPRM object

Path planner, specified as a plannerPRM object.

startState — Start state of the path

N-element real-valued row vector

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

goalState — Goal state of the path

N-element real-valued row vector

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

path — Planned path information

navPath object

Planned path information, returned as a navPath object.

solutionInfo — Solution information

structure

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

Field	Description
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