-Add parameter for allowed angle between pose goal and refined target pose