Remove extra configSolved and check idxConfigList_

The configSolved() method is redundant. Checks are added for the case
where the final configuration computed has been reached but the goal is
deemed not reached by the path planner.

Some other small redundancies are also removed.