Fixed align_goal to a working state
Merge conflict