Trajectory Optimization Algorithm