diff --git a/.gitignore b/.gitignore index 9080b3a..86366f0 100644 --- a/.gitignore +++ b/.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_*/ diff --git a/src/camera_retarget_l20/QUICK_START_O6_G20_ZH.md b/src/camera_retarget_l20/QUICK_START_O6_G20_ZH.md new file mode 100644 index 0000000..e64fb88 --- /dev/null +++ b/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`。标定时姿势越标准、越稳定，控制效果越好。 diff --git a/src/camera_retarget_l20/README.md b/src/camera_retarget_l20/README.md new file mode 100644 index 0000000..8fa8d0c --- /dev/null +++ b/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 +``` diff --git a/src/camera_retarget_l20/USAGE_O6_G20_ZH.md b/src/camera_retarget_l20/USAGE_O6_G20_ZH.md new file mode 100644 index 0000000..e9212c9 --- /dev/null +++ b/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 +``` + +这会更稳定，但会增加少量跟随延迟。 diff --git a/src/camera_retarget_l20/camera_retarget_l20/__init__.py b/src/camera_retarget_l20/camera_retarget_l20/__init__.py new file mode 100644 index 0000000..71e4d45 --- /dev/null +++ b/src/camera_retarget_l20/camera_retarget_l20/__init__.py @@ -0,0 +1,2 @@ +"""Camera-based retargeting bridge for the LinkerHand L20.""" + diff --git a/src/camera_retarget_l20/camera_retarget_l20/calibration.py b/src/camera_retarget_l20/camera_retarget_l20/calibration.py new file mode 100644 index 0000000..0d65c25 --- /dev/null +++ b/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 diff --git a/src/camera_retarget_l20/camera_retarget_l20/g20_mapping.py b/src/camera_retarget_l20/camera_retarget_l20/g20_mapping.py new file mode 100644 index 0000000..84970d3 --- /dev/null +++ b/src/camera_retarget_l20/camera_retarget_l20/g20_mapping.py @@ -0,0 +1,1050 @@ +"""Convert G20 URDF joint positions to the LinkerHand 20-value protocol.""" + +from __future__ import annotations + +from collections.abc import Sequence +from dataclasses import replace + +import numpy as np + +from .calibration import RawHandObservation +from .hand_features import HandFeatures +from .model_profiles import ( + DirectModelProfile, + FlexionChannel, + ProfiledCommandMapper, +) + + +MOTOR_JOINT_NAMES: tuple[str | None, ...] = ( + "thumb_cmc_pitch", + "index_mcp_pitch", + "middle_mcp_pitch", + "ring_mcp_pitch", + "pinky_mcp_pitch", + "thumb_cmc_roll", + "index_mcp_roll", + "middle_mcp_roll", + "ring_mcp_roll", + "pinky_mcp_roll", + "thumb_cmc_yaw", + None, + None, + None, + None, + "thumb_mcp", + "index_pip", + "middle_pip", + "ring_pip", + "pinky_pip", +) + +G20_COMMAND_NAMES: tuple[str, ...] = ( + "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", +) + +FLEXION_MOTOR_INDICES = frozenset((*range(0, 5), *range(15, 20))) +RESERVED_MOTOR_INDICES = frozenset(range(11, 15)) + +# Left-G20 endpoints from linkerhand_retarget/config/hand_config.yml. Motor 5 +# is fixed at zero by the vendor mapping; motors 6..9 run in the opposite +# direction to the flexion motors on a left hand. +LOWER_LIMIT_COMMAND: tuple[int, ...] = ( + 255, 255, 255, 255, 255, + 0, 0, 0, 0, 0, + 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, +) +UPPER_LIMIT_COMMAND: tuple[int, ...] = ( + 0, 0, 0, 0, 0, + 0, 255, 255, 255, 255, + 0, 255, 255, 255, 255, + 0, 0, 0, 0, 0, +) + +# Startup/open pose used by the SDK's G20 driver. Filtering starts here so +# enabling camera control never jumps directly to the optimizer's first pose. +DEFAULT_OPEN_POSE: tuple[int, ...] = ( + 255, 255, 255, 255, 255, + 255, 193, 148, 105, 42, + 255, 255, 255, 255, 255, + 255, 255, 255, 255, 255, +) + +# Measured full-fist pose supplied for this physical left G20. Flexion +# channels are interpolated independently between DEFAULT_OPEN_POSE and this +# pose; the other channels remain governed by the opposition/splay mappings +# below. This avoids imposing one artificial minimum on mechanically +# different thumb and finger joints. +DEFAULT_FIST_POSE: tuple[int, ...] = ( + 50, 0, 0, 0, 0, + 78, 193, 158, 128, 91, + 112, 255, 255, 255, 255, + 144, 0, 0, 0, 0, +) + +# Verified on the physical left G20. Thumb channels 5 and 10 are coupled, but +# their geometry is not linear across the palm, so use measured per-finger +# commands instead of dividing the fist travel into equal fractions. +PINCH_TARGET_OPPOSITION_COMMANDS = np.asarray( + ( + (167.0, 184.0), # index + (122.0, 148.0), # middle + (78.0, 112.0), # ring + (50.0, 90.0), # pinky + ), + dtype=float, +) +POWER_GRASP_THUMB_OPPOSITION_COMMANDS = np.asarray( + (78.0, 112.0), dtype=float +) + +# The vendor G20 mapping constrains thumb horizontal swing (motor 10) to this +# range to avoid commanding the mechanical end stop. +THUMB_YAW_MIN_COMMAND = 80.0 + +# MediaPipe landmark chains for thumb, index, middle, ring and pinky. +FINGER_LANDMARKS: tuple[tuple[int, int, int, int], ...] = ( + (1, 2, 3, 4), + (5, 6, 7, 8), + (9, 10, 11, 12), + (13, 14, 15, 16), + (17, 18, 19, 20), +) + +# Human joint travel needed to reach the configured maximum G20 closure. The +# first five values control root flexion, the final five control tip flexion. +HUMAN_CLOSE_SPAN = np.asarray( + [0.85, 1.05, 1.05, 1.05, 1.05, 0.75, 1.05, 1.05, 1.05, 1.05], + dtype=float, +) + +FOUR_FINGER_TIP_INDICES = np.asarray((8, 12, 16, 20), dtype=int) +PINCH_ACTIVE_THRESHOLD = 0.20 +PINCH_RELEASE_THRESHOLD = 0.15 +PINCH_SWITCH_MARGIN = 0.08 +PINCH_CONFIRM_FRAMES = 3 +# Monocular MediaPipe world landmarks are least reliable along the palm +# normal. Retain a small depth contribution to reject obvious crossings while +# allowing visually touching fingertips to calibrate as contact. +PINCH_PALM_NORMAL_WEIGHT = 0.25 + + +def default_g20_profile() -> DirectModelProfile: + """Return the source-compatible built-in G20 direct mapping profile.""" + + feature_roles = ("thumb", "index", "middle", "ring", "pinky") + flexion_channels = tuple( + FlexionChannel(command_index, ((feature_index, 1.0),), role) + for command_index, feature_index, role in ( + *( + (index, index, feature_roles[index]) + for index in range(5) + ), + *( + (15 + index, 5 + index, feature_roles[index]) + for index in range(5) + ), + ) + ) + return DirectModelProfile( + model="G20", + command_names=G20_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=flexion_channels, + side_command_indices=(5, 10), + pinch_target_side_commands=tuple( + tuple(float(value) for value in row) + for row in PINCH_TARGET_OPPOSITION_COMMANDS + ), + power_grasp_side_commands=tuple( + float(value) for value in POWER_GRASP_THUMB_OPPOSITION_COMMANDS + ), + abducted_side_commands=(20.0, 255.0), + splay_command_indices=(6, 7, 8, 9), + splay_scale=255.0 / 0.46, + splay_gain=1.0, + max_splay_delta=90.0, + fixed_channels=tuple((index, 255.0) for index in range(11, 15)), + 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 G20CommandMapper: + """Map named left-G20 joint radians to safe, filtered motor commands.""" + + 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"G20 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("a G20 command must contain 20 values") + if not np.all(np.isfinite(values)): + raise ValueError("G20 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 SDK's known startup 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 safety 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 = self.open_pose.copy() + 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] = ( + LOWER_LIMIT_COMMAND[motor_index] + + ratio + * ( + UPPER_LIMIT_COMMAND[motor_index] + - LOWER_LIMIT_COMMAND[motor_index] + ) + ) + + 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[10] = max(command[10], THUMB_YAW_MIN_COMMAND) + command[list(RESERVED_MOTOR_INDICES)] = self.open_pose[ + list(RESERVED_MOTOR_INDICES) + ] + 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)] = self.open_pose[ + list(RESERVED_MOTOR_INDICES) + ] + return np.rint(self.last_command).astype(int).tolist() + + +class G20MediaPipeMapper: + """Map MediaPipe's 21 hand landmarks directly to a left G20 command. + + Vector IK is useful for anthropomorphic simulation models, but it can + saturate when the robot URDF coordinate convention does not match the + camera hand frame. This mapper instead uses rotation-invariant human joint + bend angles. The operator explicitly captures a fully-open pose so camera + bias and individual hand geometry are removed before motor scaling. + """ + + def __init__( + self, + *, + ema_alpha: float = 0.35, + max_step: float = 8.0, + command_deadband: float = 0.0, + max_closure: float = 0.80, + splay_gain: float = 1.0, + max_splay_delta: float = 90.0, + thumb_splay_span: float = 0.70, + thumb_abduction_min_command: float = 20.0, + thumb_pinch_gain: float = 2.0, + pinch_thumb_closure: float = 0.85, + pinch_finger_closure: float = 0.65, + open_pose: Sequence[float] = DEFAULT_OPEN_POSE, + fist_pose: Sequence[float] = DEFAULT_FIST_POSE, + profile: DirectModelProfile | None = None, + ) -> None: + 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 <= command_deadband < max_step: + raise ValueError("command_deadband must be in [0, max_step)") + if not 0.0 <= max_closure <= 1.0: + raise ValueError("max_closure must be in [0, 1]") + if splay_gain < 0.0: + raise ValueError("splay_gain must be non-negative") + if not 0.0 <= max_splay_delta <= 255.0: + raise ValueError("max_splay_delta must be in [0, 255]") + if thumb_splay_span <= 0.0: + raise ValueError("thumb_splay_span must be positive") + if not 0.0 <= thumb_abduction_min_command <= 255.0: + raise ValueError("thumb_abduction_min_command must be in [0, 255]") + if thumb_pinch_gain < 0.0: + raise ValueError("thumb_pinch_gain must be non-negative") + if not 0.0 <= pinch_thumb_closure <= 1.0: + raise ValueError("pinch_thumb_closure must be in [0, 1]") + if not 0.0 <= pinch_finger_closure <= 1.0: + raise ValueError("pinch_finger_closure must be in [0, 1]") + + base_profile = profile or default_g20_profile() + if base_profile.model != "G20": + raise ValueError("G20MediaPipeMapper requires a G20 profile") + if profile is None: + profile_open = G20CommandMapper._validate_command(open_pose) + profile_fist = G20CommandMapper._validate_command(fist_pose) + else: + profile_open = np.asarray(profile.open_pose, dtype=float) + profile_fist = np.asarray(profile.fist_pose, dtype=float) + abducted_commands = base_profile.abducted_side_commands + if abducted_commands is not None: + abducted_commands = ( + float(thumb_abduction_min_command), + *abducted_commands[1:], + ) + base_profile = replace( + base_profile, + open_pose=tuple(profile_open), + fist_pose=tuple(profile_fist), + abducted_side_commands=abducted_commands, + ).with_runtime_overrides( + ema_alpha=ema_alpha, + max_step=max_step, + command_deadband=command_deadband, + splay_gain=splay_gain, + max_splay_delta=max_splay_delta, + pinch_thumb_closure=pinch_thumb_closure, + pinch_finger_closure=pinch_finger_closure, + ) + self.profile = base_profile + self.ema_alpha = base_profile.ema_alpha + self.max_step = base_profile.max_step + self.command_deadband = base_profile.command_deadband + self.max_closure = float(max_closure) + self.splay_gain = base_profile.splay_gain + self.max_splay_delta = base_profile.max_splay_delta + self.thumb_splay_span = float(thumb_splay_span) + self.thumb_abduction_min_command = float(thumb_abduction_min_command) + self.thumb_pinch_gain = float(thumb_pinch_gain) + self.pinch_thumb_closure = base_profile.pinch_thumb_closure + self.pinch_finger_closure = base_profile.pinch_finger_closure + self._profile_mapper = ProfiledCommandMapper( + base_profile, max_closure=self.max_closure + ) + self.open_pose = self._profile_mapper.open_pose + self.fist_pose = self._profile_mapper.fist_pose + self.last_command = self.open_pose.copy() + self.open_bends: np.ndarray | None = None + self.fist_bends: np.ndarray | None = None + self.close_spans: np.ndarray | None = None + self.open_pinch_distance: float | None = None + self.open_pinch_distances: np.ndarray | None = None + self.fist_pinch_distance: float | None = None + self.fist_thumb_splay: float | None = None + self.pinch_pinch_distance: float | None = None + self.pinch_bends: np.ndarray | None = None + self.pinch_thumb_splay: float | None = None + self.open_splay: np.ndarray | None = None + self.open_thumb_splay: float | None = None + self.last_closure = np.zeros(10, dtype=float) + self.last_pinch = 0.0 + self.last_pinch_strengths = np.zeros(4, dtype=float) + self.last_pinch_target = -1 + self.last_splay_delta = np.zeros(4, dtype=float) + self.last_splay_confidence = np.ones(4, dtype=float) + self.last_thumb_splay_delta = 0.0 + self.last_thumb_abduction = 0.0 + self.last_thumb_opposition = 0.0 + self.last_power_grasp = 0.0 + self.last_bends = np.zeros(10, dtype=float) + self.last_pinch_distances = np.zeros(4, dtype=float) + self.last_raw_command = self.open_pose.copy() + self.last_pinch_blend = 0.0 + self._selected_pinch_target = -1 + self._pending_pinch_target = -1 + self._pending_pinch_frames = 0 + + @property + def is_calibrated(self) -> bool: + return ( + self.open_bends is not None + and self.open_pinch_distances is not None + and self.open_splay is not None + and self.open_thumb_splay is not None + ) + + @property + def is_fist_calibrated(self) -> bool: + return ( + self.fist_bends is not None + and self.close_spans is not None + and self.fist_pinch_distance is not None + and self.fist_thumb_splay is not None + ) + + @property + def is_pinch_calibrated(self) -> bool: + return ( + self.pinch_pinch_distance is not None + and self.pinch_bends is not None + and self.pinch_thumb_splay is not None + ) + + @property + def is_three_pose_calibrated(self) -> bool: + return ( + self.is_calibrated + and self.is_fist_calibrated + and self.is_pinch_calibrated + ) + + @staticmethod + def _validate_landmarks(landmarks: Sequence[Sequence[float]]) -> np.ndarray: + points = np.asarray(landmarks, dtype=float) + if points.shape != (21, 3): + raise ValueError(f"MediaPipe landmarks must have shape (21, 3), got {points.shape}") + if not np.all(np.isfinite(points)): + raise ValueError("MediaPipe landmarks contain non-finite values") + return points + + @staticmethod + def _joint_bend(a: np.ndarray, b: np.ndarray, c: np.ndarray) -> float: + """Return zero for a straight joint and pi for a fully folded joint.""" + + first = a - b + second = c - b + denominator = np.linalg.norm(first) * np.linalg.norm(second) + if denominator < 1e-10: + raise ValueError("degenerate MediaPipe finger segment") + cosine = float(np.clip(np.dot(first, second) / denominator, -1.0, 1.0)) + return float(np.pi - np.arccos(cosine)) + + @classmethod + def human_features( + cls, landmarks: Sequence[Sequence[float]] + ) -> tuple[np.ndarray, np.ndarray]: + """Return ten flexion angles and four normalized thumb-tip distances.""" + + points = cls._validate_landmarks(landmarks) + root_bends: list[float] = [] + tip_bends: list[float] = [] + + for finger_index, (mcp, pip, dip, tip) in enumerate(FINGER_LANDMARKS): + if finger_index == 0: + root_bend = cls._joint_bend(points[0], points[mcp], points[pip]) + mcp_bend = cls._joint_bend(points[mcp], points[pip], points[dip]) + ip_bend = cls._joint_bend(points[pip], points[dip], points[tip]) + tip_bend = 0.60 * mcp_bend + 0.40 * ip_bend + else: + root_bend = cls._joint_bend(points[0], points[mcp], points[pip]) + pip_bend = cls._joint_bend(points[mcp], points[pip], points[dip]) + dip_bend = cls._joint_bend(points[pip], points[dip], points[tip]) + tip_bend = 0.65 * pip_bend + 0.35 * dip_bend + root_bends.append(root_bend) + tip_bends.append(tip_bend) + + palm_width = float(np.linalg.norm(points[5] - points[17])) + if palm_width < 1e-8: + raise ValueError("degenerate MediaPipe palm width") + palm_forward = points[9] - points[0] + palm_forward /= max(float(np.linalg.norm(palm_forward)), 1e-8) + palm_lateral = points[5] - points[17] + palm_lateral -= np.dot(palm_lateral, palm_forward) * palm_forward + palm_lateral /= max(float(np.linalg.norm(palm_lateral)), 1e-8) + palm_normal = np.cross(palm_forward, palm_lateral) + palm_normal /= max(float(np.linalg.norm(palm_normal)), 1e-8) + pinch_vectors = points[4] - points[FOUR_FINGER_TIP_INDICES] + forward_distance = pinch_vectors @ palm_forward + lateral_distance = pinch_vectors @ palm_lateral + normal_distance = pinch_vectors @ palm_normal + pinch_distances = np.sqrt( + forward_distance**2 + + lateral_distance**2 + + (PINCH_PALM_NORMAL_WEIGHT * normal_distance) ** 2 + ) / palm_width + return ( + np.asarray(root_bends + tip_bends, dtype=float), + np.asarray(pinch_distances, dtype=float), + ) + + @classmethod + def human_splay_features( + cls, landmarks: Sequence[Sequence[float]] + ) -> tuple[np.ndarray, np.ndarray]: + """Return signed four-finger splay angles and planar confidence.""" + + points = cls._validate_landmarks(landmarks) + forward = points[9] - points[0] + forward_norm = np.linalg.norm(forward) + if forward_norm < 1e-8: + raise ValueError("degenerate MediaPipe palm length") + forward /= forward_norm + + lateral = points[5] - points[17] + lateral -= np.dot(lateral, forward) * forward + lateral_norm = np.linalg.norm(lateral) + if lateral_norm < 1e-8: + raise ValueError("degenerate MediaPipe palm lateral axis") + lateral /= lateral_norm + + angles: list[float] = [] + confidence: list[float] = [] + for mcp, pip, _, _ in FINGER_LANDMARKS[1:]: + direction = points[pip] - points[mcp] + norm = np.linalg.norm(direction) + if norm < 1e-8: + raise ValueError("degenerate MediaPipe proximal segment") + direction /= norm + longitudinal = float(np.dot(direction, forward)) + sideways = float(np.dot(direction, lateral)) + angles.append(float(np.arctan2(sideways, longitudinal))) + confidence.append(float(np.hypot(longitudinal, sideways))) + return np.asarray(angles), np.asarray(confidence) + + @classmethod + def human_thumb_splay_feature( + cls, landmarks: Sequence[Sequence[float]] + ) -> tuple[float, float]: + """Return thumb in-palm side angle and its planar confidence.""" + + points = cls._validate_landmarks(landmarks) + forward = points[9] - points[0] + forward /= max(np.linalg.norm(forward), 1e-8) + lateral = points[5] - points[17] + lateral -= np.dot(lateral, forward) * forward + lateral /= max(np.linalg.norm(lateral), 1e-8) + + direction = points[2] - points[1] + direction /= max(np.linalg.norm(direction), 1e-8) + longitudinal = float(np.dot(direction, forward)) + sideways = float(np.dot(direction, lateral)) + angle = float(np.arctan2(sideways, longitudinal)) + confidence = float(np.hypot(longitudinal, sideways)) + return angle, confidence + + @classmethod + def observe_landmarks( + cls, landmarks: Sequence[Sequence[float]] + ) -> RawHandObservation: + """Extract all raw quantities needed by robust C/F/P calibration.""" + + bends, pinch_distances = cls.human_features(landmarks) + splay, splay_confidence = cls.human_splay_features(landmarks) + thumb_splay, thumb_splay_confidence = cls.human_thumb_splay_feature( + landmarks + ) + return RawHandObservation( + bends=bends, + pinch_distances=pinch_distances, + splay=splay, + splay_confidence=splay_confidence, + thumb_splay=thumb_splay, + thumb_splay_confidence=thumb_splay_confidence, + ) + + def calibrate_open( + self, landmarks: Sequence[Sequence[float]] + ) -> dict[str, float]: + """Capture the current fully-open human hand as the mapping zero pose.""" + + return self.calibrate_open_observation(self.observe_landmarks(landmarks)) + + def calibrate_open_observation( + self, observation: RawHandObservation + ) -> dict[str, float]: + """Apply a robustly aggregated fully-open observation.""" + + self.open_bends = observation.bends.copy() + self.fist_bends = None + self.close_spans = None + self.open_pinch_distances = np.maximum( + observation.pinch_distances, 0.35 + ) + self.open_pinch_distance = float(self.open_pinch_distances[0]) + self.fist_pinch_distance = None + self.fist_thumb_splay = None + self.pinch_pinch_distance = None + self.pinch_bends = None + self.pinch_thumb_splay = None + self.open_splay = observation.splay.copy() + self.open_thumb_splay = float(observation.thumb_splay) + self.last_closure.fill(0.0) + self.last_pinch = 0.0 + self.last_pinch_strengths.fill(0.0) + self.last_pinch_target = -1 + self.last_splay_delta.fill(0.0) + self.last_splay_confidence = observation.splay_confidence.copy() + self.last_thumb_splay_delta = 0.0 + self.last_thumb_abduction = 0.0 + self.last_thumb_opposition = 0.0 + self.last_power_grasp = 0.0 + self._reset_pinch_selector() + self.reset() + return { + "max_open_bend_rad": float(np.max(observation.bends)), + "open_pinch_ratio": float(self.open_pinch_distance), + } + + def calibrate_fist( + self, landmarks: Sequence[Sequence[float]] + ) -> dict[str, float]: + """Capture a fully-closed hand and derive user-specific bend spans.""" + + return self.calibrate_fist_observation(self.observe_landmarks(landmarks)) + + def calibrate_fist_observation( + self, observation: RawHandObservation + ) -> dict[str, float]: + """Apply a robustly aggregated fist observation after validation.""" + + if not self.is_calibrated: + raise RuntimeError("open-hand calibration is required before fist calibration") + delta = observation.bends - self.open_bends + four_finger_indices = np.asarray((1, 2, 3, 4, 6, 7, 8, 9)) + mean_four_finger_delta = float(np.mean(delta[four_finger_indices])) + if mean_four_finger_delta < 0.45: + raise ValueError( + "fist calibration rejected: curl the index through pinky fingers more" + ) + self.fist_bends = observation.bends.copy() + self.close_spans = np.maximum(delta, 0.35) + self.fist_pinch_distance = float(observation.pinch_distances[0]) + self.fist_thumb_splay = float(observation.thumb_splay) + self._reset_pinch_selector() + self.reset() + return { + "mean_four_finger_delta_rad": mean_four_finger_delta, + "min_close_span_rad": float(np.min(self.close_spans)), + "fist_pinch_ratio": self.fist_pinch_distance, + } + + def calibrate_pinch( + self, landmarks: Sequence[Sequence[float]] + ) -> dict[str, float]: + """Capture thumb/index contact as the full opposition endpoint.""" + + return self.calibrate_pinch_observation(self.observe_landmarks(landmarks)) + + def calibrate_pinch_observation( + self, observation: RawHandObservation + ) -> dict[str, float]: + """Apply a robustly aggregated thumb/index contact observation.""" + + if not self.is_calibrated: + raise RuntimeError( + "open-hand calibration is required before pinch calibration" + ) + pinch_distance = float(observation.pinch_distances[0]) + reduction = float(self.open_pinch_distance - pinch_distance) + reduction_ratio = reduction / max(float(self.open_pinch_distance), 1e-6) + if reduction_ratio < 0.60 or pinch_distance > 0.35: + raise ValueError( + "pinch calibration rejected: touch the thumb and index " + "fingertips " + f"(measured reduction={reduction_ratio:.0%}, " + f"distance={pinch_distance:.3f} palm widths; " + "required reduction>=60% and distance<=0.35)" + ) + self.pinch_pinch_distance = float(pinch_distance) + self.pinch_bends = observation.bends.copy() + self.pinch_thumb_splay = float(observation.thumb_splay) + self._reset_pinch_selector() + return { + "pinch_ratio": self.pinch_pinch_distance, + "pinch_reduction": reduction, + "pinch_reduction_ratio": reduction_ratio, + } + + def _reset_pinch_selector(self) -> None: + self._selected_pinch_target = -1 + self._pending_pinch_target = -1 + self._pending_pinch_frames = 0 + + def _select_pinch_target( + self, pinch_distances: np.ndarray, strengths: np.ndarray + ) -> int: + """Select a fingertip using confirmation, switch margin and release hysteresis.""" + + candidate = int(np.argmin(pinch_distances)) + candidate_strength = float(strengths[candidate]) + current = self._selected_pinch_target + + if current >= 0: + current_strength = float(strengths[current]) + if current_strength < PINCH_RELEASE_THRESHOLD and candidate_strength < PINCH_RELEASE_THRESHOLD: + self._reset_pinch_selector() + return -1 + if ( + candidate != current + and candidate_strength >= PINCH_ACTIVE_THRESHOLD + and candidate_strength >= current_strength + PINCH_SWITCH_MARGIN + ): + if candidate == self._pending_pinch_target: + self._pending_pinch_frames += 1 + else: + self._pending_pinch_target = candidate + self._pending_pinch_frames = 1 + if self._pending_pinch_frames >= PINCH_CONFIRM_FRAMES: + self._selected_pinch_target = candidate + self._pending_pinch_target = -1 + self._pending_pinch_frames = 0 + else: + self._pending_pinch_target = -1 + self._pending_pinch_frames = 0 + return self._selected_pinch_target + + if candidate_strength < PINCH_ACTIVE_THRESHOLD: + self._pending_pinch_target = -1 + self._pending_pinch_frames = 0 + return -1 + if candidate == self._pending_pinch_target: + self._pending_pinch_frames += 1 + else: + self._pending_pinch_target = candidate + self._pending_pinch_frames = 1 + if self._pending_pinch_frames >= PINCH_CONFIRM_FRAMES: + self._selected_pinch_target = candidate + self._pending_pinch_target = -1 + self._pending_pinch_frames = 0 + return self._selected_pinch_target + + def reset(self, command: Sequence[float] | None = None) -> None: + self.last_command = ( + self.open_pose.copy() + if command is None + else G20CommandMapper._validate_command(command).astype(float) + ) + self._profile_mapper.reset(self.last_command) + self.last_raw_command = self.last_command.copy() + self.last_pinch_blend = 0.0 + + def extract_features( + self, landmarks: Sequence[Sequence[float]] + ) -> HandFeatures: + """Extract the calibrated, model-independent human hand intent.""" + + if not self.is_calibrated: + raise RuntimeError("open-hand calibration is required") + + observation = self.observe_landmarks(landmarks) + bends = observation.bends + pinch_distances = observation.pinch_distances + splay = observation.splay + splay_confidence = observation.splay_confidence + thumb_splay = observation.thumb_splay + thumb_splay_confidence = observation.thumb_splay_confidence + self.last_bends = bends.copy() + self.last_pinch_distances = pinch_distances.copy() + close_spans = self.close_spans if self.close_spans is not None else HUMAN_CLOSE_SPAN + closure = np.clip((bends - self.open_bends) / close_spans, 0.0, 1.0) + + pinch_target = ( + self.pinch_pinch_distance + if self.pinch_pinch_distance is not None + else 0.15 + ) + pinch_denominators = np.maximum( + self.open_pinch_distances - pinch_target, 0.10 + ) + candidate_strengths = np.clip( + (self.open_pinch_distances - pinch_distances) / pinch_denominators, + 0.0, + 1.0, + ) + # The closest fingertip identifies intent, while temporal hysteresis + # prevents noisy transitions between adjacent fingers. + pinch_target_index = self._select_pinch_target( + pinch_distances, candidate_strengths + ) + pinch_strengths = np.zeros(4, dtype=float) + if pinch_target_index >= 0: + pinch = float(candidate_strengths[pinch_target_index]) + pinch_strengths[pinch_target_index] = pinch + else: + pinch = 0.0 + pinch_gain = 1.0 if self.is_pinch_calibrated else self.thumb_pinch_gain + pinch_control = float(np.clip(pinch * pinch_gain, 0.0, 1.0)) + thumb_splay_delta = float( + np.arctan2( + np.sin(thumb_splay - self.open_thumb_splay), + np.cos(thumb_splay - self.open_thumb_splay), + ) + ) + thumb_confidence_weight = float( + np.clip((thumb_splay_confidence - 0.20) / 0.60, 0.0, 1.0) + ) + legacy_geometry_abduction = float( + np.clip(-thumb_splay_delta / self.thumb_splay_span, 0.0, 1.0) + * thumb_confidence_weight + ) + # In the C/F/P workflow C already captures the fully spread thumb. + # There is no separate outward endpoint, so keep outward abduction at + # the robot's open pose and use P for the inward/opposition motion. + # Before P exists, retain the old one-pose behavior for single-G20 + # launches that intentionally calibrate only C. + thumb_abduction = ( + 0.0 if self.is_pinch_calibrated else legacy_geometry_abduction + ) + four_finger_grasp = min( + float(np.mean(closure[1:5])), + float(np.mean(closure[6:10])), + ) + power_grasp = float( + np.clip((four_finger_grasp - 0.55) / 0.35, 0.0, 1.0) + ) + # Keep thumb motion independent from the other four fingers. F stores + # the operator's real thumb side-swing endpoint, so a normal fist still + # reaches the robot's fist-side command while a four-finger-only grasp + # leaves the robot thumb open. Do not attenuate this calibrated ratio by + # planar confidence: a curled thumb naturally leaves the palm plane and + # would otherwise fail to reach its own F endpoint. + thumb_fist_opposition = 0.0 + if self.is_fist_calibrated: + fist_thumb_span = float( + np.arctan2( + np.sin(self.fist_thumb_splay - self.open_thumb_splay), + np.cos(self.fist_thumb_splay - self.open_thumb_splay), + ) + ) + if abs(fist_thumb_span) >= 0.08: + thumb_fist_opposition = float( + np.clip(thumb_splay_delta / fist_thumb_span, 0.0, 1.0) + ) + + # Thumb engagement is derived only from the thumb's own calibrated + # flexion/opposition signals. The four-finger grasp gates the F-pose + # completion but cannot activate the thumb by itself. + thumb_flexion = float( + np.clip(0.4 * closure[0] + 0.6 * closure[5], 0.0, 1.0) + ) + if self.is_fist_calibrated: + thumb_indices = np.asarray((0, 5)) + thumb_spans = ( + self.fist_bends[thumb_indices] - self.open_bends[thumb_indices] + ) + valid_thumb_spans = thumb_spans > 0.08 + if np.any(valid_thumb_spans): + calibrated_thumb = np.clip( + ( + bends[thumb_indices][valid_thumb_spans] + - self.open_bends[thumb_indices][valid_thumb_spans] + ) + / thumb_spans[valid_thumb_spans], + 0.0, + 1.0, + ) + thumb_weights = np.asarray((0.4, 0.6))[valid_thumb_spans] + thumb_flexion = max( + thumb_flexion, + float(np.average(calibrated_thumb, weights=thumb_weights)), + ) + thumb_grasp_intent = float( + np.clip( + power_grasp * max(thumb_fist_opposition, thumb_flexion), + 0.0, + 1.0, + ) + ) + + self.last_closure = closure + self.last_pinch = float(pinch) + self.last_pinch_strengths = pinch_strengths + self.last_pinch_target = pinch_target_index + # Keep the old grasp coupling only for legacy single-G20 use. In the + # three-pose workflow P exclusively controls thumb opposition, while + # C remains the maximum outward/open endpoint. + if not self.is_pinch_calibrated: + thumb_abduction = float( + np.clip( + max( + legacy_geometry_abduction, + 0.85 * closure[0], + 0.75 * pinch_control, + ), + 0.0, + 1.0, + ) + ) + if self.is_pinch_calibrated: + thumb_opposition = max(thumb_fist_opposition, thumb_grasp_intent) + else: + thumb_opposition = float( + np.clip( + max( + pinch_control, + 0.75 * closure[5], + 0.75 * legacy_geometry_abduction, + ), + 0.0, + 1.0, + ) + ) + self.last_thumb_splay_delta = thumb_splay_delta + self.last_thumb_abduction = thumb_abduction + self.last_thumb_opposition = thumb_opposition + self.last_power_grasp = power_grasp + + # Map four-finger side-to-side motion around the calibrated natural + # spread. As a finger curls out of the palm plane its splay estimate + # becomes ill-conditioned, so confidence smoothly returns it to the + # calibrated startup value instead of amplifying monocular noise. + splay_delta = np.arctan2( + np.sin(splay - self.open_splay), + np.cos(splay - self.open_splay), + ) + confidence_weight = np.clip((splay_confidence - 0.20) / 0.60, 0.0, 1.0) + splay_delta *= confidence_weight + self.last_splay_delta = splay_delta + self.last_splay_confidence = splay_confidence + return HandFeatures( + closure=closure, + pinch=float(pinch), + pinch_strengths=pinch_strengths, + splay_delta=splay_delta, + splay_confidence=splay_confidence, + thumb_splay_delta=thumb_splay_delta, + thumb_abduction=thumb_abduction, + thumb_opposition=thumb_opposition, + power_grasp=power_grasp, + ) + + def raw_command_from_features(self, features: HandFeatures) -> np.ndarray: + """Map shared human intent to the left G20 protocol.""" + + if self.is_pinch_calibrated: + command = self._profile_mapper.raw_command(features) + self.last_raw_command = command.copy() + self.last_pinch_blend = self._profile_mapper.last_pinch_blend + return command + + closure = np.asarray(features.closure, dtype=float).copy() + + # Each actuator has a different measured full-fist endpoint. Apply + # max_closure to normalized human intent, then interpolate each motor + # independently instead of clamping both thumb motors at 90. + flexion_activation = np.clip( + closure * self.max_closure, 0.0, 1.0 + ) + flexion_commands = self.open_pose[ + [*range(0, 5), *range(15, 20)] + ] + flexion_activation * ( + self.fist_pose[[*range(0, 5), *range(15, 20)]] + - self.open_pose[[*range(0, 5), *range(15, 20)]] + ) + + command = self.open_pose.copy() + command[0:5] = flexion_commands[0:5] + command[15:20] = flexion_commands[5:10] + command[5] = self.open_pose[5] + features.thumb_abduction * ( + self.thumb_abduction_min_command - self.open_pose[5] + ) + command_delta = self.splay_gain * features.splay_delta * (255.0 / 0.46) + command_delta = np.clip( + command_delta, -self.max_splay_delta, self.max_splay_delta + ) + command[6:10] = np.clip( + self.open_pose[6:10] + command_delta, 0.0, 255.0 + ) + command[10] = self.open_pose[10] + features.thumb_opposition * ( + THUMB_YAW_MIN_COMMAND - self.open_pose[10] + ) + command[list(RESERVED_MOTOR_INDICES)] = self.open_pose[ + list(RESERVED_MOTOR_INDICES) + ] + self.last_raw_command = np.clip(command, 0.0, 255.0) + self.last_pinch_blend = 0.0 + return self.last_raw_command.copy() + + def raw_command(self, landmarks: Sequence[Sequence[float]]) -> np.ndarray: + return self.raw_command_from_features(self.extract_features(landmarks)) + + def map_features(self, features: HandFeatures) -> list[int]: + raw = self.raw_command_from_features(features) + 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) + delta[np.abs(delta) <= self.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 + self._profile_mapper.last_command = self.last_command.copy() + return np.rint(self.last_command).astype(int).tolist() + + def map_landmarks(self, landmarks: Sequence[Sequence[float]]) -> list[int]: + return self.map_features(self.extract_features(landmarks)) diff --git a/src/camera_retarget_l20/camera_retarget_l20/hand_detector.py b/src/camera_retarget_l20/camera_retarget_l20/hand_detector.py new file mode 100644 index 0000000..b8c28d1 --- /dev/null +++ b/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(), + ) + diff --git a/src/camera_retarget_l20/camera_retarget_l20/hand_features.py b/src/camera_retarget_l20/camera_retarget_l20/hand_features.py new file mode 100644 index 0000000..59b3766 --- /dev/null +++ b/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)), + ) diff --git a/src/camera_retarget_l20/camera_retarget_l20/l20_mapping.py b/src/camera_retarget_l20/camera_retarget_l20/l20_mapping.py new file mode 100644 index 0000000..276b6b4 --- /dev/null +++ b/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() + diff --git a/src/camera_retarget_l20/camera_retarget_l20/model_profiles.py b/src/camera_retarget_l20/camera_retarget_l20/model_profiles.py new file mode 100644 index 0000000..5c8236a --- /dev/null +++ b/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() diff --git a/src/camera_retarget_l20/camera_retarget_l20/node.py b/src/camera_retarget_l20/camera_retarget_l20/node.py new file mode 100644 index 0000000..c267928 --- /dev/null +++ b/src/camera_retarget_l20/camera_retarget_l20/node.py @@ -0,0 +1,1258 @@ +"""ROS 2 camera-to-LinkerHand L20/G20/O6 retargeting bridge.""" + +from __future__ import annotations + +import json +import sys +import time +from pathlib import Path + +import cv2 +import numpy as np +import rclpy +from ament_index_python.packages import get_package_share_directory +from rclpy.node import Node +from rclpy.parameter import Parameter +from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy +from sensor_msgs.msg import JointState +from std_msgs.msg import String +from std_srvs.srv import SetBool, Trigger + +from .calibration import PoseCalibrationSession, RawHandObservation +from .hand_detector import MediaPipeLeftHandDetector +from .g20_mapping import ( + DEFAULT_OPEN_POSE as G20_DEFAULT_OPEN_POSE, + G20_COMMAND_NAMES, + G20CommandMapper, + G20MediaPipeMapper, +) +from .l20_mapping import ( + DEFAULT_OPEN_POSE as L20_DEFAULT_OPEN_POSE, + L20CommandMapper, +) +from .o6_mapping import ( + O6_COMMAND_NAMES, + O6MediaPipeMapper, +) +from .model_profiles import ( + DirectModelProfile, + ProfiledCommandMapper, + load_model_profiles, +) + + +COMMAND_QOS = QoSProfile( + history=HistoryPolicy.KEEP_LAST, + depth=1, + reliability=ReliabilityPolicy.RELIABLE, +) + + +class CameraHandRetargetNode(Node): + def __init__(self) -> None: + super().__init__("camera_hand_retarget") + self._declare_parameters() + + configured_targets = self.get_parameter_or( + "target_models", + Parameter("target_models", Parameter.Type.STRING_ARRAY, []), + ).value + targets = tuple( + str(model).upper() + for model in (configured_targets or []) + if str(model).strip() + ) + self.dual_mode = bool(targets) + if self.dual_mode: + if len(targets) < 2 or len(set(targets)) != len(targets): + raise ValueError( + "target_models requires at least two unique model names" + ) + if "G20" not in targets: + raise ValueError( + "direct multi-model mode currently requires G20 as the " + "calibrated human-feature extractor" + ) + self.target_models = targets + # G20's calibrated feature extractor is shared by all profiles. + self.hand_model = "G20" + else: + self.hand_model = str(self.get_parameter("hand_model").value).upper() + self.target_models = (self.hand_model,) + if self.hand_model not in {"L20", "G20"}: + raise ValueError("hand_model must be 'L20' or 'G20'") + requested_mapping_mode = str( + self.get_parameter("mapping_mode").value + ).lower() + if requested_mapping_mode not in {"auto", "dex", "mediapipe_angles"}: + raise ValueError( + "mapping_mode must be 'auto', 'dex', or 'mediapipe_angles'" + ) + self.mapping_mode = requested_mapping_mode + if self.dual_mode: + self.mapping_mode = "mediapipe_angles" + if self.mapping_mode == "auto": + self.mapping_mode = ( + "mediapipe_angles" if self.hand_model == "G20" else "dex" + ) + if self.mapping_mode == "mediapipe_angles" and self.hand_model != "G20": + raise ValueError("mediapipe_angles mapping currently supports G20 only") + self.camera_device = str(self.get_parameter("camera_device").value) + self.show_window = bool(self.get_parameter("show_window").value) + self.selfie = bool(self.get_parameter("selfie").value) + self.publish_rate = float(self.get_parameter("publish_rate").value) + self.motor_speed = int(self.get_parameter("motor_speed").value) + self.stable_frames_required = int( + self.get_parameter("stable_frames_required").value + ) + self.lost_timeout = float(self.get_parameter("lost_timeout").value) + self.lost_behavior = str(self.get_parameter("lost_behavior").value).lower() + if self.lost_behavior not in {"open", "hold"}: + raise ValueError("lost_behavior must be 'open' or 'hold'") + self.driver_state_timeout = float( + self.get_parameter("driver_state_timeout").value + ) + if self.driver_state_timeout <= 0.0: + raise ValueError("driver_state_timeout must be positive") + self.calibration_duration = float( + self.get_parameter("calibration_duration").value + ) + self.calibration_min_samples = int( + self.get_parameter("calibration_min_samples").value + ) + self.calibration_timeout = float( + self.get_parameter("calibration_timeout").value + ) + if self.calibration_duration <= 0.0: + raise ValueError("calibration_duration must be positive") + if self.calibration_min_samples < 2: + raise ValueError("calibration_min_samples must be at least two") + if self.calibration_timeout <= self.calibration_duration: + raise ValueError("calibration_timeout must exceed calibration_duration") + self.publish_debug = bool(self.get_parameter("publish_debug").value) + + ema_alpha = float(self.get_parameter("ema_alpha").value) + max_step = float(self.get_parameter("max_step").value) + command_deadband = float(self.get_parameter("command_deadband").value) + max_closure = float(self.get_parameter("max_closure").value) + self.retargeting = None + self.direct_mappers: dict[str, object] = {} + self.model_profiles: dict[str, DirectModelProfile] = {} + if self.mapping_mode == "mediapipe_angles": + profile_path = self._resolve_model_profile_path() + self.model_profiles = load_model_profiles(profile_path) + required_profiles = set(self.target_models) + missing_profiles = sorted(required_profiles.difference(self.model_profiles)) + if missing_profiles: + raise ValueError( + f"model profile file is missing: {missing_profiles}" + ) + self.get_logger().info(f"Using direct model profiles: {profile_path}") + g20_profile = self.model_profiles["G20"] + self.default_open_pose = tuple( + int(round(value)) for value in g20_profile.open_pose + ) + self.mapper = G20MediaPipeMapper( + ema_alpha=ema_alpha, + max_step=max_step, + command_deadband=command_deadband, + max_closure=max_closure, + splay_gain=float(self.get_parameter("splay_gain").value), + max_splay_delta=float( + self.get_parameter("max_splay_delta").value + ), + thumb_splay_span=float( + self.get_parameter("thumb_splay_span").value + ), + thumb_abduction_min_command=float( + self.get_parameter("thumb_abduction_min_command").value + ), + thumb_pinch_gain=float( + self.get_parameter("thumb_pinch_gain").value + ), + pinch_thumb_closure=float( + self.get_parameter("g20_pinch_thumb_closure").value + ), + pinch_finger_closure=float( + self.get_parameter("g20_pinch_finger_closure").value + ), + profile=g20_profile, + ) + self.direct_mappers["G20"] = self.mapper + for model in self.target_models: + if model == "G20": + continue + profile = self.model_profiles[model] + if model == "O6": + adapter = O6MediaPipeMapper( + ema_alpha=ema_alpha, + max_step=max_step, + command_deadband=command_deadband, + pinch_thumb_closure=float( + self.get_parameter("o6_pinch_thumb_closure").value + ), + pinch_finger_closure=float( + self.get_parameter("o6_pinch_finger_closure").value + ), + profile=profile, + ) + self.o6_mapper = adapter + else: + adapter_profile = profile.with_runtime_overrides( + ema_alpha=ema_alpha, + max_step=max_step, + command_deadband=command_deadband, + ) + adapter = ProfiledCommandMapper(adapter_profile) + self.direct_mappers[model] = adapter + self._log_profile_overrides() + else: + self.retargeting = self._build_retargeting() + robot = self.retargeting.optimizer.robot + mapper_class = L20CommandMapper + self.default_open_pose = L20_DEFAULT_OPEN_POSE + if self.hand_model == "G20": + mapper_class = G20CommandMapper + self.default_open_pose = G20_DEFAULT_OPEN_POSE + self.mapper = mapper_class( + self.retargeting.joint_names, + robot.joint_limits, + ema_alpha=ema_alpha, + max_step=max_step, + max_closure=max_closure, + ) + self.detector = MediaPipeLeftHandDetector( + selfie=self.selfie, + strict_handedness=bool( + self.get_parameter("strict_handedness").value + ), + min_detection_confidence=float( + self.get_parameter("min_detection_confidence").value + ), + min_tracking_confidence=float( + self.get_parameter("min_tracking_confidence").value + ), + ) + self.capture = self._open_camera() + + self.command_publishers = {} + self.preview_publishers = {} + self.driver_state_times: dict[str, float | None] = {} + self.driver_state_valid: dict[str, bool] = {} + self.state_subscriptions = [] + if self.dual_mode: + for model in self.target_models: + prefix = f"/{model.lower()}" + self.command_publishers[model] = self.create_publisher( + JointState, + f"{prefix}/cb_left_hand_control_cmd", + COMMAND_QOS, + ) + self.preview_publishers[model] = self.create_publisher( + JointState, f"~/{model.lower()}/target_joint_cmd", 10 + ) + self.driver_state_times[model] = None + self.driver_state_valid[model] = False + self.state_subscriptions.append( + self.create_subscription( + JointState, + f"{prefix}/cb_left_hand_state", + lambda message, target=model: self._state_callback( + target, message + ), + 10, + ) + ) + else: + control_topic = str(self.get_parameter("control_topic").value) + self.command_publisher = self.create_publisher( + JointState, control_topic, COMMAND_QOS + ) + self.preview_publisher = self.create_publisher( + JointState, "~/target_joint_cmd", 10 + ) + self.command_publishers[self.hand_model] = self.command_publisher + self.preview_publishers[self.hand_model] = self.preview_publisher + self.status_publisher = self.create_publisher(String, "~/status", 10) + self.debug_publisher = ( + self.create_publisher(String, "~/debug", 10) + if self.publish_debug + else None + ) + self.enable_service = self.create_service( + SetBool, "~/enable", self._enable_callback + ) + self.calibrate_service = self.create_service( + Trigger, "~/calibrate_open", self._calibrate_open_callback + ) + self.calibrate_fist_service = self.create_service( + Trigger, "~/calibrate_fist", self._calibrate_fist_callback + ) + self.calibrate_pinch_service = self.create_service( + Trigger, "~/calibrate_pinch", self._calibrate_pinch_callback + ) + + self.control_enabled = False + self.auto_enable_requested = bool( + self.get_parameter("start_enabled").value + ) + self.hand_detected = False + self.stable_frames = 0 + self.last_detection_time = time.monotonic() + self.last_detection_positions: np.ndarray | None = None + self.last_target_command: list[int] | None = None + self.last_target_commands: dict[str, list[int]] = {} + self.last_status_time = 0.0 + self.last_debug_time = 0.0 + self.last_frame_time = time.monotonic() + self.measured_fps = 0.0 + self.failsafe_sent = False + self.last_failsafe_reason: str | None = None + self.status_message = "preview_only" + self.calibration_session: PoseCalibrationSession | None = None + self.calibration_diagnostics: dict[str, float] = {} + self.calibration_failure_reported = False + display_models = "+".join(self.target_models) + self.window_name = f"{display_models} left camera retargeting" + + self.timer = self.create_timer(1.0 / self.publish_rate, self._process_frame) + if self.dual_mode: + self.get_logger().info( + f"{display_models} mapping ready in PREVIEW mode. Press C with an open " + "hand and the thumb spread outward, F with a fist, P with " + "thumb/index pinched, then E to enable all target hands." + ) + elif self.mapping_mode == "mediapipe_angles": + self.get_logger().info( + "G20 angle mapping ready in PREVIEW mode. Fully open the left " + "hand, press C to calibrate, then press E to enable control." + ) + else: + self.get_logger().info( + "Camera retargeting ready in PREVIEW mode. Show a stable left " + "hand, then press E or call ~/enable with data=true to control " + f"the {self.hand_model}." + ) + + def _declare_parameters(self) -> None: + self.declare_parameter("hand_model", "L20") + self.declare_parameter("target_models", Parameter.Type.STRING_ARRAY) + self.declare_parameter("mapping_mode", "auto") + self.declare_parameter("camera_device", "/dev/video0") + self.declare_parameter("camera_width", 1280) + self.declare_parameter("camera_height", 720) + self.declare_parameter("camera_fps", 30.0) + self.declare_parameter("show_window", True) + self.declare_parameter("selfie", False) + self.declare_parameter("strict_handedness", True) + self.declare_parameter("min_detection_confidence", 0.70) + self.declare_parameter("min_tracking_confidence", 0.70) + self.declare_parameter("publish_rate", 30.0) + self.declare_parameter("control_topic", "/cb_left_hand_control_cmd") + self.declare_parameter("start_enabled", False) + self.declare_parameter("motor_speed", 60) + self.declare_parameter("ema_alpha", 0.35) + self.declare_parameter("max_step", 8.0) + self.declare_parameter("command_deadband", 0.0) + self.declare_parameter("max_closure", 0.80) + self.declare_parameter("splay_gain", 1.0) + self.declare_parameter("max_splay_delta", 90.0) + self.declare_parameter("thumb_splay_span", 0.70) + self.declare_parameter("thumb_abduction_min_command", 20.0) + self.declare_parameter("thumb_pinch_gain", 2.0) + self.declare_parameter("g20_pinch_thumb_closure", 0.85) + self.declare_parameter("g20_pinch_finger_closure", 0.65) + self.declare_parameter("o6_pinch_thumb_closure", 0.85) + self.declare_parameter("o6_pinch_finger_closure", 0.65) + self.declare_parameter("stable_frames_required", 8) + self.declare_parameter("lost_timeout", 0.35) + self.declare_parameter("lost_behavior", "open") + self.declare_parameter("driver_state_timeout", 1.0) + self.declare_parameter("model_profile_file", "") + self.declare_parameter("calibration_duration", 1.5) + self.declare_parameter("calibration_min_samples", 30) + self.declare_parameter("calibration_timeout", 8.0) + self.declare_parameter("publish_debug", True) + self.declare_parameter("retargeting_config", "") + self.declare_parameter("urdf_path", "") + self.declare_parameter( + "dex_retargeting_source", "/home/lxp/projects/dex-retargeting" + ) + self.declare_parameter("retargeting_scale", 1.0) + self.declare_parameter("retargeting_filter_alpha", 0.25) + + @staticmethod + def _workspace_candidates(relative_path: Path) -> list[Path]: + candidates: list[Path] = [] + module_path = Path(__file__).resolve() + for parent in module_path.parents: + candidates.append(parent / relative_path) + candidates.append(Path.cwd() / relative_path) + return candidates + + def _resolve_config_path(self) -> Path: + configured = str(self.get_parameter("retargeting_config").value) + if configured: + path = Path(configured).expanduser().resolve() + if path.exists(): + return path + raise FileNotFoundError(f"retargeting_config does not exist: {path}") + share = Path(get_package_share_directory("camera_retarget_l20")) + path = share / "config" / f"{self.hand_model.lower()}_left_mediapipe.yml" + if not path.exists(): + raise FileNotFoundError(f"installed retargeting config not found: {path}") + return path + + def _resolve_model_profile_path(self) -> Path: + configured = str(self.get_parameter("model_profile_file").value) + if configured: + path = Path(configured).expanduser().resolve() + if path.exists(): + return path + raise FileNotFoundError(f"model_profile_file does not exist: {path}") + share = Path(get_package_share_directory("camera_retarget_l20")) + path = share / "config" / "direct_mapping_profiles.yml" + if not path.exists(): + raise FileNotFoundError(f"installed model profile not found: {path}") + return path + + def _log_profile_overrides(self) -> None: + """Report legacy ROS values that replace profile defaults.""" + + common = { + "ema_alpha": float(self.get_parameter("ema_alpha").value), + "max_step": float(self.get_parameter("max_step").value), + "command_deadband": float( + self.get_parameter("command_deadband").value + ), + "motor_speed": int(self.get_parameter("motor_speed").value), + } + for model in self.target_models: + profile = self.model_profiles[model] + profile_values = { + "ema_alpha": profile.ema_alpha, + "max_step": profile.max_step, + "command_deadband": profile.command_deadband, + "motor_speed": profile.motor_speed, + } + for field, value in common.items(): + if not np.isclose(float(value), float(profile_values[field])): + self.get_logger().info( + f"{model} profile {field}={profile_values[field]} " + f"overridden by ROS parameter {value}" + ) + model_parameters = {} + model_profile_values = { + "pinch_thumb_closure": profile.pinch_thumb_closure, + "pinch_finger_closure": profile.pinch_finger_closure, + } + if model in {"O6", "G20"}: + model_parameters.update( + { + "pinch_thumb_closure": float( + self.get_parameter( + f"{model.lower()}_pinch_thumb_closure" + ).value + ), + "pinch_finger_closure": float( + self.get_parameter( + f"{model.lower()}_pinch_finger_closure" + ).value + ), + } + ) + if model == "G20": + model_parameters.update( + { + "splay_gain": float( + self.get_parameter("splay_gain").value + ), + "max_splay_delta": float( + self.get_parameter("max_splay_delta").value + ), + } + ) + model_profile_values.update( + { + "splay_gain": profile.splay_gain, + "max_splay_delta": profile.max_splay_delta, + } + ) + for field, value in model_parameters.items(): + if not np.isclose( + float(value), float(model_profile_values[field]) + ): + self.get_logger().info( + f"{model} profile {field}=" + f"{model_profile_values[field]} overridden by ROS " + f"parameter {value}" + ) + + def _resolve_urdf_path(self) -> Path: + configured = str(self.get_parameter("urdf_path").value) + if configured: + path = Path(configured).expanduser().resolve() + if path.exists(): + return path + raise FileNotFoundError(f"urdf_path does not exist: {path}") + + model = self.hand_model.lower() + relative = Path( + "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/" + f"linker_hand/{model}_left/linkerhand_{model}_left.urdf" + ) + for candidate in self._workspace_candidates(relative): + if candidate.exists(): + return candidate.resolve() + raise FileNotFoundError( + f"Cannot locate the {self.hand_model}-left URDF. " + "Set the urdf_path ROS parameter." + ) + + def _import_retargeting_config(self): + try: + from dex_retargeting.retargeting_config import RetargetingConfig + + return RetargetingConfig + except ImportError: + source = Path( + str(self.get_parameter("dex_retargeting_source").value) + ).expanduser() + source_package = source / "src" + if source_package.exists() and str(source_package) not in sys.path: + sys.path.insert(0, str(source_package)) + from dex_retargeting.retargeting_config import RetargetingConfig + + return RetargetingConfig + + def _build_retargeting(self): + RetargetingConfig = self._import_retargeting_config() + config_path = self._resolve_config_path() + urdf_path = self._resolve_urdf_path() + override = { + "urdf_path": str(urdf_path), + "scaling_factor": float( + self.get_parameter("retargeting_scale").value + ), + "low_pass_alpha": float( + self.get_parameter("retargeting_filter_alpha").value + ), + } + self.get_logger().info(f"Using {self.hand_model} URDF: {urdf_path}") + return RetargetingConfig.load_from_file(config_path, override=override).build() + + def _open_camera(self) -> cv2.VideoCapture: + source: str | int = self.camera_device + if self.camera_device.isdigit(): + source = int(self.camera_device) + capture = cv2.VideoCapture(source, cv2.CAP_V4L2) + capture.set(cv2.CAP_PROP_FRAME_WIDTH, int(self.get_parameter("camera_width").value)) + capture.set(cv2.CAP_PROP_FRAME_HEIGHT, int(self.get_parameter("camera_height").value)) + capture.set(cv2.CAP_PROP_FPS, float(self.get_parameter("camera_fps").value)) + capture.set(cv2.CAP_PROP_BUFFERSIZE, 1) + if not capture.isOpened(): + raise RuntimeError( + f"Cannot open camera {self.camera_device}. Check /dev permissions and device path." + ) + return capture + + def _expected_command_length(self, model: str) -> int: + if model in self.model_profiles: + return self.model_profiles[model].command_length + return 6 if model == "O6" else 20 + + def _make_joint_state( + self, model: str, command: list[int], *, include_speed: bool + ) -> JointState: + expected_length = self._expected_command_length(model) + if len(command) != expected_length: + raise ValueError( + f"{model} command must contain {expected_length} values" + ) + message = JointState() + message.header.stamp = self.get_clock().now().to_msg() + if model in self.model_profiles: + message.name = list(self.model_profiles[model].command_names) + elif model == "G20": + message.name = list(G20_COMMAND_NAMES) + elif model == "O6": + message.name = list(O6_COMMAND_NAMES) + else: + message.name = [f"joint{index}" for index in range(1, 21)] + message.position = [float(value) for value in command] + message.velocity = ( + [float(self.motor_speed)] * expected_length if include_speed else [] + ) + return message + + def _open_commands(self) -> dict[str, list[int]]: + if self.model_profiles: + return { + model: [ + int(round(value)) + for value in self.model_profiles[model].open_pose + ] + for model in self.target_models + } + return {self.hand_model: list(self.default_open_pose)} + + def _publish_control(self, commands: dict[str, list[int]] | list[int]) -> None: + if isinstance(commands, list): + commands = {self.hand_model: commands} + for model, command in commands.items(): + self.command_publishers[model].publish( + self._make_joint_state(model, command, include_speed=True) + ) + + def _state_callback(self, model: str, message: JointState) -> None: + expected_length = self._expected_command_length(model) + values = np.asarray(message.position, dtype=float) + valid = ( + values.shape == (expected_length,) + and np.all(np.isfinite(values)) + and np.all(values >= 0.0) + ) + self.driver_state_valid[model] = bool(valid) + if valid: + self.driver_state_times[model] = time.monotonic() + + def _driver_health(self, now: float) -> tuple[bool, dict[str, str]]: + if not self.dual_mode: + return True, {} + details: dict[str, str] = {} + for model in self.target_models: + if self.command_publishers[model].get_subscription_count() < 1: + details[model] = "command_subscriber_missing" + continue + state_time = self.driver_state_times[model] + if not self.driver_state_valid[model] or state_time is None: + details[model] = "state_missing" + elif now - state_time > self.driver_state_timeout: + details[model] = "state_stale" + else: + details[model] = "ready" + return all(value == "ready" for value in details.values()), details + + def _set_control_enabled(self, enabled: bool) -> tuple[bool, str]: + if enabled: + if self.control_enabled: + return True, f"{'+'.join(self.target_models)} control is already enabled" + if self.calibration_session is not None and self.calibration_session.active: + return False, "Wait for the active C/F/P calibration to finish" + if not self.hand_detected or self.stable_frames < self.stable_frames_required: + return False, ( + "Left hand is not stable yet; keep it visible before enabling" + ) + if ( + self.mapping_mode == "mediapipe_angles" + and not self.mapper.is_calibrated + ): + return False, ( + "Open-hand calibration is required: fully open the left " + "hand and press C or call ~/calibrate_open" + ) + if self.dual_mode and not self.mapper.is_fist_calibrated: + return False, ( + "Fist calibration is required: close the left hand and " + "press F or call ~/calibrate_fist" + ) + if self.dual_mode and not self.mapper.is_pinch_calibrated: + return False, ( + "Pinch calibration is required: touch thumb/index tips and " + "press P or call ~/calibrate_pinch" + ) + drivers_ready, driver_details = self._driver_health(time.monotonic()) + if not drivers_ready: + return False, f"All target drivers must be ready: {driver_details}" + if self.direct_mappers: + for adapter in self.direct_mappers.values(): + adapter.reset() + else: + self.mapper.reset() + if self.retargeting is not None: + self.retargeting.reset() + self.failsafe_sent = False + self.last_failsafe_reason = None + self.control_enabled = True + self.auto_enable_requested = False + self.status_message = "control_enabled" + self._publish_control(self._open_commands()) + self.get_logger().warning( + f"REAL {'+'.join(self.target_models)} CONTROL ENABLED" + ) + return True, f"Real {'+'.join(self.target_models)} control enabled" + + self.control_enabled = False + self.auto_enable_requested = False + self.status_message = "preview_only" + self.get_logger().info( + f"Real {'+'.join(self.target_models)} control disabled; holding last command" + ) + return True, f"Real {'+'.join(self.target_models)} control disabled" + + def _enable_callback(self, request: SetBool.Request, response: SetBool.Response): + success, message = self._set_control_enabled(bool(request.data)) + response.success = success + response.message = message + return response + + def _start_calibration(self, pose: str) -> tuple[bool, str]: + if self.mapping_mode != "mediapipe_angles": + return False, "C/F/P calibration is only used by angle mapping" + if pose in {"fist", "pinch"} and not self.dual_mode: + return False, f"{pose.capitalize()} calibration requires multi-model mode" + if pose in {"fist", "pinch"} and not self.mapper.is_calibrated: + return False, "Calibrate the fully-open hand first" + if self.control_enabled: + self._set_control_enabled(False) + now = time.monotonic() + self.calibration_session = PoseCalibrationSession( + pose, + started_at=now, + duration=self.calibration_duration, + min_samples=self.calibration_min_samples, + timeout=self.calibration_timeout, + ) + self.calibration_diagnostics = {} + self.calibration_failure_reported = False + self.status_message = f"calibrating_{pose}" + pose_instruction = { + "open": "fully open hand", + "fist": "normal closed fist", + "pinch": "thumb/index fingertips touching", + }[pose] + message = ( + f"Started {pose} calibration; hold a stable {pose_instruction} for " + f"{self.calibration_duration:.1f}s" + ) + self.get_logger().info(message) + return True, message + + def _calibrate_open_pose(self) -> tuple[bool, str]: + return self._start_calibration("open") + + def _calibrate_open_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + success, message = self._calibrate_open_pose() + response.success = success + response.message = message + return response + + def _calibrate_fist_pose(self) -> tuple[bool, str]: + return self._start_calibration("fist") + + def _calibrate_fist_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + success, message = self._calibrate_fist_pose() + response.success = success + response.message = message + return response + + def _calibrate_pinch_pose(self) -> tuple[bool, str]: + return self._start_calibration("pinch") + + def _calibrate_pinch_callback( + self, request: Trigger.Request, response: Trigger.Response + ) -> Trigger.Response: + del request + success, message = self._calibrate_pinch_pose() + response.success = success + response.message = message + return response + + def _report_calibration_failure(self) -> None: + session = self.calibration_session + if ( + session is None + or session.state != "failed" + or self.calibration_failure_reported + ): + return + self.calibration_failure_reported = True + self.status_message = f"calibration_failed_{session.pose}" + self.get_logger().warning( + f"{session.pose.capitalize()} calibration failed: {session.reason}" + ) + + def _apply_calibration_result( + self, session: PoseCalibrationSession, observation: RawHandObservation + ) -> None: + try: + if session.pose == "open": + diagnostics = self.mapper.calibrate_open_observation(observation) + elif session.pose == "fist": + diagnostics = self.mapper.calibrate_fist_observation(observation) + else: + diagnostics = self.mapper.calibrate_pinch_observation(observation) + except (RuntimeError, ValueError) as error: + session.fail(str(error)) + self._report_calibration_failure() + return + for model, adapter in self.direct_mappers.items(): + if model != "G20": + adapter.reset() + self.calibration_diagnostics = diagnostics + self.status_message = f"{session.pose}_calibrated" + details = ", ".join( + f"{name}={value:.3f}" for name, value in diagnostics.items() + ) + self.get_logger().info( + f"{session.pose.capitalize()} calibration complete: {details}" + ) + + def _update_calibration( + self, landmarks: np.ndarray | None, now: float + ) -> None: + session = self.calibration_session + if session is None or not session.active: + return + if landmarks is None: + session.reason = "hand_not_detected" + session.poll(now) + self._report_calibration_failure() + return + try: + observation = self.mapper.observe_landmarks(landmarks) + except (ValueError, np.linalg.LinAlgError) as error: + session.reason = f"invalid_hand_geometry:{error}" + session.poll(now) + self._report_calibration_failure() + return + self.mapper.last_bends = observation.bends.copy() + self.mapper.last_pinch_distances = observation.pinch_distances.copy() + result = session.add(observation, now) + if result is not None: + self._apply_calibration_result(session, result) + else: + self._report_calibration_failure() + + def _handle_detection_lost(self, now: float) -> None: + if not self.control_enabled or self.failsafe_sent: + return + if now - self.last_detection_time < self.lost_timeout: + return + + if self.dual_mode or self.lost_behavior == "open": + self._publish_control(self._open_commands()) + self.status_message = "failsafe_open" + self.last_failsafe_reason = "hand_tracking_lost" + self.get_logger().error( + "Hand tracking lost: opening targets and disabling control" + ) + else: + self.status_message = "failsafe_hold" + self.last_failsafe_reason = "hand_tracking_lost" + self.get_logger().error( + f"Hand tracking lost: holding {self.hand_model} and disabling control" + ) + self.control_enabled = False + self.failsafe_sent = True + + def _handle_driver_lost(self, now: float) -> None: + if not self.dual_mode or not self.control_enabled or self.failsafe_sent: + return + drivers_ready, details = self._driver_health(now) + if drivers_ready: + return + # Publishing both open poses guarantees the still-online hand opens. + self._publish_control(self._open_commands()) + self.status_message = "driver_failsafe_open" + self.last_failsafe_reason = f"driver_health_lost: {details}" + self.control_enabled = False + self.failsafe_sent = True + self.get_logger().error( + f"Driver health lost: {details}; opening targets and disabling control" + ) + + def _calibration_status(self, now: float) -> dict: + session = self.calibration_session + if session is None: + return { + "pose": None, + "state": "idle", + "progress": 0.0, + "samples": 0, + "reason": None, + } + return { + "pose": session.pose, + "state": session.state, + "progress": round(session.progress(now), 3), + "samples": len(session.samples), + "min_samples": session.min_samples, + "reason": session.reason, + "stability": { + name: round(value, 4) + for name, value in session.last_stability.items() + }, + "diagnostics": { + name: round(value, 4) + for name, value in self.calibration_diagnostics.items() + }, + } + + def _publish_status(self, now: float) -> None: + if now - self.last_status_time < 0.25: + return + status = String() + _, driver_details = self._driver_health(now) + status_values = { + "hand_model": ( + self.hand_model + if not self.dual_mode + else "+".join(self.target_models) + ), + "target_models": list(self.target_models), + "mapping_mode": self.mapping_mode, + "open_calibrated": bool( + getattr(self.mapper, "is_calibrated", True) + ), + "fist_calibrated": bool( + getattr(self.mapper, "is_fist_calibrated", not self.dual_mode) + ), + "pinch_calibrated": bool( + getattr(self.mapper, "is_pinch_calibrated", not self.dual_mode) + ), + "control_enabled": self.control_enabled, + "hand_detected": self.hand_detected, + "stable_frames": self.stable_frames, + "stable_frames_required": self.stable_frames_required, + "camera_fps": round(self.measured_fps, 1), + "state": self.status_message, + "last_failsafe_reason": self.last_failsafe_reason, + "calibration": self._calibration_status(now), + } + if self.dual_mode: + status_values["drivers"] = driver_details + if self.mapping_mode == "mediapipe_angles": + status_values["human_closure"] = np.round( + self.mapper.last_closure, 2 + ).tolist() + status_values["human_pinch"] = round(self.mapper.last_pinch, 2) + status_values["human_pinch_strengths"] = np.round( + self.mapper.last_pinch_strengths, 2 + ).tolist() + pinch_target_names = ("index", "middle", "ring", "pinky") + status_values["human_pinch_target"] = ( + pinch_target_names[self.mapper.last_pinch_target] + if self.mapper.last_pinch_target >= 0 + else None + ) + status_values["human_splay_deg"] = np.round( + np.degrees(self.mapper.last_splay_delta), 1 + ).tolist() + status_values["splay_confidence"] = np.round( + self.mapper.last_splay_confidence, 2 + ).tolist() + status_values["thumb_splay_deg"] = round( + float(np.degrees(self.mapper.last_thumb_splay_delta)), 1 + ) + status_values["thumb_abduction"] = round( + self.mapper.last_thumb_abduction, 2 + ) + status_values["thumb_opposition"] = round( + self.mapper.last_thumb_opposition, 2 + ) + status_values["power_grasp"] = round( + self.mapper.last_power_grasp, 2 + ) + status.data = json.dumps(status_values, ensure_ascii=False) + self.status_publisher.publish(status) + self.last_status_time = now + + def _publish_debug(self, now: float) -> None: + if ( + self.debug_publisher is None + or self.mapping_mode != "mediapipe_angles" + or now - self.last_debug_time < 0.10 + ): + return + values: dict = { + "calibration": self._calibration_status(now), + "raw_bends_rad": np.round(self.mapper.last_bends, 4).tolist(), + "raw_pinch_distances": np.round( + self.mapper.last_pinch_distances, 4 + ).tolist(), + "normalized_closure": np.round( + self.mapper.last_closure, 4 + ).tolist(), + "pinch_strengths": np.round( + self.mapper.last_pinch_strengths, 4 + ).tolist(), + "pinch_target": int(self.mapper.last_pinch_target), + "power_grasp": round(self.mapper.last_power_grasp, 4), + "commands": { + "G20": { + "raw": np.round(self.mapper.last_raw_command, 2).tolist(), + "filtered": np.round(self.mapper.last_command, 2).tolist(), + "pinch_blend": round(self.mapper.last_pinch_blend, 4), + } + }, + } + for model, adapter in self.direct_mappers.items(): + if model == "G20": + continue + values["commands"][model] = { + "raw": np.round(adapter.last_raw_command, 2).tolist(), + "filtered": np.round(adapter.last_command, 2).tolist(), + "pinch_blend": round(adapter.last_pinch_blend, 4), + } + message = String() + message.data = json.dumps(values, ensure_ascii=False) + self.debug_publisher.publish(message) + self.last_debug_time = now + + def _draw_preview(self, frame: np.ndarray) -> None: + if not self.show_window: + return + mode = "REAL CONTROL" if self.control_enabled else "PREVIEW ONLY" + color = (0, 0, 255) if self.control_enabled else (0, 220, 255) + cv2.putText(frame, mode, (20, 36), cv2.FONT_HERSHEY_SIMPLEX, 0.9, color, 2) + cv2.putText( + frame, + f"hand={'OK' if self.hand_detected else 'LOST'} fps={self.measured_fps:.1f}", + (20, 70), + cv2.FONT_HERSHEY_SIMPLEX, + 0.65, + (60, 255, 60) if self.hand_detected else (0, 0, 255), + 2, + ) + session = self.calibration_session + if session is not None and session.active: + progress = int(round(100.0 * session.progress(time.monotonic()))) + cv2.putText( + frame, + f"CALIBRATING {session.pose.upper()}: {progress}% " + f"({len(session.samples)} samples)", + (20, 104), + cv2.FONT_HERSHEY_SIMPLEX, + 0.65, + (255, 180, 40), + 2, + ) + cv2.putText( + frame, + ( + "C: open+thumb out F: fist P: pinch E/D: enable/disable" + if self.dual_mode + else "C: calibrate open E: enable D: disable Q: quit" + ), + (20, frame.shape[0] - 20), + cv2.FONT_HERSHEY_SIMPLEX, + 0.6, + (255, 255, 255), + 2, + ) + cv2.imshow(self.window_name, frame) + key = cv2.waitKey(1) & 0xFF + if key in (ord("c"), ord("C")): + success, message = self._calibrate_open_pose() + if not success: + self.get_logger().warning(message) + elif key in (ord("f"), ord("F")): + success, message = self._calibrate_fist_pose() + if not success: + self.get_logger().warning(message) + elif key in (ord("p"), ord("P")): + success, message = self._calibrate_pinch_pose() + if not success: + self.get_logger().warning(message) + elif key in (ord("e"), ord("E")): + success, message = self._set_control_enabled(True) + if not success: + self.get_logger().warning(message) + elif key in (ord("d"), ord("D")): + self._set_control_enabled(False) + elif key in (ord("q"), ord("Q"), 27): + rclpy.shutdown() + + def _process_frame(self) -> None: + ok, frame = self.capture.read() + now = time.monotonic() + frame_interval = max(now - self.last_frame_time, 1e-6) + instantaneous_fps = 1.0 / frame_interval + self.measured_fps = 0.9 * self.measured_fps + 0.1 * instantaneous_fps + self.last_frame_time = now + + if not ok or frame is None: + self.hand_detected = False + self.stable_frames = 0 + self._update_calibration(None, now) + if ( + self.calibration_session is not None + and self.calibration_session.active + ): + self.status_message = ( + f"calibrating_{self.calibration_session.pose}" + ) + elif not ( + self.calibration_session is not None + and self.calibration_session.state == "failed" + ): + self.status_message = "camera_read_failed" + self._handle_driver_lost(now) + self._handle_detection_lost(now) + self._publish_status(now) + self._publish_debug(now) + return + + display_frame = cv2.flip(frame, 1) if self.selfie else frame.copy() + try: + detection = self.detector.detect(frame) + except (ValueError, np.linalg.LinAlgError) as error: + self.get_logger().warning(f"Invalid hand geometry: {error}") + detection = None + + if detection is None: + self.hand_detected = False + self.stable_frames = 0 + self._update_calibration(None, now) + if not ( + self.calibration_session is not None + and self.calibration_session.active + ): + self.status_message = "searching_left_hand" + self._handle_driver_lost(now) + self._handle_detection_lost(now) + else: + self.hand_detected = True + self.stable_frames += 1 + self.last_detection_time = now + self.last_detection_positions = detection.joint_positions.copy() + self.failsafe_sent = False + self.detector.draw_landmarks(display_frame, detection.image_landmarks) + self._update_calibration(detection.joint_positions, now) + + try: + if self.mapping_mode == "mediapipe_angles": + if self.mapper.is_calibrated: + if self.dual_mode: + features = self.mapper.extract_features( + detection.joint_positions + ) + commands = { + model: self.direct_mappers[model].map_features( + features + ) + for model in self.target_models + } + else: + commands = { + self.hand_model: self.mapper.map_landmarks( + detection.joint_positions + ) + } + else: + commands = self._open_commands() + else: + indices = self.retargeting.optimizer.target_link_human_indices + origin_indices = indices[0, :] + task_indices = indices[1, :] + reference = ( + detection.joint_positions[task_indices, :] + - detection.joint_positions[origin_indices, :] + ) + qpos = self.retargeting.retarget(reference) + commands = {self.hand_model: self.mapper.map(qpos)} + except Exception as error: + # A transient optimizer failure must behave like lost tracking, + # especially while the node is driving physical hardware. + self.hand_detected = False + self.stable_frames = 0 + self.status_message = "retargeting_failed" + self.get_logger().error(f"Retargeting failed: {error}") + self._handle_detection_lost(now) + self._publish_status(now) + self._publish_debug(now) + self._draw_preview(display_frame) + return + self.last_target_commands = commands + self.last_target_command = commands.get(self.hand_model) + for model, command in commands.items(): + self.preview_publishers[model].publish( + self._make_joint_state(model, command, include_speed=False) + ) + + just_enabled = False + calibration_active = ( + self.calibration_session is not None + and self.calibration_session.active + ) + if ( + self.auto_enable_requested + and not calibration_active + and self.stable_frames >= self.stable_frames_required + ): + just_enabled, _ = self._set_control_enabled(True) + self._handle_driver_lost(now) + if ( + self.control_enabled + and self.stable_frames >= self.stable_frames_required + and not just_enabled + ): + self._publish_control(commands) + self.status_message = "control_enabled" + elif not self.control_enabled: + if calibration_active: + self.status_message = ( + f"calibrating_{self.calibration_session.pose}" + ) + elif ( + self.calibration_session is not None + and self.calibration_session.state == "failed" + ): + self.status_message = ( + f"calibration_failed_{self.calibration_session.pose}" + ) + elif not getattr(self.mapper, "is_calibrated", True): + self.status_message = "needs_open_calibration" + elif self.dual_mode and not self.mapper.is_fist_calibrated: + self.status_message = "needs_fist_calibration" + elif self.dual_mode and not self.mapper.is_pinch_calibrated: + self.status_message = "needs_pinch_calibration" + elif self.status_message not in { + "driver_failsafe_open", + "failsafe_open", + "failsafe_hold", + }: + self.status_message = "preview_ready" + + self._publish_status(now) + self._publish_debug(now) + self._draw_preview(display_frame) + + def destroy_node(self) -> bool: + self.control_enabled = False + if hasattr(self, "capture"): + self.capture.release() + if hasattr(self, "detector"): + self.detector.close() + if self.show_window: + cv2.destroyAllWindows() + return super().destroy_node() + + +def main(args=None) -> None: + rclpy.init(args=args) + node: CameraHandRetargetNode | None = None + try: + node = CameraHandRetargetNode() + rclpy.spin(node) + except KeyboardInterrupt: + pass + finally: + if node is not None: + try: + node.destroy_node() + except KeyboardInterrupt: + pass + if rclpy.ok(): + rclpy.shutdown() + + +if __name__ == "__main__": + main() diff --git a/src/camera_retarget_l20/camera_retarget_l20/o6_mapping.py b/src/camera_retarget_l20/camera_retarget_l20/o6_mapping.py new file mode 100644 index 0000000..5adc0c8 --- /dev/null +++ b/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) diff --git a/src/camera_retarget_l20/config/direct_mapping_profiles.yml b/src/camera_retarget_l20/config/direct_mapping_profiles.yml new file mode 100644 index 0000000..41fa38f --- /dev/null +++ b/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 diff --git a/src/camera_retarget_l20/config/g20_left_mediapipe.yml b/src/camera_retarget_l20/config/g20_left_mediapipe.yml new file mode 100644 index 0000000..6fbe297 --- /dev/null +++ b/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 diff --git a/src/camera_retarget_l20/config/l20_left_mediapipe.yml b/src/camera_retarget_l20/config/l20_left_mediapipe.yml new file mode 100644 index 0000000..5a31b88 --- /dev/null +++ b/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 + diff --git a/src/camera_retarget_l20/launch/camera_g20_left.launch.py b/src/camera_retarget_l20/launch/camera_g20_left.launch.py new file mode 100644 index 0000000..bae6223 --- /dev/null +++ b/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]) diff --git a/src/camera_retarget_l20/launch/camera_l20_left.launch.py b/src/camera_retarget_l20/launch/camera_l20_left.launch.py new file mode 100644 index 0000000..f256fa5 --- /dev/null +++ b/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]) + diff --git a/src/camera_retarget_l20/launch/teleop_g20_left.launch.py b/src/camera_retarget_l20/launch/teleop_g20_left.launch.py new file mode 100644 index 0000000..a8db473 --- /dev/null +++ b/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]) diff --git a/src/camera_retarget_l20/launch/teleop_l20_left.launch.py b/src/camera_retarget_l20/launch/teleop_l20_left.launch.py new file mode 100644 index 0000000..5a8df05 --- /dev/null +++ b/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]) diff --git a/src/camera_retarget_l20/launch/teleop_o6_g20_left.launch.py b/src/camera_retarget_l20/launch/teleop_o6_g20_left.launch.py new file mode 100644 index 0000000..f04e55e --- /dev/null +++ b/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]) diff --git a/src/camera_retarget_l20/package.xml b/src/camera_retarget_l20/package.xml new file mode 100644 index 0000000..3070b16 --- /dev/null +++ b/src/camera_retarget_l20/package.xml @@ -0,0 +1,23 @@ + + + + camera_retarget_l20 + 0.2.0 + Camera retargeting bridge for physical left LinkerHand O6/G20/L20 hands. + lxp + MIT + + ament_index_python + rclpy + sensor_msgs + std_msgs + std_srvs + python3-opencv + python3-yaml + + python3-pytest + + + ament_python + + diff --git a/src/camera_retarget_l20/pyproject.toml b/src/camera_retarget_l20/pyproject.toml new file mode 100644 index 0000000..3372b34 --- /dev/null +++ b/src/camera_retarget_l20/pyproject.toml @@ -0,0 +1,4 @@ +[build-system] +requires = ["setuptools>=61"] +build-backend = "setuptools.build_meta" + diff --git a/src/camera_retarget_l20/resource/camera_retarget_l20 b/src/camera_retarget_l20/resource/camera_retarget_l20 new file mode 100644 index 0000000..8b13789 --- /dev/null +++ b/src/camera_retarget_l20/resource/camera_retarget_l20 @@ -0,0 +1 @@ + diff --git a/src/camera_retarget_l20/setup.cfg b/src/camera_retarget_l20/setup.cfg new file mode 100644 index 0000000..c0ce090 --- /dev/null +++ b/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 + diff --git a/src/camera_retarget_l20/setup.py b/src/camera_retarget_l20/setup.py new file mode 100644 index 0000000..2fd23b2 --- /dev/null +++ b/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", + ], + }, +) diff --git a/src/camera_retarget_l20/test/test_calibration_capture.py b/src/camera_retarget_l20/test/test_calibration_capture.py new file mode 100644 index 0000000..8a19af7 --- /dev/null +++ b/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" diff --git a/src/camera_retarget_l20/test/test_dual_calibration.py b/src/camera_retarget_l20/test/test_dual_calibration.py new file mode 100644 index 0000000..4737773 --- /dev/null +++ b/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] diff --git a/src/camera_retarget_l20/test/test_g20_mapping.py b/src/camera_retarget_l20/test/test_g20_mapping.py new file mode 100644 index 0000000..514a7a4 --- /dev/null +++ b/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] diff --git a/src/camera_retarget_l20/test/test_l20_mapping.py b/src/camera_retarget_l20/test/test_l20_mapping.py new file mode 100644 index 0000000..e1dffc5 --- /dev/null +++ b/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]) + diff --git a/src/camera_retarget_l20/test/test_model_profiles.py b/src/camera_retarget_l20/test/test_model_profiles.py new file mode 100644 index 0000000..fce9875 --- /dev/null +++ b/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 + ) diff --git a/src/camera_retarget_l20/test/test_o6_mapping.py b/src/camera_retarget_l20/test/test_o6_mapping.py new file mode 100644 index 0000000..3b199c5 --- /dev/null +++ b/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 diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py index f8716a5..6e7d22f 100644 --- a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py +++ b/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_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10) - self.hand_info_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_info', 10) + self.hand_cmd_sub = self.create_subscription( + JointState, + 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_pc = self.create_publisher(PointCloud2, 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_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, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_pc'), 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_pc = self.create_publisher(PointCloud2, 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_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, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_pc'), 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: - self.last_hand_post_cmd = msg.position - if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True: - self.last_hand_vel_cmd = msg.velocity - if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True: - self.last_hand_eff_cmd = msg.effort + # The hardware can be slower than the camera. Always replace a + # pending command with the newest sample and never replay an already + # applied sample; this prevents latency from accumulating in software. + position = list(msg.position) + if position: + 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: - # 优先获取手指状态并且发布 - self.last_hand_state = self.api.get_state() - time.sleep(0.003) - self.last_hand_vel = self.api.get_joint_speed() - time.sleep(0.002) + # Position commands have priority over synchronous state reads. + self._apply_pending_commands() + self._poll_state_if_due() 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: """单点式压力传感器""" diff --git a/src/linker_hand_ros2_sdk/test/test_topic_prefix.py b/src/linker_hand_ros2_sdk/test/test_topic_prefix.py new file mode 100644 index 0000000..f3f7931 --- /dev/null +++ b/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)