摄像头控制O6和G20
This commit is contained in:
+37
-1
@@ -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_*/
|
||||
|
||||
@@ -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`。标定时姿势越标准、越稳定，控制效果越好。
|
||||
@@ -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
|
||||
```
|
||||
@@ -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
|
||||
```
|
||||
|
||||
这会更稳定，但会增加少量跟随延迟。
|
||||
@@ -0,0 +1,2 @@
|
||||
"""Camera-based retargeting bridge for the LinkerHand L20."""
|
||||
|
||||
@@ -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
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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(),
|
||||
)
|
||||
|
||||
@@ -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)),
|
||||
)
|
||||
@@ -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()
|
||||
|
||||
@@ -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()
|
||||
File diff suppressed because it is too large
Load Diff
@@ -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)
|
||||
@@ -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
|
||||
@@ -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
|
||||
@@ -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
|
||||
|
||||
@@ -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])
|
||||
@@ -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])
|
||||
|
||||
@@ -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])
|
||||
@@ -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])
|
||||
@@ -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])
|
||||
@@ -0,0 +1,23 @@
|
||||
<?xml version="1.0"?>
|
||||
<?xml-model href="http://download.ros.org/schema/package_format3.xsd" schematypens="http://www.w3.org/2001/XMLSchema"?>
|
||||
<package format="3">
|
||||
<name>camera_retarget_l20</name>
|
||||
<version>0.2.0</version>
|
||||
<description>Camera retargeting bridge for physical left LinkerHand O6/G20/L20 hands.</description>
|
||||
<maintainer email="support@linker-robotics.com">lxp</maintainer>
|
||||
<license>MIT</license>
|
||||
|
||||
<exec_depend>ament_index_python</exec_depend>
|
||||
<exec_depend>rclpy</exec_depend>
|
||||
<exec_depend>sensor_msgs</exec_depend>
|
||||
<exec_depend>std_msgs</exec_depend>
|
||||
<exec_depend>std_srvs</exec_depend>
|
||||
<exec_depend>python3-opencv</exec_depend>
|
||||
<exec_depend>python3-yaml</exec_depend>
|
||||
|
||||
<test_depend>python3-pytest</test_depend>
|
||||
|
||||
<export>
|
||||
<build_type>ament_python</build_type>
|
||||
</export>
|
||||
</package>
|
||||
@@ -0,0 +1,4 @@
|
||||
[build-system]
|
||||
requires = ["setuptools>=61"]
|
||||
build-backend = "setuptools.build_meta"
|
||||
|
||||
@@ -0,0 +1 @@
|
||||
|
||||
@@ -0,0 +1,5 @@
|
||||
[develop]
|
||||
script_dir=$base/lib/camera_retarget_l20
|
||||
[install]
|
||||
install_scripts=$base/lib/camera_retarget_l20
|
||||
|
||||
@@ -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",
|
||||
],
|
||||
},
|
||||
)
|
||||
@@ -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"
|
||||
@@ -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]
|
||||
@@ -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]
|
||||
@@ -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])
|
||||
|
||||
@@ -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
|
||||
)
|
||||
@@ -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
|
||||
@@ -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)
|
||||
@@ -27,6 +51,18 @@ class LinkerHand(Node):
|
||||
self.declare_parameter('is_touch', False)
|
||||
self.declare_parameter('can', 'can0')
|
||||
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,39 +253,43 @@ 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
|
||||
)
|
||||
|
||||
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)
|
||||
if self.cmd_lock == False:
|
||||
if self.last_hand_post_cmd != None:
|
||||
self.api.finger_move(pose=self.last_hand_post_cmd)
|
||||
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 != None:
|
||||
|
||||
if self.last_hand_vel_cmd is not None:
|
||||
vel = list(self.last_hand_vel_cmd)
|
||||
if all(x == 0 for x in vel):
|
||||
pass
|
||||
else:
|
||||
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)
|
||||
@@ -196,13 +302,47 @@ class LinkerHand(Node):
|
||||
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
|
||||
# Position commands have priority over synchronous state reads.
|
||||
self._apply_pending_commands()
|
||||
self._poll_state_if_due()
|
||||
if self.cmd_lock == False:
|
||||
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:
|
||||
"""单点式压力传感器"""
|
||||
|
||||
@@ -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)
|
||||
Reference in New Issue
Block a user