🤖 SurgBot 交互演示 — 手术器械护士机器助手¶

北京理工大学具身空间智能实验室 × 雄安宣武医院神经外科

---

本 notebook 演示 五一 MVP 版本 的完整流程（无需真实机械臂）：

步骤	内容
1️⃣	环境安装与代码拉取
2️⃣	配置加载 & 参数说明
3️⃣	安全校验演示（工作空间边界）
4️⃣	📐 工作空间 + 器械槽位 3D 可视化
5️⃣	🦾 运动路径 3D 轨迹可视化
6️⃣	🔄 完整 mock 流程端到端演示

1️⃣ 环境安装与代码拉取¶

In [ ]:

# 安装依赖
!pip install loguru plotly -q

import os, sys

# 拉取最新代码
REPO = 'https://github.com/Zebedee2021/surgbot-docs-v2.git'
LOCAL = '/content/surgbot'

if os.path.exists(LOCAL):
    !cd {LOCAL} && git pull -q
    print('✅ 代码已更新')
else:
    !git clone -q {REPO} {LOCAL}
    print('✅ 代码拉取完成')

# 将 surgbot/ 代码目录加入 Python 路径
CODE_DIR = os.path.join(LOCAL, 'surgbot')
if CODE_DIR not in sys.path:
    sys.path.insert(0, CODE_DIR)

print(f'Python path: {CODE_DIR}')

# 安装依赖 !pip install loguru plotly -q import os, sys # 拉取最新代码 REPO = 'https://github.com/Zebedee2021/surgbot-docs-v2.git' LOCAL = '/content/surgbot' if os.path.exists(LOCAL): !cd {LOCAL} && git pull -q print('✅ 代码已更新') else: !git clone -q {REPO} {LOCAL} print('✅ 代码拉取完成') # 将 surgbot/ 代码目录加入 Python 路径 CODE_DIR = os.path.join(LOCAL, 'surgbot') if CODE_DIR not in sys.path: sys.path.insert(0, CODE_DIR) print(f'Python path: {CODE_DIR}')

2️⃣ 配置加载 & 参数说明¶

In [ ]:

from core.config import cfg

print('=' * 50)
print('机械臂参数')
print('=' * 50)
print(f'  IP 地址         : {cfg.robot.ip}')
print(f'  运动速度         : {cfg.robot.speed}%')
print(f'  抬升安全高度      : {cfg.robot.z_approach_offset} mm')
print(f'  Z 轴标定补偿      : {cfg.robot.z_grasp_offset} mm')
print()
print('工作空间边界（mm）')
print('=' * 50)
s = cfg.safety
print(f'  X : [{s.x_min:+.0f},  {s.x_max:+.0f}]')
print(f'  Y : [{s.y_min:+.0f}, {s.y_max:+.0f}]')
print(f'  Z : [{s.z_min:+.0f},  {s.z_max:+.0f}]')
print(f'  力反馈阈值        : {s.force_threshold} N')
print(f'  单步最大距离      : {s.max_single_step_dist} mm')
print()
print('夹爪预设')
print('=' * 50)
names = {-1:'通用松开', 0:'刀柄', 1:'镊子', 2:'剪刀', 3:'持针钳'}
for pid, preset in cfg.gripper.presets.items():
    print(f'  [{pid}] {names.get(pid,""):6s}  开合:{preset["open"]}  闭合:{preset["close"]}  力:{preset["force"]}')

from core.config import cfg print('=' * 50) print('机械臂参数') print('=' * 50) print(f' IP 地址 : {cfg.robot.ip}') print(f' 运动速度 : {cfg.robot.speed}%') print(f' 抬升安全高度 : {cfg.robot.z_approach_offset} mm') print(f' Z 轴标定补偿 : {cfg.robot.z_grasp_offset} mm') print() print('工作空间边界（mm）') print('=' * 50) s = cfg.safety print(f' X : [{s.x_min:+.0f}, {s.x_max:+.0f}]') print(f' Y : [{s.y_min:+.0f}, {s.y_max:+.0f}]') print(f' Z : [{s.z_min:+.0f}, {s.z_max:+.0f}]') print(f' 力反馈阈值 : {s.force_threshold} N') print(f' 单步最大距离 : {s.max_single_step_dist} mm') print() print('夹爪预设') print('=' * 50) names = {-1:'通用松开', 0:'刀柄', 1:'镊子', 2:'剪刀', 3:'持针钳'} for pid, preset in cfg.gripper.presets.items(): print(f' [{pid}] {names.get(pid,""):6s} 开合:{preset["open"]} 闭合:{preset["close"]} 力:{preset["force"]}')

3️⃣ 安全校验演示¶

In [ ]:

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

def check(label, point):
    try:
        safety.validate_point(point, label=label)
        print(f'  ✅ {label:30s} → 通过')
    except WorkspaceViolation as e:
        print(f'  🚫 {label:30s} → 超出工作空间')
    except SafetyError as e:
        print(f'  ⚠️  {label:30s} → {e}')

print('单点工作空间校验')
print('-' * 55)
check('slot_01 夹取点 (正常)',      [120.5, -80.0, 180.0, -180, 0, 45, 0])
check('slot_04 夹取点 (正常)',      [360.0, -80.0, 180.0, -180, 0,  0, 0])
check('X 越界 (x=700)',            [700.0, -80.0, 180.0, -180, 0,  0, 0])
check('Z 过低 (z=50, 撞托盘)',      [200.0, -80.0,  50.0, -180, 0,  0, 0])
check('关节角模式跳过边界检查',       [0.0, 32.6, -129.1, 6.7, 90.0, -90.0, 1])

print()
print('路径大步长检测')
print('-' * 55)
try:
    safety.validate_path([
        [100.0, -80.0, 180.0, -180, 0, 0, 0],
        [100.0, -80.0, 600.0, -180, 0, 0, 0],  # 单步 420mm > 400mm 限制
    ])
    print('  FAIL: 应该被拦截')
except LargeStepDetected as e:
    print(f'  🚫 大步长检测 (预期): {e}')

from core.safety_manager import safety, SafetyError, WorkspaceViolation, LargeStepDetected def check(label, point): try: safety.validate_point(point, label=label) print(f' ✅ {label:30s} → 通过') except WorkspaceViolation as e: print(f' 🚫 {label:30s} → 超出工作空间') except SafetyError as e: print(f' ⚠️ {label:30s} → {e}') print('单点工作空间校验') print('-' * 55) check('slot_01 夹取点 (正常)', [120.5, -80.0, 180.0, -180, 0, 45, 0]) check('slot_04 夹取点 (正常)', [360.0, -80.0, 180.0, -180, 0, 0, 0]) check('X 越界 (x=700)', [700.0, -80.0, 180.0, -180, 0, 0, 0]) check('Z 过低 (z=50, 撞托盘)', [200.0, -80.0, 50.0, -180, 0, 0, 0]) check('关节角模式跳过边界检查', [0.0, 32.6, -129.1, 6.7, 90.0, -90.0, 1]) print() print('路径大步长检测') print('-' * 55) try: safety.validate_path([ [100.0, -80.0, 180.0, -180, 0, 0, 0], [100.0, -80.0, 600.0, -180, 0, 0, 0], # 单步 420mm > 400mm 限制 ]) print(' FAIL: 应该被拦截') except LargeStepDetected as e: print(f' 🚫 大步长检测 (预期): {e}')

4️⃣ 工作空间 + 器械槽位 3D 可视化¶

In [ ]:

import plotly.graph_objects as go
import numpy as np
import json, os

# ── 工作空间边界框 ──────────────────────────────────
def make_workspace_box(s):
    """生成工作空间边界线（12条边）"""
    xs = [s.x_min, s.x_max]
    ys = [s.y_min, s.y_max]
    zs = [s.z_min, s.z_max]
    edges_x, edges_y, edges_z = [], [], []
    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:  # 共享两个维度 = 边
                edges_x += [x1, x2, None]
                edges_y += [y1, y2, None]
                edges_z += [z1, z2, None]
    return go.Scatter3d(x=edges_x, y=edges_y, z=edges_z,
                        mode='lines',
                        line=dict(color='rgba(100,100,255,0.3)', width=2),
                        name='工作空间边界',
                        hoverinfo='skip')

# ── 器械槽位数据（演示用，等上机标定后替换真实坐标）──
SLOTS = [
    {'slot_id':'slot_01','name':'持针器_大','xy_px':[120,80],'grasp_point':[120.0,-80.0,180.0],'rz':45.0 ,'preset':3,'color':'#e74c3c'},
    {'slot_id':'slot_02','name':'剪刀',     'xy_px':[200,80],'grasp_point':[200.0,-80.0,180.0],'rz':0.0  ,'preset':2,'color':'#3498db'},
    {'slot_id':'slot_03','name':'镊子',     'xy_px':[280,80],'grasp_point':[280.0,-80.0,180.0],'rz':90.0 ,'preset':1,'color':'#2ecc71'},
    {'slot_id':'slot_04','name':'刀柄',     'xy_px':[360,80],'grasp_point':[360.0,-80.0,180.0],'rz':30.0 ,'preset':0,'color':'#f39c12'},
    {'slot_id':'slot_05','name':'持针器_小','xy_px':[440,80],'grasp_point':[440.0,-80.0,180.0],'rz':45.0 ,'preset':3,'color':'#9b59b6'},
]

fig = go.Figure()

# 工作空间边界框
fig.add_trace(make_workspace_box(cfg.safety))

# 托盘平面（半透明）
tray_x = [50, 500, 500,  50,  50]
tray_y = [-130,-130,-30,-30,-130]
tray_z = [175]*5
fig.add_trace(go.Scatter3d(x=tray_x, y=tray_y, z=tray_z,
                            mode='lines',
                            line=dict(color='#95a5a6', width=3),
                            name='器械托盘', fill='toself'))

# 机械臂底座
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='机械臂底座'))

# 递送点
deliver = cfg.robot.deliver_pose
fig.add_trace(go.Scatter3d(x=[-50], y=[-200], z=[350],
                            mode='markers+text',
                            marker=dict(size=10, color='#1abc9c', symbol='diamond'),
                            text=['递送点（医生侧）'], textposition='top center',
                            name='递送点'))

# 器械槽位
for slot in SLOTS:
    p = slot['grasp_point']
    # 夹取点（托盘上）
    fig.add_trace(go.Scatter3d(
        x=[p[0]], y=[p[1]], z=[p[2]],
        mode='markers+text',
        marker=dict(size=10, color=slot['color'],
                    symbol='circle', line=dict(width=2, color='white')),
        text=[slot['name']], textposition='top center',
        name=slot['name'],
        hovertemplate=(
            f"<b>{slot['name']}</b><br>"
            f"槽位: {slot['slot_id']}<br>"
            f"坐标: ({p[0]:.0f}, {p[1]:.0f}, {p[2]:.0f})<br>"
            f"朝向: {slot['rz']}°<br>"
            f"夹爪预设: {slot['preset']}"
        )
    ))
    # approach 点（正上方）
    z_app = p[2] + cfg.robot.z_approach_offset
    fig.add_trace(go.Scatter3d(
        x=[p[0]], y=[p[1]], z=[z_app],
        mode='markers',
        marker=dict(size=5, color=slot['color'], symbol='circle-open'),
        name=f"{slot['name']} approach",
        showlegend=False,
        hovertemplate=f"approach 点<br>z={z_app:.0f}mm"
    ))
    # 垂直连线（approach → grasp）
    fig.add_trace(go.Scatter3d(
        x=[p[0],p[0]], y=[p[1],p[1]], z=[p[2],z_app],
        mode='lines',
        line=dict(color=slot['color'], width=1, dash='dot'),
        showlegend=False, hoverinfo='skip'
    ))

fig.update_layout(
    title=dict(text='🤖 SurgBot 工作空间 & 器械槽位分布', font=dict(size=16)),
    scene=dict(
        xaxis=dict(title='X (mm)', range=[-650, 650]),
        yaxis=dict(title='Y (mm)', range=[-750, 150]),
        zaxis=dict(title='Z (mm)', range=[-50,  600]),
        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)),
    ),
    legend=dict(x=0.01, y=0.99),
    margin=dict(l=0, r=0, b=0, t=40),
    height=620,
)
fig.show()

import plotly.graph_objects as go import numpy as np import json, os # ── 工作空间边界框 ────────────────────────────────── def make_workspace_box(s): """生成工作空间边界线（12条边）""" xs = [s.x_min, s.x_max] ys = [s.y_min, s.y_max] zs = [s.z_min, s.z_max] edges_x, edges_y, edges_z = [], [], [] 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: # 共享两个维度 = 边 edges_x += [x1, x2, None] edges_y += [y1, y2, None] edges_z += [z1, z2, None] return go.Scatter3d(x=edges_x, y=edges_y, z=edges_z, mode='lines', line=dict(color='rgba(100,100,255,0.3)', width=2), name='工作空间边界', hoverinfo='skip') # ── 器械槽位数据（演示用，等上机标定后替换真实坐标）── SLOTS = [ {'slot_id':'slot_01','name':'持针器_大','xy_px':[120,80],'grasp_point':[120.0,-80.0,180.0],'rz':45.0 ,'preset':3,'color':'#e74c3c'}, {'slot_id':'slot_02','name':'剪刀', 'xy_px':[200,80],'grasp_point':[200.0,-80.0,180.0],'rz':0.0 ,'preset':2,'color':'#3498db'}, {'slot_id':'slot_03','name':'镊子', 'xy_px':[280,80],'grasp_point':[280.0,-80.0,180.0],'rz':90.0 ,'preset':1,'color':'#2ecc71'}, {'slot_id':'slot_04','name':'刀柄', 'xy_px':[360,80],'grasp_point':[360.0,-80.0,180.0],'rz':30.0 ,'preset':0,'color':'#f39c12'}, {'slot_id':'slot_05','name':'持针器_小','xy_px':[440,80],'grasp_point':[440.0,-80.0,180.0],'rz':45.0 ,'preset':3,'color':'#9b59b6'}, ] fig = go.Figure() # 工作空间边界框 fig.add_trace(make_workspace_box(cfg.safety)) # 托盘平面（半透明） tray_x = [50, 500, 500, 50, 50] tray_y = [-130,-130,-30,-30,-130] tray_z = [175]*5 fig.add_trace(go.Scatter3d(x=tray_x, y=tray_y, z=tray_z, mode='lines', line=dict(color='#95a5a6', width=3), name='器械托盘', fill='toself')) # 机械臂底座 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='机械臂底座')) # 递送点 deliver = cfg.robot.deliver_pose fig.add_trace(go.Scatter3d(x=[-50], y=[-200], z=[350], mode='markers+text', marker=dict(size=10, color='#1abc9c', symbol='diamond'), text=['递送点（医生侧）'], textposition='top center', name='递送点')) # 器械槽位 for slot in SLOTS: p = slot['grasp_point'] # 夹取点（托盘上） fig.add_trace(go.Scatter3d( x=[p[0]], y=[p[1]], z=[p[2]], mode='markers+text', marker=dict(size=10, color=slot['color'], symbol='circle', line=dict(width=2, color='white')), text=[slot['name']], textposition='top center', name=slot['name'], hovertemplate=( f"{slot['name']}
" f"槽位: {slot['slot_id']}
" f"坐标: ({p[0]:.0f}, {p[1]:.0f}, {p[2]:.0f})
" f"朝向: {slot['rz']}°
" f"夹爪预设: {slot['preset']}" ) )) # approach 点（正上方） z_app = p[2] + cfg.robot.z_approach_offset fig.add_trace(go.Scatter3d( x=[p[0]], y=[p[1]], z=[z_app], mode='markers', marker=dict(size=5, color=slot['color'], symbol='circle-open'), name=f"{slot['name']} approach", showlegend=False, hovertemplate=f"approach 点
z={z_app:.0f}mm" )) # 垂直连线（approach → grasp） fig.add_trace(go.Scatter3d( x=[p[0],p[0]], y=[p[1],p[1]], z=[p[2],z_app], mode='lines', line=dict(color=slot['color'], width=1, dash='dot'), showlegend=False, hoverinfo='skip' )) fig.update_layout( title=dict(text='🤖 SurgBot 工作空间 & 器械槽位分布', font=dict(size=16)), scene=dict( xaxis=dict(title='X (mm)', range=[-650, 650]), yaxis=dict(title='Y (mm)', range=[-750, 150]), zaxis=dict(title='Z (mm)', range=[-50, 600]), 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)), ), legend=dict(x=0.01, y=0.99), margin=dict(l=0, r=0, b=0, t=40), height=620, ) fig.show()

5️⃣ 运动路径 3D 轨迹可视化¶

选择器械，查看完整的 approach → grasp → lift → deliver → reset 轨迹。

In [ ]:

# ── 选择要演示的器械槽位 ────────────────────────────
TARGET_SLOT = 'slot_02'   # 修改这里选择不同器械
# ─────────────────────────────────────────────────────

slot = next(s for s in SLOTS if s['slot_id'] == TARGET_SLOT)
p    = slot['grasp_point']
z_a  = p[2] + cfg.robot.z_approach_offset   # approach Z

# 递送点（近似笛卡尔，关节角模式实际不直接可视化）
DELIVER_XYZ  = [-50.0, -250.0, 350.0]
RESET_XYZ    = [  0.0,  -50.0, 400.0]

# 轨迹关键点
waypoints = [
    ('待机姿态',   RESET_XYZ,              '#95a5a6'),
    ('approach',  [p[0], p[1], z_a],       '#3498db'),
    ('grasp',     [p[0], p[1], p[2]],      '#e74c3c'),
    ('lift',      [p[0], p[1], z_a],       '#e67e22'),
    ('deliver',   DELIVER_XYZ,             '#2ecc71'),
    ('reset',     RESET_XYZ,               '#95a5a6'),
]

fig2 = go.Figure()

# 工作空间边界框（淡显）
fig2.add_trace(make_workspace_box(cfg.safety))

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

# 各阶段节点（分颜色）
phase_labels = {
    '待机姿态': '⚪ 待机',
    'approach': '🔵 approach（到位点上方）',
    'grasp':    '🔴 grasp（下降夹取）',
    'lift':     '🟠 lift（提升）',
    'deliver':  '🟢 deliver（递送到医生）',
    'reset':    '⚪ reset（回待机）',
}
for name, pt, color in waypoints:
    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=[phase_labels[name]],
        textposition='top center',
        name=phase_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+text',
    marker=dict(size=14, color=slot['color'], symbol='diamond',
                line=dict(width=2, color='white')),
    text=[f"📍 {slot['name']} ({slot['slot_id']})"],
    textposition='bottom center',
    name=f"器械槽位: {slot['name']}"
))

# Z 安全高度参考面
fig2.add_trace(go.Surface(
    x=[[-650, 650], [-650, 650]],
    y=[[-750, -750], [150, 150]],
    z=[[cfg.safety.z_min]*2, [cfg.safety.z_min]*2],
    colorscale=[[0,'rgba(231,76,60,0.1)'],[1,'rgba(231,76,60,0.1)']],
    showscale=False, name='Z 下限（托盘保护面）'
))

fig2.update_layout(
    title=dict(
        text=f'🦾 运动轨迹预览 — {slot["name"]} ({TARGET_SLOT})',
        font=dict(size=16)
    ),
    scene=dict(
        xaxis=dict(title='X (mm)', range=[-650, 650]),
        yaxis=dict(title='Y (mm)', range=[-750, 150]),
        zaxis=dict(title='Z (mm)', range=[-50,  600]),
        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)),
    ),
    legend=dict(x=0.01, y=0.99),
    margin=dict(l=0, r=0, b=0, t=40),
    height=650,
)
fig2.show()
print(f'\n📋 路径信息')
print(f'  器械      : {slot["name"]}  (preset={slot["preset"]})')
print(f'  夹取坐标  : ({p[0]:.1f}, {p[1]:.1f}, {p[2]:.1f}) mm')
print(f'  approach Z: {z_a:.1f} mm  (= grasp_z + {cfg.robot.z_approach_offset:.0f}mm)')
print(f'  末端朝向  : Rz = {slot["rz"]}°  (手柄朝向医生)')

# ── 选择要演示的器械槽位 ──────────────────────────── TARGET_SLOT = 'slot_02' # 修改这里选择不同器械 # ───────────────────────────────────────────────────── slot = next(s for s in SLOTS if s['slot_id'] == TARGET_SLOT) p = slot['grasp_point'] z_a = p[2] + cfg.robot.z_approach_offset # approach Z # 递送点（近似笛卡尔，关节角模式实际不直接可视化） DELIVER_XYZ = [-50.0, -250.0, 350.0] RESET_XYZ = [ 0.0, -50.0, 400.0] # 轨迹关键点 waypoints = [ ('待机姿态', RESET_XYZ, '#95a5a6'), ('approach', [p[0], p[1], z_a], '#3498db'), ('grasp', [p[0], p[1], p[2]], '#e74c3c'), ('lift', [p[0], p[1], z_a], '#e67e22'), ('deliver', DELIVER_XYZ, '#2ecc71'), ('reset', RESET_XYZ, '#95a5a6'), ] fig2 = go.Figure() # 工作空间边界框（淡显） fig2.add_trace(make_workspace_box(cfg.safety)) # 轨迹线 xs = [w[1][0] for w in waypoints] ys = [w[1][1] for w in waypoints] zs = [w[1][2] for w in waypoints] fig2.add_trace(go.Scatter3d( x=xs, y=ys, z=zs, mode='lines', line=dict(color='#bdc3c7', width=3), name='路径总览', showlegend=False )) # 各阶段节点（分颜色） phase_labels = { '待机姿态': '⚪ 待机', 'approach': '🔵 approach（到位点上方）', 'grasp': '🔴 grasp（下降夹取）', 'lift': '🟠 lift（提升）', 'deliver': '🟢 deliver（递送到医生）', 'reset': '⚪ reset（回待机）', } for name, pt, color in waypoints: 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=[phase_labels[name]], textposition='top center', name=phase_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+text', marker=dict(size=14, color=slot['color'], symbol='diamond', line=dict(width=2, color='white')), text=[f"📍 {slot['name']} ({slot['slot_id']})"], textposition='bottom center', name=f"器械槽位: {slot['name']}" )) # Z 安全高度参考面 fig2.add_trace(go.Surface( x=[[-650, 650], [-650, 650]], y=[[-750, -750], [150, 150]], z=[[cfg.safety.z_min]*2, [cfg.safety.z_min]*2], colorscale=[[0,'rgba(231,76,60,0.1)'],[1,'rgba(231,76,60,0.1)']], showscale=False, name='Z 下限（托盘保护面）' )) fig2.update_layout( title=dict( text=f'🦾 运动轨迹预览 — {slot["name"]} ({TARGET_SLOT})', font=dict(size=16) ), scene=dict( xaxis=dict(title='X (mm)', range=[-650, 650]), yaxis=dict(title='Y (mm)', range=[-750, 150]), zaxis=dict(title='Z (mm)', range=[-50, 600]), 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)), ), legend=dict(x=0.01, y=0.99), margin=dict(l=0, r=0, b=0, t=40), height=650, ) fig2.show() print(f'\n📋 路径信息') print(f' 器械 : {slot["name"]} (preset={slot["preset"]})') print(f' 夹取坐标 : ({p[0]:.1f}, {p[1]:.1f}, {p[2]:.1f}) mm') print(f' approach Z: {z_a:.1f} mm (= grasp_z + {cfg.robot.z_approach_offset:.0f}mm)') print(f' 末端朝向 : Rz = {slot["rz"]}° (手柄朝向医生)')

6️⃣ 完整 Mock 流程端到端演示¶

模拟语音指令 → NLP 识别 → 路径规划 → 安全校验 → Mock 执行的完整链路。

In [ ]:

from core.interfaces import InstrumentCommand, GraspTarget
from hardware.dobot_arm import DobotArm
import time

print('=' * 55)
print('SurgBot 五一 MVP — 完整 Mock 流程演示')
print('=' * 55)

# ── Step 1: 模拟语音指令（NLP 输出）───────────────
print('\n[1/5] 🎙️  语音指令输入')
voice_text = '递持针器'    # 模拟语音识别结果
print(f'       识别文本: "{voice_text}"')

cmd = InstrumentCommand(
    instrument_id='INS-031',
    name='持针器_大',
    confidence=0.93,
    source_text=voice_text,
    slot_id='slot_01'
)
print(f'       → 器械ID : {cmd.instrument_id}')
print(f'       → 器械名 : {cmd.name}')
print(f'       → 置信度 : {cmd.confidence:.0%}')

# ── Step 2: 模拟感知（查槽位注册表）──────────────
print('\n[2/5] 📷  感知模块（槽位 ROI 识别）')
slot_info = SLOTS[0]   # slot_01
p = slot_info['grasp_point']

grasp = GraspTarget(
    slot_id='slot_01',
    instrument_id='INS-031',
    grasp_point=p,
    orientation_deg=slot_info['rz'],
    confidence=0.96,
    visual_offset=[2.3, -1.1, 0.0]   # 视觉微调偏移
)
print(f'       夹取坐标 : ({grasp.x:.1f}, {grasp.y:.1f}, {grasp.z:.1f}) mm')
print(f'       末端朝向 : {grasp.orientation_deg}°  (已修正，手柄朝医生)')
print(f'       置信度   : {grasp.confidence:.0%}  ✅ > {cfg.perception.min_confidence:.0%} 阈值')
print(f'       视觉偏移  : {grasp.visual_offset} mm')

# ── Step 3: 安全校验 ──────────────────────────────
print('\n[3/5] 🛡️   安全校验')
from core.safety_manager import safety, SafetyError
z_a  = grasp.z + cfg.robot.z_approach_offset
path = [
    [grasp.x, grasp.y, z_a,       -180, 0, grasp.orientation_deg, 0],  # approach
    [grasp.x, grasp.y, grasp.z,   -180, 0, grasp.orientation_deg, 0],  # grasp
    [grasp.x, grasp.y, z_a,       -180, 0, grasp.orientation_deg, 0],  # lift
    cfg.robot.reset_pose,
    cfg.robot.deliver_pose,
]
try:
    safety.validate_grasp(grasp)
    safety.validate_path(path)
    print('       ✅ 所有路径点在工作空间内')
    print('       ✅ 感知置信度达标')
    print('       ✅ 单步移动距离正常')
except SafetyError as e:
    print(f'       🚫 安全校验失败: {e}')
    raise SystemExit('演示终止')

# ── Step 4: Mock 执行 ─────────────────────────────
print('\n[4/5] 🦾  机械臂执行（Mock 模式）')
arm = DobotArm(mock=True)

t0 = time.time()
result = arm.pick_and_deliver(
    grasp_point=grasp.grasp_point,
    rz_deg=grasp.orientation_deg,
    gripper_preset_id=slot_info['preset'],
    instrument_id=grasp.instrument_id
)
elapsed = time.time() - t0

# ── Step 5: 结果汇总 ──────────────────────────────
print(f'\n[5/5] 📊  结果汇总')
print(f'       器械名称  : {cmd.name}')
print(f'       夹取结果  : {"✅ 成功" if result else "⚠️  超时/失败（Mock 无力反馈）"}')
print(f'       总耗时    : {elapsed:.1f}s  (含 Mock 等待时间)')
print(f'       安全急停次数: {safety.stop_count} 次')
print()
print('✅ 完整流程端到端跑通')
arm.shutdown()

from core.interfaces import InstrumentCommand, GraspTarget from hardware.dobot_arm import DobotArm import time print('=' * 55) print('SurgBot 五一 MVP — 完整 Mock 流程演示') print('=' * 55) # ── Step 1: 模拟语音指令（NLP 输出）─────────────── print('\n[1/5] 🎙️ 语音指令输入') voice_text = '递持针器' # 模拟语音识别结果 print(f' 识别文本: "{voice_text}"') cmd = InstrumentCommand( instrument_id='INS-031', name='持针器_大', confidence=0.93, source_text=voice_text, slot_id='slot_01' ) print(f' → 器械ID : {cmd.instrument_id}') print(f' → 器械名 : {cmd.name}') print(f' → 置信度 : {cmd.confidence:.0%}') # ── Step 2: 模拟感知（查槽位注册表）────────────── print('\n[2/5] 📷 感知模块（槽位 ROI 识别）') slot_info = SLOTS[0] # slot_01 p = slot_info['grasp_point'] grasp = GraspTarget( slot_id='slot_01', instrument_id='INS-031', grasp_point=p, orientation_deg=slot_info['rz'], confidence=0.96, visual_offset=[2.3, -1.1, 0.0] # 视觉微调偏移 ) print(f' 夹取坐标 : ({grasp.x:.1f}, {grasp.y:.1f}, {grasp.z:.1f}) mm') print(f' 末端朝向 : {grasp.orientation_deg}° (已修正，手柄朝医生)') print(f' 置信度 : {grasp.confidence:.0%} ✅ > {cfg.perception.min_confidence:.0%} 阈值') print(f' 视觉偏移 : {grasp.visual_offset} mm') # ── Step 3: 安全校验 ────────────────────────────── print('\n[3/5] 🛡️ 安全校验') from core.safety_manager import safety, SafetyError z_a = grasp.z + cfg.robot.z_approach_offset path = [ [grasp.x, grasp.y, z_a, -180, 0, grasp.orientation_deg, 0], # approach [grasp.x, grasp.y, grasp.z, -180, 0, grasp.orientation_deg, 0], # grasp [grasp.x, grasp.y, z_a, -180, 0, grasp.orientation_deg, 0], # lift cfg.robot.reset_pose, cfg.robot.deliver_pose, ] try: safety.validate_grasp(grasp) safety.validate_path(path) print(' ✅ 所有路径点在工作空间内') print(' ✅ 感知置信度达标') print(' ✅ 单步移动距离正常') except SafetyError as e: print(f' 🚫 安全校验失败: {e}') raise SystemExit('演示终止') # ── Step 4: Mock 执行 ───────────────────────────── print('\n[4/5] 🦾 机械臂执行（Mock 模式）') arm = DobotArm(mock=True) t0 = time.time() result = arm.pick_and_deliver( grasp_point=grasp.grasp_point, rz_deg=grasp.orientation_deg, gripper_preset_id=slot_info['preset'], instrument_id=grasp.instrument_id ) elapsed = time.time() - t0 # ── Step 5: 结果汇总 ────────────────────────────── print(f'\n[5/5] 📊 结果汇总') print(f' 器械名称 : {cmd.name}') print(f' 夹取结果 : {"✅ 成功" if result else "⚠️ 超时/失败（Mock 无力反馈）"}') print(f' 总耗时 : {elapsed:.1f}s (含 Mock 等待时间)') print(f' 安全急停次数: {safety.stop_count} 次') print() print('✅ 完整流程端到端跑通') arm.shutdown()

---

📌 说明¶

• Mock 模式：所有机械臂动作均由 _MockRobot 模拟，无需连接真实硬件
• wait_for_handover 超时：Mock 中力反馈始终为 0，60 秒后自动超时并松开夹爪；真实机械臂由医生触力触发
• 槽位坐标：当前为演示用估算值，上机标定后写入 data/instrument_registry.json

下一步¶

模块	状态	负责人
core/ 骨架	✅ 完成	任松
hardware/dobot_arm.py	✅ 完成	任松
modules/perception/position_registry.py	🔄 开发中	李淑雅
modules/nlp/keyword_matcher.py	🔄 开发中	陈端端
上机标定槽位坐标	⭕ 待开始	全员

📖 完整文档：https://zebedee2021.github.io/surgbot-docs-v2/