摄像头控制O6和G20

.gitignore

@@ -2,6 +2,12 @@
 /build/
 /install/
 /log/
+/test_results/
+
+# Python virtual environments
+.venv/
+venv/
+ENV/
 
 # Python bytecode and caches
 __pycache__/
@@ -11,15 +17,45 @@ __pycache__/
 # Test and tooling caches
 .pytest_cache/
 .coverage
+.coverage.*
+coverage.xml
 htmlcov/
 .mypy_cache/
 .ruff_cache/
+.tox/
+.nox/
+.cache/
+
+# Python packaging outputs
+*.egg-info/
+.eggs/
+dist/
+wheelhouse/
+
+# IDE and editor-local files
+.vscode/
+.idea/
+*.swp
+*.swo
+*~
+
+# Operating-system metadata
+.DS_Store
+Thumbs.db
+
+# Local environment variables and secrets
+.env
+.env.*
+!.env.example
 
 # Runtime and calibration scratch files
 /logs/
 *.tmp
 *.log
 
-# ROS bag / MCAP recordings
+# ROS bag / MCAP recordings and CAN captures
+rosbag2_*/
+*.db3
 *.mcap
+candump-*
 l10_*_state_*/

src/camera_retarget_l20/QUICK_START_O6_G20_ZH.md

@@ -0,0 +1,160 @@
+# O6 + G20 摄像头手势控制：普通用户使用说明
+
+## 这套设备能做什么
+
+把你的左手放在摄像头前，系统会识别手指动作，并让 O6 和 G20 两只机械手
+同时模仿你的动作。
+
+## 使用前注意
+
+- 只使用左手操作。
+- 确保两只机械手周围没有人、线缆、工具或易碎物品。
+- 不要把手指伸进机械手内部。
+- 操作时尽量让掌心朝向摄像头，并让整只手完整出现在画面里。
+- 第一次使用时慢慢活动手指，不要突然快速握拳。
+
+## 第一步：启动程序
+
+打开终端，输入：
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source .venv/bin/activate
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+ros2 launch camera_retarget_l20 teleop_o6_g20_left.launch.py \
+  motor_speed:=250 \
+  ema_alpha:=0.55 \
+  max_step:=24.0 \
+  command_deadband:=3.0
+```
+
+程序启动后会出现摄像头窗口。刚启动时，机械手不会跟随你的手，这是正常的。
+
+## 第二步：完成三个姿势标定
+
+先用鼠标点击一下摄像头窗口，然后依次完成下面三个动作。
+
+每次按键后，都要保持姿势不动约 2 秒。看到进度完成或终端显示
+`calibration complete` 后，再做下一个动作。
+
+### 1. 按 C：完全张开
+
+- 左手五指完全伸直。
+- 手指自然分开。
+- 大拇指向外展开，不要贴着食指。
+- 保持这个姿势，按一次 `C`。
+
+### 2. 按 F：正常握拳
+
+- 像平时一样完整握拳。
+- 四根手指完全弯曲。
+- 大拇指也自然收拢，不要留在外面。
+- 保持这个姿势，按一次 `F`。
+
+### 3. 按 P：做 OK 手势
+
+- 大拇指指尖和食指指尖真正碰在一起。
+- 中指、无名指和小指尽量张开，避免遮挡。
+- 保持这个姿势，按一次 `P`。
+
+正确顺序是：
+
+```text
+C（张开） → F（握拳） → P（OK 手势）
+```
+
+如果某一步提示失败，重新摆好对应姿势，再按一次相同按键即可。
+
+## 第三步：开始控制机械手
+
+完成 C、F、P 后：
+
+1. 把左手稳定放在摄像头画面中。
+2. 按一次 `E`。
+3. 看到画面显示 `REAL CONTROL` 后，机械手开始跟随你的动作。
+
+请先缓慢尝试：
+
+1. 完全张开；
+2. 缓慢握拳；
+3. 拇指与食指捏合；
+4. 拇指与其他手指靠近。
+
+O6 和 G20 的机械结构不同，所以动作效果不会完全一样，这是正常现象。
+
+## 如何暂停
+
+按 `D`，机械手会停止跟随并保持当前位置。
+
+暂停后再次控制，需要保证左手在画面中，然后按 `E`。
+
+## 如何重新标定
+
+控制过程中也可以重新按 `C`、`F` 或 `P`。
+
+系统会自动暂停机械手跟随，然后重新采集姿势。标定完成后，需要再次按 `E`
+才能恢复控制。
+
+如果整体动作明显不准确，建议完整重做：
+
+```text
+C → F → P → E
+```
+
+## 如何安全结束
+
+1. 先把你的左手完全张开。
+2. 等待两只机械手也完全张开。
+3. 按 `D` 停止跟随。
+4. 在启动程序的终端中按 `Ctrl+C`。
+
+不要在机械手正在握紧或夹住物体时直接关闭电源。
+
+## 常用按键
+
+| 按键 | 功能 |
+|---|---|
+| `C` | 标定完全张开的手 |
+| `F` | 标定正常握拳 |
+| `P` | 标定大拇指与食指接触的 OK 手势 |
+| `E` | 开始控制机械手 |
+| `D` | 暂停控制，保持当前位置 |
+| `Q` / `Esc` | 关闭摄像头控制程序 |
+
+## 常见问题
+
+### 按键没有反应
+
+用鼠标点击一下摄像头窗口，再按键。不要在终端窗口中直接按 C、F、P、E。
+
+### 画面找不到手
+
+- 确认使用的是左手；
+- 把整只手放进画面；
+- 掌心尽量朝向摄像头；
+- 改善光线，避免画面过暗；
+- 不要让手指被衣服、桌面或其他物体遮挡。
+
+### 按 E 后机械手不动
+
+通常是 C、F、P 中有一步没有成功。观察终端提示，重新按顺序完成：
+
+```text
+C → F → P → E
+```
+
+### P 标定失败
+
+让大拇指和食指的指尖真正接触，其他三根手指张开，保持手腕和手指稳定约
+2 秒。
+
+### 机械手突然张开并停止跟随
+
+系统可能暂时没有识别到左手，或者某只机械手连接异常。这是安全保护。把左手
+重新稳定地放回画面，确认设备正常后，再按 `E`。
+
+### 动作不够准确
+
+先重新完成 `C → F → P → E`。标定时姿势越标准、越稳定，控制效果越好。

src/camera_retarget_l20/README.md

@@ -0,0 +1,212 @@
+# Left O6/G20/L20 camera retargeting
+
+Chinese guides:
+
+- Quick guide for ordinary users: [`QUICK_START_O6_G20_ZH.md`](QUICK_START_O6_G20_ZH.md)
+- Detailed operator guide: [`USAGE_O6_G20_ZH.md`](USAGE_O6_G20_ZH.md)
+
+This package bridges a webcam to physical left LinkerHand O6, G20, or L20
+hands. It can drive an O6 and a G20 from the same detected human hand:
+
+`camera -> MediaPipe -> shared calibrated hand features -> O6 6-value + G20 20-value JointState`
+
+G20 and L20 both expose 20 position values, but they do not use the same CAN
+driver or the same left-hand joint directions. Select the matching model all
+the way from URDF mapping to the SDK driver.
+
+Hardware output is disabled by default. G20 control also requires an explicit
+fully-open human-hand calibration before the `E` key can arm real control.
+
+## Physical left O6 and G20 together
+
+The combined launch owns both CAN interfaces and isolates every SDK topic under
+the model name. Stop manually started `linker_hand_sdk` processes first, and
+make sure `can0` and `can1` are both UP at 1 Mbps:
+
+```bash
+ros2 launch camera_retarget_l20 teleop_o6_g20_left.launch.py
+```
+
+The fixed hardware assignment is G20 on `can0` and O6 on `can1`. The relevant
+topics are:
+
+```text
+/g20/cb_left_hand_control_cmd       20 positions
+/g20/cb_left_hand_state             20 positions
+/o6/cb_left_hand_control_cmd         6 positions
+/o6/cb_left_hand_state               6 positions
+/camera_o6_g20_retarget/g20/target_joint_cmd
+/camera_o6_g20_retarget/o6/target_joint_cmd
+/camera_o6_g20_retarget/status
+/camera_o6_g20_retarget/debug
+```
+
+Real output remains disabled after startup. Dual mode uses speed 60,
+`ema_alpha=0.55`, and a 16-command-unit frame limit for normal responsive
+motion. It uses the complete measured G20 open/fist flexion endpoints and the
+full protocol range for G20 finger splay; O6 flexion is also allowed across
+the full 0..255 protocol range. These endpoint changes do not remove the
+per-frame rate limit, calibration gate, or tracking/driver timeout protection.
+Keep the physical hands clear and complete all three poses before pressing
+`E`. Each key starts an asynchronous capture: hold the requested pose for
+1.5 seconds until at least 30 stable frames have been accepted. The service
+response means that capture started; completion/progress is reported on the
+status and debug topics:
+
+1. `C`: fully open hand with the thumb spread outward (zero pose).
+2. `F`: normal fist (finger and thumb flexion range).
+3. `P`: touch the thumb and index fingertips.
+
+Press `D` to stop sending commands while holding the last pose. Equivalent
+service calls are:
+
+```bash
+ros2 service call /camera_o6_g20_retarget/calibrate_open std_srvs/srv/Trigger "{}"
+ros2 service call /camera_o6_g20_retarget/calibrate_fist std_srvs/srv/Trigger "{}"
+ros2 service call /camera_o6_g20_retarget/calibrate_pinch std_srvs/srv/Trigger "{}"
+ros2 service call /camera_o6_g20_retarget/enable std_srvs/srv/SetBool "{data: true}"
+```
+
+Calibration is intentionally in-memory only. C/F/P must be repeated after
+every node restart. A capture waits up to eight seconds, rejects unstable
+windows using robust per-feature statistics, and never replaces the previous
+valid pose when validation fails. Starting any calibration automatically
+disarms real output and holds the current robot pose.
+
+The direct robot mapping is loaded from
+`config/direct_mapping_profiles.yml`. This profile owns command lengths,
+joint names, measured open/fist endpoints, flexion weights, all four thumb
+opposition targets, fixed channels, and filter defaults. Existing ROS mapping
+parameters remain runtime overrides and are logged when they differ from the
+profile. The profile schema is model-oriented so a future L10 can reuse the
+same feature/adapter path after its physical endpoints have been measured.
+
+The optional debug JSON contains raw bends and fingertip distances,
+normalized closure, the confirmed pinch target, blend strength, and raw versus
+filtered O6/G20 commands:
+
+```bash
+ros2 topic echo /camera_o6_g20_retarget/status
+ros2 topic echo /camera_o6_g20_retarget/debug
+```
+
+If either driver state is absent for one second, or camera tracking is lost,
+the node commands the vendor open pose on both outputs and disarms real
+control. O6 maps thumb bend/side-swing and one bend actuator per finger; only
+the G20 has independent four-finger side-swing outputs.
+
+## Dependencies
+
+Use a Python 3.12 virtual environment that can still see ROS 2 Jazzy packages.
+MediaPipe 0.10.21 is pinned because the newer package no longer exposes the
+legacy `solutions.hands` API used by this node. The dex-retargeting project
+requires NumPy 2, so install MediaPipe without dependency resolution:
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+python3 -m venv --system-site-packages .venv
+source .venv/bin/activate
+python -m pip install -e /home/lxp/projects/dex-retargeting
+python -m pip install --no-deps mediapipe==0.10.21
+source /opt/ros/jazzy/setup.bash
+python -m colcon build --symlink-install --packages-select camera_retarget_l20 linker_hand_ros2_sdk
+source install/setup.bash
+```
+
+On hosts where `python3 -m venv` reports that `ensurepip` is unavailable, create
+the environment with `--without-pip`, then use the system pip's `--python`
+option for the two installation commands:
+
+```bash
+python3 -m venv --without-pip --system-site-packages .venv
+python3 -m pip --python .venv/bin/python install -e /home/lxp/projects/dex-retargeting
+python3 -m pip --python .venv/bin/python install --no-deps mediapipe==0.10.21
+```
+
+The logged-in user must have camera access. Add `lxp` to the `video` group once,
+then log out and back in:
+
+```bash
+sudo usermod -aG video lxp
+```
+
+## G20 preview only
+
+```bash
+ros2 launch camera_retarget_l20 camera_g20_left.launch.py
+```
+
+The target command can be inspected without moving hardware:
+
+```bash
+ros2 topic echo /camera_g20_retarget/target_joint_cmd
+ros2 topic echo /camera_g20_retarget/status
+```
+
+## Physical left G20
+
+Stop any manually started `linker_hand_sdk` first; only one driver may own
+`can0`. Keep the hand clear of people and objects. The combined launch starts
+the G20 driver at speed 30 and moves it to the SDK startup pose, but camera
+output remains locked:
+
+```bash
+ros2 launch camera_retarget_l20 teleop_g20_left.launch.py
+```
+
+The default closing range is 95%. It can be reduced for a conservative test:
+
+```bash
+ros2 launch camera_retarget_l20 teleop_g20_left.launch.py max_closure:=0.60
+```
+
+Hold the real left hand fully open and steadily in view until the preview says
+`hand=OK`. Press `C`, keep the pose stable until the 1.5-second capture
+completes, then press `E` to enable control. The equivalent service calls are:
+
+```bash
+ros2 service call /camera_g20_retarget/calibrate_open std_srvs/srv/Trigger "{}"
+ros2 service call /camera_g20_retarget/enable std_srvs/srv/SetBool "{data: true}"
+```
+
+Disable immediately with `D` or:
+
+```bash
+ros2 service call /camera_g20_retarget/enable std_srvs/srv/SetBool "{data: false}"
+```
+
+The G20 defaults limit closing travel to 95%, use speed 30 for both driver
+startup and camera motion, limit every frame to 8 command units, and
+open/disable the hand if tracking is lost for 0.35 seconds. Root and tip
+flexion is mapped from human joint angles. P calibrates the contact distance;
+at runtime the nearest of the index/middle/ring/pinky fingertips is selected
+and only that finger receives pinch coupling. Calibrated four-finger
+side-to-side motion maps to G20 channels 6 through 9. Splay confidence
+automatically fades toward the
+calibrated open pose when a curled finger is nearly perpendicular to the palm.
+Thumb CMC flexion is coupled to thumb-tip flexion. In dual C/F/P mode the C
+pose is the maximum outward thumb endpoint and the selected thumb/fingertip
+distance controls opposition. A new target must remain closest for three
+frames before it is selected; a margin and release threshold prevent index,
+middle, ring, and pinky from flickering between targets. Precision pinch is a
+continuous blend toward the configured robot pinch pose. A four-finger power
+grasp gradually suppresses this override from 65% closure and uses the four
+finger F endpoints at 90%. It no longer forces the thumb closed: thumb flexion
+uses the two measured thumb bends, while thumb side-swing follows the C-to-F
+thumb side-angle calibration. Legacy single-G20 mode retains its
+geometry-based side-swing without four-finger thumb assistance.
+G20 flexion no longer uses a common lower command or a special command-90
+thumb clamp. Each of the ten flexion motors is interpolated independently
+between the measured open pose and the measured fist pose. In particular, a
+full dual-mode fist targets 50 on the thumb base, 144 on the thumb tip, and 0
+on the eight index-through-pinky flexion channels. Single-G20 launches retain
+their configurable 95% closure multiplier.
+
+## L20 compatibility
+
+The original L20 entry points remain available:
+
+```bash
+ros2 launch camera_retarget_l20 camera_l20_left.launch.py
+ros2 launch camera_retarget_l20 teleop_l20_left.launch.py
+```

src/camera_retarget_l20/USAGE_O6_G20_ZH.md

@@ -0,0 +1,227 @@
+# 单摄像头控制左手 O6 + G20 使用说明
+
+本文面向第一次使用本系统的操作者。系统通过一个摄像头识别真人左手，
+同时控制两台左手灵巧手：
+
+- G20：连接 `can0`，接收 20 维关节命令。
+- O6：连接 `can1`，接收 6 维关节命令。
+
+摄像头节点会先提取一套统一的人手特征，再分别转换成适合 O6 和 G20 的
+`0–255` 控制值。两台手不会共用同一个命令话题。
+
+## 1. 当前启动命令
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source .venv/bin/activate
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+ros2 launch camera_retarget_l20 teleop_o6_g20_left.launch.py \
+  motor_speed:=250 \
+  ema_alpha:=0.55 \
+  max_step:=24.0 \
+  command_deadband:=3.0
+```
+
+启动成功后，终端应显示 O6、G20 分别连接 `can1`、`can0`，并弹出摄像头
+画面。刚启动时画面显示 `PREVIEW ONLY`，表示只计算预览命令，实体手还不会
+跟随摄像头运动。
+
+> 启动双手 launch 前，不要再单独启动其他 `linker_hand_sdk` 进程，否则同一
+> CAN 设备可能被多个驱动同时访问。
+
+## 2. 正确操作流程
+
+把真人左手完整放进画面，尽量保持掌心朝向摄像头，避免手指互相遮挡。看到
+画面显示 `hand=OK` 后，依次完成 `C → F → P → E`。
+
+### C：完全张开标定
+
+1. 单击一下摄像头窗口，确保键盘输入作用于该窗口。
+2. 真人左手完全张开，五指伸直。
+3. 大拇指自然向外展开，不要贴住食指。
+4. 按一次 `C`，继续保持姿态不动。
+5. 等待画面标定进度完成，或终端出现 `Open calibration complete`。
+
+这一步记录各手指的张开零点、指尖张开距离和侧摆零点。
+
+### F：正常握拳标定
+
+1. 四指完全弯曲，做一个正常握拳动作。
+2. 大拇指也要按照自然握拳方式收拢，不要单独留在外面。
+3. 按一次 `F`，保持姿态不动，直到终端出现
+   `Fist calibration complete`。
+
+这一步记录四指和大拇指的完整弯曲范围。只弯曲四指、大拇指保持伸直，会导致
+后续正常握拳时大拇指闭合不足。
+
+### P：拇指和食指捏合标定
+
+1. 张开中指、无名指和小指，减少遮挡。
+2. 让大拇指指尖与食指指尖真正接触，形成清晰的“OK”手势。
+3. 接触后按一次 `P`，保持稳定，直到终端出现
+   `Pinch calibration complete`。
+
+这一步标定拇指对掌以及指尖接触距离。标定时不是“接近”，而是两个指尖确实
+接触。
+
+每次标定需要稳定保持约 1.5 秒并取得至少 30 帧有效数据，最长等待 8 秒。
+某一步失败时不会覆盖之前的有效结果，重新摆好对应姿态再按一次该键即可。
+
+### E：启用实体手控制
+
+完成 C、F、P 后，把真人左手稳定放在画面中并按 `E`。画面变为红色
+`REAL CONTROL` 后，O6 和 G20 才会开始跟随真人手运动。
+
+启用需要同时满足：
+
+- C、F、P 三项标定均已完成；
+- 摄像头稳定识别到左手；
+- O6 和 G20 两个驱动均在线；
+- 两个命令话题都有订阅者，状态数据均正常。
+
+## 3. 运行期间的按键
+
+| 按键 | 功能 |
+|---|---|
+| `C` | 重新标定完全张开姿态 |
+| `F` | 重新标定正常握拳姿态 |
+| `P` | 重新标定拇指与食指捏合姿态 |
+| `E` | 启用 O6 + G20 实体控制 |
+| `D` | 关闭实体控制，保持最后一个命令位置 |
+| `Q` / `Esc` | 退出摄像头节点 |
+
+开始任何一次重新标定都会自动退出实体控制。标定完成后需要重新按 `E`。
+
+## 4. 安全操作
+
+- 首次按 `E` 前，确保两台灵巧手周围没有人体、线缆或易损物品。
+- 调试时先慢速移动真人手，不要突然快速握拳。
+- 摄像头跟踪丢失超过约 0.35 秒时，系统会向在线设备发送张开姿态并自动解除
+  实体控制。
+- 任一路驱动状态异常或超时时，系统会发送张开姿态并统一解除控制。
+- 需要立即停止跟随时按 `D`。
+- 正常结束程序时，先让真人手张开，确认两台实体手已经张开，再按 `D`，最后
+  在启动终端按 `Ctrl+C`。
+- 每次重启摄像头节点后，C、F、P 标定都需要重新执行，不会加载上次标定。
+
+## 5. 没有摄像头窗口时的终端操作
+
+另开一个终端并加载环境：
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source .venv/bin/activate
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+```
+
+然后可以通过服务完成同样的操作：
+
+```bash
+# C：完全张开
+ros2 service call /camera_o6_g20_retarget/calibrate_open \
+  std_srvs/srv/Trigger "{}"
+
+# F：正常握拳
+ros2 service call /camera_o6_g20_retarget/calibrate_fist \
+  std_srvs/srv/Trigger "{}"
+
+# P：拇指与食指指尖接触
+ros2 service call /camera_o6_g20_retarget/calibrate_pinch \
+  std_srvs/srv/Trigger "{}"
+
+# E：启用实体控制
+ros2 service call /camera_o6_g20_retarget/enable \
+  std_srvs/srv/SetBool "{data: true}"
+
+# D：关闭实体控制
+ros2 service call /camera_o6_g20_retarget/enable \
+  std_srvs/srv/SetBool "{data: false}"
+```
+
+服务返回成功只表示“标定采集已经开始”，仍需继续保持姿态，直到状态话题或
+启动终端报告标定完成。
+
+## 6. 状态、预览和控制话题
+
+```text
+/o6/cb_left_hand_control_cmd                         O6 六维实体命令
+/o6/cb_left_hand_state                              O6 状态
+/g20/cb_left_hand_control_cmd                       G20 二十维实体命令
+/g20/cb_left_hand_state                             G20 状态
+/camera_o6_g20_retarget/o6/target_joint_cmd         O6 命令预览
+/camera_o6_g20_retarget/g20/target_joint_cmd        G20 命令预览
+/camera_o6_g20_retarget/status                      系统状态
+/camera_o6_g20_retarget/debug                       映射调试数据
+```
+
+常用检查命令：
+
+```bash
+ros2 topic echo /camera_o6_g20_retarget/status
+ros2 topic echo /camera_o6_g20_retarget/debug
+ros2 topic echo /camera_o6_g20_retarget/o6/target_joint_cmd
+ros2 topic echo /camera_o6_g20_retarget/g20/target_joint_cmd
+```
+
+`target_joint_cmd` 在 `PREVIEW ONLY` 状态下也会发布，因此可以先观察映射值，
+确认合理后再按 `E`。
+
+## 7. 当前启动参数的含义
+
+| 参数 | 当前值 | 作用 |
+|---|---:|---|
+| `motor_speed` | 250 | 灵巧手内部电机速度，数值越大，电机允许的运动速度越高 |
+| `ema_alpha` | 0.55 | 映射平滑系数；增大响应更快，减小更平滑 |
+| `max_step` | 24 | 每一帧单个命令允许变化的最大值 |
+| `command_deadband` | 3 | 小于该范围的变化不发送，用于抑制轻微抖动 |
+
+这些参数只影响响应速度和平滑程度，不改变 C/F/P 标定数据和型号端点配置。
+
+## 8. 常见问题
+
+### 按键没有反应
+
+先用鼠标单击摄像头窗口，再按键。键盘焦点在终端或其他窗口时，摄像头程序
+收不到按键。也可以直接使用第 5 节的 ROS 服务。
+
+### P 标定一直失败
+
+- 确认已经先完成 C 和 F；
+- 大拇指与食指指尖必须真正接触；
+- 掌心尽量正对摄像头，避免指尖被手掌遮挡；
+- 中指、无名指和小指保持张开；
+- 按 P 后保持 1.5 秒以上，不要移动手腕。
+
+### 按 E 后实体手不动
+
+查看启动终端的警告以及状态话题：
+
+```bash
+ros2 topic echo /camera_o6_g20_retarget/status
+```
+
+重点检查是否仍提示 `needs_open_calibration`、`needs_fist_calibration`、
+`needs_pinch_calibration`、`state_missing` 或 `command_subscriber_missing`。
+
+### O6 和 G20 动作存在差异
+
+这是正常现象。O6 只有 6 路命令，没有四指独立侧摆；G20 使用 20 维命令，
+可以映射更多弯曲和侧摆自由度。因此两台手表现应当语义一致，但不会做到每个
+机械关节完全相同。
+
+### 动作仍有轻微抖动
+
+可以先保持当前电机速度，只增加平滑或死区，例如：
+
+```bash
+ros2 launch camera_retarget_l20 teleop_o6_g20_left.launch.py \
+  motor_speed:=250 \
+  ema_alpha:=0.45 \
+  max_step:=20.0 \
+  command_deadband:=4.0
+```
+
+这会更稳定，但会增加少量跟随延迟。

src/camera_retarget_l20/camera_retarget_l20/__init__.py

@@ -0,0 +1,2 @@
+"""Camera-based retargeting bridge for the LinkerHand L20."""
+

src/camera_retarget_l20/camera_retarget_l20/calibration.py

@@ -0,0 +1,236 @@
+"""Robust multi-frame capture for C/F/P camera calibration."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+
+@dataclass(frozen=True)
+class RawHandObservation:
+    """Uncalibrated, model-independent measurements from one camera frame."""
+
+    bends: np.ndarray
+    pinch_distances: np.ndarray
+    splay: np.ndarray
+    splay_confidence: np.ndarray
+    thumb_splay: float
+    thumb_splay_confidence: float
+
+    def __post_init__(self) -> None:
+        bends = np.asarray(self.bends, dtype=float)
+        pinch_distances = np.asarray(self.pinch_distances, dtype=float)
+        splay = np.asarray(self.splay, dtype=float)
+        splay_confidence = np.asarray(self.splay_confidence, dtype=float)
+        if bends.shape != (10,):
+            raise ValueError("bends must contain ten values")
+        if pinch_distances.shape != (4,):
+            raise ValueError("pinch_distances must contain four values")
+        if splay.shape != (4,) or splay_confidence.shape != (4,):
+            raise ValueError("splay arrays must contain four values")
+        values = np.concatenate(
+            [
+                bends,
+                pinch_distances,
+                splay,
+                splay_confidence,
+                np.asarray(
+                    [self.thumb_splay, self.thumb_splay_confidence], dtype=float
+                ),
+            ]
+        )
+        if not np.all(np.isfinite(values)):
+            raise ValueError("raw hand observation contains non-finite values")
+        object.__setattr__(self, "bends", bends)
+        object.__setattr__(self, "pinch_distances", pinch_distances)
+        object.__setattr__(self, "splay", splay)
+        object.__setattr__(
+            self, "splay_confidence", np.clip(splay_confidence, 0.0, 1.0)
+        )
+        object.__setattr__(
+            self,
+            "thumb_splay_confidence",
+            float(np.clip(self.thumb_splay_confidence, 0.0, 1.0)),
+        )
+
+
+def _circular_center(values: np.ndarray) -> np.ndarray:
+    reference = values[0]
+    wrapped_delta = np.arctan2(
+        np.sin(values - reference), np.cos(values - reference)
+    )
+    return np.arctan2(
+        np.sin(reference + np.median(wrapped_delta, axis=0)),
+        np.cos(reference + np.median(wrapped_delta, axis=0)),
+    )
+
+
+def median_observation(samples: list[RawHandObservation]) -> RawHandObservation:
+    """Return a coordinate-wise robust center for a stable sample window."""
+
+    if not samples:
+        raise ValueError("at least one calibration observation is required")
+    bends = np.stack([sample.bends for sample in samples])
+    distances = np.stack([sample.pinch_distances for sample in samples])
+    splay = np.stack([sample.splay for sample in samples])
+    confidence = np.stack([sample.splay_confidence for sample in samples])
+    thumb_splay = np.asarray([sample.thumb_splay for sample in samples])[:, None]
+    thumb_confidence = np.asarray(
+        [sample.thumb_splay_confidence for sample in samples]
+    )
+    return RawHandObservation(
+        bends=np.median(bends, axis=0),
+        pinch_distances=np.median(distances, axis=0),
+        splay=_circular_center(splay),
+        splay_confidence=np.median(confidence, axis=0),
+        thumb_splay=float(_circular_center(thumb_splay)[0]),
+        thumb_splay_confidence=float(np.median(thumb_confidence)),
+    )
+
+
+class PoseCalibrationSession:
+    """Collect a rolling stable window and expose progress without blocking ROS."""
+
+    def __init__(
+        self,
+        pose: str,
+        *,
+        started_at: float,
+        duration: float = 1.5,
+        min_samples: int = 30,
+        timeout: float = 8.0,
+        bend_mad_limit: float = 0.08,
+        distance_mad_limit: float = 0.04,
+        splay_mad_limit: float = 0.10,
+        thumb_splay_mad_limit: float = 0.10,
+    ) -> None:
+        if pose not in {"open", "fist", "pinch"}:
+            raise ValueError("pose must be open, fist, or pinch")
+        if duration <= 0.0 or timeout <= duration:
+            raise ValueError("timeout must be greater than positive duration")
+        if min_samples < 2:
+            raise ValueError("min_samples must be at least two")
+        self.pose = pose
+        self.started_at = float(started_at)
+        self.duration = float(duration)
+        self.min_samples = int(min_samples)
+        self.timeout = float(timeout)
+        self.bend_mad_limit = float(bend_mad_limit)
+        self.distance_mad_limit = float(distance_mad_limit)
+        self.splay_mad_limit = float(splay_mad_limit)
+        self.thumb_splay_mad_limit = float(thumb_splay_mad_limit)
+        self.samples: list[tuple[float, RawHandObservation]] = []
+        self.state = "collecting"
+        self.reason: str | None = None
+        self.result: RawHandObservation | None = None
+        self.last_stability: dict[str, float] = {}
+
+    @property
+    def active(self) -> bool:
+        return self.state == "collecting"
+
+    def progress(self, now: float) -> float:
+        if self.state == "complete":
+            return 1.0
+        if not self.samples:
+            return 0.0
+        elapsed = max(0.0, float(now) - self.samples[0][0])
+        return float(
+            min(elapsed / self.duration, len(self.samples) / self.min_samples, 1.0)
+        )
+
+    def _prune(self, now: float) -> None:
+        cutoff = float(now) - self.duration
+        while len(self.samples) > 1 and self.samples[1][0] <= cutoff:
+            self.samples.pop(0)
+
+    @staticmethod
+    def _mad(values: np.ndarray, center: np.ndarray) -> np.ndarray:
+        median_deviation = np.median(np.abs(values - center), axis=0)
+        robust_half_span = 0.5 * (
+            np.quantile(values, 0.90, axis=0)
+            - np.quantile(values, 0.10, axis=0)
+        )
+        return np.maximum(median_deviation, robust_half_span)
+
+    @staticmethod
+    def _circular_mad(values: np.ndarray, center: np.ndarray) -> np.ndarray:
+        delta = np.arctan2(np.sin(values - center), np.cos(values - center))
+        absolute = np.abs(delta)
+        return np.maximum(
+            np.median(absolute, axis=0),
+            np.quantile(absolute, 0.90, axis=0),
+        )
+
+    def _stable_result(self) -> RawHandObservation | None:
+        observations = [sample for _, sample in self.samples]
+        center = median_observation(observations)
+        bends = np.stack([sample.bends for sample in observations])
+        distances = np.stack([sample.pinch_distances for sample in observations])
+        splay = np.stack([sample.splay for sample in observations])
+        thumb_splay = np.asarray(
+            [sample.thumb_splay for sample in observations], dtype=float
+        )
+        bend_mad = float(np.max(self._mad(bends, center.bends)))
+        distance_mad = float(
+            np.max(self._mad(distances, center.pinch_distances))
+        )
+        splay_mad = float(np.max(self._circular_mad(splay, center.splay)))
+        thumb_splay_mad = float(
+            self._circular_mad(thumb_splay, np.asarray(center.thumb_splay))
+        )
+        self.last_stability = {
+            "bend_mad_rad": bend_mad,
+            "distance_mad_palm": distance_mad,
+            "splay_mad_rad": splay_mad,
+            "thumb_splay_mad_rad": thumb_splay_mad,
+        }
+        if bend_mad > self.bend_mad_limit:
+            self.reason = "bend_not_stable"
+            return None
+        if distance_mad > self.distance_mad_limit:
+            self.reason = "fingertip_distance_not_stable"
+            return None
+        if self.pose == "open" and splay_mad > self.splay_mad_limit:
+            self.reason = "finger_splay_not_stable"
+            return None
+        if self.pose in {"open", "pinch"} and thumb_splay_mad > self.thumb_splay_mad_limit:
+            self.reason = "thumb_splay_not_stable"
+            return None
+        if self.pose == "open" and float(np.median(center.splay_confidence)) < 0.25:
+            self.reason = "finger_splay_confidence_low"
+            return None
+        if self.pose in {"open", "pinch"} and center.thumb_splay_confidence < 0.20:
+            self.reason = "thumb_splay_confidence_low"
+            return None
+        self.reason = None
+        return center
+
+    def add(
+        self, observation: RawHandObservation, now: float
+    ) -> RawHandObservation | None:
+        if not self.active:
+            return self.result
+        self.samples.append((float(now), observation))
+        self._prune(now)
+        window_elapsed = float(now) - self.samples[0][0]
+        if len(self.samples) >= self.min_samples and window_elapsed >= self.duration:
+            result = self._stable_result()
+            if result is not None:
+                self.result = result
+                self.state = "complete"
+                return result
+        self.poll(now)
+        return None
+
+    def poll(self, now: float) -> None:
+        if self.active and float(now) - self.started_at >= self.timeout:
+            last_reason = self.reason or "insufficient_stable_samples"
+            self.state = "failed"
+            self.reason = f"calibration_timeout:{last_reason}"
+
+    def fail(self, reason: str) -> None:
+        self.state = "failed"
+        self.reason = str(reason)
+        self.result = None

src/camera_retarget_l20/camera_retarget_l20/hand_detector.py

@@ -0,0 +1,120 @@
+"""MediaPipe single-hand detector using the coordinate convention of dex-retargeting.
+
+The coordinate conversion follows the MIT-licensed dex-retargeting webcam
+example in /home/lxp/projects/dex-retargeting/example/vector_retargeting.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Any
+
+import cv2
+import mediapipe as mp
+import numpy as np
+
+
+OPERATOR2MANO_LEFT = np.array(
+    [
+        [0.0, 0.0, -1.0],
+        [1.0, 0.0, 0.0],
+        [0.0, -1.0, 0.0],
+    ]
+)
+
+
+@dataclass(frozen=True)
+class Detection:
+    joint_positions: np.ndarray
+    image_landmarks: Any
+    detected_label: str
+
+
+class MediaPipeLeftHandDetector:
+    """Track one physical left hand and return wrist-aligned 3-D landmarks."""
+
+    def __init__(
+        self,
+        *,
+        selfie: bool = False,
+        strict_handedness: bool = True,
+        min_detection_confidence: float = 0.70,
+        min_tracking_confidence: float = 0.70,
+    ) -> None:
+        self.selfie = bool(selfie)
+        self.strict_handedness = bool(strict_handedness)
+        # MediaPipe Hands assumes a mirrored/selfie image when assigning labels.
+        self.expected_label = "Left" if self.selfie else "Right"
+        self.hands = mp.solutions.hands.Hands(
+            static_image_mode=False,
+            max_num_hands=1,
+            model_complexity=1,
+            min_detection_confidence=float(min_detection_confidence),
+            min_tracking_confidence=float(min_tracking_confidence),
+        )
+
+    def close(self) -> None:
+        self.hands.close()
+
+    def detect(self, bgr_image: np.ndarray) -> Detection | None:
+        image = cv2.flip(bgr_image, 1) if self.selfie else bgr_image
+        rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
+        rgb.flags.writeable = False
+        results = self.hands.process(rgb)
+        if not results.multi_hand_landmarks or not results.multi_hand_world_landmarks:
+            return None
+
+        selected = 0
+        label = results.multi_handedness[selected].classification[0].label
+        if self.strict_handedness and label != self.expected_label:
+            return None
+
+        world_landmarks = results.multi_hand_world_landmarks[selected]
+        points = np.array(
+            [[landmark.x, landmark.y, landmark.z] for landmark in world_landmarks.landmark],
+            dtype=float,
+        )
+        if points.shape != (21, 3):
+            return None
+
+        points -= points[0:1, :]
+        wrist_frame = self.estimate_wrist_frame(points)
+        joint_positions = points @ wrist_frame @ OPERATOR2MANO_LEFT
+        if not np.all(np.isfinite(joint_positions)):
+            return None
+        return Detection(
+            joint_positions=joint_positions,
+            image_landmarks=results.multi_hand_landmarks[selected],
+            detected_label=label,
+        )
+
+    @staticmethod
+    def estimate_wrist_frame(points: np.ndarray) -> np.ndarray:
+        if points.shape != (21, 3):
+            raise ValueError("MediaPipe hand points must have shape (21, 3)")
+        palm_points = points[[0, 5, 9], :]
+        x_vector = palm_points[0] - palm_points[2]
+        centered = palm_points - np.mean(palm_points, axis=0, keepdims=True)
+        _, _, vh = np.linalg.svd(centered)
+        normal = vh[2, :]
+        x_axis = x_vector - np.dot(x_vector, normal) * normal
+        x_norm = np.linalg.norm(x_axis)
+        if x_norm < 1e-8:
+            raise ValueError("degenerate wrist frame")
+        x_axis /= x_norm
+        z_axis = np.cross(x_axis, normal)
+        if np.dot(z_axis, centered[1] - centered[2]) < 0:
+            normal *= -1.0
+            z_axis *= -1.0
+        return np.stack([x_axis, normal, z_axis], axis=1)
+
+    @staticmethod
+    def draw_landmarks(image: np.ndarray, landmarks: Any) -> None:
+        mp.solutions.drawing_utils.draw_landmarks(
+            image,
+            landmarks,
+            mp.solutions.hands.HAND_CONNECTIONS,
+            mp.solutions.drawing_styles.get_default_hand_landmarks_style(),
+            mp.solutions.drawing_styles.get_default_hand_connections_style(),
+        )
+

src/camera_retarget_l20/camera_retarget_l20/hand_features.py

@@ -0,0 +1,68 @@
+"""Model-independent features extracted from one calibrated human hand."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+
+@dataclass(frozen=True)
+class HandFeatures:
+    """Normalized intent shared by robot hands with different joint counts."""
+
+    closure: np.ndarray
+    pinch: float
+    pinch_strengths: np.ndarray
+    splay_delta: np.ndarray
+    splay_confidence: np.ndarray
+    thumb_splay_delta: float
+    thumb_abduction: float
+    thumb_opposition: float
+    power_grasp: float
+
+    def __post_init__(self) -> None:
+        closure = np.asarray(self.closure, dtype=float)
+        pinch_strengths = np.asarray(self.pinch_strengths, dtype=float)
+        splay_delta = np.asarray(self.splay_delta, dtype=float)
+        splay_confidence = np.asarray(self.splay_confidence, dtype=float)
+        if closure.shape != (10,):
+            raise ValueError("closure must contain ten root/tip values")
+        if pinch_strengths.shape != (4,):
+            raise ValueError(
+                "pinch_strengths must contain index/middle/ring/pinky values"
+            )
+        if splay_delta.shape != (4,) or splay_confidence.shape != (4,):
+            raise ValueError("four-finger splay arrays must contain four values")
+        values = np.concatenate(
+            [
+                closure,
+                pinch_strengths,
+                splay_delta,
+                splay_confidence,
+                np.asarray(
+                    [
+                        self.pinch,
+                        self.thumb_splay_delta,
+                        self.thumb_abduction,
+                        self.thumb_opposition,
+                        self.power_grasp,
+                    ],
+                    dtype=float,
+                ),
+            ]
+        )
+        if not np.all(np.isfinite(values)):
+            raise ValueError("hand features contain non-finite values")
+        object.__setattr__(self, "closure", np.clip(closure, 0.0, 1.0))
+        object.__setattr__(
+            self, "pinch_strengths", np.clip(pinch_strengths, 0.0, 1.0)
+        )
+        object.__setattr__(self, "splay_delta", splay_delta)
+        object.__setattr__(self, "splay_confidence", np.clip(splay_confidence, 0.0, 1.0))
+        for name in ("pinch", "thumb_abduction", "thumb_opposition", "power_grasp"):
+            object.__setattr__(
+                self,
+                name,
+                float(np.clip(getattr(self, name), 0.0, 1.0)),
+            )

src/camera_retarget_l20/camera_retarget_l20/l20_mapping.py

@@ -0,0 +1,170 @@
+"""Convert L20 URDF joint positions to the LinkerHand 20-byte command layout."""
+
+from __future__ import annotations
+
+from collections.abc import Sequence
+
+import numpy as np
+
+
+MOTOR_JOINT_NAMES: tuple[str | None, ...] = (
+    "thumb_joint2",
+    "index_joint1",
+    "middle_joint1",
+    "ring_joint1",
+    "little_joint1",
+    "thumb_joint0",
+    "index_joint0",
+    "middle_joint0",
+    "ring_joint0",
+    "little_joint0",
+    "thumb_joint1",
+    None,
+    None,
+    None,
+    None,
+    "thumb_joint3",
+    "index_joint2",
+    "middle_joint2",
+    "ring_joint2",
+    "little_joint2",
+)
+
+FLEXION_MOTOR_INDICES = frozenset((*range(0, 5), *range(15, 20)))
+RESERVED_MOTOR_INDICES = frozenset(range(11, 15))
+
+# This is also the L20 startup/open pose used by linker_hand_ros2_sdk.
+DEFAULT_OPEN_POSE: tuple[int, ...] = (
+    255,
+    255,
+    255,
+    255,
+    255,
+    255,
+    10,
+    100,
+    180,
+    240,
+    245,
+    255,
+    255,
+    255,
+    255,
+    255,
+    255,
+    255,
+    255,
+    255,
+)
+
+
+class L20CommandMapper:
+    """Map named L20 joint radians to safe, filtered motor commands.
+
+    The left L20 uses 16 controllable joints in a 20-value protocol. All of
+    its validated GUI mappings use the same direction: URDF lower limit maps
+    to 255 and upper limit maps to 0. Positions 11..14 are reserved.
+    """
+
+    def __init__(
+        self,
+        joint_names: Sequence[str],
+        joint_limits: Sequence[Sequence[float]],
+        *,
+        ema_alpha: float = 0.35,
+        max_step: float = 8.0,
+        max_closure: float = 0.80,
+        open_pose: Sequence[float] = DEFAULT_OPEN_POSE,
+    ) -> None:
+        if len(joint_names) != len(joint_limits):
+            raise ValueError("joint_names and joint_limits must have equal length")
+        if not 0.0 < ema_alpha <= 1.0:
+            raise ValueError("ema_alpha must be in (0, 1]")
+        if max_step <= 0.0:
+            raise ValueError("max_step must be positive")
+        if not 0.0 <= max_closure <= 1.0:
+            raise ValueError("max_closure must be in [0, 1]")
+        if len(open_pose) != 20:
+            raise ValueError("open_pose must contain 20 values")
+
+        self.joint_names = tuple(joint_names)
+        self.joint_limits = np.asarray(joint_limits, dtype=float)
+        if self.joint_limits.shape != (len(self.joint_names), 2):
+            raise ValueError("joint_limits must have shape (N, 2)")
+
+        name_to_index = {name: index for index, name in enumerate(self.joint_names)}
+        required = {name for name in MOTOR_JOINT_NAMES if name is not None}
+        missing = sorted(required.difference(name_to_index))
+        if missing:
+            raise ValueError(f"L20 URDF is missing controlled joints: {missing}")
+
+        self.motor_to_qpos = tuple(
+            None if name is None else name_to_index[name] for name in MOTOR_JOINT_NAMES
+        )
+        self.ema_alpha = float(ema_alpha)
+        self.max_step = float(max_step)
+        self.max_closure = float(max_closure)
+        self.open_pose = self._validate_command(open_pose)
+        self.last_command = self.open_pose.astype(float)
+
+    @staticmethod
+    def _validate_command(command: Sequence[float]) -> np.ndarray:
+        values = np.asarray(command, dtype=float)
+        if values.shape != (20,):
+            raise ValueError("an L20 command must contain 20 values")
+        if not np.all(np.isfinite(values)):
+            raise ValueError("L20 command contains non-finite values")
+        return np.clip(values, 0.0, 255.0)
+
+    def reset(self, command: Sequence[float] | None = None) -> None:
+        """Reset filtering, normally to the known open pose."""
+
+        self.last_command = (
+            self.open_pose.copy()
+            if command is None
+            else self._validate_command(command).astype(float)
+        )
+
+    def raw_command(self, qpos: Sequence[float]) -> np.ndarray:
+        """Return an unfiltered 20-value command with limits applied."""
+
+        qpos_array = np.asarray(qpos, dtype=float)
+        if qpos_array.shape != (len(self.joint_names),):
+            raise ValueError(
+                f"qpos must have {len(self.joint_names)} values, got {qpos_array.shape}"
+            )
+        if not np.all(np.isfinite(qpos_array)):
+            raise ValueError("qpos contains non-finite values")
+
+        command = np.full(20, 255.0, dtype=float)
+        for motor_index, qpos_index in enumerate(self.motor_to_qpos):
+            if qpos_index is None:
+                continue
+            lower, upper = self.joint_limits[qpos_index]
+            if not np.isfinite(lower) or not np.isfinite(upper) or upper <= lower:
+                raise ValueError(
+                    f"invalid limits for {self.joint_names[qpos_index]}: {lower}, {upper}"
+                )
+            ratio = np.clip((qpos_array[qpos_index] - lower) / (upper - lower), 0.0, 1.0)
+            command[motor_index] = 255.0 * (1.0 - ratio)
+
+        # Limit closing travel during camera teleoperation. Ab/adduction joints
+        # retain their full URDF-constrained range.
+        minimum_flexion_command = 255.0 * (1.0 - self.max_closure)
+        for motor_index in FLEXION_MOTOR_INDICES:
+            command[motor_index] = max(
+                command[motor_index], minimum_flexion_command
+            )
+        command[list(RESERVED_MOTOR_INDICES)] = 255.0
+        return command
+
+    def map(self, qpos: Sequence[float]) -> list[int]:
+        """Map and smooth one retargeting result."""
+
+        raw = self.raw_command(qpos)
+        filtered = self.ema_alpha * raw + (1.0 - self.ema_alpha) * self.last_command
+        delta = np.clip(filtered - self.last_command, -self.max_step, self.max_step)
+        self.last_command = np.clip(self.last_command + delta, 0.0, 255.0)
+        self.last_command[list(RESERVED_MOTOR_INDICES)] = 255.0
+        return np.rint(self.last_command).astype(int).tolist()
+

src/camera_retarget_l20/camera_retarget_l20/model_profiles.py

@@ -0,0 +1,476 @@
+"""Validated, configuration-driven direct command profiles for robot hands."""
+
+from __future__ import annotations
+
+from collections.abc import Mapping, Sequence
+from dataclasses import dataclass, replace
+from pathlib import Path
+
+import numpy as np
+import yaml
+
+from .hand_features import HandFeatures
+
+
+FEATURE_INDICES: dict[str, int] = {
+    "thumb_root": 0,
+    "index_root": 1,
+    "middle_root": 2,
+    "ring_root": 3,
+    "pinky_root": 4,
+    "thumb_tip": 5,
+    "index_tip": 6,
+    "middle_tip": 7,
+    "ring_tip": 8,
+    "pinky_tip": 9,
+}
+PINCH_ROLES: tuple[str, ...] = ("index", "middle", "ring", "pinky")
+PINCH_FINGER_ENGAGEMENT_START = 0.10
+PINCH_FINGER_ENGAGEMENT_END = 0.50
+PINCH_CONTACT_ASSIST_START = 0.75
+PINCH_CONTACT_ASSIST_END = 0.95
+PINCH_CONTACT_ASSIST_TARGETS = frozenset(("index", "middle"))
+
+
+@dataclass(frozen=True)
+class FlexionChannel:
+    """One robot flexion channel driven by weighted normalized features."""
+
+    command_index: int
+    sources: tuple[tuple[int, float], ...]
+    pinch_role: str
+
+
+@dataclass(frozen=True)
+class DirectModelProfile:
+    """All model-specific data needed by the direct MediaPipe adapter."""
+
+    model: str
+    command_names: tuple[str, ...]
+    open_pose: tuple[float, ...]
+    fist_pose: tuple[float, ...]
+    flexion_channels: tuple[FlexionChannel, ...]
+    side_command_indices: tuple[int, ...]
+    pinch_target_side_commands: tuple[tuple[float, ...], ...]
+    power_grasp_side_commands: tuple[float, ...]
+    abducted_side_commands: tuple[float, ...] | None
+    splay_command_indices: tuple[int, ...]
+    splay_scale: float
+    splay_gain: float
+    max_splay_delta: float
+    fixed_channels: tuple[tuple[int, float], ...]
+    pinch_thumb_closure: float
+    pinch_finger_closure: float
+    ema_alpha: float
+    max_step: float
+    command_deadband: float
+    motor_speed: int
+    power_grasp_start: float = 0.65
+    power_grasp_end: float = 0.90
+
+    @property
+    def command_length(self) -> int:
+        return len(self.command_names)
+
+    def with_runtime_overrides(
+        self,
+        *,
+        ema_alpha: float | None = None,
+        max_step: float | None = None,
+        command_deadband: float | None = None,
+        splay_gain: float | None = None,
+        max_splay_delta: float | None = None,
+        pinch_thumb_closure: float | None = None,
+        pinch_finger_closure: float | None = None,
+    ) -> "DirectModelProfile":
+        """Return a validated profile with legacy ROS parameter overrides."""
+
+        updated = replace(
+            self,
+            ema_alpha=self.ema_alpha if ema_alpha is None else float(ema_alpha),
+            max_step=self.max_step if max_step is None else float(max_step),
+            command_deadband=(
+                self.command_deadband
+                if command_deadband is None
+                else float(command_deadband)
+            ),
+            splay_gain=(
+                self.splay_gain if splay_gain is None else float(splay_gain)
+            ),
+            max_splay_delta=(
+                self.max_splay_delta
+                if max_splay_delta is None
+                else float(max_splay_delta)
+            ),
+            pinch_thumb_closure=(
+                self.pinch_thumb_closure
+                if pinch_thumb_closure is None
+                else float(pinch_thumb_closure)
+            ),
+            pinch_finger_closure=(
+                self.pinch_finger_closure
+                if pinch_finger_closure is None
+                else float(pinch_finger_closure)
+            ),
+        )
+        _validate_profile(updated)
+        return updated
+
+
+def _float_tuple(values: Sequence[float], field: str) -> tuple[float, ...]:
+    result = tuple(float(value) for value in values)
+    if not result or not np.all(np.isfinite(result)):
+        raise ValueError(f"{field} must contain finite values")
+    return result
+
+
+def _parse_flexion_channels(values: Sequence[Mapping]) -> tuple[FlexionChannel, ...]:
+    channels: list[FlexionChannel] = []
+    for entry in values:
+        sources_value = entry.get("sources")
+        if not isinstance(sources_value, Mapping) or not sources_value:
+            raise ValueError("each flexion channel requires non-empty sources")
+        sources: list[tuple[int, float]] = []
+        for feature_name, weight in sources_value.items():
+            if feature_name not in FEATURE_INDICES:
+                raise ValueError(f"unknown hand feature: {feature_name}")
+            sources.append((FEATURE_INDICES[str(feature_name)], float(weight)))
+        channels.append(
+            FlexionChannel(
+                command_index=int(entry["command_index"]),
+                sources=tuple(sources),
+                pinch_role=str(entry["pinch_role"]).lower(),
+            )
+        )
+    return tuple(channels)
+
+
+def _profile_from_mapping(model: str, values: Mapping) -> DirectModelProfile:
+    side = values.get("side", {})
+    splay = values.get("splay", {})
+    filters = values.get("filters", {})
+    pinch = values.get("pinch", {})
+    fixed = values.get("fixed_channels", {})
+    target_values = pinch.get("target_side_commands", {})
+    profile = DirectModelProfile(
+        model=model.upper(),
+        command_names=tuple(str(name) for name in values["command_names"]),
+        open_pose=_float_tuple(values["open_pose"], f"{model}.open_pose"),
+        fist_pose=_float_tuple(values["fist_pose"], f"{model}.fist_pose"),
+        flexion_channels=_parse_flexion_channels(values["flexion_channels"]),
+        side_command_indices=tuple(int(index) for index in side.get("indices", [])),
+        pinch_target_side_commands=tuple(
+            _float_tuple(target_values[role], f"{model}.pinch.{role}")
+            for role in PINCH_ROLES
+        ),
+        power_grasp_side_commands=_float_tuple(
+            side.get("power_grasp_commands", []),
+            f"{model}.side.power_grasp_commands",
+        ),
+        abducted_side_commands=(
+            _float_tuple(
+                side["abducted_commands"],
+                f"{model}.side.abducted_commands",
+            )
+            if "abducted_commands" in side
+            else None
+        ),
+        splay_command_indices=tuple(
+            int(index) for index in splay.get("indices", [])
+        ),
+        splay_scale=float(splay.get("scale", 1.0)),
+        splay_gain=float(splay.get("gain", 0.0)),
+        max_splay_delta=float(splay.get("max_delta", 0.0)),
+        fixed_channels=tuple(
+            (int(index), float(command)) for index, command in fixed.items()
+        ),
+        pinch_thumb_closure=float(pinch.get("thumb_closure", 0.85)),
+        pinch_finger_closure=float(pinch.get("finger_closure", 0.65)),
+        ema_alpha=float(filters.get("ema_alpha", 0.55)),
+        max_step=float(filters.get("max_step", 16.0)),
+        command_deadband=float(filters.get("command_deadband", 0.0)),
+        motor_speed=int(values.get("motor_speed", 60)),
+        power_grasp_start=float(pinch.get("power_grasp_start", 0.65)),
+        power_grasp_end=float(pinch.get("power_grasp_end", 0.90)),
+    )
+    _validate_profile(profile)
+    return profile
+
+
+def _validate_profile(profile: DirectModelProfile) -> None:
+    length = profile.command_length
+    if length < 1 or len(profile.open_pose) != length or len(profile.fist_pose) != length:
+        raise ValueError(f"{profile.model} command names/open/fist lengths differ")
+    values = np.asarray(profile.open_pose + profile.fist_pose, dtype=float)
+    if np.any(values < 0.0) or np.any(values > 255.0):
+        raise ValueError(f"{profile.model} endpoints must be in [0, 255]")
+    if not 0.0 < profile.ema_alpha <= 1.0:
+        raise ValueError("ema_alpha must be in (0, 1]")
+    if profile.max_step <= 0.0 or profile.motor_speed <= 0:
+        raise ValueError("max_step and motor_speed must be positive")
+    if not 0.0 <= profile.command_deadband < profile.max_step:
+        raise ValueError("command_deadband must be in [0, max_step)")
+    if not 0.0 <= profile.pinch_thumb_closure <= 1.0:
+        raise ValueError("pinch thumb closure must be in [0, 1]")
+    if not 0.0 <= profile.pinch_finger_closure <= 1.0:
+        raise ValueError("pinch finger closure must be in [0, 1]")
+    if not 0.0 <= profile.power_grasp_start < profile.power_grasp_end <= 1.0:
+        raise ValueError("power grasp thresholds must increase within [0, 1]")
+    if not 0.0 <= profile.max_splay_delta <= 255.0:
+        raise ValueError("max_splay_delta must be in [0, 255]")
+
+    flex_indices = [channel.command_index for channel in profile.flexion_channels]
+    side_indices = list(profile.side_command_indices)
+    splay_indices = list(profile.splay_command_indices)
+    fixed_indices = [index for index, _ in profile.fixed_channels]
+    all_indices = flex_indices + side_indices + splay_indices + fixed_indices
+    if any(index < 0 or index >= length for index in all_indices):
+        raise ValueError(f"{profile.model} contains an out-of-range command index")
+    if len(all_indices) != len(set(all_indices)):
+        raise ValueError(f"{profile.model} command channel roles overlap")
+    if len(profile.pinch_target_side_commands) != len(PINCH_ROLES):
+        raise ValueError("pinch target table must contain four fingers")
+    if any(
+        len(commands) != len(side_indices)
+        for commands in profile.pinch_target_side_commands
+    ):
+        raise ValueError("pinch side command width does not match side indices")
+    if len(profile.power_grasp_side_commands) != len(side_indices):
+        raise ValueError("power grasp side command width does not match side indices")
+    if profile.abducted_side_commands is not None and len(
+        profile.abducted_side_commands
+    ) != len(side_indices):
+        raise ValueError("abducted side command width does not match side indices")
+    if splay_indices and len(splay_indices) != 4:
+        raise ValueError("splay mapping must contain index/middle/ring/pinky channels")
+    for channel in profile.flexion_channels:
+        if channel.pinch_role not in ("thumb", *PINCH_ROLES):
+            raise ValueError(f"invalid pinch role: {channel.pinch_role}")
+        weights = np.asarray([weight for _, weight in channel.sources], dtype=float)
+        if np.any(weights < 0.0) or not np.isclose(np.sum(weights), 1.0):
+            raise ValueError("flexion source weights must be non-negative and sum to 1")
+    fixed_values = np.asarray([value for _, value in profile.fixed_channels])
+    if fixed_values.size and (
+        np.any(fixed_values < 0.0) or np.any(fixed_values > 255.0)
+    ):
+        raise ValueError("fixed channel commands must be in [0, 255]")
+
+
+def load_model_profiles(path: str | Path) -> dict[str, DirectModelProfile]:
+    """Load and validate all model profiles from YAML."""
+
+    profile_path = Path(path).expanduser().resolve()
+    with profile_path.open("r", encoding="utf-8") as stream:
+        document = yaml.safe_load(stream)
+    if not isinstance(document, Mapping) or document.get("schema_version") != 1:
+        raise ValueError("model profile file requires schema_version: 1")
+    models = document.get("models")
+    if not isinstance(models, Mapping) or not models:
+        raise ValueError("model profile file does not define any models")
+    return {
+        str(model).upper(): _profile_from_mapping(str(model), values)
+        for model, values in models.items()
+    }
+
+
+def _smoothstep(value: float, start: float, end: float) -> float:
+    ratio = float(np.clip((value - start) / (end - start), 0.0, 1.0))
+    return ratio * ratio * (3.0 - 2.0 * ratio)
+
+
+class ProfiledCommandMapper:
+    """Map shared hand intent to one profile's unfiltered/filtered command."""
+
+    def __init__(
+        self, profile: DirectModelProfile, *, max_closure: float = 1.0
+    ) -> None:
+        _validate_profile(profile)
+        if not 0.0 <= max_closure <= 1.0:
+            raise ValueError("max_closure must be in [0, 1]")
+        self.profile = profile
+        self.max_closure = float(max_closure)
+        self.open_pose = np.asarray(profile.open_pose, dtype=float)
+        self.fist_pose = np.asarray(profile.fist_pose, dtype=float)
+        self.last_command = self.open_pose.copy()
+        self.last_raw_command = self.open_pose.copy()
+        self.last_pinch_blend = 0.0
+
+    def reset(self, command: Sequence[float] | None = None) -> None:
+        if command is None:
+            values = self.open_pose.copy()
+        else:
+            values = np.asarray(command, dtype=float)
+            if values.shape != self.open_pose.shape or not np.all(np.isfinite(values)):
+                raise ValueError(
+                    f"{self.profile.model} reset command has invalid shape or values"
+                )
+            values = np.clip(values, 0.0, 255.0)
+        self.last_command = values
+        self.last_raw_command = values.copy()
+        self.last_pinch_blend = 0.0
+
+    def raw_command(self, features: HandFeatures) -> np.ndarray:
+        profile = self.profile
+        command = self.open_pose.copy()
+        closure = np.asarray(features.closure, dtype=float)
+        strengths = np.asarray(features.pinch_strengths, dtype=float)
+        selected_target = int(np.argmax(strengths))
+        pinch_strength = float(strengths[selected_target])
+        grasp_blend = _smoothstep(
+            float(features.power_grasp),
+            profile.power_grasp_start,
+            profile.power_grasp_end,
+        )
+        pinch_blend = pinch_strength * (1.0 - grasp_blend)
+        self.last_pinch_blend = float(pinch_blend)
+
+        for channel in profile.flexion_channels:
+            activation = sum(
+                closure[feature_index] * weight
+                for feature_index, weight in channel.sources
+            )
+            activation = float(np.clip(activation * self.max_closure, 0.0, 1.0))
+            index = channel.command_index
+            command[index] = self.open_pose[index] + activation * (
+                self.fist_pose[index] - self.open_pose[index]
+            )
+
+        if profile.splay_command_indices:
+            delta = (
+                profile.splay_gain
+                * np.asarray(features.splay_delta, dtype=float)
+                * profile.splay_scale
+            )
+            delta = np.clip(delta, -profile.max_splay_delta, profile.max_splay_delta)
+            indices = list(profile.splay_command_indices)
+            command[indices] = self.open_pose[indices] + delta
+
+        side_indices = list(profile.side_command_indices)
+        if side_indices:
+            open_side = self.open_pose[side_indices]
+            side_command = open_side.copy()
+            if (
+                profile.abducted_side_commands is not None
+                and features.thumb_abduction >= features.thumb_opposition
+            ):
+                side_command += features.thumb_abduction * (
+                    np.asarray(profile.abducted_side_commands) - open_side
+                )
+            elif features.thumb_opposition > 0.0:
+                side_command += features.thumb_opposition * (
+                    np.asarray(profile.power_grasp_side_commands) - open_side
+                )
+            command[side_indices] = side_command
+
+        if pinch_blend > 0.0:
+            selected_role = PINCH_ROLES[selected_target]
+            # Thumb proximity chooses the opposition target, but it must not
+            # curl that finger by itself. Gate morphology compensation with
+            # the selected human finger's own root/tip flexion so sweeping the
+            # thumb across an open hand leaves all four fingers independent.
+            selected_root_index = 1 + selected_target
+            selected_tip_index = 6 + selected_target
+            selected_finger_intent = float(
+                0.5
+                * (
+                    closure[selected_root_index]
+                    + closure[selected_tip_index]
+                )
+            )
+            finger_engagement = _smoothstep(
+                selected_finger_intent,
+                PINCH_FINGER_ENGAGEMENT_START,
+                PINCH_FINGER_ENGAGEMENT_END,
+            )
+            # Index/middle precision pinches are commonly made with the target
+            # finger almost straight. Once fingertip distance is very close to
+            # the calibrated contact endpoint, allow the robot-only closure
+            # compensation even without visible target-finger flexion. Ring
+            # and pinky keep the stricter self-flexion gate so a thumb sweep
+            # across the palm cannot drag them along.
+            contact_engagement = 0.0
+            if selected_role in PINCH_CONTACT_ASSIST_TARGETS:
+                contact_engagement = _smoothstep(
+                    pinch_strength,
+                    PINCH_CONTACT_ASSIST_START,
+                    PINCH_CONTACT_ASSIST_END,
+                )
+            finger_assist_blend = pinch_blend * max(
+                finger_engagement,
+                contact_engagement,
+            )
+            for channel in profile.flexion_channels:
+                if channel.pinch_role == "thumb":
+                    target_activation = profile.pinch_thumb_closure
+                    channel_pinch_blend = pinch_blend
+                elif channel.pinch_role == selected_role:
+                    current_activation = sum(
+                        closure[feature_index] * weight
+                        for feature_index, weight in channel.sources
+                    )
+                    target_activation = max(
+                        current_activation,
+                        profile.pinch_finger_closure,
+                    )
+                    channel_pinch_blend = finger_assist_blend
+                else:
+                    continue
+                target_activation *= self.max_closure
+                index = channel.command_index
+                target_command = self.open_pose[index] + target_activation * (
+                    self.fist_pose[index] - self.open_pose[index]
+                )
+                command[index] += channel_pinch_blend * (
+                    target_command - command[index]
+                )
+            if side_indices:
+                target_side = np.asarray(
+                    profile.pinch_target_side_commands[selected_target], dtype=float
+                )
+                command[side_indices] += pinch_blend * (
+                    target_side - command[side_indices]
+                )
+
+        if grasp_blend > 0.0:
+            # Four-finger flexion defines power_grasp, but it must not close an
+            # otherwise open thumb. Complete thumb flexion only when the shared
+            # feature layer also sees the thumb's own calibrated opposition.
+            # This restores a natural full fist without regressing the
+            # four-fingers-only gesture.
+            for channel in profile.flexion_channels:
+                channel_grasp_blend = grasp_blend
+                if channel.pinch_role == "thumb":
+                    channel_grasp_blend *= float(
+                        np.clip(features.thumb_opposition, 0.0, 1.0)
+                    )
+                index = channel.command_index
+                command[index] += channel_grasp_blend * (
+                    self.fist_pose[index] - command[index]
+                )
+
+            for index in profile.splay_command_indices:
+                command[index] += grasp_blend * (
+                    self.fist_pose[index] - command[index]
+                )
+
+        for index, value in profile.fixed_channels:
+            command[index] = value
+        self.last_raw_command = np.clip(command, 0.0, 255.0)
+        return self.last_raw_command.copy()
+
+    def map_features(self, features: HandFeatures) -> list[int]:
+        raw = self.raw_command(features)
+        filtered = (
+            self.profile.ema_alpha * raw
+            + (1.0 - self.profile.ema_alpha) * self.last_command
+        )
+        delta = np.clip(
+            filtered - self.last_command,
+            -self.profile.max_step,
+            self.profile.max_step,
+        )
+        delta[np.abs(delta) <= self.profile.command_deadband] = 0.0
+        self.last_command = np.clip(self.last_command + delta, 0.0, 255.0)
+        for index, value in self.profile.fixed_channels:
+            self.last_command[index] = value
+        return np.rint(self.last_command).astype(int).tolist()

src/camera_retarget_l20/camera_retarget_l20/o6_mapping.py

@@ -0,0 +1,151 @@
+"""Convert calibrated human-hand features to the left O6 six-value protocol."""
+
+from __future__ import annotations
+
+from collections.abc import Sequence
+from dataclasses import replace
+
+import numpy as np
+
+from .hand_features import HandFeatures
+from .model_profiles import (
+    DirectModelProfile,
+    FlexionChannel,
+    ProfiledCommandMapper,
+)
+
+
+O6_COMMAND_NAMES: tuple[str, ...] = (
+    "thumb_cmc_pitch",
+    "thumb_cmc_yaw",
+    "index_mcp_pitch",
+    "middle_mcp_pitch",
+    "ring_mcp_pitch",
+    "pinky_mcp_pitch",
+)
+
+# Open/yaw values come from LinkerHand/config/O6_positions.yaml.  The O6
+# protocol and this physical hand accept the full 0..255 flexion range, so the
+# default full-fist profile no longer stops the thumb early at the vendor's
+# conservative value 67.
+DEFAULT_OPEN_POSE: tuple[int, ...] = (255, 179, 255, 255, 255, 255)
+DEFAULT_FIST_POSE: tuple[int, ...] = (0, 151, 0, 0, 0, 0)
+# The vendor file provides only neutral/fist yaw endpoints. Use the same
+# conservative 28-count travel on the other side of neutral for abduction.
+THUMB_ABDUCTED_COMMAND = 207
+
+# Measured on the physical left O6. The single thumb-yaw actuator can align
+# with the index at 80 and reaches only the middle-finger region at its full
+# command-space endpoint. Ring/pinky pinches therefore use maximum thumb travel
+# and rely on the selected finger flexion for the remaining approach.
+PINCH_TARGET_YAW_COMMANDS = np.asarray((80.0, 0.0, 0.0, 0.0), dtype=float)
+
+
+def default_o6_profile() -> DirectModelProfile:
+    """Return the source-compatible built-in O6 profile."""
+
+    return DirectModelProfile(
+        model="O6",
+        command_names=O6_COMMAND_NAMES,
+        open_pose=tuple(float(value) for value in DEFAULT_OPEN_POSE),
+        fist_pose=tuple(float(value) for value in DEFAULT_FIST_POSE),
+        flexion_channels=(
+            FlexionChannel(0, ((0, 0.2), (5, 0.8)), "thumb"),
+            FlexionChannel(2, ((1, 0.35), (6, 0.65)), "index"),
+            FlexionChannel(3, ((2, 0.35), (7, 0.65)), "middle"),
+            FlexionChannel(4, ((3, 0.35), (8, 0.65)), "ring"),
+            FlexionChannel(5, ((4, 0.35), (9, 0.65)), "pinky"),
+        ),
+        side_command_indices=(1,),
+        pinch_target_side_commands=tuple(
+            (float(value),) for value in PINCH_TARGET_YAW_COMMANDS
+        ),
+        power_grasp_side_commands=(float(DEFAULT_FIST_POSE[1]),),
+        abducted_side_commands=(float(THUMB_ABDUCTED_COMMAND),),
+        splay_command_indices=(),
+        splay_scale=1.0,
+        splay_gain=0.0,
+        max_splay_delta=0.0,
+        fixed_channels=(),
+        pinch_thumb_closure=0.85,
+        pinch_finger_closure=0.65,
+        ema_alpha=0.35,
+        max_step=8.0,
+        command_deadband=0.0,
+        motor_speed=60,
+    )
+
+
+class O6MediaPipeMapper:
+    """Compatibility wrapper around the configuration-driven O6 adapter."""
+
+    def __init__(
+        self,
+        *,
+        ema_alpha: float | None = None,
+        max_step: float | None = None,
+        command_deadband: float | None = None,
+        pinch_thumb_closure: float | None = None,
+        pinch_finger_closure: float | None = None,
+        open_pose: Sequence[float] = DEFAULT_OPEN_POSE,
+        fist_pose: Sequence[float] = DEFAULT_FIST_POSE,
+        profile: DirectModelProfile | None = None,
+    ) -> None:
+        base_profile = profile or default_o6_profile()
+        if base_profile.model != "O6":
+            raise ValueError("O6MediaPipeMapper requires an O6 profile")
+        validated_open = self._validate_command(open_pose)
+        validated_fist = self._validate_command(fist_pose)
+        if profile is not None:
+            validated_open = np.asarray(profile.open_pose, dtype=float)
+            validated_fist = np.asarray(profile.fist_pose, dtype=float)
+        base_profile = replace(
+            base_profile,
+            open_pose=tuple(validated_open),
+            fist_pose=tuple(validated_fist),
+        ).with_runtime_overrides(
+            ema_alpha=ema_alpha,
+            max_step=max_step,
+            command_deadband=command_deadband,
+            pinch_thumb_closure=pinch_thumb_closure,
+            pinch_finger_closure=pinch_finger_closure,
+        )
+        self.profile = base_profile
+        self._mapper = ProfiledCommandMapper(base_profile)
+        self.open_pose = self._mapper.open_pose
+        self.fist_pose = self._mapper.fist_pose
+        self.ema_alpha = base_profile.ema_alpha
+        self.max_step = base_profile.max_step
+        self.command_deadband = base_profile.command_deadband
+        self.pinch_thumb_closure = base_profile.pinch_thumb_closure
+        self.pinch_finger_closure = base_profile.pinch_finger_closure
+
+    @staticmethod
+    def _validate_command(command: Sequence[float]) -> np.ndarray:
+        values = np.asarray(command, dtype=float)
+        if values.shape != (6,):
+            raise ValueError("an O6 command must contain six values")
+        if not np.all(np.isfinite(values)):
+            raise ValueError("O6 command contains non-finite values")
+        return np.clip(values, 0.0, 255.0)
+
+    def reset(self, command: Sequence[float] | None = None) -> None:
+        self._mapper.reset(command)
+
+    @property
+    def last_command(self) -> np.ndarray:
+        return self._mapper.last_command
+
+    @property
+    def last_raw_command(self) -> np.ndarray:
+        return self._mapper.last_raw_command
+
+    @property
+    def last_pinch_blend(self) -> float:
+        return self._mapper.last_pinch_blend
+
+    def raw_command(self, features: HandFeatures) -> np.ndarray:
+        return self._mapper.raw_command(features)
+
+    def map_features(self, features: HandFeatures) -> list[int]:
+        return self._mapper.map_features(features)

src/camera_retarget_l20/config/direct_mapping_profiles.yml

@@ -0,0 +1,133 @@
+schema_version: 1
+
+models:
+  O6:
+    command_names:
+      - thumb_cmc_pitch
+      - thumb_cmc_yaw
+      - index_mcp_pitch
+      - middle_mcp_pitch
+      - ring_mcp_pitch
+      - pinky_mcp_pitch
+    open_pose: [255, 179, 255, 255, 255, 255]
+    fist_pose: [0, 151, 0, 0, 0, 0]
+    flexion_channels:
+      - command_index: 0
+        sources: {thumb_root: 0.2, thumb_tip: 0.8}
+        pinch_role: thumb
+      - command_index: 2
+        sources: {index_root: 0.35, index_tip: 0.65}
+        pinch_role: index
+      - command_index: 3
+        sources: {middle_root: 0.35, middle_tip: 0.65}
+        pinch_role: middle
+      - command_index: 4
+        sources: {ring_root: 0.35, ring_tip: 0.65}
+        pinch_role: ring
+      - command_index: 5
+        sources: {pinky_root: 0.35, pinky_tip: 0.65}
+        pinch_role: pinky
+    side:
+      indices: [1]
+      abducted_commands: [207]
+      power_grasp_commands: [151]
+    pinch:
+      thumb_closure: 0.85
+      finger_closure: 0.65
+      power_grasp_start: 0.65
+      power_grasp_end: 0.90
+      target_side_commands:
+        index: [80]
+        middle: [0]
+        ring: [0]
+        pinky: [0]
+    filters:
+      ema_alpha: 0.55
+      max_step: 16.0
+      command_deadband: 2.0
+    motor_speed: 60
+
+  G20:
+    command_names:
+      - Thumb Base
+      - Index Finger Base
+      - Middle Finger Base
+      - Ring Finger Base
+      - Pinky Finger Base
+      - Thumb Abduction
+      - Index Finger Abduction
+      - Middle Finger Abduction
+      - Ring Finger Abduction
+      - Pinky Finger Abduction
+      - Thumb Horizontal Abduction
+      - Reserved 1
+      - Reserved 2
+      - Reserved 3
+      - Reserved 4
+      - Thumb Tip
+      - Index Finger Tip
+      - Middle Finger Tip
+      - Ring Finger Tip
+      - Pinky Finger Tip
+    open_pose: [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+    fist_pose: [50, 0, 0, 0, 0, 78, 193, 158, 128, 91, 112, 255, 255, 255, 255, 144, 0, 0, 0, 0]
+    flexion_channels:
+      - command_index: 0
+        sources: {thumb_root: 1.0}
+        pinch_role: thumb
+      - command_index: 1
+        sources: {index_root: 1.0}
+        pinch_role: index
+      - command_index: 2
+        sources: {middle_root: 1.0}
+        pinch_role: middle
+      - command_index: 3
+        sources: {ring_root: 1.0}
+        pinch_role: ring
+      - command_index: 4
+        sources: {pinky_root: 1.0}
+        pinch_role: pinky
+      - command_index: 15
+        sources: {thumb_tip: 1.0}
+        pinch_role: thumb
+      - command_index: 16
+        sources: {index_tip: 1.0}
+        pinch_role: index
+      - command_index: 17
+        sources: {middle_tip: 1.0}
+        pinch_role: middle
+      - command_index: 18
+        sources: {ring_tip: 1.0}
+        pinch_role: ring
+      - command_index: 19
+        sources: {pinky_tip: 1.0}
+        pinch_role: pinky
+    side:
+      indices: [5, 10]
+      abducted_commands: [20, 255]
+      power_grasp_commands: [78, 112]
+    splay:
+      indices: [6, 7, 8, 9]
+      scale: 554.3478260869565
+      gain: 1.0
+      max_delta: 255.0
+    pinch:
+      thumb_closure: 0.85
+      finger_closure: 0.65
+      power_grasp_start: 0.65
+      power_grasp_end: 0.90
+      target_side_commands:
+        index: [167, 184]
+        middle: [122, 148]
+        ring: [78, 112]
+        pinky: [50, 90]
+    fixed_channels:
+      11: 255
+      12: 255
+      13: 255
+      14: 255
+    filters:
+      ema_alpha: 0.55
+      max_step: 16.0
+      command_deadband: 2.0
+    motor_speed: 60

src/camera_retarget_l20/config/g20_left_mediapipe.yml

@@ -0,0 +1,56 @@
+retargeting:
+  type: vector
+  # Replaced at runtime by the urdf_path parameter.
+  urdf_path: linkerhand_g20_left.urdf
+
+  # The G20 URDF has 21 revolute joints. Five distal joints mimic their PIP
+  # joint, leaving these 16 independent joints for optimization.
+  target_joint_names:
+    - thumb_cmc_roll
+    - thumb_cmc_yaw
+    - thumb_cmc_pitch
+    - thumb_mcp
+    - index_mcp_roll
+    - index_mcp_pitch
+    - index_pip
+    - middle_mcp_roll
+    - middle_mcp_pitch
+    - middle_pip
+    - ring_mcp_roll
+    - ring_mcp_pitch
+    - ring_pip
+    - pinky_mcp_roll
+    - pinky_mcp_pitch
+    - pinky_pip
+
+  target_origin_link_names:
+    - hand_base_link
+    - hand_base_link
+    - hand_base_link
+    - hand_base_link
+    - hand_base_link
+    - hand_base_link
+    - hand_base_link
+    - hand_base_link
+    - hand_base_link
+    - hand_base_link
+  target_task_link_names:
+    - thumb_distal
+    - thumb_metacarpals
+    - index_distal
+    - middle_distal
+    - ring_distal
+    - pinky_distal
+    - index_proximal
+    - middle_proximal
+    - ring_proximal
+    - pinky_proximal
+
+  # MediaPipe Hands order: wrist=0, thumb=1..4, index=5..8,
+  # middle=9..12, ring=13..16, little=17..20.
+  target_link_human_indices:
+    - [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+    - [4, 2, 8, 12, 16, 20, 6, 10, 14, 18]
+
+  scaling_factor: 1.0
+  low_pass_alpha: 0.25

src/camera_retarget_l20/config/l20_left_mediapipe.yml

@@ -0,0 +1,57 @@
+retargeting:
+  type: vector
+  # Replaced at runtime by the urdf_path parameter.
+  urdf_path: linkerhand_l20_left.urdf
+
+  # The L20 has 21 revolute URDF joints, five of which are mimic joints.
+  # Only these 16 independent joints are optimized.
+  target_joint_names:
+    - index_joint0
+    - index_joint1
+    - index_joint2
+    - little_joint0
+    - little_joint1
+    - little_joint2
+    - middle_joint0
+    - middle_joint1
+    - middle_joint2
+    - ring_joint0
+    - ring_joint1
+    - ring_joint2
+    - thumb_joint0
+    - thumb_joint1
+    - thumb_joint2
+    - thumb_joint3
+
+  target_origin_link_names:
+    - base_link
+    - base_link
+    - base_link
+    - base_link
+    - base_link
+    - base_link
+    - base_link
+    - base_link
+    - base_link
+    - base_link
+  target_task_link_names:
+    - thumb_link5
+    - thumb_link3
+    - index_link4
+    - middle_link4
+    - ring_link4
+    - little_link4
+    - index_link2
+    - middle_link2
+    - ring_link2
+    - little_link2
+
+  # MediaPipe Hands order: wrist=0, thumb=1..4, index=5..8,
+  # middle=9..12, ring=13..16, little=17..20.
+  target_link_human_indices:
+    - [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+    - [4, 2, 8, 12, 16, 20, 6, 10, 14, 18]
+
+  scaling_factor: 1.0
+  low_pass_alpha: 0.25
+

src/camera_retarget_l20/launch/camera_g20_left.launch.py

@@ -0,0 +1,62 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+    arguments = [
+        DeclareLaunchArgument("camera_device", default_value="/dev/video0"),
+        DeclareLaunchArgument("show_window", default_value="true"),
+        DeclareLaunchArgument("selfie", default_value="false"),
+        DeclareLaunchArgument("motor_speed", default_value="30"),
+        DeclareLaunchArgument("max_closure", default_value="0.95"),
+        DeclareLaunchArgument("splay_gain", default_value="1.0"),
+        DeclareLaunchArgument("max_splay_delta", default_value="90.0"),
+        DeclareLaunchArgument("thumb_abduction_min_command", default_value="20.0"),
+        DeclareLaunchArgument("thumb_pinch_gain", default_value="2.0"),
+        DeclareLaunchArgument("start_enabled", default_value="false"),
+    ]
+    camera_node = Node(
+        package="camera_retarget_l20",
+        executable="camera_hand_retarget",
+        name="camera_g20_retarget",
+        output="screen",
+        parameters=[
+            {
+                "hand_model": "G20",
+                "mapping_mode": "mediapipe_angles",
+                "camera_device": LaunchConfiguration("camera_device"),
+                "show_window": ParameterValue(
+                    LaunchConfiguration("show_window"), value_type=bool
+                ),
+                "selfie": ParameterValue(
+                    LaunchConfiguration("selfie"), value_type=bool
+                ),
+                "motor_speed": ParameterValue(
+                    LaunchConfiguration("motor_speed"), value_type=int
+                ),
+                "max_closure": ParameterValue(
+                    LaunchConfiguration("max_closure"), value_type=float
+                ),
+                "splay_gain": ParameterValue(
+                    LaunchConfiguration("splay_gain"), value_type=float
+                ),
+                "max_splay_delta": ParameterValue(
+                    LaunchConfiguration("max_splay_delta"), value_type=float
+                ),
+                "thumb_abduction_min_command": ParameterValue(
+                    LaunchConfiguration("thumb_abduction_min_command"),
+                    value_type=float,
+                ),
+                "thumb_pinch_gain": ParameterValue(
+                    LaunchConfiguration("thumb_pinch_gain"), value_type=float
+                ),
+                "start_enabled": ParameterValue(
+                    LaunchConfiguration("start_enabled"), value_type=bool
+                ),
+            }
+        ],
+    )
+    return LaunchDescription(arguments + [camera_node])

src/camera_retarget_l20/launch/camera_l20_left.launch.py

@@ -0,0 +1,44 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+    arguments = [
+        DeclareLaunchArgument("camera_device", default_value="/dev/video0"),
+        DeclareLaunchArgument("show_window", default_value="true"),
+        DeclareLaunchArgument("selfie", default_value="false"),
+        DeclareLaunchArgument("motor_speed", default_value="60"),
+        DeclareLaunchArgument("max_closure", default_value="0.80"),
+        DeclareLaunchArgument("start_enabled", default_value="false"),
+    ]
+    camera_node = Node(
+        package="camera_retarget_l20",
+        executable="camera_l20_retarget",
+        name="camera_l20_retarget",
+        output="screen",
+        parameters=[
+            {
+                "camera_device": LaunchConfiguration("camera_device"),
+                "show_window": ParameterValue(
+                    LaunchConfiguration("show_window"), value_type=bool
+                ),
+                "selfie": ParameterValue(
+                    LaunchConfiguration("selfie"), value_type=bool
+                ),
+                "motor_speed": ParameterValue(
+                    LaunchConfiguration("motor_speed"), value_type=int
+                ),
+                "max_closure": ParameterValue(
+                    LaunchConfiguration("max_closure"), value_type=float
+                ),
+                "start_enabled": ParameterValue(
+                    LaunchConfiguration("start_enabled"), value_type=bool
+                ),
+            }
+        ],
+    )
+    return LaunchDescription(arguments + [camera_node])
+

src/camera_retarget_l20/launch/teleop_g20_left.launch.py

@@ -0,0 +1,79 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+    arguments = [
+        DeclareLaunchArgument("camera_device", default_value="/dev/video0"),
+        DeclareLaunchArgument("can", default_value="can0"),
+        DeclareLaunchArgument("show_window", default_value="true"),
+        DeclareLaunchArgument("selfie", default_value="false"),
+        DeclareLaunchArgument("motor_speed", default_value="30"),
+        DeclareLaunchArgument("max_closure", default_value="0.95"),
+        DeclareLaunchArgument("splay_gain", default_value="1.0"),
+        DeclareLaunchArgument("max_splay_delta", default_value="90.0"),
+        DeclareLaunchArgument("thumb_abduction_min_command", default_value="20.0"),
+        DeclareLaunchArgument("thumb_pinch_gain", default_value="2.0"),
+    ]
+    hand_driver = Node(
+        package="linker_hand_ros2_sdk",
+        executable="linker_hand_sdk",
+        name="left_g20_driver",
+        output="screen",
+        parameters=[
+            {
+                "hand_type": "left",
+                "hand_joint": "G20",
+                "is_touch": False,
+                "can": LaunchConfiguration("can"),
+                "modbus": "None",
+                "startup_speed": ParameterValue(
+                    LaunchConfiguration("motor_speed"), value_type=int
+                ),
+            }
+        ],
+    )
+    camera_node = Node(
+        package="camera_retarget_l20",
+        executable="camera_hand_retarget",
+        name="camera_g20_retarget",
+        output="screen",
+        parameters=[
+            {
+                "hand_model": "G20",
+                "mapping_mode": "mediapipe_angles",
+                "camera_device": LaunchConfiguration("camera_device"),
+                "show_window": ParameterValue(
+                    LaunchConfiguration("show_window"), value_type=bool
+                ),
+                "selfie": ParameterValue(
+                    LaunchConfiguration("selfie"), value_type=bool
+                ),
+                "motor_speed": ParameterValue(
+                    LaunchConfiguration("motor_speed"), value_type=int
+                ),
+                "max_closure": ParameterValue(
+                    LaunchConfiguration("max_closure"), value_type=float
+                ),
+                "splay_gain": ParameterValue(
+                    LaunchConfiguration("splay_gain"), value_type=float
+                ),
+                "max_splay_delta": ParameterValue(
+                    LaunchConfiguration("max_splay_delta"), value_type=float
+                ),
+                "thumb_abduction_min_command": ParameterValue(
+                    LaunchConfiguration("thumb_abduction_min_command"),
+                    value_type=float,
+                ),
+                "thumb_pinch_gain": ParameterValue(
+                    LaunchConfiguration("thumb_pinch_gain"), value_type=float
+                ),
+                # Hardware output still requires an explicit E key/service call.
+                "start_enabled": False,
+            }
+        ],
+    )
+    return LaunchDescription(arguments + [hand_driver, camera_node])

src/camera_retarget_l20/launch/teleop_l20_left.launch.py

@@ -0,0 +1,56 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+    arguments = [
+        DeclareLaunchArgument("camera_device", default_value="/dev/video0"),
+        DeclareLaunchArgument("can", default_value="can0"),
+        DeclareLaunchArgument("show_window", default_value="true"),
+        DeclareLaunchArgument("motor_speed", default_value="60"),
+        DeclareLaunchArgument("max_closure", default_value="0.80"),
+    ]
+    hand_driver = Node(
+        package="linker_hand_ros2_sdk",
+        executable="linker_hand_sdk",
+        name="left_l20_driver",
+        output="screen",
+        parameters=[
+            {
+                "hand_type": "left",
+                "hand_joint": "L20",
+                "is_touch": False,
+                "can": LaunchConfiguration("can"),
+                "modbus": "None",
+                "startup_speed": ParameterValue(
+                    LaunchConfiguration("motor_speed"), value_type=int
+                ),
+            }
+        ],
+    )
+    camera_node = Node(
+        package="camera_retarget_l20",
+        executable="camera_l20_retarget",
+        name="camera_l20_retarget",
+        output="screen",
+        parameters=[
+            {
+                "camera_device": LaunchConfiguration("camera_device"),
+                "show_window": ParameterValue(
+                    LaunchConfiguration("show_window"), value_type=bool
+                ),
+                "motor_speed": ParameterValue(
+                    LaunchConfiguration("motor_speed"), value_type=int
+                ),
+                "max_closure": ParameterValue(
+                    LaunchConfiguration("max_closure"), value_type=float
+                ),
+                # Hardware commands still require an explicit E key/service call.
+                "start_enabled": False,
+            }
+        ],
+    )
+    return LaunchDescription(arguments + [hand_driver, camera_node])

src/camera_retarget_l20/launch/teleop_o6_g20_left.launch.py

@@ -0,0 +1,187 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+O6_OPEN_POSE = [255, 179, 255, 255, 255, 255]
+G20_OPEN_POSE = [
+    255, 255, 255, 255, 255,
+    255, 193, 148, 105, 42,
+    255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255,
+]
+
+
+def generate_launch_description():
+    arguments = [
+        DeclareLaunchArgument("camera_device", default_value="/dev/video0"),
+        DeclareLaunchArgument("g20_can", default_value="can0"),
+        DeclareLaunchArgument("o6_can", default_value="can1"),
+        DeclareLaunchArgument("show_window", default_value="true"),
+        DeclareLaunchArgument("selfie", default_value="false"),
+        DeclareLaunchArgument("motor_speed", default_value="60"),
+        DeclareLaunchArgument("o6_state_poll_rate", default_value="20.0"),
+        DeclareLaunchArgument("g20_state_poll_rate", default_value="10.0"),
+        DeclareLaunchArgument("o6_velocity_poll_rate", default_value="10.0"),
+        DeclareLaunchArgument("g20_velocity_poll_rate", default_value="2.0"),
+        DeclareLaunchArgument("ema_alpha", default_value="0.55"),
+        DeclareLaunchArgument("max_step", default_value="16.0"),
+        DeclareLaunchArgument("command_deadband", default_value="2.0"),
+        # Use the measured per-joint fist endpoints without an additional
+        # closure reduction. Dynamic filtering and timeout protection remain.
+        DeclareLaunchArgument("max_closure", default_value="1.0"),
+        DeclareLaunchArgument("splay_gain", default_value="1.0"),
+        DeclareLaunchArgument("max_splay_delta", default_value="255.0"),
+        DeclareLaunchArgument("thumb_abduction_min_command", default_value="20.0"),
+        DeclareLaunchArgument("thumb_pinch_gain", default_value="2.0"),
+        DeclareLaunchArgument("g20_pinch_thumb_closure", default_value="0.85"),
+        DeclareLaunchArgument("g20_pinch_finger_closure", default_value="0.65"),
+        DeclareLaunchArgument("o6_pinch_thumb_closure", default_value="0.85"),
+        DeclareLaunchArgument("o6_pinch_finger_closure", default_value="0.65"),
+        DeclareLaunchArgument("driver_state_timeout", default_value="1.0"),
+        DeclareLaunchArgument("model_profile_file", default_value=""),
+        DeclareLaunchArgument("calibration_duration", default_value="1.5"),
+        DeclareLaunchArgument("calibration_min_samples", default_value="30"),
+        DeclareLaunchArgument("calibration_timeout", default_value="8.0"),
+        DeclareLaunchArgument("publish_debug", default_value="true"),
+    ]
+
+    o6_driver = Node(
+        package="linker_hand_ros2_sdk",
+        executable="linker_hand_sdk",
+        name="left_o6_driver",
+        output="screen",
+        parameters=[
+            {
+                "hand_type": "left",
+                "hand_joint": "O6",
+                "is_touch": False,
+                "can": LaunchConfiguration("o6_can"),
+                "modbus": "None",
+                "topic_prefix": "/o6",
+                "startup_pose": O6_OPEN_POSE,
+                "startup_speed": ParameterValue(
+                    LaunchConfiguration("motor_speed"), value_type=int
+                ),
+                "state_poll_rate": ParameterValue(
+                    LaunchConfiguration("o6_state_poll_rate"), value_type=float
+                ),
+                "velocity_poll_rate": ParameterValue(
+                    LaunchConfiguration("o6_velocity_poll_rate"),
+                    value_type=float,
+                ),
+            }
+        ],
+    )
+    g20_driver = Node(
+        package="linker_hand_ros2_sdk",
+        executable="linker_hand_sdk",
+        name="left_g20_driver",
+        output="screen",
+        parameters=[
+            {
+                "hand_type": "left",
+                "hand_joint": "G20",
+                "is_touch": False,
+                "can": LaunchConfiguration("g20_can"),
+                "modbus": "None",
+                "topic_prefix": "/g20",
+                "startup_pose": G20_OPEN_POSE,
+                "startup_speed": ParameterValue(
+                    LaunchConfiguration("motor_speed"), value_type=int
+                ),
+                "state_poll_rate": ParameterValue(
+                    LaunchConfiguration("g20_state_poll_rate"), value_type=float
+                ),
+                "velocity_poll_rate": ParameterValue(
+                    LaunchConfiguration("g20_velocity_poll_rate"),
+                    value_type=float,
+                ),
+            }
+        ],
+    )
+    camera_node = Node(
+        package="camera_retarget_l20",
+        executable="camera_hand_retarget",
+        name="camera_o6_g20_retarget",
+        output="screen",
+        parameters=[
+            {
+                "target_models": ["O6", "G20"],
+                "mapping_mode": "mediapipe_angles",
+                "camera_device": LaunchConfiguration("camera_device"),
+                "show_window": ParameterValue(
+                    LaunchConfiguration("show_window"), value_type=bool
+                ),
+                "selfie": ParameterValue(
+                    LaunchConfiguration("selfie"), value_type=bool
+                ),
+                "motor_speed": ParameterValue(
+                    LaunchConfiguration("motor_speed"), value_type=int
+                ),
+                "ema_alpha": ParameterValue(
+                    LaunchConfiguration("ema_alpha"), value_type=float
+                ),
+                "max_step": ParameterValue(
+                    LaunchConfiguration("max_step"), value_type=float
+                ),
+                "command_deadband": ParameterValue(
+                    LaunchConfiguration("command_deadband"), value_type=float
+                ),
+                "max_closure": ParameterValue(
+                    LaunchConfiguration("max_closure"), value_type=float
+                ),
+                "splay_gain": ParameterValue(
+                    LaunchConfiguration("splay_gain"), value_type=float
+                ),
+                "max_splay_delta": ParameterValue(
+                    LaunchConfiguration("max_splay_delta"), value_type=float
+                ),
+                "thumb_abduction_min_command": ParameterValue(
+                    LaunchConfiguration("thumb_abduction_min_command"),
+                    value_type=float,
+                ),
+                "thumb_pinch_gain": ParameterValue(
+                    LaunchConfiguration("thumb_pinch_gain"), value_type=float
+                ),
+                "g20_pinch_thumb_closure": ParameterValue(
+                    LaunchConfiguration("g20_pinch_thumb_closure"),
+                    value_type=float,
+                ),
+                "g20_pinch_finger_closure": ParameterValue(
+                    LaunchConfiguration("g20_pinch_finger_closure"),
+                    value_type=float,
+                ),
+                "o6_pinch_thumb_closure": ParameterValue(
+                    LaunchConfiguration("o6_pinch_thumb_closure"),
+                    value_type=float,
+                ),
+                "o6_pinch_finger_closure": ParameterValue(
+                    LaunchConfiguration("o6_pinch_finger_closure"),
+                    value_type=float,
+                ),
+                "driver_state_timeout": ParameterValue(
+                    LaunchConfiguration("driver_state_timeout"), value_type=float
+                ),
+                "model_profile_file": ParameterValue(
+                    LaunchConfiguration("model_profile_file"), value_type=str
+                ),
+                "calibration_duration": ParameterValue(
+                    LaunchConfiguration("calibration_duration"), value_type=float
+                ),
+                "calibration_min_samples": ParameterValue(
+                    LaunchConfiguration("calibration_min_samples"), value_type=int
+                ),
+                "calibration_timeout": ParameterValue(
+                    LaunchConfiguration("calibration_timeout"), value_type=float
+                ),
+                "publish_debug": ParameterValue(
+                    LaunchConfiguration("publish_debug"), value_type=bool
+                ),
+                "start_enabled": False,
+            }
+        ],
+    )
+    return LaunchDescription(arguments + [o6_driver, g20_driver, camera_node])

src/camera_retarget_l20/package.xml

@@ -0,0 +1,23 @@
+<?xml version="1.0"?>
+<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
+<package format="3">
+  <name>camera_retarget_l20</name>
+  <version>0.2.0</version>
+  <description>Camera retargeting bridge for physical left LinkerHand O6/G20/L20 hands.</description>
+  <maintainer email="support@linker-robotics.com">lxp</maintainer>
+  <license>MIT</license>
+
+  <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>rclpy</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <exec_depend>python3-opencv</exec_depend>
+  <exec_depend>python3-yaml</exec_depend>
+
+  <test_depend>python3-pytest</test_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/camera_retarget_l20/pyproject.toml

@@ -0,0 +1,4 @@
+[build-system]
+requires = ["setuptools>=61"]
+build-backend = "setuptools.build_meta"
+

src/camera_retarget_l20/resource/camera_retarget_l20

@@ -0,0 +1 @@
+

src/camera_retarget_l20/setup.cfg

@@ -0,0 +1,5 @@
+[develop]
+script_dir=$base/lib/camera_retarget_l20
+[install]
+install_scripts=$base/lib/camera_retarget_l20
+

src/camera_retarget_l20/setup.py

@@ -0,0 +1,32 @@
+from glob import glob
+from setuptools import find_packages, setup
+
+
+package_name = "camera_retarget_l20"
+
+setup(
+    name=package_name,
+    version="0.2.0",
+    packages=find_packages(),
+    data_files=[
+        (
+            "share/ament_index/resource_index/packages",
+            ["resource/" + package_name],
+        ),
+        ("share/" + package_name, ["package.xml"]),
+        ("share/" + package_name + "/config", glob("config/*.yml")),
+        ("share/" + package_name + "/launch", glob("launch/*.launch.py")),
+    ],
+    install_requires=["setuptools", "numpy", "PyYAML"],
+    zip_safe=True,
+    maintainer="lxp",
+    maintainer_email="support@linker-robotics.com",
+    description="Camera retargeting bridge for physical left LinkerHand O6/G20/L20 hands",
+    license="MIT",
+    entry_points={
+        "console_scripts": [
+            "camera_hand_retarget = camera_retarget_l20.node:main",
+            "camera_l20_retarget = camera_retarget_l20.node:main",
+        ],
+    },
+)

src/camera_retarget_l20/test/test_calibration_capture.py

@@ -0,0 +1,73 @@
+import numpy as np
+
+from camera_retarget_l20.calibration import (
+    PoseCalibrationSession,
+    RawHandObservation,
+)
+
+
+def observation(offset=0.0):
+    return RawHandObservation(
+        bends=np.full(10, 0.2 + offset),
+        pinch_distances=np.asarray([1.2, 1.4, 1.6, 1.8]) + offset,
+        splay=np.asarray([-0.2, -0.05, 0.05, 0.2]) + offset,
+        splay_confidence=np.ones(4),
+        thumb_splay=0.3 + offset,
+        thumb_splay_confidence=1.0,
+    )
+
+
+def test_stable_window_completes_after_duration_and_minimum_samples():
+    session = PoseCalibrationSession(
+        "open", started_at=0.0, duration=1.5, min_samples=30, timeout=8.0
+    )
+    result = None
+    for index in range(31):
+        result = session.add(observation(), index * 0.05)
+
+    assert session.state == "complete"
+    assert result is not None
+    assert result.bends.tolist() == [0.2] * 10
+    assert session.progress(1.5) == 1.0
+
+
+def test_coordinate_median_rejects_one_large_outlier():
+    session = PoseCalibrationSession(
+        "pinch", started_at=0.0, duration=1.5, min_samples=30, timeout=8.0
+    )
+    result = None
+    for index in range(31):
+        sample = observation(2.0 if index == 15 else 0.0)
+        result = session.add(sample, index * 0.05)
+
+    assert session.state == "complete"
+    assert result is not None
+    assert np.allclose(result.bends, 0.2)
+    assert result.pinch_distances[0] == 1.2
+
+
+def test_unstable_window_waits_then_times_out_without_result():
+    session = PoseCalibrationSession(
+        "fist", started_at=0.0, duration=1.5, min_samples=30, timeout=2.0
+    )
+    for index in range(39):
+        offset = 0.2 if index % 2 else -0.2
+        assert session.add(observation(offset), index * 0.05) is None
+
+    assert session.active
+    assert session.reason == "bend_not_stable"
+    session.poll(2.01)
+    assert session.state == "failed"
+    assert session.result is None
+    assert session.reason.startswith("calibration_timeout:")
+
+
+def test_missing_detection_times_out_without_samples():
+    session = PoseCalibrationSession(
+        "open", started_at=10.0, duration=1.5, min_samples=30, timeout=8.0
+    )
+    session.reason = "hand_not_detected"
+    session.poll(18.1)
+
+    assert session.state == "failed"
+    assert session.reason == "calibration_timeout:hand_not_detected"

src/camera_retarget_l20/test/test_dual_calibration.py

@@ -0,0 +1,236 @@
+import numpy as np
+import pytest
+
+from camera_retarget_l20.g20_mapping import (
+    DEFAULT_FIST_POSE as G20_DEFAULT_FIST_POSE,
+    DEFAULT_OPEN_POSE as G20_DEFAULT_OPEN_POSE,
+    PINCH_TARGET_OPPOSITION_COMMANDS,
+    G20MediaPipeMapper,
+    PINCH_CONFIRM_FRAMES,
+)
+from camera_retarget_l20.o6_mapping import DEFAULT_FIST_POSE, O6MediaPipeMapper
+
+
+def make_open_landmarks():
+    points = np.zeros((21, 3), dtype=float)
+    points[1:5] = np.array(
+        [[-0.20, 0.35, 0.0], [-0.40, 0.60, 0.0],
+         [-0.60, 0.85, 0.0], [-0.80, 1.10, 0.0]]
+    )
+    for root, x in zip((5, 9, 13, 17), (-0.45, -0.15, 0.15, 0.45)):
+        points[root:root + 4] = np.array(
+            [[x, 1.0, 0.0], [x, 1.7, 0.0],
+             [x, 2.3, 0.0], [x, 2.8, 0.0]]
+        )
+    return points
+
+
+def make_fist_landmarks():
+    points = make_open_landmarks()
+    points[3] = points[2] + np.array([0.0, 0.0, 0.6])
+    points[4] = points[3] + np.array([0.0, -0.5, 0.0])
+    for root in (5, 9, 13, 17):
+        points[root + 1] = points[root] + np.array([0.0, 0.0, 0.7])
+        points[root + 2] = points[root + 1] + np.array([0.0, -0.6, 0.0])
+        points[root + 3] = points[root + 2] + np.array([0.0, 0.0, -0.5])
+    return points
+
+
+def make_pinch_landmarks(finger_tip_index=8):
+    points = make_open_landmarks()
+    points[4] = points[finger_tip_index].copy()
+    return points
+
+
+def make_pinch_landmarks_with_monocular_depth_error():
+    points = make_pinch_landmarks()
+    # The fingertips overlap in the palm plane, but monocular world-landmark
+    # depth disagrees by almost one palm width.
+    points[4, 2] += 0.8
+    return points
+
+
+def make_fist_with_opposed_thumb():
+    points = make_fist_landmarks()
+    points[2] = points[1] + np.array([0.0, 0.4, 0.0])
+    points[3] = points[2] + np.array([0.0, 0.0, 0.6])
+    points[4] = points[3] + np.array([0.0, -0.5, 0.0])
+    return points
+
+
+def make_four_finger_fist_with_open_thumb():
+    points = make_fist_landmarks()
+    points[1:5] = make_open_landmarks()[1:5]
+    return points
+
+
+def test_fist_calibration_requires_open_pose():
+    mapper = G20MediaPipeMapper()
+    with pytest.raises(RuntimeError, match="open-hand"):
+        mapper.calibrate_fist(make_fist_landmarks())
+
+
+def test_fist_calibration_rejects_insufficient_motion():
+    mapper = G20MediaPipeMapper()
+    open_landmarks = make_open_landmarks()
+    mapper.calibrate_open(open_landmarks)
+    with pytest.raises(ValueError, match="rejected"):
+        mapper.calibrate_fist(open_landmarks)
+
+
+def test_one_calibrated_feature_frame_drives_both_models():
+    extractor_and_g20 = G20MediaPipeMapper(
+        ema_alpha=1.0, max_step=255.0, max_closure=1.0
+    )
+    o6 = O6MediaPipeMapper(ema_alpha=1.0, max_step=255.0)
+    extractor_and_g20.calibrate_open(make_open_landmarks())
+    extractor_and_g20.calibrate_fist(make_fist_landmarks())
+    extractor_and_g20.calibrate_pinch(make_pinch_landmarks())
+    features = extractor_and_g20.extract_features(make_fist_landmarks())
+
+    g20_command = extractor_and_g20.map_features(features)
+    o6_command = o6.map_features(features)
+
+    assert len(g20_command) == 20
+    assert len(o6_command) == 6
+    assert features.thumb_opposition == pytest.approx(1.0)
+    assert g20_command[0] == G20_DEFAULT_FIST_POSE[0]
+    assert g20_command[15] == G20_DEFAULT_FIST_POSE[15]
+    assert o6_command[0] == DEFAULT_FIST_POSE[0]
+    assert o6_command[2:] == list(DEFAULT_FIST_POSE[2:])
+
+
+def test_open_fist_and_pinch_complete_three_pose_calibration():
+    mapper = G20MediaPipeMapper()
+    mapper.calibrate_open(make_open_landmarks())
+    mapper.calibrate_fist(make_fist_landmarks())
+    mapper.calibrate_pinch(make_pinch_landmarks())
+
+    for _ in range(PINCH_CONFIRM_FRAMES):
+        pinched = mapper.extract_features(make_pinch_landmarks())
+
+    assert mapper.is_three_pose_calibrated
+    assert pinched.thumb_abduction == pytest.approx(0.0)
+    assert pinched.thumb_opposition == pytest.approx(0.0)
+    assert pinched.pinch_strengths[0] == pytest.approx(1.0)
+
+
+@pytest.mark.parametrize(
+    ("finger_tip_index", "finger_offset"),
+    ((8, 0), (12, 1), (16, 2), (20, 3)),
+)
+def test_g20_pinch_selects_nearest_finger_without_curling_an_open_target(
+    finger_tip_index, finger_offset
+):
+    mapper = G20MediaPipeMapper(
+        ema_alpha=1.0,
+        max_step=255.0,
+        max_closure=0.95,
+        pinch_thumb_closure=0.85,
+        pinch_finger_closure=0.65,
+    )
+    mapper.calibrate_open(make_open_landmarks())
+    mapper.calibrate_fist(make_fist_landmarks())
+    mapper.calibrate_pinch(make_pinch_landmarks())
+
+    for _ in range(PINCH_CONFIRM_FRAMES):
+        features = mapper.extract_features(make_pinch_landmarks(finger_tip_index))
+    command = mapper.raw_command_from_features(features)
+
+    assert features.pinch_strengths[finger_offset] == pytest.approx(1.0)
+    assert np.count_nonzero(features.pinch_strengths) == 1
+    thumb_activation = 0.85 * 0.95
+    assert command[0] == pytest.approx(
+        G20_DEFAULT_OPEN_POSE[0]
+        + thumb_activation
+        * (G20_DEFAULT_FIST_POSE[0] - G20_DEFAULT_OPEN_POSE[0])
+    )
+    assert command[15] == pytest.approx(
+        G20_DEFAULT_OPEN_POSE[15]
+        + thumb_activation
+        * (G20_DEFAULT_FIST_POSE[15] - G20_DEFAULT_OPEN_POSE[15])
+    )
+    assert command[5] == pytest.approx(
+        PINCH_TARGET_OPPOSITION_COMMANDS[finger_offset, 0]
+    )
+    assert command[10] == pytest.approx(
+        PINCH_TARGET_OPPOSITION_COMMANDS[finger_offset, 1]
+    )
+    for offset in range(4):
+        if finger_offset in (0, 1) and offset == finger_offset:
+            assert command[1 + offset] < 110.0
+            assert command[16 + offset] < 110.0
+        else:
+            assert command[1 + offset] == G20_DEFAULT_OPEN_POSE[1 + offset]
+            assert command[16 + offset] == G20_DEFAULT_OPEN_POSE[16 + offset]
+
+
+def test_pinch_calibration_validates_operator_motion():
+    mapper = G20MediaPipeMapper()
+    open_landmarks = make_open_landmarks()
+    mapper.calibrate_open(open_landmarks)
+
+    with pytest.raises(ValueError, match="pinch calibration rejected"):
+        mapper.calibrate_pinch(open_landmarks)
+
+
+def test_pinch_calibration_tolerates_monocular_depth_error_at_visual_contact():
+    mapper = G20MediaPipeMapper()
+    mapper.calibrate_open(make_open_landmarks())
+
+    result = mapper.calibrate_pinch(
+        make_pinch_landmarks_with_monocular_depth_error()
+    )
+
+    assert result["pinch_ratio"] <= 0.35
+    assert result["pinch_reduction_ratio"] >= 0.60
+
+
+def test_pinch_target_requires_confirmation_and_releases_below_threshold():
+    mapper = G20MediaPipeMapper()
+    mapper.calibrate_open(make_open_landmarks())
+    mapper.calibrate_fist(make_fist_landmarks())
+    mapper.calibrate_pinch(make_pinch_landmarks())
+
+    for _ in range(PINCH_CONFIRM_FRAMES - 1):
+        features = mapper.extract_features(make_pinch_landmarks(12))
+        assert features.pinch_strengths.tolist() == [0.0] * 4
+
+    features = mapper.extract_features(make_pinch_landmarks(12))
+    assert features.pinch_strengths[1] == pytest.approx(1.0)
+
+    released = mapper.extract_features(make_open_landmarks())
+    assert released.pinch_strengths.tolist() == [0.0] * 4
+    assert mapper.last_pinch_target == -1
+
+
+def test_calibrated_thumb_side_motion_is_independent_from_four_finger_grasp():
+    mapper = G20MediaPipeMapper(
+        ema_alpha=1.0, max_step=255.0, max_closure=1.0
+    )
+    o6 = O6MediaPipeMapper(ema_alpha=1.0, max_step=255.0)
+    mapper.calibrate_open(make_open_landmarks())
+    mapper.calibrate_fist(make_fist_with_opposed_thumb())
+    mapper.calibrate_pinch(make_pinch_landmarks())
+
+    opposed_features = mapper.extract_features(make_fist_with_opposed_thumb())
+    opposed_g20 = mapper.raw_command_from_features(opposed_features)
+    opposed_o6 = o6.raw_command(opposed_features)
+    assert opposed_features.thumb_opposition == pytest.approx(1.0)
+    assert opposed_g20[[0, 15]].tolist() == [
+        G20_DEFAULT_FIST_POSE[0],
+        G20_DEFAULT_FIST_POSE[15],
+    ]
+    assert opposed_g20[[5, 10]].tolist() == [78.0, 112.0]
+    assert opposed_o6[0] == DEFAULT_FIST_POSE[0]
+    assert opposed_o6[1] == DEFAULT_FIST_POSE[1]
+
+    open_thumb_features = mapper.extract_features(
+        make_four_finger_fist_with_open_thumb()
+    )
+    open_thumb_g20 = mapper.raw_command_from_features(open_thumb_features)
+    open_thumb_o6 = o6.raw_command(open_thumb_features)
+    assert open_thumb_features.power_grasp == pytest.approx(1.0)
+    assert open_thumb_features.thumb_opposition == pytest.approx(0.0)
+    assert open_thumb_g20[[0, 5, 10, 15]].tolist() == [255.0] * 4
+    assert open_thumb_o6[[0, 1]].tolist() == [255.0, 179.0]

src/camera_retarget_l20/test/test_g20_mapping.py

@@ -0,0 +1,246 @@
+import numpy as np
+import pytest
+
+from camera_retarget_l20.g20_mapping import (
+    DEFAULT_FIST_POSE,
+    FLEXION_MOTOR_INDICES,
+    MOTOR_JOINT_NAMES,
+    RESERVED_MOTOR_INDICES,
+    THUMB_YAW_MIN_COMMAND,
+    G20CommandMapper,
+    G20MediaPipeMapper,
+)
+
+
+JOINT_LIMITS = {
+    "thumb_cmc_roll": (0.0, 1.4),
+    "thumb_cmc_yaw": (0.0, 1.57),
+    "thumb_cmc_pitch": (0.0, 0.84),
+    "thumb_mcp": (0.0, 1.26),
+    "index_mcp_roll": (-0.23, 0.23),
+    "index_mcp_pitch": (0.0, 1.22),
+    "index_pip": (0.0, 1.74),
+    "middle_mcp_roll": (-0.23, 0.23),
+    "middle_mcp_pitch": (0.0, 1.22),
+    "middle_pip": (0.0, 1.74),
+    "ring_mcp_roll": (-0.23, 0.23),
+    "ring_mcp_pitch": (0.0, 1.22),
+    "ring_pip": (0.0, 1.74),
+    "pinky_mcp_roll": (-0.23, 0.23),
+    "pinky_mcp_pitch": (0.0, 1.22),
+    "pinky_pip": (0.0, 1.74),
+}
+
+
+def make_mapper(**kwargs):
+    names = list(JOINT_LIMITS)
+    limits = [JOINT_LIMITS[name] for name in names]
+    return G20CommandMapper(
+        names,
+        limits,
+        ema_alpha=kwargs.pop("ema_alpha", 1.0),
+        max_step=kwargs.pop("max_step", 255.0),
+        max_closure=kwargs.pop("max_closure", 1.0),
+        **kwargs,
+    )
+
+
+def qpos_at(limit_index):
+    return np.array([limits[limit_index] for limits in JOINT_LIMITS.values()])
+
+
+def test_left_g20_lower_limit_endpoints():
+    command = make_mapper().raw_command(qpos_at(0))
+    assert command[:5].tolist() == [255.0] * 5
+    assert command[5:10].tolist() == [0.0] * 5
+    assert command[10] == 255.0
+    assert command[15:].tolist() == [255.0] * 5
+    assert command[list(RESERVED_MOTOR_INDICES)].tolist() == [255.0] * 4
+
+
+def test_left_g20_upper_limit_endpoints_and_thumb_constraint():
+    command = make_mapper().raw_command(qpos_at(1))
+    assert command[:5].tolist() == [0.0] * 5
+    assert command[5] == 0.0
+    assert command[6:10].tolist() == [255.0] * 4
+    assert command[10] == THUMB_YAW_MIN_COMMAND
+    assert command[15:].tolist() == [0.0] * 5
+
+
+def test_max_closure_only_limits_flexion_motors():
+    command = make_mapper(max_closure=0.60).raw_command(qpos_at(1))
+    for index in FLEXION_MOTOR_INDICES:
+        assert command[index] == pytest.approx(102.0)
+    assert command[5] == 0.0
+    assert command[6:10].tolist() == [255.0] * 4
+    assert command[10] == THUMB_YAW_MIN_COMMAND
+
+
+def test_filter_starts_at_sdk_open_pose_and_limits_step():
+    mapper = make_mapper(ema_alpha=1.0, max_step=7.0)
+    previous = mapper.last_command.copy()
+    command = np.asarray(mapper.map(qpos_at(1)))
+    assert np.all(np.abs(command - previous) <= 7.0)
+    assert command[list(RESERVED_MOTOR_INDICES)].tolist() == [255] * 4
+
+
+def test_missing_controlled_joint_is_rejected():
+    names = [name for name in JOINT_LIMITS if name != "thumb_cmc_pitch"]
+    with pytest.raises(ValueError, match="thumb_cmc_pitch"):
+        G20CommandMapper(names, [JOINT_LIMITS[name] for name in names])
+
+
+def test_protocol_layout_has_16_controlled_and_four_reserved_slots():
+    assert len(MOTOR_JOINT_NAMES) == 20
+    assert sum(name is not None for name in MOTOR_JOINT_NAMES) == 16
+    assert {i for i, name in enumerate(MOTOR_JOINT_NAMES) if name is None} == set(
+        RESERVED_MOTOR_INDICES
+    )
+
+
+def make_open_landmarks():
+    points = np.zeros((21, 3), dtype=float)
+    points[1:5] = np.array(
+        [[-0.20, 0.35, 0.0], [-0.40, 0.60, 0.0],
+         [-0.60, 0.85, 0.0], [-0.80, 1.10, 0.0]]
+    )
+    for root, x in zip((5, 9, 13, 17), (-0.45, -0.15, 0.15, 0.45)):
+        points[root:root + 4] = np.array(
+            [[x, 1.0, 0.0], [x, 1.7, 0.0],
+             [x, 2.3, 0.0], [x, 2.8, 0.0]]
+        )
+    return points
+
+
+def make_fist_landmarks():
+    points = make_open_landmarks()
+    thumb_mcp = points[2].copy()
+    points[3] = thumb_mcp + np.array([0.0, 0.0, 0.6])
+    points[4] = points[3] + np.array([0.0, -0.5, 0.0])
+    for root in (5, 9, 13, 17):
+        mcp = points[root].copy()
+        points[root + 1] = mcp + np.array([0.0, 0.0, 0.7])
+        points[root + 2] = points[root + 1] + np.array([0.0, -0.6, 0.0])
+        points[root + 3] = points[root + 2] + np.array([0.0, 0.0, -0.5])
+    return points
+
+
+def make_four_finger_fist_with_straight_thumb():
+    points = make_open_landmarks()
+    for root in (5, 9, 13, 17):
+        mcp = points[root].copy()
+        points[root + 1] = mcp + np.array([0.0, 0.0, 0.7])
+        points[root + 2] = points[root + 1] + np.array([0.0, -0.6, 0.0])
+        points[root + 3] = points[root + 2] + np.array([0.0, 0.0, -0.5])
+    return points
+
+
+def test_mediapipe_mapper_requires_explicit_open_calibration():
+    mapper = G20MediaPipeMapper()
+    with pytest.raises(RuntimeError, match="calibration"):
+        mapper.raw_command(make_open_landmarks())
+
+
+def test_mediapipe_open_pose_maps_to_sdk_open_command():
+    mapper = G20MediaPipeMapper(
+        ema_alpha=1.0, max_step=255.0, max_closure=0.60
+    )
+    landmarks = make_open_landmarks()
+    mapper.calibrate_open(landmarks)
+    assert mapper.map_landmarks(landmarks) == mapper.open_pose.astype(int).tolist()
+
+
+def test_mediapipe_fist_closes_root_and_tip_flexion_channels():
+    mapper = G20MediaPipeMapper(
+        ema_alpha=1.0, max_step=255.0, max_closure=0.60
+    )
+    mapper.calibrate_open(make_open_landmarks())
+    command = mapper.raw_command(make_fist_landmarks())
+    for index in (*range(1, 5), *range(16, 20)):
+        expected = mapper.open_pose[index] + 0.60 * (
+            DEFAULT_FIST_POSE[index] - mapper.open_pose[index]
+        )
+        assert command[index] == pytest.approx(expected)
+    assert command[0] == mapper.open_pose[0]
+    expected_thumb_tip = mapper.open_pose[15] + 0.60 * (
+        DEFAULT_FIST_POSE[15] - mapper.open_pose[15]
+    )
+    assert command[15] == pytest.approx(expected_thumb_tip)
+    assert command[5] == mapper.open_pose[5]
+    assert command[6:10].tolist() == mapper.open_pose[6:10].tolist()
+    assert THUMB_YAW_MIN_COMMAND <= command[10] <= mapper.open_pose[10]
+
+
+def test_mediapipe_splay_moves_matching_left_g20_channels():
+    mapper = G20MediaPipeMapper(
+        ema_alpha=1.0,
+        max_step=255.0,
+        max_closure=0.85,
+        splay_gain=1.0,
+        max_splay_delta=90.0,
+    )
+    open_landmarks = make_open_landmarks()
+    mapper.calibrate_open(open_landmarks)
+
+    spread = open_landmarks.copy()
+    spread[6, 0] -= 0.20
+    spread[18, 0] += 0.20
+    command = mapper.raw_command(spread)
+
+    assert command[6] > mapper.open_pose[6]
+    assert command[9] < mapper.open_pose[9]
+    assert command[7] == pytest.approx(mapper.open_pose[7])
+    assert command[8] == pytest.approx(mapper.open_pose[8])
+
+
+def test_mediapipe_thumb_adduction_controls_g20_thumb_side_channel():
+    mapper = G20MediaPipeMapper(
+        ema_alpha=1.0,
+        max_step=255.0,
+        max_closure=0.95,
+        thumb_abduction_min_command=20.0,
+    )
+    open_landmarks = make_open_landmarks()
+    mapper.calibrate_open(open_landmarks)
+
+    adducted = open_landmarks.copy()
+    adducted[2] = adducted[1] + np.array([0.0, 0.40, 0.0])
+    command = mapper.raw_command(adducted)
+
+    assert command[5] < 100.0
+    assert command[10] < mapper.open_pose[10]
+    assert mapper.last_thumb_abduction > 0.5
+
+
+def test_four_finger_grasp_keeps_a_straight_thumb_open():
+    mapper = G20MediaPipeMapper(
+        ema_alpha=1.0,
+        max_step=255.0,
+        max_closure=0.95,
+        thumb_abduction_min_command=20.0,
+    )
+    mapper.calibrate_open(make_open_landmarks())
+    command = mapper.raw_command(make_four_finger_fist_with_straight_thumb())
+
+    assert mapper.last_power_grasp == pytest.approx(1.0)
+    assert command[0] == mapper.open_pose[0]
+    assert command[5] == mapper.open_pose[5]
+    assert command[10] == mapper.open_pose[10]
+    assert command[15] == mapper.open_pose[15]
+    assert np.all(command[1:5] < 100.0)
+    assert np.all(command[16:20] < 100.0)
+
+
+def test_mediapipe_full_closure_reaches_each_measured_g20_fist_endpoint():
+    mapper = G20MediaPipeMapper(
+        ema_alpha=1.0,
+        max_step=255.0,
+        max_closure=1.0,
+    )
+    mapper.calibrate_open(make_open_landmarks())
+    features = mapper.extract_features(make_fist_landmarks())
+    features.closure[:] = 1.0
+    command = mapper.raw_command_from_features(features)
+
+    for index in FLEXION_MOTOR_INDICES:
+        assert command[index] == DEFAULT_FIST_POSE[index]

src/camera_retarget_l20/test/test_l20_mapping.py

@@ -0,0 +1,93 @@
+import numpy as np
+import pytest
+
+from camera_retarget_l20.l20_mapping import (
+    FLEXION_MOTOR_INDICES,
+    MOTOR_JOINT_NAMES,
+    RESERVED_MOTOR_INDICES,
+    L20CommandMapper,
+)
+
+
+JOINT_LIMITS = {
+    "thumb_joint0": (-0.297, 0.683),
+    "thumb_joint1": (0.122, 1.78),
+    "thumb_joint2": (0.0, 0.87),
+    "thumb_joint3": (0.0, 1.29),
+    "thumb_joint4": (0.0, 1.29),
+    "index_joint0": (-0.26, 0.26),
+    "index_joint1": (0.0, 1.4),
+    "index_joint2": (0.0, 1.08),
+    "index_joint3": (0.0, 1.15),
+    "middle_joint0": (-0.26, 0.26),
+    "middle_joint1": (0.0, 1.4),
+    "middle_joint2": (0.0, 1.08),
+    "middle_joint3": (0.0, 1.15),
+    "ring_joint0": (-0.26, 0.26),
+    "ring_joint1": (0.0, 1.4),
+    "ring_joint2": (0.0, 1.08),
+    "ring_joint3": (0.0, 1.15),
+    "little_joint0": (-0.26, 0.26),
+    "little_joint1": (0.0, 1.4),
+    "little_joint2": (0.0, 1.08),
+    "little_joint3": (0.0, 1.15),
+}
+
+
+def make_mapper(**kwargs):
+    names = list(JOINT_LIMITS)
+    limits = [JOINT_LIMITS[name] for name in names]
+    return L20CommandMapper(
+        names,
+        limits,
+        ema_alpha=kwargs.pop("ema_alpha", 1.0),
+        max_step=kwargs.pop("max_step", 255.0),
+        max_closure=kwargs.pop("max_closure", 1.0),
+        **kwargs,
+    )
+
+
+def qpos_at(limit_index):
+    return np.array([limits[limit_index] for limits in JOINT_LIMITS.values()])
+
+
+def test_lower_limits_map_to_open_end_of_protocol_range():
+    mapper = make_mapper()
+    command = mapper.raw_command(qpos_at(0))
+    for motor_index, name in enumerate(MOTOR_JOINT_NAMES):
+        assert command[motor_index] == 255.0
+        if name is None:
+            assert motor_index in RESERVED_MOTOR_INDICES
+
+
+def test_upper_limits_map_to_zero_except_reserved_slots():
+    mapper = make_mapper()
+    command = mapper.raw_command(qpos_at(1))
+    for motor_index, name in enumerate(MOTOR_JOINT_NAMES):
+        expected = 255.0 if name is None else 0.0
+        assert command[motor_index] == expected
+
+
+def test_max_closure_only_limits_flexion_motors():
+    mapper = make_mapper(max_closure=0.60)
+    command = mapper.raw_command(qpos_at(1))
+    for index in FLEXION_MOTOR_INDICES:
+        assert command[index] == pytest.approx(102.0)
+    for index in (5, 6, 7, 8, 9, 10):
+        assert command[index] == 0.0
+
+
+def test_filter_limits_each_frame_step():
+    mapper = make_mapper(ema_alpha=1.0, max_step=7.0)
+    command = mapper.map(qpos_at(1))
+    for index in FLEXION_MOTOR_INDICES:
+        assert command[index] == 248
+    for index in RESERVED_MOTOR_INDICES:
+        assert command[index] == 255
+
+
+def test_missing_controlled_joint_is_rejected():
+    names = [name for name in JOINT_LIMITS if name != "thumb_joint2"]
+    with pytest.raises(ValueError, match="thumb_joint2"):
+        L20CommandMapper(names, [JOINT_LIMITS[name] for name in names])
+

src/camera_retarget_l20/test/test_model_profiles.py

@@ -0,0 +1,215 @@
+from dataclasses import replace
+from pathlib import Path
+
+import numpy as np
+
+from camera_retarget_l20.hand_features import HandFeatures
+from camera_retarget_l20.model_profiles import (
+    ProfiledCommandMapper,
+    load_model_profiles,
+)
+
+
+PROFILE_PATH = (
+    Path(__file__).resolve().parents[1] / "config" / "direct_mapping_profiles.yml"
+)
+
+
+def features(*, closure=None, pinch_target=None, power_grasp=0.0):
+    strengths = np.zeros(4)
+    if pinch_target is not None:
+        strengths[pinch_target] = 1.0
+    return HandFeatures(
+        closure=np.zeros(10) if closure is None else np.asarray(closure),
+        pinch=float(np.max(strengths)),
+        pinch_strengths=strengths,
+        splay_delta=np.zeros(4),
+        splay_confidence=np.ones(4),
+        thumb_splay_delta=0.0,
+        thumb_abduction=0.0,
+        thumb_opposition=float(np.max(strengths)),
+        power_grasp=power_grasp,
+    )
+
+
+def test_profile_file_defines_valid_o6_and_g20_protocols():
+    profiles = load_model_profiles(PROFILE_PATH)
+
+    assert set(profiles) == {"O6", "G20"}
+    assert profiles["O6"].command_length == 6
+    assert profiles["G20"].command_length == 20
+    assert profiles["G20"].fixed_channels == (
+        (11, 255.0),
+        (12, 255.0),
+        (13, 255.0),
+        (14, 255.0),
+    )
+
+
+def test_g20_profile_reaches_all_measured_pinch_side_endpoints():
+    profile = load_model_profiles(PROFILE_PATH)["G20"].with_runtime_overrides(
+        ema_alpha=1.0, max_step=255.0
+    )
+    mapper = ProfiledCommandMapper(profile)
+
+    for target, expected in enumerate(((167, 184), (122, 148), (78, 112), (50, 90))):
+        command = mapper.raw_command(features(pinch_target=target))
+        assert command[[5, 10]].tolist() == list(expected)
+        assert command[11:15].tolist() == [255.0] * 4
+
+
+def test_thumb_sweep_does_not_curl_an_open_pinch_target():
+    profiles = load_model_profiles(PROFILE_PATH)
+
+    for model in ("O6", "G20"):
+        profile = profiles[model].with_runtime_overrides(
+            ema_alpha=1.0, max_step=255.0
+        )
+        mapper = ProfiledCommandMapper(profile)
+        command = mapper.raw_command(features(pinch_target=3))
+        pinky_indices = [
+            channel.command_index
+            for channel in profile.flexion_channels
+            if channel.pinch_role == "pinky"
+        ]
+        assert command[pinky_indices].tolist() == [
+            profile.open_pose[index] for index in pinky_indices
+        ]
+
+
+def test_pinch_assist_activates_when_target_finger_also_flexes():
+    profiles = load_model_profiles(PROFILE_PATH)
+    closure = np.zeros(10)
+    closure[[4, 9]] = 0.5
+
+    for model in ("O6", "G20"):
+        profile = profiles[model].with_runtime_overrides(
+            ema_alpha=1.0, max_step=255.0
+        )
+        mapper = ProfiledCommandMapper(profile)
+        command = mapper.raw_command(
+            features(closure=closure, pinch_target=3)
+        )
+        for channel in profile.flexion_channels:
+            if channel.pinch_role != "pinky":
+                continue
+            index = channel.command_index
+            expected = profile.open_pose[index] + profile.pinch_finger_closure * (
+                profile.fist_pose[index] - profile.open_pose[index]
+            )
+            assert command[index] == expected
+
+
+def test_index_and_middle_contact_can_assist_a_straight_target_finger():
+    profiles = load_model_profiles(PROFILE_PATH)
+
+    for model in ("O6", "G20"):
+        profile = profiles[model].with_runtime_overrides(
+            ema_alpha=1.0, max_step=255.0
+        )
+        mapper = ProfiledCommandMapper(profile)
+        for target in (0, 1):
+            command = mapper.raw_command(features(pinch_target=target))
+            role = ("index", "middle")[target]
+            for channel in profile.flexion_channels:
+                if channel.pinch_role != role:
+                    continue
+                index = channel.command_index
+                expected = profile.open_pose[index] + profile.pinch_finger_closure * (
+                    profile.fist_pose[index] - profile.open_pose[index]
+                )
+                assert command[index] == expected
+
+
+def test_o6_profile_uses_twenty_eighty_thumb_fusion():
+    profile = load_model_profiles(PROFILE_PATH)["O6"]
+    mapper = ProfiledCommandMapper(profile)
+    closure = np.zeros(10)
+    closure[0] = 1.0
+
+    command = mapper.raw_command(features(closure=closure))
+
+    assert command[0] == 255.0 + 0.2 * (0.0 - 255.0)
+
+
+def test_command_deadband_holds_small_jitter_but_allows_real_motion():
+    profile = load_model_profiles(PROFILE_PATH)["G20"].with_runtime_overrides(
+        ema_alpha=1.0,
+        max_step=255.0,
+        command_deadband=2.0,
+    )
+    mapper = ProfiledCommandMapper(profile)
+    closure = np.zeros(10)
+
+    closure[1] = 1.5 / 255.0
+    held = mapper.map_features(features(closure=closure))
+    assert held[1] == 255
+
+    closure[1] = 4.0 / 255.0
+    moved = mapper.map_features(features(closure=closure))
+    assert moved[1] == 251
+
+
+def test_power_grasp_suppresses_precision_pinch_and_uses_fist_endpoint():
+    profile = load_model_profiles(PROFILE_PATH)["G20"]
+    mapper = ProfiledCommandMapper(profile)
+
+    command = mapper.raw_command(
+        features(closure=np.ones(10), pinch_target=0, power_grasp=0.9)
+    )
+
+    assert command.tolist() == list(profile.fist_pose)
+    assert mapper.last_pinch_blend == 0.0
+
+
+def test_four_finger_power_grasp_does_not_force_thumb_for_either_model():
+    profiles = load_model_profiles(PROFILE_PATH)
+    closure = np.zeros(10)
+    closure[[1, 2, 3, 4, 6, 7, 8, 9]] = 1.0
+    hand_features = features(closure=closure, power_grasp=1.0)
+
+    for model in ("O6", "G20"):
+        profile = profiles[model]
+        mapper = ProfiledCommandMapper(profile)
+        command = mapper.raw_command(hand_features)
+        thumb_indices = [
+            channel.command_index
+            for channel in profile.flexion_channels
+            if channel.pinch_role == "thumb"
+        ]
+        assert command[thumb_indices].tolist() == [
+            profile.open_pose[index] for index in thumb_indices
+        ]
+        assert command[list(profile.side_command_indices)].tolist() == [
+            profile.open_pose[index] for index in profile.side_command_indices
+        ]
+        for channel in profile.flexion_channels:
+            if channel.pinch_role != "thumb":
+                assert command[channel.command_index] == profile.fist_pose[
+                    channel.command_index
+                ]
+
+
+def test_thumb_intent_completes_thumb_during_power_grasp_for_either_model():
+    profiles = load_model_profiles(PROFILE_PATH)
+    closure = np.zeros(10)
+    closure[[1, 2, 3, 4, 6, 7, 8, 9]] = 1.0
+    hand_features = replace(
+        features(closure=closure, power_grasp=1.0), thumb_opposition=1.0
+    )
+
+    for model in ("O6", "G20"):
+        profile = profiles[model]
+        mapper = ProfiledCommandMapper(profile)
+        command = mapper.raw_command(hand_features)
+        thumb_indices = [
+            channel.command_index
+            for channel in profile.flexion_channels
+            if channel.pinch_role == "thumb"
+        ]
+        assert command[thumb_indices].tolist() == [
+            profile.fist_pose[index] for index in thumb_indices
+        ]
+        assert command[list(profile.side_command_indices)].tolist() == list(
+            profile.power_grasp_side_commands
+        )

src/camera_retarget_l20/test/test_o6_mapping.py

@@ -0,0 +1,118 @@
+import numpy as np
+
+from camera_retarget_l20.hand_features import HandFeatures
+from camera_retarget_l20.o6_mapping import (
+    DEFAULT_FIST_POSE,
+    DEFAULT_OPEN_POSE,
+    O6_COMMAND_NAMES,
+    O6MediaPipeMapper,
+    PINCH_TARGET_YAW_COMMANDS,
+    THUMB_ABDUCTED_COMMAND,
+)
+
+
+def make_features(closure=None, **kwargs):
+    return HandFeatures(
+        closure=np.zeros(10) if closure is None else np.asarray(closure),
+        pinch=kwargs.get("pinch", 0.0),
+        pinch_strengths=np.asarray(
+            kwargs.get("pinch_strengths", np.zeros(4)), dtype=float
+        ),
+        splay_delta=np.zeros(4),
+        splay_confidence=np.ones(4),
+        thumb_splay_delta=0.0,
+        thumb_abduction=kwargs.get("thumb_abduction", 0.0),
+        thumb_opposition=kwargs.get("thumb_opposition", 0.0),
+        power_grasp=kwargs.get("power_grasp", 0.0),
+    )
+
+
+def test_o6_protocol_order_and_open_endpoint():
+    mapper = O6MediaPipeMapper(ema_alpha=1.0, max_step=255.0)
+    assert O6_COMMAND_NAMES == (
+        "thumb_cmc_pitch",
+        "thumb_cmc_yaw",
+        "index_mcp_pitch",
+        "middle_mcp_pitch",
+        "ring_mcp_pitch",
+        "pinky_mcp_pitch",
+    )
+    assert mapper.map_features(make_features()) == list(DEFAULT_OPEN_POSE)
+
+
+def test_full_fist_reaches_configured_full_range_endpoint():
+    mapper = O6MediaPipeMapper(ema_alpha=1.0, max_step=255.0)
+    features = make_features(
+        np.ones(10),
+        thumb_opposition=1.0,
+        power_grasp=1.0,
+    )
+    assert mapper.map_features(features) == list(DEFAULT_FIST_POSE)
+
+
+def test_single_o6_finger_combines_root_and_tip_flexion():
+    mapper = O6MediaPipeMapper(ema_alpha=1.0, max_step=255.0)
+    closure = np.zeros(10)
+    closure[1] = 1.0
+    closure[6] = 0.5
+    command = mapper.raw_command(make_features(closure))
+    expected_activation = 0.35 + 0.65 * 0.5
+    expected = 255.0 + expected_activation * (0.0 - 255.0)
+    assert command[2] == expected
+    assert command[3:].tolist() == [255.0, 255.0, 255.0]
+
+
+def test_o6_filter_limits_each_frame_step():
+    mapper = O6MediaPipeMapper(ema_alpha=1.0, max_step=8.0)
+    command = mapper.map_features(
+        make_features(
+            np.ones(10),
+            thumb_opposition=1.0,
+            power_grasp=1.0,
+        )
+    )
+    assert all(abs(value - open_value) <= 8 for value, open_value in zip(command, DEFAULT_OPEN_POSE))
+
+
+def test_o6_thumb_yaw_uses_separate_abduction_and_opposition_directions():
+    mapper = O6MediaPipeMapper(ema_alpha=1.0, max_step=255.0)
+    abducted = mapper.raw_command(make_features(thumb_abduction=1.0))
+    opposed = mapper.raw_command(make_features(thumb_opposition=1.0))
+
+    assert abducted[1] == THUMB_ABDUCTED_COMMAND
+    assert opposed[1] == DEFAULT_FIST_POSE[1]
+
+
+def test_o6_pinch_couples_thumb_and_selected_finger_flexion():
+    mapper = O6MediaPipeMapper(
+        ema_alpha=1.0,
+        max_step=255.0,
+        pinch_thumb_closure=0.85,
+        pinch_finger_closure=0.65,
+    )
+
+    for target_offset in range(4):
+        strengths = np.zeros(4)
+        strengths[target_offset] = 1.0
+        closure = np.zeros(10)
+        closure[1 + target_offset] = 0.5
+        closure[6 + target_offset] = 0.5
+        command = mapper.raw_command(
+            make_features(
+                closure,
+                pinch=1.0,
+                pinch_strengths=strengths,
+                thumb_opposition=1.0,
+            )
+        )
+
+        assert command[0] == 255.0 + 0.85 * (DEFAULT_FIST_POSE[0] - 255.0)
+        assert command[1] == PINCH_TARGET_YAW_COMMANDS[target_offset]
+        for finger_offset in range(4):
+            expected = (
+                255.0
+                + 0.65 * (DEFAULT_FIST_POSE[2 + finger_offset] - 255.0)
+                if finger_offset == target_offset
+                else 255.0
+            )
+            assert command[2 + finger_offset] == expected

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py

@@ -10,6 +10,8 @@ import time
 import numpy as np
 from rclpy.node import Node                      # ROS2 节点类
 from rclpy.clock import Clock
+from rclpy.parameter import Parameter
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
 from std_msgs.msg import String, Header, Float32MultiArray
 from sensor_msgs.msg import JointState, PointCloud2, PointField
 import time, json, threading
@@ -18,6 +20,28 @@ from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
 from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
 
 
+COMMAND_QOS = QoSProfile(
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,
+    reliability=ReliabilityPolicy.RELIABLE,
+)
+
+
+def command_changed(previous, current):
+    """Return whether a non-empty command differs from the applied command."""
+    values = list(current)
+    if not values:
+        return False
+    if previous is None or len(previous) != len(values):
+        return True
+    return any(float(old) != float(new) for old, new in zip(previous, values))
+
+
+def state_poll_due(last_poll_time, now, poll_period):
+    """Keep slow CAN state reads off the latency-sensitive command path."""
+    return last_poll_time is None or now >= last_poll_time + poll_period
+
+
 class LinkerHand(Node):
     def __init__(self, name):
         super().__init__(name)
@@ -26,7 +50,19 @@ class LinkerHand(Node):
         self.declare_parameter('hand_joint', 'L6')
         self.declare_parameter('is_touch', False)
         self.declare_parameter('can', 'can0')
-        self.declare_parameter('modbus', "None")        
+        self.declare_parameter('modbus', "None")
+        # -1 keeps the model's original startup speed. Camera teleoperation can
+        # set this to a conservative value before the startup pose is sent.
+        self.declare_parameter('startup_speed', -1)
+        # Empty keeps the legacy absolute topics/startup pose. A prefix lets
+        # two same-side hands coexist without receiving each other's commands.
+        self.declare_parameter('topic_prefix', '')
+        self.declare_parameter('startup_pose', Parameter.Type.INTEGER_ARRAY)
+        # Hardware state reads are synchronous CAN transactions.  Keeping them
+        # below the command rate prevents G20's five-frame reads from starving
+        # incoming position commands.
+        self.declare_parameter('state_poll_rate', 60.0)
+        self.declare_parameter('velocity_poll_rate', 60.0)
 
         # ros时间获取
         self.stamp_clock = Clock()
@@ -36,12 +72,41 @@ class LinkerHand(Node):
         self.is_touch = self.get_parameter('is_touch').value
         self.can = self.get_parameter('can').value
         self.modbus = self.get_parameter('modbus').value
+        self.startup_speed = int(self.get_parameter('startup_speed').value)
+        if self.startup_speed < -1 or self.startup_speed > 255:
+            raise ValueError('startup_speed must be -1 or in the range [0, 255]')
+        self.topic_prefix = self.normalize_topic_prefix(
+            self.get_parameter('topic_prefix').value
+        )
+        self.state_poll_rate = float(
+            self.get_parameter('state_poll_rate').value
+        )
+        if self.state_poll_rate <= 0.0:
+            raise ValueError('state_poll_rate must be greater than zero')
+        self.state_poll_period = 1.0 / self.state_poll_rate
+        self.last_state_poll_time = None
+        self.velocity_poll_rate = float(
+            self.get_parameter('velocity_poll_rate').value
+        )
+        if self.velocity_poll_rate <= 0.0:
+            raise ValueError('velocity_poll_rate must be greater than zero')
+        self.velocity_poll_period = 1.0 / self.velocity_poll_rate
+        self.last_velocity_poll_time = None
+        configured_startup_pose = self.get_parameter_or(
+            'startup_pose',
+            Parameter('startup_pose', Parameter.Type.INTEGER_ARRAY, []),
+        ).value
+        self.startup_pose = [int(value) for value in configured_startup_pose]
+        if any(value < 0 or value > 255 for value in self.startup_pose):
+            raise ValueError('startup_pose values must be in the range [0, 255]')
         self.sdk_v = 2
         self.sleep_time = 0.005
         self.cmd_lock = False
         self.last_hand_post_cmd = None # 最新手指位置命令
         self.last_hand_vel_cmd = None # 最新手指速度命令
         self.last_hand_eff_cmd = None # 最新手指力矩命令
+        self.applied_hand_post_cmd = None
+        self.applied_hand_vel_cmd = None
 
         self.last_hand_state = [-1] * 10
         self.last_hand_vel = [-1] * 10
@@ -85,7 +150,9 @@ class LinkerHand(Node):
         self.touch_type = -1
         self.hz = 1.0/60.0
 
-        self.hand_setting_sub = self.create_subscription(String,'/cb_hand_setting_cmd', self.hand_setting_cb, 10)
+        self.hand_setting_sub = self.create_subscription(
+            String, self.topic('/cb_hand_setting_cmd'), self.hand_setting_cb, 10
+        )
         self._init_hand()
         time.sleep(1)
         self.run_count = 0 # 计数器，用于记录运行次数
@@ -94,26 +161,45 @@ class LinkerHand(Node):
         self.thread_pub_state.daemon = True
         self.thread_pub_state.start()
 
+    @staticmethod
+    def normalize_topic_prefix(prefix):
+        prefix = str(prefix).strip()
+        if not prefix or prefix == '/':
+            return ''
+        if not prefix.startswith('/'):
+            prefix = '/' + prefix
+        return prefix.rstrip('/')
+
+    def topic(self, absolute_topic):
+        if not absolute_topic.startswith('/'):
+            raise ValueError('base topic must be absolute')
+        return self.topic_prefix + absolute_topic
+
     def _init_hand(self):
         self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
         time.sleep(0.1)
         self.touch_type = self.api.get_touch_type()
-        self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+        self.hand_cmd_sub = self.create_subscription(
-        self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+            JointState,
-        self.hand_info_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_info', 10)
+            self.topic(f'/cb_{self.hand_type}_hand_control_cmd'),
+            self.hand_control_cb,
+            COMMAND_QOS,
+        )
+        self.hand_state_pub = self.create_publisher(JointState, self.topic(f'/cb_{self.hand_type}_hand_state'),10)
+        self.hand_info_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_info'), 10)
         if self.is_touch == True:
             if self.modbus != "None":
-                self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+                self.matrix_touch_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch'), 10)
-                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_pc'), 10)
-                self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_mass'), 10)
             elif self.touch_type > 1:
                 ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
-                self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+                self.matrix_touch_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch'), 10)
-                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+                self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_pc'), 10)
-                self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+                self.matrix_touch_mass_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_mass'), 10)
             elif self.touch_type != -1 and self.modbus == "None":
                 ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
-                self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+                self.touch_pub = self.create_publisher(Float32MultiArray, self.topic(f'/cb_{self.hand_type}_hand_force'), 10)
             else:
                 ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
                 self.is_touch = False
@@ -138,10 +224,26 @@ class LinkerHand(Node):
             speed = [200, 250, 250, 250, 250, 250, 250, 250, 250, 250]
         elif self.hand_joint == "L20":
             pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255]
+        elif self.hand_joint == "G20":
+            # G20 uses a different CAN protocol from L20 even though both
+            # expose a 20-value ROS command. This is the calibrated startup
+            # pose also used by linker_hand_advanced_g20.
+            pose = [255,255,255,255,255,255,193,148,105,42,245,255,255,255,255,255,255,255,255,255]
+            torque = [255] * 5
+            speed = [255] * 5
         elif self.hand_joint == "L21":
             pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
         elif self.hand_joint == "L25":
             pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+        if self.startup_pose:
+            if pose is None or len(self.startup_pose) != len(pose):
+                expected = 0 if pose is None else len(pose)
+                raise ValueError(
+                    f'startup_pose for {self.hand_joint} must contain {expected} values'
+                )
+            pose = list(self.startup_pose)
+        if self.startup_speed >= 0:
+            speed = [self.startup_speed] * len(speed)
         if pose is not None:
             for i in range(1): 
                 self.api.set_speed(speed=speed)
@@ -151,58 +253,96 @@ class LinkerHand(Node):
                 self.api.finger_move(pose=pose)
                 time.sleep(0.1)
 
-    def list_check(self,pose):
-        if isinstance(pose, list) == False:
-            return False
-        if len(self.last_hand_post_cmd) != len(pose):
-            return False
-        return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
-
     def hand_control_cb(self, msg):
-        if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+        # The hardware can be slower than the camera.  Always replace a
-            self.last_hand_post_cmd = msg.position
+        # pending command with the newest sample and never replay an already
-        if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+        # applied sample; this prevents latency from accumulating in software.
-            self.last_hand_vel_cmd = msg.velocity
+        position = list(msg.position)
-        if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+        if position:
-            self.last_hand_eff_cmd = msg.effort
+            self.last_hand_post_cmd = (
+                position
+                if command_changed(self.applied_hand_post_cmd, position)
+                else None
+            )
+
+        velocity = list(msg.velocity)
+        if velocity:
+            self.last_hand_vel_cmd = (
+                velocity
+                if command_changed(self.applied_hand_vel_cmd, velocity)
+                else None
+            )
+
+        effort = list(msg.effort)
+        if effort:
+            self.last_hand_eff_cmd = effort
+
+    def _apply_pending_commands(self):
+        if self.cmd_lock:
+            return
+
+        if self.last_hand_post_cmd is not None:
+            pose = list(self.last_hand_post_cmd)
+            self.api.finger_move(pose=pose)
+            self.applied_hand_post_cmd = pose
+            self.last_hand_post_cmd = None
+
+        if self.last_hand_vel_cmd is not None:
+            vel = list(self.last_hand_vel_cmd)
+            if not all(x == 0 for x in vel):
+                if (str(self.hand_joint).upper() == "O6" or str(self.hand_joint).upper() == "L6" or str(self.hand_joint).upper() == "L6P") and len(vel) == 6:
+                    speed = vel
+                    self.api.set_joint_speed(speed=speed)
+                elif self.hand_joint == "L7" and len(vel) == 7:
+                    speed = vel
+                    self.api.set_joint_speed(speed=speed)
+                elif self.hand_joint == "L10" and len(vel) == 10:
+                    speed = [vel[0],vel[2],vel[3],vel[4],vel[5]]
+                    self.api.set_joint_speed(speed=speed)
+                elif self.hand_joint == "L20" and len(vel) == 20:
+                    speed = [vel[10],vel[1],vel[2],vel[3],vel[4]]
+                    self.api.set_joint_speed(speed=speed)
+                elif self.hand_joint == "G20" and len(vel) == 20:
+                    # G20 speed is configured per finger, not per one of
+                    # the 20 exposed position slots.
+                    speed = [vel[0],vel[1],vel[2],vel[3],vel[4]]
+                    self.api.set_joint_speed(speed=speed)
+                elif self.hand_joint == "L21" and len(vel) == 25:
+                    speed = vel
+                    self.api.set_joint_speed(speed=speed)
+                elif self.hand_joint == "L25" and len(vel) == 25:
+                    speed = vel
+                    self.api.set_joint_speed(speed=speed)
+            self.applied_hand_vel_cmd = vel
+            self.last_hand_vel_cmd = None
+
+    def _poll_state_if_due(self):
+        if self.hand_state_pub.get_subscription_count() < 1:
+            return
+        now = time.monotonic()
+        if not state_poll_due(
+            self.last_state_poll_time, now, self.state_poll_period
+        ):
+            return
+        # Record the start time so a slow G20 read cannot immediately trigger
+        # another read on the following timer callback.
+        self.last_state_poll_time = now
+        self.last_hand_state = self.api.get_state()
+        time.sleep(0.003)
+        if state_poll_due(
+            self.last_velocity_poll_time, now, self.velocity_poll_period
+        ):
+            self.last_velocity_poll_time = now
+            self.last_hand_vel = self.api.get_joint_speed()
+            time.sleep(0.002)
 
     def run(self):
         if self.sdk_v == 1:
             self.sleep_time = 0.009
-        if self.hand_state_pub.get_subscription_count() > 0:
+        # Position commands have priority over synchronous state reads.
-            # 优先获取手指状态并且发布
+        self._apply_pending_commands()
-            self.last_hand_state = self.api.get_state()
+        self._poll_state_if_due()
-            time.sleep(0.003)
-            self.last_hand_vel = self.api.get_joint_speed()
-            time.sleep(0.002)
         if self.cmd_lock == False:
-            if self.last_hand_post_cmd != None:
-                self.api.finger_move(pose=self.last_hand_post_cmd)
-                self.last_hand_post_cmd = None
-            if self.last_hand_vel_cmd != None:
-                vel = list(self.last_hand_vel_cmd)
-                if all(x == 0 for x in vel):
-                    pass
-                else:
-                    if (str(self.hand_joint).upper() == "O6" or str(self.hand_joint).upper() == "L6" or str(self.hand_joint).upper() == "L6P") and len(vel) == 6:
-                        speed = vel
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L7" and len(vel) == 7:
-                        speed = vel
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L10" and len(vel) == 10:
-                        speed = [vel[0],vel[2],vel[3],vel[4],vel[5]]
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L20" and len(vel) == 20:
-                        speed = [vel[10],vel[1],vel[2],vel[3],vel[4]]
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L21" and len(vel) == 25:
-                        speed = vel
-                        self.api.set_joint_speed(speed=speed)
-                    elif self.hand_joint == "L25" and len(vel) == 25:
-                        speed = vel
-                        self.api.set_joint_speed(speed=speed)
-                self.last_hand_vel_cmd = None
             time.sleep(0.003)
             if self.run_count == 3 and self.is_touch == True and self.touch_type == 1 and self.modbus == "None" and self.touch_pub.get_subscription_count() > 0:
                 """单点式压力传感器"""

src/linker_hand_ros2_sdk/test/test_topic_prefix.py

@@ -0,0 +1,36 @@
+from rclpy.qos import HistoryPolicy
+
+from linker_hand_ros2_sdk.linker_hand import (
+    COMMAND_QOS,
+    LinkerHand,
+    command_changed,
+    state_poll_due,
+)
+
+
+def test_empty_topic_prefix_preserves_legacy_topics():
+    assert LinkerHand.normalize_topic_prefix("") == ""
+    assert LinkerHand.normalize_topic_prefix("/") == ""
+
+
+def test_topic_prefix_is_absolute_and_has_no_trailing_slash():
+    assert LinkerHand.normalize_topic_prefix("o6") == "/o6"
+    assert LinkerHand.normalize_topic_prefix("/g20/") == "/g20"
+
+
+def test_command_qos_keeps_only_the_latest_sample():
+    assert COMMAND_QOS.history == HistoryPolicy.KEEP_LAST
+    assert COMMAND_QOS.depth == 1
+
+
+def test_identical_commands_are_not_reapplied():
+    assert command_changed(None, [60, 60])
+    assert not command_changed([60, 60], [60, 60])
+    assert command_changed([60, 60], [60, 61])
+    assert not command_changed([60, 60], [])
+
+
+def test_state_polling_is_throttled_without_missing_deadline():
+    assert state_poll_due(None, 10.0, 0.1)
+    assert not state_poll_due(10.0, 10.09, 0.1)
+    assert state_poll_due(10.0, 10.1, 0.1)