isMotionValid

Check if path between states is valid

Syntax

[isValid,lastValid] = isMotionValid(validator,state1,state2)

Description

[isValid,lastValid] = isMotionValid(validator,state1,state2) checks if the path between two states is valid by interpolating between states. The function also returns the last valid state along the path.

Examples

Validate Path Through Occupancy Map Environment

This example shows how to validate paths through an evironment.

Load example maps. Use the simple map to create a binary occupancy map.

load exampleMaps.mat
map = occupancyMap(simpleMap);
show(map)

Specify a coarse path through the map.

path = [2 2 pi/2; 10 15 0; 17 8 -pi/2];
hold on
plot(path(:,1),path(:,2),"--o")

Create a state validator using the stateSpaceSE2 definition. Specify the map and the distance for interpolating and validating path segments.

validator = validatorOccupancyMap(stateSpaceSE2);
validator.Map = map;
validator.ValidationDistance = 0.1;

Check the points of the path are valid states. All three points are in free space, so are considered valid.

isValid = isStateValid(validator,path)

isValid = 3x1 logical array

   1
   1
   1

Check the motion between each sequential path states. The isMotionValid function interpolates along the path between states. If a path segment is invalid, plot the last valid point along the path.

startStates = [path(1,:);path(2,:)];
endStates = [path(2,:);path(3,:)];
    for i = 1:2
        [isPathValid, lastValid] = isMotionValid(validator,startStates(i,:),endStates(i,:));
        if ~isPathValid
            plot(lastValid(1),lastValid(2),'or')
        end
    end
hold off

Validate Path Through Vehicle Costmap Environment

This example shows how to validate paths through an evironment.

Load example maps. Use the simple map to create a vehicle cost map. Specify an inflation raidus of 1 meter.

load exampleMaps.mat
map = vehicleCostmap(double(simpleMap));
map.CollisionChecker = inflationCollisionChecker("InflationRadius",1);
plot(map)

Specify a coarse path through the map.

path = [3 3 pi/2; 8 15 0; 17 8 -pi/2];
hold on
plot(path(:,1),path(:,2),"--o")

Create a state validator using the stateSpaceSE2 definition. Specify the map and the distance for interpolating and validating path segments.

validator = validatorVehicleCostmap(stateSpaceSE2);
validator.Map = map;
validator.ValidationDistance = 0.1;

Check the points of the path are valid states. All three points are in free space, so are considered valid.

isValid = isStateValid(validator,path)

isValid = 3x1 logical array

   1
   1
   1

Check the motion between each sequential path states. The isMotionValid function interpolates along the path between states. If a path segment is invalid, plot the last valid point along the path.

startStates = [path(1,:);path(2,:)];
endStates = [path(2,:);path(3,:)];
    for i = 1:2
        [isPathValid, lastValid] = isMotionValid(validator,startStates(i,:),endStates(i,:));
        if ~isPathValid
            plot(lastValid(1),lastValid(2),'or')
        end
    end
hold off

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

Input Arguments

validator — State validator object

object of subclass of nav.StateValidator

State validator object, specified as an object of subclass of nav.StateValidator. These are the predefined state validator objects:

• validatorOccupancyMap

• validatorVehicleCostmap

• validatorOccupancyMap3D

state1 — Initial state positions

n-element row vector | m-by-n matrix

Initial state positions, specified as an n-element row vector or m-by-n matrix. n is the dimension of the state space specified in validator. m is the number of states to validate.

Data Types: single | double

state2 — Final state positions

n-element row vector | m-by-n matrix

Final state positions, specified as an n-element row vector or m-by-n matrix. n is the dimension of the state space specified in validator. m is the number of states to validate.

Data Types: single | double

Output Arguments

isValid — Valid states

m-element logical column vector

Valid states, returned as an m-element logical column vector.

Data Types: logical

lastValid — Final valid state along each path

n-element row vector | m-by-n matrix

Final valid state along each path, returned as an n-element row vector or m-by-n matrix. n is the dimension of the state space specified in the state space property in validator. m is the number of paths validated. Each row contains the final valid state along the associated path.

Data Types: single | double