Cleaned up arm belief joint/pos goal