plannerLineSpec.start

Load Occupancy Map

Load a probability occupancy grid into the MATLAB® workspace.

load("exampleMaps.mat","simpleMap")

Create an occupancy map from the input occupancy grid.

map = occupancyMap(simpleMap);

Define the lower and upper limits of the state space variables x, y, and theta from the occupancy map.

x = map.XWorldLimits;
y =  map.YWorldLimits;
theta = [-pi pi];

Specify the start state and the goal state for path planning.

start = [2 5 0];
goal = [22 22 0];

Display the occupancy map.

figure
show(map)
hold on

Plot Start and Goal States

Use the plannerLineSpec.start and plannerLineSpec.goal functions to plot the start and goal states, respectively.

plot(start(1),start(2),plannerLineSpec.start{:})
plot(goal(1),goal(2),plannerLineSpec.goal{:})

Configure Bi-RRT Path Planner and Find Path Between Two States

Create a state space SE(2) object using the specified state space variables.

stateSpace = stateSpaceSE2([x; y; theta]);

Check the validity of the states in the input state space by using a state validator.

stateValidator = validatorOccupancyMap(stateSpace,Map=map);

Configure the Bi-RRT path planner.

planner = plannerBiRRT(stateSpace,stateValidator);

Compute the path between the start point and the goal point using Bi-RRT path planner.

[path,info] = plan(planner,start,goal)

path = 
  navPath with properties:

      StateSpace: [1x1 stateSpaceSE2]
          States: [306x3 double]
       NumStates: 306
    MaxNumStates: Inf

info = struct with fields:
          IsPathFound: 1
             ExitFlag: 1
        NumIterations: 294
        StartTreeData: [569x3 double]
         GoalTreeData: [710x3 double]
    StartTreeNumNodes: 189
     GoalTreeNumNodes: 236

Plot Search Trees

Use the plannerLineSpec.tree function to plot the forward search tree.

startTree = plannerLineSpec.tree;
plot(info.StartTreeData(:,1),info.StartTreeData(:,2),startTree{:})

Use the plannerLineSpec.goalTree function to plot the search tree from the goal state to the start state. Set the line color for plotting the search tree from goal to start to cyan.

goalTree = plannerLineSpec.goalTree(color="cyan");
plot(info.GoalTreeData(:,1),info.GoalTreeData(:,2),goalTree{:})
legend

Plot Computed Path

Use the plannerLineSpec.path function to plot the derived path.

plot(path.States(:,1),path.States(:,2),plannerLineSpec.path{:})
legend(Position=[0.8 0.11 0.2 0.2])

Load Occupancy Map

Load a probability occupancy grid into the MATLAB® workspace.

load("exampleMaps.mat","simpleMap")

Create an occupancy map from the input occupancy grid.

map = occupancyMap(simpleMap);

Define the lower and upper limits of the state space variables x, y, and theta from the occupancy map.

x = map.XWorldLimits;
y =  map.YWorldLimits;
theta = [-pi pi];

Display the occupancy map.

figure
show(map)
hold on

Specify the start state and the goal state for path planning.

start = [2 5 0];
goal = [22 22 0];

Plot Start and Goal States

Use the plannerLineSpec.start and plannerLineSpec.goal functions to plot the start and goal states, respectively.

plot(start(1),start(2),plannerLineSpec.start{:})

Set the color for plotting the goal state to red and plot the goal state.

goalState = plannerLineSpec.goal(MarkerEdgeColor="red",MarkerFaceColor="red");
plot(goal(1),goal(2),goalState{:})

Configure RRT Path Planner

Create a state space SE(2) object using the specified state space variables.

stateSpace = stateSpaceSE2([x; y; theta]);

Check the validity of the states in the input state space by using a state validator.

stateValidator = validatorOccupancyMap(stateSpace,Map=map);

Configure the RRT path planner.

planner = plannerRRT(stateSpace,stateValidator);

Find Path Between Two States

Compute the path between the start point and the goal point using the RRT path planner.

[path,info] = plan(planner,start,goal)

path = 
  navPath with properties:

      StateSpace: [1x1 stateSpaceSE2]
          States: [320x3 double]
       NumStates: 320
    MaxNumStates: Inf

info = struct with fields:
      IsPathFound: 1
         ExitFlag: 1
         NumNodes: 1192
    NumIterations: 1331
         TreeData: [3578x3 double]

Plot Search Trees

Use the plannerLineSpec.tree function to plot the forward search tree. Set the color for plotting the search tree to green.

In addition, set the legend label to "Forward search tree".

startTree = plannerLineSpec.tree(Color="green",DisplayName="Forward search tree");
plot(info.TreeData(:,1),info.TreeData(:,2),startTree{:})

Plot Computed Path

Use the plannerLineSpec.path function to plot the derived path.

plot(path.States(:,1),path.States(:,2),plannerLineSpec.path{:})
legend(Position=[0.6 0.11 0.2 0.2])