modifyTrajectory method complete
The modifyTrajectory method in Planner is complete along with getNewVelocities. They both desperately need to be cleaned up. They are both hard to read and the logic can probably be made more efficien. I will do both of those things in the next commit. I just wanted to back these up in case some crazy accident happens and I lose them.