Main Content

interpolate

Interpolate path based on propagator step size

Since R2021b

collapse all in page

Syntax

interpolate(pathObj)

Description

interpolate(pathObj) evaluates the path based on the ControlStepSize property of the nav.StatePropagator object in the StatePropagator property of the control path pathObj, and adds all intermediate points to the control path.

example

Examples

collapse all

Open Live Script

Set State and State Propagator Parameters

Load a ternary map matrix and create an occupancyMap object.

load("exampleMaps","ternaryMap")
map = occupancyMap(ternaryMap,10);

Create a state propagator for a bicycle kinematic model using the map.

propagator = mobileRobotPropagator(Environment=map);

Set the state bounds on the state space based on the map world limits.

propagator.StateSpace.StateBounds(1:2,:) = [map.XWorldLimits; 
                                            map.YWorldLimits];

Plan Path

Create the path planner from the state propagator.

planner = plannerControlRRT(propagator);

Specify the start and goal states.

start = [10 15 0];
goal  = [40 30 0];

Plan a path between the states. For repeatable results, reset the random number generator before planning.

rng("default")
path = plan(planner,start,goal);

Check the total elapsed duration of the control path.

pathDuration(path)
ans = 
102.4000

Interpolate the path to the control step size of the propagator.

interpolate(path)

Visualize the path.

figure
show(map)
hold on
plot(start(1),start(2),"rx")
plot(goal(1),goal(2),"go")
plot(path.States(:,1),path.States(:,2),"b")
hold off

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

Plan Return Path

Plan a second path from the end of the previous path location back to the start.

path2 = plan(planner,path.States(end,:),start);

Check the total elapsed duration of the second path.

pathDuration(path2)
ans = 
100.3000

Visualize the path.

figure
show(map)
hold on
plot(start(1),start(2),"rx")
plot(goal(1),goal(2),"go")
plot(path2.States(:,1),path2.States(:,2),"m")
hold off

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

Combine Paths

Extract the sequence of motions from the second path.

states = path2.States(2:end,:);
controls = path2.Controls;
targets = path2.TargetStates;
durations = path2.Durations;

Append this sequence to the end of the first path.

append(path,states,controls,targets,durations)

Interpolate the new segments in the original path.

interpolate(path)

Check the total elapsed duration of the final path.

pathDuration(path)
ans = 
102.4000

Visualize the path.

figure
show(map)
hold on
plot(start(1),start(2),"rx")
plot(goal(1),goal(2),"go")
plot(path.States(:,1),path.States(:,2),"b")
hold off

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

Input Arguments

collapse all

Control path, specified as a navPathControl object.

Data Types: double

Version History

Introduced in R2021b

See Also

Objects

Functions