araplanner:

     Moved reinitialization of goal state to inside ReInitializeSearchStateSpace