SceneGraphCollisionChecker设置的碰撞过滤器为何在IK约束中不生效？

问题描述 #

我认为SceneGraphCollisionChecker的API可以对场景图的碰撞过滤器进行永久修改或添加，这个假设是否正确？

我手动向MultibodyPlant添加了一个碰撞体，它会与非相邻连杆中的另一刚体发生碰撞。由于碰撞刚体不属于相邻连杆，MultibodyPlant::Finalize并未将该碰撞对加入过滤器。我尝试用SceneGraphCollisionChecker识别并过滤这类“默认”碰撞，但后续调用带有MinimumDistanceConstraint的MathematicalProgram时，因约束不可行失败（报错信息：drake::multibody::MinimumDistanceConstraint[0]: -inf <= 3.000000 <= 1.000000），推测场景图并未实际过滤该碰撞对。而手动向场景图添加过滤器声明后，约束恢复可行。

我是否操作失误？还是SceneGraphCollisionChecker并未真正为场景图设置永久碰撞过滤器？我使用的是Drake 1.19.0版本。

复现代码 #

resource_file = 'drake/manipulation/models/iiwa_description/urdf/iiwa14_spheres_dense_collision.urdf'
rdb = RobotDiagramBuilder(time_step=0)
resource = FindResourceOrThrow(resource_file)
model = rdb.parser().AddModelFromFile(resource)
n_pos = rdb.plant().num_positions()

# 手动添加带碰撞几何的刚体，默认会发生碰撞
... 

# 完成Plant初始化
plant.Finalize()

# 构建Diagram
diagram = rdb.Build()

# 检查碰撞并添加过滤规则
sgcc = SceneGraphCollisionChecker(model=diagram, robot_model_instances=[model],
                                  configuration_distance_function=l2_dist, edge_step_size=0.01,
                                  env_collision_padding=0.0, self_collision_padding=0.0)

nominal_state = np.zeros((n_pos,))

# 检查碰撞状态
collision_free = sgcc.CheckConfigCollisionFree(nominal_state)  # 返回False

clearance = sgcc.CalcRobotClearance(nominal_state, 0.0)
for idx in np.argwhere(clearance.distances() < 0.0).flatten():
    body_1 = sgcc.plant().get_body(clearance.robot_indices()[idx])
    body_2 = sgcc.plant().get_body(clearance.other_indices()[idx])
    dist = clearance.distances()[idx]
    print(f'碰撞发生在 \'{body_1.name()}\' 和 \'{body_2.name()}\' 之间，距离为 {dist}')
    sgcc.SetCollisionFilteredBetween(body_1, body_2, True)

# 再次检查碰撞状态
collision_free = sgcc.CheckConfigCollisionFree(nominal_state)  # 返回True

# 求解带最小距离约束的IK优化 -> 因约束不可行失败
...

# 手动添加过滤声明
g1 = diagram.plant().CollectRegisteredGeometries([diagram.plant().GetBodyByName('body_a')])
g2 = diagram.plant().CollectRegisteredGeometries([diagram.plant().GetBodyByName('body_b')])
decl = CollisionFilterDeclaration()
decl.ExcludeBetween(g1, g2)
diagram.scene_graph().collision_filter_manager().Apply(decl)

# 再次求解IK优化 -> 约束可行，求解成功

更新：补充IK求解代码 #

我原本以为场景图对象会保存新的碰撞过滤器：

# 求解带最小距离约束的IK优化
plant_context = diagram.plant().GetMyContextFromRoot(diagram.CreateDefaultContext())
ik = InverseKinematics(diagram.plant(), plant_context)

# 添加约束
ik.AddMinimumDistanceConstraint(0.010, 0.1)
...

result = Solve(ik.prog(), initial_guess=...)  # 因约束不可行失败

解答 #

你的假设不正确，SceneGraphCollisionChecker并不会永久修改原始Diagram/SceneGraph中的碰撞过滤器，核心原因如下：

1. 实例隔离机制：SceneGraphCollisionChecker初始化时会创建原始Diagram的内部副本，所有碰撞过滤操作（比如SetCollisionFilteredBetween）仅作用于这个副本，而非你构建的原始diagram对象。这就是为什么sgcc.CheckConfigCollisionFree会返回True（副本的过滤器已更新），但原始diagram的MinimumDistanceConstraint依然检测到碰撞——原始SceneGraph的过滤器根本没被修改。

2. 操作逻辑差异：你手动调用diagram.scene_graph().collision_filter_manager().Apply(decl)时，是直接修改了原始SceneGraph的过滤器，所以后续IK求解能成功。

正确做法 #

如果你想基于SceneGraphCollisionChecker的结果修改原始Diagram的碰撞过滤器，需要：

• 从sgcc中提取需要过滤的碰撞对信息
• 按照你手动操作的方式，直接对原始diagram的SceneGraph应用CollisionFilterDeclaration

若希望IK求解使用SceneGraphCollisionChecker中的过滤规则，也可以基于sgcc内部的Diagram副本创建InverseKinematics实例，但这种方式会脱离原本的Diagram上下文，一般不推荐。

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