validatorOccupancyMap3D
State validator based on 3-D grid map
Description
The validatorOccupancyMap3D object validates states and discretized motions
based on occupancy values in a 3-D occupancy map. The object interprets obstacle-free map
locations as valid states. The object interprets occupied and unknown map locations as invalid
states.
Creation
Syntax
Description
validator = validatorOccupancyMap3D
validator = validatorOccupancyMap3D(stateSpace)stateSpace
input sets the value of the StateSpace
property.
validator = validatorOccupancyMap3D(stateSpace,Name,Value)
For example, validatorOccupancyMap3D('ValidationDistance',0.1)
creates a 3-D occupancy map validator with a sampling interval of 0.1.
Properties
Object Functions
copy | Create deep copy of state validator object |
isMotionValid | Check if path between states is valid |
isStateValid | Check if state is valid |
Examples
Validate Path Through 3-D Occupancy Map Environment
This example shows how to validate paths through a 3-D occupancy map evironment.
Load and Assign Map to State Validator
Load the 3-D occupancy map of a city block. Specify the threshold to consider cells as obstacle-free.
mapData = load('dMapCityBlock.mat');
omap = mapData.omap;
omap.FreeThreshold = 0.5;Inflate the map to add a buffer zone for safe operation around the obstacles.
inflate(omap,5);
Create an SE(3) state space object with bounds for state variables.
ss = stateSpaceSE3([-20 220;-20 220;-10 100; inf inf; inf inf; inf inf; inf inf]);
Create an occupancyMap3D based state validator using the created state space.
sv = validatorOccupancyMap3D(ss);
Assign the map to the state validator object. Specify the ValidationDistance property.
sv.Map = omap; sv.ValidationDistance = 0.1;
Plan and Visualize Path
Create the path planner and increase the maximum connection distance. Reduce the maximum number of iterations.
planner = plannerRRT(ss,sv); planner.MaxConnectionDistance = 50; planner.MaxIterations = 1000;
Create a user-defined goal reached evaluation function. Specify the GoalBias property.
planner.GoalReachedFcn = @(~,x,y)(norm(x(1:3)-y(1:3))<5); planner.GoalBias = 0.1;
Set the start and goal states.
start = [40 180 25 0.7 0.2 0 0.1]; goal = [150 33 35 0.3 0 0.1 0.6];
Plan a path with default settings.
[pthObj,solnInfo] = plan(planner,start,goal);
Check the points of the path are valid states.
isValid = isStateValid(sv,pthObj.States)
isValid = 6×1 logical array
1
1
1
1
1
1
Check the motion between each sequential path states.
isPathValid = zeros(size(pthObj.States,1)-1,1,'logical'); for i = 1:size(pthObj.States,1)-1 [isPathValid(i), ~] = isMotionValid(sv,pthObj.States(i,:),pthObj.States(i+1,:)); end isPathValid
isPathValid = 5×1 logical array
1
1
1
1
1
Visualize the results.
show(omap) hold on scatter3(start(1,1),start(1,2),start(1,3),'g','filled') % draw start state scatter3(goal(1,1),goal(1,2),goal(1,3),'r','filled') % draw goal state plot3(pthObj.States(:,1),pthObj.States(:,2),pthObj.States(:,3),'r-','LineWidth',2) % draw path
