Find optimal trajectory for reference path

The `trajectoryOptimalFrenet`

object generates an optimal,
feasible, and collision-free trajectory for the reference path.

The `plan`

function computes
an optimal trajectory between `start`

and terminal states. The function
samples multiple trajectories for each pair of states and chooses a feasible trajectory with
the least cost.

creates a `planner`

= trajectoryOptimalFrenet(`refPath`

,`validator`

)`trajectoryOptimalFrenet`

object with reference path,
`refPath`

, in the form of a *n*-by-2 array of
`[`

waypoints and a state validator,
*x*
*y*]`validator`

, specified as a `validatorOccupancyMap`

object.

`cart2frenet` | Convert Cartesian states to Frenet states |

`frenet2cart` | Convert Frenet states to Cartesian states |

`plan` | Plan optimal trajectory |

`show` | Visualize trajectories |

Self-intersections in the reference path can lead to unexpected behavior.

The planner does not support reverse driving.

Initial orientation for planning should be within

`-pi/2`

and`pi/2`

to the reference path.Keep the number of TerminalStates in check for real-time applications since computational complexity grows with it.

[1] Werling, Moritz, Julius Ziegler,
Sören Kammel, and Sebastian Thrun. "Optimal trajectory generation for dynamic street scenarios
in a frenet frame." *2010 IEEE International Conference on Robotics and
Automation*. 2010, pp. 987–993.