Improve goal align code

Improve goal align code by rotating to face goal after stepping off mat.