Added trajectory ctrl mode