[Marlow]

I'm thinking of a few things that will help with code cleanliness and perhaps optimization, such as containing the obstacles in an object that contains the potentially hyper-dimensional rectangles.

Instead of only searching within the "7 radian units" from the all points, would simply exploring the obstacle free space as efficiently and thoroughly as possible work? Then, when you are traversing the tree from from the start the finish, if a child is > "7 radian units" away from its parent, inject children between the original parent and child to meet the robot's needs.

In order for a point to be added to the search tree, the line from parent -> new child must be a partition of the space, tree, and the obstacle space.

This is turning into a really interesting and challenging problem!