如何在Drake中检查6R机器人自碰撞及ComputePointPairPenetration适用性？

核心结论 #

Drake完全支持机器人自碰撞检测，包括迭代关节位姿下的连杆碰撞检查，ComputePointPairPenetration()也适用于同一机器人的不同连杆以及不同实体间的碰撞计算。

具体实现步骤 #

1. 准备机器人模型与碰撞检测环境

首先加载你的6R机器人URDF/SDF模型，确保模型包含碰撞几何体定义（URDF中的<collision>标签），然后初始化MultibodyPlant和SceneGraph，并创建碰撞检测器：

from pydrake.multibody.plant import MultibodyPlant, Parser
from pydrake.geometry import SceneGraph, CollisionChecker

# 初始化多体植物与场景图
plant = MultibodyPlant(time_step=0.0)
scene_graph = SceneGraph()
plant.RegisterAsSourceForSceneGraph(scene_graph)

# 加载6R机器人模型（替换为你的模型路径）
parser = Parser(plant)
parser.AddModelFromFile("your_6r_robot.urdf")
plant.Finalize()

# 创建碰撞检测器，默认启用自碰撞检查
collision_checker = CollisionChecker(plant, scene_graph)

2. 迭代关节位姿的自碰撞检测

在遍历工作空间关节位姿时，每次设置关节位置后调用碰撞检测接口，判断当前位姿是否存在自碰撞：

import numpy as np

# 示例：随机采样关节位姿（实际可替换为工作空间遍历逻辑）
num_joints = plant.num_positions()
for _ in range(100):
    # 生成符合关节限位的随机位姿
    q = np.random.uniform(plant.GetPositionLowerLimits(), plant.GetPositionUpperLimits())
    # 创建上下文并设置关节位置
    context = plant.CreateDefaultContext()
    plant.SetPositions(context, q)
    # 检查自碰撞（关闭环境碰撞检测）
    collision_result = collision_checker.CheckConfigCollision(
        context, self_collisions=True, environment_collisions=False
    )
    if collision_result.has_collision:
        print(f"关节位姿 {q} 存在自碰撞")
        # 输出具体碰撞的连杆对
        for pair in collision_result.collision_pairs:
            link_a = plant.GetBodyAtIndex(pair.bodyA_index).name()
            link_b = plant.GetBodyAtIndex(pair.bodyB_index).name()
            print(f"碰撞连杆: {link_a} 和 {link_b}")

3. 使用ComputePointPairPenetration()做细粒度检测

ComputePointPairPenetration()可计算两个几何体的穿透深度与接触点，同时支持同一机器人的不同连杆、不同实体（如机器人与环境物体）。使用示例：

from pydrake.geometry import QueryObject

# 获取场景查询对象
query_object = scene_graph.get_query_output_port().Eval(context)
# 指定需要检测的两个连杆（替换为你的连杆名称）
link_a = plant.GetBodyByName("link_2")
link_b = plant.GetBodyByName("link_5")
# 获取连杆对应的碰撞几何体ID
geom_id_a = plant.GetCollisionGeometriesForBody(link_a)[0]
geom_id_b = plant.GetCollisionGeometriesForBody(link_b)[0]

# 计算穿透信息
penetration = query_object.ComputePointPairPenetration(geom_id_a, geom_id_b)
if penetration.depth > 0:
    print(f"连杆 {link_a.name()} 与 {link_b.name()} 穿透深度: {penetration.depth}")
    print(f"A连杆坐标系下接触点: {penetration.pointA}")
    print(f"B连杆坐标系下接触点: {penetration.pointB}")

关键注意事项 #

• 模型的碰撞几何体定义要准确：过粗会导致误检，过细则会增加计算开销。
• CollisionChecker默认过滤相邻连杆（关节连接的连杆）的碰撞，若需检测这类碰撞，可通过AddCollisionPair手动添加碰撞对。
• 工作空间分析时，建议结合蒙特卡洛采样等算法遍历关节空间，用碰撞检测过滤掉碰撞位姿，得到无碰撞工作空间。

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