PyDrake中CollisionFilterManager碰撞过滤未生效问题排查

我搭建了一个包含机械臂与物体的系统，希望计算系统内所有碰撞几何的有符号距离，但需排除末端执行器指尖与物体碰撞几何之间的碰撞计算。然而使用CollisionFilterManager设置排除规则后，后续调用ComputeSignedDistancePairClosestPoints时，仍会计算指尖与物体几何的有符号距离。

我编写了一个包含Plant和SceneGraph的容器类，在初始化阶段尝试配置碰撞过滤，相关代码如下：

class SimplifiedClass:

    def __init__(self, ...):
        # initializing plant, contexts, and query object port
        self.diagram = function_generating_diagram_with_plant(...)
        self.plant = self.diagram.GetSubsystemByName("plant")
        self.scene_graph = self.diagram.GetSubsystemByName("scene_graph")

        _diag_context = self.diagram.CreateDefaultContext()
        self.plant_context = self.plant.GetMyMutableContextFromRoot(_diag_context)
        self.sg_context = self.scene_graph.GetMyMutableContextFromRoot(_diag_context)

        self.qo_port = self.scene_graph.get_query_output_port()

        # applying filters
        cfm = self.scene_graph.collision_filter_manager()
        inspector = self.query_object.inspector()

        fingertip_geoms = []
        obj_collision_geoms = []

        gids = inspector.GetAllGeometryIds()
        for g in gids:
            name = inspector.GetName(g)

            # if name.endswith("ds_collision") or name.endswith("collision_1"):
            if name.endswith("tip_collision_1") and "algr" in name:
                fingertip_geoms.append(g)
            elif name.startswith("obj_collision"):
                obj_collision_geoms.append(g)

        ftip_set = GeometrySet(fingertip_geoms)
        obj_set = GeometrySet(obj_collision_geoms)

        cfm.Apply(
            CollisionFilterDeclaration()
            .ExcludeBetween(ftip_set, obj_set)
            .ExcludeWithin(obj_set)
        )

    @property
    def query_object(self):
        return self.qo_port.Eval(self.sg_context)

    def function_that_computes_signed_distances(self, ...):
        # this function calls ComputeSignedDistancePairClosestPoints(), which
        # computes signed distances even between filtered geometry pairs

我已确认初始化时获取到了正确的几何ID（名称匹配），但测试发现设置过滤前后，ComputeSignedDistancePairClosestPoints返回的SignedDistancePairs数量一致，说明过滤规则未生效，且下游调用中仍能看到应被过滤的几何对。

请问代码中是否存在明显bug？若没有，还可能在哪些环节出现问题？

代码中的明显Bug #

• 上下文不匹配：初始化时创建的_diag_context是临时默认上下文，后续计算时如果使用的是另一个更新后的上下文（比如实时运行的上下文），那么初始化时应用的过滤规则不会生效。CollisionFilterManager的规则是绑定到上下文的，必须确保规则应用到实际用于计算的上下文上。
• Query Object初始化时机过早：在__init__中调用self.query_object获取inspector时，系统可能尚未完成所有几何的加载或状态同步，虽然名称匹配正确，但此时的GeometryId可能和后续运行时的ID存在差异（比如动态加载的几何元素）。

其他可能的问题环节 #

• ComputeSignedDistancePairClosestPoints调用参数缺失：如果调用该函数时没有传入当前的CollisionFilterManager实例，或者没有启用过滤选项，过滤规则不会自动生效。需要确保调用时明确使用scene_graph.collision_filter_manager()来应用规则。
• 几何集合匹配漏洞：检查名称匹配逻辑是否覆盖了所有需要排除的几何，比如是否存在大小写不一致、后缀前缀匹配不精确的情况，导致部分指尖或物体几何未被加入排除集合。
• 系统状态更新后未重新应用规则：如果后续Plant有动态修改（比如添加/移除几何、关节状态变更），初始化时设置的过滤规则可能失效，需要在状态更新后重新应用过滤规则。
• 冲突的过滤规则：如果之前存在其他允许特定几何对的过滤规则，可能会覆盖当前的排除规则。可以调用cfm.GetDeclaration()查看当前生效的规则是否符合预期。

内容的提问来源于stack exchange，提问作者robodobo