pydrake中iiwa机械臂碰撞误报的排查与解决问询

一、更优的IIWA碰撞检测方案 #

• 使用更精细的碰撞模型：替换当前的iiwa7_with_box_collision.sdf，官方提供的iiwa7_with_cylinder_collision.sdf采用圆柱碰撞体，更贴合机械臂连杆形状，能大幅减少包围盒带来的误报。如果需要更高精度，可以基于iiwa7_no_collision.sdf手动添加凸包（convex hull）碰撞几何体，凸包能在保证计算效率的前提下更精准地匹配连杆实际轮廓。
• 校验关节限位配置：确认SDF中的<limit>关节限制与你逆运动学模块设置的范围完全一致，避免因模型本身的碰撞体在合法关节角度内出现不合理重叠。

二、跳过特定物体间的碰撞检测 #

1. 获取GeometryId对应的可读名称 #

可以通过SceneGraph和Plant的关联接口，将GeometryId映射到对应的连杆/框架名称，示例代码：

from pydrake.geometry import GeometryId

def get_geometry_related_names(scene_graph, geometry_id: GeometryId):
    frame_id = scene_graph.GetFrameId(geometry_id)
    plant = scene_graph.GetSourcePlant(frame_id)
    body = plant.GetBodyFromFrameId(frame_id)
    frame = plant.get_frame(frame_id)
    return f"连杆: {body.name()}, 框架: {frame.name()}"

调用此函数即可通过GeometryId得到对应的连杆名称，方便定位误报的碰撞对。

2. 配置碰撞过滤规则 #

Drake提供CollisionFilterDeclaration接口，可显式排除指定几何体对的碰撞检测，步骤如下：

from pydrake.geometry import CollisionFilterDeclaration

# 假设已定位到误报的两个几何体ID: geom_id_a, geom_id_b
filter_decl = CollisionFilterDeclaration()
# 排除这两个几何体之间的碰撞检测
filter_decl.ExcludeBetween(geom_id_a, geom_id_b)
# 将规则应用到SceneGraph上下文
scene_graph.ApplyCollisionFilter(scene_graph_context, filter_decl)

该配置会永久生效，后续QueryObject检测时会自动跳过这对几何体的碰撞检查。

3. 优化临时过滤方案 #

如果暂时无法通过名称定位几何体，可优化你的临时方案，仅存储碰撞对的ID组合（无序）来过滤误报：

class SimulationModel:
    # ...
    def get_collision_id_pairs(self) -> set:
        collisions = self.get_collisions()
        collision_pairs = set()
        for coll in collisions:
            # 用排序后的元组存储，避免顺序影响匹配
            pair = tuple(sorted((coll.id_A, coll.id_B)))
            collision_pairs.add(pair)
        return collision_pairs

    def __init__(self, ...):
        # ...
        self.robot_arm.set_joint_angles(BAD_ANGLES)
        self.fake_collision_pairs = self.get_collision_id_pairs()
        self.robot_arm.set_joint_angles(DEFAULT_ANGLES)

    def new_has_collisions(self) -> bool:
        current_pairs = self.get_collision_id_pairs()
        real_collisions = current_pairs - self.fake_collision_pairs
        return len(real_collisions) != 0

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