Updated simulateMotion so it ends at goal