stateSamplerMPNET

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: [3x2 double]
    trainedNetwork: [1x1 dlnetwork]

Create Maze Map for Sampling

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

rng(10,"twister")
map = mapMaze(5,1,MapSize=[12 12],MapResolution=2.0833);

Specify the start pose and goal pose.

start = [2 10 0];
goal = [5 4 0];

Visualize the input map.

figure
show(map)
hold on
plot(start(1),start(2),plannerLineSpec.start{:})
plot(goal(1),goal(2),plannerLineSpec.goal{:})
legend(Location="bestoutside")
hold off

Create MPNet State Sampler

Specify the state bounds, and create an SE(2) state space object.

x = map.XWorldLimits;
y = map.YWorldLimits;
theta = [-pi pi];
stateBounds = [x; y; theta];
stateSpace = stateSpaceSE2(stateBounds);

Configure the mpnetSE2 object to use the pretrained MPNet for predicting state samples on a random map. 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=stateBounds,EncodingSize=data.encodingSize);

Create an MPNet state sampler for computing state samples to use for motion planning. Specify the map environment, the start pose and the goal pose as inputs to the state sampler. The default value for MaxLearnedSamples is 50. This implies that the first 50 samples returned by the state sampler are learning-based samples, predicted using the pretrained MPNet.

sampler_orig = stateSamplerMPNET(stateSpace,mpnet,Environment=map,StartState=start,GoalState=goal)

sampler_orig = 
  stateSamplerMPNET with properties:

               StateSpace: [1x1 stateSpaceSE2]
    MotionPlanningNetwork: [1x1 mpnetSE2]
              Environment: [1x1 binaryOccupancyMap]
               StartState: [2 10 0]
                GoalState: [5 4 0]
        MaxLearnedSamples: 50
            GoalThreshold: 1

Sample State Space Using MPNet State Sampler

Generate 50 state samples that lie between the start pose and the goal pose using the MPNet state sampler. In this case, the MPNet state sampler generates all 50 samples by using the pretrained MPNet.

states_orig = sample(sampler_orig,50);

Create Copies of MPNet State Sampler and Vary Maximum Learned Samples

Create copies of the default MPNet state sampler object. Modify the maximum learned samples property of the sampler to study its impact on the sampling results.

Set the MaxLearnedSamples property to 10. In this case, the MPNet state sampler generates the first 10 samples using the pretrained MPNet and the remaining 40 samples using the uniform sampling approach.

sampler_2 = copy(sampler_orig);
sampler_2.MaxLearnedSamples = 10;

Set the MaxLearnedSamples property to 30. In this case, the MPNet state sampler generates the first 30 samples using the pretrained MPNet and the remaining 20 samples using the uniform sampling approach.

sampler_3 = copy(sampler_orig);
sampler_3.MaxLearnedSamples = 30;

Sample State Space Using Modified MPNet State Sampler

Use each of the modified MPNet state samplers to generate the same number of samples as that of the default sampler. In this case, use them each to generate 50 samples.

states_2 = sample(sampler_2,50);
states_3 = sample(sampler_3,50);

Plot and Compare Sampling Results

Create a figure with adequate space to display plots for all three state samplers.

fig1 = figure(Position=[0,0,1200,300]);

Plot the output of the MPNet state sampler with the maximum number of learned samples set to 10.

subplot(1,3,1,Parent=fig1)
show(map)
hold on
plot(start(1),start(2),plannerLineSpec.start{:})
plot(goal(1),goal(2),plannerLineSpec.goal{:})
plot(states_2(:,1),states_2(:,2),plannerLineSpec.state{:})
title("MaxLearnedSamples = 10")
hold off

Plot the output of the MPNet state sampler with the maximum number of learned samples set to 30.

subplot(1,3,2,Parent=fig1)
show(map)
hold on
plot(start(1),start(2),plannerLineSpec.start{:})
plot(goal(1),goal(2),plannerLineSpec.goal{:})
plot(states_3(:,1),states_3(:,2),plannerLineSpec.state{:})
title("MaxLearnedSamples = 30")
hold off

Plot the output of the default MPNet state sampler. The default value for the maximum number of learned samples is 50. As you increase the maximum number of learned samples to match the number of state samples to generate, the output samples become more focused along a path between the given start pose and the goal pose.

subplot(1,3,3,Parent=fig1)
show(map)
hold on
plot(start(1),start(2),plannerLineSpec.start{:})
plot(goal(1),goal(2),plannerLineSpec.goal{:})
plot(states_orig(:,1),states_orig(:,2),plannerLineSpec.state{:})
title("MaxLearnedSamples = 50 (default)")
hold off

Further Exploration

From the sampling results, you can infer that the MPNet state sampler returns optimal state samples if the maximum number of learned samples is equal to the maximum number of samples to be computed. However, if the MPNet is trained on a data set with fewer map environments or path samples, the prediction accuracy of the network decreases. The MPNet state sampler might return an insufficient number of learned samples between the start pose and goal pose, and you cannot use it for path planning. In such cases, the value of MaxLearnedSamples must be less than the number of samples required. This enables the MPNet state sampler to switch to the uniform sampling approach after a specified number of iterations, thus guaranteeing a path solution.

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: [3x2 double]
    trainedNetwork: [1x1 dlnetwork]

Create Maze Map for Motion Planning

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

rng(50,"twister")
map = mapMaze(5,1,MapSize=[20 20],MapResolution=1.25);

Specify the start pose and goal pose.

start = [2 8 0];
goal = [17 18 0];

Visualize the input map.

figure
show(map)
hold on
plot(start(1),start(2),plannerLineSpec.start{:})
plot(goal(1),goal(2),plannerLineSpec.goal{:})
legend(Location="bestoutside")
hold off

Create MPNet State Sampler

Specify the state bounds, and create an SE(2) state space object.

stateBounds = [0 20; 0 20; -pi pi];
stateSpace = stateSpaceSE2(stateBounds);

Configure the mpnetSE2 object to use the pretrained MPNet for predicting state samples on a random map. 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=stateBounds,EncodingSize=data.encodingSize);

Create an MPNet state sampler for computing state samples to use for motion planning. Specify the map environment, the start pose and goal pose as inputs to the state sampler.

stateSampler = stateSamplerMPNET(stateSpace,mpnet,Environment=map,StartState=start,GoalState=goal);

Validate State Space

Create a state validator to validate the samples generated by the sampler.

stateValidator = validatorOccupancyMap(stateSpace,Map=map);

Set the validation distance to 0.01.

stateValidator.ValidationDistance = 0.01;

Compute Path Using MPNet Sampling Method and RRT*

Compute an optimal path between the start pose and goal pose by using an RRT* path planner. Use the MPNet state sampler to sample the state space.

planner_MPNet = plannerRRTStar(stateSpace,stateValidator,StateSampler=stateSampler,MaxConnectionDistance=1);
[pathObj_MPNet,solutionInfo_MPNet] = plan(planner_MPNet,start,goal)

pathObj_MPNet = 
  navPath with properties:

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

solutionInfo_MPNet = struct with fields:
      IsPathFound: 1
         ExitFlag: 1
         NumNodes: 191
    NumIterations: 274
         TreeData: [575x3 double]
        PathCosts: [274x1 double]

Plot Results

Plot the results obtained using the MPNet state space sampling method. Plot the tree data and the path between the start pose and the goal pose.

figure
show(map)
hold on
plot(start(1),start(2),plannerLineSpec.start{:})
plot(goal(1),goal(2),plannerLineSpec.goal{:})
plot(solutionInfo_MPNet.TreeData(:,1),solutionInfo_MPNet.TreeData(:,2),plannerLineSpec.tree{:})
plot(pathObj_MPNet.States(:,1),pathObj_MPNet.States(:,2),plannerLineSpec.path{:})
title("Path Planning Using MPNet State Space Sampling")
legend(Location="bestoutside")
hold off