predict

Predict state samples using MPNet

Since R2023b

Syntax

statePred = predict(mpnet,start,goal)

statePred = predict(mpnet,start,goal,environment)

Description

statePred = predict(mpnet,start,goal) predicts the next state sample between a start state and goal state in a single environment. For this syntax, the Motion Planning Networks (MPNet) must have been trained on only one environment using random start poses and goal poses. The input test environment must be the same as the training environment.

statePred = predict(mpnet,start,goal,environment) predicts the next state sample between a start state and goal state in the specified test environment by using a pretrained MPNet. In this case, the MPNet must have been trained in multiple environments for random start poses and goal poses. For this syntax, the input test environment can be a different environment than any on which the planner has been trained. However,

• The grid size of the test environment must be same as that of the environments used for training.

• The test environment must have the same data distribution as the training environments.

Note

To find all samples between a start pose and goal pose, you must iteratively use the predict function. In each iteration, the predicted state becomes the new start pose for predicting the next state.

Note

To run this function, you will require the Deep Learning Toolbox™.

Examples

Predict State Samples Using MPNet Trained on Single Environment

Load a data file containing a pretrained MPNet into MATLAB® workspace. The MPNet has been trained on a single map for a Dubins vehicle. You can use this pretrained MPNet to find state samples between any start pose and goal pose on the map used for training.

inputData = load("officeMapTrainedMPNET.mat")

inputData = struct with fields:
      encodingSize: 0
       lossWeights: [100 100 10]
        officeArea: [1x1 occupancyMap]
    trainedNetwork: [1x1 dlnetwork]

Read the map used for training the network.

map = inputData.officeArea;

Specify the limits of the state space variables corresponding to the input map.

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

Configure the mpnetSE2 object to use the pretrained MPNet for predicting state samples between a start pose and goal pose. Set the EncodingSize value to 0.

mpnet = mpnetSE2(Network=inputData.trainedNetwork,StateBounds=stateBounds,EncodingSize=0);

Specify the start pose and goal pose.

start = [0 -6 0];
goal = [10 3 0];

Display the input map, and plot the start and goal poses.

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

Use the predict function iteratively to find state samples between the start and the goal poses. Use the sum of the absolute distance between the predicted state and the goal pose as the criterion for computing samples up to the goal point. Specify the distance value threshold as 1. For each new iteration, consider the predicted state sample as the new start pose. Compute the state samples until the distance between the predicted state sample and the goal pose is greater than 1. Plot the predicted state samples.

goalDistance = abs(start(1)-goal(1)) + abs(start(2)-goal(2));
while goalDistance > 1
    statenext = predict(mpnet,start,goal,map);
    start = statenext;
    goalDistance = abs(start(1)-goal(1)) + abs(start(2)-goal(2));
    plot(statenext(1),statenext(2),plannerLineSpec.state{:})
    legend("off")
end

Predict State Samples Using MPNet Trained on Multiple Environments

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 a wall thickness of 1 grid cell. You can use this pretrained MPNet to find state space samples on a random maze map with same specifications as those in the training data set.

pretrainedData = load("mazeMapTrainedMPNET")

pretrainedData = 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 a random maze map for testing the pretrained MPNet. The grid size ($\mathrm{MapSize}\times \mathrm{MapResolution}$) must be the same as that of the training data.

rng(100,"twister") % Set the seed
testMap = mapMaze(5,1,MapSize=[20 20],MapResolution=1.25);

Specify the start pose and goal pose.

start = [1 3 0];
goal = [18 18 0];

Specify the limits of the state space variables defining the test map.

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

Configure the mpnetSE2 object to use the pretrained MPNet for predicting state samples on a random map.

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

Display the input test map, and plot the start and goal poses.

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

Use the predict function iteratively to find state samples between the start and the goal poses. Use the sum of the absolute distance between the predicted state and the goal pose as the criterion for computing samples up to the goal point. Specify the distance value threshold as 1. For each new iteration, consider the predicted state sample as the new start pose. Compute the state samples until the distance between the predicted state sample and the goal pose is greater than 1. Plot the predicted state samples.

goalDistance = abs(start(1)-goal(1)) + abs(start(2)-goal(2));
while goalDistance > 1
    statenext = predict(mpnet,start,goal,testMap);
    start = statenext;
    goalDistance = abs(start(1)-goal(1)) + abs(start(2)-goal(2));
    plot(statenext(1),statenext(2),plannerLineSpec.state{:})
    legend("off")
end

Input Arguments

mpnet — Motion planning networks

mpnetSE2 object

Motion planning networks, specified as an mpnetSE2 object.

start — Start pose

three element vector

Start pose, specified as a three-element vector of the form [x y θ].

Data Types: double

goal — Goal pose

three-element vector

Goal state, specified as a three-element vector of the form [x y θ].

Data Types: double

environment — Encoded environment

binaryOccupancyMap object | occupancyMap object

Encoded environment, specified as a binaryOccupancyMap or occupancyMap object. The encoded environment is a test map of dimensions same as that of the maps used for training the network.

Output Arguments

statePred — Predicted next state

three-element vector

Predicted next state, returned as a three-element vector of the form [x, y, θ].

Data Types: double

References

[1] Qureshi, Ahmed Hussain, Yinglong Miao, Anthony Simeonov, and Michael C. Yip. “Motion Planning Networks: Bridging the Gap Between Learning-Based and Classical Motion Planners.” IEEE Transactions on Robotics 37, no. 1 (February 2021): 48–66. https://doi.org/10.1109/TRO.2020.3006716.

Extended Capabilities

C/C++ Code Generation
Generate C and C++ code using MATLAB® Coder™.

Version History

Introduced in R2023b