Fixed goal position logging in pose file