Sped up rrt.py by removing duplicate collision checks