stateSamplerUniform

Use Default State Space as Input

Create a uniform state sampler with default properties. By default, the sampler uses the SE(2) state space as the configuration space for motion planning. The limits of the state variables [x, y, theta] in the default state space are [-100 100; -100 100; -3.1416 3.1416]. The values of x and y are in meters. theta is in radians.

sampler = stateSamplerUniform;

Check the bounds of the input state space.

bounds = sampler.StateSpace.StateBounds

bounds = 3×2

 -100.0000  100.0000
 -100.0000  100.0000
   -3.1416    3.1416

Sample the input state space. Set the number of samples to select as 10.

states = sample(sampler,10);

Check the sampled state variables.

disp(states)

   62.9447   81.1584   -2.3437
   82.6752   26.4718   -2.5287
  -44.3004    9.3763    2.8746
   92.9777  -68.4774    2.9568
   91.4334   -2.9249    1.8867
  -71.6227  -15.6477    2.6121
   58.4415   91.8985    0.9785
  -92.8577   69.8259    2.7269
   35.7470   51.5480    1.5276
  -21.5546   31.0956   -2.0660

Use Custom State Space as Input

Define a custom SE(2) state space by specifying the limits of the state variables [x, y, theta]. The values of x and y are in meters. theta is in radians.

ss = stateSpaceSE2([-1 1; -2 2; -pi/2 pi/2]);

Create a uniform state sampler with the custom state space as input.

sampler = stateSamplerUniform(ss);

Sample the input state space. Set the number of samples to select as 10.

states = sample(sampler,10);

Check the sampled state variables.

disp(states)

    0.4121   -1.8727   -0.7008
   -0.9077   -1.6115    1.0162
    0.3897   -0.7316    1.4144
   -0.9311   -0.2450   -0.3721
    0.5310    1.1808   -0.9837
   -0.0205   -0.2177    0.4597
    0.4187    1.0187   -0.7036
    0.3594    0.6204   -1.0599
   -0.7620   -0.0065    1.4443
   -0.3192    0.3411   -0.8677

Create Uniform State Sampler

Load a probability occupancy grid into the MATLAB® workspace.

load("exampleMaps.mat","simpleMap");

Create an occupancy map from the input occupancy grid.

map = occupancyMap(simpleMap);

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

Create a state space SE(2) object using the specified state variables.

stateSpace = stateSpaceSE2([x; y; theta]);

Create a uniform state sampler to uniformly sample the specified state space.

sampler = stateSamplerUniform(stateSpace); 

Configure RRT Path Planner

Check the validity of the states in the input state space by using a state validator.

stateValidator = validatorOccupancyMap(stateSpace,Map=map); 

Configure the RRT path planner. Use the uniform state sampler to sample the input state space.

planner = plannerRRT(stateSpace,stateValidator,StateSampler=sampler); 

Find Optimal Path Between Two States

Specify the start point and the goal point in the input state space.

start = [5 5 0];
goal = [20 15 0]; 

Compute the optimal path between the start point and the goal point using RRT path planner.

[path,info] = plan(planner,start,goal);

Visualize the Results

Display the occupancy map.

figure
show(map)
hold on

Plot the start point and the goal point. Specify the default color and line properties for plotting the start and goal points by using the plannerLineSpec.start and plannerLineSpec.goal functions, respectively

plot(start(1),start(2),plannerLineSpec.start{:}); 
plot(goal(1),goal(2),plannerLineSpec.goal{:}); 

Plot the search tree to visualize the states explored to find the optimal path. Use the plannerLineSpec.tree function to specify the default color and line properties for plotting the search tree.

plot(info.TreeData(:,1),info.TreeData(:,2),plannerLineSpec.tree{:})

If the planner has found an optimal path between the start and goal states, plot the results. Use the plannerLineSpec.path function to specify the default color and line properties for plotting the path.

if info.IsPathFound
    plot(path.States(:,1),path.States(:,2),plannerLineSpec.path{:})
    legend
else
    disp("Path not found. Try modifying the planner parameters."); 
end