append

Add states to end of path

Since R2021b

collapse all in page

Syntax

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

Description

append(pathObj,states,controls,targets,durations) adds a sequence of states states, controls controls, targets targets, and durations durations, to the end of the path. If the path is empty, the states input must have one more row than the other input vectors and matrices. If the path contains points already, the function applies the first control to the last state in the current 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

Series of states for the path, specified as an n-by-m matrix. n is the number of points to add to the path. m is the dimension of the state vector.

Note

If the path object is empty, the states input should be an (n+1)-by-m matrix.

Data Types: double

Control input for each state, specified as an n-by-m matrix. n is the number of points to add to the path. m is the dimension of the state vector.

Data Types: double

Target state for each state in the path,specified as an n-by-m matrix. n is the number of points to add to the path. m is the dimension of the state vector.

Data Types: double

Duration of each control input, specified as an n-element vector in seconds. n is the number of points to add to the path.

Data Types: double

Version History

Introduced in R2021b

See Also

Objects

Functions