# Hybrid Sampling Method for Motion Planning in Warehouse Environment

This example shows how to combine uniform sampling and Gaussian sampling approaches for motion planning in a warehouse scenario that contains narrow passages and wide spaces. In this example, you:

• Create a custom sampler that implements a hybrid approach by combining uniform sampling and Gaussian sampling approaches to sample an input state space.

• Use the computed state samples as seeds for motion planning. Compute an optimal path between a start state and goal state by using a probabilistic roadmap (PRM) path planner.

• Compare the results of the hybrid sampling approach with that of the uniform sampling and Gaussian sampling methods.

### Introduction

Robotics and autonomous systems commonly use sampling-based planning algorithms, such as probabilistic roadmap (PRM) and rapidly-exploring random trees (RRT), to find obstacle-free paths for a robot to travel from its starting position to a desired goal. The effectiveness of these approaches depends on the strategy used to sample the configuration space of the robot. Common sampling techniques include uniform and Gaussian sampling.

• Uniform sampling generates samples uniformly across the entire configuration space of the robot. This method is simple, easy to implement and works well for environments with wide spaces. However, the sampler requires a greater number of iterations to generate optimal samples for spaces with narrow passages. You can use the `stateSamplerUniform` function to perform uniform state sampling.

• Gaussian sampling uses a Gaussian distribution to generate samples for path planning. The sampler generates samples more concentrated along the obstacle boundary. As a result, the Gaussian sampling strategy is efficient for complex workspaces that contain high obstacle density or narrow regions. In wide spaces, because the samples are more concentrated along the boundary, Gaussian sampling can result in longer paths. You can use the `stateSamplerGaussian` function to perform Gaussian state sampling.

For sampling environments that have both narrow passages and wide spaces, you can use a hybrid approach that combines the uniform and Gaussian sampling strategies. This example uses an input environment consisting of a warehouse scenario that has narrow passages and wide spaces, to validate the performance of the hybrid approach. The figure shows the floor plan of the warehouse scenario used in this example. The scenario has two sorting stations, three storage areas, and off-limits areas like offices. The robot must pick packages from a sorting station and deliver them to a given location in the storage area without colliding with any obstacles.

The rest of the example explains how to create a custom sampler and use it with the PRM planner to compute optimal paths for the robot to navigate from a given start state to the goal state, without colliding with any obstacles.

Load a `binaryOccupancyMap` object, which contains the floor plan for the warehouse, into the workspace.

```data = load("warehouseMapData.mat","warehouseMap"); map = data.warehouseMap;```

Display and inspect the map. The shelves in the storage areas are the obstacles, or occupied areas, in the input map environment. The robot must not collide with the obstacles while navigating from one location to another in the given environment.

```figure show(map) title("Input Map")```

### Validate Input States

Define the lower and upper limits of the state space variables `x`, `y`, and `theta` from the occupancy map.

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

Create a state space SE(2) object using the specified state space variables. Check the validity of states in the input state space by using a state validator. Set the validation distance to 0.01.

```ss = stateSpaceSE2(StateBounds); sv = validatorOccupancyMap(ss,Map=map); sv.ValidationDistance = 0.01;```

### Create Custom State Sampler

Use the `helperStateSamplerHybrid` helper function to create a custom sampler that is a subclass of the `nav.StateSampler` class. Specify a uniform state sampler and Gaussian state sampler as inputs to the `helperStateSamplerHybrid` helper function. Specify the percentage of total samples to be generated using uniform sampling. The `helperStateSamplerHybrid` helper function generates the remaining samples using the Gaussian sampling approach. The values of standard deviation and maximum number of attempts properties of the Gaussian state sampler have been set experimentally. The custom sampler returns a hybrid sampler object that stores the uniform and Gaussian state samplers and their respective properties. Use the hybrid sampler object as an input to a PRM path planner for motion planning.

```uniformSampler = stateSamplerUniform(sv.StateSpace); gaussianSampler = stateSamplerGaussian(sv,StandardDeviation=[1 1 1],MaxAttempts=50); percentage = 50; hybridSampler = helperStateSamplerHybrid(uniformSampler,gaussianSampler,percentage);```

### Configure PRM Path Planner

To configure the PRM path planner, use the custom state sampler to sample the input state space, and set the `MaxNumNodes` property to `2000`. You can increase the value of the `MaxNumNodes` property to increase the chance of finding a path. However, this also increases the computation time for the path planner. Use the `tic` and `toc` commands to verify that the computation time of the planner is high, due to the maximum number of nodes considered.

```tic hybridPlanner = plannerPRM(ss,sv,StateSampler=hybridSampler,MaxNumNodes=2000); toc```
```Elapsed time is 272.319997 seconds. ```

Extract the generated roadmap. Extract the nodes and edges from the roadmap.

```digraphObj = hybridPlanner.graphData; edgesHybrid = table2array(digraphObj.Edges); nodesHybrid = table2array(digraphObj.Nodes);```

Interpolate states between the two ends of the edges to get the search tree.

```linksHybrid = []; for i = 1:size(edgesHybrid,1) % Samples states interpolated at distance 0.02 meters states = interpolate(ss,nodesHybrid(edgesHybrid(i,1),:),nodesHybrid(edgesHybrid(i,2),:),0:0.02:1); % Add [NaN NaN] to the end to break the line plot linksHybrid = [linksHybrid; [states(:,1) states(:,2)]; NaN NaN]; end```

Display the nodes and the edges. The interlinked edges show collision-free paths to use for computing the path solution.

```figure show(map) hold on plot(linksHybrid(:,1),linksHybrid(:,2),plannerLineSpec.tree{:},DisplayName="Edges") plot(nodesHybrid(:,1),nodesHybrid(:,2),plannerLineSpec.state{:},DisplayName="Nodes") legend(Location="bestoutside") hold off```

### Specify Start and Goal States

Set the seed value to ensure you generate repeatable results.

`rng("default")`

Specify the number of start and goal states for the robot to navigate. You can either specify values for the start and goal states or use the `helperGenerateRandomStartGoals` helper function to randomly generate the start and goal states. This example uses the helper function to generate the start and goal states.

```numStartGoal = 3; [startLocations,goalLocations] = helperGenerateRandomStartGoals(sv,numStartGoal);```

Visualize the map with all the randomly sampled start and goal locations.

```figure show(map) title("Selected Start and Goal States") hold on plot(startLocations(:,1),startLocations(:,2),plannerLineSpec.start{:}) plot(goalLocations(:,1),goalLocations(:,2),plannerLineSpec.goal{:}) hold off legend(Location="bestoutside")```

### Compute Path from Start State to Goal State

Select a start state and goal state between which you want to compute the path. Set `index1` to your desired start state and `index2` to your desired goal state pairs to use for motion planning.

```index1 = 2; start = startLocations(index1,:); index2 = 3; goal = goalLocations(index2,:);```

Compute the path between the selected start and goal states by using the PRM planner. The path planner uses the state samples returned by the hybrid sampling approach as seeds to compute the path between the two states.

```clear pathHybrid solnInfo [pathHybrid,solnInfo] = plan(hybridPlanner,start,goal);```

Display the results. Specify the default color and line properties for plotting the start and goal points by using the `plannerLineSpec.start` and `plannerLineSpec.goal` functions, respectively. Specify the default color and line properties for plotting the path states by using the `plannerLineSpec.path` function.

```figure show(map) hold on plot(start(1),start(2),plannerLineSpec.start{:}) plot(goal(1),goal(2),plannerLineSpec.goal{:}) if solnInfo.IsPathFound plot(pathHybrid.States(:,1),pathHybrid.States(:,2),plannerLineSpec.path{:}) title("Path Computed Using Hybrid Sampling Approach and PRM Planner") else title("Path not found") end hold off legend(Location="bestoutside")```

### Compare Hybrid Approach with Uniform and Gaussian Sampling Methods

Compare the samples generated by the hybrid sampling approach with those of the uniform sampling and Gaussian sampling methods. Because of the high computational time for path planning, the example includes precomputed PRM planner outputs for the uniform sampling and Gaussian sampling methods, stored as `.mat` files.

Load the planner output for the uniform sampling method. The `MaxNumNodes` property of the planner has been experimentally set to `3000`.

```uniformPlanner = load("uniformPRM.mat"); uniformPlanner = uniformPlanner.planner;```

Extract the nodes and edges, and interpolate states between the two ends of the edges to get the search tree.

```digraphObj = uniformPlanner.graphData; edgesUniform = table2array(digraphObj.Edges); nodesUniform = table2array(digraphObj.Nodes); linksUniform = []; for i = 1:size(edgesUniform,1) % Samples states interpolated at distance 0.02 meters states = interpolate(ss,nodesUniform(edgesUniform(i,1),:),nodesUniform(edgesUniform(i,2),:),0:0.02:1); % Add [NaN NaN] to the end to break line plot linksUniform = [linksUniform; [states(:,1) states(:,2)]; NaN NaN]; end```

Load the planner output for the Gaussian sampling method. The `MaxNumNodes` property of the planner has been experimentally set to `1500`.

```gaussianPlanner = load("GaussianPRM.mat"); gaussianPlanner = gaussianPlanner.planner;```

Extract the nodes and edges, and interpolate states between the two ends of the edges to get the search tree.

```digraphObj = gaussianPlanner.graphData; edgesGaussian= table2array(digraphObj.Edges); nodesGaussian = table2array(digraphObj.Nodes); linksGaussian = []; for i = 1:size(edgesGaussian,1) % Samples states interpolated at distance 0.02 meters states = interpolate(ss,nodesGaussian(edgesGaussian(i,1),:),nodesGaussian(edgesGaussian(i,2),:),0:0.02:1); % Add [NaN NaN] to the end to break line plot linksGaussian = [linksGaussian; [states(:,1) states(:,2)]; NaN NaN]; end```

Plot the search trees and nodes obtained from the uniform, Gaussian, and hybrid sampling approaches. Use the `helperDisplaySamples` helper function to plot the search trees and nodes.

`helperDisplaySamples(map,linksUniform,nodesUniform,linksGaussian,nodesGaussian,linksHybrid,nodesHybrid)`

Select a start state and goal state between which you want to compute the path. Set `index1` to your desired start state and `index2` to your desired goal state pairs to use for motion planning.

```index1 = 2; start = startLocations(index1,:); index2 = 1; goal = goalLocations(index2,:);```

Compute the path between the selected start and goal states by using the PRM path planner.

```clear uniformPath uniformInfo [uniformPath,uniformInfo] = plan(uniformPlanner,start,goal); clear gaussianPath gaussionInfo [gaussianPath,gaussianInfo] = plan(gaussianPlanner,start,goal); clear hybridPath hybridInfo [hybridPath,hybridInfo] = plan(hybridPlanner,start,goal);```

Plot and inspect the paths generated using the uniform, Gaussian, and hybrid sampling approaches. Use the `helperDisplayStates` helper function to plot the paths.

`helperDisplayPaths(map,start,goal,uniformPath.States,gaussianPath.States,hybridPath.States)`

Compare the path lengths.

```uniformPathLength = pathLength(uniformPath); gaussianPathLength = pathLength(gaussianPath); hybridPathLength = pathLength(hybridPath); table(uniformPathLength,gaussianPathLength,hybridPathLength)```
```ans=1×3 table uniformPathLength gaussianPathLength hybridPathLength _________________ __________________ ________________ 131.33 299.23 99.14 ```

From the results, note that:

• The uniform sampler generates more samples in the wide spaces than in the narrow passages. However, to find collision-free connections between the nodes in wide spaces and those in narrow passages, it must generate a greater number of nodes. This results in higher computation time than the Gaussian and hybrid sampling approaches, even though uniform sampling finds shorter paths than the Gaussian sampling approach.

• The Gaussian sampler generates samples only along the obstacle boundaries. As a result, the Gaussian sampler returns longer paths than the uniform or hybrid samplers.

• The hybrid sampling approach generates samples both along the obstacle boundaries and in the wide spaces. The hybrid approach takes less computation time compared to the uniform sampler, and generates shorter paths than the Gaussian sampler when the start state is not close to an obstacle boundary and the distance between the start and goal states is long.

### Helper Functions

This example includes helper functions to set the properties of the figures used for plotting the planner results obtained using the uniform, Gaussian, and hybrid sampling approaches.

Use the `helperDisplaySamples` helper function to plot the search tree and nodes for each sampling approach. Specify the default color and line properties for plotting the search tree and nodes by using the `plannerLineSpec.tree` and `plannerLineSpec.state` functions, respectively.

```function helperDisplaySamples(map,linksUniform,nodesUniform,linksGaussian,nodesGaussian,linksHybrid,nodesHybrid) fig_1 = figure(Position=[0 0 1400 300]); movegui("center") panel_1 = uipanel(fig_1, ... Position=[0 0 0.33 1]); hPlot1 = axes(panel_1); show(map,Parent=hPlot1) hold on plot(linksUniform(:,1),linksUniform(:,2),plannerLineSpec.tree{:},DisplayName="Edges") plot(nodesUniform(:,1),nodesUniform(:,2),plannerLineSpec.state{:},DisplayName="Nodes") title("Results Using Uniform Sampler") hold off legend(Location="bestoutside") panel_2 = uipanel(fig_1, ... Position=[0.33 0 0.33 1]); hPlot2 = axes(panel_2); show(map,Parent=hPlot2) hold on plot(linksGaussian(:,1),linksGaussian(:,2),plannerLineSpec.tree{:},DisplayName="Edges") plot(nodesGaussian(:,1),nodesGaussian(:,2),plannerLineSpec.state{:},DisplayName="Nodes") title("Results Using Gaussian Sampler") hold off legend(Location="bestoutside") panel_3 = uipanel(fig_1,... Position=[0.66 0 0.33 1]); hPlot3 = axes(panel_3); show(map,Parent=hPlot3); hold on; plot(linksHybrid(:,1),linksHybrid(:,2),plannerLineSpec.tree{:},DisplayName="Edges"); plot(nodesHybrid(:,1),nodesHybrid(:,2),plannerLineSpec.state{:},DisplayName="Nodes"); title("Results Using Hybrid Sampler") hold off legend(Location="bestoutside") end```

Use the `helperDisplayPaths` helper function to plot the generated paths. Specify the default color and line properties for plotting the paths by using the `plannerLineSpec.path` function. Use the `plannerLineSpec.start` and `plannerLineSpec.goal` functions to plot the start and goal states, respectively.

```function helperDisplayPaths(map,start,goal,uniformStates,gaussianStates,hybridStates) fig_2 = figure(Position=[0 0 1400 300]); movegui("center") panel_1 = uipanel(fig_2, ... Position=[0 0 0.33 1]); hPlot1 = axes(panel_1); show(map,Parent=hPlot1); hold on plot(start(1),start(2),plannerLineSpec.start{:}) plot(goal(1),goal(2),plannerLineSpec.goal{:}) plot(uniformStates(:,1),uniformStates(:,2),plannerLineSpec.path{:}) title("Path Using Uniform Sampler") hold off legend(Location="bestoutside") panel_2 = uipanel(fig_2, ... Position=[0.33 0 0.33 1]); hPlot2 = axes(panel_2); show(map,Parent=hPlot2); hold on plot(start(1),start(2),plannerLineSpec.start{:}) plot(goal(1),goal(2),plannerLineSpec.goal{:}) plot(gaussianStates(:,1),gaussianStates(:,2),plannerLineSpec.path{:}) title("Path Using Gaussian Sampler") hold off legend(Location="bestoutside") panel_3 = uipanel(fig_2, ... Position=[0.66 0 0.33 1]); hPlot3 = axes(panel_3); show(map,Parent=hPlot3) hold on plot(start(1),start(2),plannerLineSpec.start{:}) plot(goal(1),goal(2),plannerLineSpec.goal{:}) plot(hybridStates(:,1),hybridStates(:,2),plannerLineSpec.path{:}) title("Path Using Hybrid Sampler") hold off legend(Location="bestoutside") end```