Fixing the while loop for the trajectory execution