Optimal Bidirectional RRT package