Fixed error. Mixed up initial and goal states.