机器人 / 系统课时

检查自碰撞矩阵与物体碰撞结果

检查自碰撞矩阵与物体碰撞结果

机械臂在 RViz 中看似没有穿模,规划却可能失败;相邻连杆即使表面接触,也可能被 MoveIt 2 正常忽略。本篇以 ROS 2 Jazzy 的 Panda 演示场景为例,核对允许碰撞矩阵,并读取当前姿态的接触对,判断问题来自自身、环境还是安全间隙。

学习目标

  • 读懂 SRDF 中 disable_collisions 与运行时 Allowed Collision Matrix(ACM)的关系。
  • 查询 move_group 的碰撞有效性与接触对象,定位自碰撞和环境碰撞。
  • 区分几何相交与净空不足,避免用“没有报碰撞”代替安全验证。

分清三类检查结果

PlanningScene 结合机器人关节状态、URDF 碰撞几何、场景物体和 ACM 做判定。checkSelfCollision() 检查机器人自身连杆之间的碰撞;checkCollision() 同时检查机器人与环境、机器人自身的碰撞。ACM 中某对对象被允许碰撞,意味着忽略该对的碰撞检测,并不意味着两个实体真的不会相交。

观察结果 典型对象 应如何判断
自碰撞 panda_link3 与另一非相邻连杆 接触双方都属于 ROBOT_LINK
环境碰撞 机械臂连杆与 box_1 一方属于 WORLD_OBJECT
净空不足 末端与箱体相距 8 mm 尚未相交,常没有接触记录,但可能低于项目要求的 20 mm 间隙

自碰撞、环境碰撞与净空不足的判定对比

审查 SRDF 的默认豁免

配置包的 config/panda.srdf(有些工程以 .srdf.xacro 生成)通过 disable_collisions 记录默认忽略的连杆对。例如 Panda 参考配置中,panda_link0 与 panda_link1 的原因为 Adjacent,panda_link0 与 panda_link2 的原因为 Never。运行时 ACM 会据此初始化;true 表示允许忽略,未豁免的配对通常仍需检测。

  1. 打开 MoveIt Setup Assistant,进入 Self-Collisions,查看已生成的矩阵;需要重算时使用 Generate Collision Matrix。
  2. 对每个勾选项核对连杆名及原因:Adjacent 表示相邻,Never 表示采样中未碰撞,Default 表示默认姿态接触,Always 表示采样中持续碰撞。
  3. 重点复核非相邻连杆、夹爪和新装工具。默认采样通常检查 10,000 个随机姿态,但“采样未碰撞”并不是永不碰撞的数学证明。

查询运行时矩阵和接触对

以下适用于 Ubuntu 24.04 和 ROS 2 Jazzy。在已加载 ROS 环境的第一终端执行 ros2 launch moveit_resources_panda_moveit_config demo.launch.py,预期 RViz 显示 Panda 且 move_group 提供场景服务。第二终端如使用源码工作空间,还需加载对应 install/setup.bash。程序查询 /get_planning_scene 和 /check_state_validity;panda_arm_hand 是参考配置中的组合规划组。

bash
source /opt/ros/jazzy/setup.bash
python3 - <<'PY'
import rclpy
from moveit_msgs.msg import ContactInformation, PlanningSceneComponents
from moveit_msgs.srv import GetPlanningScene, GetStateValidity

def call(node, srv_type, name, request):
    client = node.create_client(srv_type, name)
    if not client.wait_for_service(timeout_sec=5.0):
        raise RuntimeError(f'服务不可用: {name}')
    future = client.call_async(request)
    rclpy.spin_until_future_complete(node, future, timeout_sec=5.0)
    if not future.done() or future.result() is None:
        raise RuntimeError(f'请求失败或超时: {name}')
    return future.result()

def main():
    rclpy.init()
    node = rclpy.create_node('acm_collision_probe')
    try:
        request = GetPlanningScene.Request()
        # 2 | 128:请求当前 RobotState 与 Allowed Collision Matrix
        request.components.components = (
            PlanningSceneComponents.ROBOT_STATE |
            PlanningSceneComponents.ALLOWED_COLLISION_MATRIX
        )
        scene = call(node, GetPlanningScene, '/get_planning_scene', request).scene
        acm = scene.allowed_collision_matrix
        names, rows = list(acm.entry_names), acm.entry_values
        square = bool(names) and len(names) == len(rows) and all(
            len(row.enabled) == len(names) for row in rows
        )
        pair = ('panda_link0', 'panda_link1')
        allowed = square and all(n in names for n in pair) and (
            rows[names.index(pair[0])].enabled[names.index(pair[1])]
        )
        print('ACM 行列一致:', square)
        print('相邻连杆豁免:', allowed)

        check = GetStateValidity.Request()
        check.robot_state = scene.robot_state
        check.group_name = 'panda_arm_hand'
        result = call(node, GetStateValidity, '/check_state_validity', check)
        print('当前状态有效:', result.valid)
        print('接触数:', len(result.contacts))
        # 接触类型取自响应,不以视觉上是否接触来猜测
        for c in result.contacts[:10]:
            kinds = (c.body_type_1, c.body_type_2)
            if kinds == (ContactInformation.ROBOT_LINK,) * 2:
                label = '自碰撞'
            elif ContactInformation.WORLD_OBJECT in kinds:
                label = '环境碰撞'
            else:
                label = '附着物相关接触'
            print(f'{label}: {c.contact_body_1} / {c.contact_body_2}')
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == '__main__':
    main()
PY

预期输出(使用参考 Panda 配置、当前姿态无碰撞且未放置障碍物):

text
ACM 行列一致: True
相邻连杆豁免: True
当前状态有效: True
接触数: 0

如果出现接触,程序会在后面列出实际对象名称及类型;其数量随姿态和场景变化。若服务不可用,用 ros2 service list -t 检查实际命名空间。这里检查的是 PlanningScene 中的当前 RobotState,而不是 RViz 中尚未执行的目标姿态,也不能代表整条规划轨迹。

用障碍物复测并定位失败

  1. 在 RViz 的 MotionPlanning → Scene Objects 中添加 Box,先使用边长 0.10 m 的立方体。将它移到当前机械臂某个连杆的碰撞几何内部,按界面中的 Publish Scene 或 Publish Current Scene 提交更新;不同 RViz 插件版本的按钮位置可能不同。
  2. 重新执行上面的命令。确实相交时应出现 当前状态有效: False 和接触对;其中一方的类型应为 WORLD_OBJECT。若仍是零接触,先确认箱体已经同步到 move_group 使用的场景,再检查箱体坐标系与位置。
  3. 删除箱体并发布更新,再次检查应恢复为零接触。排查自身连杆接触时,应把待验证的候选关节状态填入 GetStateValidity.Request.robot_state,而不是仅在 RViz 拖动目标交互标记;两个 ROBOT_LINK 构成的接触对才是自碰撞证据。

RViz 场景障碍物编辑和发布按钮位置

不把净空不足当成碰撞

二值碰撞检查只回答按当前碰撞几何与 ACM 规则是否发生了被检测的相交。假设项目规定机器人与固定障碍物至少保留 20 mm 净空,那么实测距离 8 mm 即使没有接触记录,也比要求少 12 mm;25 mm 才满足这个示例阈值。20 mm 是便于演示的工程判据,不能直接当作所有机器人的安全标准。

GetStateValidity 返回的 valid 和 contacts 不提供通用的最小净距证明。需要距离数据时,可通过碰撞检测器的距离查询接口另行获取;例如 PlanningScene::distanceToCollision() 用于机器人与环境的最近距离,不能直接用它判断自碰撞净距。环境侧的 link_padding 也可能改变碰撞触发位置,应结合实际配置检查。

验收时记录三项结果:ACM 豁免是否合理、接触对是否对应真实对象、最近间隙是否达到项目阈值。只要其中一项没有核对,就不能仅凭 RViz 没有红色提示认定规划安全。

按自己的节奏,确认真正理解。

目录与阅读设置

阅读设置

17

记录一个想法

检查自碰撞矩阵与物体碰撞结果

0 / 10000 · 笔记保存到账号。未提交的修改会在关闭、离开或刷新后丢失。

登录哪都通