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.```

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```

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

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`

Version History

Introduced in R2022a