Improved TrajectoryPlanner