Initial crappy velocity move to point, working on version 2