%% Partitioning Longitudinal Terminal States in Trajectory Generation
% This example shows how to partition the longitudinal terminal states in optimal trajectory planning 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(50,100);
grid(22:24,28:33) = 1;
grid(18:20,37:42) = 1;
grid(29:31,72:77) = 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 = [0,25;30,30;75,20;100,25];

% Initialize the planner object with the reference path, and the state validator.

planner = trajectoryOptimalFrenet(refPath,statevalidator);

% Assign longitudinal terminal state, and maximum acceleration values.

planner.TerminalStates.Longitudinal = 100;
planner.FeasibilityParameters.MaxAcceleration = 10;

% Trajectory Planning
% Initial Frenet state of vehicle.

initFrenetState = zeros(1,6);

% Assign the number of partitions for the longitudinal terminal state.

planner.NumSegments = 3; 

% Plan a trajectory from initial Frenet state.

plan(planner,initFrenetState);

% Trajectory Visualization
% Visualize the map and the trajectories.

show(map)
hold on
show(planner,'Trajectory','all')

% Copyright 2019 The MathWorks, Inc.