querying collision environment via network only once
removed some couts, refactoring
started on non greedy planner