附录

算法原理

优化公式

系统使用 AdaptiveOptimizerAnalytical 优化器，采用 Huber 损失 + 手写解析梯度 + NLopt SLSQP：

\begin{aligned} \min_{q} \quad & L(q) + \lambda \left\lVert q - q_{\mathrm{prev}} \right\rVert^2 \\ \text{s.t.} \quad & q_{\min} \le q \le q_{\max} \end{aligned}

其中 $q_{\mathrm{prev}}$ 为上一帧的关节角， $\lambda$ 是 norm_delta（速度正则化权重）。

自适应混合

算法根据手指捏合状态自动切换优化策略：

L = \sum_i \left[ \alpha_i L_{\mathrm{tip\_dir\_vec}, i} + (1 - \alpha_i) L_{\mathrm{full\_hand}, i} \right]

\alpha_i = \operatorname{clip}\!\left(\frac{d_2 - d_i}{d_2 - d_1},\; 0,\; 0.7\right)

$d_i$ ：拇指到第 $i$ 个手指的指尖距离
$d_1$ 、 $d_2$ ：捏合阈值（默认：2.0 cm、4.0 cm）

TipDirVec 模式：优化指尖位置和方向，适用于精细捏合动作

FullHandVec 模式：优化全手姿态，适用于张开和抓握动作

故障排除

Q: pinocchio 安装报错？

从 PyPI 镜像源安装遇到问题，请使用官方源。注意：PyPI 上的包名为 pin（非 pinocchio）：

pip install pin==3.8.0 -i https://pypi.org/simple

Q: macOS MuJoCo 窗口无法显示？

在 macOS 上运行仿真脚本需使用 mjpython 代替 python：

mjpython teleop_sim.py --play data/avp1.pkl --hand left

Q: 视频模式提示缺少 mediapipe 或 opencv-python？

请安装视频模式依赖：

pip install -e ".[video]"

这将安装视频模式所需的 mediapipe 和 opencv-python 包。

Q: RealSense 模式无法启动或提示 pyrealsense2 缺失？

请先安装 RealSense 额外依赖：

pip install -e ".[realsense]"

同时确认 Intel RealSense 设备已连接，并且没有被其他程序占用。

Q: RealSense 报告设备忙 (device is busy)？

这通常表示相机已被其他程序占用，例如调试工具或其他采集进程。关闭相关程序后重新运行即可。

Q: 开启 --show-video 后卡顿？

--show-video 主要用于调试，会额外显示图像与关键点叠加结果。若更关注实时性能，建议关闭该选项。

自定义输入设备

接入自有手部输入设备（如手套、XR 头显或动捕系统）时，只需把设备数据转换为统一的 21 点手部关键点格式，重定向、仿真和真机管线全部原样复用，无需改动算法。

输入接口

每个输入设备只需实现一个方法：

def get_fingers_data(self) -> dict:
    return {
        "left_fingers":  np.ndarray,  # shape (21, 3), 单位：米
        "right_fingers": np.ndarray,  # shape (21, 3), 单位：米
    }

约定：

某只手不可用时返回全零数组 np.zeros((21, 3))。
21 点顺序须匹配 MediaPipe 手部关键点定义，见下表。
以手腕（0 号点）作为坐标原点。

接口对齐后，teleop_sim.py、teleop_real.py 和 tuning_tool.py 全部直接复用。

集成步骤

创建设备类。 在 example/input_devices/ 下新建 my_device.py，继承 InputDeviceBase，实现 get_fingers_data()。可参考 visionpro.py（实时 TCP）、mediapipe_replay.py（pkl 回放）或 video_mediapipe.py（视频加 MediaPipe）。
注册设备。 在 teleop_sim.py 和 teleop_real.py 的 device_map 中都加上 "my_device": lambda: MyDevice(...)，并把 "my_device" 加进 --input 的候选值。两个文件保持一致。
准备配置。 复制一份现有 YAML（如 config/adaptive_analytical_avp.yaml），按设备调整 mediapipe_rotation、segment_scaling、lp_alpha、norm_delta 和 pinch_thresholds。

调试顺序

按阶段顺序执行，不要跳步：

先录一份 pkl 样本，再接实时流。录好的样本让问题可复现，也能区分是数据问题还是算法问题。
用 tuning_tool.py --play 检查骨架叠加。 确认姿态正确、左右手未接反、三层骨架彼此跟随。

tuning_tool 三层骨架叠加

颜色	含义
橙色	原始输入关键点
青色	经 `segment_scaling` 调整后的目标
白色	机器人 FK 结果（重定向输出）

用 teleop_sim.py --play 运行 MuJoCo 仿真。 确认动作平滑、极端姿态下表现稳定。
最后接实时流和真机，仿真通过之后再连。

MediaPipe 21 点顺序

序号    关节名
 0      手腕（坐标原点）
 1      拇指 CMC       2    拇指 MCP       3    拇指 IP       4    拇指 TIP
 5      食指 MCP       6    食指 PIP       7    食指 DIP      8    食指 TIP
 9      中指 MCP      10    中指 PIP      11    中指 DIP     12    中指 TIP
13      无名指 MCP    14    无名指 PIP    15    无名指 DIP   16    无名指 TIP
17      小指 MCP      18    小指 PIP      19    小指 DIP     20    小指 TIP

MediaPipe 21 关键点定义

如果设备骨架顺序不同，在 _convert() 中重排：

# 设备原生索引，按 MediaPipe 点序排列
DEVICE_TO_MEDIAPIPE = [0, 4, 3, 2, 1, 8, 7, 6, 5, ...]
kp_mediapipe = kp_device[DEVICE_TO_MEDIAPIPE]