Added trajectory following code.