SurgBot 分步演示¶

五一 MVP 版本 — 无需机械臂，完整展示从语音指令到路径规划的全链路。

本页面由 GitHub Actions 自动构建，所有输出均为真实运行结果，非截图。点击上方按钮可在 Colab 中交互运行完整版本。

系统流程一览¶

语音指令
  ↓
Step 1 · 配置加载        core/config.py       从 config.toml 读取所有参数
  ↓
Step 2 · 安全校验        core/safety_manager  拦截越界/大步长路径
  ↓
Step 3 · 工作空间可视化   Plotly 3D           槽位坐标 & 边界直观展示
  ↓
Step 4 · 运动路径可视化   Plotly 3D           approach → grasp → deliver 轨迹
  ↓
Step 5 · 完整 Mock 流程  core/state_machine   5条指令端到端批量验证

In [1]:

Copied!





# ── 路径初始化（适配 CI 和本地两种环境）──────────────────
import os, sys
from pathlib import Path

# Notebook 演示模式：在所有 surgbot 模块导入前设置，
# 使 logger._setup_logger() 将控制台级别设为 WARNING，
# INFO 日志只写入文件，不显示在 cell 输出中。
os.environ['SURGBOT_NOTEBOOK_MODE'] = '1'

# 找到仓库根目录（无论从哪里运行）
HERE = Path(os.getcwd())
for p in [HERE, HERE.parent, HERE.parent.parent]:
    candidate = p / 'surgbot'
    if candidate.exists():
        if str(candidate) not in sys.path:
            sys.path.insert(0, str(candidate))
        REPO_ROOT = p
        break

print(f'surgbot 代码路径: {candidate}')
print(f'Python {sys.version.split()[0]}')
print('日志模式: Notebook（WARNING+ 显示于 cell，INFO+ 写入文件）')
# ── 路径初始化（适配 CI 和本地两种环境）──────────────────
import os, sys
from pathlib import Path

# Notebook 演示模式：在所有 surgbot 模块导入前设置，
# 使 logger._setup_logger() 将控制台级别设为 WARNING，
# INFO 日志只写入文件，不显示在 cell 输出中。
os.environ['SURGBOT_NOTEBOOK_MODE'] = '1'

# 找到仓库根目录（无论从哪里运行）
HERE = Path(os.getcwd())
for p in [HERE, HERE.parent, HERE.parent.parent]:
    candidate = p / 'surgbot'
    if candidate.exists():
        if str(candidate) not in sys.path:
            sys.path.insert(0, str(candidate))
        REPO_ROOT = p
        break

print(f'surgbot 代码路径: {candidate}')
print(f'Python {sys.version.split()[0]}')
print('日志模式: Notebook（WARNING+ 显示于 cell，INFO+ 写入文件）')

surgbot 代码路径: /home/runner/work/surgbot-docs-v2/surgbot-docs-v2/surgbot
Python 3.11.15
日志模式: Notebook（WARNING+ 显示于 cell，INFO+ 写入文件）

Step 1 — 配置加载¶

解决的问题（雄安测试 P0-02）：原代码速度、Z 偏移、力反馈阈值全部硬编码，每次调参都要修改 Python 文件并重启程序。

修复方案：所有参数集中到 config.toml，程序启动时由 core/config.py 一次性加载。调参只需编辑 TOML 文件，无需动代码。

参数	文件位置	说明
`robot.speed`	`[robot]`	机械臂速度百分比，演示建议 20–30
`robot.z_approach_offset`	`[robot]`	夹取点上方安全高度（mm），防止碰盘
`safety.force_threshold`	`[safety]`	力反馈触发阈值（N），越小越灵敏
`safety.max_single_step_dist`	`[safety]`	单步最大移动距离，超出则拒绝执行

In [2]:

Copied!





from core.config import cfg

rows = [
    ('机械臂 IP',      cfg.robot.ip),
    ('运动速度',        f'{cfg.robot.speed} %'),
    ('抬升安全高度',    f'{cfg.robot.z_approach_offset} mm'),
    ('X 工作范围',     f'[{cfg.safety.x_min:.0f}, {cfg.safety.x_max:.0f}] mm'),
    ('Y 工作范围',     f'[{cfg.safety.y_min:.0f}, {cfg.safety.y_max:.0f}] mm'),
    ('Z 工作范围',     f'[{cfg.safety.z_min:.0f}, {cfg.safety.z_max:.0f}] mm'),
    ('力反馈阈值',      f'{cfg.safety.force_threshold} N'),
    ('单步最大距离',    f'{cfg.safety.max_single_step_dist} mm'),
]

print(f'{"参数":<16} {"值"}')
print('-' * 40)
for k, v in rows:
    print(f'{k:<16} {v}')
from core.config import cfg

rows = [
    ('机械臂 IP',      cfg.robot.ip),
    ('运动速度',        f'{cfg.robot.speed} %'),
    ('抬升安全高度',    f'{cfg.robot.z_approach_offset} mm'),
    ('X 工作范围',     f'[{cfg.safety.x_min:.0f}, {cfg.safety.x_max:.0f}] mm'),
    ('Y 工作范围',     f'[{cfg.safety.y_min:.0f}, {cfg.safety.y_max:.0f}] mm'),
    ('Z 工作范围',     f'[{cfg.safety.z_min:.0f}, {cfg.safety.z_max:.0f}] mm'),
    ('力反馈阈值',      f'{cfg.safety.force_threshold} N'),
    ('单步最大距离',    f'{cfg.safety.max_single_step_dist} mm'),
]

print(f'{"参数":<16} {"值"}')
print('-' * 40)
for k, v in rows:
    print(f'{k:<16} {v}')

[Config] Loaded from /home/runner/work/surgbot-docs-v2/surgbot-docs-v2/surgbot/config.toml
参数               值
----------------------------------------
机械臂 IP           192.168.144.49
运动速度             30 %
抬升安全高度           150.0 mm
X 工作范围           [-600, 600] mm
Y 工作范围           [-700, 100] mm
Z 工作范围           [100, 550] mm
力反馈阈值            1.0 N
单步最大距离           600.0 mm

Step 2 — 安全校验¶

解决的问题（雄安测试 P0-03）：路径直接发给机械臂执行，无任何边界检查，曾出现末端超出托盘范围的危险动作。

修复方案：core/safety_manager.py 在每次 executePath() 调用前强制校验：

校验类型	触发条件	异常类型
工作空间边界	x/y/z 超出 config.toml 设定范围	`WorkspaceViolation`
大步长检测	相邻两点距离 > `max_single_step_dist`	`LargeStepDetected`
置信度过低	视觉置信度 < `min_confidence`	`ConfidenceTooLow`
关节角模式	`mode=1` 时跳过笛卡尔边界检查	—

注意：安全校验是阻断式的——校验不通过则路径不发送，机械臂不动。

In [3]:

Copied!





from core.safety_manager import safety, WorkspaceViolation, LargeStepDetected, SafetyError

CASES = [
    ('slot_01 夹取点',      [120.0, -80.0, 180.0, -180, 0, 45, 0], True),
    ('slot_04 夹取点',      [360.0, -80.0, 180.0, -180, 0,  0, 0], True),
    ('X 越界 (x=700)',      [700.0, -80.0, 180.0, -180, 0,  0, 0], False),
    ('Z 过低 (z=50, 撞盘)', [200.0, -80.0,  50.0, -180, 0,  0, 0], False),
    ('关节角模式（跳过）',   [0.0, 32.6, -129.1, 6.7, 90.0, -90.0, 1], True),
]

print(f'{"测试用例":<22} {"预期":<6} {"结果"}')
print('-' * 50)
all_ok = True
for label, pt, should_pass in CASES:
    try:
        safety.validate_point(pt, label=label)
        result = '✅ 通过' if should_pass else '❌ 应拦截'
        if not should_pass: all_ok = False
    except SafetyError:
        result = '🚫 已拦截' if not should_pass else '❌ 误拦截'
        if should_pass: all_ok = False
    print(f'{label:<22} {"合法" if should_pass else "违规":<6} {result}')

# 大步长检测
# 两点在工作空间内，但 X 方向距离 1000 mm > max_single_step_dist(600)
# 注：z=620 会先触发 WorkspaceViolation，所以改用 X 方向超距
try:
    safety.validate_path(
        [[-500.0, -80.0, 200.0, -180, 0, 0, 0],
         [ 500.0, -80.0, 200.0, -180, 0, 0, 0]]  # step = 1000mm
    )
    print(f'{"大步长 1000mm (X)":<22} {"违规":<6} ❌ 应拦截')
    all_ok = False
except LargeStepDetected:
    print(f'{"大步长 1000mm (X)":<22} {"违规":<6} 🚫 已拦截')
except SafetyError as e:
    # 如果其他安全错误也算拦截成功，但说明测试用例设计有误
    print(f'{"大步长 1000mm (X)":<22} {"违规":<6} ⚠️ 拦截(非预期类型): {type(e).__name__}')

print()
print('整体校验：', '✅ 全部符合预期' if all_ok else '❌ 有测试不符预期')
from core.safety_manager import safety, WorkspaceViolation, LargeStepDetected, SafetyError

CASES = [
    ('slot_01 夹取点',      [120.0, -80.0, 180.0, -180, 0, 45, 0], True),
    ('slot_04 夹取点',      [360.0, -80.0, 180.0, -180, 0,  0, 0], True),
    ('X 越界 (x=700)',      [700.0, -80.0, 180.0, -180, 0,  0, 0], False),
    ('Z 过低 (z=50, 撞盘)', [200.0, -80.0,  50.0, -180, 0,  0, 0], False),
    ('关节角模式（跳过）',   [0.0, 32.6, -129.1, 6.7, 90.0, -90.0, 1], True),
]

print(f'{"测试用例":<22} {"预期":<6} {"结果"}')
print('-' * 50)
all_ok = True
for label, pt, should_pass in CASES:
    try:
        safety.validate_point(pt, label=label)
        result = '✅ 通过' if should_pass else '❌ 应拦截'
        if not should_pass: all_ok = False
    except SafetyError:
        result = '🚫 已拦截' if not should_pass else '❌ 误拦截'
        if should_pass: all_ok = False
    print(f'{label:<22} {"合法" if should_pass else "违规":<6} {result}')

# 大步长检测
# 两点在工作空间内，但 X 方向距离 1000 mm > max_single_step_dist(600)
# 注：z=620 会先触发 WorkspaceViolation，所以改用 X 方向超距
try:
    safety.validate_path(
        [[-500.0, -80.0, 200.0, -180, 0, 0, 0],
         [ 500.0, -80.0, 200.0, -180, 0, 0, 0]]  # step = 1000mm
    )
    print(f'{"大步长 1000mm (X)":<22} {"违规":<6} ❌ 应拦截')
    all_ok = False
except LargeStepDetected:
    print(f'{"大步长 1000mm (X)":<22} {"违规":<6} 🚫 已拦截')
except SafetyError as e:
    # 如果其他安全错误也算拦截成功，但说明测试用例设计有误
    print(f'{"大步长 1000mm (X)":<22} {"违规":<6} ⚠️ 拦截(非预期类型): {type(e).__name__}')

print()
print('整体校验：', '✅ 全部符合预期' if all_ok else '❌ 有测试不符预期')

测试用例                   预期     结果
--------------------------------------------------
slot_01 夹取点            合法     ✅ 通过
slot_04 夹取点            合法     ✅ 通过
X 越界 (x=700)           违规     🚫 已拦截
Z 过低 (z=50, 撞盘)        违规     🚫 已拦截
关节角模式（跳过）              合法     ✅ 通过
大步长 1000mm (X)         违规     🚫 已拦截

整体校验： ✅ 全部符合预期

Step 3 — 工作空间 & 器械槽位可视化¶

槽位支架方案（雄安测试的核心改进）

原方案：器械随机堆叠在托盘上，YOLO 需要在整张图像中搜索，遮挡严重时置信度 < 0.7，识别失败率高。

MVP 方案：器械由护士预先放入固定槽位支架，YOLO 只需在该槽位的 小 ROI 区域 内确认器械是否到位，识别置信度提升至 > 0.9。

属性	说明
`grasp_point`	夹取点坐标 [x, y, z]，机械臂坐标系（mm）
`orientation_deg`	末端 Rz 角度（°），确保手柄朝向医生
`roi`	图像中该槽位的像素范围，YOLO 只推理此区域
`gripper_preset_id`	夹爪预设编号，对应不同器械的开合度和夹持力

下图蓝色线框为工作空间边界，彩色菱形为 5 个器械槽位，空心圆为各槽位的 approach 悬停点。

In [4]:

Copied!





import plotly.graph_objects as go
import plotly.io as pio
pio.renderers.default = 'notebook'

SLOTS = [
    {'id':'slot_01','name':'持针器_大','pt':[120.0,-80.0,180.0],'rz':45.0,'color':'#e74c3c'},
    {'id':'slot_02','name':'剪刀',     'pt':[200.0,-80.0,180.0],'rz': 0.0,'color':'#3498db'},
    {'id':'slot_03','name':'镊子',     'pt':[280.0,-80.0,180.0],'rz':90.0,'color':'#2ecc71'},
    {'id':'slot_04','name':'刀柄',     'pt':[360.0,-80.0,180.0],'rz':30.0,'color':'#f39c12'},
    {'id':'slot_05','name':'持针器_小','pt':[440.0,-80.0,180.0],'rz':45.0,'color':'#9b59b6'},
]

s = cfg.safety
xs=[s.x_min,s.x_max]; ys=[s.y_min,s.y_max]; zs=[s.z_min,s.z_max]
ex,ey,ez=[],[],[]
corners=[(x,y,z) for x in xs for y in ys for z in zs]
for i,(x1,y1,z1) in enumerate(corners):
    for x2,y2,z2 in corners[i+1:]:
        if sum([x1!=x2,y1!=y2,z1!=z2])==1:
            ex+=[x1,x2,None]; ey+=[y1,y2,None]; ez+=[z1,z2,None]

fig = go.Figure()
fig.add_trace(go.Scatter3d(x=ex,y=ey,z=ez,mode='lines',
    line=dict(color='rgba(100,100,255,0.25)',width=2),name='工作空间边界',hoverinfo='skip'))

# 托盘
fig.add_trace(go.Scatter3d(
    x=[50,510,510,50,50],y=[-130,-130,-30,-30,-130],z=[178]*5,
    mode='lines',line=dict(color='#95a5a6',width=3),name='器械托盘'))

# 机械臂底座 & 递送点
fig.add_trace(go.Scatter3d(x=[0],y=[0],z=[0],mode='markers+text',
    marker=dict(size=12,color='#2c3e50',symbol='square'),
    text=['底座'],textposition='top center',name='机械臂底座'))
fig.add_trace(go.Scatter3d(x=[-50],y=[-250],z=[350],mode='markers+text',
    marker=dict(size=10,color='#1abc9c',symbol='diamond'),
    text=['递送点'],textposition='top center',name='递送点（医生侧）'))

# 槽位
for sl in SLOTS:
    p=sl['pt']; za=p[2]+cfg.robot.z_approach_offset
    fig.add_trace(go.Scatter3d(x=[p[0]],y=[p[1]],z=[p[2]],mode='markers+text',
        marker=dict(size=11,color=sl['color'],line=dict(width=2,color='white')),
        text=[sl['name']],textposition='top center',name=sl['name'],
        hovertemplate=f"<b>{sl['name']}</b><br>{sl['id']}<br>"
            f"({p[0]:.0f},{p[1]:.0f},{p[2]:.0f}) mm<br>Rz={sl['rz']}°"))
    fig.add_trace(go.Scatter3d(x=[p[0]],y=[p[1]],z=[za],mode='markers',
        marker=dict(size=5,color=sl['color'],symbol='circle-open'),
        showlegend=False,hovertemplate=f'approach Z={za:.0f}mm'))
    fig.add_trace(go.Scatter3d(x=[p[0],p[0]],y=[p[1],p[1]],z=[p[2],za],
        mode='lines',line=dict(color=sl['color'],width=1,dash='dot'),
        showlegend=False,hoverinfo='skip'))

fig.update_layout(
    title='工作空间 & 器械槽位分布',
    scene=dict(
        xaxis=dict(title='X (mm)'),yaxis=dict(title='Y (mm)'),zaxis=dict(title='Z (mm)'),
        aspectmode='manual',aspectratio=dict(x=1.3,y=1.4,z=1.0),
        camera=dict(eye=dict(x=1.5,y=-1.8,z=1.2))),
    margin=dict(l=0,r=0,b=0,t=40),height=560)
fig.show()
import plotly.graph_objects as go
import plotly.io as pio
pio.renderers.default = 'notebook'

SLOTS = [
    {'id':'slot_01','name':'持针器_大','pt':[120.0,-80.0,180.0],'rz':45.0,'color':'#e74c3c'},
    {'id':'slot_02','name':'剪刀',     'pt':[200.0,-80.0,180.0],'rz': 0.0,'color':'#3498db'},
    {'id':'slot_03','name':'镊子',     'pt':[280.0,-80.0,180.0],'rz':90.0,'color':'#2ecc71'},
    {'id':'slot_04','name':'刀柄',     'pt':[360.0,-80.0,180.0],'rz':30.0,'color':'#f39c12'},
    {'id':'slot_05','name':'持针器_小','pt':[440.0,-80.0,180.0],'rz':45.0,'color':'#9b59b6'},
]

s = cfg.safety
xs=[s.x_min,s.x_max]; ys=[s.y_min,s.y_max]; zs=[s.z_min,s.z_max]
ex,ey,ez=[],[],[]
corners=[(x,y,z) for x in xs for y in ys for z in zs]
for i,(x1,y1,z1) in enumerate(corners):
    for x2,y2,z2 in corners[i+1:]:
        if sum([x1!=x2,y1!=y2,z1!=z2])==1:
            ex+=[x1,x2,None]; ey+=[y1,y2,None]; ez+=[z1,z2,None]

fig = go.Figure()
fig.add_trace(go.Scatter3d(x=ex,y=ey,z=ez,mode='lines',
    line=dict(color='rgba(100,100,255,0.25)',width=2),name='工作空间边界',hoverinfo='skip'))

# 托盘
fig.add_trace(go.Scatter3d(
    x=[50,510,510,50,50],y=[-130,-130,-30,-30,-130],z=[178]*5,
    mode='lines',line=dict(color='#95a5a6',width=3),name='器械托盘'))

# 机械臂底座 & 递送点
fig.add_trace(go.Scatter3d(x=[0],y=[0],z=[0],mode='markers+text',
    marker=dict(size=12,color='#2c3e50',symbol='square'),
    text=['底座'],textposition='top center',name='机械臂底座'))
fig.add_trace(go.Scatter3d(x=[-50],y=[-250],z=[350],mode='markers+text',
    marker=dict(size=10,color='#1abc9c',symbol='diamond'),
    text=['递送点'],textposition='top center',name='递送点（医生侧）'))

# 槽位
for sl in SLOTS:
    p=sl['pt']; za=p[2]+cfg.robot.z_approach_offset
    fig.add_trace(go.Scatter3d(x=[p[0]],y=[p[1]],z=[p[2]],mode='markers+text',
        marker=dict(size=11,color=sl['color'],line=dict(width=2,color='white')),
        text=[sl['name']],textposition='top center',name=sl['name'],
        hovertemplate=f"{sl['name']}
{sl['id']}
"
            f"({p[0]:.0f},{p[1]:.0f},{p[2]:.0f}) mm
Rz={sl['rz']}°"))
    fig.add_trace(go.Scatter3d(x=[p[0]],y=[p[1]],z=[za],mode='markers',
        marker=dict(size=5,color=sl['color'],symbol='circle-open'),
        showlegend=False,hovertemplate=f'approach Z={za:.0f}mm'))
    fig.add_trace(go.Scatter3d(x=[p[0],p[0]],y=[p[1],p[1]],z=[p[2],za],
        mode='lines',line=dict(color=sl['color'],width=1,dash='dot'),
        showlegend=False,hoverinfo='skip'))

fig.update_layout(
    title='工作空间 & 器械槽位分布',
    scene=dict(
        xaxis=dict(title='X (mm)'),yaxis=dict(title='Y (mm)'),zaxis=dict(title='Z (mm)'),
        aspectmode='manual',aspectratio=dict(x=1.3,y=1.4,z=1.0),
        camera=dict(eye=dict(x=1.5,y=-1.8,z=1.2))),
    margin=dict(l=0,r=0,b=0,t=40),height=560)
fig.show()

Step 4 — 运动路径轨迹¶

以剪刀（slot_02）为例，展示单次递送的完整 6 段路径：

阶段	起点 → 终点	说明
① approach	待机姿态 → 夹取点正上方	Z 高出 `z_approach_offset`（150 mm），安全悬停
② grasp	approach 点 → 夹取点	垂直下降，闭合夹爪
③ lift	夹取点 → approach 点	垂直上升，脱离托盘
④ deliver	approach 点 → 递送点	移向医生一侧，等待接取
⑤ wait	递送点（保持）	力反馈触发（医生拿走）或超时自动松夹
⑥ reset	递送点 → 待机姿态	回到初始位置，等待下一条指令

Z_approach = Z_grasp + z_approach_offset + z_compensation z_compensation 用于补偿托盘平面不平整，标定后写入 config.toml。

In [5]:

Copied!





TARGET = 'slot_02'   # 剪刀
sl = next(s for s in SLOTS if s['id']==TARGET)
p = sl['pt']; za = p[2]+cfg.robot.z_approach_offset

DELIVER = [-50.0, -250.0, 350.0]
RESET   = [  0.0,  -50.0, 400.0]

phases = [
    ('待机',    RESET,             '#95a5a6'),
    ('approach',[p[0],p[1],za],    '#3498db'),
    ('grasp',   p,                 '#e74c3c'),
    ('lift',    [p[0],p[1],za],    '#e67e22'),
    ('deliver', DELIVER,           '#2ecc71'),
    ('reset',   RESET,             '#95a5a6'),
]

fig2 = go.Figure()
xs=[w[1][0] for w in phases]; ys=[w[1][1] for w in phases]; zs=[w[1][2] for w in phases]
fig2.add_trace(go.Scatter3d(x=xs,y=ys,z=zs,mode='lines',
    line=dict(color='#bdc3c7',width=3),name='路径总览',showlegend=False))

LABELS = {'待机':'⚪ 待机','approach':'🔵 approach','grasp':'🔴 grasp（夹取）',
          'lift':'🟠 lift','deliver':'🟢 deliver（递送）','reset':'⚪ reset'}
for name,pt,color in phases:
    fig2.add_trace(go.Scatter3d(x=[pt[0]],y=[pt[1]],z=[pt[2]],
        mode='markers+text',
        marker=dict(size=12,color=color,line=dict(width=2,color='white')),
        text=[LABELS[name]],textposition='top center',name=LABELS[name],
        hovertemplate=f'<b>{name}</b><br>({pt[0]:.0f},{pt[1]:.0f},{pt[2]:.0f}) mm'))

fig2.add_trace(go.Scatter3d(x=[p[0]],y=[p[1]],z=[p[2]],mode='markers',
    marker=dict(size=15,color=sl['color'],symbol='diamond',
                line=dict(width=2,color='white')),
    name=f'器械: {sl["name"]}'))

fig2.update_layout(
    title=f'运动轨迹 — {sl["name"]} ({TARGET})',
    scene=dict(
        xaxis=dict(title='X (mm)'),yaxis=dict(title='Y (mm)'),zaxis=dict(title='Z (mm)'),
        aspectmode='manual',aspectratio=dict(x=1.3,y=1.4,z=1.0),
        camera=dict(eye=dict(x=1.6,y=-1.6,z=1.2))),
    margin=dict(l=0,r=0,b=0,t=40),height=580)
fig2.show()

print(f'器械: {sl["name"]}  |  夹取 ({p[0]:.0f},{p[1]:.0f},{p[2]:.0f}) mm  '
      f'|  approach Z={za:.0f} mm  |  Rz={sl["rz"]}°')
TARGET = 'slot_02'   # 剪刀
sl = next(s for s in SLOTS if s['id']==TARGET)
p = sl['pt']; za = p[2]+cfg.robot.z_approach_offset

DELIVER = [-50.0, -250.0, 350.0]
RESET   = [  0.0,  -50.0, 400.0]

phases = [
    ('待机',    RESET,             '#95a5a6'),
    ('approach',[p[0],p[1],za],    '#3498db'),
    ('grasp',   p,                 '#e74c3c'),
    ('lift',    [p[0],p[1],za],    '#e67e22'),
    ('deliver', DELIVER,           '#2ecc71'),
    ('reset',   RESET,             '#95a5a6'),
]

fig2 = go.Figure()
xs=[w[1][0] for w in phases]; ys=[w[1][1] for w in phases]; zs=[w[1][2] for w in phases]
fig2.add_trace(go.Scatter3d(x=xs,y=ys,z=zs,mode='lines',
    line=dict(color='#bdc3c7',width=3),name='路径总览',showlegend=False))

LABELS = {'待机':'⚪ 待机','approach':'🔵 approach','grasp':'🔴 grasp（夹取）',
          'lift':'🟠 lift','deliver':'🟢 deliver（递送）','reset':'⚪ reset'}
for name,pt,color in phases:
    fig2.add_trace(go.Scatter3d(x=[pt[0]],y=[pt[1]],z=[pt[2]],
        mode='markers+text',
        marker=dict(size=12,color=color,line=dict(width=2,color='white')),
        text=[LABELS[name]],textposition='top center',name=LABELS[name],
        hovertemplate=f'{name}
({pt[0]:.0f},{pt[1]:.0f},{pt[2]:.0f}) mm'))

fig2.add_trace(go.Scatter3d(x=[p[0]],y=[p[1]],z=[p[2]],mode='markers',
    marker=dict(size=15,color=sl['color'],symbol='diamond',
                line=dict(width=2,color='white')),
    name=f'器械: {sl["name"]}'))

fig2.update_layout(
    title=f'运动轨迹 — {sl["name"]} ({TARGET})',
    scene=dict(
        xaxis=dict(title='X (mm)'),yaxis=dict(title='Y (mm)'),zaxis=dict(title='Z (mm)'),
        aspectmode='manual',aspectratio=dict(x=1.3,y=1.4,z=1.0),
        camera=dict(eye=dict(x=1.6,y=-1.6,z=1.2))),
    margin=dict(l=0,r=0,b=0,t=40),height=580)
fig2.show()

print(f'器械: {sl["name"]}  |  夹取 ({p[0]:.0f},{p[1]:.0f},{p[2]:.0f}) mm  '
      f'|  approach Z={za:.0f} mm  |  Rz={sl["rz"]}°')

器械: 剪刀  |  夹取 (200,-80,180) mm  |  approach Z=330 mm  |  Rz=0.0°

Step 5 — 完整 Mock 流程¶

用 SurgBotStateMachine 批量验证 5 条语音指令的全链路：

语音文本  →  KeywordMatcher  →  PositionRegistry  →  SafetyManager
                                                         ↓
                                                    RulePlanner
                                                         ↓
                                                    DobotArm (mock)
                                                         ↓
                                               approach → grasp → lift
                                                → deliver → wait → reset

各模块说明

模块	文件	功能
`KeywordMatcher`	`modules/nlp/keyword_matcher.py`	精确/alias/子串三级匹配，返回 `InstrumentCommand`
`PositionRegistry`	`modules/perception/position_registry.py`	查表返回夹取坐标，MVP 阶段替代 YOLO 视觉推理
`RulePlanner`	`modules/decision/rule_planner.py`	生成 7 步动作序列，含视觉二次确认可选步骤
`DobotArm`	`hardware/dobot_arm.py`	语义接口封装，每步执行前调 `safety.validate_path()`

Mock 模式说明：mock=True 时机械臂操作由 _MockRobot 模拟，每次 wait_for_handover 等待 2 秒后自动超时（真机由医生握力触发）。所有业务逻辑（NLP、安全校验、规划）与真机完全相同。

In [6]:

Copied!





from core.state_machine import SurgBotStateMachine
import time

# 通过状态机运行完整流程（mock 模式，无需真实机械臂）
sm = SurgBotStateMachine(mock=True)
sm._arm._FORCE_WAIT_TIMEOUT = 2   # CI 演示：2s 超时

CMDS = ['递持针器', '给我剪刀', '镊子', '传刀柄', '小持针器']

print(f'{"指令":<14} {"器械":<12} {"槽位":<8} {"耗时":>6}  {"状态"}')
print('-' * 60)

for text in CMDS:
    r = sm.run(text, handover_timeout=2)
    status = 'OK' if r.success else 'FAIL'
    name   = r.instrument_name or r.error[:20]
    print(f'{text:<14} {name:<12} {r.slot_id:<8} {r.elapsed_s:>5.1f}s  [{status}]')

sm.shutdown()
print()
print(f'急停触发次数: {sm._arm._robot.stop_count if hasattr(sm._arm._robot, "stop_count") else 0}')
from core.state_machine import SurgBotStateMachine
import time

# 通过状态机运行完整流程（mock 模式，无需真实机械臂）
sm = SurgBotStateMachine(mock=True)
sm._arm._FORCE_WAIT_TIMEOUT = 2   # CI 演示：2s 超时

CMDS = ['递持针器', '给我剪刀', '镊子', '传刀柄', '小持针器']

print(f'{"指令":<14} {"器械":<12} {"槽位":<8} {"耗时":>6}  {"状态"}')
print('-' * 60)

for text in CMDS:
    r = sm.run(text, handover_timeout=2)
    status = 'OK' if r.success else 'FAIL'
    name   = r.instrument_name or r.error[:20]
    print(f'{text:<14} {name:<12} {r.slot_id:<8} {r.elapsed_s:>5.1f}s  [{status}]')

sm.shutdown()
print()
print(f'急停触发次数: {sm._arm._robot.stop_count if hasattr(sm._arm._robot, "stop_count") else 0}')

指令             器械           槽位           耗时  状态
------------------------------------------------------------

递持针器           持针器_大        slot_01    2.9s  [OK]

给我剪刀           剪刀           slot_02    2.9s  [OK]

镊子             镊子           slot_03    2.9s  [OK]

传刀柄            刀柄           slot_04    2.9s  [OK]

小持针器           持针器_小        slot_05    2.9s  [OK]

急停触发次数: 0

数据流总结¶

语音文本
  → KeywordMatcher.match()       # NLP 关键词匹配
  → PositionRegistry.find()      # 槽位注册表查询
  → safety.validate_grasp()      # 安全前置校验
  → RulePlanner.plan()           # 规则动作序列生成
  → DobotArm（mock）             # approach → grasp → lift → deliver → wait → reset

模块	文件	状态
配置	`core/config.py`	✅ 完成
接口定义	`core/interfaces.py`	✅ 完成
安全管理	`core/safety_manager.py`	✅ 完成
槽位注册表	`modules/perception/position_registry.py`	✅ 完成
关键词匹配	`modules/nlp/keyword_matcher.py`	✅ 完成
规则规划器	`modules/decision/rule_planner.py`	✅ 完成
机械臂封装	`hardware/dobot_arm.py`	✅ 完成
状态机	`core/state_machine.py`	✅ 完成

完整交互版本：