debugged followtrajectory