Add unit test for infinite recursion in collision detection.

Which uncovered a bug with overlapping robots which is also fixed in this pull request.