Visualize trajectories


show(planner) visualizes the reference path and trajectory from the candidates generated by the plan function. The trajectory is shown as a line plot. The plot also includes datatip mode, which can be used to visualize the feasibility vector and index of the trajectory from the TrajectoryList property.


show(planner,Name,Value) specifies additional options using one or more Name,Value pair arguments. Enclose each name inside single quotes (' ').

axHandle = show(planner) returns the axes handle of the figure used to plot the trajectory.


collapse all

This example shows how to plan an optimal trajectory using a trajectoryOptimalFrenet object.

Create and Assign Map to State Validator

Create a state validator object for collision checking.

stateValidator = validatorOccupancyMap;

Create an obstacle grid map.

grid = zeros(700,700);
grid(296:305,150:175) = 1;
grid(286:295,300:325) = 1;
grid(306:315,300:325) = 1;
grid(296:305,450:475) = 1;
grid(286:295,600:625) = 1;
grid(306:315,600:625) = 1;

Create a binaryOccupancyMap with the grid map.

map = binaryOccupancyMap(grid);

Assign the map to the state validator.

stateValidator.Map = map;

Plan and Visualize Trajectory

Create a reference path for the planner to follow.

refPath = [10,400;700,400];

Declare the cost function handle to prioritize left lane changes.

leftLaneChangeCost = @(states)((states(end,2) < refPath(end,2))*10);

Initialize the planner object with the reference path, the state validator, and the custom cost function.

plannerObj = trajectoryOptimalFrenet(refPath,stateValidator,'CostFunction',leftLaneChangeCost);

Assign lateral deviation values.

plannerObj.TerminalStates.Lateral = -10:10:10;

Trajectory Generation

Initialize the variables for the replanning loop.

previousDeviationValue = 0;
deviationValue = 0;
initState = zeros(1,6);

Replan until final waypoint is closest to the terminal state.

for j = 1:150
    % Deviation from the current reference path
    if initState(4) > 5
        deviationValue = 10;
    elseif initState(4) < -5
        deviationValue = -10;
    % Move reference path to the current lane
    if previousDeviationValue ~= deviationValue
        % Shift the waypoints by the deviation value
        plannerObj.Waypoints = [plannerObj.Waypoints(:,1), plannerObj.Waypoints(:,2) + deviationValue];
        % Shift the terminal states such that they remain fixed in world frame
        plannerObj.TerminalStates.Lateral = plannerObj.TerminalStates.Lateral - deviationValue;
        % Store the deviation value
        previousDeviationValue = deviationValue;
        % Update initState variable with the new planner object
        initState = cart2frenet(plannerObj,trajectory(10,1:6));
    % Generate a trajectory from initState
    trajectory = plan(plannerObj,initState);
    % Use 10th state of the current trajectory as a starting point for
    % replanning
    initState = cart2frenet(plannerObj,trajectory(10,1:6));
    % Visualize every 5th iteration
    if mod(j,5)==0
        hold on;

Input Arguments

collapse all

Optimal trajectory planner in frenet space, specified as a trajectoryOptimalFrenet object.

Name-Value Pair Arguments

Specify optional comma-separated pairs of Name,Value arguments. Name is the argument name and Value is the corresponding value. Name must appear inside quotes. You can specify several name and value pair arguments in any order as Name1,Value1,...,NameN,ValueN.

Example: 'Trajectory','all'

Axes to plot trajectory, specified as the comma-separated pair consisting of 'Parent' and either an Axes or UIAxes object. See axes or uiaxes.

Trajectory display option, specified as the comma-separated pair consisting of 'Trajectory' and either 'optimal' or 'all'.

Reference path display option, specified as the comma-separated pair consisting of 'ReferencePath' and either 'on' or 'off'.

Trajectory color display option, specified as the comma-separated pair consisting of 'TrajectoryColor' and one of the following:

  • 'acceleration'

  • 'cost'

  • 'velocity'

  • 'none'

Set this property to display the specified trajectory as a color-gradient along the specified path.

Output Arguments

collapse all

Axes handle used to plot trajectory, returned as either an axes, or uiaxes object.

Introduced in R2019b