Added an initial step that optimizes the trajectories alone individually