Rebase drake

Massive scary rebase to try to get mitdrc/drake closer to ground truth drake. Main motivation is to get the more accurate raycasting in to improve simulation. If this works a significantly improved simulation environment shouldn't be far behind~