Develop path finder functionality

[rocco8773]

Develop functionality that can computer a path from a start point to an end point that will avoid all excluded regions in the motion space.