stateSamplerMPNET

MPNet state sampler for sampling-based motion planning

Since R2023b

Description

The stateSamplerMPNET object creates a state sampler for motion planning by using Motion Planning Networks (MPNet). State space sampling using MPNet is a deep-learning-based approach that use prior knowledge to compute samples for sampling-based path planners. The state sampler uses a pretrained MPNet to compute learned samples for a fixed number of iterations, after which the sampler uses uniform sampling approach. As a result, the sampler always guarantees a path solution, if a path solution exists.

Creation

Syntax

statesampler = stateSamplerMPNET(statespace,mpnet)

statesampler = stateSamplerMPNET(___,Name=Value)

Description

statesampler = stateSamplerMPNET(statespace,mpnet) creates a state sampler object using the specified state space and pretrained MPNet. The statespace and mpnet inputs set the values of the StateSpace and MotionPlanningNetwork properties, respectively.

statesampler = stateSamplerMPNET(___,Name=Value) specifies properties using one or more name-value arguments in addition to the input arguments in the previous syntax. You can specify the Environment, StartState, GoalState, MaxLearnedSamples, and GoalThreshold properties as name-value arguments.

Note

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

Properties

StateSpace — State space for sampling

stateSpaceSE2 object (default) | stateSpaceDubins object | stateSpaceReedsShepp object | object of a subclass from nav.StateSpace class

This property is read-only.

State space for sampling, specified as a stateSpaceSE2 object, stateSpaceDubins object, stateSpaceReedsShepp object, or an object of a subclass from nav.StateSpace class.

MotionPlanningNetwork — Pretrained MPNet

mpnetSE2 object

This property is read-only.

Pretrained MPNet, specified as an mpnetSE2 object.

Environment — Map environment

binaryOccupancyMap object | occupancyMap object

Map environment, specified as a binaryOccupancyMap or occupancyMap object.

StartState — Start state

[0 0 -pi] (default) | three-element vector

Start state, specified as a three-element vector of the form [x y θ]. x, y, and θ are the state space variables.

Data Types: single

GoalState — Goal state

[0 0 -pi] (default) | three-element vector

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

Data Types: single | double

MaxLearnedSamples — Maximum number of learned samples

50 (default) | positive integer

Maximum number of learned samples, specified as a positive integer. This property determines the number of samples that the state sampler must compute using the pretrained MPNet. This property sets the number of iterations after which the state sampler computes samples using the uniform sampling approach.

For information about how this property impacts the sampling results, see Perform Informed Sampling with MPNet.

Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

GoalThreshold — Distance threshold to goal

1 (default) | positive scalar

Distance threshold to the goal, specified as a positive scalar.

Data Types: single | double | int8 | int16 | int32 | int64 | uint8 | uint16 | uint32 | uint64

Object Functions

sample	Compute state samples using MPNet state sampler
copy	Create deep copy of MPNet state sampler object

Examples

Perform Informed Sampling with MPNet

This example shows how to perform informed sampling using a pretrained MPNet. Up to a certain number of iterations, an MPNet state sampler predicts samples using the pretrained MPNet. The samples predicted using the MPNet are biased towards a subspace that contains an optimal path solution. After the specified number of iterations, the sampler switches to the uniform sampling approach. You can change the number of iterations by specifying the MaxLearnedSamples property of the stateSamplerMPNET object.

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.

Sample State Space Using Pretrained MPNet and Perform Motion Planning

Perform motion planning using state space samples predicted by pretrained Motion Planning Networks (MPNet). This process includes these steps:

• Configure the mpnetSE2 object for state space sampling by using a pretrained MPNet.

• Use the stateSamplerMPNET object to sample an SE(2) state space. The state sampler takes the mpnetSE2 object as an input and uses the pretrained MPNet for predicting state samples between a start point and goal point.

• Perform path planning using a RRT* path planner. Use state samples predicted by the pretrained MPNet as seeds for path planning.

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

References

[1] Prokudin, Sergey, Christoph Lassner, and Javier Romero. “Efficient Learning on Point Clouds with Basis Point Sets.” In 2019 IEEE/CVF International Conference on Computer Vision Workshop (ICCVW), 3072–81. Seoul, Korea (South): IEEE, 2019. https://doi.org/10.1109/ICCVW.2019.00370.

[2] 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