Initial changes for receiving multiple trajectories
This commit contains the initial changes necessary for the robot to smoothly switch trajectories
Changes to moveOnTrajectory so that the robot can move along a single trajectory with no interrupts