A Feasible Sequential Linear Programming Algorithm with Application to Time-Optimal Path Planning Problems