Drake::IK::AddMinimumDistanceLowerBoundConstraint not working as expected

Question

I used drake for IK since it provides clear interface for optimization-based IK. The embodiment I'm using is UR5 + a 3D printed tool (scoop and probe). I want to (1) filter out collisions between adjacent links and then (2) apply min dist lb constraint. I want to apply this constraint because I want robot's links be separated by certain distance.

This is how I exclude unwanted collisions, (1) within geometries of the same frame/link. (2) between geometries of adjacent frame/link.

def __exclude_collision_pairs(scene_graph: SceneGraph) -> None:
    # add collision filter to the scene graph.
    collision_filter_manager = scene_graph.collision_filter_manager()
    scene_graph_inspector = scene_graph.model_inspector()

    frame_name_to_id = dict()
    for frame_id in scene_graph_inspector.GetAllFrameIds():
        frame_name_to_id[scene_graph_inspector.GetName(frame_id)] = frame_id

    ground_geo_set = GeometrySet(frame_name_to_id['UR5::ground'])
    base_link_geo_set = GeometrySet(frame_name_to_id['UR5::base_link'])
    shoulder_link_geo_set = GeometrySet(frame_name_to_id['UR5::shoulder_link'])
    upperarm_link_geo_set = GeometrySet(frame_name_to_id['UR5::upperarm_link'])
    forearm_link_geo_set = GeometrySet(frame_name_to_id['UR5::forearm_link'])
    wrist1_link_geo_set = GeometrySet(frame_name_to_id['UR5::wrist1_link'])
    wrist2_link_geo_set = GeometrySet(frame_name_to_id['UR5::wrist2_link'])
    EE_link_geo_set = GeometrySet(frame_name_to_id['UR5::EE_link'])
    scoop_link_geo_set = GeometrySet(frame_name_to_id['UR5::scoop_link'])
    probe_link_geo_set = GeometrySet(frame_name_to_id['UR5::probe_link'])

    collision_filter_declaration = CollisionFilterDeclaration()
    collision_filter_declaration.ExcludeWithin(ground_geo_set)
    collision_filter_declaration.ExcludeWithin(base_link_geo_set)
    collision_filter_declaration.ExcludeWithin(shoulder_link_geo_set)
    collision_filter_declaration.ExcludeWithin(upperarm_link_geo_set)
    collision_filter_declaration.ExcludeWithin(forearm_link_geo_set)
    collision_filter_declaration.ExcludeWithin(wrist1_link_geo_set)
    collision_filter_declaration.ExcludeWithin(wrist2_link_geo_set)
    collision_filter_declaration.ExcludeWithin(EE_link_geo_set)
    collision_filter_declaration.ExcludeWithin(scoop_link_geo_set)
    collision_filter_declaration.ExcludeWithin(probe_link_geo_set)
    collision_filter_declaration.ExcludeBetween(ground_geo_set, base_link_geo_set)
    collision_filter_declaration.ExcludeBetween(base_link_geo_set, shoulder_link_geo_set)
    collision_filter_declaration.ExcludeBetween(shoulder_link_geo_set, upperarm_link_geo_set)
    collision_filter_declaration.ExcludeBetween(upperarm_link_geo_set, forearm_link_geo_set)
    collision_filter_declaration.ExcludeBetween(forearm_link_geo_set, wrist1_link_geo_set)
    collision_filter_declaration.ExcludeBetween(wrist1_link_geo_set, wrist2_link_geo_set)
    collision_filter_declaration.ExcludeBetween(wrist2_link_geo_set, EE_link_geo_set)
    collision_filter_declaration.ExcludeBetween(wrist2_link_geo_set, scoop_link_geo_set)
    collision_filter_declaration.ExcludeBetween(EE_link_geo_set, scoop_link_geo_set)
    collision_filter_declaration.ExcludeBetween(probe_link_geo_set, scoop_link_geo_set)
    collision_filter_manager.Apply(collision_filter_declaration)

This is how I used for IK

builder = DiagramBuilder()
plant, scene_graph = AddMultibodyPlantSceneGraph(builder, time_step=0)
parser = Parser(plant)
robot = parser.AddModels(self._bot_urdf_path)
__exclude_collision_pairs(scene_graph)  
plant.Finalize()
diagram = builder.Build()
ik_context = diagram.CreateDefaultContext()
ik_plant_context = plant.GetMyMutableContextFromRoot(ik_context)
ee_frame = plant.GetFrameByName(self._bot_params['ee_link_name'])
ik = InverseKinematics(plant, ik_plant_context)
# ik.AddMinimumDistanceLowerBoundConstraint(bound=0.0001, influence_distance_offset=0.0005) # hard-coded
ik.prog().AddBoundingBoxConstraint(plant.GetPositionLowerLimits(),  # [-2pi, 2pi]
    plant.GetPositionUpperLimits(), ik.q()) 
if init_guess is None:
    init_guess = self.GetConfiguration()
ik.prog().SetInitialGuess(ik.q(), init_guess)
ik.prog().AddQuadraticCost(
    (ik.q() - init_guess).dot(np.diag(weights)).dot(ik.q() - init_guess))
ik.AddPositionConstraint(ee_frame, [0, 0, 0], plant.world_frame(), 
    translation - 1e-5 * np.ones(3), translation + 1e-5 * np.ones(3))
ik.AddOrientationConstraint(ee_frame, RotationMatrix(), plant.world_frame(), 
                            RotationMatrix(orientation), 1e-5)
result = Solve(ik.prog())
if kVerbose and result.get_solver_id().name() != 'SNOPT':
    print("ScoopingBot::__inverse_kinematics -- SNOPT is not available")
if kVerbose and not result.is_success():
    print("ScoopingBot::__inverse_kinematics -- IK failed")
    print(result.GetInfeasibleConstraintNames(ik.prog()))
    return False, None
return True, result.GetSolution(ik.q())

I doubled check collision pairs after filtering, those I intended to filter out were successfully filtered out. But when I apply the min dist lb constraint, even with lb = 0, it's not working. I checked the infeasible constraints reported by SNOPT and the best effort solution it can give, I notices two cases. (1) The min dist constraint was violated (2) the min dist constraint was not violated by the position constraint (target translation) and orientation constraint (target orientation) needs 1e-2 relaxation, which is not acceptable, that's 1cm difference. Without applying this min dist constraint, the IK worked perfectly fine, even without relaxation on translation and 1e-3~1e-5 relaxation on orientation. BTW, if I apply min dist constraint but with lb = -100, it worked and it's collision free as well.