Attempt to decompose SBPL Planning steps, to be able to benchmark and compare with OMPL