add plan stage, collapse pre-grasp filter with approach stage