Fix bug where Agent would not always reset to the initial state

Oftentimes the Agent would not believe itself to have reset to the
initial state when it hit the goal, and so would remain stuck in
the goal state. Add code to make the agent reset itself.