Optimal Sampling-Based Trajectory Planning For Autonomous Systems in Urban Environments