plan

Find obstacle-free path between two states

Since R2024a

Syntax

path = plan(planner,start,goal)

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

Description

path = plan(planner,start,goal) computes an obstacle-free path between start and goal states using MPNet (Motion Planning Networks) path planner.

[path,solutionInfo] = plan(planner,start,goal) also returns additional information about the states in the computed path as a structure.

Examples

Plan Path Between Two States Using MPNet Path Planner

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

Plan Path in Dubins State Space Using MPNet Path Planner

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 compute paths between any start state and goal state 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];

Create a Dubins state space and set the minimum turning radius to 0.2.

stateSpace = stateSpaceDubins(stateBounds);
stateSpace.MinTurningRadius = 0.2;

Create a state validator object to use for motion planning. Set the validation distance to 0.1 m.

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

Set the seed value to generate repeatable results.

rng(100,"twister");

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

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

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)

pathObj = 
  navPath with properties:

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

solutionInfo = struct with fields:
        IsPathFound: 1
      LearnedStates: [23x3 double]
       BeaconStates: [4x3 double]
    ClassicalStates: [0x3 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");

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

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 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: [23x3 double]
       BeaconStates: [4x3 double]
    ClassicalStates: [0x3 double]

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

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

Plot the learned states and beacon states along with the computed path. From the figure, you can infer that the neural path planning approach successfully computed a collision-free path in two places where beacon states are present. Hence, the MPNet path planner did not resort to the classical RRT* path planning approach. The final states of the planned path are the ones returned by the neural path planning approach.

figure
show(map)
hold on
plot(pathObj.States(:,1),pathObj.States(:,2),ppath{:})
plot(solutionInfo.LearnedStates(:,1),solutionInfo.LearnedStates(:,2),lstate{:})
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

Input Arguments

planner — MPNet path planner

plannerMPNET object

MPNet path planner, specified as a plannerMPNET object.

start — Start state

three-element vector

Start state, specified as a three-element vector of the form [x y theta]. x and y specify the position in meters, and theta specifies the orientation angle in radians.

Data Types: single | double

goal — Goal state

three-element vector

Goal state, specified as a three-element vector of the form [x y theta]. x and y specify the position in meters, and theta specifies the orientation angle in radians.

Data Types: single | double

Output Arguments

path — Obstacle-free path

navPath object

Obstacle-free path, returned as a navPath object.

solutionInfo — Information about path and states in the path

structure

Information about the path and states in the computed path, returned as a structure with the following fields:

Fields of solutionInfo

Fields	Description
IsPathFound	Indicates whether a path is found. The value of this field is returned as 1, if a path is found. Otherwise, the value returned is 0.
LearnedStates	Contains states computed using bidirectional neural path planning. This field is returned as a M-by-3 matrix. Each row in the matrix is of the form [x y theta]. x and y specify the position in meters, and theta specifies the orientation angle in radians. M denotes the number of states computed using neural path planning. If no states were computed, the function returns an empty value for this field.
BeaconStates	Contains consecutive states in the free space that were not connectable due to collisions with an obstacle. This field is returned as a P-by-3 matrix. Each row in the matrix is of the form [x y theta]. x and y specify the position in meters, and theta specifies the orientation angle in radians. If there were no beacon states, then the function returns an empty value for this field. For information about beacons states, see the Algorithms section.
ClassicalStates	Contains states computed using classical path planning. This field is returned as a N-by-3 matrix. Each row in the matrix is of the form [x y theta]. x and y specify the position in meters, and theta specifies the orientation angle in radians. N denotes the number of states computed using classical path planning. If no states were computed, the function returns an empty value for this field.

Algorithms

The plan function implements the MPNet path planning algorithm presented in [1]. This section gives a brief overview of the algorithm.

• The MPNet path planner computes a coarse path consisting of learned states that lie along the global path between start and goal states. If the path connecting any learned states is not collision-free, the MPNet path planner performs replanning only for that segment in the coarse path.

• Two consecutive learned states that are in the free space and cannot be connected without colliding with an obstacle are termed beacon states. The MPNet path planner selects these beacon states as new start and goal points and attempts to find a collision-free path to connect them. This step is referred to as replanning. The path planner makes a limited number of attempts to find this path. If it is not able to find the path within a specified number of attempts, it switches to using a classical path planner. The MPNet path planner determines the maximum number of attempts it can make before switching to a classical planner by using the MaxNumLearnedStates property.

• At every planning and replanning step, the planner applies the lazy states contraction (LSC) approach to remove redundant states computed by the planner. This results in reduced computational complexity and helps the planner find the shortest collision-free path between the actual start and goal states.

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 R2024a