Trying to use Matlab's PRM path planner (plannerPRM) in 3D

Hi,

I am trying to benchmark different path planners on a 3D work environement using a common 3D occupancy map. So far I managed to get results for the RRT* but I can't get a working PRM.

Is there a way to use Matlab's PRM in 3D, i.e. force the sampling in 3D space ?

I slightly modified the code I used for the RRT* by adding parts from the plannerPRM doc example (plotting needs to be edited to accound for extra dimension).

%% Configuration Space Definition
X_boundary = [0, 510];
Y_boundary = [0, 530];
Z_boundary = [0, 80];
StateSpace = stateSpaceSE3;
StateSpace.StateBounds = [X_boundary; Y_boundary; Z_boundary;-1 1; -1 1; -1 1; -1 1];


%% Add Obstacle 
ObstacleSpace = importOccupancyMap3D("...(path to file)...\Simu1.ot");

%% Create state validator
StateValidator = validatorOccupancyMap3D(StateSpace);

% assign occupancy map to validator
StateValidator.Map = ObstacleSpace;
StateValidator.ValidationDistance = 0.1;

%% Path planner 
planner = plannerPRM(StateSpace,StateValidator);

start = [150 160 21 1 0 0 0];
goal = [250 450 61 1 0 0 0];

graph = graphData(planner);

edges = table2array(graph.Edges);
nodes = table2array(graph.Nodes);

show(StateValidator.Map)

hold on
plot3(nodes(:,1),nodes(:,2),nodes(:,3),"*","Color","b","LineWidth",2)
for i = 1:size(edges,1)
    % Samples states at distance 0.02 meters.
    states = interpolate(StateSpace,nodes(edges(i,1),:), ...
                         nodes(edges(i,2),:),0:0.02:1);
    plot3(states(:,1),states(:,2),state(:,3),"Color","b")
end
plot3(start(1),start(2),start(3),"*","Color","g","LineWidth",3)
plot3(goal(1),goal(2),goal(3),"*","Color","r","LineWidth",3)

rng(100,"twister");
[pthObj, solnInfo] = plan(planner,start,goal);

if solnInfo.IsPathFound
    interpolate(pthObj,1000);
    plot(pthObj.States(:,1),pthObj.States(:,2), ...
         "Color",[0.85 0.325 0.098],"LineWidth",2)
else
    disp("Path not found")
end
hold off

Answer 1

[Olivier Völlmin]

Nevermind, I managed to get it to work, it was due to several issues on my side.

Here's the working code:

%% Path Planner using Matlab's PRM

clear; 
close all; 
clc;

%% Configuration Space Definition
X_boundary = [0, 510];
Y_boundary = [0, 530];
Z_boundary = [0, 80];
StateSpace = stateSpaceSE3;
StateSpace.StateBounds = [X_boundary; Y_boundary; Z_boundary;-1 1; -1 1; -1 1; -1 1];


%% Add Obstacle 
ObstacleSpace = importOccupancyMap3D("...(path to file)...\Simu1.ot");

%show(ObstacleSpace)
%inflate(ObstacleSpace,1)

%% Create state validator
StateValidator = validatorOccupancyMap3D(StateSpace);
% assign occupancy map to validator
StateValidator.Map = ObstacleSpace;
StateValidator.ValidationDistance = 0.1;

%% Path planner 
planner = plannerPRM(StateSpace,StateValidator);

start = [150 160 21 1 0 0 0];
goal = [250 450 61 1 0 0 0];

graph = graphData(planner);

edges = table2array(graph.Edges);
nodes = table2array(graph.Nodes);

show(StateValidator.Map)

hold on
plot3(nodes(:,1),nodes(:,2),nodes(:,3),"*","Color","b","LineWidth",2)
for i = 1:size(edges,1)
    % Samples states at distance 0.02 meters.
    states = interpolate(StateSpace,nodes(edges(i,1),:), ...
                         nodes(edges(i,2),:),0:0.02:1);
    plot3(states(:,1),states(:,2),states(:,3),"Color","b")
end
plot3(start(1),start(2),start(3),"*","Color","g","LineWidth",3)
plot3(goal(1),goal(2),goal(3),"*","Color","r","LineWidth",3)

rng(100,"twister");
[pthObj, solnInfo] = plan(planner,start,goal);

if solnInfo.IsPathFound
    interpolate(pthObj,1000);
    plot3(pthObj.States(:,1),pthObj.States(:,2),pthObj.States(:,3),...
         "Color",[0.85 0.325 0.098],"LineWidth",2)
else
    disp("Path not found")
end
hold off

[Faruk Yagiz]

Hi,

Did you manage to try other algorithms on Matlab as well. I also only managed to work on RRT* but so far I think others can only work on 2D environment or am I wrong?