手套-真手-mujoco仿真延迟测量完整版

.gitattributes

@@ -0,0 +1 @@
+*.STL filter=lfs diff=lfs merge=lfs -text

.gitignore

@@ -0,0 +1,61 @@
+# ROS 2 / colcon build outputs
+/build/
+/install/
+/log/
+/test_results/
+
+# Python virtual environments
+.venv/
+venv/
+ENV/
+
+# Python bytecode and caches
+__pycache__/
+*.py[cod]
+*$py.class
+
+# 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 and CAN captures
+rosbag2_*/
+*.db3
+*.mcap
+candump-*
+l10_*_state_*/

Readme.md

@@ -0,0 +1,419 @@
+# 1. LinkerFFG Glove
+
+## 1.1 Product Introduction
+For specific product introduction, please refer to the calibration example description contained within
+Attachment 1: Linker FFG (FFG01) Product Instruction Manual
+
+## 1.2 Software Usage
+This product provides ROS1 and ROS2 SDK packages under Ubuntu, working together with Linkerhand's ROS1 SDK and ROS2 SDK to achieve teleoperation actions
+For specific usage, please jump to Chapter 4: Teleoperation Retargeting System
+
+## 1.3 Communication Protocol
+For specific product introduction, please refer to Attachment 2: Linker FFG (FFG01) Communication Protocol
+
+---
+
+# 2. Teleoperation SDK System
+
+## 2.1 System Introduction
+This system's retargeting program is developed based on the ROS1/ROS2 platform, consisting of one node, four publisher topics, and two subscription topics
+The Linker FFG glove pushes data to the retargeting program via serial port protocol
+
+### 2.1.1 Download Address
+https://github.com/linker-bot/linkerhand-ros-teleo
+Please contact customer service to obtain the latest SDK
+
+### 2.1.2 Installing the Teleoperation Retargeting Program
+The teleoperation retargeting program is provided in src form, therefore requiring users to compile it themselves for use on different platforms. The teleoperation retargeting program provides a ROS2 version; the ROS1 version is still under development
+
+Copy src to the workspace, then compile
+```bash
+colcon build --symlink-install
+```
+
+There may be permission issues during the process preventing execution; use the following command
+```bash
+chmod 777 -R *
+```
+
+### 2.1.3 Program Configuration
+Enter the ls command in this directory
+```bash
+cd src/linkerhand_retarget/linkerhand_retarget/config
+ls -1
+```
+After entering the above command, you should normally enter the config directory. After `ls -1`, you should see the following list
+```
+base_config.yml
+body_custom_pose.yml
+body_unity_pose.yml
+hand_config.yml
+human_hand_info.yml
+linker_hand_info.yml
+model_config.yml
+retarget_config.yml
+speed_config.yml
+```
+
+The only configuration file we need to modify is `base_config.yml`; other configuration files must not be modified.
+
+The following is an introduction to the configuration content in `base_config.yml`:
+
+**1. Configure Robot Hand Type**
+```yaml
+robotname_r: l10
+robotname_l: l10
+```
+- LinkerHand_L10 series hands should be configured as: `l10`
+- LinkerHand_L20 series hands should be configured as: `l20`
+- LinkerHand_O7 series hands should be configured as: `o7`
+- LinkerHand_T25 series hands should be configured as: `t25`
+- LinkerHand_L25 series hands should be configured as: `l25`
+- LinkerHand_L6 series hands should be configured as: `l6`
+- LinkerHand_O6 series hands should be configured as: `o6`
+
+**Note:** All the above must be in lowercase; uppercase will not be correctly recognized.
+
+**2. Configure Data Printing**
+```yaml
+debug:
+  joint_pub_debug: false
+  joint_motor_debug_r: true
+  joint_motor_debug_l: false
+```
+- `joint_pub_debug`: Set to true to print the content after Topic Pub, refresh rate is slow, can display left and right hands simultaneously
+- `joint_motor_debug_r`: Set to true to quickly refresh the output data of the right hand, range 255-0, representing the change from open to closed
+- `joint_motor_debug_l`: Set to true to quickly refresh the output data of the left hand, range 255-0, representing the change from open to closed
+
+**3. Configure Data Source**
+```yaml
+motion_type: linkerforce
+```
+- `motion_type`: When using the LinkerFFG glove, set to `linkerforce`, `motion_device` does not need to be set
+
+### 2.1.4 Program Launch (ROS2 Version)
+Next, execute the following command to confirm the USB device list on your device. Then insert the 2 force-feedback gloves, execute the command again, and confirm the refreshed USB device numbers. Take `/dev/ttyUSB0` and `/dev/ttyUSB1` as examples:
+```bash
+ls /dev/ttyUSB*
+sudo chmod 666 /dev/ttyUSB0
+sudo chmod 666 /dev/ttyUSB1
+```
+
+Then execute
+```bash
+# There are two launch modes,
+# One with calibration. Must be executed for the first time without calibration
+ros2 run linkerhand_retarget handretarget --ros-args -p calibration := True
+# One normal launch, calibration not executed by default
+ros2 run linkerhand_retarget handretarget 
+```
+
+**Notes:**
+For customer convenience, a startup script is provided in the same directory as `src`, named `startup_linkerforce.sh`
+
+```bash
+#!/bin/bash
+# LinkerForce Glove Teleoperation Startup Script
+
+# Set serial port permissions
+sudo chmod 666 /dev/ttyUSB0
+sudo chmod 666 /dev/ttyUSB1
+
+# source ROS workspace (please enter the correct directory)
+source ~/project/linkerhand_telop_ws/ros2/v2.8.7/install/setup.bash
+
+# Launch program (forced calibration)
+ros2 run linkerhand_retarget handretarget --ros-args -p calibration:=True
+
+# Launch without calibration
+# ros2 run linkerhand_retarget handretarget 
+```
+
+After editing, launch it like this
+```bash
+./startup_linkerforce.sh
+# If you encounter Tab not recognized, grant permissions
+sudo chmod a+x ./startup_linkerforce.sh
+```
+
+### 2.1.5 Program Launch (ROS1 Version)
+Next, execute the following command to confirm the USB device list on your device. Then insert the 2 force-feedback gloves, execute the command again, and confirm the refreshed USB device numbers. Take `/dev/ttyUSB0` and `/dev/ttyUSB1` as examples:
+```bash
+ls /dev/ttyUSB*
+sudo chmod 666 /dev/ttyUSB0
+sudo chmod 666 /dev/ttyUSB1
+```
+
+Then execute
+```bash
+# There are two launch modes,
+# One with calibration. Must be executed for the first time without calibration
+rosrun ros_linkerhand_retarget handretarget.py _calibrate:=true
+# One normal launch, calibration not executed by default
+rosrun ros_linkerhand_retarget handretarget.py
+```
+
+**Notes:**
+For customer convenience, a startup script is provided in the same directory as `src`, named `startup_linkerforce.sh`
+
+```bash
+#!/bin/bash
+# LinkerForce Glove Teleoperation Startup Script
+
+# Set serial port permissions
+sudo chmod 666 /dev/ttyUSB0
+sudo chmod 666 /dev/ttyUSB1
+
+# source ROS workspace (please enter the correct directory)
+source ~/linkerhand_telop/ros1/v2.8.6/install/setup.bash
+
+# Launch program (forced calibration)
+rosrun ros_linkerhand_retarget handretarget.py _calibrate:=true
+
+# Launch without calibration
+# rosrun ros_linkerhand_retarget handretarget.py
+```
+
+After editing, launch it like this
+```bash
+./startup_linkerforce.sh
+# If you encounter Tab not recognized, grant permissions
+sudo chmod a+x ./startup_linkerforce.sh
+```
+
+## 2.2 Topic Description
+The working principle of the SDK is to clean and organize the data transmitted from the upper computer, then distribute the data uniformly via Topics. Therefore, Topics are the data transfer stations and an important basis for judging whether the teleoperation is working properly.
+
+- ROS1 command: `rostopic list`
+- ROS2 command: `ros2 topic list`
+
+**1. ROS2 command:**
+```bash
+ros2 topic echo /cb_left_hand_control_cmd
+# or
+ros2 topic echo /cb_right_hand_control_cmd
+```
+
+### Data Description
+
+#### L25 Robotic Hand (21 DOF)
+| Joint | Finger | Action | Default Value |
+|-------|--------|--------|---------------|
+| joint1 | Thumb | Proximal Flexion | 255 |
+| joint2 | Index | Proximal Flexion | 255 |
+| joint3 | Middle | Proximal Flexion | 255 |
+| joint4 | Ring | Proximal Flexion | 255 |
+| joint5 | Pinky | Proximal Flexion | 255 |
+| joint6 | Thumb | Abduction | 255 |
+| joint7 | Index | Abduction | 128 |
+| joint8 | Middle | Abduction | 128 |
+| joint9 | Ring | Abduction | 128 |
+| joint10 | Pinky | Abduction | 128 |
+| joint11 | Thumb | Rotation | 255 |
+| joint12 | Reserved | - | - |
+| joint13 | Reserved | - | - |
+| joint14 | Reserved | - | - |
+| joint15 | Reserved | - | - |
+| joint16 | Thumb | Distal Flexion | 255 |
+| joint17 | Index | Distal Flexion | 255 |
+| joint18 | Middle | Distal Flexion | 255 |
+| joint19 | Ring | Distal Flexion | 255 |
+| joint20 | Pinky | Distal Flexion | 255 |
+| joint21 | Thumb | Tip Flexion | 255 |
+| joint22 | Index | Tip Flexion | 255 |
+| joint23 | Middle | Tip Flexion | 255 |
+| joint24 | Ring | Tip Flexion | 255 |
+| joint25 | Pinky | Tip Flexion | 255 |
+
+#### L20 Robotic Hand (16 DOF)
+| Joint | Finger | Action | Default Value |
+|-------|--------|--------|---------------|
+| joint1 | Thumb | Proximal Flexion | 255 |
+| joint2 | Index | Proximal Flexion | 255 |
+| joint3 | Middle | Proximal Flexion | 255 |
+| joint4 | Ring | Proximal Flexion | 255 |
+| joint5 | Pinky | Proximal Flexion | 255 |
+| joint6 | Thumb | Abduction | 255 |
+| joint7 | Index | Abduction | 128 |
+| joint8 | Middle | Abduction | 128 |
+| joint9 | Ring | Abduction | 128 |
+| joint10 | Pinky | Abduction | 128 |
+| joint11 | Thumb | Rotation | 255 |
+| joint12 | Reserved | - | - |
+| joint13 | Reserved | - | - |
+| joint14 | Reserved | - | - |
+| joint15 | Reserved | - | - |
+| joint16 | Thumb | Tip Flexion | 255 |
+| joint17 | Index | Tip Flexion | 255 |
+| joint18 | Middle | Tip Flexion | 255 |
+| joint19 | Ring | Tip Flexion | 255 |
+| joint20 | Pinky | Tip Flexion | 255 |
+
+#### L10 Robotic Hand (10 DOF)
+| Joint | Finger | Action | Default Value |
+|-------|--------|--------|---------------|
+| joint1 | Thumb | Proximal Flexion | 255 |
+| joint2 | Thumb | Abduction | 128 |
+| joint3 | Index | Proximal Flexion | 255 |
+| joint4 | Middle | Proximal Flexion | 255 |
+| joint5 | Ring | Proximal Flexion | 255 |
+| joint6 | Pinky | Proximal Flexion | 255 |
+| joint7 | Index | Abduction | 128 |
+| joint8 | Ring | Abduction | 128 |
+| joint9 | Pinky | Abduction | 128 |
+| joint10 | Thumb | Rotation | 255 |
+
+#### O7 Robotic Hand (7 DOF)
+| Joint | Finger | Action | Default Value |
+|-------|--------|--------|---------------|
+| joint1 | Thumb | Proximal Flexion | 255 |
+| joint2 | Thumb | Abduction | 128 |
+| joint3 | Index | Proximal Flexion | 255 |
+| joint4 | Middle | Proximal Flexion | 255 |
+| joint5 | Ring | Proximal Flexion | 255 |
+| joint6 | Pinky | Proximal Flexion | 255 |
+| joint7 | Thumb | Rotation | 255 |
+
+#### O6/L6 Robotic Hand (7 DOF)
+| Joint | Finger | Action | Default Value |
+|-------|--------|--------|---------------|
+| joint1 | Thumb | Proximal Flexion | 255 |
+| joint2 | Thumb | Abduction | 128 |
+| joint3 | Index | Proximal Flexion | 255 |
+| joint4 | Middle | Proximal Flexion | 255 |
+| joint5 | Ring | Proximal Flexion | 255 |
+| joint6 | Pinky | Proximal Flexion | 255 |
+
+## 2.3 Troubleshooting Self-Check
+
+### 2.3.1 Network Issues
+The teleoperation and control devices are connected via the network. Network abnormalities can cause devices to malfunction. First, check if the network cable connections are correct, ensure that both IPs can ping each other, and confirm that the corresponding ports are open on the firewall.
+
+### 2.3.2 Topic Anomalies
+Under normal network conditions, you can confirm whether topics exist on other network devices using commands.
+Refer to section 2.2 Topic Description for details.
+
+## 2.4 Mapping Parameter Settings
+
+### 2.4.1 Principle
+The mapping system has 3 modes
+
+| Mapping Mode | English Equivalent | Chinese Equivalent | Applicable Scenario |
+|--------------|--------------------|--------------------|---------------------|
+| Three-State Mapping | origin-opose-fist | Open Hand-O Pose-Fist | Aims to balance the maximum range of the robotic hand while satisfying pinch gestures. Suitable for most mapping requirements. |
+| Dual-End Mapping | origin-fist | Open Hand-Fist | Maximizes the full range of the robotic hand to the greatest extent. |
+| Pinch Mapping | origin-opose- | Open Hand-O Pose | Targets pinch gestures primarily, aiming to improve granularity for grasping actions. |
+
+### 2.4.2 Configuration Files
+This mapping system configures parameter binding for the robotic hand. The mapping configuration directory is located at:
+```
+~/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config
+```
+
+Currently provided mapping parameter packages are as follows:
+```
+g20_config.py
+l10_config.py
+l20_config.py
+l6_config.py
+o6_config.py
+o7_config.py
+```
+
+### 2.4.3 Configuration Introduction
+The following uses `l6_config.py` as an example.
+
+#### 2.4.3.1 Finger Configuration Constant `FINGER_CONFIGS`
+
+##### 2.4.3.1.1 Common Constant Table
+(Content omitted)
+
+##### 2.4.3.1.2 `reverse_motion` Attribute
+Reverse signal: Default is `False`
+
+##### 2.4.3.1.3 `dynamic_weight` Attribute
+Dynamic weight: Used for detailed expression within intervals. Default is `None`, not enabled in this version.
+
+##### 2.4.3.1.4 `extended_mapping` Attribute
+Extended mapping configuration
+```python
+'extended_mapping': {    # New extended mapping configuration
+    'enabled': True,
+    'scale_factor': 1.2,  # Scaling factor, default 1.0
+    'extended_exp_factor': 80  # New: Extension index parameter
+}
+```
+- `enabled`: Enabled only for pinch mapping; `False` means disabled.
+- `scale_factor`: Scaling factor, used to quickly lock onto the O-Pose position.
+- `extended_exp_factor`: Extension parameter, used for continuation actions after the O-Pose. A smaller value makes it harder to reach the mechanical limits of the robotic hand, allowing for more detailed expression of grasping. A larger value makes it easier to quickly reach the drive joint limit positions.
+
+#### 2.4.3.2 Mapping Order `MAPPING_ORDER`
+Taking L6 as an example, indicates the priority order of processed joints
+```python
+MAPPING_ORDER = [
+    'thumb_abduction', 'thumb_flexion', 
+    'index', 'middle', 'ring', 'pinky'
+]
+```
+
+#### 2.4.3.3 State Configuration `STATE_CONFIG` (Not Enabled)
+
+#### 2.4.3.4 Robotic Hand OPOSE Calibration Position: `ROBOT_OPOSE_LEFT`
+Taking the L6 configuration as an example, the following angles correspond to the OPOSE position of the left robotic hand. Note that the following order corresponds to the URDF sequence.
+```python
+ROBOT_OPOSE_LEFT = [
+    1.4, 0.54, 0.0, 0.38, 0.0, 0.38, 0.0, 0.38, 0.0, 0.38, 0.0
+]
+```
+
+#### 2.4.3.5 Robotic Hand OPOSE Calibration Position: `ROBOT_OPOSE_RIGHT`
+Taking the L6 configuration as an example, the following angles correspond to the OPOSE position of the right robotic hand. Note that the following order corresponds to the URDF sequence.
+```python
+ROBOT_OPOSE_RIGHT = [
+    1.4, 0.54, 0.0, 0.38, 0.0, 0.38, 0.0, 0.38, 0.0, 0.38, 0.0
+]
+```
+
+### 2.4.4 Robotic Hand Calibration
+Sections 2.4.3.4 and 2.4.3.5 introduced the OPOSE calibration positions for the left and right robotic hands respectively. These are used to handle discrepancies between the current robotic hand position and the URDF. When using for the first time, modifications need to be made in the following code.
+
+Path: `./src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand`
+
+Taking the L6 robotic hand as an example, after opening `linkerforce_l6.py`, around line 149:
+```python
+if self.calibrationoriginal is not None 
+and self.calibrationfistpose is not None 
+and self.calibrationopose is not None:
+    # for i in range(20):
+    #     self.multi_state_mapper.debug_value[i] = joint_arc[i]
+    # arc_value = self.multi_state_mapper.map_glove_to_robot(joint_arc)   # Comment out
+    arc_value = ROBOT_OPOSE_RIGHT          # Enable calibration
+    qpos[17] = self.g_jointpositions_arc[1] = arc_value[0]
+    qpos[20] = self.g_jointpositions_arc[0] = arc_value[1]
+    qpos[1] = self.g_jointpositions_arc[2] = arc_value[3]
+    qpos[9] = self.g_jointpositions_arc[3] = arc_value[5]
+    qpos[13] = self.g_jointpositions_arc[4] = arc_value[7]
+    qpos[5] = self.g_jointpositions_arc[5] = arc_value[9]
+```
+
+Rewrite it as shown in the example above to enable calibration for the right robotic hand. After launching, the robotic hand will be fixed at the current angle according to the mapped angle.
+
+The corresponding section for the left hand is around line 360:
+```python
+if self.calibrationoriginal is not None and self.calibrationfistpose is not None and self.calibrationopose is not None:
+    # for i in range(20):
+    #     self.multi_state_mapper.debug_value[i] = joint_arc[i]
+    # arc_value = self.multi_state_mapper.map_glove_to_robot(joint_arc) # Comment out
+    # Enable calibration
+    arc_value = ROBOT_OPOSE_LEFT
+    qpos[17] = self.g_jointpositions_arc[1] = arc_value[0]
+    qpos[20] = self.g_jointpositions_arc[0] = arc_value[1]
+    qpos[1] = self.g_jointpositions_arc[2] = arc_value[3]
+    qpos[9] = self.g_jointpositions_arc[3] = arc_value[5]
+    qpos[13] = self.g_jointpositions_arc[4] = arc_value[7]
+    qpos[5] = self.g_jointpositions_arc[5] = arc_value[9]
+```
+
+Rewrite it as shown in the example above to enable calibration for the right robotic hand. After launching, the robotic hand will be fixed at the current angle according to the mapped angle.
+
+Once both left and right hands have reached the desired pinch position, you can restore the original state and use the teleoperation system normally.

Readme_ZH.md

@@ -0,0 +1,419 @@
+# 1. LinkerFFG手套
+
+## 1.1 产品介绍
+本产品的具体介绍参考，内含标定示例说明
+附件1、Linker FFG(FFG01)产品说明手册
+
+## 1.2 软件使用
+本产品提供了Ubuntu下的ROS1和ROS2 SDK包，配合Linkerhand的ROS1 SDK，ROS2 SDK实现遥操作
+具体使用请跳转到第四章节 遥操重定向系统
+
+## 1.3 通讯协议
+本产品的具体介绍参考附件2、Linker FFG(FFG01) 通讯协议
+
+---
+
+# 2. 遥操SDK系统
+
+## 2.1 系统介绍
+本系统重定向程序是基于ROS1/ROS2平台开发的，由一个节点和四个publisher的话题以及两个subscription的话题组成
+Linker FFG手套通过串口协议进行数据推送到重定向程序
+
+### 2.1.1 下载地址
+https://github.com/linker-bot/linkerhand-ros-teleo
+请联系客服获取最新SDK
+
+### 2.1.2 安装遥操重定向程序
+遥操重定向程序以src的形式提供，因此需要用户自行编译，才可以在多个不同的平台下使用，遥操重定向程序提供了ROS2版本,ROS1版尚在开发中
+
+将src复制到工作空间中，然后进行编译
+```bash
+colcon build --symlink-install
+```
+
+过程中可能会存在权限不足的问题导致无法运行，使用以下代码
+```bash
+chmod 777 -R *
+```
+
+### 2.1.3 程序配置
+该目录输入ls命令
+```bash
+cd src/linkerhand_retarget/linkerhand_retarget/config
+ls -1
+```
+输入以上命令后正常应该进入config的目录，ls -1后即可看到以下清单
+```
+base_config.yml
+body_custom_pose.yml
+body_unity_pose.yml
+hand_config.yml
+human_hand_info.yml
+linker_hand_info.yml
+model_config.yml
+retarget_config.yml
+speed_config.yml
+```
+
+其中我们要唯一修改的配置文件是base_config.yml，其他配置文件均不可修改
+
+以下是针对base_config.yml的配置内容展开进行介绍：
+
+**1. 配置机械手类型**
+```yaml
+robotname_r: l10
+robotname_l: l10
+```
+- LinkerHand_L10系列的手要配置成:l10
+- LinkerHand_L20系列的手要配置成:l20
+- LinkerHand_O7系列的手要配置成:o7
+- LinkerHand_T25系列的手要配置成:t25
+- LinkerHand_L25系列的手要配置成:l25
+- LinkerHand_L6系列的手要配置成:l6
+- LinkerHand_O6系列的手要配置成:o6
+
+注意事项：以上均为小写，大写不会被正确识别
+
+**2. 配置数据打印**
+```yaml
+debug:
+  joint_pub_debug: false
+  joint_motor_debug_r: true
+  joint_motor_debug_l: false
+```
+- joint_pub_debug:设置为true可打印Topic Pub后的内容，刷新率较慢，可左右手同时显示
+- joint_motor_debug_r:设置为true可快速刷新右手的输出数据，范围在255-0，代表张开到聚拢的变化幅度
+- joint_motor_debug_l:设置为true可快速刷新左手的输出数据，范围在255-0，代表张开到聚拢的变化幅度
+
+**3. 配置数据源**
+```yaml
+motion_type: linkerforce
+```
+- motion_type:使用LinkerFFG手套要设置为linkerforce，motion_device无需设置
+
+### 2.1.4 程序启动(ROS2版)
+之后执行以下代码确认本设备的USB清单，然后分别插入2个力反馈手套，再次执行命令，确认刷新出来的USB设备序号，以/dev/ttyUSB0和/dev/ttyUSB1为例
+```bash
+ls /dev/ttyUSB*
+sudo chmod 666 /dev/ttyUSB0
+sudo chmod 666 /dev/ttyUSB1
+```
+
+然后执行
+```bash
+# 启动分为两种，
+# 一种带标定。第一次没标定过必须执行
+ros2 run linkerhand_retarget handretarget --ros-args -p calibration := True
+# 一种正常启动，默认不执行标定
+ros2 run linkerhand_retarget handretarget 
+```
+
+**注意事项：**
+为了客户启动方便，提供了一个启动脚本，在src同级目录下，文件名为startup_linkerforce.sh
+
+```bash
+#!/bin/bash
+# LinkerForce 手套遥操作启动脚本
+
+# 设置串口权限
+sudo chmod 666 /dev/ttyUSB0
+sudo chmod 666 /dev/ttyUSB1
+
+# source ROS 工作空间(请输入正确的目录)
+source ~/project/linkerhand_telop_ws/ros2/v2.8.7/install/setup.bash
+
+# 启动程序（强制标定）
+ros2 run linkerhand_retarget handretarget --ros-args -p calibration:=True
+
+# 无标定启动
+# ros2 run linkerhand_retarget handretarget 
+```
+
+编辑完成后即可这样启动
+```bash
+./startup_linkerforce.sh
+# 如果遇到Tab无法识别的情况，请赋予权限
+sudo chmod a+x ./startup_linkerforce.sh
+```
+
+### 2.1.5 程序启动(ROS1版)
+之后执行以下代码确认本设备的USB清单，然后分别插入2个力反馈手套，再次执行命令，确认刷新出来的USB设备序号，以/dev/ttyUSB0和/dev/ttyUSB1为例
+```bash
+ls /dev/ttyUSB*
+sudo chmod 666 /dev/ttyUSB0
+sudo chmod 666 /dev/ttyUSB1
+```
+
+然后执行
+```bash
+# 启动分为两种，
+# 一种带标定。第一次没标定过必须执行
+rosrun ros_linkerhand_retarget handretarget.py _calibrate:=true
+# 一种正常启动，默认不执行标定
+rosrun ros_linkerhand_retarget handretarget.py
+```
+
+**注意事项：**
+为了客户启动方便，提供了一个启动脚本，在src同级目录下，文件名为startup_linkerforce.sh
+
+```bash
+#!/bin/bash
+# LinkerForce 手套遥操作启动脚本
+
+# 设置串口权限
+sudo chmod 666 /dev/ttyUSB0
+sudo chmod 666 /dev/ttyUSB1
+
+# source ROS 工作空间(请输入正确的目录)
+source ~/linkerhand_telop/ros1/v2.8.6/install/setup.bash
+
+# 启动程序（强制标定）
+rosrun ros_linkerhand_retarget handretarget.py _calibrate:=true
+
+# 无标定启动
+# rosrun ros_linkerhand_retarget handretarget.py
+```
+
+编辑完成后即可这样启动
+```bash
+./startup_linkerforce.sh
+# 如果遇到Tab无法识别的情况，请赋予权限
+sudo chmod a+x ./startup_linkerforce.sh
+```
+
+## 2.2 Topic话题说明
+SDK的工作原理是将上位机传输过来的数据经过清洗和整理后，统一通过Topic的方式进行数据分发，因此Topic是数据中转站，也是判断遥操工作是否正常的重要依据
+
+- ROS1命令：`rostopic list`
+- ROS2命令：`ros2 topic list`
+
+**1. ROS2命令：**
+```bash
+ros2 topic echo /cb_left_hand_control_cmd
+# 或
+ros2 topic echo /cb_right_hand_control_cmd
+```
+
+### 数据说明
+
+#### L25机械手（21个自由度）
+| 关节 | 手指 | 动作 | 默认值 |
+|------|------|------|--------|
+| joint1 | 拇指 | 根部弯曲 | 255 |
+| joint2 | 食指 | 根部弯曲 | 255 |
+| joint3 | 中指 | 根部弯曲 | 255 |
+| joint4 | 无名指 | 根部弯曲 | 255 |
+| joint5 | 小指 | 根部弯曲 | 255 |
+| joint6 | 拇指 | 侧摆 | 255 |
+| joint7 | 食指 | 侧摆 | 128 |
+| joint8 | 中指 | 侧摆 | 128 |
+| joint9 | 无名指 | 侧摆 | 128 |
+| joint10 | 小指 | 侧摆 | 128 |
+| joint11 | 拇指 | 旋转 | 255 |
+| joint12 | 预留 | - | - |
+| joint13 | 预留 | - | - |
+| joint14 | 预留 | - | - |
+| joint15 | 预留 | - | - |
+| joint16 | 拇指 | 第二关节弯曲 | 255 |
+| joint17 | 食指 | 第二关节弯曲 | 255 |
+| joint18 | 中指 | 第二关节弯曲 | 255 |
+| joint19 | 无名指 | 第二关节弯曲 | 255 |
+| joint20 | 小指 | 第二关节弯曲 | 255 |
+| joint21 | 拇指 | 末端弯曲 | 255 |
+| joint22 | 食指 | 末端弯曲 | 255 |
+| joint23 | 中指 | 末端弯曲 | 255 |
+| joint24 | 无名指 | 末端弯曲 | 255 |
+| joint25 | 小指 | 末端弯曲 | 255 |
+
+#### L20机械手（16个自由度）
+| 关节 | 手指 | 动作 | 默认值 |
+|------|------|------|--------|
+| joint1 | 拇指 | 根部弯曲 | 255 |
+| joint2 | 食指 | 根部弯曲 | 255 |
+| joint3 | 中指 | 根部弯曲 | 255 |
+| joint4 | 无名指 | 根部弯曲 | 255 |
+| joint5 | 小指 | 根部弯曲 | 255 |
+| joint6 | 拇指 | 侧摆 | 255 |
+| joint7 | 食指 | 侧摆 | 128 |
+| joint8 | 中指 | 侧摆 | 128 |
+| joint9 | 无名指 | 侧摆 | 128 |
+| joint10 | 小指 | 侧摆 | 128 |
+| joint11 | 拇指 | 旋转 | 255 |
+| joint12 | 预留 | - | - |
+| joint13 | 预留 | - | - |
+| joint14 | 预留 | - | - |
+| joint15 | 预留 | - | - |
+| joint16 | 拇指 | 末端弯曲 | 255 |
+| joint17 | 食指 | 末端弯曲 | 255 |
+| joint18 | 中指 | 末端弯曲 | 255 |
+| joint19 | 无名指 | 末端弯曲 | 255 |
+| joint20 | 小指 | 末端弯曲 | 255 |
+
+#### L10机械手（10个自由度）
+| 关节 | 手指 | 动作 | 默认值 |
+|------|------|------|--------|
+| joint1 | 拇指 | 根部弯曲 | 255 |
+| joint2 | 拇指 | 侧摆 | 128 |
+| joint3 | 食指 | 根部弯曲 | 255 |
+| joint4 | 中指 | 根部弯曲 | 255 |
+| joint5 | 无名指 | 根部弯曲 | 255 |
+| joint6 | 小指 | 根部弯曲 | 255 |
+| joint7 | 食指 | 侧摆 | 128 |
+| joint8 | 无名指 | 侧摆 | 128 |
+| joint9 | 小指 | 侧摆 | 128 |
+| joint10 | 拇指 | 旋转 | 255 |
+
+#### O7机械手（7个自由度）
+| 关节 | 手指 | 动作 | 默认值 |
+|------|------|------|--------|
+| joint1 | 拇指 | 根部弯曲 | 255 |
+| joint2 | 拇指 | 侧摆 | 128 |
+| joint3 | 食指 | 根部弯曲 | 255 |
+| joint4 | 中指 | 根部弯曲 | 255 |
+| joint5 | 无名指 | 根部弯曲 | 255 |
+| joint6 | 小指 | 根部弯曲 | 255 |
+| joint7 | 拇指 | 旋转 | 255 |
+
+#### O6/L6机械手（7个自由度）
+| 关节 | 手指 | 动作 | 默认值 |
+|------|------|------|--------|
+| joint1 | 拇指 | 根部弯曲 | 255 |
+| joint2 | 拇指 | 侧摆 | 128 |
+| joint3 | 食指 | 根部弯曲 | 255 |
+| joint4 | 中指 | 根部弯曲 | 255 |
+| joint5 | 无名指 | 根部弯曲 | 255 |
+| joint6 | 小指 | 根部弯曲 | 255 |
+
+## 2.3 故障自检排查
+
+### 2.3.1 网络故障
+遥操和控制设备都是通过网络连接的，网络异常会导致彼此设备工作不正常，首先要检查网线的连接是否正确，保证双方的ip相互ping通以及防火墙要确保对应的端口是开放的
+
+### 2.3.2 话题异常
+在网络正常的前提下，可以在其他网络的设备上通过命令确认话题是否存在
+具体细节参考 2.2 Topic话题说明
+
+## 2.4 映射参数的设置
+
+### 2.4.1 原理
+映射系统有3种模式
+
+| 映射模式 | 对应英文 | 对应中文 | 适用场景 |
+|----------|----------|----------|----------|
+| 三态映射 | origin-opose-fist | 张手-O手势-握拳 | 希望能兼顾机械手最大行程，同时能满足对指的，可以满足绝大部分映射要求 |
+| 双端映射 | origin-fist | 张手-握拳 | 最大限度覆盖机械手的全部行程 |
+| 对指映射 | origin-opose- | 张手-O手势 | 以对指为主要目标，以抓取为目标，提高抓取的颗粒度 |
+
+### 2.4.2 配置文件
+该映射系统配置对机械手进行参数绑定，映射配置目录在
+```
+~/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config
+```
+
+目前提供的映射参数包如下
+```
+g20_config.py
+l10_config.py
+l20_config.py
+l6_config.py
+o6_config.py
+o7_config.py
+```
+
+### 2.4.3 配置介绍
+以下以l6_config.py为例
+
+#### 2.4.3.1 手指配置常量FINGER_CONFIGS
+
+##### 2.4.3.1.1 公共常量表
+（内容省略）
+
+##### 2.4.3.1.2 reverse_motion属性
+反转信号：默认为False
+
+##### 2.4.3.1.3 dynamic_weight属性
+动态权重：用于区间细节表达，默认为None，本版本不启用
+
+##### 2.4.3.1.4 extended_mapping属性
+扩展映射配置
+```python
+'extended_mapping': {    # 新增扩展映射配置
+    'enabled': True,
+    'scale_factor': 1.2,  # 缩放因子，默认1.0
+    'extended_exp_factor': 80  # 新增：延伸指数参数
+}
+```
+- enabled：仅在对指映射启用，False为不启用
+- scale_factor：缩放因子，用于快速锁定在opose的姿态位
+- extended_exp_factor：延伸参数，用于opose之后的延续动作，越小表示很难达到机械手的机械位置，利于更多的表达抓取细节，数值越大越容易快速达到驱动关节极限位置
+
+#### 2.4.3.2 映射顺序MAPPING_ORDER
+以L6为例，表示处理的关节顺序优先级
+```python
+MAPPING_ORDER = [
+    'thumb_abduction', 'thumb_flexion', 
+    'index', 'middle', 'ring', 'pinky'
+]
+```
+
+#### 2.4.3.3 状态配置STATE_CONFIG（未启用）
+
+#### 2.4.3.4 机械手OPOSE校准位：ROBOT_OPOSE_LEFT
+以下以L6的配置为例，以下角度对应左手机械手的OPOSE位置，注意以下顺序对应URDF的序列
+```python
+ROBOT_OPOSE_LEFT = [
+    1.4, 0.54, 0.0, 0.38, 0.0, 0.38, 0.0, 0.38, 0.0, 0.38, 0.0
+]
+```
+
+#### 2.4.3.5 机械手OPOSE校准位：ROBOT_OPOSE_RIGHT
+以下以L6的配置为例，以下角度对应右手机械手的OPOSE位置，注意以下顺序对应URDF的序列
+```python
+ROBOT_OPOSE_RIGHT = [
+    1.4, 0.54, 0.0, 0.38, 0.0, 0.38, 0.0, 0.38, 0.0, 0.38, 0.0
+]
+```
+
+### 2.4.4 机械手校准
+在2.4.3.4和2.4.3.5章节已经分别介绍了左右机械手OPOSE校准位，用于处理当前机械手位置和URDF之间的差异，第一次使用的时候，需要在以下代码进行改写
+
+路径：`./src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand`
+
+以L6机械手为例，在打开linkerforce_l6.py之后，在约计149行
+```python
+if self.calibrationoriginal is not None 
+and self.calibrationfistpose is not None 
+and self.calibrationopose is not None:
+    # for i in range(20):
+    #     self.multi_state_mapper.debug_value[i] = joint_arc[i]
+    # arc_value = self.multi_state_mapper.map_glove_to_robot(joint_arc)   # 注释掉
+    arc_value = ROBOT_OPOSE_RIGHT          # 启用校准
+    qpos[17] = self.g_jointpositions_arc[1] = arc_value[0]
+    qpos[20] = self.g_jointpositions_arc[0] = arc_value[1]
+    qpos[1] = self.g_jointpositions_arc[2] = arc_value[3]
+    qpos[9] = self.g_jointpositions_arc[3] = arc_value[5]
+    qpos[13] = self.g_jointpositions_arc[4] = arc_value[7]
+    qpos[5] = self.g_jointpositions_arc[5] = arc_value[9]
+```
+
+改写成如上图的示例，即可启用右机械手的校准，启动后就会让机械手按照映射的角度固定在当前角度
+
+左手对应的在360行
+```python
+if self.calibrationoriginal is not None and self.calibrationfistpose is not None and self.calibrationopose is not None:
+    # for i in range(20):
+    #     self.multi_state_mapper.debug_value[i] = joint_arc[i]
+    # arc_value = self.multi_state_mapper.map_glove_to_robot(joint_arc) # 注释掉
+    # 启用校准
+    arc_value = ROBOT_OPOSE_LEFT
+    qpos[17] = self.g_jointpositions_arc[1] = arc_value[0]
+    qpos[20] = self.g_jointpositions_arc[0] = arc_value[1]
+    qpos[1] = self.g_jointpositions_arc[2] = arc_value[3]
+    qpos[9] = self.g_jointpositions_arc[3] = arc_value[5]
+    qpos[13] = self.g_jointpositions_arc[4] = arc_value[7]
+    qpos[5] = self.g_jointpositions_arc[5] = arc_value[9]
+```
+
+改写成如上图的示例，即可启用右机械手的校准，启动后就会让机械手按照映射的角度固定在当前角度
+
+当左右两手都达到期望的对指位置后，就可以恢复原状，按正常顺序使用遥操系统

readme.txt

@@ -0,0 +1,9 @@
+colcon build --symlink-install
+source install/setup.bash
+
+启动分为两种，
+一种带标定
+ros2 run linkerhand_retarget handretarget --ros-args -p calibration := True
+一种正常启动，默认不执行标定
+ros2 run linkerhand_retarget handretarget 
+还是这些流程

src/camera_retarget_l20/QUICK_START_O6_G20_ZH.md

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

src/camera_retarget_l20/README.md

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

src/camera_retarget_l20/USAGE_O6_G20_ZH.md

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

src/camera_retarget_l20/camera_retarget_l20/__init__.py

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

src/camera_retarget_l20/camera_retarget_l20/calibration.py

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

src/camera_retarget_l20/camera_retarget_l20/hand_detector.py

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

src/camera_retarget_l20/camera_retarget_l20/hand_features.py

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

src/camera_retarget_l20/camera_retarget_l20/l20_mapping.py

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

src/camera_retarget_l20/camera_retarget_l20/model_profiles.py

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

src/camera_retarget_l20/camera_retarget_l20/o6_mapping.py

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

src/camera_retarget_l20/config/direct_mapping_profiles.yml

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

src/camera_retarget_l20/config/g20_left_mediapipe.yml

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

src/camera_retarget_l20/config/l20_left_mediapipe.yml

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

src/camera_retarget_l20/launch/camera_g20_left.launch.py

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

src/camera_retarget_l20/launch/camera_l20_left.launch.py

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

src/camera_retarget_l20/launch/teleop_g20_left.launch.py

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

src/camera_retarget_l20/launch/teleop_l20_left.launch.py

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

src/camera_retarget_l20/launch/teleop_o6_g20_left.launch.py

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

src/camera_retarget_l20/package.xml

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

src/camera_retarget_l20/pyproject.toml

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

src/camera_retarget_l20/resource/camera_retarget_l20

@@ -0,0 +1 @@
+

src/camera_retarget_l20/setup.cfg

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

src/camera_retarget_l20/setup.py

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

src/camera_retarget_l20/test/test_calibration_capture.py

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

src/camera_retarget_l20/test/test_dual_calibration.py

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

src/camera_retarget_l20/test/test_g20_mapping.py

@@ -0,0 +1,247 @@
+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_ABDUCTION_MIN_COMMAND,
+    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() == [255.0, 0.0, 0.0, 0.0, 0.0]
+    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] == THUMB_ABDUCTION_MIN_COMMAND
+    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] == THUMB_ABDUCTION_MIN_COMMAND
+    assert command[6:10].tolist() == [255.0] * 4
+    assert command[10] == THUMB_YAW_MIN_COMMAND
+
+
+def test_filter_starts_at_sdk_open_pose_and_limits_step():
+    mapper = make_mapper(ema_alpha=1.0, max_step=7.0)
+    previous = mapper.last_command.copy()
+    command = np.asarray(mapper.map(qpos_at(1)))
+    assert np.all(np.abs(command - previous) <= 7.0)
+    assert command[list(RESERVED_MOTOR_INDICES)].tolist() == [255] * 4
+
+
+def test_missing_controlled_joint_is_rejected():
+    names = [name for name in JOINT_LIMITS if name != "thumb_cmc_pitch"]
+    with pytest.raises(ValueError, match="thumb_cmc_pitch"):
+        G20CommandMapper(names, [JOINT_LIMITS[name] for name in names])
+
+
+def test_protocol_layout_has_16_controlled_and_four_reserved_slots():
+    assert len(MOTOR_JOINT_NAMES) == 20
+    assert sum(name is not None for name in MOTOR_JOINT_NAMES) == 16
+    assert {i for i, name in enumerate(MOTOR_JOINT_NAMES) if name is None} == set(
+        RESERVED_MOTOR_INDICES
+    )
+
+
+def make_open_landmarks():
+    points = np.zeros((21, 3), dtype=float)
+    points[1:5] = np.array(
+        [[-0.20, 0.35, 0.0], [-0.40, 0.60, 0.0],
+         [-0.60, 0.85, 0.0], [-0.80, 1.10, 0.0]]
+    )
+    for root, x in zip((5, 9, 13, 17), (-0.45, -0.15, 0.15, 0.45)):
+        points[root:root + 4] = np.array(
+            [[x, 1.0, 0.0], [x, 1.7, 0.0],
+             [x, 2.3, 0.0], [x, 2.8, 0.0]]
+        )
+    return points
+
+
+def make_fist_landmarks():
+    points = make_open_landmarks()
+    thumb_mcp = points[2].copy()
+    points[3] = thumb_mcp + np.array([0.0, 0.0, 0.6])
+    points[4] = points[3] + np.array([0.0, -0.5, 0.0])
+    for root in (5, 9, 13, 17):
+        mcp = points[root].copy()
+        points[root + 1] = mcp + np.array([0.0, 0.0, 0.7])
+        points[root + 2] = points[root + 1] + np.array([0.0, -0.6, 0.0])
+        points[root + 3] = points[root + 2] + np.array([0.0, 0.0, -0.5])
+    return points
+
+
+def make_four_finger_fist_with_straight_thumb():
+    points = make_open_landmarks()
+    for root in (5, 9, 13, 17):
+        mcp = points[root].copy()
+        points[root + 1] = mcp + np.array([0.0, 0.0, 0.7])
+        points[root + 2] = points[root + 1] + np.array([0.0, -0.6, 0.0])
+        points[root + 3] = points[root + 2] + np.array([0.0, 0.0, -0.5])
+    return points
+
+
+def test_mediapipe_mapper_requires_explicit_open_calibration():
+    mapper = G20MediaPipeMapper()
+    with pytest.raises(RuntimeError, match="calibration"):
+        mapper.raw_command(make_open_landmarks())
+
+
+def test_mediapipe_open_pose_maps_to_sdk_open_command():
+    mapper = G20MediaPipeMapper(
+        ema_alpha=1.0, max_step=255.0, max_closure=0.60
+    )
+    landmarks = make_open_landmarks()
+    mapper.calibrate_open(landmarks)
+    assert mapper.map_landmarks(landmarks) == mapper.open_pose.astype(int).tolist()
+
+
+def test_mediapipe_fist_closes_root_and_tip_flexion_channels():
+    mapper = G20MediaPipeMapper(
+        ema_alpha=1.0, max_step=255.0, max_closure=0.60
+    )
+    mapper.calibrate_open(make_open_landmarks())
+    command = mapper.raw_command(make_fist_landmarks())
+    for index in (*range(1, 5), *range(16, 20)):
+        expected = mapper.open_pose[index] + 0.60 * (
+            DEFAULT_FIST_POSE[index] - mapper.open_pose[index]
+        )
+        assert command[index] == pytest.approx(expected)
+    assert command[0] == mapper.open_pose[0]
+    expected_thumb_tip = mapper.open_pose[15] + 0.60 * (
+        DEFAULT_FIST_POSE[15] - mapper.open_pose[15]
+    )
+    assert command[15] == pytest.approx(expected_thumb_tip)
+    assert command[5] == mapper.open_pose[5]
+    assert command[6:10].tolist() == mapper.open_pose[6:10].tolist()
+    assert THUMB_YAW_MIN_COMMAND <= command[10] <= mapper.open_pose[10]
+
+
+def test_mediapipe_splay_moves_matching_left_g20_channels():
+    mapper = G20MediaPipeMapper(
+        ema_alpha=1.0,
+        max_step=255.0,
+        max_closure=0.85,
+        splay_gain=1.0,
+        max_splay_delta=90.0,
+    )
+    open_landmarks = make_open_landmarks()
+    mapper.calibrate_open(open_landmarks)
+
+    spread = open_landmarks.copy()
+    spread[6, 0] -= 0.20
+    spread[18, 0] += 0.20
+    command = mapper.raw_command(spread)
+
+    assert command[6] > mapper.open_pose[6]
+    assert command[9] < mapper.open_pose[9]
+    assert command[7] == pytest.approx(mapper.open_pose[7])
+    assert command[8] == pytest.approx(mapper.open_pose[8])
+
+
+def test_mediapipe_thumb_adduction_controls_g20_thumb_side_channel():
+    mapper = G20MediaPipeMapper(
+        ema_alpha=1.0,
+        max_step=255.0,
+        max_closure=0.95,
+        thumb_abduction_min_command=20.0,
+    )
+    open_landmarks = make_open_landmarks()
+    mapper.calibrate_open(open_landmarks)
+
+    adducted = open_landmarks.copy()
+    adducted[2] = adducted[1] + np.array([0.0, 0.40, 0.0])
+    command = mapper.raw_command(adducted)
+
+    assert command[5] < 100.0
+    assert command[10] < mapper.open_pose[10]
+    assert mapper.last_thumb_abduction > 0.5
+
+
+def test_four_finger_grasp_keeps_a_straight_thumb_open():
+    mapper = G20MediaPipeMapper(
+        ema_alpha=1.0,
+        max_step=255.0,
+        max_closure=0.95,
+        thumb_abduction_min_command=20.0,
+    )
+    mapper.calibrate_open(make_open_landmarks())
+    command = mapper.raw_command(make_four_finger_fist_with_straight_thumb())
+
+    assert mapper.last_power_grasp == pytest.approx(1.0)
+    assert command[0] == mapper.open_pose[0]
+    assert command[5] == mapper.open_pose[5]
+    assert command[10] == mapper.open_pose[10]
+    assert command[15] == mapper.open_pose[15]
+    assert np.all(command[1:5] < 100.0)
+    assert np.all(command[16:20] < 100.0)
+
+
+def test_mediapipe_full_closure_reaches_each_measured_g20_fist_endpoint():
+    mapper = G20MediaPipeMapper(
+        ema_alpha=1.0,
+        max_step=255.0,
+        max_closure=1.0,
+    )
+    mapper.calibrate_open(make_open_landmarks())
+    features = mapper.extract_features(make_fist_landmarks())
+    features.closure[:] = 1.0
+    command = mapper.raw_command_from_features(features)
+
+    for index in FLEXION_MOTOR_INDICES:
+        assert command[index] == DEFAULT_FIST_POSE[index]

src/camera_retarget_l20/test/test_l20_mapping.py

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

src/camera_retarget_l20/test/test_model_profiles.py

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

src/camera_retarget_l20/test/test_o6_mapping.py

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

src/gui_control/gui_control/config/constants.py

@@ -0,0 +1,262 @@
+# hand_config_const.py
+from typing import Dict, List, Optional
+from dataclasses import dataclass, field
+from types import MappingProxyType
+
+@dataclass(frozen=True)        # frozen=True 让实例真正只读
+class HandConfig:
+    joint_names: List[str] = field(default_factory=list)
+    joint_names_en: Optional[List[str]] = None
+    init_pos: List[int] = field(default_factory=list)
+    preset_actions: Optional[Dict[str, List[int]]] = None
+
+# ------------------------------------------------------------------
+# 常量字典（仅构建一次）
+# ------------------------------------------------------------------
+_HAND_CONFIGS: Dict[str, HandConfig] = {
+    "L25": HandConfig(
+        joint_names=["大拇指根部", "食指根部", "中指根部", "无名指根部", "小拇指根部",
+                     "大拇指侧摆", "食指侧摆", "中指侧摆", "无名指侧摆", "小拇指侧摆",
+                     "大拇指横滚", "预留", "预留", "预留", "预留", "大拇指中部", "食指中部",
+                     "中指中部", "无名指中部", "小拇指中部", "大拇指指尖", "食指指尖",
+                     "中指指尖", "无名指指尖", "小拇指指尖"],
+        init_pos=[255] * 25,
+        preset_actions={
+            "握拳": [0] * 25,
+            "张开": [255] * 25,
+            "OK": [255, 255, 255, 255, 255, 255, 255, 255, 255, 255,
+                   255, 255, 255, 255, 255, 255, 0, 0, 255, 255,
+                   0, 0, 0, 255, 255]
+        }
+    ),
+    "L21": HandConfig(
+        joint_names=["大拇指根部", "食指根部", "中指根部", "无名指根部", "小拇指根部",
+                     "大拇指侧摆", "食指侧摆", "中指侧摆", "无名指侧摆", "小拇指侧摆",
+                     "大拇指横滚", "预留", "预留", "预留", "预留", "大拇指中部", "预留",
+                     "预留", "预留", "预留", "大拇指指尖", "食指指尖", "中指指尖",
+                     "无名指指尖", "小拇指指尖"],
+        init_pos=[255] * 25
+    ),
+    "L20": HandConfig(
+        joint_names=["拇指根部", "食指根部", "中指根部", "无名指根部", "小指根部",
+                     "拇指侧摆", "食指侧摆", "中指侧摆", "无名指侧摆", "小指侧摆",
+                     "拇指横摆", "预留", "预留", "预留", "预留", "拇指尖部", "食指末端",
+                     "中指末端", "无名指末端", "小指末端"],
+        init_pos=[255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+        preset_actions={
+            "握拳": [40, 0, 0, 0, 0, 131, 10, 100, 180, 240, 19, 255, 255, 255, 255, 135, 0, 0, 0, 0],
+            "张开": [255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "OK": [191, 95, 255, 255, 255, 136, 107, 100, 180, 240, 72, 255, 255, 255, 255, 116, 99, 255, 255, 255],
+            "点赞": [255, 0, 0, 0, 0, 127, 10, 100, 180, 240, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0],
+            "拇指对食指": [0, 0, 255, 255, 255, 186, 10, 100, 180, 240, 0, 255, 255, 255, 255, 255, 183, 255, 255, 255],
+            "拇指对中指": [0, 255, 0, 255, 255, 145, 10, 100, 180, 240, 0, 255, 255, 255, 255, 255, 255, 202, 255, 255],
+            "拇指对无名指": [0, 255, 255, 0, 255, 108, 10, 100, 180, 240, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "拇指对小指": [0, 255, 255, 255, 0, 70, 10, 100, 180, 240, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "准备1": [255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "壹": [40, 255, 0, 0, 0, 131, 125, 100, 180, 240, 19, 255, 255, 255, 255, 135, 255, 0, 0, 0],
+            "贰": [40, 255, 255, 0, 0, 81, 35, 177, 180, 240, 19, 255, 255, 255, 255, 135, 255, 255, 0, 0],
+            "叁": [40, 255, 255, 255, 0, 161, 62, 123, 180, 240, 13, 255, 255, 255, 255, 0, 255, 255, 255, 0],
+            "肆": [40, 255, 255, 255, 255, 161, 62, 123, 180, 242, 13, 255, 255, 255, 255, 0, 255, 255, 255, 255],
+            "伍": [255, 255, 255, 255, 255, 255, 10, 100, 180, 240, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "陆": [255, 0, 0, 0, 255, 220, 10, 100, 180, 255, 255, 255, 255, 255, 255, 255, 0, 0, 0, 255],
+            "漆": [0, 0, 0, 0, 0, 161, 10, 127, 180, 219, 18, 255, 255, 255, 255, 255, 195, 205, 0, 0],
+            "捌": [255, 255, 0, 0, 0, 202, 104, 100, 180, 240, 233, 255, 255, 255, 255, 255, 255, 0, 0, 0],
+            "玖": [40, 255, 0, 0, 0, 131, 103, 100, 180, 240, 19, 255, 255, 255, 255, 135, 47, 0, 0, 0],
+            
+        }
+    ),
+    "G20": HandConfig(
+        joint_names=["拇指根部", "食指根部", "中指根部", "无名指根部", "小指根部",
+                     "拇指侧摆", "食指侧摆", "中指侧摆", "无名指侧摆", "小指侧摆",
+                     "拇指横摆", "预留", "预留", "预留", "预留", "拇指尖部", "食指末端",
+                     "中指末端", "无名指末端", "小指末端"],
+        init_pos=[255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+        preset_actions={
+            "点赞": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
+            "握拳": [96, 0, 0, 0, 0, 0, 193, 158, 128, 91, 132, 255, 255, 255, 255, 144, 0, 0, 0, 0],
+            "张开": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "OK": [148, 110, 255, 255, 255, 44, 164, 100, 114, 127, 178, 255, 255, 255, 255, 94, 71, 255, 255, 255],
+            "拇指对中指": [191, 255, 55, 255, 255, 96, 95, 100, 114, 127, 105, 255, 255, 255, 255, 94, 255, 108, 255, 255],
+            "拇指对无名指": [191, 255, 255, 72, 255, 115, 95, 100, 114, 127, 60, 255, 255, 255, 255, 94, 255, 255, 97, 255],
+            "拇指对小指": [191, 255, 255, 255, 55, 0, 95, 100, 114, 121, 70, 255, 255, 255, 255, 94, 255, 255, 255, 100],
+            "准备1": [255, 0, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 0, 0, 0, 0],
+            "壹": [96, 255, 0, 0, 0, 0, 190, 161, 127, 80, 68, 255, 255, 255, 255, 144, 255, 0, 0, 0],
+            "贰": [96, 255, 255, 0, 0, 0, 190, 66, 127, 80, 68, 255, 255, 255, 255, 144, 255, 255, 0, 0],
+            "叁": [96, 255, 255, 255, 0, 0, 200, 132, 76, 80, 68, 255, 255, 255, 255, 144, 255, 255, 255, 0],
+            "肆": [80, 255, 255, 255, 255, 78, 200, 132, 114, 48, 129, 255, 255, 255, 255, 64, 255, 255, 255, 255],
+            "伍": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "陆": [255, 0, 0, 0, 255, 255, 156, 126, 125, 42, 245, 255, 255, 255, 255, 255, 0, 0, 0, 255],
+            "漆": [38, 0, 0, 0, 0, 55, 156, 126, 125, 117, 145, 255, 255, 255, 255, 255, 164, 163, 0, 0],
+            "捌": [255, 255, 0, 0, 0, 255, 162, 162, 144, 100, 210, 255, 255, 255, 255, 255, 255, 0, 0, 0],
+            "玖": [67, 255, 0, 0, 0, 37, 162, 162, 144, 100, 85, 255, 255, 255, 255, 169, 0, 0, 0, 0],
+            "动作1": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "动作2": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "动作3": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "动作4": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "动作5": [255, 255, 255, 255, 255, 255, 255, 255, 247, 255, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "动作6": [255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "动作7": [255, 255, 255, 255, 255, 255, 255, 255, 247, 255, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "动作8": [255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "动作9": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 210, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "根部1": [0, 0, 0, 0, 0, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "根部2": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "根部1": [0, 0, 0, 0, 0, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "根部2": [255, 255, 255, 255, 255, 255, 125, 129, 125, 130, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "末端1": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 125, 0, 0, 0, 0],
+            "末端2": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "末端3": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 125, 0, 0, 0, 0],
+            "末端4": [6, 0, 0, 0, 0, 255, 125, 129, 125, 130, 219, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+            "默认": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+
+
+        }
+    ),
+    # 大拇指关节球版L10
+    # "L10": HandConfig(
+    #     joint_names_en=["thumb_cmc_pitch", "thumb_cmc_roll", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
+    #                     "index_mcp_roll", "ring_mcp_roll", "pinky_mcp_roll", "thumb_cmc_yaw"],
+    #     joint_names=["拇指根部", "拇指侧摆", "食指根部", "中指根部", "无名指根部",
+    #                  "小指根部", "食指侧摆", "无名指侧摆", "小指侧摆", "拇指旋转"],
+    #     init_pos=[255] * 10,
+    #     preset_actions={
+    #         "握拳": [75, 128, 0, 0, 0, 0, 128, 128, 128, 57],
+    #         "张开": [255, 128, 255, 255, 255, 255, 128, 128, 128, 128],
+    #         "OK": [110, 128, 75, 255, 255, 255, 128, 128, 128, 68],
+    #         "点赞": [255, 145, 0, 0, 0, 0, 0, 255, 255, 65]
+    #     }
+    # ),
+    # 大拇指关节齿轮版L10
+    "L10": HandConfig(
+        joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
+                        "index_mcp_roll", "ring_mcp_roll", "pinky_mcp_roll", "thumb_cmc_roll"],
+        joint_names=["拇指根部", "拇指侧摆", "食指根部", "中指根部", "无名指根部",
+                     "小指根部", "食指侧摆", "无名指侧摆", "小指侧摆", "拇指旋转"],
+        init_pos=[255] * 10,
+        preset_actions={
+            "张开": [255, 255, 255, 255, 255, 255, 128, 67, 89, 255],
+            "点赞": [255, 255, 0, 0, 0, 0, 128, 67, 89, 255],
+            "握拳": [90, 0, 0, 0, 0, 0, 128, 67, 89, 197],
+            "壹": [55, 0, 255, 0, 0, 0, 128, 67, 89, 124],
+            "贰": [55, 0, 255, 255, 0, 0, 128, 67, 89, 124],
+            "叁": [116, 255, 255, 255, 255, 0, 128, 67, 89, 255],
+            "肆": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255],
+            "伍": [255, 255, 255, 255, 255, 255, 128, 67, 89, 255],
+            "陆": [255, 255, 0, 0, 0, 255, 128, 67, 89, 255],
+            "柒1": [255, 37, 119, 112, 0, 0, 128, 67, 89, 211],
+            "柒2": [91, 37, 119, 112, 0, 0, 128, 67, 89, 211],
+            "捌": [255, 255, 255, 0, 0, 0, 128, 67, 89, 255],
+            "玖": [59, 0, 134, 0, 0, 0, 128, 67, 89, 153],
+            "侧摆0": [255, 0, 255, 255, 255, 255, 128, 67, 89, 153],
+            "侧摆1": [0, 0, 255, 255, 255, 255, 255, 255, 255, 255],
+            "侧摆2": [0, 0, 255, 255, 255, 255, 0, 0, 0, 255],
+            "侧摆3": [0, 0, 255, 255, 255, 255, 255, 255, 255, 255],
+            "侧摆4": [0, 0, 255, 255, 255, 255, 0, 0, 0, 255],
+            "侧摆5": [0, 0, 255, 255, 255, 255, 255, 255, 255, 255],
+            "侧摆6": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255],
+            "弯曲1": [209, 0, 124, 122, 123, 122, 128, 67, 89, 255],
+            "弯曲2": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255],
+            "弯曲3": [209, 0, 124, 122, 123, 122, 128, 67, 89, 255],
+            "弯曲4": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255],
+            "弯曲5": [209, 0, 124, 122, 123, 122, 128, 67, 89, 255],
+            "弯曲6": [0, 0, 255, 255, 255, 255, 128, 67, 89, 255],
+            "OK": [84, 39, 122, 255, 255, 255, 128, 67, 89, 255],
+            "拇指压感1": [134, 39, 99, 255, 255, 255, 128, 67, 89, 255],
+            "拇指压感2": [73, 39, 91, 255, 255, 255, 128, 67, 89, 255],
+            "食指压感1": [151, 39, 190, 255, 255, 255, 128, 67, 89, 255],
+            "食指压感2": [58, 39, 103, 255, 255, 255, 128, 67, 89, 255],
+            "中指压感1": [40, 39, 255, 128, 255, 255, 128, 67, 89, 209],
+            "中指压感2": [40, 39, 255, 89, 255, 255, 128, 67, 89, 209],
+            "无名指压感1": [51, 39, 255, 255, 139, 255, 128, 67, 89, 154],
+            "无名指压感2": [51, 39, 255, 255, 83, 255, 128, 67, 89, 154],
+            "小拇指压感1": [62, 39, 255, 255, 255, 155, 128, 67, 89, 101],
+            "小拇指压感2": [62, 39, 255, 255, 255, 75, 128, 67, 89, 101],
+        }
+    ),
+    # 大拇指关节球版L7
+    # "L7": HandConfig(
+    #     joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲",
+    #                  "小拇指弯曲", "拇指旋转"],
+    #     init_pos=[250] * 7,
+    #     preset_actions={
+    #         "点赞": [255, 111, 0, 0, 0, 0, 86],
+    #         "握拳": [71, 79, 0, 0, 0, 0, 64],
+    #         "张开": [255, 111, 250, 250, 250, 250, 55],
+    #         "OK": [141, 111, 168, 250, 250, 250, 86],
+            
+    #     }
+    # ),
+    # 大拇指关节齿轮版L7
+    "L7": HandConfig(
+        joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch", "thumb_cmc_roll"],
+        joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲", "拇指旋转"],
+        init_pos=[250] * 7,
+        preset_actions={
+            "张开": [255, 111, 250, 250, 250, 250, 55],
+            "点赞": [255, 255, 0, 0, 0, 0, 255],
+            "赞2": [255, 0, 0, 0, 0, 0, 255],
+            "握拳": [65, 0, 0, 0, 0, 0, 93],
+            "壹": [66, 0, 255, 0, 0, 0, 93],
+            "壹1": [61, 0, 255, 0, 0, 0, 255],
+            "贰": [0, 0, 255, 255, 0, 0, 255],
+            "叁": [0, 0, 255, 255, 255, 0, 255],
+            "肆": [0, 0, 255, 255, 255, 255, 119],
+            "伍": [255, 111, 250, 250, 250, 250, 55],
+            "OK": [99, 15, 146, 250, 250, 250, 206],
+            "拇指压感1": [99, 15, 206, 250, 250, 250, 206],
+            "拇指压感2": [109, 15, 70, 250, 250, 250, 206],
+            "食指压感1": [99, 15, 206, 250, 250, 250, 206],
+            "食指压感2": [69, 15, 140, 250, 250, 250, 206],
+            "中指压感": [82, 15, 255, 136, 250, 250, 170],
+            "无名指压感": [70, 15, 255, 255, 141, 250, 125],
+            "小拇指压感": [70, 15, 255, 255, 255, 120, 78],
+            "准备1": [70, 15, 255, 255, 255, 255, 78]
+            
+        }
+    ),
+    "O6": HandConfig(
+        joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "pinky_mcp_pitch", "ring_mcp_pitch"],
+        joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲"],
+        init_pos=[250] * 6,
+        preset_actions={
+            "张开": [250, 250, 250, 250, 250, 250],
+            "壹": [125, 18, 255, 0, 0, 0],
+            "贰": [92, 87, 255, 255, 0, 0],
+            "叁": [92, 87, 255, 255, 255, 0],
+            "肆": [92, 87, 255, 255, 255, 255],
+            "伍": [255, 255, 255, 255, 255, 255],
+            "OK": [139, 91, 103, 250, 250, 250],
+            "点赞": [250, 79, 0, 0, 0, 0],
+            "握拳": [102, 18, 0, 0, 0, 0],
+        }
+    ),
+    "L6": HandConfig(
+        joint_names_en=["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "pinky_mcp_pitch", "ring_mcp_pitch"],
+        joint_names=["大拇指弯曲", "大拇指横摆", "食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲"],
+        init_pos=[250] * 6,
+        preset_actions={
+            "张开": [250, 250, 250, 250, 250, 250],
+            "壹": [0, 18, 255, 0, 0, 0],
+            "贰": [0, 39, 255, 255, 0, 0],
+            "叁": [0, 39, 255, 255, 255, 0],
+            "肆": [0, 0, 255, 255, 255, 255],
+            "伍": [255, 255, 255, 255, 255, 255],
+            "OK": [74, 13, 153, 255, 255, 255],
+            "点赞": [255, 255, 0, 0, 0, 0],
+            "握拳": [79, 11, 0, 0, 0, 0],
+            "序列动作1": [250, 250, 250, 250, 250, 250],
+            "序列动作2": [250, 250, 0, 250, 250, 0],
+            "序列动作3": [250, 250, 0, 0, 0, 0],
+            "序列动作4": [250, 250, 0, 0, 0, 255],
+            "序列动作5": [250, 250, 0, 0, 255, 255],
+            "序列动作6": [250, 250, 0, 255, 255, 255],
+            "序列动作7": [250, 250, 250, 250, 250, 250],
+            "食指压感准备1": [0, 18, 255, 0, 0, 0],
+            "食指压感测试": [9, 42, 55, 250, 250, 250],
+            "食指压感准备2": [0, 18, 255, 0, 0, 0],
+            "拇指压感准备1": [139, 18, 130, 0, 0, 0],
+            "拇指压感测试": [39, 30, 122, 250, 250, 250],
+            "拇指压感准备2": [139, 18, 130, 0, 0, 0]
+        }
+    ),
+}
+HAND_CONFIGS = MappingProxyType(_HAND_CONFIGS)

src/gui_control/gui_control/gui_control.py

@@ -0,0 +1,808 @@
+import sys
+import time, json
+import threading
+from dataclasses import dataclass
+from typing import List, Dict
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String, Header
+from sensor_msgs.msg import JointState
+from PyQt5.QtCore import Qt, pyqtSignal, QTimer, QObject, QEvent
+from PyQt5.QtWidgets import (
+    QApplication, QWidget, QVBoxLayout, QHBoxLayout, QGridLayout, 
+    QSlider, QLabel, QPushButton, QGroupBox, QScrollArea, QTabWidget, 
+    QFrame, QSplitter, QMessageBox, QTextEdit
+)
+from PyQt5.QtGui import QFont
+
+from .utils.mapping import *
+
+from .config.constants import _HAND_CONFIGS
+LOOP_TIME = 1000 # 循环动作间隔时间 毫秒
+class ROS2NodeManager(QObject):
+    """ROS2节点管理器，处理ROS通信"""
+    status_updated = pyqtSignal(str, str)  # 状态类型, 消息内容
+
+    def __init__(self, node_name: str = "hand_control_node"):
+        super().__init__()
+        self.node = None
+        self.publisher = None
+        self.joint_state = JointState()
+        self.joint_state.header = Header()
+        
+        # 初始化ROS2节点
+        self.init_node(node_name)
+
+    def init_node(self, node_name: str):
+        """初始化ROS2节点"""
+        try:
+            if not rclpy.ok():
+                rclpy.init(args=None)
+            self.node = Node(node_name)
+            
+            # 声明参数
+            self.node.declare_parameter('hand_type', 'right')
+            self.node.declare_parameter('hand_joint', 'L10')
+            self.node.declare_parameter('topic_hz', 30)
+            self.node.declare_parameter('is_arc', False)
+            
+            # 获取参数
+            self.hand_type = self.node.get_parameter('hand_type').value
+            self.hand_joint = self.node.get_parameter('hand_joint').value
+            self.hz = self.node.get_parameter('topic_hz').value
+            self.is_arc = self.node.get_parameter('is_arc').value
+            
+            if self.is_arc == True:
+                # 创建发布者
+                self.publisher_arc = self.node.create_publisher(
+                    JointState, f'/cb_{self.hand_type}_hand_control_cmd_arc', 10
+                )
+            # 创建发布者
+            self.publisher = self.node.create_publisher(
+                JointState, f'/cb_{self.hand_type}_hand_control_cmd', 10
+            )
+                    # 新增 speed / torque 发布者
+            self.speed_pub = self.node.create_publisher(
+                String, f'/cb_hand_setting_cmd', 10)
+            self.torque_pub = self.node.create_publisher(
+                String, f'/cb_hand_setting_cmd', 10)
+            self.status_updated.emit("info", f"ROS2节点初始化成功: {self.hand_type} {self.hand_joint}")
+            
+            # 启动ROS2自旋线程
+            self.spin_thread = threading.Thread(target=self.spin_node, daemon=True)
+            self.spin_thread.start()
+        except Exception as e:
+            self.status_updated.emit("error", f"ROS2初始化失败: {str(e)}")
+            raise
+
+    def spin_node(self):
+        """运行ROS2节点自旋循环"""
+        while rclpy.ok() and self.node:
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+
+    def publish_joint_state(self, positions: List[int]):
+        """发布关节状态消息"""
+        if not self.publisher or not self.node:
+            self.status_updated.emit("error", "ROS2发布者未初始化")
+            return
+            
+        try:
+            self.joint_state.header.stamp = self.node.get_clock().now().to_msg()
+            self.joint_state.position = [float(pos) for pos in positions]
+            # self.joint_state.velocity = [0.1] * len(positions)
+            # self.joint_state.effort = [0.01] * len(positions)
+            # 如果有关节名称，添加到消息中
+            #hand_config = HandConfig.from_hand_type(self.hand_joint)
+            hand_config = _HAND_CONFIGS[self.hand_joint]
+            if len(hand_config.joint_names) == len(positions):
+                if hand_config.joint_names_en != None:
+                    self.joint_state.name = hand_config.joint_names_en
+                else:
+                    self.joint_state.name = hand_config.joint_names
+                
+            self.publisher.publish(self.joint_state)
+            if self.is_arc == True:
+                if self.hand_joint == "O6":
+                    if self.hand_type == "left":
+                        pose = range_to_arc_left(positions,self.hand_joint)
+                    elif self.hand_type == "right":
+                        pose = range_to_arc_right(positions,self.hand_joint)
+                elif self.hand_joint == "L7" or self.hand_joint == "L21" or self.hand_joint == "L25":
+                    if self.hand_type == "left":
+                        pose = range_to_arc_left(positions,self.hand_joint)
+                    elif self.hand_type == "right":
+                        pose = range_to_arc_right(positions,self.hand_joint)
+                elif self.hand_joint == "L10":
+                    if self.hand_type == "left":
+                        pose = range_to_arc_left_10(positions)
+                    elif self.hand_type == "right":
+                        pose = range_to_arc_right_10(positions)
+                elif self.hand_joint == "L20":
+                    if self.hand_type == "left":
+                        pose = range_to_arc_left_l20(positions)
+                    elif self.hand_type == "right":
+                        pose = range_to_arc_right_l20(positions)
+                else:
+                    #print(f"当前{self.hand_joint} {self.hand_type}不支持弧度转换", flush=True)
+                    pass
+                self.joint_state.position = [float(pos) for pos in pose]
+                self.publisher_arc.publish(self.joint_state)
+            self.status_updated.emit("info", "关节状态已发布")
+        except Exception as e:
+            self.status_updated.emit("error", f"发布失败: {str(e)}")
+
+    def publish_speed(self, val: int):
+        joint_len = 0
+        if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"):
+            joint_len = 6
+        elif self.hand_joint == "L7":
+            joint_len = 7
+        elif self.hand_joint == "L10":
+            joint_len = 10
+        else:
+            joint_len = 5
+        msg = String()
+        v = [val] * joint_len
+        data = {
+            "setting_cmd": "set_speed",
+            "params": {"hand_type":self.hand_type,"speed": v},
+        }
+        msg.data = json.dumps(data)
+        print(f"速度值：{v}", flush=True)
+        self.speed_pub.publish(msg)
+
+    def publish_torque(self, val: int):
+        joint_len = 0
+        if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"):
+            joint_len = 6
+        elif self.hand_joint == "L7":
+            joint_len = 7
+        elif self.hand_joint == "L10":
+            joint_len = 10
+        else:
+            joint_len = 5
+        msg = String()
+        v = [val] * joint_len
+        data = {
+            "setting_cmd": "set_max_torque_limits",
+            "params": {"hand_type":self.hand_type,"torque": v},
+        }
+        
+        msg.data = json.dumps(data)
+        print(f"扭矩值：{v}", flush=True)
+        self.torque_pub.publish(msg)
+
+    def shutdown(self):
+        """关闭ROS2节点"""
+        if self.node:
+            self.node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+class HandControlGUI(QWidget):
+    """灵巧手控制界面"""
+    status_updated = pyqtSignal(str, str)  # 状态类型, 消息内容
+
+    def __init__(self, ros_manager: ROS2NodeManager):
+        super().__init__()
+        
+        # 循环控制变量
+        self.cycle_timer = None  # 循环定时器
+        self.current_action_index = -1  # 当前动作索引
+        self.preset_buttons = []  # 存储预设动作按钮引用
+        
+        # 设置ROS管理器
+        self.ros_manager = ros_manager
+        self.ros_manager.status_updated.connect(self.update_status)
+        
+        # 获取手部配置
+        self.hand_joint = self.ros_manager.hand_joint
+        self.hand_type = self.ros_manager.hand_type
+        self.hand_config = _HAND_CONFIGS[self.hand_joint]
+        
+        # 初始化UI
+        self.init_ui()
+        
+        # 设置定时器发布关节状态
+        self.publish_timer = QTimer(self)
+        self.publish_timer.setInterval(int(1000 / self.ros_manager.hz))
+        self.publish_timer.timeout.connect(self.publish_joint_state)
+        self.publish_timer.start()
+
+    def init_ui(self):
+        """初始化用户界面"""
+        # 设置窗口属性
+        self.setWindowTitle(f'灵巧手控制界面 - {self.hand_type} {self.hand_joint}')
+        self.setMinimumSize(1200, 900)
+        
+        # 设置样式
+        self.setStyleSheet("""
+            QWidget {
+                font-family: 'Microsoft YaHei', 'SimHei', sans-serif;
+                font-size: 12px;
+            }
+            QGroupBox {
+                border: 1px solid #CCCCCC;
+                border-radius: 6px;
+                margin-top: 6px;
+                padding: 10px;
+            }
+            QGroupBox::title {
+                subcontrol-origin: margin;
+                left: 10px;
+                padding: 0 5px 0 5px;
+                color: #165DFF;
+                font-weight: bold;
+            }
+            QSlider::groove:horizontal {
+                border: 1px solid #999999;
+                height: 8px;
+                border-radius: 4px;
+                background: #CCCCCC;
+                margin: 2px 0;
+            }
+            QSlider::handle:horizontal {
+                background: qlineargradient(x1:0, y1:0, x2:1, y2:1, stop:0 #165DFF, stop:1 #0E42D2);
+                border: 1px solid #5C8AFF;
+                width: 18px;
+                margin: -5px 0;
+                border-radius: 9px;
+            }
+            QPushButton {
+                background-color: #E0E0E0;
+                border: 1px solid #CCCCCC;
+                border-radius: 4px;
+                padding: 5px 10px;
+                min-width: 80px;
+            }
+            QPushButton:hover {
+                background-color: #F0F0F0;
+            }
+            QPushButton:pressed {
+                background-color: #D0D0D0;
+            }
+            QPushButton[category="preset"] {
+                background-color: #E6F7FF;
+                color: #1890FF;
+                border-color: #91D5FF;
+            }
+            QPushButton[category="preset"]:hover {
+                background-color: #B3E0FF;
+            }
+            QPushButton[category="action"] {
+                background-color: #FFF7E6;
+                color: #FA8C16;
+                border-color: #FFD591;
+            }
+            QPushButton[category="action"]:hover {
+                background-color: #FFE6B3;
+            }
+            QPushButton[category="danger"] {
+                background-color: #FFF1F0;
+                color: #F5222D;
+                border-color: #FFCCC7;
+            }
+            QPushButton[category="danger"]:hover {
+                background-color: #FFE8E6;
+            }
+            QLabel#StatusLabel {
+                padding: 5px;
+                border-radius: 4px;
+            }
+            QLabel#StatusInfo {
+                background-color: #F0F7FF;
+                color: #0066CC;
+            }
+            QLabel#StatusError {
+                background-color: #FFF0F0;
+                color: #CC0000;
+            }
+            /* 数值显示面板样式 */
+            QTextEdit#ValueDisplay {
+                background-color: #F8F8F8;
+                border: 1px solid #CCCCCC;
+                border-radius: 4px;
+                padding: 10px;
+                font-family: Consolas, monospace;
+                font-size: 12px;
+            }
+        """)
+        
+        # 创建主垂直布局
+        main_layout = QVBoxLayout(self)
+        
+        # 创建水平分割器（原有三个面板）
+        splitter = QSplitter(Qt.Horizontal)
+        
+        # 创建左侧关节控制面板
+        self.joint_control_panel = self.create_joint_control_panel()
+        splitter.addWidget(self.joint_control_panel)
+        
+        # 创建中间预设动作面板
+        self.preset_actions_panel = self.create_preset_actions_panel()
+        splitter.addWidget(self.preset_actions_panel)
+        
+        # 创建右侧状态监控面板
+        self.status_monitor_panel = self.create_status_monitor_panel()
+        splitter.addWidget(self.status_monitor_panel)
+        
+        # 设置分割器比例
+        splitter.setSizes([500, 300, 400])
+        
+        # 添加分割器到主布局，并设置拉伸因子为1（可伸缩）
+        main_layout.addWidget(splitter, stretch=1)
+        
+        # 创建并添加数值显示面板，设置拉伸因子为0（不可伸缩）
+        self.value_display_panel = self.create_value_display_panel()
+        main_layout.addWidget(self.value_display_panel, stretch=0)
+        
+        # 初始更新数值显示
+        self.update_value_display()
+
+    def create_joint_control_panel(self):
+        """创建关节控制面板"""
+        panel = QWidget()
+        layout = QVBoxLayout(panel)
+        
+        # 创建标题
+        title_label = QLabel(f"关节控制 - {self.hand_joint}")
+        title_label.setFont(QFont("Microsoft YaHei", 14, QFont.Bold))
+        layout.addWidget(title_label)
+
+        # 创建滑动条滚动区域
+        scroll_area = QScrollArea()
+        scroll_area.setWidgetResizable(True)
+        scroll_area.setFrameShape(QFrame.NoFrame)
+        
+        scroll_content = QWidget()
+        self.sliders_layout = QGridLayout(scroll_content)
+        self.sliders_layout.setSpacing(10)
+        
+        # 创建滑动条
+        self.create_joint_sliders()
+        
+        scroll_area.setWidget(scroll_content)
+        layout.addWidget(scroll_area)
+        
+        return panel
+
+    
+
+    def create_joint_sliders(self):
+        """创建关节滑动条"""
+        # 清除现有滑动条
+        for i in reversed(range(self.sliders_layout.count())):
+            item = self.sliders_layout.itemAt(i)
+            if item.widget():
+                item.widget().deleteLater()
+        
+        # 创建新滑动条
+        self.sliders = []
+        self.slider_labels = []
+        
+        for i, (name, value) in enumerate(zip(
+            self.hand_config.joint_names, self.hand_config.init_pos
+        )):
+            # 创建标签
+            label = QLabel(f"{name}: {value}")
+            label.setMinimumWidth(120)
+            
+            # 创建滑动条
+            slider = QSlider(Qt.Horizontal)
+            slider.setRange(0, 255)
+            slider.setValue(value)
+            slider.valueChanged.connect(
+                lambda val, idx=i: self.on_slider_value_changed(idx, val)
+            )
+            
+            # 添加到布局
+            row, col = divmod(i, 1)
+            self.sliders_layout.addWidget(label, row, 0)
+            self.sliders_layout.addWidget(slider, row, 1)
+            
+            self.sliders.append(slider)
+            self.slider_labels.append(label)
+
+    def create_preset_actions_panel(self):
+        """创建预设动作面板"""
+        panel = QWidget()
+        layout = QVBoxLayout(panel)
+        
+        # 自定义预设动作
+        sys_preset_group = QGroupBox("自定义预设动作(名称不可重复)")
+        sys_preset_layout = QGridLayout(sys_preset_group)
+        sys_preset_layout.setSpacing(8)
+        
+        # 添加系统预设动作按钮
+        self.create_system_preset_buttons(sys_preset_layout)
+        layout.addWidget(sys_preset_group)
+        
+        # 添加动作按钮
+        actions_layout = QHBoxLayout()
+        
+        # 添加循环运行按钮
+        self.cycle_button = QPushButton("循环预设动作")
+        self.cycle_button.setProperty("category", "action")
+        self.cycle_button.clicked.connect(self.on_cycle_clicked)
+        actions_layout.addWidget(self.cycle_button)
+        
+        self.home_button = QPushButton("回到初始位置")
+        self.home_button.setProperty("category", "action")
+        self.home_button.clicked.connect(self.on_home_clicked)
+        actions_layout.addWidget(self.home_button)
+        
+        self.stop_button = QPushButton("停止所有动作")
+        self.stop_button.setProperty("category", "danger")
+        self.stop_button.clicked.connect(self.on_stop_clicked)
+        actions_layout.addWidget(self.stop_button)
+        
+        layout.addLayout(actions_layout)
+        
+        return panel
+
+    def create_system_preset_buttons(self, parent_layout):
+        """创建系统预设动作按钮"""
+        self.preset_buttons = []  # 清空按钮列表
+        if self.hand_config.preset_actions:
+            buttons = []
+            for idx, (name, positions) in enumerate(self.hand_config.preset_actions.items()):
+                button = QPushButton(name)
+                button.setProperty("category", "preset")
+                button.clicked.connect(
+                    lambda checked, pos=positions: self.on_preset_action_clicked(pos)
+                )
+                buttons.append(button)
+                self.preset_buttons.append(button)  # 保存按钮引用
+                
+            # 添加到网格布局
+            cols = 2
+            for i, button in enumerate(buttons):
+                row, col = divmod(i, cols)
+                parent_layout.addWidget(button, row, col)
+
+    def create_status_monitor_panel(self):
+        """创建状态监控面板（速度/扭矩各占一行，并实时显示滑块值）"""
+        panel = QWidget()
+        layout = QVBoxLayout(panel)
+
+        # —— 1. 标题 ——
+        title_label = QLabel("状态监控")
+        title_label.setFont(QFont("Microsoft YaHei", 14, QFont.Bold))
+        layout.addWidget(title_label)
+
+        # —— 2. 新增：速度与扭矩设置（每行一个）——
+        quick_set_gb = QGroupBox("快速设置")
+        qv_layout = QVBoxLayout(quick_set_gb)
+
+        # 速度行
+        speed_hbox = QHBoxLayout()
+        speed_hbox.addWidget(QLabel("速度:"))
+        self.speed_slider = QSlider(Qt.Horizontal)
+        self.speed_slider.setRange(0, 255)
+        self.speed_slider.setValue(255)
+        self.speed_slider.setMinimumWidth(150)
+        speed_hbox.addWidget(self.speed_slider)
+        self.speed_val_lbl = QLabel("255")          # 实时值
+        self.speed_val_lbl.setMinimumWidth(30)
+        speed_hbox.addWidget(self.speed_val_lbl)
+        self.speed_btn = QPushButton("设置速度")
+        self.speed_btn.clicked.connect(
+            lambda: (
+                self.ros_manager.publish_speed(self.speed_slider.value()),
+                self.status_updated.emit(
+                    "info", f"速度已设为 {self.speed_slider.value()}")
+            ))
+        speed_hbox.addWidget(self.speed_btn)
+        speed_hbox.addStretch()
+        qv_layout.addLayout(speed_hbox)
+
+        # 扭矩行
+        torque_hbox = QHBoxLayout()
+        torque_hbox.addWidget(QLabel("扭矩:"))
+        self.torque_slider = QSlider(Qt.Horizontal)
+        self.torque_slider.setRange(0, 255)
+        self.torque_slider.setValue(255)
+        self.torque_slider.setMinimumWidth(150)
+        torque_hbox.addWidget(self.torque_slider)
+        self.torque_val_lbl = QLabel("255")
+        self.torque_val_lbl.setMinimumWidth(30)
+        torque_hbox.addWidget(self.torque_val_lbl)
+        self.torque_btn = QPushButton("设置扭矩")
+        self.torque_btn.clicked.connect(
+            lambda: (
+                self.ros_manager.publish_torque(self.torque_slider.value()),
+                self.status_updated.emit(
+                    "info", f"扭矩已设为 {self.torque_slider.value()}")
+            ))
+        torque_hbox.addWidget(self.torque_btn)
+        torque_hbox.addStretch()
+        qv_layout.addLayout(torque_hbox)
+
+        layout.addWidget(quick_set_gb)
+
+        # —— 3. 原有标签页部分，完全不动 ——
+        tab_widget = QTabWidget()
+
+        # 系统信息标签页
+        sys_info_widget = QWidget()
+        sys_info_layout = QVBoxLayout(sys_info_widget)
+
+        conn_group = QGroupBox("连接状态")
+        conn_layout = QVBoxLayout(conn_group)
+        if self.ros_manager.publisher.get_subscription_count() > 0:
+            self.connection_status = QLabel("ROS2节点已连接")
+            self.connection_status.setObjectName("StatusLabel")
+            self.connection_status.setObjectName("StatusInfo")
+        else:
+            self.connection_status = QLabel("ROS2节点未连接")
+            self.connection_status.setObjectName("StatusLabel")
+            self.connection_status.setObjectName("StatusError")
+        conn_layout.addWidget(self.connection_status)
+
+        hand_info_group = QGroupBox("手部信息")
+        hand_info_layout = QVBoxLayout(hand_info_group)
+        info_text = f"""手部类型: {self.hand_type}
+关节型号: {self.hand_joint}
+关节数量: {len(self.hand_config.joint_names)}
+发布频率: {self.ros_manager.hz} Hz"""
+        self.hand_info_label = QLabel(info_text)
+        self.hand_info_label.setWordWrap(True)
+        hand_info_layout.addWidget(self.hand_info_label)
+
+        sys_info_layout.addWidget(conn_group)
+        sys_info_layout.addWidget(hand_info_group)
+        sys_info_layout.addStretch()
+        tab_widget.addTab(sys_info_widget, "系统信息")
+
+        # 状态日志标签页
+        log_widget = QWidget()
+        log_layout = QVBoxLayout(log_widget)
+        self.status_log = QLabel("等待系统启动...")
+        self.status_log.setObjectName("StatusLabel")
+        self.status_log.setObjectName("StatusInfo")
+        self.status_log.setWordWrap(True)
+        self.status_log.setMinimumHeight(300)
+        log_layout.addWidget(self.status_log)
+        clear_log_btn = QPushButton("清除日志")
+        clear_log_btn.clicked.connect(self.clear_status_log)
+        log_layout.addWidget(clear_log_btn)
+        tab_widget.addTab(log_widget, "状态日志")
+
+        layout.addWidget(tab_widget)
+
+        # —— 4. 实时更新滑块值 ——
+        self.speed_slider.valueChanged.connect(
+            lambda v: self.speed_val_lbl.setText(str(v)))
+        self.torque_slider.valueChanged.connect(
+            lambda v: self.torque_val_lbl.setText(str(v)))
+        return panel
+
+    def create_value_display_panel(self):
+        """创建滑动条数值显示面板"""
+        panel = QGroupBox("关节数值列表")
+        layout = QVBoxLayout(panel)
+        
+        # 设置布局上下间隔为20像素
+        layout.setContentsMargins(10, 20, 10, 20)
+        
+        self.value_display = QTextEdit()
+        self.value_display.setObjectName("ValueDisplay")
+        self.value_display.setReadOnly(True)  # 设置只读模式，允许复制
+        self.value_display.setMinimumHeight(60)  # 调整最小高度
+        self.value_display.setMaximumHeight(80)  # 限制最大高度
+        self.value_display.setText("[]")
+        
+        layout.addWidget(self.value_display)
+        
+        return panel
+
+    def on_slider_value_changed(self, index: int, value: int):
+        """滑动条值改变事件处理"""
+        if 0 <= index < len(self.slider_labels):
+            joint_name = self.hand_config.joint_names[index]
+            self.slider_labels[index].setText(f"{joint_name}: {value}")
+            
+        # 更新数值显示
+        self.update_value_display()
+
+    def update_value_display(self):
+        """更新数值显示面板内容"""
+        # 获取所有滑动条的当前值
+        values = [slider.value() for slider in self.sliders]
+        
+        # 格式化显示为列表形式
+        self.value_display.setText(f"{values}")
+
+    def on_preset_action_clicked(self, positions: List[int]):
+        """预设动作按钮点击事件处理"""
+        if len(positions) != len(self.sliders):
+            QMessageBox.warning(
+                self, "动作不匹配", 
+                f"预设动作关节数量({len(positions)})与当前关节数量({len(self.sliders)})不匹配"
+            )
+            return
+            
+        # 更新滑动条
+        for i, (slider, pos) in enumerate(zip(self.sliders, positions)):
+            slider.setValue(pos)
+            self.on_slider_value_changed(i, pos)
+            
+        # 发布关节状态
+        self.publish_joint_state()
+
+    def on_home_clicked(self):
+        """回到初始位置按钮点击事件处理"""
+        for slider, pos in zip(self.sliders, self.hand_config.init_pos):
+            slider.setValue(pos)
+            
+        self.publish_joint_state()
+        self.status_updated.emit("info", "回到初始位置")
+        
+        # 更新数值显示
+        self.update_value_display()
+
+    def on_stop_clicked(self):
+        """停止所有动作按钮点击事件处理"""
+        # 停止循环定时器
+        if self.cycle_timer and self.cycle_timer.isActive():
+            self.cycle_timer.stop()
+            self.cycle_timer = None
+            self.cycle_button.setText("循环运行预设动作")
+            self.reset_preset_buttons_color()
+            
+        self.status_updated.emit("warning", "已停止所有动作")
+
+    def on_cycle_clicked(self):
+        """循环运行预设动作按钮点击事件处理"""
+        if not self.hand_config.preset_actions:
+            QMessageBox.warning(self, "无预设动作", "当前手部型号没有预设动作可循环运行")
+            return
+            
+        if self.cycle_timer and self.cycle_timer.isActive():
+            # 停止循环
+            self.cycle_timer.stop()
+            self.cycle_timer = None
+            self.cycle_button.setText("循环运行预设动作")
+            self.reset_preset_buttons_color()
+            self.status_updated.emit("info", "已停止循环运行预设动作")
+        else:
+            # 开始循环
+            self.current_action_index = -1  # 重置索引
+            self.cycle_timer = QTimer(self)
+            self.cycle_timer.timeout.connect(self.run_next_action)
+            self.cycle_timer.start(LOOP_TIME)  # 2秒间隔
+            self.cycle_button.setText("停止循环运行")
+            self.status_updated.emit("info", "开始循环运行预设动作")
+            self.run_next_action()  # 立即运行第一个动作
+
+    def run_next_action(self):
+        """运行下一个预设动作"""
+        if not self.hand_config.preset_actions:
+            return
+            
+        # 重置所有按钮颜色
+        self.reset_preset_buttons_color()
+        
+        # 计算下一个动作索引
+        self.current_action_index = (self.current_action_index + 1) % len(self.hand_config.preset_actions)
+        
+        # 获取下一个动作
+        action_names = list(self.hand_config.preset_actions.keys())
+        action_name = action_names[self.current_action_index]
+        action_positions = self.hand_config.preset_actions[action_name]
+        
+        # 执行动作
+        self.on_preset_action_clicked(action_positions)
+        
+        # 高亮当前动作按钮
+        if 0 <= self.current_action_index < len(self.preset_buttons):
+            button = self.preset_buttons[self.current_action_index]
+            button.setStyleSheet("background-color: green; color: white; border-color: #91D5FF;")
+            
+        self.status_updated.emit("info", f"运行预设动作: {action_name}")
+
+    def reset_preset_buttons_color(self):
+        """重置所有预设按钮颜色"""
+        for button in self.preset_buttons:
+            button.setStyleSheet("")  # 恢复默认样式
+            button.setProperty("category", "preset")  # 恢复类别属性
+            # 强制样式刷新
+            button.style().unpolish(button)
+            button.style().polish(button)
+
+    def on_joint_type_changed(self, joint_type: str):
+        """关节类型改变事件处理"""
+        self.hand_joint = joint_type
+        self.hand_config = _HAND_CONFIGS[self.hand_joint]
+        
+        # 更新手部信息
+        info_text = f"""手部类型: {self.hand_type}
+关节型号: {self.hand_joint}
+关节数量: {len(self.hand_config.joint_names)}
+发布频率: {self.ros_manager.hz} Hz"""
+        self.hand_info_label.setText(info_text)
+        
+        # 重新创建滑动条和预设按钮
+        self.create_joint_sliders()
+        self.create_system_preset_buttons(self.sys_preset_layout)  # 假设sys_preset_layout是类变量
+        
+        # 更新数值显示
+        self.update_value_display()
+        self.status_updated.emit("info", f"已切换到手部型号: {joint_type}")
+
+    def publish_joint_state(self):
+        """发布当前关节状态"""
+        positions = [slider.value() for slider in self.sliders]
+        self.ros_manager.publish_joint_state(positions)
+
+    def update_status(self, status_type: str, message: str):
+        """更新状态显示"""
+        # 更新连接状态
+        if status_type == "info" and "ROS2节点初始化成功" in message:
+            self.connection_status.setText("ROS2节点已连接")
+            self.connection_status.setObjectName("StatusLabel")
+            self.connection_status.setObjectName("StatusInfo")
+            
+        # 更新日志
+        current_time = time.strftime("%H:%M:%S")
+        log_entry = f"[{current_time}] {message}\n"
+        current_log = self.status_log.text()
+        
+        if len(current_log) > 10000:  # 限制日志长度
+            current_log = current_log[-10000:]
+            
+        self.status_log.setText(log_entry + current_log)
+        
+        # 设置日志样式
+        self.status_log.setObjectName("StatusLabel")
+        if status_type == "error":
+            self.status_log.setObjectName("StatusError")
+        else:
+            self.status_log.setObjectName("StatusInfo")
+
+    def clear_status_log(self):
+        """清除状态日志"""
+        self.status_log.setText("日志已清除")
+        self.status_log.setObjectName("StatusLabel")
+        self.status_log.setObjectName("StatusInfo")
+
+    def closeEvent(self, event):
+        """窗口关闭事件处理"""
+        if self.cycle_timer and self.cycle_timer.isActive():
+            self.cycle_timer.stop()
+        super().closeEvent(event)
+
+def main(args=None):
+    """主函数"""
+    try:
+        # 创建ROS2节点管理器
+        ros_manager = ROS2NodeManager()
+        
+        # 创建Qt应用
+        app = QApplication(sys.argv)
+        
+        # 创建GUI
+        window = HandControlGUI(ros_manager)
+        
+        # 连接状态更新信号
+        ros_manager.status_updated.connect(window.update_status)
+        window.status_updated = ros_manager.status_updated
+        
+        # 显示窗口
+        window.show()
+        
+        # 运行应用
+        exit_code = app.exec_()
+        
+        # 清理ROS2
+        if rclpy.ok():
+            ros_manager.node.destroy_node()
+            rclpy.shutdown()
+            
+        sys.exit(exit_code)
+    except Exception as e:
+        print(f"应用程序启动失败: {str(e)}")
+        sys.exit(1)
+
+if __name__ == '__main__':
+    main()

src/gui_control/gui_control/gui_control.py.ttk

@@ -0,0 +1,790 @@
+import sys
+import time
+import json
+import threading
+from dataclasses import dataclass
+from typing import List, Dict
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import String, Header
+from sensor_msgs.msg import JointState
+import tkinter as tk
+from tkinter import ttk, scrolledtext, messagebox
+import tkinter.font as tkfont
+
+from .utils.mapping import *
+from .config.constants import _HAND_CONFIGS
+LOOP_TIME = 1000 # 循环动作间隔时间 毫秒
+class ROS2NodeManager:
+    """ROS2节点管理器，处理ROS通信"""
+    
+    def __init__(self, node_name: str = "hand_control_node"):
+        self.node = None
+        self.publisher = None
+        self.joint_state = JointState()
+        self.joint_state.header = Header()
+        self.status_callbacks = []
+        
+        # 初始化ROS2节点
+        self.init_node(node_name)
+
+    def add_status_callback(self, callback):
+        """添加状态回调函数"""
+        self.status_callbacks.append(callback)
+
+    def emit_status(self, status_type: str, message: str):
+        """发射状态信号"""
+        for callback in self.status_callbacks:
+            callback(status_type, message)
+
+    def init_node(self, node_name: str):
+        """初始化ROS2节点"""
+        try:
+            if not rclpy.ok():
+                rclpy.init(args=None)
+            self.node = Node(node_name)
+            
+            # 声明参数
+            self.node.declare_parameter('hand_type', 'left')
+            self.node.declare_parameter('hand_joint', 'L10')
+            self.node.declare_parameter('topic_hz', 30)
+            self.node.declare_parameter('is_arc', False)
+            
+            # 获取参数
+            self.hand_type = self.node.get_parameter('hand_type').value
+            self.hand_joint = self.node.get_parameter('hand_joint').value
+            self.hz = self.node.get_parameter('topic_hz').value
+            self.is_arc = self.node.get_parameter('is_arc').value
+            
+            if self.is_arc == True:
+                # 创建发布者
+                self.publisher_arc = self.node.create_publisher(
+                    JointState, f'/cb_{self.hand_type}_hand_control_cmd_arc', 10
+                )
+            # 创建发布者
+            self.publisher = self.node.create_publisher(
+                JointState, f'/cb_{self.hand_type}_hand_control_cmd', 10
+            )
+            # 新增 speed / torque 发布者
+            self.speed_pub = self.node.create_publisher(
+                String, f'/cb_hand_setting_cmd', 10)
+            self.torque_pub = self.node.create_publisher(
+                String, f'/cb_hand_setting_cmd', 10)
+            self.emit_status("info", f"ROS2节点初始化成功: {self.hand_type} {self.hand_joint}")
+            
+            # 启动ROS2自旋线程
+            self.spin_thread = threading.Thread(target=self.spin_node, daemon=True)
+            self.spin_thread.start()
+        except Exception as e:
+            self.emit_status("error", f"ROS2初始化失败: {str(e)}")
+            raise
+
+    def spin_node(self):
+        """运行ROS2节点自旋循环"""
+        while rclpy.ok() and self.node:
+            rclpy.spin_once(self.node, timeout_sec=0.1)
+
+    def publish_joint_state(self, positions: List[int]):
+        """发布关节状态消息"""
+        if not self.publisher or not self.node:
+            self.emit_status("error", "ROS2发布者未初始化")
+            return
+            
+        try:
+            self.joint_state.header.stamp = self.node.get_clock().now().to_msg()
+            self.joint_state.position = [float(pos) for pos in positions]
+            hand_config = _HAND_CONFIGS[self.hand_joint]
+            if len(hand_config.joint_names) == len(positions):
+                if hand_config.joint_names_en != None:
+                    self.joint_state.name = hand_config.joint_names_en
+                else:
+                    self.joint_state.name = hand_config.joint_names
+                
+            self.publisher.publish(self.joint_state)
+            if self.is_arc == True:
+                if self.hand_joint == "O6":
+                    if self.hand_type == "left":
+                        pose = range_to_arc_left(positions,self.hand_joint)
+                    elif self.hand_type == "right":
+                        pose = range_to_arc_right(positions,self.hand_joint)
+                elif self.hand_joint == "L7" or self.hand_joint == "L21" or self.hand_joint == "L25":
+                    if self.hand_type == "left":
+                        pose = range_to_arc_left(positions,self.hand_joint)
+                    elif self.hand_type == "right":
+                        pose = range_to_arc_right(positions,self.hand_joint)
+                elif self.hand_joint == "L10":
+                    if self.hand_type == "left":
+                        pose = range_to_arc_left_10(positions)
+                    elif self.hand_type == "right":
+                        pose = range_to_arc_right_10(positions)
+                elif self.hand_joint == "L20":
+                    if self.hand_type == "left":
+                        pose = range_to_arc_left_l20(positions)
+                    elif self.hand_type == "right":
+                        pose = range_to_arc_right_l20(positions)
+                else:
+                    print(f"当前{self.hand_joint} {self.hand_type}不支持弧度转换", flush=True)
+                self.joint_state.position = [float(pos) for pos in pose]
+                self.publisher_arc.publish(self.joint_state)
+            self.emit_status("info", "关节状态已发布")
+        except Exception as e:
+            self.emit_status("error", f"发布失败: {str(e)}")
+
+    def publish_speed(self, val: int):
+        joint_len = 0
+        if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"):
+            joint_len = 6
+        elif self.hand_joint == "L7":
+            joint_len = 7
+        elif self.hand_joint == "L10":
+            joint_len = 10
+        else:
+            joint_len = 5
+        msg = String()
+        v = [val] * joint_len
+        data = {
+            "setting_cmd": "set_speed",
+            "params": {"hand_type":self.hand_type,"speed": v},
+        }
+        msg.data = json.dumps(data)
+        print(f"速度值：{v}", flush=True)
+        self.speed_pub.publish(msg)
+
+    def publish_torque(self, val: int):
+        joint_len = 0
+        if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"):
+            joint_len = 6
+        elif self.hand_joint == "L7":
+            joint_len = 7
+        elif self.hand_joint == "L10":
+            joint_len = 10
+        else:
+            joint_len = 5
+        msg = String()
+        v = [val] * joint_len
+        data = {
+            "setting_cmd": "set_max_torque_limits",
+            "params": {"hand_type":self.hand_type,"torque": v},
+        }
+        
+        msg.data = json.dumps(data)
+        print(f"扭矩值：{v}", flush=True)
+        self.torque_pub.publish(msg)
+
+    def shutdown(self):
+        """关闭ROS2节点"""
+        if self.node:
+            self.node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+class HandControlGUI:
+    """灵巧手控制界面"""
+    
+    def __init__(self, ros_manager: ROS2NodeManager):
+        self.ros_manager = ros_manager
+        self.ros_manager.add_status_callback(self.update_status)
+        
+        # 获取手部配置
+        self.hand_joint = self.ros_manager.hand_joint
+        self.hand_type = self.ros_manager.hand_type
+        self.hand_config = _HAND_CONFIGS[self.hand_joint]
+        
+        # 循环控制变量
+        self.cycle_timer = None
+        self.current_action_index = -1
+        self.preset_buttons = []
+        
+        # 初始化UI
+        self.init_ui()
+        
+        # 设置定时器发布关节状态
+        self.publish_timer_id = None
+        self.start_publish_timer()
+
+    def init_ui(self):
+        """初始化用户界面"""
+        # 创建主窗口
+        self.root = tk.Tk()
+        self.root.title(f'灵巧手控制界面 - {self.hand_type} {self.hand_joint}')
+        self.root.geometry('1300x700')
+        
+        # 设置样式
+        self.style = ttk.Style()
+        self.style.configure('TFrame', background='#f0f0f0')
+        self.style.configure('TLabel', background='#f0f0f0', font=('Microsoft YaHei', 10))
+        self.style.configure('Title.TLabel', font=('Microsoft YaHei', 14, 'bold'), foreground='#165DFF')
+        self.style.configure('Group.TLabelframe', borderwidth=2, relief='groove')
+        self.style.configure('Group.TLabelframe.Label', font=('Microsoft YaHei', 10, 'bold'), foreground='#165DFF')
+        # 配置信息框样式 - 灰色背景
+        self.style.configure('Info.TFrame', background='#e0e0e0')
+        # 配置高亮样式 - 滑动时的背景色
+        self.style.configure('Highlight.TFrame', background='#e6f7ff')  # 浅蓝色背景
+        
+        # 初始化当前点击按钮索引
+        self.current_clicked_button = None
+        
+        # 创建主框架
+        main_frame = ttk.Frame(self.root)
+        main_frame.pack(fill=tk.BOTH, expand=True, padx=10, pady=10)
+        
+        # 创建水平分割的框架
+        self.paned_window = ttk.PanedWindow(main_frame, orient=tk.HORIZONTAL)
+        self.paned_window.pack(fill=tk.BOTH, expand=True)
+        
+        # 创建左侧关节控制面板
+        self.joint_control_panel = self.create_joint_control_panel()
+        self.paned_window.add(self.joint_control_panel, weight=5)
+        
+        # 创建中间预设动作面板
+        self.preset_actions_panel = self.create_preset_actions_panel()
+        self.paned_window.add(self.preset_actions_panel, weight=3)
+        
+        # 创建右侧状态监控面板
+        self.status_monitor_panel = self.create_status_monitor_panel()
+        self.paned_window.add(self.status_monitor_panel, weight=4)
+        
+        # 创建底部数值显示面板
+        self.value_display_panel = self.create_value_display_panel()
+        self.value_display_panel.pack(fill=tk.X, pady=(10, 0))
+        
+        # 绑定窗口大小变化事件
+        self.joint_control_panel.bind('<Configure>', self.on_joint_panel_resize)
+        
+        # 初始更新数值显示
+        self.update_value_display()
+
+    def create_joint_control_panel(self):
+        """创建关节控制面板"""
+        frame = ttk.Frame(self.root)
+        frame.config(width=600)  # 设置最小宽度为400像素
+        frame.pack_propagate(False)  # 阻止子控件改变框架大小
+        # 创建标题
+        title_label = ttk.Label(frame, text=f"关节控制 - {self.hand_joint}", style='Title.TLabel')
+        title_label.pack(pady=(0, 10))
+        
+        # 创建滚动框架
+        self.canvas = tk.Canvas(frame, bg='#f0f0f0', highlightthickness=0)
+        self.scrollbar = ttk.Scrollbar(frame, orient=tk.VERTICAL, command=self.canvas.yview)
+        self.scrollable_frame = ttk.Frame(self.canvas)
+        
+        self.scrollable_frame.bind(
+            "<Configure>",
+            self.on_scrollable_frame_configure  # 修改为调用方法
+        )
+        
+        self.canvas.create_window((0, 0), window=self.scrollable_frame, anchor="nw")
+        self.canvas.configure(yscrollcommand=self.scrollbar.set)
+        
+        # 创建滑动条
+        self.create_joint_sliders(self.scrollable_frame)
+        
+        self.canvas.pack(side=tk.LEFT, fill=tk.BOTH, expand=True)
+        # 初始时不显示滚动条
+        self.scrollbar.pack(side=tk.RIGHT, fill=tk.Y)
+        self.scrollbar.pack_forget()  # 隐藏滚动条
+        
+        return frame
+    def on_scrollable_frame_configure(self, event):
+        """滚动区域配置变化事件 - 动态显示/隐藏滚动条"""
+        # 更新滚动区域
+        self.canvas.configure(scrollregion=self.canvas.bbox("all"))
+        
+        # 检查是否需要显示滚动条
+        scrollable_height = self.scrollable_frame.winfo_reqheight()  # 内容所需高度
+        canvas_height = self.canvas.winfo_height()  # 画布实际高度
+        
+        # 如果内容高度大于画布高度，显示滚动条；否则隐藏
+        if scrollable_height > canvas_height and canvas_height > 0:
+            self.scrollbar.pack(side=tk.RIGHT, fill=tk.Y)
+        else:
+            self.scrollbar.pack_forget()
+
+    def create_joint_sliders(self, parent):
+        """创建关节滑动条"""
+        self.sliders = []
+        self.slider_labels = []
+        self.slider_frames = []
+        
+        # 存储父框架的引用，用于后续调整大小
+        self.sliders_parent = parent
+        
+        for i, (name, value) in enumerate(zip(
+            self.hand_config.joint_names, self.hand_config.init_pos
+        )):
+            # 创建框架 - 添加左右边距
+            slider_frame = ttk.Frame(parent)
+            slider_frame.pack(fill=tk.X, pady=5, padx=(10,0))  # 左右各10像素边距
+            self.slider_frames.append(slider_frame)
+            
+            # 创建标签
+            label = ttk.Label(slider_frame, text=f"{name}: {value}", width=15)
+            label.pack(side=tk.LEFT, padx=(0, 10))
+            
+            # 创建滑动条 - 初始长度设为300，但会随窗口调整
+            slider = ttk.Scale(slider_frame, from_=0, to=255, value=value, 
+                              orient=tk.HORIZONTAL, length=300,  # 初始长度
+                              command=lambda val, idx=i: self.on_slider_value_changed(idx, val))
+            slider.pack(side=tk.LEFT, fill=tk.X, expand=True)
+            
+            self.sliders.append(slider)
+            self.slider_labels.append(label)
+
+    def on_joint_panel_resize(self, event):
+        """关节控制面板大小变化事件"""
+        # 获取面板当前宽度
+        panel_width = event.width
+        
+        # 为所有滑动条设置新的长度
+        for slider in self.sliders:
+            # 计算新的滑动条长度（面板宽度减去标签和其他元素的估计宽度）
+            # 标签宽度约120像素 + 左右边距各10像素 + 标签与滑动条间距10像素
+            new_length = max(panel_width - 150, 100)  # 最小长度100像素
+            # 重新配置滑动条
+            slider.configure(length=new_length-30)
+        # 面板大小变化后重新检查是否需要滚动条
+        self.root.after(100, self.check_scrollbar_visibility)  # 延迟检查，确保高度已更新
+
+    def check_scrollbar_visibility(self):
+        """检查滚动条可见性"""
+        # 只有在画布已经有实际高度时才检查
+        if self.canvas.winfo_height() > 0:
+            scrollable_height = self.scrollable_frame.winfo_reqheight()
+            canvas_height = self.canvas.winfo_height()
+            
+            if scrollable_height > canvas_height:
+                self.scrollbar.pack(side=tk.RIGHT, fill=tk.Y)
+            else:
+                self.scrollbar.pack_forget()
+
+    def create_preset_actions_panel(self):
+        """创建预设动作面板"""
+        frame = ttk.Frame(self.root)
+        
+        # 系统预设动作
+        sys_preset_group = ttk.LabelFrame(frame, text="系统预设", style='Group.TLabelframe')
+        sys_preset_group.pack(fill=tk.BOTH, expand=True, pady=(0, 10))
+        
+        # 创建系统预设动作按钮 - 3列布局
+        self.create_system_preset_buttons(sys_preset_group)
+        
+        # 动作按钮框架
+        actions_frame = ttk.Frame(frame)
+        actions_frame.pack(fill=tk.X, pady=10)
+        
+        # 循环运行按钮
+        self.cycle_button = ttk.Button(actions_frame, text="循环预设动作", 
+                                      command=self.on_cycle_clicked, width=12)
+        self.cycle_button.pack(side=tk.LEFT, padx=5)
+        
+        # 回到初始位置按钮
+        self.home_button = ttk.Button(actions_frame, text="回到初始位置", 
+                                     command=self.on_home_clicked, width=12)
+        self.home_button.pack(side=tk.LEFT, padx=5)
+        
+        # 停止所有动作按钮
+        self.stop_button = ttk.Button(actions_frame, text="停止所有动作", 
+                                     command=self.on_stop_clicked, width=12)
+        self.stop_button.pack(side=tk.LEFT, padx=5)
+        
+        return frame
+
+    def create_system_preset_buttons(self, parent):
+        """创建系统预设动作按钮 - 3列布局"""
+        self.preset_buttons = []
+        if self.hand_config.preset_actions:
+            buttons_frame = ttk.Frame(parent)
+            buttons_frame.pack(fill=tk.BOTH, expand=True, padx=10, pady=10)
+            
+            buttons = []
+            for idx, (name, positions) in enumerate(self.hand_config.preset_actions.items()):
+                # 使用 tk.Button 而不是 ttk.Button，以便更好地控制背景色
+                button = tk.Button(buttons_frame, text=name, bg='#E6F7FF', fg='#1890FF', width=15,
+                                relief='raised', bd=1, font=('Microsoft YaHei', 10),
+                                command=lambda pos=positions, btn_idx=idx: self.on_preset_action_clicked(pos, btn_idx))
+                buttons.append(button)
+                self.preset_buttons.append(button)
+            
+            # 3列布局
+            cols = 3
+            for i, button in enumerate(buttons):
+                row, col = divmod(i, cols)
+                button.grid(row=row, column=col, sticky='ew', padx=5, pady=5)
+            
+            # 配置列权重
+            for i in range(cols):
+                buttons_frame.columnconfigure(i, weight=1)
+
+    def create_status_monitor_panel(self):
+        """创建状态监控面板"""
+        frame = ttk.Frame(self.root)
+        
+        # 标题
+        title_label = ttk.Label(frame, text="状态监控", style='Title.TLabel')
+        title_label.pack(pady=(0, 10))
+        
+        # 快速设置框架
+        quick_set_group = ttk.LabelFrame(frame, text="快速设置", style='Group.TLabelframe')
+        quick_set_group.pack(fill=tk.X, pady=(0, 10))
+        
+        # 速度设置
+        speed_frame = ttk.Frame(quick_set_group)
+        speed_frame.pack(fill=tk.X, padx=10, pady=5)
+        
+        ttk.Label(speed_frame, text="速度:").pack(side=tk.LEFT)
+        self.speed_var = tk.IntVar(value=255)
+        self.speed_slider = ttk.Scale(speed_frame, from_=0, to=255, 
+                                     variable=self.speed_var, orient=tk.HORIZONTAL)
+        self.speed_slider.pack(side=tk.LEFT, fill=tk.X, expand=True, padx=5)
+        self.speed_val_label = ttk.Label(speed_frame, text="255", width=4)
+        self.speed_val_label.pack(side=tk.LEFT)
+        self.speed_btn = ttk.Button(speed_frame, text="设置速度",
+                                   command=self.on_speed_set)
+        self.speed_btn.pack(side=tk.LEFT, padx=5)
+        
+        # 扭矩设置
+        torque_frame = ttk.Frame(quick_set_group)
+        torque_frame.pack(fill=tk.X, padx=10, pady=5)
+        
+        ttk.Label(torque_frame, text="扭矩:").pack(side=tk.LEFT)
+        self.torque_var = tk.IntVar(value=255)
+        self.torque_slider = ttk.Scale(torque_frame, from_=0, to=255, 
+                                      variable=self.torque_var, orient=tk.HORIZONTAL)
+        self.torque_slider.pack(side=tk.LEFT, fill=tk.X, expand=True, padx=5)
+        self.torque_val_label = ttk.Label(torque_frame, text="255", width=4)
+        self.torque_val_label.pack(side=tk.LEFT)
+        self.torque_btn = ttk.Button(torque_frame, text="设置扭矩",
+                                    command=self.on_torque_set)
+        self.torque_btn.pack(side=tk.LEFT, padx=5)
+        
+        # 绑定滑块值变化事件
+        self.speed_var.trace('w', self.on_speed_changed)
+        self.torque_var.trace('w', self.on_torque_changed)
+        
+        # 创建标签页
+        notebook = ttk.Notebook(frame)
+        notebook.pack(fill=tk.BOTH, expand=True)
+        
+        # 系统信息标签页
+        sys_info_frame = ttk.Frame(notebook)
+        notebook.add(sys_info_frame, text="系统信息")
+        
+        # 连接状态 - 灰色背景，左对齐
+        conn_group = ttk.LabelFrame(sys_info_frame, text="连接状态", style='Group.TLabelframe')
+        conn_group.pack(fill=tk.X, pady=5)
+        
+        # 创建灰色背景的框架
+        conn_content_frame = ttk.Frame(conn_group, style='Info.TFrame')
+        conn_content_frame.pack(fill=tk.X, padx=10, pady=10)
+        
+        if self.ros_manager.publisher.get_subscription_count() > 0:
+            self.connection_status = ttk.Label(conn_content_frame, text="ROS2节点已连接", 
+                                              foreground="green", background='#e0e0e0',
+                                              anchor='w')  # 左对齐
+        else:
+            self.connection_status = ttk.Label(conn_content_frame, text="ROS2节点未连接", 
+                                              foreground="red", background='#e0e0e0',
+                                              anchor='w')  # 左对齐
+        self.connection_status.pack(fill=tk.X)
+        
+        # 手部信息 - 灰色背景，左对齐
+        hand_info_group = ttk.LabelFrame(sys_info_frame, text="手部信息", style='Group.TLabelframe')
+        hand_info_group.pack(fill=tk.X, pady=5)
+        
+        # 创建灰色背景的框架
+        hand_info_content_frame = ttk.Frame(hand_info_group, style='Info.TFrame')
+        hand_info_content_frame.pack(fill=tk.X, padx=10, pady=10)
+        
+        info_text = f"""手部类型: {self.hand_type}
+关节型号: {self.hand_joint}
+关节数量: {len(self.hand_config.joint_names)}
+发布频率: {self.ros_manager.hz} Hz"""
+        self.hand_info_label = ttk.Label(hand_info_content_frame, text=info_text,
+                                        background='#e0e0e0', anchor='w', justify='left')  # 左对齐
+        self.hand_info_label.pack(fill=tk.X)
+        
+        # 状态日志标签页
+        log_frame = ttk.Frame(notebook)
+        notebook.add(log_frame, text="状态日志")
+        
+        # 日志文本框
+        self.status_log = scrolledtext.ScrolledText(log_frame, height=15, width=50)
+        self.status_log.pack(fill=tk.BOTH, expand=True, padx=10, pady=10)
+        self.status_log.insert(tk.END, "等待系统启动...\n")
+        self.status_log.config(state=tk.DISABLED)
+        
+        # 清除日志按钮
+        clear_log_btn = ttk.Button(log_frame, text="清除日志", 
+                                  command=self.clear_status_log)
+        clear_log_btn.pack(pady=5)
+        
+        return frame
+
+    def create_value_display_panel(self):
+        """创建滑动条数值显示面板"""
+        frame = ttk.LabelFrame(self.root, text="关节数值列表", style='Group.TLabelframe')
+        
+        # 创建按钮框架（放在显示框上方）
+        button_frame = ttk.Frame(frame)
+        button_frame.pack(fill=tk.X, padx=10, pady=(10, 5))
+        
+        # 复制按钮 - 左对齐
+        copy_button = ttk.Button(button_frame, text="复制到剪切板", 
+                            command=self.copy_values_to_clipboard, width=10)
+        copy_button.pack(side=tk.LEFT)
+        
+        # 数值显示框
+        self.value_display = scrolledtext.ScrolledText(frame, height=4, width=100)
+        self.value_display.pack(fill=tk.BOTH, expand=True, padx=10, pady=(0, 10))
+        self.value_display.insert(tk.END, "[]")
+        self.value_display.config(state=tk.DISABLED)
+        
+        return frame
+
+    def copy_values_to_clipboard(self):
+        """复制关节数值到系统剪切板"""
+        try:
+            # 获取当前按钮引用
+            button = self.root.focus_get()
+            original_text = "复制到剪切板"
+            
+            # 获取文本框内容
+            content = self.value_display.get(1.0, tk.END).strip()
+            
+            # 清除文本框的选中状态
+            self.value_display.tag_remove(tk.SEL, "1.0", tk.END)
+            
+            # 复制到剪切板
+            self.root.clipboard_clear()
+            self.root.clipboard_append(content)
+            
+            # 改变按钮文本提示复制成功
+            if isinstance(button, ttk.Button):
+                button.config(text="已复制!")
+                # 1.5秒后恢复原文本
+                self.root.after(1500, lambda: button.config(text=original_text))
+            
+            self.update_status("info", f"关节数值已复制到剪切板")
+            
+        except Exception as e:
+            self.update_status("error", f"复制失败: {str(e)}")
+
+    def on_slider_value_changed(self, index: int, value: str):
+        """滑动条值改变事件处理"""
+        value_int = int(float(value))
+        if 0 <= index < len(self.slider_labels):
+            joint_name = self.hand_config.joint_names[index]
+            self.slider_labels[index].config(text=f"{joint_name}: {value_int}")
+            
+        # 更新数值显示
+        self.update_value_display()
+
+    def update_value_display(self):
+        """更新数值显示面板内容"""
+        values = [int(float(slider.get())) for slider in self.sliders]
+        
+        self.value_display.config(state=tk.NORMAL)
+        self.value_display.delete(1.0, tk.END)
+        self.value_display.insert(tk.END, f"{values}")
+        self.value_display.config(state=tk.DISABLED)
+
+    def on_preset_action_clicked(self, positions: List[int], button_index: int = None):
+        """预设动作按钮点击事件处理"""
+        if len(positions) != len(self.sliders):
+            messagebox.showwarning(
+                "动作不匹配", 
+                f"预设动作关节数量({len(positions)})与当前关节数量({len(self.sliders)})不匹配"
+            )
+            return
+        
+        # 重置所有按钮颜色
+        self.reset_preset_buttons_color()
+        
+        # 高亮当前点击的按钮
+        if button_index is not None and 0 <= button_index < len(self.preset_buttons):
+            self.preset_buttons[button_index].config(bg='#1890FF', fg='white')
+            self.current_clicked_button = button_index
+        
+        # 更新滑动条
+        for i, (slider, pos) in enumerate(zip(self.sliders, positions)):
+            slider.set(pos)
+            self.on_slider_value_changed(i, str(pos))
+            
+        # 发布关节状态
+        self.publish_joint_state()
+
+    def on_home_clicked(self):
+        """回到初始位置按钮点击事件处理"""
+        for slider, pos in zip(self.sliders, self.hand_config.init_pos):
+            slider.set(pos)
+            
+        self.publish_joint_state()
+        self.update_status("info", "回到初始位置")
+        
+        # 更新数值显示
+        self.update_value_display()
+
+    def on_stop_clicked(self):
+        """停止所有动作按钮点击事件处理"""
+        # 停止循环定时器
+        if self.cycle_timer:
+            self.root.after_cancel(self.cycle_timer)
+            self.cycle_timer = None
+            self.cycle_button.config(text="循环运行预设动作")
+            self.reset_preset_buttons_color()
+            
+        self.update_status("warning", "已停止所有动作")
+
+    def on_cycle_clicked(self):
+        """循环运行预设动作按钮点击事件处理"""
+        if not self.hand_config.preset_actions:
+            messagebox.showwarning("无预设动作", "当前手部型号没有预设动作可循环运行")
+            return
+            
+        if self.cycle_timer:
+            # 停止循环
+            self.root.after_cancel(self.cycle_timer)
+            self.cycle_timer = None
+            self.cycle_button.config(text="循环运行预设动作")
+            self.reset_preset_buttons_color()
+            # 如果有之前点击的按钮，恢复其点击状态
+            if hasattr(self, 'current_clicked_button') and self.current_clicked_button is not None:
+                if 0 <= self.current_clicked_button < len(self.preset_buttons):
+                    self.preset_buttons[self.current_clicked_button].config(bg='#1890FF', fg='white')
+            self.update_status("info", "已停止循环运行预设动作")
+        else:
+            # 开始循环
+            self.current_action_index = -1
+            self.cycle_button.config(text="停止循环运行")
+            self.update_status("info", "开始循环运行预设动作")
+            self.run_next_action()
+
+    def run_next_action(self):
+        """运行下一个预设动作"""
+        if not self.hand_config.preset_actions:
+            return
+            
+        # 重置所有按钮颜色
+        self.reset_preset_buttons_color()
+        
+        # 计算下一个动作索引
+        self.current_action_index = (self.current_action_index + 1) % len(self.hand_config.preset_actions)
+        
+        # 获取下一个动作
+        action_names = list(self.hand_config.preset_actions.keys())
+        action_name = action_names[self.current_action_index]
+        action_positions = self.hand_config.preset_actions[action_name]
+        
+        # 执行动作
+        self.on_preset_action_clicked(action_positions, self.current_action_index)
+        
+        # 高亮当前动作按钮（循环模式使用不同的颜色）
+        if 0 <= self.current_action_index < len(self.preset_buttons):
+            button = self.preset_buttons[self.current_action_index]
+            button.config(bg='#52C41A', fg='white')  # 绿色表示循环中的按钮
+        
+        self.update_status("info", f"运行预设动作: {action_name}")
+        
+        # 设置下一个动作定时器
+        self.cycle_timer = self.root.after(LOOP_TIME, self.run_next_action)
+
+    def reset_preset_buttons_color(self):
+        """重置所有预设按钮颜色"""
+        for button in self.preset_buttons:
+            button.config(bg='#E6F7FF', fg='#1890FF')  # 恢复默认颜色
+
+    def on_speed_changed(self, *args):
+        """速度滑块值改变事件"""
+        self.speed_val_label.config(text=str(self.speed_var.get()))
+
+    def on_torque_changed(self, *args):
+        """扭矩滑块值改变事件"""
+        self.torque_val_label.config(text=str(self.torque_var.get()))
+
+    def on_speed_set(self):
+        """设置速度"""
+        speed_val = self.speed_var.get()
+        self.ros_manager.publish_speed(speed_val)
+        self.update_status("info", f"速度已设为 {speed_val}")
+
+    def on_torque_set(self):
+        """设置扭矩"""
+        torque_val = self.torque_var.get()
+        self.ros_manager.publish_torque(torque_val)
+        self.update_status("info", f"扭矩已设为 {torque_val}")
+
+    def publish_joint_state(self):
+        """发布当前关节状态"""
+        positions = [int(float(slider.get())) for slider in self.sliders]
+        self.ros_manager.publish_joint_state(positions)
+
+    def start_publish_timer(self):
+        """开始发布定时器"""
+        self.publish_joint_state()
+        interval = int(1000 / self.ros_manager.hz)
+        self.publish_timer_id = self.root.after(interval, self.start_publish_timer)
+
+    def update_status(self, status_type: str, message: str):
+        """更新状态显示"""
+        # 更新连接状态
+        if status_type == "info" and "ROS2节点初始化成功" in message:
+            self.connection_status.config(text="ROS2节点已连接", foreground="green")
+            
+        # 更新日志
+        current_time = time.strftime("%H:%M:%S")
+        log_entry = f"[{current_time}] {message}\n"
+        
+        self.status_log.config(state=tk.NORMAL)
+        self.status_log.insert(tk.END, log_entry)
+        self.status_log.see(tk.END)
+        
+        # 限制日志长度
+        log_content = self.status_log.get(1.0, tk.END)
+        if len(log_content) > 10000:
+            self.status_log.delete(1.0, f"{len(log_content)-10000}.0")
+            
+        self.status_log.config(state=tk.DISABLED)
+        
+        # 设置日志颜色
+        if status_type == "error":
+            # 可以在Tkinter中为不同消息类型添加颜色标记
+            pass
+
+    def clear_status_log(self):
+        """清除状态日志"""
+        self.status_log.config(state=tk.NORMAL)
+        self.status_log.delete(1.0, tk.END)
+        self.status_log.insert(tk.END, "日志已清除\n")
+        self.status_log.config(state=tk.DISABLED)
+
+    def run(self):
+        """运行GUI"""
+        try:
+            self.root.mainloop()
+        finally:
+            # 清理定时器
+            if self.cycle_timer:
+                self.root.after_cancel(self.cycle_timer)
+            if self.publish_timer_id:
+                self.root.after_cancel(self.publish_timer_id)
+            # 关闭ROS2节点
+            self.ros_manager.shutdown()
+
+def main(args=None):
+    """主函数"""
+    try:
+        # 创建ROS2节点管理器
+        ros_manager = ROS2NodeManager()
+        
+        # 创建GUI
+        gui = HandControlGUI(ros_manager)
+        
+        # 运行应用
+        gui.run()
+            
+    except Exception as e:
+        print(f"应用程序启动失败: {str(e)}")
+        sys.exit(1)
+
+if __name__ == '__main__':
+    main()

src/gui_control/gui_control/utils/color_msg.py

@@ -0,0 +1,27 @@
+#! /usr/bin/env python3
+
+import time
+
+class ColorMsg():
+    def __init__(self,msg: str,color: str = '', timestamp: bool = True) -> None:
+        self.msg = msg
+        self.color = color
+        self.timestamp = timestamp
+        self.colorMsg(msg=self.msg, color=self.color, timestamp=self.timestamp)
+
+    def colorMsg(self,msg: str, color: str = '', timestamp: bool = True):
+        str = ""
+        if timestamp:
+            str += time.strftime('%Y-%m-%d %H:%M:%S',
+                                time.localtime(time.time())) + "  "
+        if color == "red":
+            str += "\033[1;31;40m"
+        elif color == "green":
+            str += "\033[1;32;40m"
+        elif color == "yellow":
+            str += "\033[1;33;40m"
+        else:
+            print(str + msg)
+            return
+        str += msg + "\033[0m"
+        print(str)

src/gui_control/gui_control/utils/mapping.py

@@ -0,0 +1,383 @@
+#---------------------------------------------------------------------------------------------------
+# L6 L
+l6_l_min = [0, 0, 0, 0, 0, 0]
+l6_l_max = [0.99, 1.39, 1.26, 1.26, 1.26, 1.26]
+l6_l_derict = [-1, -1, -1, -1, -1, -1]
+# L6 R
+l6_r_min = [0, 0, 0, 0, 0, 0]
+l6_r_max = [0.99, 1.39, 1.26, 1.26, 1.26, 1.26]
+l6_r_derict = [-1, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# O6 L
+o6_l_min = [0, 0, 0, 0, 0, 0]
+o6_l_max = [0.58, 1.36, 1.6, 1.6, 1.6, 1.6]
+o6_l_derict = [-1, -1, -1, -1, -1, -1]
+# O6 R
+o6_r_min = [0, 0, 0, 0, 0, 0]
+o6_r_max = [0.58, 1.36, 1.6, 1.6, 1.6, 1.6]
+o6_r_derict = [-1, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L7 L OK
+l7_l_min = [0, 0, 0, 0, 0, 0, 0]
+l7_l_max = [0.44, 1.43, 1.62, 1.62, 1.62, 1.62, 1.01]
+l7_l_derict = [-1, -1, -1, -1, -1, -1, -1]
+# L7 R OK (urdf后续会更改！！！)
+l7_r_min = [0, -1.43, 0, 0, 0, 0, 0]
+l7_r_max = [0.75, 0, 1.62, 1.62, 1.62, 1.62, 1.54]
+l7_r_derict = [-1, 0, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L10 L OK
+l10_l_min = [0, 0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.52]
+l10_l_max = [1.45, 1.43, 1.62, 1.62, 1.62, 1.62, 0.26, 0, 0, 1.01]
+l10_l_derict = [-1, -1, -1, -1, -1, -1, 0, -1, -1, -1]
+# L10 R OK
+l10_r_min = [0, 0, 0, 0, 0, 0, -0.26, 0, 0, -0.52]
+l10_r_max = [0.75, 1.43, 1.62, 1.62, 1.62, 1.62, 0.21, 0.21, 0.34, 1.01]
+l10_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, -1]
+#---------------------------------------------------------------------------------------------------
+# L20 L OK
+l20_l_min = [0, 0, 0, 0, 0, -0.297, -0.26, -0.26, -0.26, -0.26, 0.122, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l20_l_max = [0.87, 1.4, 1.4, 1.4, 1.4, 0.683, 0.26, 0.26, 0.26, 0.26, 1.78, 0, 0, 0, 0, 1.29, 1.08, 1.08, 1.08, 1.08]
+l20_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+# L20 R OK
+l20_r_min = [0, 0, 0, 0, 0, -0.297, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l20_r_max = [0.87, 1.4, 1.4, 1.4, 1.4, 0.683, 0.26, 0.26, 0.26, 0.26, 1.78, 0, 0, 0, 0, 1.29, 1.08, 1.08, 1.08, 1.08]
+l20_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L21 L OK
+l21_l_min = [0, 0, 0, 0, 0, 0, 0, -0.18, -0.18, 0, -0.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l21_l_max = [1, 1.57, 1.57, 1.57, 1.57, 1.6, 0.18, 0.18, 0.18, 0.18, 0.6, 0, 0, 0, 0, 1.57, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57]
+l21_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+# L21 R OK
+l21_r_min = [0, 0, 0, 0, 0, 0, -0.18, -0.18, -0.18, -0.18, -0.6, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l21_r_max = [1, 1.57, 1.57, 1.57, 1.57, 1.6, 0.18, 0.18, 0.18, 0.18, 0.6, 0, 0, 0, 0, 1.57, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57]
+l21_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+#---------------------------------------------------------------------------------------------------
+# L25 L OK
+l25_l_min = [0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l25_l_max = [0.9, 1.57, 1.57, 1.57, 1.57, 1.3, 0.26, 0.26, 0.26, 0.26, 0.61, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57]
+l25_l_derict = [-1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]
+# L25 R OK
+l25_r_min = [0, 0, 0, 0, 0, 0, -0.26, -0.26, -0.26, -0.26, -0.26, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+l25_r_max = [0.9, 1.57, 1.57, 1.57, 1.57, 1.3, 0.26, 0.26, 0.26, 0.26, 0.61, 0, 0, 0, 0, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57, 1.57]
+l25_r_derict = [-1, -1, -1, -1, -1, -1, 0, 0, 0, 0, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+
+def range_to_arc_left(left_range,hand_joint):
+    num=0
+    if hand_joint == "L6":
+        num = 6
+        l_min = l6_l_min
+        l_max = l6_l_max
+        l_derict = l6_l_derict
+    elif hand_joint == "O6":
+        num = 6
+        l_min = o6_l_min
+        l_max = o6_l_max
+        l_derict = o6_l_derict
+    elif hand_joint == "L7":
+        num = 7
+        l_min = l7_l_min
+        l_max = l7_l_max
+        l_derict = l7_l_derict
+    elif hand_joint == "L10":
+        num = 10
+        l_min = l10_l_min
+        l_max = l10_l_max
+        l_derict = l10_l_derict
+    elif hand_joint == "L20":
+        num = 20
+        l_min = l20_l_min
+        l_max = l20_l_max
+        l_derict = l20_l_derict
+    elif hand_joint == "L21":
+        num = 25
+        l_min = l21_l_min
+        l_max = l21_l_max
+        l_derict = l21_l_derict
+    hand_arc = [0] * num
+    for i in range(num):
+        if hand_joint == "L20":
+            if 11 <= i <= 14: continue
+        if hand_joint == "L21":
+            if 11 <= i <= 14: continue
+            if 16 <= i <= 19: continue
+        val_l = is_within_range(left_range[i], 0, 255)
+        if l_derict[i] == -1:
+            hand_arc[i] = scale_value(val_l, 0, 255, l_max[i], l_min[i])
+        else:
+            hand_arc[i] = scale_value(val_l, 0, 255, l_min[i], l_max[i])
+    return hand_arc
+
+def range_to_arc_right(right_range,hand_joint):
+    num=0
+    if hand_joint == "L6":
+        num = 6
+        r_min = l6_r_min
+        r_max = l6_r_max
+        r_derict = l6_r_derict
+    elif hand_joint == "O6":
+        num = 6
+        r_min = o6_r_min
+        r_max = o6_r_max
+        r_derict = o6_r_derict
+    elif hand_joint == "L7":
+        num = 7
+        r_min = l7_r_min
+        r_max = l7_r_max
+        r_derict = l7_r_derict
+    elif hand_joint == "L10":
+        num = 10
+        r_min = l10_r_min
+        r_max = l10_r_max
+        r_derict = l10_r_derict
+    elif hand_joint == "L20":
+        num = 20
+        r_min = l20_r_min
+        r_max = l20_r_max
+        r_derict = l20_r_derict
+    elif hand_joint == "L21":
+        num = 25
+        r_min = l21_r_min
+        r_max = l21_r_max
+        r_derict = l21_r_derict
+    hand_arc = [0] * num
+    for i in range(num):
+        if hand_joint == "L20":
+            if 11 <= i <= 14: continue
+        if hand_joint == "L21":
+            if 11 <= i <= 14: continue
+            if 16 <= i <= 19: continue
+        val_r = is_within_range(right_range[i], 0, 255)
+        if r_derict[i] == -1:
+            hand_arc[i] = scale_value(val_r, 0, 255, r_max[i], r_min[i])
+        else:
+            hand_arc[i] = scale_value(val_r, 0, 255, r_min[i], r_max[i])
+    return hand_arc
+        
+'''
+def arc_to_range_left(left_arc,hand_joint):
+    num=0
+    if hand_joint == "L7":
+        num = 7
+        l_min = l7_l_min
+        l_max = l7_l_max
+        l_derict = l7_l_derict
+    elif hand_joint == "L10":
+        num = 10
+        l_min = l10_l_min
+        l_max = l10_l_max
+        l_derict = l10_l_derict
+    elif hand_joint == "L20":
+        num = 20
+        l_min = l20_l_min
+        l_max = l20_l_max
+        l_derict = l20_l_derict
+    elif hand_joint == "L21":
+        num = 25
+        l_min = l21_l_min
+        l_max = l21_l_max
+        l_derict = l21_l_derict
+    hand_range = [0] * num
+    for i in range(num):
+        if hand_joint == "L20":
+            if 11 <= i <= 14: continue
+        if hand_joint == "L21":
+            if 11 <= i <= 14: continue
+            if 16 <= i <= 19: continue
+        val_l = is_within_range(left_arc[i], 0, 255)
+        if l_derict[i] == -1:
+            hand_range[i] = scale_value(val_l, 0, 255, l_max[i], l_min[i])
+        else:
+            hand_range[i] = scale_value(val_l, 0, 255, l_min[i], l_max[i])
+    return hand_range
+    '''
+def arc_to_range_left(hand_arc_l,hand_joint):
+    num=0
+    if hand_joint == "O6":
+        num = 6
+        l_min = o6_l_min
+        l_max = o6_l_max
+        l_derict = o6_l_derict
+    elif hand_joint == "L7":
+        num = 7
+        l_min = l7_l_min
+        l_max = l7_l_max
+        l_derict = l7_l_derict
+    elif hand_joint == "L10":
+        num = 10
+        l_min = l10_l_min
+        l_max = l10_l_max
+        l_derict = l10_l_derict
+    elif hand_joint == "L20":
+        num = 20
+        l_min = l20_l_min
+        l_max = l20_l_max
+        l_derict = l20_l_derict
+    elif hand_joint == "L21":
+        num = 25
+        l_min = l21_l_min
+        l_max = l21_l_max
+        l_derict = l21_l_derict
+    hand_range = [0] * num
+    #hand_range_l = [0] * 7
+    for i in range(num):
+        if hand_joint == "L20":
+            if 11 <= i <= 14: continue
+        if hand_joint == "L21":
+            if 11 <= i <= 14: continue
+            if 16 <= i <= 19: continue
+        val_l = is_within_range(hand_arc_l[i], l_min[i], l_max[i])
+        if l_derict[i] == -1:
+            hand_range[i] = scale_value(val_l, l_min[i], l_max[i], 255, 0)
+        else:
+            hand_range[i] = scale_value(val_l, l_min[i], l_max[i], 0, 255)
+
+    return hand_range
+
+def arc_to_range_right(right_arc,hand_joint):
+    num=0
+    if hand_joint == "O6":
+        num = 6
+        r_min = o6_r_min
+        r_max = o6_r_max
+        r_derict = o6_r_derict
+    elif hand_joint == "L7":
+        num = 7
+        r_min = l7_r_min
+        r_max = l7_r_max
+        r_derict = l7_r_derict
+    elif hand_joint == "L10":
+        num = 10
+        r_min = l10_r_min
+        r_max = l10_r_max
+        r_derict = l10_r_derict
+    elif hand_joint == "L20":
+        num = 20
+        r_min = l20_r_min
+        r_max = l20_r_max
+        r_derict = l20_r_derict
+    elif hand_joint == "L21":
+        num = 25
+        r_min = l21_r_min
+        r_max = l21_r_max
+        r_derict = l21_r_derict
+    hand_range = [0] * num
+    for i in range(num):
+        if hand_joint == "L20":
+            if 11 <= i <= 14: continue
+        if hand_joint == "L21":
+            if 11 <= i <= 14: continue
+            if 16 <= i <= 19: continue
+        val_r = is_within_range(right_arc[i], r_min[i], r_max[i])
+        if r_derict[i] == -1:
+            hand_range[i] = scale_value(val_r, r_min[i], r_max[i], 255, 0)
+        else:
+            hand_range[i] = scale_value(val_r, r_min[i], r_max[i], 0, 255)
+    return hand_range
+
+
+
+
+def range_to_arc_right_l20(hand_range_r):
+    hand_arc_r = [0] * 20
+    for i in range(20):
+        if 11 <= i <= 14: continue
+        val_r = is_within_range(hand_range_r[i], 0, 255)
+        if l20_r_derict[i] == -1:
+            hand_arc_r[i] = scale_value(val_r, 0, 255, l20_r_max[i], l20_r_min[i])
+        else:
+            hand_arc_r[i] = scale_value(val_r, 0, 255, l20_r_min[i], l20_r_max[i])
+    return hand_arc_r
+
+
+def range_to_arc_left_l20(hand_range_l):
+    hand_arc_l = [0] * 20
+    for i in range(20):
+        if 11 <= i <= 14: continue
+        val_l = is_within_range(hand_range_l[i], 0, 255)
+        if l20_l_derict[i] == -1:
+            hand_arc_l[i] = scale_value(val_l, 0, 255, l20_l_max[i], l20_l_min[i])
+        else:
+            hand_arc_l[i] = scale_value(val_l, 0, 255, l20_l_min[i], l20_l_max[i])
+    return hand_arc_l
+
+
+def arc_to_range_right_l20(hand_arc_r):
+    hand_range_r = [0] * 20
+    for i in range(20):
+        if 11 <= i <= 14: continue
+        val_r = is_within_range(hand_arc_r[i], l20_r_min[i], l20_r_max[i])
+        if l20_r_derict[i] == -1:
+            hand_range_r[i] = scale_value(val_r, l20_r_min[i], l20_r_max[i], 255, 0)
+        else:
+            hand_range_r[i] = scale_value(val_r, l20_r_min[i], l20_r_max[i], 0, 255)
+    return hand_range_r
+
+
+def arc_to_range_left_l20(hand_arc_l):
+    hand_range_l = [0] * 20
+    for i in range(20):
+        if 11 <= i <= 14: continue
+        val_l = is_within_range(hand_arc_l[i], l20_l_min[i], l20_l_max[i])
+        if l20_l_derict[i] == -1:
+            hand_range_l[i] = scale_value(val_l, l20_l_min[i], l20_l_max[i], 255, 0)
+        else:
+            hand_range_l[i] = scale_value(val_l, l20_l_min[i], l20_l_max[i], 0, 255)
+
+    return hand_range_l
+
+
+def range_to_arc_right_10(hand_range_r):
+    hand_arc_r = [0] * 10
+    for i in range(10):
+        val_r = is_within_range(hand_range_r[i], 0, 255)
+        if l10_r_derict[i] == -1:
+            hand_arc_r[i] = scale_value(val_r, 0, 255, l10_r_max[i], l10_r_min[i])
+        else:
+            hand_arc_r[i] = scale_value(val_r, 0, 255, l10_r_min[i], l10_r_max[i])
+
+    return hand_arc_r
+
+
+def range_to_arc_left_10(hand_range_l):
+    hand_arc_l = [0] * 10
+    for i in range(10):
+        val_l = is_within_range(hand_range_l[i], 0, 255)
+        if l10_l_derict[i] == -1:
+            hand_arc_l[i] = scale_value(val_l, 0, 255, l10_l_max[i], l10_l_min[i])
+        else:
+            hand_arc_l[i] = scale_value(val_l, 0, 255, l10_l_min[i], l10_l_max[i])
+    return hand_arc_l
+
+
+def arc_to_range_right_10(hand_arc_r):
+    hand_range_r = [0] * 10
+    for i in range(10):
+        val_r = is_within_range(hand_arc_r[i], l10_r_min[i], l10_r_max[i])
+        if l10_r_derict[i] == -1:
+            hand_range_r[i] = scale_value(val_r, l10_r_min[i], l10_r_max[i], 255, 0)
+        else:
+            hand_range_r[i] = scale_value(val_r, l10_r_min[i], l10_r_max[i], 0, 255)
+    return hand_range_r
+
+
+def arc_to_range_left_10(hand_arc_l):
+    hand_range_l = [0] * 10
+    for i in range(10):
+        val_l = is_within_range(hand_arc_l[i], l10_l_min[i], l10_l_max[i])
+        if l10_l_derict[i] == -1:
+            hand_range_l[i] = scale_value(val_l, l10_l_min[i], l10_l_max[i], 255, 0)
+        else:
+            hand_range_l[i] = scale_value(val_l, l10_l_min[i], l10_l_max[i], 0, 255)
+
+    return hand_range_l
+
+
+def scale_value(original_value, a_min, a_max, b_min, b_max):
+    return (original_value - a_min) * (b_max - b_min) / (a_max - a_min) + b_min
+
+
+def is_within_range(value, min_value, max_value):
+    return min(max_value, max(min_value, value))

src/gui_control/launch/gui_control.launch.py

@@ -0,0 +1,59 @@
+#!/usr/bin/env python3
+from launch import LaunchDescription
+from launch.actions import IncludeLaunchDescription, DeclareLaunchArgument
+from launch.conditions import IfCondition
+from launch.launch_description_sources import PythonLaunchDescriptionSource
+from launch.substitutions import PathJoinSubstitution, LaunchConfiguration
+from launch_ros.substitutions import FindPackageShare
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    # 声明参数：是否显示压力图
+    declare_show_diagram = DeclareLaunchArgument(
+        'show_pressure_diagram',
+        default_value='true',
+        description='是否启动压力图窗口: true | false'
+    )
+
+    # 根据 show_pressure_diagram 参数决定是否包含 pressure_diagram launch 文件
+    pressure_diagram_launch = IncludeLaunchDescription(
+        PythonLaunchDescriptionSource(
+            PathJoinSubstitution([
+                FindPackageShare('pressure_diagram'),
+                'launch',
+                'pressure_diagram.launch.py'
+            ])
+        ),
+        condition=IfCondition(LaunchConfiguration('show_pressure_diagram'))
+    )
+
+    return LaunchDescription([
+        declare_show_diagram,
+        pressure_diagram_launch,
+        Node(
+            package='gui_control',
+            executable='gui_control',
+            name='right_hand_control_node',
+            output='screen',
+            parameters=[{
+                'hand_type': 'right',  # 配置Linker Hand灵巧手类型 left | right 字母为小写
+                'hand_joint': "L10",  # O6\L6\L7\L10\L20\G20(工业版)\L21 字母为大写
+                'topic_hz': 30, # topic发布频率
+                'is_touch': False, # 是否有压力传感器
+                'is_arc': False, # 是否发布弧度值topic
+            }],
+        ),
+        # Node(
+        #     package='gui_control',
+        #     executable='gui_control',
+        #     name='right_hand_control_node',
+        #     output='screen',
+        #     parameters=[{
+        #         'hand_type': 'right',
+        #         'hand_joint': "L10",
+        #         'topic_hz': 30,
+        #         'is_touch': True,
+        #     }],
+        # ),
+    ])

src/gui_control/package.xml

@@ -0,0 +1,21 @@
+<?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>gui_control</name>
+  <version>0.0.0</version>
+  <description>TODO: Package description</description>
+  <maintainer email="linker-robot@todo.todo">linker-robot</maintainer>
+  <license>TODO: License declaration</license>
+
+  <test_depend>ament_copyright</test_depend>
+  <test_depend>ament_flake8</test_depend>
+  <test_depend>ament_pep257</test_depend>
+  <test_depend>python3-pytest</test_depend>
+
+  <exec_depend>linker_hand_ros2_sdk</exec_depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+    
+  </export>
+</package>

src/gui_control/pyproject.toml

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

src/gui_control/setup.cfg

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

src/gui_control/setup.py

@@ -0,0 +1,31 @@
+#!/usr/bin/env python3 
+# -*- coding: utf-8 -*-
+import os
+from glob import glob
+from setuptools import find_packages, setup
+package_name = 'gui_control'
+setup(
+    name=package_name,
+    version='0.0.0',
+    packages=find_packages(exclude=['test']),
+    data_files=[
+        ('share/ament_index/resource_index/packages',
+            ['resource/' + package_name]),
+        (os.path.join('share', 'gui_control', 'launch'), glob('launch/*.launch.py')),
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='linker-robot',
+    maintainer_email='linker-robot@todo.todo',
+    description='TODO: Package description',
+    license='TODO: License declaration',
+    extras_require={
+        'test': ['pytest'],
+    },
+    entry_points={
+        'console_scripts': [
+            'gui_control = gui_control.gui_control:main'
+        ],
+    },
+)

src/gui_control/test/test_copyright.py

@@ -0,0 +1,25 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_copyright.main import main
+import pytest
+
+
+# Remove the `skip` decorator once the source file(s) have a copyright header
+@pytest.mark.skip(reason='No copyright header has been placed in the generated source file.')
+@pytest.mark.copyright
+@pytest.mark.linter
+def test_copyright():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found errors'

src/gui_control/test/test_flake8.py

@@ -0,0 +1,25 @@
+# Copyright 2017 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_flake8.main import main_with_errors
+import pytest
+
+
+@pytest.mark.flake8
+@pytest.mark.linter
+def test_flake8():
+    rc, errors = main_with_errors(argv=[])
+    assert rc == 0, \
+        'Found %d code style errors / warnings:\n' % len(errors) + \
+        '\n'.join(errors)

src/gui_control/test/test_pep257.py

@@ -0,0 +1,23 @@
+# Copyright 2015 Open Source Robotics Foundation, Inc.
+#
+# Licensed under the Apache License, Version 2.0 (the "License");
+# you may not use this file except in compliance with the License.
+# You may obtain a copy of the License at
+#
+#     http://www.apache.org/licenses/LICENSE-2.0
+#
+# Unless required by applicable law or agreed to in writing, software
+# distributed under the License is distributed on an "AS IS" BASIS,
+# WITHOUT WARRANTIES OR CONDITIONS OF ANY KIND, either express or implied.
+# See the License for the specific language governing permissions and
+# limitations under the License.
+
+from ament_pep257.main import main
+import pytest
+
+
+@pytest.mark.linter
+@pytest.mark.pep257
+def test_pep257():
+    rc = main(argv=['.', 'test'])
+    assert rc == 0, 'Found code style errors / warnings'

src/latency_analyzer/latency_analyzer/analyzer_node.py

@@ -0,0 +1,490 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+延时分析节点
+- 自动检测手部运动 → 开始采集 → 停止后计算统计 → 保存 CSV → 等待下一轮
+- 无需按键，无人工误差
+"""
+
+import sys
+import os
+import time
+import csv
+import math
+import signal
+from pathlib import Path
+from collections import defaultdict
+from typing import Optional, List, Dict
+
+import rclpy
+from rclpy.node import Node
+from std_msgs.msg import Float64MultiArray
+from sensor_msgs.msg import JointState
+
+# 复用同包内的协议常量
+from .latency_stamp import (
+    TOPIC_LATENCY_STAMP,
+    SOURCE_RETARGET, SOURCE_SDK, SOURCE_MUJOCO,
+    IDX_SEQ, IDX_SOURCE,
+    IDX_T0, IDX_T1, IDX_T2, IDX_T3, IDX_T4, IDX_T5,
+    DEFAULT_MOTION_THRESHOLD, DEFAULT_STILL_FRAMES, DEFAULT_MIN_COLLECT_FRAMES,
+)
+
+
+class MotionDetector:
+    """运动检测器：基于关节位置变化自动判断手是否在动"""
+
+    def __init__(self, threshold: float = DEFAULT_MOTION_THRESHOLD,
+                 still_frames: int = DEFAULT_STILL_FRAMES,
+                 min_frames: int = DEFAULT_MIN_COLLECT_FRAMES):
+        self.threshold = threshold
+        self.still_frames = still_frames
+        self.min_frames = min_frames
+        self._prev_positions: Optional[List[float]] = None
+        self._still_count = 0
+        self._moving = False
+        self._frame_count = 0
+
+    def update(self, positions: List[float]) -> Optional[str]:
+        """
+        每帧调用，返回状态变化: 'started' | 'stopped' | None
+        """
+        if self._prev_positions is None:
+            self._prev_positions = list(positions)
+            return None
+
+        # 计算总变化量
+        diff = sum(abs(p - pp) for p, pp in zip(positions, self._prev_positions))
+        self._prev_positions = list(positions)
+
+        if diff > self.threshold:
+            self._still_count = 0
+            self._frame_count += 1
+            if not self._moving:
+                self._moving = True
+                self._frame_count = 1
+                return 'started'
+        else:
+            self._still_count += 1
+            if self._moving:
+                self._frame_count += 1
+                if self._still_count >= self.still_frames:
+                    self._moving = False
+                    if self._frame_count >= self.min_frames:
+                        return 'stopped'
+                    else:
+                        # 帧数不够，丢弃
+                        self._frame_count = 0
+                        return 'discarded'
+
+        return None
+
+    @property
+    def is_moving(self) -> bool:
+        return self._moving
+
+
+class LatencyFrame:
+    """单帧延时数据"""
+    __slots__ = ('seq', 'wall_time', 't0', 't1', 't2', 't3', 't4', 't5',
+                 't_mujoco', 'has_retarget', 'has_sdk', 'has_mujoco')
+
+    def __init__(self, seq: int):
+        self.seq = seq
+        self.wall_time = time.time()
+        self.t0 = 0.0
+        self.t1 = 0.0
+        self.t2 = 0.0
+        self.t3 = 0.0
+        self.t4 = 0.0
+        self.t5 = 0.0
+        self.t_mujoco = 0.0
+        self.has_retarget = False
+        self.has_sdk = False
+        self.has_mujoco = False
+
+    def fill_retarget(self, t0: float, t1: float, t2: float, t3: float):
+        self.t0, self.t1, self.t2, self.t3 = t0, t1, t2, t3
+        self.has_retarget = True
+
+    def fill_sdk(self, t4: float, t5: float):
+        self.t4, self.t5 = t4, t5
+        self.has_sdk = True
+
+    def fill_mujoco(self, t_mujoco: float):
+        self.t_mujoco = t_mujoco
+        self.has_mujoco = True
+
+    @property
+    def is_complete(self) -> bool:
+        return self.has_retarget and self.has_sdk
+
+    @property
+    def stage1_ms(self) -> float:
+        """手套→映射 (t1-t0) ms"""
+        return (self.t1 - self.t0) * 1000 if self.t0 and self.t1 else 0
+
+    @property
+    def stage2_ms(self) -> float:
+        """重映射计算 (t2-t1) ms"""
+        return (self.t2 - self.t1) * 1000 if self.t1 and self.t2 else 0
+
+    @property
+    def stage3_ms(self) -> float:
+        """ROS2 传输 (t4-t3) ms"""
+        return (self.t4 - self.t3) * 1000 if self.t3 and self.t4 else 0
+
+    @property
+    def stage4_ms(self) -> float:
+        """SDK→CAN (t5-t4) ms"""
+        return (self.t5 - self.t4) * 1000 if self.t4 and self.t5 else 0
+
+    @property
+    def sim_latency_ms(self) -> float:
+        """仿真手延迟 (t_mujoco - t0) ms"""
+        return (self.t_mujoco - self.t0) * 1000 if self.t0 and self.t_mujoco else 0
+
+    @property
+    def real_total_ms(self) -> float:
+        """真手延迟 (t5-t0) ms"""
+        return (self.t5 - self.t0) * 1000 if self.t0 and self.t5 else 0
+
+    @property
+    def sim_minus_real_ms(self) -> float:
+        """真手与仿真手延迟差 = 仿真 - 真手 (负值=仿真更快) ms"""
+        return (self.t_mujoco - self.t5) * 1000 if self.t5 and self.t_mujoco else 0
+
+    def to_csv_row(self) -> dict:
+        return {
+            'seq': self.seq,
+            'wall_time': f'{self.wall_time:.6f}',
+            't0': f'{self.t0:.9f}',
+            't1': f'{self.t1:.9f}',
+            't2': f'{self.t2:.9f}',
+            't3': f'{self.t3:.9f}',
+            't4': f'{self.t4:.9f}',
+            't5': f'{self.t5:.9f}',
+            't_mujoco': f'{self.t_mujoco:.9f}',
+            'stage1_glove_to_map_ms': f'{self.stage1_ms:.4f}',
+            'stage2_compute_ms': f'{self.stage2_ms:.4f}',
+            'stage3_ros2_transport_ms': f'{self.stage3_ms:.4f}',
+            'stage4_sdk_to_can_ms': f'{self.stage4_ms:.4f}',
+            'sim_latency_ms': f'{self.sim_latency_ms:.4f}',
+            'real_total_ms': f'{self.real_total_ms:.4f}',
+            'sim_minus_real_ms': f'{self.sim_minus_real_ms:.4f}',
+        }
+
+
+# 手型号 → 关节数映射 (与 SDK 一致)
+_HAND_JOINT_COUNT = {
+    'O6': 6, 'L6': 6, 'L7': 7, 'L10': 10,
+    'L20': 20, 'G20': 20, 'L21': 25, 'L25': 25,
+}
+
+
+class LatencyAnalyzer(Node):
+    def __init__(self):
+        super().__init__('latency_analyzer')
+
+        # 参数
+        self.declare_parameter('output_dir', '/home/csj/linkerhand_retarget_ros2/src/latency_analyzer/experiments')
+        self.declare_parameter('trial_name', 'baseline')
+        self.declare_parameter('hand_type', 'right')
+        self.declare_parameter('hand_joint', 'L10')
+        self.declare_parameter('motion_threshold', DEFAULT_MOTION_THRESHOLD)
+        self.declare_parameter('still_frames', DEFAULT_STILL_FRAMES)
+        self.declare_parameter('min_collect_frames', DEFAULT_MIN_COLLECT_FRAMES)
+
+        self._output_base = Path(self.get_parameter('output_dir').value)
+        self._trial_name = self.get_parameter('trial_name').value
+        self._trial_counter = 0
+        self._hand_type = self.get_parameter('hand_type').value
+        self._hand_joint = self.get_parameter('hand_joint').value
+
+        # 根据手型号自动适配运动阈值: 每关节约 3 个单位
+        joint_count = _HAND_JOINT_COUNT.get(self._hand_joint.upper(), 10)
+        auto_threshold = self.get_parameter('motion_threshold').value
+        if auto_threshold == DEFAULT_MOTION_THRESHOLD:
+            auto_threshold = joint_count * 3
+        still_frames = self.get_parameter('still_frames').value
+        min_frames = self.get_parameter('min_collect_frames').value
+        self._detector = MotionDetector(auto_threshold, still_frames, min_frames)
+
+        # 延时数据: seq → LatencyFrame 精确匹配 (同帧 seq 相同)
+        self._frames: Dict[int, LatencyFrame] = {}
+        self._collecting: List[LatencyFrame] = []   # 已匹配的完整帧
+        self._current_trial_dir: Optional[Path] = None
+
+        # 诊断计数器
+        self._diag_cnt_retarget = 0
+        self._diag_cnt_sdk = 0
+        self._diag_cnt_complete = 0
+        self._diag_cnt_cmd = 0
+        self._diag_max_seq_retarget = 0
+        self._diag_max_seq_sdk = 0
+
+        # 订阅 /latency_stamp
+        self._latency_sub = self.create_subscription(
+            Float64MultiArray, TOPIC_LATENCY_STAMP,
+            self._latency_callback, 100
+        )
+
+        # 订阅 /cb_{hand_type}_hand_control_cmd (运动检测, 适配左右手)
+        cmd_topic = f'/cb_{self._hand_type}_hand_control_cmd'
+        self._cmd_sub = self.create_subscription(
+            JointState, cmd_topic,
+            self._cmd_callback, 100
+        )
+
+        # 诊断定时器: 每 5 秒打印一次数据到达情况
+        self._diag_timer = self.create_timer(5.0, self._diag_print)
+
+        self.get_logger().info(f'手型号: {self._hand_type} {self._hand_joint} ({joint_count}关节)')
+        self.get_logger().info(f'运动检测话题: {cmd_topic}')
+        self.get_logger().info(f'输出目录: {self._output_base}')
+        self.get_logger().info(f'运动阈值: {auto_threshold}, 静止帧数: {still_frames}, 最少帧数: {min_frames}')
+        self.get_logger().info('等待手部运动...')
+
+    # ---- 回调 ----
+
+    def _latency_callback(self, msg: Float64MultiArray):
+        """接收延时时间戳，按 seq 精确匹配同一帧"""
+        data = msg.data
+        if len(data) < 4:
+            return
+
+        seq = int(data[IDX_SEQ])
+        source = data[IDX_SOURCE]
+
+        if source == SOURCE_RETARGET:
+            self._diag_cnt_retarget += 1
+            if seq > self._diag_max_seq_retarget:
+                self._diag_max_seq_retarget = seq
+        elif source == SOURCE_SDK:
+            self._diag_cnt_sdk += 1
+            if seq > self._diag_max_seq_sdk:
+                self._diag_max_seq_sdk = seq
+        elif source == SOURCE_MUJOCO:
+            self._diag_cnt_sdk += 1  # 复用 SDK 计数器 (MuJoCo 也是 "接收端")
+
+        # seq=0 表示未正确传递，丢弃
+        if seq == 0:
+            return
+
+        # 获取或创建该 seq 对应的帧
+        if seq not in self._frames:
+            self._frames[seq] = LatencyFrame(seq)
+
+        frame = self._frames[seq]
+        was_complete = frame.is_complete
+
+        if source == SOURCE_RETARGET:
+            frame.fill_retarget(data[IDX_T0], data[IDX_T1], data[IDX_T2], data[IDX_T3])
+        elif source == SOURCE_SDK:
+            frame.fill_sdk(data[IDX_T4], data[IDX_T5])
+        elif source == SOURCE_MUJOCO:
+            frame.fill_mujoco(data[IDX_T0])  # MuJoCo 的 t_mujoco 存在 data[2] = IDX_T0 位置
+
+        # 帧刚刚变为完整 + 正在运动中 → 采集
+        if not was_complete and frame.is_complete:
+            self._diag_cnt_complete += 1
+            if self._detector.is_moving:
+                self._collecting.append(frame)
+
+        # 清理旧帧
+        if len(self._frames) > 500:
+            oldest = min(self._frames.keys())
+            del self._frames[oldest]
+
+    def _cmd_callback(self, msg: JointState):
+        """接收关节指令，仅用于运动检测"""
+        if not msg.position:
+            return
+
+        self._diag_cnt_cmd += 1
+
+        positions = list(msg.position)
+        state = self._detector.update(positions)
+
+        if state == 'started':
+            self._on_motion_start()
+        elif state == 'stopped':
+            self._on_motion_stop()
+        elif state == 'discarded':
+            self.get_logger().warn('运动帧数不足，丢弃本轮数据')
+
+    def _diag_print(self):
+        """诊断输出: 每 5 秒打印数据到达情况"""
+        self.get_logger().info(
+            f'[诊断] latency消息: retarget={self._diag_cnt_retarget} '
+            f'SDK={self._diag_cnt_sdk} 完整帧={self._diag_cnt_complete} '
+            f'cmd消息={self._diag_cnt_cmd} '
+            f'运动中={self._detector.is_moving} '
+            f'缓冲={len(self._frames)} 采集={len(self._collecting)} '
+            f'max_seq: R={self._diag_max_seq_retarget} S={self._diag_max_seq_sdk}'
+        )
+
+    # ---- 运动状态处理 ----
+
+    def _on_motion_start(self):
+        self.get_logger().info('=== 检测到手部运动，开始采集延时数据 ===')
+        self._collecting.clear()
+        self._frames.clear()
+
+    def _on_motion_stop(self):
+        self.get_logger().info(f'=== 手部静止，采集结束，共 {len(self._collecting)} 帧 ===')
+
+        if len(self._collecting) < 10:
+            self.get_logger().warn('有效帧数不足，跳过统计')
+            self._collecting.clear()
+            return
+
+        self._save_and_report()
+
+    # ---- 统计与保存 ----
+
+    def _compute_stats(self, values: List[float]) -> dict:
+        """计算统计量"""
+        if not values:
+            return {'mean': 0, 'p50': 0, 'p95': 0, 'p99': 0, 'std': 0, 'min': 0, 'max': 0, 'count': 0}
+
+        sorted_vals = sorted(values)
+        n = len(sorted_vals)
+
+        return {
+            'mean': sum(values) / n,
+            'p50': sorted_vals[n // 2],
+            'p95': sorted_vals[int(n * 0.95)],
+            'p99': sorted_vals[int(n * 0.99)],
+            'std': (sum((v - sum(values) / n) ** 2 for v in values) / n) ** 0.5,
+            'min': min(values),
+            'max': max(values),
+            'count': n,
+        }
+
+    def _save_and_report(self):
+        """保存数据并打印统计报告"""
+        self._trial_counter += 1
+
+        # 创建输出目录
+        ts = time.strftime('%Y-%m-%d_%H%M%S')
+        trial_dir = self._output_base / f'{self._trial_name}_{self._trial_counter:03d}_{ts}'
+        trial_dir.mkdir(parents=True, exist_ok=True)
+        self._current_trial_dir = trial_dir
+
+        # 分离完整帧和部分帧
+        complete = [f for f in self._collecting if f.is_complete]
+
+        self.get_logger().info(f'完整帧: {len(complete)} / 总帧: {len(self._collecting)}')
+
+        # 统一筛选: 所有环节的延时值必须 > 0
+        valid = [f for f in complete
+                 if f.stage1_ms > 0 and f.stage2_ms > 0
+                 and f.stage3_ms > 0 and f.stage4_ms > 0
+                 and f.real_total_ms > 0]
+
+        self.get_logger().info(f'有效帧: {len(valid)} (阶段值全部 > 0)')
+
+        # 从同一批帧提取各环节延时，保证可加性
+        stage1 = [f.stage1_ms for f in valid]
+        stage2 = [f.stage2_ms for f in valid]
+        stage3 = [f.stage3_ms for f in valid]
+        stage4 = [f.stage4_ms for f in valid]
+        sim_lat = [f.sim_latency_ms for f in valid if f.sim_latency_ms > 0]
+        real_tot = [f.real_total_ms for f in valid]
+        sim_minus_real = [f.sim_minus_real_ms for f in valid if f.sim_minus_real_ms != 0]
+
+        # 计算统计
+        stats = {
+            '环节1: 手套→映射': self._compute_stats(stage1),
+            '环节2: 重映射计算': self._compute_stats(stage2),
+            '环节3: ROS2传输': self._compute_stats(stage3),
+            '环节4: SDK→CAN': self._compute_stats(stage4),
+            '真手延迟 (t5-t0)': self._compute_stats(real_tot),
+        }
+        if sim_lat:
+            stats['仿真手延迟 (tm-t0)'] = self._compute_stats(sim_lat)
+        if sim_minus_real:
+            stats['仿真-真手 (负=仿真更快)'] = self._compute_stats(sim_minus_real)
+
+        # 计算占比 (各环节均值之和 = 100%)
+        ratio_names = ['环节1: 手套→映射', '环节2: 重映射计算', '环节3: ROS2传输', '环节4: SDK→CAN']
+        sum_means = sum(stats[n]['mean'] for n in ratio_names if 'mean' in stats[n])
+        if sum_means > 0:
+            for name in ratio_names:
+                stats[name]['ratio'] = stats[name]['mean'] / sum_means * 100
+
+        # ---- 打印表格 ----
+        self._print_table(stats)
+
+        # ---- 保存原始数据 CSV ----
+        raw_path = trial_dir / 'raw_data.csv'
+        with open(raw_path, 'w', newline='') as f:
+            # 用第一批有效帧的字段名作为 CSV 列头
+            fieldnames = list(LatencyFrame(0).to_csv_row().keys())
+            writer = csv.DictWriter(f, fieldnames=fieldnames)
+            writer.writeheader()
+            for frame in valid:
+                writer.writerow(frame.to_csv_row())
+        self.get_logger().info(f'原始数据已保存: {raw_path}')
+
+        # ---- 保存统计汇总 CSV ----
+        summary_path = trial_dir / 'summary.csv'
+        with open(summary_path, 'w', newline='') as f:
+            writer = csv.writer(f)
+            writer.writerow(['环节', '均值ms', 'P50ms', 'P95ms', 'P99ms', 'Std', 'Min', 'Max', '帧数', '占比%'])
+            for name, s in stats.items():
+                has_stats = 'count' in s and s['count'] > 0
+                ratio = f'{s.get("ratio", 0):.1f}' if 'ratio' in s else '-'
+                if has_stats:
+                    writer.writerow([name, f'{s["mean"]:.3f}', f'{s["p50"]:.3f}', f'{s["p95"]:.3f}',
+                                     f'{s["p99"]:.3f}', f'{s["std"]:.3f}', f'{s["min"]:.3f}',
+                                     f'{s["max"]:.3f}', s['count'], ratio])
+                else:
+                    writer.writerow([name, f'{s.get("mean", 0):.3f}', '-', '-', '-', '-', '-', '-', 0, ratio])
+        self.get_logger().info(f'统计汇总已保存: {summary_path}')
+
+        # 重置，等待下一轮
+        self._collecting.clear()
+
+    def _print_table(self, stats: dict):
+        """打印延时统计表"""
+        sep = '─' * 85
+        self.get_logger().info(f'\n{sep}')
+        self.get_logger().info(
+            f'{"环节":<16} {"均值":>7} {"P50":>7} {"P95":>7} {"P99":>7} {"Std":>7} {"占比":>6}'
+        )
+        self.get_logger().info(sep)
+        for name, s in stats.items():
+            ratio = s.get('ratio', 0)
+            if 'count' in s and s['count'] > 0:
+                self.get_logger().info(
+                    f'{name:<16} {s["mean"]:6.2f}ms {s["p50"]:6.2f}ms '
+                    f'{s["p95"]:6.2f}ms {s["p99"]:6.2f}ms {s["std"]:6.2f}ms {ratio:5.1f}%'
+                )
+            elif 'mean' in s:
+                self.get_logger().info(
+                    f'{name:<16} {s["mean"]:6.2f}ms {"-":>7} {"-":>7} {"-":>7} {"-":>7} {ratio:5.1f}%'
+                )
+        self.get_logger().info(f'{sep}\n')
+
+    def set_trial_name(self, name: str):
+        self._trial_name = name
+        self._trial_counter = 0
+
+
+def main(args=None):
+    rclpy.init(args=args)
+    node = LatencyAnalyzer()
+
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.destroy_node()
+        rclpy.shutdown()
+
+
+if __name__ == '__main__':
+    main()

src/latency_analyzer/latency_analyzer/latency_stamp.py

@@ -0,0 +1,65 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+延时数据结构和协议常量
+- 被 latency_analyzer 使用
+- retarget.py 和 advanced_l10.py 按此格式发布 Float64MultiArray
+"""
+
+# ---- 话题名 ----
+TOPIC_LATENCY_STAMP = "/latency_stamp"
+
+# ---- Float64MultiArray data 格式 ----
+# retarget 端发布: [seq, SOURCE_RETARGET, t0, t1, t2, t3]
+# SDK 端发布:     [seq, SOURCE_SDK,     t4, t5, 0,  0 ]
+SOURCE_RETARGET = 0.0
+SOURCE_SDK = 1.0
+SOURCE_MUJOCO = 2.0
+SOURCE_MUJOCO_DISPLAY = 3.0
+
+# ---- 各字段索引 (data 数组) ----
+IDX_SEQ = 0
+IDX_SOURCE = 1
+IDX_T0 = 2
+IDX_T1 = 3
+IDX_T2 = 4
+IDX_T3 = 5
+IDX_T4 = 2  # SDK 消息复用索引 2,3
+IDX_T5 = 3
+
+# ---- JointState header.frame_id 格式 ----
+# retarget 发布时设置 frame_id = f"{FRAME_ID_PREFIX}{seq}"
+FRAME_ID_PREFIX = "lat_"
+
+# ---- 运动检测默认参数 ----
+DEFAULT_MOTION_THRESHOLD = 30      # 10个关节变化绝对值之和 > 此值 = 运动中
+DEFAULT_STILL_FRAMES = 500         # 5秒 @ 100Hz = 确认停止
+DEFAULT_MIN_COLLECT_FRAMES = 30    # 最少采集帧数，过滤误触发
+
+
+def make_frame_id(seq: int) -> str:
+    return f"{FRAME_ID_PREFIX}{seq}"
+
+
+def parse_frame_id(frame_id: str) -> int:
+    try:
+        if frame_id.startswith(FRAME_ID_PREFIX):
+            return int(frame_id[len(FRAME_ID_PREFIX):])
+    except (ValueError, IndexError):
+        pass
+    return -1
+
+
+def build_retarget_msg(seq: int, t0: float, t1: float, t2: float, t3: float) -> list:
+    """构建 retarget 端延时消息"""
+    return [float(seq), SOURCE_RETARGET, t0, t1, t2, t3]
+
+
+def build_sdk_msg(seq: int, t4: float, t5: float) -> list:
+    """构建 SDK 端延时消息"""
+    return [float(seq), SOURCE_SDK, t4, t5, 0.0, 0.0]
+
+
+def build_mujoco_msg(seq: int, t_mujoco: float) -> list:
+    """构建 MuJoCo 端延时消息"""
+    return [float(seq), SOURCE_MUJOCO, t_mujoco, 0.0, 0.0, 0.0]

src/latency_analyzer/package.xml

@@ -0,0 +1,18 @@
+<?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>latency_analyzer</name>
+  <version>0.1.0</version>
+  <description>End-to-end latency measurement and analysis for LinkerHand retargeting pipeline</description>
+  <maintainer email="user@example.com">user</maintainer>
+  <license>MIT</license>
+
+  <depend>rclpy</depend>
+  <depend>std_msgs</depend>
+  <depend>sensor_msgs</depend>
+  <depend>linkerhand_retarget</depend>
+
+  <export>
+    <build_type>ament_python</build_type>
+  </export>
+</package>

src/latency_analyzer/setup.cfg

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

src/latency_analyzer/setup.py

@@ -0,0 +1,23 @@
+from setuptools import setup
+
+package_name = 'latency_analyzer'
+
+setup(
+    name=package_name,
+    version='0.1.0',
+    packages=[package_name],
+    data_files=[
+        ('share/' + package_name, ['package.xml']),
+    ],
+    install_requires=['setuptools'],
+    zip_safe=True,
+    maintainer='user',
+    maintainer_email='user@example.com',
+    description='End-to-end latency measurement and analysis for LinkerHand retargeting pipeline',
+    license='MIT',
+    entry_points={
+        'console_scripts': [
+            'analyzer = latency_analyzer.analyzer_node:main',
+        ],
+    },
+)

src/linker_hand_mujoco_ros2/README_CN.MD

@@ -0,0 +1,60 @@
+# 1. **概述**
+
+灵心巧手，创造万物。
+
+LinkerHand 灵巧手 ROS2 SDK 是由灵心巧手（北京）科技有限公司开发的一款软件工具，用于驱动其灵巧手系列产品，并提供功能示例。它支持多种设备（如笔记本、台式机、树莓派、Jetson 等），主要服务于人型机器人、工业自动化和科研院所等领域，适用于人型机器人、柔性化生产线、具身大模型训练和数据采集等场景。
+
+# 1.1 **说明**
+本程序为LinkerHand制作系列灵巧手Mujoco仿真环境，便于使用者熟悉LinkerHand灵巧手系列产品的使用方式方法，以及进行仿真环境下的模型训练和数据采集
+
+# 2. **使用说明**
+```bash
+$ mkdir -p linker_hand_mujoco_ros2/src    #创建目录
+$ cd linker_hand_mujoco_ros2/src    #进入目录
+$ # 1. 克隆仓库（使用 sparse 模式 + blob 过滤，节省空间）
+$ git clone --filter=blob:none --sparse https://github.com/linker-bot/linkerhand-sim.git
+$ # 2. 进入仓库目录
+$ cd linkerhand-sim
+$ # 3. 设置 sparse-checkout 目录
+$ git sparse-checkout set linker_hand_mujoco_ros2
+$ cd linker_hand_mujoco_ros2/src/linker_hand_sim/
+$ pip install -r requirements.txt
+$ /usr/bin/python3 -m pip install mujoco # 使用ROS2调用mujoco必须使用系统环境下的python3安装
+```
+- 修改linker_hand_mujoco_ros2/launch/linker_hand_mujoco_ros2.launch.py
+根据文件内参数说明修改即可
+```bash
+$ cd linker_hand_mujoco_ros2/
+$ colcon build --symlink-install
+$ source ./install/setup.bash
+$ ros2 launch linker_hand_mujoco_ros2 linker_hand_mujoco_ros2.launch.py
+```
+
+# 3. **topic说明**
+- /cb_right_hand_control_cmd or /cb_left_hand_control_cmd
+```bash
+ros2 topic pub /joint_states sensor_msgs/msg/JointState '
+{
+  header: { stamp: { sec: 0, nanosec: 0 }, frame_id: "" },
+  name: [],
+  position: [200.0, 200.0, 200.0, 200.0, 200.0, 200.0, 200.0, 200.0, 200.0, 200.0],
+  velocity: [],
+  effort: []
+}'
+
+```
+- position 说明
+  L6:  ["大拇指弯曲", "大拇指横摆","食指弯曲", "中指弯曲", "无名指弯曲", "小拇指弯曲"]
+  
+  L7:  ["大拇指弯曲", "大拇指横摆","食指弯曲", "中指弯曲", "无名指弯曲","小拇指弯曲","拇指旋转"]
+
+  L10: ["拇指根部", "拇指侧摆","食指根部", "中指根部", "无名指根部","小指根部","食指侧摆","无名指侧摆","小指侧摆","拇指旋转"]
+
+  L20: ["拇指根部", "食指根部", "中指根部", "无名指根部","小指根部","拇指侧摆","食指侧摆","中指侧摆","无名指侧摆","小指侧摆","拇指横摆","预留","预留","预留","预留","拇指尖部","食指末端","中指末端","无名指末端","小指末端"]
+
+  L21: ["大拇指根部", "食指根部", "中指根部","无名指根部","小拇指根部","大拇指侧摆","食指侧摆","中指侧摆","无名指侧摆","小拇指侧摆","大拇指横滚","预留","预留","预留","预留","大拇指中部","预留","预留","预留","预留","大拇指指尖","食指指尖","中指指尖","无名指指尖","小拇指指尖"]
+
+  L25: ["大拇指根部", "食指根部", "中指根部","无名指根部","小拇指根部","大拇指侧摆","食指侧摆","中指侧摆","无名指侧摆","小拇指侧摆","大拇指横滚","预留","预留","预留","预留","大拇指中部","食指中部","中指中部","无名指中部","小拇指中部","大拇指指尖","食指指尖","中指指尖","无名指指尖","小拇指指尖"]
+
+# 3.1 **GUI控制**
+可以使用 linker_hand_ros2_sdk的[gui_control](https://github.com/linkerbotai/linker_hand_ros2_sdk/blob/main/README_CN.md)控制仿真环境

src/linker_hand_mujoco_ros2/launch/hand_curve_recorder.launch.py

@@ -0,0 +1,35 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+
+
+def generate_launch_description():
+    return LaunchDescription([
+        DeclareLaunchArgument("hand_type", default_value="left"),
+        DeclareLaunchArgument("channel", default_value="3"),  # 中指
+        DeclareLaunchArgument("label", default_value="run"),  # sim / real
+        DeclareLaunchArgument("output_dir", default_value="reports/hand_curves"),
+        DeclareLaunchArgument("hz", default_value="100.0"),
+        # 话题留空则用 /cb_{hand}_hand_control_cmd 与 /cb_{hand}_hand_state
+        DeclareLaunchArgument("cmd_topic", default_value=""),
+        DeclareLaunchArgument("state_topic", default_value=""),
+        DeclareLaunchArgument("current_topic", default_value=""),
+        Node(
+            package="linker_hand_mujoco_ros2",
+            executable="hand_curve_recorder",
+            name="hand_curve_recorder",
+            output="screen",
+            parameters=[{
+                "hand_type": LaunchConfiguration("hand_type"),
+                "channel": LaunchConfiguration("channel"),
+                "label": LaunchConfiguration("label"),
+                "output_dir": LaunchConfiguration("output_dir"),
+                "hz": LaunchConfiguration("hz"),
+                "cmd_topic": LaunchConfiguration("cmd_topic"),
+                "state_topic": LaunchConfiguration("state_topic"),
+                "current_topic": LaunchConfiguration("current_topic"),
+                "use_cmd_as_joint": True,
+            }],
+        ),
+    ])

src/linker_hand_mujoco_ros2/launch/linker_hand_mujoco_ros2.launch.py

@@ -0,0 +1,19 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+    return LaunchDescription([
+        Node(
+            package='linker_hand_mujoco_ros2',
+            executable='linker_hand_mujoco_ros2_node',
+            name='linker_hand_mujoco_ros2_node',
+            output='screen',
+            parameters=[{
+                'hand_type': 'left',
+                # O6 / L6 / L7 / L10 / L20 / L21
+                'hand_joint': "O6",
+                'topic_hz': 30,
+                'is_touch': True,
+            }],
+        ),
+    ])

src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/hand_curve_recorder.py

@@ -0,0 +1,252 @@
+#!/usr/bin/env python3
+"""通用手部曲线录制 + 画图（仿真 / 真机同一套 ROS 话题）。
+
+默认订阅：
+  /cb_<hand>_hand_control_cmd   指令 0～255 (sensor_msgs/JointState.position)
+  /cb_<hand>_hand_state         反馈 0～255 (position)；电流可放 effort，单位 A
+
+真机有 state 就录 joint + current；只有 cmd 也能录指令曲线。
+Ctrl+C 结束 → 写 CSV + 画指定通道的 角/速/流(或力矩) 三图。
+"""
+
+from __future__ import annotations
+
+import csv
+import os
+import time
+from datetime import datetime
+from pathlib import Path
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Float32MultiArray
+
+# O6/L6 六维通道名（与控制协议一致）
+CHANNEL_NAMES = [
+    "thumb_bend",
+    "thumb_yaw",
+    "index",
+    "middle",
+    "ring",
+    "pinky",
+]
+
+
+def _pad(seq, n, fill=float("nan")):
+    out = [fill] * n
+    for i, v in enumerate(list(seq)[:n]):
+        out[i] = float(v)
+    return out
+
+
+class HandCurveRecorder(Node):
+    def __init__(self):
+        super().__init__("hand_curve_recorder")
+
+        self.declare_parameter("hand_type", "left")
+        self.declare_parameter("cmd_topic", "")  # 空则自动 /cb_{hand}_hand_control_cmd
+        self.declare_parameter("state_topic", "")  # 空则自动 /cb_{hand}_hand_state
+        self.declare_parameter("current_topic", "")  # 可选 Float32MultiArray；空则用 state.effort
+        self.declare_parameter("n_channels", 6)
+        self.declare_parameter("channel", 3)  # 画图关注通道：中指=3
+        self.declare_parameter("output_dir", "reports/hand_curves")
+        self.declare_parameter("label", "run")  # 文件名标签 sim / real / ...
+        self.declare_parameter("hz", 100.0)  # 落盘采样率（定时器）
+        self.declare_parameter("use_cmd_as_joint", True)  # 无 state 时用 cmd 当 joint 画图
+
+        hand = self.get_parameter("hand_type").value
+        self.n = int(self.get_parameter("n_channels").value)
+        self.channel = int(self.get_parameter("channel").value)
+        self.use_cmd_as_joint = bool(self.get_parameter("use_cmd_as_joint").value)
+        self.label = str(self.get_parameter("label").value)
+
+        cmd_topic = self.get_parameter("cmd_topic").value or f"/cb_{hand}_hand_control_cmd"
+        state_topic = self.get_parameter("state_topic").value or f"/cb_{hand}_hand_state"
+        current_topic = self.get_parameter("current_topic").value
+
+        out = Path(self.get_parameter("output_dir").value).expanduser()
+        if not out.is_absolute():
+            # 相对路径：相对当前工作目录
+            out = Path.cwd() / out
+        out.mkdir(parents=True, exist_ok=True)
+        self.output_dir = out
+
+        self._lock_cmd = [float("nan")] * self.n
+        self._lock_joint = [float("nan")] * self.n
+        self._lock_current = [float("nan")] * self.n
+        self._have_state = False
+        self._have_current = False
+        self._rows = []
+        self._t0 = time.perf_counter()
+
+        self.create_subscription(JointState, cmd_topic, self._on_cmd, 50)
+        self.create_subscription(JointState, state_topic, self._on_state, 50)
+        if current_topic:
+            self.create_subscription(
+                Float32MultiArray, current_topic, self._on_current, 50
+            )
+
+        period = 1.0 / max(1.0, float(self.get_parameter("hz").value))
+        self.create_timer(period, self._on_timer)
+
+        self.get_logger().info(
+            f"录制中 | cmd={cmd_topic} | state={state_topic} | "
+            f"current={current_topic or 'state.effort'} | "
+            f"plot_channel={self.channel}({CHANNEL_NAMES[self.channel] if 0 <= self.channel < len(CHANNEL_NAMES) else '?'}) | "
+            f"out={self.output_dir}"
+        )
+        self.get_logger().info("对端发控制指令即可；Ctrl+C 结束并画图")
+
+    def _on_cmd(self, msg: JointState):
+        self._lock_cmd = _pad(msg.position, self.n)
+
+    def _on_state(self, msg: JointState):
+        self._have_state = True
+        self._lock_joint = _pad(msg.position, self.n)
+        if msg.effort and len(msg.effort) > 0:
+            self._have_current = True
+            self._lock_current = _pad(msg.effort, self.n)
+
+    def _on_current(self, msg: Float32MultiArray):
+        self._have_current = True
+        self._lock_current = _pad(msg.data, self.n)
+
+    def _on_timer(self):
+        t = time.perf_counter() - self._t0
+        joint = list(self._lock_joint)
+        if (not self._have_state) and self.use_cmd_as_joint:
+            joint = list(self._lock_cmd)
+        self._rows.append(
+            {
+                "t_s": t,
+                "cmd": list(self._lock_cmd),
+                "joint": joint,
+                "current": list(self._lock_current),
+            }
+        )
+
+    def save_and_plot(self):
+        if not self._rows:
+            self.get_logger().warn("没有录到数据，跳过保存")
+            return
+
+        stamp = datetime.now().strftime("%Y%m%d_%H%M%S")
+        ch = self.channel
+        ch_name = CHANNEL_NAMES[ch] if 0 <= ch < len(CHANNEL_NAMES) else f"ch{ch}"
+        stem = f"{self.label}_{ch_name}_{stamp}"
+
+        csv_path = self.output_dir / f"{stem}.csv"
+        with csv_path.open("w", newline="") as f:
+            w = csv.writer(f)
+            header = ["t_s"]
+            for i in range(self.n):
+                name = CHANNEL_NAMES[i] if i < len(CHANNEL_NAMES) else f"ch{i}"
+                header += [f"cmd_{name}", f"joint_{name}", f"current_{name}"]
+            w.writerow(header)
+            for row in self._rows:
+                line = [f"{row['t_s']:.6f}"]
+                for i in range(self.n):
+                    line += [
+                        f"{row['cmd'][i]:.6f}",
+                        f"{row['joint'][i]:.6f}",
+                        f"{row['current'][i]:.6f}",
+                    ]
+                w.writerow(line)
+
+        self.get_logger().info(f"CSV → {csv_path}  ({len(self._rows)} samples)")
+        self.get_logger().info(
+            f"state={'yes' if self._have_state else 'no(use cmd)'}  "
+            f"current={'yes' if self._have_current else 'no'}"
+        )
+
+        png_path = self.output_dir / f"{stem}_qvt.png"
+        try:
+            self._plot(png_path, ch, ch_name)
+            self.get_logger().info(f"图  → {png_path}")
+        except Exception as e:
+            self.get_logger().error(f"画图失败: {e}")
+
+    def _plot(self, png_path: Path, ch: int, ch_name: str):
+        import matplotlib
+
+        matplotlib.use("Agg")
+        import matplotlib.pyplot as plt
+
+        t = np.array([r["t_s"] for r in self._rows], dtype=float)
+        cmd = np.array([r["cmd"][ch] for r in self._rows], dtype=float)
+        joint = np.array([r["joint"][ch] for r in self._rows], dtype=float)
+        current = np.array([r["current"][ch] for r in self._rows], dtype=float)
+
+        # 速度：对 joint(u8) 差分；若只有 cmd 则对 cmd 差分
+        y = joint.copy()
+        if np.all(np.isnan(y)):
+            y = cmd.copy()
+        # 用秒做梯度 → u8/s
+        v = np.gradient(y, t)
+        # 简单移动平均低通
+        if len(v) >= 5:
+            kernel = np.ones(5) / 5.0
+            v = np.convolve(v, kernel, mode="same")
+
+        fig, axes = plt.subplots(3, 1, figsize=(10, 8), sharex=True)
+        src = "joint" if self._have_state else "cmd(as joint)"
+        fig.suptitle(
+            f"hand_curve_recorder  ch={ch}:{ch_name}  label={self.label}  src={src}",
+            fontsize=12,
+        )
+
+        axes[0].plot(t, cmd, "k--", lw=1.2, label="command_u8")
+        axes[0].plot(t, joint, "C0", lw=1.6, label="joint_u8")
+        axes[0].set_ylabel("position (0-255)")
+        axes[0].legend(loc="best")
+        axes[0].grid(True, alpha=0.3)
+
+        axes[1].plot(t, v, "C1", lw=1.6, label="d(joint)/dt")
+        axes[1].set_ylabel("velocity (u8/s)")
+        axes[1].legend(loc="best")
+        axes[1].grid(True, alpha=0.3)
+
+        ylab = "current / effort"
+        if self._have_current:
+            axes[2].plot(t, current, "C3", lw=1.6, label="current/effort")
+        else:
+            axes[2].text(
+                0.5,
+                0.5,
+                "no current topic / state.effort\n(only cmd+joint recorded)",
+                ha="center",
+                va="center",
+                transform=axes[2].transAxes,
+            )
+        axes[2].set_ylabel(ylab)
+        axes[2].set_xlabel("time (s)")
+        axes[2].legend(loc="best")
+        axes[2].grid(True, alpha=0.3)
+
+        fig.tight_layout()
+        fig.savefig(png_path, dpi=140)
+        plt.close(fig)
+
+
+def main(args=None):
+    # 无显示环境时 matplotlib 可写缓存
+    os.environ.setdefault("MPLCONFIGDIR", str(Path.cwd() / ".mplconfig"))
+    Path(os.environ["MPLCONFIGDIR"]).mkdir(parents=True, exist_ok=True)
+
+    rclpy.init(args=args)
+    node = HandCurveRecorder()
+    try:
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        node.save_and_plot()
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2.py

@@ -0,0 +1,218 @@
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from std_msgs.msg import Float64MultiArray
+import time
+import threading
+import sys
+import os
+
+import numpy as np
+import mujoco
+import mujoco.viewer
+from PyQt5.QtWidgets import QApplication
+from .utils.mapping import *
+from .utils.joint_monitor import JointMonitorWindow
+
+JOINT_CONFIG = {
+    "L6": {
+        "map": L6_JOINT_MAP,
+        "arc": L6_JOINT_ARC,
+        "mimic": None,
+    },
+    "O6": {
+        "map": O6_JOINT_MAP,
+        "arc": O6_JOINT_ARC,
+        "mimic": O6_MIMIC,
+    },
+    "L7": {
+        "map": L7_JOINT_MAP,
+        "arc": L7_JOINT_ARC,
+        "mimic": None,
+    },
+    "L10": {
+        "map": L10_JOINT_MAP,
+        "arc": L10_JOINT_ARC,
+        "mimic": None,
+    },
+    "L20": {
+        "map": L20_JOINT_MAP,
+        "arc": L20_JOINT_ARC,
+        "mimic": None,
+    },
+    "L21": {
+        "map": L21_JOINT_MAP,
+        "arc": L21_JOINT_ARC,
+        "mimic": None,
+    },
+}
+
+
+class MujocoNode(Node):
+    def __init__(self):
+        super().__init__('linker_hand_mujoco_ros2_node')
+        self.declare_parameter("hand_type", "right")
+        self.hand_type = self.get_parameter('hand_type').get_parameter_value().string_value
+        self.declare_parameter("hand_joint", "L10")
+        self.hand_joint = self.get_parameter('hand_joint').get_parameter_value().string_value
+        self.create_subscription(
+            JointState,
+            f"/cb_{self.hand_type}_hand_control_cmd",
+            self.hand_cb,
+            10,
+        )
+        # 延时测量发布器
+        self.latency_pub = self.create_publisher(Float64MultiArray, '/latency_stamp', 100)
+        self._latest_t_mujoco = 0.0
+        self._latest_seq = 0
+
+        joint_config = JOINT_CONFIG.get(self.hand_joint)
+        if joint_config:
+            self.joint_map = joint_config["map"]
+            self.joint_arc = joint_config["arc"]
+            self.joint_mimic = joint_config.get("mimic")
+        else:
+            self.joint_map = None
+            self.joint_arc = None
+            self.joint_mimic = None
+
+        XML_PATH = (
+            os.path.dirname(os.path.abspath(__file__))
+            + f"/urdf/{self.hand_joint.upper()}/linker_hand_{self.hand_joint.lower()}_{self.hand_type}/"
+            f"linker_hand_{self.hand_joint.lower()}_{self.hand_type}.xml"
+        )
+
+        self.model = mujoco.MjModel.from_xml_path(XML_PATH)
+        self.model.dof_damping[:] = 0.8
+        self.data = mujoco.MjData(self.model)
+
+        print("=" * 20, flush=True)
+        print(mujoco.mj_versionString(), flush=True)
+        print("=" * 20, flush=True)
+        self.data.qpos[:] = 0
+        self.data.qvel[:] = 0
+        self.model.opt.disableflags = 0 if self.hand_joint == "O6" else 1
+        mujoco.mj_forward(self.model, self.data)
+
+        self.joint_names = []
+        self.joint_qpos_addrs = []
+        self.joint_ranges = []
+        for i in range(self.model.njnt):
+            # 只显示铰链关节（手控相关），跳过 free / slide
+            if self.model.jnt_type[i] != int(mujoco.mjtJoint.mjJNT_HINGE):
+                continue
+            name = mujoco.mj_id2name(self.model, mujoco.mjtObj.mjOBJ_JOINT, i) or f"joint_{i}"
+            qposadr = int(self.model.jnt_qposadr[i])
+            lo = float(self.model.jnt_range[i, 0])
+            hi = float(self.model.jnt_range[i, 1])
+            if hi <= lo:
+                lo, hi = 0.0, 1.57
+            self.joint_names.append(name)
+            self.joint_qpos_addrs.append(qposadr)
+            self.joint_ranges.append((lo, hi))
+            print(f"Joint {i}: {name} range=[{lo:.3f}, {hi:.3f}]", flush=True)
+
+        joint_count = self.model.nu
+        self.ctrl_values = np.zeros(joint_count)
+        self.ctrl_ranges = self.model.actuator_ctrlrange.copy()
+
+        self.joint_ui = JointMonitorWindow(
+            self.joint_names,
+            self.joint_ranges,
+            title=f"Joint ({self.hand_joint} {self.hand_type})",
+        )
+        self.joint_ui.show()
+
+        self._ui_update_interval = 0.05  # 20 Hz 刷新进度条
+        self._last_ui_update = 0.0
+
+        # MuJoCo viewer 在主线程跑, 此处只做初始化, 循环在 run_viewer() 中
+
+    def hand_cb(self, data):
+        # ---- 延时: MuJoCo 收到 ----
+        t_mujoco = time.perf_counter()
+        seq = 0
+        fid = data.header.frame_id if data.header.frame_id else ''
+        if fid.startswith("lat_"):
+            try:
+                seq = int(fid[4:])
+            except ValueError:
+                pass
+        self._latest_t_mujoco = t_mujoco
+        self._latest_seq = seq
+
+        position = data.position
+        try:
+            if self.joint_map is not None:
+                if self.hand_type == "left":
+                    tmp = range_to_arc_left(position, self.hand_joint)
+                elif self.hand_type == "right":
+                    tmp = range_to_arc_right(position, self.hand_joint)
+                else:
+                    return
+                res = self.map_position_array(tmp, self.joint_map)
+                if self.joint_mimic:
+                    res = apply_mimic(res, self.joint_mimic, self.joint_arc)
+                self.ctrl_values[:] = res
+        except Exception as e:
+            self.get_logger().error(f"Error in hand_cb: {e}")
+
+        # 发布延时时间戳 (MuJoCo 端: t_mujoco)
+        latency_msg = Float64MultiArray()
+        latency_msg.data = [float(seq), 2.0, t_mujoco, 0.0, 0.0, 0.0]
+        self.latency_pub.publish(latency_msg)
+
+    def map_position_array(self, position, joint_map):
+        mapped_array = [0.0] * len(joint_map)
+        for target_idx, source_idx in joint_map.items():
+            if source_idx < len(position):
+                mapped_array[target_idx] = position[source_idx]
+        return mapped_array
+
+
+    def run_viewer(self):
+        """主线程中运行 MuJoCo viewer + Qt UI + ROS2 spin"""
+        with mujoco.viewer.launch_passive(self.model, self.data) as viewer:
+            print("MuJoCo viewer running...", flush=True)
+            while viewer.is_running():
+                # 处理 Qt UI 事件 (关节监视器)
+                QApplication.processEvents()
+                # 处理 ROS2 回调
+                rclpy.spin_once(self, timeout_sec=0.001)
+                # 物理步进
+                self.data.ctrl[:] = self.ctrl_values
+                mujoco.mj_step(self.model, self.data)
+                viewer.sync()
+                # ---- 延时: 显示器刷新后 ----
+                if self._latest_t_mujoco > 0:
+                    t_display = time.perf_counter()
+                    latency_msg = Float64MultiArray()
+                    latency_msg.data = [float(self._latest_seq), 3.0,
+                                        self._latest_t_mujoco, t_display, 0.0, 0.0]
+                    self.latency_pub.publish(latency_msg)
+                # 更新关节监视器
+                now = time.time()
+                if now - self._last_ui_update >= self._ui_update_interval:
+                    self._last_ui_update = now
+                    values = [float(self.data.qpos[addr]) for addr in self.joint_qpos_addrs]
+                    self.joint_ui.update_values(values)
+
+
+def main(args=None):
+    # Qt GUI 必须在主线程初始化
+    app = QApplication.instance() or QApplication(sys.argv)
+
+    rclpy.init(args=args)
+    node = MujocoNode()
+
+    # MuJoCo viewer + ROS2 都在主线程，避免 GLFW 线程冲突
+    node.run_viewer()
+
+    node.destroy_node()
+    if rclpy.ok():
+        rclpy.shutdown()
+    sys.exit(0)
+
+
+if __name__ == '__main__':
+    main()

src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/linker_hand_l10_left.xml

@@ -0,0 +1,195 @@
+<mujoco model="linker_hand_l10_left">
+    <compiler angle="radian" />
+    <size njmax="500" nconmax="100" />
+    <asset>
+        <!-- 蓝白棋盘纹理 -->
+        <texture name="grid_tex" type="2d" builtin="checker" rgb1="0.2 0.3 0.8" rgb2="1 1 1" width="512" height="512"/>
+        
+        <!-- 关联纹理的材质 -->
+        <material name="grid_material" texture="grid_tex" texrepeat="10 10" specular="0.3" shininess="0.5"/>
+        <mesh name="base_link" file="base_link.STL" />
+        <mesh name="thumb_link0" file="thumb_link0.STL" />
+        <mesh name="thumb_link1" file="thumb_link1.STL" />
+        <mesh name="thumb_link2" file="thumb_link2.STL" />
+        <mesh name="thumb_link3" file="thumb_link3.STL" />
+        <mesh name="thumb_link4" file="thumb_link4.STL" />
+        <mesh name="thumb_link5" file="thumb_link5.STL" />
+        <mesh name="index_link0" file="index_link0.STL" />
+        <mesh name="index_link1" file="index_link1.STL" />
+        <mesh name="index_link2" file="index_link2.STL" />
+        <mesh name="index_link3" file="index_link3.STL" />
+        <mesh name="index_link4" file="index_link4.STL" />
+        <mesh name="middle_link0" file="middle_link0.STL" />
+        <mesh name="middle_link1" file="middle_link1.STL" />
+        <mesh name="middle_link2" file="middle_link2.STL" />
+        <mesh name="middle_link3" file="middle_link3.STL" />
+        <mesh name="ring_link0" file="ring_link0.STL" />
+        <mesh name="ring_link1" file="ring_link1.STL" />
+        <mesh name="ring_link2" file="ring_link2.STL" />
+        <mesh name="ring_link3" file="ring_link3.STL" />
+        <mesh name="ring_link4" file="ring_link4.STL" />
+        <mesh name="little_link0" file="little_link0.STL" />
+        <mesh name="little_link1" file="little_link1.STL" />
+        <mesh name="little_link2" file="little_link2.STL" />
+        <mesh name="little_link3" file="little_link3.STL" />
+        <mesh name="little_link4" file="little_link4.STL" />
+    </asset>
+    <visual>
+        <quality shadowsize="4096"/>
+        <map znear="0.05"/>
+        <global offwidth="1280" offheight="720"/>
+    </visual>
+
+  <default>
+    <joint limited="true" damping="0.1" armature="0.01"/>
+    <motor ctrllimited="true"/>
+    <geom condim="3" contype="1" conaffinity="1" solref="0.01 1" solimp="0.9 0.95 0.001" friction="1.0 0.005 0.0001"/>
+    <equality solref="0.01 1"/>
+  </default>
+    <worldbody>
+        <!-- 蓝白方格地面 -->
+        <geom 
+        name="floor" 
+        type="plane" 
+        pos="0 0 0" 
+        size="5 5 0.1" 
+        material="grid_material"
+        conaffinity="1" 
+        condim="3"
+        />
+        <light directional="true" diffuse=".4 .4 .4" specular=".1 .1 .1" pos="0 0 5" dir="0 0 -1"/>
+        <geom type="mesh" rgba="1 1 1 1" mesh="base_link" />
+        <body name="thumb_link0" pos="-0.017939 -0.018426 0.086237">
+            <inertial pos="0.0231869 -9.48627e-06 -0.00324883" quat="0.702183 -0.0892113 -0.0895304 0.700688" mass="0.0115245" diaginertia="4.3707e-06 3.16784e-06 1.87527e-06" />
+            <joint name="thumb_joint0" pos="0 0 0" axis="1 0 0" limited="true" range="-1.01 0.52" />
+            <geom type="mesh" rgba="1 1 1 1" mesh="thumb_link0" />
+            <body name="thumb_link1" pos="0.0303 0 0.016812">
+                <inertial pos="0.00028238 -0.000440867 -0.0153488" quat="0.699016 0.6344 0.221684 0.24448" mass="0.0170446" diaginertia="3.40367e-06 2.61452e-06 1.93957e-06" />
+                <joint name="thumb_joint1" pos="0 0 0" axis="0 0 1" limited="true" range="0 1.43" />
+                <geom type="mesh" rgba="1 1 1 1" mesh="thumb_link1" />
+                <body name="thumb_link2" pos="0.020969 -0.020033 -0.018293" quat="0.944089 0 0 0.32969">
+                    <inertial pos="-0.00445352 -0.0191765 0.0112773" quat="0.839152 0.543897 0.000148238 0.000249164" mass="0.0154404" diaginertia="4.16317e-06 3.18425e-06 1.43135e-06" />
+                    <joint name="thumb_joint2" pos="0 0 0" axis="1 0 0" limited="true" range="-0.44 0" />
+                    <geom type="mesh" rgba="1 1 1 1" mesh="thumb_link2" />
+                    <body name="thumb_link3" pos="0.00418 -0.042527 0.02726">
+                        <inertial pos="-0.00818377 -0.0138382 0.00588046" quat="0.567554 0.421896 -0.422261 0.567082" mass="0.00549845" diaginertia="9.94148e-07 8.89652e-07 3.98192e-07" />
+                        <joint name="thumb_joint3" pos="0 0 0" axis="1 0 0" limited="true" range="-1.45 0" />
+                        <geom type="mesh" rgba="1 1 1 1" mesh="thumb_link3" />
+                        <body name="thumb_link4" pos="-0.00173 -0.034152 0.013346">
+                            <inertial pos="-0.00625482 -0.0105872 0.00370366" quat="0.483693 0.514433 -0.515025 0.485952" mass="0.00484896" diaginertia="3.57916e-07 3.20877e-07 1.7935e-07" />
+                            <joint name="thumb_joint4" pos="0 0 0" axis="1 0 0" limited="true" range="-0.7 0" />
+                            <geom type="mesh" rgba="1 1 1 1" mesh="thumb_link4" />
+                            <geom pos="-0.0055 -0.028238 0.0018333" type="mesh" rgba="1 1 1 1" mesh="thumb_link5" />
+                        </body>
+                    </body>
+                </body>
+            </body>
+        </body>
+        <body name="index_link0" pos="0.011645 -0.02293 0.15849">
+            <inertial pos="-0.0145152 1.54228e-05 0.00937118" quat="0.48049 0.517058 0.523404 0.477314" mass="0.00412866" diaginertia="3.83464e-07 3.29927e-07 2.43301e-07" />
+            <joint name="index_joint0" pos="0 0 0" axis="1 0 0" limited="true" range="0 0.26" />
+            <geom type="mesh" rgba="1 1 1 1" mesh="index_link0" />
+            <body name="index_link1" pos="-0.012765 -0.0085 0.017575">
+                <inertial pos="-0.00432244 0.00844417 0.0189705" quat="0.987574 0.00135795 -0.156092 0.0181975" mass="0.00319121" diaginertia="2.91659e-07 2.67771e-07 1.37647e-07" />
+                <joint name="index_joint1" pos="0 0 0" axis="0 1 0" limited="true" range="0 1.41" />
+                <geom type="mesh" rgba="1 1 1 1" mesh="index_link1" />
+                <body name="index_link2" pos="-0.0051959 0.0031156 0.033276">
+                    <inertial pos="-0.00422895 0.00578281 0.0108417" quat="0.972764 0.0203089 -0.230849 -0.00507891" mass="0.00326734" diaginertia="2.97604e-07 2.61394e-07 1.21583e-07" />
+                    <joint name="index_joint2" pos="0 0 0" axis="0 1 0" limited="true" range="0 1.62" />
+                    <geom type="mesh" rgba="1 1 1 1" mesh="index_link2" />
+                    <body name="index_link3" pos="-0.0097853 5.4404e-05 0.024348">
+                        <inertial pos="-0.00330443 0.00532954 0.0112038" quat="0.941985 0.00477793 -0.334601 0.0261255" mass="0.0038651" diaginertia="2.21793e-07 2.06719e-07 9.47172e-08" />
+                        <joint name="index_joint3" pos="0 0 0" axis="0 1 0" limited="true" range="0 0.96" />
+                        <geom type="mesh" rgba="1 1 1 1" mesh="index_link3" />
+                        <geom pos="-0.014675 0.00425 0.022509" type="mesh" rgba="1 1 1 1" mesh="index_link4" />
+                    </body>
+                </body>
+            </body>
+        </body>
+        <body name="middle_link0" pos="-0.0031059 -0.01243 0.18107">
+            <inertial pos="-0.00432244 0.00844417 0.0189705" quat="0.987574 0.00135795 -0.156092 0.0181975" mass="0.00319121" diaginertia="2.91659e-07 2.67771e-07 1.37647e-07" />
+            <joint name="middle_joint0" pos="0 0 0" axis="0 1 0" limited="true" range="0 1.41" />
+            <geom type="mesh" rgba="1 1 1 1" mesh="middle_link0" />
+            <body name="middle_link1" pos="-0.0051959 0.0031156 0.033276">
+                <inertial pos="-0.00422895 0.00578281 0.0108417" quat="0.972764 0.0203089 -0.230849 -0.0050789" mass="0.00326734" diaginertia="2.97604e-07 2.61394e-07 1.21583e-07" />
+                <joint name="middle_joint1" pos="0 0 0" axis="0 1 0" limited="true" range="0 1.62" />
+                <geom type="mesh" rgba="1 1 1 1" mesh="middle_link1" />
+                <body name="middle_link2" pos="-0.0097853 -0.0001456 0.024348">
+                    <inertial pos="-0.00330443 0.00552954 0.0112038" quat="0.941985 0.00477788 -0.334601 0.0261251" mass="0.0038651" diaginertia="2.21793e-07 2.06719e-07 9.47172e-08" />
+                    <joint name="middle_joint2" pos="0 0 0" axis="0 1 0" limited="true" range="0 0.96" />
+                    <geom type="mesh" rgba="1 1 1 1" mesh="middle_link2" />
+                    <geom pos="-0.014675 0.00445 0.022509" type="mesh" rgba="1 1 1 1" mesh="middle_link3" />
+                </body>
+            </body>
+        </body>
+        <body name="ring_link0" pos="0.011645 0.01507 0.15849">
+            <inertial pos="-0.0145152 1.54245e-05 0.00937119" quat="0.48049 0.517058 0.523404 0.477313" mass="0.00412867" diaginertia="3.83464e-07 3.29927e-07 2.43301e-07" />
+            <joint name="ring_joint0" pos="0 0 0" axis="1 0 0" limited="true" range="-0.26 0" />
+            <geom type="mesh" rgba="1 1 1 1" mesh="ring_link0" />
+            <body name="ring_link1" pos="-0.012765 -0.0085 0.017575">
+                <inertial pos="-0.00432244 0.00844417 0.0189705" quat="0.987574 0.00135795 -0.156092 0.0181975" mass="0.00319121" diaginertia="2.91659e-07 2.67771e-07 1.37647e-07" />
+                <joint name="ring_joint1" pos="0 0 0" axis="0 1 0" limited="true" range="0 1.41" />
+                <geom type="mesh" rgba="1 1 1 1" mesh="ring_link1" />
+                <body name="ring_link2" pos="-0.0051959 0.0031156 0.033276">
+                    <inertial pos="-0.00422895 0.00578281 0.0108417" quat="0.972764 0.0203089 -0.230849 -0.0050789" mass="0.00326734" diaginertia="2.97604e-07 2.61394e-07 1.21583e-07" />
+                    <joint name="ring_joint2" pos="0 0 0" axis="0 1 0" limited="true" range="0 1.62" />
+                    <geom type="mesh" rgba="1 1 1 1" mesh="ring_link2" />
+                    <body name="ring_link3" pos="-0.0097853 -0.0001456 0.024348">
+                        <inertial pos="-0.00330443 0.00552954 0.0112038" quat="0.941985 0.00477791 -0.334601 0.0261255" mass="0.0038651" diaginertia="2.21793e-07 2.06719e-07 9.47172e-08" />
+                        <joint name="ring_joint3" pos="0 0 0" axis="0 1 0" limited="true" range="0 0.96" />
+                        <geom type="mesh" rgba="1 1 1 1" mesh="ring_link3" />
+                        <geom pos="-0.014675 0.00445 0.022509" type="mesh" rgba="1 1 1 1" mesh="ring_link4" />
+                    </body>
+                </body>
+            </body>
+        </body>
+        <body name="little_link0" pos="0.013645 0.03407 0.15349">
+            <inertial pos="-0.0145152 1.54245e-05 0.00937119" quat="0.48049 0.517058 0.523404 0.477313" mass="0.00412867" diaginertia="3.83464e-07 3.29927e-07 2.43301e-07" />
+            <joint name="little_joint0" pos="0 0 0" axis="1 0 0" limited="true" range="-0.26 0" />
+            <geom type="mesh" rgba="1 1 1 1" mesh="little_link0" />
+            <body name="little_link1" pos="-0.012765 -0.0085 0.017575">
+                <inertial pos="-0.00432244 0.00844417 0.0189705" quat="0.987574 0.00135795 -0.156092 0.0181975" mass="0.00319121" diaginertia="2.91659e-07 2.67771e-07 1.37647e-07" />
+                <joint name="little_joint1" pos="0 0 0" axis="0 1 0" limited="true" range="0 1.41" />
+                <geom type="mesh" rgba="1 1 1 1" mesh="little_link1" />
+                <body name="little_link2" pos="-0.0051959 0.0031156 0.033276">
+                    <inertial pos="-0.00422895 0.00578281 0.0108417" quat="0.972764 0.0203089 -0.230849 -0.0050789" mass="0.00326734" diaginertia="2.97604e-07 2.61394e-07 1.21583e-07" />
+                    <joint name="little_joint2" pos="0 0 0" axis="0 1 0" limited="true" range="0 1.62" />
+                    <geom type="mesh" rgba="1 1 1 1" mesh="little_link2" />
+                    <body name="little_link3" pos="-0.0097853 -0.0001456 0.024348">
+                        <inertial pos="-0.00330443 0.00552954 0.0112038" quat="0.941985 0.00477783 -0.334601 0.0261254" mass="0.0038651" diaginertia="2.21793e-07 2.06719e-07 9.47172e-08" />
+                        <joint name="little_joint3" pos="0 0 0" axis="0 1 0" limited="true" range="0 0.96" />
+                        <geom type="mesh" rgba="1 1 1 1" mesh="little_link3" />
+                        <geom pos="-0.014675 0.00445 0.022509" type="mesh" rgba="1 1 1 1" mesh="little_link4" />
+                    </body>
+                </body>
+            </body>
+        </body>
+    </worldbody>
+    <actuator>
+        <position name="thumb_pos0" joint="thumb_joint0" kp="10" ctrlrange="-0.1396 0.349"/>
+        <position name="thumb_pos1" joint="thumb_joint1" kp="10" ctrlrange="0 1.57"/>
+        <position name="thumb_pos2" joint="thumb_joint2" kp="10" ctrlrange="-1.57 0"/>
+        <position name="thumb_pos3" joint="thumb_joint3" kp="10" ctrlrange="-1.57 0"/>
+        <position name="thumb_pos4" joint="thumb_joint4" kp="10" ctrlrange="-1.57 0"/>
+        
+        <position name="index_pos0" joint="index_joint0" kp="10" ctrlrange="-0.26 0.26"/>
+        <position name="index_pos1" joint="index_joint1" kp="10" ctrlrange="0 1.396"/>
+        <position name="index_pos2" joint="index_joint2" kp="10" ctrlrange="0 1.57"/>
+        <position name="index_pos3" joint="index_joint3" kp="10" ctrlrange="0 1.57"/>
+        
+        <position name="middle_pos0" joint="middle_joint0" kp="10" ctrlrange="0 1.57"/>
+        <position name="middle_pos1" joint="middle_joint1" kp="10" ctrlrange="0 1.57"/>
+        <position name="middle_pos2" joint="middle_joint2" kp="10" ctrlrange="0 1.57"/>
+
+        
+        <position name="ring_pos0" joint="ring_joint0" kp="10" ctrlrange="-0.26 0.26"/>
+        <position name="ring_pos1" joint="ring_joint1" kp="10" ctrlrange="0 1.57"/>
+        <position name="ring_pos2" joint="ring_joint2" kp="10" ctrlrange="0 1.57"/>
+        <position name="ring_pos3" joint="ring_joint3" kp="10" ctrlrange="0 1.57"/>
+        
+        <position name="little_pos0" joint="little_joint0" kp="10" ctrlrange="-0.26 0.26"/>
+        <position name="little_pos1" joint="little_joint1" kp="10" ctrlrange="0 1.57"/>
+        <position name="little_pos2" joint="little_joint2" kp="10" ctrlrange="0 1.57"/>
+        <position name="little_pos3" joint="little_joint3" kp="10" ctrlrange="0 1.57"/>
+    </actuator>
+</mujoco>

src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/linker_hand_l10_right.xml

@@ -0,0 +1,195 @@
+<mujoco model="linker_hand_l10_right">
+    <compiler angle="radian" />
+    <size njmax="500" nconmax="100" />
+    <asset>
+        <!-- 蓝白棋盘纹理 -->
+        <texture name="grid_tex" type="2d" builtin="checker" rgb1="0.2 0.3 0.8" rgb2="1 1 1" width="512" height="512"/>
+        
+        <!-- 关联纹理的材质 -->
+        <material name="grid_material" texture="grid_tex" texrepeat="10 10" specular="0.3" shininess="0.5"/>
+        <mesh name="base_link" file="base_link.STL" />
+        <mesh name="thumb_link0" file="thumb_link0.STL" />
+        <mesh name="thumb_link1" file="thumb_link1.STL" />
+        <mesh name="thumb_link2" file="thumb_link2.STL" />
+        <mesh name="thumb_link3" file="thumb_link3.STL" />
+        <mesh name="thumb_link4" file="thumb_link4.STL" />
+        <mesh name="thumb_link5" file="thumb_link5.STL" />
+        <mesh name="index_link0" file="index_link0.STL" />
+        <mesh name="index_link1" file="index_link1.STL" />
+        <mesh name="index_link2" file="index_link2.STL" />
+        <mesh name="index_link3" file="index_link3.STL" />
+        <mesh name="index_link4" file="index_link4.STL" />
+        <mesh name="middle_link0" file="middle_link0.STL" />
+        <mesh name="middle_link1" file="middle_link1.STL" />
+        <mesh name="middle_link2" file="middle_link2.STL" />
+        <mesh name="middle_link3" file="middle_link3.STL" />
+        <mesh name="ring_link0" file="ring_link0.STL" />
+        <mesh name="ring_link1" file="ring_link1.STL" />
+        <mesh name="ring_link2" file="ring_link2.STL" />
+        <mesh name="ring_link3" file="ring_link3.STL" />
+        <mesh name="ring_link4" file="ring_link4.STL" />
+        <mesh name="little_link0" file="little_link0.STL" />
+        <mesh name="little_link1" file="little_link1.STL" />
+        <mesh name="little_link2" file="little_link2.STL" />
+        <mesh name="little_link3" file="little_link3.STL" />
+        <mesh name="little_link4" file="little_link4.STL" />
+    </asset>
+    <visual>
+        <quality shadowsize="4096"/>
+        <map znear="0.05"/>
+        <global offwidth="1280" offheight="720"/>
+    </visual>
+
+  <default>
+    <joint limited="true" damping="0.1" armature="0.01"/>
+    <motor ctrllimited="true"/>
+    <geom condim="3" contype="1" conaffinity="1" solref="0.01 1" solimp="0.9 0.95 0.001" friction="1.0 0.005 0.0001"/>
+    <equality solref="0.01 1"/>
+  </default>
+    <worldbody>
+    <!-- 蓝白方格地面 -->
+        <geom 
+        name="floor" 
+        type="plane" 
+        pos="0 0 0" 
+        size="5 5 0.1" 
+        material="grid_material"
+        conaffinity="1" 
+        condim="3"
+        />
+        <light directional="true" diffuse=".4 .4 .4" specular=".1 .1 .1" pos="0 0 5" dir="0 0 -1"/>
+        <geom type="mesh" rgba="1 1 1 1" mesh="base_link" />
+        <body name="thumb_link0" pos="-0.017939 0.018426 0.057469">
+            <inertial pos="0.0231878 -0.00228979 -0.00230333" quat="0.186304 0.682904 -0.613061 0.35084" mass="0.0115253" diaginertia="4.37121e-06 3.168e-06 1.87569e-06" />
+            <joint name="thumb_joint0" pos="0 0 0" axis="1 0 0" limited="true" range="-0.52 1.01" />
+            <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="thumb_link0" />
+            <body name="thumb_link1" pos="0.0303 -0.0097852 -0.0097852" quat="0.872224 -0.361289 -0.126164 -0.304595">
+                <inertial pos="-5.30054e-05 0.000521078 0.0153022" quat="0.671886 0.740655 -0.00048468 -2.01133e-05" mass="0.0170451" diaginertia="3.40365e-06 2.61468e-06 1.93956e-06" />
+                <joint name="thumb_joint1" pos="0 0 0" axis="0 0 -1" limited="true" range="0 1.43" />
+                <geom type="mesh" rgba="0.4 0.4 0.4 1" mesh="thumb_link1" />
+                <body name="thumb_link2" pos="0.0039399 0.028731 0.012357" quat="0.959788 0.280726 0 0">
+                    <inertial pos="-0.00445707 0.020789 -0.0017564" quat="0.758907 0.651199 -0.000433103 0.000104848" mass="0.0151262" diaginertia="4.07088e-06 3.0171e-06 1.34222e-06" />
+                    <joint name="thumb_joint2" pos="0 0 0" axis="1 0 0" limited="true" range="0 0.44" />
+                    <geom type="mesh" rgba="1 1 1 1" mesh="thumb_link2" />
+                    <body name="thumb_link3" pos="-0.01062 0.050514 0" quat="0.99537 -0.0961214 0 0">
+                        <inertial pos="0.0066161 0.01503 0.00044042" quat="0.51966 0.479591 -0.479089 0.520019" mass="0.0054986" diaginertia="9.94194e-07 8.897e-07 3.98196e-07" />
+                        <joint name="thumb_joint3" pos="0 0 0" axis="1 0 0" limited="true" range="0 0.75" />
+                        <geom type="mesh" rgba="0.50196 1 0.50196 1" mesh="thumb_link3" />
+                        <body name="thumb_link4" pos="0.00532 0.036667 0" quat="0.998255 -0.0590506 0 0">
+                            <inertial pos="0.0012996 0.0146213 0.000964046" quat="0.588218 0.391594 -0.392011 0.589051" mass="0.00540097" diaginertia="4.47191e-07 3.63974e-07 1.90089e-07" />
+                            <joint name="thumb_joint4" pos="0 0 0" axis="1 0 0" limited="true" range="0 0.7" />
+                            <geom type="mesh" rgba="1 1 1 1" mesh="thumb_link4" />
+                            <geom pos="0.0023001 0.037723 0" quat="-3.64259e-06 -4.73241e-07 0.128836 0.991666" type="mesh" rgba="1 1 1 1" mesh="thumb_link5" />
+                        </body>
+                    </body>
+                </body>
+            </body>
+        </body>
+        <body name="index_link0" pos="0.011645 0.02293 0.12973">
+            <inertial pos="-0.014515 -1.5021e-05 0.00937" quat="0.477436 0.523331 0.516976 0.480537" mass="0.0041283" diaginertia="3.83396e-07 3.29912e-07 2.43232e-07" />
+            <joint name="index_joint0" pos="0 0 0" axis="1 0 0" limited="true" range="-0.26 0" />
+            <geom type="mesh" rgba="1 0.6 0 1" mesh="index_link0" />
+            <body name="index_link1" pos="-0.012765 0.0085 0.017575">
+                <inertial pos="-0.00432247 -0.00844417 0.0189706" quat="0.98757 -0.00134839 -0.1561 -0.0183156" mass="0.00319124" diaginertia="2.91666e-07 2.67777e-07 1.37647e-07" />
+                <joint name="index_joint1" pos="0 0 0" axis="0 1 0" limited="true" range="0 1.41" />
+                <geom type="mesh" rgba="0.792157 0.819608 0.921569 1" mesh="index_link1" />
+                <body name="index_link2" pos="-0.0051959 -0.0031156 0.033276">
+                    <inertial pos="-0.00408694 -0.00578281 0.0108961" quat="0.974253 -0.0202765 -0.224482 0.00521904" mass="0.00326733" diaginertia="2.97602e-07 2.61391e-07 1.21583e-07" />
+                    <joint name="index_joint2" pos="0 0 0" axis="0 1 0" limited="true" range="0 1.62" />
+                    <geom type="mesh" rgba="1 1 1 1" mesh="index_link2" />
+                    <body name="index_link3" pos="-0.0094663 -5.4404e-05 0.024474">
+                        <inertial pos="-0.00571071 -0.00522636 0.0156925" quat="0.935586 -0.00416504 -0.353015 -0.00641165" mass="0.00411717" diaginertia="3.07149e-07 2.56695e-07 1.04416e-07" />
+                        <joint name="index_joint3" pos="0 0 0" axis="0 1 0" limited="true" range="0 0.96" />
+                        <geom type="mesh" rgba="1 1 1 1" mesh="index_link3" />
+                        <geom pos="-0.014222 -0.00417 0.034954" type="mesh" rgba="1 1 1 1" mesh="index_link4" />
+                    </body>
+                </body>
+            </body>
+        </body>
+        <body name="middle_link0" pos="-0.0031059 0.01243 0.1523">
+            <inertial pos="-0.0043225 -0.0084442 0.018971" quat="0.987571 -0.00134849 -0.156095 -0.0183158" mass="0.0031912" diaginertia="2.91668e-07 2.67779e-07 1.37643e-07" />
+            <joint name="middle_joint0" pos="0 0 0" axis="0 1 0" limited="true" range="0 1.41" />
+            <geom type="mesh" rgba="0.79216 0.81961 0.92157 1" mesh="middle_link0" />
+            <body name="middle_link1" pos="-0.0051959 -0.0031156 0.033276">
+                <inertial pos="-0.00408694 -0.00578281 0.0108961" quat="0.974253 -0.0202765 -0.224482 0.00521904" mass="0.00326733" diaginertia="2.97602e-07 2.61391e-07 1.21583e-07" />
+                <joint name="middle_joint1" pos="0 0 0" axis="0 1 0" limited="true" range="0 1.62" />
+                <geom type="mesh" rgba="1 1 1 1" mesh="middle_link1" />
+                <body name="middle_link2" pos="-0.0094663 -5.4404e-05 0.024474">
+                    <inertial pos="-0.00571071 -0.00522636 0.0156925" quat="0.935586 -0.00416504 -0.353015 -0.00641165" mass="0.00411717" diaginertia="3.07149e-07 2.56695e-07 1.04416e-07" />
+                    <joint name="middle_joint2" pos="0 0 0" axis="0 1 0" limited="true" range="0 0.96" />
+                    <geom type="mesh" rgba="1 1 1 1" mesh="middle_link2" />
+                    <geom pos="-0.014222 -0.00417 0.034954" type="mesh" rgba="1 1 1 1" mesh="middle_link3" />
+                </body>
+            </body>
+        </body>
+        <body name="ring_link0" pos="0.011645 -0.01507 0.12973">
+            <inertial pos="-0.014515 -1.5022e-05 0.00937" quat="0.477436 0.523331 0.516976 0.480537" mass="0.0041283" diaginertia="3.83396e-07 3.29912e-07 2.43232e-07" />
+            <joint name="ring_joint0" pos="0 0 0" axis="1 0 0" limited="true" range="0 0.13" />
+            <geom type="mesh" rgba="1 0.6 0 1" mesh="ring_link0" />
+            <body name="ring_link1" pos="-0.012765 0.0085 0.017575">
+                <inertial pos="-0.00432247 -0.00844417 0.0189706" quat="0.98757 -0.00134839 -0.1561 -0.0183156" mass="0.00319124" diaginertia="2.91666e-07 2.67777e-07 1.37647e-07" />
+                <joint name="ring_joint1" pos="0 0 0" axis="0 1 0" limited="true" range="0 1.41" />
+                <geom type="mesh" rgba="0.792157 0.819608 0.921569 1" mesh="ring_link1" />
+                <body name="ring_link2" pos="-0.0051959 -0.0031156 0.033276">
+                    <inertial pos="-0.00408695 -0.00578281 0.0108961" quat="0.974253 -0.0202766 -0.224482 0.00521876" mass="0.00326733" diaginertia="2.97602e-07 2.61391e-07 1.21583e-07" />
+                    <joint name="ring_joint2" pos="0 0 0" axis="0 1 0" limited="true" range="0 1.62" />
+                    <geom type="mesh" rgba="0.490196 0.490196 0.490196 1" mesh="ring_link2" />
+                    <body name="ring_link3" pos="-0.0094663 -5.4399e-05 0.024474">
+                        <inertial pos="-0.00571071 -0.00522636 0.0156925" quat="0.935586 -0.00416504 -0.353015 -0.00641165" mass="0.00411717" diaginertia="3.07149e-07 2.56695e-07 1.04416e-07" />
+                        <joint name="ring_joint3" pos="0 0 0" axis="0 1 0" limited="true" range="0 0.96" />
+                        <geom type="mesh" rgba="1 1 1 1" mesh="ring_link3" />
+                        <geom pos="-0.014222 -0.00417 0.034954" type="mesh" rgba="1 1 1 1" mesh="ring_link4" />
+                    </body>
+                </body>
+            </body>
+        </body>
+        <body name="little_link0" pos="0.013645 -0.03407 0.12473">
+            <inertial pos="-0.014515 -1.5014e-05 0.00937" quat="0.477436 0.523331 0.516975 0.480537" mass="0.0041283" diaginertia="3.83396e-07 3.29912e-07 2.43232e-07" />
+            <joint name="little_joint0" pos="0 0 0" axis="1 0 0" limited="true" range="0 0.26" />
+            <geom type="mesh" rgba="1 0.6 0 1" mesh="little_link0" />
+            <body name="little_link1" pos="-0.012765 0.0085 0.017575">
+                <inertial pos="-0.00432248 -0.00844417 0.0189706" quat="0.98757 -0.00134844 -0.1561 -0.0183159" mass="0.00319124" diaginertia="2.91666e-07 2.67777e-07 1.37647e-07" />
+                <joint name="little_joint1" pos="0 0 0" axis="0 1 0" limited="true" range="0 1.41" />
+                <geom type="mesh" rgba="0.490196 0.490196 0.490196 1" mesh="little_link1" />
+                <body name="little_link2" pos="-0.0051959 -0.0031156 0.033276">
+                    <inertial pos="-0.00408694 -0.00578281 0.0108961" quat="0.974253 -0.0202765 -0.224482 0.00521904" mass="0.00326733" diaginertia="2.97602e-07 2.61391e-07 1.21583e-07" />
+                    <joint name="little_joint2" pos="0 0 0" axis="0 1 0" limited="true" range="0 1.62" />
+                    <geom type="mesh" rgba="1 1 1 1" mesh="little_link2" />
+                    <body name="little_link3" pos="-0.0094663 -5.4404e-05 0.024474">
+                        <inertial pos="-0.00571072 -0.00522636 0.0156925" quat="0.935586 -0.00416514 -0.353015 -0.00641192" mass="0.00411717" diaginertia="3.07149e-07 2.56695e-07 1.04416e-07" />
+                        <joint name="little_joint3" pos="0 0 0" axis="0 1 0" limited="true" range="0 0.96" />
+                        <geom type="mesh" rgba="1 1 1 1" mesh="little_link3" />
+                        <geom pos="-0.014222 -0.00417 0.034954" type="mesh" rgba="1 1 1 1" mesh="little_link4" />
+                    </body>
+                </body>
+            </body>
+        </body>
+    </worldbody>
+    <actuator>
+        <position name="thumb_pos0" joint="thumb_joint0" kp="10" ctrlrange="-0.52 1.01"/>
+        <position name="thumb_pos1" joint="thumb_joint1" kp="10" ctrlrange="0 1.43"/>
+        <position name="thumb_pos2" joint="thumb_joint2" kp="10" ctrlrange="0 0.44"/>
+        <position name="thumb_pos3" joint="thumb_joint3" kp="10" ctrlrange="0 0.75"/>
+        <position name="thumb_pos4" joint="thumb_joint4" kp="10" ctrlrange="0 0.7"/>
+        
+        <position name="index_pos0" joint="index_joint0" kp="10" ctrlrange="-0.26 0"/>
+        <position name="index_pos1" joint="index_joint1" kp="10" ctrlrange="0 1.41"/>
+        <position name="index_pos2" joint="index_joint2" kp="10" ctrlrange="0 1.62"/>
+        <position name="index_pos3" joint="index_joint3" kp="10" ctrlrange="0 0.96"/>
+        
+        <position name="middle_pos0" joint="middle_joint0" kp="10" ctrlrange="0 1.41"/>
+        <position name="middle_pos1" joint="middle_joint1" kp="10" ctrlrange="0 1.62"/>
+        <position name="middle_pos2" joint="middle_joint2" kp="10" ctrlrange="0 0.96"/>
+
+        
+        <position name="ring_pos0" joint="ring_joint0" kp="10" ctrlrange="0 0.13"/>
+        <position name="ring_pos1" joint="ring_joint1" kp="10" ctrlrange="0 1.41"/>
+        <position name="ring_pos2" joint="ring_joint2" kp="10" ctrlrange="0 1.62"/>
+        <position name="ring_pos3" joint="ring_joint3" kp="10" ctrlrange="0 0.96"/>
+        
+        <position name="little_pos0" joint="little_joint0" kp="10" ctrlrange="0 0.26"/>
+        <position name="little_pos1" joint="little_joint1" kp="10" ctrlrange="0 1.41"/>
+        <position name="little_pos2" joint="little_joint2" kp="10" ctrlrange="0 1.62"/>
+        <position name="little_pos3" joint="little_joint3" kp="10" ctrlrange="0 0.96"/>
+    </actuator>
+</mujoco>