plannerMPNET

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]

Set the seed value to generate repeatable results.

rng(10,"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=[10 10],MapResolution=2.5);

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

Generate multiple random start and goal states by using the sampleStartGoal function.

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

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,:);

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

Compute Path Using 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.

planner = plannerMPNET(stateValidator,mpnet);

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

[pathObj,solutionInfo] = plan(planner,start,goal);

Display Planned Path

Display the navPath object returned by the MPNet path planner. The number of states in the planned path and the associated state vectors are specified by the NumStates and States properties of the navPath object, respectively.

disp(pathObj)

  navPath with properties:

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

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 properties to display the computed path by using the plannerLineSpec.path function.

ppath = plannerLineSpec.path(LineWidth=1,Marker="o",MarkerSize=8,MarkerFaceColor="white",DisplayName="Planned path");

Plot the planned path.

figure
show(map)
hold on
plot(pathObj.States(:,1),pathObj.States(:,2),ppath{:})
plot(start(1),start(2),sstate{:})
plot(goal(1),goal(2),gstate{:})
legend(Location="bestoutside")
hold off

Display Additional Data

Display the solutionInfo structure returned by the MPNet path planner. This structure stores the learned states, classical states, and beacon states computed by the MPNet path planner. If any of these three types of states are not computed by the MPNet path planner, the corresponding field value is set to empty.

disp(solutionInfo)

        IsPathFound: 1
      LearnedStates: [50x3 double]
       BeaconStates: [2x3 double]
    ClassicalStates: [9x3 double]

Set the line and marker properties to display the learned states, classical states, and beacon states by using the plannerLineSpec.state.

lstate = plannerLineSpec.state(DisplayName="Learned states",MarkerSize=3);
cstate = plannerLineSpec.state(DisplayName="Classical states",MarkerSize=3,MarkerFaceColor="green",MarkerEdgeColor="green");
bstate = plannerLineSpec.state(MarkerEdgeColor="magenta",MarkerSize=7,DisplayName="Beacon states",Marker="^");

Plot the learned states, classical states, and beacon states along with the computed path. From the figure, you can infer that the neural path planning approach was unable to compute a collision-free path where beacon states are present. Hence, the MPNet path planner resorted to the classical RRT* path planning approach. The final states of the planned path constitutes states returned by neural path planning and classical path planning approaches.

figure
show(map)
hold on
plot(pathObj.States(:,1),pathObj.States(:,2),ppath{:})
plot(solutionInfo.LearnedStates(:,1),solutionInfo.LearnedStates(:,2),lstate{:})
plot(solutionInfo.ClassicalStates(:,1),solutionInfo.ClassicalStates(:,2),cstate{:})
plot(solutionInfo.BeaconStates(:,1),solutionInfo.BeaconStates(:,2),bstate{:})
plot(start(1),start(2),sstate{:})
plot(goal(1),goal(2),gstate{:})
legend(Location="bestoutside")
hold off

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]

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: [1x1 stateSpaceSE2]
          States: [6x3 double]
       NumStates: 6
    MaxNumStates: Inf

solutionInfo1 = struct with fields:
        IsPathFound: 1
      LearnedStates: [50x3 double]
       BeaconStates: [2x3 double]
    ClassicalStates: [20x3 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: [1x1 stateSpaceSE2]
          States: [5x3 double]
       NumStates: 5
    MaxNumStates: Inf

solutionInfo2 = struct with fields:
        IsPathFound: 1
      LearnedStates: [50x3 double]
       BeaconStates: [2x3 double]
    ClassicalStates: [7x3 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

Load and Visualize Training Data set

Load the data set from a .mat file. The data set contains 400,000 different paths for 200 maze map environments. The data set has been generated for a pre-defined parameters of mazeMap function. The first column of the data set contains the maps and the second column contains the optimal paths for randomly sampled start, goal states from the corresponding maps. The size of data set is 75MB.

% Download and extract the maze map dataset
if ~exist("mazeMapDataset.mat","file")
    datasetURL = "https://ssd.mathworks.com/supportfiles/nav/data/mazeMapDataset.zip";
    websave("mazeMapDataset.zip", datasetURL);
    unzip("mazeMapDataset.zip")
end

% Load the maze map dataset
load("mazeMapDataset.mat","dataset","stateSpace")
head(dataset)

             Map                  Path     
    ______________________    _____________

    1×1 binaryOccupancyMap    {14×3 single}
    1×1 binaryOccupancyMap    { 8×3 single}
    1×1 binaryOccupancyMap    {24×3 single}
    1×1 binaryOccupancyMap    {23×3 single}
    1×1 binaryOccupancyMap    {17×3 single}
    1×1 binaryOccupancyMap    {15×3 single}
    1×1 binaryOccupancyMap    { 7×3 single}
    1×1 binaryOccupancyMap    {10×3 single}

The data set was generated using the examplerHelperGenerateDataForMazeMaps helper function. The examplerHelperGenerateDataForMazeMaps helper function uses the mapMaze function to generate random maze maps of size 10-by-10 and resolution 2.5 m. The width and wall thickness of the maps was set to 5m and 1 m, respectively.

passageWidth = 5;
wallThickness = 1;
map = mapMaze(passageWidth,wallThickness,MapSize=[10 10],MapResolution=2.5)

Then, the start states and goal states are randomly generated for each map. The optimal path between the start and goal states are computed using plannerRRTStar path planner. The ContinueAfterGoalReached and MaxIterations parameters are set to true and 5000, respectively, to generate the optimal paths.

planner = plannerRRTStar(stateSpace,stateValidator); % Uses default uniform state sampling
planner.ContinueAfterGoalReached = true; % Optimize after goal is reached
planner.MaxIterations = 5000; % Maximum iterations to run the planner

Visualize a few random samples from the training data set. Each sample contains a map and the optimal path generated for a given start and goal state.

figure
for i=1:4
    subplot(2,2,i)
    ind = randi(height(dataset)); % Select a random sample
    map = dataset(ind,:).Map; % Get map from Map column of the table
    pathStates = dataset(ind,:).Path{1}; % Get path from Path column of the table
    start = pathStates(1,:);
    goal = pathStates(end,:);

    % Plot the data
    show(map);
    hold on
    plot(pathStates(:,1),pathStates(:,2),plannerLineSpec.path{:})
    plot(start(1),start(2),plannerLineSpec.start{:})
    plot(goal(1),goal(2),plannerLineSpec.goal{:})
end
legend(Location="bestoutside")

You can modify the helper function to generate new maps and train the MPNet from scratch. The dataset generation may take a few days depending upon CPU configuration and the number of maps you want to generate for training. To accelerate dataset generation, you can use Parallel Computing Toolbox™.

Create Motion Planning Networks

Create Motion Planning Networks (MPNet) object for SE(2) state space using mpnetSE2. The mpnetSE2 object loads a preconfigured MPNet that you can use for training. Alternatively, you can use the mpnetLayers function to create a MPNet with different number of inputs and hidden layers to train on the data set.

mpnet = mpnetSE2;

Set the StateBounds, LossWeights, and EncodingSize properties of the mpnetSE2 object. Set the StateBounds using the StateBounds property of the stateSpace object.

mpnet.StateBounds = stateSpace.StateBounds;

Specify the weights for each state space variables using the LossWeights property of the mpnetSE2 object. You must specify the weights for each state space variable $\mathit{x}$, $\mathit{y}$, and $\theta$ of SE(2) state space. For a SE(2) state space, we do not consider the robot kinematics such as the turning radius. Hence, you can assign zero weight value for the $\theta$ variable.

mpnet.LossWeights = [100 100 0];

Specify the value for EncodingSize property of the mpnetSE2 object as [9 9]. Before training the network, the mpnetSE2 object encodes the input map environments to a compact representation of size 9-by-9.

mpnet.EncodingSize = [9 9];

Prepare Data for Training

Split the dataset into train and test sets in the ratio 0.8:0.2. The training set is used to train the Network weights by minimizing the training loss, validation set is used to check the validation loss during the training.

split = 0.8;
trainData = dataset(1:split*end,:);
validationData = dataset(split*end+1:end,:);

Prepare the data for training by converting the raw data containing the maps and paths into a format required to train the MPNet.

dsTrain = mpnetPrepareData(trainData,mpnet);
dsValidation = mpnetPrepareData(validationData,mpnet);

Visualize prepared dataset. The first column of sample contains the encoded map data, encoded current state and goal states. The second column contains the encoded next state. The encoded state is computed as $[x,y,\cos (\theta ),\sin (\theta \left)\right]$ and normalized to the range of [0, 1].

preparedDataSample = read(dsValidation);
preparedDataSample(1,:)

ans=1×2 cell array
    {[0.2607 0.4112 0.6846 0.9647 0.9138 0.5757 0.4883 1.3733e-04 0.0549 0.1646 0 0.1646 0.1646 0.1646 0.1646 0.1646 0.0549 0.1646 0.8244 0.0870 0.9383 0.8244 0.8244 0.8244 0.8244 0.1646 0.1646 0.8244 0.0870 0.9020 0.0094 0.0870 0.0870 0.0870 3.9316e-16 0.1646 0.8244 0.0870 0.9020 0.0094 0.9020 0.9043 0.9383 0.1646 0.1646 0.8244 0.0870 0.9020 0.0094 0.9020 0.0870 0.8244 0.1646 0.1646 1 0.9043 0.9020 0.0094 0.9020 0.0870 0.8244 0.1646 0.1646 0.8313 0.0870 0.0870 0.0094 0.9020 0.0870 0.8244 0.1646 0.1646 0.9333 0.8244 0.8244 0.8244 0.9383 0.0870 0.8244 0.1646 0.0549 0.1646 0.1646 0.1646 0.1646 0.1646 2.6928e-16 0.1646 0.0549]}    {[0.2720 0.4130 0.6786 0.9670]}

Train Deep Learning Network

Use the trainnet function to train the MPNet. Training this network might take a long time depending on the hardware you use. Set the doTraining value to true to train the network.

doTraining = false;

Specify trainingOptions (Deep Learning Toolbox) for training the deep learning network:

You can consider the training to be successful once the training loss and validation loss converge close to zero.

if doTraining
    options = trainingOptions("adam",...
        MiniBatchSize=2048,...
        MaxEpochs=50,...
        Shuffle="every-epoch",...
        ValidationData=dsValidation,...
        ValidationFrequency=2000,...
        Plots="training-progress");

    % Train network
    [net,info] = trainnet(dsTrain,mpnet.Network,@mpnet.loss,options);

    % Update Network property of mpnet object with net
    mpnet.Network = net;
end

You can save the trained network and the details of input map environment to a .mat file and use it to perform motion planning. In the rest of example, you will use a pretrained MPNet to directly perform motion planning on an unknown map environment.

Load a .mat file containing the pretrained network. The network has been trained on various, randomly generated maze maps stored in the mazeMapDataset.mat file. The .mat file contains the trained network and details of the maze maps used for training the network.

if ~doTraining
    data = load("mazeMapTrainedMPNET.mat")
    mpnet.Network = data.trainedNetwork;
end

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]

Perform Motion Planning Using Trained MPNet

Create a random maze map for testing the trained MPNet for path panning. The grid size (MapSize×MapResolution) of the test map must be same the as that of the maps used for training the MPNet.

Click the Run button to generate a new map.

 
mazeParams = data.mazeParams;
map = mapMaze(mazeParams{:});
figure
show(map)

Create a state validator object.

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

Create a MPNet path planner using the state validator and the MPNet object as inputs.

planner = plannerMPNET(stateValidator,mpnet);

Generate multiple random start and goal states 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,:);

Plan path between the start and goal states using the trained MPNet.

[pathObj,solutionInfo] = plan(planner,start,goal)

pathObj = 
  navPath with properties:

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

solutionInfo = struct with fields:
        IsPathFound: 1
      LearnedStates: [32×3 double]
       BeaconStates: [2×3 double]
    ClassicalStates: [0×3 double]

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 properties to display the computed path by using the plannerLineSpec.path function.

ppath = plannerLineSpec.path(LineWidth=1,Marker="o",MarkerSize=8,MarkerFaceColor="white",DisplayName="Planned path");

Plot the planned path.

figure
show(map)
hold on
plot(pathObj.States(:,1),pathObj.States(:,2),ppath{:})
plot(start(1),start(2),sstate{:})
plot(goal(1),goal(2),gstate{:})
legend(Location="bestoutside")
hold off