copy

Load Pretrained MPNet

Load a data file containing a pretrained MPNet into the MATLAB® workspace. The MPNet has been trained on various 2-D maze maps with widths and heights of 10 meters and resolutions of 2.5 cells per meter. Each maze map contains a passage width of 5 grid cells and wall thickness of 1 grid cell.

data = load("mazeMapTrainedMPNET.mat")

data = struct with fields:
      encodingSize: [9 9]
       lossWeights: [100 100 0]
        mazeParams: {[5]  [1]  'MapSize'  [10 10]  'MapResolution'  [2.5000]}
       stateBounds: [3×2 double]
    trainedNetwork: [1×1 dlnetwork]

Set the seed value to generate repeatable results.

rng(70,"twister")

Create Maze Map for Motion Planning

Create a random maze map for motion planning. The grid size ($\mathrm{MapSize}\times \mathrm{MapResolution}$) must be same as that of the maps used for training the MPNet.

map = mapMaze(5,1,MapSize=[12 12],MapResolution=2.0833);

Create State Validator

Create a state validator object to use for motion planning.

stateSpace = stateSpaceSE2(data.stateBounds);
stateValidator = validatorOccupancyMap(stateSpace,Map=map);
stateValidator.ValidationDistance = 0.1;

Select Start and Goal States

Select a start and goal state by using the sampleStartGoal function.

[startStates,goalStates] = sampleStartGoal(stateValidator,500);

Compute distance between the generated start and goal states.

stateDistance= distance(stateSpace,startStates,goalStates);

Select two states that are farthest from each other as the start and goal for motion planning.

[dist,index] = max(stateDistance);
start = startStates(index,:);
goal = goalStates(index,:);

Create MPNet Path Planner

Configure the mpnetSE2 object to use the pretrained MPNet for path planning. Set the EncodingSize property values of the mpnetSE2 object to that of the value used for training the network.

mpnet = mpnetSE2(Network=data.trainedNetwork,StateBounds=data.stateBounds,EncodingSize=data.encodingSize);

Create MPNet path planner using the state validator and the pretrained MPNet. Plan a path between the select start and goal states using the MPNet path planner.

planner{1} = plannerMPNET(stateValidator,mpnet);
[pathObj1,solutionInfo1] = plan(planner{1},start,goal)

pathObj1 = 
  navPath with properties:

      StateSpace: [1×1 stateSpaceSE2]
          States: [6×3 double]
       NumStates: 6
    MaxNumStates: Inf

solutionInfo1 = struct with fields:
        IsPathFound: 1
      LearnedStates: [50×3 double]
       BeaconStates: [2×3 double]
    ClassicalStates: [20×3 double]

Create Copy of MPNet Path Planner

Create a copy of the first instance of the MPNet path planner.

planner{2} = copy(planner{1});

Modify Classical Path Planning Approach

Specify bi-directional RRT (Bi-RRT) planner as the classical path planning approach for MPNet path planner. Set the maximum connection distance value to 1.

classicalPlanner = plannerBiRRT(stateSpace,stateValidator,MaxConnectionDistance=1);
planner{2}.ClassicalPlannerFcn = @classicalPlanner.plan;

Plan a path between the select start and goal states using the modified MPNet path planner.

[pathObj2,solutionInfo2] = plan(planner{2},start,goal)

pathObj2 = 
  navPath with properties:

      StateSpace: [1×1 stateSpaceSE2]
          States: [5×3 double]
       NumStates: 5
    MaxNumStates: Inf

solutionInfo2 = struct with fields:
        IsPathFound: 1
      LearnedStates: [50×3 double]
       BeaconStates: [2×3 double]
    ClassicalStates: [7×3 double]

Visualize Results

Set the line and marker properties to display the start and goal states by using the plannerLineSpec.start and plannerLineSpec.goal functions, respectively.

sstate = plannerLineSpec.start(DisplayName="Start state",MarkerSize=6);
gstate = plannerLineSpec.goal(DisplayName="Goal state",MarkerSize=6);

Set the line and marker properties to display the computed by using the plannerLineSpec.path function.

ppath1 = plannerLineSpec.path(LineWidth=1,Marker="o",MarkerSize=8,MarkerFaceColor="white",DisplayName="Path computed using RRT* for classical path planning");
ppath2 = plannerLineSpec.path(LineWidth=1,Marker="o",MarkerSize=8,MarkerFaceColor="red",DisplayName="Path computed using Bi-RRT for classical path planning");

Plot the computed paths. You can infer that the MPNet path planner offers better results when you use Bi-RRT path planner for classical path planning.

figure
show(map)
hold on
plot(pathObj1.States(:,1),pathObj1.States(:,2),ppath1{:})
plot(pathObj2.States(:,1),pathObj2.States(:,2),ppath2{:})
plot(start(1),start(2),sstate{:})
plot(goal(1),goal(2),gstate{:})
legend(Location="southoutside")
hold off