copy

Create deep copy of Gaussian state sampler object

Since R2023b

collapse all in page

Syntax

sampler2 = copy(sampler1)

Description

example

sampler2 = copy(sampler1) creates a deep copy of the Gaussian state sampler object.

Examples

collapse all

Sample SE(2) State Space Using Gaussian State Sampler

Open Live Script

Sample an SE(2) state space using a Gaussian state sampler, and observe the impact of the sampler parameter values on the sampling results.

Set the seed value to ensure you generate the same results.

rng(50,"twister");

Create a simple occupancy map with a narrow passage.

map = binaryOccupancyMap;
occupied = [5*ones(9,1),[1; 2; 3; 4; 5; 7; 8; 9; 10]];
setOccupancy(map,occupied,1);
figure(Position=[0, 0, 200, 200])
show(map)

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 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([x; y; theta]);
sv = validatorOccupancyMap(ss,Map=map);
sv.ValidationDistance = 0.01;

Sample State Space Using Gaussian State Sampler

Create a Gaussian state sampler with default parameter values. By default:

The maximum number of attempts that the sampler must take for finding the state samples is set to 10.
The standard deviation values along the $x, y,$ and $θ$ directions are set to 0.1, 0.1, and 0.0628, respectively.

sampler_orig = stateSamplerGaussian(sv)

sampler_orig = 
  stateSamplerGaussian with properties:

           StateSpace: [1x1 stateSpaceSE2]
       StateValidator: [1x1 validatorOccupancyMap]
    StandardDeviation: [0.1000 0.1000 0.0628]
          MaxAttempts: 10

Generate 40 samples for motion planning from the input state space.

states_orig = sample(sampler_orig,40);

You can generate optimal samples by modifying the maximum number of attempts and standard deviation values. If the samples are scattered all over the input space, increase the maximum number of attempts and the standard deviation values to concentrate the state samples around the obstacle boundary.

Vary Maximum Number of Attempts

Create copies of the original state sampler object and modify the maximum number of attempts, property of the sampler, MaxAttempts, to study its impact on the sampling results. Set the standard deviation values to default values.

Set the maximum number of attempts to find valid samples to 100, and generate 40 new samples from the input state space.

sampler_2 = copy(sampler_orig);
sampler_2.MaxAttempts = 100;
states_2 = sample(sampler_2,40);

Set the maximum number of attempts to find valid samples to 200, and generate 40 new samples from the input state space.

sampler_3 = copy(sampler_orig);
sampler_3.MaxAttempts = 200;
states_3 = sample(sampler_3,40);

Display the results using the helperDisplayStates helper function. Note that, as the number of attempts increases, the samples concentrate more around the obstacle boundary.

helperDisplayStates(map,states_orig,sampler_2,states_2,sampler_3,states_3,"MaxAttempts");

Vary Standard Deviation

Create copies of the original state sampler object and modify the standard deviation, property of the sampler, StandardDeviation, to study its impact on the sampling results. Set the maximum number of attempts to 200.

Generate 40 samples with the default standard deviation values.

sampler_orig.MaxAttempts = 200;
states_orig = sample(sampler_orig,40);

Set the standard deviation values to [0.01 0.01 0.06]. Generate 40 new samples from the input state space.

sampler_4 = copy(sampler_orig);
sampler_4.StandardDeviation = [0.01 0.01 0.06];
states_4 = sample(sampler_4,40);

Set the standard deviation values to [0.5 0.5 0.06]. Generate 40 new samples from the input state space.

sampler_5 = copy(sampler_orig);
sampler_5.StandardDeviation = [0.5 0.5 0.06];
states_5 = sample(sampler_5,40);

Display the results using the helperDisplayStates helper function. Note that, as you increase the standard deviation values, the samples concentrate more around the obstacle boundary. However, if the standard deviation values are greater than the width of the narrow passages in the input space, the sampler generates incorrect results.

helperDisplayStates(map,states_orig,sampler_4,states_4,sampler_5,states_5,"Std.Deviation");

Helper Function

helperDisplayStates displays results using a custom figure window.

function helperDisplayStates(map,states_orig,sampler_2,states_2,sampler_3,states_3,select)
if select == "MaxAttempts"
    title_1 = "MaxAttempts = 10 (Default value)";
    title_2 = strcat("MaxAttempts = ",num2str(sampler_2.MaxAttempts));
    title_3 = strcat("MaxAttempts = ",num2str(sampler_3.MaxAttempts));
elseif select == "Std.Deviation"
    title_1 = "StandardDeviation = [0.1 0.1 0.06] (Default value)";
    title_2 = strcat("StandardDeviation = [0.01 0.01 0.06]");
    title_3 = strcat("StandardDeviation = [0.5 0.5 0.06]");
end

fig_1 = figure(Position=[0 0 700 300]);
movegui("center")
panel_1 = uipanel(fig_1, ...
    Position=[0 0 0.33 1], ...
    Title=title_1);
hPlot1 = axes(panel_1);
show(map,Parent=hPlot1);
hold on;
plot(states_orig(:,1),states_orig(:,2),plannerLineSpec.state{:})
hold off

panel_2 = uipanel(fig_1, ...
    Position=[0.33 0 0.33 1], ...
    Title=title_2);
hPlot2 = axes(panel_2);
show(map,Parent=hPlot2);
hold on;
plot(states_2(:,1),states_2(:,2),plannerLineSpec.state{:})
hold off

panel_3 = uipanel(fig_1, ...
    Position=[0.66 0 0.33 1], ...
    Title=title_3);
hPlot3 = axes(panel_3);
show(map,Parent=hPlot3);
hold on;
plot(states_3(:,1),states_3(:,2),plannerLineSpec.state{:})
hold off
end

Modify Gaussian State Sampler Parameters and Compare Results

Open Live Script

Modify one or more parameters of a Gaussian state sampler and compare its effect on motion planning results. To accomplish this, you can create a single instance of the Gaussian state sampler object, and then, use the copy function of the stateSamplerGaussian object to create a deep copy of the existing object instance, modify the desired parameters, and use it with the planner to generate different results.

Set the random number seed, to ensure repeatability.

rng(50,"twister");

Create Occupancy Map and Find State Variables

Load a probability occupancy grid into MATLAB® workspace.

load("narrowPassageMap.mat","narrowPassage");

Create an occupancy map from the input occupancy grid.

map = binaryOccupancyMap(narrowPassage,50);

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 Gaussian State Sampler

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.

ss = stateSpaceSE2([x; y; theta]);
sv = validatorOccupancyMap(ss,Map=map);

Create a Gaussian state sampler using the state validator. Check the default parameter values.

sampler_1 = stateSamplerGaussian(sv)

sampler_1 = 
  stateSamplerGaussian with properties:

           StateSpace: [1x1 stateSpaceSE2]
       StateValidator: [1x1 validatorOccupancyMap]
    StandardDeviation: [0.1030 0.0644 0.0628]
          MaxAttempts: 10

Copy State Sampler and Modify Parameter Values

Create a copy of the first instance of the Gaussian state sampler.

sampler_2 = copy(sampler_1);

Modify the standard deviation and maximum attempts properties of the Gaussian state sampler.

sampler_2.StandardDeviation = [0.3 0.1 0.1];
sampler_2.MaxAttempts = 150;

Configure PRM Path Planner

Configure two PRM path planners. Use the original and the modified Gaussian state samplers to sample the input state space.

planner_1 = plannerPRM(ss,sv,StateSampler=sampler_1,MaxNumNodes=900); 
planner_2 = plannerPRM(sampler_2.StateSpace,sampler_2.StateValidator, ...
    StateSampler=sampler_2,MaxNumNodes=900);

Find Optimal Path Between Two States

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

startPose = [1 1 0];
goalPose = [9 1 0];

Compute the optimal path between the start point and the goal point using the PRM path planners.

[pathObj_1,info_1] = plan(planner_1,startPose,goalPose);
[pathObj_2,info_2] = plan(planner_2,startPose,goalPose);

Compare Results

Plot the results obtained using the two different Gaussian state samplers for motion planning. Use the plannerLineSpec.start and plannerLineSpec.goal functions for plotting the start and goal points, respectively. If the planner with default parameters 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.

figure
show(map)
hold on
plot(startPose(1),startPose(2),plannerLineSpec.start{:}); 
plot(goalPose(1),goalPose(2),plannerLineSpec.goal{:}); 
if info_1.IsPathFound
    plot(pathObj_1.States(:,1),pathObj_1.States(:,2),plannerLineSpec.path{:})
    title("Using Gaussian State Sampler with Default Parameters")
    legend
else
    disp("Path not found. Try modifying validator, sampler or planner parameters."); 
end
hold off

Plot the results obtained using the Gaussian state sampler with custom parameter values.

figure
show(map)
hold on
plot(startPose(1),startPose(2),plannerLineSpec.start{:}); 
plot(goalPose(1),goalPose(2),plannerLineSpec.goal{:}); 
if info_2.IsPathFound
    plot(pathObj_2.States(:,1),pathObj_2.States(:,2),plannerLineSpec.path{:})
    title("Using Gaussian State Sampler with Custom Parameters")
    legend
else
    disp("Path not found. Try modifying validator, sampler or planner parameters."); 
end
hold off