First implementation of visibility-PRM planner