Fix while loop for composed trajectory