Solve when init and goal config in same state

The graph has loop edge for every state (node), which can be used to
connect init and goal config directly if they are both in the same
state.