add stop if not_reached for ApproachPose