Main Content

isMotionValid

Check if path between states is valid

collapse all in page

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.

example

Examples

collapse all

Open Live Script

This example shows how to validate paths through an environment.

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

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

Figure contains an axes object. The axes object with title Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains an object of type image.

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")

Figure contains an axes object. The axes object with title Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 2 objects of type image, line.

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

Figure contains an axes object. The axes object with title Occupancy Grid, xlabel X [meters], ylabel Y [meters] contains 3 objects of type image, line. One or more of the lines displays its values using only markers

This example uses:

Open Live Script

This example shows how to validate paths through an environment.

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

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

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 2 objects of type image, patch. This object represents Inflated Areas.

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")

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 3 objects of type image, patch, line. This object represents Inflated Areas.

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

Figure contains an axes object. The axes object with xlabel X, ylabel Y contains 4 objects of type image, patch, line. One or more of the lines displays its values using only markers This object represents Inflated Areas.

Open Live Script

Create a 3-D occupancy map and associated state validator. Plan, validate, and visualize a path through the occupancy map.

Load and Assign Map to State Validator

Load a 3-D occupancy map of a city block into the workspace. Specify a threshold for which cells to consider as obstacle-free.

mapData = load('dMapCityBlock.mat');
omap = mapData.omap;
omap.FreeThreshold = 0.5;

Inflate the occupancy map to add a buffer zone for safe operation around the obstacles.

inflate(omap,1)

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 a 3-D occupancy map state validator using the created state space.

sv = validatorOccupancyMap3D(ss);

Assign the occupancy map to the state validator object. Specify the sampling distance interval.

sv.Map = omap;
sv.ValidationDistance = 0.1;

Plan and Visualize Path

Create a path planner with increased maximum connection distance. Reduce the maximum number of iterations.

planner = plannerRRT(ss,sv);
planner.MaxConnectionDistance = 50;
planner.MaxIterations = 1000;

Create a user-defined evaluation function for determining whether the path reaches the goal. Specify the probability of choosing the goal state during sampling.

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 using the specified start, goal, and planner.

[pthObj,solnInfo] = plan(planner,start,goal);

Check that the points of the path are valid states.

isValid = isStateValid(sv,pthObj.States)
isValid = 7x1 logical array

   1
   1
   1
   1
   1
   1
   1

Check that the motion between each sequential path state is valid.

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 = 6x1 logical array

   1
   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

Figure contains an axes object. The axes object with title Occupancy Map, xlabel X [meters], ylabel Y [meters] contains 4 objects of type patch, scatter, line.

Input Arguments

collapse all

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

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

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

collapse all

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

Data Types: logical

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

Version History

Introduced in R2019b

See Also

| | |