Add to roadmap when inStatePlanner fails

Previously, when the inStatePlanner fails to compute path between two
configurations on the same leaf, the roadmap generated is not added to
the main roadmap of StatesPathFinder. Now it is added so that the
roadmap gets expanded whenever inStatePlanner is used. The good side
effect is fairer comparison between different algorithms when comparing by the number
of nodes in the roadmap.