Add debug output on configuration validity
New method in solver to try creating a direct path between init and goal configurations. Call for a roadmap reset if a collision obstacle is loaded