SurgBot × MuJoCo 物理仿真演示¶
近似机器人模型（UR5/Panda 风格 mocap EE）代替 Dobot CR5，走通完整仿真流程。
运行环境自动适配：
- ☁️ Google Colab（Python 3.10）：自动安装 MuJoCo 3.x，真实物理仿真 + 相机渲染
- 💻 本地 Python ≤ 3.12：需提前
pip install mujoco，本地物理仿真- 🔁 本地 Python 3.13+ / CI：自动降级为 NumPy 插值 + Plotly 等效可视化
仿真流程¶
Step 0 · 环境安装 pip install mujoco（Colab 自动执行）
↓
Step 1 · 场景说明 MJCF 结构、mm ↔ m 坐标映射表
↓
Step 2 · 3D 场景可视化 Plotly 交互图（托盘 + 5 器械槽位 + mocap EE）
↓
Step 3 · 轨迹仿真 approach → grasp → lift → deliver（物理步进 or NumPy）
↓
Step 4 · 接触力分析 夹取过程力曲线，grasp_success 判定
↓
Step 5 · 相机渲染 MuJoCo 俯视 / 侧视 RGB 图像（有 MuJoCo 时）
↓
Step 5b· 仿真视频 双相机并排 GIF（front_view + side_obs，imageio 合成）
↓
Step 6 · 全链路集成 SurgBotStateMachine(sim=True / mock=True) 端到端验证
Step 0 — 环境安装（Colab / 本地均适用）¶
In [ ]:
Copied!
# ── Step 0：依赖安装（Colab / 本地均适用）─────────────────────────────
import os, sys
IN_COLAB = "google.colab" in sys.modules or os.path.exists("/content")
if IN_COLAB:
print("检测到 Colab 环境，开始安装依赖...")
# ① 系统库
# libosmesa6 → osmesa 运行时（CPU 软件渲染，无需 GPU/EGL）
# libosmesa6-dev → 头文件（部分发行版 Renderer 需要）
# libgl1-mesa-glx → OpenGL 基础库
os.system(
"apt-get install -y -q "
"libosmesa6 libosmesa6-dev "
"libgl1-mesa-glx libglib2.0-0 2>/dev/null"
)
# ② Python 依赖
# pyopengl → osmesa Renderer 后端必需
os.system("pip install -q 'mujoco>=3.1' plotly loguru pyopengl")
# ③ 拉取 surgbot-docs-v2 代码
REPO = "https://github.com/Zebedee2021/surgbot-docs-v2.git"
LOCAL = "/content/surgbot-docs"
if os.path.exists(LOCAL):
os.system(f"cd {LOCAL} && git pull -q")
print("✅ 代码已更新")
else:
os.system(f"git clone -q {REPO} {LOCAL}")
print("✅ 代码拉取完成")
CODE_DIR = f"{LOCAL}/surgbot"
else:
print("本地环境，跳过自动安装")
CODE_DIR = None
print("Step 0 完成")
# ── Step 0：依赖安装（Colab / 本地均适用）─────────────────────────────
import os, sys
IN_COLAB = "google.colab" in sys.modules or os.path.exists("/content")
if IN_COLAB:
print("检测到 Colab 环境，开始安装依赖...")
# ① 系统库
# libosmesa6 → osmesa 运行时（CPU 软件渲染，无需 GPU/EGL）
# libosmesa6-dev → 头文件（部分发行版 Renderer 需要）
# libgl1-mesa-glx → OpenGL 基础库
os.system(
"apt-get install -y -q "
"libosmesa6 libosmesa6-dev "
"libgl1-mesa-glx libglib2.0-0 2>/dev/null"
)
# ② Python 依赖
# pyopengl → osmesa Renderer 后端必需
os.system("pip install -q 'mujoco>=3.1' plotly loguru pyopengl")
# ③ 拉取 surgbot-docs-v2 代码
REPO = "https://github.com/Zebedee2021/surgbot-docs-v2.git"
LOCAL = "/content/surgbot-docs"
if os.path.exists(LOCAL):
os.system(f"cd {LOCAL} && git pull -q")
print("✅ 代码已更新")
else:
os.system(f"git clone -q {REPO} {LOCAL}")
print("✅ 代码拉取完成")
CODE_DIR = f"{LOCAL}/surgbot"
else:
print("本地环境，跳过自动安装")
CODE_DIR = None
print("Step 0 完成")
In [ ]:
Copied!
# ── 路径初始化 ────────────────────────────────────────────────────────────
import os, sys
from pathlib import Path
os.environ["SURGBOT_NOTEBOOK_MODE"] = "1" # 日志静默
IN_COLAB = "google.colab" in sys.modules or os.path.exists("/content")
if IN_COLAB:
CODE_DIR = "/content/surgbot-docs/surgbot"
if CODE_DIR not in sys.path:
sys.path.insert(0, CODE_DIR)
SURGBOT_ROOT = Path(CODE_DIR)
else:
HERE = Path(os.getcwd())
SURGBOT_ROOT = None
for _p in [HERE, HERE.parent, HERE.parent.parent]:
_c = _p / "surgbot"
if _c.exists():
if str(_c) not in sys.path:
sys.path.insert(0, str(_c))
SURGBOT_ROOT = _c
break
# ── MuJoCo 渲染后端：osmesa（CPU 软件渲染）优先 ─────────────────────────
# osmesa 不依赖 GPU / EGL，在 Colab CPU runtime 和无显示器服务器均可用。
# 必须在 import mujoco 以及创建 Renderer 之前设置。
os.environ.setdefault("MUJOCO_GL", "osmesa")
# ── MuJoCo 可用性探测 ────────────────────────────────────────────────────
try:
import mujoco
HAS_MUJOCO = True
_mj_ver = mujoco.__version__
except ImportError:
HAS_MUJOCO = False
_mj_ver = "N/A"
import numpy as np
import plotly.graph_objects as go
import plotly.io as pio
pio.renderers.default = "colab" if IN_COLAB else "notebook"
print(f"环境 : {"Google Colab" if IN_COLAB else "本地"}")
print(f"Python : {sys.version.split()[0]}")
print(f"MuJoCo : {_mj_ver if HAS_MUJOCO else "未安装（NumPy 等效模式）"}")
print(f"surgbot 路径 : {SURGBOT_ROOT}")
print(f"MUJOCO_GL : {os.environ.get("MUJOCO_GL", "未设置")}")
# ── 路径初始化 ────────────────────────────────────────────────────────────
import os, sys
from pathlib import Path
os.environ["SURGBOT_NOTEBOOK_MODE"] = "1" # 日志静默
IN_COLAB = "google.colab" in sys.modules or os.path.exists("/content")
if IN_COLAB:
CODE_DIR = "/content/surgbot-docs/surgbot"
if CODE_DIR not in sys.path:
sys.path.insert(0, CODE_DIR)
SURGBOT_ROOT = Path(CODE_DIR)
else:
HERE = Path(os.getcwd())
SURGBOT_ROOT = None
for _p in [HERE, HERE.parent, HERE.parent.parent]:
_c = _p / "surgbot"
if _c.exists():
if str(_c) not in sys.path:
sys.path.insert(0, str(_c))
SURGBOT_ROOT = _c
break
# ── MuJoCo 渲染后端：osmesa（CPU 软件渲染）优先 ─────────────────────────
# osmesa 不依赖 GPU / EGL，在 Colab CPU runtime 和无显示器服务器均可用。
# 必须在 import mujoco 以及创建 Renderer 之前设置。
os.environ.setdefault("MUJOCO_GL", "osmesa")
# ── MuJoCo 可用性探测 ────────────────────────────────────────────────────
try:
import mujoco
HAS_MUJOCO = True
_mj_ver = mujoco.__version__
except ImportError:
HAS_MUJOCO = False
_mj_ver = "N/A"
import numpy as np
import plotly.graph_objects as go
import plotly.io as pio
pio.renderers.default = "colab" if IN_COLAB else "notebook"
print(f"环境 : {"Google Colab" if IN_COLAB else "本地"}")
print(f"Python : {sys.version.split()[0]}")
print(f"MuJoCo : {_mj_ver if HAS_MUJOCO else "未安装（NumPy 等效模式）"}")
print(f"surgbot 路径 : {SURGBOT_ROOT}")
print(f"MUJOCO_GL : {os.environ.get("MUJOCO_GL", "未设置")}")
Step 1 — MJCF 场景与坐标映射¶
weld 约束 = 免写 IK¶
data.mocap_pos[0] = target_m # 设置目标笛卡尔坐标
# ↓ MuJoCo Newton 约束求解器
# ↓ 自动计算 6 个关节角满足 weld 等式
mj_step() × N # 运行若干步让臂收敛到目标
机械臂几何参数（UR5e 近似，纯 MJCF 原始几何）¶
| 连杆 | 长度 | 几何体 |
|---|---|---|
| 底座 | h=0.150 m | cylinder（灰色） |
| 肩偏置 | 0.131 m Y | capsule（蓝色） |
| 大臂（J2→J3） | 0.425 m X | capsule（蓝色） |
| 前臂（J3→J4） | 0.392 m X | capsule（浅蓝） |
| 腕偏置（J4→J5） | 0.093 m -Y | capsule（浅蓝） |
| 腕2（J5→EE） | 0.095 m X | capsule（浅蓝） |
| 末端法兰 + 夹爪 | — | sphere（橙）+ capsule（棕） |
坐标映射¶
| 来源 | 单位 | 示例（slot_01） |
|---|---|---|
instrument_registry.json grasp_point |
mm | [120, -80, 181] |
MJCF geom pos / mocap 目标 |
m | [0.120, -0.080, 0.218] |
In [ ]:
Copied!
# ── 槽位参数（mm，与 instrument_registry.json 一致）──────────
SLOTS_MM = [
{'id': 'slot_01', 'name': '持针器_大', 'pt': [120.0, -80.0, 181.0], 'rz': 45.0, 'color': '#e74c3c'},
{'id': 'slot_02', 'name': '剪刀', 'pt': [200.0, -80.0, 181.0], 'rz': 0.0, 'color': '#3498db'},
{'id': 'slot_03', 'name': '镊子', 'pt': [280.0, -80.0, 181.0], 'rz': 90.0, 'color': '#2ecc71'},
{'id': 'slot_04', 'name': '刀柄', 'pt': [360.0, -80.0, 181.0], 'rz': 30.0, 'color': '#f39c12'},
{'id': 'slot_05', 'name': '持针器_小', 'pt': [440.0, -80.0, 181.0], 'rz': 45.0, 'color': '#9b59b6'},
]
Z_APPROACH_OFFSET_MM = 150.0
DELIVER_MM = [-50.0, -250.0, 350.0]
RESET_MM = [ 0.0, -50.0, 400.0]
print(f'{"槽位":<10} {"名称":<12} {"mm 坐标":<28} {"m 坐标（MJCF）"}')
print('─' * 72)
for s in SLOTS_MM:
p = s['pt']
pm = [v * 0.001 for v in p]
print(f'{s["id"]:<10} {s["name"]:<12}'
f' [{p[0]:>5.0f},{p[1]:>5.0f},{p[2]:>5.0f}] mm'
f' [{pm[0]:>6.3f},{pm[1]:>6.3f},{pm[2]:>6.3f}] m')
# ── 槽位参数（mm，与 instrument_registry.json 一致）──────────
SLOTS_MM = [
{'id': 'slot_01', 'name': '持针器_大', 'pt': [120.0, -80.0, 181.0], 'rz': 45.0, 'color': '#e74c3c'},
{'id': 'slot_02', 'name': '剪刀', 'pt': [200.0, -80.0, 181.0], 'rz': 0.0, 'color': '#3498db'},
{'id': 'slot_03', 'name': '镊子', 'pt': [280.0, -80.0, 181.0], 'rz': 90.0, 'color': '#2ecc71'},
{'id': 'slot_04', 'name': '刀柄', 'pt': [360.0, -80.0, 181.0], 'rz': 30.0, 'color': '#f39c12'},
{'id': 'slot_05', 'name': '持针器_小', 'pt': [440.0, -80.0, 181.0], 'rz': 45.0, 'color': '#9b59b6'},
]
Z_APPROACH_OFFSET_MM = 150.0
DELIVER_MM = [-50.0, -250.0, 350.0]
RESET_MM = [ 0.0, -50.0, 400.0]
print(f'{"槽位":<10} {"名称":<12} {"mm 坐标":<28} {"m 坐标（MJCF）"}')
print('─' * 72)
for s in SLOTS_MM:
p = s['pt']
pm = [v * 0.001 for v in p]
print(f'{s["id"]:<10} {s["name"]:<12}'
f' [{p[0]:>5.0f},{p[1]:>5.0f},{p[2]:>5.0f}] mm'
f' [{pm[0]:>6.3f},{pm[1]:>6.3f},{pm[2]:>6.3f}] m')
Step 2 — 3D 场景可视化（Plotly 交互）¶
Plotly 图与 MuJoCo SCENE_XML 中的几何体位置精确对应，可用于标定结果的视觉验证：
| 元素 | 颜色 | 说明 |
|---|---|---|
| 灰线框 | 灰 | 器械托盘轮廓 |
| 彩色实心点 | 各色 | 5 个器械夹取点 |
| 空心圆 | 各色 | approach 悬停点（+150 mm） |
| 绿色菱形 | 绿 | 递送点（医生侧） |
| 橙色球 | 橙 | EE mocap 初始位置 |
In [ ]:
Copied!
fig = go.Figure()
# 托盘轮廓
fig.add_trace(go.Scatter3d(
x=[50, 510, 510, 50, 50], y=[-140, -140, -20, -20, -140], z=[180]*5,
mode='lines', line=dict(color='#95a5a6', width=4), name='器械托盘'))
# 机械臂底座
fig.add_trace(go.Scatter3d(
x=[0], y=[0], z=[0], mode='markers+text',
marker=dict(size=14, color='#2c3e50', symbol='square'),
text=['底座'], textposition='top center', name='机械臂底座'))
# 递送点
fig.add_trace(go.Scatter3d(
x=[DELIVER_MM[0]], y=[DELIVER_MM[1]], z=[DELIVER_MM[2]],
mode='markers+text',
marker=dict(size=12, color='#1abc9c', symbol='diamond'),
text=['递送点'], textposition='top center', name='递送点（医生侧）'))
# EE mocap 初始位置
fig.add_trace(go.Scatter3d(
x=[280], y=[-80], z=[360], mode='markers+text',
marker=dict(size=10, color='#e67e22', symbol='circle'),
text=['EE (init)'], textposition='top center', name='末端执行器（mocap）'))
# 各槽位
for sl in SLOTS_MM:
p = sl['pt']
za = p[2] + Z_APPROACH_OFFSET_MM
fig.add_trace(go.Scatter3d(
x=[p[0]], y=[p[1]], z=[p[2]], mode='markers+text',
marker=dict(size=12, 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='SurgBot 仿真场景（Plotly 交互，坐标单位：mm）',
scene=dict(
xaxis=dict(title='X (mm)'), yaxis=dict(title='Y (mm)'), zaxis=dict(title='Z (mm)'),
aspectmode='manual', aspectratio=dict(x=1.2, y=1.4, z=1.0),
camera=dict(eye=dict(x=1.6, y=-1.8, z=1.2))),
margin=dict(l=0, r=0, b=0, t=40), height=580)
fig.show()
fig = go.Figure()
# 托盘轮廓
fig.add_trace(go.Scatter3d(
x=[50, 510, 510, 50, 50], y=[-140, -140, -20, -20, -140], z=[180]*5,
mode='lines', line=dict(color='#95a5a6', width=4), name='器械托盘'))
# 机械臂底座
fig.add_trace(go.Scatter3d(
x=[0], y=[0], z=[0], mode='markers+text',
marker=dict(size=14, color='#2c3e50', symbol='square'),
text=['底座'], textposition='top center', name='机械臂底座'))
# 递送点
fig.add_trace(go.Scatter3d(
x=[DELIVER_MM[0]], y=[DELIVER_MM[1]], z=[DELIVER_MM[2]],
mode='markers+text',
marker=dict(size=12, color='#1abc9c', symbol='diamond'),
text=['递送点'], textposition='top center', name='递送点（医生侧）'))
# EE mocap 初始位置
fig.add_trace(go.Scatter3d(
x=[280], y=[-80], z=[360], mode='markers+text',
marker=dict(size=10, color='#e67e22', symbol='circle'),
text=['EE (init)'], textposition='top center', name='末端执行器（mocap）'))
# 各槽位
for sl in SLOTS_MM:
p = sl['pt']
za = p[2] + Z_APPROACH_OFFSET_MM
fig.add_trace(go.Scatter3d(
x=[p[0]], y=[p[1]], z=[p[2]], mode='markers+text',
marker=dict(size=12, 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='SurgBot 仿真场景（Plotly 交互，坐标单位：mm）', scene=dict( xaxis=dict(title='X (mm)'), yaxis=dict(title='Y (mm)'), zaxis=dict(title='Z (mm)'), aspectmode='manual', aspectratio=dict(x=1.2, y=1.4, z=1.0), camera=dict(eye=dict(x=1.6, y=-1.8, z=1.2))), margin=dict(l=0, r=0, b=0, t=40), height=580) fig.show()
{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='SurgBot 仿真场景（Plotly 交互，坐标单位：mm）', scene=dict( xaxis=dict(title='X (mm)'), yaxis=dict(title='Y (mm)'), zaxis=dict(title='Z (mm)'), aspectmode='manual', aspectratio=dict(x=1.2, y=1.4, z=1.0), camera=dict(eye=dict(x=1.6, y=-1.8, z=1.2))), margin=dict(l=0, r=0, b=0, t=40), height=580) fig.show()
Step 3 — 轨迹仿真¶
以 剪刀（slot_02） 为例，仿真完整递送路径：
| 阶段 | 描述 | MuJoCo 实现 |
|---|---|---|
| approach | 移到夹取点正上方 | 小步更新 data.mocap_pos，每步 mj_step() |
| grasp | 垂直下降到夹取点 | 同上 |
| lift | 垂直上升离开托盘 | 同上 |
| deliver | 移向递送点 | 同上 |
无 MuJoCo：NumPy 线性插值生成等效轨迹，图形输出与有 MuJoCo 时一致。
In [ ]:
Copied!
TARGET = 'slot_02'
sl = next(s for s in SLOTS_MM if s['id'] == TARGET)
p = np.array(sl['pt'], dtype=float)
za = p[2] + Z_APPROACH_OFFSET_MM
waypoints = [
('待机', np.array(RESET_MM)),
('approach', np.array([p[0], p[1], za])),
('grasp', p.copy()),
('lift', np.array([p[0], p[1], za])),
('deliver', np.array(DELIVER_MM)),
('reset', np.array(RESET_MM)),
]
N_INTERP = 30
if HAS_MUJOCO:
try:
from hardware.mujoco_robot import MuJoCoRobot
robot = MuJoCoRobot()
wp_mm = [w[1] for w in waypoints]
positions, forces = robot.trajectory_record(wp_mm, steps_per_segment=N_INTERP)
traj_mm = np.array(positions) * 1000.0
force_arr = np.array(forces)
t_arr = np.arange(len(force_arr))
sim_mode = 'MuJoCo 物理仿真'
robot.close()
except Exception as e:
print(f'[MuJoCo] 失败，回退 NumPy: {e}')
HAS_MUJOCO = False
if not HAS_MUJOCO:
segs = []
for i in range(len(waypoints) - 1):
segs.append(np.linspace(waypoints[i][1], waypoints[i+1][1], N_INTERP))
traj_mm = np.vstack(segs)
n = len(traj_mm)
force_arr = np.zeros(n)
g0, g1 = 2*N_INTERP, 3*N_INTERP
t_g = np.arange(g1 - g0)
force_arr[g0:g1] = 0.08 * np.sin(np.pi * t_g / len(t_g))
t_arr = np.arange(n)
sim_mode = 'NumPy 插值（MuJoCo 等效）'
# 3D 轨迹图
COLORS6 = ['#95a5a6','#3498db','#e74c3c','#e67e22','#2ecc71','#95a5a6']
LABELS = {'待机':'⚪ 待机','approach':'🔵 approach','grasp':'🔴 grasp',
'lift':'🟠 lift','deliver':'🟢 deliver','reset':'⚪ reset'}
fig3 = go.Figure()
fig3.add_trace(go.Scatter3d(
x=traj_mm[:,0], y=traj_mm[:,1], z=traj_mm[:,2],
mode='lines', line=dict(color='#bdc3c7', width=3), name='EE 轨迹'))
for (name, pt), color in zip(waypoints, COLORS6):
fig3.add_trace(go.Scatter3d(
x=[pt[0]], y=[pt[1]], z=[pt[2]], mode='markers+text',
marker=dict(size=11, 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'))
fig3.update_layout(
title=f'EE 轨迹 — {sl["name"]} ({TARGET}) | {sim_mode}',
scene=dict(
xaxis=dict(title='X (mm)'), yaxis=dict(title='Y (mm)'), zaxis=dict(title='Z (mm)'),
aspectmode='manual', aspectratio=dict(x=1.2, 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=560)
fig3.show()
total_dist = sum(np.linalg.norm(traj_mm[i+1]-traj_mm[i]) for i in range(len(traj_mm)-1))
print(f'\n仿真模式 : {sim_mode}')
print(f'轨迹总点数 : {len(traj_mm)}')
print(f'路径总长度 : {total_dist:.1f} mm')
TARGET = 'slot_02'
sl = next(s for s in SLOTS_MM if s['id'] == TARGET)
p = np.array(sl['pt'], dtype=float)
za = p[2] + Z_APPROACH_OFFSET_MM
waypoints = [
('待机', np.array(RESET_MM)),
('approach', np.array([p[0], p[1], za])),
('grasp', p.copy()),
('lift', np.array([p[0], p[1], za])),
('deliver', np.array(DELIVER_MM)),
('reset', np.array(RESET_MM)),
]
N_INTERP = 30
if HAS_MUJOCO:
try:
from hardware.mujoco_robot import MuJoCoRobot
robot = MuJoCoRobot()
wp_mm = [w[1] for w in waypoints]
positions, forces = robot.trajectory_record(wp_mm, steps_per_segment=N_INTERP)
traj_mm = np.array(positions) * 1000.0
force_arr = np.array(forces)
t_arr = np.arange(len(force_arr))
sim_mode = 'MuJoCo 物理仿真'
robot.close()
except Exception as e:
print(f'[MuJoCo] 失败，回退 NumPy: {e}')
HAS_MUJOCO = False
if not HAS_MUJOCO:
segs = []
for i in range(len(waypoints) - 1):
segs.append(np.linspace(waypoints[i][1], waypoints[i+1][1], N_INTERP))
traj_mm = np.vstack(segs)
n = len(traj_mm)
force_arr = np.zeros(n)
g0, g1 = 2*N_INTERP, 3*N_INTERP
t_g = np.arange(g1 - g0)
force_arr[g0:g1] = 0.08 * np.sin(np.pi * t_g / len(t_g))
t_arr = np.arange(n)
sim_mode = 'NumPy 插值（MuJoCo 等效）'
# 3D 轨迹图
COLORS6 = ['#95a5a6','#3498db','#e74c3c','#e67e22','#2ecc71','#95a5a6']
LABELS = {'待机':'⚪ 待机','approach':'🔵 approach','grasp':'🔴 grasp',
'lift':'🟠 lift','deliver':'🟢 deliver','reset':'⚪ reset'}
fig3 = go.Figure()
fig3.add_trace(go.Scatter3d(
x=traj_mm[:,0], y=traj_mm[:,1], z=traj_mm[:,2],
mode='lines', line=dict(color='#bdc3c7', width=3), name='EE 轨迹'))
for (name, pt), color in zip(waypoints, COLORS6):
fig3.add_trace(go.Scatter3d(
x=[pt[0]], y=[pt[1]], z=[pt[2]], mode='markers+text',
marker=dict(size=11, 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')) fig3.update_layout( title=f'EE 轨迹 — {sl["name"]} ({TARGET}) | {sim_mode}', scene=dict( xaxis=dict(title='X (mm)'), yaxis=dict(title='Y (mm)'), zaxis=dict(title='Z (mm)'), aspectmode='manual', aspectratio=dict(x=1.2, 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=560) fig3.show() total_dist = sum(np.linalg.norm(traj_mm[i+1]-traj_mm[i]) for i in range(len(traj_mm)-1)) print(f'\n仿真模式 : {sim_mode}') print(f'轨迹总点数 : {len(traj_mm)}') print(f'路径总长度 : {total_dist:.1f} mm')
({pt[0]:.0f},{pt[1]:.0f},{pt[2]:.0f}) mm')) fig3.update_layout( title=f'EE 轨迹 — {sl["name"]} ({TARGET}) | {sim_mode}', scene=dict( xaxis=dict(title='X (mm)'), yaxis=dict(title='Y (mm)'), zaxis=dict(title='Z (mm)'), aspectmode='manual', aspectratio=dict(x=1.2, 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=560) fig3.show() total_dist = sum(np.linalg.norm(traj_mm[i+1]-traj_mm[i]) for i in range(len(traj_mm)-1)) print(f'\n仿真模式 : {sim_mode}') print(f'轨迹总点数 : {len(traj_mm)}') print(f'路径总长度 : {total_dist:.1f} mm')
Step 4 — 接触力分析¶
夹取判定逻辑（MuJoCoRobot.close_gripper()）：
GRASP_FORCE_THRESHOLD_N = 0.05 # N
def close_gripper(self, preset_id=1) -> bool:
force = self.get_contact_force() # mujoco.mj_contactForce 所有接触点最大值
return force > GRASP_FORCE_THRESHOLD_N
红色虚线为判定阈值（0.05 N），右图为 grasp 阶段放大视图。
In [ ]:
Copied!
import plotly.subplots as sp
GRASP_THRESH = 0.05
fig4 = sp.make_subplots(
rows=1, cols=2,
subplot_titles=['接触力时间序列（全程）', '夹取阶段放大'],
horizontal_spacing=0.10)
fig4.add_trace(go.Scatter(x=t_arr, y=force_arr, mode='lines',
line=dict(color='#3498db', width=1.5), name='接触力 (N)'), row=1, col=1)
fig4.add_hline(y=GRASP_THRESH, line_dash='dash', line_color='red',
annotation_text='阈值 0.05N', row=1, col=1)
# grasp 阶段放大
mask = force_arr > 0.001
if mask.any():
idx = np.where(mask)[0]
i0, i1 = max(0, idx[0]-5), min(len(force_arr)-1, idx[-1]+10)
else:
i0, i1 = len(force_arr)//3, len(force_arr)*2//3
fig4.add_trace(go.Scatter(x=t_arr[i0:i1], y=force_arr[i0:i1],
mode='lines+markers', line=dict(color='#e74c3c', width=2),
marker=dict(size=4), name='夹取段'), row=1, col=2)
fig4.add_hline(y=GRASP_THRESH, line_dash='dash', line_color='red',
annotation_text='阈值', row=1, col=2)
fig4.update_layout(title=f'接触力分析 | {sim_mode}', height=360)
fig4.update_xaxes(title_text='仿真步', row=1, col=1)
fig4.update_xaxes(title_text='仿真步', row=1, col=2)
fig4.update_yaxes(title_text='力 (N)', row=1, col=1)
fig4.update_yaxes(title_text='力 (N)', row=1, col=2)
fig4.show()
max_f = force_arr.max()
grasp_ok = max_f > GRASP_THRESH
print(f'最大接触力 : {max_f:.4f} N')
print(f'夹取判定 : {"✅ grasp_success=True" if grasp_ok else "❌ grasp_success=False（力不足）"}')
import plotly.subplots as sp
GRASP_THRESH = 0.05
fig4 = sp.make_subplots(
rows=1, cols=2,
subplot_titles=['接触力时间序列（全程）', '夹取阶段放大'],
horizontal_spacing=0.10)
fig4.add_trace(go.Scatter(x=t_arr, y=force_arr, mode='lines',
line=dict(color='#3498db', width=1.5), name='接触力 (N)'), row=1, col=1)
fig4.add_hline(y=GRASP_THRESH, line_dash='dash', line_color='red',
annotation_text='阈值 0.05N', row=1, col=1)
# grasp 阶段放大
mask = force_arr > 0.001
if mask.any():
idx = np.where(mask)[0]
i0, i1 = max(0, idx[0]-5), min(len(force_arr)-1, idx[-1]+10)
else:
i0, i1 = len(force_arr)//3, len(force_arr)*2//3
fig4.add_trace(go.Scatter(x=t_arr[i0:i1], y=force_arr[i0:i1],
mode='lines+markers', line=dict(color='#e74c3c', width=2),
marker=dict(size=4), name='夹取段'), row=1, col=2)
fig4.add_hline(y=GRASP_THRESH, line_dash='dash', line_color='red',
annotation_text='阈值', row=1, col=2)
fig4.update_layout(title=f'接触力分析 | {sim_mode}', height=360)
fig4.update_xaxes(title_text='仿真步', row=1, col=1)
fig4.update_xaxes(title_text='仿真步', row=1, col=2)
fig4.update_yaxes(title_text='力 (N)', row=1, col=1)
fig4.update_yaxes(title_text='力 (N)', row=1, col=2)
fig4.show()
max_f = force_arr.max()
grasp_ok = max_f > GRASP_THRESH
print(f'最大接触力 : {max_f:.4f} N')
print(f'夹取判定 : {"✅ grasp_success=True" if grasp_ok else "❌ grasp_success=False（力不足）"}')
Step 5 — 相机渲染（MuJoCo 视角图像）¶
三个虚拟相机同时渲染，front_view 可以看到完整的 UR5 近似机械臂：
| 相机 | 位置 | 显示内容 |
|---|---|---|
front_view |
正前斜上方 | 6-DOF 机械臂全貌 + 托盘 |
overhead |
正上方 0.72 m | 俯视托盘 + EE 位置 |
side_obs |
斜侧方 | 侧视臂高度 & 托盘 |
EGL headless 渲染（MUJOCO_GL=egl），Colab T4 GPU 直接支持，无需 X11 display。
机械臂几何体说明：底座灰色柱体 → 蓝色连杆（J1~J5）→ 橙色末端法兰 → 棕色夹爪， 完全由 capsule / box / sphere 原始几何体构成，无需外部 mesh 文件。
In [ ]:
Copied!
# ════════════════════════════════════════════════════════════════════════
# Step 5 — MuJoCo 相机渲染
#
# 本 cell 独立检测 mujoco，不依赖前序 cell 的 HAS_MUJOCO 状态。
# 渲染后端：MUJOCO_GL=osmesa（CPU 软件渲染，无需 GPU）
# 安装：apt-get install libosmesa6-dev && pip install pyopengl
# ════════════════════════════════════════════════════════════════════════
import os, sys, traceback
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt
import matplotlib.gridspec as gridspec
import numpy as _np
# ── 强制 osmesa（必须在 Renderer 创建前设置）────────────────────────────
os.environ["MUJOCO_GL"] = "osmesa"
_render_ok = False
_render_err = ""
try:
import mujoco as _mj
# ── 直接导入 SCENE_XML / _HOME_QPOS（不实例化完整 MuJoCoRobot）────
# 轻量初始化：跳过 1000 步热身，仅 300 步即可得到合理姿态渲染图。
from hardware.mujoco_robot import SCENE_XML, _HOME_QPOS
_model = _mj.MjModel.from_xml_string(SCENE_XML)
_data = _mj.MjData(_model)
# 设置 home 关节角
_nq = min(len(_HOME_QPOS), _model.nq)
_data.qpos[:_nq] = _HOME_QPOS[:_nq]
_data.ctrl[:_model.nu] = _data.qpos[:_model.nu]
_mj.mj_forward(_model, _data)
# 轻量热身（300 步，让 weld 约束收敛到 home 姿态）
print("[渲染] 仿真热身中（300 步）...", end="", flush=True)
for _i in range(300):
_data.ctrl[:_model.nu] = _data.qpos[:_model.nu]
_mj.mj_step(_model, _data)
print(" 完成")
# EE 移到 slot_02（剪刀）approach 点，让臂呈现更典型的抓取前姿态
_target = _np.array([0.200, -0.080, 0.331]) # m
_start = _data.mocap_pos[0].copy()
_steps = 200
print("[渲染] EE 移动到 slot_02 approach 点...", end="", flush=True)
for _k in range(_steps):
_t = (_k + 1) / _steps
_data.mocap_pos[0] = _start + _t * (_target - _start)
_data.ctrl[:_model.nu] = _data.qpos[:_model.nu]
_mj.mj_step(_model, _data)
print(" 完成")
# ── 三相机渲染 ────────────────────────────────────────────────────────
_cameras = [
("front_view", 640, 480, "机械臂全貌（右前斜视）"),
("overhead", 640, 480, "俯视——托盘 + 5 器械"),
("side_obs", 640, 480, "侧视——EE 与器械关系"),
]
_imgs = []
_renderer = None
for _cam, _w, _h, _label in _cameras:
if _renderer is None or _renderer.width != _w or _renderer.height != _h:
if _renderer is not None:
_renderer.close()
_renderer = _mj.Renderer(_model, height=_h, width=_w)
_renderer.update_scene(_data, camera=_cam)
_imgs.append((_label, _renderer.render().copy()))
if _renderer is not None:
_renderer.close()
# ── 显示 ──────────────────────────────────────────────────────────────
_fig = plt.figure(figsize=(17, 5.5))
_gs = gridspec.GridSpec(1, 3, wspace=0.04)
for _idx, (_label, _img) in enumerate(_imgs):
_ax = _fig.add_subplot(_gs[_idx])
_ax.imshow(_img)
_ax.set_title(_label, fontsize=11, pad=6)
_ax.axis("off")
_ee = _data.site_xpos[_model.site("ee_site").id]
_fig.suptitle(
f"MuJoCo {_mj.__version__} 渲染 | "
f"UR5 近似臂 | "
f"EE = ({_ee[0]*1e3:.1f}, {_ee[1]*1e3:.1f}, {_ee[2]*1e3:.1f}) mm | "
f"MUJOCO_GL = {os.environ.get("MUJOCO_GL", "?")}",
fontsize=12, y=1.01
)
plt.tight_layout()
plt.show()
print("
✅ 渲染完成 — front_view 可见 6-DOF 机械臂全貌")
_render_ok = True
except ModuleNotFoundError as _e:
_render_err = f"缺少模块：{_e}"
print(f"
⚠️ {_render_err}")
print(" Colab 请先运行 cell-install 安装 mujoco 和 pyopengl")
except Exception as _e:
_render_err = str(_e)
print(f"
❌ 渲染异常：{_e}")
traceback.print_exc()
# ── 占位符（仅在渲染失败时显示）────────────────────────────────────────
if not _render_ok:
_ph_fig, _ph_axes = plt.subplots(1, 3, figsize=(17, 5.5), facecolor="#0d1117")
_ph_data = [
("front_view — 机械臂全貌（示意）",
"6-DOF UR5 近似臂
"
" J1 腰转 / J2 肩 / J3 肘
"
" J4/J5/J6 腕部 + 夹爪
"
" weld 约束 → 自动 IK
"
" mocap 红球 = EE 目标点"),
("overhead — 俯视托盘（示意）",
"[01 持针器] [02 剪刀]
"
"[03 镊子 ] [04 刀柄]
"
"[05 持针器_小 ]
"
" ← 实际坐标来自 instrument_registry.json"),
("side_obs — 侧视（示意）",
"EE (橙色夹爪)
"
" ↓
"
" ┌─────────┐
"
" │ 托 盘 │
"
" └─────────┘
"
f"失败原因：{_render_err or "mujoco 未安装"}"),
]
for _ax, (_title, _txt) in zip(_ph_axes, _ph_data):
_ax.set_facecolor("#111827")
_ax.text(0.5, 0.5, _txt, color="#60a5fa", fontsize=10.5,
ha="center", va="center", transform=_ax.transAxes,
fontfamily="monospace", linespacing=1.6)
_ax.set_title(_title, color="#e5e7eb", fontsize=10.5)
_ax.axis("off")
plt.tight_layout()
plt.show()
print("ℹ️ 渲染失败时显示占位示意图。")
print(" Colab 修复步骤：")
print(" 1. 重新运行 cell-install（确保安装了 libosmesa6-dev + pyopengl）")
print(" 2. 重新运行 cell-init")
print(" 3. 重新运行本 cell")
# ════════════════════════════════════════════════════════════════════════
# Step 5 — MuJoCo 相机渲染
#
# 本 cell 独立检测 mujoco，不依赖前序 cell 的 HAS_MUJOCO 状态。
# 渲染后端：MUJOCO_GL=osmesa（CPU 软件渲染，无需 GPU）
# 安装：apt-get install libosmesa6-dev && pip install pyopengl
# ════════════════════════════════════════════════════════════════════════
import os, sys, traceback
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as plt
import matplotlib.gridspec as gridspec
import numpy as _np
# ── 强制 osmesa（必须在 Renderer 创建前设置）────────────────────────────
os.environ["MUJOCO_GL"] = "osmesa"
_render_ok = False
_render_err = ""
try:
import mujoco as _mj
# ── 直接导入 SCENE_XML / _HOME_QPOS（不实例化完整 MuJoCoRobot）────
# 轻量初始化：跳过 1000 步热身，仅 300 步即可得到合理姿态渲染图。
from hardware.mujoco_robot import SCENE_XML, _HOME_QPOS
_model = _mj.MjModel.from_xml_string(SCENE_XML)
_data = _mj.MjData(_model)
# 设置 home 关节角
_nq = min(len(_HOME_QPOS), _model.nq)
_data.qpos[:_nq] = _HOME_QPOS[:_nq]
_data.ctrl[:_model.nu] = _data.qpos[:_model.nu]
_mj.mj_forward(_model, _data)
# 轻量热身（300 步，让 weld 约束收敛到 home 姿态）
print("[渲染] 仿真热身中（300 步）...", end="", flush=True)
for _i in range(300):
_data.ctrl[:_model.nu] = _data.qpos[:_model.nu]
_mj.mj_step(_model, _data)
print(" 完成")
# EE 移到 slot_02（剪刀）approach 点，让臂呈现更典型的抓取前姿态
_target = _np.array([0.200, -0.080, 0.331]) # m
_start = _data.mocap_pos[0].copy()
_steps = 200
print("[渲染] EE 移动到 slot_02 approach 点...", end="", flush=True)
for _k in range(_steps):
_t = (_k + 1) / _steps
_data.mocap_pos[0] = _start + _t * (_target - _start)
_data.ctrl[:_model.nu] = _data.qpos[:_model.nu]
_mj.mj_step(_model, _data)
print(" 完成")
# ── 三相机渲染 ────────────────────────────────────────────────────────
_cameras = [
("front_view", 640, 480, "机械臂全貌（右前斜视）"),
("overhead", 640, 480, "俯视——托盘 + 5 器械"),
("side_obs", 640, 480, "侧视——EE 与器械关系"),
]
_imgs = []
_renderer = None
for _cam, _w, _h, _label in _cameras:
if _renderer is None or _renderer.width != _w or _renderer.height != _h:
if _renderer is not None:
_renderer.close()
_renderer = _mj.Renderer(_model, height=_h, width=_w)
_renderer.update_scene(_data, camera=_cam)
_imgs.append((_label, _renderer.render().copy()))
if _renderer is not None:
_renderer.close()
# ── 显示 ──────────────────────────────────────────────────────────────
_fig = plt.figure(figsize=(17, 5.5))
_gs = gridspec.GridSpec(1, 3, wspace=0.04)
for _idx, (_label, _img) in enumerate(_imgs):
_ax = _fig.add_subplot(_gs[_idx])
_ax.imshow(_img)
_ax.set_title(_label, fontsize=11, pad=6)
_ax.axis("off")
_ee = _data.site_xpos[_model.site("ee_site").id]
_fig.suptitle(
f"MuJoCo {_mj.__version__} 渲染 | "
f"UR5 近似臂 | "
f"EE = ({_ee[0]*1e3:.1f}, {_ee[1]*1e3:.1f}, {_ee[2]*1e3:.1f}) mm | "
f"MUJOCO_GL = {os.environ.get("MUJOCO_GL", "?")}",
fontsize=12, y=1.01
)
plt.tight_layout()
plt.show()
print("
✅ 渲染完成 — front_view 可见 6-DOF 机械臂全貌")
_render_ok = True
except ModuleNotFoundError as _e:
_render_err = f"缺少模块：{_e}"
print(f"
⚠️ {_render_err}")
print(" Colab 请先运行 cell-install 安装 mujoco 和 pyopengl")
except Exception as _e:
_render_err = str(_e)
print(f"
❌ 渲染异常：{_e}")
traceback.print_exc()
# ── 占位符（仅在渲染失败时显示）────────────────────────────────────────
if not _render_ok:
_ph_fig, _ph_axes = plt.subplots(1, 3, figsize=(17, 5.5), facecolor="#0d1117")
_ph_data = [
("front_view — 机械臂全貌（示意）",
"6-DOF UR5 近似臂
"
" J1 腰转 / J2 肩 / J3 肘
"
" J4/J5/J6 腕部 + 夹爪
"
" weld 约束 → 自动 IK
"
" mocap 红球 = EE 目标点"),
("overhead — 俯视托盘（示意）",
"[01 持针器] [02 剪刀]
"
"[03 镊子 ] [04 刀柄]
"
"[05 持针器_小 ]
"
" ← 实际坐标来自 instrument_registry.json"),
("side_obs — 侧视（示意）",
"EE (橙色夹爪)
"
" ↓
"
" ┌─────────┐
"
" │ 托 盘 │
"
" └─────────┘
"
f"失败原因：{_render_err or "mujoco 未安装"}"),
]
for _ax, (_title, _txt) in zip(_ph_axes, _ph_data):
_ax.set_facecolor("#111827")
_ax.text(0.5, 0.5, _txt, color="#60a5fa", fontsize=10.5,
ha="center", va="center", transform=_ax.transAxes,
fontfamily="monospace", linespacing=1.6)
_ax.set_title(_title, color="#e5e7eb", fontsize=10.5)
_ax.axis("off")
plt.tight_layout()
plt.show()
print("ℹ️ 渲染失败时显示占位示意图。")
print(" Colab 修复步骤：")
print(" 1. 重新运行 cell-install（确保安装了 libosmesa6-dev + pyopengl）")
print(" 2. 重新运行 cell-init")
print(" 3. 重新运行本 cell")
Step 5b — 仿真轨迹视频¶
录制完整「抓取 → 递送 → 回原位」流程的 MuJoCo 渲染 GIF。
| 参数 | 值 |
|---|---|
| 录制相机 | front_view（右前斜视）并排 side_obs（侧视） |
| 轨迹 | home → approach → grasp → lift → deliver → return |
| 输出格式 | GIF（imageio 合成，Colab 内嵌显示） |
| 后备模式 | mujoco 不可用时生成 Matplotlib 3D 轨迹动画 |
In [ ]:
Copied!
# ════════════════════════════════════════════════════════════════════════
# Step 5b — MuJoCo 仿真轨迹视频生成
#
# 输出：左右并排的 GIF（front_view + side_obs），嵌入 notebook 显示。
# 后备：mujoco 不可用时展示 Matplotlib 3D 轨迹交互动画。
# ════════════════════════════════════════════════════════════════════════
import os, sys, traceback
import numpy as _np
from pathlib import Path
# imageio 合成 GIF
try:
import imageio.v3 as _iio
_HAS_IMAGEIO = True
except ImportError:
os.system("pip install -q imageio")
try:
import imageio.v3 as _iio
_HAS_IMAGEIO = True
except Exception:
_HAS_IMAGEIO = False
os.environ["MUJOCO_GL"] = "osmesa" # CPU 软渲染，Colab CPU runtime 可用
# ── 轨迹定义（单位：m） ────────────────────────────────────────────────────────
_TRAJ = [
("home", _np.array([0.280, -0.080, 0.400])), # 初始位置
("approach_02", _np.array([0.200, -0.080, 0.330])), # slot_02 剪刀：接近
("grasp_02", _np.array([0.200, -0.080, 0.200])), # 下降抓取
("lift_02", _np.array([0.200, -0.080, 0.420])), # 提升
("swing", _np.array([0.080, -0.180, 0.460])), # 摆臂过渡
("deliver", _np.array([-0.050, -0.250, 0.360])), # 递送给术者
("retract", _np.array([0.120, -0.160, 0.430])), # 撤回过渡
("home", _np.array([0.280, -0.080, 0.400])), # 归位
]
_video_ok = False
try:
import mujoco as _mj
from hardware.mujoco_robot import SCENE_XML, _HOME_QPOS
# ── 初始化 ─────────────────────────────────────────────────────────────
_model = _mj.MjModel.from_xml_string(SCENE_XML)
_data = _mj.MjData(_model)
_nq = min(len(_HOME_QPOS), _model.nq)
_data.qpos[:_nq] = _HOME_QPOS[:_nq]
_data.ctrl[:_model.nu] = _HOME_QPOS[:_model.nu]
_mj.mj_forward(_model, _data)
print("[视频] 场景热身中（300步）...", end="", flush=True)
for _ in range(300):
_data.ctrl[:_model.nu] = _data.qpos[:_model.nu]
_mj.mj_step(_model, _data)
print(" OK")
# ── 录制参数 ────────────────────────────────────────────────────────────
_W, _H = 640, 360 # 单个相机分辨率（16:9）
_FPS = 24
_SEG_STEPS = 80 # 每段轨迹仿真步数
_EVERY_N = 4 # 每 N 步渲染一帧
_frames: list = []
_rl = _mj.Renderer(_model, height=_H, width=_W)
_rr = _mj.Renderer(_model, height=_H, width=_W)
print(f"[视频] 录制 {len(_TRAJ)-1} 段轨迹 ...")
for _si in range(len(_TRAJ) - 1):
_ls, _ps = _TRAJ[_si]
_le, _pe = _TRAJ[_si + 1]
print(f" [{_si+1}/{len(_TRAJ)-1}] {_ls} → {_le}", flush=True)
for _k in range(_SEG_STEPS):
_t = (_k + 1) / _SEG_STEPS
_data.mocap_pos[0] = _ps + _t * (_pe - _ps)
_data.ctrl[:_model.nu] = _data.qpos[:_model.nu]
_mj.mj_step(_model, _data)
if _k % _EVERY_N == 0:
_rl.update_scene(_data, camera="front_view")
_rr.update_scene(_data, camera="side_obs")
_fl = _rl.render().copy()
_fr = _rr.render().copy()
# 左右并排拼帧，顶部加深色标题条
_bar = _np.full((26, _W * 2, 3), [10, 18, 38], dtype=_np.uint8)
_frame = _np.concatenate([_bar,
_np.concatenate([_fl, _fr], axis=1)], axis=0)
_frames.append(_frame)
_rl.close(); _rr.close()
print(f"[视频] 共 {len(_frames)} 帧")
# ── 保存 GIF ───────────────────────────────────────────────────────────
_gif = Path("mujoco_sim.gif")
if _HAS_IMAGEIO and _frames:
_iio.imwrite(str(_gif), _frames, fps=_FPS, loop=0)
print(f"[视频] ✅ 保存完成：{_gif} ({_gif.stat().st_size/1024:.0f} KB)")
else:
print("⚠️ imageio 未安装，跳过 GIF 保存")
# ── 内嵌显示 ───────────────────────────────────────────────────────────
from IPython.display import Image as _Img, display as _disp
if _gif.exists():
_disp(_Img(filename=str(_gif), width=900))
else:
import matplotlib.pyplot as _plt
_plt.figure(figsize=(9, 3))
_plt.imshow(_frames[len(_frames) // 2])
_plt.axis("off"); _plt.title("仿真截图（中间帧）"); _plt.show()
_video_ok = True
except Exception as _ve:
traceback.print_exc()
print(f"\n⚠️ MuJoCo 视频失败（{_ve}），切换至 Matplotlib 后备动画")
# ── Matplotlib 3D 轨迹动画（后备，无需 mujoco）────────────────────────────────
if not _video_ok:
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as _plt
import matplotlib.animation as _anim
from mpl_toolkits.mplot3d import Axes3D # noqa: F401
_pts = _np.array([p * 1000 for _, p in _TRAJ]) # m → mm
# 样条插值（线性），每段 30 点
_smooth = _np.concatenate([
_pts[i] + _np.outer(_np.linspace(0, 1, 30), _pts[i+1] - _pts[i])
for i in range(len(_pts) - 1)
])
_fig = _plt.figure(figsize=(10, 6), facecolor="#0d1117")
_ax = _fig.add_subplot(111, projection="3d", facecolor="#111827")
for _sp in [_ax.xaxis, _ax.yaxis, _ax.zaxis]:
_sp.pane.fill = False
_ax.set_xlabel("X (mm)", color="#9ca3af")
_ax.set_ylabel("Y (mm)", color="#9ca3af")
_ax.set_zlabel("Z (mm)", color="#9ca3af")
_ax.tick_params(colors="#6b7280")
_ax.set_title("EE 轨迹动画（Matplotlib 后备）", color="#e5e7eb", pad=12)
# 托盘面
_ax.bar3d(40, -148, 168, 480, 136, 10, alpha=0.20, color="#92400e", zsort="min")
# 器械槽（5色）
_slot_c = ["#c0c0c0", "#ffd700", "#22c55e", "#f97316", "#a855f7"]
for _xi, _ci in zip([120, 200, 280, 360, 440], _slot_c):
_ax.scatter([_xi], [-80], [218], c=_ci, s=90, zorder=5)
# 递送区
_ax.scatter([-50], [-250], [350], c="#22d3ee", s=150, marker="*", zorder=5)
# 路径虚线
_ax.plot(_smooth[:, 0], _smooth[:, 1], _smooth[:, 2],
"--", color="#3b82f6", lw=1, alpha=0.35)
# 动点 + 轨迹
_dot, = _ax.plot([], [], [], "o", color="#f97316", ms=11, zorder=6)
_line, = _ax.plot([], [], [], "-", color="#f97316", lw=2.5, alpha=0.8)
def _upd(fi):
_dot.set_data_3d([ _smooth[fi, 0]], [_smooth[fi, 1]], [_smooth[fi, 2]])
_line.set_data_3d(_smooth[:fi+1, 0], _smooth[:fi+1, 1], _smooth[:fi+1, 2])
return _dot, _line
_ani = _anim.FuncAnimation(
_fig, _upd, frames=len(_smooth), interval=35, blit=True)
from IPython.display import HTML as _HTML, display as _disp
_disp(_HTML(_ani.to_jshtml()))
_plt.close(_fig)
print("ℹ️ Matplotlib 3D 动画（安装 mujoco>=3.1 后可看真实物理渲染视频）")
# ════════════════════════════════════════════════════════════════════════
# Step 5b — MuJoCo 仿真轨迹视频生成
#
# 输出：左右并排的 GIF（front_view + side_obs），嵌入 notebook 显示。
# 后备：mujoco 不可用时展示 Matplotlib 3D 轨迹交互动画。
# ════════════════════════════════════════════════════════════════════════
import os, sys, traceback
import numpy as _np
from pathlib import Path
# imageio 合成 GIF
try:
import imageio.v3 as _iio
_HAS_IMAGEIO = True
except ImportError:
os.system("pip install -q imageio")
try:
import imageio.v3 as _iio
_HAS_IMAGEIO = True
except Exception:
_HAS_IMAGEIO = False
os.environ["MUJOCO_GL"] = "osmesa" # CPU 软渲染，Colab CPU runtime 可用
# ── 轨迹定义（单位：m） ────────────────────────────────────────────────────────
_TRAJ = [
("home", _np.array([0.280, -0.080, 0.400])), # 初始位置
("approach_02", _np.array([0.200, -0.080, 0.330])), # slot_02 剪刀：接近
("grasp_02", _np.array([0.200, -0.080, 0.200])), # 下降抓取
("lift_02", _np.array([0.200, -0.080, 0.420])), # 提升
("swing", _np.array([0.080, -0.180, 0.460])), # 摆臂过渡
("deliver", _np.array([-0.050, -0.250, 0.360])), # 递送给术者
("retract", _np.array([0.120, -0.160, 0.430])), # 撤回过渡
("home", _np.array([0.280, -0.080, 0.400])), # 归位
]
_video_ok = False
try:
import mujoco as _mj
from hardware.mujoco_robot import SCENE_XML, _HOME_QPOS
# ── 初始化 ─────────────────────────────────────────────────────────────
_model = _mj.MjModel.from_xml_string(SCENE_XML)
_data = _mj.MjData(_model)
_nq = min(len(_HOME_QPOS), _model.nq)
_data.qpos[:_nq] = _HOME_QPOS[:_nq]
_data.ctrl[:_model.nu] = _HOME_QPOS[:_model.nu]
_mj.mj_forward(_model, _data)
print("[视频] 场景热身中（300步）...", end="", flush=True)
for _ in range(300):
_data.ctrl[:_model.nu] = _data.qpos[:_model.nu]
_mj.mj_step(_model, _data)
print(" OK")
# ── 录制参数 ────────────────────────────────────────────────────────────
_W, _H = 640, 360 # 单个相机分辨率（16:9）
_FPS = 24
_SEG_STEPS = 80 # 每段轨迹仿真步数
_EVERY_N = 4 # 每 N 步渲染一帧
_frames: list = []
_rl = _mj.Renderer(_model, height=_H, width=_W)
_rr = _mj.Renderer(_model, height=_H, width=_W)
print(f"[视频] 录制 {len(_TRAJ)-1} 段轨迹 ...")
for _si in range(len(_TRAJ) - 1):
_ls, _ps = _TRAJ[_si]
_le, _pe = _TRAJ[_si + 1]
print(f" [{_si+1}/{len(_TRAJ)-1}] {_ls} → {_le}", flush=True)
for _k in range(_SEG_STEPS):
_t = (_k + 1) / _SEG_STEPS
_data.mocap_pos[0] = _ps + _t * (_pe - _ps)
_data.ctrl[:_model.nu] = _data.qpos[:_model.nu]
_mj.mj_step(_model, _data)
if _k % _EVERY_N == 0:
_rl.update_scene(_data, camera="front_view")
_rr.update_scene(_data, camera="side_obs")
_fl = _rl.render().copy()
_fr = _rr.render().copy()
# 左右并排拼帧，顶部加深色标题条
_bar = _np.full((26, _W * 2, 3), [10, 18, 38], dtype=_np.uint8)
_frame = _np.concatenate([_bar,
_np.concatenate([_fl, _fr], axis=1)], axis=0)
_frames.append(_frame)
_rl.close(); _rr.close()
print(f"[视频] 共 {len(_frames)} 帧")
# ── 保存 GIF ───────────────────────────────────────────────────────────
_gif = Path("mujoco_sim.gif")
if _HAS_IMAGEIO and _frames:
_iio.imwrite(str(_gif), _frames, fps=_FPS, loop=0)
print(f"[视频] ✅ 保存完成：{_gif} ({_gif.stat().st_size/1024:.0f} KB)")
else:
print("⚠️ imageio 未安装，跳过 GIF 保存")
# ── 内嵌显示 ───────────────────────────────────────────────────────────
from IPython.display import Image as _Img, display as _disp
if _gif.exists():
_disp(_Img(filename=str(_gif), width=900))
else:
import matplotlib.pyplot as _plt
_plt.figure(figsize=(9, 3))
_plt.imshow(_frames[len(_frames) // 2])
_plt.axis("off"); _plt.title("仿真截图（中间帧）"); _plt.show()
_video_ok = True
except Exception as _ve:
traceback.print_exc()
print(f"\n⚠️ MuJoCo 视频失败（{_ve}），切换至 Matplotlib 后备动画")
# ── Matplotlib 3D 轨迹动画（后备，无需 mujoco）────────────────────────────────
if not _video_ok:
import matplotlib
matplotlib.use("Agg")
import matplotlib.pyplot as _plt
import matplotlib.animation as _anim
from mpl_toolkits.mplot3d import Axes3D # noqa: F401
_pts = _np.array([p * 1000 for _, p in _TRAJ]) # m → mm
# 样条插值（线性），每段 30 点
_smooth = _np.concatenate([
_pts[i] + _np.outer(_np.linspace(0, 1, 30), _pts[i+1] - _pts[i])
for i in range(len(_pts) - 1)
])
_fig = _plt.figure(figsize=(10, 6), facecolor="#0d1117")
_ax = _fig.add_subplot(111, projection="3d", facecolor="#111827")
for _sp in [_ax.xaxis, _ax.yaxis, _ax.zaxis]:
_sp.pane.fill = False
_ax.set_xlabel("X (mm)", color="#9ca3af")
_ax.set_ylabel("Y (mm)", color="#9ca3af")
_ax.set_zlabel("Z (mm)", color="#9ca3af")
_ax.tick_params(colors="#6b7280")
_ax.set_title("EE 轨迹动画（Matplotlib 后备）", color="#e5e7eb", pad=12)
# 托盘面
_ax.bar3d(40, -148, 168, 480, 136, 10, alpha=0.20, color="#92400e", zsort="min")
# 器械槽（5色）
_slot_c = ["#c0c0c0", "#ffd700", "#22c55e", "#f97316", "#a855f7"]
for _xi, _ci in zip([120, 200, 280, 360, 440], _slot_c):
_ax.scatter([_xi], [-80], [218], c=_ci, s=90, zorder=5)
# 递送区
_ax.scatter([-50], [-250], [350], c="#22d3ee", s=150, marker="*", zorder=5)
# 路径虚线
_ax.plot(_smooth[:, 0], _smooth[:, 1], _smooth[:, 2],
"--", color="#3b82f6", lw=1, alpha=0.35)
# 动点 + 轨迹
_dot, = _ax.plot([], [], [], "o", color="#f97316", ms=11, zorder=6)
_line, = _ax.plot([], [], [], "-", color="#f97316", lw=2.5, alpha=0.8)
def _upd(fi):
_dot.set_data_3d([ _smooth[fi, 0]], [_smooth[fi, 1]], [_smooth[fi, 2]])
_line.set_data_3d(_smooth[:fi+1, 0], _smooth[:fi+1, 1], _smooth[:fi+1, 2])
return _dot, _line
_ani = _anim.FuncAnimation(
_fig, _upd, frames=len(_smooth), interval=35, blit=True)
from IPython.display import HTML as _HTML, display as _disp
_disp(_HTML(_ani.to_jshtml()))
_plt.close(_fig)
print("ℹ️ Matplotlib 3D 动画（安装 mujoco>=3.1 后可看真实物理渲染视频）")
Step 6 — 全链路集成验证¶
# MuJoCo 可用 → 物理仿真后端
sm = SurgBotStateMachine(sim=True)
# MuJoCo 不可用 → 纯 Python Mock 后端
sm = SurgBotStateMachine(mock=True)
两种后端对上层业务逻辑完全透明：NLP、安全校验、规划器代码零修改。
语音文本
→ KeywordMatcher 关键词匹配（精确 / alias / 子串）
→ PositionRegistry 槽位注册表查坐标
→ SafetyManager 工作空间边界 + 步长校验
→ RulePlanner 规则动作序列生成
→ DobotArm approach → grasp → lift → deliver → reset
└─ MuJoCoRobot (sim=True) 物理仿真
└─ _MockRobot (mock=True) 纯 Python
In [ ]:
Copied!
from core.state_machine import SurgBotStateMachine
_kwargs = dict(sim=True) if HAS_MUJOCO else dict(mock=True)
_label = 'MuJoCo 物理仿真' if HAS_MUJOCO else 'Mock（纯 Python）'
print(f'后端模式: {_label}\n')
sm = SurgBotStateMachine(**_kwargs)
sm._arm._FORCE_WAIT_TIMEOUT = 2
CMDS = ['递持针器', '给我剪刀', '镊子', '传刀柄', '小持针器']
print(f'{"指令":<14} {"器械":<12} {"槽位":<8} {"耗时":>6} {"状态"}')
print('─' * 60)
results = []
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[:18] if r.error else '?')
print(f'{text:<14} {name:<12} {str(r.slot_id):<8} {r.elapsed_s:>5.1f}s [{status}]')
results.append(r)
sm.shutdown()
ok = sum(1 for r in results if r.success)
print(f'\n成功 / 总计 : {ok} / {len(results)}')
print(f'后端 : {_label}')
from core.state_machine import SurgBotStateMachine
_kwargs = dict(sim=True) if HAS_MUJOCO else dict(mock=True)
_label = 'MuJoCo 物理仿真' if HAS_MUJOCO else 'Mock（纯 Python）'
print(f'后端模式: {_label}\n')
sm = SurgBotStateMachine(**_kwargs)
sm._arm._FORCE_WAIT_TIMEOUT = 2
CMDS = ['递持针器', '给我剪刀', '镊子', '传刀柄', '小持针器']
print(f'{"指令":<14} {"器械":<12} {"槽位":<8} {"耗时":>6} {"状态"}')
print('─' * 60)
results = []
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[:18] if r.error else '?')
print(f'{text:<14} {name:<12} {str(r.slot_id):<8} {r.elapsed_s:>5.1f}s [{status}]')
results.append(r)
sm.shutdown()
ok = sum(1 for r in results if r.success)
print(f'\n成功 / 总计 : {ok} / {len(results)}')
print(f'后端 : {_label}')
总结¶
仿真架构¶
SurgBotStateMachine
│
DobotArm(sim=True)
│
MuJoCoRobot ← 与 _MockRobot 接口完全兼容
│
MJCF 场景（嵌入式字符串，无外部文件）
┌─────────────────────┐
│ mocap EE │ data.mocap_pos = target_m（无需 IK）
│ 5 × 器械圆柱体 │ 静态 cylinder geom
│ overhead / side 相机│ mujoco.Renderer → RGB ndarray
└─────────────────────┘
关键设计决策¶
| 决策 | 选择 | 原因 |
|---|---|---|
| 运动学 | mocap body | 上层全用笛卡尔坐标，无需 IK |
| 场景定义 | 嵌入式 MJCF 字符串 | 无外部文件依赖，CI / Colab 友好 |
| 接触力 | mj_contactForce |
直接获取夹持力，判断夹取成功 |
| 相机渲染 | mujoco.Renderer + EGL |
Colab / 无显示器服务器 headless |
| 降级策略 | NumPy 等效 | Python 3.13+ / CI 自动降级 |
| 后端切换 | sim= / mock= 参数 |
业务逻辑对后端零侵入 |
分步 Mock 演示 → 分步演示（内嵌）