append

Add states to end of path

Since R2021b

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.

Examples

Combine Two Kinodynamic Paths with Controls for Mobile Robot

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

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

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

Input Arguments

pathObj — Control path

navControlPath

Control path, specified as a navPathControl object.

Data Types: double

states — Series of states for path

n-by-m matrix

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

controls — Control input for each state

n-by-m matrix

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

targets — Target state for each state in path

n-element vector in seconds

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

durations — Duration of each control input

n-element vector in seconds

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