Drake中SceneGraphCollisionChecker碰撞过滤异常问题咨询

我在使用Drake检测包含桌子、机械臂和桌面零件的场景碰撞时，调用SceneGraphCollisionChecker.CalcContextRobotClearance方法检测到桌子（ingress_table_link）与零件（part1_link）存在负距离（即碰撞）——但这是预期接触，需要忽略该碰撞。尝试为两者添加碰撞过滤器时，触发Drake内部错误：

RuntimeError: Drake internal error at planning/scene_graph_collision_checker.cc:217 in DoCalcContextRobotClearance(): Collision between bodies [ingress_table::ingress_table_link] and [part1::part1_link] should already be filtered

相关代码如下（通过set_collision_filter参数控制是否设置碰撞过滤器）：

from pathlib import Path
from time import time

from pydrake.all import (
    CollisionCheckerContext,
    Meshcat,
    RigidTransform,
    RobotDiagramBuilder,
    RollPitchYaw,
    SceneGraphCollisionChecker,
)
from pydrake.systems.analysis import Simulator
from pydrake.visualization import AddDefaultVisualization

ROOT_PATH = Path(__file__).parent.parent.parent.as_posix()
ROBOT_URDF_PATH = ROOT_PATH + "/component_configs/abb/urdf/irb1200_7_70.urdf"
PART1_URDF_PATH = ROOT_PATH + "/resources/test_objects/part1.urdf"
TABLE_URDF_PATH = ROOT_PATH + "/component_configs/ingress_table/urdf/ingress_table.urdf"


def launch_abb_simulator(
    time_step: float,
    realtime_rate: float,
    set_collision_filter: bool = False,
) -> None:
    robot_diagram_builder = RobotDiagramBuilder(time_step=0)
    builder = robot_diagram_builder.builder()
    plant = robot_diagram_builder.plant()
    parser = robot_diagram_builder.parser()

    # Add robot
    robot_model = parser.AddModels(ROBOT_URDF_PATH)
    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("base_link"))

    # Add table
    table_model = parser.AddModels(TABLE_URDF_PATH)
    translation = [0, 0.5, 0.325]
    rotation = RollPitchYaw(0, 0, 0).ToQuaternion()
    table_transform = RigidTransform(rotation, translation)
    plant.WeldFrames(plant.world_frame(), plant.GetFrameByName("ingress_table_link"), table_transform)

    # Add the part object
    part_model = parser.AddModels(PART1_URDF_PATH)

    # Finalize the scene and add visualization
    plant.Finalize()
    meshcat = Meshcat()
    AddDefaultVisualization(builder, meshcat)
    diagram = robot_diagram_builder.Build()
    simulator = Simulator(diagram)

    # Set context
    collision_context = CollisionCheckerContext(diagram)
    # plant_context = collision_context.plant_context()
    simulator_context = simulator.get_mutable_context()
    plant_sim_context = plant.GetMyContextFromRoot(simulator_context)

    # Set the initial positions, with the part on the table
    robot_joint_positions = [0, 0, 0, 0, 0, 0]
    part_quaternion = [1, 0, 0, 0]
    part_translation = [0, 0.5, 0.4]
    positions = robot_joint_positions + part_quaternion + part_translation

    plant.SetPositions(plant_sim_context, positions)

    # Make a collision checker
    collision_checker = SceneGraphCollisionChecker(
        model=diagram,
        robot_model_instances=robot_model + table_model + part_model,
        edge_step_size=0.01,
        env_collision_padding=0.0,
        self_collision_padding=0.0,
    )

    if set_collision_filter:
        collision_checker.SetCollisionFilteredBetween(
            plant.GetBodyByName("ingress_table_link"),
            plant.GetBodyByName("part1_link"),
            filter_collision=True,
        )

    simulator.Initialize()
    simulator.set_publish_every_time_step(False)
    simulator.set_target_realtime_rate(realtime_rate)

    while True:
        meshcat.PublishRecording()
        simulator.AdvanceTo(simulator_context.get_time() + time_step)

        clearance = collision_checker.CalcContextRobotClearance(
            collision_context,
            plant.GetPositions(plant_sim_context),
            100,
        )
        for i, dist in enumerate(clearance.distances()):
            if dist < 0:
                robot = clearance.robot_indices()[i]
                other = clearance.other_indices()[i]
                print(f"{time()}: {plant.get_body(robot)} is in collision with {plant.get_body(other)}")


if __name__ == "__main__":
    launch_abb_simulator(
        time_step=3e-3,
        realtime_rate=1.0,
        set_collision_filter=False,
    )

问题根源 #

SceneGraphCollisionChecker通过robot_model_instances参数区分机器人部件和环境部件：只有被指定为机器人的部件才会被主动检查碰撞，环境部件之间的碰撞Drake要求必须在plant.Finalize()之前完成过滤，而不是在运行时通过SetCollisionFilteredBetween设置。

你之前把桌子、零件都加入了robot_model_instances，导致Drake将它们视为机器人部件，但桌子是焊接到世界的固定部件，两者的碰撞逻辑不符合机器人部件的检查规则，从而触发内部错误。

解决方案 #

1. 区分机器人与环境部件 #

仅将机械臂设为robot_model_instances，桌子和零件归为环境部件。

2. 在Plant构建阶段提前设置碰撞过滤 #

在plant.Finalize()之前，调用CollisionFilterDeclaration过滤桌子和零件的碰撞。

修改后的核心代码片段 #

# ... 原有添加模型的代码 ...

# Add the part object
part_model = parser.AddModels(PART1_URDF_PATH)

# 在Finalize之前添加碰撞过滤：忽略桌子和零件的碰撞
table_body = plant.GetBodyByName("ingress_table_link")
part_body = plant.GetBodyByName("part1_link")
plant.CollisionFilterDeclaration().ExcludeBetween(
    table_body.collision_geometry_ids(),
    part_body.collision_geometry_ids()
)

# Finalize the scene and add visualization
plant.Finalize()

# ... 后续上下文设置代码 ...

# 初始化碰撞检查器：仅将机械臂设为机器人部件
collision_checker = SceneGraphCollisionChecker(
    model=diagram,
    robot_model_instances=robot_model,  # 仅包含机械臂模型实例
    edge_step_size=0.01,
    env_collision_padding=0.0,
    self_collision_padding=0.0,
)

# 移除原有的SetCollisionFilteredBetween调用，因为已在Plant阶段完成过滤

效果说明 #

修改后，桌子和零件的碰撞会被提前过滤，CalcContextRobotClearance只会检查机械臂与环境（桌子、零件）的碰撞以及机械臂自碰撞，既不会触发内部错误，也不会报告桌子和零件的预期碰撞。

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