Move smoothed_trajectory_control to own file and commented.