机器人 / 系统课时

定位障碍物或自碰撞误判

定位障碍物或自碰撞误判

Panda 在 RViz 中看似避开桌面却始终规划失败,应先核对 MoveIt 2 实际使用的碰撞场景。本课以 ROS 2 Jazzy 为例,读取障碍物、接触对与允许碰撞关系,定位几何尺寸、坐标变换或自碰撞配置造成的误判。

学习目标

  • 在 RViz 中区分可视网格、碰撞几何与场景障碍物。
  • 查询 move_group 使用的场景和碰撞对,锁定误判来源。
  • 修正碰撞对象或 ACM,并用同一规划请求复测。

固定失败现场并查看碰撞几何

先启动包含 Panda、move_group 和 RViz 的 MoveIt 2 演示或项目配置,确认规划组为 panda_arm。记录失败时的起点关节角、目标位姿、规划时间及场景版本;排错期间停止机械臂运动,只点击 Plan,不要点击 Execute。

在 RViz 左侧 Displays 中展开 MotionPlanning 或 PlanningScene 显示项:

  1. 在 Scene Geometry 中开启 Show Scene Geometry,检查桌子、箱体等场景物体是否存在。
  2. 在 Scene Robot 中关闭 Show Robot Visual、开启 Show Robot Collision,看真正参与碰撞检查的机器人外形。
  3. 检查 Global Options → Fixed Frame 能通过 TF 连到 panda_link0;如果场景中有 world,先确认 world 与基座的变换,不要仅靠画面位置判断是否相交。

在已加载 ROS 2 Jazzy 和项目工作空间的终端执行:

bash
ros2 service list -t | grep -E 'get_planning_scene|check_state_validity'

预期输出(采用默认服务命名的 move_group):

text
/check_state_validity [moveit_msgs/srv/GetStateValidity]
/get_planning_scene [moveit_msgs/srv/GetPlanningScene]

如果服务带命名空间,后面的程序需同步修改服务名称;若查询不到服务,先检查 move_group 是否启动及状态验证能力是否启用。

用服务取出碰撞对象和接触对

只观察 RViz 画面不能证明规划器内部看到的物体相同。以下程序先从 /get_planning_scene 取得世界对象、附着对象与机器人状态,再把该状态交给 /check_state_validity,列出发生碰撞的对象名称。将代码保存为 scene_collision_probe.py。

python
import rclpy
from rclpy.node import Node
from moveit_msgs.msg import PlanningSceneComponents
from moveit_msgs.srv import GetPlanningScene, GetStateValidity

def call_service(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 = Node("scene_collision_probe")
    try:
        scene_req = GetPlanningScene.Request()
        c = PlanningSceneComponents
        # 同时取出附着物,避免把抓取对象误当成普通世界障碍物
        scene_req.components.components = (
            c.ROBOT_STATE_ATTACHED_OBJECTS
            | c.WORLD_OBJECT_GEOMETRY
            | c.ALLOWED_COLLISION_MATRIX
        )
        scene = call_service(
            node, GetPlanningScene, "/get_planning_scene", scene_req
        ).scene

        world = scene.world.collision_objects
        attached = scene.robot_state.attached_collision_objects
        print(f"world={len(world)} attached={len(attached)}")
        for obj in world:
            p = obj.pose.position
            print(
                f"object={obj.id} frame={obj.header.frame_id} "
                f"origin=({p.x:.3f},{p.y:.3f},{p.z:.3f})"
            )
            for primitive, pose in zip(obj.primitives, obj.primitive_poses):
                print(
                    f"  shape={primitive.type} dims={list(primitive.dimensions)} "
                    f"local_z={pose.position.z:.3f}"
                )

        validity_req = GetStateValidity.Request()
        validity_req.robot_state = scene.robot_state
        validity_req.group_name = "panda_arm"
        verdict = call_service(
            node, GetStateValidity, "/check_state_validity", validity_req
        )
        # 同一物体对可能产生多个接触点,这里按物体对去重
        pairs = sorted({
            tuple(sorted((hit.contact_body_1, hit.contact_body_2)))
            for hit in verdict.contacts
        })
        print(f"valid={verdict.valid} collision_pairs={len(pairs)}")
        for left, right in pairs:
            print(f"collision={left} <-> {right}")
    finally:
        node.destroy_node()
        rclpy.shutdown()

if __name__ == "__main__":
    main()

在已配置环境的终端运行 python3 scene_collision_probe.py。预期输出格式如下,具体计数与名称由现场场景决定:

text
world=<N> attached=<M>
object=<ID> frame=<FRAME> origin=(<X>,<Y>,<Z>)
  shape=<TYPE> dims=[<DIMENSIONS>] local_z=<Z>
valid=<True或False> collision_pairs=<K>
collision=<LINK> <-> <OBJECT_OR_LINK>

只有存在世界对象时才显示 object、shape 行,只有碰撞时才显示 collision 行。valid=False 且存在碰撞对时可据此定位;valid=True 只说明这一组关节状态在当前场景与未额外指定约束的条件下有效,不证明整条轨迹无碰撞。若机器人在运动,两次服务调用可能取到不同时间的场景,复现时应保持静止。

将碰撞对映射回几何与坐标

先按名称区分 panda_link5 ↔ table 一类机械臂与世界物体的接触,以及 panda_link2 ↔ panda_link4 一类连杆间自碰撞,再核对真正参与计算的几何。

Panda 场景障碍物误判与自碰撞检查关系

例如桌面实测顶面为 z=0.75 m,用 BOX 代表厚度 0.04 m 的桌面时,其中心应在 z=0.73 m。若误把顶面高度填成箱体中心高度,碰撞体顶面就会到 0.77 m,平白多占用 0.02 m 的空间。箱体尺寸应按 X、Y、Z 三个方向的完整长度填写,例如 [0.80, 0.60, 0.04],单位是米。

检查时按以下顺序定位:

  1. 世界物体碰撞:核对对象 id、header.frame_id、pose、primitive_poses 与尺寸;物体位姿和形状局部位姿需要组合,不能把两个位置直接当成同一坐标。查询结果可能已归一到规划坐标系,还须检查原始发布代码中的 frame_id 是否正确并可经 TF 转换。
  2. 机器人自碰撞:检查 URDF 对应 link 的 <collision> 几何与 <origin>,不要拿 <visual> 网格判断安全距离。过大的简化胶囊或错误原点可能让两连杆在未实际接触时相交。
  3. 抓取状态碰撞:若物体已附着于夹爪,应在 robot_state.attached_collision_objects 中查看,并核对 link_name 与 touch_links;同一物体意外留在世界对象中会带来重复几何或错误接触关系。

修正允许碰撞关系并复测

ACM(Allowed Collision Matrix)记录哪些指定对象对可以忽略碰撞;在返回消息中,可通过 entry_names 的索引查看 entry_values[i].enabled[j],若没有显式条目,还须检查 default_entry_names 和 default_entry_values。SRDF 中的 disable_collisions 常用于经过验证的相邻连杆或永不接触的连杆对;对于抓取物体与手指的预期接触,则优先检查附着物的 touch_links。不要为了使 plan() 成功而全局放开自碰撞或把桌面列入允许碰撞对象。

发现的问题 修正位置 复核证据
桌面箱体高了 0.02 m 创建 CollisionObject 的尺寸或位姿 场景查询的尺寸、位置与实测一致
自碰撞仅由错误网格造成 URDF <collision> 几何和原点 同一关节角不再报告错误连杆对
合法抓取接触被禁止 附着对象的 touch_links,或经审查的 ACM 仅预期接触对被放行,其他碰撞仍可检出

变更 SRDF 的自碰撞配置后,需要重新加载机器人配置,通常重启相关 MoveIt 节点;修改世界障碍物时,可通过 PlanningSceneInterface::applyCollisionObject() 同步更新,再查询确认新值已进入 move_group,不能只修改 RViz 中的显示状态。

最后保持相同起点、相同目标、相同规划器与超时复测:先用诊断程序确认误报的碰撞对消失,再在 RViz 中只执行 Plan 并检查生成轨迹没有穿过桌面。可将规划时间设为 5 s 并重复 10 次记录成功次数;若当前状态已有效却仍规划失败,应转查目标可达性、路径约束与采样超时,而不继续放宽碰撞检查。

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

目录与阅读设置

阅读设置

17

记录一个想法

定位障碍物或自碰撞误判

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

登录哪都通