commit ff4bbe911477bb7060b4a60dcc724dee74ab728a
Author: chenshijue <24121145@bjtu.edu.cn>
Date: Tue Jul 28 17:10:49 2026 +0800
手套-真手-mujoco仿真延迟测量完整版
diff --git a/.gitattributes b/.gitattributes
new file mode 100644
index 0000000..351b3ad
--- /dev/null
+++ b/.gitattributes
@@ -0,0 +1 @@
+*.STL filter=lfs diff=lfs merge=lfs -text
diff --git a/.gitignore b/.gitignore
new file mode 100644
index 0000000..86366f0
--- /dev/null
+++ b/.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_*/
diff --git a/Readme.md b/Readme.md
new file mode 100644
index 0000000..f14db3d
--- /dev/null
+++ b/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.
\ No newline at end of file
diff --git a/Readme_ZH.md b/Readme_ZH.md
new file mode 100644
index 0000000..cfa1461
--- /dev/null
+++ b/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]
+```
+
+改写成如上图的示例，即可启用右机械手的校准，启动后就会让机械手按照映射的角度固定在当前角度
+
+当左右两手都达到期望的对指位置后，就可以恢复原状，按正常顺序使用遥操系统
\ No newline at end of file
diff --git a/readme.txt b/readme.txt
new file mode 100644
index 0000000..72f6f40
--- /dev/null
+++ b/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
+还是这些流程
\ No newline at end of file
diff --git a/src/camera_retarget_l20/QUICK_START_O6_G20_ZH.md b/src/camera_retarget_l20/QUICK_START_O6_G20_ZH.md
new file mode 100644
index 0000000..e64fb88
--- /dev/null
+++ b/src/camera_retarget_l20/QUICK_START_O6_G20_ZH.md
@@ -0,0 +1,160 @@
+# O6 + G20 摄像头手势控制：普通用户使用说明
+
+## 这套设备能做什么
+
+把你的左手放在摄像头前，系统会识别手指动作，并让 O6 和 G20 两只机械手
+同时模仿你的动作。
+
+## 使用前注意
+
+- 只使用左手操作。
+- 确保两只机械手周围没有人、线缆、工具或易碎物品。
+- 不要把手指伸进机械手内部。
+- 操作时尽量让掌心朝向摄像头，并让整只手完整出现在画面里。
+- 第一次使用时慢慢活动手指，不要突然快速握拳。
+
+## 第一步：启动程序
+
+打开终端，输入：
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source .venv/bin/activate
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+ros2 launch camera_retarget_l20 teleop_o6_g20_left.launch.py \
+ motor_speed:=250 \
+ ema_alpha:=0.55 \
+ max_step:=24.0 \
+ command_deadband:=3.0
+```
+
+程序启动后会出现摄像头窗口。刚启动时，机械手不会跟随你的手，这是正常的。
+
+## 第二步：完成三个姿势标定
+
+先用鼠标点击一下摄像头窗口，然后依次完成下面三个动作。
+
+每次按键后，都要保持姿势不动约 2 秒。看到进度完成或终端显示
+`calibration complete` 后，再做下一个动作。
+
+### 1. 按 C：完全张开
+
+- 左手五指完全伸直。
+- 手指自然分开。
+- 大拇指向外展开，不要贴着食指。
+- 保持这个姿势，按一次 `C`。
+
+### 2. 按 F：正常握拳
+
+- 像平时一样完整握拳。
+- 四根手指完全弯曲。
+- 大拇指也自然收拢，不要留在外面。
+- 保持这个姿势，按一次 `F`。
+
+### 3. 按 P：做 OK 手势
+
+- 大拇指指尖和食指指尖真正碰在一起。
+- 中指、无名指和小指尽量张开，避免遮挡。
+- 保持这个姿势，按一次 `P`。
+
+正确顺序是：
+
+```text
+C（张开） → F（握拳） → P（OK 手势）
+```
+
+如果某一步提示失败，重新摆好对应姿势，再按一次相同按键即可。
+
+## 第三步：开始控制机械手
+
+完成 C、F、P 后：
+
+1. 把左手稳定放在摄像头画面中。
+2. 按一次 `E`。
+3. 看到画面显示 `REAL CONTROL` 后，机械手开始跟随你的动作。
+
+请先缓慢尝试：
+
+1. 完全张开；
+2. 缓慢握拳；
+3. 拇指与食指捏合；
+4. 拇指与其他手指靠近。
+
+O6 和 G20 的机械结构不同，所以动作效果不会完全一样，这是正常现象。
+
+## 如何暂停
+
+按 `D`，机械手会停止跟随并保持当前位置。
+
+暂停后再次控制，需要保证左手在画面中，然后按 `E`。
+
+## 如何重新标定
+
+控制过程中也可以重新按 `C`、`F` 或 `P`。
+
+系统会自动暂停机械手跟随，然后重新采集姿势。标定完成后，需要再次按 `E`
+才能恢复控制。
+
+如果整体动作明显不准确，建议完整重做：
+
+```text
+C → F → P → E
+```
+
+## 如何安全结束
+
+1. 先把你的左手完全张开。
+2. 等待两只机械手也完全张开。
+3. 按 `D` 停止跟随。
+4. 在启动程序的终端中按 `Ctrl+C`。
+
+不要在机械手正在握紧或夹住物体时直接关闭电源。
+
+## 常用按键
+
+| 按键 | 功能 |
+|---|---|
+| `C` | 标定完全张开的手 |
+| `F` | 标定正常握拳 |
+| `P` | 标定大拇指与食指接触的 OK 手势 |
+| `E` | 开始控制机械手 |
+| `D` | 暂停控制，保持当前位置 |
+| `Q` / `Esc` | 关闭摄像头控制程序 |
+
+## 常见问题
+
+### 按键没有反应
+
+用鼠标点击一下摄像头窗口，再按键。不要在终端窗口中直接按 C、F、P、E。
+
+### 画面找不到手
+
+- 确认使用的是左手；
+- 把整只手放进画面；
+- 掌心尽量朝向摄像头；
+- 改善光线，避免画面过暗；
+- 不要让手指被衣服、桌面或其他物体遮挡。
+
+### 按 E 后机械手不动
+
+通常是 C、F、P 中有一步没有成功。观察终端提示，重新按顺序完成：
+
+```text
+C → F → P → E
+```
+
+### P 标定失败
+
+让大拇指和食指的指尖真正接触，其他三根手指张开，保持手腕和手指稳定约
+2 秒。
+
+### 机械手突然张开并停止跟随
+
+系统可能暂时没有识别到左手，或者某只机械手连接异常。这是安全保护。把左手
+重新稳定地放回画面，确认设备正常后，再按 `E`。
+
+### 动作不够准确
+
+先重新完成 `C → F → P → E`。标定时姿势越标准、越稳定，控制效果越好。
diff --git a/src/camera_retarget_l20/README.md b/src/camera_retarget_l20/README.md
new file mode 100644
index 0000000..8fa8d0c
--- /dev/null
+++ b/src/camera_retarget_l20/README.md
@@ -0,0 +1,212 @@
+# Left O6/G20/L20 camera retargeting
+
+Chinese guides:
+
+- Quick guide for ordinary users: [`QUICK_START_O6_G20_ZH.md`](QUICK_START_O6_G20_ZH.md)
+- Detailed operator guide: [`USAGE_O6_G20_ZH.md`](USAGE_O6_G20_ZH.md)
+
+This package bridges a webcam to physical left LinkerHand O6, G20, or L20
+hands. It can drive an O6 and a G20 from the same detected human hand:
+
+`camera -> MediaPipe -> shared calibrated hand features -> O6 6-value + G20 20-value JointState`
+
+G20 and L20 both expose 20 position values, but they do not use the same CAN
+driver or the same left-hand joint directions. Select the matching model all
+the way from URDF mapping to the SDK driver.
+
+Hardware output is disabled by default. G20 control also requires an explicit
+fully-open human-hand calibration before the `E` key can arm real control.
+
+## Physical left O6 and G20 together
+
+The combined launch owns both CAN interfaces and isolates every SDK topic under
+the model name. Stop manually started `linker_hand_sdk` processes first, and
+make sure `can0` and `can1` are both UP at 1 Mbps:
+
+```bash
+ros2 launch camera_retarget_l20 teleop_o6_g20_left.launch.py
+```
+
+The fixed hardware assignment is G20 on `can0` and O6 on `can1`. The relevant
+topics are:
+
+```text
+/g20/cb_left_hand_control_cmd 20 positions
+/g20/cb_left_hand_state 20 positions
+/o6/cb_left_hand_control_cmd 6 positions
+/o6/cb_left_hand_state 6 positions
+/camera_o6_g20_retarget/g20/target_joint_cmd
+/camera_o6_g20_retarget/o6/target_joint_cmd
+/camera_o6_g20_retarget/status
+/camera_o6_g20_retarget/debug
+```
+
+Real output remains disabled after startup. Dual mode uses speed 60,
+`ema_alpha=0.55`, and a 16-command-unit frame limit for normal responsive
+motion. It uses the complete measured G20 open/fist flexion endpoints and the
+full protocol range for G20 finger splay; O6 flexion is also allowed across
+the full 0..255 protocol range. These endpoint changes do not remove the
+per-frame rate limit, calibration gate, or tracking/driver timeout protection.
+Keep the physical hands clear and complete all three poses before pressing
+`E`. Each key starts an asynchronous capture: hold the requested pose for
+1.5 seconds until at least 30 stable frames have been accepted. The service
+response means that capture started; completion/progress is reported on the
+status and debug topics:
+
+1. `C`: fully open hand with the thumb spread outward (zero pose).
+2. `F`: normal fist (finger and thumb flexion range).
+3. `P`: touch the thumb and index fingertips.
+
+Press `D` to stop sending commands while holding the last pose. Equivalent
+service calls are:
+
+```bash
+ros2 service call /camera_o6_g20_retarget/calibrate_open std_srvs/srv/Trigger "{}"
+ros2 service call /camera_o6_g20_retarget/calibrate_fist std_srvs/srv/Trigger "{}"
+ros2 service call /camera_o6_g20_retarget/calibrate_pinch std_srvs/srv/Trigger "{}"
+ros2 service call /camera_o6_g20_retarget/enable std_srvs/srv/SetBool "{data: true}"
+```
+
+Calibration is intentionally in-memory only. C/F/P must be repeated after
+every node restart. A capture waits up to eight seconds, rejects unstable
+windows using robust per-feature statistics, and never replaces the previous
+valid pose when validation fails. Starting any calibration automatically
+disarms real output and holds the current robot pose.
+
+The direct robot mapping is loaded from
+`config/direct_mapping_profiles.yml`. This profile owns command lengths,
+joint names, measured open/fist endpoints, flexion weights, all four thumb
+opposition targets, fixed channels, and filter defaults. Existing ROS mapping
+parameters remain runtime overrides and are logged when they differ from the
+profile. The profile schema is model-oriented so a future L10 can reuse the
+same feature/adapter path after its physical endpoints have been measured.
+
+The optional debug JSON contains raw bends and fingertip distances,
+normalized closure, the confirmed pinch target, blend strength, and raw versus
+filtered O6/G20 commands:
+
+```bash
+ros2 topic echo /camera_o6_g20_retarget/status
+ros2 topic echo /camera_o6_g20_retarget/debug
+```
+
+If either driver state is absent for one second, or camera tracking is lost,
+the node commands the vendor open pose on both outputs and disarms real
+control. O6 maps thumb bend/side-swing and one bend actuator per finger; only
+the G20 has independent four-finger side-swing outputs.
+
+## Dependencies
+
+Use a Python 3.12 virtual environment that can still see ROS 2 Jazzy packages.
+MediaPipe 0.10.21 is pinned because the newer package no longer exposes the
+legacy `solutions.hands` API used by this node. The dex-retargeting project
+requires NumPy 2, so install MediaPipe without dependency resolution:
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+python3 -m venv --system-site-packages .venv
+source .venv/bin/activate
+python -m pip install -e /home/lxp/projects/dex-retargeting
+python -m pip install --no-deps mediapipe==0.10.21
+source /opt/ros/jazzy/setup.bash
+python -m colcon build --symlink-install --packages-select camera_retarget_l20 linker_hand_ros2_sdk
+source install/setup.bash
+```
+
+On hosts where `python3 -m venv` reports that `ensurepip` is unavailable, create
+the environment with `--without-pip`, then use the system pip's `--python`
+option for the two installation commands:
+
+```bash
+python3 -m venv --without-pip --system-site-packages .venv
+python3 -m pip --python .venv/bin/python install -e /home/lxp/projects/dex-retargeting
+python3 -m pip --python .venv/bin/python install --no-deps mediapipe==0.10.21
+```
+
+The logged-in user must have camera access. Add `lxp` to the `video` group once,
+then log out and back in:
+
+```bash
+sudo usermod -aG video lxp
+```
+
+## G20 preview only
+
+```bash
+ros2 launch camera_retarget_l20 camera_g20_left.launch.py
+```
+
+The target command can be inspected without moving hardware:
+
+```bash
+ros2 topic echo /camera_g20_retarget/target_joint_cmd
+ros2 topic echo /camera_g20_retarget/status
+```
+
+## Physical left G20
+
+Stop any manually started `linker_hand_sdk` first; only one driver may own
+`can0`. Keep the hand clear of people and objects. The combined launch starts
+the G20 driver at speed 30 and moves it to the SDK startup pose, but camera
+output remains locked:
+
+```bash
+ros2 launch camera_retarget_l20 teleop_g20_left.launch.py
+```
+
+The default closing range is 95%. It can be reduced for a conservative test:
+
+```bash
+ros2 launch camera_retarget_l20 teleop_g20_left.launch.py max_closure:=0.60
+```
+
+Hold the real left hand fully open and steadily in view until the preview says
+`hand=OK`. Press `C`, keep the pose stable until the 1.5-second capture
+completes, then press `E` to enable control. The equivalent service calls are:
+
+```bash
+ros2 service call /camera_g20_retarget/calibrate_open std_srvs/srv/Trigger "{}"
+ros2 service call /camera_g20_retarget/enable std_srvs/srv/SetBool "{data: true}"
+```
+
+Disable immediately with `D` or:
+
+```bash
+ros2 service call /camera_g20_retarget/enable std_srvs/srv/SetBool "{data: false}"
+```
+
+The G20 defaults limit closing travel to 95%, use speed 30 for both driver
+startup and camera motion, limit every frame to 8 command units, and
+open/disable the hand if tracking is lost for 0.35 seconds. Root and tip
+flexion is mapped from human joint angles. P calibrates the contact distance;
+at runtime the nearest of the index/middle/ring/pinky fingertips is selected
+and only that finger receives pinch coupling. Calibrated four-finger
+side-to-side motion maps to G20 channels 6 through 9. Splay confidence
+automatically fades toward the
+calibrated open pose when a curled finger is nearly perpendicular to the palm.
+Thumb CMC flexion is coupled to thumb-tip flexion. In dual C/F/P mode the C
+pose is the maximum outward thumb endpoint and the selected thumb/fingertip
+distance controls opposition. A new target must remain closest for three
+frames before it is selected; a margin and release threshold prevent index,
+middle, ring, and pinky from flickering between targets. Precision pinch is a
+continuous blend toward the configured robot pinch pose. A four-finger power
+grasp gradually suppresses this override from 65% closure and uses the four
+finger F endpoints at 90%. It no longer forces the thumb closed: thumb flexion
+uses the two measured thumb bends, while thumb side-swing follows the C-to-F
+thumb side-angle calibration. Legacy single-G20 mode retains its
+geometry-based side-swing without four-finger thumb assistance.
+G20 flexion no longer uses a common lower command or a special command-90
+thumb clamp. Each of the ten flexion motors is interpolated independently
+between the measured open pose and the measured fist pose. In particular, a
+full dual-mode fist targets 50 on the thumb base, 144 on the thumb tip, and 0
+on the eight index-through-pinky flexion channels. Single-G20 launches retain
+their configurable 95% closure multiplier.
+
+## L20 compatibility
+
+The original L20 entry points remain available:
+
+```bash
+ros2 launch camera_retarget_l20 camera_l20_left.launch.py
+ros2 launch camera_retarget_l20 teleop_l20_left.launch.py
+```
diff --git a/src/camera_retarget_l20/USAGE_O6_G20_ZH.md b/src/camera_retarget_l20/USAGE_O6_G20_ZH.md
new file mode 100644
index 0000000..e9212c9
--- /dev/null
+++ b/src/camera_retarget_l20/USAGE_O6_G20_ZH.md
@@ -0,0 +1,227 @@
+# 单摄像头控制左手 O6 + G20 使用说明
+
+本文面向第一次使用本系统的操作者。系统通过一个摄像头识别真人左手，
+同时控制两台左手灵巧手：
+
+- G20：连接 `can0`，接收 20 维关节命令。
+- O6：连接 `can1`，接收 6 维关节命令。
+
+摄像头节点会先提取一套统一的人手特征，再分别转换成适合 O6 和 G20 的
+`0–255` 控制值。两台手不会共用同一个命令话题。
+
+## 1. 当前启动命令
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source .venv/bin/activate
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+
+ros2 launch camera_retarget_l20 teleop_o6_g20_left.launch.py \
+ motor_speed:=250 \
+ ema_alpha:=0.55 \
+ max_step:=24.0 \
+ command_deadband:=3.0
+```
+
+启动成功后，终端应显示 O6、G20 分别连接 `can1`、`can0`，并弹出摄像头
+画面。刚启动时画面显示 `PREVIEW ONLY`，表示只计算预览命令，实体手还不会
+跟随摄像头运动。
+
+> 启动双手 launch 前，不要再单独启动其他 `linker_hand_sdk` 进程，否则同一
+> CAN 设备可能被多个驱动同时访问。
+
+## 2. 正确操作流程
+
+把真人左手完整放进画面，尽量保持掌心朝向摄像头，避免手指互相遮挡。看到
+画面显示 `hand=OK` 后，依次完成 `C → F → P → E`。
+
+### C：完全张开标定
+
+1. 单击一下摄像头窗口，确保键盘输入作用于该窗口。
+2. 真人左手完全张开，五指伸直。
+3. 大拇指自然向外展开，不要贴住食指。
+4. 按一次 `C`，继续保持姿态不动。
+5. 等待画面标定进度完成，或终端出现 `Open calibration complete`。
+
+这一步记录各手指的张开零点、指尖张开距离和侧摆零点。
+
+### F：正常握拳标定
+
+1. 四指完全弯曲，做一个正常握拳动作。
+2. 大拇指也要按照自然握拳方式收拢，不要单独留在外面。
+3. 按一次 `F`，保持姿态不动，直到终端出现
+ `Fist calibration complete`。
+
+这一步记录四指和大拇指的完整弯曲范围。只弯曲四指、大拇指保持伸直，会导致
+后续正常握拳时大拇指闭合不足。
+
+### P：拇指和食指捏合标定
+
+1. 张开中指、无名指和小指，减少遮挡。
+2. 让大拇指指尖与食指指尖真正接触，形成清晰的“OK”手势。
+3. 接触后按一次 `P`，保持稳定，直到终端出现
+ `Pinch calibration complete`。
+
+这一步标定拇指对掌以及指尖接触距离。标定时不是“接近”，而是两个指尖确实
+接触。
+
+每次标定需要稳定保持约 1.5 秒并取得至少 30 帧有效数据，最长等待 8 秒。
+某一步失败时不会覆盖之前的有效结果，重新摆好对应姿态再按一次该键即可。
+
+### E：启用实体手控制
+
+完成 C、F、P 后，把真人左手稳定放在画面中并按 `E`。画面变为红色
+`REAL CONTROL` 后，O6 和 G20 才会开始跟随真人手运动。
+
+启用需要同时满足：
+
+- C、F、P 三项标定均已完成；
+- 摄像头稳定识别到左手；
+- O6 和 G20 两个驱动均在线；
+- 两个命令话题都有订阅者，状态数据均正常。
+
+## 3. 运行期间的按键
+
+| 按键 | 功能 |
+|---|---|
+| `C` | 重新标定完全张开姿态 |
+| `F` | 重新标定正常握拳姿态 |
+| `P` | 重新标定拇指与食指捏合姿态 |
+| `E` | 启用 O6 + G20 实体控制 |
+| `D` | 关闭实体控制，保持最后一个命令位置 |
+| `Q` / `Esc` | 退出摄像头节点 |
+
+开始任何一次重新标定都会自动退出实体控制。标定完成后需要重新按 `E`。
+
+## 4. 安全操作
+
+- 首次按 `E` 前，确保两台灵巧手周围没有人体、线缆或易损物品。
+- 调试时先慢速移动真人手，不要突然快速握拳。
+- 摄像头跟踪丢失超过约 0.35 秒时，系统会向在线设备发送张开姿态并自动解除
+ 实体控制。
+- 任一路驱动状态异常或超时时，系统会发送张开姿态并统一解除控制。
+- 需要立即停止跟随时按 `D`。
+- 正常结束程序时，先让真人手张开，确认两台实体手已经张开，再按 `D`，最后
+ 在启动终端按 `Ctrl+C`。
+- 每次重启摄像头节点后，C、F、P 标定都需要重新执行，不会加载上次标定。
+
+## 5. 没有摄像头窗口时的终端操作
+
+另开一个终端并加载环境：
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source .venv/bin/activate
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+```
+
+然后可以通过服务完成同样的操作：
+
+```bash
+# C：完全张开
+ros2 service call /camera_o6_g20_retarget/calibrate_open \
+ std_srvs/srv/Trigger "{}"
+
+# F：正常握拳
+ros2 service call /camera_o6_g20_retarget/calibrate_fist \
+ std_srvs/srv/Trigger "{}"
+
+# P：拇指与食指指尖接触
+ros2 service call /camera_o6_g20_retarget/calibrate_pinch \
+ std_srvs/srv/Trigger "{}"
+
+# E：启用实体控制
+ros2 service call /camera_o6_g20_retarget/enable \
+ std_srvs/srv/SetBool "{data: true}"
+
+# D：关闭实体控制
+ros2 service call /camera_o6_g20_retarget/enable \
+ std_srvs/srv/SetBool "{data: false}"
+```
+
+服务返回成功只表示“标定采集已经开始”，仍需继续保持姿态，直到状态话题或
+启动终端报告标定完成。
+
+## 6. 状态、预览和控制话题
+
+```text
+/o6/cb_left_hand_control_cmd O6 六维实体命令
+/o6/cb_left_hand_state O6 状态
+/g20/cb_left_hand_control_cmd G20 二十维实体命令
+/g20/cb_left_hand_state G20 状态
+/camera_o6_g20_retarget/o6/target_joint_cmd O6 命令预览
+/camera_o6_g20_retarget/g20/target_joint_cmd G20 命令预览
+/camera_o6_g20_retarget/status 系统状态
+/camera_o6_g20_retarget/debug 映射调试数据
+```
+
+常用检查命令：
+
+```bash
+ros2 topic echo /camera_o6_g20_retarget/status
+ros2 topic echo /camera_o6_g20_retarget/debug
+ros2 topic echo /camera_o6_g20_retarget/o6/target_joint_cmd
+ros2 topic echo /camera_o6_g20_retarget/g20/target_joint_cmd
+```
+
+`target_joint_cmd` 在 `PREVIEW ONLY` 状态下也会发布，因此可以先观察映射值，
+确认合理后再按 `E`。
+
+## 7. 当前启动参数的含义
+
+| 参数 | 当前值 | 作用 |
+|---|---:|---|
+| `motor_speed` | 250 | 灵巧手内部电机速度，数值越大，电机允许的运动速度越高 |
+| `ema_alpha` | 0.55 | 映射平滑系数；增大响应更快，减小更平滑 |
+| `max_step` | 24 | 每一帧单个命令允许变化的最大值 |
+| `command_deadband` | 3 | 小于该范围的变化不发送，用于抑制轻微抖动 |
+
+这些参数只影响响应速度和平滑程度，不改变 C/F/P 标定数据和型号端点配置。
+
+## 8. 常见问题
+
+### 按键没有反应
+
+先用鼠标单击摄像头窗口，再按键。键盘焦点在终端或其他窗口时，摄像头程序
+收不到按键。也可以直接使用第 5 节的 ROS 服务。
+
+### P 标定一直失败
+
+- 确认已经先完成 C 和 F；
+- 大拇指与食指指尖必须真正接触；
+- 掌心尽量正对摄像头，避免指尖被手掌遮挡；
+- 中指、无名指和小指保持张开；
+- 按 P 后保持 1.5 秒以上，不要移动手腕。
+
+### 按 E 后实体手不动
+
+查看启动终端的警告以及状态话题：
+
+```bash
+ros2 topic echo /camera_o6_g20_retarget/status
+```
+
+重点检查是否仍提示 `needs_open_calibration`、`needs_fist_calibration`、
+`needs_pinch_calibration`、`state_missing` 或 `command_subscriber_missing`。
+
+### O6 和 G20 动作存在差异
+
+这是正常现象。O6 只有 6 路命令，没有四指独立侧摆；G20 使用 20 维命令，
+可以映射更多弯曲和侧摆自由度。因此两台手表现应当语义一致，但不会做到每个
+机械关节完全相同。
+
+### 动作仍有轻微抖动
+
+可以先保持当前电机速度，只增加平滑或死区，例如：
+
+```bash
+ros2 launch camera_retarget_l20 teleop_o6_g20_left.launch.py \
+ motor_speed:=250 \
+ ema_alpha:=0.45 \
+ max_step:=20.0 \
+ command_deadband:=4.0
+```
+
+这会更稳定，但会增加少量跟随延迟。
diff --git a/src/camera_retarget_l20/camera_retarget_l20/__init__.py b/src/camera_retarget_l20/camera_retarget_l20/__init__.py
new file mode 100644
index 0000000..71e4d45
--- /dev/null
+++ b/src/camera_retarget_l20/camera_retarget_l20/__init__.py
@@ -0,0 +1,2 @@
+"""Camera-based retargeting bridge for the LinkerHand L20."""
+
diff --git a/src/camera_retarget_l20/camera_retarget_l20/calibration.py b/src/camera_retarget_l20/camera_retarget_l20/calibration.py
new file mode 100644
index 0000000..0d65c25
--- /dev/null
+++ b/src/camera_retarget_l20/camera_retarget_l20/calibration.py
@@ -0,0 +1,236 @@
+"""Robust multi-frame capture for C/F/P camera calibration."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+
+@dataclass(frozen=True)
+class RawHandObservation:
+ """Uncalibrated, model-independent measurements from one camera frame."""
+
+ bends: np.ndarray
+ pinch_distances: np.ndarray
+ splay: np.ndarray
+ splay_confidence: np.ndarray
+ thumb_splay: float
+ thumb_splay_confidence: float
+
+ def __post_init__(self) -> None:
+ bends = np.asarray(self.bends, dtype=float)
+ pinch_distances = np.asarray(self.pinch_distances, dtype=float)
+ splay = np.asarray(self.splay, dtype=float)
+ splay_confidence = np.asarray(self.splay_confidence, dtype=float)
+ if bends.shape != (10,):
+ raise ValueError("bends must contain ten values")
+ if pinch_distances.shape != (4,):
+ raise ValueError("pinch_distances must contain four values")
+ if splay.shape != (4,) or splay_confidence.shape != (4,):
+ raise ValueError("splay arrays must contain four values")
+ values = np.concatenate(
+ [
+ bends,
+ pinch_distances,
+ splay,
+ splay_confidence,
+ np.asarray(
+ [self.thumb_splay, self.thumb_splay_confidence], dtype=float
+ ),
+ ]
+ )
+ if not np.all(np.isfinite(values)):
+ raise ValueError("raw hand observation contains non-finite values")
+ object.__setattr__(self, "bends", bends)
+ object.__setattr__(self, "pinch_distances", pinch_distances)
+ object.__setattr__(self, "splay", splay)
+ object.__setattr__(
+ self, "splay_confidence", np.clip(splay_confidence, 0.0, 1.0)
+ )
+ object.__setattr__(
+ self,
+ "thumb_splay_confidence",
+ float(np.clip(self.thumb_splay_confidence, 0.0, 1.0)),
+ )
+
+
+def _circular_center(values: np.ndarray) -> np.ndarray:
+ reference = values[0]
+ wrapped_delta = np.arctan2(
+ np.sin(values - reference), np.cos(values - reference)
+ )
+ return np.arctan2(
+ np.sin(reference + np.median(wrapped_delta, axis=0)),
+ np.cos(reference + np.median(wrapped_delta, axis=0)),
+ )
+
+
+def median_observation(samples: list[RawHandObservation]) -> RawHandObservation:
+ """Return a coordinate-wise robust center for a stable sample window."""
+
+ if not samples:
+ raise ValueError("at least one calibration observation is required")
+ bends = np.stack([sample.bends for sample in samples])
+ distances = np.stack([sample.pinch_distances for sample in samples])
+ splay = np.stack([sample.splay for sample in samples])
+ confidence = np.stack([sample.splay_confidence for sample in samples])
+ thumb_splay = np.asarray([sample.thumb_splay for sample in samples])[:, None]
+ thumb_confidence = np.asarray(
+ [sample.thumb_splay_confidence for sample in samples]
+ )
+ return RawHandObservation(
+ bends=np.median(bends, axis=0),
+ pinch_distances=np.median(distances, axis=0),
+ splay=_circular_center(splay),
+ splay_confidence=np.median(confidence, axis=0),
+ thumb_splay=float(_circular_center(thumb_splay)[0]),
+ thumb_splay_confidence=float(np.median(thumb_confidence)),
+ )
+
+
+class PoseCalibrationSession:
+ """Collect a rolling stable window and expose progress without blocking ROS."""
+
+ def __init__(
+ self,
+ pose: str,
+ *,
+ started_at: float,
+ duration: float = 1.5,
+ min_samples: int = 30,
+ timeout: float = 8.0,
+ bend_mad_limit: float = 0.08,
+ distance_mad_limit: float = 0.04,
+ splay_mad_limit: float = 0.10,
+ thumb_splay_mad_limit: float = 0.10,
+ ) -> None:
+ if pose not in {"open", "fist", "pinch"}:
+ raise ValueError("pose must be open, fist, or pinch")
+ if duration <= 0.0 or timeout <= duration:
+ raise ValueError("timeout must be greater than positive duration")
+ if min_samples < 2:
+ raise ValueError("min_samples must be at least two")
+ self.pose = pose
+ self.started_at = float(started_at)
+ self.duration = float(duration)
+ self.min_samples = int(min_samples)
+ self.timeout = float(timeout)
+ self.bend_mad_limit = float(bend_mad_limit)
+ self.distance_mad_limit = float(distance_mad_limit)
+ self.splay_mad_limit = float(splay_mad_limit)
+ self.thumb_splay_mad_limit = float(thumb_splay_mad_limit)
+ self.samples: list[tuple[float, RawHandObservation]] = []
+ self.state = "collecting"
+ self.reason: str | None = None
+ self.result: RawHandObservation | None = None
+ self.last_stability: dict[str, float] = {}
+
+ @property
+ def active(self) -> bool:
+ return self.state == "collecting"
+
+ def progress(self, now: float) -> float:
+ if self.state == "complete":
+ return 1.0
+ if not self.samples:
+ return 0.0
+ elapsed = max(0.0, float(now) - self.samples[0][0])
+ return float(
+ min(elapsed / self.duration, len(self.samples) / self.min_samples, 1.0)
+ )
+
+ def _prune(self, now: float) -> None:
+ cutoff = float(now) - self.duration
+ while len(self.samples) > 1 and self.samples[1][0] <= cutoff:
+ self.samples.pop(0)
+
+ @staticmethod
+ def _mad(values: np.ndarray, center: np.ndarray) -> np.ndarray:
+ median_deviation = np.median(np.abs(values - center), axis=0)
+ robust_half_span = 0.5 * (
+ np.quantile(values, 0.90, axis=0)
+ - np.quantile(values, 0.10, axis=0)
+ )
+ return np.maximum(median_deviation, robust_half_span)
+
+ @staticmethod
+ def _circular_mad(values: np.ndarray, center: np.ndarray) -> np.ndarray:
+ delta = np.arctan2(np.sin(values - center), np.cos(values - center))
+ absolute = np.abs(delta)
+ return np.maximum(
+ np.median(absolute, axis=0),
+ np.quantile(absolute, 0.90, axis=0),
+ )
+
+ def _stable_result(self) -> RawHandObservation | None:
+ observations = [sample for _, sample in self.samples]
+ center = median_observation(observations)
+ bends = np.stack([sample.bends for sample in observations])
+ distances = np.stack([sample.pinch_distances for sample in observations])
+ splay = np.stack([sample.splay for sample in observations])
+ thumb_splay = np.asarray(
+ [sample.thumb_splay for sample in observations], dtype=float
+ )
+ bend_mad = float(np.max(self._mad(bends, center.bends)))
+ distance_mad = float(
+ np.max(self._mad(distances, center.pinch_distances))
+ )
+ splay_mad = float(np.max(self._circular_mad(splay, center.splay)))
+ thumb_splay_mad = float(
+ self._circular_mad(thumb_splay, np.asarray(center.thumb_splay))
+ )
+ self.last_stability = {
+ "bend_mad_rad": bend_mad,
+ "distance_mad_palm": distance_mad,
+ "splay_mad_rad": splay_mad,
+ "thumb_splay_mad_rad": thumb_splay_mad,
+ }
+ if bend_mad > self.bend_mad_limit:
+ self.reason = "bend_not_stable"
+ return None
+ if distance_mad > self.distance_mad_limit:
+ self.reason = "fingertip_distance_not_stable"
+ return None
+ if self.pose == "open" and splay_mad > self.splay_mad_limit:
+ self.reason = "finger_splay_not_stable"
+ return None
+ if self.pose in {"open", "pinch"} and thumb_splay_mad > self.thumb_splay_mad_limit:
+ self.reason = "thumb_splay_not_stable"
+ return None
+ if self.pose == "open" and float(np.median(center.splay_confidence)) < 0.25:
+ self.reason = "finger_splay_confidence_low"
+ return None
+ if self.pose in {"open", "pinch"} and center.thumb_splay_confidence < 0.20:
+ self.reason = "thumb_splay_confidence_low"
+ return None
+ self.reason = None
+ return center
+
+ def add(
+ self, observation: RawHandObservation, now: float
+ ) -> RawHandObservation | None:
+ if not self.active:
+ return self.result
+ self.samples.append((float(now), observation))
+ self._prune(now)
+ window_elapsed = float(now) - self.samples[0][0]
+ if len(self.samples) >= self.min_samples and window_elapsed >= self.duration:
+ result = self._stable_result()
+ if result is not None:
+ self.result = result
+ self.state = "complete"
+ return result
+ self.poll(now)
+ return None
+
+ def poll(self, now: float) -> None:
+ if self.active and float(now) - self.started_at >= self.timeout:
+ last_reason = self.reason or "insufficient_stable_samples"
+ self.state = "failed"
+ self.reason = f"calibration_timeout:{last_reason}"
+
+ def fail(self, reason: str) -> None:
+ self.state = "failed"
+ self.reason = str(reason)
+ self.result = None
diff --git a/src/camera_retarget_l20/camera_retarget_l20/g20_mapping.py b/src/camera_retarget_l20/camera_retarget_l20/g20_mapping.py
new file mode 100644
index 0000000..a260749
--- /dev/null
+++ b/src/camera_retarget_l20/camera_retarget_l20/g20_mapping.py
@@ -0,0 +1,1052 @@
+"""Convert G20 URDF joint positions to the LinkerHand 20-value protocol."""
+
+from __future__ import annotations
+
+from collections.abc import Sequence
+from dataclasses import replace
+
+import numpy as np
+
+from .calibration import RawHandObservation
+from .hand_features import HandFeatures
+from .model_profiles import (
+ DirectModelProfile,
+ FlexionChannel,
+ ProfiledCommandMapper,
+)
+
+
+MOTOR_JOINT_NAMES: tuple[str | None, ...] = (
+ "thumb_cmc_pitch",
+ "index_mcp_pitch",
+ "middle_mcp_pitch",
+ "ring_mcp_pitch",
+ "pinky_mcp_pitch",
+ "thumb_cmc_roll",
+ "index_mcp_roll",
+ "middle_mcp_roll",
+ "ring_mcp_roll",
+ "pinky_mcp_roll",
+ "thumb_cmc_yaw",
+ None,
+ None,
+ None,
+ None,
+ "thumb_mcp",
+ "index_pip",
+ "middle_pip",
+ "ring_pip",
+ "pinky_pip",
+)
+
+G20_COMMAND_NAMES: tuple[str, ...] = (
+ "Thumb Base",
+ "Index Finger Base",
+ "Middle Finger Base",
+ "Ring Finger Base",
+ "Pinky Finger Base",
+ "Thumb Abduction",
+ "Index Finger Abduction",
+ "Middle Finger Abduction",
+ "Ring Finger Abduction",
+ "Pinky Finger Abduction",
+ "Thumb Horizontal Abduction",
+ "Reserved 1",
+ "Reserved 2",
+ "Reserved 3",
+ "Reserved 4",
+ "Thumb Tip",
+ "Index Finger Tip",
+ "Middle Finger Tip",
+ "Ring Finger Tip",
+ "Pinky Finger Tip",
+)
+
+FLEXION_MOTOR_INDICES = frozenset((*range(0, 5), *range(15, 20)))
+RESERVED_MOTOR_INDICES = frozenset(range(11, 15))
+
+# Left-G20 endpoints from linkerhand_retarget/config/hand_config.yml. Motor 5
+# follows thumb roll like the right G20; motors 6..9 run in the opposite
+# direction to the flexion motors on a left hand.
+LOWER_LIMIT_COMMAND: tuple[int, ...] = (
+ 255, 255, 255, 255, 255,
+ 255, 0, 0, 0, 0,
+ 255, 255, 255, 255, 255,
+ 255, 255, 255, 255, 255,
+)
+UPPER_LIMIT_COMMAND: tuple[int, ...] = (
+ 0, 0, 0, 0, 0,
+ 0, 255, 255, 255, 255,
+ 0, 255, 255, 255, 255,
+ 0, 0, 0, 0, 0,
+)
+
+# Startup/open pose used by the SDK's G20 driver. Filtering starts here so
+# enabling camera control never jumps directly to the optimizer's first pose.
+DEFAULT_OPEN_POSE: tuple[int, ...] = (
+ 255, 255, 255, 255, 255,
+ 255, 193, 148, 105, 42,
+ 255, 255, 255, 255, 255,
+ 255, 255, 255, 255, 255,
+)
+
+# Measured full-fist pose supplied for this physical left G20. Flexion
+# channels are interpolated independently between DEFAULT_OPEN_POSE and this
+# pose; the other channels remain governed by the opposition/splay mappings
+# below. This avoids imposing one artificial minimum on mechanically
+# different thumb and finger joints.
+DEFAULT_FIST_POSE: tuple[int, ...] = (
+ 50, 0, 0, 0, 0,
+ 78, 193, 158, 128, 91,
+ 112, 255, 255, 255, 255,
+ 144, 0, 0, 0, 0,
+)
+
+# Verified on the physical left G20. Thumb channels 5 and 10 are coupled, but
+# their geometry is not linear across the palm, so use measured per-finger
+# commands instead of dividing the fist travel into equal fractions.
+PINCH_TARGET_OPPOSITION_COMMANDS = np.asarray(
+ (
+ (167.0, 184.0), # index
+ (122.0, 148.0), # middle
+ (78.0, 112.0), # ring
+ (50.0, 90.0), # pinky
+ ),
+ dtype=float,
+)
+POWER_GRASP_THUMB_OPPOSITION_COMMANDS = np.asarray(
+ (78.0, 112.0), dtype=float
+)
+
+# Physically verified left-G20 limits keep both thumb side-motion channels
+# away from their mechanical end stops.
+THUMB_ABDUCTION_MIN_COMMAND = 20.0
+THUMB_YAW_MIN_COMMAND = 80.0
+
+# MediaPipe landmark chains for thumb, index, middle, ring and pinky.
+FINGER_LANDMARKS: tuple[tuple[int, int, int, int], ...] = (
+ (1, 2, 3, 4),
+ (5, 6, 7, 8),
+ (9, 10, 11, 12),
+ (13, 14, 15, 16),
+ (17, 18, 19, 20),
+)
+
+# Human joint travel needed to reach the configured maximum G20 closure. The
+# first five values control root flexion, the final five control tip flexion.
+HUMAN_CLOSE_SPAN = np.asarray(
+ [0.85, 1.05, 1.05, 1.05, 1.05, 0.75, 1.05, 1.05, 1.05, 1.05],
+ dtype=float,
+)
+
+FOUR_FINGER_TIP_INDICES = np.asarray((8, 12, 16, 20), dtype=int)
+PINCH_ACTIVE_THRESHOLD = 0.20
+PINCH_RELEASE_THRESHOLD = 0.15
+PINCH_SWITCH_MARGIN = 0.08
+PINCH_CONFIRM_FRAMES = 3
+# Monocular MediaPipe world landmarks are least reliable along the palm
+# normal. Retain a small depth contribution to reject obvious crossings while
+# allowing visually touching fingertips to calibrate as contact.
+PINCH_PALM_NORMAL_WEIGHT = 0.25
+
+
+def default_g20_profile() -> DirectModelProfile:
+ """Return the source-compatible built-in G20 direct mapping profile."""
+
+ feature_roles = ("thumb", "index", "middle", "ring", "pinky")
+ flexion_channels = tuple(
+ FlexionChannel(command_index, ((feature_index, 1.0),), role)
+ for command_index, feature_index, role in (
+ *(
+ (index, index, feature_roles[index])
+ for index in range(5)
+ ),
+ *(
+ (15 + index, 5 + index, feature_roles[index])
+ for index in range(5)
+ ),
+ )
+ )
+ return DirectModelProfile(
+ model="G20",
+ command_names=G20_COMMAND_NAMES,
+ open_pose=tuple(float(value) for value in DEFAULT_OPEN_POSE),
+ fist_pose=tuple(float(value) for value in DEFAULT_FIST_POSE),
+ flexion_channels=flexion_channels,
+ side_command_indices=(5, 10),
+ pinch_target_side_commands=tuple(
+ tuple(float(value) for value in row)
+ for row in PINCH_TARGET_OPPOSITION_COMMANDS
+ ),
+ power_grasp_side_commands=tuple(
+ float(value) for value in POWER_GRASP_THUMB_OPPOSITION_COMMANDS
+ ),
+ abducted_side_commands=(20.0, 255.0),
+ splay_command_indices=(6, 7, 8, 9),
+ splay_scale=255.0 / 0.46,
+ splay_gain=1.0,
+ max_splay_delta=90.0,
+ fixed_channels=tuple((index, 255.0) for index in range(11, 15)),
+ pinch_thumb_closure=0.85,
+ pinch_finger_closure=0.65,
+ ema_alpha=0.35,
+ max_step=8.0,
+ command_deadband=0.0,
+ motor_speed=60,
+ )
+
+
+class G20CommandMapper:
+ """Map named left-G20 joint radians to safe, filtered motor commands."""
+
+ def __init__(
+ self,
+ joint_names: Sequence[str],
+ joint_limits: Sequence[Sequence[float]],
+ *,
+ ema_alpha: float = 0.35,
+ max_step: float = 8.0,
+ max_closure: float = 0.80,
+ open_pose: Sequence[float] = DEFAULT_OPEN_POSE,
+ ) -> None:
+ if len(joint_names) != len(joint_limits):
+ raise ValueError("joint_names and joint_limits must have equal length")
+ if not 0.0 < ema_alpha <= 1.0:
+ raise ValueError("ema_alpha must be in (0, 1]")
+ if max_step <= 0.0:
+ raise ValueError("max_step must be positive")
+ if not 0.0 <= max_closure <= 1.0:
+ raise ValueError("max_closure must be in [0, 1]")
+ if len(open_pose) != 20:
+ raise ValueError("open_pose must contain 20 values")
+
+ self.joint_names = tuple(joint_names)
+ self.joint_limits = np.asarray(joint_limits, dtype=float)
+ if self.joint_limits.shape != (len(self.joint_names), 2):
+ raise ValueError("joint_limits must have shape (N, 2)")
+
+ name_to_index = {name: index for index, name in enumerate(self.joint_names)}
+ required = {name for name in MOTOR_JOINT_NAMES if name is not None}
+ missing = sorted(required.difference(name_to_index))
+ if missing:
+ raise ValueError(f"G20 URDF is missing controlled joints: {missing}")
+
+ self.motor_to_qpos = tuple(
+ None if name is None else name_to_index[name] for name in MOTOR_JOINT_NAMES
+ )
+ self.ema_alpha = float(ema_alpha)
+ self.max_step = float(max_step)
+ self.max_closure = float(max_closure)
+ self.open_pose = self._validate_command(open_pose)
+ self.last_command = self.open_pose.astype(float)
+
+ @staticmethod
+ def _validate_command(command: Sequence[float]) -> np.ndarray:
+ values = np.asarray(command, dtype=float)
+ if values.shape != (20,):
+ raise ValueError("a G20 command must contain 20 values")
+ if not np.all(np.isfinite(values)):
+ raise ValueError("G20 command contains non-finite values")
+ return np.clip(values, 0.0, 255.0)
+
+ def reset(self, command: Sequence[float] | None = None) -> None:
+ """Reset filtering, normally to the SDK's known startup pose."""
+
+ self.last_command = (
+ self.open_pose.copy()
+ if command is None
+ else self._validate_command(command).astype(float)
+ )
+
+ def raw_command(self, qpos: Sequence[float]) -> np.ndarray:
+ """Return an unfiltered 20-value command with safety limits applied."""
+
+ qpos_array = np.asarray(qpos, dtype=float)
+ if qpos_array.shape != (len(self.joint_names),):
+ raise ValueError(
+ f"qpos must have {len(self.joint_names)} values, got {qpos_array.shape}"
+ )
+ if not np.all(np.isfinite(qpos_array)):
+ raise ValueError("qpos contains non-finite values")
+
+ command = self.open_pose.copy()
+ for motor_index, qpos_index in enumerate(self.motor_to_qpos):
+ if qpos_index is None:
+ continue
+ lower, upper = self.joint_limits[qpos_index]
+ if not np.isfinite(lower) or not np.isfinite(upper) or upper <= lower:
+ raise ValueError(
+ f"invalid limits for {self.joint_names[qpos_index]}: {lower}, {upper}"
+ )
+ ratio = np.clip(
+ (qpos_array[qpos_index] - lower) / (upper - lower), 0.0, 1.0
+ )
+ command[motor_index] = (
+ LOWER_LIMIT_COMMAND[motor_index]
+ + ratio
+ * (
+ UPPER_LIMIT_COMMAND[motor_index]
+ - LOWER_LIMIT_COMMAND[motor_index]
+ )
+ )
+
+ minimum_flexion_command = 255.0 * (1.0 - self.max_closure)
+ for motor_index in FLEXION_MOTOR_INDICES:
+ command[motor_index] = max(
+ command[motor_index], minimum_flexion_command
+ )
+ command[5] = max(command[5], THUMB_ABDUCTION_MIN_COMMAND)
+ command[10] = max(command[10], THUMB_YAW_MIN_COMMAND)
+ command[list(RESERVED_MOTOR_INDICES)] = self.open_pose[
+ list(RESERVED_MOTOR_INDICES)
+ ]
+ return command
+
+ def map(self, qpos: Sequence[float]) -> list[int]:
+ """Map and smooth one retargeting result."""
+
+ raw = self.raw_command(qpos)
+ filtered = self.ema_alpha * raw + (1.0 - self.ema_alpha) * self.last_command
+ delta = np.clip(filtered - self.last_command, -self.max_step, self.max_step)
+ self.last_command = np.clip(self.last_command + delta, 0.0, 255.0)
+ self.last_command[list(RESERVED_MOTOR_INDICES)] = self.open_pose[
+ list(RESERVED_MOTOR_INDICES)
+ ]
+ return np.rint(self.last_command).astype(int).tolist()
+
+
+class G20MediaPipeMapper:
+ """Map MediaPipe's 21 hand landmarks directly to a left G20 command.
+
+ Vector IK is useful for anthropomorphic simulation models, but it can
+ saturate when the robot URDF coordinate convention does not match the
+ camera hand frame. This mapper instead uses rotation-invariant human joint
+ bend angles. The operator explicitly captures a fully-open pose so camera
+ bias and individual hand geometry are removed before motor scaling.
+ """
+
+ def __init__(
+ self,
+ *,
+ ema_alpha: float = 0.35,
+ max_step: float = 8.0,
+ command_deadband: float = 0.0,
+ max_closure: float = 0.80,
+ splay_gain: float = 1.0,
+ max_splay_delta: float = 90.0,
+ thumb_splay_span: float = 0.70,
+ thumb_abduction_min_command: float = 20.0,
+ thumb_pinch_gain: float = 2.0,
+ pinch_thumb_closure: float = 0.85,
+ pinch_finger_closure: float = 0.65,
+ open_pose: Sequence[float] = DEFAULT_OPEN_POSE,
+ fist_pose: Sequence[float] = DEFAULT_FIST_POSE,
+ profile: DirectModelProfile | None = None,
+ ) -> None:
+ if not 0.0 < ema_alpha <= 1.0:
+ raise ValueError("ema_alpha must be in (0, 1]")
+ if max_step <= 0.0:
+ raise ValueError("max_step must be positive")
+ if not 0.0 <= command_deadband < max_step:
+ raise ValueError("command_deadband must be in [0, max_step)")
+ if not 0.0 <= max_closure <= 1.0:
+ raise ValueError("max_closure must be in [0, 1]")
+ if splay_gain < 0.0:
+ raise ValueError("splay_gain must be non-negative")
+ if not 0.0 <= max_splay_delta <= 255.0:
+ raise ValueError("max_splay_delta must be in [0, 255]")
+ if thumb_splay_span <= 0.0:
+ raise ValueError("thumb_splay_span must be positive")
+ if not 0.0 <= thumb_abduction_min_command <= 255.0:
+ raise ValueError("thumb_abduction_min_command must be in [0, 255]")
+ if thumb_pinch_gain < 0.0:
+ raise ValueError("thumb_pinch_gain must be non-negative")
+ if not 0.0 <= pinch_thumb_closure <= 1.0:
+ raise ValueError("pinch_thumb_closure must be in [0, 1]")
+ if not 0.0 <= pinch_finger_closure <= 1.0:
+ raise ValueError("pinch_finger_closure must be in [0, 1]")
+
+ base_profile = profile or default_g20_profile()
+ if base_profile.model != "G20":
+ raise ValueError("G20MediaPipeMapper requires a G20 profile")
+ if profile is None:
+ profile_open = G20CommandMapper._validate_command(open_pose)
+ profile_fist = G20CommandMapper._validate_command(fist_pose)
+ else:
+ profile_open = np.asarray(profile.open_pose, dtype=float)
+ profile_fist = np.asarray(profile.fist_pose, dtype=float)
+ abducted_commands = base_profile.abducted_side_commands
+ if abducted_commands is not None:
+ abducted_commands = (
+ float(thumb_abduction_min_command),
+ *abducted_commands[1:],
+ )
+ base_profile = replace(
+ base_profile,
+ open_pose=tuple(profile_open),
+ fist_pose=tuple(profile_fist),
+ abducted_side_commands=abducted_commands,
+ ).with_runtime_overrides(
+ ema_alpha=ema_alpha,
+ max_step=max_step,
+ command_deadband=command_deadband,
+ splay_gain=splay_gain,
+ max_splay_delta=max_splay_delta,
+ pinch_thumb_closure=pinch_thumb_closure,
+ pinch_finger_closure=pinch_finger_closure,
+ )
+ self.profile = base_profile
+ self.ema_alpha = base_profile.ema_alpha
+ self.max_step = base_profile.max_step
+ self.command_deadband = base_profile.command_deadband
+ self.max_closure = float(max_closure)
+ self.splay_gain = base_profile.splay_gain
+ self.max_splay_delta = base_profile.max_splay_delta
+ self.thumb_splay_span = float(thumb_splay_span)
+ self.thumb_abduction_min_command = float(thumb_abduction_min_command)
+ self.thumb_pinch_gain = float(thumb_pinch_gain)
+ self.pinch_thumb_closure = base_profile.pinch_thumb_closure
+ self.pinch_finger_closure = base_profile.pinch_finger_closure
+ self._profile_mapper = ProfiledCommandMapper(
+ base_profile, max_closure=self.max_closure
+ )
+ self.open_pose = self._profile_mapper.open_pose
+ self.fist_pose = self._profile_mapper.fist_pose
+ self.last_command = self.open_pose.copy()
+ self.open_bends: np.ndarray | None = None
+ self.fist_bends: np.ndarray | None = None
+ self.close_spans: np.ndarray | None = None
+ self.open_pinch_distance: float | None = None
+ self.open_pinch_distances: np.ndarray | None = None
+ self.fist_pinch_distance: float | None = None
+ self.fist_thumb_splay: float | None = None
+ self.pinch_pinch_distance: float | None = None
+ self.pinch_bends: np.ndarray | None = None
+ self.pinch_thumb_splay: float | None = None
+ self.open_splay: np.ndarray | None = None
+ self.open_thumb_splay: float | None = None
+ self.last_closure = np.zeros(10, dtype=float)
+ self.last_pinch = 0.0
+ self.last_pinch_strengths = np.zeros(4, dtype=float)
+ self.last_pinch_target = -1
+ self.last_splay_delta = np.zeros(4, dtype=float)
+ self.last_splay_confidence = np.ones(4, dtype=float)
+ self.last_thumb_splay_delta = 0.0
+ self.last_thumb_abduction = 0.0
+ self.last_thumb_opposition = 0.0
+ self.last_power_grasp = 0.0
+ self.last_bends = np.zeros(10, dtype=float)
+ self.last_pinch_distances = np.zeros(4, dtype=float)
+ self.last_raw_command = self.open_pose.copy()
+ self.last_pinch_blend = 0.0
+ self._selected_pinch_target = -1
+ self._pending_pinch_target = -1
+ self._pending_pinch_frames = 0
+
+ @property
+ def is_calibrated(self) -> bool:
+ return (
+ self.open_bends is not None
+ and self.open_pinch_distances is not None
+ and self.open_splay is not None
+ and self.open_thumb_splay is not None
+ )
+
+ @property
+ def is_fist_calibrated(self) -> bool:
+ return (
+ self.fist_bends is not None
+ and self.close_spans is not None
+ and self.fist_pinch_distance is not None
+ and self.fist_thumb_splay is not None
+ )
+
+ @property
+ def is_pinch_calibrated(self) -> bool:
+ return (
+ self.pinch_pinch_distance is not None
+ and self.pinch_bends is not None
+ and self.pinch_thumb_splay is not None
+ )
+
+ @property
+ def is_three_pose_calibrated(self) -> bool:
+ return (
+ self.is_calibrated
+ and self.is_fist_calibrated
+ and self.is_pinch_calibrated
+ )
+
+ @staticmethod
+ def _validate_landmarks(landmarks: Sequence[Sequence[float]]) -> np.ndarray:
+ points = np.asarray(landmarks, dtype=float)
+ if points.shape != (21, 3):
+ raise ValueError(f"MediaPipe landmarks must have shape (21, 3), got {points.shape}")
+ if not np.all(np.isfinite(points)):
+ raise ValueError("MediaPipe landmarks contain non-finite values")
+ return points
+
+ @staticmethod
+ def _joint_bend(a: np.ndarray, b: np.ndarray, c: np.ndarray) -> float:
+ """Return zero for a straight joint and pi for a fully folded joint."""
+
+ first = a - b
+ second = c - b
+ denominator = np.linalg.norm(first) * np.linalg.norm(second)
+ if denominator < 1e-10:
+ raise ValueError("degenerate MediaPipe finger segment")
+ cosine = float(np.clip(np.dot(first, second) / denominator, -1.0, 1.0))
+ return float(np.pi - np.arccos(cosine))
+
+ @classmethod
+ def human_features(
+ cls, landmarks: Sequence[Sequence[float]]
+ ) -> tuple[np.ndarray, np.ndarray]:
+ """Return ten flexion angles and four normalized thumb-tip distances."""
+
+ points = cls._validate_landmarks(landmarks)
+ root_bends: list[float] = []
+ tip_bends: list[float] = []
+
+ for finger_index, (mcp, pip, dip, tip) in enumerate(FINGER_LANDMARKS):
+ if finger_index == 0:
+ root_bend = cls._joint_bend(points[0], points[mcp], points[pip])
+ mcp_bend = cls._joint_bend(points[mcp], points[pip], points[dip])
+ ip_bend = cls._joint_bend(points[pip], points[dip], points[tip])
+ tip_bend = 0.60 * mcp_bend + 0.40 * ip_bend
+ else:
+ root_bend = cls._joint_bend(points[0], points[mcp], points[pip])
+ pip_bend = cls._joint_bend(points[mcp], points[pip], points[dip])
+ dip_bend = cls._joint_bend(points[pip], points[dip], points[tip])
+ tip_bend = 0.65 * pip_bend + 0.35 * dip_bend
+ root_bends.append(root_bend)
+ tip_bends.append(tip_bend)
+
+ palm_width = float(np.linalg.norm(points[5] - points[17]))
+ if palm_width < 1e-8:
+ raise ValueError("degenerate MediaPipe palm width")
+ palm_forward = points[9] - points[0]
+ palm_forward /= max(float(np.linalg.norm(palm_forward)), 1e-8)
+ palm_lateral = points[5] - points[17]
+ palm_lateral -= np.dot(palm_lateral, palm_forward) * palm_forward
+ palm_lateral /= max(float(np.linalg.norm(palm_lateral)), 1e-8)
+ palm_normal = np.cross(palm_forward, palm_lateral)
+ palm_normal /= max(float(np.linalg.norm(palm_normal)), 1e-8)
+ pinch_vectors = points[4] - points[FOUR_FINGER_TIP_INDICES]
+ forward_distance = pinch_vectors @ palm_forward
+ lateral_distance = pinch_vectors @ palm_lateral
+ normal_distance = pinch_vectors @ palm_normal
+ pinch_distances = np.sqrt(
+ forward_distance**2
+ + lateral_distance**2
+ + (PINCH_PALM_NORMAL_WEIGHT * normal_distance) ** 2
+ ) / palm_width
+ return (
+ np.asarray(root_bends + tip_bends, dtype=float),
+ np.asarray(pinch_distances, dtype=float),
+ )
+
+ @classmethod
+ def human_splay_features(
+ cls, landmarks: Sequence[Sequence[float]]
+ ) -> tuple[np.ndarray, np.ndarray]:
+ """Return signed four-finger splay angles and planar confidence."""
+
+ points = cls._validate_landmarks(landmarks)
+ forward = points[9] - points[0]
+ forward_norm = np.linalg.norm(forward)
+ if forward_norm < 1e-8:
+ raise ValueError("degenerate MediaPipe palm length")
+ forward /= forward_norm
+
+ lateral = points[5] - points[17]
+ lateral -= np.dot(lateral, forward) * forward
+ lateral_norm = np.linalg.norm(lateral)
+ if lateral_norm < 1e-8:
+ raise ValueError("degenerate MediaPipe palm lateral axis")
+ lateral /= lateral_norm
+
+ angles: list[float] = []
+ confidence: list[float] = []
+ for mcp, pip, _, _ in FINGER_LANDMARKS[1:]:
+ direction = points[pip] - points[mcp]
+ norm = np.linalg.norm(direction)
+ if norm < 1e-8:
+ raise ValueError("degenerate MediaPipe proximal segment")
+ direction /= norm
+ longitudinal = float(np.dot(direction, forward))
+ sideways = float(np.dot(direction, lateral))
+ angles.append(float(np.arctan2(sideways, longitudinal)))
+ confidence.append(float(np.hypot(longitudinal, sideways)))
+ return np.asarray(angles), np.asarray(confidence)
+
+ @classmethod
+ def human_thumb_splay_feature(
+ cls, landmarks: Sequence[Sequence[float]]
+ ) -> tuple[float, float]:
+ """Return thumb in-palm side angle and its planar confidence."""
+
+ points = cls._validate_landmarks(landmarks)
+ forward = points[9] - points[0]
+ forward /= max(np.linalg.norm(forward), 1e-8)
+ lateral = points[5] - points[17]
+ lateral -= np.dot(lateral, forward) * forward
+ lateral /= max(np.linalg.norm(lateral), 1e-8)
+
+ direction = points[2] - points[1]
+ direction /= max(np.linalg.norm(direction), 1e-8)
+ longitudinal = float(np.dot(direction, forward))
+ sideways = float(np.dot(direction, lateral))
+ angle = float(np.arctan2(sideways, longitudinal))
+ confidence = float(np.hypot(longitudinal, sideways))
+ return angle, confidence
+
+ @classmethod
+ def observe_landmarks(
+ cls, landmarks: Sequence[Sequence[float]]
+ ) -> RawHandObservation:
+ """Extract all raw quantities needed by robust C/F/P calibration."""
+
+ bends, pinch_distances = cls.human_features(landmarks)
+ splay, splay_confidence = cls.human_splay_features(landmarks)
+ thumb_splay, thumb_splay_confidence = cls.human_thumb_splay_feature(
+ landmarks
+ )
+ return RawHandObservation(
+ bends=bends,
+ pinch_distances=pinch_distances,
+ splay=splay,
+ splay_confidence=splay_confidence,
+ thumb_splay=thumb_splay,
+ thumb_splay_confidence=thumb_splay_confidence,
+ )
+
+ def calibrate_open(
+ self, landmarks: Sequence[Sequence[float]]
+ ) -> dict[str, float]:
+ """Capture the current fully-open human hand as the mapping zero pose."""
+
+ return self.calibrate_open_observation(self.observe_landmarks(landmarks))
+
+ def calibrate_open_observation(
+ self, observation: RawHandObservation
+ ) -> dict[str, float]:
+ """Apply a robustly aggregated fully-open observation."""
+
+ self.open_bends = observation.bends.copy()
+ self.fist_bends = None
+ self.close_spans = None
+ self.open_pinch_distances = np.maximum(
+ observation.pinch_distances, 0.35
+ )
+ self.open_pinch_distance = float(self.open_pinch_distances[0])
+ self.fist_pinch_distance = None
+ self.fist_thumb_splay = None
+ self.pinch_pinch_distance = None
+ self.pinch_bends = None
+ self.pinch_thumb_splay = None
+ self.open_splay = observation.splay.copy()
+ self.open_thumb_splay = float(observation.thumb_splay)
+ self.last_closure.fill(0.0)
+ self.last_pinch = 0.0
+ self.last_pinch_strengths.fill(0.0)
+ self.last_pinch_target = -1
+ self.last_splay_delta.fill(0.0)
+ self.last_splay_confidence = observation.splay_confidence.copy()
+ self.last_thumb_splay_delta = 0.0
+ self.last_thumb_abduction = 0.0
+ self.last_thumb_opposition = 0.0
+ self.last_power_grasp = 0.0
+ self._reset_pinch_selector()
+ self.reset()
+ return {
+ "max_open_bend_rad": float(np.max(observation.bends)),
+ "open_pinch_ratio": float(self.open_pinch_distance),
+ }
+
+ def calibrate_fist(
+ self, landmarks: Sequence[Sequence[float]]
+ ) -> dict[str, float]:
+ """Capture a fully-closed hand and derive user-specific bend spans."""
+
+ return self.calibrate_fist_observation(self.observe_landmarks(landmarks))
+
+ def calibrate_fist_observation(
+ self, observation: RawHandObservation
+ ) -> dict[str, float]:
+ """Apply a robustly aggregated fist observation after validation."""
+
+ if not self.is_calibrated:
+ raise RuntimeError("open-hand calibration is required before fist calibration")
+ delta = observation.bends - self.open_bends
+ four_finger_indices = np.asarray((1, 2, 3, 4, 6, 7, 8, 9))
+ mean_four_finger_delta = float(np.mean(delta[four_finger_indices]))
+ if mean_four_finger_delta < 0.45:
+ raise ValueError(
+ "fist calibration rejected: curl the index through pinky fingers more"
+ )
+ self.fist_bends = observation.bends.copy()
+ self.close_spans = np.maximum(delta, 0.35)
+ self.fist_pinch_distance = float(observation.pinch_distances[0])
+ self.fist_thumb_splay = float(observation.thumb_splay)
+ self._reset_pinch_selector()
+ self.reset()
+ return {
+ "mean_four_finger_delta_rad": mean_four_finger_delta,
+ "min_close_span_rad": float(np.min(self.close_spans)),
+ "fist_pinch_ratio": self.fist_pinch_distance,
+ }
+
+ def calibrate_pinch(
+ self, landmarks: Sequence[Sequence[float]]
+ ) -> dict[str, float]:
+ """Capture thumb/index contact as the full opposition endpoint."""
+
+ return self.calibrate_pinch_observation(self.observe_landmarks(landmarks))
+
+ def calibrate_pinch_observation(
+ self, observation: RawHandObservation
+ ) -> dict[str, float]:
+ """Apply a robustly aggregated thumb/index contact observation."""
+
+ if not self.is_calibrated:
+ raise RuntimeError(
+ "open-hand calibration is required before pinch calibration"
+ )
+ pinch_distance = float(observation.pinch_distances[0])
+ reduction = float(self.open_pinch_distance - pinch_distance)
+ reduction_ratio = reduction / max(float(self.open_pinch_distance), 1e-6)
+ if reduction_ratio < 0.60 or pinch_distance > 0.35:
+ raise ValueError(
+ "pinch calibration rejected: touch the thumb and index "
+ "fingertips "
+ f"(measured reduction={reduction_ratio:.0%}, "
+ f"distance={pinch_distance:.3f} palm widths; "
+ "required reduction>=60% and distance<=0.35)"
+ )
+ self.pinch_pinch_distance = float(pinch_distance)
+ self.pinch_bends = observation.bends.copy()
+ self.pinch_thumb_splay = float(observation.thumb_splay)
+ self._reset_pinch_selector()
+ return {
+ "pinch_ratio": self.pinch_pinch_distance,
+ "pinch_reduction": reduction,
+ "pinch_reduction_ratio": reduction_ratio,
+ }
+
+ def _reset_pinch_selector(self) -> None:
+ self._selected_pinch_target = -1
+ self._pending_pinch_target = -1
+ self._pending_pinch_frames = 0
+
+ def _select_pinch_target(
+ self, pinch_distances: np.ndarray, strengths: np.ndarray
+ ) -> int:
+ """Select a fingertip using confirmation, switch margin and release hysteresis."""
+
+ candidate = int(np.argmin(pinch_distances))
+ candidate_strength = float(strengths[candidate])
+ current = self._selected_pinch_target
+
+ if current >= 0:
+ current_strength = float(strengths[current])
+ if current_strength < PINCH_RELEASE_THRESHOLD and candidate_strength < PINCH_RELEASE_THRESHOLD:
+ self._reset_pinch_selector()
+ return -1
+ if (
+ candidate != current
+ and candidate_strength >= PINCH_ACTIVE_THRESHOLD
+ and candidate_strength >= current_strength + PINCH_SWITCH_MARGIN
+ ):
+ if candidate == self._pending_pinch_target:
+ self._pending_pinch_frames += 1
+ else:
+ self._pending_pinch_target = candidate
+ self._pending_pinch_frames = 1
+ if self._pending_pinch_frames >= PINCH_CONFIRM_FRAMES:
+ self._selected_pinch_target = candidate
+ self._pending_pinch_target = -1
+ self._pending_pinch_frames = 0
+ else:
+ self._pending_pinch_target = -1
+ self._pending_pinch_frames = 0
+ return self._selected_pinch_target
+
+ if candidate_strength < PINCH_ACTIVE_THRESHOLD:
+ self._pending_pinch_target = -1
+ self._pending_pinch_frames = 0
+ return -1
+ if candidate == self._pending_pinch_target:
+ self._pending_pinch_frames += 1
+ else:
+ self._pending_pinch_target = candidate
+ self._pending_pinch_frames = 1
+ if self._pending_pinch_frames >= PINCH_CONFIRM_FRAMES:
+ self._selected_pinch_target = candidate
+ self._pending_pinch_target = -1
+ self._pending_pinch_frames = 0
+ return self._selected_pinch_target
+
+ def reset(self, command: Sequence[float] | None = None) -> None:
+ self.last_command = (
+ self.open_pose.copy()
+ if command is None
+ else G20CommandMapper._validate_command(command).astype(float)
+ )
+ self._profile_mapper.reset(self.last_command)
+ self.last_raw_command = self.last_command.copy()
+ self.last_pinch_blend = 0.0
+
+ def extract_features(
+ self, landmarks: Sequence[Sequence[float]]
+ ) -> HandFeatures:
+ """Extract the calibrated, model-independent human hand intent."""
+
+ if not self.is_calibrated:
+ raise RuntimeError("open-hand calibration is required")
+
+ observation = self.observe_landmarks(landmarks)
+ bends = observation.bends
+ pinch_distances = observation.pinch_distances
+ splay = observation.splay
+ splay_confidence = observation.splay_confidence
+ thumb_splay = observation.thumb_splay
+ thumb_splay_confidence = observation.thumb_splay_confidence
+ self.last_bends = bends.copy()
+ self.last_pinch_distances = pinch_distances.copy()
+ close_spans = self.close_spans if self.close_spans is not None else HUMAN_CLOSE_SPAN
+ closure = np.clip((bends - self.open_bends) / close_spans, 0.0, 1.0)
+
+ pinch_target = (
+ self.pinch_pinch_distance
+ if self.pinch_pinch_distance is not None
+ else 0.15
+ )
+ pinch_denominators = np.maximum(
+ self.open_pinch_distances - pinch_target, 0.10
+ )
+ candidate_strengths = np.clip(
+ (self.open_pinch_distances - pinch_distances) / pinch_denominators,
+ 0.0,
+ 1.0,
+ )
+ # The closest fingertip identifies intent, while temporal hysteresis
+ # prevents noisy transitions between adjacent fingers.
+ pinch_target_index = self._select_pinch_target(
+ pinch_distances, candidate_strengths
+ )
+ pinch_strengths = np.zeros(4, dtype=float)
+ if pinch_target_index >= 0:
+ pinch = float(candidate_strengths[pinch_target_index])
+ pinch_strengths[pinch_target_index] = pinch
+ else:
+ pinch = 0.0
+ pinch_gain = 1.0 if self.is_pinch_calibrated else self.thumb_pinch_gain
+ pinch_control = float(np.clip(pinch * pinch_gain, 0.0, 1.0))
+ thumb_splay_delta = float(
+ np.arctan2(
+ np.sin(thumb_splay - self.open_thumb_splay),
+ np.cos(thumb_splay - self.open_thumb_splay),
+ )
+ )
+ thumb_confidence_weight = float(
+ np.clip((thumb_splay_confidence - 0.20) / 0.60, 0.0, 1.0)
+ )
+ legacy_geometry_abduction = float(
+ np.clip(-thumb_splay_delta / self.thumb_splay_span, 0.0, 1.0)
+ * thumb_confidence_weight
+ )
+ # In the C/F/P workflow C already captures the fully spread thumb.
+ # There is no separate outward endpoint, so keep outward abduction at
+ # the robot's open pose and use P for the inward/opposition motion.
+ # Before P exists, retain the old one-pose behavior for single-G20
+ # launches that intentionally calibrate only C.
+ thumb_abduction = (
+ 0.0 if self.is_pinch_calibrated else legacy_geometry_abduction
+ )
+ four_finger_grasp = min(
+ float(np.mean(closure[1:5])),
+ float(np.mean(closure[6:10])),
+ )
+ power_grasp = float(
+ np.clip((four_finger_grasp - 0.55) / 0.35, 0.0, 1.0)
+ )
+ # Keep thumb motion independent from the other four fingers. F stores
+ # the operator's real thumb side-swing endpoint, so a normal fist still
+ # reaches the robot's fist-side command while a four-finger-only grasp
+ # leaves the robot thumb open. Do not attenuate this calibrated ratio by
+ # planar confidence: a curled thumb naturally leaves the palm plane and
+ # would otherwise fail to reach its own F endpoint.
+ thumb_fist_opposition = 0.0
+ if self.is_fist_calibrated:
+ fist_thumb_span = float(
+ np.arctan2(
+ np.sin(self.fist_thumb_splay - self.open_thumb_splay),
+ np.cos(self.fist_thumb_splay - self.open_thumb_splay),
+ )
+ )
+ if abs(fist_thumb_span) >= 0.08:
+ thumb_fist_opposition = float(
+ np.clip(thumb_splay_delta / fist_thumb_span, 0.0, 1.0)
+ )
+
+ # Thumb engagement is derived only from the thumb's own calibrated
+ # flexion/opposition signals. The four-finger grasp gates the F-pose
+ # completion but cannot activate the thumb by itself.
+ thumb_flexion = float(
+ np.clip(0.4 * closure[0] + 0.6 * closure[5], 0.0, 1.0)
+ )
+ if self.is_fist_calibrated:
+ thumb_indices = np.asarray((0, 5))
+ thumb_spans = (
+ self.fist_bends[thumb_indices] - self.open_bends[thumb_indices]
+ )
+ valid_thumb_spans = thumb_spans > 0.08
+ if np.any(valid_thumb_spans):
+ calibrated_thumb = np.clip(
+ (
+ bends[thumb_indices][valid_thumb_spans]
+ - self.open_bends[thumb_indices][valid_thumb_spans]
+ )
+ / thumb_spans[valid_thumb_spans],
+ 0.0,
+ 1.0,
+ )
+ thumb_weights = np.asarray((0.4, 0.6))[valid_thumb_spans]
+ thumb_flexion = max(
+ thumb_flexion,
+ float(np.average(calibrated_thumb, weights=thumb_weights)),
+ )
+ thumb_grasp_intent = float(
+ np.clip(
+ power_grasp * max(thumb_fist_opposition, thumb_flexion),
+ 0.0,
+ 1.0,
+ )
+ )
+
+ self.last_closure = closure
+ self.last_pinch = float(pinch)
+ self.last_pinch_strengths = pinch_strengths
+ self.last_pinch_target = pinch_target_index
+ # Keep the old grasp coupling only for legacy single-G20 use. In the
+ # three-pose workflow P exclusively controls thumb opposition, while
+ # C remains the maximum outward/open endpoint.
+ if not self.is_pinch_calibrated:
+ thumb_abduction = float(
+ np.clip(
+ max(
+ legacy_geometry_abduction,
+ 0.85 * closure[0],
+ 0.75 * pinch_control,
+ ),
+ 0.0,
+ 1.0,
+ )
+ )
+ if self.is_pinch_calibrated:
+ thumb_opposition = max(thumb_fist_opposition, thumb_grasp_intent)
+ else:
+ thumb_opposition = float(
+ np.clip(
+ max(
+ pinch_control,
+ 0.75 * closure[5],
+ 0.75 * legacy_geometry_abduction,
+ ),
+ 0.0,
+ 1.0,
+ )
+ )
+ self.last_thumb_splay_delta = thumb_splay_delta
+ self.last_thumb_abduction = thumb_abduction
+ self.last_thumb_opposition = thumb_opposition
+ self.last_power_grasp = power_grasp
+
+ # Map four-finger side-to-side motion around the calibrated natural
+ # spread. As a finger curls out of the palm plane its splay estimate
+ # becomes ill-conditioned, so confidence smoothly returns it to the
+ # calibrated startup value instead of amplifying monocular noise.
+ splay_delta = np.arctan2(
+ np.sin(splay - self.open_splay),
+ np.cos(splay - self.open_splay),
+ )
+ confidence_weight = np.clip((splay_confidence - 0.20) / 0.60, 0.0, 1.0)
+ splay_delta *= confidence_weight
+ self.last_splay_delta = splay_delta
+ self.last_splay_confidence = splay_confidence
+ return HandFeatures(
+ closure=closure,
+ pinch=float(pinch),
+ pinch_strengths=pinch_strengths,
+ splay_delta=splay_delta,
+ splay_confidence=splay_confidence,
+ thumb_splay_delta=thumb_splay_delta,
+ thumb_abduction=thumb_abduction,
+ thumb_opposition=thumb_opposition,
+ power_grasp=power_grasp,
+ )
+
+ def raw_command_from_features(self, features: HandFeatures) -> np.ndarray:
+ """Map shared human intent to the left G20 protocol."""
+
+ if self.is_pinch_calibrated:
+ command = self._profile_mapper.raw_command(features)
+ self.last_raw_command = command.copy()
+ self.last_pinch_blend = self._profile_mapper.last_pinch_blend
+ return command
+
+ closure = np.asarray(features.closure, dtype=float).copy()
+
+ # Each actuator has a different measured full-fist endpoint. Apply
+ # max_closure to normalized human intent, then interpolate each motor
+ # independently instead of clamping both thumb motors at 90.
+ flexion_activation = np.clip(
+ closure * self.max_closure, 0.0, 1.0
+ )
+ flexion_commands = self.open_pose[
+ [*range(0, 5), *range(15, 20)]
+ ] + flexion_activation * (
+ self.fist_pose[[*range(0, 5), *range(15, 20)]]
+ - self.open_pose[[*range(0, 5), *range(15, 20)]]
+ )
+
+ command = self.open_pose.copy()
+ command[0:5] = flexion_commands[0:5]
+ command[15:20] = flexion_commands[5:10]
+ command[5] = self.open_pose[5] + features.thumb_abduction * (
+ self.thumb_abduction_min_command - self.open_pose[5]
+ )
+ command_delta = self.splay_gain * features.splay_delta * (255.0 / 0.46)
+ command_delta = np.clip(
+ command_delta, -self.max_splay_delta, self.max_splay_delta
+ )
+ command[6:10] = np.clip(
+ self.open_pose[6:10] + command_delta, 0.0, 255.0
+ )
+ command[10] = self.open_pose[10] + features.thumb_opposition * (
+ THUMB_YAW_MIN_COMMAND - self.open_pose[10]
+ )
+ command[list(RESERVED_MOTOR_INDICES)] = self.open_pose[
+ list(RESERVED_MOTOR_INDICES)
+ ]
+ self.last_raw_command = np.clip(command, 0.0, 255.0)
+ self.last_pinch_blend = 0.0
+ return self.last_raw_command.copy()
+
+ def raw_command(self, landmarks: Sequence[Sequence[float]]) -> np.ndarray:
+ return self.raw_command_from_features(self.extract_features(landmarks))
+
+ def map_features(self, features: HandFeatures) -> list[int]:
+ raw = self.raw_command_from_features(features)
+ filtered = self.ema_alpha * raw + (1.0 - self.ema_alpha) * self.last_command
+ delta = np.clip(filtered - self.last_command, -self.max_step, self.max_step)
+ delta[np.abs(delta) <= self.command_deadband] = 0.0
+ self.last_command = np.clip(self.last_command + delta, 0.0, 255.0)
+ for index, value in self.profile.fixed_channels:
+ self.last_command[index] = value
+ self._profile_mapper.last_command = self.last_command.copy()
+ return np.rint(self.last_command).astype(int).tolist()
+
+ def map_landmarks(self, landmarks: Sequence[Sequence[float]]) -> list[int]:
+ return self.map_features(self.extract_features(landmarks))
diff --git a/src/camera_retarget_l20/camera_retarget_l20/hand_detector.py b/src/camera_retarget_l20/camera_retarget_l20/hand_detector.py
new file mode 100644
index 0000000..b8c28d1
--- /dev/null
+++ b/src/camera_retarget_l20/camera_retarget_l20/hand_detector.py
@@ -0,0 +1,120 @@
+"""MediaPipe single-hand detector using the coordinate convention of dex-retargeting.
+
+The coordinate conversion follows the MIT-licensed dex-retargeting webcam
+example in /home/lxp/projects/dex-retargeting/example/vector_retargeting.
+"""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+from typing import Any
+
+import cv2
+import mediapipe as mp
+import numpy as np
+
+
+OPERATOR2MANO_LEFT = np.array(
+ [
+ [0.0, 0.0, -1.0],
+ [1.0, 0.0, 0.0],
+ [0.0, -1.0, 0.0],
+ ]
+)
+
+
+@dataclass(frozen=True)
+class Detection:
+ joint_positions: np.ndarray
+ image_landmarks: Any
+ detected_label: str
+
+
+class MediaPipeLeftHandDetector:
+ """Track one physical left hand and return wrist-aligned 3-D landmarks."""
+
+ def __init__(
+ self,
+ *,
+ selfie: bool = False,
+ strict_handedness: bool = True,
+ min_detection_confidence: float = 0.70,
+ min_tracking_confidence: float = 0.70,
+ ) -> None:
+ self.selfie = bool(selfie)
+ self.strict_handedness = bool(strict_handedness)
+ # MediaPipe Hands assumes a mirrored/selfie image when assigning labels.
+ self.expected_label = "Left" if self.selfie else "Right"
+ self.hands = mp.solutions.hands.Hands(
+ static_image_mode=False,
+ max_num_hands=1,
+ model_complexity=1,
+ min_detection_confidence=float(min_detection_confidence),
+ min_tracking_confidence=float(min_tracking_confidence),
+ )
+
+ def close(self) -> None:
+ self.hands.close()
+
+ def detect(self, bgr_image: np.ndarray) -> Detection | None:
+ image = cv2.flip(bgr_image, 1) if self.selfie else bgr_image
+ rgb = cv2.cvtColor(image, cv2.COLOR_BGR2RGB)
+ rgb.flags.writeable = False
+ results = self.hands.process(rgb)
+ if not results.multi_hand_landmarks or not results.multi_hand_world_landmarks:
+ return None
+
+ selected = 0
+ label = results.multi_handedness[selected].classification[0].label
+ if self.strict_handedness and label != self.expected_label:
+ return None
+
+ world_landmarks = results.multi_hand_world_landmarks[selected]
+ points = np.array(
+ [[landmark.x, landmark.y, landmark.z] for landmark in world_landmarks.landmark],
+ dtype=float,
+ )
+ if points.shape != (21, 3):
+ return None
+
+ points -= points[0:1, :]
+ wrist_frame = self.estimate_wrist_frame(points)
+ joint_positions = points @ wrist_frame @ OPERATOR2MANO_LEFT
+ if not np.all(np.isfinite(joint_positions)):
+ return None
+ return Detection(
+ joint_positions=joint_positions,
+ image_landmarks=results.multi_hand_landmarks[selected],
+ detected_label=label,
+ )
+
+ @staticmethod
+ def estimate_wrist_frame(points: np.ndarray) -> np.ndarray:
+ if points.shape != (21, 3):
+ raise ValueError("MediaPipe hand points must have shape (21, 3)")
+ palm_points = points[[0, 5, 9], :]
+ x_vector = palm_points[0] - palm_points[2]
+ centered = palm_points - np.mean(palm_points, axis=0, keepdims=True)
+ _, _, vh = np.linalg.svd(centered)
+ normal = vh[2, :]
+ x_axis = x_vector - np.dot(x_vector, normal) * normal
+ x_norm = np.linalg.norm(x_axis)
+ if x_norm < 1e-8:
+ raise ValueError("degenerate wrist frame")
+ x_axis /= x_norm
+ z_axis = np.cross(x_axis, normal)
+ if np.dot(z_axis, centered[1] - centered[2]) < 0:
+ normal *= -1.0
+ z_axis *= -1.0
+ return np.stack([x_axis, normal, z_axis], axis=1)
+
+ @staticmethod
+ def draw_landmarks(image: np.ndarray, landmarks: Any) -> None:
+ mp.solutions.drawing_utils.draw_landmarks(
+ image,
+ landmarks,
+ mp.solutions.hands.HAND_CONNECTIONS,
+ mp.solutions.drawing_styles.get_default_hand_landmarks_style(),
+ mp.solutions.drawing_styles.get_default_hand_connections_style(),
+ )
+
diff --git a/src/camera_retarget_l20/camera_retarget_l20/hand_features.py b/src/camera_retarget_l20/camera_retarget_l20/hand_features.py
new file mode 100644
index 0000000..59b3766
--- /dev/null
+++ b/src/camera_retarget_l20/camera_retarget_l20/hand_features.py
@@ -0,0 +1,68 @@
+"""Model-independent features extracted from one calibrated human hand."""
+
+from __future__ import annotations
+
+from dataclasses import dataclass
+
+import numpy as np
+
+
+@dataclass(frozen=True)
+class HandFeatures:
+ """Normalized intent shared by robot hands with different joint counts."""
+
+ closure: np.ndarray
+ pinch: float
+ pinch_strengths: np.ndarray
+ splay_delta: np.ndarray
+ splay_confidence: np.ndarray
+ thumb_splay_delta: float
+ thumb_abduction: float
+ thumb_opposition: float
+ power_grasp: float
+
+ def __post_init__(self) -> None:
+ closure = np.asarray(self.closure, dtype=float)
+ pinch_strengths = np.asarray(self.pinch_strengths, dtype=float)
+ splay_delta = np.asarray(self.splay_delta, dtype=float)
+ splay_confidence = np.asarray(self.splay_confidence, dtype=float)
+ if closure.shape != (10,):
+ raise ValueError("closure must contain ten root/tip values")
+ if pinch_strengths.shape != (4,):
+ raise ValueError(
+ "pinch_strengths must contain index/middle/ring/pinky values"
+ )
+ if splay_delta.shape != (4,) or splay_confidence.shape != (4,):
+ raise ValueError("four-finger splay arrays must contain four values")
+ values = np.concatenate(
+ [
+ closure,
+ pinch_strengths,
+ splay_delta,
+ splay_confidence,
+ np.asarray(
+ [
+ self.pinch,
+ self.thumb_splay_delta,
+ self.thumb_abduction,
+ self.thumb_opposition,
+ self.power_grasp,
+ ],
+ dtype=float,
+ ),
+ ]
+ )
+ if not np.all(np.isfinite(values)):
+ raise ValueError("hand features contain non-finite values")
+ object.__setattr__(self, "closure", np.clip(closure, 0.0, 1.0))
+ object.__setattr__(
+ self, "pinch_strengths", np.clip(pinch_strengths, 0.0, 1.0)
+ )
+ object.__setattr__(self, "splay_delta", splay_delta)
+ object.__setattr__(self, "splay_confidence", np.clip(splay_confidence, 0.0, 1.0))
+ for name in ("pinch", "thumb_abduction", "thumb_opposition", "power_grasp"):
+ object.__setattr__(
+ self,
+ name,
+ float(np.clip(getattr(self, name), 0.0, 1.0)),
+ )
diff --git a/src/camera_retarget_l20/camera_retarget_l20/l20_mapping.py b/src/camera_retarget_l20/camera_retarget_l20/l20_mapping.py
new file mode 100644
index 0000000..276b6b4
--- /dev/null
+++ b/src/camera_retarget_l20/camera_retarget_l20/l20_mapping.py
@@ -0,0 +1,170 @@
+"""Convert L20 URDF joint positions to the LinkerHand 20-byte command layout."""
+
+from __future__ import annotations
+
+from collections.abc import Sequence
+
+import numpy as np
+
+
+MOTOR_JOINT_NAMES: tuple[str | None, ...] = (
+ "thumb_joint2",
+ "index_joint1",
+ "middle_joint1",
+ "ring_joint1",
+ "little_joint1",
+ "thumb_joint0",
+ "index_joint0",
+ "middle_joint0",
+ "ring_joint0",
+ "little_joint0",
+ "thumb_joint1",
+ None,
+ None,
+ None,
+ None,
+ "thumb_joint3",
+ "index_joint2",
+ "middle_joint2",
+ "ring_joint2",
+ "little_joint2",
+)
+
+FLEXION_MOTOR_INDICES = frozenset((*range(0, 5), *range(15, 20)))
+RESERVED_MOTOR_INDICES = frozenset(range(11, 15))
+
+# This is also the L20 startup/open pose used by linker_hand_ros2_sdk.
+DEFAULT_OPEN_POSE: tuple[int, ...] = (
+ 255,
+ 255,
+ 255,
+ 255,
+ 255,
+ 255,
+ 10,
+ 100,
+ 180,
+ 240,
+ 245,
+ 255,
+ 255,
+ 255,
+ 255,
+ 255,
+ 255,
+ 255,
+ 255,
+ 255,
+)
+
+
+class L20CommandMapper:
+ """Map named L20 joint radians to safe, filtered motor commands.
+
+ The left L20 uses 16 controllable joints in a 20-value protocol. All of
+ its validated GUI mappings use the same direction: URDF lower limit maps
+ to 255 and upper limit maps to 0. Positions 11..14 are reserved.
+ """
+
+ def __init__(
+ self,
+ joint_names: Sequence[str],
+ joint_limits: Sequence[Sequence[float]],
+ *,
+ ema_alpha: float = 0.35,
+ max_step: float = 8.0,
+ max_closure: float = 0.80,
+ open_pose: Sequence[float] = DEFAULT_OPEN_POSE,
+ ) -> None:
+ if len(joint_names) != len(joint_limits):
+ raise ValueError("joint_names and joint_limits must have equal length")
+ if not 0.0 < ema_alpha <= 1.0:
+ raise ValueError("ema_alpha must be in (0, 1]")
+ if max_step <= 0.0:
+ raise ValueError("max_step must be positive")
+ if not 0.0 <= max_closure <= 1.0:
+ raise ValueError("max_closure must be in [0, 1]")
+ if len(open_pose) != 20:
+ raise ValueError("open_pose must contain 20 values")
+
+ self.joint_names = tuple(joint_names)
+ self.joint_limits = np.asarray(joint_limits, dtype=float)
+ if self.joint_limits.shape != (len(self.joint_names), 2):
+ raise ValueError("joint_limits must have shape (N, 2)")
+
+ name_to_index = {name: index for index, name in enumerate(self.joint_names)}
+ required = {name for name in MOTOR_JOINT_NAMES if name is not None}
+ missing = sorted(required.difference(name_to_index))
+ if missing:
+ raise ValueError(f"L20 URDF is missing controlled joints: {missing}")
+
+ self.motor_to_qpos = tuple(
+ None if name is None else name_to_index[name] for name in MOTOR_JOINT_NAMES
+ )
+ self.ema_alpha = float(ema_alpha)
+ self.max_step = float(max_step)
+ self.max_closure = float(max_closure)
+ self.open_pose = self._validate_command(open_pose)
+ self.last_command = self.open_pose.astype(float)
+
+ @staticmethod
+ def _validate_command(command: Sequence[float]) -> np.ndarray:
+ values = np.asarray(command, dtype=float)
+ if values.shape != (20,):
+ raise ValueError("an L20 command must contain 20 values")
+ if not np.all(np.isfinite(values)):
+ raise ValueError("L20 command contains non-finite values")
+ return np.clip(values, 0.0, 255.0)
+
+ def reset(self, command: Sequence[float] | None = None) -> None:
+ """Reset filtering, normally to the known open pose."""
+
+ self.last_command = (
+ self.open_pose.copy()
+ if command is None
+ else self._validate_command(command).astype(float)
+ )
+
+ def raw_command(self, qpos: Sequence[float]) -> np.ndarray:
+ """Return an unfiltered 20-value command with limits applied."""
+
+ qpos_array = np.asarray(qpos, dtype=float)
+ if qpos_array.shape != (len(self.joint_names),):
+ raise ValueError(
+ f"qpos must have {len(self.joint_names)} values, got {qpos_array.shape}"
+ )
+ if not np.all(np.isfinite(qpos_array)):
+ raise ValueError("qpos contains non-finite values")
+
+ command = np.full(20, 255.0, dtype=float)
+ for motor_index, qpos_index in enumerate(self.motor_to_qpos):
+ if qpos_index is None:
+ continue
+ lower, upper = self.joint_limits[qpos_index]
+ if not np.isfinite(lower) or not np.isfinite(upper) or upper <= lower:
+ raise ValueError(
+ f"invalid limits for {self.joint_names[qpos_index]}: {lower}, {upper}"
+ )
+ ratio = np.clip((qpos_array[qpos_index] - lower) / (upper - lower), 0.0, 1.0)
+ command[motor_index] = 255.0 * (1.0 - ratio)
+
+ # Limit closing travel during camera teleoperation. Ab/adduction joints
+ # retain their full URDF-constrained range.
+ minimum_flexion_command = 255.0 * (1.0 - self.max_closure)
+ for motor_index in FLEXION_MOTOR_INDICES:
+ command[motor_index] = max(
+ command[motor_index], minimum_flexion_command
+ )
+ command[list(RESERVED_MOTOR_INDICES)] = 255.0
+ return command
+
+ def map(self, qpos: Sequence[float]) -> list[int]:
+ """Map and smooth one retargeting result."""
+
+ raw = self.raw_command(qpos)
+ filtered = self.ema_alpha * raw + (1.0 - self.ema_alpha) * self.last_command
+ delta = np.clip(filtered - self.last_command, -self.max_step, self.max_step)
+ self.last_command = np.clip(self.last_command + delta, 0.0, 255.0)
+ self.last_command[list(RESERVED_MOTOR_INDICES)] = 255.0
+ return np.rint(self.last_command).astype(int).tolist()
+
diff --git a/src/camera_retarget_l20/camera_retarget_l20/model_profiles.py b/src/camera_retarget_l20/camera_retarget_l20/model_profiles.py
new file mode 100644
index 0000000..5c8236a
--- /dev/null
+++ b/src/camera_retarget_l20/camera_retarget_l20/model_profiles.py
@@ -0,0 +1,476 @@
+"""Validated, configuration-driven direct command profiles for robot hands."""
+
+from __future__ import annotations
+
+from collections.abc import Mapping, Sequence
+from dataclasses import dataclass, replace
+from pathlib import Path
+
+import numpy as np
+import yaml
+
+from .hand_features import HandFeatures
+
+
+FEATURE_INDICES: dict[str, int] = {
+ "thumb_root": 0,
+ "index_root": 1,
+ "middle_root": 2,
+ "ring_root": 3,
+ "pinky_root": 4,
+ "thumb_tip": 5,
+ "index_tip": 6,
+ "middle_tip": 7,
+ "ring_tip": 8,
+ "pinky_tip": 9,
+}
+PINCH_ROLES: tuple[str, ...] = ("index", "middle", "ring", "pinky")
+PINCH_FINGER_ENGAGEMENT_START = 0.10
+PINCH_FINGER_ENGAGEMENT_END = 0.50
+PINCH_CONTACT_ASSIST_START = 0.75
+PINCH_CONTACT_ASSIST_END = 0.95
+PINCH_CONTACT_ASSIST_TARGETS = frozenset(("index", "middle"))
+
+
+@dataclass(frozen=True)
+class FlexionChannel:
+ """One robot flexion channel driven by weighted normalized features."""
+
+ command_index: int
+ sources: tuple[tuple[int, float], ...]
+ pinch_role: str
+
+
+@dataclass(frozen=True)
+class DirectModelProfile:
+ """All model-specific data needed by the direct MediaPipe adapter."""
+
+ model: str
+ command_names: tuple[str, ...]
+ open_pose: tuple[float, ...]
+ fist_pose: tuple[float, ...]
+ flexion_channels: tuple[FlexionChannel, ...]
+ side_command_indices: tuple[int, ...]
+ pinch_target_side_commands: tuple[tuple[float, ...], ...]
+ power_grasp_side_commands: tuple[float, ...]
+ abducted_side_commands: tuple[float, ...] | None
+ splay_command_indices: tuple[int, ...]
+ splay_scale: float
+ splay_gain: float
+ max_splay_delta: float
+ fixed_channels: tuple[tuple[int, float], ...]
+ pinch_thumb_closure: float
+ pinch_finger_closure: float
+ ema_alpha: float
+ max_step: float
+ command_deadband: float
+ motor_speed: int
+ power_grasp_start: float = 0.65
+ power_grasp_end: float = 0.90
+
+ @property
+ def command_length(self) -> int:
+ return len(self.command_names)
+
+ def with_runtime_overrides(
+ self,
+ *,
+ ema_alpha: float | None = None,
+ max_step: float | None = None,
+ command_deadband: float | None = None,
+ splay_gain: float | None = None,
+ max_splay_delta: float | None = None,
+ pinch_thumb_closure: float | None = None,
+ pinch_finger_closure: float | None = None,
+ ) -> "DirectModelProfile":
+ """Return a validated profile with legacy ROS parameter overrides."""
+
+ updated = replace(
+ self,
+ ema_alpha=self.ema_alpha if ema_alpha is None else float(ema_alpha),
+ max_step=self.max_step if max_step is None else float(max_step),
+ command_deadband=(
+ self.command_deadband
+ if command_deadband is None
+ else float(command_deadband)
+ ),
+ splay_gain=(
+ self.splay_gain if splay_gain is None else float(splay_gain)
+ ),
+ max_splay_delta=(
+ self.max_splay_delta
+ if max_splay_delta is None
+ else float(max_splay_delta)
+ ),
+ pinch_thumb_closure=(
+ self.pinch_thumb_closure
+ if pinch_thumb_closure is None
+ else float(pinch_thumb_closure)
+ ),
+ pinch_finger_closure=(
+ self.pinch_finger_closure
+ if pinch_finger_closure is None
+ else float(pinch_finger_closure)
+ ),
+ )
+ _validate_profile(updated)
+ return updated
+
+
+def _float_tuple(values: Sequence[float], field: str) -> tuple[float, ...]:
+ result = tuple(float(value) for value in values)
+ if not result or not np.all(np.isfinite(result)):
+ raise ValueError(f"{field} must contain finite values")
+ return result
+
+
+def _parse_flexion_channels(values: Sequence[Mapping]) -> tuple[FlexionChannel, ...]:
+ channels: list[FlexionChannel] = []
+ for entry in values:
+ sources_value = entry.get("sources")
+ if not isinstance(sources_value, Mapping) or not sources_value:
+ raise ValueError("each flexion channel requires non-empty sources")
+ sources: list[tuple[int, float]] = []
+ for feature_name, weight in sources_value.items():
+ if feature_name not in FEATURE_INDICES:
+ raise ValueError(f"unknown hand feature: {feature_name}")
+ sources.append((FEATURE_INDICES[str(feature_name)], float(weight)))
+ channels.append(
+ FlexionChannel(
+ command_index=int(entry["command_index"]),
+ sources=tuple(sources),
+ pinch_role=str(entry["pinch_role"]).lower(),
+ )
+ )
+ return tuple(channels)
+
+
+def _profile_from_mapping(model: str, values: Mapping) -> DirectModelProfile:
+ side = values.get("side", {})
+ splay = values.get("splay", {})
+ filters = values.get("filters", {})
+ pinch = values.get("pinch", {})
+ fixed = values.get("fixed_channels", {})
+ target_values = pinch.get("target_side_commands", {})
+ profile = DirectModelProfile(
+ model=model.upper(),
+ command_names=tuple(str(name) for name in values["command_names"]),
+ open_pose=_float_tuple(values["open_pose"], f"{model}.open_pose"),
+ fist_pose=_float_tuple(values["fist_pose"], f"{model}.fist_pose"),
+ flexion_channels=_parse_flexion_channels(values["flexion_channels"]),
+ side_command_indices=tuple(int(index) for index in side.get("indices", [])),
+ pinch_target_side_commands=tuple(
+ _float_tuple(target_values[role], f"{model}.pinch.{role}")
+ for role in PINCH_ROLES
+ ),
+ power_grasp_side_commands=_float_tuple(
+ side.get("power_grasp_commands", []),
+ f"{model}.side.power_grasp_commands",
+ ),
+ abducted_side_commands=(
+ _float_tuple(
+ side["abducted_commands"],
+ f"{model}.side.abducted_commands",
+ )
+ if "abducted_commands" in side
+ else None
+ ),
+ splay_command_indices=tuple(
+ int(index) for index in splay.get("indices", [])
+ ),
+ splay_scale=float(splay.get("scale", 1.0)),
+ splay_gain=float(splay.get("gain", 0.0)),
+ max_splay_delta=float(splay.get("max_delta", 0.0)),
+ fixed_channels=tuple(
+ (int(index), float(command)) for index, command in fixed.items()
+ ),
+ pinch_thumb_closure=float(pinch.get("thumb_closure", 0.85)),
+ pinch_finger_closure=float(pinch.get("finger_closure", 0.65)),
+ ema_alpha=float(filters.get("ema_alpha", 0.55)),
+ max_step=float(filters.get("max_step", 16.0)),
+ command_deadband=float(filters.get("command_deadband", 0.0)),
+ motor_speed=int(values.get("motor_speed", 60)),
+ power_grasp_start=float(pinch.get("power_grasp_start", 0.65)),
+ power_grasp_end=float(pinch.get("power_grasp_end", 0.90)),
+ )
+ _validate_profile(profile)
+ return profile
+
+
+def _validate_profile(profile: DirectModelProfile) -> None:
+ length = profile.command_length
+ if length < 1 or len(profile.open_pose) != length or len(profile.fist_pose) != length:
+ raise ValueError(f"{profile.model} command names/open/fist lengths differ")
+ values = np.asarray(profile.open_pose + profile.fist_pose, dtype=float)
+ if np.any(values < 0.0) or np.any(values > 255.0):
+ raise ValueError(f"{profile.model} endpoints must be in [0, 255]")
+ if not 0.0 < profile.ema_alpha <= 1.0:
+ raise ValueError("ema_alpha must be in (0, 1]")
+ if profile.max_step <= 0.0 or profile.motor_speed <= 0:
+ raise ValueError("max_step and motor_speed must be positive")
+ if not 0.0 <= profile.command_deadband < profile.max_step:
+ raise ValueError("command_deadband must be in [0, max_step)")
+ if not 0.0 <= profile.pinch_thumb_closure <= 1.0:
+ raise ValueError("pinch thumb closure must be in [0, 1]")
+ if not 0.0 <= profile.pinch_finger_closure <= 1.0:
+ raise ValueError("pinch finger closure must be in [0, 1]")
+ if not 0.0 <= profile.power_grasp_start < profile.power_grasp_end <= 1.0:
+ raise ValueError("power grasp thresholds must increase within [0, 1]")
+ if not 0.0 <= profile.max_splay_delta <= 255.0:
+ raise ValueError("max_splay_delta must be in [0, 255]")
+
+ flex_indices = [channel.command_index for channel in profile.flexion_channels]
+ side_indices = list(profile.side_command_indices)
+ splay_indices = list(profile.splay_command_indices)
+ fixed_indices = [index for index, _ in profile.fixed_channels]
+ all_indices = flex_indices + side_indices + splay_indices + fixed_indices
+ if any(index < 0 or index >= length for index in all_indices):
+ raise ValueError(f"{profile.model} contains an out-of-range command index")
+ if len(all_indices) != len(set(all_indices)):
+ raise ValueError(f"{profile.model} command channel roles overlap")
+ if len(profile.pinch_target_side_commands) != len(PINCH_ROLES):
+ raise ValueError("pinch target table must contain four fingers")
+ if any(
+ len(commands) != len(side_indices)
+ for commands in profile.pinch_target_side_commands
+ ):
+ raise ValueError("pinch side command width does not match side indices")
+ if len(profile.power_grasp_side_commands) != len(side_indices):
+ raise ValueError("power grasp side command width does not match side indices")
+ if profile.abducted_side_commands is not None and len(
+ profile.abducted_side_commands
+ ) != len(side_indices):
+ raise ValueError("abducted side command width does not match side indices")
+ if splay_indices and len(splay_indices) != 4:
+ raise ValueError("splay mapping must contain index/middle/ring/pinky channels")
+ for channel in profile.flexion_channels:
+ if channel.pinch_role not in ("thumb", *PINCH_ROLES):
+ raise ValueError(f"invalid pinch role: {channel.pinch_role}")
+ weights = np.asarray([weight for _, weight in channel.sources], dtype=float)
+ if np.any(weights < 0.0) or not np.isclose(np.sum(weights), 1.0):
+ raise ValueError("flexion source weights must be non-negative and sum to 1")
+ fixed_values = np.asarray([value for _, value in profile.fixed_channels])
+ if fixed_values.size and (
+ np.any(fixed_values < 0.0) or np.any(fixed_values > 255.0)
+ ):
+ raise ValueError("fixed channel commands must be in [0, 255]")
+
+
+def load_model_profiles(path: str | Path) -> dict[str, DirectModelProfile]:
+ """Load and validate all model profiles from YAML."""
+
+ profile_path = Path(path).expanduser().resolve()
+ with profile_path.open("r", encoding="utf-8") as stream:
+ document = yaml.safe_load(stream)
+ if not isinstance(document, Mapping) or document.get("schema_version") != 1:
+ raise ValueError("model profile file requires schema_version: 1")
+ models = document.get("models")
+ if not isinstance(models, Mapping) or not models:
+ raise ValueError("model profile file does not define any models")
+ return {
+ str(model).upper(): _profile_from_mapping(str(model), values)
+ for model, values in models.items()
+ }
+
+
+def _smoothstep(value: float, start: float, end: float) -> float:
+ ratio = float(np.clip((value - start) / (end - start), 0.0, 1.0))
+ return ratio * ratio * (3.0 - 2.0 * ratio)
+
+
+class ProfiledCommandMapper:
+ """Map shared hand intent to one profile's unfiltered/filtered command."""
+
+ def __init__(
+ self, profile: DirectModelProfile, *, max_closure: float = 1.0
+ ) -> None:
+ _validate_profile(profile)
+ if not 0.0 <= max_closure <= 1.0:
+ raise ValueError("max_closure must be in [0, 1]")
+ self.profile = profile
+ self.max_closure = float(max_closure)
+ self.open_pose = np.asarray(profile.open_pose, dtype=float)
+ self.fist_pose = np.asarray(profile.fist_pose, dtype=float)
+ self.last_command = self.open_pose.copy()
+ self.last_raw_command = self.open_pose.copy()
+ self.last_pinch_blend = 0.0
+
+ def reset(self, command: Sequence[float] | None = None) -> None:
+ if command is None:
+ values = self.open_pose.copy()
+ else:
+ values = np.asarray(command, dtype=float)
+ if values.shape != self.open_pose.shape or not np.all(np.isfinite(values)):
+ raise ValueError(
+ f"{self.profile.model} reset command has invalid shape or values"
+ )
+ values = np.clip(values, 0.0, 255.0)
+ self.last_command = values
+ self.last_raw_command = values.copy()
+ self.last_pinch_blend = 0.0
+
+ def raw_command(self, features: HandFeatures) -> np.ndarray:
+ profile = self.profile
+ command = self.open_pose.copy()
+ closure = np.asarray(features.closure, dtype=float)
+ strengths = np.asarray(features.pinch_strengths, dtype=float)
+ selected_target = int(np.argmax(strengths))
+ pinch_strength = float(strengths[selected_target])
+ grasp_blend = _smoothstep(
+ float(features.power_grasp),
+ profile.power_grasp_start,
+ profile.power_grasp_end,
+ )
+ pinch_blend = pinch_strength * (1.0 - grasp_blend)
+ self.last_pinch_blend = float(pinch_blend)
+
+ for channel in profile.flexion_channels:
+ activation = sum(
+ closure[feature_index] * weight
+ for feature_index, weight in channel.sources
+ )
+ activation = float(np.clip(activation * self.max_closure, 0.0, 1.0))
+ index = channel.command_index
+ command[index] = self.open_pose[index] + activation * (
+ self.fist_pose[index] - self.open_pose[index]
+ )
+
+ if profile.splay_command_indices:
+ delta = (
+ profile.splay_gain
+ * np.asarray(features.splay_delta, dtype=float)
+ * profile.splay_scale
+ )
+ delta = np.clip(delta, -profile.max_splay_delta, profile.max_splay_delta)
+ indices = list(profile.splay_command_indices)
+ command[indices] = self.open_pose[indices] + delta
+
+ side_indices = list(profile.side_command_indices)
+ if side_indices:
+ open_side = self.open_pose[side_indices]
+ side_command = open_side.copy()
+ if (
+ profile.abducted_side_commands is not None
+ and features.thumb_abduction >= features.thumb_opposition
+ ):
+ side_command += features.thumb_abduction * (
+ np.asarray(profile.abducted_side_commands) - open_side
+ )
+ elif features.thumb_opposition > 0.0:
+ side_command += features.thumb_opposition * (
+ np.asarray(profile.power_grasp_side_commands) - open_side
+ )
+ command[side_indices] = side_command
+
+ if pinch_blend > 0.0:
+ selected_role = PINCH_ROLES[selected_target]
+ # Thumb proximity chooses the opposition target, but it must not
+ # curl that finger by itself. Gate morphology compensation with
+ # the selected human finger's own root/tip flexion so sweeping the
+ # thumb across an open hand leaves all four fingers independent.
+ selected_root_index = 1 + selected_target
+ selected_tip_index = 6 + selected_target
+ selected_finger_intent = float(
+ 0.5
+ * (
+ closure[selected_root_index]
+ + closure[selected_tip_index]
+ )
+ )
+ finger_engagement = _smoothstep(
+ selected_finger_intent,
+ PINCH_FINGER_ENGAGEMENT_START,
+ PINCH_FINGER_ENGAGEMENT_END,
+ )
+ # Index/middle precision pinches are commonly made with the target
+ # finger almost straight. Once fingertip distance is very close to
+ # the calibrated contact endpoint, allow the robot-only closure
+ # compensation even without visible target-finger flexion. Ring
+ # and pinky keep the stricter self-flexion gate so a thumb sweep
+ # across the palm cannot drag them along.
+ contact_engagement = 0.0
+ if selected_role in PINCH_CONTACT_ASSIST_TARGETS:
+ contact_engagement = _smoothstep(
+ pinch_strength,
+ PINCH_CONTACT_ASSIST_START,
+ PINCH_CONTACT_ASSIST_END,
+ )
+ finger_assist_blend = pinch_blend * max(
+ finger_engagement,
+ contact_engagement,
+ )
+ for channel in profile.flexion_channels:
+ if channel.pinch_role == "thumb":
+ target_activation = profile.pinch_thumb_closure
+ channel_pinch_blend = pinch_blend
+ elif channel.pinch_role == selected_role:
+ current_activation = sum(
+ closure[feature_index] * weight
+ for feature_index, weight in channel.sources
+ )
+ target_activation = max(
+ current_activation,
+ profile.pinch_finger_closure,
+ )
+ channel_pinch_blend = finger_assist_blend
+ else:
+ continue
+ target_activation *= self.max_closure
+ index = channel.command_index
+ target_command = self.open_pose[index] + target_activation * (
+ self.fist_pose[index] - self.open_pose[index]
+ )
+ command[index] += channel_pinch_blend * (
+ target_command - command[index]
+ )
+ if side_indices:
+ target_side = np.asarray(
+ profile.pinch_target_side_commands[selected_target], dtype=float
+ )
+ command[side_indices] += pinch_blend * (
+ target_side - command[side_indices]
+ )
+
+ if grasp_blend > 0.0:
+ # Four-finger flexion defines power_grasp, but it must not close an
+ # otherwise open thumb. Complete thumb flexion only when the shared
+ # feature layer also sees the thumb's own calibrated opposition.
+ # This restores a natural full fist without regressing the
+ # four-fingers-only gesture.
+ for channel in profile.flexion_channels:
+ channel_grasp_blend = grasp_blend
+ if channel.pinch_role == "thumb":
+ channel_grasp_blend *= float(
+ np.clip(features.thumb_opposition, 0.0, 1.0)
+ )
+ index = channel.command_index
+ command[index] += channel_grasp_blend * (
+ self.fist_pose[index] - command[index]
+ )
+
+ for index in profile.splay_command_indices:
+ command[index] += grasp_blend * (
+ self.fist_pose[index] - command[index]
+ )
+
+ for index, value in profile.fixed_channels:
+ command[index] = value
+ self.last_raw_command = np.clip(command, 0.0, 255.0)
+ return self.last_raw_command.copy()
+
+ def map_features(self, features: HandFeatures) -> list[int]:
+ raw = self.raw_command(features)
+ filtered = (
+ self.profile.ema_alpha * raw
+ + (1.0 - self.profile.ema_alpha) * self.last_command
+ )
+ delta = np.clip(
+ filtered - self.last_command,
+ -self.profile.max_step,
+ self.profile.max_step,
+ )
+ delta[np.abs(delta) <= self.profile.command_deadband] = 0.0
+ self.last_command = np.clip(self.last_command + delta, 0.0, 255.0)
+ for index, value in self.profile.fixed_channels:
+ self.last_command[index] = value
+ return np.rint(self.last_command).astype(int).tolist()
diff --git a/src/camera_retarget_l20/camera_retarget_l20/node.py b/src/camera_retarget_l20/camera_retarget_l20/node.py
new file mode 100644
index 0000000..c267928
--- /dev/null
+++ b/src/camera_retarget_l20/camera_retarget_l20/node.py
@@ -0,0 +1,1258 @@
+"""ROS 2 camera-to-LinkerHand L20/G20/O6 retargeting bridge."""
+
+from __future__ import annotations
+
+import json
+import sys
+import time
+from pathlib import Path
+
+import cv2
+import numpy as np
+import rclpy
+from ament_index_python.packages import get_package_share_directory
+from rclpy.node import Node
+from rclpy.parameter import Parameter
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+from sensor_msgs.msg import JointState
+from std_msgs.msg import String
+from std_srvs.srv import SetBool, Trigger
+
+from .calibration import PoseCalibrationSession, RawHandObservation
+from .hand_detector import MediaPipeLeftHandDetector
+from .g20_mapping import (
+ DEFAULT_OPEN_POSE as G20_DEFAULT_OPEN_POSE,
+ G20_COMMAND_NAMES,
+ G20CommandMapper,
+ G20MediaPipeMapper,
+)
+from .l20_mapping import (
+ DEFAULT_OPEN_POSE as L20_DEFAULT_OPEN_POSE,
+ L20CommandMapper,
+)
+from .o6_mapping import (
+ O6_COMMAND_NAMES,
+ O6MediaPipeMapper,
+)
+from .model_profiles import (
+ DirectModelProfile,
+ ProfiledCommandMapper,
+ load_model_profiles,
+)
+
+
+COMMAND_QOS = QoSProfile(
+ history=HistoryPolicy.KEEP_LAST,
+ depth=1,
+ reliability=ReliabilityPolicy.RELIABLE,
+)
+
+
+class CameraHandRetargetNode(Node):
+ def __init__(self) -> None:
+ super().__init__("camera_hand_retarget")
+ self._declare_parameters()
+
+ configured_targets = self.get_parameter_or(
+ "target_models",
+ Parameter("target_models", Parameter.Type.STRING_ARRAY, []),
+ ).value
+ targets = tuple(
+ str(model).upper()
+ for model in (configured_targets or [])
+ if str(model).strip()
+ )
+ self.dual_mode = bool(targets)
+ if self.dual_mode:
+ if len(targets) < 2 or len(set(targets)) != len(targets):
+ raise ValueError(
+ "target_models requires at least two unique model names"
+ )
+ if "G20" not in targets:
+ raise ValueError(
+ "direct multi-model mode currently requires G20 as the "
+ "calibrated human-feature extractor"
+ )
+ self.target_models = targets
+ # G20's calibrated feature extractor is shared by all profiles.
+ self.hand_model = "G20"
+ else:
+ self.hand_model = str(self.get_parameter("hand_model").value).upper()
+ self.target_models = (self.hand_model,)
+ if self.hand_model not in {"L20", "G20"}:
+ raise ValueError("hand_model must be 'L20' or 'G20'")
+ requested_mapping_mode = str(
+ self.get_parameter("mapping_mode").value
+ ).lower()
+ if requested_mapping_mode not in {"auto", "dex", "mediapipe_angles"}:
+ raise ValueError(
+ "mapping_mode must be 'auto', 'dex', or 'mediapipe_angles'"
+ )
+ self.mapping_mode = requested_mapping_mode
+ if self.dual_mode:
+ self.mapping_mode = "mediapipe_angles"
+ if self.mapping_mode == "auto":
+ self.mapping_mode = (
+ "mediapipe_angles" if self.hand_model == "G20" else "dex"
+ )
+ if self.mapping_mode == "mediapipe_angles" and self.hand_model != "G20":
+ raise ValueError("mediapipe_angles mapping currently supports G20 only")
+ self.camera_device = str(self.get_parameter("camera_device").value)
+ self.show_window = bool(self.get_parameter("show_window").value)
+ self.selfie = bool(self.get_parameter("selfie").value)
+ self.publish_rate = float(self.get_parameter("publish_rate").value)
+ self.motor_speed = int(self.get_parameter("motor_speed").value)
+ self.stable_frames_required = int(
+ self.get_parameter("stable_frames_required").value
+ )
+ self.lost_timeout = float(self.get_parameter("lost_timeout").value)
+ self.lost_behavior = str(self.get_parameter("lost_behavior").value).lower()
+ if self.lost_behavior not in {"open", "hold"}:
+ raise ValueError("lost_behavior must be 'open' or 'hold'")
+ self.driver_state_timeout = float(
+ self.get_parameter("driver_state_timeout").value
+ )
+ if self.driver_state_timeout <= 0.0:
+ raise ValueError("driver_state_timeout must be positive")
+ self.calibration_duration = float(
+ self.get_parameter("calibration_duration").value
+ )
+ self.calibration_min_samples = int(
+ self.get_parameter("calibration_min_samples").value
+ )
+ self.calibration_timeout = float(
+ self.get_parameter("calibration_timeout").value
+ )
+ if self.calibration_duration <= 0.0:
+ raise ValueError("calibration_duration must be positive")
+ if self.calibration_min_samples < 2:
+ raise ValueError("calibration_min_samples must be at least two")
+ if self.calibration_timeout <= self.calibration_duration:
+ raise ValueError("calibration_timeout must exceed calibration_duration")
+ self.publish_debug = bool(self.get_parameter("publish_debug").value)
+
+ ema_alpha = float(self.get_parameter("ema_alpha").value)
+ max_step = float(self.get_parameter("max_step").value)
+ command_deadband = float(self.get_parameter("command_deadband").value)
+ max_closure = float(self.get_parameter("max_closure").value)
+ self.retargeting = None
+ self.direct_mappers: dict[str, object] = {}
+ self.model_profiles: dict[str, DirectModelProfile] = {}
+ if self.mapping_mode == "mediapipe_angles":
+ profile_path = self._resolve_model_profile_path()
+ self.model_profiles = load_model_profiles(profile_path)
+ required_profiles = set(self.target_models)
+ missing_profiles = sorted(required_profiles.difference(self.model_profiles))
+ if missing_profiles:
+ raise ValueError(
+ f"model profile file is missing: {missing_profiles}"
+ )
+ self.get_logger().info(f"Using direct model profiles: {profile_path}")
+ g20_profile = self.model_profiles["G20"]
+ self.default_open_pose = tuple(
+ int(round(value)) for value in g20_profile.open_pose
+ )
+ self.mapper = G20MediaPipeMapper(
+ ema_alpha=ema_alpha,
+ max_step=max_step,
+ command_deadband=command_deadband,
+ max_closure=max_closure,
+ splay_gain=float(self.get_parameter("splay_gain").value),
+ max_splay_delta=float(
+ self.get_parameter("max_splay_delta").value
+ ),
+ thumb_splay_span=float(
+ self.get_parameter("thumb_splay_span").value
+ ),
+ thumb_abduction_min_command=float(
+ self.get_parameter("thumb_abduction_min_command").value
+ ),
+ thumb_pinch_gain=float(
+ self.get_parameter("thumb_pinch_gain").value
+ ),
+ pinch_thumb_closure=float(
+ self.get_parameter("g20_pinch_thumb_closure").value
+ ),
+ pinch_finger_closure=float(
+ self.get_parameter("g20_pinch_finger_closure").value
+ ),
+ profile=g20_profile,
+ )
+ self.direct_mappers["G20"] = self.mapper
+ for model in self.target_models:
+ if model == "G20":
+ continue
+ profile = self.model_profiles[model]
+ if model == "O6":
+ adapter = O6MediaPipeMapper(
+ ema_alpha=ema_alpha,
+ max_step=max_step,
+ command_deadband=command_deadband,
+ pinch_thumb_closure=float(
+ self.get_parameter("o6_pinch_thumb_closure").value
+ ),
+ pinch_finger_closure=float(
+ self.get_parameter("o6_pinch_finger_closure").value
+ ),
+ profile=profile,
+ )
+ self.o6_mapper = adapter
+ else:
+ adapter_profile = profile.with_runtime_overrides(
+ ema_alpha=ema_alpha,
+ max_step=max_step,
+ command_deadband=command_deadband,
+ )
+ adapter = ProfiledCommandMapper(adapter_profile)
+ self.direct_mappers[model] = adapter
+ self._log_profile_overrides()
+ else:
+ self.retargeting = self._build_retargeting()
+ robot = self.retargeting.optimizer.robot
+ mapper_class = L20CommandMapper
+ self.default_open_pose = L20_DEFAULT_OPEN_POSE
+ if self.hand_model == "G20":
+ mapper_class = G20CommandMapper
+ self.default_open_pose = G20_DEFAULT_OPEN_POSE
+ self.mapper = mapper_class(
+ self.retargeting.joint_names,
+ robot.joint_limits,
+ ema_alpha=ema_alpha,
+ max_step=max_step,
+ max_closure=max_closure,
+ )
+ self.detector = MediaPipeLeftHandDetector(
+ selfie=self.selfie,
+ strict_handedness=bool(
+ self.get_parameter("strict_handedness").value
+ ),
+ min_detection_confidence=float(
+ self.get_parameter("min_detection_confidence").value
+ ),
+ min_tracking_confidence=float(
+ self.get_parameter("min_tracking_confidence").value
+ ),
+ )
+ self.capture = self._open_camera()
+
+ self.command_publishers = {}
+ self.preview_publishers = {}
+ self.driver_state_times: dict[str, float | None] = {}
+ self.driver_state_valid: dict[str, bool] = {}
+ self.state_subscriptions = []
+ if self.dual_mode:
+ for model in self.target_models:
+ prefix = f"/{model.lower()}"
+ self.command_publishers[model] = self.create_publisher(
+ JointState,
+ f"{prefix}/cb_left_hand_control_cmd",
+ COMMAND_QOS,
+ )
+ self.preview_publishers[model] = self.create_publisher(
+ JointState, f"~/{model.lower()}/target_joint_cmd", 10
+ )
+ self.driver_state_times[model] = None
+ self.driver_state_valid[model] = False
+ self.state_subscriptions.append(
+ self.create_subscription(
+ JointState,
+ f"{prefix}/cb_left_hand_state",
+ lambda message, target=model: self._state_callback(
+ target, message
+ ),
+ 10,
+ )
+ )
+ else:
+ control_topic = str(self.get_parameter("control_topic").value)
+ self.command_publisher = self.create_publisher(
+ JointState, control_topic, COMMAND_QOS
+ )
+ self.preview_publisher = self.create_publisher(
+ JointState, "~/target_joint_cmd", 10
+ )
+ self.command_publishers[self.hand_model] = self.command_publisher
+ self.preview_publishers[self.hand_model] = self.preview_publisher
+ self.status_publisher = self.create_publisher(String, "~/status", 10)
+ self.debug_publisher = (
+ self.create_publisher(String, "~/debug", 10)
+ if self.publish_debug
+ else None
+ )
+ self.enable_service = self.create_service(
+ SetBool, "~/enable", self._enable_callback
+ )
+ self.calibrate_service = self.create_service(
+ Trigger, "~/calibrate_open", self._calibrate_open_callback
+ )
+ self.calibrate_fist_service = self.create_service(
+ Trigger, "~/calibrate_fist", self._calibrate_fist_callback
+ )
+ self.calibrate_pinch_service = self.create_service(
+ Trigger, "~/calibrate_pinch", self._calibrate_pinch_callback
+ )
+
+ self.control_enabled = False
+ self.auto_enable_requested = bool(
+ self.get_parameter("start_enabled").value
+ )
+ self.hand_detected = False
+ self.stable_frames = 0
+ self.last_detection_time = time.monotonic()
+ self.last_detection_positions: np.ndarray | None = None
+ self.last_target_command: list[int] | None = None
+ self.last_target_commands: dict[str, list[int]] = {}
+ self.last_status_time = 0.0
+ self.last_debug_time = 0.0
+ self.last_frame_time = time.monotonic()
+ self.measured_fps = 0.0
+ self.failsafe_sent = False
+ self.last_failsafe_reason: str | None = None
+ self.status_message = "preview_only"
+ self.calibration_session: PoseCalibrationSession | None = None
+ self.calibration_diagnostics: dict[str, float] = {}
+ self.calibration_failure_reported = False
+ display_models = "+".join(self.target_models)
+ self.window_name = f"{display_models} left camera retargeting"
+
+ self.timer = self.create_timer(1.0 / self.publish_rate, self._process_frame)
+ if self.dual_mode:
+ self.get_logger().info(
+ f"{display_models} mapping ready in PREVIEW mode. Press C with an open "
+ "hand and the thumb spread outward, F with a fist, P with "
+ "thumb/index pinched, then E to enable all target hands."
+ )
+ elif self.mapping_mode == "mediapipe_angles":
+ self.get_logger().info(
+ "G20 angle mapping ready in PREVIEW mode. Fully open the left "
+ "hand, press C to calibrate, then press E to enable control."
+ )
+ else:
+ self.get_logger().info(
+ "Camera retargeting ready in PREVIEW mode. Show a stable left "
+ "hand, then press E or call ~/enable with data=true to control "
+ f"the {self.hand_model}."
+ )
+
+ def _declare_parameters(self) -> None:
+ self.declare_parameter("hand_model", "L20")
+ self.declare_parameter("target_models", Parameter.Type.STRING_ARRAY)
+ self.declare_parameter("mapping_mode", "auto")
+ self.declare_parameter("camera_device", "/dev/video0")
+ self.declare_parameter("camera_width", 1280)
+ self.declare_parameter("camera_height", 720)
+ self.declare_parameter("camera_fps", 30.0)
+ self.declare_parameter("show_window", True)
+ self.declare_parameter("selfie", False)
+ self.declare_parameter("strict_handedness", True)
+ self.declare_parameter("min_detection_confidence", 0.70)
+ self.declare_parameter("min_tracking_confidence", 0.70)
+ self.declare_parameter("publish_rate", 30.0)
+ self.declare_parameter("control_topic", "/cb_left_hand_control_cmd")
+ self.declare_parameter("start_enabled", False)
+ self.declare_parameter("motor_speed", 60)
+ self.declare_parameter("ema_alpha", 0.35)
+ self.declare_parameter("max_step", 8.0)
+ self.declare_parameter("command_deadband", 0.0)
+ self.declare_parameter("max_closure", 0.80)
+ self.declare_parameter("splay_gain", 1.0)
+ self.declare_parameter("max_splay_delta", 90.0)
+ self.declare_parameter("thumb_splay_span", 0.70)
+ self.declare_parameter("thumb_abduction_min_command", 20.0)
+ self.declare_parameter("thumb_pinch_gain", 2.0)
+ self.declare_parameter("g20_pinch_thumb_closure", 0.85)
+ self.declare_parameter("g20_pinch_finger_closure", 0.65)
+ self.declare_parameter("o6_pinch_thumb_closure", 0.85)
+ self.declare_parameter("o6_pinch_finger_closure", 0.65)
+ self.declare_parameter("stable_frames_required", 8)
+ self.declare_parameter("lost_timeout", 0.35)
+ self.declare_parameter("lost_behavior", "open")
+ self.declare_parameter("driver_state_timeout", 1.0)
+ self.declare_parameter("model_profile_file", "")
+ self.declare_parameter("calibration_duration", 1.5)
+ self.declare_parameter("calibration_min_samples", 30)
+ self.declare_parameter("calibration_timeout", 8.0)
+ self.declare_parameter("publish_debug", True)
+ self.declare_parameter("retargeting_config", "")
+ self.declare_parameter("urdf_path", "")
+ self.declare_parameter(
+ "dex_retargeting_source", "/home/lxp/projects/dex-retargeting"
+ )
+ self.declare_parameter("retargeting_scale", 1.0)
+ self.declare_parameter("retargeting_filter_alpha", 0.25)
+
+ @staticmethod
+ def _workspace_candidates(relative_path: Path) -> list[Path]:
+ candidates: list[Path] = []
+ module_path = Path(__file__).resolve()
+ for parent in module_path.parents:
+ candidates.append(parent / relative_path)
+ candidates.append(Path.cwd() / relative_path)
+ return candidates
+
+ def _resolve_config_path(self) -> Path:
+ configured = str(self.get_parameter("retargeting_config").value)
+ if configured:
+ path = Path(configured).expanduser().resolve()
+ if path.exists():
+ return path
+ raise FileNotFoundError(f"retargeting_config does not exist: {path}")
+ share = Path(get_package_share_directory("camera_retarget_l20"))
+ path = share / "config" / f"{self.hand_model.lower()}_left_mediapipe.yml"
+ if not path.exists():
+ raise FileNotFoundError(f"installed retargeting config not found: {path}")
+ return path
+
+ def _resolve_model_profile_path(self) -> Path:
+ configured = str(self.get_parameter("model_profile_file").value)
+ if configured:
+ path = Path(configured).expanduser().resolve()
+ if path.exists():
+ return path
+ raise FileNotFoundError(f"model_profile_file does not exist: {path}")
+ share = Path(get_package_share_directory("camera_retarget_l20"))
+ path = share / "config" / "direct_mapping_profiles.yml"
+ if not path.exists():
+ raise FileNotFoundError(f"installed model profile not found: {path}")
+ return path
+
+ def _log_profile_overrides(self) -> None:
+ """Report legacy ROS values that replace profile defaults."""
+
+ common = {
+ "ema_alpha": float(self.get_parameter("ema_alpha").value),
+ "max_step": float(self.get_parameter("max_step").value),
+ "command_deadband": float(
+ self.get_parameter("command_deadband").value
+ ),
+ "motor_speed": int(self.get_parameter("motor_speed").value),
+ }
+ for model in self.target_models:
+ profile = self.model_profiles[model]
+ profile_values = {
+ "ema_alpha": profile.ema_alpha,
+ "max_step": profile.max_step,
+ "command_deadband": profile.command_deadband,
+ "motor_speed": profile.motor_speed,
+ }
+ for field, value in common.items():
+ if not np.isclose(float(value), float(profile_values[field])):
+ self.get_logger().info(
+ f"{model} profile {field}={profile_values[field]} "
+ f"overridden by ROS parameter {value}"
+ )
+ model_parameters = {}
+ model_profile_values = {
+ "pinch_thumb_closure": profile.pinch_thumb_closure,
+ "pinch_finger_closure": profile.pinch_finger_closure,
+ }
+ if model in {"O6", "G20"}:
+ model_parameters.update(
+ {
+ "pinch_thumb_closure": float(
+ self.get_parameter(
+ f"{model.lower()}_pinch_thumb_closure"
+ ).value
+ ),
+ "pinch_finger_closure": float(
+ self.get_parameter(
+ f"{model.lower()}_pinch_finger_closure"
+ ).value
+ ),
+ }
+ )
+ if model == "G20":
+ model_parameters.update(
+ {
+ "splay_gain": float(
+ self.get_parameter("splay_gain").value
+ ),
+ "max_splay_delta": float(
+ self.get_parameter("max_splay_delta").value
+ ),
+ }
+ )
+ model_profile_values.update(
+ {
+ "splay_gain": profile.splay_gain,
+ "max_splay_delta": profile.max_splay_delta,
+ }
+ )
+ for field, value in model_parameters.items():
+ if not np.isclose(
+ float(value), float(model_profile_values[field])
+ ):
+ self.get_logger().info(
+ f"{model} profile {field}="
+ f"{model_profile_values[field]} overridden by ROS "
+ f"parameter {value}"
+ )
+
+ def _resolve_urdf_path(self) -> Path:
+ configured = str(self.get_parameter("urdf_path").value)
+ if configured:
+ path = Path(configured).expanduser().resolve()
+ if path.exists():
+ return path
+ raise FileNotFoundError(f"urdf_path does not exist: {path}")
+
+ model = self.hand_model.lower()
+ relative = Path(
+ "src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/"
+ f"linker_hand/{model}_left/linkerhand_{model}_left.urdf"
+ )
+ for candidate in self._workspace_candidates(relative):
+ if candidate.exists():
+ return candidate.resolve()
+ raise FileNotFoundError(
+ f"Cannot locate the {self.hand_model}-left URDF. "
+ "Set the urdf_path ROS parameter."
+ )
+
+ def _import_retargeting_config(self):
+ try:
+ from dex_retargeting.retargeting_config import RetargetingConfig
+
+ return RetargetingConfig
+ except ImportError:
+ source = Path(
+ str(self.get_parameter("dex_retargeting_source").value)
+ ).expanduser()
+ source_package = source / "src"
+ if source_package.exists() and str(source_package) not in sys.path:
+ sys.path.insert(0, str(source_package))
+ from dex_retargeting.retargeting_config import RetargetingConfig
+
+ return RetargetingConfig
+
+ def _build_retargeting(self):
+ RetargetingConfig = self._import_retargeting_config()
+ config_path = self._resolve_config_path()
+ urdf_path = self._resolve_urdf_path()
+ override = {
+ "urdf_path": str(urdf_path),
+ "scaling_factor": float(
+ self.get_parameter("retargeting_scale").value
+ ),
+ "low_pass_alpha": float(
+ self.get_parameter("retargeting_filter_alpha").value
+ ),
+ }
+ self.get_logger().info(f"Using {self.hand_model} URDF: {urdf_path}")
+ return RetargetingConfig.load_from_file(config_path, override=override).build()
+
+ def _open_camera(self) -> cv2.VideoCapture:
+ source: str | int = self.camera_device
+ if self.camera_device.isdigit():
+ source = int(self.camera_device)
+ capture = cv2.VideoCapture(source, cv2.CAP_V4L2)
+ capture.set(cv2.CAP_PROP_FRAME_WIDTH, int(self.get_parameter("camera_width").value))
+ capture.set(cv2.CAP_PROP_FRAME_HEIGHT, int(self.get_parameter("camera_height").value))
+ capture.set(cv2.CAP_PROP_FPS, float(self.get_parameter("camera_fps").value))
+ capture.set(cv2.CAP_PROP_BUFFERSIZE, 1)
+ if not capture.isOpened():
+ raise RuntimeError(
+ f"Cannot open camera {self.camera_device}. Check /dev permissions and device path."
+ )
+ return capture
+
+ def _expected_command_length(self, model: str) -> int:
+ if model in self.model_profiles:
+ return self.model_profiles[model].command_length
+ return 6 if model == "O6" else 20
+
+ def _make_joint_state(
+ self, model: str, command: list[int], *, include_speed: bool
+ ) -> JointState:
+ expected_length = self._expected_command_length(model)
+ if len(command) != expected_length:
+ raise ValueError(
+ f"{model} command must contain {expected_length} values"
+ )
+ message = JointState()
+ message.header.stamp = self.get_clock().now().to_msg()
+ if model in self.model_profiles:
+ message.name = list(self.model_profiles[model].command_names)
+ elif model == "G20":
+ message.name = list(G20_COMMAND_NAMES)
+ elif model == "O6":
+ message.name = list(O6_COMMAND_NAMES)
+ else:
+ message.name = [f"joint{index}" for index in range(1, 21)]
+ message.position = [float(value) for value in command]
+ message.velocity = (
+ [float(self.motor_speed)] * expected_length if include_speed else []
+ )
+ return message
+
+ def _open_commands(self) -> dict[str, list[int]]:
+ if self.model_profiles:
+ return {
+ model: [
+ int(round(value))
+ for value in self.model_profiles[model].open_pose
+ ]
+ for model in self.target_models
+ }
+ return {self.hand_model: list(self.default_open_pose)}
+
+ def _publish_control(self, commands: dict[str, list[int]] | list[int]) -> None:
+ if isinstance(commands, list):
+ commands = {self.hand_model: commands}
+ for model, command in commands.items():
+ self.command_publishers[model].publish(
+ self._make_joint_state(model, command, include_speed=True)
+ )
+
+ def _state_callback(self, model: str, message: JointState) -> None:
+ expected_length = self._expected_command_length(model)
+ values = np.asarray(message.position, dtype=float)
+ valid = (
+ values.shape == (expected_length,)
+ and np.all(np.isfinite(values))
+ and np.all(values >= 0.0)
+ )
+ self.driver_state_valid[model] = bool(valid)
+ if valid:
+ self.driver_state_times[model] = time.monotonic()
+
+ def _driver_health(self, now: float) -> tuple[bool, dict[str, str]]:
+ if not self.dual_mode:
+ return True, {}
+ details: dict[str, str] = {}
+ for model in self.target_models:
+ if self.command_publishers[model].get_subscription_count() < 1:
+ details[model] = "command_subscriber_missing"
+ continue
+ state_time = self.driver_state_times[model]
+ if not self.driver_state_valid[model] or state_time is None:
+ details[model] = "state_missing"
+ elif now - state_time > self.driver_state_timeout:
+ details[model] = "state_stale"
+ else:
+ details[model] = "ready"
+ return all(value == "ready" for value in details.values()), details
+
+ def _set_control_enabled(self, enabled: bool) -> tuple[bool, str]:
+ if enabled:
+ if self.control_enabled:
+ return True, f"{'+'.join(self.target_models)} control is already enabled"
+ if self.calibration_session is not None and self.calibration_session.active:
+ return False, "Wait for the active C/F/P calibration to finish"
+ if not self.hand_detected or self.stable_frames < self.stable_frames_required:
+ return False, (
+ "Left hand is not stable yet; keep it visible before enabling"
+ )
+ if (
+ self.mapping_mode == "mediapipe_angles"
+ and not self.mapper.is_calibrated
+ ):
+ return False, (
+ "Open-hand calibration is required: fully open the left "
+ "hand and press C or call ~/calibrate_open"
+ )
+ if self.dual_mode and not self.mapper.is_fist_calibrated:
+ return False, (
+ "Fist calibration is required: close the left hand and "
+ "press F or call ~/calibrate_fist"
+ )
+ if self.dual_mode and not self.mapper.is_pinch_calibrated:
+ return False, (
+ "Pinch calibration is required: touch thumb/index tips and "
+ "press P or call ~/calibrate_pinch"
+ )
+ drivers_ready, driver_details = self._driver_health(time.monotonic())
+ if not drivers_ready:
+ return False, f"All target drivers must be ready: {driver_details}"
+ if self.direct_mappers:
+ for adapter in self.direct_mappers.values():
+ adapter.reset()
+ else:
+ self.mapper.reset()
+ if self.retargeting is not None:
+ self.retargeting.reset()
+ self.failsafe_sent = False
+ self.last_failsafe_reason = None
+ self.control_enabled = True
+ self.auto_enable_requested = False
+ self.status_message = "control_enabled"
+ self._publish_control(self._open_commands())
+ self.get_logger().warning(
+ f"REAL {'+'.join(self.target_models)} CONTROL ENABLED"
+ )
+ return True, f"Real {'+'.join(self.target_models)} control enabled"
+
+ self.control_enabled = False
+ self.auto_enable_requested = False
+ self.status_message = "preview_only"
+ self.get_logger().info(
+ f"Real {'+'.join(self.target_models)} control disabled; holding last command"
+ )
+ return True, f"Real {'+'.join(self.target_models)} control disabled"
+
+ def _enable_callback(self, request: SetBool.Request, response: SetBool.Response):
+ success, message = self._set_control_enabled(bool(request.data))
+ response.success = success
+ response.message = message
+ return response
+
+ def _start_calibration(self, pose: str) -> tuple[bool, str]:
+ if self.mapping_mode != "mediapipe_angles":
+ return False, "C/F/P calibration is only used by angle mapping"
+ if pose in {"fist", "pinch"} and not self.dual_mode:
+ return False, f"{pose.capitalize()} calibration requires multi-model mode"
+ if pose in {"fist", "pinch"} and not self.mapper.is_calibrated:
+ return False, "Calibrate the fully-open hand first"
+ if self.control_enabled:
+ self._set_control_enabled(False)
+ now = time.monotonic()
+ self.calibration_session = PoseCalibrationSession(
+ pose,
+ started_at=now,
+ duration=self.calibration_duration,
+ min_samples=self.calibration_min_samples,
+ timeout=self.calibration_timeout,
+ )
+ self.calibration_diagnostics = {}
+ self.calibration_failure_reported = False
+ self.status_message = f"calibrating_{pose}"
+ pose_instruction = {
+ "open": "fully open hand",
+ "fist": "normal closed fist",
+ "pinch": "thumb/index fingertips touching",
+ }[pose]
+ message = (
+ f"Started {pose} calibration; hold a stable {pose_instruction} for "
+ f"{self.calibration_duration:.1f}s"
+ )
+ self.get_logger().info(message)
+ return True, message
+
+ def _calibrate_open_pose(self) -> tuple[bool, str]:
+ return self._start_calibration("open")
+
+ def _calibrate_open_callback(
+ self, request: Trigger.Request, response: Trigger.Response
+ ) -> Trigger.Response:
+ del request
+ success, message = self._calibrate_open_pose()
+ response.success = success
+ response.message = message
+ return response
+
+ def _calibrate_fist_pose(self) -> tuple[bool, str]:
+ return self._start_calibration("fist")
+
+ def _calibrate_fist_callback(
+ self, request: Trigger.Request, response: Trigger.Response
+ ) -> Trigger.Response:
+ del request
+ success, message = self._calibrate_fist_pose()
+ response.success = success
+ response.message = message
+ return response
+
+ def _calibrate_pinch_pose(self) -> tuple[bool, str]:
+ return self._start_calibration("pinch")
+
+ def _calibrate_pinch_callback(
+ self, request: Trigger.Request, response: Trigger.Response
+ ) -> Trigger.Response:
+ del request
+ success, message = self._calibrate_pinch_pose()
+ response.success = success
+ response.message = message
+ return response
+
+ def _report_calibration_failure(self) -> None:
+ session = self.calibration_session
+ if (
+ session is None
+ or session.state != "failed"
+ or self.calibration_failure_reported
+ ):
+ return
+ self.calibration_failure_reported = True
+ self.status_message = f"calibration_failed_{session.pose}"
+ self.get_logger().warning(
+ f"{session.pose.capitalize()} calibration failed: {session.reason}"
+ )
+
+ def _apply_calibration_result(
+ self, session: PoseCalibrationSession, observation: RawHandObservation
+ ) -> None:
+ try:
+ if session.pose == "open":
+ diagnostics = self.mapper.calibrate_open_observation(observation)
+ elif session.pose == "fist":
+ diagnostics = self.mapper.calibrate_fist_observation(observation)
+ else:
+ diagnostics = self.mapper.calibrate_pinch_observation(observation)
+ except (RuntimeError, ValueError) as error:
+ session.fail(str(error))
+ self._report_calibration_failure()
+ return
+ for model, adapter in self.direct_mappers.items():
+ if model != "G20":
+ adapter.reset()
+ self.calibration_diagnostics = diagnostics
+ self.status_message = f"{session.pose}_calibrated"
+ details = ", ".join(
+ f"{name}={value:.3f}" for name, value in diagnostics.items()
+ )
+ self.get_logger().info(
+ f"{session.pose.capitalize()} calibration complete: {details}"
+ )
+
+ def _update_calibration(
+ self, landmarks: np.ndarray | None, now: float
+ ) -> None:
+ session = self.calibration_session
+ if session is None or not session.active:
+ return
+ if landmarks is None:
+ session.reason = "hand_not_detected"
+ session.poll(now)
+ self._report_calibration_failure()
+ return
+ try:
+ observation = self.mapper.observe_landmarks(landmarks)
+ except (ValueError, np.linalg.LinAlgError) as error:
+ session.reason = f"invalid_hand_geometry:{error}"
+ session.poll(now)
+ self._report_calibration_failure()
+ return
+ self.mapper.last_bends = observation.bends.copy()
+ self.mapper.last_pinch_distances = observation.pinch_distances.copy()
+ result = session.add(observation, now)
+ if result is not None:
+ self._apply_calibration_result(session, result)
+ else:
+ self._report_calibration_failure()
+
+ def _handle_detection_lost(self, now: float) -> None:
+ if not self.control_enabled or self.failsafe_sent:
+ return
+ if now - self.last_detection_time < self.lost_timeout:
+ return
+
+ if self.dual_mode or self.lost_behavior == "open":
+ self._publish_control(self._open_commands())
+ self.status_message = "failsafe_open"
+ self.last_failsafe_reason = "hand_tracking_lost"
+ self.get_logger().error(
+ "Hand tracking lost: opening targets and disabling control"
+ )
+ else:
+ self.status_message = "failsafe_hold"
+ self.last_failsafe_reason = "hand_tracking_lost"
+ self.get_logger().error(
+ f"Hand tracking lost: holding {self.hand_model} and disabling control"
+ )
+ self.control_enabled = False
+ self.failsafe_sent = True
+
+ def _handle_driver_lost(self, now: float) -> None:
+ if not self.dual_mode or not self.control_enabled or self.failsafe_sent:
+ return
+ drivers_ready, details = self._driver_health(now)
+ if drivers_ready:
+ return
+ # Publishing both open poses guarantees the still-online hand opens.
+ self._publish_control(self._open_commands())
+ self.status_message = "driver_failsafe_open"
+ self.last_failsafe_reason = f"driver_health_lost: {details}"
+ self.control_enabled = False
+ self.failsafe_sent = True
+ self.get_logger().error(
+ f"Driver health lost: {details}; opening targets and disabling control"
+ )
+
+ def _calibration_status(self, now: float) -> dict:
+ session = self.calibration_session
+ if session is None:
+ return {
+ "pose": None,
+ "state": "idle",
+ "progress": 0.0,
+ "samples": 0,
+ "reason": None,
+ }
+ return {
+ "pose": session.pose,
+ "state": session.state,
+ "progress": round(session.progress(now), 3),
+ "samples": len(session.samples),
+ "min_samples": session.min_samples,
+ "reason": session.reason,
+ "stability": {
+ name: round(value, 4)
+ for name, value in session.last_stability.items()
+ },
+ "diagnostics": {
+ name: round(value, 4)
+ for name, value in self.calibration_diagnostics.items()
+ },
+ }
+
+ def _publish_status(self, now: float) -> None:
+ if now - self.last_status_time < 0.25:
+ return
+ status = String()
+ _, driver_details = self._driver_health(now)
+ status_values = {
+ "hand_model": (
+ self.hand_model
+ if not self.dual_mode
+ else "+".join(self.target_models)
+ ),
+ "target_models": list(self.target_models),
+ "mapping_mode": self.mapping_mode,
+ "open_calibrated": bool(
+ getattr(self.mapper, "is_calibrated", True)
+ ),
+ "fist_calibrated": bool(
+ getattr(self.mapper, "is_fist_calibrated", not self.dual_mode)
+ ),
+ "pinch_calibrated": bool(
+ getattr(self.mapper, "is_pinch_calibrated", not self.dual_mode)
+ ),
+ "control_enabled": self.control_enabled,
+ "hand_detected": self.hand_detected,
+ "stable_frames": self.stable_frames,
+ "stable_frames_required": self.stable_frames_required,
+ "camera_fps": round(self.measured_fps, 1),
+ "state": self.status_message,
+ "last_failsafe_reason": self.last_failsafe_reason,
+ "calibration": self._calibration_status(now),
+ }
+ if self.dual_mode:
+ status_values["drivers"] = driver_details
+ if self.mapping_mode == "mediapipe_angles":
+ status_values["human_closure"] = np.round(
+ self.mapper.last_closure, 2
+ ).tolist()
+ status_values["human_pinch"] = round(self.mapper.last_pinch, 2)
+ status_values["human_pinch_strengths"] = np.round(
+ self.mapper.last_pinch_strengths, 2
+ ).tolist()
+ pinch_target_names = ("index", "middle", "ring", "pinky")
+ status_values["human_pinch_target"] = (
+ pinch_target_names[self.mapper.last_pinch_target]
+ if self.mapper.last_pinch_target >= 0
+ else None
+ )
+ status_values["human_splay_deg"] = np.round(
+ np.degrees(self.mapper.last_splay_delta), 1
+ ).tolist()
+ status_values["splay_confidence"] = np.round(
+ self.mapper.last_splay_confidence, 2
+ ).tolist()
+ status_values["thumb_splay_deg"] = round(
+ float(np.degrees(self.mapper.last_thumb_splay_delta)), 1
+ )
+ status_values["thumb_abduction"] = round(
+ self.mapper.last_thumb_abduction, 2
+ )
+ status_values["thumb_opposition"] = round(
+ self.mapper.last_thumb_opposition, 2
+ )
+ status_values["power_grasp"] = round(
+ self.mapper.last_power_grasp, 2
+ )
+ status.data = json.dumps(status_values, ensure_ascii=False)
+ self.status_publisher.publish(status)
+ self.last_status_time = now
+
+ def _publish_debug(self, now: float) -> None:
+ if (
+ self.debug_publisher is None
+ or self.mapping_mode != "mediapipe_angles"
+ or now - self.last_debug_time < 0.10
+ ):
+ return
+ values: dict = {
+ "calibration": self._calibration_status(now),
+ "raw_bends_rad": np.round(self.mapper.last_bends, 4).tolist(),
+ "raw_pinch_distances": np.round(
+ self.mapper.last_pinch_distances, 4
+ ).tolist(),
+ "normalized_closure": np.round(
+ self.mapper.last_closure, 4
+ ).tolist(),
+ "pinch_strengths": np.round(
+ self.mapper.last_pinch_strengths, 4
+ ).tolist(),
+ "pinch_target": int(self.mapper.last_pinch_target),
+ "power_grasp": round(self.mapper.last_power_grasp, 4),
+ "commands": {
+ "G20": {
+ "raw": np.round(self.mapper.last_raw_command, 2).tolist(),
+ "filtered": np.round(self.mapper.last_command, 2).tolist(),
+ "pinch_blend": round(self.mapper.last_pinch_blend, 4),
+ }
+ },
+ }
+ for model, adapter in self.direct_mappers.items():
+ if model == "G20":
+ continue
+ values["commands"][model] = {
+ "raw": np.round(adapter.last_raw_command, 2).tolist(),
+ "filtered": np.round(adapter.last_command, 2).tolist(),
+ "pinch_blend": round(adapter.last_pinch_blend, 4),
+ }
+ message = String()
+ message.data = json.dumps(values, ensure_ascii=False)
+ self.debug_publisher.publish(message)
+ self.last_debug_time = now
+
+ def _draw_preview(self, frame: np.ndarray) -> None:
+ if not self.show_window:
+ return
+ mode = "REAL CONTROL" if self.control_enabled else "PREVIEW ONLY"
+ color = (0, 0, 255) if self.control_enabled else (0, 220, 255)
+ cv2.putText(frame, mode, (20, 36), cv2.FONT_HERSHEY_SIMPLEX, 0.9, color, 2)
+ cv2.putText(
+ frame,
+ f"hand={'OK' if self.hand_detected else 'LOST'} fps={self.measured_fps:.1f}",
+ (20, 70),
+ cv2.FONT_HERSHEY_SIMPLEX,
+ 0.65,
+ (60, 255, 60) if self.hand_detected else (0, 0, 255),
+ 2,
+ )
+ session = self.calibration_session
+ if session is not None and session.active:
+ progress = int(round(100.0 * session.progress(time.monotonic())))
+ cv2.putText(
+ frame,
+ f"CALIBRATING {session.pose.upper()}: {progress}% "
+ f"({len(session.samples)} samples)",
+ (20, 104),
+ cv2.FONT_HERSHEY_SIMPLEX,
+ 0.65,
+ (255, 180, 40),
+ 2,
+ )
+ cv2.putText(
+ frame,
+ (
+ "C: open+thumb out F: fist P: pinch E/D: enable/disable"
+ if self.dual_mode
+ else "C: calibrate open E: enable D: disable Q: quit"
+ ),
+ (20, frame.shape[0] - 20),
+ cv2.FONT_HERSHEY_SIMPLEX,
+ 0.6,
+ (255, 255, 255),
+ 2,
+ )
+ cv2.imshow(self.window_name, frame)
+ key = cv2.waitKey(1) & 0xFF
+ if key in (ord("c"), ord("C")):
+ success, message = self._calibrate_open_pose()
+ if not success:
+ self.get_logger().warning(message)
+ elif key in (ord("f"), ord("F")):
+ success, message = self._calibrate_fist_pose()
+ if not success:
+ self.get_logger().warning(message)
+ elif key in (ord("p"), ord("P")):
+ success, message = self._calibrate_pinch_pose()
+ if not success:
+ self.get_logger().warning(message)
+ elif key in (ord("e"), ord("E")):
+ success, message = self._set_control_enabled(True)
+ if not success:
+ self.get_logger().warning(message)
+ elif key in (ord("d"), ord("D")):
+ self._set_control_enabled(False)
+ elif key in (ord("q"), ord("Q"), 27):
+ rclpy.shutdown()
+
+ def _process_frame(self) -> None:
+ ok, frame = self.capture.read()
+ now = time.monotonic()
+ frame_interval = max(now - self.last_frame_time, 1e-6)
+ instantaneous_fps = 1.0 / frame_interval
+ self.measured_fps = 0.9 * self.measured_fps + 0.1 * instantaneous_fps
+ self.last_frame_time = now
+
+ if not ok or frame is None:
+ self.hand_detected = False
+ self.stable_frames = 0
+ self._update_calibration(None, now)
+ if (
+ self.calibration_session is not None
+ and self.calibration_session.active
+ ):
+ self.status_message = (
+ f"calibrating_{self.calibration_session.pose}"
+ )
+ elif not (
+ self.calibration_session is not None
+ and self.calibration_session.state == "failed"
+ ):
+ self.status_message = "camera_read_failed"
+ self._handle_driver_lost(now)
+ self._handle_detection_lost(now)
+ self._publish_status(now)
+ self._publish_debug(now)
+ return
+
+ display_frame = cv2.flip(frame, 1) if self.selfie else frame.copy()
+ try:
+ detection = self.detector.detect(frame)
+ except (ValueError, np.linalg.LinAlgError) as error:
+ self.get_logger().warning(f"Invalid hand geometry: {error}")
+ detection = None
+
+ if detection is None:
+ self.hand_detected = False
+ self.stable_frames = 0
+ self._update_calibration(None, now)
+ if not (
+ self.calibration_session is not None
+ and self.calibration_session.active
+ ):
+ self.status_message = "searching_left_hand"
+ self._handle_driver_lost(now)
+ self._handle_detection_lost(now)
+ else:
+ self.hand_detected = True
+ self.stable_frames += 1
+ self.last_detection_time = now
+ self.last_detection_positions = detection.joint_positions.copy()
+ self.failsafe_sent = False
+ self.detector.draw_landmarks(display_frame, detection.image_landmarks)
+ self._update_calibration(detection.joint_positions, now)
+
+ try:
+ if self.mapping_mode == "mediapipe_angles":
+ if self.mapper.is_calibrated:
+ if self.dual_mode:
+ features = self.mapper.extract_features(
+ detection.joint_positions
+ )
+ commands = {
+ model: self.direct_mappers[model].map_features(
+ features
+ )
+ for model in self.target_models
+ }
+ else:
+ commands = {
+ self.hand_model: self.mapper.map_landmarks(
+ detection.joint_positions
+ )
+ }
+ else:
+ commands = self._open_commands()
+ else:
+ indices = self.retargeting.optimizer.target_link_human_indices
+ origin_indices = indices[0, :]
+ task_indices = indices[1, :]
+ reference = (
+ detection.joint_positions[task_indices, :]
+ - detection.joint_positions[origin_indices, :]
+ )
+ qpos = self.retargeting.retarget(reference)
+ commands = {self.hand_model: self.mapper.map(qpos)}
+ except Exception as error:
+ # A transient optimizer failure must behave like lost tracking,
+ # especially while the node is driving physical hardware.
+ self.hand_detected = False
+ self.stable_frames = 0
+ self.status_message = "retargeting_failed"
+ self.get_logger().error(f"Retargeting failed: {error}")
+ self._handle_detection_lost(now)
+ self._publish_status(now)
+ self._publish_debug(now)
+ self._draw_preview(display_frame)
+ return
+ self.last_target_commands = commands
+ self.last_target_command = commands.get(self.hand_model)
+ for model, command in commands.items():
+ self.preview_publishers[model].publish(
+ self._make_joint_state(model, command, include_speed=False)
+ )
+
+ just_enabled = False
+ calibration_active = (
+ self.calibration_session is not None
+ and self.calibration_session.active
+ )
+ if (
+ self.auto_enable_requested
+ and not calibration_active
+ and self.stable_frames >= self.stable_frames_required
+ ):
+ just_enabled, _ = self._set_control_enabled(True)
+ self._handle_driver_lost(now)
+ if (
+ self.control_enabled
+ and self.stable_frames >= self.stable_frames_required
+ and not just_enabled
+ ):
+ self._publish_control(commands)
+ self.status_message = "control_enabled"
+ elif not self.control_enabled:
+ if calibration_active:
+ self.status_message = (
+ f"calibrating_{self.calibration_session.pose}"
+ )
+ elif (
+ self.calibration_session is not None
+ and self.calibration_session.state == "failed"
+ ):
+ self.status_message = (
+ f"calibration_failed_{self.calibration_session.pose}"
+ )
+ elif not getattr(self.mapper, "is_calibrated", True):
+ self.status_message = "needs_open_calibration"
+ elif self.dual_mode and not self.mapper.is_fist_calibrated:
+ self.status_message = "needs_fist_calibration"
+ elif self.dual_mode and not self.mapper.is_pinch_calibrated:
+ self.status_message = "needs_pinch_calibration"
+ elif self.status_message not in {
+ "driver_failsafe_open",
+ "failsafe_open",
+ "failsafe_hold",
+ }:
+ self.status_message = "preview_ready"
+
+ self._publish_status(now)
+ self._publish_debug(now)
+ self._draw_preview(display_frame)
+
+ def destroy_node(self) -> bool:
+ self.control_enabled = False
+ if hasattr(self, "capture"):
+ self.capture.release()
+ if hasattr(self, "detector"):
+ self.detector.close()
+ if self.show_window:
+ cv2.destroyAllWindows()
+ return super().destroy_node()
+
+
+def main(args=None) -> None:
+ rclpy.init(args=args)
+ node: CameraHandRetargetNode | None = None
+ try:
+ node = CameraHandRetargetNode()
+ rclpy.spin(node)
+ except KeyboardInterrupt:
+ pass
+ finally:
+ if node is not None:
+ try:
+ node.destroy_node()
+ except KeyboardInterrupt:
+ pass
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/camera_retarget_l20/camera_retarget_l20/o6_mapping.py b/src/camera_retarget_l20/camera_retarget_l20/o6_mapping.py
new file mode 100644
index 0000000..5adc0c8
--- /dev/null
+++ b/src/camera_retarget_l20/camera_retarget_l20/o6_mapping.py
@@ -0,0 +1,151 @@
+"""Convert calibrated human-hand features to the left O6 six-value protocol."""
+
+from __future__ import annotations
+
+from collections.abc import Sequence
+from dataclasses import replace
+
+import numpy as np
+
+from .hand_features import HandFeatures
+from .model_profiles import (
+ DirectModelProfile,
+ FlexionChannel,
+ ProfiledCommandMapper,
+)
+
+
+O6_COMMAND_NAMES: tuple[str, ...] = (
+ "thumb_cmc_pitch",
+ "thumb_cmc_yaw",
+ "index_mcp_pitch",
+ "middle_mcp_pitch",
+ "ring_mcp_pitch",
+ "pinky_mcp_pitch",
+)
+
+# Open/yaw values come from LinkerHand/config/O6_positions.yaml. The O6
+# protocol and this physical hand accept the full 0..255 flexion range, so the
+# default full-fist profile no longer stops the thumb early at the vendor's
+# conservative value 67.
+DEFAULT_OPEN_POSE: tuple[int, ...] = (255, 179, 255, 255, 255, 255)
+DEFAULT_FIST_POSE: tuple[int, ...] = (0, 151, 0, 0, 0, 0)
+# The vendor file provides only neutral/fist yaw endpoints. Use the same
+# conservative 28-count travel on the other side of neutral for abduction.
+THUMB_ABDUCTED_COMMAND = 207
+
+# Measured on the physical left O6. The single thumb-yaw actuator can align
+# with the index at 80 and reaches only the middle-finger region at its full
+# command-space endpoint. Ring/pinky pinches therefore use maximum thumb travel
+# and rely on the selected finger flexion for the remaining approach.
+PINCH_TARGET_YAW_COMMANDS = np.asarray((80.0, 0.0, 0.0, 0.0), dtype=float)
+
+
+def default_o6_profile() -> DirectModelProfile:
+ """Return the source-compatible built-in O6 profile."""
+
+ return DirectModelProfile(
+ model="O6",
+ command_names=O6_COMMAND_NAMES,
+ open_pose=tuple(float(value) for value in DEFAULT_OPEN_POSE),
+ fist_pose=tuple(float(value) for value in DEFAULT_FIST_POSE),
+ flexion_channels=(
+ FlexionChannel(0, ((0, 0.2), (5, 0.8)), "thumb"),
+ FlexionChannel(2, ((1, 0.35), (6, 0.65)), "index"),
+ FlexionChannel(3, ((2, 0.35), (7, 0.65)), "middle"),
+ FlexionChannel(4, ((3, 0.35), (8, 0.65)), "ring"),
+ FlexionChannel(5, ((4, 0.35), (9, 0.65)), "pinky"),
+ ),
+ side_command_indices=(1,),
+ pinch_target_side_commands=tuple(
+ (float(value),) for value in PINCH_TARGET_YAW_COMMANDS
+ ),
+ power_grasp_side_commands=(float(DEFAULT_FIST_POSE[1]),),
+ abducted_side_commands=(float(THUMB_ABDUCTED_COMMAND),),
+ splay_command_indices=(),
+ splay_scale=1.0,
+ splay_gain=0.0,
+ max_splay_delta=0.0,
+ fixed_channels=(),
+ pinch_thumb_closure=0.85,
+ pinch_finger_closure=0.65,
+ ema_alpha=0.35,
+ max_step=8.0,
+ command_deadband=0.0,
+ motor_speed=60,
+ )
+
+
+class O6MediaPipeMapper:
+ """Compatibility wrapper around the configuration-driven O6 adapter."""
+
+ def __init__(
+ self,
+ *,
+ ema_alpha: float | None = None,
+ max_step: float | None = None,
+ command_deadband: float | None = None,
+ pinch_thumb_closure: float | None = None,
+ pinch_finger_closure: float | None = None,
+ open_pose: Sequence[float] = DEFAULT_OPEN_POSE,
+ fist_pose: Sequence[float] = DEFAULT_FIST_POSE,
+ profile: DirectModelProfile | None = None,
+ ) -> None:
+ base_profile = profile or default_o6_profile()
+ if base_profile.model != "O6":
+ raise ValueError("O6MediaPipeMapper requires an O6 profile")
+ validated_open = self._validate_command(open_pose)
+ validated_fist = self._validate_command(fist_pose)
+ if profile is not None:
+ validated_open = np.asarray(profile.open_pose, dtype=float)
+ validated_fist = np.asarray(profile.fist_pose, dtype=float)
+ base_profile = replace(
+ base_profile,
+ open_pose=tuple(validated_open),
+ fist_pose=tuple(validated_fist),
+ ).with_runtime_overrides(
+ ema_alpha=ema_alpha,
+ max_step=max_step,
+ command_deadband=command_deadband,
+ pinch_thumb_closure=pinch_thumb_closure,
+ pinch_finger_closure=pinch_finger_closure,
+ )
+ self.profile = base_profile
+ self._mapper = ProfiledCommandMapper(base_profile)
+ self.open_pose = self._mapper.open_pose
+ self.fist_pose = self._mapper.fist_pose
+ self.ema_alpha = base_profile.ema_alpha
+ self.max_step = base_profile.max_step
+ self.command_deadband = base_profile.command_deadband
+ self.pinch_thumb_closure = base_profile.pinch_thumb_closure
+ self.pinch_finger_closure = base_profile.pinch_finger_closure
+
+ @staticmethod
+ def _validate_command(command: Sequence[float]) -> np.ndarray:
+ values = np.asarray(command, dtype=float)
+ if values.shape != (6,):
+ raise ValueError("an O6 command must contain six values")
+ if not np.all(np.isfinite(values)):
+ raise ValueError("O6 command contains non-finite values")
+ return np.clip(values, 0.0, 255.0)
+
+ def reset(self, command: Sequence[float] | None = None) -> None:
+ self._mapper.reset(command)
+
+ @property
+ def last_command(self) -> np.ndarray:
+ return self._mapper.last_command
+
+ @property
+ def last_raw_command(self) -> np.ndarray:
+ return self._mapper.last_raw_command
+
+ @property
+ def last_pinch_blend(self) -> float:
+ return self._mapper.last_pinch_blend
+
+ def raw_command(self, features: HandFeatures) -> np.ndarray:
+ return self._mapper.raw_command(features)
+
+ def map_features(self, features: HandFeatures) -> list[int]:
+ return self._mapper.map_features(features)
diff --git a/src/camera_retarget_l20/config/direct_mapping_profiles.yml b/src/camera_retarget_l20/config/direct_mapping_profiles.yml
new file mode 100644
index 0000000..41fa38f
--- /dev/null
+++ b/src/camera_retarget_l20/config/direct_mapping_profiles.yml
@@ -0,0 +1,133 @@
+schema_version: 1
+
+models:
+ O6:
+ command_names:
+ - thumb_cmc_pitch
+ - thumb_cmc_yaw
+ - index_mcp_pitch
+ - middle_mcp_pitch
+ - ring_mcp_pitch
+ - pinky_mcp_pitch
+ open_pose: [255, 179, 255, 255, 255, 255]
+ fist_pose: [0, 151, 0, 0, 0, 0]
+ flexion_channels:
+ - command_index: 0
+ sources: {thumb_root: 0.2, thumb_tip: 0.8}
+ pinch_role: thumb
+ - command_index: 2
+ sources: {index_root: 0.35, index_tip: 0.65}
+ pinch_role: index
+ - command_index: 3
+ sources: {middle_root: 0.35, middle_tip: 0.65}
+ pinch_role: middle
+ - command_index: 4
+ sources: {ring_root: 0.35, ring_tip: 0.65}
+ pinch_role: ring
+ - command_index: 5
+ sources: {pinky_root: 0.35, pinky_tip: 0.65}
+ pinch_role: pinky
+ side:
+ indices: [1]
+ abducted_commands: [207]
+ power_grasp_commands: [151]
+ pinch:
+ thumb_closure: 0.85
+ finger_closure: 0.65
+ power_grasp_start: 0.65
+ power_grasp_end: 0.90
+ target_side_commands:
+ index: [80]
+ middle: [0]
+ ring: [0]
+ pinky: [0]
+ filters:
+ ema_alpha: 0.55
+ max_step: 16.0
+ command_deadband: 2.0
+ motor_speed: 60
+
+ G20:
+ command_names:
+ - Thumb Base
+ - Index Finger Base
+ - Middle Finger Base
+ - Ring Finger Base
+ - Pinky Finger Base
+ - Thumb Abduction
+ - Index Finger Abduction
+ - Middle Finger Abduction
+ - Ring Finger Abduction
+ - Pinky Finger Abduction
+ - Thumb Horizontal Abduction
+ - Reserved 1
+ - Reserved 2
+ - Reserved 3
+ - Reserved 4
+ - Thumb Tip
+ - Index Finger Tip
+ - Middle Finger Tip
+ - Ring Finger Tip
+ - Pinky Finger Tip
+ open_pose: [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ fist_pose: [50, 0, 0, 0, 0, 78, 193, 158, 128, 91, 112, 255, 255, 255, 255, 144, 0, 0, 0, 0]
+ flexion_channels:
+ - command_index: 0
+ sources: {thumb_root: 1.0}
+ pinch_role: thumb
+ - command_index: 1
+ sources: {index_root: 1.0}
+ pinch_role: index
+ - command_index: 2
+ sources: {middle_root: 1.0}
+ pinch_role: middle
+ - command_index: 3
+ sources: {ring_root: 1.0}
+ pinch_role: ring
+ - command_index: 4
+ sources: {pinky_root: 1.0}
+ pinch_role: pinky
+ - command_index: 15
+ sources: {thumb_tip: 1.0}
+ pinch_role: thumb
+ - command_index: 16
+ sources: {index_tip: 1.0}
+ pinch_role: index
+ - command_index: 17
+ sources: {middle_tip: 1.0}
+ pinch_role: middle
+ - command_index: 18
+ sources: {ring_tip: 1.0}
+ pinch_role: ring
+ - command_index: 19
+ sources: {pinky_tip: 1.0}
+ pinch_role: pinky
+ side:
+ indices: [5, 10]
+ abducted_commands: [20, 255]
+ power_grasp_commands: [78, 112]
+ splay:
+ indices: [6, 7, 8, 9]
+ scale: 554.3478260869565
+ gain: 1.0
+ max_delta: 255.0
+ pinch:
+ thumb_closure: 0.85
+ finger_closure: 0.65
+ power_grasp_start: 0.65
+ power_grasp_end: 0.90
+ target_side_commands:
+ index: [167, 184]
+ middle: [122, 148]
+ ring: [78, 112]
+ pinky: [50, 90]
+ fixed_channels:
+ 11: 255
+ 12: 255
+ 13: 255
+ 14: 255
+ filters:
+ ema_alpha: 0.55
+ max_step: 16.0
+ command_deadband: 2.0
+ motor_speed: 60
diff --git a/src/camera_retarget_l20/config/g20_left_mediapipe.yml b/src/camera_retarget_l20/config/g20_left_mediapipe.yml
new file mode 100644
index 0000000..6fbe297
--- /dev/null
+++ b/src/camera_retarget_l20/config/g20_left_mediapipe.yml
@@ -0,0 +1,56 @@
+retargeting:
+ type: vector
+ # Replaced at runtime by the urdf_path parameter.
+ urdf_path: linkerhand_g20_left.urdf
+
+ # The G20 URDF has 21 revolute joints. Five distal joints mimic their PIP
+ # joint, leaving these 16 independent joints for optimization.
+ target_joint_names:
+ - thumb_cmc_roll
+ - thumb_cmc_yaw
+ - thumb_cmc_pitch
+ - thumb_mcp
+ - index_mcp_roll
+ - index_mcp_pitch
+ - index_pip
+ - middle_mcp_roll
+ - middle_mcp_pitch
+ - middle_pip
+ - ring_mcp_roll
+ - ring_mcp_pitch
+ - ring_pip
+ - pinky_mcp_roll
+ - pinky_mcp_pitch
+ - pinky_pip
+
+ target_origin_link_names:
+ - hand_base_link
+ - hand_base_link
+ - hand_base_link
+ - hand_base_link
+ - hand_base_link
+ - hand_base_link
+ - hand_base_link
+ - hand_base_link
+ - hand_base_link
+ - hand_base_link
+ target_task_link_names:
+ - thumb_distal
+ - thumb_metacarpals
+ - index_distal
+ - middle_distal
+ - ring_distal
+ - pinky_distal
+ - index_proximal
+ - middle_proximal
+ - ring_proximal
+ - pinky_proximal
+
+ # MediaPipe Hands order: wrist=0, thumb=1..4, index=5..8,
+ # middle=9..12, ring=13..16, little=17..20.
+ target_link_human_indices:
+ - [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [4, 2, 8, 12, 16, 20, 6, 10, 14, 18]
+
+ scaling_factor: 1.0
+ low_pass_alpha: 0.25
diff --git a/src/camera_retarget_l20/config/l20_left_mediapipe.yml b/src/camera_retarget_l20/config/l20_left_mediapipe.yml
new file mode 100644
index 0000000..5a31b88
--- /dev/null
+++ b/src/camera_retarget_l20/config/l20_left_mediapipe.yml
@@ -0,0 +1,57 @@
+retargeting:
+ type: vector
+ # Replaced at runtime by the urdf_path parameter.
+ urdf_path: linkerhand_l20_left.urdf
+
+ # The L20 has 21 revolute URDF joints, five of which are mimic joints.
+ # Only these 16 independent joints are optimized.
+ target_joint_names:
+ - index_joint0
+ - index_joint1
+ - index_joint2
+ - little_joint0
+ - little_joint1
+ - little_joint2
+ - middle_joint0
+ - middle_joint1
+ - middle_joint2
+ - ring_joint0
+ - ring_joint1
+ - ring_joint2
+ - thumb_joint0
+ - thumb_joint1
+ - thumb_joint2
+ - thumb_joint3
+
+ target_origin_link_names:
+ - base_link
+ - base_link
+ - base_link
+ - base_link
+ - base_link
+ - base_link
+ - base_link
+ - base_link
+ - base_link
+ - base_link
+ target_task_link_names:
+ - thumb_link5
+ - thumb_link3
+ - index_link4
+ - middle_link4
+ - ring_link4
+ - little_link4
+ - index_link2
+ - middle_link2
+ - ring_link2
+ - little_link2
+
+ # MediaPipe Hands order: wrist=0, thumb=1..4, index=5..8,
+ # middle=9..12, ring=13..16, little=17..20.
+ target_link_human_indices:
+ - [0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [4, 2, 8, 12, 16, 20, 6, 10, 14, 18]
+
+ scaling_factor: 1.0
+ low_pass_alpha: 0.25
+
diff --git a/src/camera_retarget_l20/launch/camera_g20_left.launch.py b/src/camera_retarget_l20/launch/camera_g20_left.launch.py
new file mode 100644
index 0000000..bae6223
--- /dev/null
+++ b/src/camera_retarget_l20/launch/camera_g20_left.launch.py
@@ -0,0 +1,62 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+ arguments = [
+ DeclareLaunchArgument("camera_device", default_value="/dev/video0"),
+ DeclareLaunchArgument("show_window", default_value="true"),
+ DeclareLaunchArgument("selfie", default_value="false"),
+ DeclareLaunchArgument("motor_speed", default_value="30"),
+ DeclareLaunchArgument("max_closure", default_value="0.95"),
+ DeclareLaunchArgument("splay_gain", default_value="1.0"),
+ DeclareLaunchArgument("max_splay_delta", default_value="90.0"),
+ DeclareLaunchArgument("thumb_abduction_min_command", default_value="20.0"),
+ DeclareLaunchArgument("thumb_pinch_gain", default_value="2.0"),
+ DeclareLaunchArgument("start_enabled", default_value="false"),
+ ]
+ camera_node = Node(
+ package="camera_retarget_l20",
+ executable="camera_hand_retarget",
+ name="camera_g20_retarget",
+ output="screen",
+ parameters=[
+ {
+ "hand_model": "G20",
+ "mapping_mode": "mediapipe_angles",
+ "camera_device": LaunchConfiguration("camera_device"),
+ "show_window": ParameterValue(
+ LaunchConfiguration("show_window"), value_type=bool
+ ),
+ "selfie": ParameterValue(
+ LaunchConfiguration("selfie"), value_type=bool
+ ),
+ "motor_speed": ParameterValue(
+ LaunchConfiguration("motor_speed"), value_type=int
+ ),
+ "max_closure": ParameterValue(
+ LaunchConfiguration("max_closure"), value_type=float
+ ),
+ "splay_gain": ParameterValue(
+ LaunchConfiguration("splay_gain"), value_type=float
+ ),
+ "max_splay_delta": ParameterValue(
+ LaunchConfiguration("max_splay_delta"), value_type=float
+ ),
+ "thumb_abduction_min_command": ParameterValue(
+ LaunchConfiguration("thumb_abduction_min_command"),
+ value_type=float,
+ ),
+ "thumb_pinch_gain": ParameterValue(
+ LaunchConfiguration("thumb_pinch_gain"), value_type=float
+ ),
+ "start_enabled": ParameterValue(
+ LaunchConfiguration("start_enabled"), value_type=bool
+ ),
+ }
+ ],
+ )
+ return LaunchDescription(arguments + [camera_node])
diff --git a/src/camera_retarget_l20/launch/camera_l20_left.launch.py b/src/camera_retarget_l20/launch/camera_l20_left.launch.py
new file mode 100644
index 0000000..f256fa5
--- /dev/null
+++ b/src/camera_retarget_l20/launch/camera_l20_left.launch.py
@@ -0,0 +1,44 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+ arguments = [
+ DeclareLaunchArgument("camera_device", default_value="/dev/video0"),
+ DeclareLaunchArgument("show_window", default_value="true"),
+ DeclareLaunchArgument("selfie", default_value="false"),
+ DeclareLaunchArgument("motor_speed", default_value="60"),
+ DeclareLaunchArgument("max_closure", default_value="0.80"),
+ DeclareLaunchArgument("start_enabled", default_value="false"),
+ ]
+ camera_node = Node(
+ package="camera_retarget_l20",
+ executable="camera_l20_retarget",
+ name="camera_l20_retarget",
+ output="screen",
+ parameters=[
+ {
+ "camera_device": LaunchConfiguration("camera_device"),
+ "show_window": ParameterValue(
+ LaunchConfiguration("show_window"), value_type=bool
+ ),
+ "selfie": ParameterValue(
+ LaunchConfiguration("selfie"), value_type=bool
+ ),
+ "motor_speed": ParameterValue(
+ LaunchConfiguration("motor_speed"), value_type=int
+ ),
+ "max_closure": ParameterValue(
+ LaunchConfiguration("max_closure"), value_type=float
+ ),
+ "start_enabled": ParameterValue(
+ LaunchConfiguration("start_enabled"), value_type=bool
+ ),
+ }
+ ],
+ )
+ return LaunchDescription(arguments + [camera_node])
+
diff --git a/src/camera_retarget_l20/launch/teleop_g20_left.launch.py b/src/camera_retarget_l20/launch/teleop_g20_left.launch.py
new file mode 100644
index 0000000..a8db473
--- /dev/null
+++ b/src/camera_retarget_l20/launch/teleop_g20_left.launch.py
@@ -0,0 +1,79 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+ arguments = [
+ DeclareLaunchArgument("camera_device", default_value="/dev/video0"),
+ DeclareLaunchArgument("can", default_value="can0"),
+ DeclareLaunchArgument("show_window", default_value="true"),
+ DeclareLaunchArgument("selfie", default_value="false"),
+ DeclareLaunchArgument("motor_speed", default_value="30"),
+ DeclareLaunchArgument("max_closure", default_value="0.95"),
+ DeclareLaunchArgument("splay_gain", default_value="1.0"),
+ DeclareLaunchArgument("max_splay_delta", default_value="90.0"),
+ DeclareLaunchArgument("thumb_abduction_min_command", default_value="20.0"),
+ DeclareLaunchArgument("thumb_pinch_gain", default_value="2.0"),
+ ]
+ hand_driver = Node(
+ package="linker_hand_ros2_sdk",
+ executable="linker_hand_sdk",
+ name="left_g20_driver",
+ output="screen",
+ parameters=[
+ {
+ "hand_type": "left",
+ "hand_joint": "G20",
+ "is_touch": False,
+ "can": LaunchConfiguration("can"),
+ "modbus": "None",
+ "startup_speed": ParameterValue(
+ LaunchConfiguration("motor_speed"), value_type=int
+ ),
+ }
+ ],
+ )
+ camera_node = Node(
+ package="camera_retarget_l20",
+ executable="camera_hand_retarget",
+ name="camera_g20_retarget",
+ output="screen",
+ parameters=[
+ {
+ "hand_model": "G20",
+ "mapping_mode": "mediapipe_angles",
+ "camera_device": LaunchConfiguration("camera_device"),
+ "show_window": ParameterValue(
+ LaunchConfiguration("show_window"), value_type=bool
+ ),
+ "selfie": ParameterValue(
+ LaunchConfiguration("selfie"), value_type=bool
+ ),
+ "motor_speed": ParameterValue(
+ LaunchConfiguration("motor_speed"), value_type=int
+ ),
+ "max_closure": ParameterValue(
+ LaunchConfiguration("max_closure"), value_type=float
+ ),
+ "splay_gain": ParameterValue(
+ LaunchConfiguration("splay_gain"), value_type=float
+ ),
+ "max_splay_delta": ParameterValue(
+ LaunchConfiguration("max_splay_delta"), value_type=float
+ ),
+ "thumb_abduction_min_command": ParameterValue(
+ LaunchConfiguration("thumb_abduction_min_command"),
+ value_type=float,
+ ),
+ "thumb_pinch_gain": ParameterValue(
+ LaunchConfiguration("thumb_pinch_gain"), value_type=float
+ ),
+ # Hardware output still requires an explicit E key/service call.
+ "start_enabled": False,
+ }
+ ],
+ )
+ return LaunchDescription(arguments + [hand_driver, camera_node])
diff --git a/src/camera_retarget_l20/launch/teleop_l20_left.launch.py b/src/camera_retarget_l20/launch/teleop_l20_left.launch.py
new file mode 100644
index 0000000..5a8df05
--- /dev/null
+++ b/src/camera_retarget_l20/launch/teleop_l20_left.launch.py
@@ -0,0 +1,56 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+def generate_launch_description():
+ arguments = [
+ DeclareLaunchArgument("camera_device", default_value="/dev/video0"),
+ DeclareLaunchArgument("can", default_value="can0"),
+ DeclareLaunchArgument("show_window", default_value="true"),
+ DeclareLaunchArgument("motor_speed", default_value="60"),
+ DeclareLaunchArgument("max_closure", default_value="0.80"),
+ ]
+ hand_driver = Node(
+ package="linker_hand_ros2_sdk",
+ executable="linker_hand_sdk",
+ name="left_l20_driver",
+ output="screen",
+ parameters=[
+ {
+ "hand_type": "left",
+ "hand_joint": "L20",
+ "is_touch": False,
+ "can": LaunchConfiguration("can"),
+ "modbus": "None",
+ "startup_speed": ParameterValue(
+ LaunchConfiguration("motor_speed"), value_type=int
+ ),
+ }
+ ],
+ )
+ camera_node = Node(
+ package="camera_retarget_l20",
+ executable="camera_l20_retarget",
+ name="camera_l20_retarget",
+ output="screen",
+ parameters=[
+ {
+ "camera_device": LaunchConfiguration("camera_device"),
+ "show_window": ParameterValue(
+ LaunchConfiguration("show_window"), value_type=bool
+ ),
+ "motor_speed": ParameterValue(
+ LaunchConfiguration("motor_speed"), value_type=int
+ ),
+ "max_closure": ParameterValue(
+ LaunchConfiguration("max_closure"), value_type=float
+ ),
+ # Hardware commands still require an explicit E key/service call.
+ "start_enabled": False,
+ }
+ ],
+ )
+ return LaunchDescription(arguments + [hand_driver, camera_node])
diff --git a/src/camera_retarget_l20/launch/teleop_o6_g20_left.launch.py b/src/camera_retarget_l20/launch/teleop_o6_g20_left.launch.py
new file mode 100644
index 0000000..f04e55e
--- /dev/null
+++ b/src/camera_retarget_l20/launch/teleop_o6_g20_left.launch.py
@@ -0,0 +1,187 @@
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+
+
+O6_OPEN_POSE = [255, 179, 255, 255, 255, 255]
+G20_OPEN_POSE = [
+ 255, 255, 255, 255, 255,
+ 255, 193, 148, 105, 42,
+ 255, 255, 255, 255, 255,
+ 255, 255, 255, 255, 255,
+]
+
+
+def generate_launch_description():
+ arguments = [
+ DeclareLaunchArgument("camera_device", default_value="/dev/video0"),
+ DeclareLaunchArgument("g20_can", default_value="can0"),
+ DeclareLaunchArgument("o6_can", default_value="can1"),
+ DeclareLaunchArgument("show_window", default_value="true"),
+ DeclareLaunchArgument("selfie", default_value="false"),
+ DeclareLaunchArgument("motor_speed", default_value="60"),
+ DeclareLaunchArgument("o6_state_poll_rate", default_value="20.0"),
+ DeclareLaunchArgument("g20_state_poll_rate", default_value="10.0"),
+ DeclareLaunchArgument("o6_velocity_poll_rate", default_value="10.0"),
+ DeclareLaunchArgument("g20_velocity_poll_rate", default_value="2.0"),
+ DeclareLaunchArgument("ema_alpha", default_value="0.55"),
+ DeclareLaunchArgument("max_step", default_value="16.0"),
+ DeclareLaunchArgument("command_deadband", default_value="2.0"),
+ # Use the measured per-joint fist endpoints without an additional
+ # closure reduction. Dynamic filtering and timeout protection remain.
+ DeclareLaunchArgument("max_closure", default_value="1.0"),
+ DeclareLaunchArgument("splay_gain", default_value="1.0"),
+ DeclareLaunchArgument("max_splay_delta", default_value="255.0"),
+ DeclareLaunchArgument("thumb_abduction_min_command", default_value="20.0"),
+ DeclareLaunchArgument("thumb_pinch_gain", default_value="2.0"),
+ DeclareLaunchArgument("g20_pinch_thumb_closure", default_value="0.85"),
+ DeclareLaunchArgument("g20_pinch_finger_closure", default_value="0.65"),
+ DeclareLaunchArgument("o6_pinch_thumb_closure", default_value="0.85"),
+ DeclareLaunchArgument("o6_pinch_finger_closure", default_value="0.65"),
+ DeclareLaunchArgument("driver_state_timeout", default_value="1.0"),
+ DeclareLaunchArgument("model_profile_file", default_value=""),
+ DeclareLaunchArgument("calibration_duration", default_value="1.5"),
+ DeclareLaunchArgument("calibration_min_samples", default_value="30"),
+ DeclareLaunchArgument("calibration_timeout", default_value="8.0"),
+ DeclareLaunchArgument("publish_debug", default_value="true"),
+ ]
+
+ o6_driver = Node(
+ package="linker_hand_ros2_sdk",
+ executable="linker_hand_sdk",
+ name="left_o6_driver",
+ output="screen",
+ parameters=[
+ {
+ "hand_type": "left",
+ "hand_joint": "O6",
+ "is_touch": False,
+ "can": LaunchConfiguration("o6_can"),
+ "modbus": "None",
+ "topic_prefix": "/o6",
+ "startup_pose": O6_OPEN_POSE,
+ "startup_speed": ParameterValue(
+ LaunchConfiguration("motor_speed"), value_type=int
+ ),
+ "state_poll_rate": ParameterValue(
+ LaunchConfiguration("o6_state_poll_rate"), value_type=float
+ ),
+ "velocity_poll_rate": ParameterValue(
+ LaunchConfiguration("o6_velocity_poll_rate"),
+ value_type=float,
+ ),
+ }
+ ],
+ )
+ g20_driver = Node(
+ package="linker_hand_ros2_sdk",
+ executable="linker_hand_sdk",
+ name="left_g20_driver",
+ output="screen",
+ parameters=[
+ {
+ "hand_type": "left",
+ "hand_joint": "G20",
+ "is_touch": False,
+ "can": LaunchConfiguration("g20_can"),
+ "modbus": "None",
+ "topic_prefix": "/g20",
+ "startup_pose": G20_OPEN_POSE,
+ "startup_speed": ParameterValue(
+ LaunchConfiguration("motor_speed"), value_type=int
+ ),
+ "state_poll_rate": ParameterValue(
+ LaunchConfiguration("g20_state_poll_rate"), value_type=float
+ ),
+ "velocity_poll_rate": ParameterValue(
+ LaunchConfiguration("g20_velocity_poll_rate"),
+ value_type=float,
+ ),
+ }
+ ],
+ )
+ camera_node = Node(
+ package="camera_retarget_l20",
+ executable="camera_hand_retarget",
+ name="camera_o6_g20_retarget",
+ output="screen",
+ parameters=[
+ {
+ "target_models": ["O6", "G20"],
+ "mapping_mode": "mediapipe_angles",
+ "camera_device": LaunchConfiguration("camera_device"),
+ "show_window": ParameterValue(
+ LaunchConfiguration("show_window"), value_type=bool
+ ),
+ "selfie": ParameterValue(
+ LaunchConfiguration("selfie"), value_type=bool
+ ),
+ "motor_speed": ParameterValue(
+ LaunchConfiguration("motor_speed"), value_type=int
+ ),
+ "ema_alpha": ParameterValue(
+ LaunchConfiguration("ema_alpha"), value_type=float
+ ),
+ "max_step": ParameterValue(
+ LaunchConfiguration("max_step"), value_type=float
+ ),
+ "command_deadband": ParameterValue(
+ LaunchConfiguration("command_deadband"), value_type=float
+ ),
+ "max_closure": ParameterValue(
+ LaunchConfiguration("max_closure"), value_type=float
+ ),
+ "splay_gain": ParameterValue(
+ LaunchConfiguration("splay_gain"), value_type=float
+ ),
+ "max_splay_delta": ParameterValue(
+ LaunchConfiguration("max_splay_delta"), value_type=float
+ ),
+ "thumb_abduction_min_command": ParameterValue(
+ LaunchConfiguration("thumb_abduction_min_command"),
+ value_type=float,
+ ),
+ "thumb_pinch_gain": ParameterValue(
+ LaunchConfiguration("thumb_pinch_gain"), value_type=float
+ ),
+ "g20_pinch_thumb_closure": ParameterValue(
+ LaunchConfiguration("g20_pinch_thumb_closure"),
+ value_type=float,
+ ),
+ "g20_pinch_finger_closure": ParameterValue(
+ LaunchConfiguration("g20_pinch_finger_closure"),
+ value_type=float,
+ ),
+ "o6_pinch_thumb_closure": ParameterValue(
+ LaunchConfiguration("o6_pinch_thumb_closure"),
+ value_type=float,
+ ),
+ "o6_pinch_finger_closure": ParameterValue(
+ LaunchConfiguration("o6_pinch_finger_closure"),
+ value_type=float,
+ ),
+ "driver_state_timeout": ParameterValue(
+ LaunchConfiguration("driver_state_timeout"), value_type=float
+ ),
+ "model_profile_file": ParameterValue(
+ LaunchConfiguration("model_profile_file"), value_type=str
+ ),
+ "calibration_duration": ParameterValue(
+ LaunchConfiguration("calibration_duration"), value_type=float
+ ),
+ "calibration_min_samples": ParameterValue(
+ LaunchConfiguration("calibration_min_samples"), value_type=int
+ ),
+ "calibration_timeout": ParameterValue(
+ LaunchConfiguration("calibration_timeout"), value_type=float
+ ),
+ "publish_debug": ParameterValue(
+ LaunchConfiguration("publish_debug"), value_type=bool
+ ),
+ "start_enabled": False,
+ }
+ ],
+ )
+ return LaunchDescription(arguments + [o6_driver, g20_driver, camera_node])
diff --git a/src/camera_retarget_l20/package.xml b/src/camera_retarget_l20/package.xml
new file mode 100644
index 0000000..3070b16
--- /dev/null
+++ b/src/camera_retarget_l20/package.xml
@@ -0,0 +1,23 @@
+
+
+
+ camera_retarget_l20
+ 0.2.0
+ Camera retargeting bridge for physical left LinkerHand O6/G20/L20 hands.
+ lxp
+ MIT
+
+ ament_index_python
+ rclpy
+ sensor_msgs
+ std_msgs
+ std_srvs
+ python3-opencv
+ python3-yaml
+
+ python3-pytest
+
+
+ ament_python
+
+
diff --git a/src/camera_retarget_l20/pyproject.toml b/src/camera_retarget_l20/pyproject.toml
new file mode 100644
index 0000000..3372b34
--- /dev/null
+++ b/src/camera_retarget_l20/pyproject.toml
@@ -0,0 +1,4 @@
+[build-system]
+requires = ["setuptools>=61"]
+build-backend = "setuptools.build_meta"
+
diff --git a/src/camera_retarget_l20/resource/camera_retarget_l20 b/src/camera_retarget_l20/resource/camera_retarget_l20
new file mode 100644
index 0000000..8b13789
--- /dev/null
+++ b/src/camera_retarget_l20/resource/camera_retarget_l20
@@ -0,0 +1 @@
+
diff --git a/src/camera_retarget_l20/setup.cfg b/src/camera_retarget_l20/setup.cfg
new file mode 100644
index 0000000..c0ce090
--- /dev/null
+++ b/src/camera_retarget_l20/setup.cfg
@@ -0,0 +1,5 @@
+[develop]
+script_dir=$base/lib/camera_retarget_l20
+[install]
+install_scripts=$base/lib/camera_retarget_l20
+
diff --git a/src/camera_retarget_l20/setup.py b/src/camera_retarget_l20/setup.py
new file mode 100644
index 0000000..2fd23b2
--- /dev/null
+++ b/src/camera_retarget_l20/setup.py
@@ -0,0 +1,32 @@
+from glob import glob
+from setuptools import find_packages, setup
+
+
+package_name = "camera_retarget_l20"
+
+setup(
+ name=package_name,
+ version="0.2.0",
+ packages=find_packages(),
+ data_files=[
+ (
+ "share/ament_index/resource_index/packages",
+ ["resource/" + package_name],
+ ),
+ ("share/" + package_name, ["package.xml"]),
+ ("share/" + package_name + "/config", glob("config/*.yml")),
+ ("share/" + package_name + "/launch", glob("launch/*.launch.py")),
+ ],
+ install_requires=["setuptools", "numpy", "PyYAML"],
+ zip_safe=True,
+ maintainer="lxp",
+ maintainer_email="support@linker-robotics.com",
+ description="Camera retargeting bridge for physical left LinkerHand O6/G20/L20 hands",
+ license="MIT",
+ entry_points={
+ "console_scripts": [
+ "camera_hand_retarget = camera_retarget_l20.node:main",
+ "camera_l20_retarget = camera_retarget_l20.node:main",
+ ],
+ },
+)
diff --git a/src/camera_retarget_l20/test/test_calibration_capture.py b/src/camera_retarget_l20/test/test_calibration_capture.py
new file mode 100644
index 0000000..8a19af7
--- /dev/null
+++ b/src/camera_retarget_l20/test/test_calibration_capture.py
@@ -0,0 +1,73 @@
+import numpy as np
+
+from camera_retarget_l20.calibration import (
+ PoseCalibrationSession,
+ RawHandObservation,
+)
+
+
+def observation(offset=0.0):
+ return RawHandObservation(
+ bends=np.full(10, 0.2 + offset),
+ pinch_distances=np.asarray([1.2, 1.4, 1.6, 1.8]) + offset,
+ splay=np.asarray([-0.2, -0.05, 0.05, 0.2]) + offset,
+ splay_confidence=np.ones(4),
+ thumb_splay=0.3 + offset,
+ thumb_splay_confidence=1.0,
+ )
+
+
+def test_stable_window_completes_after_duration_and_minimum_samples():
+ session = PoseCalibrationSession(
+ "open", started_at=0.0, duration=1.5, min_samples=30, timeout=8.0
+ )
+ result = None
+ for index in range(31):
+ result = session.add(observation(), index * 0.05)
+
+ assert session.state == "complete"
+ assert result is not None
+ assert result.bends.tolist() == [0.2] * 10
+ assert session.progress(1.5) == 1.0
+
+
+def test_coordinate_median_rejects_one_large_outlier():
+ session = PoseCalibrationSession(
+ "pinch", started_at=0.0, duration=1.5, min_samples=30, timeout=8.0
+ )
+ result = None
+ for index in range(31):
+ sample = observation(2.0 if index == 15 else 0.0)
+ result = session.add(sample, index * 0.05)
+
+ assert session.state == "complete"
+ assert result is not None
+ assert np.allclose(result.bends, 0.2)
+ assert result.pinch_distances[0] == 1.2
+
+
+def test_unstable_window_waits_then_times_out_without_result():
+ session = PoseCalibrationSession(
+ "fist", started_at=0.0, duration=1.5, min_samples=30, timeout=2.0
+ )
+ for index in range(39):
+ offset = 0.2 if index % 2 else -0.2
+ assert session.add(observation(offset), index * 0.05) is None
+
+ assert session.active
+ assert session.reason == "bend_not_stable"
+ session.poll(2.01)
+ assert session.state == "failed"
+ assert session.result is None
+ assert session.reason.startswith("calibration_timeout:")
+
+
+def test_missing_detection_times_out_without_samples():
+ session = PoseCalibrationSession(
+ "open", started_at=10.0, duration=1.5, min_samples=30, timeout=8.0
+ )
+ session.reason = "hand_not_detected"
+ session.poll(18.1)
+
+ assert session.state == "failed"
+ assert session.reason == "calibration_timeout:hand_not_detected"
diff --git a/src/camera_retarget_l20/test/test_dual_calibration.py b/src/camera_retarget_l20/test/test_dual_calibration.py
new file mode 100644
index 0000000..4737773
--- /dev/null
+++ b/src/camera_retarget_l20/test/test_dual_calibration.py
@@ -0,0 +1,236 @@
+import numpy as np
+import pytest
+
+from camera_retarget_l20.g20_mapping import (
+ DEFAULT_FIST_POSE as G20_DEFAULT_FIST_POSE,
+ DEFAULT_OPEN_POSE as G20_DEFAULT_OPEN_POSE,
+ PINCH_TARGET_OPPOSITION_COMMANDS,
+ G20MediaPipeMapper,
+ PINCH_CONFIRM_FRAMES,
+)
+from camera_retarget_l20.o6_mapping import DEFAULT_FIST_POSE, O6MediaPipeMapper
+
+
+def make_open_landmarks():
+ points = np.zeros((21, 3), dtype=float)
+ points[1:5] = np.array(
+ [[-0.20, 0.35, 0.0], [-0.40, 0.60, 0.0],
+ [-0.60, 0.85, 0.0], [-0.80, 1.10, 0.0]]
+ )
+ for root, x in zip((5, 9, 13, 17), (-0.45, -0.15, 0.15, 0.45)):
+ points[root:root + 4] = np.array(
+ [[x, 1.0, 0.0], [x, 1.7, 0.0],
+ [x, 2.3, 0.0], [x, 2.8, 0.0]]
+ )
+ return points
+
+
+def make_fist_landmarks():
+ points = make_open_landmarks()
+ points[3] = points[2] + np.array([0.0, 0.0, 0.6])
+ points[4] = points[3] + np.array([0.0, -0.5, 0.0])
+ for root in (5, 9, 13, 17):
+ points[root + 1] = points[root] + np.array([0.0, 0.0, 0.7])
+ points[root + 2] = points[root + 1] + np.array([0.0, -0.6, 0.0])
+ points[root + 3] = points[root + 2] + np.array([0.0, 0.0, -0.5])
+ return points
+
+
+def make_pinch_landmarks(finger_tip_index=8):
+ points = make_open_landmarks()
+ points[4] = points[finger_tip_index].copy()
+ return points
+
+
+def make_pinch_landmarks_with_monocular_depth_error():
+ points = make_pinch_landmarks()
+ # The fingertips overlap in the palm plane, but monocular world-landmark
+ # depth disagrees by almost one palm width.
+ points[4, 2] += 0.8
+ return points
+
+
+def make_fist_with_opposed_thumb():
+ points = make_fist_landmarks()
+ points[2] = points[1] + np.array([0.0, 0.4, 0.0])
+ points[3] = points[2] + np.array([0.0, 0.0, 0.6])
+ points[4] = points[3] + np.array([0.0, -0.5, 0.0])
+ return points
+
+
+def make_four_finger_fist_with_open_thumb():
+ points = make_fist_landmarks()
+ points[1:5] = make_open_landmarks()[1:5]
+ return points
+
+
+def test_fist_calibration_requires_open_pose():
+ mapper = G20MediaPipeMapper()
+ with pytest.raises(RuntimeError, match="open-hand"):
+ mapper.calibrate_fist(make_fist_landmarks())
+
+
+def test_fist_calibration_rejects_insufficient_motion():
+ mapper = G20MediaPipeMapper()
+ open_landmarks = make_open_landmarks()
+ mapper.calibrate_open(open_landmarks)
+ with pytest.raises(ValueError, match="rejected"):
+ mapper.calibrate_fist(open_landmarks)
+
+
+def test_one_calibrated_feature_frame_drives_both_models():
+ extractor_and_g20 = G20MediaPipeMapper(
+ ema_alpha=1.0, max_step=255.0, max_closure=1.0
+ )
+ o6 = O6MediaPipeMapper(ema_alpha=1.0, max_step=255.0)
+ extractor_and_g20.calibrate_open(make_open_landmarks())
+ extractor_and_g20.calibrate_fist(make_fist_landmarks())
+ extractor_and_g20.calibrate_pinch(make_pinch_landmarks())
+ features = extractor_and_g20.extract_features(make_fist_landmarks())
+
+ g20_command = extractor_and_g20.map_features(features)
+ o6_command = o6.map_features(features)
+
+ assert len(g20_command) == 20
+ assert len(o6_command) == 6
+ assert features.thumb_opposition == pytest.approx(1.0)
+ assert g20_command[0] == G20_DEFAULT_FIST_POSE[0]
+ assert g20_command[15] == G20_DEFAULT_FIST_POSE[15]
+ assert o6_command[0] == DEFAULT_FIST_POSE[0]
+ assert o6_command[2:] == list(DEFAULT_FIST_POSE[2:])
+
+
+def test_open_fist_and_pinch_complete_three_pose_calibration():
+ mapper = G20MediaPipeMapper()
+ mapper.calibrate_open(make_open_landmarks())
+ mapper.calibrate_fist(make_fist_landmarks())
+ mapper.calibrate_pinch(make_pinch_landmarks())
+
+ for _ in range(PINCH_CONFIRM_FRAMES):
+ pinched = mapper.extract_features(make_pinch_landmarks())
+
+ assert mapper.is_three_pose_calibrated
+ assert pinched.thumb_abduction == pytest.approx(0.0)
+ assert pinched.thumb_opposition == pytest.approx(0.0)
+ assert pinched.pinch_strengths[0] == pytest.approx(1.0)
+
+
+@pytest.mark.parametrize(
+ ("finger_tip_index", "finger_offset"),
+ ((8, 0), (12, 1), (16, 2), (20, 3)),
+)
+def test_g20_pinch_selects_nearest_finger_without_curling_an_open_target(
+ finger_tip_index, finger_offset
+):
+ mapper = G20MediaPipeMapper(
+ ema_alpha=1.0,
+ max_step=255.0,
+ max_closure=0.95,
+ pinch_thumb_closure=0.85,
+ pinch_finger_closure=0.65,
+ )
+ mapper.calibrate_open(make_open_landmarks())
+ mapper.calibrate_fist(make_fist_landmarks())
+ mapper.calibrate_pinch(make_pinch_landmarks())
+
+ for _ in range(PINCH_CONFIRM_FRAMES):
+ features = mapper.extract_features(make_pinch_landmarks(finger_tip_index))
+ command = mapper.raw_command_from_features(features)
+
+ assert features.pinch_strengths[finger_offset] == pytest.approx(1.0)
+ assert np.count_nonzero(features.pinch_strengths) == 1
+ thumb_activation = 0.85 * 0.95
+ assert command[0] == pytest.approx(
+ G20_DEFAULT_OPEN_POSE[0]
+ + thumb_activation
+ * (G20_DEFAULT_FIST_POSE[0] - G20_DEFAULT_OPEN_POSE[0])
+ )
+ assert command[15] == pytest.approx(
+ G20_DEFAULT_OPEN_POSE[15]
+ + thumb_activation
+ * (G20_DEFAULT_FIST_POSE[15] - G20_DEFAULT_OPEN_POSE[15])
+ )
+ assert command[5] == pytest.approx(
+ PINCH_TARGET_OPPOSITION_COMMANDS[finger_offset, 0]
+ )
+ assert command[10] == pytest.approx(
+ PINCH_TARGET_OPPOSITION_COMMANDS[finger_offset, 1]
+ )
+ for offset in range(4):
+ if finger_offset in (0, 1) and offset == finger_offset:
+ assert command[1 + offset] < 110.0
+ assert command[16 + offset] < 110.0
+ else:
+ assert command[1 + offset] == G20_DEFAULT_OPEN_POSE[1 + offset]
+ assert command[16 + offset] == G20_DEFAULT_OPEN_POSE[16 + offset]
+
+
+def test_pinch_calibration_validates_operator_motion():
+ mapper = G20MediaPipeMapper()
+ open_landmarks = make_open_landmarks()
+ mapper.calibrate_open(open_landmarks)
+
+ with pytest.raises(ValueError, match="pinch calibration rejected"):
+ mapper.calibrate_pinch(open_landmarks)
+
+
+def test_pinch_calibration_tolerates_monocular_depth_error_at_visual_contact():
+ mapper = G20MediaPipeMapper()
+ mapper.calibrate_open(make_open_landmarks())
+
+ result = mapper.calibrate_pinch(
+ make_pinch_landmarks_with_monocular_depth_error()
+ )
+
+ assert result["pinch_ratio"] <= 0.35
+ assert result["pinch_reduction_ratio"] >= 0.60
+
+
+def test_pinch_target_requires_confirmation_and_releases_below_threshold():
+ mapper = G20MediaPipeMapper()
+ mapper.calibrate_open(make_open_landmarks())
+ mapper.calibrate_fist(make_fist_landmarks())
+ mapper.calibrate_pinch(make_pinch_landmarks())
+
+ for _ in range(PINCH_CONFIRM_FRAMES - 1):
+ features = mapper.extract_features(make_pinch_landmarks(12))
+ assert features.pinch_strengths.tolist() == [0.0] * 4
+
+ features = mapper.extract_features(make_pinch_landmarks(12))
+ assert features.pinch_strengths[1] == pytest.approx(1.0)
+
+ released = mapper.extract_features(make_open_landmarks())
+ assert released.pinch_strengths.tolist() == [0.0] * 4
+ assert mapper.last_pinch_target == -1
+
+
+def test_calibrated_thumb_side_motion_is_independent_from_four_finger_grasp():
+ mapper = G20MediaPipeMapper(
+ ema_alpha=1.0, max_step=255.0, max_closure=1.0
+ )
+ o6 = O6MediaPipeMapper(ema_alpha=1.0, max_step=255.0)
+ mapper.calibrate_open(make_open_landmarks())
+ mapper.calibrate_fist(make_fist_with_opposed_thumb())
+ mapper.calibrate_pinch(make_pinch_landmarks())
+
+ opposed_features = mapper.extract_features(make_fist_with_opposed_thumb())
+ opposed_g20 = mapper.raw_command_from_features(opposed_features)
+ opposed_o6 = o6.raw_command(opposed_features)
+ assert opposed_features.thumb_opposition == pytest.approx(1.0)
+ assert opposed_g20[[0, 15]].tolist() == [
+ G20_DEFAULT_FIST_POSE[0],
+ G20_DEFAULT_FIST_POSE[15],
+ ]
+ assert opposed_g20[[5, 10]].tolist() == [78.0, 112.0]
+ assert opposed_o6[0] == DEFAULT_FIST_POSE[0]
+ assert opposed_o6[1] == DEFAULT_FIST_POSE[1]
+
+ open_thumb_features = mapper.extract_features(
+ make_four_finger_fist_with_open_thumb()
+ )
+ open_thumb_g20 = mapper.raw_command_from_features(open_thumb_features)
+ open_thumb_o6 = o6.raw_command(open_thumb_features)
+ assert open_thumb_features.power_grasp == pytest.approx(1.0)
+ assert open_thumb_features.thumb_opposition == pytest.approx(0.0)
+ assert open_thumb_g20[[0, 5, 10, 15]].tolist() == [255.0] * 4
+ assert open_thumb_o6[[0, 1]].tolist() == [255.0, 179.0]
diff --git a/src/camera_retarget_l20/test/test_g20_mapping.py b/src/camera_retarget_l20/test/test_g20_mapping.py
new file mode 100644
index 0000000..b720713
--- /dev/null
+++ b/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]
diff --git a/src/camera_retarget_l20/test/test_l20_mapping.py b/src/camera_retarget_l20/test/test_l20_mapping.py
new file mode 100644
index 0000000..e1dffc5
--- /dev/null
+++ b/src/camera_retarget_l20/test/test_l20_mapping.py
@@ -0,0 +1,93 @@
+import numpy as np
+import pytest
+
+from camera_retarget_l20.l20_mapping import (
+ FLEXION_MOTOR_INDICES,
+ MOTOR_JOINT_NAMES,
+ RESERVED_MOTOR_INDICES,
+ L20CommandMapper,
+)
+
+
+JOINT_LIMITS = {
+ "thumb_joint0": (-0.297, 0.683),
+ "thumb_joint1": (0.122, 1.78),
+ "thumb_joint2": (0.0, 0.87),
+ "thumb_joint3": (0.0, 1.29),
+ "thumb_joint4": (0.0, 1.29),
+ "index_joint0": (-0.26, 0.26),
+ "index_joint1": (0.0, 1.4),
+ "index_joint2": (0.0, 1.08),
+ "index_joint3": (0.0, 1.15),
+ "middle_joint0": (-0.26, 0.26),
+ "middle_joint1": (0.0, 1.4),
+ "middle_joint2": (0.0, 1.08),
+ "middle_joint3": (0.0, 1.15),
+ "ring_joint0": (-0.26, 0.26),
+ "ring_joint1": (0.0, 1.4),
+ "ring_joint2": (0.0, 1.08),
+ "ring_joint3": (0.0, 1.15),
+ "little_joint0": (-0.26, 0.26),
+ "little_joint1": (0.0, 1.4),
+ "little_joint2": (0.0, 1.08),
+ "little_joint3": (0.0, 1.15),
+}
+
+
+def make_mapper(**kwargs):
+ names = list(JOINT_LIMITS)
+ limits = [JOINT_LIMITS[name] for name in names]
+ return L20CommandMapper(
+ names,
+ limits,
+ ema_alpha=kwargs.pop("ema_alpha", 1.0),
+ max_step=kwargs.pop("max_step", 255.0),
+ max_closure=kwargs.pop("max_closure", 1.0),
+ **kwargs,
+ )
+
+
+def qpos_at(limit_index):
+ return np.array([limits[limit_index] for limits in JOINT_LIMITS.values()])
+
+
+def test_lower_limits_map_to_open_end_of_protocol_range():
+ mapper = make_mapper()
+ command = mapper.raw_command(qpos_at(0))
+ for motor_index, name in enumerate(MOTOR_JOINT_NAMES):
+ assert command[motor_index] == 255.0
+ if name is None:
+ assert motor_index in RESERVED_MOTOR_INDICES
+
+
+def test_upper_limits_map_to_zero_except_reserved_slots():
+ mapper = make_mapper()
+ command = mapper.raw_command(qpos_at(1))
+ for motor_index, name in enumerate(MOTOR_JOINT_NAMES):
+ expected = 255.0 if name is None else 0.0
+ assert command[motor_index] == expected
+
+
+def test_max_closure_only_limits_flexion_motors():
+ mapper = make_mapper(max_closure=0.60)
+ command = mapper.raw_command(qpos_at(1))
+ for index in FLEXION_MOTOR_INDICES:
+ assert command[index] == pytest.approx(102.0)
+ for index in (5, 6, 7, 8, 9, 10):
+ assert command[index] == 0.0
+
+
+def test_filter_limits_each_frame_step():
+ mapper = make_mapper(ema_alpha=1.0, max_step=7.0)
+ command = mapper.map(qpos_at(1))
+ for index in FLEXION_MOTOR_INDICES:
+ assert command[index] == 248
+ for index in RESERVED_MOTOR_INDICES:
+ assert command[index] == 255
+
+
+def test_missing_controlled_joint_is_rejected():
+ names = [name for name in JOINT_LIMITS if name != "thumb_joint2"]
+ with pytest.raises(ValueError, match="thumb_joint2"):
+ L20CommandMapper(names, [JOINT_LIMITS[name] for name in names])
+
diff --git a/src/camera_retarget_l20/test/test_model_profiles.py b/src/camera_retarget_l20/test/test_model_profiles.py
new file mode 100644
index 0000000..fce9875
--- /dev/null
+++ b/src/camera_retarget_l20/test/test_model_profiles.py
@@ -0,0 +1,215 @@
+from dataclasses import replace
+from pathlib import Path
+
+import numpy as np
+
+from camera_retarget_l20.hand_features import HandFeatures
+from camera_retarget_l20.model_profiles import (
+ ProfiledCommandMapper,
+ load_model_profiles,
+)
+
+
+PROFILE_PATH = (
+ Path(__file__).resolve().parents[1] / "config" / "direct_mapping_profiles.yml"
+)
+
+
+def features(*, closure=None, pinch_target=None, power_grasp=0.0):
+ strengths = np.zeros(4)
+ if pinch_target is not None:
+ strengths[pinch_target] = 1.0
+ return HandFeatures(
+ closure=np.zeros(10) if closure is None else np.asarray(closure),
+ pinch=float(np.max(strengths)),
+ pinch_strengths=strengths,
+ splay_delta=np.zeros(4),
+ splay_confidence=np.ones(4),
+ thumb_splay_delta=0.0,
+ thumb_abduction=0.0,
+ thumb_opposition=float(np.max(strengths)),
+ power_grasp=power_grasp,
+ )
+
+
+def test_profile_file_defines_valid_o6_and_g20_protocols():
+ profiles = load_model_profiles(PROFILE_PATH)
+
+ assert set(profiles) == {"O6", "G20"}
+ assert profiles["O6"].command_length == 6
+ assert profiles["G20"].command_length == 20
+ assert profiles["G20"].fixed_channels == (
+ (11, 255.0),
+ (12, 255.0),
+ (13, 255.0),
+ (14, 255.0),
+ )
+
+
+def test_g20_profile_reaches_all_measured_pinch_side_endpoints():
+ profile = load_model_profiles(PROFILE_PATH)["G20"].with_runtime_overrides(
+ ema_alpha=1.0, max_step=255.0
+ )
+ mapper = ProfiledCommandMapper(profile)
+
+ for target, expected in enumerate(((167, 184), (122, 148), (78, 112), (50, 90))):
+ command = mapper.raw_command(features(pinch_target=target))
+ assert command[[5, 10]].tolist() == list(expected)
+ assert command[11:15].tolist() == [255.0] * 4
+
+
+def test_thumb_sweep_does_not_curl_an_open_pinch_target():
+ profiles = load_model_profiles(PROFILE_PATH)
+
+ for model in ("O6", "G20"):
+ profile = profiles[model].with_runtime_overrides(
+ ema_alpha=1.0, max_step=255.0
+ )
+ mapper = ProfiledCommandMapper(profile)
+ command = mapper.raw_command(features(pinch_target=3))
+ pinky_indices = [
+ channel.command_index
+ for channel in profile.flexion_channels
+ if channel.pinch_role == "pinky"
+ ]
+ assert command[pinky_indices].tolist() == [
+ profile.open_pose[index] for index in pinky_indices
+ ]
+
+
+def test_pinch_assist_activates_when_target_finger_also_flexes():
+ profiles = load_model_profiles(PROFILE_PATH)
+ closure = np.zeros(10)
+ closure[[4, 9]] = 0.5
+
+ for model in ("O6", "G20"):
+ profile = profiles[model].with_runtime_overrides(
+ ema_alpha=1.0, max_step=255.0
+ )
+ mapper = ProfiledCommandMapper(profile)
+ command = mapper.raw_command(
+ features(closure=closure, pinch_target=3)
+ )
+ for channel in profile.flexion_channels:
+ if channel.pinch_role != "pinky":
+ continue
+ index = channel.command_index
+ expected = profile.open_pose[index] + profile.pinch_finger_closure * (
+ profile.fist_pose[index] - profile.open_pose[index]
+ )
+ assert command[index] == expected
+
+
+def test_index_and_middle_contact_can_assist_a_straight_target_finger():
+ profiles = load_model_profiles(PROFILE_PATH)
+
+ for model in ("O6", "G20"):
+ profile = profiles[model].with_runtime_overrides(
+ ema_alpha=1.0, max_step=255.0
+ )
+ mapper = ProfiledCommandMapper(profile)
+ for target in (0, 1):
+ command = mapper.raw_command(features(pinch_target=target))
+ role = ("index", "middle")[target]
+ for channel in profile.flexion_channels:
+ if channel.pinch_role != role:
+ continue
+ index = channel.command_index
+ expected = profile.open_pose[index] + profile.pinch_finger_closure * (
+ profile.fist_pose[index] - profile.open_pose[index]
+ )
+ assert command[index] == expected
+
+
+def test_o6_profile_uses_twenty_eighty_thumb_fusion():
+ profile = load_model_profiles(PROFILE_PATH)["O6"]
+ mapper = ProfiledCommandMapper(profile)
+ closure = np.zeros(10)
+ closure[0] = 1.0
+
+ command = mapper.raw_command(features(closure=closure))
+
+ assert command[0] == 255.0 + 0.2 * (0.0 - 255.0)
+
+
+def test_command_deadband_holds_small_jitter_but_allows_real_motion():
+ profile = load_model_profiles(PROFILE_PATH)["G20"].with_runtime_overrides(
+ ema_alpha=1.0,
+ max_step=255.0,
+ command_deadband=2.0,
+ )
+ mapper = ProfiledCommandMapper(profile)
+ closure = np.zeros(10)
+
+ closure[1] = 1.5 / 255.0
+ held = mapper.map_features(features(closure=closure))
+ assert held[1] == 255
+
+ closure[1] = 4.0 / 255.0
+ moved = mapper.map_features(features(closure=closure))
+ assert moved[1] == 251
+
+
+def test_power_grasp_suppresses_precision_pinch_and_uses_fist_endpoint():
+ profile = load_model_profiles(PROFILE_PATH)["G20"]
+ mapper = ProfiledCommandMapper(profile)
+
+ command = mapper.raw_command(
+ features(closure=np.ones(10), pinch_target=0, power_grasp=0.9)
+ )
+
+ assert command.tolist() == list(profile.fist_pose)
+ assert mapper.last_pinch_blend == 0.0
+
+
+def test_four_finger_power_grasp_does_not_force_thumb_for_either_model():
+ profiles = load_model_profiles(PROFILE_PATH)
+ closure = np.zeros(10)
+ closure[[1, 2, 3, 4, 6, 7, 8, 9]] = 1.0
+ hand_features = features(closure=closure, power_grasp=1.0)
+
+ for model in ("O6", "G20"):
+ profile = profiles[model]
+ mapper = ProfiledCommandMapper(profile)
+ command = mapper.raw_command(hand_features)
+ thumb_indices = [
+ channel.command_index
+ for channel in profile.flexion_channels
+ if channel.pinch_role == "thumb"
+ ]
+ assert command[thumb_indices].tolist() == [
+ profile.open_pose[index] for index in thumb_indices
+ ]
+ assert command[list(profile.side_command_indices)].tolist() == [
+ profile.open_pose[index] for index in profile.side_command_indices
+ ]
+ for channel in profile.flexion_channels:
+ if channel.pinch_role != "thumb":
+ assert command[channel.command_index] == profile.fist_pose[
+ channel.command_index
+ ]
+
+
+def test_thumb_intent_completes_thumb_during_power_grasp_for_either_model():
+ profiles = load_model_profiles(PROFILE_PATH)
+ closure = np.zeros(10)
+ closure[[1, 2, 3, 4, 6, 7, 8, 9]] = 1.0
+ hand_features = replace(
+ features(closure=closure, power_grasp=1.0), thumb_opposition=1.0
+ )
+
+ for model in ("O6", "G20"):
+ profile = profiles[model]
+ mapper = ProfiledCommandMapper(profile)
+ command = mapper.raw_command(hand_features)
+ thumb_indices = [
+ channel.command_index
+ for channel in profile.flexion_channels
+ if channel.pinch_role == "thumb"
+ ]
+ assert command[thumb_indices].tolist() == [
+ profile.fist_pose[index] for index in thumb_indices
+ ]
+ assert command[list(profile.side_command_indices)].tolist() == list(
+ profile.power_grasp_side_commands
+ )
diff --git a/src/camera_retarget_l20/test/test_o6_mapping.py b/src/camera_retarget_l20/test/test_o6_mapping.py
new file mode 100644
index 0000000..3b199c5
--- /dev/null
+++ b/src/camera_retarget_l20/test/test_o6_mapping.py
@@ -0,0 +1,118 @@
+import numpy as np
+
+from camera_retarget_l20.hand_features import HandFeatures
+from camera_retarget_l20.o6_mapping import (
+ DEFAULT_FIST_POSE,
+ DEFAULT_OPEN_POSE,
+ O6_COMMAND_NAMES,
+ O6MediaPipeMapper,
+ PINCH_TARGET_YAW_COMMANDS,
+ THUMB_ABDUCTED_COMMAND,
+)
+
+
+def make_features(closure=None, **kwargs):
+ return HandFeatures(
+ closure=np.zeros(10) if closure is None else np.asarray(closure),
+ pinch=kwargs.get("pinch", 0.0),
+ pinch_strengths=np.asarray(
+ kwargs.get("pinch_strengths", np.zeros(4)), dtype=float
+ ),
+ splay_delta=np.zeros(4),
+ splay_confidence=np.ones(4),
+ thumb_splay_delta=0.0,
+ thumb_abduction=kwargs.get("thumb_abduction", 0.0),
+ thumb_opposition=kwargs.get("thumb_opposition", 0.0),
+ power_grasp=kwargs.get("power_grasp", 0.0),
+ )
+
+
+def test_o6_protocol_order_and_open_endpoint():
+ mapper = O6MediaPipeMapper(ema_alpha=1.0, max_step=255.0)
+ assert O6_COMMAND_NAMES == (
+ "thumb_cmc_pitch",
+ "thumb_cmc_yaw",
+ "index_mcp_pitch",
+ "middle_mcp_pitch",
+ "ring_mcp_pitch",
+ "pinky_mcp_pitch",
+ )
+ assert mapper.map_features(make_features()) == list(DEFAULT_OPEN_POSE)
+
+
+def test_full_fist_reaches_configured_full_range_endpoint():
+ mapper = O6MediaPipeMapper(ema_alpha=1.0, max_step=255.0)
+ features = make_features(
+ np.ones(10),
+ thumb_opposition=1.0,
+ power_grasp=1.0,
+ )
+ assert mapper.map_features(features) == list(DEFAULT_FIST_POSE)
+
+
+def test_single_o6_finger_combines_root_and_tip_flexion():
+ mapper = O6MediaPipeMapper(ema_alpha=1.0, max_step=255.0)
+ closure = np.zeros(10)
+ closure[1] = 1.0
+ closure[6] = 0.5
+ command = mapper.raw_command(make_features(closure))
+ expected_activation = 0.35 + 0.65 * 0.5
+ expected = 255.0 + expected_activation * (0.0 - 255.0)
+ assert command[2] == expected
+ assert command[3:].tolist() == [255.0, 255.0, 255.0]
+
+
+def test_o6_filter_limits_each_frame_step():
+ mapper = O6MediaPipeMapper(ema_alpha=1.0, max_step=8.0)
+ command = mapper.map_features(
+ make_features(
+ np.ones(10),
+ thumb_opposition=1.0,
+ power_grasp=1.0,
+ )
+ )
+ assert all(abs(value - open_value) <= 8 for value, open_value in zip(command, DEFAULT_OPEN_POSE))
+
+
+def test_o6_thumb_yaw_uses_separate_abduction_and_opposition_directions():
+ mapper = O6MediaPipeMapper(ema_alpha=1.0, max_step=255.0)
+ abducted = mapper.raw_command(make_features(thumb_abduction=1.0))
+ opposed = mapper.raw_command(make_features(thumb_opposition=1.0))
+
+ assert abducted[1] == THUMB_ABDUCTED_COMMAND
+ assert opposed[1] == DEFAULT_FIST_POSE[1]
+
+
+def test_o6_pinch_couples_thumb_and_selected_finger_flexion():
+ mapper = O6MediaPipeMapper(
+ ema_alpha=1.0,
+ max_step=255.0,
+ pinch_thumb_closure=0.85,
+ pinch_finger_closure=0.65,
+ )
+
+ for target_offset in range(4):
+ strengths = np.zeros(4)
+ strengths[target_offset] = 1.0
+ closure = np.zeros(10)
+ closure[1 + target_offset] = 0.5
+ closure[6 + target_offset] = 0.5
+ command = mapper.raw_command(
+ make_features(
+ closure,
+ pinch=1.0,
+ pinch_strengths=strengths,
+ thumb_opposition=1.0,
+ )
+ )
+
+ assert command[0] == 255.0 + 0.85 * (DEFAULT_FIST_POSE[0] - 255.0)
+ assert command[1] == PINCH_TARGET_YAW_COMMANDS[target_offset]
+ for finger_offset in range(4):
+ expected = (
+ 255.0
+ + 0.65 * (DEFAULT_FIST_POSE[2 + finger_offset] - 255.0)
+ if finger_offset == target_offset
+ else 255.0
+ )
+ assert command[2 + finger_offset] == expected
diff --git a/src/gui_control/gui_control/__init__.py b/src/gui_control/gui_control/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/gui_control/gui_control/config/constants.py b/src/gui_control/gui_control/config/constants.py
new file mode 100644
index 0000000..952ac15
--- /dev/null
+++ b/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)
diff --git a/src/gui_control/gui_control/gui_control.py b/src/gui_control/gui_control/gui_control.py
new file mode 100644
index 0000000..7b7bef6
--- /dev/null
+++ b/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()
diff --git a/src/gui_control/gui_control/gui_control.py.ttk b/src/gui_control/gui_control/gui_control.py.ttk
new file mode 100644
index 0000000..aa86c30
--- /dev/null
+++ b/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('', 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(
+ "",
+ 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()
diff --git a/src/gui_control/gui_control/utils/color_msg.py b/src/gui_control/gui_control/utils/color_msg.py
new file mode 100644
index 0000000..059d60a
--- /dev/null
+++ b/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)
\ No newline at end of file
diff --git a/src/gui_control/gui_control/utils/mapping.py b/src/gui_control/gui_control/utils/mapping.py
new file mode 100644
index 0000000..deb6a19
--- /dev/null
+++ b/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))
diff --git a/src/gui_control/launch/gui_control.launch.py b/src/gui_control/launch/gui_control.launch.py
new file mode 100644
index 0000000..49c3996
--- /dev/null
+++ b/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,
+ # }],
+ # ),
+ ])
diff --git a/src/gui_control/package.xml b/src/gui_control/package.xml
new file mode 100644
index 0000000..b444511
--- /dev/null
+++ b/src/gui_control/package.xml
@@ -0,0 +1,21 @@
+
+
+
+ gui_control
+ 0.0.0
+ TODO: Package description
+ linker-robot
+ TODO: License declaration
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+ linker_hand_ros2_sdk
+
+
+ ament_python
+
+
+
diff --git a/src/gui_control/pyproject.toml b/src/gui_control/pyproject.toml
new file mode 100644
index 0000000..638dd9c
--- /dev/null
+++ b/src/gui_control/pyproject.toml
@@ -0,0 +1,3 @@
+[build-system]
+requires = ["setuptools>=61.0"]
+build-backend = "setuptools.build_meta"
diff --git a/src/gui_control/resource/gui_control b/src/gui_control/resource/gui_control
new file mode 100644
index 0000000..e69de29
diff --git a/src/gui_control/setup.cfg b/src/gui_control/setup.cfg
new file mode 100644
index 0000000..4691687
--- /dev/null
+++ b/src/gui_control/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/gui_control
+[install]
+install_scripts=$base/lib/gui_control
diff --git a/src/gui_control/setup.py b/src/gui_control/setup.py
new file mode 100644
index 0000000..9f7a812
--- /dev/null
+++ b/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'
+ ],
+ },
+)
diff --git a/src/gui_control/test/test_copyright.py b/src/gui_control/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/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'
diff --git a/src/gui_control/test/test_flake8.py b/src/gui_control/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/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)
diff --git a/src/gui_control/test/test_pep257.py b/src/gui_control/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/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'
diff --git a/src/latency_analyzer/latency_analyzer/__init__.py b/src/latency_analyzer/latency_analyzer/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/latency_analyzer/latency_analyzer/analyzer_node.py b/src/latency_analyzer/latency_analyzer/analyzer_node.py
new file mode 100644
index 0000000..e5155c0
--- /dev/null
+++ b/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()
diff --git a/src/latency_analyzer/latency_analyzer/latency_stamp.py b/src/latency_analyzer/latency_analyzer/latency_stamp.py
new file mode 100644
index 0000000..99cc8ee
--- /dev/null
+++ b/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]
diff --git a/src/latency_analyzer/package.xml b/src/latency_analyzer/package.xml
new file mode 100644
index 0000000..c22ce69
--- /dev/null
+++ b/src/latency_analyzer/package.xml
@@ -0,0 +1,18 @@
+
+
+
+ latency_analyzer
+ 0.1.0
+ End-to-end latency measurement and analysis for LinkerHand retargeting pipeline
+ user
+ MIT
+
+ rclpy
+ std_msgs
+ sensor_msgs
+ linkerhand_retarget
+
+
+ ament_python
+
+
diff --git a/src/latency_analyzer/setup.cfg b/src/latency_analyzer/setup.cfg
new file mode 100644
index 0000000..b3de2a2
--- /dev/null
+++ b/src/latency_analyzer/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/latency_analyzer
+[install]
+install_scripts=$base/lib/latency_analyzer
diff --git a/src/latency_analyzer/setup.py b/src/latency_analyzer/setup.py
new file mode 100644
index 0000000..7ab3b71
--- /dev/null
+++ b/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',
+ ],
+ },
+)
diff --git a/src/linker_hand_mujoco_ros2/README_CN.MD b/src/linker_hand_mujoco_ros2/README_CN.MD
new file mode 100644
index 0000000..0aa8d5e
--- /dev/null
+++ b/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)控制仿真环境
diff --git a/src/linker_hand_mujoco_ros2/launch/hand_curve_recorder.launch.py b/src/linker_hand_mujoco_ros2/launch/hand_curve_recorder.launch.py
new file mode 100644
index 0000000..b1017a4
--- /dev/null
+++ b/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,
+ }],
+ ),
+ ])
diff --git a/src/linker_hand_mujoco_ros2/launch/linker_hand_mujoco_ros2.launch.py b/src/linker_hand_mujoco_ros2/launch/linker_hand_mujoco_ros2.launch.py
new file mode 100644
index 0000000..1e5a79c
--- /dev/null
+++ b/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,
+ }],
+ ),
+ ])
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/__init__.py b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/hand_curve_recorder.py b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/hand_curve_recorder.py
new file mode 100644
index 0000000..dce09f5
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/hand_curve_recorder.py
@@ -0,0 +1,252 @@
+#!/usr/bin/env python3
+"""通用手部曲线录制 + 画图（仿真 / 真机同一套 ROS 话题）。
+
+默认订阅：
+ /cb__hand_control_cmd 指令 0～255 (sensor_msgs/JointState.position)
+ /cb__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()
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2.py b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2.py
new file mode 100644
index 0000000..af5a4d9
--- /dev/null
+++ b/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()
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/base_link.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/base_link.STL
new file mode 100644
index 0000000..9b5b829
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:28a98b15c4e1a9efac5845b13d88eb352dcb0a297dd25c69b247a5bc01ce9f3e
+size 1666284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link0.STL
new file mode 100644
index 0000000..4e3d5ca
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7c3d43706062d2303f84666337eb3c503038d47059a871a817882958a985f9aa
+size 256884
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link1.STL
new file mode 100644
index 0000000..9080edf
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0be74517180e60134c06c8d50d30b96098017585c9b7f607f2c2130e0ae11b0c
+size 519284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link2.STL
new file mode 100644
index 0000000..1807a0e
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:679997180a67ec28a8c2227bea58fed7a540ca7535fcae5393f94db5c43e7919
+size 573484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link3.STL
new file mode 100644
index 0000000..18adce9
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9cb61e85e4d61a5b63a315348aadd23919115cd54ffa12afced0fc2de929f982
+size 682784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link4.STL
new file mode 100644
index 0000000..aaa7520
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/index_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0d3a1584cf566616b84868ffbcf3b82545fbf259e3ed40c2661456799192ceae
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/linker_hand_l10_left.urdf b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/linker_hand_l10_left.urdf
new file mode 100644
index 0000000..76cf37e
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/linker_hand_l10_left.urdf
@@ -0,0 +1,1512 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/linker_hand_l10_left.xml b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/linker_hand_l10_left.xml
new file mode 100644
index 0000000..d235e37
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/linker_hand_l10_left.xml
@@ -0,0 +1,195 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link0.STL
new file mode 100644
index 0000000..eb1861a
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7afaaaa27481b0427785070df79e3251a1dfd6fdcd22b2089a117f75bd90c623
+size 256884
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link1.STL
new file mode 100644
index 0000000..cdb5796
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c8759223942b77ab08666af40c834724920d138e10629de15a45af740c75ba22
+size 519284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link2.STL
new file mode 100644
index 0000000..dfb76eb
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:bc49c7f4bf13de34f1e893e993cf1c21f9475bbba837a0dea3157dec1b49c297
+size 573484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link3.STL
new file mode 100644
index 0000000..9786eae
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ff45ec96203c305f53724de8aeb9e02c32eb6ffc61497aa2275e128e104bb500
+size 682784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link4.STL
new file mode 100644
index 0000000..befee60
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/little_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:25c31141f955d73c7bae5f505e90041e1710cff72401704f135fdde3e72c76d9
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link0.STL
new file mode 100644
index 0000000..8c30ca4
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:21c5311fef9933ee8b59ea2935a1c407a33d37bf1088167ee4948e80be0a234c
+size 519284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link1.STL
new file mode 100644
index 0000000..2866f3b
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ce912434165ca8d9cd33643f7251c22fbe12c55cc73d837f655337db234a5d63
+size 573484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link2.STL
new file mode 100644
index 0000000..63ea564
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:472f53ed3b676f49f0ad160a973ec0830558df31b63d9a1c9220573c7a885555
+size 682784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link3.STL
new file mode 100644
index 0000000..6ab996d
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/middle_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d1e2a600af4c477829ac553dc8a02ebbaadf42ec3a6e7199dc7d38d0b967687a
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link0.STL
new file mode 100644
index 0000000..309640d
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ab8e34568c6cae073f1229ea54d2204a3f3c69704fc1446a7c9595987b39c6bb
+size 256884
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link1.STL
new file mode 100644
index 0000000..48851b9
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a379fd662d6337e265e6b2ab957dc861eca7bc44c7a06150dda0fa2ca69f5347
+size 519284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link2.STL
new file mode 100644
index 0000000..91052f2
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8b231d68309ec84c732df40f703f4b3ae7a8f00369f79c488804d05a62dd4182
+size 573484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link3.STL
new file mode 100644
index 0000000..8addb2f
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7801ed795a1b4d4d78da6ab3cd87a481460c2ee6e29d56be162a239a63ff3a40
+size 682784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link4.STL
new file mode 100644
index 0000000..51a99b2
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/ring_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:51d96da28e051c953465ba0eb6867cc0ac5903a1f319c502eb92545fc72c79ca
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link0.STL
new file mode 100644
index 0000000..a8426ef
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7ed8c26294c961e3edb4c6fb2e331a2a16b02f9c2c8f9bdbb0dcea2741838ee1
+size 385684
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link1.STL
new file mode 100644
index 0000000..0481409
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c6b07079056fad59c1b232f035fe0a48495ca2d9e6639b02d3635afdfb436bdd
+size 392584
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link2.STL
new file mode 100644
index 0000000..02843ab
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f88d7c7dc37703512b59225d58fb494cde1de2fbb3ca0525cbbdb059b7bd5b0d
+size 986384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link3.STL
new file mode 100644
index 0000000..a6cf47b
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e0819ace8213c6ebb0a97c5fba5e904e2984aad31ac2b129b61068a2fd9c9c7b
+size 225184
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link4.STL
new file mode 100644
index 0000000..12138eb
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:030aad2997c9501f4cad40f074a26228f53501cfd5099c9d33096235c7fb880d
+size 283384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link5.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link5.STL
new file mode 100644
index 0000000..a54c2fa
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_left/thumb_link5.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:82513e0a64a64aa0f7f4f3b85a5b8a5f09187d55e7fed5e9aa40fe3407066423
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/base_link.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/base_link.STL
new file mode 100644
index 0000000..955d770
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:34662efe4b9d4a34cc5066945d2ac95ef66d48926ace3adc9c871808cbde851e
+size 1509984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link0.STL
new file mode 100644
index 0000000..efc95c1
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0a82724b15185a15534eefccf69127becfff74cc7b43579d228368b9e21a6ec8
+size 257984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link1.STL
new file mode 100644
index 0000000..2e67d56
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:24dd990af121925ae6836dce055b4ff992209fdb7bf06390cd2797c3e7e7f247
+size 521284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link2.STL
new file mode 100644
index 0000000..ea9760f
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:68b823511f995073faccfcc349d21e66cc0678165e5164867da43958c6c2cabe
+size 572984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link3.STL
new file mode 100644
index 0000000..73ea2cd
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2d59058d948c3b957043a393dfc6ed4bef5ca6473b34ca978395ebdf371e555c
+size 746784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link4.STL
new file mode 100644
index 0000000..3e13eaf
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/index_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6d1359761f91062c97afd95e0fa92b6ea0e3339735dfc859c918e78f090031d1
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/linker_hand_l10_right.urdf b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/linker_hand_l10_right.urdf
new file mode 100644
index 0000000..c522f35
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/linker_hand_l10_right.urdf
@@ -0,0 +1,1537 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/linker_hand_l10_right.xml b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/linker_hand_l10_right.xml
new file mode 100644
index 0000000..9441116
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/linker_hand_l10_right.xml
@@ -0,0 +1,195 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link0.STL
new file mode 100644
index 0000000..214afd8
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:26e8533229f302952431d8a7377b978dfee2442829d3de46636360504ec0dfc1
+size 257984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link1.STL
new file mode 100644
index 0000000..14ae20e
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:56376eb7b25a0e08ad0f9cc0e2c79d01bc984c99e4ced630eb07a28768dd88fa
+size 521284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link2.STL
new file mode 100644
index 0000000..28b5f1a
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f0e5a3ca00f8935873096793fb7328879b53f2458268033a7f8c39aa92679610
+size 572984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link3.STL
new file mode 100644
index 0000000..1b23ac6
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:43f092a01de5b3b427a49ef5dd1351ccf1b50a662db2f6b5249b5ed4fe918333
+size 746784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link4.STL
new file mode 100644
index 0000000..1af2f41
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/little_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4336131b69b091b21dbdfbe38c3fb40d1ecb0242fbcb5b8e656c5835559a4eff
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link0.STL
new file mode 100644
index 0000000..9682313
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:85c9267c2d16022d712e7b367ef2aabef9e31e134c3c63fb45b084515b12de10
+size 521284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link1.STL
new file mode 100644
index 0000000..c982b97
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ab5028794863195998b14659f43a6d1902e55b50d72322232c44345ac47a1e5c
+size 572984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link2.STL
new file mode 100644
index 0000000..0c37d7f
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f818f2791654887012d820178e3e1da18188eef2ddc2803011c4fb1f9ffa03aa
+size 746784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link3.STL
new file mode 100644
index 0000000..5cc1bb5
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/middle_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2133f86ea956222ef16bfa601f1c2952e4630cba1f3c5cf41a7c40b442e8acdc
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link0.STL
new file mode 100644
index 0000000..e347da4
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b20c5bec23d8b556e3b90cbb0aa451110a7c0fa9cb38db640e3ad0cce129fc47
+size 257984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link1.STL
new file mode 100644
index 0000000..e8cbf9c
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:13a0fdc7302d2e90de835f9331ac1c157239e9de9fbe92b289af06803a2618a0
+size 521284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link2.STL
new file mode 100644
index 0000000..0d46ec5
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7aa1779441cb58ee2a15ef850b813dbeb3cbe089ac4a54af64a101d3e3484902
+size 572984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link3.STL
new file mode 100644
index 0000000..e8291d5
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6afe65c8555fe99eb9e660074fa8e976ae53fed9cebe57a63885798a366d9765
+size 746784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link4.STL
new file mode 100644
index 0000000..f1b6f7b
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/ring_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:45c8752a716edc970d9263940f2e2ac4df5523f85d5a20bc8cc17b64902f7c2f
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link0.STL
new file mode 100644
index 0000000..9f95649
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3eca4e14a33c4e159a74ff3a60c0234172c8ed58f190d64b4a511c49c1fe829c
+size 386584
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link1.STL
new file mode 100644
index 0000000..b5d4461
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b09b4d4301f0d1f1791945952a853b634f2bee75951f02e8704d1106231fcf86
+size 392984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link2.STL
new file mode 100644
index 0000000..2b197fc
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5b5d4b4701098fc64002d88e056bc2ef81295ee535731728efbf0189b85547d2
+size 327684
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link3.STL
new file mode 100644
index 0000000..8cc7ee2
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9b5ef49f1b08859c1f16b6513a49ccab12e235413c541511a528744caa8ccfc3
+size 225084
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link4.STL
new file mode 100644
index 0000000..c551d71
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d2621267fcdde28faddb52af9518c041ab8d763b57aec6cd917d3e5cd1e13cbd
+size 844784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link5.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link5.STL
new file mode 100644
index 0000000..8492f9d
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L10/linker_hand_l10_right/thumb_link5.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ad1a9626c75a421cf3bef28da1bf4a267f20ef4367f16a955c320bbf0e849551
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/base_link.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/base_link.STL
new file mode 100644
index 0000000..113acdf
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:36032fc64c2759d21b3297a59429015e8b8f3de45de2aa53561b1481e63baa7d
+size 1318484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link0.STL
new file mode 100644
index 0000000..50ab66c
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:34f25900c3fd06d874d44993446fe198bc221d4b84c4c22fcab962482443eec9
+size 69384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link1.STL
new file mode 100644
index 0000000..797d714
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d638ad9fa7c2a4a8db33d831fb012251897a9e5592be3d7c9955e29930c0f75f
+size 953184
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link2.STL
new file mode 100644
index 0000000..0a5093e
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4e590cbd0ed18c7c3a575aadda5c8a2414c3a56c17e06536da0fc76b6cdea463
+size 884884
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link3.STL
new file mode 100644
index 0000000..3118860
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ddfcdd3f2b5a605722b4693c1909c220214f43772c2a72d2bfa5ba8b625c2e03
+size 964984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link4.STL
new file mode 100644
index 0000000..22d6034
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/index_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:de9f192a5bb842176719a2dd044925b2c25042c89d654d453cd155eff5018603
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/linker_hand_l20_left.urdf b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/linker_hand_l20_left.urdf
new file mode 100644
index 0000000..3105751
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/linker_hand_l20_left.urdf
@@ -0,0 +1,1536 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/linker_hand_l20_left.xml b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/linker_hand_l20_left.xml
new file mode 100644
index 0000000..f8e6968
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/linker_hand_l20_left.xml
@@ -0,0 +1,225 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link0.STL
new file mode 100644
index 0000000..5734316
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:79e2fdef62fcc0b7fbb10038a02bcc271cc92d2ad15fcca5d80db76deaa2557e
+size 69384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link1.STL
new file mode 100644
index 0000000..1823f7a
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:120c49d3a35b603cfdaedbb42bae03e9a48bff0eaef0f9c80b7ebfc6ed068409
+size 953184
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link2.STL
new file mode 100644
index 0000000..e218dcc
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:24610b3f72455c7b5b22a9ad8fc31c6062cc5215cea2e56f88a61964335ee0d8
+size 884884
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link3.STL
new file mode 100644
index 0000000..cfdc1cc
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3e7b3ae67baa9a4b74d92a1cffe26039bc6e02a472ea0077d73803ae675b9e75
+size 964984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link4.STL
new file mode 100644
index 0000000..820682c
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/little_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:88e0f66b7ffe5c6f5f955cdeea3f8532afd65985dc07c3e8823f35412e823b13
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link0.STL
new file mode 100644
index 0000000..ac63ca2
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b72b0a97bea433e9e7e10128027f34d3eae928792424476bbbfd57d4e084a84d
+size 69384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link1.STL
new file mode 100644
index 0000000..fd7da3c
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7ecb4635a36d9af4ef5a26be64fd85e447f10eace4ca2426709f0a023cccdd76
+size 953184
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link2.STL
new file mode 100644
index 0000000..e5f7616
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:44816865bd111ee423d0754efff4247c504c02efee8cbb86fb7c7b54ce3eb3f4
+size 884884
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link3.STL
new file mode 100644
index 0000000..94cc874
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b17ca9d2a0fe2281f45c6cf46a76d7afab0fd7a72303fe566f82d6418cf2c4cb
+size 964984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link4.STL
new file mode 100644
index 0000000..a7f4175
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/middle_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f4760d7c9d75b30ec198a8604e368aa5c643272d77ebb557f6d86613f999430d
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link0.STL
new file mode 100644
index 0000000..abce9ba
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:07b19ee8c4a0b83d22e05b3603dec5ff17bcafb527695a2126b157e93deaf5e1
+size 69384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link1.STL
new file mode 100644
index 0000000..dd50307
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2f7a7f80d8d81e78c7bb6b28bae2f53ad35aeceeda638614c84ba73fb25f1159
+size 953184
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link2.STL
new file mode 100644
index 0000000..e218dcc
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:24610b3f72455c7b5b22a9ad8fc31c6062cc5215cea2e56f88a61964335ee0d8
+size 884884
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link3.STL
new file mode 100644
index 0000000..fe2abaa
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d13663b39464ccce25d09fa6e09e6b37899010edf35d14c60547e2f49f544989
+size 964984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link4.STL
new file mode 100644
index 0000000..8faae23
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/ring_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:759435162e5690bdc3eddf738ca532e48555972c03eed39c429b26efdad185b7
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link0.STL
new file mode 100644
index 0000000..fcff4bd
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c12be7106618f74b18765743c5a8cf4f4bda2d323fdaab4eed8ff1cfec3199cf
+size 129384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link1.STL
new file mode 100644
index 0000000..11dbcd5
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:584d309e9a1de6d69d346279f6176a924ae0449a34df2398e5a119967e004726
+size 82484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link2.STL
new file mode 100644
index 0000000..261a183
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1275fbad6d563aecb647d31407664f1d9cb75d04e2f5e8a037d7b9b338e9a8dc
+size 748684
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link3.STL
new file mode 100644
index 0000000..77cf70a
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e3a7b7da64944f736d927df90b3b58a22612cd883a3ffdb098d6d6dc47e2a963
+size 342484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link4.STL
new file mode 100644
index 0000000..44b4a66
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3f8fbfa60d8770c0149530705ab8a8b2578ba48f5705f98b4b08871fbd7b4de2
+size 1101184
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link5.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link5.STL
new file mode 100644
index 0000000..72fe017
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_left/thumb_link5.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:cbad1b069f357d36d1bffa61301c9fbb3065df68a216c119e3dee98925897289
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/base_link.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/base_link.STL
new file mode 100644
index 0000000..b1567b9
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:731f774d963c2eda9f32aad600e3d1c1128693ae36b6ee27f2aec29b8eed287f
+size 1567784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link0.STL
new file mode 100644
index 0000000..525cc39
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ab783c0338c7972d7397d88f29b688596353ad03789facc807ae0613f9a9861c
+size 69284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link1.STL
new file mode 100644
index 0000000..894a556
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a4e16722dcf166e6005fd80d1faffdbbf9df64b9ae376f7891ae8f703fd19e3e
+size 953284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link2.STL
new file mode 100644
index 0000000..fd450b3
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7216c7c71d57c9edcbcb01fcdfd249573a58903c172120293db7556b833b61e8
+size 882784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link3.STL
new file mode 100644
index 0000000..e1ea37f
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:447924798632d089a6900540ebae4ab5be7b93b3ad67915eafed511d5131a0c7
+size 963384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link4.STL
new file mode 100644
index 0000000..3c62495
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/index_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:37e920c77ee1ca58c90dc3549800c2f20e847e4a78dcd8f8599ea708ee0efbbc
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/linker_hand_l20_right.urdf b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/linker_hand_l20_right.urdf
new file mode 100644
index 0000000..e4f0d52
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/linker_hand_l20_right.urdf
@@ -0,0 +1,1536 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/linker_hand_l20_right.xml b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/linker_hand_l20_right.xml
new file mode 100644
index 0000000..440a833
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/linker_hand_l20_right.xml
@@ -0,0 +1,202 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link0.STL
new file mode 100644
index 0000000..fa79a75
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b261557ad5a447cfee6e6c70a209a97b0392d31f175bd2a1afabf03a5acae1e3
+size 69284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link1.STL
new file mode 100644
index 0000000..f4f2106
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:03a814cabbc9465734338c685f8c2f4d4abc72be942ec5b2560085335cc593da
+size 953284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link2.STL
new file mode 100644
index 0000000..bbbe999
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:940e6cdc2ef76ee21caa2aefda27e228d3d6e7dab7fbaf0e590aaf6ee1b2c0ee
+size 882784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link3.STL
new file mode 100644
index 0000000..e1ea37f
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:447924798632d089a6900540ebae4ab5be7b93b3ad67915eafed511d5131a0c7
+size 963384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link4.STL
new file mode 100644
index 0000000..fcd1492
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/little_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b67260805deeaec089c146fb10886ada742d7dec7ecd8c6844a10e20345d39a7
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link0.STL
new file mode 100644
index 0000000..3d7a594
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:11111b64d5158c8fdc66f504691c87066f3d0b75fe5185934893b7a306b730e5
+size 69284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link1.STL
new file mode 100644
index 0000000..3153337
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f7a2420b05cbe5a6eec8abccb6428922dcec64bf666e12cf2bf56f27419d8375
+size 953284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link2.STL
new file mode 100644
index 0000000..2ab0e20
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:357f2271df50349b5e25f9629744c9917893f3181021b44087b6e9310a66d0d8
+size 882784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link3.STL
new file mode 100644
index 0000000..e1ea37f
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:447924798632d089a6900540ebae4ab5be7b93b3ad67915eafed511d5131a0c7
+size 963384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link4.STL
new file mode 100644
index 0000000..23e5fa4
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/middle_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f141b7e2e9ad790316576d7a992e14b446a90d28330e57480e92aac8d609253c
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link0.STL
new file mode 100644
index 0000000..fdb4991
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:067cda8408826e1ae33b914524f1c3add71215d9fbfcc1a3219d034751655e97
+size 69284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link1.STL
new file mode 100644
index 0000000..726a1dd
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1c1bb48037e12c21eced4de0d816dba3eca7ce4074350551a51df5239bd8d300
+size 953284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link2.STL
new file mode 100644
index 0000000..a0948d0
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9d524a4300a6bb74ed21ed7dfafe3e3ba078cc36d0e6e7bbec9bf812ccd7edb8
+size 882784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link3.STL
new file mode 100644
index 0000000..e1ea37f
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:447924798632d089a6900540ebae4ab5be7b93b3ad67915eafed511d5131a0c7
+size 963384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link4.STL
new file mode 100644
index 0000000..d0c2c90
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/ring_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6aa46d40f2f6bc31cbfccbd918d5ae4a2066abde96239dc2e1427c2c2a988b19
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link0.STL
new file mode 100644
index 0000000..d0a5306
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:766b60994e95f859573ba41e76cb1dc2b16f5ddc22d1e1bd3284787aece83bc0
+size 129384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link1.STL
new file mode 100644
index 0000000..90a9f42
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2216255bbbe159f4546937e97ef9fc71ed10e7826633f214c0817bb09ccad81e
+size 82484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link2.STL
new file mode 100644
index 0000000..ffa9dfe
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:32b264fe991d7f7c7adbe6e0b9129a99521c45197892f1bc6eddff2c560cfc9e
+size 751084
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link3.STL
new file mode 100644
index 0000000..49bf761
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1470810dfeb1e40acb1f0fe1850db55166f45018239eeb4cfc5db659c75630a5
+size 341284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link4.STL
new file mode 100644
index 0000000..1799c50
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:15878e8c96d192fa9cbaef77bae65a97b21132855698440d1aa0bf3d74cd1efd
+size 1101584
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link5.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link5.STL
new file mode 100644
index 0000000..2bb674a
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L20/linker_hand_l20_right/thumb_link5.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:73de2617e13616040475356f60851f3a196b25154188696b1628360d37ff2ae5
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/hand_base_link.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/hand_base_link.STL
new file mode 100644
index 0000000..f2b9e05
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0bcc8322d1a83ccbb58a45afe5044231b80447acbdf94d6e35437c67bd76c2e9
+size 1239084
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/index_metacarpals.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/index_metacarpals.STL
new file mode 100644
index 0000000..396921a
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/index_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7866eb4f650a62ffcd6eab4449d8fb6b53064e955a79e85d09e9e26d27231424
+size 3741334
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/index_middle.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/index_middle.STL
new file mode 100644
index 0000000..97dd64d
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:49544c49e90b04a083bc90a0be71a31eb7c1e2b7fdaeee4895c1961638d7989a
+size 733984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/index_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/index_proximal.STL
new file mode 100644
index 0000000..c97f9ca
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d655b11c5ff1138b7330c8b60472f9b9117c8341a8e7ce07157e2b303be4c4a0
+size 678484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/linker_hand_l21_left.urdf b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/linker_hand_l21_left.urdf
new file mode 100644
index 0000000..0173301
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/linker_hand_l21_left.urdf
@@ -0,0 +1,1033 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/linker_hand_l21_left.xml b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/linker_hand_l21_left.xml
new file mode 100644
index 0000000..6532f89
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/linker_hand_l21_left.xml
@@ -0,0 +1,163 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/linkerhand_l21_left.xml.bak b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/linkerhand_l21_left.xml.bak
new file mode 100644
index 0000000..b598a60
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/linkerhand_l21_left.xml.bak
@@ -0,0 +1,183 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+ >
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/middle_metacarpals.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/middle_metacarpals.STL
new file mode 100644
index 0000000..4306dc4
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/middle_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:53284a18c1210c3ce13a440817694ec6051451e97695a55f59b012044d324046
+size 3646134
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/middle_middle.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/middle_middle.STL
new file mode 100644
index 0000000..884c78f
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:539f7e4a23d93fc407e846be78171b0a8c73e8663b75b829930029a7c9617e1f
+size 733484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/middle_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/middle_proximal.STL
new file mode 100644
index 0000000..b24a595
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d2c71f9907c7ac7a03121541d4e379ed5fd818e41c706987d775307919c3f530
+size 664384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/pinky_metacarpals.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/pinky_metacarpals.STL
new file mode 100644
index 0000000..836e209
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/pinky_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9262e21886d43aac7d541eee47ab64d388ace38d1ab4f6854b5456f66feaa0c6
+size 3731484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/pinky_middle.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/pinky_middle.STL
new file mode 100644
index 0000000..c182eab
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3181e60739a780920e84b96d986565fd348cc065705b49eb95612446b07e7052
+size 734884
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/pinky_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/pinky_proximal.STL
new file mode 100644
index 0000000..17660ae
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b022fd154bbd77d09c1636f92efc842f4a3fbb3017986d863ab04f5876fd2dcb
+size 652184
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/ring_metacarpals.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/ring_metacarpals.STL
new file mode 100644
index 0000000..14c1241
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/ring_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7c309c792504d6b2c584aed3f13ebbc4a21d97fee69789e8a793b8b56addcfbc
+size 2975834
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/ring_middle.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/ring_middle.STL
new file mode 100644
index 0000000..09e8167
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e1e0a3dcf6dbeb90b61c808402eba709da4dadf8684fb430318b7211a283948b
+size 732384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/ring_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/ring_proximal.STL
new file mode 100644
index 0000000..a49d585
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1a49c3f971492dd559217200f0e09556033efcb3686a3ac2e2f5b651e9f482f7
+size 690484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_distal.STL
new file mode 100644
index 0000000..c214026
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3587838fd24c9b383784666036563cbff5f905bc7a7bb72335bc5d35c335f0bf
+size 711434
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_metacarpals.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_metacarpals.STL
new file mode 100644
index 0000000..b5aa9c8
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0272073e607d8b8f568af13cf90dd7b6b51202de85a8c7ea26ee91d32604453e
+size 1069934
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_metacarpals_base1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..208ae72
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:aa7c035a64a77a8c7459cf0eacca79affd1dfc0bf76bc17f151fddc51eb63176
+size 967484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_metacarpals_base2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..35c9fc1
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2380fc48a7a097aac08f2953aa1292af765f710939f73d13bcaaa768c85bcd2a
+size 503434
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_proximal.STL
new file mode 100644
index 0000000..ac997a0
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_left/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0ffe01cde5a9810c0235a65caa3aa4e6b9b4b7f2892923c6649ef51856be66fb
+size 999084
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/hand_base_link.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/hand_base_link.STL
new file mode 100644
index 0000000..5bac936
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7d029b9477bc2e81e45234553366e5a00628819c512b2525a2de2725ba5d3612
+size 3319484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/index_metacarpals.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/index_metacarpals.STL
new file mode 100644
index 0000000..93ae4a7
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/index_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:179b9458833f66a414e7afb17627a7c908f3368db385574eb9d680329b464ab0
+size 2916384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/index_middle.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/index_middle.STL
new file mode 100644
index 0000000..5985ab0
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9d29cab9b42587137b2ffaf0007ef3a0e2a738fec09c4f65f63a8e400cb336fa
+size 742084
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/index_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/index_proximal.STL
new file mode 100644
index 0000000..274459a
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:eb4989bcb6548f3789c3bc48908c65829b36e48da3104a7e5eefab1b0f77e730
+size 642384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/linker_hand_l21_right.urdf b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/linker_hand_l21_right.urdf
new file mode 100644
index 0000000..f42dc99
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/linker_hand_l21_right.urdf
@@ -0,0 +1,1033 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/linker_hand_l21_right.xml b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/linker_hand_l21_right.xml
new file mode 100644
index 0000000..8a2f735
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/linker_hand_l21_right.xml
@@ -0,0 +1,163 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/middle_metacarpals.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/middle_metacarpals.STL
new file mode 100644
index 0000000..8daa296
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/middle_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b75dc93c77258d8937bd963b63302626d93b6523b5804528acf5051dd1ce43f8
+size 2881384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/middle_middle.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/middle_middle.STL
new file mode 100644
index 0000000..ab01668
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2c78ce09487e182d3e9bc04e9aeaa3703bde3bfa39b7fa93ef28b3f7360d8c7b
+size 742084
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/middle_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/middle_proximal.STL
new file mode 100644
index 0000000..dd1df3f
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5791bdd9a8a15dc83d94a551862727526bc2117f44251019f007d4ecf00c2af1
+size 642384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/pinky_metacarpals.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/pinky_metacarpals.STL
new file mode 100644
index 0000000..e861fdc
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/pinky_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8b2a8769f855fb89458b3bbdd260a3a40e3d0476964cb47a41744cd61980785a
+size 2864084
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/pinky_middle.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/pinky_middle.STL
new file mode 100644
index 0000000..e435b03
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7bd0bc14bba67ff470781ac07a3830fd37b37d65c77afd20ff46891706863750
+size 742084
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/pinky_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/pinky_proximal.STL
new file mode 100644
index 0000000..fc23377
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b59cedeb7dadad0f90995d5155903023f6c7e5ea7d5d78e9ede81c87b0c13b3a
+size 642384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/ring_metacarpals.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/ring_metacarpals.STL
new file mode 100644
index 0000000..fffe865
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/ring_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:99f1c8836676325e7dadb57ee230bb7d6dcd443de663580e93ee674a258397d3
+size 2897784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/ring_middle.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/ring_middle.STL
new file mode 100644
index 0000000..aef8672
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fb78a0a9d08b3aaa4cedcef721b35529e19bef094c9074b1c21f5201903054fe
+size 742084
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/ring_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/ring_proximal.STL
new file mode 100644
index 0000000..5a8e13a
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:662f3984ffddaecbd26f56c7139113d63a57b31bb385285db2fcb3a801565ae2
+size 642384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_distal.STL
new file mode 100644
index 0000000..3784f66
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c27f90ee7ce7cf0252eb22ad6909fa3c8534387d6a8787f76f1819645c35b6c6
+size 758534
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_metacarpals.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_metacarpals.STL
new file mode 100644
index 0000000..cd2ae88
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:17311da4c364f79c3b5ca460e9e80c48f919ae352f7119cb828cd8ad2baffb07
+size 366784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_metacarpals_base1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..52e0569
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6f7114551ec68d258c324a28fd52c3d01dd63373b53e482d18f46faed7e829e1
+size 1112384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_metacarpals_base2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..e07d3a5
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a5fc858752623b8e0cfafd411495ea71ee2b6fe0c04f43d323a638d97b3a713a
+size 520234
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_proximal.STL
new file mode 100644
index 0000000..c3f6c6d
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L21/linker_hand_l21_right/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7490a69e5462e5aac717b79c311f9f75c9b0f643165dfe1a72eb5090203368d8
+size 1087384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/hand_base_link.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/hand_base_link.STL
new file mode 100644
index 0000000..874d757
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2dc0d80eb470c0ccd6e338650f383efea75a5b77eee7e51d2b6a1da553afcdfe
+size 6296134
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/index_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/index_distal.STL
new file mode 100644
index 0000000..d50b6ee
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4f2699ea27c60f6c28a3ca0cae64ddab40b24b4929bbc23e112cbe08f7ae311c
+size 581734
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/index_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/index_proximal.STL
new file mode 100644
index 0000000..e2e7af5
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:dd77da3a5c18d236c46c3adb51e7be8262e64926a0ee9e996741609d5ed0b08d
+size 640384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/linker_hand_l6_left.xml b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/linker_hand_l6_left.xml
new file mode 100644
index 0000000..1761791
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/linker_hand_l6_left.xml
@@ -0,0 +1,127 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/linkerhand_o6_left.urdf b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/linkerhand_o6_left.urdf
new file mode 100644
index 0000000..884c3fb
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/linkerhand_o6_left.urdf
@@ -0,0 +1,706 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/middle_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/middle_distal.STL
new file mode 100644
index 0000000..53916eb
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b92ed1e3af1d6ef3389287a45c27433a1584a1233fcb0825ee48411b59299697
+size 581734
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/middle_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/middle_proximal.STL
new file mode 100644
index 0000000..cdb518e
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ed2daec64e98506a9fdd3e63708f5644c5d0660ef54c7711c83f15351fc8eb85
+size 640384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/pinky_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/pinky_distal.STL
new file mode 100644
index 0000000..f45efac
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:01a8db3f672c232cd3430c2314e1422cc14950c6f2e03606f9f4d20e66c30311
+size 581734
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/pinky_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/pinky_proximal.STL
new file mode 100644
index 0000000..d1c3b74
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8d0a3df9d46abe366d27132a49a45552031b57d1684fdf3adf7969a19faf6bf0
+size 640384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/ring_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/ring_distal.STL
new file mode 100644
index 0000000..dda3885
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a21ba5ccb885f939788ef701d99a94781d0001ffe2b43a3d79a32c376dab988b
+size 581734
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/ring_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/ring_proximal.STL
new file mode 100644
index 0000000..603eaa5
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d17d67b52a5f02d9ff5dbb0b7315fea055f0ee43a0f2443faf7df28942cf3663
+size 640384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/thumb_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/thumb_distal.STL
new file mode 100644
index 0000000..48df9aa
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:baf3c1b16f94925bcd06768af7ec811de1a97d78f253d26adcd502d35067cf75
+size 823084
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/thumb_metacarpals.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/thumb_metacarpals.STL
new file mode 100644
index 0000000..77e9619
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c91dc63ccd81ca270d166e9e276665ea3fbe6bb42fbee31d1c7bb545b4238045
+size 1403784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/thumb_metacarpals_base2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..aae46e8
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_left/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9e8bd4b4adce43f74df492c1cd3890457b2ac0151c12582c5a8faaf6cdc9f1d9
+size 686384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/hand_base_link.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/hand_base_link.STL
new file mode 100644
index 0000000..a833fde
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f7fc8dae5d375e5a33251593c46f8b882c3a1aaafe89fa9544f0ef57d657f5de
+size 6341484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/index_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/index_distal.STL
new file mode 100644
index 0000000..867ab7c
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f07c23647d61bbc3e1c195fd94a726fa1a7828d128100ad49176af4c2364645f
+size 580434
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/index_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/index_proximal.STL
new file mode 100644
index 0000000..a59ef30
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:38c58f62b9911203069925c3d374703772927ffd3fa98049400fd75e66e7194a
+size 637984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/linker_hand_l6_right.xml b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/linker_hand_l6_right.xml
new file mode 100644
index 0000000..3b7a7d1
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/linker_hand_l6_right.xml
@@ -0,0 +1,127 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/linkerhand_o6_right.urdf b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/linkerhand_o6_right.urdf
new file mode 100644
index 0000000..359e81c
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/linkerhand_o6_right.urdf
@@ -0,0 +1,705 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/middle_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/middle_distal.STL
new file mode 100644
index 0000000..fd557e4
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:15d9e1daab1e75a1ce6808a64d37effd1f7cd179b9d1ecb44f3a4be70d0e186e
+size 580434
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/middle_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/middle_proximal.STL
new file mode 100644
index 0000000..3411791
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:392519c735b9658d3417d793a00f6a0f655b8e8b0a64fdd096eb2bcb31a4de65
+size 637984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/pinky_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/pinky_distal.STL
new file mode 100644
index 0000000..9e0b71c
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1b4b1c44dadd484bc3564ad622e0b7609c60eb8d76937b2a10c7d262cddc96e0
+size 580434
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/pinky_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/pinky_proximal.STL
new file mode 100644
index 0000000..9872e92
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:50a1131fbead7bcf79ed1f435d4d799bd29f637ce81aa5220f31dc8b75a43777
+size 637984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/ring_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/ring_distal.STL
new file mode 100644
index 0000000..55821c9
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e6488d4c8d8a8aa2d857c51178472f50e398dac1a2552cf90d2cf08561ae69cf
+size 580434
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/ring_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/ring_proximal.STL
new file mode 100644
index 0000000..0beb3fa
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e7485a837556d6c8a6101617bb113a625d65a878a00ac297b096d59b89629d5b
+size 637984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/thumb_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/thumb_distal.STL
new file mode 100644
index 0000000..0df8a0a
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c3642be300b1ae3f7f5af071f5b1cfd4dec99e610e57e7bfe44a3b804886a360
+size 820784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/thumb_metacarpals.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/thumb_metacarpals.STL
new file mode 100644
index 0000000..f1a8785
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6bb54d668aea4f71251d48dc010897e08717b0cd13f63b304357fdc36894b897
+size 1403884
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/thumb_metacarpals_base2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..b1c2022
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L6/linker_hand_l6_right/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d92665c1cf3304617674cb4bc64cb20bc42ee49238043a427c222e32b4bdfe56
+size 680684
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/base_link.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/base_link.STL
new file mode 100644
index 0000000..9b5b829
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:28a98b15c4e1a9efac5845b13d88eb352dcb0a297dd25c69b247a5bc01ce9f3e
+size 1666284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link0.STL
new file mode 100644
index 0000000..4e3d5ca
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7c3d43706062d2303f84666337eb3c503038d47059a871a817882958a985f9aa
+size 256884
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link1.STL
new file mode 100644
index 0000000..9080edf
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0be74517180e60134c06c8d50d30b96098017585c9b7f607f2c2130e0ae11b0c
+size 519284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link2.STL
new file mode 100644
index 0000000..1807a0e
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:679997180a67ec28a8c2227bea58fed7a540ca7535fcae5393f94db5c43e7919
+size 573484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link3.STL
new file mode 100644
index 0000000..18adce9
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9cb61e85e4d61a5b63a315348aadd23919115cd54ffa12afced0fc2de929f982
+size 682784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link4.STL
new file mode 100644
index 0000000..aaa7520
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/index_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0d3a1584cf566616b84868ffbcf3b82545fbf259e3ed40c2661456799192ceae
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/linker_hand_l7_left.xml b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/linker_hand_l7_left.xml
new file mode 100644
index 0000000..a6ab383
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/linker_hand_l7_left.xml
@@ -0,0 +1,197 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/linkerhand_o7_left.urdf b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/linkerhand_o7_left.urdf
new file mode 100644
index 0000000..bc956d9
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/linkerhand_o7_left.urdf
@@ -0,0 +1,1512 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link0.STL
new file mode 100644
index 0000000..eb1861a
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7afaaaa27481b0427785070df79e3251a1dfd6fdcd22b2089a117f75bd90c623
+size 256884
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link1.STL
new file mode 100644
index 0000000..cdb5796
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c8759223942b77ab08666af40c834724920d138e10629de15a45af740c75ba22
+size 519284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link2.STL
new file mode 100644
index 0000000..dfb76eb
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:bc49c7f4bf13de34f1e893e993cf1c21f9475bbba837a0dea3157dec1b49c297
+size 573484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link3.STL
new file mode 100644
index 0000000..9786eae
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ff45ec96203c305f53724de8aeb9e02c32eb6ffc61497aa2275e128e104bb500
+size 682784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link4.STL
new file mode 100644
index 0000000..befee60
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/little_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:25c31141f955d73c7bae5f505e90041e1710cff72401704f135fdde3e72c76d9
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link0.STL
new file mode 100644
index 0000000..8c30ca4
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:21c5311fef9933ee8b59ea2935a1c407a33d37bf1088167ee4948e80be0a234c
+size 519284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link1.STL
new file mode 100644
index 0000000..2866f3b
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ce912434165ca8d9cd33643f7251c22fbe12c55cc73d837f655337db234a5d63
+size 573484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link2.STL
new file mode 100644
index 0000000..63ea564
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:472f53ed3b676f49f0ad160a973ec0830558df31b63d9a1c9220573c7a885555
+size 682784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link3.STL
new file mode 100644
index 0000000..6ab996d
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/middle_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d1e2a600af4c477829ac553dc8a02ebbaadf42ec3a6e7199dc7d38d0b967687a
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link0.STL
new file mode 100644
index 0000000..309640d
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ab8e34568c6cae073f1229ea54d2204a3f3c69704fc1446a7c9595987b39c6bb
+size 256884
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link1.STL
new file mode 100644
index 0000000..48851b9
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a379fd662d6337e265e6b2ab957dc861eca7bc44c7a06150dda0fa2ca69f5347
+size 519284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link2.STL
new file mode 100644
index 0000000..91052f2
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8b231d68309ec84c732df40f703f4b3ae7a8f00369f79c488804d05a62dd4182
+size 573484
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link3.STL
new file mode 100644
index 0000000..8addb2f
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7801ed795a1b4d4d78da6ab3cd87a481460c2ee6e29d56be162a239a63ff3a40
+size 682784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link4.STL
new file mode 100644
index 0000000..51a99b2
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/ring_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:51d96da28e051c953465ba0eb6867cc0ac5903a1f319c502eb92545fc72c79ca
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link0.STL
new file mode 100644
index 0000000..a8426ef
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7ed8c26294c961e3edb4c6fb2e331a2a16b02f9c2c8f9bdbb0dcea2741838ee1
+size 385684
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link1.STL
new file mode 100644
index 0000000..0481409
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c6b07079056fad59c1b232f035fe0a48495ca2d9e6639b02d3635afdfb436bdd
+size 392584
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link2.STL
new file mode 100644
index 0000000..02843ab
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f88d7c7dc37703512b59225d58fb494cde1de2fbb3ca0525cbbdb059b7bd5b0d
+size 986384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link3.STL
new file mode 100644
index 0000000..a6cf47b
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e0819ace8213c6ebb0a97c5fba5e904e2984aad31ac2b129b61068a2fd9c9c7b
+size 225184
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link4.STL
new file mode 100644
index 0000000..12138eb
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:030aad2997c9501f4cad40f074a26228f53501cfd5099c9d33096235c7fb880d
+size 283384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link5.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link5.STL
new file mode 100644
index 0000000..a54c2fa
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_left/thumb_link5.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:82513e0a64a64aa0f7f4f3b85a5b8a5f09187d55e7fed5e9aa40fe3407066423
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/base_link.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/base_link.STL
new file mode 100644
index 0000000..955d770
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:34662efe4b9d4a34cc5066945d2ac95ef66d48926ace3adc9c871808cbde851e
+size 1509984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link0.STL
new file mode 100644
index 0000000..efc95c1
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0a82724b15185a15534eefccf69127becfff74cc7b43579d228368b9e21a6ec8
+size 257984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link1.STL
new file mode 100644
index 0000000..2e67d56
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:24dd990af121925ae6836dce055b4ff992209fdb7bf06390cd2797c3e7e7f247
+size 521284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link2.STL
new file mode 100644
index 0000000..ea9760f
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:68b823511f995073faccfcc349d21e66cc0678165e5164867da43958c6c2cabe
+size 572984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link3.STL
new file mode 100644
index 0000000..73ea2cd
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2d59058d948c3b957043a393dfc6ed4bef5ca6473b34ca978395ebdf371e555c
+size 746784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link4.STL
new file mode 100644
index 0000000..3e13eaf
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/index_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6d1359761f91062c97afd95e0fa92b6ea0e3339735dfc859c918e78f090031d1
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/linker_hand_l7_right.urdf b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/linker_hand_l7_right.urdf
new file mode 100644
index 0000000..733e337
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/linker_hand_l7_right.urdf
@@ -0,0 +1,1537 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/linker_hand_l7_right.xml b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/linker_hand_l7_right.xml
new file mode 100644
index 0000000..8c67e4a
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/linker_hand_l7_right.xml
@@ -0,0 +1,180 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link0.STL
new file mode 100644
index 0000000..214afd8
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:26e8533229f302952431d8a7377b978dfee2442829d3de46636360504ec0dfc1
+size 257984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link1.STL
new file mode 100644
index 0000000..14ae20e
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:56376eb7b25a0e08ad0f9cc0e2c79d01bc984c99e4ced630eb07a28768dd88fa
+size 521284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link2.STL
new file mode 100644
index 0000000..28b5f1a
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f0e5a3ca00f8935873096793fb7328879b53f2458268033a7f8c39aa92679610
+size 572984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link3.STL
new file mode 100644
index 0000000..1b23ac6
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:43f092a01de5b3b427a49ef5dd1351ccf1b50a662db2f6b5249b5ed4fe918333
+size 746784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link4.STL
new file mode 100644
index 0000000..1af2f41
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/little_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4336131b69b091b21dbdfbe38c3fb40d1ecb0242fbcb5b8e656c5835559a4eff
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link0.STL
new file mode 100644
index 0000000..9682313
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:85c9267c2d16022d712e7b367ef2aabef9e31e134c3c63fb45b084515b12de10
+size 521284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link1.STL
new file mode 100644
index 0000000..c982b97
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ab5028794863195998b14659f43a6d1902e55b50d72322232c44345ac47a1e5c
+size 572984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link2.STL
new file mode 100644
index 0000000..0c37d7f
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f818f2791654887012d820178e3e1da18188eef2ddc2803011c4fb1f9ffa03aa
+size 746784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link3.STL
new file mode 100644
index 0000000..5cc1bb5
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/middle_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2133f86ea956222ef16bfa601f1c2952e4630cba1f3c5cf41a7c40b442e8acdc
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link0.STL
new file mode 100644
index 0000000..e347da4
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b20c5bec23d8b556e3b90cbb0aa451110a7c0fa9cb38db640e3ad0cce129fc47
+size 257984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link1.STL
new file mode 100644
index 0000000..e8cbf9c
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:13a0fdc7302d2e90de835f9331ac1c157239e9de9fbe92b289af06803a2618a0
+size 521284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link2.STL
new file mode 100644
index 0000000..0d46ec5
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7aa1779441cb58ee2a15ef850b813dbeb3cbe089ac4a54af64a101d3e3484902
+size 572984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link3.STL
new file mode 100644
index 0000000..e8291d5
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6afe65c8555fe99eb9e660074fa8e976ae53fed9cebe57a63885798a366d9765
+size 746784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link4.STL
new file mode 100644
index 0000000..f1b6f7b
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/ring_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:45c8752a716edc970d9263940f2e2ac4df5523f85d5a20bc8cc17b64902f7c2f
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link0.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link0.STL
new file mode 100644
index 0000000..9f95649
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3eca4e14a33c4e159a74ff3a60c0234172c8ed58f190d64b4a511c49c1fe829c
+size 386584
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link1.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link1.STL
new file mode 100644
index 0000000..b5d4461
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b09b4d4301f0d1f1791945952a853b634f2bee75951f02e8704d1106231fcf86
+size 392984
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link2.STL
new file mode 100644
index 0000000..2b197fc
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5b5d4b4701098fc64002d88e056bc2ef81295ee535731728efbf0189b85547d2
+size 327684
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link3.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link3.STL
new file mode 100644
index 0000000..8cc7ee2
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9b5ef49f1b08859c1f16b6513a49ccab12e235413c541511a528744caa8ccfc3
+size 225084
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link4.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link4.STL
new file mode 100644
index 0000000..c551d71
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d2621267fcdde28faddb52af9518c041ab8d763b57aec6cd917d3e5cd1e13cbd
+size 844784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link5.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link5.STL
new file mode 100644
index 0000000..8492f9d
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/L7/linker_hand_l7_right/thumb_link5.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ad1a9626c75a421cf3bef28da1bf4a267f20ef4367f16a955c320bbf0e849551
+size 7284
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/hand_base_link.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/hand_base_link.STL
new file mode 100644
index 0000000..874d757
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2dc0d80eb470c0ccd6e338650f383efea75a5b77eee7e51d2b6a1da553afcdfe
+size 6296134
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/index_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/index_distal.STL
new file mode 100644
index 0000000..d50b6ee
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4f2699ea27c60f6c28a3ca0cae64ddab40b24b4929bbc23e112cbe08f7ae311c
+size 581734
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/index_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/index_proximal.STL
new file mode 100644
index 0000000..e2e7af5
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:dd77da3a5c18d236c46c3adb51e7be8262e64926a0ee9e996741609d5ed0b08d
+size 640384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/linker_hand_o6_left.xml b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/linker_hand_o6_left.xml
new file mode 100644
index 0000000..45b4e90
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/linker_hand_o6_left.xml
@@ -0,0 +1,157 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/middle_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/middle_distal.STL
new file mode 100644
index 0000000..53916eb
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b92ed1e3af1d6ef3389287a45c27433a1584a1233fcb0825ee48411b59299697
+size 581734
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/middle_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/middle_proximal.STL
new file mode 100644
index 0000000..cdb518e
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ed2daec64e98506a9fdd3e63708f5644c5d0660ef54c7711c83f15351fc8eb85
+size 640384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/pinky_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/pinky_distal.STL
new file mode 100644
index 0000000..f45efac
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:01a8db3f672c232cd3430c2314e1422cc14950c6f2e03606f9f4d20e66c30311
+size 581734
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/pinky_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/pinky_proximal.STL
new file mode 100644
index 0000000..d1c3b74
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8d0a3df9d46abe366d27132a49a45552031b57d1684fdf3adf7969a19faf6bf0
+size 640384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/ring_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/ring_distal.STL
new file mode 100644
index 0000000..dda3885
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a21ba5ccb885f939788ef701d99a94781d0001ffe2b43a3d79a32c376dab988b
+size 581734
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/ring_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/ring_proximal.STL
new file mode 100644
index 0000000..603eaa5
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d17d67b52a5f02d9ff5dbb0b7315fea055f0ee43a0f2443faf7df28942cf3663
+size 640384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/thumb_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/thumb_distal.STL
new file mode 100644
index 0000000..48df9aa
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:baf3c1b16f94925bcd06768af7ec811de1a97d78f253d26adcd502d35067cf75
+size 823084
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/thumb_metacarpals.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/thumb_metacarpals.STL
new file mode 100644
index 0000000..77e9619
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c91dc63ccd81ca270d166e9e276665ea3fbe6bb42fbee31d1c7bb545b4238045
+size 1403784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/thumb_metacarpals_base2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..aae46e8
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_left/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9e8bd4b4adce43f74df492c1cd3890457b2ac0151c12582c5a8faaf6cdc9f1d9
+size 686384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/hand_base_link.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/hand_base_link.STL
new file mode 100644
index 0000000..874d757
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2dc0d80eb470c0ccd6e338650f383efea75a5b77eee7e51d2b6a1da553afcdfe
+size 6296134
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/index_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/index_distal.STL
new file mode 100644
index 0000000..d50b6ee
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4f2699ea27c60f6c28a3ca0cae64ddab40b24b4929bbc23e112cbe08f7ae311c
+size 581734
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/index_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/index_proximal.STL
new file mode 100644
index 0000000..e2e7af5
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:dd77da3a5c18d236c46c3adb51e7be8262e64926a0ee9e996741609d5ed0b08d
+size 640384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/linker_hand_o6_right.xml b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/linker_hand_o6_right.xml
new file mode 100644
index 0000000..8723091
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/linker_hand_o6_right.xml
@@ -0,0 +1,157 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/middle_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/middle_distal.STL
new file mode 100644
index 0000000..53916eb
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b92ed1e3af1d6ef3389287a45c27433a1584a1233fcb0825ee48411b59299697
+size 581734
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/middle_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/middle_proximal.STL
new file mode 100644
index 0000000..cdb518e
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ed2daec64e98506a9fdd3e63708f5644c5d0660ef54c7711c83f15351fc8eb85
+size 640384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/pinky_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/pinky_distal.STL
new file mode 100644
index 0000000..f45efac
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:01a8db3f672c232cd3430c2314e1422cc14950c6f2e03606f9f4d20e66c30311
+size 581734
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/pinky_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/pinky_proximal.STL
new file mode 100644
index 0000000..d1c3b74
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8d0a3df9d46abe366d27132a49a45552031b57d1684fdf3adf7969a19faf6bf0
+size 640384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/ring_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/ring_distal.STL
new file mode 100644
index 0000000..dda3885
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a21ba5ccb885f939788ef701d99a94781d0001ffe2b43a3d79a32c376dab988b
+size 581734
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/ring_proximal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/ring_proximal.STL
new file mode 100644
index 0000000..603eaa5
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d17d67b52a5f02d9ff5dbb0b7315fea055f0ee43a0f2443faf7df28942cf3663
+size 640384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/thumb_distal.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/thumb_distal.STL
new file mode 100644
index 0000000..48df9aa
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:baf3c1b16f94925bcd06768af7ec811de1a97d78f253d26adcd502d35067cf75
+size 823084
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/thumb_metacarpals.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/thumb_metacarpals.STL
new file mode 100644
index 0000000..77e9619
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c91dc63ccd81ca270d166e9e276665ea3fbe6bb42fbee31d1c7bb545b4238045
+size 1403784
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/thumb_metacarpals_base2.STL b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..aae46e8
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/urdf/O6/linker_hand_o6_right/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9e8bd4b4adce43f74df492c1cd3890457b2ac0151c12582c5a8faaf6cdc9f1d9
+size 686384
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/__init__.py b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/joint_monitor.py b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/joint_monitor.py
new file mode 100644
index 0000000..a228a10
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/joint_monitor.py
@@ -0,0 +1,148 @@
+"""MuJoCo 关节实时监控面板（进度条显示各关节角度）。"""
+
+from PyQt5.QtCore import Qt, pyqtSignal
+from PyQt5.QtGui import QFont
+from PyQt5.QtWidgets import (
+ QFrame,
+ QHBoxLayout,
+ QLabel,
+ QProgressBar,
+ QScrollArea,
+ QSizePolicy,
+ QVBoxLayout,
+ QWidget,
+)
+
+
+class JointMonitorWindow(QWidget):
+ """深色 Joint 面板：左侧关节名，右侧进度条 + 数值。"""
+
+ values_updated = pyqtSignal(object) # list[float]
+
+ def __init__(self, joint_names, joint_ranges, title="Joint", parent=None):
+ super().__init__(parent)
+ self.joint_names = list(joint_names)
+ self.joint_ranges = [(float(lo), float(hi)) for lo, hi in joint_ranges]
+ self._bars = []
+ self._value_labels = []
+
+ self.setWindowTitle(f"{title} Monitor")
+ self.setMinimumWidth(760)
+ self.setMinimumHeight(860)
+ self.resize(860, 980)
+ self.setStyleSheet(
+ """
+ QWidget {
+ background-color: #2b2b2b;
+ color: #d0d0d0;
+ font-size: 22px;
+ }
+ QLabel#header {
+ background-color: #5a1a1a;
+ color: #f0f0f0;
+ font-size: 30px;
+ font-weight: bold;
+ padding: 18px 22px;
+ border-radius: 4px;
+ }
+ QLabel#jointName {
+ color: #e8e8e8;
+ font-size: 22px;
+ font-weight: 600;
+ }
+ QLabel#jointValue {
+ color: #ffffff;
+ font-size: 24px;
+ font-weight: bold;
+ min-width: 140px;
+ }
+ QProgressBar {
+ border: 2px solid #666;
+ border-radius: 5px;
+ background-color: #1e1e1e;
+ text-align: center;
+ color: #e0e0e0;
+ min-height: 38px;
+ max-height: 38px;
+ }
+ QProgressBar::chunk {
+ background-color: #a0a0a0;
+ border-radius: 4px;
+ }
+ QScrollArea {
+ border: none;
+ }
+ """
+ )
+
+ root = QVBoxLayout(self)
+ root.setContentsMargins(18, 18, 18, 18)
+ root.setSpacing(16)
+
+ header = QLabel(title)
+ header.setObjectName("header")
+ header.setFont(QFont("Sans Serif", 24, QFont.Bold))
+ root.addWidget(header)
+
+ scroll = QScrollArea()
+ scroll.setWidgetResizable(True)
+ scroll.setHorizontalScrollBarPolicy(Qt.ScrollBarAlwaysOff)
+ body = QWidget()
+ body_layout = QVBoxLayout(body)
+ body_layout.setContentsMargins(8, 8, 8, 8)
+ body_layout.setSpacing(14)
+
+ for name, (lo, hi) in zip(self.joint_names, self.joint_ranges):
+ row = QFrame()
+ row_layout = QHBoxLayout(row)
+ row_layout.setContentsMargins(6, 6, 6, 6)
+ row_layout.setSpacing(18)
+
+ name_label = QLabel(name)
+ name_label.setObjectName("jointName")
+ name_label.setMinimumWidth(260)
+ name_label.setSizePolicy(QSizePolicy.Fixed, QSizePolicy.Preferred)
+
+ bar = QProgressBar()
+ bar.setRange(0, 1000)
+ bar.setValue(0)
+ bar.setFormat("")
+ bar.setTextVisible(False)
+
+ value_label = QLabel("0.000")
+ value_label.setObjectName("jointValue")
+ value_label.setAlignment(Qt.AlignRight | Qt.AlignVCenter)
+
+ row_layout.addWidget(name_label)
+ row_layout.addWidget(bar, stretch=1)
+ row_layout.addWidget(value_label)
+ body_layout.addWidget(row)
+ self._bars.append(bar)
+ self._value_labels.append(value_label)
+
+ body_layout.addStretch(1)
+ scroll.setWidget(body)
+ root.addWidget(scroll)
+
+ self.values_updated.connect(self._on_values_updated)
+
+ def update_values(self, values):
+ """线程安全：任意线程调用，通过信号刷新 UI。"""
+ self.values_updated.emit(list(values))
+
+ def _on_values_updated(self, values):
+ for i, val in enumerate(values):
+ if i >= len(self._bars):
+ break
+ lo, hi = self.joint_ranges[i]
+ span = hi - lo
+ if span <= 1e-9:
+ pct = 0
+ else:
+ pct = int(1000 * max(0.0, min(1.0, (float(val) - lo) / span)))
+ self._bars[i].setValue(pct)
+ if abs(val) < 1e-4 and val != 0.0:
+ text = f"{val:.2e}"
+ else:
+ text = f"{val:.3f}"
+ self._value_labels[i].setText(text)
diff --git a/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/mapping.py b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/mapping.py
new file mode 100644
index 0000000..6816d1c
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/linker_hand_mujoco_ros2/utils/mapping.py
@@ -0,0 +1,240 @@
+import sys,os
+# /---------------------------------------------------------------
+L6_JOINT_MAP = {
+ 0:1, 1:0, 2:0, 3:2, 4:2, 5:3, 6:3, 7:4, 8:4, 9:5, 10:5
+ }
+L6_JOINT_ARC = [(0, 1.54), (0, 0.52), (0,0.96), (0,1.57), (0,1.40), (0,1.57), (0,1.40), (0,1.57), (0,1.40), (0,1.57), (0,1.40)]
+
+# O6：与 L6 同构（6 路控制），使用 mujoco_testwork 验证后的关节限位
+# actuator 顺序: yaw, pitch, ip, index_mcp, index_dip, middle_*, ring_*, pinky_*
+O6_JOINT_MAP = {
+ 0: 1, 1: 0, 2: 0, 3: 2, 4: 2, 5: 3, 6: 3, 7: 4, 8: 4, 9: 5, 10: 5
+ }
+O6_JOINT_ARC = [
+ (0, 1.36), (0, 0.58), (0, 1.08),
+ (0, 1.60), (0, 1.43),
+ (0, 1.60), (0, 1.43),
+ (0, 1.60), (0, 1.43),
+ (0, 1.60), (0, 1.43),
+]
+# mimic: (slave_actuator_idx, master_actuator_idx, ratio)
+O6_MIMIC = [
+ (2, 1, 1.86), # thumb_ip <- thumb_cmc_pitch
+ (4, 3, 0.89), # index_dip <- index_mcp
+ (6, 5, 0.89),
+ (8, 7, 0.89),
+ (10, 9, 0.89),
+]
+
+# /---------------------------------------------------------------
+L7_JOINT_MAP = {
+ 0: 6, 1: 1, 2: 0, 3: 0, 4: 0,
+ 5: 2, 6: 2, 7: 2,
+ 8: 3, 9: 3, 10: 3,
+ 11: 4, 12: 4, 13: 4,
+ 14: 5, 15: 5, 16: 5
+}
+L7_JOINT_ARC = [(-0.52,1.01), (0,1.43), (0,0.44), (0,1.45), (0,1.57), (0,1.41), (0,1.62), (0,0.96), (0,1.41), (0,1.62), (0,0.96), (0,1.41), (0,1.62), (0,0.96), (0,1.41), (0,1.62), (0,0.96)]
+# /---------------------------------------------------------------
+
+L10_JOINT_MAP = {
+ 0: 9, 1: 1, 2: 0, 3: 0, 4: 0, 5: 6,
+ 6: 2, 7: 2, 8: 2, 9: 3, 10: 3, 11: 3,
+ 12: 7, 13: 4, 14: 4, 15: 4, 16: 8, 17: 5,
+ 18: 5, 19: 5
+}
+L10_JOINT_ARC = [(-0.1396, 0.349), (0, 1.57), (-1.57, 0), (-1.57, 0), (-1.57, 0), (-0.26, 0.26), (0, 1.396), (0, 1.57), (0, 1.57), (0, 1.57), (0, 1.57), (0, 1.57), (-0.26, 0.26), (0, 1.57), (0, 1.57), (0, 1.57), (-0.26, 0.26), (0, 1.57), (0, 1.57), (0, 1.57)]
+# /---------------------------------------------------------------
+L20_JOINT_MAP = {
+ 0: 10, 1: 5, 2: 0, 3: 15, 4: 15,
+ 5: 6, 6: 1, 7: 16, 8: 16,
+ 9: 7, 10: 2, 11: 17, 12: 17,
+ 13: 8, 14: 3, 15: 18, 16: 18,
+ 17: 9, 18: 4, 19: 19, 20:19
+}
+
+
+L20_JOINT_ARC = [(-0.297,0.683), (0.122,1.78), (0,0.87), (0,1.29), (0,1.29), (-0.26,0.26), (0,1.4), (0,1.08), (0,1.15), (-0.26,0.26), (0,1.4), (0,1.08), (0,1.15), (-0.26,0.26), (0,1.4), (0,1.08), (0,1.15), (-0.26,0.26), (0,1.4), (0,1.08), (0,1.15)]
+
+# /---------------------------------------------------------------
+# 注意L21的拇指控制为数组后4位
+L21_JOINT_MAP = {
+ 0: 6, 1: 1, 2: 21,
+ 3: 7, 4: 2, 5: 22,
+ 6: 8, 7: 3, 8: 23,
+ 9: 9, 10: 4, 11: 24,
+ 12: 10, 13: 5,
+ 14: 0, 15: 15, 16: 20
+}
+L21_JOINT_ARC = [(-0.18, 0.18),(0, 1.57),(0, 1.57),(-0.18, 0.18),(0, 1.57),(0, 1.57),(-0.18, 0.18),(0, 1.57),(0, 1.57),(0, 0.18),(0, 1.57),(0, 1.57),(-0.6, 0.6),(0, 1.6),(0, 1),(0, 1.57),(0, 1.57)]
+
+
+
+#---------------------------------------------------------------------------------------------------
+# L6 L
+l6_l_min = [0, 0, 0, 0, 0, 0]
+l6_l_max = [1.54, 1.52, 1.57, 1.57, 1.57, 1.57]
+l6_l_derict = [-1, -1, -1, -1, -1, -1]
+# L6 R
+l6_r_min = [0, 0, 0, 0, 0, 0]
+l6_r_max = [1.54, 1.52, 1.57, 1.57, 1.57, 1.57]
+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.30, 1.60, 1.60, 1.60, 1.60]
+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.60, 1.60, 1.60, 1.60]
+o6_r_derict = [-1, -1, -1, -1, -1, -1]
+#---------------------------------------------------------------------------------------------------
+# L7 L OK
+l7_l_min = [0, 0, 0, 0, 0, 0, -0.52]
+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, 0.13, 0.26, 1.01]
+l10_r_derict = [-1, -1, -1, -1, -1, -1, -1, 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 in ("L6", "O6"):
+ num = 6
+ if hand_joint == "O6":
+ l_min, l_max, l_derict = o6_l_min, o6_l_max, o6_l_derict
+ else:
+ l_min, l_max, l_derict = l6_l_min, l6_l_max, l6_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 in ("L6", "O6"):
+ num = 6
+ if hand_joint == "O6":
+ r_min, r_max, r_derict = o6_r_min, o6_r_max, o6_r_derict
+ else:
+ r_min, r_max, r_derict = l6_r_min, l6_r_max, l6_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 apply_mimic(ctrl_values, mimic_rules, joint_arc):
+ """按主从比例写入从动关节目标，并裁剪到 ctrlrange。"""
+ if not mimic_rules:
+ return ctrl_values
+ out = list(ctrl_values)
+ for slave, master, ratio in mimic_rules:
+ val = out[master] * ratio
+ lo, hi = joint_arc[slave]
+ out[slave] = max(lo, min(hi, val))
+ return out
+
+
+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))
\ No newline at end of file
diff --git a/src/linker_hand_mujoco_ros2/package.xml b/src/linker_hand_mujoco_ros2/package.xml
new file mode 100644
index 0000000..e53ff61
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/package.xml
@@ -0,0 +1,22 @@
+
+
+
+ linker_hand_mujoco_ros2
+ 0.0.0
+ TODO: Package description
+ linkerhand
+ TODO: License declaration
+
+ rclpy
+ std_msgs
+ sensor_msgs
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+
+ ament_python
+
+
diff --git a/src/linker_hand_mujoco_ros2/requirements.txt b/src/linker_hand_mujoco_ros2/requirements.txt
new file mode 100644
index 0000000..b9df699
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/requirements.txt
@@ -0,0 +1,20 @@
+python-can
+dm_env
+pexpect
+pyquaternion
+pyagxrobots
+pycryptodome
+ipython
+h5py
+PyYAML
+tqdm
+wandb
+pybullet
+mediapipe
+pyqt5
+pyqtgraph
+dm_control
+uvicorn
+matplotlib
+sapien
+mujoco
diff --git a/src/linker_hand_mujoco_ros2/resource/linker_hand_mujoco_ros2 b/src/linker_hand_mujoco_ros2/resource/linker_hand_mujoco_ros2
new file mode 100644
index 0000000..e69de29
diff --git a/src/linker_hand_mujoco_ros2/setup.cfg b/src/linker_hand_mujoco_ros2/setup.cfg
new file mode 100644
index 0000000..0b18a0d
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/linker_hand_mujoco_ros2
+[install]
+install_scripts=$base/lib/linker_hand_mujoco_ros2
diff --git a/src/linker_hand_mujoco_ros2/setup.py b/src/linker_hand_mujoco_ros2/setup.py
new file mode 100644
index 0000000..652c296
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/setup.py
@@ -0,0 +1,30 @@
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'linker_hand_mujoco_ros2'
+
+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', package_name, 'launch'), glob('launch/*.launch.py')),
+ ('share/' + package_name, ['package.xml']),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='linkerhand',
+ maintainer_email='linkerhand@todo.todo',
+ description='TODO: Package description',
+ license='TODO: License declaration',
+ tests_require=['pytest'],
+ entry_points={
+ 'console_scripts': [
+ 'linker_hand_mujoco_ros2_node=linker_hand_mujoco_ros2.linker_hand_mujoco_ros2:main',
+ 'hand_curve_recorder=linker_hand_mujoco_ros2.hand_curve_recorder:main',
+ ],
+ },
+)
diff --git a/src/linker_hand_mujoco_ros2/test/test_copyright.py b/src/linker_hand_mujoco_ros2/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/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'
diff --git a/src/linker_hand_mujoco_ros2/test/test_flake8.py b/src/linker_hand_mujoco_ros2/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/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)
diff --git a/src/linker_hand_mujoco_ros2/test/test_pep257.py b/src/linker_hand_mujoco_ros2/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/src/linker_hand_mujoco_ros2/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'
diff --git a/src/linker_hand_ros2_sdk/launch/linker_hand.launch.py b/src/linker_hand_ros2_sdk/launch/linker_hand.launch.py
new file mode 100644
index 0000000..7c090a6
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/launch/linker_hand.launch.py
@@ -0,0 +1,21 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ return LaunchDescription([
+ Node(
+ package='linker_hand_ros2_sdk',
+ executable='linker_hand_sdk',
+ name='linker_hand_sdk',
+ output='screen',
+ parameters=[{
+ 'hand_type': 'left', # 配置Linker Hand灵巧手类型 left | right 字母为小写
+ 'hand_joint': "O6", # O6\L6P\L6\L7\L10\L20\G20(工业版)\L21 字母为大写
+ 'is_touch': True, # 配置Linker Hand灵巧手是否有压力传感器 True | False
+ 'can': 'can0', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1。注：蓝色盒子为Linux下can0，WIN下位PCAN_USBBUS1。透明盒子Linux下为can0，WIN下为0
+ "modbus": "None" # "None" | "/dev/ttyUSB0" 这里需要修改为实际的Modbus总线名称 如果是win系统则 COM* Ubuntu则为/dev/ttyUSB* 注意添加sudo chmod 777 /dev/ttyUSB*权限
+ }],
+ ),
+ ])
diff --git a/src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py b/src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py
new file mode 100644
index 0000000..4d81d46
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/launch/linker_hand_double.launch.py
@@ -0,0 +1,35 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ return LaunchDescription([
+ Node(
+ package='linker_hand_ros2_sdk',
+ executable='linker_hand_sdk',
+ name='linker_hand_sdk_left',
+ output='screen',
+ parameters=[{
+ 'hand_type': 'left', # 配置Linker Hand灵巧手类型 left | right 字母为小写
+ 'hand_joint': "G20", # O6\L6P\L6\L7\L10\L20\G20(工业版)\L21 字母为大写
+ 'is_touch': True, # 配置Linker Hand灵巧手是否有压力传感器 True | False
+ 'can': 'can0', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1
+ "modbus": "None" # "None" | "/dev/ttyUSB0" 这里需要修改为实际的Modbus总线名称 如果是win系统则 COM* Ubuntu则为/dev/ttyUSB*
+ }],
+ ),
+
+ Node(
+ package='linker_hand_ros2_sdk',
+ executable='linker_hand_sdk',
+ name='linker_hand_sdk_right',
+ output='screen',
+ parameters=[{
+ 'hand_type': 'right', # 配置Linker Hand灵巧手类型 left | right 字母为小写
+ 'hand_joint': "G20", # O6\L6P\L6\L7\L10\L20\G20(工业版)\L21 字母为大写
+ 'is_touch': True, # 配置Linker Hand灵巧手是否有压力传感器 True | False
+ 'can': 'can1', # 这里需要修改为实际的CAN总线名称 如果是win系统则类似于 PCAN_USBBUS1
+ "modbus": "None" # "None" | "/dev/ttyUSB0" 这里需要修改为实际的Modbus总线名称 如果是win系统则 COM* Ubuntu则为/dev/ttyUSB*
+ }],
+ ),
+ ])
diff --git a/src/linker_hand_ros2_sdk/launch/test.bak b/src/linker_hand_ros2_sdk/launch/test.bak
new file mode 100644
index 0000000..a4d8d10
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/launch/test.bak
@@ -0,0 +1,31 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ return LaunchDescription([
+ Node(
+ package='linker_hand_ros2_sdk',
+ executable='linker_hand_sdk',
+ name='linker_hand_sdk_left',
+ output='screen',
+ parameters=[{
+ 'hand_type': 'left',
+ 'hand_joint': "L10", # 这里需要修改为实际Linker Hand的型号 L7、L10、L20、L21、L25
+ 'is_touch': True, # 是否带有压力传感器
+ 'can': 'can0', # 这里需要修改为实际的CAN总线名称
+ }],
+ ),
+
+ Node(
+ package='linker_hand_ros2_sdk',
+ executable='linker_hand_sdk',
+ name='linker_hand_sdk_right',
+ output='screen',
+ parameters=[{
+ 'hand_type': 'right',
+ 'hand_joint': "L10", # 这里需要修改为实际Linker Hand的型号 L7、L10、L20、L21、L25
+ 'is_touch': True, # 是否带有压力传感器
+ 'can': 'can0', # 这里需要修改为实际的CAN总线名称
+ }],
+ ),
+ ])
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/__init__.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L10_positions.yaml b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L10_positions.yaml
new file mode 100644
index 0000000..ea7439d
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L10_positions.yaml
@@ -0,0 +1,146 @@
+LEFT_HAND:
+- ACTION_NAME: 张开
+ POSITION:
+ - 255
+ - 70
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+- ACTION_NAME: 捏合5CM
+ POSITION:
+ - 165
+ - 70
+ - 165
+ - 165
+ - 255
+ - 255
+ - 113
+ - 255
+ - 255
+ - 88
+- ACTION_NAME: 捏合1CM
+ POSITION:
+ - 150
+ - 70
+ - 155
+ - 155
+ - 255
+ - 255
+ - 113
+ - 255
+ - 255
+ - 88
+- ACTION_NAME: 握3CM物品
+ POSITION:
+ - 113
+ - 70
+ - 85
+ - 85
+ - 85
+ - 85
+ - 85
+ - 255
+ - 255
+ - 88
+- ACTION_NAME: 准备抓握
+ POSITION:
+ - 255
+ - 70
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 121
+- ACTION_NAME: 拇指弯曲
+ POSITION:
+ - 35
+ - 140
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 30
+- ACTION_NAME: 食指弯曲
+ POSITION:
+ - 255
+ - 70
+ - 0
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+- ACTION_NAME: shishi
+ POSITION:
+ - 85
+ - 30
+ - 255
+ - 0
+ - 0
+ - 255
+ - 0
+ - 0
+ - 0
+ - 66
+RIGHT_HAND:
+- ACTION_NAME: 张开
+ POSITION:
+ - 255
+ - 70
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+- ACTION_NAME: 捏合5CM
+ POSITION:
+ - 165
+ - 70
+ - 165
+ - 165
+ - 255
+ - 255
+ - 113
+ - 255
+ - 255
+ - 88
+- ACTION_NAME: 捏合1CM
+ POSITION:
+ - 150
+ - 70
+ - 155
+ - 155
+ - 255
+ - 255
+ - 113
+ - 255
+ - 255
+ - 88
+- ACTION_NAME: 准备抓握
+ POSITION:
+ - 255
+ - 70
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 121
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L20_positions.yaml b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L20_positions.yaml
new file mode 100644
index 0000000..e69de29
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L21_positions.yaml b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L21_positions.yaml
new file mode 100644
index 0000000..a23101c
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L21_positions.yaml
@@ -0,0 +1,110 @@
+LEFT_HAND:
+- ACTION_NAME: 张开
+ POSITION:
+ - 96
+ - 255
+ - 255
+ - 255
+ - 255
+ - 150
+ - 114
+ - 151
+ - 189
+ - 255
+ - 180
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+- ACTION_NAME: 握拳
+ POSITION:
+ - 177
+ - 0
+ - 0
+ - 0
+ - 0
+ - 51
+ - 114
+ - 151
+ - 189
+ - 255
+ - 79
+ - 255
+ - 255
+ - 255
+ - 255
+ - 131
+ - 222
+ - 244
+ - 255
+ - 255
+ - 0
+ - 0
+ - 0
+ - 0
+ - 0
+RIGHT_HAND:
+- ACTION_NAME: 张开
+ POSITION:
+ - 96
+ - 255
+ - 255
+ - 255
+ - 255
+ - 150
+ - 114
+ - 151
+ - 189
+ - 255
+ - 180
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+- ACTION_NAME: 握拳
+ POSITION:
+ - 230
+ - 80
+ - 51
+ - 42
+ - 7
+ - 35
+ - 114
+ - 151
+ - 189
+ - 255
+ - 58
+ - 255
+ - 255
+ - 255
+ - 255
+ - 133
+ - 5
+ - 0
+ - 0
+ - 0
+ - 30
+ - 0
+ - 0
+ - 0
+ - 0
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L25_positions.yaml b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L25_positions.yaml
new file mode 100644
index 0000000..709b0c3
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L25_positions.yaml
@@ -0,0 +1,83 @@
+LEFT_HAND:
+- ACTION_NAME: 张开
+ POSITION:
+ - 96
+ - 255
+ - 255
+ - 255
+ - 255
+ - 150
+ - 114
+ - 151
+ - 189
+ - 255
+ - 180
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+RIGHT_HAND:
+- ACTION_NAME: 张开
+ POSITION:
+ - 96
+ - 255
+ - 255
+ - 255
+ - 255
+ - 150
+ - 114
+ - 151
+ - 189
+ - 255
+ - 180
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
+- ACTION_NAME: 握拳
+ POSITION:
+ - 230
+ - 80
+ - 51
+ - 42
+ - 7
+ - 35
+ - 114
+ - 151
+ - 189
+ - 255
+ - 58
+ - 255
+ - 255
+ - 255
+ - 255
+ - 133
+ - 5
+ - 0
+ - 0
+ - 0
+ - 30
+ - 0
+ - 0
+ - 0
+ - 0
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L6_positions.yaml b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L6_positions.yaml
new file mode 100644
index 0000000..9caaf4a
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L6_positions.yaml
@@ -0,0 +1,26 @@
+LEFT_HAND:
+- ACTION_NAME: 握拳
+ POSITION:
+ - 67
+ - 151
+ - 0
+ - 0
+ - 0
+ - 0
+- ACTION_NAME: 张开
+ POSITION:
+ - 255
+ - 179
+ - 255
+ - 255
+ - 255
+ - 255
+RIGHT_HAND:
+- ACTION_NAME: 张开
+ POSITION:
+ - 255
+ - 70
+ - 255
+ - 255
+ - 255
+ - 255
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L7_positions.yaml b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L7_positions.yaml
new file mode 100644
index 0000000..d573967
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/L7_positions.yaml
@@ -0,0 +1,29 @@
+LEFT_HAND:
+- ACTION_NAME: 握拳
+ POSITION:
+ - 67
+ - 151
+ - 0
+ - 0
+ - 0
+ - 0
+ - 37
+- ACTION_NAME: 张开
+ POSITION:
+ - 255
+ - 179
+ - 255
+ - 255
+ - 255
+ - 255
+ - 83
+RIGHT_HAND:
+- ACTION_NAME: 张开
+ POSITION:
+ - 255
+ - 70
+ - 255
+ - 255
+ - 255
+ - 255
+ - 255
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/O6_positions.yaml b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/O6_positions.yaml
new file mode 100644
index 0000000..9caaf4a
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/O6_positions.yaml
@@ -0,0 +1,26 @@
+LEFT_HAND:
+- ACTION_NAME: 握拳
+ POSITION:
+ - 67
+ - 151
+ - 0
+ - 0
+ - 0
+ - 0
+- ACTION_NAME: 张开
+ POSITION:
+ - 255
+ - 179
+ - 255
+ - 255
+ - 255
+ - 255
+RIGHT_HAND:
+- ACTION_NAME: 张开
+ POSITION:
+ - 255
+ - 70
+ - 255
+ - 255
+ - 255
+ - 255
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/__init__.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml
new file mode 100644
index 0000000..f3a7511
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/config/setting.yaml
@@ -0,0 +1,58 @@
+VERSION: 3.1.1 # 支持O6、L6在RS485模式
+LINKER_HAND: # 手部配置信息
+ LEFT_HAND:
+ EXISTS: True # 是否存在左手
+ TOUCH: True # 是否有压力传感器
+ CAN: "can0" # 配置CAN端口 默认can0 如果MODUBS不为"None",则CAN配置失效。 如果是win系统则类似于 PCAN_USBBUS1。注：蓝色盒子为Linux下can0，WIN下位PCAN_USBBUS1。透明盒子Linux下为can0，WIN下为0
+ MODBUS: "None" # 通讯协议是否为485 默认None 如果启动485，则是设备端口 /dev/ttyUSB* CAN配置失效 当前只支持O6/L6.后续版本正在努力增加中
+ JOINT: L10 # 左手型号 O6/L6/L7/L10/L20/G20/L21/L25/
+ NAME: # 默认值，不用修改
+ - joint41
+ - joint42
+ - joint43
+ - joint44
+ - joint45
+ - joint46
+ - joint47
+ - joint48
+ - joint49
+ - joint50
+ - joint51
+ - joint52
+ - joint53
+ - joint54
+ - joint55
+ - joint56
+ - joint57
+ - joint58
+ - joint59
+ - joint60
+
+ RIGHT_HAND:
+ EXISTS: False # 是否存在右手
+ TOUCH: False # 是否有压力传感器
+ CAN: "can0" # 配置CAN端口 默认can0 如果MODUBS不为"None",则CAN配置失效。 如果是win系统则类似于 PCAN_USBBUS1。注：蓝色盒子为Linux下can0，WIN下位PCAN_USBBUS1。透明盒子Linux下为can0，WIN下为0
+ MODBUS: "None" # 通讯协议是否为485 默认None 如果启动485，则是设备端口 /dev/ttyUSB* CAN配置失效 当前只支持O6/L6.后续版本正在努力增加中
+ JOINT: L10 # 右手型号 O6/L6/L7/L10/L20/G20/L21/L25/
+ NAME: # 默认值，不用修改
+ - joint71
+ - joint72
+ - joint73
+ - joint77
+ - joint75
+ - joint76
+ - joint77
+ - joint78
+ - joint79
+ - joint80
+ - joint81
+ - joint82
+ - joint83
+ - joint84
+ - joint88
+ - joint86
+ - joint87
+ - joint88
+ - joint89
+ - joint90
+PASSWORD: "12345678" # 由于与can通讯，需要激活通讯接口用到系统管理员密码。只有Linux系统需要，windows系统不需要。RS485不需要修改，RS485需要给/dev/ttyUSB* 777权限
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/__init__.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/__init__.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_g20_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_g20_can.py
new file mode 100644
index 0000000..9553aed
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_g20_can.py
@@ -0,0 +1,1269 @@
+#!/usr/bin/env python3
+import can
+import time, sys, os
+import threading
+import numpy as np
+from enum import Enum
+from utils.open_can import OpenCan
+from can.exceptions import CanError
+from utils.color_msg import ColorMsg
+current_dir = os.path.dirname(os.path.abspath(__file__))
+target_dir = os.path.abspath(os.path.join(current_dir, ".."))
+sys.path.append(target_dir)
+"""
+拇指41: [拇指侧摆, 拇指横摆, 拇指根部, 预留, 预留, 拇指尖部]
+食指42: [食指侧摆, 预留, 食指根部, 预留, 预留, 食指末端]
+中指43: [中指侧摆, 预留, 中指根部, 预留, 预留, 中指末端]
+无名指44: [无名指侧摆, 预留, 无名指根部, 预留, 预留, 无名指末端]
+小指45: [小指侧摆, 预留, 小指根部, 预留, 预留, 小指末端]
+"""
+CMD_MAP = [
+ "拇指根部",
+ "食指根部",
+ "中指根部",
+ "无名指根部",
+ "小指根部",
+ "拇指侧摆",
+ "食指侧摆",
+ "中指侧摆",
+ "无名指侧摆",
+ "小指侧摆",
+ "拇指横摆",
+ "预留",
+ "预留",
+ "预留",
+ "预留",
+ "拇指尖部",
+ "食指末端",
+ "中指末端",
+ "无名指末端",
+ "小指末端"
+]
+
+class FrameProperty(Enum):
+ # 手指运动控制 - 并联型控制指令（控制所有手指同一关节）
+ ROLL_POS = 0x01 # 横滚关节位置
+ YAW_POS = 0x02 # 航向关节位置
+ ROOT1_POS = 0x03 # 指根1关节位置
+ ROOT2_POS = 0x04 # 指根2关节位置
+ ROOT3_POS = 0x05 # 指根3关节位置
+ TIP_POS = 0x06 # 指尖关节位置
+
+ # 关节速度指令
+ ROLL_SPEED = 0x09 # 横滚关节速度
+ YAW_SPEED = 0x0A # 航向关节速度
+ ROOT1_SPEED = 0x0B # 指根1关节速度
+ ROOT2_SPEED = 0x0C # 指根2关节速度
+ ROOT3_SPEED = 0x0D # 指根3关节速度
+ TIP_SPEED = 0x0E # 指尖关节速度
+
+ # 关节扭矩指令
+ ROLL_TORQUE = 0x11 # 横滚关节扭矩
+ YAW_TORQUE = 0x12 # 航向关节扭矩
+ ROOT1_TORQUE = 0x13 # 指根1关节扭矩
+ ROOT2_TORQUE = 0x14 # 指根2关节扭矩
+ ROOT3_TORQUE = 0x15 # 指根3关节扭矩
+ TIP_TORQUE = 0x16 # 指尖关节扭矩
+
+ # 关节故障码
+ ROLL_FAULT = 0x19 # 横滚关节故障码
+ YAW_FAULT = 0x1A # 航向关节故障码
+ ROOT1_FAULT = 0x1B # 指根1关节故障码
+ ROOT2_FAULT = 0x1C # 指根2关节故障码
+ ROOT3_FAULT = 0x1D # 指根3关节故障码
+ TIP_FAULT = 0x1E # 指尖关节故障码
+
+ # 关节温度
+ ROLL_TEMPERATURE = 0x21 # 横滚关节过温保护阈值
+ YAW_TEMPERATURE = 0x22 # 航向关节过温保护阈值
+ ROOT1_TEMPERATURE = 0x23 # 指根1关节过温保护阈值
+ ROOT2_TEMPERATURE = 0x24 # 指根2关节过温保护阈值
+ ROOT3_TEMPERATURE = 0x25 # 指根3关节过温保护阈值
+ TIP_TEMPERATURE = 0x26 # 指尖关节过温保护阈值
+
+ # 手指运动控制 - 串联型控制指令（控制同一手指所有关节）
+ THUMB_POS = 0x41 # 大拇指指关节位置
+ INDEX_POS = 0x42 # 食指关节位置
+ MIDDLE_POS = 0x43 # 中指关节位置
+ RING_POS = 0x44 # 无名指关节位置
+ LITTLE_POS = 0x45 # 小拇指关节位置
+
+ # 手指速度
+ THUMB_SPEED = 0x49 # 大拇指速度
+ INDEX_SPEED = 0x4A # 食指速度
+ MIDDLE_SPEED = 0x4B # 中指速度
+ RING_SPEED = 0x4C # 无名指速度
+ LITTLE_SPEED = 0x4D # 小拇指速度
+
+ # 手指扭矩
+ THUMB_TORQUE = 0x51 # 大拇指扭矩
+ INDEX_TORQUE = 0x52 # 食指扭矩
+ MIDDLE_TORQUE = 0x53 # 中指扭矩
+ RING_TORQUE = 0x54 # 无名指扭矩
+ LITTLE_TORQUE = 0x55 # 小拇指扭矩
+
+ # 手指故障码
+ THUMB_FAULT = 0x59 # 大拇指故障码
+ INDEX_FAULT = 0x5A # 食指故障码
+ MIDDLE_FAULT = 0x5B # 中指故障码
+ RING_FAULT = 0x5C # 无名指故障码
+ LITTLE_FAULT = 0x5D # 小拇指故障码
+
+ # 手指温度
+ THUMB_TEMPERATURE = 0x61 # 大拇指过温保护阈值
+ INDEX_TEMPERATURE = 0x62 # 食指过温保护阈值
+ MIDDLE_TEMPERATURE = 0x63 # 中指过温保护阈值
+ RING_TEMPERATURE = 0x64 # 无名指过温保护阈值
+ LITTLE_TEMPERATURE = 0x65 # 小拇指过温保护阈值
+
+ # 手指运动控制 - 合并指令区域
+ FINGER_SPEED = 0x81 # 设置手指速度
+ FINGER_TORQUE = 0x82 # 设置手指输出扭矩
+ FINGER_FAULT = 0x83 # 清除手指故障及故障码
+ FINGER_TEMPERATURE = 0x84 # 手指各关节温度
+
+ # 指尖传感器数据
+ HAND_NORMAL_FORCE = 0x90 # 五指法向压力
+ HAND_TANGENTIAL_FORCE = 0x91 # 五指切向压力
+ HAND_TANGENTIAL_FORCE_DIR = 0x92 # 五指切向方向
+ HAND_APPROACH_INC = 0x93 # 五指接近感应
+
+ # 手指所有数据
+ THUMB_ALL_DATA = 0x98 # 大拇指所有数据
+ INDEX_ALL_DATA = 0x99 # 食指所有数据
+ MIDDLE_ALL_DATA = 0x9A # 中指所有数据
+ RING_ALL_DATA = 0x9B # 无名指所有数据
+ LITTLE_ALL_DATA = 0x9C # 小拇指所有数据
+
+ # 触觉传感器
+ TOUCH_SENSOR_TYPE = 0xB0 # 触觉传感器类型
+ THUMB_TOUCH = 0xB1 # 大拇指触觉传感
+ INDEX_TOUCH = 0xB2 # 食指触觉传感
+ MIDDLE_TOUCH = 0xB3 # 中指触觉传感
+ RING_TOUCH = 0xB4 # 无名指触觉传感
+ LITTLE_TOUCH = 0xB5 # 小拇指触觉传感
+ PALM_TOUCH = 0xB6 # 手掌指触觉传感
+
+ # 查询指令
+ HAND_UID_GET = 0xC0 # 唯一标识码查询
+ HAND_HARDWARE_VERSION_GET = 0xC1 # 硬件版本查询
+ HAND_SOFTWARE_VERSION_GET = 0xC2 # 软件版本查询
+ HAND_COMM_ID_GET = 0xC3 # 设备id查询
+ HAND_STRUCT_VERSION_GET = 0xC4 # 结构版本号查询
+
+ # 出厂指令
+ HOST_CMD_HAND_ERASE_POS_CALI = 0xCD # 擦除位置校准值
+ HAND_COMM_ID_SET = 0xD1 # 通信ID设置
+ HAND_UID_SET = 0xF0 # 唯一标识码设置
+
+class LinkerHandG20Can:
+ def __init__(self, can_channel='can0', baudrate=1000000, can_id=0x28, yaml=""):
+ self.can_id = can_id
+ self.can_channel = can_channel
+ self.baudrate = baudrate
+ self.open_can = OpenCan(load_yaml=yaml)
+
+ self.running = True
+
+ # 初始化数据存储变量
+ self.last_thumb_pos, self.last_index_pos, self.last_ring_pos, self.last_middle_pos, self.last_little_pos = None, None, None, None, None
+ self.last_root1, self.last_yaw, self.last_roll, self.last_root2, self.last_tip = None, None, None, None, None
+
+ # 并联控制数据存储
+ self.x01, self.x02, self.x03, self.x04, self.x05, self.x06 = [], [], [], [], [], []
+ self.x09, self.x0A, self.x0B, self.x0C, self.x0D, self.x0E = [], [], [], [], [], []
+ self.x11, self.x12, self.x13, self.x14, self.x15, self.x16 = [], [], [], [], [], []
+ self.x19, self.x1A, self.x1B, self.x1C, self.x1D, self.x1E = [], [], [], [], [], []
+ self.x21, self.x22, self.x23, self.x24, self.x25, self.x26 = [], [], [], [], [], []
+
+ # 串联控制数据存储
+ self.x41, self.x42, self.x43, self.x44, self.x45 = [], [], [], [], []
+ self.x49, self.x4A, self.x4B, self.x4C, self.x4D = [0] * 6, [0] * 6, [0] * 6, [0] * 6, [0] * 6
+ self.x51, self.x52, self.x53, self.x54, self.x55 = [], [], [], [], []
+ self.x59, self.x5A, self.x5B, self.x5C, self.x5D = [], [], [], [], []
+ self.x61, self.x62, self.x63, self.x64, self.x65 = [], [], [], [], []
+
+ # 合并指令区域数据存储
+ self.x81, self.x82, self.x83, self.x84 = [], [], [], []
+
+ # 传感器数据存储
+ self.x90, self.x91, self.x92, self.x93 = [], [], [], []
+ self.x98, self.x99, self.x9A, self.x9B, self.x9C = [], [], [], [], []
+ self.xB0, self.xB1, self.xB2, self.xB3, self.xB4, self.xB5, self.xB6 = [], [], [], [], [], [], []
+ self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 5 for _ in range(4)]
+
+ # 查询指令数据存储
+ self.xC0, self.xC1, self.xC2, self.xC3, self.xC4 = [], [], [], [], []
+ self.serial_number = []
+ self.serial_number_map = {
+ 0: 0,
+ 1: 1,
+ 2: 2,
+ 3: 3,
+ }
+ # 触觉传感器矩阵数据
+ self.thumb_matrix = np.full((12, 6), -1)
+ self.index_matrix = np.full((12, 6), -1)
+ self.middle_matrix = np.full((12, 6), -1)
+ self.ring_matrix = np.full((12, 6), -1)
+ self.little_matrix = np.full((12, 6), -1)
+ self.matrix_map = {
+ 0: 0, 16: 1, 32: 2, 48: 3, 64: 4, 80: 5,
+ 96: 6, 112: 7, 128: 8, 144: 9, 160: 10, 176: 11,
+ }
+ # 全掌触觉数据缓存
+ self.thumb_matrix_palm = np.full((23, 9), -1)
+ self.thumb_matrix_palm_tmp = []
+ self.thumb_matrix_palm_mass = [-1, -1, -1]
+
+ self.index_matrix_palm = np.full((23, 9), -1)
+ self.index_matrix_palm_tmp = []
+ self.index_matrix_palm_mass = [-1, -1, -1]
+
+ self.middle_matrix_palm = np.full((23, 9), -1)
+ self.middle_matrix_palm_tmp = []
+ self.middle_matrix_palm_mass = [-1, -1, -1]
+
+ self.ring_matrix_palm = np.full((23, 9), -1)
+ self.ring_matrix_palm_tmp = []
+ self.ring_matrix_palm_mass = [-1, -1, -1]
+
+ self.little_matrix_palm = np.full((23, 9), -1)
+ self.little_matrix_palm_tmp = []
+ self.little_matrix_palm_mass = [-1, -1, -1]
+
+ self.palm_matrix_palm = np.full((28, 20), -1)
+ self.palm_matrix_palm_tmp = []
+ self.palm_matrix_palm_mass = [-1, -1]
+
+ self.bus = self.init_can_bus(channel=self.can_channel, baudrate=baudrate)
+ # 启动接收线程
+ self.receive_thread = threading.Thread(target=self.receive_response)
+ self.receive_thread.daemon = True
+ self.receive_thread.start()
+ self._check_touch_type()
+
+ self.xB0 = self.get_touch_sensor_type() # 获取触觉传感器类型,如果返回值为5：TSSP_JZG(全手掌，指尖11x9,指中6x9,指根6x9,数据以23行9列形式返回，五指各有3个合力值。手掌20x28，手掌有两个合力值，分为上掌上半部分和下半部分。)
+
+ def _check_touch_type(self):
+ '''根据SN编码判断压感类型'''
+ self.sn = self.get_serial_number()
+ time.sleep(0.1)
+ if self.sn != "-1":
+ parts = self.sn.split("-")
+ if parts[4] == "A":
+ self.touch_type = 1
+ elif parts[4] == "B":
+ self.touch_type = 2
+ self.touch_code = 0xC6 # 6*12
+ elif parts[4] == "J":
+ self.touch_type = 3
+ elif parts[4] == "F":
+ self.touch_type = 4
+ self.touch_code = 0xA4 # 4*10
+ elif parts[4] == "Z":
+ self.touch_type = -1
+ else:
+ # 如果没有SN编码则根据返回数据进行判断
+ self.touch_type = self.get_touch_type()
+
+ def init_can_bus(self, channel, baudrate):
+ """
+ 尝试按优先级连接 CAN 总线，并实现回退机制。
+ """
+ # --- 统一异常处理块开始 ---
+ try:
+ if sys.platform == "linux":
+ # Linux 优先级：1. socketcan
+ try:
+ self.open_can.open_can(self.can_channel)
+ # 尝试 socketcan
+ bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+ ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+ raise # 重新抛出异常，让外层 try 捕获
+ elif sys.platform == "win32":
+ # Windows 优先级：1. pcan
+ try:
+ bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+ # Windows 优先级：2. candle (回退方法)
+ try:
+ bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+ raise # 两个接口都失败，抛出异常
+ else:
+ raise EnvironmentError("Unsupported platform for CAN interface")
+ # --- 统一异常处理块结束 ---
+ except Exception as e:
+ # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+ ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+ # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+ raise
+
+ def send_command(self, frame_property, data_list, sleep_time=0.003):
+ """
+ 发送指令到CAN总线
+ :param frame_property: 数据帧属性
+ :param data_list: 数据载荷
+ """
+ frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ data = [frame_property_value] + [int(val) for val in data_list]
+ msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ try:
+ self.bus.send(msg)
+ except can.CanError as e:
+ print(f"Failed to send message: {e}")
+ self.open_can.open_can(self.can_channel)
+ time.sleep(1)
+ self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+ time.sleep(1)
+ if self.is_can:
+ self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+ else:
+ print("Reconnecting CAN devices ....")
+ time.sleep(sleep_time)
+
+ def receive_response(self):
+ """
+ 接收并处理CAN总线响应消息
+ """
+ while self.running:
+ try:
+ msg = self.bus.recv(timeout=1.0)
+ if msg:
+ self.process_response(msg)
+ except can.CanError as e:
+ print(f"Error receiving message: {e}")
+
+ def process_response(self, msg):
+ """
+ 处理CAN响应消息
+ """
+ if msg.arbitration_id == self.can_id:
+ frame_type = msg.data[0]
+ response_data = msg.data[1:]
+ if len(list(response_data)) == 0:
+ return
+ # 并联控制指令响应
+ if frame_type == 0x01: self.x01 = list(response_data)
+ elif frame_type == 0x02: self.x02 = list(response_data)
+ elif frame_type == 0x03: self.x03 = list(response_data)
+ elif frame_type == 0x04: self.x04 = list(response_data)
+ elif frame_type == 0x05: self.x05 = list(response_data)
+ elif frame_type == 0x06: self.x06 = list(response_data)
+ elif frame_type == 0x09: self.x09 = list(response_data)
+ elif frame_type == 0x0A: self.x0A = list(response_data)
+ elif frame_type == 0x0B: self.x0B = list(response_data)
+ elif frame_type == 0x0C: self.x0C = list(response_data)
+ elif frame_type == 0x0D: self.x0D = list(response_data)
+ elif frame_type == 0x0E: self.x0E = list(response_data)
+ elif frame_type == 0x11: self.x11 = list(response_data)
+ elif frame_type == 0x12: self.x12 = list(response_data)
+ elif frame_type == 0x13: self.x13 = list(response_data)
+ elif frame_type == 0x14: self.x14 = list(response_data)
+ elif frame_type == 0x15: self.x15 = list(response_data)
+ elif frame_type == 0x16: self.x16 = list(response_data)
+ elif frame_type == 0x19: self.x19 = list(response_data)
+ elif frame_type == 0x1A: self.x1A = list(response_data)
+ elif frame_type == 0x1B: self.x1B = list(response_data)
+ elif frame_type == 0x1C: self.x1C = list(response_data)
+ elif frame_type == 0x1D: self.x1D = list(response_data)
+ elif frame_type == 0x1E: self.x1E = list(response_data)
+ elif frame_type == 0x21: self.x21 = list(response_data)
+ elif frame_type == 0x22: self.x22 = list(response_data)
+ elif frame_type == 0x23: self.x23 = list(response_data)
+ elif frame_type == 0x24: self.x24 = list(response_data)
+ elif frame_type == 0x25: self.x25 = list(response_data)
+ elif frame_type == 0x26: self.x26 = list(response_data)
+
+ # 串联控制指令响应
+ elif frame_type == 0x41: self.x41 = list(response_data)
+ elif frame_type == 0x42: self.x42 = list(response_data)
+ elif frame_type == 0x43: self.x43 = list(response_data)
+ elif frame_type == 0x44: self.x44 = list(response_data)
+ elif frame_type == 0x45: self.x45 = list(response_data)
+ elif frame_type == 0x49: self.x49 = list(response_data)
+ elif frame_type == 0x4A: self.x4A = list(response_data)
+ elif frame_type == 0x4B: self.x4B = list(response_data)
+ elif frame_type == 0x4C: self.x4C = list(response_data)
+ elif frame_type == 0x4D: self.x4D = list(response_data)
+ elif frame_type == 0x51: self.x51 = list(response_data)
+ elif frame_type == 0x52: self.x52 = list(response_data)
+ elif frame_type == 0x53: self.x53 = list(response_data)
+ elif frame_type == 0x54: self.x54 = list(response_data)
+ elif frame_type == 0x55: self.x55 = list(response_data)
+ elif frame_type == 0x59: self.x59 = list(response_data)
+ elif frame_type == 0x5A: self.x5A = list(response_data)
+ elif frame_type == 0x5B: self.x5B = list(response_data)
+ elif frame_type == 0x5C: self.x5C = list(response_data)
+ elif frame_type == 0x5D: self.x5D = list(response_data)
+ elif frame_type == 0x61: self.x61 = list(response_data)
+ elif frame_type == 0x62: self.x62 = list(response_data)
+ elif frame_type == 0x63: self.x63 = list(response_data)
+ elif frame_type == 0x64: self.x64 = list(response_data)
+ elif frame_type == 0x65: self.x65 = list(response_data)
+
+ # 合并指令区域响应
+ elif frame_type == 0x81: self.x81 = list(response_data)
+ elif frame_type == 0x82: self.x82 = list(response_data)
+ elif frame_type == 0x83: self.x83 = list(response_data)
+ elif frame_type == 0x84: self.x84 = list(response_data)
+
+ # 传感器数据响应
+ elif frame_type == 0x90: self.x90 = list(response_data)
+ elif frame_type == 0x91: self.x91 = list(response_data)
+ elif frame_type == 0x92: self.x92 = list(response_data)
+ elif frame_type == 0x93: self.x93 = list(response_data)
+ elif frame_type == 0x98: self.x98 = list(response_data)
+ elif frame_type == 0x99: self.x99 = list(response_data)
+ elif frame_type == 0x9A: self.x9A = list(response_data)
+ elif frame_type == 0x9B: self.x9B = list(response_data)
+ elif frame_type == 0x9C: self.x9C = list(response_data)
+
+ # 触觉传感器响应
+ elif frame_type == 0xB0: self.xB0 = list(response_data)
+ elif frame_type == 0xB1:
+ d = list(response_data)
+ if self.xB0[0] == 5:
+ """全掌矩阵"""
+ self.thumb_matrix_palm_tmp.append(d) # 将返回帧存储到缓存
+ if len(d) == 4 and d[0] == 22 and d[1] == 7: # 如果是最后一帧
+ self.thumb_matrix_palm = self.build_matrix(self.thumb_matrix_palm_tmp) # 将帧数据排列成23行9列矩阵
+ self.thumb_matrix_palm_tmp=[] # 重置缓存数据
+ if len(d) == 7 and d[0] == 255:
+ self.thumb_matrix_palm_mass = self.build_matrix_mass(d) # [指尖合力值, 指中合力值, 指根合力值]
+ else:
+ if len(d) == 2:
+ self.xB1 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.thumb_matrix[index] = d[1:]
+
+ elif frame_type == 0xB2:
+ d = list(response_data)
+ if self.xB0[0] == 5:
+ """全掌矩阵"""
+ self.index_matrix_palm_tmp.append(d) # 将返回帧存储到缓存
+ if len(d) == 4 and d[0] == 22 and d[1] == 7: # 如果是最后一帧
+ self.index_matrix_palm = self.build_matrix(self.index_matrix_palm_tmp) # 将帧数据排列成23行9列矩阵
+ self.index_matrix_palm_tmp=[] # 重置缓存数据
+ if len(d) == 7 and d[0] == 255:
+ self.index_matrix_palm_mass = self.build_matrix_mass(d) # [指尖合力值, 指中合力值, 指根合力值]
+ else:
+ if len(d) == 2:
+ self.xB2 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.index_matrix[index] = d[1:]
+ elif frame_type == 0xB3:
+ d = list(response_data)
+ if self.xB0[0] == 5:
+ """全掌矩阵"""
+ self.middle_matrix_palm_tmp.append(d) # 将返回帧存储到缓存
+ if len(d) == 4 and d[0] == 22 and d[1] == 7: # 如果是最后一帧
+ self.middle_matrix_palm = self.build_matrix(self.middle_matrix_palm_tmp) # 将帧数据排列成23行9列矩阵
+ self.middle_matrix_palm_tmp=[] # 重置缓存数据
+ if len(d) == 7 and d[0] == 255:
+ self.middle_matrix_palm_mass = self.build_matrix_mass(d) # [指尖合力值, 指中合力值, 指根合力值]
+ else:
+ if len(d) == 2:
+ self.xB3 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.middle_matrix[index] = d[1:]
+ elif frame_type == 0xB4:
+ d = list(response_data)
+ if self.xB0[0] == 5:
+ """全掌矩阵"""
+ self.ring_matrix_palm_tmp.append(d) # 将返回帧存储到缓存
+ if len(d) == 4 and d[0] == 22 and d[1] == 7: # 如果是最后一帧
+ self.ring_matrix_palm = self.build_matrix(self.ring_matrix_palm_tmp) # 将帧数据排列成23行9列矩阵
+ self.ring_matrix_palm_tmp=[] # 重置缓存数据
+ if len(d) == 7 and d[0] == 255:
+ self.ring_matrix_palm_mass = self.build_matrix_mass(d) # [指尖合力值, 指中合力值, 指根合力值]
+ else:
+ if len(d) == 2:
+ self.xB4 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.ring_matrix[index] = d[1:]
+ elif frame_type == 0xB5:
+ d = list(response_data)
+ if self.xB0[0] == 5:
+ """全掌矩阵"""
+ self.little_matrix_palm_tmp.append(d) # 将返回帧存储到缓存
+ if len(d) == 4 and d[0] == 22 and d[1] == 7: # 如果是最后一帧
+ self.little_matrix_palm = self.build_matrix(self.little_matrix_palm_tmp) # 将帧数据排列成23行9列矩阵
+ self.little_matrix_palm_tmp=[] # 重置缓存数据
+ if len(d) == 7 and d[0] == 255:
+ self.little_matrix_palm_mass = self.build_matrix_mass(d) # [指尖合力值, 指中合力值, 指根合力值]
+ else:
+ if len(d) == 2:
+ self.xB5 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.little_matrix[index] = d[1:]
+ elif frame_type == 0xB6:
+ d = list(response_data)
+ if self.xB0[0] == 5:
+ """全掌矩阵"""
+ self.palm_matrix_palm_tmp.append(d) # 将返回帧存储到缓存
+ if len(d) == 7 and d[0] == 27 and d[1] == 5: # 如果是最后一帧
+ self.palm_matrix_palm = self.build_matrix(self.palm_matrix_palm_tmp) # 将帧数据排列成23行9列矩阵
+ self.palm_matrix_palm_tmp=[] # 重置缓存数据
+ if len(d) == 5 and d[0] == 255:
+ self.palm_matrix_palm_mass = self.build_matrix_mass(d)
+ else:
+ self.xB6 = d
+
+
+ # 查询指令响应
+ # elif frame_type == 0xC0: self.xC0 = list(response_data)
+ elif frame_type == 0xC0:
+ d = list(response_data)
+ index = self.serial_number_map.get(d[0])
+ if index is not None:
+ self.serial_number=self.serial_number + d[1:]
+ else:
+ self.serial_number=self.serial_number + [-1] * 6
+ elif frame_type == 0xC1: self.xC1 = list(response_data)
+ elif frame_type == 0xC2: self.xC2 = list(response_data)
+ elif frame_type == 0xC3: self.xC3 = list(response_data)
+ elif frame_type == 0xC4: self.xC4 = list(response_data)
+
+
+
+ def build_matrix(self, data, r=23, c=9):
+ rows, cols = r, c
+ matrix = np.full((rows, cols), -1)
+
+ for item in data:
+ if len(item) == 4:
+ # 最后一行：从列坐标开始放
+ row, col, v1, v2 = item
+ start_col = col # 改为 col，而不是 col+1
+ values = [v1, v2]
+
+ cur_row, cur_col = row, start_col
+ for val in values:
+ if cur_col >= cols:
+ cur_row += 1
+ cur_col = 0
+ if cur_row < rows:
+ matrix[cur_row][cur_col] = val
+ cur_col += 1
+ break
+
+ elif len(item) == 7:
+ # 普通行：从列坐标开始放
+ row, col, v1, v2, v3, v4, v5 = item
+ start_col = col # 改为 col，而不是 col+1
+ values = [v1, v2, v3, v4, v5]
+
+ cur_row, cur_col = row, start_col
+ for val in values:
+ if cur_col >= cols:
+ cur_row += 1
+ cur_col = 0
+ if cur_row < rows:
+ matrix[cur_row][cur_col] = val
+ cur_col += 1
+
+ return matrix
+
+ def build_matrix_mass(self, hex_data):
+ """
+ 处理返回的和力值的帧数据,手指返回合力值长度为3，[指尖，指中，指根]
+ 手掌返回为长度为2。[上半部,下半部]
+ 解析 CAN 数据（支持 5 字节或 7 字节）
+
+ 参数:
+ hex_data: 十六进制列表，如 [0xFF, 0x27, 0x02, 0x51, 0x05, 0x20, 0x04] (7字节)
+ 或 [0xFF, 0x92, 0x09, 0xF8, 0x00] (5字节)
+
+ 返回:
+ (id, values) 其中 id 是 int，values 是包含 int 的列表（2个或3个）
+ """
+ if len(hex_data) not in [5, 7]:
+ raise ValueError(f"数据长度不支持，需要 5 或 7 字节，实际: {len(hex_data)}")
+
+ # 获取 ID（第一个字节）
+ can_id = hex_data[0]
+
+ # 计算有多少组数据（每组2字节）
+ data_bytes = hex_data[1:] # 去掉 ID
+ num_values = len(data_bytes) // 2
+
+ # 解析数据组（小端序）
+ values = []
+ for i in range(num_values):
+ low_byte = data_bytes[i*2] # 低位字节
+ high_byte = data_bytes[i*2 + 1] # 高位字节
+ # 小端拼接：低位 + 高位<<8
+ value = low_byte | (high_byte << 8)
+ values.append(value)
+
+ return values
+
+
+ # 并联控制指令方法
+ def set_roll_positions(self, joint_ranges):
+ """设置所有手指横滚关节位置"""
+ self.send_command(FrameProperty.ROLL_POS, joint_ranges)
+
+ def set_yaw_positions(self, joint_ranges):
+ """设置所有手指航向关节位置"""
+ self.send_command(FrameProperty.YAW_POS, joint_ranges)
+
+ def set_root1_positions(self, joint_ranges):
+ """设置所有手指指根1关节位置"""
+ self.send_command(FrameProperty.ROOT1_POS, joint_ranges)
+
+ def set_root2_positions(self, joint_ranges):
+ """设置所有手指指根2关节位置"""
+ self.send_command(FrameProperty.ROOT2_POS, joint_ranges)
+
+ def set_root3_positions(self, joint_ranges):
+ """设置所有手指指根3关节位置"""
+ self.send_command(FrameProperty.ROOT3_POS, joint_ranges)
+
+ def set_tip_positions(self, joint_ranges=[80]*5):
+ """设置所有手指指尖关节位置"""
+ self.send_command(FrameProperty.TIP_POS, joint_ranges)
+
+ # 串联控制指令方法
+ def set_thumb_positions(self, joint_ranges):
+ """设置大拇指所有关节位置"""
+ self.send_command(FrameProperty.THUMB_POS, joint_ranges)
+
+ def set_index_positions(self, joint_ranges):
+ """设置食指所有关节位置"""
+ self.send_command(FrameProperty.INDEX_POS, joint_ranges)
+
+ def set_middle_positions(self, joint_ranges):
+ """设置中指所有关节位置"""
+ self.send_command(FrameProperty.MIDDLE_POS, joint_ranges)
+
+ def set_ring_positions(self, joint_ranges):
+ """设置无名指所有关节位置"""
+ self.send_command(FrameProperty.RING_POS, joint_ranges)
+
+ def set_little_positions(self, joint_ranges):
+ """设置小拇指所有关节位置"""
+ self.send_command(FrameProperty.LITTLE_POS, joint_ranges)
+
+ # 扭矩设置方法
+ def set_thumb_torque(self, torque_values):
+ """设置大拇指扭矩"""
+ self.send_command(FrameProperty.THUMB_TORQUE, torque_values)
+
+ def set_index_torque(self, torque_values):
+ """设置食指扭矩"""
+ self.send_command(FrameProperty.INDEX_TORQUE, torque_values)
+
+ def set_middle_torque(self, torque_values):
+ """设置中指扭矩"""
+ self.send_command(FrameProperty.MIDDLE_TORQUE, torque_values)
+
+ def set_ring_torque(self, torque_values):
+ """设置无名指扭矩"""
+ self.send_command(FrameProperty.RING_TORQUE, torque_values)
+
+ def set_little_torque(self, torque_values):
+ """设置小拇指扭矩"""
+ self.send_command(FrameProperty.LITTLE_TORQUE, torque_values)
+
+ # 速度设置方法
+ def set_thumb_speed(self, speed_values):
+ """设置大拇指速度"""
+ self.send_command(FrameProperty.THUMB_SPEED, speed_values)
+
+ def set_index_speed(self, speed_values):
+ """设置食指速度"""
+ self.send_command(FrameProperty.INDEX_SPEED, speed_values)
+
+ def set_middle_speed(self, speed_values):
+ """设置中指速度"""
+ self.send_command(FrameProperty.MIDDLE_SPEED, speed_values)
+
+ def set_ring_speed(self, speed_values):
+ """设置无名指速度"""
+ self.send_command(FrameProperty.RING_SPEED, speed_values)
+
+ def set_little_speed(self, speed_values):
+ """设置小拇指速度"""
+ self.send_command(FrameProperty.LITTLE_SPEED, speed_values)
+
+ # 查询方法
+ def get_thumb_positions(self):
+ """获取大拇指所有关节当前位置"""
+ self.send_command(FrameProperty.THUMB_POS, [])
+ return self.x41
+
+ def get_index_positions(self):
+ """获取食指所有关节当前位置"""
+ self.send_command(FrameProperty.INDEX_POS, [])
+ return self.x42
+
+ def get_middle_positions(self):
+ """获取中指所有关节当前位置"""
+ self.send_command(FrameProperty.MIDDLE_POS, [])
+ return self.x43
+
+ def get_ring_positions(self):
+ """获取无名指所有关节当前位置"""
+ self.send_command(FrameProperty.RING_POS, [])
+ return self.x44
+
+ def get_little_positions(self):
+ """获取小拇指所有关节当前位置"""
+ self.send_command(FrameProperty.LITTLE_POS, [])
+ return self.x45
+
+
+ def get_thumb_speed(self):
+ """获取大拇指速度"""
+ self.send_command(FrameProperty.THUMB_SPEED, [])
+
+ def get_index_speed(self):
+ """获取食指速度"""
+ self.send_command(FrameProperty.INDEX_SPEED, [])
+
+ def get_middle_speed(self):
+ """获取中指速度"""
+ self.send_command(FrameProperty.MIDDLE_SPEED, [])
+
+ def get_ring_speed(self):
+ """获取无名指速度"""
+ self.send_command(FrameProperty.RING_SPEED, [])
+
+ def get_little_speed(self):
+ """获取小拇指速度"""
+ self.send_command(FrameProperty.LITTLE_SPEED, [])
+
+ def get_thumb_torque(self):
+ """获取大拇指扭矩"""
+ self.send_command(FrameProperty.THUMB_TORQUE, [])
+
+ def get_index_torque(self):
+ """获取食指扭矩"""
+ self.send_command(FrameProperty.INDEX_TORQUE, [])
+
+ def get_middle_torque(self):
+ """获取中指扭矩"""
+ self.send_command(FrameProperty.MIDDLE_TORQUE, [])
+
+ def get_ring_torque(self):
+ """获取无名指扭矩"""
+ self.send_command(FrameProperty.RING_TORQUE, [])
+
+ def get_little_torque(self):
+ """获取小拇指扭矩"""
+ self.send_command(FrameProperty.LITTLE_TORQUE, [])
+
+ def get_thumb_fault(self):
+ """获取大拇指所有关节故障码"""
+ self.send_command(FrameProperty.THUMB_FAULT, [])
+ return self.x59
+
+ def get_index_fault(self):
+ """获取食指所有关节故障码"""
+ self.send_command(FrameProperty.INDEX_FAULT, [])
+ return self.x5A
+
+ def get_middle_fault(self):
+ """获取中指所有关节故障码"""
+ self.send_command(FrameProperty.MIDDLE_FAULT, [])
+ return self.x5B
+
+ def get_ring_fault(self):
+ """获取无名指所有关节故障码"""
+ self.send_command(FrameProperty.RING_FAULT, [])
+ return self.x5C
+
+ def get_little_fault(self):
+ """获取小拇指所有关节故障码"""
+ self.send_command(FrameProperty.LITTLE_FAULT, [])
+ return self.x5D
+
+ def get_thumb_temperature(self):
+ """获取大拇指所有关节当前温度"""
+ self.send_command(FrameProperty.THUMB_TEMPERATURE, [])
+ return self.x61
+
+ def get_index_temperature(self):
+ """获取食指所有关节当前温度"""
+ self.send_command(FrameProperty.INDEX_TEMPERATURE, [])
+ return self.x62
+
+ def get_middle_temperature(self):
+ """获取中指所有关节当前温度"""
+ self.send_command(FrameProperty.MIDDLE_TEMPERATURE, [])
+ return self.x63
+
+ def get_ring_temperature(self):
+ """获取无名指所有关节当前温度"""
+ self.send_command(FrameProperty.RING_TEMPERATURE, [])
+ return self.x64
+
+ def get_little_temperature(self):
+ """获取小拇指所有关节当前温度"""
+ self.send_command(FrameProperty.LITTLE_TEMPERATURE, [])
+ return self.x65
+
+ # 合并指令区域方法
+ def set_finger_speed(self, speed_values):
+ """设置手指速度"""
+ self.send_command(FrameProperty.FINGER_SPEED, speed_values)
+
+ def set_finger_torque(self, torque_values):
+ """设置手指输出扭矩"""
+ self.send_command(FrameProperty.FINGER_TORQUE, torque_values)
+
+ def clear_finger_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+ """清除手指故障及故障码"""
+ self.send_command(FrameProperty.FINGER_FAULT, finger_mask)
+ return self.x83
+
+ def get_finger_temperature(self):
+ """获取手指各关节温度"""
+ self.send_command(FrameProperty.FINGER_TEMPERATURE, [])
+ return self.x84
+
+ # 传感器数据获取方法
+ def get_normal_force(self):
+ """获取五指法向力"""
+ self.send_command(FrameProperty.HAND_NORMAL_FORCE, [])
+ return self.x90
+
+ def get_tangential_force(self):
+ """获取五指切向力"""
+ self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE, [])
+ return self.x91
+
+ def get_tangential_force_dir(self):
+ """获取五指切向力方向"""
+ self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE_DIR, [])
+ return self.x92
+
+ def get_approach_inc(self):
+ """获取五指接近感应"""
+ self.send_command(FrameProperty.HAND_APPROACH_INC, [])
+ return self.x93
+
+ def get_force(self):
+ '''Get pressure sensor data'''
+ return [self.x90,self.x91,self.x92,self.x93]
+
+ # 触觉传感器方法
+ def get_touch_sensor_type(self):
+ """获取触觉传感器类型 暂仅支持G20"""
+ self.send_command(FrameProperty.TOUCH_SENSOR_TYPE, [])
+ return self.xB0[0]
+
+ def get_thumb_touch(self):
+ """获取大拇指触觉传感数据"""
+ if self.xB0[0] == 5:
+ d = [23, 9, 1]
+ sleep_time = 0.015
+ else:
+ d = [0xC6]
+ sleep_time = 0.007
+ self.send_command(FrameProperty.THUMB_TOUCH, d, sleep_time=sleep_time)
+ #return self.thumb_matrix
+
+ def get_index_touch(self):
+ """获取食指触觉传感数据"""
+ if self.xB0[0] == 5:
+ d = [23, 9, 1]
+ sleep_time = 0.015
+ else:
+ d = [0xC6]
+ sleep_time = 0.007
+ self.send_command(FrameProperty.INDEX_TOUCH, d, sleep_time=sleep_time)
+ #return self.xB2
+
+ def get_middle_touch(self):
+ """获取中指触觉传感数据"""
+ if self.xB0[0] == 5:
+ d = [23, 9, 1]
+ sleep_time = 0.015
+ else:
+ d = [0xC6]
+ sleep_time = 0.007
+ self.send_command(FrameProperty.MIDDLE_TOUCH, d, sleep_time=sleep_time)
+ #33333return self.xB3
+
+ def get_ring_touch(self):
+ """获取无名指触觉传感数据"""
+ if self.xB0[0] == 5:
+ d = [23, 9, 1]
+ sleep_time = 0.015
+ else:
+ d = [0xC6]
+ sleep_time = 0.007
+ self.send_command(FrameProperty.RING_TOUCH, d, sleep_time=sleep_time)
+ #return self.xB4
+
+ def get_little_touch(self):
+ """获取小拇指触觉传感数据"""
+ if self.xB0[0] == 5:
+ d = [23, 9, 1]
+ sleep_time = 0.015
+ else:
+ d = [0xC6]
+ sleep_time = 0.007
+ self.send_command(FrameProperty.LITTLE_TOUCH, d, sleep_time=sleep_time)
+ #return self.xB5
+
+ def get_palm_touch(self):
+ """获取手掌触觉传感数据"""
+ if self.xB0[0] == 5:
+ d = [28, 20, 1]
+ sleep_time = 0.035
+ else:
+ d = [0xC6]
+ sleep_time = 0.007
+ self.send_command(FrameProperty.PALM_TOUCH, d, sleep_time=sleep_time)
+ #return self.xB6
+
+ # 查询指令方法
+ def get_uid(self):
+ """获取设备唯一标识码"""
+ self.send_command(FrameProperty.HAND_UID_GET, [])
+ return self.xC0
+
+ def get_hardware_version(self):
+ """获取硬件版本"""
+ self.send_command(FrameProperty.HAND_HARDWARE_VERSION_GET, [])
+ return self.xC1
+
+ def get_software_version(self):
+ """获取软件版本"""
+ self.send_command(FrameProperty.HAND_SOFTWARE_VERSION_GET, [])
+ return self.xC2
+
+ def get_comm_id(self):
+ """获取设备通信ID"""
+ self.send_command(FrameProperty.HAND_COMM_ID_GET, [])
+ return self.xC3
+
+ def get_struct_version(self):
+ """获取结构版本号"""
+ self.send_command(FrameProperty.HAND_STRUCT_VERSION_GET, [])
+ return self.xC4
+
+ # 出厂指令方法
+ def erase_position_calibration(self):
+ """擦除位置校准值"""
+ self.send_command(FrameProperty.HOST_CMD_HAND_ERASE_POS_CALI, [])
+
+ def set_comm_id(self, new_id):
+ """设置通信ID"""
+ self.send_command(FrameProperty.HAND_COMM_ID_SET, [new_id])
+
+ def set_uid(self, uid_data):
+ """设置唯一标识码（内部出厂使用）"""
+ self.send_command(FrameProperty.HAND_UID_SET, uid_data)
+
+ # 辅助方法
+ def slice_list(self, input_list, slice_size):
+ """将列表按指定大小切片"""
+ return [input_list[i:i + slice_size] for i in range(0, len(input_list), slice_size)]
+ # -----------------------------------------------------
+ # API指令区域
+ #------------------------------------------------------
+ def set_joint_positions(self, joint_ranges):
+ """API接口:设置手指所有关节位置"""
+ j = self.cmd_range_to_joint_range(cmd_list=joint_ranges)
+ self.set_thumb_positions(j[0])
+ self.set_index_positions(j[1])
+ self.set_middle_positions(j[2])
+ self.set_ring_positions(j[3])
+ self.set_little_positions(j[4])
+
+ def set_speed(self, speed=[250] * 5):
+ """API接口:设置手指速度"""
+ self.set_thumb_speed(speed_values=[speed[0]] * 6)
+ self.set_index_speed(speed_values=[speed[1]] * 6)
+ self.set_middle_speed(speed_values=[speed[2]] * 6)
+ self.set_ring_speed(speed_values=[speed[3]] * 6)
+ self.set_little_speed(speed_values=[speed[4]] * 6)
+
+ def set_torque(self, torque=[250] * 5):
+ """API接口:设置手指最大扭矩"""
+ self.set_thumb_torque(torque_values=[torque[0]] * 6)
+ self.set_index_torque(torque_values=[torque[1]] * 6)
+ self.set_middle_torque(torque_values=[torque[2]] * 6)
+ self.set_ring_torque(torque_values=[torque[3]] * 6)
+ self.set_little_torque(torque_values=[torque[4]] * 6)
+
+
+ def get_version(self):
+ """API接口:获取手指嵌入式版本信息"""
+ return self.get_software_version()
+
+ def get_current_status(self):
+ """API接口:获取手指当前状态"""
+ self.get_thumb_positions()
+ self.get_index_positions()
+ self.get_middle_positions()
+ self.get_ring_positions()
+ self.get_little_positions()
+ time.sleep(0.002)
+ s = [self.x41, self.x42, self.x43, self.x44, self.x45]
+ cmd_state = self.joint_state_to_cmd_state(state=s)
+ return cmd_state
+
+ def get_current_pub_status(self):
+ """API接口:获取手指当前状态"""
+ self.get_current_status()
+
+ def get_speed(self):
+ """API接口:获取手指速度"""
+ self.get_thumb_speed()
+ self.get_index_speed()
+ self.get_middle_speed()
+ self.get_ring_speed()
+ self.get_little_speed()
+ time.sleep(0.002)
+
+ joint_speed = [self.x49, self.x4A, self.x4B, self.x4C, self.x4D]
+ state_speed = self.joint_state_to_cmd_state(state=joint_speed)
+ return state_speed
+
+ def get_touch_type(self):
+ """API接口:获取手指触觉传感器类型"""
+ self.send_command(0xb0,[],sleep_time=0.03)
+ self.send_command(0xb1,[],sleep_time=0.03)
+ t = []
+ for i in range(3):
+ t = self.xB1
+ time.sleep(0.01)
+ if len(t) == 2:
+ return 2
+ else:
+ self.send_command(0x20,[],sleep_time=0.03)
+ time.sleep(0.01)
+ if self.normal_force[0] == -1:
+ return -1
+ else:
+ return 1
+
+ def get_matrix_touch(self):
+ """API接口:获取手指触摸传感器数据"""
+ self.get_thumb_touch()
+ self.get_index_touch()
+ self.get_middle_touch()
+ self.get_ring_touch()
+ self.get_little_touch()
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_matrix_touch_v2(self):
+ """API接口:获取手指触摸传感器数据"""
+ return self.get_matrix_touch()
+
+ def get_thumb_matrix_touch(self,sleep_time=0):
+ """API接口:获取[大拇指]指触摸传感器数据"""
+ self.get_thumb_touch()
+ if self.xB0[0] == 5:
+ data = self.thumb_matrix_palm
+ else:
+ data = self.thumb_matrix
+ return data
+
+ def get_index_matrix_touch(self,sleep_time=0):
+ """API接口:获取[食指]指触摸传感器数据"""
+ self.get_index_touch()
+ if self.xB0[0] == 5:
+ data = self.index_matrix_palm
+ else:
+ data = self.index_matrix
+ return data
+
+ def get_middle_matrix_touch(self,sleep_time=0):
+ """API接口:获取[中指]指触摸传感器数据"""
+ self.get_middle_touch()
+ if self.xB0[0] == 5:
+ data = self.middle_matrix_palm
+ else:
+ data = self.middle_matrix
+ return data
+
+ def get_ring_matrix_touch(self,sleep_time=0):
+ """API接口:获取[无名指]指触摸传感器数据"""
+ self.get_ring_touch()
+ if self.xB0[0] == 5:
+ data = self.ring_matrix_palm
+ else:
+ data = self.ring_matrix
+ return data
+
+ def get_little_matrix_touch(self,sleep_time=0):
+ """API接口:获取[小指]指触摸传感器数据"""
+ self.get_little_touch()
+ if self.xB0[0] == 5:
+ data = self.little_matrix_palm
+ else:
+ data = self.little_matrix
+ return data
+
+ def get_palm_matrix_touch(self,sleep_time=0):
+ """API接口:获取[小指]指触摸传感器数据"""
+ self.get_palm_touch()
+ if self.xB0[0] == 5:
+ data = self.palm_matrix_palm
+ else:
+ data = self.palm_matrix
+ return data
+
+ def get_torque(self):
+ """API接口:获取手指最大扭矩"""
+ self.get_thumb_torque()
+ self.get_index_torque()
+ self.get_middle_torque()
+ self.get_ring_torque()
+ self.get_little_torque()
+ time.sleep(0.003)
+ t = [self.x51, self.x52, self.x53, self.x54, self.x55]
+ cmd_torque = self.joint_state_to_cmd_state(state=t)
+ return cmd_torque
+
+ def get_current(self):
+ """API接口:获取手指电流"""
+ return [-1] * 20
+
+ def get_temperature(self):
+ """API接口:获取手指温度"""
+ joint_temperature = [self.get_thumb_temperature(), self.get_index_temperature(), self.get_middle_temperature(), self.get_ring_temperature(), self.get_little_temperature()]
+ cmd_temperature = self.joint_state_to_cmd_state(state=joint_temperature)
+ return cmd_temperature
+
+
+ def get_fault(self):
+ """API接口:获取手指故障代码"""
+ joint_fault = [self.get_thumb_fault(), self.get_index_fault(), self.get_middle_fault(), self.get_ring_fault(), self.get_little_fault()]
+ cmd_fault = self.joint_state_to_cmd_state(state=joint_fault)
+ return cmd_fault
+
+ def clear_faults(self):
+ """API接口:清除手指故障代码"""
+ self.clear_finger_faults(finger_mask=[1, 1, 1, 1, 1])
+
+ def cmd_range_to_joint_range(self,cmd_list):
+ """根据手指映射关系，将手指控制命令列表转换为手指分组数据形式"""
+ # 定义手指映射规则
+ finger_mapping = {
+ '拇指': [10, 5, 0, 11, 12, 15],
+ '食指': [6, 11, 1, 13, 14, 16],
+ '中指': [7, 12, 2, 13, 14, 17],
+ '无名指': [8, 13, 3, 14, 15, 18],
+ '小指': [9, 14, 4, 15, 16, 19]
+ }
+
+ result = []
+
+ for finger, indices in finger_mapping.items():
+ finger_data = [cmd_list[i] for i in indices]
+ result.append(finger_data)
+
+ return result
+
+
+ def joint_state_to_cmd_state(self, state):
+ """
+ 将关节状态转换为命令状态
+ :param state: list2 格式的数据，5×6 的二维列表
+ :return: list1 格式的 20 维列表
+ """
+ # 初始化结果列表，20个位置，预留位默认为0
+ result = [0] * 20
+
+ # list1 索引映射:
+ # 0:拇指根部, 1:食指根部, 2:中指根部, 3:无名指根部, 4:小指根部
+ # 5:拇指侧摆, 6:食指侧摆, 7:中指侧摆, 8:无名指侧摆, 9:小指侧摆
+ # 10:拇指横摆, 11-14:预留, 15:拇指尖部, 16:食指末端, 17:中指末端, 18:无名指末端, 19:小指末端
+
+ # list2 每行结构: [侧摆/横摆, 0, 根部, 0, 0, 末端/尖部]
+ # 拇指行: [横摆, 侧摆, 根部, 0, 0, 尖部] — 注意拇指特殊，第1列是横摆，第2列是侧摆
+ # 其他指: [侧摆, 0, 根部, 0, 0, 末端]
+
+ # 拇指 (第0行) — 特殊处理
+ result[10] = state[0][0] # 拇指横摆
+ result[5] = state[0][1] # 拇指侧摆
+ result[0] = state[0][2] # 拇指根部
+ result[15] = state[0][5] # 拇指尖部
+
+ # 食指 (第1行)
+ result[6] = state[1][0] # 食指侧摆
+ result[1] = state[1][2] # 食指根部
+ result[16] = state[1][5] # 食指末端
+
+ # 中指 (第2行)
+ result[7] = state[2][0] # 中指侧摆
+ result[2] = state[2][2] # 中指根部
+ result[17] = state[2][5] # 中指末端
+
+ # 无名指 (第3行)
+ result[8] = state[3][0] # 无名指侧摆
+ result[3] = state[3][2] # 无名指根部
+ result[18] = state[3][5] # 无名指末端
+
+ # 小指 (第4行)
+ result[9] = state[4][0] # 小指侧摆
+ result[4] = state[4][2] # 小指根部
+ result[19] = state[4][5] # 小指末端
+
+ # 预留位 11-14 保持为 0
+
+ return result
+
+
+ def _list_d_value(self, list1, list2):
+ """检查两个列表的值是否有显著差异"""
+ if list1 is None:
+ return True
+ for a, b in zip(list1, list2):
+ if abs(b - a) > 2:
+ return True
+ return False
+
+ def close_can_interface(self):
+ """关闭CAN接口"""
+ if self.bus:
+ self.bus.shutdown()
+ self.running = False
+
+ def get_serial_number(self):
+ try:
+ self.send_command(0xC0,[],sleep_time=0.005)
+ # 1. 使用 bytes() 函数将整数列表转换为字节对象
+ # bytes() 接收一个由 0-255 之间的整数组成的列表。
+ byte_data = bytes(self.serial_number)
+ # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串
+ result_string = byte_data.decode('ascii')
+ if result_string == "":
+ return "-1"
+ else:
+ # print(f"原始 ASCII 码列表: {self.serial_number}")
+ # print(f"解码后的字符串: {result_string}")
+ return result_string
+ except:
+ return "-1"
+ def get_finger_order(self):
+ return ["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", "Reserved", "Reserved", "Reserved", "Thumb Tip", "Index Finger Tip", "Middle Finger Tip", "Ring Finger Tip", "Pinky Finger Tip"]
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l10_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l10_can.py
new file mode 100644
index 0000000..f445a58
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l10_can.py
@@ -0,0 +1,531 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import can
+import time,sys
+import threading
+import numpy as np
+#from tabulate import tabulate
+from enum import Enum
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+
+
+
+class FrameProperty(Enum):
+ INVALID_FRAME_PROPERTY = 0x00
+ JOINT_POSITION_RCO = 0x01
+ MAX_PRESS_RCO = 0x02
+ MAX_PRESS_RCO2 = 0x03
+ JOINT_POSITION2_RCO = 0x04
+ JOINT_SPEED = 0x05
+ JOINT_SPEED2 = 0x06
+ REQUEST_DATA_RETURN = 0x09
+ JOINT_POSITION_N = 0x11
+ MAX_PRESS_N = 0x12
+ HAND_NORMAL_FORCE = 0X20
+ HAND_TANGENTIAL_FORCE = 0X21
+ HAND_TANGENTIAL_FORCE_DIR = 0X22
+ HAND_APPROACH_INC = 0X23
+ MOTOR_TEMPERATURE_1 = 0x33
+ MOTOR_TEMPERATURE_2 = 0x34
+
+class LinkerHandL10Can:
+ def __init__(self,can_id, can_channel='can0', baudrate=1000000, yaml=""):
+ self.can_id = can_id
+ self.can_channel = can_channel
+ self.baudrate = baudrate
+ self.open_can = OpenCan(load_yaml=yaml)
+ self.is_cmd = False
+ self.x01 = [-1] * 5
+ self.x02 = [-1] * 5
+ self.x03 = [-1] * 5
+ self.x04 = [-1] * 5
+ self.x05 = [-1] * 5
+ self.x06 = [-1] * 5
+ self.x33 = self.x34 = [0] * 5
+ # Fault codes
+ self.x35,self.x36 = [0] * 5,[0] * 5
+ # New pressure sensors
+ self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+
+ self.thumb_matrix = np.full((12, 6), -1)
+ self.index_matrix = np.full((12, 6), -1)
+ self.middle_matrix = np.full((12, 6), -1)
+ self.ring_matrix = np.full((12, 6), -1)
+ self.little_matrix = np.full((12, 6), -1)
+ self.matrix_map = {
+ 0: 0,
+ 16: 1,
+ 32: 2,
+ 48: 3,
+ 64: 4,
+ 80: 5,
+ 96: 6,
+ 112: 7,
+ 128: 8,
+ 144: 9,
+ 160: 10,
+ 176: 11,
+ }
+ self.serial_number = []
+ self.serial_number_map = {
+ 0: 0,
+ 1: 1,
+ 2: 2,
+ 3: 3,
+ }
+ self.can_id = can_id
+ self.joint_angles = [0] * 10
+ self.pressures = [200] * 5 # Default torque 200
+ self.bus = self.init_can_bus(can_channel, baudrate)
+ self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 5 for _ in range(4)]
+ self.version = None
+ # Start receiving thread
+ self.running = True
+ self.receive_thread = threading.Thread(target=self.receive_response)
+ self.receive_thread.daemon = True
+ self.receive_thread.start()
+ self.version = self.get_version()
+
+ def init_can_bus(self, channel, baudrate):
+ """
+ 尝试按优先级连接 CAN 总线，并实现回退机制。
+ """
+ # --- 统一异常处理块开始 ---
+ try:
+ if sys.platform == "linux":
+ # Linux 优先级：1. socketcan
+ try:
+ self.open_can.open_can(self.can_channel)
+ # 尝试 socketcan
+ bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+ ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+ raise # 重新抛出异常，让外层 try 捕获
+ elif sys.platform == "win32":
+ # Windows 优先级：1. pcan
+ try:
+ bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+ # Windows 优先级：2. candle (回退方法)
+ try:
+ bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+ raise # 两个接口都失败，抛出异常
+ else:
+ raise EnvironmentError("Unsupported platform for CAN interface")
+ # --- 统一异常处理块结束 ---
+ except Exception as e:
+ # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+ ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+ # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+ raise
+
+ def send_frame(self, frame_property, data_list,sleep=0.002):
+ """Send a single CAN frame with specified properties and data."""
+ frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ data = [frame_property_value] + [int(val) for val in data_list]
+ msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ try:
+ self.bus.send(msg)
+ except can.CanError as e:
+ print(f"Failed to send message: {e}")
+ self.open_can.open_can(self.can_channel)
+ time.sleep(1)
+ self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+ time.sleep(1)
+ if self.is_can:
+ self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+ else:
+ print("Reconnecting CAN devices ....")
+ # time.sleep(1)
+ #
+ time.sleep(sleep)
+
+ def set_joint_positions(self, joint_angles):
+ """Set the positions of 10 joints (joint_angles: list of 10 values)."""
+ self.joint_angles = joint_angles
+ self.is_cmd = True
+ # Send angle control in frames, L10 protocol splits into first 6 and last 4
+ self.send_frame(FrameProperty.JOINT_POSITION2_RCO, self.joint_angles[6:])
+ #time.sleep(0.001)
+ self.send_frame(FrameProperty.JOINT_POSITION_RCO, self.joint_angles[:6])
+ #time.sleep(0.002)
+ self.is_cmd = False
+
+
+ def set_max_torque_limits(self, pressures,type="get"):
+ """Set maximum torque limits"""
+ if type == "get":
+ self.pressures = [0.0]
+ else:
+ self.pressures = pressures[:5]
+ #self.send_frame(FrameProperty.MAX_PRESS_RCO, self.pressures)
+
+
+ def set_joint_speed_l10(self,speed=[180]*5):
+ self.x05 = speed
+ for i in range(2):
+ time.sleep(0.01)
+ self.send_frame(0x05, speed)
+ def set_speed(self,speed=[180]*5):
+ if len(speed) == 5:
+ self.x05 = speed
+ for i in range(2):
+ time.sleep(0.01)
+ self.send_frame(0x05, speed)
+ elif len(speed) == 10:
+ for i in range(2):
+ time.sleep(0.01)
+ self.send_frame(0x05, speed[:5])
+ self.send_frame(0x06, speed[5:])
+ else:
+ raise ValueError("Speed list must have 10 elements.")
+ def request_all_status(self):
+ """Get all joint positions and pressures."""
+ self.send_frame(FrameProperty.REQUEST_DATA_RETURN, [])
+ ''' -------------------Pressure Sensors---------------------- '''
+ def get_normal_force(self):
+ self.send_frame(FrameProperty.HAND_NORMAL_FORCE,[],sleep=0.004)
+
+ def get_tangential_force(self):
+ self.send_frame(FrameProperty.HAND_TANGENTIAL_FORCE,[],sleep=0.004)
+
+ def get_tangential_force_dir(self):
+ self.send_frame(FrameProperty.HAND_TANGENTIAL_FORCE_DIR,[],sleep=0.004)
+ def get_approach_inc(self):
+ self.send_frame(FrameProperty.HAND_APPROACH_INC,[],sleep=0.004)
+ ''' -------------------Motor Temperature---------------------- '''
+ def get_motor_temperature(self):
+ self.send_frame(FrameProperty.MOTOR_TEMPERATURE_1,[],sleep=0.01)
+ self.send_frame(FrameProperty.MOTOR_TEMPERATURE_2,[],sleep=0.01)
+ # Motor fault codes
+ def get_motor_fault_code(self):
+ self.send_frame(0x35,[],sleep=0.1)
+ self.send_frame(0x36,[],sleep=0.1)
+ def receive_response(self):
+ """Receive CAN responses and process them."""
+ while self.running:
+ try:
+ msg = self.bus.recv(timeout=1.0)
+ if msg:
+ self.process_response(msg)
+ except can.CanError as e:
+ print(f"Error receiving CAN message: {e}")
+
+ def process_response(self, msg):
+ """Process received CAN messages."""
+ if msg.arbitration_id == self.can_id:
+ frame_type = msg.data[0]
+ response_data = msg.data[1:]
+ if len(list(response_data)) == 0:
+ return
+ if frame_type == FrameProperty.JOINT_POSITION_RCO.value: # 0x01
+ self.x01 = list(response_data)
+ elif frame_type == FrameProperty.MAX_PRESS_RCO.value: # 0x02
+ self.x02 = list(response_data)
+ elif frame_type == FrameProperty.MAX_PRESS_RCO2.value: # 0x03
+ self.x03 = list(response_data)
+ elif frame_type == FrameProperty.JOINT_POSITION2_RCO.value: # 0x04
+ self.x04 = list(response_data)
+ elif frame_type == 0x05:
+ self.x05 = list(response_data)
+ elif frame_type == 0x06:
+ self.x06 = list(response_data)
+ elif frame_type == 0x20:
+ # Five-finger normal force
+ d = list(response_data)
+ self.normal_force = [float(i) for i in d]
+ elif frame_type == 0x21:
+ # Five-finger tangential force
+ d = list(response_data)
+ self.tangential_force = [float(i) for i in d]
+ elif frame_type == 0x22:
+ # Five-finger tangential force direction
+ d = list(response_data)
+ self.tangential_force_dir = [float(i) for i in d]
+ elif frame_type == 0x23:
+ # Five-finger approach increment
+ d = list(response_data)
+ self.approach_inc = [float(i) for i in d]
+ elif frame_type == 0x33:
+ self.x33 = list(response_data)
+ elif frame_type == 0x34:
+ self.x34 = list(response_data)
+ elif frame_type == 0x35:
+ self.x35 = list(response_data)
+ elif frame_type == 0x36:
+ self.x36 = list(response_data)
+ elif frame_type == 0xb0:
+ self.xb0 = list(response_data)
+ elif frame_type == 0xb1:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb1 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.thumb_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb2:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb2 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.index_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb3:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb3 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.middle_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb4:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb4 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.ring_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb5:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb5 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.little_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0x64:
+ self.version = list(response_data)
+ elif frame_type == 0xC2: # version number
+ self.version = list(response_data)
+ elif frame_type == 0xC0:
+ d = list(response_data)
+ index = self.serial_number_map.get(d[0])
+ if index is not None:
+ self.serial_number=self.serial_number + d[1:]
+ else:
+ self.serial_number=self.serial_number + [-1] * 6
+
+ def get_version(self):
+ self.send_frame(0x64, [], sleep=0.1)
+ time.sleep(0.1)
+ if self.version is None:
+ self.send_frame(0xC2, [], sleep=0.1)
+ time.sleep(0.1)
+ return self.version
+
+ def set_torque(self,torque=[]):
+ '''Set maximum torque'''
+ if len(torque) == 5:
+ self.send_frame(0x02, torque)
+ time.sleep(0.002)
+ self.send_frame(0x03,torque)
+ elif len(torque) > 5:
+ self.send_frame(0x02, torque[:5])
+ time.sleep(0.002)
+ self.send_frame(0x03,torque[5:])
+
+
+ def get_current_status(self):
+ '''Get current joint status'''
+ if self.is_cmd == False:
+ #if self.version != None and self.version[4] > 35:
+ self.send_frame(0x01,[],sleep=0.003)
+ self.send_frame(0x04,[],sleep=0.003)
+ state = self.x01 + self.x04
+ return state
+ else:
+ state = self.x01 + self.x04
+ return state
+
+ def get_current_pub_status(self):
+ state = self.x01 + self.x04
+ return state
+
+ def get_speed(self):
+ '''Get current speed'''
+ self.send_frame(0x05,[],sleep=0.003)
+ self.send_frame(0x06,[],sleep=0.003)
+ return self.x05 + self.x06
+
+ def get_force(self):
+ '''Get pressure sensor data'''
+ return [self.normal_force,self.tangential_force , self.tangential_force_dir , self.approach_inc]
+ def get_temperature(self):
+ '''Get current motor temperature'''
+ self.get_motor_temperature()
+ return self.x33+self.x34
+
+ def get_touch_type(self):
+ '''Get touch type'''
+ self.send_frame(0xb0,[],sleep=0.03)
+ self.send_frame(0xb1,[],sleep=0.03)
+ t = []
+ for i in range(3):
+ t = self.xb1
+ time.sleep(0.01)
+ if len(t) == 2:
+ return 2
+ else:
+ self.send_frame(0x20,[],sleep=0.03)
+ time.sleep(0.01)
+ if self.normal_force[0] == -1:
+ return -1
+ else:
+ return 1
+
+ def get_touch(self):
+ '''Get touch data'''
+ self.send_frame(0xb1,[],sleep=0.03)
+ self.send_frame(0xb2,[],sleep=0.03)
+ self.send_frame(0xb3,[],sleep=0.03)
+ self.send_frame(0xb4,[],sleep=0.03)
+ self.send_frame(0xb5,[],sleep=0.03)
+ return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available
+
+ def get_matrix_touch(self):
+ self.send_frame(0xb1,[0xc6],sleep=0.06)
+ self.send_frame(0xb2,[0xc6],sleep=0.06)
+ self.send_frame(0xb3,[0xc6],sleep=0.06)
+ self.send_frame(0xb4,[0xc6],sleep=0.06)
+ self.send_frame(0xb5,[0xc6],sleep=0.06)
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_matrix_touch_v2(self):
+ self.send_frame(0xb1,[0xc6],sleep=0.005)
+ self.send_frame(0xb2,[0xc6],sleep=0.005)
+ self.send_frame(0xb3,[0xc6],sleep=0.005)
+ self.send_frame(0xb4,[0xc6],sleep=0.005)
+ self.send_frame(0xb5,[0xc6],sleep=0.005)
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_thumb_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb1,[0xc6],sleep=sleep_time)
+ return self.thumb_matrix
+
+ def get_index_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb2,[0xc6],sleep=sleep_time)
+ return self.index_matrix
+
+ def get_middle_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb3,[0xc6],sleep=sleep_time)
+ return self.middle_matrix
+
+ def get_ring_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb4,[0xc6],sleep=sleep_time)
+ return self.ring_matrix
+
+ def get_little_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb5,[0xc6],sleep=sleep_time)
+ return self.little_matrix
+
+
+ def get_torque(self):
+ '''Get current motor torque'''
+ if self.version != None and self.version[4]< 36:
+ return [-1] * 5
+ else:
+ self.send_frame(0x02, [])
+ time.sleep(0.002)
+ self.send_frame(0x03,[])
+ time.sleep(0.002)
+ return self.x02+self.x03
+
+ def get_fault(self):
+ '''Get motor fault'''
+ self.get_motor_fault_code()
+ return self.x35+self.x36
+
+ def get_current(self):
+ '''Get current'''
+ #return [-1] * 5
+ self.send_frame(0x02, [])
+ time.sleep(0.002)
+ self.send_frame(0x03,[])
+ return self.x02+self.x03
+
+ def get_serial_number(self):
+ try:
+ self.send_frame(0xC0,[],sleep=0.005)
+ # 1. 使用 bytes() 函数将整数列表转换为字节对象
+ # bytes() 接收一个由 0-255 之间的整数组成的列表。
+ byte_data = bytes(self.serial_number)
+ # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串
+ result_string = byte_data.decode('ascii')
+ if result_string == "":
+ return "-1"
+ else:
+ # print(f"原始 ASCII 码列表: {self.serial_number}")
+ # print(f"解码后的字符串: {result_string}")
+ return result_string
+ except:
+ return "-1"
+
+ def get_finger_order(self):
+ return ["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"]
+
+ def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+ """L10 暂不支持清除故障码"""
+ pass
+
+ def show_fun_table(self):
+ # if len(data) != 8 or data[0] != 0x64:
+ # raise ValueError("数据格式不正确")
+ data = self.version
+ result = {
+ "自由度": data[0],
+ "机械版本": data[1],
+ "版本序号": data[2],
+ "手方向": chr(data[3]), # ASCII 转字符
+ "软件版本": f"V{data[4] >> 4}.{data[4] & 0x0F}",
+ "硬件版本": f"V{data[5] >> 4}.{data[5] & 0x0F}",
+ "修订标志": data[6],
+ "set_position": "Y",
+ "set_torque": "Y",
+ "set_speed": "Y",
+ "get_version": "Y",
+ "get_current_status": "Y",
+ "get_speed": "Y",
+ "get_temperature": "Y",
+ "get_touch_type": "Y",
+ "get_matrix_touch": "Y",
+ "get_fault": "Y",
+ "get_current": "current == torque"
+ }
+
+ #return [data[0],data[1],data[2],chr(data[3]),f"V{data[4] >> 4}.{data[4] & 0x0F}",f"V{data[5] >> 4}.{data[5] & 0x0F}",data[6]]
+ table = [[k, v] for k, v in result.items()]
+ #print(tabulate(table, tablefmt="grid"), flush=True)
+
+
+ # # 示例数据
+ # data = [0x64, 0x15, 0x03, 0x0A, 0x4C, 0x11, 0x22, 0x01]
+ # parsed = parse_version_data(data)
+
+ # # 打印结果
+ # for k, v in parsed.items():
+ # print(f"{k}: {v}")
+
+
+ def close_can_interface(self):
+ """Stop the CAN communication."""
+ self.running = False
+ if self.receive_thread.is_alive():
+ self.receive_thread.join()
+ if self.bus:
+ self.bus.shutdown()
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l20_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l20_can.py
new file mode 100644
index 0000000..5b5f36e
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l20_can.py
@@ -0,0 +1,478 @@
+import sys
+import time
+import can
+import threading
+from enum import Enum
+import numpy as np
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+
+class FrameProperty(Enum):
+ INVALID_FRAME_PROPERTY = 0x00 # Invalid CAN frame property | No return
+ JOINT_PITCH_R = 0x01 # Short frame pitch angle - finger base flexion | Returns this type of data
+ JOINT_YAW_R = 0x02 # Short frame yaw angle - finger abduction/adduction | Returns this type of data
+ JOINT_ROLL_R = 0x03 # Short frame roll angle - only used for thumb | Returns this type of data
+ JOINT_TIP_R = 0x04 # Short frame fingertip angle control | Returns this type of data
+ JOINT_SPEED_R = 0x05 # Short frame speed - motor running speed control | Returns this type of data
+ JOINT_CURRENT_R = 0x06 # Short frame current - motor running current feedback | Returns this type of data
+ JOINT_FAULT_R = 0x07 # Short frame fault - motor running fault feedback | Returns this type of data
+ REQUEST_DATA_RETURN = 0x09 # Request data return | Returns all data
+ JOINT_PITCH_NR = 0x11 # Pitch angle - finger base flexion | No return for this type of data
+ JOINT_YAW_NR = 0x12 # Yaw angle - finger abduction/adduction | No return for this type of data
+ JOINT_ROLL_NR = 0x13 # Roll angle - only used for thumb | No return for this type of data
+ JOINT_TIP_NR = 0x14 # Fingertip angle control | No return for this type of data
+ JOINT_SPEED_NR = 0x15 # Speed - motor running speed control | No return for this type of data
+ JOINT_CURRENT_NR = 0x16 # Current - motor running current feedback | No return for this type of data
+ JOINT_FAULT_NR = 0x17 # Fault - motor running fault feedback | No return for this type of data
+ HAND_UID = 0xC0 # Device unique identifier Read only --------
+ HAND_HARDWARE_VERSION = 0xC1 # Hardware version Read only --------
+ HAND_SOFTWARE_VERSION = 0xC2 # Software version Read only --------
+ HAND_COMM_ID = 0xC3 # Device ID Read/Write 1 byte
+ HAND_SAVE_PARAMETER = 0xCF # Save parameters Write only --------
+
+
+class LinkerHandL20Can:
+ def __init__(self, can_channel='can0', baudrate=1000000, can_id=0x28,yaml=""):
+ self.can_id = can_id
+ self.can_channel = can_channel
+ self.baudrate = baudrate
+ self.open_can = OpenCan(load_yaml=yaml)
+
+ self.running = True
+ self.x05 = [255] * 5
+ self.x06, self.x07 = [],[]
+ # New pressure sensors
+ self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+ self.x09 = self.x0b = self.x0c = self.x0d = [-1] * 5
+ self.thumb_matrix = np.full((12, 6), -1)
+ self.index_matrix = np.full((12, 6), -1)
+ self.middle_matrix = np.full((12, 6), -1)
+ self.ring_matrix = np.full((12, 6), -1)
+ self.little_matrix = np.full((12, 6), -1)
+ self.matrix_map = {
+ 0: 0,
+ 16: 1,
+ 32: 2,
+ 48: 3,
+ 64: 4,
+ 80: 5,
+ 96: 6,
+ 112: 7,
+ 128: 8,
+ 144: 9,
+ 160: 10,
+ 176: 11,
+ }
+
+ # Initialize CAN bus according to operating system
+ # try:
+ # if sys.platform == "linux":
+ # self.open_can.open_can(self.can_channel)
+ # time.sleep(0.1)
+ # self.bus = can.interface.Bus(
+ # channel=can_channel, interface="socketcan", bitrate=baudrate,
+ # can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+ # )
+ # elif sys.platform == "win32":
+ # self.bus = can.interface.Bus(
+ # channel=can_channel, interface='pcan', bitrate=baudrate,
+ # can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+ # )
+ # else:
+ # raise EnvironmentError("Unsupported platform for CAN interface")
+ # except:
+ # print("Please insert CAN device",flush=True)
+ self.bus = self.init_can_bus(channel=self.can_channel, baudrate=baudrate)
+ # Initialize data storage
+ self.x01, self.x02, self.x03, self.x04 = [[-1] * 5 for _ in range(4)]
+ self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = \
+ [[-1] * 5 for _ in range(4)]
+
+ # Start receive thread
+ self.get_touch_type()
+ time.sleep(0.1)
+ self.receive_thread = threading.Thread(target=self.receive_response)
+ self.receive_thread.daemon = True
+ self.receive_thread.start()
+
+ def init_can_bus(self, channel, baudrate):
+ """
+ 尝试按优先级连接 CAN 总线，并实现回退机制。
+ """
+ # --- 统一异常处理块开始 ---
+ try:
+ if sys.platform == "linux":
+ # Linux 优先级：1. socketcan
+ try:
+ self.open_can.open_can(self.can_channel)
+ # 尝试 socketcan
+ bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+ ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+ raise # 重新抛出异常，让外层 try 捕获
+ elif sys.platform == "win32":
+ # Windows 优先级：1. pcan
+ try:
+ bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+ # Windows 优先级：2. candle (回退方法)
+ try:
+ bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+ raise # 两个接口都失败，抛出异常
+ else:
+ raise EnvironmentError("Unsupported platform for CAN interface")
+ # --- 统一异常处理块结束 ---
+ except Exception as e:
+ # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+ ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+ # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+ raise
+ # def send_command(self, frame_property, data_list):
+ # print("66666")
+ # """
+ # Send command to CAN bus
+ # :param frame_property: Data frame property
+ # :param data_list: Data payload
+ # """
+ # frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ # data = [frame_property_value] + [int(val) for val in data_list]
+ # msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ # try:
+ # self.bus.send(msg)
+ # print(f"Message sent: ID={hex(self.can_id)}, Data={data}")
+ # except can.CanError as e:
+ # print(f"Failed to send message: {e}")
+
+ def receive_response(self):
+ """
+ Receive and process CAN bus response messages
+ """
+ while self.running:
+ try:
+ msg = self.bus.recv(timeout=1.0) # Blocking receive, 1 second timeout
+ if msg:
+ self.process_response(msg)
+ except can.CanError as e:
+ print(f"Error receiving message666: {e}",flush=True)
+
+
+ def set_finger_base(self, angles):
+ self.send_command(FrameProperty.JOINT_PITCH_NR, angles)
+
+ def set_finger_tip(self, angles):
+ self.send_command(FrameProperty.JOINT_TIP_NR, angles)
+
+ def set_finger_middle(self, angles):
+ self.send_command(FrameProperty.JOINT_YAW_NR, angles)
+
+ def set_thumb_roll(self, angle):
+ self.send_command(FrameProperty.JOINT_ROLL_NR, angle)
+
+ def send_command(self, frame_property, data_list,sleep=0.002):
+ frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ data = [frame_property_value] + [int(val) for val in data_list]
+
+ msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ try:
+ self.bus.send(msg)
+ except can.CanError:
+ print("Message NOT sent")
+ self.open_can.open_can(self.can_channel)
+ time.sleep(1)
+ self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+ time.sleep(1)
+ if self.is_can:
+ self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+ else:
+ print("Reconnecting CAN devices ....",flush=True)
+ time.sleep(sleep)
+
+ def set_joint_pitch(self, frame, angles):
+ self.send_command(frame, angles)
+
+ def set_joint_yaw(self, angles):
+ self.send_command(0x02, angles)
+
+ def set_joint_roll(self, thumb_roll):
+ self.send_command(0x03, [thumb_roll, 0, 0, 0, 0])
+
+ def set_joint_speed(self, speed):
+ self.x05 = speed
+ self.send_command(0x05, speed)
+ def set_electric_current(self, e_c=[]):
+ self.send_command(0x06, e_c)
+
+ def get_normal_force(self):
+ self.send_command(0x20,[])
+
+ def get_tangential_force(self):
+ self.send_command(0x21,[])
+
+
+ def get_tangential_force_dir(self):
+ self.send_command(0x22,[])
+
+ def get_approach_inc(self):
+ self.send_command(0x23,[])
+
+
+
+
+ def get_electric_current(self, e_c=[]):
+ self.send_command(0x06, e_c)
+
+ def request_device_info(self):
+ self.send_command(0xC0, [0])
+ self.send_command(0xC1, [0])
+ self.send_command(0xC2, [0])
+
+ def save_parameters(self):
+ self.send_command(0xCF, [])
+ def process_response(self, msg):
+ if msg.arbitration_id == self.can_id:
+ frame_type = msg.data[0]
+ response_data = msg.data[1:]
+ if len(list(response_data)) == 0:
+ return
+ if frame_type == 0x01:
+ self.x01 = list(response_data)
+ elif frame_type == 0x02:
+ self.x02 = list(response_data)
+ elif frame_type == 0x03:
+ self.x03 = list(response_data)
+ elif frame_type == 0x04:
+ self.x04 = list(response_data)
+ elif frame_type == 0xC0:
+ print(f"Device ID info: {response_data}")
+ if self.can_id == 0x28:
+ self.right_hand_info = response_data
+ elif self.can_id == 0x27:
+ self.left_hand_info = response_data
+ elif frame_type == 0x05:
+ self.x05 = list(response_data)
+ elif frame_type == 0x06:
+ self.x06 = list(response_data)
+ elif frame_type == 0x07:
+ self.x07 = list(response_data)
+ elif frame_type == 0x09:
+ self.x09 = list(response_data)
+ elif frame_type == 0x0B:
+ self.x0b = list(response_data)
+ elif frame_type == 0x0C:
+ self.x0c = list(response_data)
+ elif frame_type == 0x0D:
+ self.x0d = list(response_data)
+ elif frame_type == 0x20:
+ d = list(response_data)
+ self.normal_force = [float(i) for i in d]
+ elif frame_type == 0x21:
+ d = list(response_data)
+ self.tangential_force = [float(i) for i in d]
+ elif frame_type == 0x22:
+ d = list(response_data)
+ self.tangential_force_dir = [float(i) for i in d]
+ elif frame_type == 0x23:
+ d = list(response_data)
+ self.approach_inc = [float(i) for i in d]
+ elif frame_type == 0xb0:
+ self.xb0 = list(response_data)
+ elif frame_type == 0xb1:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb1 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.thumb_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb2:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb2 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.index_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb3:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb3 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.middle_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb4:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb4 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.ring_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb5:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb5 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.little_matrix[index] = d[1:] # Remove the first flag bit
+ def pose_slice(self, p):
+ """Slice the joint array into finger action arrays"""
+ try:
+ finger_base = [int(val) for val in p[0:5]] # Finger base
+ yaw_angles = [int(val) for val in p[5:10]] # Yaw
+ thumb_yaw = [int(val) for val in p[10:15]] # Thumb yaw to palm, others are 0
+ finger_tip = [int(val) for val in p[15:20]] # Fingertip flexion
+ return finger_base, yaw_angles, thumb_yaw, finger_tip
+ except Exception as e:
+ print(e)
+ def set_joint_positions(self, position):
+ if len(position) != 20:
+ print("L20 finger joint length is incorrect")
+ return
+ finger_base, yaw_angles, thumb_yaw, finger_tip = self.pose_slice(position)
+ self.set_thumb_roll(thumb_yaw) # Thumb yaw to palm movement
+ self.set_finger_tip(finger_tip) # Fingertip movement
+ self.set_finger_base(finger_base) # Finger base movement
+ self.set_finger_middle(yaw_angles) # Yaw movement
+ def set_speed(self, speed=[]):
+ if len(speed) != 5:
+ raise ValueError("Speed list must have 5 elements.")
+ return
+ self.send_command(0x05,speed)
+ def set_torque(self, torque=[]):
+ '''Set torque, not supported for L20'''
+ print("Set torque, not supported for L20")
+ def set_current(self, current=[]):
+ '''Set current'''
+ self.set_electric_current(e_c=current)
+ def get_version(self):
+ '''Get version, currently not supported'''
+ return [0] * 5
+ def get_current_status(self):
+ '''Get current finger joint status'''
+ self.send_command(0x01,[],sleep=0.01)
+ self.send_command(0x02,[],sleep=0.01)
+ self.send_command(0x03,[],sleep=0.01)
+ self.send_command(0x04,[],sleep=0.01)
+ return self.x01 + self.x02 + self.x03 + self.x04
+
+ def get_current_pub_status(self):
+ time.sleep(0.01)
+ return self.x01 + self.x02 + self.x03 + self.x04
+
+ def get_speed(self):
+ '''Get current motor speed'''
+ self.send_command(0x05, [0])
+ time.sleep(0.001)
+ return self.x05
+ def get_current(self):
+ '''Get current threshold'''
+ self.send_command(0x06, [0])
+ return self.x06
+ def get_torque(self):
+ '''Get current motor torque, not supported for L20'''
+ return [0] * 5
+ def get_fault(self):
+ self.send_command(0x07,[])
+ time.sleep(0.01)
+ return self.x07
+
+ def get_temperature(self):
+ '''Get motor temperature'''
+ self.send_command(0x09,[])
+ self.send_command(0x0b,[])
+ self.send_command(0x0c,[])
+ self.send_command(0x0d,[])
+
+ return self.x09+self.x0b+self.x0c+self.x0d
+
+ def clear_faults(self):
+ '''Clear motor faults'''
+ self.send_command(0x07, [1, 1, 1, 1, 1])
+
+ def get_touch_type(self):
+ '''Get touch type'''
+ t = []
+ for i in range(3):
+ self.send_command(0xb0,[],sleep=0.03)
+ if self.xb0 == [2]:
+ return 2
+ elif self.xb0 == [1]:
+ return 1
+ else:
+ self.send_command(0x20,[],sleep=0.03)
+ time.sleep(0.01)
+ if self.normal_force[0] == -1:
+ return -1
+
+ def get_touch(self):
+ '''Get touch data'''
+ self.send_command(0xb1,[],sleep=0.03)
+ self.send_command(0xb2,[],sleep=0.03)
+ self.send_command(0xb3,[],sleep=0.03)
+ self.send_command(0xb4,[],sleep=0.03)
+ self.send_command(0xb5,[],sleep=0.03)
+ return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available
+
+ def get_matrix_touch(self):
+ self.send_command(0xb1,[0xc6],sleep=0.04)
+ self.send_command(0xb2,[0xc6],sleep=0.04)
+ self.send_command(0xb3,[0xc6],sleep=0.04)
+ self.send_command(0xb4,[0xc6],sleep=0.04)
+ self.send_command(0xb5,[0xc6],sleep=0.04)
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+
+ def get_thumb_matrix_touch(self,sleep_time=0.009):
+ self.send_command(0xb1,[0xc6],sleep=sleep_time)
+ return self.thumb_matrix
+
+ def get_index_matrix_touch(self,sleep_time=0.009):
+ self.send_command(0xb2,[0xc6],sleep=sleep_time)
+ return self.index_matrix
+
+ def get_middle_matrix_touch(self,sleep_time=0.009):
+ self.send_command(0xb3,[0xc6],sleep=sleep_time)
+ return self.middle_matrix
+
+ def get_ring_matrix_touch(self,sleep_time=0.009):
+ self.send_command(0xb4,[0xc6],sleep=sleep_time)
+ return self.ring_matrix
+
+ def get_little_matrix_touch(self,sleep_time=0.009):
+ self.send_command(0xb5,[0xc6],sleep=sleep_time)
+ return self.little_matrix
+
+
+ def get_faults(self):
+ '''Get motor fault codes'''
+ self.send_command(0x07, [])
+ return self.x07
+ def get_force(self):
+ '''Get pressure sensor data'''
+ return [self.normal_force,self.tangential_force,self.tangential_force_dir,self.approach_inc]
+
+ def get_serial_number(self):
+ return [0] * 6
+
+ def show_fun_table(self):
+ pass
+
+ def get_finger_order(self):
+ return []
+
+ def close_can_interface(self):
+ if self.bus:
+ self.bus.shutdown() # Close CAN bus
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l21_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l21_can.py
new file mode 100644
index 0000000..66ba97a
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l21_can.py
@@ -0,0 +1,822 @@
+#!/usr/bin/env python3
+import can
+import time, sys, os
+import threading
+import numpy as np
+from enum import Enum
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+current_dir = os.path.dirname(os.path.abspath(__file__))
+target_dir = os.path.abspath(os.path.join(current_dir, ".."))
+sys.path.append(target_dir)
+
+class FrameProperty(Enum):
+ # Finger motion control - parallel control commands
+ ROLL_POS = 0x01 # Roll joint position
+ YAWPOS = 0x02 # Yaw joint position
+ ROOT1_POS = 0x03 # Root joint 1 position
+ ROOT2_POS = 0x04 # Root joint 2 position
+ ROOT3_POS = 0x05 # Root joint 3 position
+ TIP_POS = 0x06 # Fingertip joint position
+ # Finger motion control - serial control commands
+ THUMB_POS = 0x41 # Thumb joint position
+ INDEX_POS = 0x42 # Index finger joint position
+ MIDDLE_POS = 0x43 # Middle finger joint position
+ RING_POS = 0x44 # Ring finger joint position
+ LITTLE_POS = 0x45 # Little finger joint position
+
+ # Finger motion control - speed
+ ROLL_SPEED = 0x09 # Roll joint speed
+ YAW_SPEED = 0x0A # Yaw joint speed
+ ROOT1_SPEED = 0x0B # Root joint 1 speed
+ ROOT2_SPEED = 0x0C # Root joint 2 speed
+ ROOT3_SPEED = 0x0D # Root joint 3 speed
+ TIP_SPEED = 0x0E # Fingertip joint speed
+ THUMB_SPEED = 0x49 # Thumb speed
+ INDEX_SPEED = 0x4A # Index finger speed
+ MIDDLE_SPEED = 0x4B # Middle finger speed
+ RING_SPEED = 0x4C # Ring finger speed
+ LITTLE_SPEED = 0x4D # Little finger speed
+
+ # Finger motion control - torque
+ ROLL_TORQUE = 0x11 # Roll joint torque
+ YAW_TORQUE = 0x12 # Yaw joint torque
+ ROOT1_TORQUE = 0x13 # Root joint 1 torque
+ ROOT2_TORQUE = 0x14 # Root joint 2 torque
+ ROOT3_TORQUE = 0x15 # Root joint 3 torque
+ TIP_TORQUE = 0x16 # Fingertip joint torque
+ THUMB_TORQUE = 0x51 # Thumb torque
+ INDEX_TORQUE = 0x52 # Index finger torque
+ MIDDLE_TORQUE = 0x53 # Middle finger torque
+ RING_TORQUE = 0x54 # Ring finger torque
+ LITTLE_TORQUE = 0x55 # Little finger torque
+
+ THUMB_FAULT = 0x59 # Thumb fault code | Returns this type of data
+ INDEX_FAULT = 0x5A # Index finger fault code | Returns this type of data
+ MIDDLE_FAULT = 0x5B # Middle finger fault code | Returns this type of data
+ RING_FAULT = 0x5C # Ring finger fault code | Returns this type of data
+ LITTLE_FAULT = 0x5D # Little finger fault code | Returns this type of data
+
+ # Finger faults and temperature
+ ROLL_FAULT = 0x19 # Roll joint fault code
+ YAW_FAULT = 0x1A # Yaw joint fault code
+ ROOT1_FAULT = 0x1B # Root joint 1 fault code
+ ROOT2_FAULT = 0x1C # Root joint 2 fault code
+ ROOT3_FAULT = 0x1D # Root joint 3 fault code
+ TIP_FAULT = 0x1E # Fingertip joint fault code
+ ROLL_TEMPERATURE = 0x21 # Roll joint over-temperature protection threshold
+ YAW_TEMPERATURE = 0x22 # Yaw joint over-temperature protection threshold
+ ROOT1_TEMPERATURE = 0x23 # Root joint 1 over-temperature protection threshold
+ ROOT2_TEMPERATURE = 0x24 # Root joint 2 over-temperature protection threshold
+ ROOT3_TEMPERATURE = 0x25 # Root joint 3 over-temperature protection threshold
+ TIP_TEMPERATURE = 0x26 # Fingertip joint over-temperature protection threshold
+ THUMB_TEMPERATURE = 0x61 # Thumb over-temperature protection threshold
+ INDEX_TEMPERATURE = 0x62 # Index finger over-temperature protection threshold
+ MIDDLE_TEMPERATURE = 0x63 # Middle finger over-temperature protection threshold
+ RING_TEMPERATURE = 0x64 # Ring finger over-temperature protection threshold
+ LITTLE_TEMPERATURE = 0x65 # Little finger over-temperature protection threshold
+
+ # Configuration and preset actions
+ HAND_UID = 0xC0 # Device unique identifier
+ HAND_HARDWARE_VERSION = 0xC1 # Hardware version
+ HAND_SOFTWARE_VERSION = 0xC2 # Software version
+ HAND_COMM_ID = 0xC3 # Device ID
+ HAND_FACTORY_RESET = 0xCE # Restore factory settings
+ HAND_SAVE_PARAMETER = 0xCF # Save parameters
+
+ # Tactile sensor data
+ HAND_NORMAL_FORCE = 0x90 # Normal force of five fingers
+ HAND_TANGENTIAL_FORCE = 0x91 # Tangential force of five fingers
+ HAND_TANGENTIAL_FORCE_DIR = 0x92 # Tangential direction of five fingers
+ HAND_APPROACH_INC = 0x93 # Approach sensing of five fingers
+
+ TOUCH_SENSOR_TYPE = 0xB0 # Sensor type
+ THUMB_TOUCH = 0xB1 # Thumb tactile sensing
+ INDEX_TOUCH = 0xB2 # Index finger tactile sensing
+ MIDDLE_TOUCH = 0xB3 # Middle finger tactile sensing
+ RING_TOUCH = 0xB4 # Ring finger tactile sensing
+ LITTLE_TOUCH = 0xB5 # Little finger tactile sensing
+ PALM_TOUCH = 0xB6 # Palm tactile sensing
+
+ # Action control
+ ACTION_PLAY = 0xA0 # Action
+
+ # Combined command area
+ FINGER_SPEED = 0x81 # Set maximum finger speed
+ FINGER_TORQUE = 0x82 # Set maximum finger torque
+ FINGER_FAULT = 0x83 # Clear finger faults and fault codes
+ FINGER_TEMPERATURE = 0x84 # Finger joint temperatures
+
+class LinkerHandL21Can:
+ def __init__(self, can_channel='can0', baudrate=1000000, can_id=0x28,yaml=""):
+ self.can_id = can_id
+ self.can_channel = can_channel
+ self.baudrate = baudrate
+ self.open_can = OpenCan(load_yaml=yaml)
+
+ self.running = True
+ self.last_thumb_pos, self.last_index_pos,self.last_ring_pos,self.last_middle_pos, self.last_little_pos = None,None,None,None,None
+ self.x01, self.x02, self.x03, self.x04,self.x05,self.x06,self.x07, self.x08,self.x09,self.x0A,self.x0B,self.x0C,self.x0D,self.x0E,self.speed = [],[],[],[],[],[],[],[],[],[],[],[],[],[],[]
+ self.last_root1,self.last_yaw,self.last_roll,self.last_root2,self.last_tip = None,None,None,None,None
+ # Speed
+ self.x49, self.x4a, self.x4b, self.x4c, self.x4d,self.xc1 = [],[],[],[],[],[]
+ self.x41,self.x42,self.x43,self.x44,self.x45 = [],[],[],[],[]
+ self.x83 = [-1] * 5
+ # Torque
+ self.x51, self.x52, self.x53, self.x54,self.x55 = [],[],[],[],[]
+ # Fault codes
+ self.x59,self.x5a,self.x5b,self.x5c,self.x5d = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+ # Temperature thresholds
+ self.x61,self.x62,self.x63,self.x64,self.x65 = [],[],[],[],[]
+ # Pressure sensors
+ self.x90,self.x91,self.x92,self.x93 = [],[],[],[]
+ # New pressure sensors
+ self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5,self.xb6 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+ self.thumb_matrix = np.full((12, 6), -1)
+ self.index_matrix = np.full((12, 6), -1)
+ self.middle_matrix = np.full((12, 6), -1)
+ self.ring_matrix = np.full((12, 6), -1)
+ self.little_matrix = np.full((12, 6), -1)
+ self.matrix_map = {
+ 0: 0,
+ 16: 1,
+ 32: 2,
+ 48: 3,
+ 64: 4,
+ 80: 5,
+ 96: 6,
+ 112: 7,
+ 128: 8,
+ 144: 9,
+ 160: 10,
+ 176: 11,
+ }
+ # Initialize CAN bus according to operating system
+ # try:
+ # if sys.platform == "linux":
+ # self.open_can.open_can(self.can_channel)
+ # time.sleep(0.1)
+ # self.bus = can.interface.Bus(
+ # channel=can_channel, interface="socketcan", bitrate=baudrate,
+ # can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+ # )
+ # elif sys.platform == "win32":
+ # self.bus = can.interface.Bus(
+ # channel=can_channel, interface='pcan', bitrate=baudrate,
+ # can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+ # )
+ # else:
+ # raise EnvironmentError("Unsupported platform for CAN interface")
+ # except:
+ # print("Please insert CAN device")
+ self.bus = self.init_can_bus(channel=self.can_channel, baudrate=baudrate)
+
+ # Start receive thread
+ self.receive_thread = threading.Thread(target=self.receive_response)
+ self.receive_thread.daemon = True
+ self.receive_thread.start()
+
+ def init_can_bus(self, channel, baudrate):
+ """
+ 尝试按优先级连接 CAN 总线，并实现回退机制。
+ """
+ # --- 统一异常处理块开始 ---
+ try:
+ if sys.platform == "linux":
+ # Linux 优先级：1. socketcan
+ try:
+ self.open_can.open_can(self.can_channel)
+ # 尝试 socketcan
+ bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+ ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+ raise # 重新抛出异常，让外层 try 捕获
+ elif sys.platform == "win32":
+ # Windows 优先级：1. pcan
+ try:
+ bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+ # Windows 优先级：2. candle (回退方法)
+ try:
+ bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+ raise # 两个接口都失败，抛出异常
+ else:
+ raise EnvironmentError("Unsupported platform for CAN interface")
+ # --- 统一异常处理块结束 ---
+ except Exception as e:
+ # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+ ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+ # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+ raise
+
+ def send_command(self, frame_property, data_list,sleep_time=0.003):
+ """
+ Send command to CAN bus
+ :param frame_property: Data frame property
+ :param data_list: Data payload
+ """
+ frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ data = [frame_property_value] + [int(val) for val in data_list]
+ msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ try:
+ self.bus.send(msg)
+ except can.CanError as e:
+ print(f"Failed to send message: {e}")
+ self.open_can.open_can(self.can_channel)
+ time.sleep(1)
+ self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+ time.sleep(1)
+ if self.is_can:
+ self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+ else:
+ print("Reconnecting CAN devices ....")
+ time.sleep(sleep_time)
+
+ def receive_response(self):
+ """
+ Receive and process CAN bus response messages
+ """
+ while self.running:
+ try:
+ msg = self.bus.recv(timeout=1.0)
+ if msg:
+ self.process_response(msg)
+ except can.CanError as e:
+ print(f"Error receiving message: {e}")
+
+
+ def set_joint_positions(self, joint_ranges):
+ if len(joint_ranges) == 25:
+ l21_pose = self.joint_map(joint_ranges)
+ # Use list comprehension to split the list into subarrays of 6 elements each
+ chunks = [l21_pose[i:i+6] for i in range(0, 30, 6)]
+ for i in range(3):
+ self.send_command(FrameProperty.THUMB_POS, chunks[0])
+ time.sleep(0.001)
+ self.send_command(FrameProperty.INDEX_POS, chunks[1])
+ time.sleep(0.001)
+ self.send_command(FrameProperty.MIDDLE_POS, chunks[2])
+ time.sleep(0.001)
+ self.send_command(FrameProperty.RING_POS, chunks[3])
+ time.sleep(0.001)
+ self.send_command(FrameProperty.LITTLE_POS, chunks[4])
+ time.sleep(0.001)
+
+ def set_joint_positions_by_topic(self, joint_ranges):
+ if len(joint_ranges) == 25:
+ l21_pose = self.slice_list(joint_ranges,5)
+ if self._list_d_value(self.last_root1, l21_pose[0]):
+ self.set_root1_positions(l21_pose[0])
+ self.last_root1 = l21_pose[0]
+ if self._list_d_value(self.last_yaw, l21_pose[1]):
+ self.set_yaw_positions(l21_pose[1])
+ self.last_yaw = l21_pose[1]
+ if self._list_d_value(self.last_roll, l21_pose[2]):
+ self.set_roll_positions(l21_pose[2])
+ self.last_roll = l21_pose[2]
+ if self._list_d_value(self.last_root2, l21_pose[3]):
+ self.set_root2_positions(l21_pose[3])
+ self.last_root2 = l21_pose[3]
+ if self._list_d_value(self.last_tip, l21_pose[4]):
+ self.set_tip_positions(l21_pose[4])
+ self.last_tip = l21_pose[4]
+
+
+ def slice_list(self, input_list, slice_size):
+ """
+ Slice a list into pieces of specified size.
+
+ Args:
+ input_list (list): The list to be sliced.
+ slice_size (int): Number of elements per slice.
+
+ Returns:
+ list of lists: The sliced list.
+ """
+ sliced_list = [input_list[i:i + slice_size] for i in range(0, len(input_list), slice_size)]
+ return sliced_list
+
+ def _list_d_value(self,list1, list2):
+ if list1 == None:
+ return True
+ for a, b in zip(list1, list2):
+ if abs(b - a) > 2:
+ return True
+ break
+ return False
+ # Set all finger roll joint positions
+ def set_roll_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROLL_POS, joint_ranges)
+ # Set all finger yaw joint positions
+ def set_yaw_positions(self, joint_ranges):
+ self.send_command(FrameProperty.YAW_POS, joint_ranges)
+ # Set all finger root1 joint positions
+ def set_root1_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROOT1_POS, joint_ranges)
+ # Set all finger root2 joint positions
+ def set_root2_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROOT2_POS, joint_ranges)
+ # Set all finger root3 joint positions
+ def set_root3_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROOT3_POS, joint_ranges)
+ # Set all finger tip joint positions
+ def set_tip_positions(self, joint_ranges=[80]*5):
+ self.send_command(FrameProperty.TIP_POS, joint_ranges)
+ # Set thumb torque
+ def set_thumb_torque(self, j=[]):
+ self.send_command(FrameProperty.THUMB_TORQUE, j)
+ # Set index finger torque
+ def set_index_torque(self, j=[]):
+ self.send_command(FrameProperty.INDEX_TORQUE, j)
+ # Set middle finger torque
+ def set_middle_torque(self, j=[]):
+ self.send_command(FrameProperty.MIDDLE_TORQUE, j)
+ # Set ring finger torque
+ def set_ring_torque(self, j=[]):
+ self.send_command(FrameProperty.RING_TORQUE, j)
+ # Set little finger torque
+ def set_little_torque(self, j=[]):
+ self.send_command(FrameProperty.LITTLE_TORQUE, j)
+
+ # Get thumb joint positions
+ def get_thumb_positions(self,j=[0]):
+ self.send_command(FrameProperty.THUMB_POS, j)
+ # Get index finger joint positions
+ def get_index_positions(self, j=[0]):
+ self.send_command(FrameProperty.INDEX_POS,j)
+ # Get middle finger joint positions
+ def get_middle_positions(self, j=[0]):
+ self.send_command(FrameProperty.MIDDLE_POS,j)
+ # Get ring finger joint positions
+ def get_ring_positions(self, j=[0]):
+ self.send_command(FrameProperty.RING_POS,j)
+ # Get little finger joint positions
+ def get_little_positions(self, j=[0]):
+ self.send_command(FrameProperty.LITTLE_POS, j)
+ # Get all thumb motor fault codes
+ def get_thumbn_fault(self,j=[]):
+ self.send_command(FrameProperty.THUMB_FAULT,j)
+ # Get all index finger motor fault codes
+ def get_index_fault(self,j=[]):
+ self.send_command(FrameProperty.INDEX_FAULT,j)
+ # Get all middle finger motor fault codes
+ def get_middle_fault(self,j=[]):
+ self.send_command(FrameProperty.MIDDLE_FAULT,j)
+ # Get all ring finger motor fault codes
+ def get_ring_fault(self,j=[]):
+ self.send_command(FrameProperty.RING_FAULT,j)
+ # Get all little finger motor fault codes
+ def get_little_fault(self,j=[]):
+ self.send_command(FrameProperty.LITTLE_FAULT,j)
+ # Get thumb temperature threshold
+ def get_thumb_threshold(self,j=[]):
+ self.send_command(FrameProperty.THUMB_TEMPERATURE, '')
+ # Get index finger temperature threshold
+ def get_index_threshold(self,j=[]):
+ self.send_command(FrameProperty.INDEX_TEMPERATURE, j)
+ # Get middle finger temperature threshold
+ def get_middle_threshold(self,j=[]):
+ self.send_command(FrameProperty.MIDDLE_TEMPERATURE, j)
+ # Get ring finger temperature threshold
+ def get_ring_threshold(self,j=[]):
+ self.send_command(FrameProperty.RING_TEMPERATURE, j)
+ # Get little finger temperature threshold
+ def get_little_threshold(self,j=[]):
+ self.send_command(FrameProperty.LITTLE_TEMPERATURE, j)
+
+ # Disable mode 01
+ def set_disability_mode(self, j=[1,1,1,1,1]):
+ self.send_command(0x85,j)
+ # Enable mode 00
+ def set_enable_mode(self, j=[00,00,00,00,00]):
+ self.send_command(0x85,j)
+
+ # Set all finger torques
+ def set_torque(self,torque=[250]*5):
+ t = torque[0]
+ i = torque[1]
+ m = torque[2]
+ r = torque[3]
+ l = torque[4]
+ self.set_thumb_torque(j=[t]*5)
+ self.set_index_torque(j=[i]*5)
+ self.set_middle_torque(j=[m]*5)
+ self.set_ring_torque(j=[r]*5)
+ self.set_little_torque(j=[l]*5)
+
+ def set_speed(self, speed):
+ self.speed = speed
+ if len(speed) < 25:
+ thumb_speed = [self.speed[0]]*5
+ index_speed = [self.speed[1]]*5
+ middle_speed = [self.speed[2]]*5
+ ring_speed = [self.speed[3]]*5
+ little_speed = [self.speed[4]]*5
+ else:
+ thumb_speed = [self.speed[0],self.speed[1],self.speed[2],self.speed[3],self.speed[4]]
+ index_speed = [self.speed[5],self.speed[6],self.speed[7],self.speed[8],self.speed[9]]
+ middle_speed = [self.speed[10],self.speed[11],self.speed[12],self.speed[13],self.speed[14]]
+ ring_speed = [self.speed[15],self.speed[16],self.speed[17],self.speed[18],self.speed[19]]
+ little_speed = [self.speed[20],self.speed[21],self.speed[22],self.speed[23],self.speed[24]]
+ self.send_command(FrameProperty.THUMB_SPEED, thumb_speed)
+ self.send_command(FrameProperty.INDEX_SPEED, index_speed)
+ self.send_command(FrameProperty.MIDDLE_SPEED, middle_speed)
+ self.send_command(FrameProperty.RING_SPEED, ring_speed)
+ self.send_command(FrameProperty.LITTLE_SPEED, little_speed)
+
+ def set_finger_torque(self, torque):
+ self.send_command(0x42, torque)
+
+ def request_device_info(self):
+ self.send_command(0xC0, [0])
+ self.send_command(0xC1, [0])
+ self.send_command(0xC2, [0])
+
+ def save_parameters(self):
+ self.send_command(0xCF, [])
+ def process_response(self, msg):
+ if msg.arbitration_id == self.can_id:
+ frame_type = msg.data[0]
+ response_data = msg.data[1:]
+ if len(list(response_data)) == 0:
+ return
+ if frame_type == 0x01:
+ self.x01 = list(response_data)
+ elif frame_type == 0x02:
+ self.x02 = list(response_data)
+ elif frame_type == 0x03:
+ self.x03 = list(response_data)
+ elif frame_type == 0x04:
+ self.x04 = list(response_data)
+ elif frame_type == 0x05:
+ self.x05 = list(response_data)
+ elif frame_type == 0x06:
+ self.x06 = list(response_data)
+ elif frame_type == 0xC0:
+ print(f"Device ID info: {response_data}")
+ if self.can_id == 0x28:
+ self.right_hand_info = response_data
+ elif self.can_id == 0x27:
+ self.left_hand_info = response_data
+ elif frame_type == 0x08:
+ self.x08 = list(response_data)
+ elif frame_type == 0x09:
+ self.x09 = list(response_data)
+ elif frame_type == 0x0A:
+ self.x0A = list(response_data)
+ elif frame_type == 0x0B:
+ self.x0B = list(response_data)
+ elif frame_type == 0x0C:
+ self.x0C = list(response_data)
+ elif frame_type == 0x0D:
+ self.x0D = list(response_data)
+ elif frame_type == 0x22:
+ d = list(response_data)
+ self.tangential_force_dir = [float(i) for i in d]
+ elif frame_type == 0x23:
+ d = list(response_data)
+ self.approach_inc = [float(i) for i in d]
+ elif frame_type == 0x41:
+ self.x41 = list(response_data)
+ elif frame_type == 0x42:
+ self.x42 = list(response_data)
+ elif frame_type == 0x43:
+ self.x43 = list(response_data)
+ elif frame_type == 0x44:
+ self.x44 = list(response_data)
+ elif frame_type == 0x45:
+ self.x45 = list(response_data)
+ elif frame_type == 0x49:
+ self.x49 = list(response_data)
+ elif frame_type == 0x4a:
+ self.x4a = list(response_data)
+ elif frame_type == 0x4b:
+ self.x4b = list(response_data)
+ elif frame_type == 0x4c:
+ self.x4c = list(response_data)
+ elif frame_type == 0x4d:
+ self.x4d = list(response_data)
+ elif frame_type == 0xc1:
+ self.xc1 = list(response_data)
+ elif frame_type == 0x51:
+ self.x51 = list(response_data)
+ elif frame_type == 0x52:
+ self.x52 = list(response_data)
+ elif frame_type == 0x53:
+ self.x53 = list(response_data)
+ elif frame_type == 0x54:
+ self.x54 = list(response_data)
+ elif frame_type == 0x55:
+ self.x55 = list(response_data)
+ elif frame_type == 0x59:
+ self.x59 = list(response_data)
+ elif frame_type == 0x5a:
+ self.x5a = list(response_data)
+ elif frame_type == 0x5b:
+ self.x5b = list(response_data)
+ elif frame_type == 0x5c:
+ self.x5c = list(response_data)
+ elif frame_type == 0x5d:
+ self.x5d = list(response_data)
+ elif frame_type == 0x61:
+ self.x61 = list(response_data)
+ elif frame_type == 0x62:
+ self.x62 = list(response_data)
+ elif frame_type == 0x63:
+ self.x63 = list(response_data)
+ elif frame_type == 0x64:
+ self.x64 = list(response_data)
+ elif frame_type == 0x65:
+ self.x65 = list(response_data)
+ elif frame_type == 0x83:
+ self.x83 = list(response_data)
+ elif frame_type == 0x90:
+ self.x90 = list(response_data)
+ elif frame_type == 0x91:
+ self.x91 = list(response_data)
+ elif frame_type == 0x92:
+ self.x92 = list(response_data)
+ elif frame_type == 0x93:
+ self.x93 = list(response_data)
+ elif frame_type == 0xb0:
+ self.xb0 = list(response_data)
+ elif frame_type == 0xb1:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb1 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.thumb_matrix[index] = d[1:]
+ elif frame_type == 0xb2:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb2 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.index_matrix[index] = d[1:]
+ elif frame_type == 0xb3:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb3 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.middle_matrix[index] = d[1:]
+ elif frame_type == 0xb4:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb4 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.ring_matrix[index] = d[1:]
+ elif frame_type == 0xb5:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb5 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.little_matrix[index] = d[1:]
+ elif frame_type == 0xb6:
+ self.xb6 = list(response_data)
+
+ def joint_map(self, pose):
+ # l21 CAN data by default receives 30 data
+ l21_pose = [0.0] * 30
+
+ mapping = {
+ 0: 10, 1: 5, 2: 0, 3: 15, 4: None, 5: 20,
+ 6: None, 7: 6, 8: 1, 9: 16, 10: None, 11: 21,
+ 12: None, 13: 7, 14: 2, 15: 17, 16: None, 17: 22,
+ 18: None, 19: 8, 20: 3, 21: 18, 22: None, 23: 23,
+ 24: None, 25: 9, 26: 4, 27: 19, 28: None, 29: 24
+ }
+
+ for l21_idx, pose_idx in mapping.items():
+ if pose_idx is not None:
+ l21_pose[l21_idx] = pose[pose_idx]
+
+ return l21_pose
+
+ def state_to_cmd(self, l21_state):
+ pose = [0.0] * 25
+
+ mapping = {
+ 0: 10, 1: 5, 2: 0, 3: 15, 5: 20, 7: 6,
+ 8: 1, 9: 16, 11: 21, 13:7, 14: 2, 15: 17, 17: 22,
+ 19: 8, 20: 3, 21: 18, 23: 23, 25: 9, 26: 4,
+ 27: 19, 29: 24
+ }
+ for l21_idx, pose_idx in mapping.items():
+ pose[pose_idx] = l21_state[l21_idx]
+ return pose
+ def action_play(self):
+ self.send_command(0xA0,[])
+ def get_current_status(self, j=''):
+ self.send_command(FrameProperty.THUMB_POS, j,sleep_time=0.001)
+ self.send_command(FrameProperty.INDEX_POS,j,sleep_time=0.001)
+ self.send_command(FrameProperty.MIDDLE_POS,j,sleep_time=0.001)
+ self.send_command(FrameProperty.RING_POS,j,sleep_time=0.001)
+ self.send_command(FrameProperty.LITTLE_POS, j,sleep_time=0.001)
+ state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45
+ if len(state) == 30:
+ l21_state = self.state_to_cmd(l21_state=state)
+ return l21_state
+
+ def get_current_pub_status(self):
+ state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45
+ if len(state) == 30:
+ l21_state = self.state_to_cmd(l21_state=state)
+ return l21_state
+
+ def get_current_state_topic(self):
+ self.send_command(0x01,[])
+ self.send_command(0x02,[])
+ self.send_command(0x03,[])
+ self.send_command(0x04,[])
+ self.send_command(0x06,[])
+ state = self.x03+self.x02+self.x01+self.x04+self.x06
+ return state
+
+ def get_speed(self,j=''):
+ self.send_command(FrameProperty.THUMB_SPEED, j)
+ self.send_command(FrameProperty.INDEX_SPEED, j)
+ self.send_command(FrameProperty.MIDDLE_SPEED, j)
+ self.send_command(FrameProperty.RING_SPEED, j)
+ self.send_command(FrameProperty.LITTLE_SPEED, j)
+ speed = self.x49+ self.x4a+ self.x4b+ self.x4c+ self.x4d
+ if len(speed) == 30:
+ l21_speed = self.state_to_cmd(l21_state=speed)
+ return l21_speed
+
+ # def get_finger_torque(self):
+ # return self.finger_torque()
+ def get_fault(self):
+ self.get_thumbn_fault()
+ self.get_index_fault()
+ self.get_middle_fault()
+ self.get_ring_fault()
+ self.get_little_fault()
+ return [self.x59]+[self.x5a]+[self.x5b]+[self.x5c]+[self.x5d]
+ def get_threshold(self):
+ self.get_thumb_threshold()
+ self.get_index_threshold()
+ self.get_middle_threshold()
+ self.get_ring_threshold()
+ self.get_little_threshold()
+ return [self.x61]+[self.x62]+[self.x63]+[self.x64]+[self.x65]
+ def get_version(self):
+ if self.xc1 == []:
+ self.send_command(FrameProperty.HAND_HARDWARE_VERSION,[])
+ return self.xc1
+ def get_normal_force(self):
+ self.send_command(FrameProperty.HAND_NORMAL_FORCE,[])
+ return self.x90
+ def get_tangential_force(self):
+ self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE,[])
+ return self.x91
+ def get_tangential_force_dir(self):
+ self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE_DIR,[])
+ return self.x92
+ def get_approach_inc(self):
+ self.send_command(FrameProperty.HAND_APPROACH_INC,[])
+ return self.x93
+
+ def get_touch_type(self):
+ '''Get tactile sensor type data'''
+ self.send_command(FrameProperty.TOUCH_SENSOR_TYPE,[])
+ try:
+ return self.xb0[0]
+ except:
+ pass
+ def get_finger_torque(self):
+ self.send_command(FrameProperty.THUMB_TORQUE,[])
+ self.send_command(FrameProperty.INDEX_TORQUE,[])
+ self.send_command(FrameProperty.MIDDLE_TORQUE,[])
+ self.send_command(FrameProperty.RING_TORQUE,[])
+ self.send_command(FrameProperty.LITTLE_TORQUE,[])
+ return self.x51+self.x52+self.x53+self.x54+self.x55
+
+ def get_torque(self):
+ return self.get_finger_torque()
+
+ def get_thumb_touch(self):
+ '''Get thumb tactile sensor data'''
+ self.send_command(FrameProperty.THUMB_TOUCH,[],sleep_time=0.015)
+ return self.xb1
+
+ def get_index_touch(self):
+ '''Get index finger tactile sensor data'''
+ self.send_command(FrameProperty.INDEX_TOUCH,[0xc6],sleep_time=0.015)
+ return self.xb2
+
+ def get_middle_touch(self):
+ '''Get middle finger tactile sensor data'''
+ self.send_command(FrameProperty.MIDDLE_TOUCH,[],sleep_time=0.015)
+ return self.xb3
+
+ def get_ring_touch(self):
+ '''Get ring finger tactile sensor data'''
+ self.send_command(FrameProperty.RING_TOUCH,[],sleep_time=0.015)
+ return self.xb4
+
+ def get_little_touch(self):
+ '''Get little finger tactile sensor data'''
+ self.send_command(FrameProperty.LITTLE_TOUCH,[],sleep_time=0.015)
+ return self.xb5
+
+ def get_palm_touch(self):
+ '''Get palm tactile sensor data'''
+ self.send_command(FrameProperty.PALM_TOUCH,[],sleep_time=0.015)
+ return self.xb6
+
+ def get_force(self):
+ '''Get pressure sensor data'''
+ return [self.x90,self.x91 , self.x92 , self.x93]
+
+ def get_touch(self):
+ '''Get tactile sensor data'''
+ self.get_thumb_touch()
+ self.get_index_touch()
+ self.get_middle_touch()
+ self.get_ring_touch()
+ self.get_little_touch()
+ self.get_palm_touch()
+ try:
+ return [self.xb1[1],self.xb2[1] , self.xb3[1] , self.xb4[1],self.xb5[1],self.xb6[1]]
+ except:
+ pass
+
+ def get_matrix_touch(self):
+ self.send_command(0xb1,[0xc6],sleep_time=0.04)
+ self.send_command(0xb2,[0xc6],sleep_time=0.04)
+ self.send_command(0xb3,[0xc6],sleep_time=0.04)
+ self.send_command(0xb4,[0xc6],sleep_time=0.04)
+ self.send_command(0xb5,[0xc6],sleep_time=0.04)
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_current(self):
+ '''Not supported yet'''
+ return [0] * 21
+ def get_temperature(self):
+ self.get_thumb_threshold()
+ self.get_index_threshold()
+ self.get_middle_threshold()
+ self.get_ring_threshold()
+ self.get_little_threshold()
+ return self.x61+self.x62+self.x63+self.x64+self.x65
+
+ def get_serial_number(self):
+ return [0] * 6
+
+ def get_finger_order(self):
+ return [
+ "thumb_root",
+ "index_finger_root",
+ "middle_finger_root",
+ "ring_finger_root",
+ "little_finger_root",
+ "thumb_abduction",
+ "index_finger_abduction",
+ "middle_finger_abduction",
+ "ring_finger_abduction",
+ "little_finger_abduction",
+ "thumb_roll",
+ "reserved",
+ "reserved",
+ "reserved",
+ "reserved",
+ "thumb_middle_joint",
+ "reserved",
+ "reserved",
+ "reserved",
+ "reserved",
+ "thumb_tip",
+ "index_finger_tip",
+ "middle_finger_tip",
+ "ring_finger_tip",
+ "little_finger_tip"
+ ]
+
+ def clear_faults(self):
+ '''Clear motor faults'''
+ self.send_command(0x83, [1, 1, 1, 1, 1],sleep_time=0.003)
+ return self.x83
+
+ def close_can_interface(self):
+ if self.bus:
+ self.bus.shutdown() # Close CAN bus
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l24_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l24_can.py
new file mode 100644
index 0000000..d5192f6
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l24_can.py
@@ -0,0 +1,448 @@
+#!/usr/bin/env python3
+import can
+import time,sys,os
+import threading
+import numpy as np
+from enum import Enum
+current_dir = os.path.dirname(os.path.abspath(__file__))
+target_dir = os.path.abspath(os.path.join(current_dir, ".."))
+sys.path.append(target_dir)
+from utils.color_msg import ColorMsg
+
+class FrameProperty(Enum):
+ INVALID_FRAME_PROPERTY = 0x00 # 无效的can帧属性 | 无返回
+ # 并行指令区域
+ ROLL_POS = 0x01 # 横滚关节位置 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ YAW_POS = 0x02 # 航向关节位置 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ ROOT1_POS = 0x03 # 指根1关节位置 | 最接近手掌的指根关节
+ ROOT2_POS = 0x04 # 指根2关节位置 | 最接近手掌的指根关节
+ ROOT3_POS = 0x05 # 指根3关节位置 | 最接近手掌的指根关节
+ TIP_POS = 0x06 # 指尖关节位置 | 最接近手掌的指根关节
+
+ ROLL_SPEED = 0x09 # 横滚关节速度 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ YAW_SPEED = 0x0A # 航向关节速度 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ ROOT1_SPEED = 0x0B # 指根1关节速度 | 最接近手掌的指根关节
+ ROOT2_SPEED = 0x0C # 指根2关节速度 | 最接近手掌的指根关节
+ ROOT3_SPEED = 0x0D # 指根3关节速度 | 最接近手掌的指根关节
+ TIP_SPEED = 0x0E # 指尖关节速度 | 最接近手掌的指根关节
+
+ ROLL_TORQUE = 0x11 # 横滚关节扭矩 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ YAW_TORQUE = 0x12 # 航向关节扭矩 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ ROOT1_TORQUE = 0x13 # 指根1关节扭矩 | 最接近手掌的指根关节
+ ROOT2_TORQUE = 0x14 # 指根2关节扭矩 | 最接近手掌的指根关节
+ ROOT3_TORQUE = 0x15 # 指根3关节扭矩 | 最接近手掌的指根关节
+ TIP_TORQUE = 0x16 # 指尖关节扭矩 | 最接近手掌的指根关节
+
+ ROLL_FAULT = 0x19 # 横滚关节故障码 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ YAW_FAULT = 0x1A # 航向关节故障码 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ ROOT1_FAULT = 0x1B # 指根1关节故障码 | 最接近手掌的指根关节
+ ROOT2_FAULT = 0x1C # 指根2关节故障码 | 最接近手掌的指根关节
+ ROOT3_FAULT = 0x1D # 指根3关节故障码 | 最接近手掌的指根关节
+ TIP_FAULT = 0x1E # 指尖关节故障码 | 最接近手掌的指根关节
+
+ ROLL_TEMPERATURE = 0x21 # 横滚关节温度 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ YAW_TEMPERATURE = 0x22 # 航向关节温度 | 坐标系建在每个手指的指根部位，按手指伸直的状态去定义旋转角度
+ ROOT1_TEMPERATURE = 0x23 # 指根1关节温度 | 最接近手掌的指根关节
+ ROOT2_TEMPERATURE = 0x24 # 指根2关节温度 | 最接近手掌的指根关节
+ ROOT3_TEMPERATURE = 0x25 # 指根3关节温度 | 最接近手掌的指根关节
+ TIP_TEMPERATURE = 0x26 # 指尖关节温度 | 最接近手掌的指根关节
+ # 并行指令区域
+
+ # 串行指令区域
+ THUMB_POS = 0x41 # 大拇指指关节位置 | 返回本类型数据
+ INDEX_POS = 0x42 # 食指关节位置 | 返回本类型数据
+ MIDDLE_POS = 0x43 # 中指关节位置 | 返回本类型数据
+ RING_POS = 0x44 # 无名指关节位置 | 返回本类型数据
+ LITTLE_POS = 0x45 # 小拇指关节位置 | 返回本类型数据
+
+ THUMB_SPEED = 0x49 # 大拇指速度 | 返回本类型数据
+ INDEX_SPEED = 0x4A # 食指速度 | 返回本类型数据
+ MIDDLE_SPEED = 0x4B # 中指速度 | 返回本类型数据
+ RING_SPEED = 0x4C # 无名指速度 | 返回本类型数据
+ LITTLE_SPEED = 0x4D # 小拇指速度 | 返回本类型数据
+
+ THUMB_TORQUE = 0x51 # 大拇指扭矩 | 返回本类型数据
+ INDEX_TORQUE = 0x52 # 食指扭矩 | 返回本类型数据
+ MIDDLE_TORQUE = 0x53 # 中指扭矩 | 返回本类型数据
+ RING_TORQUE = 0x54 # 无名指扭矩 | 返回本类型数据
+ LITTLE_TORQUE = 0x55 # 小拇指扭矩 | 返回本类型数据
+
+ THUMB_FAULT = 0x59 # 大拇指故障码 | 返回本类型数据
+ INDEX_FAULT = 0x5A # 食指故障码 | 返回本类型数据
+ MIDDLE_FAULT = 0x5B # 中指故障码 | 返回本类型数据
+ RING_FAULT = 0x5C # 无名指故障码 | 返回本类型数据
+ LITTLE_FAULT = 0x5D # 小拇指故障码 | 返回本类型数据
+
+ THUMB_TEMPERATURE = 0x61 # 大拇指温度 | 返回本类型数据
+ INDEX_TEMPERATURE = 0x62 # 食指温度 | 返回本类型数据
+ MIDDLE_TEMPERATURE = 0x63 # 中指温度 | 返回本类型数据
+ RING_TEMPERATURE = 0x64 # 无名指温度 | 返回本类型数据
+ LITTLE_TEMPERATURE = 0x65 # 小拇指温度 | 返回本类型数据
+ # 串行指令区域
+
+ # 合并指令区域，同一手指非必要单控数据合并
+ FINGER_SPEED = 0x81 # 手指速度 | 返回本类型数据
+ FINGER_TORQUE = 0x82 # 转矩 | 返回本类型数据
+ FINGER_FAULT = 0x83 # 手指故障码 | 返回本类型数据
+
+ # 指尖传感器数据组
+ HAND_NORMAL_FORCE = 0x90 # 五指法向压力
+ HAND_TANGENTIAL_FORCE = 0x91 # 五指切向压力
+ HAND_TANGENTIAL_FORCE_DIR = 0x92 # 五指切向方向
+ HAND_APPROACH_INC = 0x93 # 五指接近感应
+
+ THUMB_ALL_DATA = 0x98 # 大拇指所有数据
+ INDEX_ALL_DATA = 0x99 # 食指所有数据
+ MIDDLE_ALL_DATA = 0x9A # 中指所有数据
+ RING_ALL_DATA = 0x9B # 无名指所有数据
+ LITTLE_ALL_DATA = 0x9C # 小拇指所有数据
+ # 动作指令 ·ACTION
+ ACTION_PLAY = 0xA0 # 动作
+
+ # 配置命令·CONFIG
+ HAND_UID = 0xC0 # 设备唯一标识码
+ HAND_HARDWARE_VERSION = 0xC1 # 硬件版本
+ HAND_SOFTWARE_VERSION = 0xC2 # 软件版本
+ HAND_COMM_ID = 0xC3 # 设备id
+ HAND_FACTORY_RESET = 0xCE # 恢复出厂设置
+ HAND_SAVE_PARAMETER = 0xCF # 保存参数
+
+ WHOLE_FRAME = 0xF0 # 整帧传输 | 返回一字节帧属性+整个结构体485及网络传输专属
+
+class LinkerHandL24Can:
+ def __init__(self, config, can_channel='can0', baudrate=1000000, can_id=0x28):
+ self.config = config
+ self.can_id = can_id
+ self.running = True
+ self.x01, self.x02, self.x03, self.x04,self.x05,self.x06,self.x07, self.x08,self.x09,self.x0A,self.x0B,self.x0C,self.x0D,self.x0E,self.speed = [],[],[],[],[],[],[],[],[],[],[],[],[],[],[]
+ # 速度
+ self.x49, self.x4a, self.x4b, self.x4c, self.x4d = [],[],[],[],[]
+ self.x41,self.x42,self.x43,self.x44,self.x45 = [],[],[],[],[]
+ # 根据操作系统初始化 CAN 总线
+ if sys.platform == "linux":
+ self.bus = can.interface.Bus(
+ channel=can_channel, interface="socketcan", bitrate=baudrate,
+ can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+ )
+ elif sys.platform == "win32":
+ self.bus = can.interface.Bus(
+ channel=can_channel, interface='pcan', bitrate=baudrate,
+ can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+ )
+ else:
+ raise EnvironmentError("Unsupported platform for CAN interface")
+
+ # 根据 can_id 初始化 publisher 和相关参数
+ if can_id == 0x28: # 左手
+ self.hand_exists = config['LINKER_HAND']['LEFT_HAND']['EXISTS']
+ self.hand_joint = config['LINKER_HAND']['LEFT_HAND']['JOINT']
+ self.hand_names = config['LINKER_HAND']['LEFT_HAND']['NAME']
+ elif can_id == 0x27: # 右手
+
+ self.hand_exists = config['LINKER_HAND']['RIGHT_HAND']['EXISTS']
+ self.hand_joint = config['LINKER_HAND']['RIGHT_HAND']['JOINT']
+ self.hand_names = config['LINKER_HAND']['RIGHT_HAND']['NAME']
+
+
+ # 启动接收线程
+ self.receive_thread = threading.Thread(target=self.receive_response)
+ self.receive_thread.daemon = True
+ self.receive_thread.start()
+
+ def send_command(self, frame_property, data_list):
+ """
+ 发送命令到 CAN 总线
+ :param frame_property: 数据帧属性
+ :param data_list: 数据载荷
+ """
+ frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ data = [frame_property_value] + [int(val) for val in data_list]
+ msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ try:
+ self.bus.send(msg)
+ #print(f"Message sent: ID={hex(self.can_id)}, Data={data}")
+ except can.CanError as e:
+ print(f"Failed to send message: {e}")
+ time.sleep(0.002)
+
+ def receive_response(self):
+ """
+ 接收并处理 CAN 总线的响应消息
+ """
+ while self.running:
+ try:
+ msg = self.bus.recv(timeout=1.0) # 阻塞接收，1 秒超时
+ if msg:
+ self.process_response(msg)
+ except can.CanError as e:
+ print(f"Error receiving message: {e}")
+
+
+ def set_joint_positions(self, joint_ranges):
+ if len(joint_ranges) == 25:
+ l24_pose = self.joint_map(joint_ranges)
+ # 使用列表推导式将列表每6个元素切成一个子数组
+ chunks = [l24_pose[i:i+6] for i in range(0, 30, 6)]
+ self.send_command(FrameProperty.THUMB_POS, chunks[0])
+ self.send_command(FrameProperty.INDEX_POS, chunks[1])
+ self.send_command(FrameProperty.MIDDLE_POS, chunks[2])
+ self.send_command(FrameProperty.RING_POS, chunks[3])
+ self.send_command(FrameProperty.LITTLE_POS, chunks[4])
+ #self.set_tip_positions(joint_ranges[:5])
+ #print(l24_pose)
+
+ # 设置所有手指横滚关节位置
+ def set_roll_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROLL_POS, joint_ranges)
+ # 设置所有手指航向关节位置
+ def set_yaw_positions(self, joint_ranges):
+ self.send_command(FrameProperty.YAW_POS, joint_ranges)
+ # 设置所有手指指根1关节位置
+ def set_root1_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROOT1_POS, joint_ranges)
+ # 设置所有手指指根2关节位置
+ def set_root2_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROOT2_POS, joint_ranges)
+ # 设置所有手指指根3关节位置
+ def set_root3_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROOT3_POS, joint_ranges)
+ # 设置所有手指指尖关节位置
+ def set_tip_positions(self, joint_ranges=[80]*5):
+ self.send_command(FrameProperty.TIP_POS, joint_ranges)
+ # 获取大拇指指关节位置
+ def get_thumb_positions(self,j=[0]):
+ self.send_command(FrameProperty.THUMB_POS, j)
+ # 获取食指关节位置
+ def get_index_positions(self, j=[0]):
+ self.send_command(FrameProperty.INDEX_POS,j)
+ # 获取中指关节位置
+ def get_middle_positions(self, j=[0]):
+ self.send_command(FrameProperty.MIDDLE_POS,j)
+ # 获取无名指关节位置
+ def get_ring_positions(self, j=[0]):
+ self.send_command(FrameProperty.RING_POS,j)
+ # 获取小拇指关节位置
+ def get_little_positions(self, j=[0]):
+ self.send_command(FrameProperty.LITTLE_POS, j)
+ # 失能01模式
+ def set_disability_mode(self, j=[1,1,1,1,1]):
+ self.send_command(0x85,j)
+ # 使能00模式
+ def set_enable_mode(self, j=[00,00,00,00,00]):
+ self.send_command(0x85,j)
+
+
+ def set_speed(self, speed):
+ self.speed = [speed]*6
+ ColorMsg(msg=f"L24设置速度为:{self.speed}", color="yellow")
+ self.send_command(FrameProperty.THUMB_SPEED, self.speed)
+ self.send_command(FrameProperty.INDEX_SPEED, self.speed)
+ self.send_command(FrameProperty.MIDDLE_SPEED, self.speed)
+ self.send_command(FrameProperty.RING_SPEED, self.speed)
+ self.send_command(FrameProperty.LITTLE_SPEED, self.speed)
+
+ def set_finger_torque(self, torque):
+ self.send_command(0x42, torque)
+
+ def request_device_info(self):
+ self.send_command(0xC0, [0])
+ self.send_command(0xC1, [0])
+ self.send_command(0xC2, [0])
+
+ def save_parameters(self):
+ self.send_command(0xCF, [])
+ def process_response(self, msg):
+ if msg.arbitration_id == self.can_id:
+ frame_type = msg.data[0]
+ response_data = msg.data[1:]
+ if frame_type == 0x01:
+ self.x01 = list(response_data)
+ elif frame_type == 0x02:
+ self.x02 = list(response_data)
+ elif frame_type == 0x03:
+ self.x03 = list(response_data)
+ elif frame_type == 0x04:
+ self.x04 = list(response_data)
+ elif frame_type == 0x05:
+ self.x05 = list(response_data)
+ elif frame_type == 0x06:
+ self.x06 = list(response_data)
+ print("_-"*20)
+ print(self.x06)
+ elif frame_type == 0xC0:
+ print(f"Device ID info: {response_data}")
+ if self.can_id == 0x28:
+ self.right_hand_info = response_data
+ elif self.can_id == 0x27:
+ self.left_hand_info = response_data
+ elif frame_type == 0x08:
+ self.x08 = list(response_data)
+ elif frame_type == 0x09:
+ self.x09 = list(response_data)
+ elif frame_type == 0x0A:
+ self.x0A = list(response_data)
+ elif frame_type == 0x0B:
+ self.x0B = list(response_data)
+ elif frame_type == 0x0C:
+ self.x0C = list(response_data)
+ elif frame_type == 0x0D:
+ self.x0D = list(response_data)
+ elif frame_type == 0x22:
+ #ColorMsg(msg=f"五指切向压力方向：{list(response_data)}")
+ d = list(response_data)
+ self.tangential_force_dir = [float(i) for i in d]
+ elif frame_type == 0x23:
+ #ColorMsg(msg=f"五指接近度：{list(response_data)}")
+ d = list(response_data)
+ self.approach_inc = [float(i) for i in d]
+ elif frame_type == 0x41: # 拇指关节位置返回值
+ self.x41 = list(response_data)
+ elif frame_type == 0x42: # 食指关节位置返回值
+ self.x42 = list(response_data)
+ elif frame_type == 0x43: # 中指关节位置返回值
+ self.x43 = list(response_data)
+ elif frame_type == 0x44: # 无名指关节位置返回值
+ self.x44 = list(response_data)
+ elif frame_type == 0x45: # 小拇指关节位置返回值
+ self.x45 = list(response_data)
+ elif frame_type == 0x49: # 拇指速度返回值
+ self.x49 = list(response_data)
+ elif frame_type == 0x4a: # 食指速度返回值
+ self.x4a = list(response_data)
+ elif frame_type == 0x4b: # 中指速度返回值
+ self.x4b = list(response_data)
+ elif frame_type == 0x4c: # 无名指速度返回值
+ self.x4c = list(response_data)
+ elif frame_type == 0x4d: # 小拇指速度返回值
+ self.x4d = list(response_data)
+
+ # topic映射L24
+ def joint_map(self, pose):
+ # L24 CAN数据默认接收30个数据
+ l24_pose = [0.0] * 30 # 初始化l24_pose为30个0.0
+
+ # 映射表，通过字典简化映射关系
+ mapping = {
+ 0: 10, 1: 5, 2: 0, 3: 15, 4: None, 5: 20,
+ 6: None, 7: 6, 8: 1, 9: 16, 10: None, 11: 21,
+ 12: None, 13: None, 14: 2, 15: 17, 16: None, 17: 22,
+ 18: None, 19: 8, 20: 3, 21: 18, 22: None, 23: 23,
+ 24: None, 25: 9, 26: 4, 27: 19, 28: None, 29: 24
+ }
+
+ # 遍历映射字典，进行值的映射
+ for l24_idx, pose_idx in mapping.items():
+ if pose_idx is not None:
+ l24_pose[l24_idx] = pose[pose_idx]
+
+ return l24_pose
+
+ # 将L24的状态值转换为CMD格式的状态值
+ def state_to_cmd(self, l24_state):
+ # L24 CAN默认接收30个数据，初始化pose为25个0.0
+ pose = [0.0] * 25 # 原来控制L24的指令数据为25个
+
+ # 映射关系，字典中存储l24_state索引和pose索引之间的映射关系
+ mapping = {
+ 0: 10, 1: 5, 2: 0, 3: 15, 5: 20, 7: 6,
+ 8: 1, 9: 16, 11: 21, 14: 2, 15: 17, 17: 22,
+ 19: 8, 20: 3, 21: 18, 23: 23, 25: 9, 26: 4,
+ 27: 19, 29: 24
+ }
+ # 遍历映射字典，更新pose的值
+ for l24_idx, pose_idx in mapping.items():
+ pose[pose_idx] = l24_state[l24_idx]
+ return pose
+
+ # 获取所有关节数据
+ def get_current_status(self, j=''):
+ time.sleep(0.01)
+ self.send_command(FrameProperty.THUMB_POS, j)
+ self.send_command(FrameProperty.INDEX_POS,j)
+ self.send_command(FrameProperty.MIDDLE_POS,j)
+ self.send_command(FrameProperty.RING_POS,j)
+ self.send_command(FrameProperty.LITTLE_POS, j)
+ #return self.x41, self.x42, self.x43, self.x44, self.x45
+ time.sleep(0.1)
+ state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45
+ if len(state) == 30:
+ l24_state = self.state_to_cmd(l24_state=state)
+ return l24_state
+
+ def get_speed(self,j=''):
+ time.sleep(0.1)
+ self.send_command(FrameProperty.THUMB_SPEED, j) # 大拇指速度
+ self.send_command(FrameProperty.INDEX_SPEED, j) # 食指速度
+ self.send_command(FrameProperty.MIDDLE_SPEED, j) # 中指速度
+ self.send_command(FrameProperty.RING_SPEED, j) # 无名指速度
+ self.send_command(FrameProperty.LITTLE_SPEED, j) # 小拇指速度
+ speed = self.x49+ self.x4a+ self.x4b+ self.x4c+ self.x4d
+ if len(speed) == 30:
+ l24_speed = self.state_to_cmd(l24_state=speed)
+ return l24_speed
+
+ def get_finger_torque(self):
+ return self.finger_torque
+ # def get_current(self):
+ # return self.x06
+ # def get_fault(self):
+ # return self.x07
+
+ def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+ """L24 暂不支持清除故障码"""
+ pass
+
+ def close_can_interface(self):
+ if self.bus:
+ self.bus.shutdown() # 关闭 CAN 总线
+
+ '''
+ 这个方法只用于展示数据关系映射，使用的话最好使用上面的方法
+ '''
+ def joint_map_2(self, pose):
+ l24_pose = [0.0]*30 #L24 CAN默认接收30个数据 pose控制L24发送的指令数据默认25个，这里进行映射
+ '''
+ 需要进行映射
+ # L24 CAN数据格式
+ #["拇指横摆0-10", "拇指侧摆1-5", "拇指根部2-0", "拇指中部3-15", "预留4-", "拇指指尖5-20", "预留6-", "食指侧摆7-6", "食指根部8-1", "食指中部9-16", "预留10-", "食指指尖11-21", "预留12-", "预留13-", "中指根部14-2", "中指中部15-17", "预留16-", "中指指尖17-22", "预留18-", "无名指侧摆19-8", "无名指根部20-3", "无名指中部21-18", "预留22-", "无名指指尖23-23", "预留24-", "小指侧摆25-9", "小指根部26-4", "小指中部27-19", "预留28-", "小指指尖29-24"]
+ # CMD 接收到的数据格式
+ #["拇指根部0", "食指根部1", "中指根部2", "无名指根部3","小指根部4","拇指侧摆5","食指侧摆6","中指侧摆","无名指侧摆8","小指侧摆9","拇指横摆10","预留","预留","预留","预留","拇指中部15","食指中部16","中指中部17","无名指中部18","小指中部19","拇指指尖20","食指指尖21","中指指尖22","无名指指尖23","小指指尖24"]
+ '''
+ l24_pose[0] = pose[10]
+ l24_pose[1] = pose[5]
+ l24_pose[2] = pose[0]
+ l24_pose[3] = pose[15]
+ l24_pose[4] = 0.0
+ l24_pose[5] = pose[20]
+ l24_pose[6] = 0.0
+ l24_pose[7] = pose[6]
+ l24_pose[8] = pose[1]
+ l24_pose[9] = pose[16]
+ l24_pose[10] = 0.0
+ l24_pose[11] = pose[21]
+ l24_pose[12] = 0.0
+ l24_pose[13] = 0.0
+ l24_pose[14] = pose[2]
+ l24_pose[15] = pose[17]
+ l24_pose[16] = 0.0
+ l24_pose[17] = pose[22]
+ l24_pose[18] = 0.0
+ l24_pose[19] = pose[8]
+ l24_pose[20] = pose[3]
+ l24_pose[21] = pose[18]
+ l24_pose[22] = 0.0
+ l24_pose[23] = pose[23]
+ l24_pose[24] = 0.0
+ l24_pose[25] = pose[9]
+ l24_pose[26] = pose[4]
+ l24_pose[27] = pose[19]
+ l24_pose[28] = 0.0
+ l24_pose[29] = pose[24]
+ return l24_pose
+
+ def get_finger_order(self):
+ return []
+ def get_serial_number(self):
+ return [0] * 6
+ def show_fun_table(self):
+ pass
\ No newline at end of file
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l25_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l25_can.py
new file mode 100644
index 0000000..97f91b4
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l25_can.py
@@ -0,0 +1,848 @@
+#!/usr/bin/env python3
+import can
+import time,sys,os
+import threading
+import numpy as np
+from enum import Enum
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+current_dir = os.path.dirname(os.path.abspath(__file__))
+target_dir = os.path.abspath(os.path.join(current_dir, ".."))
+sys.path.append(target_dir)
+
+class FrameProperty(Enum):
+ INVALID_FRAME_PROPERTY = 0x00 # Invalid CAN frame property | No response
+ # Parallel command area
+ ROLL_POS = 0x01 # Roll joint position | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger [10,11,12,13,14]
+ YAW_POS = 0x02 # Yaw joint position | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger [5,6,7,8,9]
+ ROOT1_POS = 0x03 # Root1 joint position | The root joint closest to the palm [0,1,2,3,4]
+ ROOT2_POS = 0x04 # Root2 joint position | The root joint closest to the palm [15, 16,17,18,19]
+ ROOT3_POS = 0x05 # Root3 joint position | The root joint closest to the palm Not available
+ TIP_POS = 0x06 # Fingertip joint position | The root joint closest to the palm [20,21,22,23,24]
+
+ ROLL_SPEED = 0x09 # Roll joint speed | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+ YAW_SPEED = 0x0A # Yaw joint speed | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+ ROOT1_SPEED = 0x0B # Root1 joint speed | The root joint closest to the palm
+ ROOT2_SPEED = 0x0C # Root2 joint speed | The root joint closest to the palm
+ ROOT3_SPEED = 0x0D # Root3 joint speed | The root joint closest to the palm
+ TIP_SPEED = 0x0E # Fingertip joint speed | The root joint closest to the palm
+
+ ROLL_TORQUE = 0x11 # Roll joint torque | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+ YAW_TORQUE = 0x12 # Yaw joint torque | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+ ROOT1_TORQUE = 0x13 # Root1 joint torque | The root joint closest to the palm
+ ROOT2_TORQUE = 0x14 # Root2 joint torque | The root joint closest to the palm
+ ROOT3_TORQUE = 0x15 # Root3 joint torque | The root joint closest to the palm
+ TIP_TORQUE = 0x16 # Fingertip joint torque | The root joint closest to the palm
+
+ ROLL_FAULT = 0x19 # Roll joint fault code | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+ YAW_FAULT = 0x1A # Yaw joint fault code | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+ ROOT1_FAULT = 0x1B # Root1 joint fault code | The root joint closest to the palm
+ ROOT2_FAULT = 0x1C # Root2 joint fault code | The root joint closest to the palm
+ ROOT3_FAULT = 0x1D # Root3 joint fault code | The root joint closest to the palm
+ TIP_FAULT = 0x1E # Fingertip joint fault code | The root joint closest to the palm
+
+ ROLL_TEMPERATURE = 0x21 # Roll joint temperature | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+ YAW_TEMPERATURE = 0x22 # Yaw joint temperature | The coordinate system is built at the root of each finger, and the rotation angle is defined according to the straightened state of the finger
+ ROOT1_TEMPERATURE = 0x23 # Root1 joint temperature | The root joint closest to the palm
+ ROOT2_TEMPERATURE = 0x24 # Root2 joint temperature | The root joint closest to the palm
+ ROOT3_TEMPERATURE = 0x25 # Root3 joint temperature | The root joint closest to the palm
+ TIP_TEMPERATURE = 0x26 # Fingertip joint temperature | The root joint closest to the palm
+ # Parallel command area
+
+ # Serial command area
+ THUMB_POS = 0x41 # Thumb joint position | Returns this type of data
+ INDEX_POS = 0x42 # Index finger joint position | Returns this type of data
+ MIDDLE_POS = 0x43 # Middle finger joint position | Returns this type of data
+ RING_POS = 0x44 # Ring finger joint position | Returns this type of data
+ LITTLE_POS = 0x45 # Little finger joint position | Returns this type of data
+
+ THUMB_SPEED = 0x49 # Thumb speed | Returns this type of data
+ INDEX_SPEED = 0x4A # Index finger speed | Returns this type of data
+ MIDDLE_SPEED = 0x4B # Middle finger speed | Returns this type of data
+ RING_SPEED = 0x4C # Ring finger speed | Returns this type of data
+ LITTLE_SPEED = 0x4D # Little finger speed | Returns this type of data
+
+ THUMB_TORQUE = 0x51 # Thumb torque | Returns this type of data
+ INDEX_TORQUE = 0x52 # Index finger torque | Returns this type of data
+ MIDDLE_TORQUE = 0x53 # Middle finger torque | Returns this type of data
+ RING_TORQUE = 0x54 # Ring finger torque | Returns this type of data
+ LITTLE_TORQUE = 0x55 # Little finger torque | Returns this type of data
+
+ THUMB_FAULT = 0x59 # Thumb fault code | Returns this type of data
+ INDEX_FAULT = 0x5A # Index finger fault code | Returns this type of data
+ MIDDLE_FAULT = 0x5B # Middle finger fault code | Returns this type of data
+ RING_FAULT = 0x5C # Ring finger fault code | Returns this type of data
+ LITTLE_FAULT = 0x5D # Little finger fault code | Returns this type of data
+
+ THUMB_TEMPERATURE = 0x61 # Thumb temperature | Returns this type of data
+ INDEX_TEMPERATURE = 0x62 # Index finger temperature | Returns this type of data
+ MIDDLE_TEMPERATURE = 0x63 # Middle finger temperature | Returns this type of data
+ RING_TEMPERATURE = 0x64 # Ring finger temperature | Returns this type of data
+ LITTLE_TEMPERATURE = 0x65 # Little finger temperature | Returns this type of data
+ # Serial command area
+
+ # Merged command area, non-essential single control data of the same finger is merged
+ FINGER_SPEED = 0x81 # Finger speed | Returns this type of data
+ FINGER_TORQUE = 0x82 # Torque | Returns this type of data
+ FINGER_FAULT = 0x83 # Finger fault code | Returns this type of data
+
+ # Fingertip sensor data group
+ HAND_NORMAL_FORCE = 0x90 # Normal force of five fingers
+ HAND_TANGENTIAL_FORCE = 0x91 # Tangential force of five fingers
+ HAND_TANGENTIAL_FORCE_DIR = 0x92 # Tangential direction of five fingers
+ HAND_APPROACH_INC = 0x93 # Proximity sensing of five fingers
+
+ THUMB_ALL_DATA = 0x98 # All data of thumb
+ INDEX_ALL_DATA = 0x99 # All data of index finger
+ MIDDLE_ALL_DATA = 0x9A # All data of middle finger
+ RING_ALL_DATA = 0x9B # All data of ring finger
+ LITTLE_ALL_DATA = 0x9C # All data of little finger
+ # Action command ·ACTION
+ ACTION_PLAY = 0xA0 # Action
+
+ # Configuration command ·CONFIG
+ HAND_UID = 0xC0 # Device unique identifier
+ HAND_HARDWARE_VERSION = 0xC1 # Hardware version
+ HAND_SOFTWARE_VERSION = 0xC2 # Software version
+ HAND_COMM_ID = 0xC3 # Device id
+ HAND_FACTORY_RESET = 0xCE # Restore factory settings
+ HAND_SAVE_PARAMETER = 0xCF # Save parameters
+
+ WHOLE_FRAME = 0xF0 # Whole frame transmission | Returns one byte frame property + the entire structure for 485 and network transmission only
+
+class LinkerHandL25Can:
+ def __init__(self, can_channel='can0', baudrate=1000000, can_id=0x28,yaml=""):
+ self.can_id = can_id
+ self.can_channel = can_channel
+ self.baudrate = baudrate
+ self.open_can = OpenCan(load_yaml=yaml)
+
+ self.running = True
+ self.last_thumb_pos, self.last_index_pos,self.last_ring_pos,self.last_middle_pos, self.last_little_pos = None,None,None,None,None
+ self.x01, self.x02, self.x03, self.x04,self.x05,self.x06,self.x07, self.x08,self.x09,self.x0A,self.x0B,self.x0C,self.x0D,self.x0E,self.speed = [],[],[],[],[],[],[],[],[],[],[],[],[],[],[]
+ self.last_root1,self.last_yaw,self.last_roll,self.last_root2,self.last_tip = None,None,None,None,None
+ # 速度
+ self.x49, self.x4a, self.x4b, self.x4c, self.x4d,self.xc1 = [],[],[],[],[],[]
+ self.x41,self.x42,self.x43,self.x44,self.x45 = [],[],[],[],[]
+ # 扭矩
+ self.x51, self.x52, self.x53, self.x54,self.x55 = [],[],[],[],[]
+ # 故障码
+ self.x59,self.x5a,self.x5b,self.x5c,self.x5d = [],[],[],[],[]
+ # 温度阈值
+ self.x61,self.x62,self.x63,self.x64,self.x65 = [],[],[],[],[]
+ # 压感
+ self.x90,self.x91,self.x92,self.x93 = [],[],[],[]
+ # 新压感
+ self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+ self.thumb_matrix = np.full((12, 6), -1)
+ self.index_matrix = np.full((12, 6), -1)
+ self.middle_matrix = np.full((12, 6), -1)
+ self.ring_matrix = np.full((12, 6), -1)
+ self.little_matrix = np.full((12, 6), -1)
+ self.matrix_map = {
+ 0: 0,
+ 16: 1,
+ 32: 2,
+ 48: 3,
+ 64: 4,
+ 80: 5,
+ 96: 6,
+ 112: 7,
+ 128: 8,
+ 144: 9,
+ 160: 10,
+ 176: 11,
+ }
+ # 根据操作系统初始化 CAN 总线
+ # try:
+ # if sys.platform == "linux":
+ # self.open_can.open_can(self.can_channel)
+ # time.sleep(0.1)
+ # self.bus = can.interface.Bus(
+ # channel=can_channel, interface="socketcan", bitrate=baudrate,
+ # can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+ # )
+ # elif sys.platform == "win32":
+ # self.bus = can.interface.Bus(
+ # channel=can_channel, interface='pcan', bitrate=baudrate,
+ # can_filters=[{"can_id": can_id, "can_mask": 0x7FF}]
+ # )
+ # else:
+ # raise EnvironmentError("Unsupported platform for CAN interface")
+ # except:
+ # print("Please insert CAN device")
+ self.bus = self.init_can_bus(channel=self.can_channel, baudrate=baudrate)
+ # 启动接收线程
+ self.receive_thread = threading.Thread(target=self.receive_response)
+ self.receive_thread.daemon = True
+ self.receive_thread.start()
+
+ def init_can_bus(self, channel, baudrate):
+ """
+ 尝试按优先级连接 CAN 总线，并实现回退机制。
+ """
+ # --- 统一异常处理块开始 ---
+ try:
+ if sys.platform == "linux":
+ # Linux 优先级：1. socketcan
+ try:
+ self.open_can.open_can(self.can_channel)
+ # 尝试 socketcan
+ bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+ ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+ raise # 重新抛出异常，让外层 try 捕获
+ elif sys.platform == "win32":
+ # Windows 优先级：1. pcan
+ try:
+ bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+ # Windows 优先级：2. candle (回退方法)
+ try:
+ bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+ raise # 两个接口都失败，抛出异常
+ else:
+ raise EnvironmentError("Unsupported platform for CAN interface")
+ # --- 统一异常处理块结束 ---
+ except Exception as e:
+ # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+ ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+ # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+ raise
+
+ def send_command(self, frame_property, data_list):
+ """
+ Send command to CAN bus
+ :param frame_property: Data frame properties
+ :param data_list: Data payload
+ """
+ frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ data = [frame_property_value] + [int(val) for val in data_list]
+ msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ try:
+ self.bus.send(msg)
+ #print(f"Message sent: ID={hex(self.can_id)}, Data={data}")
+ except can.CanError as e:
+ print(f"Failed to send message: {e}")
+ self.open_can.open_can(self.can_channel)
+ time.sleep(1)
+ self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+ time.sleep(1)
+ if self.is_can:
+ self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+ else:
+ print("Reconnecting CAN devices ....")
+ time.sleep(0.001)
+
+ def receive_response(self):
+ """
+ Receive and process response messages from CAN bus
+ """
+ while self.running:
+ try:
+ msg = self.bus.recv(timeout=1.0) # 阻塞接收，1 秒超时
+ if msg:
+ self.process_response(msg)
+ except can.CanError as e:
+ print(f"Error receiving message: {e}")
+
+
+ def set_joint_positions(self, joint_ranges):
+ if len(joint_ranges) == 25:
+ l25_pose = self.joint_map(joint_ranges)
+ # 使用列表推导式将列表每6个元素切成一个子数组
+ chunks = [l25_pose[i:i+6] for i in range(0, 30, 6)]
+ self.send_command(FrameProperty.THUMB_POS, chunks[0])
+ #time.sleep(0.001)
+ self.send_command(FrameProperty.INDEX_POS, chunks[1])
+ #time.sleep(0.001)
+ self.send_command(FrameProperty.MIDDLE_POS, chunks[2])
+ #time.sleep(0.001)
+ self.send_command(FrameProperty.RING_POS, chunks[3])
+ #time.sleep(0.001)
+ self.send_command(FrameProperty.LITTLE_POS, chunks[4])
+ #time.sleep(0.001)
+
+ def set_joint_positions_by_topic(self, joint_ranges):
+ if len(joint_ranges) == 25:
+ # Finger Joint Position Constants
+ #ROLL_POS = 0x01 # Roll joint position | Coordinate system based on finger base, rotation angle defined when finger is straight [10,11,12,13,14]
+ #YAW_POS = 0x02 # Yaw joint position | Coordinate system based on finger base, rotation angle defined when finger is straight [5,6,7,8,9]
+ #ROOT1_POS = 0x03 # Root1 joint position | Joint closest to the palm [0,1,2,3,4]
+ #ROOT2_POS = 0x04 # Root2 joint position | Joint closest to the palm [15,16,17,18,19]
+ #ROOT3_POS = 0x05 # Root3 joint position | Joint closest to the palm (currently unused)
+ #TIP_POS = 0x06 # Tip joint position | Joint closest to the palm [20,21,22,23,24]
+
+ # Finger joint names mapping (Chinese to English translation):
+ # ["Thumb root", "Index root", "Middle root", "Ring root", "Pinky root",
+ # "Thumb yaw", "Index yaw", "Middle yaw", "Ring yaw", "Pinky yaw",
+ # "Thumb roll", "Reserved", "Reserved", "Reserved", "Reserved",
+ # "Thumb middle", "Index middle", "Middle middle", "Ring middle", "Pinky middle",
+ # "Thumb tip", "Index tip", "Middle tip", "Ring tip", "Pinky tip"]
+
+
+ l25_pose = self.slice_list(joint_ranges,5)
+ if self._list_d_value(self.last_root1, l25_pose[0]):
+ self.set_root1_positions(l25_pose[0])
+ self.last_root1 = l25_pose[0]
+ if self._list_d_value(self.last_yaw, l25_pose[1]):
+ self.set_yaw_positions(l25_pose[1])
+ self.last_yaw = l25_pose[1]
+ if self._list_d_value(self.last_roll, l25_pose[2]):
+ self.set_roll_positions(l25_pose[2])
+ self.last_roll = l25_pose[2]
+ if self._list_d_value(self.last_root2, l25_pose[3]):
+ self.set_root2_positions(l25_pose[3])
+ self.last_root2 = l25_pose[3]
+ if self._list_d_value(self.last_tip, l25_pose[4]):
+ self.set_tip_positions(l25_pose[4])
+ self.last_tip = l25_pose[4]
+
+
+ def slice_list(self, input_list, slice_size):
+ """
+ Split a list into chunks of specified size.
+
+ Parameters:
+ input_list (list): The list to be chunked.
+ slice_size (int): Number of elements in each chunk.
+
+ Returns:
+ list of lists: The chunked list.
+ """
+ # Implementation using list comprehension
+ sliced_list = [input_list[i:i + slice_size] for i in range(0, len(input_list), slice_size)]
+ return sliced_list
+
+ def _list_d_value(self,list1, list2):
+ if list1 == None:
+ return True
+ for a, b in zip(list1, list2):
+ if abs(b - a) > 2:
+ return True
+ break
+ return False
+ # Set roll joint positions for all fingers
+ def set_roll_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROLL_POS, joint_ranges)
+ # Set yaw joint positions for all fingers
+ def set_yaw_positions(self, joint_ranges):
+ print(joint_ranges)
+ self.send_command(FrameProperty.YAW_POS, joint_ranges)
+ # Set base joint 1 positions for all fingers
+ def set_root1_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROOT1_POS, joint_ranges)
+ # Set base joint 2 positions for all fingers
+ def set_root2_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROOT2_POS, joint_ranges)
+ # Set base joint 3 positions for all fingers
+ def set_root3_positions(self, joint_ranges):
+ self.send_command(FrameProperty.ROOT3_POS, joint_ranges)
+ # Set fingertip joint positions for all fingers
+ def set_tip_positions(self, joint_ranges=[80]*5):
+ self.send_command(FrameProperty.TIP_POS, joint_ranges)
+ # Set thumb torque parameters
+ def set_thumb_torque(self, j=[]):
+ self.send_command(FrameProperty.THUMB_TORQUE, j)
+ # Set index finger torque
+ def set_index_torque(self, j=[]):
+ self.send_command(FrameProperty.INDEX_TORQUE, j)
+ # Set middle finger torque
+ def set_middle_torque(self, j=[]):
+ self.send_command(FrameProperty.MIDDLE_TORQUE, j)
+ # Set ring finger torque
+ def set_ring_torque(self, j=[]):
+ self.send_command(FrameProperty.RING_TORQUE, j)
+ # Set little finger torque
+ def set_little_torque(self, j=[]):
+ self.send_command(FrameProperty.LITTLE_TORQUE, j)
+
+ # Get thumb joint position
+ def get_thumb_positions(self,j=[0]):
+ self.send_command(FrameProperty.THUMB_POS, j)
+ # Get index finger joint positions
+ def get_index_positions(self, j=[0]):
+ self.send_command(FrameProperty.INDEX_POS,j)
+ # Get middle finger joint position
+ def get_middle_positions(self, j=[0]):
+ self.send_command(FrameProperty.MIDDLE_POS,j)
+ # Retrieve the position of the ring finger joint
+ def get_ring_positions(self, j=[0]):
+ self.send_command(FrameProperty.RING_POS,j)
+ # Retrieve the position of the little finger joint
+ def get_little_positions(self, j=[0]):
+ self.send_command(FrameProperty.LITTLE_POS, j)
+ # All fault codes of motors in the thumb
+ def get_thumbn_fault(self,j=[]):
+ self.send_command(FrameProperty.THUMB_FAULT,j)
+ # All motor fault codes for the index finger
+ def get_index_fault(self,j=[]):
+ self.send_command(FrameProperty.INDEX_FAULT,j)
+ # All motor fault codes for the middle finger
+ def get_middle_fault(self,j=[]):
+ self.send_command(FrameProperty.MIDDLE_FAULT,j)
+ # All motor fault codes for the ring finger
+ def get_ring_fault(self,j=[]):
+ self.send_command(FrameProperty.RING_FAULT,j)
+ # All motor fault codes for the little finger
+ def get_little_fault(self,j=[]):
+ self.send_command(FrameProperty.LITTLE_FAULT,j)
+ # Temperature threshold for the thumb motors
+ def get_thumb_threshold(self,j=[]):
+ self.send_command(FrameProperty.THUMB_TEMPERATURE, '')
+ # Temperature threshold for the index finger motors
+ def get_index_threshold(self,j=[]):
+ self.send_command(FrameProperty.INDEX_TEMPERATURE, j)
+ # Temperature threshold for the middle finger motors
+ def get_middle_threshold(self,j=[]):
+ self.send_command(FrameProperty.MIDDLE_TEMPERATURE, j)
+ # Temperature threshold for the ring finger motors
+ def get_ring_threshold(self,j=[]):
+ self.send_command(FrameProperty.RING_TEMPERATURE, j)
+ # Little finger temperature threshold
+ def get_little_threshold(self,j=[]):
+ self.send_command(FrameProperty.LITTLE_TEMPERATURE, j)
+
+
+ def set_disability_mode(self, j=[1,1,1,1,1]):
+ self.send_command(0x85,j)
+
+ def set_enable_mode(self, j=[00,00,00,00,00]):
+ self.send_command(0x85,j)
+
+ # Set torque for all fingers
+ def set_torque(self,torque=[250]*5):
+ t = torque[0]
+ i = torque[1]
+ m = torque[2]
+ r = torque[3]
+ l = torque[4]
+ self.set_thumb_torque(j=[t]*5)
+ self.set_index_torque(j=[i]*5)
+ self.set_middle_torque(j=[m]*5)
+ self.set_ring_torque(j=[r]*5)
+ self.set_little_torque(j=[l]*5)
+
+ def set_speed(self, speed):
+ self.speed = speed
+ if len(speed) < 25:
+ thumb_speed = [self.speed[0]]*5
+ index_speed = [self.speed[1]]*5
+ middle_speed = [self.speed[2]]*5
+ ring_speed = [self.speed[3]]*5
+ little_speed = [self.speed[4]]*5
+ else:
+ thumb_speed = [self.speed[0],self.speed[1],self.speed[2],self.speed[3],self.speed[4]]
+ index_speed = [self.speed[5],self.speed[6],self.speed[7],self.speed[8],self.speed[9]]
+ middle_speed = [self.speed[10],self.speed[11],self.speed[12],self.speed[13],self.speed[14]]
+ ring_speed = [self.speed[15],self.speed[16],self.speed[17],self.speed[18],self.speed[19]]
+ little_speed = [self.speed[20],self.speed[21],self.speed[22],self.speed[23],self.speed[24]]
+ self.send_command(FrameProperty.THUMB_SPEED, thumb_speed)
+ self.send_command(FrameProperty.INDEX_SPEED, index_speed)
+ self.send_command(FrameProperty.MIDDLE_SPEED, middle_speed)
+ self.send_command(FrameProperty.RING_SPEED, ring_speed)
+ self.send_command(FrameProperty.LITTLE_SPEED, little_speed)
+
+ def set_finger_torque(self, torque):
+ self.send_command(0x42, torque)
+
+ def request_device_info(self):
+ self.send_command(0xC0, [0])
+ self.send_command(0xC1, [0])
+ self.send_command(0xC2, [0])
+
+ def save_parameters(self):
+ self.send_command(0xCF, [])
+ def process_response(self, msg):
+ if msg.arbitration_id == self.can_id:
+ frame_type = msg.data[0]
+ response_data = msg.data[1:]
+ if len(list(response_data)) == 0:
+ return
+ if frame_type == 0x01:
+ self.x01 = list(response_data)
+ elif frame_type == 0x02:
+ self.x02 = list(response_data)
+ elif frame_type == 0x03:
+ self.x03 = list(response_data)
+ elif frame_type == 0x04:
+ self.x04 = list(response_data)
+ elif frame_type == 0x05:
+ self.x05 = list(response_data)
+ elif frame_type == 0x06:
+ self.x06 = list(response_data)
+ elif frame_type == 0xC0:
+ print(f"Device ID info: {response_data}")
+ if self.can_id == 0x28:
+ self.right_hand_info = response_data
+ elif self.can_id == 0x27:
+ self.left_hand_info = response_data
+ elif frame_type == 0x08:
+ self.x08 = list(response_data)
+ elif frame_type == 0x09:
+ self.x09 = list(response_data)
+ elif frame_type == 0x0A:
+ self.x0A = list(response_data)
+ elif frame_type == 0x0B:
+ self.x0B = list(response_data)
+ elif frame_type == 0x0C:
+ self.x0C = list(response_data)
+ elif frame_type == 0x0D:
+ self.x0D = list(response_data)
+ elif frame_type == 0x22:
+ d = list(response_data)
+ self.tangential_force_dir = [float(i) for i in d]
+ elif frame_type == 0x23:
+ d = list(response_data)
+ self.approach_inc = [float(i) for i in d]
+ elif frame_type == 0x41:
+ self.x41 = list(response_data)
+ elif frame_type == 0x42:
+
+ self.x42 = list(response_data)
+ elif frame_type == 0x43:
+ self.x43 = list(response_data)
+ elif frame_type == 0x44:
+
+ self.x44 = list(response_data)
+ elif frame_type == 0x45:
+ self.x45 = list(response_data)
+ elif frame_type == 0x49:
+ self.x49 = list(response_data)
+ elif frame_type == 0x4a:
+ self.x4a = list(response_data)
+ elif frame_type == 0x4b:
+ self.x4b = list(response_data)
+ elif frame_type == 0x4c:
+ self.x4c = list(response_data)
+ elif frame_type == 0x4d:
+ self.x4d = list(response_data)
+ elif frame_type == 0xc1:
+ self.xc1 = list(response_data)
+ elif frame_type == 0x51:
+ self.x51 = list(response_data)
+ elif frame_type == 0x52:
+ self.x52 = list(response_data)
+ elif frame_type == 0x53:
+ self.x53 = list(response_data)
+ elif frame_type == 0x54:
+ self.x54 = list(response_data)
+ elif frame_type == 0x55:
+ self.x55 = list(response_data)
+ elif frame_type == 0x59:
+ self.x59 = list(response_data)
+ elif frame_type == 0x5a:
+ self.x5a = list(response_data)
+ elif frame_type == 0x5b:
+ self.x5b = list(response_data)
+ elif frame_type == 0x5c:
+ self.x5c = list(response_data)
+ elif frame_type == 0x5d:
+ self.x5d = list(response_data)
+ elif frame_type == 0x61:
+ self.x61 = list(response_data)
+ elif frame_type == 0x62:
+ self.x62 = list(response_data)
+ elif frame_type == 0x63:
+ self.x63 = list(response_data)
+ elif frame_type == 0x64:
+ self.x64 = list(response_data)
+ elif frame_type == 0x65:
+ self.x65 = list(response_data)
+ elif frame_type == 0x90:
+ self.x90 = list(response_data)
+ elif frame_type == 0x91:
+ self.x91 = list(response_data)
+ elif frame_type == 0x92:
+ self.x92 = list(response_data)
+ elif frame_type == 0x93:
+ self.x93 = list(response_data)
+ elif frame_type == 0xb0:
+ self.xb0 = list(response_data)
+ elif frame_type == 0xb1:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb1 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.thumb_matrix[index] = d[1:]
+ elif frame_type == 0xb2:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb2 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.index_matrix[index] = d[1:]
+ elif frame_type == 0xb3:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb3 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.middle_matrix[index] = d[1:]
+ elif frame_type == 0xb4:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb4 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.ring_matrix[index] = d[1:]
+ elif frame_type == 0xb5:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb5 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.little_matrix[index] = d[1:]
+
+
+ def joint_map(self, pose):
+ l25_pose = [0.0] * 30
+
+ # 映射表，通过字典简化映射关系
+ mapping = {
+ 0: 10, 1: 5, 2: 0, 3: 15, 4: None, 5: 20,
+ 6: None, 7: 6, 8: 1, 9: 16, 10: None, 11: 21,
+ 12: None, 13: 7, 14: 2, 15: 17, 16: None, 17: 22,
+ 18: None, 19: 8, 20: 3, 21: 18, 22: None, 23: 23,
+ 24: None, 25: 9, 26: 4, 27: 19, 28: None, 29: 24
+ }
+
+ # 遍历映射字典，进行值的映射
+ for l25_idx, pose_idx in mapping.items():
+ if pose_idx is not None:
+ l25_pose[l25_idx] = pose[pose_idx]
+
+ return l25_pose
+
+
+ def state_to_cmd(self, l25_state):
+
+ pose = [0.0] * 25
+ mapping = {
+ 0: 10, 1: 5, 2: 0, 3: 15, 5: 20, 7: 6,
+ 8: 1, 9: 16, 11: 21, 13:7, 14: 2, 15: 17, 17: 22,
+ 19: 8, 20: 3, 21: 18, 23: 23, 25: 9, 26: 4,
+ 27: 19, 29: 24
+ }
+ # 遍历映射字典，更新pose的值
+ for l25_idx, pose_idx in mapping.items():
+ pose[pose_idx] = l25_state[l25_idx]
+ return pose
+ def action_play(self):
+ self.send_command(0xA0,[])
+
+ def get_current_status(self, j=''):
+ self.send_command(FrameProperty.THUMB_POS, j)
+ #time.sleep(0.001)
+ self.send_command(FrameProperty.INDEX_POS,j)
+ #time.sleep(0.001)
+ self.send_command(FrameProperty.MIDDLE_POS,j)
+ #time.sleep(0.001)
+ self.send_command(FrameProperty.RING_POS,j)
+ #time.sleep(0.001)
+ self.send_command(FrameProperty.LITTLE_POS, j)
+ #time.sleep(0.001)
+ state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45
+ if len(state) == 30:
+ l25_state = self.state_to_cmd(l25_state=state)
+ return l25_state
+
+ def get_current_pub_status(self):
+ state= self.x41+ self.x42+ self.x43+ self.x44+ self.x45
+ if len(state) == 30:
+ l25_state = self.state_to_cmd(l25_state=state)
+ return l25_state
+
+ def get_current_state_topic(self):
+ self.send_command(0x01,[])
+ #time.sleep(0.001)
+ self.send_command(0x02,[])
+ # time.sleep(0.001)
+ self.send_command(0x03,[])
+ #time.sleep(0.001)
+ self.send_command(0x04,[])
+ #time.sleep(0.001)
+ self.send_command(0x06,[])
+ #time.sleep(0.001)
+ state = self.x03+self.x02+self.x01+self.x04+self.x06
+ return state
+ def get_speed(self,j=''):
+ self.send_command(FrameProperty.THUMB_SPEED, j)
+ #time.sleep(0.01)
+ self.send_command(FrameProperty.INDEX_SPEED, j)
+ #time.sleep(0.01)
+ self.send_command(FrameProperty.MIDDLE_SPEED, j)
+ #time.sleep(0.01)
+ self.send_command(FrameProperty.RING_SPEED, j)
+ #time.sleep(0.01)
+ self.send_command(FrameProperty.LITTLE_SPEED, j)
+ #time.sleep(0.01)
+ speed = self.x49+ self.x4a+ self.x4b+ self.x4c+ self.x4d
+ if len(speed) == 30:
+ l25_speed = self.state_to_cmd(l25_state=speed)
+ return l25_speed
+
+ def get_finger_torque(self):
+ self.send_command(FrameProperty.THUMB_TORQUE,[])
+ self.send_command(FrameProperty.INDEX_TORQUE,[])
+ self.send_command(FrameProperty.MIDDLE_TORQUE,[])
+ self.send_command(FrameProperty.RING_TORQUE,[])
+ self.send_command(FrameProperty.LITTLE_TORQUE,[])
+ return self.x51+self.x52+self.x53+self.x54+self.x55
+
+ def get_torque(self):
+ return self.get_finger_torque()
+ def get_fault(self):
+ self.get_thumbn_fault()
+ #time.sleep(0.001)
+ self.get_index_fault()
+ #time.sleep(0.001)
+ self.get_middle_fault()
+ #time.sleep(0.001)
+ self.get_ring_fault()
+ #time.sleep(0.001)
+ self.get_little_fault()
+ #time.sleep(0.001)
+ return [self.x59]+[self.x5a]+[self.x5b]+[self.x5c]+[self.x5d]
+ def get_threshold(self):
+ self.get_thumb_threshold()
+ self.get_index_threshold()
+ self.get_middle_threshold()
+ self.get_ring_threshold()
+ self.get_little_threshold()
+ return [self.x61]+[self.x62]+[self.x63]+[self.x64]+[self.x65]
+ def get_version(self):
+ if self.xc1 == []:
+ self.send_command(FrameProperty.HAND_HARDWARE_VERSION,[])
+ return self.xc1
+ def get_normal_force(self):
+ self.send_command(FrameProperty.HAND_NORMAL_FORCE,[])
+ return self.x90
+ def get_tangential_force(self):
+ self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE,[])
+ return self.x91
+ def get_tangential_force_dir(self):
+ self.send_command(FrameProperty.HAND_TANGENTIAL_FORCE_DIR,[])
+ return self.x92
+ def get_approach_inc(self):
+ self.send_command(FrameProperty.HAND_APPROACH_INC,[])
+ return self.x93
+ def get_force(self):
+ '''获取压感数据'''
+ return [self.x90,self.x91 , self.x92 , self.x93]
+
+ def get_matrix_touch(self):
+ self.send_command(0xb1,[0xc6])
+ time.sleep(0.03)
+ self.send_command(0xb2,[0xc6])
+ time.sleep(0.03)
+ self.send_command(0xb3,[0xc6])
+ time.sleep(0.03)
+ self.send_command(0xb4,[0xc6])
+ time.sleep(0.03)
+ self.send_command(0xb5,[0xc6])
+ time.sleep(0.03)
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_touch_type(self):
+ '''Get touch type'''
+ self.send_command(0xb1,[])
+ time.sleep(0.03)
+ if len(self.xb1) == 2:
+ return 2
+ else:
+ return -1
+
+ def get_touch(self):
+ '''Get touch data (not supported yet)'''
+ return [-1] * 6
+
+
+ def get_current(self):
+ return [0] * 21
+ def get_temperature(self):
+ self.get_thumb_threshold()
+ self.get_index_threshold()
+ self.get_middle_threshold()
+ self.get_ring_threshold()
+ self.get_little_threshold()
+ return [self.x61]+[self.x62]+[self.x63]+[self.x64]+[self.x65]
+
+ def get_finger_order(self):
+ return ["Thumb root", "Index root", "Middle root", "Ring root", "Little root",
+ "Thumb abduction", "Index abduction", "Middle abduction", "Ring abduction", "Little abduction",
+ "Thumb roll", "Reserved", "Reserved", "Reserved", "Reserved",
+ "Thumb middle", "Index middle", "Middle middle", "Ring middle", "Little middle",
+ "Thumb tip", "Index tip", "Middle tip", "Ring tip", "Little tip"]
+
+ def close_can_interface(self):
+ if self.bus:
+ self.bus.shutdown()
+ def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+ """L25 暂不支持清除故障码"""
+ pass
+ '''
+ 这个方法只用于展示数据关系映射，使用的话最好使用上面的方法
+ '''
+ def joint_map_2(self, pose):
+ l25_pose = [0.0]*30 #L25 CAN默认接收30个数据 pose控制L25发送的指令数据默认25个，这里进行映射
+ '''
+ 需要进行映射
+ # L25 CAN数据格式
+ #["拇指横摆0-10", "拇指侧摆1-5", "拇指根部2-0", "拇指中部3-15", "预留4-", "拇指指尖5-20", "预留6-", "食指侧摆7-6", "食指根部8-1", "食指中部9-16", "预留10-", "食指指尖11-21", "预留12-", "预留13-", "中指根部14-2", "中指中部15-17", "预留16-", "中指指尖17-22", "预留18-", "无名指侧摆19-8", "无名指根部20-3", "无名指中部21-18", "预留22-", "无名指指尖23-23", "预留24-", "小指侧摆25-9", "小指根部26-4", "小指中部27-19", "预留28-", "小指指尖29-24"]
+ # CMD 接收到的数据格式
+ #["拇指根部0", "食指根部1", "中指根部2", "无名指根部3","小指根部4","拇指侧摆5","食指侧摆6","中指侧摆","无名指侧摆8","小指侧摆9","拇指横摆10","预留","预留","预留","预留","拇指中部15","食指中部16","中指中部17","无名指中部18","小指中部19","拇指指尖20","食指指尖21","中指指尖22","无名指指尖23","小指指尖24"]
+ '''
+ l25_pose[0] = pose[10]
+ l25_pose[1] = pose[5]
+ l25_pose[2] = pose[0]
+ l25_pose[3] = pose[15]
+ l25_pose[4] = 0.0
+ l25_pose[5] = pose[20]
+ l25_pose[6] = 0.0
+ l25_pose[7] = pose[6]
+ l25_pose[8] = pose[1]
+ l25_pose[9] = pose[16]
+ l25_pose[10] = 0.0
+ l25_pose[11] = pose[21]
+ l25_pose[12] = 0.0
+ l25_pose[13] = 0.0
+ l25_pose[14] = pose[2]
+ l25_pose[15] = pose[17]
+ l25_pose[16] = 0.0
+ l25_pose[17] = pose[22]
+ l25_pose[18] = 0.0
+ l25_pose[19] = pose[8]
+ l25_pose[20] = pose[3]
+ l25_pose[21] = pose[18]
+ l25_pose[22] = 0.0
+ l25_pose[23] = pose[23]
+ l25_pose[24] = 0.0
+ l25_pose[25] = pose[9]
+ l25_pose[26] = pose[4]
+ l25_pose[27] = pose[19]
+ l25_pose[28] = 0.0
+ l25_pose[29] = pose[24]
+ return l25_pose
+
+ def get_serial_number(self):
+ return [0] * 6
+ def show_fun_table(self):
+ pass
\ No newline at end of file
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py
new file mode 100644
index 0000000..d5cca5d
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l6_can.py
@@ -0,0 +1,426 @@
+import can
+import time, sys
+import threading
+import numpy as np
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+
+
+class LinkerHandL6Can:
+ def __init__(self, can_id, can_channel='can0', baudrate=1000000,yaml=""):
+ self.can_id = can_id
+ self.can_channel = can_channel
+ self.baudrate = baudrate
+ self.open_can = OpenCan(load_yaml=yaml)
+
+ self.x01 = [0] * 6 # 关节位置
+ self.x02 = [-1] * 6 # 转矩限制
+ self.x05 = [0] * 6 # 速度
+ self.x07 = [-1] * 6 # 加速度
+ self.x33 = [0] * 6 # 温度
+ self.x35 = [0] * 6 # 关节错误码
+ self.x36 = [-1] * 6 # 电流
+ self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+
+ self.thumb_matrix = np.full((12, 6), -1)
+ self.index_matrix = np.full((12, 6), -1)
+ self.middle_matrix = np.full((12, 6), -1)
+ self.ring_matrix = np.full((12, 6), -1)
+ self.little_matrix = np.full((12, 6), -1)
+ self.matrix_map = {
+ 0: 0,
+ 16: 1,
+ 32: 2,
+ 48: 3,
+ 64: 4,
+ 80: 5,
+ 96: 6,
+ 112: 7,
+ 128: 8,
+ 144: 9,
+ 160: 10,
+ 176: 11,
+ }
+ self.serial_number = []
+ self.serial_number_map = {
+ 0: 0,
+ 1: 1,
+ 2: 2,
+ 3: 3,
+ }
+ # Fault codes
+
+ self.joint_angles = [0] * 6
+ self.pressures = [200] * 6 # Default torque 200
+ self.bus = self.init_can_bus(can_channel, baudrate)
+ self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 6 for _ in range(4)]
+ self.is_lock = False
+ self.version = None
+ # Start the receiving thread
+ self.running = True
+ self.receive_thread = threading.Thread(target=self.receive_response)
+ self.receive_thread.daemon = True
+ self.receive_thread.start()
+
+ def init_can_bus(self, channel, baudrate):
+ """
+ 尝试按优先级连接 CAN 总线，并实现回退机制。
+ """
+ # --- 统一异常处理块开始 ---
+ try:
+ if sys.platform == "linux":
+ # Linux 优先级：1. socketcan
+ try:
+ self.open_can.open_can(self.can_channel)
+ # 尝试 socketcan
+ bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+ ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+ raise # 重新抛出异常，让外层 try 捕获
+ elif sys.platform == "win32":
+ # Windows 优先级：1. pcan
+ try:
+ bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+ # Windows 优先级：2. candle (回退方法)
+ try:
+ bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+ raise # 两个接口都失败，抛出异常
+ else:
+ raise EnvironmentError("Unsupported platform for CAN interface")
+ # --- 统一异常处理块结束 ---
+ except Exception as e:
+ # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+ ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+ # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+ raise
+
+ def send_frame(self, frame_property, data_list,sleep=0.003):
+ """Send a single CAN frame with specified properties and data."""
+ frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ data = [frame_property_value] + [int(val) for val in data_list]
+ msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ try:
+ self.bus.send(msg)
+ except can.CanError as e:
+ print(f"Failed to send message: {e}")
+ self.open_can.open_can(self.can_channel)
+ time.sleep(1)
+ self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+ time.sleep(1)
+ if self.is_can:
+ self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+ else:
+ print("Reconnecting CAN devices ....")
+ time.sleep(sleep)
+
+ def set_joint_positions(self, joint_angles):
+ """Set the positions of 10 joints (joint_angles: list of 10 values)."""
+ if len(joint_angles) > 6:
+ self.joint_angles = joint_angles[:6]
+ else:
+ self.joint_angles = joint_angles
+ # Send angle control in frames
+ self.send_frame(0x01, self.joint_angles, sleep=0.003)
+
+ def set_max_torque_limits(self, pressures, type="get"):
+ """Set maximum torque limits."""
+ if type == "get":
+ self.pressures = [0.0]
+ else:
+ self.pressures = pressures[:6]
+
+ def set_torque(self, torque=[180] * 6):
+ """Set L6 maximum torque limits."""
+ if len(torque) != 6:
+ raise ValueError("Torque list must have 6 elements.")
+ return
+ self.send_frame(0x02, torque)
+
+ def set_speed(self, speed=[180] * 6):
+ """Set L6 speed."""
+ if len(speed) != 6:
+ raise ValueError("Speed list must have 6 elements.")
+ return
+ self.x05 = speed
+ for i in range(2):
+ time.sleep(0.001)
+ self.send_frame(0x05, speed)
+
+ ''' -------------------Pressure Sensors---------------------- '''
+ def get_normal_force(self):
+ self.send_frame(0x20, [],sleep=0.01)
+
+ def get_tangential_force(self):
+ self.send_frame(0x21, [],sleep=0.01)
+
+ def get_tangential_force_dir(self):
+ self.send_frame(0x22, [],sleep=0.01)
+
+ def get_approach_inc(self):
+ self.send_frame(0x23, [],sleep=0.01)
+
+ ''' -------------------Motor Temperature---------------------- '''
+ def get_motor_temperature(self):
+ self.send_frame(0x33, [])
+
+ # Motor fault codes
+ def get_motor_fault_code(self):
+ self.send_frame(0x35, [])
+
+ def receive_response(self):
+ """Receive CAN responses and process them."""
+ while self.running:
+ try:
+ msg = self.bus.recv(timeout=1.0)
+ if msg:
+ self.process_response(msg)
+ except can.CanError as e:
+ print(f"Error receiving CAN message: {e}")
+
+ def process_response(self, msg):
+ """Process received CAN messages."""
+ #if msg.arbitration_id == self.can_id:
+ if msg.arbitration_id in (self.can_id, self.can_id + 8):
+ try:
+ frame_type = msg.data[0]
+ response_data = msg.data[1:]
+ if len(list(response_data)) == 0:
+ return
+ except:
+ return
+ if frame_type == 0x01: # 0x01
+ self.x01 = list(response_data)
+ elif frame_type == 0x02: # 0x02
+ self.x02 = list(response_data)
+ elif frame_type == 0x05: # Set speed
+ self.x05 = list(response_data)
+ elif frame_type == 0x20:
+ d = list(response_data)
+ self.normal_force = [float(i) for i in d]
+ elif frame_type == 0x21:
+ d = list(response_data)
+ self.tangential_force = [float(i) for i in d]
+ elif frame_type == 0x22:
+ d = list(response_data)
+ self.tangential_force_dir = [float(i) for i in d]
+ elif frame_type == 0x23:
+ d = list(response_data)
+ self.approach_inc = [float(i) for i in d]
+ elif frame_type == 0x33: # L6 temperature
+ self.x33 = list(response_data)
+ elif frame_type == 0x35: # L6 fault codes
+ self.x35 = list(response_data)
+ elif frame_type == 0x36: # L6 电流
+ self.x36 = list(response_data)
+ elif frame_type == 0xb0:
+ self.xb0 = list(response_data)
+ elif frame_type == 0xb1:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb1 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.thumb_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb2:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb2 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.index_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb3:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb3 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.middle_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb4:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb4 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.ring_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb5:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb5 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.little_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0x64: # L6 version number
+ self.version = list(response_data)
+ elif frame_type == 0xC2: # L6 version number
+ self.version = list(response_data)
+ elif frame_type == 0xC0:
+ d = list(response_data)
+ index = self.serial_number_map.get(d[0])
+ if index is not None:
+ self.serial_number=self.serial_number + d[1:]
+ else:
+ self.serial_number=self.serial_number + [-1] * 6
+
+
+
+ def get_version(self):
+ self.send_frame(0x64, [],sleep=0.1)
+ time.sleep(0.1)
+ if self.version is None:
+ self.send_frame(0xC2, [],sleep=0.1)
+ time.sleep(0.1)
+ return self.version
+
+ def get_current_status(self):
+ self.send_frame(0x01, [],sleep=0.005)
+ return self.x01
+
+ def get_current_pub_status(self):
+ return self.x01
+
+ def get_speed(self):
+ #self.send_frame(0x05, [],sleep=0.003)
+ #print("L6暂不支持读取实时速度")
+ return [0] * 6
+
+ def get_current(self):
+ '''Not supported yet.'''
+ self.send_frame(0x36, [],sleep=0.005)
+ return self.x36
+
+
+
+ def get_torque(self):
+ '''Not supported yet.'''
+ self.send_frame(0x2, [],sleep=0.01)
+ return self.x02
+
+ def get_touch_type(self):
+ '''Get touch type'''
+ self.send_frame(0xb1,[])
+ t = []
+ for i in range(3):
+ t = self.xb1
+ time.sleep(0.01)
+ if len(t) == 2:
+ return 2
+ else:
+ self.send_frame(0x20,[],sleep=0.03)
+ time.sleep(0.01)
+ if self.normal_force[0] == -1:
+ return -1
+ else:
+ return 1
+
+
+ def get_touch(self):
+ '''Get touch data'''
+ self.send_frame(0xb1,[],sleep=0.03)
+ self.send_frame(0xb2,[],sleep=0.03)
+ self.send_frame(0xb3,[],sleep=0.03)
+ self.send_frame(0xb4,[],sleep=0.03)
+ self.send_frame(0xb5,[],sleep=0.03)
+ return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available
+
+ def get_matrix_touch(self):
+ self.send_frame(0xb1,[0xc6],sleep=0.01)
+ self.send_frame(0xb2,[0xc6],sleep=0.01)
+ self.send_frame(0xb3,[0xc6],sleep=0.01)
+ self.send_frame(0xb4,[0xc6],sleep=0.01)
+ self.send_frame(0xb5,[0xc6],sleep=0.01)
+
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_matrix_touch_v2(self):
+ self.send_frame(0xb1,[0xc6],sleep=0.009)
+ self.send_frame(0xb2,[0xc6],sleep=0.009)
+ self.send_frame(0xb3,[0xc6],sleep=0.009)
+ self.send_frame(0xb4,[0xc6],sleep=0.009)
+ self.send_frame(0xb5,[0xc6],sleep=0.009)
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_thumb_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb1,[0xc6],sleep=sleep_time)
+ return self.thumb_matrix
+
+ def get_index_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb2,[0xc6],sleep=sleep_time)
+ return self.index_matrix
+
+ def get_middle_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb3,[0xc6],sleep=sleep_time)
+ return self.middle_matrix
+
+ def get_ring_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb4,[0xc6],sleep=sleep_time)
+ return self.ring_matrix
+
+ def get_little_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb5,[0xc6],sleep=sleep_time)
+ return self.little_matrix
+
+ def get_force(self):
+ '''Get pressure.'''
+ return [self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc]
+
+ def get_temperature(self):
+ '''Get temperature.'''
+ self.get_motor_temperature()
+ return self.x33
+
+ def get_fault(self):
+ '''Get faults.'''
+ self.get_motor_fault_code()
+ return self.x35
+
+ def get_finger_order(self):
+ return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+
+ def show_fun_table(self):
+ pass
+
+ def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+ """O6 暂不支持清除故障码"""
+ pass
+
+ def get_serial_number(self):
+ try:
+ self.send_frame(0xC0,[],sleep=0.005)
+ # 1. 使用 bytes() 函数将整数列表转换为字节对象
+ # bytes() 接收一个由 0-255 之间的整数组成的列表。
+ byte_data = bytes(self.serial_number)
+ # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串
+ result_string = byte_data.decode('ascii')
+ if result_string == "":
+ return "-1"
+ else:
+ # print(f"原始 ASCII 码列表: {self.serial_number}")
+ # print(f"解码后的字符串: {result_string}")
+ return result_string
+ except:
+ return "-1"
+
+ def close_can_interface(self):
+ """Stop the CAN communication."""
+ self.running = False
+ if self.receive_thread.is_alive():
+ self.receive_thread.join()
+ if self.bus:
+ self.bus.shutdown()
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l7_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l7_can.py
new file mode 100644
index 0000000..446d24a
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_l7_can.py
@@ -0,0 +1,419 @@
+import can
+import time, sys
+import threading
+import numpy as np
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+
+
+class LinkerHandL7Can:
+ def __init__(self, can_id, can_channel='can0', baudrate=1000000,yaml=""):
+ self.can_id = can_id
+ self.can_channel = can_channel
+ self.baudrate = baudrate
+ self.open_can = OpenCan(load_yaml=yaml)
+
+ self.x01 = [0] * 7
+ self.x02 = [-1] * 7
+ self.x05 = [0] * 7
+ self.x33 = [0] * 7
+ self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+ self.thumb_matrix = np.full((12, 6), -1)
+ self.index_matrix = np.full((12, 6), -1)
+ self.middle_matrix = np.full((12, 6), -1)
+ self.ring_matrix = np.full((12, 6), -1)
+ self.little_matrix = np.full((12, 6), -1)
+ self.matrix_map = {
+ 0: 0,
+ 16: 1,
+ 32: 2,
+ 48: 3,
+ 64: 4,
+ 80: 5,
+ 96: 6,
+ 112: 7,
+ 128: 8,
+ 144: 9,
+ 160: 10,
+ 176: 11,
+ }
+ self.serial_number = []
+ self.serial_number_map = {
+ 0: 0,
+ 1: 1,
+ 2: 2,
+ 3: 3,
+ }
+ # Fault codes
+ self.x35 = [0] * 7, [0] * 7
+ self.joint_angles = [0] * 10
+ self.pressures = [200] * 7 # Default torque 200
+ self.bus = self.init_can_bus(can_channel, baudrate)
+ self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 7 for _ in range(4)]
+ self.is_lock = False
+ self.version = None
+ # Start the receiving thread
+ self.running = True
+ self.receive_thread = threading.Thread(target=self.receive_response)
+ self.receive_thread.daemon = True
+ self.receive_thread.start()
+
+ def init_can_bus(self, channel, baudrate):
+ """
+ 尝试按优先级连接 CAN 总线，并实现回退机制。
+ """
+ # --- 统一异常处理块开始 ---
+ try:
+ if sys.platform == "linux":
+ # Linux 优先级：1. socketcan
+ try:
+ self.open_can.open_can(self.can_channel)
+ # 尝试 socketcan
+ bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+ ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+ raise # 重新抛出异常，让外层 try 捕获
+ elif sys.platform == "win32":
+ # Windows 优先级：1. pcan
+ try:
+ bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+ # Windows 优先级：2. candle (回退方法)
+ try:
+ bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+ raise # 两个接口都失败，抛出异常
+ else:
+ raise EnvironmentError("Unsupported platform for CAN interface")
+ # --- 统一异常处理块结束 ---
+ except Exception as e:
+ # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+ ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+ # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+ raise
+
+ def send_frame(self, frame_property, data_list,sleep=0.005):
+ """Send a single CAN frame with specified properties and data."""
+ frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ data = [frame_property_value] + [int(val) for val in data_list]
+ msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ try:
+ self.bus.send(msg)
+ except can.CanError as e:
+ print(f"Failed to send message: {e}")
+ self.open_can.open_can(self.can_channel)
+ time.sleep(1)
+ self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+ time.sleep(1)
+ if self.is_can:
+ self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+ else:
+ print("Reconnecting CAN devices ....")
+ time.sleep(sleep)
+
+ def set_joint_positions(self, joint_angles):
+ """Set the positions of 10 joints (joint_angles: list of 10 values)."""
+ self.is_lock = True
+ if len(joint_angles) > 7:
+ self.joint_angles = joint_angles[:7]
+ else:
+ self.joint_angles = joint_angles
+ # Send angle control in frames
+ self.send_frame(0x01, self.joint_angles, sleep=0.003)
+ self.is_lock = False
+
+ def set_max_torque_limits(self, pressures, type="get"):
+ """Set maximum torque limits."""
+ if type == "get":
+ self.pressures = [0.0]
+ else:
+ self.pressures = pressures[:7]
+
+ def set_torque(self, torque=[180] * 7):
+ """Set L7 maximum torque limits."""
+ if len(torque) != 7:
+ raise ValueError("Torque list must have 7 elements.")
+ return
+ self.send_frame(0x02, torque)
+
+ def set_speed(self, speed=[180] * 7):
+ """Set L7 speed."""
+ if len(speed) != 7:
+ raise ValueError("Speed list must have 7 elements.")
+ return
+ self.x05 = speed
+ for i in range(2):
+ time.sleep(0.001)
+ self.send_frame(0x05, speed)
+
+ ''' -------------------Pressure Sensors---------------------- '''
+ def get_normal_force(self):
+ self.send_frame(0x20, [],sleep=0.004)
+
+ def get_tangential_force(self):
+ self.send_frame(0x21, [],sleep=0.004)
+
+ def get_tangential_force_dir(self):
+ self.send_frame(0x22, [],sleep=0.004)
+
+ def get_approach_inc(self):
+ self.send_frame(0x23, [],sleep=0.004)
+
+ ''' -------------------Motor Temperature---------------------- '''
+ def get_motor_temperature(self):
+ self.send_frame(0x33, [])
+
+ # Motor fault codes
+ def get_motor_fault_code(self):
+ self.send_frame(0x35, [])
+
+ def receive_response(self):
+ """Receive CAN responses and process them."""
+ while self.running:
+ try:
+ msg = self.bus.recv(timeout=1.0)
+ if msg:
+ self.process_response(msg)
+ except can.CanError as e:
+ print(f"Error receiving CAN message: {e}")
+
+ def process_response(self, msg):
+ """Process received CAN messages."""
+ if msg.arbitration_id == self.can_id:
+ frame_type = msg.data[0]
+ response_data = msg.data[1:]
+ if len(list(response_data)) == 0:
+ return
+ if frame_type == 0x01: # 0x01
+ self.x01 = list(response_data)
+ elif frame_type == 0x02: # 0x02
+ self.x02 = list(response_data)
+ elif frame_type == 0x05: # Set speed
+ self.x05 = list(response_data)
+ elif frame_type == 0x20:
+ d = list(response_data)
+ self.normal_force = [float(i) for i in d]
+ elif frame_type == 0x21:
+ d = list(response_data)
+ self.tangential_force = [float(i) for i in d]
+ elif frame_type == 0x22:
+ d = list(response_data)
+ self.tangential_force_dir = [float(i) for i in d]
+ elif frame_type == 0x23:
+ d = list(response_data)
+ self.approach_inc = [float(i) for i in d]
+ elif frame_type == 0x33: # L7 temperature
+ self.x33 = list(response_data)
+ elif frame_type == 0x35: # L7 fault codes
+ self.x35 = list(response_data)
+ elif frame_type == 0xb0:
+ self.xb0 = list(response_data)
+ elif frame_type == 0xb1:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb1 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.thumb_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb2:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb2 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.index_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb3:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb3 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.middle_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb4:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb4 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.ring_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb5:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb5 = d
+ elif len(d) == 7:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.little_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0x64: # L7 version number
+ self.version = list(response_data)
+ elif frame_type == 0xC2: # O6 version number
+ self.version = list(response_data)
+ elif frame_type == 0xC0:
+ d = list(response_data)
+ index = self.serial_number_map.get(d[0])
+ if index is not None:
+ self.serial_number=self.serial_number + d[1:]
+ else:
+ self.serial_number=self.serial_number + [-1] * 6
+
+ def get_version(self):
+ self.send_frame(0x64, [], sleep=0.1)
+ time.sleep(0.1)
+ if self.version is None:
+ self.send_frame(0xC2, [], sleep=0.1)
+ time.sleep(0.1)
+ return self.version
+
+ def get_current_status(self):
+ if self.is_lock:
+ return self.x01
+ elif self.is_lock == False:
+ self.send_frame(0x01, [],sleep=0.003)
+ return self.x01
+
+ def get_current_pub_status(self):
+ return self.x01
+
+ def get_speed(self):
+ self.send_frame(0x05, [],sleep=0.003)
+ return self.x05
+
+ def get_current(self):
+ '''Not supported yet.'''
+ self.send_frame(0x2, [],sleep=0.1)
+ return self.x02
+
+
+
+ def get_torque(self):
+ '''Not supported yet.'''
+ self.send_frame(0x2, [],sleep=0.01)
+ return self.x02
+
+ def get_touch_type(self):
+ '''Get touch type'''
+ self.send_frame(0xb1,[])
+ t = []
+ for i in range(3):
+ t = self.xb1
+ time.sleep(0.01)
+ if len(t) == 2:
+ return 2
+ else:
+ self.send_frame(0x20,[],sleep=0.03)
+ time.sleep(0.01)
+ if self.normal_force[0] == -1:
+ return -1
+ else:
+ return 1
+
+
+ def get_touch(self):
+ '''Get touch data'''
+ self.send_frame(0xb1,[],sleep=0.03)
+ self.send_frame(0xb2,[],sleep=0.03)
+ self.send_frame(0xb3,[],sleep=0.03)
+ self.send_frame(0xb4,[],sleep=0.03)
+ self.send_frame(0xb5,[],sleep=0.03)
+ return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available
+
+ def get_matrix_touch(self):
+ self.send_frame(0xb1,[0xc6],sleep=0.01)
+ self.send_frame(0xb2,[0xc6],sleep=0.01)
+ self.send_frame(0xb3,[0xc6],sleep=0.01)
+ self.send_frame(0xb4,[0xc6],sleep=0.01)
+ self.send_frame(0xb5,[0xc6],sleep=0.01)
+
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_matrix_touch_v2(self):
+ self.send_frame(0xb1,[0xc6],sleep=0.005)
+ self.send_frame(0xb2,[0xc6],sleep=0.005)
+ self.send_frame(0xb3,[0xc6],sleep=0.005)
+ self.send_frame(0xb4,[0xc6],sleep=0.005)
+ self.send_frame(0xb5,[0xc6],sleep=0.005)
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+
+ def get_thumb_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb1,[0xc6],sleep=sleep_time)
+ return self.thumb_matrix
+
+ def get_index_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb2,[0xc6],sleep=sleep_time)
+ return self.index_matrix
+
+ def get_middle_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb3,[0xc6],sleep=sleep_time)
+ return self.middle_matrix
+
+ def get_ring_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb4,[0xc6],sleep=sleep_time)
+ return self.ring_matrix
+
+ def get_little_matrix_touch(self,sleep_time=0.005):
+ self.send_frame(0xb5,[0xc6],sleep=sleep_time)
+ return self.little_matrix
+
+ def get_force(self):
+ '''Get pressure.'''
+ return [self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc]
+
+ def get_temperature(self):
+ '''Get temperature.'''
+ self.get_motor_temperature()
+ return self.x33
+
+ def get_fault(self):
+ '''Get faults.'''
+ self.get_motor_fault_code()
+ return self.x35
+
+ def get_serial_number(self):
+ try:
+ self.send_frame(0xC0,[],sleep=0.005)
+ # 1. 使用 bytes() 函数将整数列表转换为字节对象
+ # bytes() 接收一个由 0-255 之间的整数组成的列表。
+ byte_data = bytes(self.serial_number)
+ # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串
+ result_string = byte_data.decode('ascii')
+ if result_string == "":
+ return "-1"
+ else:
+ # print(f"原始 ASCII 码列表: {self.serial_number}")
+ # print(f"解码后的字符串: {result_string}")
+ return result_string
+ except:
+ return "-1"
+
+ def get_finger_order(self):
+ return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch", "thumb_cmc_roll"]
+
+ def show_fun_table(self):
+ pass
+
+ def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+ """L7 暂不支持清除故障码"""
+ pass
+
+ def close_can_interface(self):
+ """Stop the CAN communication."""
+ self.running = False
+ if self.receive_thread.is_alive():
+ self.receive_thread.join()
+ if self.bus:
+ self.bus.shutdown()
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_o6_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_o6_can.py
new file mode 100644
index 0000000..5c95ab8
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_o6_can.py
@@ -0,0 +1,447 @@
+import can
+import time, sys
+import threading
+import numpy as np
+from utils.open_can import OpenCan
+from utils.color_msg import ColorMsg
+from can.exceptions import CanError
+
+
+class LinkerHandO6Can:
+ def __init__(self, can_id, can_channel='can0', baudrate=1000000,yaml=""):
+ self.can_id = can_id
+ self.can_channel = can_channel
+ self.baudrate = baudrate
+ self.open_can = OpenCan(load_yaml=yaml)
+
+ self.x01 = [0] * 6 # 关节位置
+ self.x02 = [-1] * 6 # 转矩限制
+ self.x05 = [0] * 6 # 速度
+ self.x07 = [-1] * 6 # 加速度
+ self.x33 = [0] * 6 # 温度
+ self.x35 = [0] * 6 # 关节错误码
+ self.x36 = [-1] * 6 # 电流
+ self.xb0,self.xb1,self.xb2,self.xb3,self.xb4,self.xb5 = [-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5,[-1] * 5
+
+ self.thumb_matrix = np.full((10, 4), -1)
+ self.index_matrix = np.full((10, 4), -1)
+ self.middle_matrix = np.full((10, 4), -1)
+ self.ring_matrix = np.full((10, 4), -1)
+ self.little_matrix = np.full((10, 4), -1)
+ self.matrix_map = {
+ 0: 0,
+ 16: 1,
+ 32: 2,
+ 48: 3,
+ 64: 4,
+ 80: 5,
+ 96: 6,
+ 112: 7,
+ 128: 8,
+ 144: 9,
+ 160: 10,
+ 176: 11,
+ }
+ self.serial_number = []
+ self.serial_number_map = {
+ 0: 0,
+ 1: 1,
+ 2: 2,
+ 3: 3,
+ }
+ # Fault codes
+
+ self.joint_angles = [0] * 6
+ self.pressures = [200] * 6 # Default torque 200
+ self.bus = self.init_can_bus(can_channel, baudrate)
+ self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc = [[-1] * 6 for _ in range(4)]
+ self.is_lock = False
+ self.version = None
+ # Start the receiving thread
+ self.running = True
+
+ self.receive_thread = threading.Thread(target=self.receive_response)
+ self.receive_thread.daemon = True
+ self.receive_thread.start()
+ time.sleep(0.1)
+ self._check_touch_type()
+
+ def _check_touch_type(self):
+ '''根据SN编码判断压感类型'''
+ self.sn = self.get_serial_number()
+ time.sleep(0.1)
+ if self.sn != "-1":
+ parts = self.sn.split("-")
+ if parts[4] == "A":
+ self.touch_type = 1
+ elif parts[4] == "B":
+ self.touch_type = 2
+ self.touch_code = 0xA4 # 6*12 O6 一律0XA4
+ elif parts[4] == "J":
+ self.touch_type = 3
+ elif parts[4] == "F":
+ self.touch_type = 4
+ self.touch_code = 0xA4 # 4*10
+ elif parts[4] == "Z":
+ self.touch_type = -1
+ else:
+ # 如果没有SN编码则根据返回数据进行判断
+ self.touch_type = self.get_touch_type()
+
+
+ def init_can_bus(self, channel, baudrate):
+ """
+ 尝试按优先级连接 CAN 总线，并实现回退机制。
+ """
+ # --- 统一异常处理块开始 ---
+ try:
+ if sys.platform == "linux":
+ # Linux 优先级：1. socketcan
+ try:
+ self.open_can.open_can(self.can_channel)
+ # 尝试 socketcan
+ bus = can.interface.Bus(channel=channel, interface="socketcan", bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='socketcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ # 如果 socketcan 失败，可以考虑在这里尝试其他 Linux 接口 (如 'pcan')
+ ColorMsg(msg=f"socketcan 接口连接失败: {e}", color="yellow")
+ raise # 重新抛出异常，让外层 try 捕获
+ elif sys.platform == "win32":
+ # Windows 优先级：1. pcan
+ try:
+ bus = can.interface.Bus(channel=channel, interface='pcan', bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='pcan', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"pcan 接口连接失败，尝试回退到 'candle': {e}", color="yellow")
+ # Windows 优先级：2. candle (回退方法)
+ try:
+ bus = can.Bus(interface="candle", channel=channel, bitrate=baudrate)
+ ColorMsg(msg=f"成功连接: interface='candle', channel='{channel}'", color="green")
+ return bus
+ except CanError as e:
+ ColorMsg(msg=f"candle 接口连接失败: {e}", color="yellow")
+ raise # 两个接口都失败，抛出异常
+ else:
+ raise EnvironmentError("Unsupported platform for CAN interface")
+ # --- 统一异常处理块结束 ---
+ except Exception as e:
+ # 如果任何一个接口尝试失败并抛出异常（包括 EnvironmentError）
+ ColorMsg(msg=f"致命错误：所有 CAN 接口连接尝试均失败或平台不受支持。请检查设备连接或驱动安装和配置文件中CAN参数的配置。\n错误详情: {e}", color="red")
+ # 保持 raise 动作，将错误信息传递给调用者，避免程序继续运行
+ raise
+
+ def send_frame(self, frame_property, data_list,sleep=0.005):
+ """Send a single CAN frame with specified properties and data."""
+ frame_property_value = int(frame_property.value) if hasattr(frame_property, 'value') else frame_property
+ data = [frame_property_value] + [int(val) for val in data_list]
+ msg = can.Message(arbitration_id=self.can_id, data=data, is_extended_id=False)
+ try:
+ self.bus.send(msg)
+ except can.CanError as e:
+ print(f"Failed to send message: {e}")
+ self.open_can.open_can(self.can_channel)
+ time.sleep(1)
+ self.is_can = self.open_can.is_can_up_sysfs(interface=self.can_channel)
+ time.sleep(1)
+ if self.is_can:
+ self.bus = can.interface.Bus(channel=self.can_channel, interface="socketcan", bitrate=self.baudrate)
+ else:
+ print("Reconnecting CAN devices ....")
+ time.sleep(sleep)
+
+ def set_joint_positions(self, joint_angles):
+ """Set the positions of 10 joints (joint_angles: list of 10 values)."""
+ if len(joint_angles) > 6:
+ self.joint_angles = joint_angles[:6]
+ else:
+ self.joint_angles = joint_angles
+ # Send angle control in frames
+ self.send_frame(0x01, self.joint_angles, sleep=0.003)
+
+ def set_max_torque_limits(self, pressures, type="get"):
+ """Set maximum torque limits."""
+ if type == "get":
+ self.pressures = [0.0]
+ else:
+ self.pressures = pressures[:6]
+
+ def set_torque(self, torque=[180] * 6):
+ """Set L6 maximum torque limits."""
+ if len(torque) != 6:
+ raise ValueError("Torque list must have 6 elements.")
+ return
+ self.send_frame(0x02, torque)
+
+ def set_speed(self, speed=[180] * 6):
+ """Set L6 speed."""
+ if len(speed) != 6:
+ raise ValueError("Speed list must have 6 elements.")
+ return
+ self.x05 = speed
+ for i in range(2):
+ time.sleep(0.001)
+ self.send_frame(0x05, speed)
+
+ ''' -------------------Pressure Sensors---------------------- '''
+ def get_normal_force(self):
+ self.send_frame(0x20, [],sleep=0.01)
+
+ def get_tangential_force(self):
+ self.send_frame(0x21, [],sleep=0.01)
+
+ def get_tangential_force_dir(self):
+ self.send_frame(0x22, [],sleep=0.01)
+
+ def get_approach_inc(self):
+ self.send_frame(0x23, [],sleep=0.01)
+
+ ''' -------------------Motor Temperature---------------------- '''
+ def get_motor_temperature(self):
+ self.send_frame(0x33, [])
+
+ # Motor fault codes
+ def get_motor_fault_code(self):
+ self.send_frame(0x35, [])
+
+ def receive_response(self):
+ """Receive CAN responses and process them."""
+ while self.running:
+ try:
+ msg = self.bus.recv(timeout=1.0)
+ if msg:
+ self.process_response(msg)
+ except can.CanError as e:
+ print(f"Error receiving CAN message: {e}")
+
+ def process_response(self, msg):
+ """Process received CAN messages."""
+ if msg.arbitration_id == self.can_id:
+ frame_type = msg.data[0]
+ response_data = msg.data[1:]
+ if len(list(response_data)) == 0:
+ return
+ if frame_type == 0x01: # 0x01
+ self.x01 = list(response_data)
+ elif frame_type == 0x02: # 0x02
+ self.x02 = list(response_data)
+ elif frame_type == 0x05: # Set speed
+ self.x05 = list(response_data)
+ elif frame_type == 0x20:
+ d = list(response_data)
+ self.normal_force = [float(i) for i in d]
+ elif frame_type == 0x21:
+ d = list(response_data)
+ self.tangential_force = [float(i) for i in d]
+ elif frame_type == 0x22:
+ d = list(response_data)
+ self.tangential_force_dir = [float(i) for i in d]
+ elif frame_type == 0x23:
+ d = list(response_data)
+ self.approach_inc = [float(i) for i in d]
+ elif frame_type == 0x33: # O6 temperature
+ self.x33 = list(response_data)
+ elif frame_type == 0x35: # O6 fault codes
+ self.x35 = list(response_data)
+ elif frame_type == 0x36: # O6 电流
+ self.x36 = list(response_data)
+ elif frame_type == 0xb0:
+ self.xb0 = list(response_data)
+ elif frame_type == 0xb1:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb1 = d
+ elif len(d) == 5:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.thumb_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb2:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb2 = d
+ elif len(d) == 5:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.index_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb3:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb3 = d
+ elif len(d) == 5:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.middle_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb4:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb4 = d
+ elif len(d) == 5:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.ring_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0xb5:
+ d = list(response_data)
+ if len(d) == 2:
+ self.xb5 = d
+ elif len(d) == 5:
+ index = self.matrix_map.get(d[0])
+ if index is not None:
+ self.little_matrix[index] = d[1:] # Remove the first flag bit
+ elif frame_type == 0x64: # O6 version number
+ self.version = list(response_data)
+ elif frame_type == 0xC2: # O6 version number
+ self.version = list(response_data)
+ elif frame_type == 0xC0:
+ d = list(response_data)
+ index = self.serial_number_map.get(d[0])
+ if index is not None:
+ self.serial_number += d[1:]
+ else:
+ self.serial_number=self.serial_number + [-1] * 6
+
+
+
+ def get_version(self):
+ self.send_frame(0x64, [],sleep=0.1)
+ time.sleep(0.1)
+ if self.version is None:
+ self.send_frame(0xC2, [],sleep=0.1)
+ time.sleep(0.1)
+ return self.version
+
+ def get_current_status(self):
+ self.send_frame(0x01, [],sleep=0.005)
+ return self.x01
+
+ def get_current_pub_status(self):
+ return self.x01
+
+ def get_speed(self):
+ self.send_frame(0x05, [],sleep=0.002)
+ #print("L6暂不支持读取实时速度")
+ return self.x05
+
+ def get_current(self):
+ '''Not supported yet.'''
+ self.send_frame(0x36, [],sleep=0.005)
+ return self.x36
+
+
+
+ def get_torque(self):
+ '''Not supported yet.'''
+ self.send_frame(0x2, [],sleep=0.01)
+ return self.x02
+
+ def get_touch_type(self):
+ '''Get touch type'''
+ self.send_frame(0xb1,[])
+ t = []
+ for i in range(3):
+ t = self.xb1
+ time.sleep(0.01)
+ if len(t) == 2:
+ return 2
+ else:
+ self.send_frame(0x20,[],sleep=0.03)
+ time.sleep(0.01)
+ if self.normal_force[0] == -1:
+ return -1
+ else:
+ return 1
+
+
+ def get_touch(self):
+ '''Get touch data'''
+ self.send_frame(0xb1,[],sleep=0.03)
+ self.send_frame(0xb2,[],sleep=0.03)
+ self.send_frame(0xb3,[],sleep=0.03)
+ self.send_frame(0xb4,[],sleep=0.03)
+ self.send_frame(0xb5,[],sleep=0.03)
+ return [self.xb1[1],self.xb2[1],self.xb3[1],self.xb4[1],self.xb5[1],0] # The last digit is palm, currently not available
+
+ def get_matrix_touch(self):
+ self.send_frame(0xb1,[self.touch_code],sleep=0.01)
+ self.send_frame(0xb2,[self.touch_code],sleep=0.01)
+ self.send_frame(0xb3,[self.touch_code],sleep=0.01)
+ self.send_frame(0xb4,[self.touch_code],sleep=0.01)
+ self.send_frame(0xb5,[self.touch_code],sleep=0.01)
+
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_matrix_touch_v2(self):
+ self.send_frame(0xb1,[self.touch_code],sleep=0.009)
+ self.send_frame(0xb2,[self.touch_code],sleep=0.009)
+ self.send_frame(0xb3,[self.touch_code],sleep=0.009)
+ self.send_frame(0xb4,[self.touch_code],sleep=0.009)
+ self.send_frame(0xb5,[self.touch_code],sleep=0.009)
+ return self.thumb_matrix , self.index_matrix , self.middle_matrix , self.ring_matrix , self.little_matrix
+
+ def get_thumb_matrix_touch(self,sleep_time=0.002):
+ self.send_frame(0xb1,[self.touch_code],sleep=sleep_time)
+ return self.thumb_matrix
+
+ def get_index_matrix_touch(self,sleep_time=0.002):
+ self.send_frame(0xb2,[self.touch_code],sleep=sleep_time)
+ return self.index_matrix
+
+ def get_middle_matrix_touch(self,sleep_time=0.002):
+ self.send_frame(0xb3,[self.touch_code],sleep=sleep_time)
+ return self.middle_matrix
+
+ def get_ring_matrix_touch(self,sleep_time=0.002):
+ self.send_frame(0xb4,[self.touch_code],sleep=sleep_time)
+ return self.ring_matrix
+
+ def get_little_matrix_touch(self,sleep_time=0.002):
+ self.send_frame(0xb5,[self.touch_code],sleep=sleep_time)
+ return self.little_matrix
+
+ def get_force(self):
+ '''Get pressure.'''
+ return [self.normal_force, self.tangential_force, self.tangential_force_dir, self.approach_inc]
+
+ def get_temperature(self):
+ '''Get temperature.'''
+ self.get_motor_temperature()
+ return self.x33
+
+ def get_fault(self):
+ '''Get faults.'''
+ self.get_motor_fault_code()
+ return self.x35
+
+ def get_finger_order(self):
+ return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+
+ def clear_faults(self, finger_mask=[1, 1, 1, 1, 1]):
+ """O6 暂不支持清除故障码"""
+ pass
+ def show_fun_table(self):
+ pass
+
+ def get_serial_number(self):
+ try:
+ self.send_frame(0xC0,[],sleep=0.005)
+ # 1. 使用 bytes() 函数将整数列表转换为字节对象
+ # bytes() 接收一个由 0-255 之间的整数组成的列表。
+ byte_data = bytes(self.serial_number)
+ # 2. 使用 .decode() 方法将字节对象解码为 ASCII 字符串
+ result_string = byte_data.decode('ascii')
+ if result_string == "":
+ return "-1"
+ else:
+ # print(f"原始 ASCII 码列表: {self.serial_number}")
+ # print(f"解码后的字符串: {result_string}")
+ return result_string
+ except:
+ return "-1"
+
+ def close_can_interface(self):
+ """Stop the CAN communication."""
+ self.running = False
+ if self.receive_thread.is_alive():
+ self.receive_thread.join()
+ if self.bus:
+ self.bus.shutdown()
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l10_rs485.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l10_rs485.py
new file mode 100644
index 0000000..8db78ab
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l10_rs485.py
@@ -0,0 +1,345 @@
+#!/usr/bin/env python3
+import os
+import time
+import struct
+from typing import Dict, List
+import numpy as np
+from pymodbus.client import ModbusSerialClient
+_INTERVAL = 0.005 # 8 ms
+class LinkerHandL10RS485:
+ KEYS = ["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"]
+
+ def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
+ self.slave = hand_id
+ self.cli = ModbusSerialClient(
+ port=modbus_port,
+ baudrate=baudrate,
+ bytesize=8,
+ parity="N",
+ stopbits=1,
+ timeout=0.05, # 50 ms 超时
+ retries=3, # 重试次数
+ retry_on_empty=True,
+ handle_local_echo=False
+ )
+ # 在 pymodbus 3.5.1 中，连接需要显式调用 connect()
+ self.connected = self.cli.connect()
+ if not self.connected:
+ raise ConnectionError(f"RS485 connect fail to {modbus_port}")
+
+ # --------------------------------------------------
+ # 批量读取接口
+ # --------------------------------------------------
+ def read_angles(self) -> List[int]:
+ time.sleep(_INTERVAL)
+ rsp = self.cli.read_input_registers(address=0, count=10, slave=self.slave)
+ if rsp.isError():
+ raise RuntimeError(f"read_angles failed: {rsp}")
+ return rsp.registers
+
+ def read_torques(self) -> List[int]:
+ time.sleep(_INTERVAL)
+ rsp = self.cli.read_input_registers(address=10, count=10, slave=self.slave)
+ if rsp.isError():
+ raise RuntimeError(f"read_torques failed: {rsp}")
+ return rsp.registers
+
+ def read_speeds(self) -> List[int]:
+ time.sleep(_INTERVAL)
+ rsp = self.cli.read_input_registers(address=20, count=10, slave=self.slave)
+ if rsp.isError():
+ raise RuntimeError(f"read_speeds failed: {rsp}")
+ return rsp.registers
+
+ def read_temperatures(self) -> List[int]:
+ time.sleep(_INTERVAL)
+ rsp = self.cli.read_input_registers(address=40, count=10, slave=self.slave)
+ if rsp.isError():
+ raise RuntimeError(f"read_temperatures failed: {rsp}")
+ return rsp.registers
+
+ def read_error_codes(self) -> List[int]:
+ time.sleep(_INTERVAL)
+ rsp = self.cli.read_input_registers(address=50, count=10, slave=self.slave)
+ if rsp.isError():
+ raise RuntimeError(f"read_error_codes failed: {rsp}")
+ return rsp.registers
+
+ def read_versions(self) -> dict:
+ time.sleep(_INTERVAL)
+ rsp = self.cli.read_input_registers(address=158, count=6, slave=self.slave)
+ if rsp.isError():
+ raise RuntimeError(f"read_versions failed: {rsp}")
+ keys = ["hand_freedom", "hand_version", "hand_number",
+ "hand_direction", "software_version", "hardware_version"]
+ #return dict(zip(keys, rsp.registers))
+ return rsp.registers
+
+ # --------------------------------------------------
+ # 5 个压力传感器
+ # --------------------------------------------------
+ def read_pressure_thumb(self) -> np.ndarray:
+ return np.array(self._pressure(1), dtype=np.uint8)
+
+ def read_pressure_index(self) -> np.ndarray:
+ return np.array(self._pressure(2), dtype=np.uint8)
+
+ def read_pressure_middle(self) -> np.ndarray:
+ return np.array(self._pressure(3), dtype=np.uint8)
+
+ def read_pressure_ring(self) -> np.ndarray:
+ return np.array(self._pressure(4), dtype=np.uint8)
+
+ def read_pressure_pinky(self) -> np.ndarray:
+ return np.array(self._pressure(5), dtype=np.uint8)
+
+ # def _pressure(self, finger: int) -> List[int]:
+ # time.sleep(_INTERVAL)
+ # # 先选择手指
+ # wrsp = self.cli.write_register(address=60, value=finger, slave=self.slave)
+ # if wrsp.isError():
+ # raise RuntimeError(f"write finger select {finger} failed: {wrsp}")
+
+ # time.sleep(_INTERVAL)
+ # # 读取压力传感器数据 (96个寄存器)
+ # rrsp = self.cli.read_input_registers(address=62, count=96, slave=self.slave)
+ # if rrsp.isError():
+ # raise RuntimeError(f"read pressure finger={finger} failed: {rrsp}")
+ # return np.array(rrsp.registers, dtype=np.uint8)
+ def _pressure(self, finger: int) -> np.ndarray:
+ """
+ 6x12 (72点) 矩阵尺寸。
+ Modbus 地址 60/62。
+ """
+ rows = 12 # 12 行
+ cols = 6 # 6 列
+ finger_size = rows * cols # 72 个数据点
+
+ # modbus 地址和计数
+ write_address = 70 # 写入手指选择
+ read_address = 72 # 读取压力数据
+ read_count = 96 # 读取 96 个寄存器
+ skip_count = 10 # 跳过前 10 个校验点
+
+ # 0. 参数校验和手指写入值确定
+ if finger < 1 or finger > 5:
+ raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。")
+
+ finger_write_value = finger
+
+ # 1. 写入手指选择寄存器 (地址 60)
+ time.sleep(0.008)
+ wrsp = self.cli.write_register(address=write_address, value=finger_write_value, slave=self.slave)
+ if wrsp.isError():
+ raise RuntimeError(f"写入手指选择 {finger} 到地址 {write_address} 失败: {wrsp}")
+
+ # 写入后等待片刻
+ time.sleep(0.008)
+
+ # 2. 读取地址 62 的数据
+ rrsp = self.cli.read_input_registers(address=read_address, count=read_count, slave=self.slave)
+
+ if rrsp.isError():
+ raise RuntimeError(f"读取地址 {read_address} 压力数据失败: {rrsp}")
+
+ registers_16bit: List[int] = rrsp.registers
+
+ # 3. 核心数据处理
+ # a. 提取低 8 位数据 (得到 96 个 8 位数据点)
+ final_data_96 = [reg_value & 255 for reg_value in registers_16bit]
+
+ # b. 跳过前 10 个校验/头部数据点 (得到 86 个有效数据点)
+ effective_data = np.array(final_data_96[skip_count:], dtype=np.uint8)
+ # c. 截取当前手指的矩阵数据 (从 86 个有效点中截取 72 个点)
+ start_idx = 0
+ end_idx = finger_size # 72
+
+ finger_data_flat = effective_data[start_idx:end_idx]
+
+ # d. 验证数据长度
+ if finger_data_flat.size != finger_size:
+ raise ValueError(
+ f"数据提取失败。期望 {finger_size} 点 ({rows}x{cols})，"
+ f"但仅截取到 {finger_data_flat.size} 点。请检查协议，确认地址 62 是否一次性返回了所有手指数据。"
+ )
+
+ # e. 重塑为二维矩阵 (12 行 6 列)
+ finger_matrix = finger_data_flat.reshape((rows, cols))
+
+ return finger_matrix
+
+
+
+
+ # --------------------------------------------------
+ # 批量写入接口
+ # --------------------------------------------------
+ def write_angles(self, vals: List[int]):
+ vals = [int(x) for x in vals]
+ if not self.is_valid_10xuint8(vals):
+ raise ValueError("需要 10 个 0-255 整数")
+
+ time.sleep(_INTERVAL)
+ rsp = self.cli.write_registers(address=0, values=vals, slave=self.slave)
+ if rsp.isError():
+ raise RuntimeError(f"write_angles failed: {rsp}")
+
+ def write_speeds(self, vals: List[int]):
+ vals = [int(x) for x in vals]
+ if not self.is_valid_10xuint8(vals):
+ raise ValueError("需要 10 个 0-255 整数")
+
+ time.sleep(_INTERVAL)
+ rsp = self.cli.write_registers(address=20, values=vals, slave=self.slave)
+ if rsp.isError():
+ raise RuntimeError(f"write_speeds failed: {rsp}")
+
+ def write_torques(self, vals: List[int]):
+ vals = [int(x) for x in vals]
+ if not self.is_valid_10xuint8(vals):
+ raise ValueError("需要 10 个 0-255 整数")
+
+ time.sleep(_INTERVAL)
+ rsp = self.cli.write_registers(address=10, values=vals, slave=self.slave)
+ if rsp.isError():
+ raise RuntimeError(f"write_torques failed: {rsp}")
+
+ # --------------------------------------------------
+ # 上下文管理
+ # --------------------------------------------------
+ def close(self):
+ if self.connected:
+ self.cli.close()
+ self.connected = False
+
+ def __enter__(self):
+ return self
+
+ def __exit__(self, exc_type, exc_val, exc_tb):
+ self.close()
+
+ # --------------------------------------------------
+ # 工具函数
+ # --------------------------------------------------
+ def is_valid_10xuint8(self, lst) -> bool:
+ if len(lst) != 10:
+ return False
+ return all(isinstance(x, int) and 0 <= x <= 255 for x in lst)
+
+ # --------------------------------------------------
+ # 固定 API 接口
+ # --------------------------------------------------
+ def set_joint_positions(self, joint_angles=None):
+ joint_angles = joint_angles or [0] * 10
+ self.write_angles(joint_angles)
+
+ def set_speed(self, speed=None):
+ speed = speed or [200] * 10
+ self.write_speeds(speed)
+
+ def set_torque(self, torque=None):
+ torque = torque or [200] * 10
+ self.write_torques(torque)
+
+ def set_current(self, current=None):
+ print("当前L10不支持设置电流", flush=True)
+
+ def get_version(self) -> dict:
+ return self.read_versions()
+
+ def get_current(self):
+ print("当前L10不支持获取电流", flush=True)
+
+ def get_state(self) -> List[int]:
+ return self.read_angles()
+
+ def get_state_for_pub(self) -> List[int]:
+ return self.get_state()
+
+ def get_current_status(self) -> List[int]:
+ return self.get_state()
+
+ def get_speed(self) -> List[int]:
+ return self.read_speeds()
+
+ def get_joint_speed(self) -> List[int]:
+ return self.get_speed()
+
+ def get_touch_type(self) -> int:
+ return 2
+
+ def get_normal_force(self) -> List[int]:
+ return [-1] * 5
+
+ def get_tangential_force(self) -> List[int]:
+ return [-1] * 5
+
+ def get_approach_inc(self) -> List[int]:
+ return [-1] * 5
+
+ def get_touch(self) -> List[int]:
+ return [-1] * 5
+
+ def get_thumb_matrix_touch(self,sleep_time=0):
+ return self._pressure(1)
+
+ def get_index_matrix_touch(self,sleep_time=0):
+ return self._pressure(2)
+
+ def get_middle_matrix_touch(self,sleep_time=0):
+ return self._pressure(3)
+
+ def get_ring_matrix_touch(self,sleep_time=0):
+ return self._pressure(4)
+
+ def get_little_matrix_touch(self,sleep_time=0):
+ return self._pressure(5)
+
+ def get_matrix_touch(self) -> List[List[int]]:
+ return self.get_thumb_matrix_touch(),self.get_index_matrix_touch(), self.get_middle_matrix_touch(), self.get_ring_matrix_touch(), self.get_little_matrix_touch()
+
+ def get_matrix_touch_v2(self) -> List[List[int]]:
+ return self.get_matrix_touch()
+
+ def get_torque(self) -> List[int]:
+ return self.read_torques()
+
+ def get_temperature(self) -> List[int]:
+ return self.read_temperatures()
+
+ def get_fault(self) -> List[int]:
+ return self.read_error_codes()
+
+ def get_serial_number(self):
+ return [0] * 6
+
+ def clear_faults(self):
+ pass
+
+# ------------------- demo -------------------
+if __name__ == "__main__":
+ try:
+ with LinkerHandL10RS485(hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200) as hand:
+ print("连接成功!")
+
+ # 测试读取角度
+ angles = hand.read_angles()
+ print("角度:", dict(zip(LinkerHandL10RS485.KEYS, angles)))
+
+ # 测试读取版本信息
+ ver = hand.get_version()
+ print("版本信息:", ver)
+
+ # 测试压力传感器
+ print("拇指压力传感器数据长度:", len(hand.read_pressure_thumb()))
+
+ # 测试其他读取功能
+ print("电流:", hand.read_torques())
+ print("速度:", hand.read_speeds())
+ print("温度:", hand.read_temperatures())
+ print("错误码:", hand.read_error_codes())
+
+ except Exception as e:
+ print(f"错误: {e}")
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py
new file mode 100644
index 0000000..81db0e2
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py
@@ -0,0 +1,460 @@
+#!/usr/bin/env python3
+import os
+import time
+from pymodbus.client import ModbusSerialClient
+from typing import List, Dict
+import numpy as np
+
+_INTERVAL = 0.006 # 8 ms
+
+class LinkerHandL6RS485:
+ """L6机械手 Modbus-RTU 控制类"""
+
+ # 6个关节名称
+ JOINT_NAMES = ["thumb_pitch", "thumb_yaw", "index_pitch",
+ "middle_pitch", "ring_pitch", "little_pitch"]
+
+ # 手指名称
+ FINGER_NAMES = ["thumb", "index", "middle", "ring", "little"]
+
+ def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
+ """
+ 初始化L6机械手
+ hand_id: 右手0x27(39), 左手0x28(40)
+ modbus_port: 串口设备路径
+ baudrate: 波特率，固定115200
+ """
+ self.slave = hand_id
+ self.cli = ModbusSerialClient(
+ port=modbus_port,
+ baudrate=baudrate,
+ bytesize=8,
+ parity="N",
+ stopbits=1,
+ timeout=0.05
+ )
+ # pymodbus 3.5.1 需要显式连接
+ self.connected = self.cli.connect()
+ if not self.connected:
+ raise ConnectionError(f"RS485连接失败，端口: {modbus_port}")
+
+ def _read_input_registers(self, address: int, count: int) -> List[int]:
+ """读取输入寄存器"""
+ time.sleep(_INTERVAL)
+ result = self.cli.read_input_registers(address=address, count=count, slave=self.slave)
+ if result.isError():
+ raise RuntimeError(f"读取输入寄存器失败: address={address}, count={count}")
+ return result.registers
+
+ def _write_register(self, address: int, value: int):
+ """写入单个寄存器"""
+ time.sleep(_INTERVAL)
+ result = self.cli.write_register(address=address, value=value, slave=self.slave)
+ if result.isError():
+ raise RuntimeError(f"写入寄存器失败: address={address}, value={value}")
+
+ def _write_registers(self, address: int, values: List[int]):
+ """写入多个寄存器"""
+ time.sleep(_INTERVAL)
+ result = self.cli.write_registers(address=address, values=values, slave=self.slave)
+ if result.isError():
+ raise RuntimeError(f"写入多个寄存器失败: address={address}, values={values}")
+
+ # --------------------------------------------------
+ # 基础读取接口
+ # --------------------------------------------------
+
+ def read_angles(self) -> List[int]:
+ """读取6个关节角度 (输入寄存器 0-5)"""
+ return self._read_input_registers(0, 6)
+
+ def read_torques(self) -> List[int]:
+ """读取6个关节转矩 (输入寄存器 6-11)"""
+ return self._read_input_registers(6, 6)
+
+ def read_speeds(self) -> List[int]:
+ """读取6个关节速度 (输入寄存器 12-17)"""
+ return self._read_input_registers(12, 6)
+
+ def read_temperatures(self) -> List[int]:
+ """读取6个关节温度 (输入寄存器 18-23)"""
+ return self._read_input_registers(18, 6)
+
+ def read_error_codes(self) -> List[int]:
+ """读取6个关节错误码 (输入寄存器 24-29)"""
+ return self._read_input_registers(24, 6)
+
+ # --------------------------------------------------
+ # 压力传感器接口
+ # --------------------------------------------------
+
+ # def _pressure(self, finger: int) -> List[int]:
+ # """内部：选手指 → 读压力数据"""
+ # # 选择手指 (保持寄存器 36)
+ # self._write_register(36, finger)
+ # time.sleep(_INTERVAL)
+ # # 读取压力数据 (输入寄存器 52-122)
+ # return np.array(self._read_input_registers(52, 71))
+ def _pressure(self, finger: int) -> np.ndarray:
+ """
+ 6x12 (72点) 矩阵尺寸。
+ Modbus 地址 60/62。
+ """
+ rows = 12 # 12 行
+ cols = 6 # 6 列
+ finger_size = rows * cols # 72 个数据点
+
+ # modbus 地址和计数 (按协议文档)
+ write_address = 36 # 写入手指选择 (保持寄存器)
+ read_address = 52 # 读取压力数据 (输入寄存器)
+ read_count = 71 # 读取 71 个寄存器
+ skip_count = 0 # 不跳过数据点
+
+ # 0. 参数校验和手指写入值确定
+ if finger < 1 or finger > 5:
+ raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。")
+
+ finger_write_value = finger
+
+ # 1. 写入手指选择寄存器 (地址 36)
+ time.sleep(0.08)
+ wrsp = self.cli.write_register(address=write_address, value=finger_write_value, slave=self.slave)
+ if wrsp.isError():
+ raise RuntimeError(f"写入手指选择 {finger} 到地址 {write_address} 失败: {wrsp}")
+
+ # 写入后等待片刻
+ time.sleep(0.08)
+
+ # 2. 读取地址 52 的数据
+ rrsp = self.cli.read_input_registers(address=read_address, count=read_count, slave=self.slave)
+
+ if rrsp.isError():
+ raise RuntimeError(f"读取地址 {read_address} 压力数据失败: {rrsp}")
+
+ registers_16bit: List[int] = rrsp.registers
+
+ # 3. 核心数据处理
+ # a. 提取低 8 位数据 (得到 71 个 8 位数据点)
+ final_data_71 = [reg_value & 255 for reg_value in registers_16bit]
+
+ # b. 不跳过数据点 (按协议)
+ effective_data = np.array(final_data_71, dtype=np.uint8)
+
+ # c. 截取当前手指的矩阵数据 (71 个有效点中截取 72 个点，可能需要多读)
+ # 注: 协议返回 71 个点，手指数 1-5，每个手指需要 72 点
+ # 这里取全部数据
+ start_idx = 0
+ end_idx = min(len(effective_data), finger_size) # 取较小值
+
+ finger_data_flat = effective_data[start_idx:end_idx]
+
+ # d. 验证数据长度
+ if finger_data_flat.size < finger_size:
+ # 如果数据不足，尝试多读一些
+ rrsp2 = self.cli.read_input_registers(address=read_address + read_count, count=10, slave=self.slave)
+ if not rrsp2.isError():
+ extra_data = [reg_value & 255 for reg_value in rrsp2.registers]
+ finger_data_flat = np.concatenate([finger_data_flat, np.array(extra_data, dtype=np.uint8)])
+
+ # 最终确保有足够数据
+ if finger_data_flat.size >= finger_size:
+ finger_data_flat = finger_data_flat[:finger_size]
+ else:
+ raise ValueError(
+ f"数据提取失败。期望 {finger_size} 点 ({rows}x{cols})，"
+ f"但仅获取到 {finger_data_flat.size} 点。请检查协议。"
+ )
+
+ # e. 重塑为二维矩阵 (12 行 6 列)
+ finger_matrix = finger_data_flat.reshape((rows, cols))
+
+ return finger_matrix
+
+ def read_pressure_thumb(self) -> np.ndarray:
+ """读取大拇指压力数据"""
+ return np.array(self._pressure(1), dtype=np.uint8)
+
+ def read_pressure_index(self) -> np.ndarray:
+ """读取食指压力数据"""
+ return np.array(self._pressure(2), dtype=np.uint8)
+
+ def read_pressure_middle(self) -> np.ndarray:
+ """读取中指压力数据"""
+ return np.array(self._pressure(3), dtype=np.uint8)
+
+ def read_pressure_ring(self) -> np.ndarray:
+ """读取无名指压力数据"""
+ return np.array(self._pressure(4), dtype=np.uint8)
+
+ def read_pressure_little(self) -> np.ndarray:
+ """读取小拇指压力数据"""
+ return np.array(self._pressure(5), dtype=np.uint8)
+
+ # --------------------------------------------------
+ # 版本信息接口
+ # --------------------------------------------------
+
+ def read_versions(self) -> Dict[str, int]:
+ """读取版本信息 (输入寄存器 148-155)"""
+ result = self._read_input_registers(148, 8)
+
+ return {
+ "hand_freedom": result[0],
+ "hand_version": result[1],
+ "hand_number": result[2],
+ "hand_direction": result[3],
+ "software_version_major": result[4],
+ "software_version_minor": result[5] if len(result) > 5 else 0,
+ "software_version_revision": result[6] if len(result) > 6 else 0,
+ "hardware_version": result[7] if len(result) > 7 else 0
+ }
+
+ # --------------------------------------------------
+ # 写入接口
+ # --------------------------------------------------
+
+ def write_angles(self, vals: List[int]):
+ """设置6个关节角度 (保持寄存器 0-5)"""
+ vals = [int(x) for x in vals]
+ if not self.is_valid_6xuint8(vals):
+ raise ValueError("需要6个0-255的整数")
+ self._write_registers(0, vals)
+
+ def write_torques(self, vals: List[int]):
+ """设置6个关节转矩 (保持寄存器 6-11)"""
+ vals = [int(x) for x in vals]
+ if not self.is_valid_6xuint8(vals):
+ raise ValueError("需要6个0-255的整数")
+ self._write_registers(6, vals)
+
+ def write_speeds(self, vals: List[int]):
+ """设置6个关节速度 (保持寄存器 12-17)"""
+ vals = [int(x) for x in vals]
+ if not self.is_valid_6xuint8(vals):
+ raise ValueError("需要6个0-255的整数")
+ self._write_registers(12, vals)
+
+ # --------------------------------------------------
+ # 上下文管理
+ # --------------------------------------------------
+
+ def close(self):
+ """关闭连接"""
+ if self.connected:
+ self.cli.close()
+ self.connected = False
+
+ def __enter__(self):
+ return self
+
+ def __exit__(self, exc_type, exc_val, exc_tb):
+ self.close()
+
+ # --------------------------------------------------
+ # API固定接口函数
+ # --------------------------------------------------
+
+ def is_valid_6xuint8(self, lst) -> bool:
+ """验证6个0-255的整数列表"""
+ if len(lst) != 6:
+ return False
+ return all(isinstance(x, int) and 0 <= x <= 255 for x in lst)
+
+ def set_joint_positions(self, joint_angles=None):
+ """设置关节位置"""
+ joint_angles = joint_angles or [0] * 6
+ self.write_angles(joint_angles)
+
+ def set_speed(self, speed=None):
+ """设置速度"""
+ speed = speed or [200] * 6
+ self.write_speeds(speed)
+
+ def set_torque(self, torque=None):
+ """设置扭矩"""
+ torque = torque or [200] * 6
+ self.write_torques(torque)
+
+ def set_current(self, current=None):
+ """设置电流 (L6不支持)"""
+ print("当前L6不支持设置电流", flush=True)
+
+ def get_version(self) -> list:
+ """获取版本信息"""
+ versions = self.read_versions()
+ return [
+ versions.get("hand_freedom", 0),
+ versions.get("hand_version", 0),
+ versions.get("hand_number", 0),
+ versions.get("hand_direction", 0),
+ versions.get("software_version_major", 0),
+ versions.get("hardware_version", 0)
+ ]
+
+ def get_current(self):
+ """获取电流 (L6不支持)"""
+ print("当前L6不支持获取电流", flush=True)
+ return []
+
+ def get_state(self) -> list:
+ """获取关节状态"""
+ return self.read_angles()
+
+ def get_state_for_pub(self) -> list:
+ return self.get_state()
+
+ def get_current_status(self) -> list:
+ return self.get_state()
+
+ def get_speed(self) -> list:
+ """获取当前速度"""
+ return self.read_speeds()
+
+ def get_joint_speed(self) -> list:
+ return self.get_speed()
+
+ def get_touch_type(self) -> int:
+ """获取压感类型 (2=矩阵式)"""
+ return 2
+
+ def get_normal_force(self) -> list:
+ """获取压感数据：点式"""
+ return [-1] * 5
+
+ def get_tangential_force(self) -> list:
+ """获取压感数据：点式"""
+ return [-1] * 5
+
+ def get_approach_inc(self) -> list:
+ """获取压感数据：点式"""
+ return [-1] * 5
+
+ def get_touch(self) -> list:
+ return [-1] * 5
+
+ def get_thumb_matrix_touch(self,sleep_time=0):
+ return self._pressure(1)
+
+ def get_index_matrix_touch(self,sleep_time=0):
+ return self._pressure(2)
+
+ def get_middle_matrix_touch(self,sleep_time=0):
+ return self._pressure(3)
+
+ def get_ring_matrix_touch(self,sleep_time=0):
+ return self._pressure(4)
+
+ def get_little_matrix_touch(self,sleep_time=0):
+ return self._pressure(5)
+
+ def get_matrix_touch(self) -> list:
+ """获取压感数据：矩阵式"""
+ return [self._pressure(1), self._pressure(2), self._pressure(3),
+ self._pressure(4), self._pressure(5)]
+
+ def get_matrix_touch_v2(self) -> list:
+ """获取压感数据：矩阵式"""
+ return self.get_matrix_touch()
+
+ def get_torque(self) -> list:
+ """获取当前扭矩"""
+ return self.read_torques()
+
+ def get_temperature(self) -> list:
+ """获取当前电机温度"""
+ return self.read_temperatures()
+
+ def get_fault(self) -> list:
+ """获取当前电机故障码"""
+ return self.read_error_codes()
+
+ def get_serial_number(self):
+ return [0] * 6
+
+ def get_finger_order(self):
+ return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+
+ # --------------------------------------------------
+ # 便捷方法
+ # --------------------------------------------------
+
+ def relax(self):
+ """所有手指伸直"""
+ self.set_joint_positions([255] * 6)
+
+ def fist(self):
+ """所有手指握拳"""
+ self.set_joint_positions([0] * 6)
+
+ def dump_status(self):
+ """打印状态信息"""
+ print("=" * 50)
+ print("L6机械手状态信息")
+ print("=" * 50)
+
+ try:
+ # 关节状态
+ angles = self.read_angles()
+ torques = self.read_torques()
+ speeds = self.read_speeds()
+ temps = self.read_temperatures()
+ errors = self.read_error_codes()
+
+ print("关节状态:")
+ for i, name in enumerate(self.JOINT_NAMES):
+ print(f" {name:15s}: 角度={angles[i]:3d}, 扭矩={torques[i]:3d}, "
+ f"速度={speeds[i]:3d}, 温度={temps[i]:2d}℃, 错误={errors[i]:2d}")
+
+ # 版本信息
+ versions = self.read_versions()
+ print("\n版本信息:")
+ for key, value in versions.items():
+ print(f" {key:20s}: {value}")
+
+ # 压力传感器测试
+ print("\n压力传感器测试:")
+ thumb_pressure = self.read_pressure_thumb()
+ print(f"大拇指压力数据长度: {len(thumb_pressure)}")
+
+ except Exception as e:
+ print(f"读取状态时出错: {e}")
+
+ print("=" * 50)
+
+
+# ------------------- 演示程序 -------------------
+if __name__ == "__main__":
+ # 使用示例
+ try:
+ with LinkerHandL6RS485(hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200) as hand:
+ print("连接成功!")
+
+ # 打印状态信息
+ hand.dump_status()
+
+ # 测试基本控制
+ print("\n测试控制功能...")
+ print("伸直手指...")
+ hand.relax()
+ time.sleep(2)
+
+ print("握拳...")
+ hand.fist()
+ time.sleep(2)
+
+ print("恢复伸直...")
+ hand.relax()
+
+ # 测试压力传感器
+ print("\n测试压力传感器...")
+ thumb_matrix = hand.get_thumb_matrix_touch()
+ print(f"大拇指压力数据: {len(thumb_matrix)}个点")
+
+ # 获取所有手指压力数据
+ all_matrices = hand.get_matrix_touch()
+ for i, name in enumerate(hand.FINGER_NAMES):
+ matrix = all_matrices[i]
+ print(f"{name}手指压力数据长度: {len(matrix)}")
+
+ except Exception as e:
+ print(f"错误: {e}")
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py.bak b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py.bak
new file mode 100644
index 0000000..d68b621
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l6_rs485.py.bak
@@ -0,0 +1,444 @@
+#!/usr/bin/env python3
+import os
+import time
+from pymodbus.client import ModbusSerialClient
+from typing import List, Dict
+import numpy as np
+
+_INTERVAL = 0.006 # 8 ms
+
+class LinkerHandL6RS485:
+ """L6机械手 Modbus-RTU 控制类"""
+
+ # 6个关节名称
+ JOINT_NAMES = ["thumb_pitch", "thumb_yaw", "index_pitch",
+ "middle_pitch", "ring_pitch", "little_pitch"]
+
+ # 手指名称
+ FINGER_NAMES = ["thumb", "index", "middle", "ring", "little"]
+
+ def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
+ """
+ 初始化L6机械手
+ hand_id: 右手0x27(39), 左手0x28(40)
+ modbus_port: 串口设备路径
+ baudrate: 波特率，固定115200
+ """
+ self.slave = hand_id
+ self.cli = ModbusSerialClient(
+ port=modbus_port,
+ baudrate=baudrate,
+ bytesize=8,
+ parity="N",
+ stopbits=1,
+ timeout=0.05
+ )
+ # pymodbus 3.5.1 需要显式连接
+ self.connected = self.cli.connect()
+ if not self.connected:
+ raise ConnectionError(f"RS485连接失败，端口: {modbus_port}")
+
+ def _read_input_registers(self, address: int, count: int) -> List[int]:
+ """读取输入寄存器"""
+ time.sleep(_INTERVAL)
+ result = self.cli.read_input_registers(address=address, count=count, slave=self.slave)
+ if result.isError():
+ raise RuntimeError(f"读取输入寄存器失败: address={address}, count={count}")
+ return result.registers
+
+ def _write_register(self, address: int, value: int):
+ """写入单个寄存器"""
+ time.sleep(_INTERVAL)
+ result = self.cli.write_register(address=address, value=value, slave=self.slave)
+ if result.isError():
+ raise RuntimeError(f"写入寄存器失败: address={address}, value={value}")
+
+ def _write_registers(self, address: int, values: List[int]):
+ """写入多个寄存器"""
+ time.sleep(_INTERVAL)
+ result = self.cli.write_registers(address=address, values=values, slave=self.slave)
+ if result.isError():
+ raise RuntimeError(f"写入多个寄存器失败: address={address}, values={values}")
+
+ # --------------------------------------------------
+ # 基础读取接口
+ # --------------------------------------------------
+
+ def read_angles(self) -> List[int]:
+ """读取6个关节角度 (输入寄存器 0-5)"""
+ return self._read_input_registers(0, 6)
+
+ def read_torques(self) -> List[int]:
+ """读取6个关节转矩 (输入寄存器 6-11)"""
+ return self._read_input_registers(6, 6)
+
+ def read_speeds(self) -> List[int]:
+ """读取6个关节速度 (输入寄存器 12-17)"""
+ return self._read_input_registers(12, 6)
+
+ def read_temperatures(self) -> List[int]:
+ """读取6个关节温度 (输入寄存器 18-23)"""
+ return self._read_input_registers(18, 6)
+
+ def read_error_codes(self) -> List[int]:
+ """读取6个关节错误码 (输入寄存器 24-29)"""
+ return self._read_input_registers(24, 6)
+
+ # --------------------------------------------------
+ # 压力传感器接口
+ # --------------------------------------------------
+
+ # def _pressure(self, finger: int) -> List[int]:
+ # """内部：选手指 → 读压力数据"""
+ # # 选择手指 (保持寄存器 36)
+ # self._write_register(36, finger)
+ # time.sleep(_INTERVAL)
+ # # 读取压力数据 (输入寄存器 52-122)
+ # return np.array(self._read_input_registers(52, 71))
+ def _pressure(self, finger: int) -> np.ndarray:
+ """
+ 6x12 (72点) 矩阵尺寸。
+ Modbus 地址 60/62。
+ """
+ rows = 12 # 12 行
+ cols = 6 # 6 列
+ finger_size = rows * cols # 72 个数据点
+
+ # modbus 地址和计数
+ write_address = 60 # 写入手指选择
+ read_address = 62 # 读取压力数据
+ read_count = 96 # 读取 96 个寄存器
+ skip_count = 10 # 跳过前 10 个校验点
+
+ # 0. 参数校验和手指写入值确定
+ if finger < 1 or finger > 5:
+ raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。")
+
+ finger_write_value = finger
+
+ # 1. 写入手指选择寄存器 (地址 60)
+ time.sleep(0.008)
+ wrsp = self.cli.write_register(address=write_address, value=finger_write_value, slave=self.slave)
+ if wrsp.isError():
+ raise RuntimeError(f"写入手指选择 {finger} 到地址 {write_address} 失败: {wrsp}")
+
+ # 写入后等待片刻
+ time.sleep(0.008)
+
+ # 2. 读取地址 62 的数据
+ rrsp = self.cli.read_input_registers(address=read_address, count=read_count, slave=self.slave)
+
+ if rrsp.isError():
+ raise RuntimeError(f"读取地址 {read_address} 压力数据失败: {rrsp}")
+
+ registers_16bit: List[int] = rrsp.registers
+
+ # 3. 核心数据处理
+ # a. 提取低 8 位数据 (得到 96 个 8 位数据点)
+ final_data_96 = [reg_value & 255 for reg_value in registers_16bit]
+
+ # b. 跳过前 10 个校验/头部数据点 (得到 86 个有效数据点)
+ effective_data = np.array(final_data_96[skip_count:], dtype=np.uint8)
+ # c. 截取当前手指的矩阵数据 (从 86 个有效点中截取 72 个点)
+ start_idx = 0
+ end_idx = finger_size # 72
+
+ finger_data_flat = effective_data[start_idx:end_idx]
+
+ # d. 验证数据长度
+ if finger_data_flat.size != finger_size:
+ raise ValueError(
+ f"数据提取失败。期望 {finger_size} 点 ({rows}x{cols})，"
+ f"但仅截取到 {finger_data_flat.size} 点。请检查协议，确认地址 62 是否一次性返回了所有手指数据。"
+ )
+
+ # e. 重塑为二维矩阵 (12 行 6 列)
+ finger_matrix = finger_data_flat.reshape((rows, cols))
+
+ return finger_matrix
+
+ def read_pressure_thumb(self) -> np.ndarray:
+ """读取大拇指压力数据"""
+ return np.array(self._pressure(1), dtype=np.uint8)
+
+ def read_pressure_index(self) -> np.ndarray:
+ """读取食指压力数据"""
+ return np.array(self._pressure(2), dtype=np.uint8)
+
+ def read_pressure_middle(self) -> np.ndarray:
+ """读取中指压力数据"""
+ return np.array(self._pressure(3), dtype=np.uint8)
+
+ def read_pressure_ring(self) -> np.ndarray:
+ """读取无名指压力数据"""
+ return np.array(self._pressure(4), dtype=np.uint8)
+
+ def read_pressure_little(self) -> np.ndarray:
+ """读取小拇指压力数据"""
+ return np.array(self._pressure(5), dtype=np.uint8)
+
+ # --------------------------------------------------
+ # 版本信息接口
+ # --------------------------------------------------
+
+ def read_versions(self) -> Dict[str, int]:
+ """读取版本信息 (输入寄存器 148-155)"""
+ result = self._read_input_registers(148, 8)
+
+ return {
+ "hand_freedom": result[0],
+ "hand_version": result[1],
+ "hand_number": result[2],
+ "hand_direction": result[3],
+ "software_version_major": result[4],
+ "software_version_minor": result[5] if len(result) > 5 else 0,
+ "software_version_revision": result[6] if len(result) > 6 else 0,
+ "hardware_version": result[7] if len(result) > 7 else 0
+ }
+
+ # --------------------------------------------------
+ # 写入接口
+ # --------------------------------------------------
+
+ def write_angles(self, vals: List[int]):
+ """设置6个关节角度 (保持寄存器 0-5)"""
+ vals = [int(x) for x in vals]
+ if not self.is_valid_6xuint8(vals):
+ raise ValueError("需要6个0-255的整数")
+ self._write_registers(0, vals)
+
+ def write_torques(self, vals: List[int]):
+ """设置6个关节转矩 (保持寄存器 6-11)"""
+ vals = [int(x) for x in vals]
+ if not self.is_valid_6xuint8(vals):
+ raise ValueError("需要6个0-255的整数")
+ self._write_registers(6, vals)
+
+ def write_speeds(self, vals: List[int]):
+ """设置6个关节速度 (保持寄存器 12-17)"""
+ vals = [int(x) for x in vals]
+ if not self.is_valid_6xuint8(vals):
+ raise ValueError("需要6个0-255的整数")
+ self._write_registers(12, vals)
+
+ # --------------------------------------------------
+ # 上下文管理
+ # --------------------------------------------------
+
+ def close(self):
+ """关闭连接"""
+ if self.connected:
+ self.cli.close()
+ self.connected = False
+
+ def __enter__(self):
+ return self
+
+ def __exit__(self, exc_type, exc_val, exc_tb):
+ self.close()
+
+ # --------------------------------------------------
+ # API固定接口函数
+ # --------------------------------------------------
+
+ def is_valid_6xuint8(self, lst) -> bool:
+ """验证6个0-255的整数列表"""
+ if len(lst) != 6:
+ return False
+ return all(isinstance(x, int) and 0 <= x <= 255 for x in lst)
+
+ def set_joint_positions(self, joint_angles=None):
+ """设置关节位置"""
+ joint_angles = joint_angles or [0] * 6
+ self.write_angles(joint_angles)
+
+ def set_speed(self, speed=None):
+ """设置速度"""
+ speed = speed or [200] * 6
+ self.write_speeds(speed)
+
+ def set_torque(self, torque=None):
+ """设置扭矩"""
+ torque = torque or [200] * 6
+ self.write_torques(torque)
+
+ def set_current(self, current=None):
+ """设置电流 (L6不支持)"""
+ print("当前L6不支持设置电流", flush=True)
+
+ def get_version(self) -> list:
+ """获取版本信息"""
+ versions = self.read_versions()
+ return [
+ versions.get("hand_freedom", 0),
+ versions.get("hand_version", 0),
+ versions.get("hand_number", 0),
+ versions.get("hand_direction", 0),
+ versions.get("software_version_major", 0),
+ versions.get("hardware_version", 0)
+ ]
+
+ def get_current(self):
+ """获取电流 (L6不支持)"""
+ print("当前L6不支持获取电流", flush=True)
+ return []
+
+ def get_state(self) -> list:
+ """获取关节状态"""
+ return self.read_angles()
+
+ def get_state_for_pub(self) -> list:
+ return self.get_state()
+
+ def get_current_status(self) -> list:
+ return self.get_state()
+
+ def get_speed(self) -> list:
+ """获取当前速度"""
+ return self.read_speeds()
+
+ def get_joint_speed(self) -> list:
+ return self.get_speed()
+
+ def get_touch_type(self) -> int:
+ """获取压感类型 (2=矩阵式)"""
+ return 2
+
+ def get_normal_force(self) -> list:
+ """获取压感数据：点式"""
+ return [-1] * 5
+
+ def get_tangential_force(self) -> list:
+ """获取压感数据：点式"""
+ return [-1] * 5
+
+ def get_approach_inc(self) -> list:
+ """获取压感数据：点式"""
+ return [-1] * 5
+
+ def get_touch(self) -> list:
+ return [-1] * 5
+
+ def get_thumb_matrix_touch(self,sleep_time=0):
+ return self._pressure(1)
+
+ def get_index_matrix_touch(self,sleep_time=0):
+ return self._pressure(2)
+
+ def get_middle_matrix_touch(self,sleep_time=0):
+ return self._pressure(3)
+
+ def get_ring_matrix_touch(self,sleep_time=0):
+ return self._pressure(4)
+
+ def get_little_matrix_touch(self,sleep_time=0):
+ return self._pressure(5)
+
+ def get_matrix_touch(self) -> list:
+ """获取压感数据：矩阵式"""
+ return [self._pressure(1), self._pressure(2), self._pressure(3),
+ self._pressure(4), self._pressure(5)]
+
+ def get_matrix_touch_v2(self) -> list:
+ """获取压感数据：矩阵式"""
+ return self.get_matrix_touch()
+
+ def get_torque(self) -> list:
+ """获取当前扭矩"""
+ return self.read_torques()
+
+ def get_temperature(self) -> list:
+ """获取当前电机温度"""
+ return self.read_temperatures()
+
+ def get_fault(self) -> list:
+ """获取当前电机故障码"""
+ return self.read_error_codes()
+
+ def get_serial_number(self):
+ return [0] * 6
+
+ # --------------------------------------------------
+ # 便捷方法
+ # --------------------------------------------------
+
+ def relax(self):
+ """所有手指伸直"""
+ self.set_joint_positions([255] * 6)
+
+ def fist(self):
+ """所有手指握拳"""
+ self.set_joint_positions([0] * 6)
+
+ def dump_status(self):
+ """打印状态信息"""
+ print("=" * 50)
+ print("L6机械手状态信息")
+ print("=" * 50)
+
+ try:
+ # 关节状态
+ angles = self.read_angles()
+ torques = self.read_torques()
+ speeds = self.read_speeds()
+ temps = self.read_temperatures()
+ errors = self.read_error_codes()
+
+ print("关节状态:")
+ for i, name in enumerate(self.JOINT_NAMES):
+ print(f" {name:15s}: 角度={angles[i]:3d}, 扭矩={torques[i]:3d}, "
+ f"速度={speeds[i]:3d}, 温度={temps[i]:2d}℃, 错误={errors[i]:2d}")
+
+ # 版本信息
+ versions = self.read_versions()
+ print("\n版本信息:")
+ for key, value in versions.items():
+ print(f" {key:20s}: {value}")
+
+ # 压力传感器测试
+ print("\n压力传感器测试:")
+ thumb_pressure = self.read_pressure_thumb()
+ print(f"大拇指压力数据长度: {len(thumb_pressure)}")
+
+ except Exception as e:
+ print(f"读取状态时出错: {e}")
+
+ print("=" * 50)
+
+
+# ------------------- 演示程序 -------------------
+if __name__ == "__main__":
+ # 使用示例
+ try:
+ with LinkerHandL6RS485(hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200) as hand:
+ print("连接成功!")
+
+ # 打印状态信息
+ hand.dump_status()
+
+ # 测试基本控制
+ print("\n测试控制功能...")
+ print("伸直手指...")
+ hand.relax()
+ time.sleep(2)
+
+ print("握拳...")
+ hand.fist()
+ time.sleep(2)
+
+ print("恢复伸直...")
+ hand.relax()
+
+ # 测试压力传感器
+ print("\n测试压力传感器...")
+ thumb_matrix = hand.get_thumb_matrix_touch()
+ print(f"大拇指压力数据: {len(thumb_matrix)}个点")
+
+ # 获取所有手指压力数据
+ all_matrices = hand.get_matrix_touch()
+ for i, name in enumerate(hand.FINGER_NAMES):
+ matrix = all_matrices[i]
+ print(f"{name}手指压力数据长度: {len(matrix)}")
+
+ except Exception as e:
+ print(f"错误: {e}")
\ No newline at end of file
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l7_rs485.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l7_rs485.py
new file mode 100644
index 0000000..274a1ae
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_l7_rs485.py
@@ -0,0 +1,423 @@
+#!/usr/bin/env python3
+import time
+from typing import List, Dict, Union
+import numpy as np
+from pymodbus.client import ModbusSerialClient
+from pymodbus.exceptions import ModbusException
+
+# --- 协议常量和寄存器地址定义 (根据 O7 协议文件) ---
+
+# RS485 通信设置
+DEFAULT_BAUDRATE = 115200
+
+# O7机械手七个可控关节的键名 (根据保持寄存器和输入寄存器地址 0-6)
+O7_JOINT_KEYS = [
+ "Thumb_Pitch", "Thumb_Yaw", "Index_Pitch", "Middle_Pitch",
+ "Ring_Pitch", "Little_Pitch", "Thumb_Roll"
+]
+
+# 保持寄存器地址 (写操作 FC 16)
+HR_ADDR = {
+ "Position_Start": 0, # 关节目标位置 (7 个寄存器: 0-6)
+ "Torque_Start": 7, # 关节目标转矩 (7 个寄存器: 7-13)
+ "Speed_Start": 14, # 关节目标速度 (7 个寄存器: 14-20)
+ "Pressure_Select": 42 # 压力传感器数据选择 (1 个寄存器)
+ # 21-41 为堵转保护阈值、时间和扭矩，暂未实现
+}
+
+# 输入寄存器地址 (读操作 FC 04)
+IR_ADDR = {
+ "Current_Position_Start": 0, # 当前关节位置 (7 个寄存器: 0-6)
+ "Current_Torque_Start": 7, # 当前关节转矩 (7 个寄存器: 7-13)
+ "Current_Speed_Start": 14, # 当前关节速度 (7 个寄存器: 14-20)
+ "Current_Temperature_Start": 21, # 当前关节温度 (7 个寄存器: 21-27)
+ "Error_Code_Start": 28, # 当前关节错误码 (7 个寄存器: 28-34)
+ "Tip_Force_Start": 35, # 指尖力数据 (20 个寄存器: 35-54)
+ "Pressure_Data_Start": 57, # 压力传感器数据起始 (96 个寄存器: 57-152)
+ "Version_Start": 153 # 版本信息 (6 个寄存器: 153-158)
+}
+
+# 辅助常量
+_JOINT_COUNT = 7
+_VERSION_COUNT = 6
+_TIP_FORCE_COUNT = 20
+_PRESSURE_REG_COUNT = 96
+_PRESSURE_ROWS = 12 # 从 IR 56 (0xC6) 推断
+_PRESSURE_COLS = 6 # 从 IR 56 (0xC6) 推断
+_PRESSURE_DATA_SIZE = _PRESSURE_ROWS * _PRESSURE_COLS # 72
+_PRESSURE_HEADER_SKIP = 10 # 假设跳过 10 个头部/校验字节
+
+# 通信间隔时间 (使用 L10 参考中的 5ms)
+_INTERVAL = 0.005
+
+
+class LinkerHandL7RS485:
+ """
+ O7机械手 Modbus RTU (RS485) 控制类。
+ 使用 pymodbus 3.5.1 版本和 O7 机械手协议。
+ """
+ def __init__(self,
+ hand_id: int = 0x27,
+ modbus_port: str = "/dev/ttyUSB0",
+ baudrate: int = DEFAULT_BAUDRATE,
+ timeout: float = 0.05):
+ """
+ 初始化 Modbus 客户端。
+
+ :param hand_id: Modbus 从站地址 (0x27: 右手, 0x28: 左手)
+ :param modbus_port: 串口名称
+ :param baudrate: 波特率 (默认为 115200)
+ :param timeout: 通信超时时间 (秒)
+ """
+ self.slave = hand_id
+ self.cli = ModbusSerialClient(
+ port=modbus_port,
+ baudrate=baudrate,
+ bytesize=8,
+ parity="N",
+ stopbits=1, # 确保与 pymodbus 3.x 兼容的写法
+ timeout=timeout,
+ retries=3, # 重试次数
+ retry_on_empty=True,
+ handle_local_echo=False,
+ method='rtu'
+ )
+
+ # 尝试连接
+ self.connected = self.cli.connect()
+ if not self.connected:
+ raise ConnectionError(f"RS485 connect fail to {modbus_port} with ID {hex(hand_id)}.")
+
+ print(f"O7机械手 Modbus ID {hex(hand_id)} 连接成功到 {modbus_port}。")
+
+
+ # --------------------------------------------------
+ # 核心读写函数 (基于 pymodbus 3.5.1)
+ # --------------------------------------------------
+
+ def _read_input_registers(self, address: int, count: int) -> List[int]:
+ """封装 Modbus 读取输入寄存器 (FC 04) 操作。"""
+ time.sleep(_INTERVAL)
+ try:
+ rsp = self.cli.read_input_registers(
+ address=address,
+ count=count,
+ slave=self.slave
+ )
+
+ # 使用 L10 参考中验证过的 3.x 兼容错误检查
+ if rsp.isError():
+ raise RuntimeError(f"Modbus FC04 读取失败。地址: {address}, 错误: {rsp}")
+
+ return rsp.registers
+
+ except ModbusException as e:
+ # 捕获通信超时、CRC 错误等 Modbus 异常
+ raise RuntimeError(f"Modbus 通信异常。地址: {address}, 错误: {e}")
+ except Exception as e:
+ raise RuntimeError(f"未知读取异常。地址: {address}, 错误: {e}")
+
+ def _write_holding_registers(self, address: int, values: List[int]):
+ """封装 Modbus 写入保持寄存器 (FC 16) 操作。"""
+ time.sleep(_INTERVAL)
+
+ # 批量写入 (FC 16)
+ if len(values) > 1:
+ write_func = self.cli.write_registers
+ # 单个写入 (FC 06)
+ elif len(values) == 1:
+ write_func = lambda address, values, slave: self.cli.write_register(address, values[0], slave)
+ else:
+ raise ValueError("写入值列表不能为空。")
+
+ try:
+ rsp = write_func(
+ address=address,
+ values=values,
+ slave=self.slave
+ )
+
+ if rsp.isError():
+ raise RuntimeError(f"Modbus FC16 写入失败。地址: {address}, 错误: {rsp}")
+
+ except ModbusException as e:
+ raise RuntimeError(f"Modbus 通信异常。地址: {address}, 错误: {e}")
+ except Exception as e:
+ raise RuntimeError(f"未知写入异常。地址: {address}, 错误: {e}")
+
+ # --------------------------------------------------
+ # 读操作 (Read API)
+ # --------------------------------------------------
+
+ def get_joint_positions(self) -> Dict[str, int]:
+ """读取当前关节位置 (地址 0-6)。"""
+ registers = self._read_input_registers(IR_ADDR["Current_Position_Start"], _JOINT_COUNT)
+ #return dict(zip(O7_JOINT_KEYS, registers))
+ return registers
+
+ def get_current_torques(self) -> Dict[str, int]:
+ """读取当前关节转矩 (地址 7-13)。"""
+ registers = self._read_input_registers(IR_ADDR["Current_Torque_Start"], _JOINT_COUNT)
+ #return dict(zip(O7_JOINT_KEYS, registers))
+ return registers
+
+ def get_current_speeds(self) -> Dict[str, int]:
+ """读取当前关节速度 (地址 14-20)。"""
+ registers = self._read_input_registers(IR_ADDR["Current_Speed_Start"], _JOINT_COUNT)
+ #return dict(zip(O7_JOINT_KEYS, registers))
+ return registers
+
+ def get_temperatures(self) -> Dict[str, int]:
+ """读取当前关节温度 (地址 21-27)。"""
+ registers = self._read_input_registers(IR_ADDR["Current_Temperature_Start"], _JOINT_COUNT)
+ #return dict(zip(O7_JOINT_KEYS, registers))
+ return registers
+
+ def get_error_codes(self) -> Dict[str, int]:
+ """读取当前关节错误码 (地址 28-34)。"""
+ registers = self._read_input_registers(IR_ADDR["Error_Code_Start"], _JOINT_COUNT)
+ #return dict(zip(O7_JOINT_KEYS, registers))
+ return registers
+
+ def get_tip_forces(self) -> List[int]:
+ """读取指尖法向力、切向力等数据 (地址 35-54)。"""
+ return self._read_input_registers(IR_ADDR["Tip_Force_Start"], _TIP_FORCE_COUNT)
+
+ def get_version(self) -> List[int]:
+ """读取版本信息 (地址 153-158)。"""
+ return self._read_input_registers(IR_ADDR["Version_Start"], _VERSION_COUNT)
+
+ def get_pressure_matrix(self, finger_id: int) -> np.ndarray:
+ """
+ 读取特定手指的压力传感器数据矩阵。
+
+ :param finger_id: 手指编号 (1: 大拇指, 2: 食指, 3: 中指, 4: 无名指, 5: 小拇指)
+ :return: 12x6 的压力数据矩阵 (np.ndarray)
+ """
+ if not (1 <= finger_id <= 5):
+ raise ValueError(f"无效的手指编号: {finger_id}。应在 1 到 5 之间。")
+
+ # 1. 写入手指选择寄存器 (HR 42)
+ # 使用单个写入 (FC 06)
+ self._write_holding_registers(HR_ADDR["Pressure_Select"], [finger_id])
+
+ # 2. 读取压力传感器数据 (IR 57, 96 个寄存器)
+ time.sleep(_INTERVAL) # 等待数据更新
+ registers_16bit: List[int] = self._read_input_registers(
+ IR_ADDR["Pressure_Data_Start"],
+ _PRESSURE_REG_COUNT
+ )
+
+ # 3. 数据解析 (假设与 L10 类似的数据格式: 低 8 位有效，有头部数据)
+
+ # a. 提取低 8 位数据 (得到 96 个 8 位数据点)
+ final_data_96 = [reg_value & 255 for reg_value in registers_16bit]
+
+ # b. 跳过头部数据点
+ effective_data = np.array(final_data_96[_PRESSURE_HEADER_SKIP:], dtype=np.uint8)
+
+ # c. 截取当前手指的矩阵数据 (72 个点)
+ finger_data_flat = effective_data[:_PRESSURE_DATA_SIZE]
+
+ if finger_data_flat.size != _PRESSURE_DATA_SIZE:
+ raise ValueError(
+ f"压力数据提取失败。期望 {_PRESSURE_DATA_SIZE} 点，"
+ f"但仅截取到 {finger_data_flat.size} 点。请检查协议解析逻辑。"
+ )
+
+ # d. 重塑为二维矩阵 (12 行 6 列)
+ finger_matrix = finger_data_flat.reshape((_PRESSURE_ROWS, _PRESSURE_COLS))
+ return finger_matrix
+
+
+ # --------------------------------------------------
+ # 写操作 (Write API)
+ # --------------------------------------------------
+
+ def set_joint_positions(self, joint_angles: List[int]):
+ """
+ 设置所有 7 个关节的目标位置 (地址 0-6)。
+ :param joint_angles: 7 个 0-255 的整数值列表
+ """
+ if len(joint_angles) != _JOINT_COUNT:
+ raise ValueError(f"需要 {_JOINT_COUNT} 个关节位置值，提供了 {len(joint_angles)} 个。")
+ self._write_holding_registers(HR_ADDR["Position_Start"], joint_angles)
+
+ def set_torques(self, torques: List[int]):
+ """
+ 设置所有 7 个关节的目标转矩 (地址 7-13)。
+ :param torques: 7 个 0-255 的整数值列表
+ """
+ if len(torques) != _JOINT_COUNT:
+ raise ValueError(f"需要 {_JOINT_COUNT} 个关节转矩值，提供了 {len(torques)} 个。")
+ self._write_holding_registers(HR_ADDR["Torque_Start"], torques)
+
+
+ def set_speeds(self, speeds: List[int]):
+ """
+ 设置所有 7 个关节的目标速度 (地址 14-20)。
+ :param speeds: 7 个 0-255 的整数值列表
+ """
+ if len(speeds) != _JOINT_COUNT:
+ raise ValueError(f"需要 {_JOINT_COUNT} 个关节速度值，提供了 {len(speeds)} 个。")
+ self._write_holding_registers(HR_ADDR["Speed_Start"], speeds)
+
+ def set_speed(self, speed:List[int] = [200] * 7):
+ self.set_speeds(speed)
+
+ def set_torque(self, torque: List[int] = [250] * 7):
+ self.set_torques(torque)
+
+ def set_current(self, current=None):
+ print("当前L7不支持设置电流", flush=True)
+
+ def get_current(self):
+ #print("当前L7不支持获取电流", flush=True)
+ return [-1] * 7
+
+ def get_state(self) -> List[int]:
+ return self.get_joint_positions()
+
+
+ def get_state_for_pub(self) -> List[int]:
+ return self.get_joint_positions()
+
+ def get_current_status(self) -> List[int]:
+ return self.get_joint_positions()
+
+ def get_speed(self) -> List[int]:
+ return self.get_current_speeds()
+
+ def get_joint_speed(self) -> List[int]:
+ return self.get_speed()
+
+ def get_touch_type(self) -> int:
+ return 2
+
+ def get_normal_force(self) -> List[int]:
+ return [-1] * 5
+
+ def get_tangential_force(self) -> List[int]:
+ return [-1] * 5
+
+ def get_approach_inc(self) -> List[int]:
+ return [-1] * 5
+
+ def get_touch(self) -> List[int]:
+ return [-1] * 5
+
+ def get_thumb_matrix_touch(self,sleep_time=0):
+ return self.get_pressure_matrix(finger_id=1)
+
+ def get_index_matrix_touch(self,sleep_time=0):
+ return self.get_pressure_matrix(finger_id=2)
+
+ def get_middle_matrix_touch(self,sleep_time=0):
+ return self.get_pressure_matrix(finger_id=3)
+
+ def get_ring_matrix_touch(self,sleep_time=0):
+ return self.get_pressure_matrix(finger_id=4)
+
+ def get_little_matrix_touch(self,sleep_time=0):
+ return self.get_pressure_matrix(finger_id=5)
+
+ def get_matrix_touch(self) -> List[List[int]]:
+ return self.get_thumb_matrix_touch(),self.get_index_matrix_touch(), self.get_middle_matrix_touch(), self.get_ring_matrix_touch(), self.get_little_matrix_touch()
+
+ def get_matrix_touch_v2(self) -> List[List[int]]:
+ return self.get_matrix_touch()
+
+ def get_torque(self) -> List[int]:
+ return self.get_current_torques()
+
+ def get_temperature(self) -> List[int]:
+ return self.get_temperatures()
+
+ def get_fault(self) -> List[int]:
+ return self.get_error_codes()
+
+ def get_serial_number(self):
+ return [0] * 6
+
+ def get_finger_order(self):
+ return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch", "thumb_cmc_roll"]
+
+ def show_fun_table(self):
+ pass
+
+ def clear_faults(self):
+ pass
+ # --------------------------------------------------
+ # 上下文管理
+ # --------------------------------------------------
+
+ def close(self):
+ """断开 Modbus 连接。"""
+ if self.connected:
+ self.cli.close()
+ self.connected = False
+ print("Modbus 连接已断开。")
+
+ def __enter__(self):
+ return self
+
+ def __exit__(self, exc_type, exc_val, exc_tb):
+ self.close()
+
+
+# ------------------- Demo/使用示例 -------------------
+if __name__ == "__main__":
+ # --- 配置区域 ---
+ # 右手 Modbus ID: 0x27 (39)
+ # 左手 Modbus ID: 0x28 (40)
+ TARGET_HAND_ID = 0x28 # <--- 请根据需要修改为 0x27 或 0x28
+ PORT = "/dev/ttyUSB0" # <--- 请修改为您的实际串口，例如 'COM3'
+
+ try:
+ # 使用上下文管理器，确保连接自动关闭
+ with LinkerHandL7RS485(hand_id=TARGET_HAND_ID, modbus_port=PORT) as hand:
+ print("\n--- 1. 读取当前状态 ---")
+
+ # 读取当前关节位置、速度、转矩
+ angles = hand.get_joint_positions()
+ print(f"当前关节位置 (7DOF): {angles}")
+
+ speeds = hand.get_current_speeds()
+ print(f"当前关节速度: {speeds}")
+
+ # 读取传感器和错误信息
+ temps = hand.get_temperatures()
+ print(f"关节温度: {temps}")
+
+ errors = hand.get_error_codes()
+ print(f"关节错误码: {errors}")
+
+ # 读取版本
+ version_info = hand.get_version()
+ print(f"版本信息 (Hand_freedom, ..., hardware_version): {version_info}")
+
+ # --- 2. 写入指令示例 ---
+ print("\n--- 2. 写入指令示例 (设置所有关节到 128) ---")
+
+ # 假设要将所有关节位置设置到中间值 128
+ target_angles = [128] * _JOINT_COUNT
+ hand.set_joint_positions(target_angles)
+ print(f"写入目标角度: {target_angles}")
+
+ # 假设要设置所有关节的速度到 100
+ target_speeds = [100] * _JOINT_COUNT
+ hand.set_speeds(target_speeds)
+ print(f"写入目标速度: {target_speeds}")
+
+ # --- 3. 压力传感器读取示例 ---
+ print("\n--- 3. 压力传感器读取 (大拇指 1) ---")
+ thumb_matrix = hand.get_pressure_matrix(finger_id=1)
+ print(f"大拇指压力矩阵 (12x6):")
+ print(thumb_matrix)
+
+ except ConnectionError as e:
+ print(f"致命错误: 连接失败。{e}")
+ except RuntimeError as e:
+ print(f"致命错误: Modbus 操作失败。{e}")
+ except Exception as e:
+ print(f"捕获到未知异常: {e}")
\ No newline at end of file
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py
new file mode 100644
index 0000000..98240fd
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py
@@ -0,0 +1,671 @@
+#!/usr/bin/env python3
+"""
+O6 机械手 Modbus-RTU 控制类 (基于 pymodbus 3.5.1)
+"""
+
+import os
+import time
+from typing import List, Dict, Any # 引入 Any 来表示灵活的输入类型
+import numpy as np
+import logging
+from threading import Lock # 用于线程安全和总线仲裁
+
+# 导入 pymodbus 客户端
+from pymodbus.client import ModbusSerialClient
+from struct import error as StructError
+
+logging.basicConfig(
+ level=logging.INFO,
+ format="[%(asctime)s] %(levelname)-8s %(message)s",
+ datefmt="%H:%M:%S"
+)
+
+# ------------------------------------------------------------------
+# 读输入寄存器地址枚举（功能码 04，只读）- 按照 O6 协议文档定义
+# ------------------------------------------------------------------
+REG_RD_CURRENT_THUMB_PITCH = 0 # 大拇指弯曲角度（0-255，小=弯，大=伸）
+REG_RD_CURRENT_THUMB_YAW = 1 # 大拇指横摆角度（0-255，小=靠掌心，大=远离）
+REG_RD_CURRENT_INDEX_PITCH = 2 # 食指弯曲角度
+REG_RD_CURRENT_MIDDLE_PITCH = 3 # 中指弯曲角度
+REG_RD_CURRENT_RING_PITCH = 4 # 无名指弯曲角度
+REG_RD_CURRENT_LITTLE_PITCH = 5 # 小拇指弯曲角度
+REG_RD_CURRENT_THUMB_TORQUE = 6 # 大拇指弯曲转矩（0-255）
+REG_RD_CURRENT_THUMB_YAW_TORQUE = 7 # 大拇指横摆转矩
+REG_RD_CURRENT_INDEX_TORQUE = 8 # 食指转矩
+REG_RD_CURRENT_MIDDLE_TORQUE = 9 # 中指转矩
+REG_RD_CURRENT_RING_TORQUE = 10 # 无名指转矩
+REG_RD_CURRENT_LITTLE_TORQUE = 11 # 小拇指转矩
+REG_RD_CURRENT_THUMB_SPEED = 12 # 大拇指弯曲速度（0-255）
+REG_RD_CURRENT_THUMB_YAW_SPEED = 13 # 大拇指横摆速度
+REG_RD_CURRENT_INDEX_SPEED = 14 # 食指速度
+REG_RD_CURRENT_MIDDLE_SPEED = 15 # 中指速度
+REG_RD_CURRENT_RING_SPEED = 16 # 无名指速度
+REG_RD_CURRENT_LITTLE_SPEED = 17 # 小拇指速度
+REG_RD_THUMB_TEMP = 18 # 大拇指弯曲温度（0-70℃）
+REG_RD_THUMB_YAW_TEMP = 19 # 大拇指横摆温度
+REG_RD_INDEX_TEMP = 20 # 食指温度
+REG_RD_MIDDLE_TEMP = 21 # 中指温度
+REG_RD_RING_TEMP = 22 # 无名指温度
+REG_RD_LITTLE_TEMP = 23 # 小拇指温度
+REG_RD_THUMB_ERROR = 24 # 大拇指错误码
+REG_RD_THUMB_YAW_ERROR = 25 # 大拇指横摆错误码
+REG_RD_INDEX_ERROR = 26 # 食指错误码
+REG_RD_MIDDLE_ERROR = 27 # 中指错误码
+REG_RD_RING_ERROR = 28 # 无名指错误码
+REG_RD_LITTLE_ERROR = 29 # 小拇指错误码
+
+# 版本号/设备编号寄存器（地址 30-44，共15个寄存器）
+REG_RD_HAND_FREEDOM = 30 # Hand_freedom - 设备编号 / 自由度（与机械手上标签相同）
+REG_RD_HAND_VERSION = 31 # hand_version - 手版本
+REG_RD_HAND_NUMBER_HIGH = 32 # hand_number_高位 - 设备编号（高字节）
+REG_RD_HAND_NUMBER_MID = 33 # hand_number_中位 - 设备编号（中字节）
+REG_RD_HAND_NUMBER_LOW = 34 # hand_number_低位 - 设备编号（低字节）
+REG_RD_HAND_DIRECTION = 35 # hand_direction - 手方向（左/右）
+REG_RD_HARDWARE_VERSION_HIGH = 36 # hardware_version_高位 - 硬件版本（高字节）
+REG_RD_HARDWARE_VERSION_MID = 37 # hardware_version_中位 - 硬件版本（中字节）
+REG_RD_HARDWARE_VERSION_LOW = 38 # hardware_version_低位 - 硬件版本（低字节）
+REG_RD_SOFTWARE_VERSION_HIGH = 39 # software_version_高位 - 软件版本（高字节）
+REG_RD_SOFTWARE_VERSION_MID = 40 # software_version_中位 - 软件版本（中字节）
+REG_RD_SOFTWARE_VERSION_LOW = 41 # software_version_低位 - 软件版本（低字节）
+REG_RD_MECHANICAL_VERSION_HIGH = 42 # mechanical_version_高位 - 机械版本（高字节）
+REG_RD_MECHANICAL_VERSION_MID = 43 # mechanical_version_中位 - 机械版本（中字节）
+REG_RD_MECHANICAL_VERSION_LOW = 44 # mechanical_version_低位 - 机械版本（低字节）
+
+# 力传感器寄存器（地址 45-87+，动态范围）
+REG_RD_PRESSURE_SENSING_ID = 45 # Pressure_Sensing_ID - 压力传感器ID (0-5)
+REG_RD_PRESSURE_SENSING_SPEC = 46 # Pressure_Sensing_Specifications - 传感器数据规格
+
+
+# ------------------------------------------------------------------
+# 写保持寄存器地址枚举（功能码 16，读写）- 保持原样
+# ------------------------------------------------------------------
+REG_WR_THUMB_PITCH = 0 # 大拇指弯曲角度（0-255）
+REG_WR_THUMB_YAW = 1 # 大拇指横摆角度
+REG_WR_INDEX_PITCH = 2 # 食指弯曲角度
+REG_WR_MIDDLE_PITCH = 3 # 中指弯曲角度
+REG_WR_RING_PITCH = 4 # 无名指弯曲角度
+REG_WR_LITTLE_PITCH = 5 # 小拇指弯曲角度
+REG_WR_THUMB_TORQUE = 6 # 大拇指弯曲转矩
+REG_WR_THUMB_YAW_TORQUE = 7 # 大拇指横摆转矩
+REG_WR_INDEX_TORQUE = 8 # 食指转矩
+REG_WR_MIDDLE_TORQUE = 9 # 中指转矩
+REG_WR_RING_TORQUE = 10 # 无名指转矩
+REG_WR_LITTLE_TORQUE = 11 # 小拇指转矩
+REG_WR_THUMB_SPEED = 12 # 大拇指弯曲速度
+REG_WR_THUMB_YAW_SPEED = 13 # 大拇指横摆速度
+REG_WR_INDEX_SPEED = 14 # 食指速度
+REG_WR_MIDDLE_SPEED = 15 # 中指速度
+REG_WR_RING_SPEED = 16 # 无名指速度
+REG_WR_LITTLE_SPEED = 17 # 小拇指速度
+
+
+class LinkerHandO6RS485:
+ """O6 机械手 Modbus-RTU 控制类，使用 pymodbus 3.5.1"""
+
+ TTL_TIMEOUT = 0.15 # 串口超时
+ FRAME_GAP = 0.030 # 30 ms
+
+ # KEYS for easy indexing
+ JOINT_KEYS = ["thumb_pitch", "thumb_yaw", "index_pitch",
+ "middle_pitch", "ring_pitch", "little_pitch"]
+
+ def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
+ self._id = hand_id
+ self._last_ts = 0.0 # 上一次帧结束时间
+ self._lock = Lock() # 总线访问锁
+
+ # 使用 pymodbus 3.x 客户端
+ self.cli = ModbusSerialClient(
+ port=modbus_port,
+ baudrate=baudrate,
+ bytesize=8,
+ parity="N",
+ stopbits=1,
+ timeout=self.TTL_TIMEOUT,
+ handle_local_echo=False
+ )
+
+ try:
+ logging.info(f"Connecting to Modbus RTU on {modbus_port}...")
+ self.connected = self.cli.connect()
+ if not self.connected:
+ raise ConnectionError(f"RS485 connect fail to {modbus_port}")
+ logging.info("Connection successful.")
+ except Exception as e:
+ logging.error(f"Initialization failed: {e}")
+ raise
+
+ # ----------------------------------------------------------
+ # 辅助方法
+ # ----------------------------------------------------------
+ def _bus_free(self):
+ """保证距离上一帧 ≥ 30 ms"""
+ with self._lock:
+ elapse = time.perf_counter() - self._last_ts
+ if elapse < self.FRAME_GAP:
+ time.sleep(self.FRAME_GAP - elapse)
+
+ def _execute_read(self, address: int, count: int) -> List[int]:
+ """执行 Modbus 读取操作 (功能码 04), 带总线仲裁。"""
+ self._bus_free()
+
+ rsp = self.cli.read_input_registers(
+ address=address,
+ count=count,
+ slave=self._id
+ )
+
+ self._last_ts = time.perf_counter()
+
+ if rsp.isError():
+ raise RuntimeError(f"Modbus Read Failed (Addr={address}, Count={count}): {rsp}")
+
+ # 确保返回的值是 Python 原生整数
+ return [int(reg) for reg in rsp.registers]
+
+ def _execute_write(self, address: int, values: List[int]):
+ """执行 Modbus 批量写入操作 (功能码 16), 带总线仲裁。"""
+ self._bus_free()
+
+ # values 必须是 Python 原生整数列表
+ rsp = self.cli.write_registers(
+ address=address,
+ values=values,
+ slave=self._id
+ )
+
+ self._last_ts = time.perf_counter()
+
+ if rsp.isError():
+ raise RuntimeError(f"Modbus Write Failed (Addr={address}, Values={values}): {rsp}")
+
+ # ----------------------------------------------------------
+ # 批量读取和数据封装（优化通信效率）
+ # ----------------------------------------------------------
+ def read_all_angles(self) -> List[int]:
+ return self._execute_read(REG_RD_CURRENT_THUMB_PITCH, 6)
+
+ def read_all_torques(self) -> List[int]:
+ return self._execute_read(REG_RD_CURRENT_THUMB_TORQUE, 6)
+
+ def read_all_speeds(self) -> List[int]:
+ return self._execute_read(REG_RD_CURRENT_THUMB_SPEED, 6)
+
+ def read_all_temperatures(self) -> List[int]:
+ return self._execute_read(REG_RD_THUMB_TEMP, 6)
+
+ def read_all_errors(self) -> List[int]:
+ return self._execute_read(REG_RD_THUMB_ERROR, 6)
+
+ # ----------------------------------------------------------
+ # 版本号/设备编号读取（按照协议文档：地址30-44，共15个寄存器）
+ # ----------------------------------------------------------
+ def read_all_versions(self) -> str:
+ """一次性读取全部15个寄存器 (地址30-44)，返回以 '.' 连接的字符串。
+
+ 按 O6 协议文档的版本号格式返回：
+ hand_freedom.hand_version.hand_number_high.hand_number_mid.hand_number_low
+ .hand_direction.hardware_ver_hardware_ver_m.hardware_ver_l
+ .software_ver_h.software_ver_m.software_ver_l
+ .mechanical_ver_h.mechanical_ver_m.mechanical_ver_l
+
+ 例如: "6.1.001.002.003.0.1.2.3.4.5.6.7.8.9"
+ """
+ raw = self._execute_read(REG_RD_HAND_FREEDOM, 15)
+ return ".".join(str(v) for v in raw)
+
+ # ----------------------------------------------------------
+ # 基于 read_all_versions() 的设备编号解析方法
+ # ----------------------------------------------------------
+ def _parse_versions(self):
+ """解析 read_all_versions() 返回的字符串为字典"""
+ parts = self.read_all_versions().split(".")
+ if len(parts) < 15:
+ return {}
+ return {
+ "hand_freedom": int(parts[0]),
+ "hand_version": int(parts[1]),
+ "hand_number_high": int(parts[2]),
+ "hand_number_mid": int(parts[3]),
+ "hand_number_low": int(parts[4]),
+ "hand_direction": int(parts[5]),
+ "hw_ver_high": int(parts[6]),
+ "hw_ver_mid": int(parts[7]),
+ "hw_ver_low": int(parts[8]),
+ "sw_ver_high": int(parts[9]),
+ "sw_ver_mid": int(parts[10]),
+ "sw_ver_low": int(parts[11]),
+ "mech_ver_high": int(parts[12]),
+ "mech_ver_mid": int(parts[13]),
+ "mech_ver_low": int(parts[14]),
+ }
+
+ def get_device_number(self) -> str:
+ """获取设备编号（与机械手上标签相同）。格式：高位+中位+低位 拼接的字符串。"""
+ v = self._parse_versions()
+ if not v:
+ return "0"
+ high, mid, low = v["hand_number_high"], v["hand_number_mid"], v["hand_number_low"]
+ # 将每个字节格式化为无前导零的整数（与标签显示一致）
+ return f"{high}{mid}{low}"
+
+ def get_device_number_value(self) -> int:
+ """获取设备编号数值"""
+ v = self._parse_versions()
+ if not v:
+ return 0
+ high, mid, low = v["hand_number_high"], v["hand_number_mid"], v["hand_number_low"]
+ return high * 65536 + mid * 256 + low
+
+ def get_hardware_version(self) -> str:
+ """获取硬件版本号。格式：高.中.低"""
+ v = self._parse_versions()
+ if not v:
+ return "0.0.0"
+ return f"{v['hw_ver_high']}.{v['hw_ver_mid']}.{v['hw_ver_low']}"
+
+ def get_software_version(self) -> str:
+ """获取软件版本号。格式：高.中.低"""
+ v = self._parse_versions()
+ if not v:
+ return "0.0.0"
+ return f"{v['sw_ver_high']}.{v['sw_ver_mid']}.{v['sw_ver_low']}"
+
+ def get_mechanical_version(self) -> str:
+ """获取机械版本号。格式：高.中.低"""
+ v = self._parse_versions()
+ if not v:
+ return "0.0.0"
+ return f"{v['mech_ver_high']}.{v['mech_ver_mid']}.{v['mech_ver_low']}"
+
+ def get_hand_freedom(self) -> int:
+ """获取自由度（与机械手上标签相同）"""
+ v = self._parse_versions()
+ return v.get("hand_freedom", 0)
+
+ def get_hand_version_raw(self) -> int:
+ """获取手版本原始值"""
+ v = self._parse_versions()
+ return v.get("hand_version", 0)
+
+ def get_hand_direction(self) -> str:
+ """获取手方向，转换为字符：76→'L', 82→'R'"""
+ v = self._parse_versions()
+ val = v.get("hand_direction", 0)
+ if val in (76, 82):
+ return chr(val)
+ # fallback: 直接转为字符（如果值在可打印范围内）
+ return chr(val) if 32 < val < 128 else f'Unknown({val})'
+
+
+ # ----------------------------------------------------------
+ # 只读属性（单个寄存器读取）
+ # ----------------------------------------------------------
+ def _read_reg(self, addr: int) -> int:
+ """读单个输入寄存器（功能码 04），带 30 ms 帧间隔"""
+ return self._execute_read(addr, 1)[0]
+
+ def get_thumb_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_PITCH)
+ def get_thumb_yaw(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW)
+ def get_index_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_PITCH)
+ def get_middle_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_PITCH)
+ def get_ring_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_PITCH)
+ def get_little_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_PITCH)
+
+ def get_thumb_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_TORQUE)
+ def get_thumb_yaw_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW_TORQUE)
+ def get_index_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_TORQUE)
+ def get_middle_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_TORQUE)
+ def get_ring_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_TORQUE)
+ def get_little_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_TORQUE)
+
+ def get_thumb_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_SPEED)
+ def get_thumb_yaw_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW_SPEED)
+ def get_index_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_SPEED)
+ def get_middle_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_SPEED)
+ def get_ring_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_SPEED)
+ def get_little_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_SPEED)
+
+ def get_thumb_temp(self) -> int: return self._read_reg(REG_RD_THUMB_TEMP)
+ def get_thumb_yaw_temp(self) -> int: return self._read_reg(REG_RD_THUMB_YAW_TEMP)
+ def get_index_temp(self) -> int: return self._read_reg(REG_RD_INDEX_TEMP)
+ def get_middle_temp(self) -> int: return self._read_reg(REG_RD_MIDDLE_TEMP)
+ def get_ring_temp(self) -> int: return self._read_reg(REG_RD_RING_TEMP)
+ def get_little_temp(self) -> int: return self._read_reg(REG_RD_LITTLE_TEMP)
+
+ def get_thumb_error(self) -> int: return self._read_reg(REG_RD_THUMB_ERROR)
+ def get_thumb_yaw_error(self) -> int: return self._read_reg(REG_RD_THUMB_YAW_ERROR)
+ def get_index_error(self) -> int: return self._read_reg(REG_RD_INDEX_ERROR)
+ def get_middle_error(self) -> int: return self._read_reg(REG_RD_MIDDLE_ERROR)
+ def get_ring_error(self) -> int: return self._read_reg(REG_RD_RING_ERROR)
+ def get_little_error(self) -> int: return self._read_reg(REG_RD_LITTLE_ERROR)
+
+
+ # ----------------------------------------------------------
+ # 批量 Getter (使用 read_all_... 方法)
+ # ----------------------------------------------------------
+ def get_state(self) -> List[int]:
+ """获取手指电机状态 (角度)"""
+ return self.read_all_angles()
+
+ def get_torque(self) -> List[int]:
+ """获取当前扭矩"""
+ return self.read_all_torques()
+
+ def get_speed(self) -> List[int]:
+ """获取当前速度"""
+ return self.read_all_speeds()
+
+ def get_temperature(self) -> List[int]:
+ """获取当前电机温度"""
+ return self.read_all_temperatures()
+
+ def get_fault(self) -> List[int]:
+ """获取当前电机故障码"""
+ return self.read_all_errors()
+
+ def get_version(self) -> str:
+ """获取当前固件版本号（已转换为字符串格式）"""
+ return self.read_all_versions()
+
+
+ # ----------------------------------------------------------
+ # 写保持寄存器 (单个寄存器写入)
+ # ----------------------------------------------------------
+ def _write_reg(self, addr: int, value: int):
+ """写单个保持寄存器（功能码 16），带 30 ms 帧间隔"""
+ if not 0 <= value <= 255:
+ raise ValueError("value must be 0-255")
+
+ # 确保 value 是 Python 原生 int
+ self._execute_write(addr, [int(value)])
+
+ def _write_regs(self, addr: int, values: List[int]):
+ """写多个保持寄存器（功能码 16），带 30 ms 帧间隔"""
+ # 此时 values 应该已经是经过 is_valid_6xuint8 验证并转换的 Python int 列表
+ if not all(0 <= v <= 255 for v in values):
+ # 这行理论上不应触发，因为上层调用已校验
+ raise ValueError("All values must be 0-255")
+ self._execute_write(addr, values)
+
+
+ def set_thumb_pitch(self, v: int): self._write_reg(REG_WR_THUMB_PITCH, v)
+ def set_thumb_yaw(self, v: int): self._write_reg(REG_WR_THUMB_YAW, v)
+ def set_index_pitch(self, v: int): self._write_reg(REG_WR_INDEX_PITCH, v)
+ def set_middle_pitch(self, v: int): self._write_reg(REG_WR_MIDDLE_PITCH, v)
+ def set_ring_pitch(self, v: int): self._write_reg(REG_WR_RING_PITCH, v)
+ def set_little_pitch(self, v: int): self._write_reg(REG_WR_LITTLE_PITCH, v)
+
+ def set_thumb_torque(self, v: int): self._write_reg(REG_WR_THUMB_TORQUE, v)
+ def set_thumb_yaw_torque(self, v: int): self._write_reg(REG_WR_THUMB_YAW_TORQUE, v)
+ def set_index_torque(self, v: int): self._write_reg(REG_WR_INDEX_TORQUE, v)
+ def set_middle_torque(self, v: int): self._write_reg(REG_WR_MIDDLE_TORQUE, v)
+ def set_ring_torque(self, v: int): self._write_reg(REG_WR_RING_TORQUE, v)
+ def set_little_torque(self, v: int): self._write_reg(REG_WR_LITTLE_TORQUE, v)
+
+ def set_thumb_speed(self, v: int): self._write_reg(REG_WR_THUMB_SPEED, v)
+ def set_thumb_yaw_speed(self, v: int): self._write_reg(REG_WR_THUMB_YAW_SPEED, v)
+ def set_index_speed(self, v: int): self._write_reg(REG_WR_INDEX_SPEED, v)
+ def set_middle_speed(self, v: int): self._write_reg(REG_WR_MIDDLE_SPEED, v)
+ def set_ring_speed(self, v: int): self._write_reg(REG_WR_RING_SPEED, v)
+ def set_little_speed(self, v: int): self._write_reg(REG_WR_LITTLE_SPEED, v)
+
+ # ----------------------------------------------------------
+ # 固定函数 (采用批量写入优化)
+ # ----------------------------------------------------------
+ def is_valid_6xuint8(self, lst: List[Any]) -> bool:
+ """
+ 验证6个0-255的整数列表。
+ 允许输入包含浮点数、NumPy整数等可转换为 int 的类型，并进行范围校验。
+ """
+ if not (isinstance(lst, list) and len(lst) == 6):
+ return False
+
+ try:
+ # 关键：尝试将所有元素转换为 Python 原生 int
+ int_values = [int(v) for v in lst]
+ except (ValueError, TypeError):
+ # 转换失败，列表中包含不可转换的元素
+ return False
+
+ # 校验转换后的整数列表是否在 0-255 范围内
+ return all(0 <= x <= 255 for x in int_values)
+
+ def set_joint_positions(self, joint_angles: List[Any] = None):
+ joint_angles = joint_angles or [0] * 6
+
+ if not self.is_valid_6xuint8(joint_angles):
+ logging.error(f"Invalid joint angles received: {joint_angles}")
+ raise ValueError("Joint angles must be a list of 6 values between 0 and 255 (convertible to int).")
+
+ # 强制转换为 Modbus 兼容的 Python 原生 int 列表
+ int_angles = [int(v) for v in joint_angles]
+
+ # 批量写入 6 个角度寄存器 (从 REG_WR_THUMB_PITCH 地址 0 开始, count=6)
+ self._write_regs(REG_WR_THUMB_PITCH, int_angles)
+
+ def set_speed(self, speed: List[Any] = None):
+ speed = speed or [200] * 6
+ if not self.is_valid_6xuint8(speed):
+ logging.error(f"Invalid speed values received: {speed}")
+ raise ValueError("Speed values must be a list of 6 values between 0 and 255 (convertible to int).")
+
+ int_speed = [int(v) for v in speed]
+ self._write_regs(REG_WR_THUMB_SPEED, int_speed)
+
+ def set_torque(self, torque: List[Any] = None):
+ torque = torque or [200] * 6
+ if not self.is_valid_6xuint8(torque):
+ logging.error(f"Invalid torque values received: {torque}")
+ raise ValueError("Torque values must be a list of 6 values between 0 and 255 (convertible to int).")
+
+ int_torque = [int(v) for v in torque]
+ self._write_regs(REG_WR_THUMB_TORQUE, int_torque)
+
+ # ... (其他固定函数保持不变) ...
+
+ def set_current(self, current: List[int] = None):
+ print("当前O6不支持设置电流", flush=True)
+ pass
+
+ def get_state_for_pub(self) -> list:
+ return self.get_state()
+
+ def get_current_status(self) -> list:
+ return self.get_state()
+
+ def get_joint_speed(self) -> list:
+ return self.get_speed()
+
+ def get_touch_type(self) -> list:
+ return -1
+
+ def get_normal_force(self) -> list:
+ return [-1] * 5
+
+ def get_tangential_force(self) -> list:
+ return [-1] * 5
+
+ def get_approach_inc(self) -> list:
+ return [-1] * 5
+
+ def get_touch(self) -> list:
+ return [-1] * 5
+
+ def _pressure(self, finger: int) -> np.ndarray:
+ """
+ 读取压力传感器数据 (10x4矩阵)
+ """
+ rows = 10 # 10行
+ cols = 4 # 4列
+ finger_size = rows * cols # 40个数据点
+
+ # modbus 地址 (按O6协议文档)
+ write_address = 18 # 写入手指选择 (保持寄存器)
+ read_address = 47 # 读取压力数据 (输入寄存器)
+ read_count = 40 # 读取40个寄存器
+
+ # 0. 参数校验
+ if finger < 1 or finger > 5:
+ raise ValueError(f"无效的手指编号: {finger}。手指编号应在 1 到 5 之间。")
+
+ # 1. 写入手指选择寄存器 (地址18)
+ time.sleep(0.01)
+ self._write_reg(write_address, finger)
+
+ # 2. 读取压力数据
+ data = self._execute_read(read_address, read_count)
+
+ # 3. 转换为numpy数组并重塑为10x4矩阵
+ finger_matrix = np.array(data, dtype=np.uint8).reshape((rows, cols))
+
+ return finger_matrix
+
+ def get_thumb_matrix_touch(self,sleep_time=0):
+ return np.array(self._pressure(1), dtype=np.uint8)
+
+
+ def get_index_matrix_touch(self,sleep_time=0):
+ return np.array(self._pressure(2), dtype=np.uint8)
+
+ def get_middle_matrix_touch(self,sleep_time=0):
+ return np.array(self._pressure(3), dtype=np.uint8)
+
+ def get_ring_matrix_touch(self,sleep_time=0):
+ return np.array(self._pressure(4), dtype=np.uint8)
+
+ def get_little_matrix_touch(self,sleep_time=0):
+ return np.array(self._pressure(5), dtype=np.uint8)
+
+ def get_matrix_touch(self) -> list:
+ thumb_matrix = np.full((12, 6), -1)
+ index_matrix = np.full((12, 6), -1)
+ middle_matrix = np.full((12, 6), -1)
+ ring_matrix = np.full((12, 6), -1)
+ little_matrix = np.full((12, 6), -1)
+ return thumb_matrix , index_matrix , middle_matrix , ring_matrix , little_matrix
+
+ def get_serial_number(self):
+
+ return "["+str(self.get_hand_freedom())+"-"+str(self.get_mechanical_version())+"-"+str(self.get_device_number())+"-"+str(self.get_hand_direction())+"]"
+
+ def get_matrix_touch_v2(self) -> list:
+ return self.get_matrix_touch()
+
+ def get_finger_order(self):
+ return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+
+ def clear_faults(self):
+ pass
+
+ def close(self):
+ if hasattr(self, 'connected') and self.connected:
+ self.cli.close()
+ self.connected = False
+ logging.info("Modbus connection closed.")
+
+ def __enter__(self):
+ return self
+
+ def __exit__(self, exc_type, exc_val, exc_tb):
+ self.close()
+
+ # ----------------------------------------------------------
+ # 便捷函数
+ # ----------------------------------------------------------
+ def set_all_fingers(self, pitch: int):
+ """同时设置五指弯曲角度（0-255），使用批量写入"""
+ # 允许传入 float/numpy int 等可转换为 int 的类型
+ try:
+ pitch_int = int(pitch)
+ except (ValueError, TypeError):
+ raise ValueError("Pitch value must be a number convertible to int (0-255)")
+
+ if not 0 <= pitch_int <= 255:
+ raise ValueError("Pitch value must be 0-255")
+
+ # 批量设置所有 6 个关节的角度
+ self.set_joint_positions([pitch_int] * 6)
+
+ def relax(self):
+ """全部手指伸直（255）"""
+ self.set_all_fingers(255)
+
+ def fist(self):
+ """全部手指弯曲（0）"""
+ self.set_all_fingers(0)
+
+ def dump_status(self):
+ """打印当前所有可读状态 (使用批量读取优化)"""
+ print("--------- O6 Hand Status ---------")
+
+ angles = self.get_state()
+ temps = self.get_temperature()
+ errors = self.get_fault()
+
+ # 解析版本号字符串
+ v = self._parse_versions()
+
+ if v:
+ device_num_str = f"{v['hand_number_high']}{v['hand_number_mid']}{v['hand_number_low']}"
+ hw_ver = f"{v['hw_ver_high']}.{v['hw_ver_mid']}.{v['hw_ver_low']}"
+ sw_ver = f"{v['sw_ver_high']}.{v['sw_ver_mid']}.{v['sw_ver_low']}"
+ mech_ver = f"{v['mech_ver_high']}.{v['mech_ver_mid']}.{v['mech_ver_low']}"
+
+ print(f"Device Number: {device_num_str}")
+ print(f"HWSW Version: HW={hw_ver} SW={sw_ver}")
+ print(f"Mechanical Ver: {mech_ver}")
+ print(f"Hand Freedom: {v['hand_freedom']}")
+ print(f"Full Version: {self.read_all_versions()}")
+
+ print(f"Joint Angles: {angles}")
+ print(f"Temperature: {temps}℃")
+ print(f"Error Codes: {errors}")
+ print("----------------------------------")
+
+
+# ------------------------------------------------------------------
+# 命令行快速测试
+# ------------------------------------------------------------------
+if __name__ == "__main__":
+ import argparse
+
+ # 假设默认站号是 0x27 (39)
+ DEFAULT_HAND_ID = 0x27
+
+ parser = argparse.ArgumentParser(description="O6 Hand Modbus tester (using pymodbus 3.5.1)")
+ parser.add_argument("-p", "--port", required=True, help="串口, 如 /dev/ttyUSB0")
+ parser.add_argument("-l", "--left", action="store_const", const=0x28, default=DEFAULT_HAND_ID, dest='hand_id', help="左手 (0x28)，默认右手 (0x27)")
+
+ args = parser.parse_args()
+
+ try:
+ # 使用 with 语句确保连接关闭，这是 pymodbus 的推荐用法
+ with LinkerHandO6RS485(hand_id=args.hand_id, modbus_port=args.port, baudrate=115200) as hand:
+ hand.dump_status()
+
+ # 测试新增的设备编号读取方法
+ print("\n--- 设备信息 ---")
+ print(f"设备编号（字符串）: {hand.get_device_number()}")
+ print(f"设备编号（数值）: {hand.get_device_number_value()}")
+ print(f"硬件版本号: {hand.get_hardware_version()}")
+ print(f"软件版本号: {hand.get_software_version()}")
+ print(f"机械版本号: {hand.get_mechanical_version()}")
+
+ print("\n执行 relax → 伸直")
+ hand.relax()
+ time.sleep(1)
+ print("执行 fist → 握拳")
+ hand.fist()
+ time.sleep(1)
+ hand.relax()
+ print("演示完成")
+
+ except ConnectionError as e:
+ print(f"连接错误: {e}")
+ except RuntimeError as e:
+ print(f"Modbus 运行时错误: {e}")
+ except StructError as e:
+ print(f"数据结构错误 (请检查输入数据类型是否为原生int): {e}")
+ except Exception as e:
+ print(f"发生其他错误: {e}")
\ No newline at end of file
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py.bak b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py.bak
new file mode 100644
index 0000000..cb08dd9
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/rs485/linker_hand_o6_rs485.py.bak
@@ -0,0 +1,1157 @@
+#!/usr/bin/env python3
+"""
+O6 机械手 Modbus-RTU 控制类 (基于 pymodbus 3.5.1)
+"""
+
+import os
+import time
+from typing import List, Dict, Any # 引入 Any 来表示灵活的输入类型
+import numpy as np
+import logging
+from threading import Lock # 用于线程安全和总线仲裁
+
+# 导入 pymodbus 客户端
+from pymodbus.client import ModbusSerialClient
+from struct import error as StructError
+
+logging.basicConfig(
+ level=logging.INFO,
+ format="[%(asctime)s] %(levelname)-8s %(message)s",
+ datefmt="%H:%M:%S"
+)
+
+# ------------------------------------------------------------------
+# 读输入寄存器地址枚举（功能码 04，只读）- 保持原样
+# ------------------------------------------------------------------
+REG_RD_CURRENT_THUMB_PITCH = 0 # 大拇指弯曲角度（0-255，小=弯，大=伸）
+REG_RD_CURRENT_THUMB_YAW = 1 # 大拇指横摆角度（0-255，小=靠掌心，大=远离）
+REG_RD_CURRENT_INDEX_PITCH = 2 # 食指弯曲角度
+REG_RD_CURRENT_MIDDLE_PITCH = 3 # 中指弯曲角度
+REG_RD_CURRENT_RING_PITCH = 4 # 无名指弯曲角度
+REG_RD_CURRENT_LITTLE_PITCH = 5 # 小拇指弯曲角度
+REG_RD_CURRENT_THUMB_TORQUE = 6 # 大拇指弯曲转矩（0-255）
+REG_RD_CURRENT_THUMB_YAW_TORQUE = 7 # 大拇指横摆转矩
+REG_RD_CURRENT_INDEX_TORQUE = 8 # 食指转矩
+REG_RD_CURRENT_MIDDLE_TORQUE = 9 # 中指转矩
+REG_RD_CURRENT_RING_TORQUE = 10 # 无名指转矩
+REG_RD_CURRENT_LITTLE_TORQUE = 11 # 小拇指转矩
+REG_RD_CURRENT_THUMB_SPEED = 12 # 大拇指弯曲速度（0-255）
+REG_RD_CURRENT_THUMB_YAW_SPEED = 13 # 大拇指横摆速度
+REG_RD_CURRENT_INDEX_SPEED = 14 # 食指速度
+REG_RD_CURRENT_MIDDLE_SPEED = 15 # 中指速度
+REG_RD_CURRENT_RING_SPEED = 16 # 无名指速度
+REG_RD_CURRENT_LITTLE_SPEED = 17 # 小拇指速度
+REG_RD_THUMB_TEMP = 18 # 大拇指弯曲温度（0-70℃）
+REG_RD_THUMB_YAW_TEMP = 19 # 大拇指横摆温度
+REG_RD_INDEX_TEMP = 20 # 食指温度
+REG_RD_MIDDLE_TEMP = 21 # 中指温度
+REG_RD_RING_TEMP = 22 # 无名指温度
+REG_RD_LITTLE_TEMP = 23 # 小拇指温度
+REG_RD_THUMB_ERROR = 24 # 大拇指错误码
+REG_RD_THUMB_YAW_ERROR = 25 # 大拇指横摆错误码
+REG_RD_INDEX_ERROR = 26 # 食指错误码
+REG_RD_MIDDLE_ERROR = 27 # 中指错误码
+REG_RD_RING_ERROR = 28 # 无名指错误码
+REG_RD_LITTLE_ERROR = 29 # 小拇指错误码
+REG_RD_HAND_FREEDOM = 30 # 版本号（与机械手标签相同）
+REG_RD_HAND_VERSION = 31 # 手版本
+REG_RD_HAND_NUMBER = 32 # 手编号
+REG_RD_HAND_DIRECTION = 33 # 手方向（左/右）
+REG_RD_SOFTWARE_VERSION = 34 # 软件版本
+REG_RD_HARDWARE_VERSION = 35 # 硬件版本
+
+
+# ------------------------------------------------------------------
+# 写保持寄存器地址枚举（功能码 16，读写）- 保持原样
+# ------------------------------------------------------------------
+REG_WR_THUMB_PITCH = 0 # 大拇指弯曲角度（0-255）
+REG_WR_THUMB_YAW = 1 # 大拇指横摆角度
+REG_WR_INDEX_PITCH = 2 # 食指弯曲角度
+REG_WR_MIDDLE_PITCH = 3 # 中指弯曲角度
+REG_WR_RING_PITCH = 4 # 无名指弯曲角度
+REG_WR_LITTLE_PITCH = 5 # 小拇指弯曲角度
+REG_WR_THUMB_TORQUE = 6 # 大拇指弯曲转矩
+REG_WR_THUMB_YAW_TORQUE = 7 # 大拇指横摆转矩
+REG_WR_INDEX_TORQUE = 8 # 食指转矩
+REG_WR_MIDDLE_TORQUE = 9 # 中指转矩
+REG_WR_RING_TORQUE = 10 # 无名指转矩
+REG_WR_LITTLE_TORQUE = 11 # 小拇指转矩
+REG_WR_THUMB_SPEED = 12 # 大拇指弯曲速度
+REG_WR_THUMB_YAW_SPEED = 13 # 大拇指横摆速度
+REG_WR_INDEX_SPEED = 14 # 食指速度
+REG_WR_MIDDLE_SPEED = 15 # 中指速度
+REG_WR_RING_SPEED = 16 # 无名指速度
+REG_WR_LITTLE_SPEED = 17 # 小拇指速度
+
+
+class LinkerHandO6RS485:
+ """O6 机械手 Modbus-RTU 控制类，使用 pymodbus 3.5.1"""
+
+ TTL_TIMEOUT = 0.15 # 串口超时
+ FRAME_GAP = 0.030 # 30 ms
+
+ # KEYS for easy indexing
+ JOINT_KEYS = ["thumb_pitch", "thumb_yaw", "index_pitch",
+ "middle_pitch", "ring_pitch", "little_pitch"]
+
+ def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
+ self._id = hand_id
+ self._last_ts = 0.0 # 上一次帧结束时间
+ self._lock = Lock() # 总线访问锁
+
+ # 使用 pymodbus 3.x 客户端
+ self.cli = ModbusSerialClient(
+ port=modbus_port,
+ baudrate=baudrate,
+ bytesize=8,
+ parity="N",
+ stopbits=1,
+ timeout=self.TTL_TIMEOUT,
+ handle_local_echo=False
+ )
+
+ try:
+ logging.info(f"Connecting to Modbus RTU on {modbus_port}...")
+ self.connected = self.cli.connect()
+ if not self.connected:
+ raise ConnectionError(f"RS485 connect fail to {modbus_port}")
+ logging.info("Connection successful.")
+ except Exception as e:
+ logging.error(f"Initialization failed: {e}")
+ raise
+
+ # ----------------------------------------------------------
+ # 辅助方法
+ # ----------------------------------------------------------
+ def _bus_free(self):
+ """保证距离上一帧 ≥ 30 ms"""
+ with self._lock:
+ elapse = time.perf_counter() - self._last_ts
+ if elapse < self.FRAME_GAP:
+ time.sleep(self.FRAME_GAP - elapse)
+
+ def _execute_read(self, address: int, count: int) -> List[int]:
+ """执行 Modbus 读取操作 (功能码 04), 带总线仲裁。"""
+ self._bus_free()
+
+ rsp = self.cli.read_input_registers(
+ address=address,
+ count=count,
+ slave=self._id
+ )
+
+ self._last_ts = time.perf_counter()
+
+ if rsp.isError():
+ raise RuntimeError(f"Modbus Read Failed (Addr={address}, Count={count}): {rsp}")
+
+ # 确保返回的值是 Python 原生整数
+ return [int(reg) for reg in rsp.registers]
+
+ def _execute_write(self, address: int, values: List[int]):
+ """执行 Modbus 批量写入操作 (功能码 16), 带总线仲裁。"""
+ self._bus_free()
+
+ # values 必须是 Python 原生整数列表
+ rsp = self.cli.write_registers(
+ address=address,
+ values=values,
+ slave=self._id
+ )
+
+ self._last_ts = time.perf_counter()
+
+ if rsp.isError():
+ raise RuntimeError(f"Modbus Write Failed (Addr={address}, Values={values}): {rsp}")
+
+ # ----------------------------------------------------------
+ # 批量读取和数据封装（优化通信效率）
+ # ----------------------------------------------------------
+ def read_all_angles(self) -> List[int]:
+ return self._execute_read(REG_RD_CURRENT_THUMB_PITCH, 6)
+
+ def read_all_torques(self) -> List[int]:
+ return self._execute_read(REG_RD_CURRENT_THUMB_TORQUE, 6)
+
+ def read_all_speeds(self) -> List[int]:
+ return self._execute_read(REG_RD_CURRENT_THUMB_SPEED, 6)
+
+ def read_all_temperatures(self) -> List[int]:
+ return self._execute_read(REG_RD_THUMB_TEMP, 6)
+
+ def read_all_errors(self) -> List[int]:
+ return self._execute_read(REG_RD_THUMB_ERROR, 6)
+
+ def read_all_versions(self) -> List[int]:
+ return self._execute_read(REG_RD_HAND_FREEDOM, 6)
+
+
+ # ----------------------------------------------------------
+ # 只读属性（单个寄存器读取）
+ # ----------------------------------------------------------
+ def _read_reg(self, addr: int) -> int:
+ """读单个输入寄存器（功能码 04），带 30 ms 帧间隔"""
+ return self._execute_read(addr, 1)[0]
+
+ def get_thumb_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_PITCH)
+ def get_thumb_yaw(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW)
+ def get_index_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_PITCH)
+ def get_middle_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_PITCH)
+ def get_ring_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_PITCH)
+ def get_little_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_PITCH)
+
+ def get_thumb_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_TORQUE)
+ def get_thumb_yaw_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW_TORQUE)
+ def get_index_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_TORQUE)
+ def get_middle_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_TORQUE)
+ def get_ring_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_TORQUE)
+ def get_little_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_TORQUE)
+
+ def get_thumb_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_SPEED)
+ def get_thumb_yaw_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW_SPEED)
+ def get_index_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_SPEED)
+ def get_middle_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_SPEED)
+ def get_ring_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_SPEED)
+ def get_little_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_SPEED)
+
+ def get_thumb_temp(self) -> int: return self._read_reg(REG_RD_THUMB_TEMP)
+ def get_thumb_yaw_temp(self) -> int: return self._read_reg(REG_RD_THUMB_YAW_TEMP)
+ def get_index_temp(self) -> int: return self._read_reg(REG_RD_INDEX_TEMP)
+ def get_middle_temp(self) -> int: return self._read_reg(REG_RD_MIDDLE_TEMP)
+ def get_ring_temp(self) -> int: return self._read_reg(REG_RD_RING_TEMP)
+ def get_little_temp(self) -> int: return self._read_reg(REG_RD_LITTLE_TEMP)
+
+ def get_thumb_error(self) -> int: return self._read_reg(REG_RD_THUMB_ERROR)
+ def get_thumb_yaw_error(self) -> int: return self._read_reg(REG_RD_THUMB_YAW_ERROR)
+ def get_index_error(self) -> int: return self._read_reg(REG_RD_INDEX_ERROR)
+ def get_middle_error(self) -> int: return self._read_reg(REG_RD_MIDDLE_ERROR)
+ def get_ring_error(self) -> int: return self._read_reg(REG_RD_RING_ERROR)
+ def get_little_error(self) -> int: return self._read_reg(REG_RD_LITTLE_ERROR)
+
+ def get_hand_freedom(self) -> int: return self._read_reg(REG_RD_HAND_FREEDOM)
+ def get_hand_version(self) -> int: return self._read_reg(REG_RD_HAND_VERSION)
+ def get_hand_number(self) -> int: return self._read_reg(REG_RD_HAND_NUMBER)
+ def get_hand_direction(self) -> int: return self._read_reg(REG_RD_HAND_DIRECTION)
+ def get_software_version(self) -> int: return self._read_reg(REG_RD_SOFTWARE_VERSION)
+ def get_hardware_version(self) -> int: return self._read_reg(REG_RD_HARDWARE_VERSION)
+
+ # ----------------------------------------------------------
+ # 批量 Getter (使用 read_all_... 方法)
+ # ----------------------------------------------------------
+ def get_state(self) -> List[int]:
+ """获取手指电机状态 (角度)"""
+ return self.read_all_angles()
+
+ def get_torque(self) -> List[int]:
+ """获取当前扭矩"""
+ return self.read_all_torques()
+
+ def get_speed(self) -> List[int]:
+ """获取当前速度"""
+ return self.read_all_speeds()
+
+ def get_temperature(self) -> List[int]:
+ """获取当前电机温度"""
+ return self.read_all_temperatures()
+
+ def get_fault(self) -> List[int]:
+ """获取当前电机故障码"""
+ return self.read_all_errors()
+
+ def get_version(self) -> list:
+ """获取当前固件版本号"""
+ return self.read_all_versions()
+
+
+ # ----------------------------------------------------------
+ # 写保持寄存器 (单个寄存器写入)
+ # ----------------------------------------------------------
+ def _write_reg(self, addr: int, value: int):
+ """写单个保持寄存器（功能码 16），带 30 ms 帧间隔"""
+ if not 0 <= value <= 255:
+ raise ValueError("value must be 0-255")
+
+ # 确保 value 是 Python 原生 int
+ self._execute_write(addr, [int(value)])
+
+ def _write_regs(self, addr: int, values: List[int]):
+ """写多个保持寄存器（功能码 16），带 30 ms 帧间隔"""
+ # 此时 values 应该已经是经过 is_valid_6xuint8 验证并转换的 Python int 列表
+ if not all(0 <= v <= 255 for v in values):
+ # 这行理论上不应触发，因为上层调用已校验
+ raise ValueError("All values must be 0-255")
+ self._execute_write(addr, values)
+
+
+ def set_thumb_pitch(self, v: int): self._write_reg(REG_WR_THUMB_PITCH, v)
+ def set_thumb_yaw(self, v: int): self._write_reg(REG_WR_THUMB_YAW, v)
+ def set_index_pitch(self, v: int): self._write_reg(REG_WR_INDEX_PITCH, v)
+ def set_middle_pitch(self, v: int): self._write_reg(REG_WR_MIDDLE_PITCH, v)
+ def set_ring_pitch(self, v: int): self._write_reg(REG_WR_RING_PITCH, v)
+ def set_little_pitch(self, v: int): self._write_reg(REG_WR_LITTLE_PITCH, v)
+
+ def set_thumb_torque(self, v: int): self._write_reg(REG_WR_THUMB_TORQUE, v)
+ def set_thumb_yaw_torque(self, v: int): self._write_reg(REG_WR_THUMB_YAW_TORQUE, v)
+ def set_index_torque(self, v: int): self._write_reg(REG_WR_INDEX_TORQUE, v)
+ def set_middle_torque(self, v: int): self._write_reg(REG_WR_MIDDLE_TORQUE, v)
+ def set_ring_torque(self, v: int): self._write_reg(REG_WR_RING_TORQUE, v)
+ def set_little_torque(self, v: int): self._write_reg(REG_WR_LITTLE_TORQUE, v)
+
+ def set_thumb_speed(self, v: int): self._write_reg(REG_WR_THUMB_SPEED, v)
+ def set_thumb_yaw_speed(self, v: int): self._write_reg(REG_WR_THUMB_YAW_SPEED, v)
+ def set_index_speed(self, v: int): self._write_reg(REG_WR_INDEX_SPEED, v)
+ def set_middle_speed(self, v: int): self._write_reg(REG_WR_MIDDLE_SPEED, v)
+ def set_ring_speed(self, v: int): self._write_reg(REG_WR_RING_SPEED, v)
+ def set_little_speed(self, v: int): self._write_reg(REG_WR_LITTLE_SPEED, v)
+
+ # ----------------------------------------------------------
+ # 固定函数 (采用批量写入优化)
+ # ----------------------------------------------------------
+ def is_valid_6xuint8(self, lst: List[Any]) -> bool:
+ """
+ 验证6个0-255的整数列表。
+ 允许输入包含浮点数、NumPy整数等可转换为 int 的类型，并进行范围校验。
+ """
+ if not (isinstance(lst, list) and len(lst) == 6):
+ return False
+
+ try:
+ # 关键：尝试将所有元素转换为 Python 原生 int
+ int_values = [int(v) for v in lst]
+ except (ValueError, TypeError):
+ # 转换失败，列表中包含不可转换的元素
+ return False
+
+ # 校验转换后的整数列表是否在 0-255 范围内
+ return all(0 <= x <= 255 for x in int_values)
+
+ def set_joint_positions(self, joint_angles: List[Any] = None):
+ joint_angles = joint_angles or [0] * 6
+
+ if not self.is_valid_6xuint8(joint_angles):
+ logging.error(f"Invalid joint angles received: {joint_angles}")
+ raise ValueError("Joint angles must be a list of 6 values between 0 and 255 (convertible to int).")
+
+ # 强制转换为 Modbus 兼容的 Python 原生 int 列表
+ int_angles = [int(v) for v in joint_angles]
+
+ # 批量写入 6 个角度寄存器 (从 REG_WR_THUMB_PITCH 地址 0 开始, count=6)
+ self._write_regs(REG_WR_THUMB_PITCH, int_angles)
+
+ def set_speed(self, speed: List[Any] = None):
+ speed = speed or [200] * 6
+ if not self.is_valid_6xuint8(speed):
+ logging.error(f"Invalid speed values received: {speed}")
+ raise ValueError("Speed values must be a list of 6 values between 0 and 255 (convertible to int).")
+
+ int_speed = [int(v) for v in speed]
+ self._write_regs(REG_WR_THUMB_SPEED, int_speed)
+
+ def set_torque(self, torque: List[Any] = None):
+ torque = torque or [200] * 6
+ if not self.is_valid_6xuint8(torque):
+ logging.error(f"Invalid torque values received: {torque}")
+ raise ValueError("Torque values must be a list of 6 values between 0 and 255 (convertible to int).")
+
+ int_torque = [int(v) for v in torque]
+ self._write_regs(REG_WR_THUMB_TORQUE, int_torque)
+
+ # ... (其他固定函数保持不变) ...
+
+ def set_current(self, current: List[int] = None):
+ print("当前O6不支持设置电流", flush=True)
+ pass
+
+ def get_state_for_pub(self) -> list:
+ return self.get_state()
+
+ def get_current_status(self) -> list:
+ return self.get_state()
+
+ def get_joint_speed(self) -> list:
+ return self.get_speed()
+
+ def get_touch_type(self) -> list:
+ return -1
+
+ def get_normal_force(self) -> list:
+ return [-1] * 5
+
+ def get_tangential_force(self) -> list:
+ return [-1] * 5
+
+ def get_approach_inc(self) -> list:
+ return [-1] * 5
+
+ def get_touch(self) -> list:
+ return [-1] * 5
+
+ def get_thumb_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_index_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_middle_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_ring_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_little_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_matrix_touch(self) -> list:
+ thumb_matrix = np.full((12, 6), -1)
+ index_matrix = np.full((12, 6), -1)
+ middle_matrix = np.full((12, 6), -1)
+ ring_matrix = np.full((12, 6), -1)
+ little_matrix = np.full((12, 6), -1)
+ return thumb_matrix , index_matrix , middle_matrix , ring_matrix , little_matrix
+
+ def get_serial_number(self):
+ return [0] * 6
+
+ def get_matrix_touch_v2(self) -> list:
+ return self.get_matrix_touch()
+
+ def get_finger_order(self):
+ return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+
+ # ----------------------------------------------------------
+ # 上下文管理
+ # ----------------------------------------------------------
+ def close(self):
+ if hasattr(self, 'connected') and self.connected:
+ self.cli.close()
+ self.connected = False
+ logging.info("Modbus connection closed.")
+
+ def __enter__(self):
+ return self
+
+ def __exit__(self, exc_type, exc_val, exc_tb):
+ self.close()
+
+ # ----------------------------------------------------------
+ # 便捷函数
+ # ----------------------------------------------------------
+ def set_all_fingers(self, pitch: int):
+ """同时设置五指弯曲角度（0-255），使用批量写入"""
+ # 允许传入 float/numpy int 等可转换为 int 的类型
+ try:
+ pitch_int = int(pitch)
+ except (ValueError, TypeError):
+ raise ValueError("Pitch value must be a number convertible to int (0-255)")
+
+ if not 0 <= pitch_int <= 255:
+ raise ValueError("Pitch value must be 0-255")
+
+ # 批量设置所有 6 个关节的角度
+ self.set_joint_positions([pitch_int] * 6)
+
+ def relax(self):
+ """全部手指伸直（255）"""
+ self.set_all_fingers(255)
+
+ def fist(self):
+ """全部手指弯曲（0）"""
+ self.set_all_fingers(0)
+
+ def dump_status(self):
+ """打印当前所有可读状态 (使用批量读取优化)"""
+ print("--------- O6 Hand Status ---------")
+
+ angles = self.get_state()
+ temps = self.get_temperature()
+ errors = self.get_fault()
+ versions = self.get_version()
+
+ print(f"Joint Angles: {angles}")
+ print(f"Temperature: {temps}℃")
+ print(f"Error Codes: {errors}")
+ print(f"Versions: {versions}")
+ print("----------------------------------")
+
+# ------------------------------------------------------------------
+# 命令行快速测试
+# ------------------------------------------------------------------
+if __name__ == "__main__":
+ import argparse
+
+ # 假设默认站号是 0x27 (39)
+ DEFAULT_HAND_ID = 0x27
+
+ parser = argparse.ArgumentParser(description="O6 Hand Modbus tester (using pymodbus 3.5.1)")
+ parser.add_argument("-p", "--port", required=True, help="串口, 如 /dev/ttyUSB0")
+ parser.add_argument("-l", "--left", action="store_const", const=0x28, default=DEFAULT_HAND_ID, dest='hand_id', help="左手 (0x28)，默认右手 (0x27)")
+
+ args = parser.parse_args()
+
+ try:
+ # 使用 with 语句确保连接关闭，这是 pymodbus 的推荐用法
+ with LinkerHandO6RS485(hand_id=args.hand_id, modbus_port=args.port, baudrate=115200) as hand:
+ hand.dump_status()
+ print("执行 relax → 伸直")
+ hand.relax()
+ time.sleep(1)
+ print("执行 fist → 握拳")
+ hand.fist()
+ time.sleep(1)
+ hand.relax()
+ print("演示完成")
+
+ except ConnectionError as e:
+ print(f"连接错误: {e}")
+ except RuntimeError as e:
+ print(f"Modbus 运行时错误: {e}")
+ except StructError as e:
+ print(f"数据结构错误 (请检查输入数据类型是否为原生int): {e}")
+ except Exception as e:
+ print(f"发生其他错误: {e}")
+
+
+==========================================================================================================================
+
+
+#!/usr/bin/env python3
+"""
+O6 机械手 Modbus-RTU 控制类 (基于 pymodbus 3.5.1)
+"""
+
+import os
+import time
+from typing import List, Dict, Any # 引入 Any 来表示灵活的输入类型
+import numpy as np
+import logging
+from threading import Lock # 用于线程安全和总线仲裁
+
+# 导入 pymodbus 客户端
+from pymodbus.client import ModbusSerialClient
+from struct import error as StructError
+
+logging.basicConfig(
+ level=logging.INFO,
+ format="[%(asctime)s] %(levelname)-8s %(message)s",
+ datefmt="%H:%M:%S"
+)
+
+# ------------------------------------------------------------------
+# 读输入寄存器地址枚举（功能码 04，只读）- 按照 O6 协议文档定义
+# ------------------------------------------------------------------
+REG_RD_CURRENT_THUMB_PITCH = 0 # 大拇指弯曲角度（0-255，小=弯，大=伸）
+REG_RD_CURRENT_THUMB_YAW = 1 # 大拇指横摆角度（0-255，小=靠掌心，大=远离）
+REG_RD_CURRENT_INDEX_PITCH = 2 # 食指弯曲角度
+REG_RD_CURRENT_MIDDLE_PITCH = 3 # 中指弯曲角度
+REG_RD_CURRENT_RING_PITCH = 4 # 无名指弯曲角度
+REG_RD_CURRENT_LITTLE_PITCH = 5 # 小拇指弯曲角度
+REG_RD_CURRENT_THUMB_TORQUE = 6 # 大拇指弯曲转矩（0-255）
+REG_RD_CURRENT_THUMB_YAW_TORQUE = 7 # 大拇指横摆转矩
+REG_RD_CURRENT_INDEX_TORQUE = 8 # 食指转矩
+REG_RD_CURRENT_MIDDLE_TORQUE = 9 # 中指转矩
+REG_RD_CURRENT_RING_TORQUE = 10 # 无名指转矩
+REG_RD_CURRENT_LITTLE_TORQUE = 11 # 小拇指转矩
+REG_RD_CURRENT_THUMB_SPEED = 12 # 大拇指弯曲速度（0-255）
+REG_RD_CURRENT_THUMB_YAW_SPEED = 13 # 大拇指横摆速度
+REG_RD_CURRENT_INDEX_SPEED = 14 # 食指速度
+REG_RD_CURRENT_MIDDLE_SPEED = 15 # 中指速度
+REG_RD_CURRENT_RING_SPEED = 16 # 无名指速度
+REG_RD_CURRENT_LITTLE_SPEED = 17 # 小拇指速度
+REG_RD_THUMB_TEMP = 18 # 大拇指弯曲温度（0-70℃）
+REG_RD_THUMB_YAW_TEMP = 19 # 大拇指横摆温度
+REG_RD_INDEX_TEMP = 20 # 食指温度
+REG_RD_MIDDLE_TEMP = 21 # 中指温度
+REG_RD_RING_TEMP = 22 # 无名指温度
+REG_RD_LITTLE_TEMP = 23 # 小拇指温度
+REG_RD_THUMB_ERROR = 24 # 大拇指错误码
+REG_RD_THUMB_YAW_ERROR = 25 # 大拇指横摆错误码
+REG_RD_INDEX_ERROR = 26 # 食指错误码
+REG_RD_MIDDLE_ERROR = 27 # 中指错误码
+REG_RD_RING_ERROR = 28 # 无名指错误码
+REG_RD_LITTLE_ERROR = 29 # 小拇指错误码
+
+# 版本号/设备编号寄存器（地址 30-44，共15个寄存器）
+REG_RD_HAND_FREEDOM = 30 # Hand_freedom - 设备编号 / 自由度（与机械手上标签相同）
+REG_RD_HAND_VERSION = 31 # hand_version - 手版本
+REG_RD_HAND_NUMBER_HIGH = 32 # hand_number_高位 - 设备编号（高字节）
+REG_RD_HAND_NUMBER_MID = 33 # hand_number_中位 - 设备编号（中字节）
+REG_RD_HAND_NUMBER_LOW = 34 # hand_number_低位 - 设备编号（低字节）
+REG_RD_HAND_DIRECTION = 35 # hand_direction - 手方向（左/右）
+REG_RD_HARDWARE_VERSION_HIGH = 36 # hardware_version_高位 - 硬件版本（高字节）
+REG_RD_HARDWARE_VERSION_MID = 37 # hardware_version_中位 - 硬件版本（中字节）
+REG_RD_HARDWARE_VERSION_LOW = 38 # hardware_version_低位 - 硬件版本（低字节）
+REG_RD_SOFTWARE_VERSION_HIGH = 39 # software_version_高位 - 软件版本（高字节）
+REG_RD_SOFTWARE_VERSION_MID = 40 # software_version_中位 - 软件版本（中字节）
+REG_RD_SOFTWARE_VERSION_LOW = 41 # software_version_低位 - 软件版本（低字节）
+REG_RD_MECHANICAL_VERSION_HIGH = 42 # mechanical_version_高位 - 机械版本（高字节）
+REG_RD_MECHANICAL_VERSION_MID = 43 # mechanical_version_中位 - 机械版本（中字节）
+REG_RD_MECHANICAL_VERSION_LOW = 44 # mechanical_version_低位 - 机械版本（低字节）
+
+# 力传感器寄存器（地址 45-87+，动态范围）
+REG_RD_PRESSURE_SENSING_ID = 45 # Pressure_Sensing_ID - 压力传感器ID (0-5)
+REG_RD_PRESSURE_SENSING_SPEC = 46 # Pressure_Sensing_Specifications - 传感器数据规格
+
+
+# ------------------------------------------------------------------
+# 写保持寄存器地址枚举（功能码 16，读写）- 保持原样
+# ------------------------------------------------------------------
+REG_WR_THUMB_PITCH = 0 # 大拇指弯曲角度（0-255）
+REG_WR_THUMB_YAW = 1 # 大拇指横摆角度
+REG_WR_INDEX_PITCH = 2 # 食指弯曲角度
+REG_WR_MIDDLE_PITCH = 3 # 中指弯曲角度
+REG_WR_RING_PITCH = 4 # 无名指弯曲角度
+REG_WR_LITTLE_PITCH = 5 # 小拇指弯曲角度
+REG_WR_THUMB_TORQUE = 6 # 大拇指弯曲转矩
+REG_WR_THUMB_YAW_TORQUE = 7 # 大拇指横摆转矩
+REG_WR_INDEX_TORQUE = 8 # 食指转矩
+REG_WR_MIDDLE_TORQUE = 9 # 中指转矩
+REG_WR_RING_TORQUE = 10 # 无名指转矩
+REG_WR_LITTLE_TORQUE = 11 # 小拇指转矩
+REG_WR_THUMB_SPEED = 12 # 大拇指弯曲速度
+REG_WR_THUMB_YAW_SPEED = 13 # 大拇指横摆速度
+REG_WR_INDEX_SPEED = 14 # 食指速度
+REG_WR_MIDDLE_SPEED = 15 # 中指速度
+REG_WR_RING_SPEED = 16 # 无名指速度
+REG_WR_LITTLE_SPEED = 17 # 小拇指速度
+
+
+class LinkerHandO6RS485:
+ """O6 机械手 Modbus-RTU 控制类，使用 pymodbus 3.5.1"""
+
+ TTL_TIMEOUT = 0.15 # 串口超时
+ FRAME_GAP = 0.030 # 30 ms
+
+ # KEYS for easy indexing
+ JOINT_KEYS = ["thumb_pitch", "thumb_yaw", "index_pitch",
+ "middle_pitch", "ring_pitch", "little_pitch"]
+
+ def __init__(self, hand_id=0x27, modbus_port="/dev/ttyUSB0", baudrate=115200):
+ self._id = hand_id
+ self._last_ts = 0.0 # 上一次帧结束时间
+ self._lock = Lock() # 总线访问锁
+
+ # 使用 pymodbus 3.x 客户端
+ self.cli = ModbusSerialClient(
+ port=modbus_port,
+ baudrate=baudrate,
+ bytesize=8,
+ parity="N",
+ stopbits=1,
+ timeout=self.TTL_TIMEOUT,
+ handle_local_echo=False
+ )
+
+ try:
+ logging.info(f"Connecting to Modbus RTU on {modbus_port}...")
+ self.connected = self.cli.connect()
+ if not self.connected:
+ raise ConnectionError(f"RS485 connect fail to {modbus_port}")
+ logging.info("Connection successful.")
+ except Exception as e:
+ logging.error(f"Initialization failed: {e}")
+ raise
+
+ # ----------------------------------------------------------
+ # 辅助方法
+ # ----------------------------------------------------------
+ def _bus_free(self):
+ """保证距离上一帧 ≥ 30 ms"""
+ with self._lock:
+ elapse = time.perf_counter() - self._last_ts
+ if elapse < self.FRAME_GAP:
+ time.sleep(self.FRAME_GAP - elapse)
+
+ def _execute_read(self, address: int, count: int) -> List[int]:
+ """执行 Modbus 读取操作 (功能码 04), 带总线仲裁。"""
+ self._bus_free()
+
+ rsp = self.cli.read_input_registers(
+ address=address,
+ count=count,
+ slave=self._id
+ )
+
+ self._last_ts = time.perf_counter()
+
+ if rsp.isError():
+ raise RuntimeError(f"Modbus Read Failed (Addr={address}, Count={count}): {rsp}")
+
+ # 确保返回的值是 Python 原生整数
+ return [int(reg) for reg in rsp.registers]
+
+ def _execute_write(self, address: int, values: List[int]):
+ """执行 Modbus 批量写入操作 (功能码 16), 带总线仲裁。"""
+ self._bus_free()
+
+ # values 必须是 Python 原生整数列表
+ rsp = self.cli.write_registers(
+ address=address,
+ values=values,
+ slave=self._id
+ )
+
+ self._last_ts = time.perf_counter()
+
+ if rsp.isError():
+ raise RuntimeError(f"Modbus Write Failed (Addr={address}, Values={values}): {rsp}")
+
+ # ----------------------------------------------------------
+ # 批量读取和数据封装（优化通信效率）
+ # ----------------------------------------------------------
+ def read_all_angles(self) -> List[int]:
+ return self._execute_read(REG_RD_CURRENT_THUMB_PITCH, 6)
+
+ def read_all_torques(self) -> List[int]:
+ return self._execute_read(REG_RD_CURRENT_THUMB_TORQUE, 6)
+
+ def read_all_speeds(self) -> List[int]:
+ return self._execute_read(REG_RD_CURRENT_THUMB_SPEED, 6)
+
+ def read_all_temperatures(self) -> List[int]:
+ return self._execute_read(REG_RD_THUMB_TEMP, 6)
+
+ def read_all_errors(self) -> List[int]:
+ return self._execute_read(REG_RD_THUMB_ERROR, 6)
+
+ # ----------------------------------------------------------
+ # 版本号/设备编号读取（按照协议文档：地址30-44，共15个寄存器）
+ # ----------------------------------------------------------
+ def read_all_versions(self) -> str:
+ """一次性读取全部15个寄存器 (地址30-44)，返回以 '.' 连接的字符串。
+
+ 按 O6 协议文档的版本号格式返回：
+ hand_freedom.hand_version.hand_number_high.hand_number_mid.hand_number_low
+ .hand_direction.hardware_ver_hardware_ver_m.hardware_ver_l
+ .software_ver_h.software_ver_m.software_ver_l
+ .mechanical_ver_h.mechanical_ver_m.mechanical_ver_l
+
+ 例如: "6.1.001.002.003.0.1.2.3.4.5.6.7.8.9"
+ """
+ raw = self._execute_read(REG_RD_HAND_FREEDOM, 15)
+ return ".".join(str(v) for v in raw)
+
+ # ----------------------------------------------------------
+ # 基于 read_all_versions() 的设备编号解析方法
+ # ----------------------------------------------------------
+ def _parse_versions(self):
+ """解析 read_all_versions() 返回的字符串为字典"""
+ parts = self.read_all_versions().split(".")
+ if len(parts) < 15:
+ return {}
+ return {
+ "hand_freedom": int(parts[0]),
+ "hand_version": int(parts[1]),
+ "hand_number_high": int(parts[2]),
+ "hand_number_mid": int(parts[3]),
+ "hand_number_low": int(parts[4]),
+ "hand_direction": int(parts[5]),
+ "hw_ver_high": int(parts[6]),
+ "hw_ver_mid": int(parts[7]),
+ "hw_ver_low": int(parts[8]),
+ "sw_ver_high": int(parts[9]),
+ "sw_ver_mid": int(parts[10]),
+ "sw_ver_low": int(parts[11]),
+ "mech_ver_high": int(parts[12]),
+ "mech_ver_mid": int(parts[13]),
+ "mech_ver_low": int(parts[14]),
+ }
+
+ def get_device_number(self) -> str:
+ """获取设备编号（与机械手上标签相同）。格式：高位+中位+低位 拼接的字符串。"""
+ v = self._parse_versions()
+ if not v:
+ return "0"
+ high, mid, low = v["hand_number_high"], v["hand_number_mid"], v["hand_number_low"]
+ # 将每个字节格式化为无前导零的整数（与标签显示一致）
+ return f"{high}{mid}{low}"
+
+ def get_device_number_value(self) -> int:
+ """获取设备编号数值"""
+ v = self._parse_versions()
+ if not v:
+ return 0
+ high, mid, low = v["hand_number_high"], v["hand_number_mid"], v["hand_number_low"]
+ return high * 65536 + mid * 256 + low
+
+ def get_hardware_version(self) -> str:
+ """获取硬件版本号。格式：高.中.低"""
+ v = self._parse_versions()
+ if not v:
+ return "0.0.0"
+ return f"{v['hw_ver_high']}.{v['hw_ver_mid']}.{v['hw_ver_low']}"
+
+ def get_software_version(self) -> str:
+ """获取软件版本号。格式：高.中.低"""
+ v = self._parse_versions()
+ if not v:
+ return "0.0.0"
+ return f"{v['sw_ver_high']}.{v['sw_ver_mid']}.{v['sw_ver_low']}"
+
+ def get_mechanical_version(self) -> str:
+ """获取机械版本号。格式：高.中.低"""
+ v = self._parse_versions()
+ if not v:
+ return "0.0.0"
+ return f"{v['mech_ver_high']}.{v['mech_ver_mid']}.{v['mech_ver_low']}"
+
+ def get_hand_freedom(self) -> int:
+ """获取自由度（与机械手上标签相同）"""
+ v = self._parse_versions()
+ return v.get("hand_freedom", 0)
+
+ def get_hand_version_raw(self) -> int:
+ """获取手版本原始值"""
+ v = self._parse_versions()
+ return v.get("hand_version", 0)
+
+ def get_hand_direction(self) -> str:
+ """获取手方向，转换为字符：76→'L', 82→'R'"""
+ v = self._parse_versions()
+ val = v.get("hand_direction", 0)
+ if val in (76, 82):
+ return chr(val)
+ # fallback: 直接转为字符（如果值在可打印范围内）
+ return chr(val) if 32 < val < 128 else f'Unknown({val})'
+
+
+ # ----------------------------------------------------------
+ # 只读属性（单个寄存器读取）
+ # ----------------------------------------------------------
+ def _read_reg(self, addr: int) -> int:
+ """读单个输入寄存器（功能码 04），带 30 ms 帧间隔"""
+ return self._execute_read(addr, 1)[0]
+
+ def get_thumb_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_PITCH)
+ def get_thumb_yaw(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW)
+ def get_index_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_PITCH)
+ def get_middle_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_PITCH)
+ def get_ring_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_PITCH)
+ def get_little_pitch(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_PITCH)
+
+ def get_thumb_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_TORQUE)
+ def get_thumb_yaw_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW_TORQUE)
+ def get_index_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_TORQUE)
+ def get_middle_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_TORQUE)
+ def get_ring_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_TORQUE)
+ def get_little_torque(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_TORQUE)
+
+ def get_thumb_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_SPEED)
+ def get_thumb_yaw_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_THUMB_YAW_SPEED)
+ def get_index_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_INDEX_SPEED)
+ def get_middle_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_MIDDLE_SPEED)
+ def get_ring_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_RING_SPEED)
+ def get_little_speed(self) -> int: return self._read_reg(REG_RD_CURRENT_LITTLE_SPEED)
+
+ def get_thumb_temp(self) -> int: return self._read_reg(REG_RD_THUMB_TEMP)
+ def get_thumb_yaw_temp(self) -> int: return self._read_reg(REG_RD_THUMB_YAW_TEMP)
+ def get_index_temp(self) -> int: return self._read_reg(REG_RD_INDEX_TEMP)
+ def get_middle_temp(self) -> int: return self._read_reg(REG_RD_MIDDLE_TEMP)
+ def get_ring_temp(self) -> int: return self._read_reg(REG_RD_RING_TEMP)
+ def get_little_temp(self) -> int: return self._read_reg(REG_RD_LITTLE_TEMP)
+
+ def get_thumb_error(self) -> int: return self._read_reg(REG_RD_THUMB_ERROR)
+ def get_thumb_yaw_error(self) -> int: return self._read_reg(REG_RD_THUMB_YAW_ERROR)
+ def get_index_error(self) -> int: return self._read_reg(REG_RD_INDEX_ERROR)
+ def get_middle_error(self) -> int: return self._read_reg(REG_RD_MIDDLE_ERROR)
+ def get_ring_error(self) -> int: return self._read_reg(REG_RD_RING_ERROR)
+ def get_little_error(self) -> int: return self._read_reg(REG_RD_LITTLE_ERROR)
+
+
+ # ----------------------------------------------------------
+ # 批量 Getter (使用 read_all_... 方法)
+ # ----------------------------------------------------------
+ def get_state(self) -> List[int]:
+ """获取手指电机状态 (角度)"""
+ return self.read_all_angles()
+
+ def get_torque(self) -> List[int]:
+ """获取当前扭矩"""
+ return self.read_all_torques()
+
+ def get_speed(self) -> List[int]:
+ """获取当前速度"""
+ return self.read_all_speeds()
+
+ def get_temperature(self) -> List[int]:
+ """获取当前电机温度"""
+ return self.read_all_temperatures()
+
+ def get_fault(self) -> List[int]:
+ """获取当前电机故障码"""
+ return self.read_all_errors()
+
+ def get_version(self) -> str:
+ """获取当前固件版本号（已转换为字符串格式）"""
+ return self.read_all_versions()
+
+
+ # ----------------------------------------------------------
+ # 写保持寄存器 (单个寄存器写入)
+ # ----------------------------------------------------------
+ def _write_reg(self, addr: int, value: int):
+ """写单个保持寄存器（功能码 16），带 30 ms 帧间隔"""
+ if not 0 <= value <= 255:
+ raise ValueError("value must be 0-255")
+
+ # 确保 value 是 Python 原生 int
+ self._execute_write(addr, [int(value)])
+
+ def _write_regs(self, addr: int, values: List[int]):
+ """写多个保持寄存器（功能码 16），带 30 ms 帧间隔"""
+ # 此时 values 应该已经是经过 is_valid_6xuint8 验证并转换的 Python int 列表
+ if not all(0 <= v <= 255 for v in values):
+ # 这行理论上不应触发，因为上层调用已校验
+ raise ValueError("All values must be 0-255")
+ self._execute_write(addr, values)
+
+
+ def set_thumb_pitch(self, v: int): self._write_reg(REG_WR_THUMB_PITCH, v)
+ def set_thumb_yaw(self, v: int): self._write_reg(REG_WR_THUMB_YAW, v)
+ def set_index_pitch(self, v: int): self._write_reg(REG_WR_INDEX_PITCH, v)
+ def set_middle_pitch(self, v: int): self._write_reg(REG_WR_MIDDLE_PITCH, v)
+ def set_ring_pitch(self, v: int): self._write_reg(REG_WR_RING_PITCH, v)
+ def set_little_pitch(self, v: int): self._write_reg(REG_WR_LITTLE_PITCH, v)
+
+ def set_thumb_torque(self, v: int): self._write_reg(REG_WR_THUMB_TORQUE, v)
+ def set_thumb_yaw_torque(self, v: int): self._write_reg(REG_WR_THUMB_YAW_TORQUE, v)
+ def set_index_torque(self, v: int): self._write_reg(REG_WR_INDEX_TORQUE, v)
+ def set_middle_torque(self, v: int): self._write_reg(REG_WR_MIDDLE_TORQUE, v)
+ def set_ring_torque(self, v: int): self._write_reg(REG_WR_RING_TORQUE, v)
+ def set_little_torque(self, v: int): self._write_reg(REG_WR_LITTLE_TORQUE, v)
+
+ def set_thumb_speed(self, v: int): self._write_reg(REG_WR_THUMB_SPEED, v)
+ def set_thumb_yaw_speed(self, v: int): self._write_reg(REG_WR_THUMB_YAW_SPEED, v)
+ def set_index_speed(self, v: int): self._write_reg(REG_WR_INDEX_SPEED, v)
+ def set_middle_speed(self, v: int): self._write_reg(REG_WR_MIDDLE_SPEED, v)
+ def set_ring_speed(self, v: int): self._write_reg(REG_WR_RING_SPEED, v)
+ def set_little_speed(self, v: int): self._write_reg(REG_WR_LITTLE_SPEED, v)
+
+ # ----------------------------------------------------------
+ # 固定函数 (采用批量写入优化)
+ # ----------------------------------------------------------
+ def is_valid_6xuint8(self, lst: List[Any]) -> bool:
+ """
+ 验证6个0-255的整数列表。
+ 允许输入包含浮点数、NumPy整数等可转换为 int 的类型，并进行范围校验。
+ """
+ if not (isinstance(lst, list) and len(lst) == 6):
+ return False
+
+ try:
+ # 关键：尝试将所有元素转换为 Python 原生 int
+ int_values = [int(v) for v in lst]
+ except (ValueError, TypeError):
+ # 转换失败，列表中包含不可转换的元素
+ return False
+
+ # 校验转换后的整数列表是否在 0-255 范围内
+ return all(0 <= x <= 255 for x in int_values)
+
+ def set_joint_positions(self, joint_angles: List[Any] = None):
+ joint_angles = joint_angles or [0] * 6
+
+ if not self.is_valid_6xuint8(joint_angles):
+ logging.error(f"Invalid joint angles received: {joint_angles}")
+ raise ValueError("Joint angles must be a list of 6 values between 0 and 255 (convertible to int).")
+
+ # 强制转换为 Modbus 兼容的 Python 原生 int 列表
+ int_angles = [int(v) for v in joint_angles]
+
+ # 批量写入 6 个角度寄存器 (从 REG_WR_THUMB_PITCH 地址 0 开始, count=6)
+ self._write_regs(REG_WR_THUMB_PITCH, int_angles)
+
+ def set_speed(self, speed: List[Any] = None):
+ speed = speed or [200] * 6
+ if not self.is_valid_6xuint8(speed):
+ logging.error(f"Invalid speed values received: {speed}")
+ raise ValueError("Speed values must be a list of 6 values between 0 and 255 (convertible to int).")
+
+ int_speed = [int(v) for v in speed]
+ self._write_regs(REG_WR_THUMB_SPEED, int_speed)
+
+ def set_torque(self, torque: List[Any] = None):
+ torque = torque or [200] * 6
+ if not self.is_valid_6xuint8(torque):
+ logging.error(f"Invalid torque values received: {torque}")
+ raise ValueError("Torque values must be a list of 6 values between 0 and 255 (convertible to int).")
+
+ int_torque = [int(v) for v in torque]
+ self._write_regs(REG_WR_THUMB_TORQUE, int_torque)
+
+ # ... (其他固定函数保持不变) ...
+
+ def set_current(self, current: List[int] = None):
+ print("当前O6不支持设置电流", flush=True)
+ pass
+
+ def get_state_for_pub(self) -> list:
+ return self.get_state()
+
+ def get_current_status(self) -> list:
+ return self.get_state()
+
+ def get_joint_speed(self) -> list:
+ return self.get_speed()
+
+ def get_touch_type(self) -> list:
+ return -1
+
+ def get_normal_force(self) -> list:
+ return [-1] * 5
+
+ def get_tangential_force(self) -> list:
+ return [-1] * 5
+
+ def get_approach_inc(self) -> list:
+ return [-1] * 5
+
+ def get_touch(self) -> list:
+ return [-1] * 5
+
+ def get_thumb_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_index_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_middle_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_ring_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_little_matrix_touch(self,sleep_time=0):
+ return np.full((12, 6), -1)
+
+ def get_matrix_touch(self) -> list:
+ thumb_matrix = np.full((12, 6), -1)
+ index_matrix = np.full((12, 6), -1)
+ middle_matrix = np.full((12, 6), -1)
+ ring_matrix = np.full((12, 6), -1)
+ little_matrix = np.full((12, 6), -1)
+ return thumb_matrix , index_matrix , middle_matrix , ring_matrix , little_matrix
+
+ def get_serial_number(self):
+
+ return "["+str(self.get_hand_freedom())+"-"+str(self.get_mechanical_version())+"-"+str(self.get_device_number())+"-"+str(self.get_hand_direction())+"]"
+
+ def get_matrix_touch_v2(self) -> list:
+ return self.get_matrix_touch()
+
+ def get_finger_order(self):
+ return ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"]
+
+ def clear_faults(self):
+ pass
+
+ def close(self):
+ if hasattr(self, 'connected') and self.connected:
+ self.cli.close()
+ self.connected = False
+ logging.info("Modbus connection closed.")
+
+ def __enter__(self):
+ return self
+
+ def __exit__(self, exc_type, exc_val, exc_tb):
+ self.close()
+
+ # ----------------------------------------------------------
+ # 便捷函数
+ # ----------------------------------------------------------
+ def set_all_fingers(self, pitch: int):
+ """同时设置五指弯曲角度（0-255），使用批量写入"""
+ # 允许传入 float/numpy int 等可转换为 int 的类型
+ try:
+ pitch_int = int(pitch)
+ except (ValueError, TypeError):
+ raise ValueError("Pitch value must be a number convertible to int (0-255)")
+
+ if not 0 <= pitch_int <= 255:
+ raise ValueError("Pitch value must be 0-255")
+
+ # 批量设置所有 6 个关节的角度
+ self.set_joint_positions([pitch_int] * 6)
+
+ def relax(self):
+ """全部手指伸直（255）"""
+ self.set_all_fingers(255)
+
+ def fist(self):
+ """全部手指弯曲（0）"""
+ self.set_all_fingers(0)
+
+ def dump_status(self):
+ """打印当前所有可读状态 (使用批量读取优化)"""
+ print("--------- O6 Hand Status ---------")
+
+ angles = self.get_state()
+ temps = self.get_temperature()
+ errors = self.get_fault()
+
+ # 解析版本号字符串
+ v = self._parse_versions()
+
+ if v:
+ device_num_str = f"{v['hand_number_high']}{v['hand_number_mid']}{v['hand_number_low']}"
+ hw_ver = f"{v['hw_ver_high']}.{v['hw_ver_mid']}.{v['hw_ver_low']}"
+ sw_ver = f"{v['sw_ver_high']}.{v['sw_ver_mid']}.{v['sw_ver_low']}"
+ mech_ver = f"{v['mech_ver_high']}.{v['mech_ver_mid']}.{v['mech_ver_low']}"
+
+ print(f"Device Number: {device_num_str}")
+ print(f"HWSW Version: HW={hw_ver} SW={sw_ver}")
+ print(f"Mechanical Ver: {mech_ver}")
+ print(f"Hand Freedom: {v['hand_freedom']}")
+ print(f"Full Version: {self.read_all_versions()}")
+
+ print(f"Joint Angles: {angles}")
+ print(f"Temperature: {temps}℃")
+ print(f"Error Codes: {errors}")
+ print("----------------------------------")
+
+
+# ------------------------------------------------------------------
+# 命令行快速测试
+# ------------------------------------------------------------------
+if __name__ == "__main__":
+ import argparse
+
+ # 假设默认站号是 0x27 (39)
+ DEFAULT_HAND_ID = 0x27
+
+ parser = argparse.ArgumentParser(description="O6 Hand Modbus tester (using pymodbus 3.5.1)")
+ parser.add_argument("-p", "--port", required=True, help="串口, 如 /dev/ttyUSB0")
+ parser.add_argument("-l", "--left", action="store_const", const=0x28, default=DEFAULT_HAND_ID, dest='hand_id', help="左手 (0x28)，默认右手 (0x27)")
+
+ args = parser.parse_args()
+
+ try:
+ # 使用 with 语句确保连接关闭，这是 pymodbus 的推荐用法
+ with LinkerHandO6RS485(hand_id=args.hand_id, modbus_port=args.port, baudrate=115200) as hand:
+ hand.dump_status()
+
+ # 测试新增的设备编号读取方法
+ print("\n--- 设备信息 ---")
+ print(f"设备编号（字符串）: {hand.get_device_number()}")
+ print(f"设备编号（数值）: {hand.get_device_number_value()}")
+ print(f"硬件版本号: {hand.get_hardware_version()}")
+ print(f"软件版本号: {hand.get_software_version()}")
+ print(f"机械版本号: {hand.get_mechanical_version()}")
+
+ print("\n执行 relax → 伸直")
+ hand.relax()
+ time.sleep(1)
+ print("执行 fist → 握拳")
+ hand.fist()
+ time.sleep(1)
+ hand.relax()
+ print("演示完成")
+
+ except ConnectionError as e:
+ print(f"连接错误: {e}")
+ except RuntimeError as e:
+ print(f"Modbus 运行时错误: {e}")
+ except StructError as e:
+ print(f"数据结构错误 (请检查输入数据类型是否为原生int): {e}")
+ except Exception as e:
+ print(f"发生其他错误: {e}")
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/linker_hand_api.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/linker_hand_api.py
new file mode 100644
index 0000000..631a7c9
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/linker_hand_api.py
@@ -0,0 +1,355 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import sys, os, time,threading
+sys.path.append(os.path.dirname(os.path.abspath(__file__)))
+from utils.mapping import *
+from utils.color_msg import ColorMsg
+from utils.load_write_yaml import LoadWriteYaml
+from utils.open_can import OpenCan
+
+class LinkerHandApi:
+ def __init__(self, hand_type="left", hand_joint="L10", modbus = "None",can="can0"): # Ubuntu:can0 win:PCAN_USBBUS1
+ self.last_position = []
+ self.yaml = LoadWriteYaml()
+ self.config = self.yaml.load_setting_yaml()
+ self.version = self.config["VERSION"]
+ self.can = can
+ ColorMsg(msg=f"Current SDK version: {self.version}", color="green")
+ self.hand_joint = hand_joint
+ self.hand_type = hand_type
+ self.is_palm_touch = -1 # 是否为全掌压力传感器
+ if self.hand_type == "left":
+ self.hand_id = 0x28 # Left hand
+ if self.hand_type == "right":
+ self.hand_id = 0x27 # Right hand
+ if self.hand_joint.upper() == "O6":
+ if modbus != "None":
+ from core.rs485.linker_hand_o6_rs485 import LinkerHandO6RS485
+ self.hand = LinkerHandO6RS485(hand_id=self.hand_id,modbus_port=modbus,baudrate=115200)
+ else:
+ from core.can.linker_hand_o6_can import LinkerHandO6Can
+ self.hand = LinkerHandO6Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+ if self.hand_joint == "L6":
+ if modbus != "None":
+ from core.rs485.linker_hand_l6_rs485 import LinkerHandL6RS485
+ self.hand = LinkerHandL6RS485(hand_id=self.hand_id,modbus_port=modbus,baudrate=115200)
+ else:
+ from core.can.linker_hand_l6_can import LinkerHandL6Can
+ self.hand = LinkerHandL6Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+ if self.hand_joint == "L7":
+ if modbus != "None":
+ from core.rs485.linker_hand_l7_rs485 import LinkerHandL7RS485
+ self.hand = LinkerHandL7RS485(hand_id=self.hand_id,modbus_port=modbus,baudrate=115200)
+ else:
+ from core.can.linker_hand_l7_can import LinkerHandL7Can
+ self.hand = LinkerHandL7Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+ if self.hand_joint == "L10":
+ if modbus != "None":
+ from core.rs485.linker_hand_l10_rs485 import LinkerHandL10RS485
+ self.hand = LinkerHandL10RS485(hand_id=self.hand_id,modbus_port=modbus,baudrate=115200)
+ else:
+ from core.can.linker_hand_l10_can import LinkerHandL10Can
+ self.hand = LinkerHandL10Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+ if self.hand_joint == "L20":
+ from core.can.linker_hand_l20_can import LinkerHandL20Can
+ self.hand = LinkerHandL20Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+ if self.hand_joint == "G20":
+ from core.can.linker_hand_g20_can import LinkerHandG20Can
+ self.hand = LinkerHandG20Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+ time.sleep(0.01)
+ self.is_palm_touch = self.hand.get_touch_sensor_type()
+ ColorMsg(msg=f"传感器类型:{self.is_palm_touch}")
+ if self.hand_joint == "L21":
+ from core.can.linker_hand_l21_can import LinkerHandL21Can
+ self.hand = LinkerHandL21Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+ if self.hand_joint == "L25":
+ from core.can.linker_hand_l25_can import LinkerHandL25Can
+ self.hand = LinkerHandL25Can(can_id=self.hand_id,can_channel=self.can, yaml=self.yaml)
+ # Open can0
+ if sys.platform == "linux" and modbus=="None":
+ self.open_can = OpenCan(load_yaml=self.yaml)
+ self.open_can.open_can(self.can)
+ self.is_can = self.open_can.is_can_up_sysfs(interface=self.can)
+ if not self.is_can:
+ ColorMsg(msg=f"{self.can} interface is not open", color="red")
+ sys.exit(1)
+ version = self.get_embedded_version()
+ self.serial_number = self.get_serial_number()
+ if version == None or len(version) == 0:
+ ColorMsg(msg="Warning: Hardware version number not recognized, it is recommended to terminate the program and re insert USB to CAN conversion", color="yellow")
+ else:
+ ColorMsg(msg=f"Embedded:{version}", color="green")
+ ColorMsg(msg=f"Linker Hand Serial Number: {self.serial_number}", color="green")
+
+
+ # Five-finger movement
+ def finger_move(self, pose=[]):
+ '''
+ Five-finger movement
+ @params: pose list L7 len(7) | L10 len(10) | L20 len(20) | L25 len(25) 0~255
+ '''
+
+ if len(pose) == 0:
+ return
+ pose = [int(v) for v in pose]
+ if any(not isinstance(x, (int, float)) or x < 0 or x > 255 for x in pose):
+ ColorMsg(msg=f"The numerical range cannot be less than 0 or greater than 255",color="red")
+ return
+ if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6") and len(pose) == 6:
+ self.hand.set_joint_positions(pose)
+ elif self.hand_joint == "L7" and len(pose) == 7:
+ self.hand.set_joint_positions(pose)
+ elif self.hand_joint == "L10" and len(pose) == 10:
+ self.hand.set_joint_positions(pose)
+ elif self.hand_joint == "L20" and len(pose) == 20:
+ self.hand.set_joint_positions(pose)
+ elif self.hand_joint == "G20" and len(pose) == 20:
+ self.hand.set_joint_positions(pose)
+ elif self.hand_joint == "L21" and len(pose) == 25:
+ self.hand.set_joint_positions(pose)
+ elif self.hand_joint == "L25" and len(pose) == 25:
+ self.hand.set_joint_positions(pose)
+ else:
+ ColorMsg(msg=f"Current LinkerHand is {self.hand_type}{self.hand_joint}, action sequence is {pose}, does not match", color="red")
+ self.last_position = pose
+
+ def _get_normal_force(self):
+ '''# Get normal force'''
+ self.hand.get_normal_force()
+
+ def _get_tangential_force(self):
+ '''# Get tangential force'''
+ self.hand.get_tangential_force()
+
+ def _get_tangential_force_dir(self):
+ '''# Get tangential force direction'''
+ self.hand.get_tangential_force_dir()
+
+ def _get_approach_inc(self):
+ '''# Get approach increment'''
+ self.hand.get_approach_inc()
+
+
+ def set_speed(self, speed=[100]*5):
+ '''# Set speed'''
+ has_non_int = any(not isinstance(x, (int, float)) or x < 0 or x > 255 for x in speed)
+ if has_non_int:
+ print("Set Speed The numerical range can only be positive integers or floating-point numbers between 0 and 255", flush=True)
+ return
+ if len(speed) < 5:
+ print("数据长度不够,至少5个元素", flush=True)
+ return
+ if self.hand_joint == "L7" and len(speed) < 7:
+ print("数据长度不够,至少7个元素", flush=True)
+ return
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} set speed to {speed}", color="green")
+ self.hand.set_speed(speed=speed)
+
+ def set_joint_speed(self, speed=[100]*5):
+ '''Set speed by topic'''
+ if len(speed) == 0:
+ return
+ if any(not isinstance(x, (int, float)) or x < 10 or x > 255 for x in speed):
+ ColorMsg(msg=f"The numerical range cannot be less than 10 or greater than 255",color="red")
+ return
+ self.hand.set_speed(speed=speed)
+
+ def set_torque(self, torque=[180] * 5):
+ '''Set maximum torque'''
+ has_non_int = any(not isinstance(x, (int, float)) or x < 0 or x > 255 for x in torque)
+ if has_non_int:
+ print("Set Torque The numerical range can only be positive integers or floating-point numbers between 0 and 255", flush=True)
+ return
+ if len(torque) < 5:
+ print("数据长度不够,至少5个元素", flush=True)
+ return
+ if self.hand_joint == "L7" and len(torque) < 7:
+ print("数据长度不够,至少7个元素", flush=True)
+ return
+ if (self.hand_joint == "L6" or self.hand_joint == "O6") and len(torque) != 6:
+ print("L6 or O6数据长度错误,至少6个元素", flush=True)
+ return
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} set maximum torque to {torque}", color="green")
+ return self.hand.set_torque(torque=torque)
+
+
+ def set_current(self, current=[250] * 5):
+ '''Set current L7/L10/L25 not supported'''
+ if any(not isinstance(x, (int, float)) or x < 0 or x > 255 for x in current):
+ print("Set Current The numerical range can only be positive integers or floating-point numbers between 0 and 255", flush=True)
+ return
+ if self.hand_joint == "L20":
+ return self.hand.set_current(current=current)
+ else:
+ pass
+
+ def get_embedded_version(self):
+ '''Get embedded version'''
+ return self.hand.get_version()
+
+ def get_serial_number(self):
+ '''Get serial number'''
+ try:
+ return self.hand.sn
+ except:
+ return self.hand.get_serial_number()
+
+ def get_current(self):
+ '''Get current'''
+ return self.hand.get_current()
+
+ def get_state(self):
+ '''Get current joint state'''
+ return self.hand.get_current_status()
+
+
+ def get_state_for_pub(self):
+ return self.hand.get_current_pub_status()
+
+ def get_speed(self):
+ '''Get speed'''
+ return self.hand.get_speed()
+
+
+ def get_joint_speed(self):
+ speed = []
+ if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6":
+ return self.hand.get_speed()
+ elif self.hand_joint == "L7":
+ return self.hand.get_speed()
+ elif self.hand_joint == "L10":
+ speed = self.hand.get_speed()
+ return speed
+ elif self.hand_joint == "G20":
+ return self.hand.get_speed()
+ elif self.hand_joint == "L20":
+ speed = self.hand.get_speed()
+ return [255, speed[1], speed[2], speed[3], speed[4], 255, 255, 255, 255, 255, speed[0], 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ elif self.hand_joint == "L21":
+ return self.hand.get_speed()
+ elif self.hand_joint == "L25":
+ return self.hand.get_speed()
+
+ def get_touch_type(self):
+ '''Get touch type'''
+ try:
+ return self.hand.touch_type
+ except:
+ return self.hand.get_touch_type()
+
+ def get_force(self):
+ '''Get normal force, tangential force, tangential force direction, approach sensing data'''
+ self._get_normal_force()
+ self._get_tangential_force()
+ self._get_tangential_force_dir()
+ self._get_approach_inc()
+ return self.hand.get_force()
+
+ def get_touch(self):
+ '''Get touch data'''
+ return self.hand.get_touch()
+
+ def get_matrix_touch(self):
+ return self.hand.get_matrix_touch()
+
+ def get_matrix_touch_v2(self):
+ return self.hand.get_matrix_touch_v2()
+
+
+ def get_thumb_matrix_touch(self,sleep_time=0):
+ if sleep_time > 0:
+ return self.hand.get_thumb_matrix_touch(sleep_time=sleep_time)
+ else:
+ return self.hand.get_thumb_matrix_touch()
+
+ def get_index_matrix_touch(self,sleep_time=0):
+ if sleep_time > 0:
+ return self.hand.get_index_matrix_touch(sleep_time=sleep_time)
+ else:
+ return self.hand.get_index_matrix_touch()
+
+ def get_middle_matrix_touch(self,sleep_time=0):
+ if sleep_time > 0:
+ return self.hand.get_middle_matrix_touch(sleep_time=sleep_time)
+ else:
+ return self.hand.get_middle_matrix_touch()
+
+ def get_ring_matrix_touch(self,sleep_time=0):
+ if sleep_time > 0:
+ return self.hand.get_ring_matrix_touch(sleep_time=sleep_time)
+ else:
+ return self.hand.get_ring_matrix_touch()
+
+ def get_little_matrix_touch(self,sleep_time=0):
+ if sleep_time > 0:
+ return self.hand.get_little_matrix_touch(sleep_time=sleep_time)
+ else:
+ return self.hand.get_little_matrix_touch()
+
+ def get_palm_matrix_touch(self,sleep_time=0):
+ if self.is_palm_touch == 5:
+ if sleep_time > 0:
+ return self.hand.get_palm_matrix_touch(sleep_time=sleep_time)
+ else:
+ return self.hand.get_palm_matrix_touch()
+
+ def get_torque(self):
+ '''Get current maximum torque'''
+ return self.hand.get_torque()
+
+ def get_temperature(self):
+ '''Get current motor temperature'''
+ return self.hand.get_temperature()
+
+ def get_fault(self):
+ '''Get motor fault code'''
+ return self.hand.get_fault()
+
+ def clear_faults(self):
+ '''Clear motor fault codes Not supported yet, currently only supports L20'''
+ self.hand.clear_faults()
+ return [0] * 5
+
+ def set_enable(self):
+ '''Set motor enable Only supports L25'''
+ if self.hand_joint == "L25":
+ self.hand.set_enable_mode()
+ else:
+ pass
+
+ def set_disable(self):
+ '''Set motor disable Only supports L25'''
+ if self.hand_joint == "L25":
+ self.hand.set_disability_mode()
+ else:
+ pass
+
+ def get_finger_order(self):
+ '''Get finger motor order'''
+ # if self.hand_joint == "L21" or self.hand_joint == "L25" or self.hand_joint == "G20":
+ # return self.hand.get_finger_order()
+ # else:
+ # return []
+ return self.hand.get_finger_order()
+
+ def range_to_arc_left(self, state, hand_joint):
+ return range_to_arc_left(left_range=state, hand_joint=hand_joint)
+
+ def range_to_arc_right(self, state, hand_joint):
+ return range_to_arc_right(right_range=state, hand_joint=hand_joint)
+
+ def arc_to_range_left(self,state,hand_joint):
+ return arc_to_range_left(hand_arc_l=state,hand_joint=hand_joint)
+
+ def arc_to_range_right(self,state,hand_joint):
+ return arc_to_range_right(right_arc=state,hand_joint=hand_joint)
+
+ def show_fun_table(self):
+ self.hand.show_fun_table()
+
+ def close_can(self):
+ if sys.platform == "linux" and modbus=="None":
+ self.open_can.close_can(can=self.can)
+
+if __name__ == "__main__":
+ hand = LinkerHandApi(hand_type="right", hand_joint="L10")
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/__init__.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/color_msg.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/color_msg.py
new file mode 100644
index 0000000..9ba06ac
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/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, flush=True)
+ return
+ str += msg + "\033[0m"
+ print(str, flush=True)
\ No newline at end of file
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/init_linker_hand.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/init_linker_hand.py
new file mode 100644
index 0000000..6bfcae7
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/init_linker_hand.py
@@ -0,0 +1,81 @@
+'''
+Author: HJX
+Date: 2025-04-01 14:09:21
+LastEditors: Please set LastEditors
+LastEditTime: 2025-04-08 11:18:23
+FilePath: /Linker_Hand_SDK_ROS/src/linker_hand_sdk_ros/scripts/LinkerHand/utils/init_linker_hand.py
+Description:
+symbol_custom_string_obkorol_copyright:
+'''
+import yaml, os, sys
+sys.path.append(os.path.dirname(os.path.abspath(__file__)))
+from load_write_yaml import LoadWriteYaml
+
+class InitLinkerHand():
+ def __init__(self):
+ self.yaml = LoadWriteYaml()
+ self.setting = self.yaml.load_setting_yaml()
+
+ def current_hand(self):
+ '''
+ 初始化灵巧手
+ return: hand_joint str L7/L10/L20/L21/L25, hand_type str left or right
+ '''
+ # 左手是否配置
+ self.left_hand = None
+ self.left_hand_joint = None
+ self.left_hand_type = None
+ self.left_hand_force = None
+ self.left_hand_pose = None
+ self.left_hand_torque = [200, 200, 200, 200, 200]
+ self.left_hand_speed = [80, 200, 200, 200, 200]
+ # 右手是否配置
+ self.right_hand = None
+ self.right_hand_joint = None
+ self.right_hand_type = None
+ self.right_hand_force = None
+ self.right_hand_pose = None
+ self.right_hand_torque = [200, 200, 200, 200, 200]
+ self.right_hand_speed = [80, 200, 200, 200, 200]
+ if self.setting['LINKER_HAND']['LEFT_HAND']['EXISTS'] == True:
+ self.left_hand = True
+ self.left_hand_joint = self.setting['LINKER_HAND']['LEFT_HAND']['JOINT']
+ self.left_hand_type = "left"
+ self.left_hand_force = self.setting['LINKER_HAND']['LEFT_HAND']['TOUCH']
+ if self.left_hand_joint == "L7":
+ # The data length of L7 is 7, reinitialize here
+ self.left_hand_pose = [255, 200, 255, 255, 255, 255, 180]
+ self.left_hand_torque = [250, 250, 250, 250, 250, 250, 250]
+ self.left_hand_speed = [120, 180, 180, 180, 180, 180, 180]
+ elif self.left_hand_joint == "L10":
+ self.left_hand_pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
+ elif self.left_hand_joint == "L20":
+ self.left_hand_pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255]
+ elif self.left_hand_joint == "L21":
+ self.left_hand_pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ elif self.left_hand_joint == "L25":
+ self.left_hand_pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ # 判断右手是否配置
+ if self.setting['LINKER_HAND']['RIGHT_HAND']['EXISTS'] == True:
+ self.right_hand = True
+ self.right_hand_joint = self.setting['LINKER_HAND']['RIGHT_HAND']['JOINT']
+ self.right_hand_type = "right"
+ self.right_hand_force = self.setting['LINKER_HAND']['RIGHT_HAND']['TOUCH']
+ if self.right_hand_joint == "L7":
+ # The data length of L7 is 7, reinitialize here
+ self.right_hand_pose = [255, 200, 255, 255, 255, 255, 180]
+ self.right_hand_torque = [250, 250, 250, 250, 250, 250, 250]
+ self.right_hand_speed = [120, 250, 250, 250, 250, 250, 250]
+ elif self.right_hand_joint == "L10":
+ self.right_hand_pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
+ elif self.right_hand_joint == "L20":
+ self.right_hand_pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255]
+ elif self.right_hand_joint == "L21":
+ self.right_hand_pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ elif self.right_hand_joint == "L25":
+ self.right_hand_pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+
+
+ return self.left_hand ,self.left_hand_joint ,self.left_hand_type ,self.left_hand_force,self.left_hand_pose, self.left_hand_torque, self.left_hand_speed ,self.right_hand ,self.right_hand_joint ,self.right_hand_type ,self.right_hand_force,self.right_hand_pose, self.right_hand_torque, self.right_hand_speed,self.setting
+
+
\ No newline at end of file
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/load_write_yaml.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/load_write_yaml.py
new file mode 100644
index 0000000..160674c
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/load_write_yaml.py
@@ -0,0 +1,101 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+'''
+Author: HJX
+Date: 2025-04-01 14:09:21
+LastEditors: Please set LastEditors
+LastEditTime: 2025-04-11 10:19:01
+FilePath: /LinkerHand_Python_SDK/LinkerHand/utils/load_write_yaml.py
+Description:
+symbol_custom_string_obkorol_copyright:
+'''
+import yaml, os, sys
+class LoadWriteYaml():
+ def __init__(self):
+ # 由于是API形式，这里要给配置文件目录绝对路径
+ #yaml_path = "/home/linkerhand/ROS2/linker_hand_ros2_sdk/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand"
+ yaml_path = os.path.dirname(os.path.abspath(__file__)) + "/../../LinkerHand"
+ self.setting_path = yaml_path+"/config/setting.yaml"
+ self.l7_positions = yaml_path+"/config/L7_positions.yaml"
+ self.l10_positions = yaml_path+"/config/L10_positions.yaml"
+ self.l20_positions = yaml_path+"/config/L20_positions.yaml"
+ self.l21_positions = yaml_path+"/config/L21_positions.yaml"
+ self.l25_positions = yaml_path+"/config/L25_positions.yaml"
+
+
+ def load_setting_yaml(self):
+ try:
+ with open(self.setting_path, 'r', encoding='utf-8') as file:
+ setting = yaml.safe_load(file)
+ self.sdk_version = setting["VERSION"]
+ self.left_hand_exists = setting['LINKER_HAND']['LEFT_HAND']['EXISTS']
+ self.left_hand_names = setting['LINKER_HAND']['LEFT_HAND']['NAME']
+ self.left_hand_joint = setting['LINKER_HAND']['LEFT_HAND']['JOINT']
+ self.left_hand_force = setting['LINKER_HAND']['LEFT_HAND']['TOUCH']
+ self.right_hand_exists = setting['LINKER_HAND']['RIGHT_HAND']['EXISTS']
+ self.right_hand_names = setting['LINKER_HAND']['RIGHT_HAND']['NAME']
+ self.right_hand_joint = setting['LINKER_HAND']['RIGHT_HAND']['JOINT']
+ self.right_hand_force = setting['LINKER_HAND']['RIGHT_HAND']['TOUCH']
+ self.password = setting['PASSWORD']
+ except Exception as e:
+ setting = None
+ print(f"Error reading setting.yaml: {e}")
+ self.setting = setting
+ return self.setting
+
+ def load_action_yaml(self,hand_joint="",hand_type=""):
+ if hand_joint == "L20":
+ action_path = self.l20_positions
+ elif hand_joint == "L10":
+ action_path = self.l10_positions
+ elif hand_joint == "L25":
+ action_path = self.l25_positions
+ elif hand_joint == "L21":
+ action_path = self.l21_positions
+ elif hand_joint == "L7":
+ action_path = self.l7_positions
+ print(action_path)
+ try:
+ with open(action_path, 'r', encoding='utf-8') as file:
+ yaml_data = yaml.safe_load(file)
+ if hand_type == "left":
+ self.action_yaml = yaml_data["LEFT_HAND"]
+ else:
+ self.action_yaml = yaml_data["RIGHT_HAND"]
+ except Exception as e:
+ self.action_yaml = None
+ print(f"yaml配置文件不存在: {e}")
+ return self.action_yaml
+
+ def write_to_yaml(self, action_name, action_pos,hand_joint="",hand_type=""):
+ a = False
+ if hand_joint == "L20":
+ action_path = self.l20_positions
+ elif hand_joint == "L10":
+ action_path = self.l10_positions
+ elif hand_joint == "L7":
+ action_path = self.l7_positions
+ elif hand_joint == "L21":
+ action_path = self.l21_positions
+ elif hand_joint == "L25":
+ action_path = self.l25_positions
+ try:
+ with open(action_path, 'r', encoding='utf-8') as file:
+ yaml_data = yaml.safe_load(file)
+ print(yaml_data)
+ if hand_type == "left":
+ if yaml_data["LEFT_HAND"] == None:
+ yaml_data["LEFT_HAND"] = []
+ yaml_data["LEFT_HAND"].append({"ACTION_NAME": action_name, "POSITION": action_pos})
+ elif hand_type == "right":
+ if yaml_data["RIGHT_HAND"] == None:
+ yaml_data["RIGHT_HAND"] = []
+ yaml_data["RIGHT_HAND"].append({"ACTION_NAME": action_name, "POSITION": action_pos})
+ with open(action_path, 'w', encoding='utf-8') as file:
+ yaml.safe_dump(yaml_data, file, allow_unicode=True)
+ a = True
+ except Exception as e:
+ a = False
+ print(f"Error writing to yaml file: {e}")
+ return a
+
\ No newline at end of file
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/mapping.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/mapping.py
new file mode 100644
index 0000000..deb6a19
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/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))
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/open_can.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/open_can.py
new file mode 100644
index 0000000..96aa932
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/utils/open_can.py
@@ -0,0 +1,145 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+'''
+Author: HJX
+Date: 2025-04-01 14:09:21
+LastEditors: Please set LastEditors
+LastEditTime: 2025-04-11 09:15:31
+FilePath: /Linker_Hand_SDK_ROS/src/linker_hand_sdk_ros/scripts/LinkerHand/utils/open_can.py
+Description:
+symbol_custom_string_obkorol_copyright:
+'''
+import sys,os,time,subprocess
+sys.path.append(os.path.dirname(os.path.abspath(__file__)))
+from color_msg import ColorMsg
+from load_write_yaml import LoadWriteYaml
+# from ament_index_python.packages import get_package_share_directory
+import os
+
+
+class OpenCan:
+ def __init__(self,load_yaml=None):
+ self.yaml = LoadWriteYaml()
+ self.password = self.yaml.load_setting_yaml()["PASSWORD"]
+
+ def open_can0(self):
+ try:
+ # 检查 can0 接口是否已存在并处于 up 状态
+ result = subprocess.run(
+ ["ip", "link", "show", "can0"],
+ check=True,
+ text=True,
+ capture_output=True
+ )
+ if "state UP" in result.stdout:
+ return
+ # 如果没有处于 UP 状态，则配置接口
+ subprocess.run(
+ ["sudo", "-S", "ip", "link", "set", "can0", "up", "type", "can", "bitrate", "1000000"],
+ input=f"{self.password}\n",
+ check=True,
+ text=True,
+ capture_output=True
+ )
+
+ except subprocess.CalledProcessError as e:
+ pass
+ except Exception as e:
+ pass
+ def open_can(self,can="can0"):
+ try:
+ # 检查 can0 接口是否已存在并处于 up 状态
+ result = subprocess.run(
+ ["ip", "link", "show", can],
+ check=True,
+ text=True,
+ capture_output=True
+ )
+ if "state UP" in result.stdout:
+ return
+ # 如果没有处于 UP 状态，则配置接口
+ subprocess.run(
+ ["sudo", "-S", "ip", "link", "set", can, "up", "type", "can", "bitrate", "1000000"],
+ input=f"{self.password}\n",
+ check=True,
+ text=True,
+ capture_output=True
+ )
+ except subprocess.CalledProcessError as e:
+ pass
+ except Exception as e:
+ pass
+
+
+ def is_can_up_sysfs(self, interface="can0"):
+ # 检查接口目录是否存在
+ if not os.path.exists(f"/sys/class/net/{interface}"):
+ return False
+ # 读取接口状态
+ try:
+ with open(f"/sys/class/net/{interface}/operstate", "r") as f:
+ state = f.read().strip()
+ if state == "up":
+ return True
+ except Exception as e:
+ print(f"Error reading CAN interface state: {e}")
+ return False
+
+ def close_can0(self):
+ try:
+ # 检查 can0 接口是否存在
+ result = subprocess.run(
+ ["ip", "link", "show", "can0"],
+ check=True,
+ text=True,
+ capture_output=True
+ )
+
+ # 如果接口存在且处于 UP 状态，则关闭它
+ if "state UP" in result.stdout:
+ subprocess.run(
+ ["sudo", "-S", "ip", "link", "set", "can0", "down"],
+ input=f"{self.password}\n",
+ check=True,
+ text=True,
+ capture_output=True
+ )
+ return True
+ return False
+
+ except subprocess.CalledProcessError as e:
+ print(f"Error closing CAN interface: {e}")
+ return False
+ except Exception as e:
+ print(f"Unexpected error: {e}")
+ return False
+
+ def close_can(self,can="can0"):
+ try:
+ # 检查 can0 接口是否存在
+ result = subprocess.run(
+ ["ip", "link", "show", can],
+ check=True,
+ text=True,
+ capture_output=True
+ )
+
+ # 如果接口存在且处于 UP 状态，则关闭它
+ if "state UP" in result.stdout:
+ subprocess.run(
+ ["sudo", "-S", "ip", "link", "set", can, "down"],
+ input=f"{self.password}\n",
+ check=True,
+ text=True,
+ capture_output=True
+ )
+ return True
+ return False
+
+ except subprocess.CalledProcessError as e:
+ print(f"Error closing CAN interface: {e}")
+ return False
+ except Exception as e:
+ print(f"Unexpected error: {e}")
+ return False
+
\ No newline at end of file
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/__init__.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py
new file mode 100644
index 0000000..dc91de3
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py
@@ -0,0 +1,700 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+'''
+编译: colcon build --symlink-install
+启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
+'''
+from re import A
+import rclpy,sys # ROS2 Python接口库
+import time
+import numpy as np
+from rclpy.node import Node # ROS2 节点类
+from rclpy.clock import Clock
+from rclpy.parameter import Parameter
+from rcl_interfaces.msg import ParameterDescriptor
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+from std_msgs.msg import String, Header, Float32MultiArray, Float64MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+
+COMMAND_QOS = QoSProfile(
+ history=HistoryPolicy.KEEP_LAST,
+ depth=1,
+ reliability=ReliabilityPolicy.RELIABLE,
+)
+
+
+def command_changed(previous, current):
+ """Return whether a non-empty command differs from the applied command."""
+ values = list(current)
+ if not values:
+ return False
+ if previous is None or len(previous) != len(values):
+ return True
+ return any(float(old) != float(new) for old, new in zip(previous, values))
+
+
+def state_poll_due(last_poll_time, now, poll_period):
+ """Keep slow CAN state reads off the latency-sensitive command path."""
+ return last_poll_time is None or now >= last_poll_time + poll_period
+
+
+class HandPIDController:
+ """
+ 外层 PID 控制器 — 用实际位置反馈加速电机响应
+
+ cmd[i] = target[i] + (P + I + D) * boost
+ P = kp * error
+ I = ki * ∫error·dt (带 anti-windup)
+ D = kd * d(error)/dt
+ 最终指令钳位在 [0, 255], boost 钳位在 [-max_boost, +max_boost]
+ """
+
+ def __init__(self, kp: float = 1.5, ki: float = 0.0, kd: float = 0.0,
+ max_boost: int = 40, dead_zone: int = 3,
+ integral_max: float = 100.0, enabled: bool = True):
+ self.kp = kp
+ self.ki = ki
+ self.kd = kd
+ self.max_boost = max_boost # 单帧最大额外偏移
+ self.dead_zone = dead_zone # |error| < 此值不补偿
+ self.integral_max = integral_max # 积分上限 (anti-windup)
+ self.enabled = enabled
+ self._last_error: list = None
+ self._integral: list = None
+ self._last_time: float = None
+
+ def apply(self, target: list, actual: list) -> list:
+ if not self.enabled or not actual:
+ return target
+ if len(actual) != len(target):
+ return target
+ if actual[0] == -1: # 手状态未初始化
+ return target
+
+ now = time.perf_counter()
+ dt = now - self._last_time if self._last_time else 0.01
+ dt = max(dt, 0.001) # 防止 dt=0 导致除零
+ dt = min(dt, 0.1) # 防止长时间停顿时 dt 过大
+ self._last_time = now
+
+ n = len(target)
+ if self._last_error is None or len(self._last_error) != n:
+ self._last_error = [0.0] * n
+ self._integral = [0.0] * n
+
+ boosted = []
+ for i in range(n):
+ error = target[i] - actual[i]
+
+ if abs(error) < self.dead_zone:
+ # 死区内: 积分衰减, 不补偿
+ self._integral[i] *= 0.9
+ self._last_error[i] = 0.0
+ boosted.append(int(target[i]))
+ continue
+
+ # ---- P ----
+ p_term = self.kp * error
+
+ # ---- I (anti-windup) ----
+ self._integral[i] += error * dt
+ self._integral[i] = max(-self.integral_max,
+ min(self.integral_max, self._integral[i]))
+ i_term = self.ki * self._integral[i]
+
+ # ---- D ----
+ d_term = self.kd * (error - self._last_error[i]) / dt
+ self._last_error[i] = error
+
+ # ---- 合成 ----
+ boost = p_term + i_term + d_term
+ boost = max(-self.max_boost, min(self.max_boost, boost))
+ cmd = target[i] + boost
+ cmd = max(0.0, min(255.0, cmd))
+ boosted.append(int(cmd))
+
+ return boosted
+
+ def reset(self):
+ self._last_error = None
+ self._integral = None
+ self._last_time = None
+
+
+class LinkerHand(Node):
+ def __init__(self, name):
+ super().__init__(name)
+ # 声明参数（带默认值）
+ self.declare_parameter('hand_type', 'left')
+ self.declare_parameter('hand_joint', 'L6')
+ self.declare_parameter('is_touch', False)
+ self.declare_parameter('can', 'can0')
+ self.declare_parameter('modbus', "None")
+ # -1 keeps the model's original startup speed. Camera teleoperation can
+ # set this to a conservative value before the startup pose is sent.
+ self.declare_parameter('startup_speed', -1)
+ # Empty keeps the legacy absolute topics/startup pose. A prefix lets
+ # two same-side hands coexist without receiving each other's commands.
+ self.declare_parameter('topic_prefix', '')
+ self.declare_parameter('startup_pose', Parameter.Type.INTEGER_ARRAY)
+ # Hardware state reads are synchronous CAN transactions. Keeping them
+ # below the command rate prevents G20's five-frame reads from starving
+ # incoming position commands.
+ self.declare_parameter('state_poll_rate', 60.0)
+ self.declare_parameter('velocity_poll_rate', 60.0)
+
+ # ---- PID 控制器参数 ----
+ pid_desc = ParameterDescriptor(dynamic_typing=True)
+ self.declare_parameter('pid_enabled', True, pid_desc)
+ self.declare_parameter('pid_kp', 1.5, pid_desc)
+ self.declare_parameter('pid_ki', 0.0, pid_desc)
+ self.declare_parameter('pid_kd', 0.0, pid_desc)
+ self.declare_parameter('pid_max_boost', 40, pid_desc)
+ self.declare_parameter('pid_dead_zone', 3, pid_desc)
+ self.declare_parameter('pid_integral_max', 100.0, pid_desc)
+
+ # ros时间获取
+ self.stamp_clock = Clock()
+ # 获取参数值
+ self.hand_type = self.get_parameter('hand_type').value
+ self.hand_joint = self.get_parameter('hand_joint').value
+ self.is_touch = self.get_parameter('is_touch').value
+ self.can = self.get_parameter('can').value
+ self.modbus = self.get_parameter('modbus').value
+ self.startup_speed = int(self.get_parameter('startup_speed').value)
+ if self.startup_speed < -1 or self.startup_speed > 255:
+ raise ValueError('startup_speed must be -1 or in the range [0, 255]')
+ self.topic_prefix = self.normalize_topic_prefix(
+ self.get_parameter('topic_prefix').value
+ )
+ self.state_poll_rate = float(
+ self.get_parameter('state_poll_rate').value
+ )
+ if self.state_poll_rate <= 0.0:
+ raise ValueError('state_poll_rate must be greater than zero')
+ self.state_poll_period = 1.0 / self.state_poll_rate
+ self.last_state_poll_time = None
+ self.velocity_poll_rate = float(
+ self.get_parameter('velocity_poll_rate').value
+ )
+ if self.velocity_poll_rate <= 0.0:
+ raise ValueError('velocity_poll_rate must be greater than zero')
+ self.velocity_poll_period = 1.0 / self.velocity_poll_rate
+ self.last_velocity_poll_time = None
+ configured_startup_pose = self.get_parameter_or(
+ 'startup_pose',
+ Parameter('startup_pose', Parameter.Type.INTEGER_ARRAY, []),
+ ).value
+ self.startup_pose = [int(value) for value in configured_startup_pose]
+ if any(value < 0 or value > 255 for value in self.startup_pose):
+ raise ValueError('startup_pose values must be in the range [0, 255]')
+ self.sdk_v = 2
+ self.sleep_time = 0.005
+ self.cmd_lock = False
+ self.last_hand_post_cmd = None # 最新手指位置命令
+ self.last_hand_vel_cmd = None # 最新手指速度命令
+ self.last_hand_eff_cmd = None # 最新手指力矩命令
+ self.applied_hand_post_cmd = None
+ self.applied_hand_vel_cmd = None
+
+ # 延时测量
+ self._latency_seq: int = 0
+ self._latency_t4: float = 0.0
+
+ self.last_hand_state = [-1] * 10
+ self.last_hand_vel = [-1] * 10
+ self.force = [[-1] * 5] * 4
+ self.matrix_dic = {
+ "stamp":{
+ "sec": 0,
+ "nanosec": 0,
+ },
+ "thumb_matrix":[[-1] * 6 for _ in range(12)],
+ "index_matrix":[[-1] * 6 for _ in range(12)],
+ "middle_matrix":[[-1] * 6 for _ in range(12)],
+ "ring_matrix":[[-1] * 6 for _ in range(12)],
+ "little_matrix":[[-1] * 6 for _ in range(12)]
+ }
+ # 压感矩阵合值，单位g 克
+ self.matrix_mass_dic = {
+ "stamp":{
+ "secs": 0,
+ "nsecs": 0,
+ },
+ "thumb_mass":[-1],
+ "index_mass":[-1],
+ "middle_mass":[-1],
+ "ring_mass":[-1],
+ "little_mass":[-1]
+ }
+ self.last_hand_info = {
+ "version": [-1], # Dexterous hand version number
+ "hand_joint": self.hand_joint, # Dexterous hand joint type
+ "speed": [-1] * 10, # Current speed threshold of the dexterous hand
+ "current": [-1] * 10, # Current of the dexterous hand
+ "fault": [-1] * 10, # Current fault of the dexterous hand
+ "motor_temperature": [-1] * 10, # Current motor temperature of the dexterous hand
+ "torque": [-1] * 10, # Current torque of the dexterous hand
+ "is_touch":self.is_touch,
+ "touch_type": -1,
+ "finger_order": None # Finger motor order
+ }
+ self.version = []
+ self.touch_type = -1
+
+ # PID 控制器
+ self.pid = HandPIDController(
+ kp=self.get_parameter('pid_kp').value,
+ ki=self.get_parameter('pid_ki').value,
+ kd=self.get_parameter('pid_kd').value,
+ max_boost=self.get_parameter('pid_max_boost').value,
+ dead_zone=self.get_parameter('pid_dead_zone').value,
+ integral_max=self.get_parameter('pid_integral_max').value,
+ enabled=self.get_parameter('pid_enabled').value,
+ )
+
+ self.hz = 1.0/60.0
+
+ self.hand_setting_sub = self.create_subscription(
+ String, self.topic('/cb_hand_setting_cmd'), self.hand_setting_cb, 10
+ )
+ self._init_hand()
+ time.sleep(1)
+ self.run_count = 0 # 计数器，用于记录运行次数
+ self.timer = self.create_timer(0.01, self.run) # 100 Hz
+ self.thread_pub_state = threading.Thread(target=self.pub_state)
+ self.thread_pub_state.daemon = True
+ self.thread_pub_state.start()
+
+ @staticmethod
+ def normalize_topic_prefix(prefix):
+ prefix = str(prefix).strip()
+ if not prefix or prefix == '/':
+ return ''
+ if not prefix.startswith('/'):
+ prefix = '/' + prefix
+ return prefix.rstrip('/')
+
+ def topic(self, absolute_topic):
+ if not absolute_topic.startswith('/'):
+ raise ValueError('base topic must be absolute')
+ return self.topic_prefix + absolute_topic
+
+ def _init_hand(self):
+ self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+ time.sleep(0.1)
+ self.touch_type = self.api.get_touch_type()
+ self.hand_cmd_sub = self.create_subscription(
+ JointState,
+ self.topic(f'/cb_{self.hand_type}_hand_control_cmd'),
+ self.hand_control_cb,
+ COMMAND_QOS,
+ )
+ self.hand_state_pub = self.create_publisher(JointState, self.topic(f'/cb_{self.hand_type}_hand_state'),10)
+ self.hand_info_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_info'), 10)
+
+ # 延时测量发布器
+ self.latency_pub = self.create_publisher(Float64MultiArray, '/latency_stamp', 100)
+
+ if self.is_touch == True:
+ if self.modbus != "None":
+ self.matrix_touch_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch'), 10)
+ self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_pc'), 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_mass'), 10)
+ elif self.touch_type > 1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+ self.matrix_touch_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch'), 10)
+ self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_pc'), 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, self.topic(f'/cb_{self.hand_type}_hand_matrix_touch_mass'), 10)
+ elif self.touch_type != -1 and self.modbus == "None":
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+ self.touch_pub = self.create_publisher(Float32MultiArray, self.topic(f'/cb_{self.hand_type}_hand_force'), 10)
+ else:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+ self.is_touch = False
+
+ self.embedded_version = self.api.get_embedded_version()
+ pose = None
+ torque = [200, 200, 200, 200, 200]
+ speed = [200, 250, 250, 250, 250]
+ if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6" or self.hand_joint.upper() == "L6P":
+ pose = [200, 255, 255, 255, 255, 180]
+ torque = [250, 250, 250, 250, 250, 250]
+ # O6 最大速度阈值
+ speed = [200, 250, 250, 250, 250, 250]
+ elif self.hand_joint == "L7":
+ # The data length of L7 is 7, reinitialize here
+ pose = [255, 200, 255, 255, 255, 255, 180]
+ torque = [250, 250, 250, 250, 250, 250, 250]
+ speed = [120, 250, 250, 250, 250, 250, 250]
+ elif self.hand_joint == "L10":
+ torque = [255] * 10
+ pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
+ speed = [200, 250, 250, 250, 250, 250, 250, 250, 250, 250]
+ elif self.hand_joint == "L20":
+ pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255]
+ elif self.hand_joint == "G20":
+ # G20 uses a different CAN protocol from L20 even though both
+ # expose a 20-value ROS command. This is the calibrated startup
+ # pose also used by linker_hand_advanced_g20.
+ pose = [255,255,255,255,255,255,193,148,105,42,245,255,255,255,255,255,255,255,255,255]
+ torque = [255] * 5
+ speed = [255] * 5
+ elif self.hand_joint == "L21":
+ pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ elif self.hand_joint == "L25":
+ pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ if self.startup_pose:
+ if pose is None or len(self.startup_pose) != len(pose):
+ expected = 0 if pose is None else len(pose)
+ raise ValueError(
+ f'startup_pose for {self.hand_joint} must contain {expected} values'
+ )
+ pose = list(self.startup_pose)
+ if self.startup_speed >= 0:
+ speed = [self.startup_speed] * len(speed)
+ if pose is not None:
+ for i in range(1):
+ self.api.set_speed(speed=speed)
+ time.sleep(0.1)
+ self.api.set_torque(torque=torque)
+ time.sleep(0.1)
+ self.api.finger_move(pose=pose)
+ time.sleep(0.1)
+
+ def hand_control_cb(self, msg):
+ # 延时: 记录 SDK 收到时间 t4，从 frame_id 解析 seq
+ self._latency_t4 = time.perf_counter()
+ seq = 0
+ fid = msg.header.frame_id if msg.header.frame_id else ''
+ if fid.startswith("lat_"):
+ try:
+ seq = int(fid[4:])
+ except ValueError:
+ pass
+ self._latency_seq = seq
+
+ # The hardware can be slower than the camera. Always replace a
+ # pending command with the newest sample and never replay an already
+ # applied sample; this prevents latency from accumulating in software.
+ position = list(msg.position)
+ if position:
+ self.last_hand_post_cmd = (
+ position
+ if command_changed(self.applied_hand_post_cmd, position)
+ else None
+ )
+
+ velocity = list(msg.velocity)
+ if velocity:
+ self.last_hand_vel_cmd = (
+ velocity
+ if command_changed(self.applied_hand_vel_cmd, velocity)
+ else None
+ )
+
+ effort = list(msg.effort)
+ if effort:
+ self.last_hand_eff_cmd = effort
+
+ def _apply_pending_commands(self):
+ if self.cmd_lock:
+ return
+
+ if self.last_hand_post_cmd is not None:
+ pose = list(self.last_hand_post_cmd)
+
+ # ---- PID 补偿: 目标 vs 实际位置 ----
+ actual = self.last_hand_state # pub_state 线程缓存的最新实际位置
+ PID_pose = self.pid.apply(pose, actual)
+
+ print(f"[PID] target={pose}\n actual={actual}\n pidcmd={PID_pose}", flush=True)
+
+ self.api.finger_move(pose=PID_pose)
+ t5 = time.perf_counter() # ---- 延时: CAN 发送后 ----
+ self.applied_hand_post_cmd = PID_pose
+ self.last_hand_post_cmd = None
+
+ # 发布延时时间戳 (SDK 端: t4-t5)
+ latency_msg = Float64MultiArray()
+ latency_msg.data = [float(self._latency_seq), 1.0,
+ self._latency_t4, t5, 0.0, 0.0]
+ self.latency_pub.publish(latency_msg)
+
+ if self.last_hand_vel_cmd is not None:
+ vel = list(self.last_hand_vel_cmd)
+ if not all(x == 0 for x in vel):
+ if (str(self.hand_joint).upper() == "O6" or str(self.hand_joint).upper() == "L6" or str(self.hand_joint).upper() == "L6P") and len(vel) == 6:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L7" and len(vel) == 7:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L10" and len(vel) == 10:
+ speed = [vel[0],vel[2],vel[3],vel[4],vel[5]]
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L20" and len(vel) == 20:
+ speed = [vel[10],vel[1],vel[2],vel[3],vel[4]]
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "G20" and len(vel) == 20:
+ # G20 speed is configured per finger, not per one of
+ # the 20 exposed position slots.
+ speed = [vel[0],vel[1],vel[2],vel[3],vel[4]]
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L21" and len(vel) == 25:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L25" and len(vel) == 25:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ self.applied_hand_vel_cmd = vel
+ self.last_hand_vel_cmd = None
+
+ def _poll_state_if_due(self):
+ now = time.monotonic()
+ if not state_poll_due(
+ self.last_state_poll_time, now, self.state_poll_period
+ ):
+ return
+ # Record the start time so a slow G20 read cannot immediately trigger
+ # another read on the following timer callback.
+ self.last_state_poll_time = now
+ self.last_hand_state = self.api.get_state()
+ # print(f"[PID-DEBUG] last_hand_state={self.last_hand_state}", flush=True)
+ time.sleep(0.003)
+ if state_poll_due(
+ self.last_velocity_poll_time, now, self.velocity_poll_period
+ ):
+ self.last_velocity_poll_time = now
+ self.last_hand_vel = self.api.get_joint_speed()
+ time.sleep(0.002)
+
+ def run(self):
+ if self.sdk_v == 1:
+ self.sleep_time = 0.009
+ # Position commands have priority over synchronous state reads.
+ self._apply_pending_commands()
+ self._poll_state_if_due()
+ if self.cmd_lock == False:
+ time.sleep(0.003)
+ if self.run_count == 3 and self.is_touch == True and self.touch_type == 1 and self.modbus == "None" and self.touch_pub.get_subscription_count() > 0:
+ """单点式压力传感器"""
+ self.force = self.api.get_force()
+ if self.is_touch == True and (self.touch_type > 1 or self.modbus != "None") and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
+ """矩阵式压力传感器"""
+ if self.run_count == 3:
+ self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=self.sleep_time).tolist()
+ if self.run_count == 4:
+ self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=self.sleep_time).tolist()
+ if self.run_count == 5:
+ self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=self.sleep_time).tolist()
+ if self.run_count == 6:
+ self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=self.sleep_time).tolist()
+ if self.run_count == 7:
+ self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=self.sleep_time).tolist()
+ time.sleep(0.005)
+ if self.run_count == 8 and self.hand_info_pub.get_subscription_count() > 0:
+ """手部信息"""
+ self.last_hand_info = {
+ "version": self.embedded_version, # Dexterous hand version number
+ "hand_joint": self.hand_joint, # Dexterous hand joint type
+ "speed": self.api.get_speed(), # Current speed threshold of the dexterous hand
+ "current": self.api.get_current(), # Current of the dexterous hand
+ "fault": self.api.get_fault(), # Current fault of the dexterous hand
+ "motor_temperature": self.api.get_temperature(), # Current motor temperature of the dexterous hand
+ "torque": self.api.get_torque(), # Current torque of the dexterous hand
+ "is_touch":self.is_touch,
+ "touch_type": self.touch_type,
+ "finger_order": self.api.get_finger_order() # Finger motor order
+ }
+
+ if self.run_count == 9:
+ self.api.clear_faults() # 自动清除错误编码
+ self.run_count = 0
+ self.run_count += 1
+ time.sleep(0.003)
+
+
+ def pub_state(self):
+ while True:
+ if self.hand_state_pub.get_subscription_count() > 0:
+ msg = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+ self.hand_state_pub.publish(msg)
+ if self.is_touch == True and self.touch_type == 1 and self.modbus == "None" and self.touch_pub.get_subscription_count() > 0:
+ msg = Float32MultiArray()
+ msg.data = [float(val) for sublist in self.force for val in sublist]
+ self.touch_pub.publish(msg)
+ if self.is_touch == True and (self.touch_type > 1 or self.modbus != "None") and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
+ # 发布矩阵压感数据JSON格式
+ self.pub_matrix_dic()
+ # 发布矩阵压感和值JSON格式
+ self.pub_matrix_mass(dic=self.matrix_dic)
+ # 发布矩阵压感点云格式
+ self.pub_matrix_point_cloud()
+ if self.hand_info_pub.get_subscription_count() > 0:
+ msg = String()
+ msg.data = json.dumps(self.last_hand_info)
+ self.hand_info_pub.publish(msg)
+ time.sleep(self.hz)
+
+ def pub_matrix_mass(self, dic):
+ """发布矩阵数据合值 单位g 克 JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_mass_dic["stamp"]["secs"] = t_secs
+ self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+ self.matrix_mass_dic["unit"] = "g"
+ self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+ self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+ self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+ self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+ self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+ msg.data = json.dumps(self.matrix_mass_dic)
+ self.matrix_touch_mass_pub.publish(msg)
+
+ def pub_matrix_point_cloud(self):
+ """发布矩阵数据点云格式"""
+ tmp_dic = self.matrix_dic.copy()
+ del tmp_dic['stamp'] # 去掉时间戳字段
+ all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数
+ # 摊平到一维：360 个 float
+ flat_list = [v for frame in all_matrices for v in frame] # 360
+ flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+ fields = [PointField(
+ name='val',
+ offset=0,
+ datatype=PointField.UINT8,
+ count=1
+ )]
+ pc = PointCloud2()
+ pc.header.stamp = self.stamp_clock.now().to_msg()
+ pc.header.frame_id = ''
+ pc.height = 1
+ pc.width = flat.size # 360
+ pc.fields = fields
+ pc.is_bigendian = False
+ pc.point_step = 1 # 1 个 float32
+ pc.row_step = pc.point_step * pc.width
+ pc.data = flat.tobytes() # 1440 字节
+ self.matrix_touch_pub_pc.publish(pc)
+
+ def pub_matrix_dic(self):
+ """发布矩阵数据JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_dic["stamp"]["secs"] = t_secs
+ self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+ msg.data = json.dumps(self.matrix_dic)
+ self.matrix_touch_pub.publish(msg)
+
+ def joint_state_msg(self, pose,vel=[]):
+ joint_state = JointState()
+ joint_state.header = Header()
+ joint_state.header.stamp = self.get_clock().now().to_msg()
+ joint_state.name = self.api.get_finger_order()
+ joint_state.position = [float(x) for x in pose]
+ if len(vel) > 1:
+ joint_state.velocity = [float(x) for x in vel]
+ else:
+ joint_state.velocity = [0.0] * len(pose)
+ joint_state.effort = [0.0] * len(pose)
+ return joint_state
+
+
+
+
+
+ def hand_setting_cb(self,msg):
+ '''控制命令回调'''
+ data = json.loads(msg.data)
+ print(f"Received setting command: {data['setting_cmd']}",flush=True)
+ try:
+ if data["params"]["hand_type"] == "left":
+ hand = self.api
+ hand_left = True
+ elif data["params"]["hand_type"] == "right":
+ hand = self.api
+ hand_right = True
+ else:
+ print("Please specify the hand part to be set",flush=True)
+ return
+ self.cmd_lock = True
+ # Set maximum torque
+ if data["setting_cmd"] == "set_max_torque_limits": # Set maximum torque
+ torque = list(data["params"]["torque"])
+ hand.set_torque(torque=torque)
+
+ if data["setting_cmd"] == "set_speed": # Set speed
+ if isinstance(data["params"]["speed"], list) == True:
+ speed = data["params"]["speed"]
+ hand.set_speed(speed=speed)
+ else:
+ ColorMsg(msg=f"Speed parameter error, speed must be a list", color="red")
+ if data["setting_cmd"] == "clear_faults": # Clear faults
+ if hand_left == True and self.hand_joint == "L10" :
+ ColorMsg(msg=f"L10 left hand cannot clear faults")
+ elif hand_right == True and self.hand_joint == "L10" :
+ ColorMsg(msg=f"L10 right hand cannot clear faults")
+ else:
+ hand.clear_faults()
+ if data["setting_cmd"] == "get_faults": # Get faults
+ f = hand.get_fault()
+ ColorMsg(msg=f"Get faults: {f}")
+ if data["setting_cmd"] == "electric_current": # Get current
+ ColorMsg(msg=f"Get current: {hand.get_current()}")
+ if data["setting_cmd"] == "set_electric_current": # Set current
+ if isinstance(data["params"]["current"], list) == True:
+ hand.set_current(data["params"]["current"])
+ if data["setting_cmd"] == "show_fun_table": # Get faults
+ f = hand.show_fun_table()
+ except:
+ print("命令参数错误")
+ self.cmd_lock = False
+ finally:
+ self.cmd_lock = False
+
+
+ def close_can(self):
+ self.api.open_can.close_can(can=self.can)
+ sys.exit(0)
+
+
+def main(args=None):
+ try:
+ rclpy.init(args=args)
+ node = LinkerHand("linker_hand_sdk")
+ embedded_version = node.embedded_version or []
+ hand_joint = node.hand_joint.upper()
+ if len(embedded_version) == 3 or hand_joint in {"O6", "L6", "G20"}:
+ ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+ node.sdk_v = 2
+ elif len(embedded_version) == 6 and hand_joint == "L10":
+ ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+ node.sdk_v = 2
+ elif len(embedded_version) > 4 and ((embedded_version[0]==10 and embedded_version[4]>35) or (embedded_version[0]==7 and embedded_version[4]>50) or (embedded_version[0] == 6)):
+ ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+ node.sdk_v = 2
+ else:
+ ColorMsg(msg=f"SDK V1", color="green")
+ node.sdk_v = 1
+ rclpy.spin(node) # 主循环，监听 ROS 回调
+ except KeyboardInterrupt:
+ print("收到 Ctrl+C，准备退出...")
+ finally:
+ # node.close_can() # 关闭 CAN 或其他硬件资源
+ # node.destroy_node() # 销毁 ROS 节点
+ # rclpy.shutdown() # 关闭 ROS
+ print("程序已退出。")
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py.bak b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py.bak
new file mode 100644
index 0000000..81c31a9
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py.bak
@@ -0,0 +1,414 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+'''
+编译: colcon build --symlink-install
+启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
+'''
+from re import A
+import rclpy,sys # ROS2 Python接口库
+import time
+import numpy as np
+from rclpy.node import Node # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+
+class LinkerHand(Node):
+ def __init__(self, name):
+ super().__init__(name)
+ # 声明参数（带默认值）
+ self.declare_parameter('hand_type', 'left')
+ self.declare_parameter('hand_joint', 'L6')
+ self.declare_parameter('is_touch', False)
+ self.declare_parameter('can', 'can0')
+ self.declare_parameter('modbus', "None")
+
+ # ros时间获取
+ self.stamp_clock = Clock()
+ # 获取参数值
+ self.hand_type = self.get_parameter('hand_type').value
+ self.hand_joint = self.get_parameter('hand_joint').value
+ self.is_touch = self.get_parameter('is_touch').value
+ self.can = self.get_parameter('can').value
+ self.modbus = self.get_parameter('modbus').value
+ self.sdk_v = 2
+ self.sleep_time = 0.005
+ self.cmd_lock = False
+ self.last_hand_post_cmd = None # 最新手指位置命令
+ self.last_hand_vel_cmd = None # 最新手指速度命令
+ self.last_hand_eff_cmd = None # 最新手指力矩命令
+
+ self.last_hand_state = [-1] * 10
+ self.last_hand_vel = [-1] * 10
+ self.force = [[-1] * 5] * 4
+ self.matrix_dic = {
+ "stamp":{
+ "sec": 0,
+ "nanosec": 0,
+ },
+ "thumb_matrix":[[-1] * 6 for _ in range(12)],
+ "index_matrix":[[-1] * 6 for _ in range(12)],
+ "middle_matrix":[[-1] * 6 for _ in range(12)],
+ "ring_matrix":[[-1] * 6 for _ in range(12)],
+ "little_matrix":[[-1] * 6 for _ in range(12)]
+ }
+ # 压感矩阵合值，单位g 克
+ self.matrix_mass_dic = {
+ "stamp":{
+ "secs": 0,
+ "nsecs": 0,
+ },
+ "thumb_mass":[-1],
+ "index_mass":[-1],
+ "middle_mass":[-1],
+ "ring_mass":[-1],
+ "little_mass":[-1]
+ }
+ self.last_hand_info = {
+ "version": [-1], # Dexterous hand version number
+ "hand_joint": self.hand_joint, # Dexterous hand joint type
+ "speed": [-1] * 10, # Current speed threshold of the dexterous hand
+ "current": [-1] * 10, # Current of the dexterous hand
+ "fault": [-1] * 10, # Current fault of the dexterous hand
+ "motor_temperature": [-1] * 10, # Current motor temperature of the dexterous hand
+ "torque": [-1] * 10, # Current torque of the dexterous hand
+ "is_touch":self.is_touch,
+ "touch_type": -1,
+ "finger_order": None # Finger motor order
+ }
+ self.version = []
+ self.touch_type = -1
+ self.hz = 1.0/60.0
+
+ self.hand_setting_sub = self.create_subscription(String,'/cb_hand_setting_cmd', self.hand_setting_cb, 10)
+ self._init_hand()
+ time.sleep(1)
+ self.run_count = 0 # 计数器，用于记录运行次数
+ self.timer = self.create_timer(0.01, self.run) # 100 Hz
+ self.thread_pub_state = threading.Thread(target=self.pub_state)
+ self.thread_pub_state.daemon = True
+ self.thread_pub_state.start()
+
+ def _init_hand(self):
+ self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+ time.sleep(0.1)
+ self.touch_type = self.api.get_touch_type()
+ self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+ self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+ self.hand_info_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_info', 10)
+ if self.is_touch == True:
+ if self.touch_type > 1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+ self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+ self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+ elif self.touch_type != -1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+ self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+ else:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+ self.is_touch = False
+ self.embedded_version = self.api.get_embedded_version()
+ pose = None
+ torque = [200, 200, 200, 200, 200]
+ speed = [200, 250, 250, 250, 250]
+ if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6" or self.hand_joint.upper() == "L6P":
+ pose = [200, 255, 255, 255, 255, 180]
+ torque = [250, 250, 250, 250, 250, 250]
+ # O6 最大速度阈值
+ speed = [200, 250, 250, 250, 250, 250]
+ elif self.hand_joint == "L7":
+ # The data length of L7 is 7, reinitialize here
+ pose = [255, 200, 255, 255, 255, 255, 180]
+ torque = [250, 250, 250, 250, 250, 250, 250]
+ speed = [120, 250, 250, 250, 250, 250, 250]
+ elif self.hand_joint == "L10":
+ torque = [255] * 10
+ pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
+ speed = [200, 250, 250, 250, 250, 250, 250, 250, 250, 250]
+ elif self.hand_joint == "L20":
+ pose = [255,255,255,255,255,255,10,100,180,240,245,255,255,255,255,255,255,255,255,255]
+ elif self.hand_joint == "L21":
+ pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ elif self.hand_joint == "L25":
+ pose = [75, 255, 255, 255, 255, 176, 97, 81, 114, 147, 202, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ if pose is not None:
+ for i in range(1):
+ self.api.set_speed(speed=speed)
+ time.sleep(0.1)
+ self.api.set_torque(torque=torque)
+ time.sleep(0.1)
+ self.api.finger_move(pose=pose)
+ time.sleep(0.1)
+
+ def list_check(self,pose):
+ if isinstance(pose, list) == False:
+ return False
+ if len(self.last_hand_post_cmd) != len(pose):
+ return False
+ return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+
+ def hand_control_cb(self, msg):
+ if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+ self.last_hand_post_cmd = msg.position
+ if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+ self.last_hand_vel_cmd = msg.velocity
+ if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+ self.last_hand_eff_cmd = msg.effort
+
+ def run(self):
+ if self.sdk_v == 1:
+ self.sleep_time = 0.009
+ if self.hand_state_pub.get_subscription_count() > 0:
+ # 优先获取手指状态并且发布
+ self.last_hand_state = self.api.get_state()
+ time.sleep(0.003)
+ self.last_hand_vel = self.api.get_joint_speed()
+ time.sleep(0.002)
+ if self.cmd_lock == False:
+ if self.last_hand_post_cmd != None:
+ self.api.finger_move(pose=self.last_hand_post_cmd)
+ self.last_hand_post_cmd = None
+ if self.last_hand_vel_cmd != None:
+ vel = list(self.last_hand_vel_cmd)
+ if all(x == 0 for x in vel):
+ pass
+ else:
+ if (str(self.hand_joint).upper() == "O6" or str(self.hand_joint).upper() == "L6" or str(self.hand_joint).upper() == "L6P") and len(vel) == 6:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L7" and len(vel) == 7:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L10" and len(vel) == 10:
+ speed = [vel[0],vel[2],vel[3],vel[4],vel[5]]
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L20" and len(vel) == 20:
+ speed = [vel[10],vel[1],vel[2],vel[3],vel[4]]
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L21" and len(vel) == 25:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L25" and len(vel) == 25:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ self.last_hand_vel_cmd = None
+ time.sleep(0.003)
+ if self.run_count == 3 and self.is_touch == True and self.touch_type == 1 and self.touch_pub.get_subscription_count() > 0:
+ """单点式压力传感器"""
+ self.force = self.api.get_force()
+ if self.is_touch == True and self.touch_type > 1 and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
+ """矩阵式压力传感器"""
+ if self.run_count == 3:
+ self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=self.sleep_time).tolist()
+ if self.run_count == 4:
+ self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=self.sleep_time).tolist()
+ if self.run_count == 5:
+ self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=self.sleep_time).tolist()
+ if self.run_count == 6:
+ self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=self.sleep_time).tolist()
+ if self.run_count == 7:
+ self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=self.sleep_time).tolist()
+ time.sleep(0.005)
+ if self.run_count == 8 and self.hand_info_pub.get_subscription_count() > 0:
+ """手部信息"""
+ self.last_hand_info = {
+ "version": self.embedded_version, # Dexterous hand version number
+ "hand_joint": self.hand_joint, # Dexterous hand joint type
+ "speed": self.api.get_speed(), # Current speed threshold of the dexterous hand
+ "current": self.api.get_current(), # Current of the dexterous hand
+ "fault": self.api.get_fault(), # Current fault of the dexterous hand
+ "motor_temperature": self.api.get_temperature(), # Current motor temperature of the dexterous hand
+ "torque": self.api.get_torque(), # Current torque of the dexterous hand
+ "is_touch":self.is_touch,
+ "touch_type": self.touch_type,
+ "finger_order": self.api.get_finger_order() # Finger motor order
+ }
+ if self.run_count == 9:
+ self.run_count = 0
+ self.run_count += 1
+ time.sleep(0.003)
+
+
+ def pub_state(self):
+ while True:
+ if self.hand_state_pub.get_subscription_count() > 0:
+ msg = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+ self.hand_state_pub.publish(msg)
+ if self.is_touch == True and self.touch_type == 1 and self.touch_pub.get_subscription_count() > 0:
+ msg = Float32MultiArray()
+ msg.data = [float(val) for sublist in self.force for val in sublist]
+ self.touch_pub.publish(msg)
+ if self.is_touch == True and self.touch_type > 1 and (self.matrix_touch_pub.get_subscription_count() > 0 or self.matrix_touch_mass_pub.get_subscription_count() > 0 or self.matrix_touch_pub_pc.get_subscription_count() > 0):
+ # 发布矩阵压感数据JSON格式
+ self.pub_matrix_dic()
+ # 发布矩阵压感和值JSON格式
+ self.pub_matrix_mass(dic=self.matrix_dic)
+ # 发布矩阵压感点云格式
+ self.pub_matrix_point_cloud()
+ if self.hand_info_pub.get_subscription_count() > 0:
+ msg = String()
+ msg.data = json.dumps(self.last_hand_info)
+ self.hand_info_pub.publish(msg)
+ time.sleep(self.hz)
+
+ def pub_matrix_mass(self, dic):
+ """发布矩阵数据合值 单位g 克 JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_mass_dic["stamp"]["secs"] = t_secs
+ self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+ self.matrix_mass_dic["unit"] = "g"
+ self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+ self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+ self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+ self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+ self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+ msg.data = json.dumps(self.matrix_mass_dic)
+ self.matrix_touch_mass_pub.publish(msg)
+
+ def pub_matrix_point_cloud(self):
+ """发布矩阵数据点云格式"""
+ tmp_dic = self.matrix_dic.copy()
+ del tmp_dic['stamp'] # 去掉时间戳字段
+ all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数
+ # 摊平到一维：360 个 float
+ flat_list = [v for frame in all_matrices for v in frame] # 360
+ flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+ fields = [PointField(
+ name='val',
+ offset=0,
+ datatype=PointField.UINT8,
+ count=1
+ )]
+ pc = PointCloud2()
+ pc.header.stamp = self.stamp_clock.now().to_msg()
+ pc.header.frame_id = ''
+ pc.height = 1
+ pc.width = flat.size # 360
+ pc.fields = fields
+ pc.is_bigendian = False
+ pc.point_step = 1 # 1 个 float32
+ pc.row_step = pc.point_step * pc.width
+ pc.data = flat.tobytes() # 1440 字节
+ self.matrix_touch_pub_pc.publish(pc)
+
+ def pub_matrix_dic(self):
+ """发布矩阵数据JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_dic["stamp"]["secs"] = t_secs
+ self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+ msg.data = json.dumps(self.matrix_dic)
+ self.matrix_touch_pub.publish(msg)
+
+ def joint_state_msg(self, pose,vel=[]):
+ joint_state = JointState()
+ joint_state.header = Header()
+ joint_state.header.stamp = self.get_clock().now().to_msg()
+ joint_state.name = self.api.get_finger_order()
+ joint_state.position = [float(x) for x in pose]
+ if len(vel) > 1:
+ joint_state.velocity = [float(x) for x in vel]
+ else:
+ joint_state.velocity = [0.0] * len(pose)
+ joint_state.effort = [0.0] * len(pose)
+ return joint_state
+
+
+
+
+
+ def hand_setting_cb(self,msg):
+ '''控制命令回调'''
+ data = json.loads(msg.data)
+ print(f"Received setting command: {data['setting_cmd']}",flush=True)
+ try:
+ if data["params"]["hand_type"] == "left":
+ hand = self.api
+ hand_left = True
+ elif data["params"]["hand_type"] == "right":
+ hand = self.api
+ hand_right = True
+ else:
+ print("Please specify the hand part to be set",flush=True)
+ return
+ self.cmd_lock = True
+ # Set maximum torque
+ if data["setting_cmd"] == "set_max_torque_limits": # Set maximum torque
+ torque = list(data["params"]["torque"])
+ hand.set_torque(torque=torque)
+
+ if data["setting_cmd"] == "set_speed": # Set speed
+ if isinstance(data["params"]["speed"], list) == True:
+ speed = data["params"]["speed"]
+ hand.set_speed(speed=speed)
+ else:
+ ColorMsg(msg=f"Speed parameter error, speed must be a list", color="red")
+ if data["setting_cmd"] == "clear_faults": # Clear faults
+ if hand_left == True and self.hand_joint == "L10" :
+ ColorMsg(msg=f"L10 left hand cannot clear faults")
+ elif hand_right == True and self.hand_joint == "L10" :
+ ColorMsg(msg=f"L10 right hand cannot clear faults")
+ else:
+ hand.clear_faults()
+ if data["setting_cmd"] == "get_faults": # Get faults
+ f = hand.get_fault()
+ ColorMsg(msg=f"Get faults: {f}")
+ if data["setting_cmd"] == "electric_current": # Get current
+ ColorMsg(msg=f"Get current: {hand.get_current()}")
+ if data["setting_cmd"] == "set_electric_current": # Set current
+ if isinstance(data["params"]["current"], list) == True:
+ hand.set_current(data["params"]["current"])
+ if data["setting_cmd"] == "show_fun_table": # Get faults
+ f = hand.show_fun_table()
+ except:
+ print("命令参数错误")
+ self.cmd_lock = False
+ finally:
+ self.cmd_lock = False
+
+
+ def close_can(self):
+ self.api.open_can.close_can(can=self.can)
+ sys.exit(0)
+
+
+def main(args=None):
+ try:
+ rclpy.init(args=args)
+ node = LinkerHand("linker_hand_sdk")
+ embedded_version = node.embedded_version
+ if len(embedded_version) == 3 or node.hand_joint.upper() == "O6" or node.hand_joint.upper() == "L6" or node.hand_joint.upper() == "G20":
+ ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+ node.sdk_v = 2
+ elif len(embedded_version) == 6 and node.hand_joint == "L10":
+ ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+ node.sdk_v = 2
+ elif len(embedded_version) > 4 and ((embedded_version[0]==10 and embedded_version[4]>35) or (embedded_version[0]==7 and embedded_version[4]>50) or (embedded_version[0] == 6)):
+ ColorMsg(msg=f"New Matrix Touch For SDK V2", color="green")
+ node.sdk_v = 2
+ else:
+ ColorMsg(msg=f"SDK V1", color="green")
+ node.sdk_v = 1
+ rclpy.spin(node) # 主循环，监听 ROS 回调
+ except KeyboardInterrupt:
+ print("收到 Ctrl+C，准备退出...")
+ finally:
+ # node.close_can() # 关闭 CAN 或其他硬件资源
+ # node.destroy_node() # 销毁 ROS 节点
+ # rclpy.shutdown() # 关闭 ROS
+ print("程序已退出。")
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_g20.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_g20.py
new file mode 100644
index 0000000..2bf1f83
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_g20.py
@@ -0,0 +1,258 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+from re import A
+import rclpy,sys # ROS2 Python接口库
+import time
+import argparse
+import numpy as np
+from rclpy.node import Node # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+# Linker Hand 型号
+HAND_JOINT = "G20"
+# 默认手指关节位置
+DEFAULT_POSITION = [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+# 默认手指关节速度
+DEFAULT_SPEED=[255, 255, 255, 255, 255]
+# 默认手指关节力矩
+DEFAULT_TORQUE = [255, 255, 255, 255, 255]
+# 压感传感器延迟时间
+TOUCH_SLEEP_TIME = 0.003
+
+
+class LinkerHandAdvancedG20(Node):
+ def __init__(self, name, hand_type, can, is_touch):
+ super().__init__(name)
+ self.hand_type = hand_type
+ self.hand_joint = HAND_JOINT
+ if is_touch == "true":
+ self.is_touch = True
+ else:
+ self.is_touch = False
+ self.can = can
+ self.modbus = "None"
+ time.sleep(0.1)
+ self._check_linker_hand_type()
+ self.last_hand_post_cmd = None # 最新手指位置命令
+ self.last_hand_vel_cmd = None # 最新手指速度命令
+ self.last_hand_eff_cmd = None # 最新手指力矩命令
+ self.matrix_dic = {
+ "stamp":{
+ "sec": 0,
+ "nanosec": 0,
+ },
+ "thumb_matrix":[[-1] * 6 for _ in range(12)],
+ "index_matrix":[[-1] * 6 for _ in range(12)],
+ "middle_matrix":[[-1] * 6 for _ in range(12)],
+ "ring_matrix":[[-1] * 6 for _ in range(12)],
+ "little_matrix":[[-1] * 6 for _ in range(12)]
+ }
+ # 压感矩阵合值，单位g 克
+ self.matrix_mass_dic = {
+ "stamp":{
+ "secs": 0,
+ "nsecs": 0,
+ },
+ "thumb_mass":[-1],
+ "index_mass":[-1],
+ "middle_mass":[-1],
+ "ring_mass":[-1],
+ "little_mass":[-1]
+ }
+ self.hz = 1.0/60.0
+ # ros时间获取
+ self.stamp_clock = Clock()
+ self._init_hand()
+ time.sleep(2)
+ self.count = 0
+ self.timer = self.create_timer(self.hz, self.run) # 100 Hz
+
+
+ def _check_linker_hand_type(self):
+ if self.modbus != "None":
+ ColorMsg(msg=f"Modbus暂不支持", color="red")
+ sys.exit(0)
+ if self.hand_joint.upper() != "G20":
+ ColorMsg(msg=f"Linker Hand hand_joint参数错误", color="red")
+ sys.exit(0)
+
+ def _init_hand(self):
+ self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+ time.sleep(0.1)
+ self.touch_type = self.api.get_touch_type()
+ self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+ self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+ if self.is_touch == True:
+ if self.touch_type > 1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+ self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+ self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+ elif self.touch_type != -1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+ self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+ else:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+ self.is_touch = False
+ self.embedded_version = self.api.get_embedded_version()
+ self.api.set_speed(speed=DEFAULT_SPEED)
+ time.sleep(0.1)
+ self.api.set_torque(torque=DEFAULT_TORQUE)
+ time.sleep(0.1)
+ self.api.finger_move(pose=DEFAULT_POSITION)
+ time.sleep(0.1)
+
+ def hand_control_cb(self, msg):
+ if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+ self.last_hand_post_cmd = msg.position
+ if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+ self.last_hand_vel_cmd = msg.velocity
+ if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+ self.last_hand_eff_cmd = msg.effort
+
+ def list_check(self,pose):
+ if isinstance(pose, list) == False:
+ return False
+ if len(self.last_hand_post_cmd) != len(pose):
+ return False
+ return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+
+ def joint_state_msg(self, pose,vel=[]):
+ joint_state = JointState()
+ joint_state.header = Header()
+ joint_state.header.stamp = self.get_clock().now().to_msg()
+ joint_state.name = self.api.get_finger_order()
+ joint_state.position = [float(x) for x in pose]
+ if len(vel) > 1:
+ joint_state.velocity = [float(x) for x in vel]
+ else:
+ joint_state.velocity = [0.0] * len(pose)
+ joint_state.effort = [0.0] * len(pose)
+ return joint_state
+
+ def run(self):
+ # 执行手控制指令
+ if self.last_hand_post_cmd != None:
+ self.api.finger_move(pose=self.last_hand_post_cmd)
+ self.last_hand_post_cmd = None
+ # 优先获取手指状态并且发布
+ self.last_hand_state = self.api.get_state()
+ self.last_hand_vel = [0.0] * len(self.last_hand_state)
+ # 发布手状态
+ msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+ self.hand_state_pub.publish(msg_state)
+
+ if self.is_touch == True:
+ # 获取压感数据
+ if self.count == 2:
+ self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist()
+ if self.count == 4:
+ self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist()
+ if self.count == 6:
+ self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist()
+ if self.count == 8:
+ self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist()
+ if self.count == 10:
+ self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=TOUCH_SLEEP_TIME).tolist()
+ # 发布矩阵压感数据JSON格式
+ self.pub_matrix_dic()
+ # 发布矩阵压感和值JSON格式
+ self.pub_matrix_mass(dic=self.matrix_dic)
+ # 发布矩阵压感点云格式
+ self.pub_matrix_point_cloud()
+ self.count += 1
+ if self.count == 11:
+ self.count = 0
+
+ def pub_matrix_dic(self):
+ """发布矩阵数据JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_dic["stamp"]["secs"] = t_secs
+ self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+ msg.data = json.dumps(self.matrix_dic)
+ self.matrix_touch_pub.publish(msg)
+
+ def pub_matrix_mass(self, dic):
+ """发布矩阵数据合值 单位g 克 JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_mass_dic["stamp"]["secs"] = t_secs
+ self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+ self.matrix_mass_dic["unit"] = "g"
+ self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+ self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+ self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+ self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+ self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+ msg.data = json.dumps(self.matrix_mass_dic)
+ self.matrix_touch_mass_pub.publish(msg)
+
+ def pub_matrix_point_cloud(self):
+ tmp_dic = self.matrix_dic.copy()
+ del tmp_dic['stamp'] # 去掉时间戳字段
+ all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数 or 5 帧，每帧 4×10=40 个数 列x行
+ # 摊平到一维
+ flat_list = [v for frame in all_matrices for v in frame]
+ flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+ fields = [PointField(name='val', offset=0, datatype=PointField.UINT8, count=1)]
+ pc = PointCloud2()
+ pc.header.stamp = self.get_clock().now().to_msg()
+ pc.header.frame_id = '' # 可改成你需要的坐标系
+ pc.height = 1
+ pc.width = flat.size # 360
+ pc.fields = fields
+ pc.is_bigendian = False
+ pc.point_step = 1 # 1 个 float32
+ pc.row_step = pc.point_step * pc.width
+ pc.data = flat.tobytes() # 1440 字节
+ self.matrix_touch_pub_pc.publish(pc)
+
+
+ def close_can(self):
+ self.api.open_can.close_can(can=self.can)
+ sys.exit(0)
+
+
+def main(args=None):
+ '''
+ 本节点用于收集手指状态和压感数据。
+ '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+ /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz
+ '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz
+ 启动命令:
+ ros2 run linker_hand_ros2_sdk linker_hand_advanced_g20 --hand_type left --can can0 --is_touch true
+ '''
+ try:
+ rclpy.init(args=args)
+ parser = argparse.ArgumentParser()
+ parser.add_argument('--hand_type', required=True)
+ parser.add_argument('--can', required=True)
+ parser.add_argument('--is_touch', choices=['true','false'], required=True)
+
+ args = parser.parse_args()
+ node = LinkerHandAdvancedG20(name="linker_hand_advanced_g20",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch)
+ embedded_version = node.embedded_version
+ rclpy.spin(node) # 主循环，监听 ROS 回调
+ except KeyboardInterrupt:
+ print("收到 Ctrl+C，准备退出...")
+ finally:
+ # node.close_can() # 关闭 CAN 或其他硬件资源
+ # node.destroy_node() # 销毁 ROS 节点
+ # rclpy.shutdown() # 关闭 ROS
+ print("程序已退出。")
\ No newline at end of file
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l10.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l10.py
new file mode 100644
index 0000000..8237ac7
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l10.py
@@ -0,0 +1,273 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+from re import A
+import rclpy,sys # ROS2 Python接口库
+import time
+import argparse
+import numpy as np
+from rclpy.node import Node # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray, Float64MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+class LinkerHandAdvancedL10(Node):
+ def __init__(self, name, hand_type, can, is_touch):
+ super().__init__(name)
+ self.hand_type = hand_type
+ self.hand_joint = "L10"
+ if is_touch == "true":
+ self.is_touch = True
+ else:
+ self.is_touch = False
+ self.can = can
+ self.modbus = "None"
+ time.sleep(0.1)
+ self._check_linker_hand_type()
+ self.last_hand_post_cmd = None # 最新手指位置命令
+ self.last_hand_vel_cmd = None # 最新手指速度命令
+ self.last_hand_eff_cmd = None # 最新手指力矩命令
+ self.count = 0 # 循环计数器
+
+ # 延时测量
+ self._latency_seq: int = 0
+ self._latency_t4: float = 0.0
+
+ self.matrix_dic = {
+ "stamp":{
+ "sec": 0,
+ "nanosec": 0,
+ },
+ "thumb_matrix":[[-1] * 6 for _ in range(12)],
+ "index_matrix":[[-1] * 6 for _ in range(12)],
+ "middle_matrix":[[-1] * 6 for _ in range(12)],
+ "ring_matrix":[[-1] * 6 for _ in range(12)],
+ "little_matrix":[[-1] * 6 for _ in range(12)]
+ }
+ # 压感矩阵合值，单位g 克
+ self.matrix_mass_dic = {
+ "stamp":{
+ "secs": 0,
+ "nsecs": 0,
+ },
+ "thumb_mass":[-1],
+ "index_mass":[-1],
+ "middle_mass":[-1],
+ "ring_mass":[-1],
+ "little_mass":[-1]
+ }
+ self.hz = 1.0/60.0
+ # ros时间获取
+ self.stamp_clock = Clock()
+ self._init_hand()
+ time.sleep(2)
+ self.timer = self.create_timer(self.hz, self.run) # 100 Hz
+
+
+ def _check_linker_hand_type(self):
+ if self.modbus != "None":
+ ColorMsg(msg=f"Modbus暂不支持", color="red")
+ sys.exit(0)
+ if self.hand_joint.upper() != "L10":
+ ColorMsg(msg=f"L10以外其他Linker Hand暂不支持", color="red")
+ sys.exit(0)
+
+
+ def _init_hand(self):
+ self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+ time.sleep(0.1)
+ self.touch_type = self.api.get_touch_type()
+ self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+ self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+
+ # 延时测量发布器
+ self.latency_pub = self.create_publisher(Float64MultiArray, '/latency_stamp', 100)
+
+ if self.is_touch == True:
+ if self.touch_type > 1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+ self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+ self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+ elif self.touch_type != -1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+ self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+ else:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+ self.is_touch = False
+ self.embedded_version = self.api.get_embedded_version()
+ if self.hand_joint.upper() == "L10":
+ pose = [255, 200, 255, 255, 255, 255, 180, 180, 180, 41]
+ torque = [255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ speed = [255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+ self.api.set_speed(speed=speed)
+ time.sleep(0.1)
+ self.api.set_torque(torque=torque)
+ time.sleep(0.1)
+ self.api.finger_move(pose=pose)
+ time.sleep(0.1)
+ self.serial_number = self.api.get_serial_number()
+
+ def hand_control_cb(self, msg):
+ # 延时: 记录 SDK 收到时间 t4，从 frame_id 解析 seq
+ t4 = time.perf_counter()
+ seq = 0
+ if msg.header.frame_id.startswith("lat_"):
+ try:
+ seq = int(msg.header.frame_id[4:])
+ except ValueError:
+ pass
+ self._latency_seq = seq
+ self._latency_t4 = t4
+
+ if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+ self.last_hand_post_cmd = msg.position
+ if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+ self.last_hand_vel_cmd = msg.velocity
+ if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+ self.last_hand_eff_cmd = msg.effort
+
+ def list_check(self,pose):
+ if isinstance(pose, list) == False:
+ return False
+ if len(self.last_hand_post_cmd) != len(pose):
+ return False
+ return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+
+ def joint_state_msg(self, pose,vel=[]):
+ joint_state = JointState()
+ joint_state.header = Header()
+ joint_state.header.stamp = self.get_clock().now().to_msg()
+ joint_state.name = self.api.get_finger_order()
+ joint_state.position = [float(x) for x in pose]
+ if len(vel) > 1:
+ joint_state.velocity = [float(x) for x in vel]
+ else:
+ joint_state.velocity = [0.0] * len(pose)
+ joint_state.effort = [0.0] * len(pose)
+ return joint_state
+
+ def run(self):
+ # 执行手控制指令
+ if self.last_hand_post_cmd != None:
+ self.api.finger_move(pose=self.last_hand_post_cmd)
+ t5 = time.perf_counter() # ---- 延时: CAN 发送后 ----
+ self.last_hand_post_cmd = None
+
+ # 发布延时时间戳 (SDK 端: t4-t5)
+ latency_msg = Float64MultiArray()
+ latency_msg.data = [float(self._latency_seq), 1.0,
+ self._latency_t4, t5, 0.0, 0.0]
+ self.latency_pub.publish(latency_msg)
+ # 优先获取手指状态并且发布
+ self.last_hand_state = self.api.get_state()
+ self.last_hand_vel = [0.0] * len(self.last_hand_state)
+ # 发布手状态
+ msg_state = self.joint_state_msg(self.last_hand_state)
+ self.hand_state_pub.publish(msg_state)
+ # 获取压感数据
+ if self.is_touch == True:
+ self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=0.003).tolist()
+ self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=0.004).tolist()
+ self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=0.003).tolist()
+ self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=0.004).tolist()
+ self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.004).tolist()
+ # 发布矩阵压感数据JSON格式
+ self.pub_matrix_dic()
+ # 发布矩阵压感和值JSON格式
+ self.pub_matrix_mass(dic=self.matrix_dic)
+ # 发布矩阵压感点云格式
+ self.pub_matrix_point_cloud()
+
+
+ def pub_matrix_dic(self):
+ """发布矩阵数据JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_dic["stamp"]["secs"] = t_secs
+ self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+ msg.data = json.dumps(self.matrix_dic)
+ self.matrix_touch_pub.publish(msg)
+
+ def pub_matrix_mass(self, dic):
+ """发布矩阵数据合值 单位g 克 JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_mass_dic["stamp"]["secs"] = t_secs
+ self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+ self.matrix_mass_dic["unit"] = "g"
+ self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+ self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+ self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+ self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+ self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+ msg.data = json.dumps(self.matrix_mass_dic)
+ self.matrix_touch_mass_pub.publish(msg)
+
+ def pub_matrix_point_cloud(self):
+ tmp_dic = self.matrix_dic.copy()
+ del tmp_dic['stamp'] # 去掉时间戳字段
+ all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数 or 5 帧，每帧 4×10=40 个数 列x行
+ # 摊平到一维
+ flat_list = [v for frame in all_matrices for v in frame]
+ flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+ fields = [PointField(name='val', offset=0, datatype=PointField.UINT8, count=1)]
+ pc = PointCloud2()
+ pc.header.stamp = self.get_clock().now().to_msg()
+ pc.header.frame_id = '' # 可改成你需要的坐标系
+ pc.height = 1
+ pc.width = flat.size # 360
+ pc.fields = fields
+ pc.is_bigendian = False
+ pc.point_step = 1 # 1 个 float32
+ pc.row_step = pc.point_step * pc.width
+ pc.data = flat.tobytes() # 1440 字节
+ self.matrix_touch_pub_pc.publish(pc)
+
+
+ def close_can(self):
+ self.api.open_can.close_can(can=self.can)
+ sys.exit(0)
+
+
+def main(args=None):
+ '''
+ 本节点用于收集手指状态和压感数据。
+ '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+ /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz
+ '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz
+ '/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+ 启动命令:
+ ros2 run linker_hand_ros2_sdk linker_hand_advanced_l10 --hand_type left --can can0 --is_touch true
+ '''
+ try:
+ rclpy.init(args=args)
+ parser = argparse.ArgumentParser()
+ parser.add_argument('--hand_type', required=True)
+ parser.add_argument('--can', required=True)
+ parser.add_argument('--is_touch', choices=['true','false'], required=True)
+
+ args = parser.parse_args()
+ node = LinkerHandAdvancedL10(name="linker_hand_advanced_l10",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch)
+ embedded_version = node.embedded_version
+ rclpy.spin(node) # 主循环，监听 ROS 回调
+ except KeyboardInterrupt:
+ print("收到 Ctrl+C，准备退出...")
+ finally:
+ # node.close_can() # 关闭 CAN 或其他硬件资源
+ # node.destroy_node() # 销毁 ROS 节点
+ # rclpy.shutdown() # 关闭 ROS
+ print("程序已退出。")
\ No newline at end of file
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l6.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l6.py
new file mode 100644
index 0000000..2672283
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l6.py
@@ -0,0 +1,251 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+'''
+编译: colcon build --symlink-install
+启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
+'''
+from re import A
+import rclpy,sys # ROS2 Python接口库
+import time
+import numpy as np
+import argparse
+from rclpy.node import Node # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+class LinkerHandAdvancedL6(Node):
+ def __init__(self, name, hand_type, can, is_touch):
+ super().__init__(name)
+ self.hand_type = hand_type
+ self.hand_joint = "L6"
+ if is_touch == "true":
+ self.is_touch = True
+ else:
+ self.is_touch = False
+ self.can = can
+ self.modbus = "None"
+ time.sleep(0.1)
+ self._check_linker_hand_type()
+ self.last_hand_post_cmd = None # 最新手指位置命令
+ self.last_hand_vel_cmd = None # 最新手指速度命令
+ self.last_hand_eff_cmd = None # 最新手指力矩命令
+ self.count = 0 # 循环计数器
+ self.matrix_dic = {
+ "stamp":{
+ "sec": 0,
+ "nanosec": 0,
+ },
+ "thumb_matrix":[[-1] * 6 for _ in range(12)],
+ "index_matrix":[[-1] * 6 for _ in range(12)],
+ "middle_matrix":[[-1] * 6 for _ in range(12)],
+ "ring_matrix":[[-1] * 6 for _ in range(12)],
+ "little_matrix":[[-1] * 6 for _ in range(12)]
+ }
+ # 压感矩阵合值，单位g 克
+ self.matrix_mass_dic = {
+ "stamp":{
+ "secs": 0,
+ "nsecs": 0,
+ },
+ "thumb_mass":[-1],
+ "index_mass":[-1],
+ "middle_mass":[-1],
+ "ring_mass":[-1],
+ "little_mass":[-1]
+ }
+ self.hz = 1.0/60.0
+ # ros时间获取
+ self.stamp_clock = Clock()
+ self._init_hand()
+ time.sleep(2)
+ self.timer = self.create_timer(self.hz, self.run) # 60 Hz
+
+
+ def _check_linker_hand_type(self):
+ if self.modbus != "None":
+ ColorMsg(msg=f"Modbus暂不支持", color="red")
+ sys.exit(0)
+ if self.hand_joint.upper() != "L6":
+ ColorMsg(msg=f"L6以外其他Linker Hand暂不支持", color="red")
+ sys.exit(0)
+
+ def _init_hand(self):
+ self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+ time.sleep(0.1)
+ self.touch_type = self.api.get_touch_type()
+ self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+ self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+ if self.is_touch == True:
+ if self.touch_type > 1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+ self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+ self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+ elif self.touch_type != -1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+ self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+ else:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+ self.is_touch = False
+ self.embedded_version = self.api.get_embedded_version()
+ if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6":
+ pose = [200, 255, 255, 255, 255, 180]
+ torque = [255, 255, 255, 255, 255, 255]
+ # O6 最大速度阈值
+ speed = [255, 255, 255, 255, 255, 255]
+ self.api.set_speed(speed=speed)
+ time.sleep(0.1)
+ self.api.set_torque(torque=torque)
+ time.sleep(0.1)
+ self.api.finger_move(pose=pose)
+ time.sleep(0.1)
+
+ def hand_control_cb(self, msg):
+ if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+ self.last_hand_post_cmd = msg.position
+ if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+ self.last_hand_vel_cmd = msg.velocity
+ if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+ self.last_hand_eff_cmd = msg.effort
+
+ def list_check(self,pose):
+ if isinstance(pose, list) == False:
+ return False
+ if len(self.last_hand_post_cmd) != len(pose):
+ return False
+ return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+
+ def joint_state_msg(self, pose,vel=[]):
+ joint_state = JointState()
+ joint_state.header = Header()
+ joint_state.header.stamp = self.get_clock().now().to_msg()
+ joint_state.name = self.api.get_finger_order()
+ joint_state.position = [float(x) for x in pose]
+ if len(vel) > 1:
+ joint_state.velocity = [float(x) for x in vel]
+ else:
+ joint_state.velocity = [0.0] * len(pose)
+ joint_state.effort = [0.0] * len(pose)
+ return joint_state
+
+ def run(self):
+ # 执行手控制指令
+ if self.last_hand_post_cmd != None:
+ self.api.finger_move(pose=self.last_hand_post_cmd)
+ self.last_hand_post_cmd = None
+ #time.sleep(0.002)
+ # 优先获取手指状态并且发布
+ self.last_hand_state = self.api.get_state()
+ self.last_hand_vel = self.api.get_joint_speed()
+ # 发布手状态
+ msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+ self.hand_state_pub.publish(msg_state)
+ time.sleep(0.002)
+ # 获取压感数据
+ if self.is_touch == True:
+ self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=0.003).tolist()
+ self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=0.003).tolist()
+ self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=0.003).tolist()
+ self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=0.003).tolist()
+ self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.003).tolist()
+ # 发布矩阵压感数据JSON格式
+ self.pub_matrix_dic()
+ # 发布矩阵压感和值JSON格式
+ self.pub_matrix_mass(dic=self.matrix_dic)
+ # 发布矩阵压感点云格式
+ self.pub_matrix_point_cloud()
+
+ def pub_matrix_dic(self):
+ """发布矩阵数据JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_dic["stamp"]["secs"] = t_secs
+ self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+ msg.data = json.dumps(self.matrix_dic)
+ self.matrix_touch_pub.publish(msg)
+
+ def pub_matrix_mass(self, dic):
+ """发布矩阵数据合值 单位g 克 JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_mass_dic["stamp"]["secs"] = t_secs
+ self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+ self.matrix_mass_dic["unit"] = "g"
+ self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+ self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+ self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+ self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+ self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+ msg.data = json.dumps(self.matrix_mass_dic)
+ self.matrix_touch_mass_pub.publish(msg)
+
+ def pub_matrix_point_cloud(self):
+ tmp_dic = self.matrix_dic.copy()
+ del tmp_dic['stamp'] # 去掉时间戳字段
+ all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数 or 5 帧，每帧 4×10=40 个数 列x行
+ # 摊平到一维
+ flat_list = [v for frame in all_matrices for v in frame]
+ flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+ fields = [PointField(name='val', offset=0, datatype=PointField.UINT8, count=1)]
+ pc = PointCloud2()
+ pc.header.stamp = self.get_clock().now().to_msg()
+ pc.header.frame_id = '' # 可改成你需要的坐标系
+ pc.height = 1
+ pc.width = flat.size # 360
+ pc.fields = fields
+ pc.is_bigendian = False
+ pc.point_step = 1 # 1 个 float32
+ pc.row_step = pc.point_step * pc.width
+ pc.data = flat.tobytes() # 1440 字节
+ self.matrix_touch_pub_pc.publish(pc)
+
+
+
+ def close_can(self):
+ self.api.open_can.close_can(can=self.can)
+ sys.exit(0)
+
+
+def main(args=None):
+ '''
+ 本节点用于收集手指状态和压感数据。
+ '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+ /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 40Hz
+ '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 40Hz
+ 启动命令:
+ ros2 run linker_hand_ros2_sdk linker_hand_advanced_l6 --hand_type left --can can0 --is_touch true
+ '''
+ try:
+ rclpy.init(args=args)
+ parser = argparse.ArgumentParser()
+ parser.add_argument('--hand_type', required=True)
+ parser.add_argument('--can', required=True)
+ parser.add_argument('--is_touch', choices=['true','false'], required=True)
+
+ args = parser.parse_args()
+ node = LinkerHandAdvancedL6(name="linker_hand_advanced_l6",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch)
+ embedded_version = node.embedded_version
+ if embedded_version[2] < 8 and len(embedded_version) != 3:
+ ColorMsg(msg=f"固件版本过低，请升级固件到V{embedded_version[0]}.{embedded_version[1]}.8及以上版本", color="red")
+ sys.exit(0)
+ rclpy.spin(node) # 主循环，监听 ROS 回调
+ except KeyboardInterrupt:
+ print("收到 Ctrl+C，准备退出...")
+ finally:
+ # node.close_can() # 关闭 CAN 或其他硬件资源
+ # node.destroy_node() # 销毁 ROS 节点
+ # rclpy.shutdown() # 关闭 ROS
+ print("程序已退出。")
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l7.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l7.py
new file mode 100644
index 0000000..7ed58bd
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_l7.py
@@ -0,0 +1,281 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+'''
+编译: colcon build --symlink-install
+启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
+'''
+from re import A
+import rclpy,sys # ROS2 Python接口库
+import time
+import argparse
+import numpy as np
+from rclpy.node import Node # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+class LinkerHandAdvancedL7(Node):
+ def __init__(self, name, hand_type, can, is_touch):
+ super().__init__(name)
+ self.hand_type = hand_type
+ self.hand_joint = "L7"
+ if is_touch == "true":
+ self.is_touch = True
+ else:
+ self.is_touch = False
+ self.can = can
+ self.modbus = "None"
+ time.sleep(0.1)
+ self._check_linker_hand_type()
+ self.last_hand_post_cmd = None # 最新手指位置命令
+ self.last_hand_vel_cmd = None # 最新手指速度命令
+ self.last_hand_eff_cmd = None # 最新手指力矩命令
+ self.count = 0 # 循环计数器
+ self.matrix_dic = {
+ "stamp":{
+ "sec": 0,
+ "nanosec": 0,
+ },
+ "thumb_matrix":[[-1] * 6 for _ in range(12)],
+ "index_matrix":[[-1] * 6 for _ in range(12)],
+ "middle_matrix":[[-1] * 6 for _ in range(12)],
+ "ring_matrix":[[-1] * 6 for _ in range(12)],
+ "little_matrix":[[-1] * 6 for _ in range(12)]
+ }
+ # 压感矩阵合值，单位g 克
+ self.matrix_mass_dic = {
+ "stamp":{
+ "secs": 0,
+ "nsecs": 0,
+ },
+ "thumb_mass":[-1],
+ "index_mass":[-1],
+ "middle_mass":[-1],
+ "ring_mass":[-1],
+ "little_mass":[-1]
+ }
+ self.hz = 1.0/60.0
+ # ros时间获取
+ self.stamp_clock = Clock()
+ self._init_hand()
+ time.sleep(2)
+ self.timer = self.create_timer(self.hz, self.run) # 100 Hz
+
+
+ def _check_linker_hand_type(self):
+ if self.modbus != "None":
+ ColorMsg(msg=f"Modbus暂不支持", color="red")
+ sys.exit(0)
+ if self.hand_joint.upper() != "L7":
+ ColorMsg(msg=f"L6以外其他Linker Hand暂不支持", color="red")
+ sys.exit(0)
+
+
+ def _init_hand(self):
+ self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+ time.sleep(0.1)
+ self.touch_type = self.api.get_touch_type()
+ self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+ self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+ if self.is_touch == True:
+ if self.touch_type > 1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+ self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+ self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+ elif self.touch_type != -1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+ self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+ else:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+ self.is_touch = False
+ self.embedded_version = self.api.get_embedded_version()
+ if self.hand_joint.upper() == "L7":
+ pose = [255, 200, 255, 255, 255, 255, 180]
+ torque = [255] * 7
+ speed = [255] * 7
+ self.api.set_speed(speed=speed)
+ time.sleep(0.1)
+ self.api.set_torque(torque=torque)
+ time.sleep(0.1)
+ self.api.finger_move(pose=pose)
+ time.sleep(0.1)
+ self.serial_number = self.api.get_serial_number()
+
+ def hand_control_cb(self, msg):
+ if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+ self.last_hand_post_cmd = msg.position
+ if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+ self.last_hand_vel_cmd = msg.velocity
+ if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+ self.last_hand_eff_cmd = msg.effort
+
+ def list_check(self,pose):
+ if isinstance(pose, list) == False:
+ return False
+ if len(self.last_hand_post_cmd) != len(pose):
+ return False
+ return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+
+ def joint_state_msg(self, pose,vel=[]):
+ joint_state = JointState()
+ joint_state.header = Header()
+ joint_state.header.stamp = self.get_clock().now().to_msg()
+ joint_state.name = self.api.get_finger_order()
+ joint_state.position = [float(x) for x in pose]
+ if len(vel) > 1:
+ joint_state.velocity = [float(x) for x in vel]
+ else:
+ joint_state.velocity = [0.0] * len(pose)
+ joint_state.effort = [0.0] * len(pose)
+ return joint_state
+
+ def run(self):
+ # 优先获取手指状态并且发布
+ self.last_hand_state = self.api.get_state()
+ self.last_hand_vel = self.api.get_joint_speed()
+ # 发布手状态
+ msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+ self.hand_state_pub.publish(msg_state)
+ # 执行手控制指令
+ if self.last_hand_post_cmd != None:
+ self.api.finger_move(pose=self.last_hand_post_cmd)
+ time.sleep(0.003)
+ self.last_hand_post_cmd = None
+ if self.last_hand_vel_cmd != None:
+ vel = list(self.last_hand_vel_cmd)
+ if all(x == 0 for x in vel):
+ pass
+ else:
+ if (str(self.hand_joint).upper() == "O6" or str(self.hand_joint).upper() == "L6" or str(self.hand_joint).upper() == "L6P") and len(vel) == 6:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L7" and len(vel) == 7:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L10" and len(vel) == 10:
+ speed = [vel[0],vel[2],vel[3],vel[4],vel[5]]
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L20" and len(vel) == 20:
+ speed = [vel[10],vel[1],vel[2],vel[3],vel[4]]
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L21" and len(vel) == 25:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ elif self.hand_joint == "L25" and len(vel) == 25:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ self.last_hand_vel_cmd = None
+ time.sleep(0.005)
+ # 获取压感数据
+ if self.is_touch == True:
+ if self.count == 3:
+ self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=0.006).tolist()
+ if self.count == 4:
+ self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=0.006).tolist()
+ if self.count == 5:
+ self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=0.006).tolist()
+ if self.count == 6:
+ self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=0.006).tolist()
+ if self.count == 7:
+ self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.006).tolist()
+ # 发布矩阵压感数据JSON格式
+ self.pub_matrix_dic()
+ # 发布矩阵压感和值JSON格式
+ self.pub_matrix_mass(dic=self.matrix_dic)
+ # 发布矩阵压感点云格式
+ self.pub_matrix_point_cloud()
+ self.count += 1
+ if self.count == 8:
+ self.count = 0
+ time.sleep(0.006)
+
+ def pub_matrix_dic(self):
+ """发布矩阵数据JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_dic["stamp"]["secs"] = t_secs
+ self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+ msg.data = json.dumps(self.matrix_dic)
+ self.matrix_touch_pub.publish(msg)
+
+ def pub_matrix_mass(self, dic):
+ """发布矩阵数据合值 单位g 克 JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_mass_dic["stamp"]["secs"] = t_secs
+ self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+ self.matrix_mass_dic["unit"] = "g"
+ self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+ self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+ self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+ self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+ self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+ msg.data = json.dumps(self.matrix_mass_dic)
+ self.matrix_touch_mass_pub.publish(msg)
+
+ def pub_matrix_point_cloud(self):
+ tmp_dic = self.matrix_dic.copy()
+ del tmp_dic['stamp'] # 去掉时间戳字段
+ all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数 or 5 帧，每帧 4×10=40 个数 列x行
+ # 摊平到一维
+ flat_list = [v for frame in all_matrices for v in frame]
+ flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+ fields = [PointField(name='val', offset=0, datatype=PointField.UINT8, count=1)]
+ pc = PointCloud2()
+ pc.header.stamp = self.get_clock().now().to_msg()
+ pc.header.frame_id = '' # 可改成你需要的坐标系
+ pc.height = 1
+ pc.width = flat.size # 360
+ pc.fields = fields
+ pc.is_bigendian = False
+ pc.point_step = 1 # 1 个 float32
+ pc.row_step = pc.point_step * pc.width
+ pc.data = flat.tobytes() # 1440 字节
+ self.matrix_touch_pub_pc.publish(pc)
+
+
+ def close_can(self):
+ self.api.open_can.close_can(can=self.can)
+ sys.exit(0)
+
+
+def main(args=None):
+ '''
+ 本节点用于收集手指状态和压感数据。
+ '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+ /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 40Hz
+ '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 40Hz
+ 启动命令:
+ ros2 run linker_hand_ros2_sdk linker_hand_advanced_l7 --hand_type left --can can0 --is_touch true
+ '''
+ try:
+ rclpy.init(args=args)
+ parser = argparse.ArgumentParser()
+ parser.add_argument('--hand_type', required=True)
+ parser.add_argument('--can', required=True)
+ parser.add_argument('--is_touch', choices=['true','false'], required=True)
+
+ args = parser.parse_args()
+ node = LinkerHandAdvancedL7(name="linker_hand_collect_l7",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch)
+ embedded_version = node.embedded_version
+ rclpy.spin(node) # 主循环，监听 ROS 回调
+ except KeyboardInterrupt:
+ print("收到 Ctrl+C，准备退出...")
+ finally:
+ # node.close_can() # 关闭 CAN 或其他硬件资源
+ # node.destroy_node() # 销毁 ROS 节点
+ # rclpy.shutdown() # 关闭 ROS
+ print("程序已退出。")
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_o6.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_o6.py
new file mode 100644
index 0000000..42638c8
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_advanced_o6.py
@@ -0,0 +1,252 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+'''
+编译: colcon build --symlink-install
+启动命令:ros2 run linker_hand_ros2_sdk linker_hand_sdk
+'''
+from re import A
+import rclpy,sys # ROS2 Python接口库
+import time
+import argparse
+import numpy as np
+from rclpy.node import Node # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+class LinkerHandAdvancedO6(Node):
+ def __init__(self, name, hand_type, can, is_touch):
+ super().__init__(name)
+ self.hand_type = hand_type
+ self.hand_joint = "O6"
+ if is_touch == "true":
+ self.is_touch = True
+ else:
+ self.is_touch = False
+ self.can = can
+ self.modbus = "None"
+ time.sleep(0.1)
+ self._check_linker_hand_type()
+ self.last_hand_post_cmd = None # 最新手指位置命令
+ self.last_hand_vel_cmd = None # 最新手指速度命令
+ self.last_hand_eff_cmd = None # 最新手指力矩命令
+ self.matrix_dic = {
+ "stamp":{
+ "sec": 0,
+ "nanosec": 0,
+ },
+ "thumb_matrix":[[-1] * 6 for _ in range(12)],
+ "index_matrix":[[-1] * 6 for _ in range(12)],
+ "middle_matrix":[[-1] * 6 for _ in range(12)],
+ "ring_matrix":[[-1] * 6 for _ in range(12)],
+ "little_matrix":[[-1] * 6 for _ in range(12)]
+ }
+ # 压感矩阵合值，单位g 克
+ self.matrix_mass_dic = {
+ "stamp":{
+ "secs": 0,
+ "nsecs": 0,
+ },
+ "thumb_mass":[-1],
+ "index_mass":[-1],
+ "middle_mass":[-1],
+ "ring_mass":[-1],
+ "little_mass":[-1]
+ }
+ self.hz = 1.0/60.0
+ # ros时间获取
+ self.stamp_clock = Clock()
+ self._init_hand()
+ time.sleep(2)
+ self.timer = self.create_timer(self.hz, self.run) # 100 Hz
+
+
+ def _check_linker_hand_type(self):
+ if self.modbus != "None":
+ ColorMsg(msg=f"Modbus暂不支持", color="red")
+ sys.exit(0)
+ if self.hand_joint.upper() != "O6":
+ ColorMsg(msg=f"O6以外其他Linker Hand暂不支持", color="red")
+ sys.exit(0)
+
+ def _init_hand(self):
+ self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+ time.sleep(0.1)
+ self.touch_type = self.api.get_touch_type()
+ self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+ self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+ if self.is_touch == True:
+ if self.touch_type > 1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+ self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+ self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+ elif self.touch_type != -1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+ self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+ else:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+ self.is_touch = False
+ self.embedded_version = self.api.get_embedded_version()
+ if self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6":
+ pose = [200, 255, 255, 255, 255, 180]
+ torque = [255, 255, 255, 255, 255, 255]
+ # O6 最大速度阈值
+ speed = [255, 255, 255, 255, 255, 255]
+ self.api.set_speed(speed=speed)
+ time.sleep(0.1)
+ self.api.set_torque(torque=torque)
+ time.sleep(0.1)
+ self.api.finger_move(pose=pose)
+ time.sleep(0.1)
+
+ def hand_control_cb(self, msg):
+ if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+ self.last_hand_post_cmd = msg.position
+ if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+ self.last_hand_vel_cmd = msg.velocity
+ if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+ self.last_hand_eff_cmd = msg.effort
+
+ def list_check(self,pose):
+ if isinstance(pose, list) == False:
+ return False
+ if len(self.last_hand_post_cmd) != len(pose):
+ return False
+ return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+
+ def joint_state_msg(self, pose,vel=[]):
+ joint_state = JointState()
+ joint_state.header = Header()
+ joint_state.header.stamp = self.get_clock().now().to_msg()
+ joint_state.name = self.api.get_finger_order()
+ joint_state.position = [float(x) for x in pose]
+ if len(vel) > 1:
+ joint_state.velocity = [float(x) for x in vel]
+ else:
+ joint_state.velocity = [0.0] * len(pose)
+ joint_state.effort = [0.0] * len(pose)
+ return joint_state
+
+ def run(self):
+ # 执行手控制指令
+ if self.last_hand_post_cmd != None:
+ self.api.finger_move(pose=self.last_hand_post_cmd)
+ self.last_hand_post_cmd = None
+ if self.last_hand_vel_cmd != None:
+ vel = list(self.last_hand_vel_cmd)
+ if all(x == 0 for x in vel):
+ pass
+ else:
+ speed = vel
+ self.api.set_joint_speed(speed=speed)
+ self.last_hand_vel_cmd = None
+ # 优先获取手指状态并且发布
+ self.last_hand_state = self.api.get_state()
+ self.last_hand_vel = self.api.get_joint_speed()
+ # 发布手状态
+ msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+ self.hand_state_pub.publish(msg_state)
+ if self.is_touch == True:
+ # 获取压感数据
+ self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=0.002).tolist()
+ self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=0.002).tolist()
+ self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=0.002).tolist()
+ self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=0.002).tolist()
+ self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=0.002).tolist()
+ # 发布矩阵压感数据JSON格式
+ self.pub_matrix_dic()
+ # 发布矩阵压感和值JSON格式
+ self.pub_matrix_mass(dic=self.matrix_dic)
+ # 发布矩阵压感点云格式
+ self.pub_matrix_point_cloud()
+
+ def pub_matrix_dic(self):
+ """发布矩阵数据JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_dic["stamp"]["secs"] = t_secs
+ self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+ msg.data = json.dumps(self.matrix_dic)
+ self.matrix_touch_pub.publish(msg)
+
+ def pub_matrix_mass(self, dic):
+ """发布矩阵数据合值 单位g 克 JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_mass_dic["stamp"]["secs"] = t_secs
+ self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+ self.matrix_mass_dic["unit"] = "g"
+ self.matrix_mass_dic["thumb_mass"] = sum(sum(row) for row in dic["thumb_matrix"])
+ self.matrix_mass_dic["index_mass"] = sum(sum(row) for row in dic["index_matrix"])
+ self.matrix_mass_dic["middle_mass"] = sum(sum(row) for row in dic["middle_matrix"])
+ self.matrix_mass_dic["ring_mass"] = sum(sum(row) for row in dic["ring_matrix"])
+ self.matrix_mass_dic["little_mass"] = sum(sum(row) for row in dic["little_matrix"])
+ msg.data = json.dumps(self.matrix_mass_dic)
+ self.matrix_touch_mass_pub.publish(msg)
+
+ def pub_matrix_point_cloud(self):
+ tmp_dic = self.matrix_dic.copy()
+ del tmp_dic['stamp'] # 去掉时间戳字段
+ all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数 or 5 帧，每帧 4×10=40 个数 列x行
+ # 摊平到一维
+ flat_list = [v for frame in all_matrices for v in frame]
+ flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+ fields = [PointField(name='val', offset=0, datatype=PointField.UINT8, count=1)]
+ pc = PointCloud2()
+ pc.header.stamp = self.get_clock().now().to_msg()
+ pc.header.frame_id = '' # 可改成你需要的坐标系
+ pc.height = 1
+ pc.width = flat.size # 360
+ pc.fields = fields
+ pc.is_bigendian = False
+ pc.point_step = 1 # 1 个 float32
+ pc.row_step = pc.point_step * pc.width
+ pc.data = flat.tobytes() # 1440 字节
+ self.matrix_touch_pub_pc.publish(pc)
+
+
+ def close_can(self):
+ self.api.open_can.close_can(can=self.can)
+ sys.exit(0)
+
+
+def main(args=None):
+ '''
+ 本节点用于收集手指状态和压感数据。
+ '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+ /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 50Hz
+ '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 50Hz
+ 启动命令:
+ ros2 run linker_hand_ros2_sdk linker_hand_advanced_o6 --hand_type right --can can0 --is_touch true
+ '''
+ try:
+ rclpy.init(args=args)
+ parser = argparse.ArgumentParser()
+ parser.add_argument('--hand_type', required=True)
+ parser.add_argument('--can', required=True)
+ parser.add_argument('--is_touch', choices=['true','false'], required=True)
+
+ args = parser.parse_args()
+ node = LinkerHandAdvancedO6(name="linker_hand_advanced_o6",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch)
+ embedded_version = node.embedded_version
+ rclpy.spin(node) # 主循环，监听 ROS 回调
+ except KeyboardInterrupt:
+ print("收到 Ctrl+C，准备退出...")
+ finally:
+ # node.close_can() # 关闭 CAN 或其他硬件资源
+ # node.destroy_node() # 销毁 ROS 节点
+ # rclpy.shutdown() # 关闭 ROS
+ print("程序已退出。")
\ No newline at end of file
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_g20_palm_touch.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_g20_palm_touch.py
new file mode 100644
index 0000000..eda447c
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand_g20_palm_touch.py
@@ -0,0 +1,268 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+from re import A
+import rclpy,sys # ROS2 Python接口库
+import time
+import argparse
+import numpy as np
+from rclpy.node import Node # ROS2 节点类
+from rclpy.clock import Clock
+from std_msgs.msg import String, Header, Float32MultiArray
+from sensor_msgs.msg import JointState, PointCloud2, PointField
+import time, json, threading
+from linker_hand_ros2_sdk.LinkerHand.linker_hand_api import LinkerHandApi
+from linker_hand_ros2_sdk.LinkerHand.utils.color_msg import ColorMsg
+from linker_hand_ros2_sdk.LinkerHand.utils.open_can import OpenCan
+
+# Linker Hand 型号
+HAND_JOINT = "G20"
+# 默认手指关节位置
+DEFAULT_POSITION = [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 245, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+# 默认手指关节速度
+DEFAULT_SPEED=[255, 255, 255, 255, 255]
+# 默认手指关节力矩
+DEFAULT_TORQUE = [255, 255, 255, 255, 255]
+# 压感传感器延迟时间
+TOUCH_SLEEP_TIME = 0.003
+
+
+
+class LinkerHandAdvancedG20(Node):
+ def __init__(self, name, hand_type, can, is_touch):
+ super().__init__(name)
+ self.hand_type = hand_type
+ self.hand_joint = HAND_JOINT
+ if is_touch == "true":
+ self.is_touch = True
+ else:
+ self.is_touch = False
+ self.can = can
+ self.modbus = "None"
+ time.sleep(0.1)
+ self._check_linker_hand_type()
+ self.last_hand_post_cmd = None # 最新手指位置命令
+ self.last_hand_vel_cmd = None # 最新手指速度命令
+ self.last_hand_eff_cmd = None # 最新手指力矩命令
+ self.matrix_dic = {
+ "stamp":{
+ "sec": 0,
+ "nanosec": 0,
+ },
+ "thumb_matrix":[[-1] * 6 for _ in range(12)],
+ "index_matrix":[[-1] * 6 for _ in range(12)],
+ "middle_matrix":[[-1] * 6 for _ in range(12)],
+ "ring_matrix":[[-1] * 6 for _ in range(12)],
+ "little_matrix":[[-1] * 6 for _ in range(12)]
+ }
+ # 压感矩阵合值，单位g 克
+ self.matrix_mass_dic = {
+ "stamp":{
+ "secs": 0,
+ "nsecs": 0,
+ },
+ "thumb_mass":[-1],
+ "index_mass":[-1],
+ "middle_mass":[-1],
+ "ring_mass":[-1],
+ "little_mass":[-1]
+ }
+ self.hz = 1.0/60.0
+ # ros时间获取
+ self.stamp_clock = Clock()
+ self._init_hand()
+ time.sleep(2)
+ self.count = 0
+ self.timer = self.create_timer(self.hz, self.run)
+
+
+ def _check_linker_hand_type(self):
+ if self.modbus != "None":
+ ColorMsg(msg=f"Modbus暂不支持", color="red")
+ sys.exit(0)
+ if self.hand_joint.upper() != "G20":
+ ColorMsg(msg=f"Linker Hand hand_joint参数错误", color="red")
+ sys.exit(0)
+
+ def _init_hand(self):
+ self.api = LinkerHandApi(hand_type=self.hand_type, hand_joint=self.hand_joint,modbus=self.modbus,can=self.can)
+ time.sleep(0.1)
+ self.touch_type = self.api.get_touch_type()
+ self.hand_cmd_sub = self.create_subscription(JointState, f'/cb_{self.hand_type}_hand_control_cmd', self.hand_control_cb,10)
+ self.hand_state_pub = self.create_publisher(JointState, f'/cb_{self.hand_type}_hand_state',10)
+ if self.is_touch == True:
+ if self.touch_type > 1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with matrix pressure sensing", color='green')
+ self.matrix_touch_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch', 10)
+ #self.matrix_touch_pub_pc = self.create_publisher(PointCloud2, f'/cb_{self.hand_type}_hand_matrix_touch_pc', 10)
+ self.matrix_touch_mass_pub = self.create_publisher(String, f'/cb_{self.hand_type}_hand_matrix_touch_mass', 10)
+ elif self.touch_type != -1:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Equipped with pressure sensor", color="green")
+ self.touch_pub = self.create_publisher(Float32MultiArray, f'/cb_{self.hand_type}_hand_force', 10)
+ else:
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} Not equipped with any pressure sensors", color="red")
+ self.is_touch = False
+ self.embedded_version = self.api.get_embedded_version()
+ self.api.set_speed(speed=DEFAULT_SPEED)
+ time.sleep(0.1)
+ self.api.set_torque(torque=DEFAULT_TORQUE)
+ time.sleep(0.1)
+ self.api.finger_move(pose=DEFAULT_POSITION)
+ time.sleep(0.1)
+ self.palm_touch = self.api.is_palm_touch
+ if self.palm_touch == 5:
+ self.touch_sleep_time = 0.03
+ ColorMsg(msg=f"{self.hand_type} {self.hand_joint} 全掌压感版", color="green")
+ else:
+ self.touch_sleep_time = 0.003
+
+ def hand_control_cb(self, msg):
+ if self.last_hand_post_cmd == None or self.list_check(msg.position) == True:
+ self.last_hand_post_cmd = msg.position
+ if self.last_hand_vel_cmd == None or self.list_check(msg.velocity) == True:
+ self.last_hand_vel_cmd = msg.velocity
+ if self.last_hand_eff_cmd == None or self.list_check(msg.effort) == True:
+ self.last_hand_eff_cmd = msg.effort
+
+ def list_check(self,pose):
+ if isinstance(pose, list) == False:
+ return False
+ if len(self.last_hand_post_cmd) != len(pose):
+ return False
+ return any(abs(self.last_hand_post_cmd - pose) >= 3 for self.last_hand_post_cmd, pose in zip(self.last_hand_post_cmd, pose))
+
+ def joint_state_msg(self, pose,vel=[]):
+ joint_state = JointState()
+ joint_state.header = Header()
+ joint_state.header.stamp = self.get_clock().now().to_msg()
+ joint_state.name = self.api.get_finger_order()
+ joint_state.position = [float(x) for x in pose]
+ if len(vel) > 1:
+ joint_state.velocity = [float(x) for x in vel]
+ else:
+ joint_state.velocity = [0.0] * len(pose)
+ joint_state.effort = [0.0] * len(pose)
+ return joint_state
+
+ def run(self):
+ # 执行手控制指令
+ if self.last_hand_post_cmd != None:
+ self.api.finger_move(pose=self.last_hand_post_cmd)
+ self.last_hand_post_cmd = None
+ # 优先获取手指状态并且发布
+ self.last_hand_state = self.api.get_state()
+ self.last_hand_vel = [0.0] * len(self.last_hand_state)
+ # 发布手状态
+ msg_state = self.joint_state_msg(self.last_hand_state, self.last_hand_vel)
+ self.hand_state_pub.publish(msg_state)
+
+ if self.is_touch == True:
+ # 获取压感数据
+ if self.count == 2:
+ self.matrix_dic["thumb_matrix"] = self.api.get_thumb_matrix_touch(sleep_time=self.touch_sleep_time).tolist()
+ if self.count == 4:
+ self.matrix_dic["index_matrix"] = self.api.get_index_matrix_touch(sleep_time=self.touch_sleep_time).tolist()
+ if self.count == 6:
+ self.matrix_dic["middle_matrix"] = self.api.get_middle_matrix_touch(sleep_time=self.touch_sleep_time).tolist()
+ if self.count == 8:
+ self.matrix_dic["ring_matrix"] = self.api.get_ring_matrix_touch(sleep_time=self.touch_sleep_time).tolist()
+ if self.count == 10:
+ self.matrix_dic["little_matrix"] = self.api.get_little_matrix_touch(sleep_time=self.touch_sleep_time).tolist()
+ if self.count == 14 and self.palm_touch == 5:
+ self.matrix_dic["palm_matrix"] = self.api.get_palm_matrix_touch(sleep_time=self.touch_sleep_time).tolist()
+ # 发布矩阵压感数据JSON格式
+ self.pub_matrix_dic()
+ # 发布矩阵压感和值JSON格式
+ self.pub_matrix_mass(dic=self.matrix_dic)
+ # 发布矩阵压感点云格式
+ #self.pub_matrix_point_cloud()
+ self.count += 1
+ if self.count == 15:
+ self.count = 0
+
+ def pub_matrix_dic(self):
+ """发布矩阵数据JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_dic["stamp"]["secs"] = t_secs
+ self.matrix_dic["stamp"]["nsecs"] = t_nsecs
+ msg.data = json.dumps(self.matrix_dic)
+ self.matrix_touch_pub.publish(msg)
+
+ def pub_matrix_mass(self, dic):
+ """发布矩阵数据合值 单位g 克 JSON格式"""
+ msg = String()
+ # 获取当前的 ROS 时间
+ current_time = self.stamp_clock.now()
+ # 提取 secs 和 nsecs
+ t_secs = current_time.to_msg().sec
+ t_nsecs = current_time.to_msg().nanosec
+ self.matrix_mass_dic["stamp"]["secs"] = t_secs
+ self.matrix_mass_dic["stamp"]["nsecs"] = t_nsecs
+ self.matrix_mass_dic["unit"] = "g"
+ self.matrix_mass_dic["thumb_mass"] = self.api.hand.thumb_matrix_palm_mass
+ self.matrix_mass_dic["index_mass"] = self.api.hand.index_matrix_palm_mass
+ self.matrix_mass_dic["middle_mass"] = self.api.hand.middle_matrix_palm_mass
+ self.matrix_mass_dic["ring_mass"] = self.api.hand.ring_matrix_palm_mass
+ self.matrix_mass_dic["little_mass"] = self.api.hand.little_matrix_palm_mass
+ self.matrix_mass_dic["palm_mass"] = self.api.hand.palm_matrix_palm_mass
+ msg.data = json.dumps(self.matrix_mass_dic)
+ self.matrix_touch_mass_pub.publish(msg)
+
+ # def pub_matrix_point_cloud(self):
+ # tmp_dic = self.matrix_dic.copy()
+ # del tmp_dic['stamp'] # 去掉时间戳字段
+ # all_matrices = list(tmp_dic.values()) # 5 帧，每帧 6×12=72 个数 or 5 帧，每帧 4×10=40 个数 列x行
+ # # 摊平到一维
+ # flat_list = [v for frame in all_matrices for v in frame]
+ # flat = np.concatenate([np.asarray(np.clip(c, 0, 255), dtype=np.uint8) for c in flat_list])
+ # fields = [PointField(name='val', offset=0, datatype=PointField.UINT8, count=1)]
+ # pc = PointCloud2()
+ # pc.header.stamp = self.get_clock().now().to_msg()
+ # pc.header.frame_id = '' # 可改成你需要的坐标系
+ # pc.height = 1
+ # pc.width = flat.size # 360
+ # pc.fields = fields
+ # pc.is_bigendian = False
+ # pc.point_step = 1 # 1 个 float32
+ # pc.row_step = pc.point_step * pc.width
+ # pc.data = flat.tobytes() # 1440 字节
+ # self.matrix_touch_pub_pc.publish(pc)
+
+
+ def close_can(self):
+ self.api.open_can.close_can(can=self.can)
+ sys.exit(0)
+
+
+def main(args=None):
+ '''
+ 本节点用于收集手指状态和压感数据。
+ '/cb_{self.hand_type}_hand_control_cmd' 话题类型为 sensor_msgs/msg/JointState 控制话题，限制 30Hz
+ /cb_{self.hand_type}_hand_state 话题类型为 sensor_msgs/msg/JointState 30Hz
+ '/cb_{self.hand_type}_hand_matrix_touch' 话题类型为 std_msgs/msg/String 30Hz
+ 启动命令:
+ ros2 run linker_hand_ros2_sdk linker_hand_g20_palm_touch --hand_type left --can can0 --is_touch true
+ '''
+ try:
+ rclpy.init(args=args)
+ parser = argparse.ArgumentParser()
+ parser.add_argument('--hand_type', required=True)
+ parser.add_argument('--can', required=True)
+ parser.add_argument('--is_touch', choices=['true','false'], required=True)
+
+ args = parser.parse_args()
+ node = LinkerHandAdvancedG20(name="linker_hand_g20_palm_touch",hand_type=args.hand_type,can=args.can,is_touch=args.is_touch)
+ embedded_version = node.embedded_version
+ rclpy.spin(node) # 主循环，监听 ROS 回调
+ except KeyboardInterrupt:
+ print("收到 Ctrl+C，准备退出...")
+ finally:
+ node.close_can() # 关闭 CAN 或其他硬件资源
+ # node.destroy_node() # 销毁 ROS 节点
+ # rclpy.shutdown() # 关闭 ROS
+ print("程序已退出。")
\ No newline at end of file
diff --git a/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/o6_joint_curve.py b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/o6_joint_curve.py
new file mode 100644
index 0000000..2a06b5a
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/o6_joint_curve.py
@@ -0,0 +1,331 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""Record O6 joint positions and plot angle/derived-velocity curves.
+
+The O6 ``0x05`` value exposed by the SDK is a configured speed, not a measured
+joint velocity. This recorder therefore always derives velocity from the
+position samples and their monotonic timestamps.
+"""
+
+import argparse
+import csv
+from datetime import datetime
+import json
+import math
+from pathlib import Path
+import sys
+import time
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from rclpy.utilities import remove_ros_args
+from sensor_msgs.msg import JointState
+
+
+O6_JOINT_NAMES = (
+ "thumb_cmc_pitch",
+ "thumb_cmc_yaw",
+ "index_mcp_pitch",
+ "middle_mcp_pitch",
+ "ring_mcp_pitch",
+ "pinky_mcp_pitch",
+)
+
+# Nominal SDK ranges. A hand-specific lookup JSON should be used for formal
+# measurements because individual hands can differ from these values.
+O6_NOMINAL_MAX_DEG = dict(
+ zip(O6_JOINT_NAMES, np.rad2deg([0.58, 1.36, 1.6, 1.6, 1.6, 1.6]))
+)
+
+
+class AngleMapper:
+ """Convert O6 uint8 positions to degrees using lookup tables or nominal ranges."""
+
+ def __init__(self, mapping_json=None):
+ self.tables = {}
+ self.source = "SDK nominal range"
+ if mapping_json:
+ self._load(Path(mapping_json).expanduser())
+
+ def _load(self, path):
+ with path.open("r", encoding="utf-8") as stream:
+ data = json.load(stream)
+
+ joints = data.get("joints", data)
+ if not isinstance(joints, dict):
+ raise ValueError("mapping JSON must contain an object named 'joints'")
+
+ for joint_name, values in joints.items():
+ if not isinstance(values, dict):
+ continue
+ joint_u8 = values.get("joint_u8")
+ joint_real_deg = values.get("joint_real_deg")
+ if joint_u8 is None or joint_real_deg is None:
+ continue
+ if len(joint_u8) != len(joint_real_deg) or len(joint_u8) < 2:
+ raise ValueError(
+ f"{joint_name}: joint_u8 and joint_real_deg need equal lengths >= 2"
+ )
+
+ x = np.asarray(joint_u8, dtype=float)
+ y = np.asarray(joint_real_deg, dtype=float)
+ if not np.all(np.isfinite(x)) or not np.all(np.isfinite(y)):
+ raise ValueError(f"{joint_name}: mapping contains a non-finite value")
+ order = np.argsort(x)
+ x = x[order]
+ y = y[order]
+ if np.any(np.diff(x) <= 0):
+ raise ValueError(f"{joint_name}: joint_u8 values must be unique")
+ self.tables[joint_name] = (x, y)
+
+ if not self.tables:
+ raise ValueError(
+ "mapping JSON has no joint with joint_u8 and joint_real_deg arrays"
+ )
+ self.source = str(path)
+
+ def to_degrees(self, joint_name, position_u8):
+ if joint_name in self.tables:
+ x, y = self.tables[joint_name]
+ return float(np.interp(position_u8, x, y))
+
+ if joint_name not in O6_NOMINAL_MAX_DEG:
+ raise KeyError(f"no O6 nominal range or lookup table for {joint_name}")
+ position_u8 = float(np.clip(position_u8, 0.0, 255.0))
+ return (255.0 - position_u8) * O6_NOMINAL_MAX_DEG[joint_name] / 255.0
+
+
+def derive_velocity(time_s, angle_deg, cutoff_hz=5.0):
+ """Differentiate angle samples and apply a first-order low-pass filter."""
+ time_s = np.asarray(time_s, dtype=float)
+ angle_deg = np.asarray(angle_deg, dtype=float)
+ if len(time_s) != len(angle_deg):
+ raise ValueError("time and angle arrays must have the same length")
+ if len(time_s) < 2:
+ return np.zeros_like(angle_deg)
+ if np.any(np.diff(time_s) <= 0):
+ raise ValueError("timestamps must be strictly increasing")
+
+ velocity = np.gradient(angle_deg, time_s)
+ if cutoff_hz <= 0.0:
+ return velocity
+
+ filtered = np.empty_like(velocity)
+ filtered[0] = velocity[0]
+ rc = 1.0 / (2.0 * math.pi * cutoff_hz)
+ for index in range(1, len(velocity)):
+ dt = time_s[index] - time_s[index - 1]
+ alpha = dt / (rc + dt)
+ filtered[index] = filtered[index - 1] + alpha * (
+ velocity[index] - filtered[index - 1]
+ )
+ return filtered
+
+
+class O6JointCurveRecorder(Node):
+ def __init__(
+ self,
+ hand_type,
+ duration,
+ output_dir,
+ joint_name,
+ mapping_json,
+ filter_hz,
+ ):
+ super().__init__("o6_joint_curve")
+ self.hand_type = hand_type
+ self.duration = duration
+ self.output_dir = Path(output_dir).expanduser()
+ self.joint_name = joint_name
+ self.filter_hz = filter_hz
+ self.mapper = AngleMapper(mapping_json)
+ self.samples = []
+ self.start_time = None
+ self.saved_paths = None
+ self.finished = False
+
+ topic = f"/cb_{hand_type}_hand_state"
+ self.subscription = self.create_subscription(
+ JointState, topic, self._state_callback, 100
+ )
+ self.stop_timer = self.create_timer(0.05, self._check_duration)
+ self.get_logger().info(
+ f"Recording {topic}; duration={duration:.3f}s; angle map={self.mapper.source}"
+ )
+
+ def _state_callback(self, msg):
+ if self.finished or not msg.position:
+ return
+
+ names = tuple(msg.name) if len(msg.name) == len(msg.position) else O6_JOINT_NAMES
+ if len(msg.position) != len(names):
+ self.get_logger().warning(
+ f"Ignoring JointState with {len(msg.position)} positions and "
+ f"{len(msg.name)} names"
+ )
+ return
+
+ now = time.perf_counter()
+ if self.start_time is None:
+ self.start_time = now
+ relative_time = now - self.start_time
+ for name, position in zip(names, msg.position):
+ if self.joint_name != "all" and name != self.joint_name:
+ continue
+ try:
+ angle_deg = self.mapper.to_degrees(name, position)
+ except KeyError as error:
+ self.get_logger().warning(str(error))
+ continue
+ self.samples.append((relative_time, name, float(position), angle_deg))
+
+ def _check_duration(self):
+ if self.finished or self.start_time is None or self.duration <= 0.0:
+ return
+ if time.perf_counter() - self.start_time >= self.duration:
+ self.finish()
+ rclpy.shutdown()
+
+ def finish(self):
+ if self.finished:
+ return self.saved_paths
+ self.finished = True
+ if not self.samples:
+ self.get_logger().warning("No joint-state sample received; no files were written")
+ return None
+ self.saved_paths = export_curves(
+ samples=self.samples,
+ output_dir=self.output_dir,
+ hand_type=self.hand_type,
+ mapping_source=self.mapper.source,
+ cutoff_hz=self.filter_hz,
+ )
+ csv_path, plot_path = self.saved_paths
+ self.get_logger().info(f"Saved samples: {csv_path}")
+ self.get_logger().info(f"Saved curves: {plot_path}")
+ return self.saved_paths
+
+
+def export_curves(samples, output_dir, hand_type, mapping_source, cutoff_hz):
+ """Export long-form CSV and a two-panel PNG for all recorded joints."""
+ import matplotlib
+
+ matplotlib.use("Agg")
+ import matplotlib.pyplot as plt
+
+ output_dir = Path(output_dir).expanduser()
+ output_dir.mkdir(parents=True, exist_ok=True)
+ stem = f"o6_{hand_type}_{datetime.now().strftime('%Y%m%d_%H%M%S')}"
+ csv_path = output_dir / f"{stem}.csv"
+ plot_path = output_dir / f"{stem}.png"
+
+ joint_samples = {}
+ for sample in samples:
+ joint_samples.setdefault(sample[1], []).append(sample)
+
+ processed = {}
+ for joint_name, values in joint_samples.items():
+ time_s = np.asarray([value[0] for value in values], dtype=float)
+ position_u8 = np.asarray([value[2] for value in values], dtype=float)
+ angle_deg = np.asarray([value[3] for value in values], dtype=float)
+ velocity_deg_s = derive_velocity(time_s, angle_deg, cutoff_hz)
+ processed[joint_name] = (time_s, position_u8, angle_deg, velocity_deg_s)
+
+ with csv_path.open("w", encoding="utf-8", newline="") as stream:
+ writer = csv.writer(stream)
+ writer.writerow(
+ ["t_s", "joint_name", "position_u8", "angle_deg", "velocity_deg_s"]
+ )
+ for joint_name, arrays in processed.items():
+ for row in zip(*arrays):
+ writer.writerow(
+ [
+ f"{row[0]:.9f}",
+ joint_name,
+ f"{row[1]:.6f}",
+ f"{row[2]:.6f}",
+ f"{row[3]:.6f}",
+ ]
+ )
+
+ figure, axes = plt.subplots(2, 1, sharex=True, figsize=(12, 8))
+ for joint_name, (time_s, _position_u8, angle_deg, velocity_deg_s) in processed.items():
+ axes[0].plot(time_s, angle_deg, label=joint_name)
+ axes[1].plot(time_s, velocity_deg_s, label=joint_name)
+
+ axes[0].set_ylabel("Angle (deg)")
+ axes[0].set_title(f"O6 {hand_type} joint curves\nangle map: {mapping_source}")
+ axes[1].set_ylabel("Velocity (deg/s)")
+ axes[1].set_xlabel("Time (s)")
+ axes[1].set_title(f"Derived velocity, first-order low-pass {cutoff_hz:g} Hz")
+ for axis in axes:
+ axis.grid(True, alpha=0.3)
+ axis.legend(loc="best", fontsize="small")
+ figure.tight_layout()
+ figure.savefig(plot_path, dpi=160)
+ plt.close(figure)
+ return csv_path, plot_path
+
+
+def _parse_args(argv):
+ parser = argparse.ArgumentParser(
+ description="Record O6 angle and angle-derived velocity curves"
+ )
+ parser.add_argument("--hand-type", choices=("left", "right"), default="right")
+ parser.add_argument(
+ "--duration",
+ type=float,
+ default=10.0,
+ help="recording seconds; <= 0 records until Ctrl+C (default: 10)",
+ )
+ parser.add_argument(
+ "--output-dir", default="logs/O6/curves", help="CSV/PNG output directory"
+ )
+ parser.add_argument(
+ "--joint",
+ default="all",
+ choices=("all",) + O6_JOINT_NAMES,
+ help="plot one joint or all six (default: all)",
+ )
+ parser.add_argument(
+ "--mapping-json",
+ help="optional hand_.json containing joints..joint_u8 and joint_real_deg",
+ )
+ parser.add_argument(
+ "--filter-hz",
+ type=float,
+ default=5.0,
+ help="velocity low-pass cutoff; <= 0 disables filtering (default: 5)",
+ )
+ parsed = parser.parse_args(remove_ros_args(args=argv)[1:])
+ if parsed.duration == 0.0:
+ parsed.duration = -1.0
+ return parsed
+
+
+def main(args=None):
+ argv = sys.argv if args is None else [sys.argv[0], *args]
+ parsed = _parse_args(argv)
+ rclpy.init(args=args)
+ node = O6JointCurveRecorder(
+ hand_type=parsed.hand_type,
+ duration=parsed.duration,
+ output_dir=parsed.output_dir,
+ joint_name=parsed.joint,
+ mapping_json=parsed.mapping_json,
+ filter_hz=parsed.filter_hz,
+ )
+ try:
+ rclpy.spin(node)
+ except KeyboardInterrupt:
+ node.get_logger().info("Stopping on Ctrl+C")
+ finally:
+ node.finish()
+ node.destroy_node()
+ if rclpy.ok():
+ rclpy.shutdown()
+
+
+if __name__ == "__main__":
+ main()
diff --git a/src/linker_hand_ros2_sdk/package.xml b/src/linker_hand_ros2_sdk/package.xml
new file mode 100644
index 0000000..943c8c2
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/package.xml
@@ -0,0 +1,24 @@
+
+
+
+ linker_hand_ros2_sdk
+ 0.0.0
+ TODO: Package description
+ linker-robot
+ TODO: License declaration
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+ rclpy
+ launch
+ sensor_msgs
+ python3-numpy
+ python3-matplotlib
+
+
+ ament_python
+
+
diff --git a/src/linker_hand_ros2_sdk/pyproject.toml b/src/linker_hand_ros2_sdk/pyproject.toml
new file mode 100644
index 0000000..638dd9c
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/pyproject.toml
@@ -0,0 +1,3 @@
+[build-system]
+requires = ["setuptools>=61.0"]
+build-backend = "setuptools.build_meta"
diff --git a/src/linker_hand_ros2_sdk/resource/linker_hand_ros2_sdk b/src/linker_hand_ros2_sdk/resource/linker_hand_ros2_sdk
new file mode 100644
index 0000000..e69de29
diff --git a/src/linker_hand_ros2_sdk/setup.cfg b/src/linker_hand_ros2_sdk/setup.cfg
new file mode 100644
index 0000000..eb74b8f
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/linker_hand_ros2_sdk
+[install]
+install_scripts=$base/lib/linker_hand_ros2_sdk
diff --git a/src/linker_hand_ros2_sdk/setup.py b/src/linker_hand_ros2_sdk/setup.py
new file mode 100644
index 0000000..5a1479f
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/setup.py
@@ -0,0 +1,51 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'linker_hand_ros2_sdk'
+
+this_dir = os.path.abspath(os.path.dirname(__file__))
+custom_dir = os.path.join(this_dir, package_name, "LinkerHand")
+
+data_files = [
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+]
+
+# for root, dirs, files in os.walk(custom_dir):
+# if files:
+# relative_path = os.path.relpath(root, os.path.join(this_dir, package_name))
+# target_path = os.path.join('share', package_name, relative_path)
+# # 修复这里：路径必须是相对路径
+# files_full_path = [os.path.relpath(os.path.join(root, f), start=os.getcwd()) for f in files]
+# data_files.append((target_path, files_full_path))
+
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(include=[package_name, f"{package_name}.*"]),
+ data_files=data_files,
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='linker-robot',
+ maintainer_email='linker-robot@todo.todo',
+ description='ROS2 SDK for Linker Hand',
+ license='TODO: License declaration',
+ entry_points={
+ 'console_scripts': [
+ 'linker_hand_sdk = linker_hand_ros2_sdk.linker_hand:main',
+ 'linker_hand_advanced_o6 = linker_hand_ros2_sdk.linker_hand_advanced_o6:main',
+ 'o6_joint_curve = linker_hand_ros2_sdk.o6_joint_curve:main',
+ 'linker_hand_advanced_l6 = linker_hand_ros2_sdk.linker_hand_advanced_l6:main',
+ 'linker_hand_advanced_l7 = linker_hand_ros2_sdk.linker_hand_advanced_l7:main',
+ 'linker_hand_advanced_l10 = linker_hand_ros2_sdk.linker_hand_advanced_l10:main',
+ 'linker_hand_advanced_g20 = linker_hand_ros2_sdk.linker_hand_advanced_g20:main',
+ 'linker_hand_g20_palm_touch = linker_hand_ros2_sdk.linker_hand_g20_palm_touch:main',
+ ],
+ },
+)
diff --git a/src/linker_hand_ros2_sdk/test/test_copyright.py b/src/linker_hand_ros2_sdk/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/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'
diff --git a/src/linker_hand_ros2_sdk/test/test_flake8.py b/src/linker_hand_ros2_sdk/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/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)
diff --git a/src/linker_hand_ros2_sdk/test/test_pep257.py b/src/linker_hand_ros2_sdk/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/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'
diff --git a/src/linker_hand_ros2_sdk/test/test_topic_prefix.py b/src/linker_hand_ros2_sdk/test/test_topic_prefix.py
new file mode 100644
index 0000000..f3f7931
--- /dev/null
+++ b/src/linker_hand_ros2_sdk/test/test_topic_prefix.py
@@ -0,0 +1,36 @@
+from rclpy.qos import HistoryPolicy
+
+from linker_hand_ros2_sdk.linker_hand import (
+ COMMAND_QOS,
+ LinkerHand,
+ command_changed,
+ state_poll_due,
+)
+
+
+def test_empty_topic_prefix_preserves_legacy_topics():
+ assert LinkerHand.normalize_topic_prefix("") == ""
+ assert LinkerHand.normalize_topic_prefix("/") == ""
+
+
+def test_topic_prefix_is_absolute_and_has_no_trailing_slash():
+ assert LinkerHand.normalize_topic_prefix("o6") == "/o6"
+ assert LinkerHand.normalize_topic_prefix("/g20/") == "/g20"
+
+
+def test_command_qos_keeps_only_the_latest_sample():
+ assert COMMAND_QOS.history == HistoryPolicy.KEEP_LAST
+ assert COMMAND_QOS.depth == 1
+
+
+def test_identical_commands_are_not_reapplied():
+ assert command_changed(None, [60, 60])
+ assert not command_changed([60, 60], [60, 60])
+ assert command_changed([60, 60], [60, 61])
+ assert not command_changed([60, 60], [])
+
+
+def test_state_polling_is_throttled_without_missing_deadline():
+ assert state_poll_due(None, 10.0, 0.1)
+ assert not state_poll_due(10.0, 10.09, 0.1)
+ assert state_poll_due(10.0, 10.1, 0.1)
diff --git a/src/linkerhand_retarget/linkerhand_retarget/__init__.py b/src/linkerhand_retarget/linkerhand_retarget/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/ReadMe.md b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/ReadMe.md
new file mode 100644
index 0000000..80dff26
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/ReadMe.md
@@ -0,0 +1,19 @@
+## v1.0.0.1 update，更新日期2025-03-31
+1. 调整linker_hand_l10_left拇指旋转角不当的问题，子版本号变更为1.0.0.1
+2. 调整linker_hand_l10_left无名指偏航角的大小，子版本号变更为1.0.0.2
+3. 调整linker_hand_l10_left小指偏航角的大小，子版本号变更为1.0.0.3
+4. linker_hand_l10_right的拇指旋转角处于异常状态，需结构重新设定，版本封存
+5. 调整linker_hand_l20_right拇指旋转角不当的问题，子版本号变更为1.0.0.1
+6. 调整linker_hand_l20_right拇指偏航角不当的问题，子版本号变更为1.0.0.2
+7. 调整linker_hand_t25_left四指横滚角不当的问题，子版本号变更为1.0.0.1
+
+## v1.0.0.0 create
+1. 版本创建
+2. 添加linker_hand_l10_left，版本号v1.6.7995.38578
+3. 添加linker_hand_l10_right，版本号v1.0.0
+4. 添加linker_hand_l20_left，版本号v1.0.0
+5. 添加linker_hand_l20_right，版本号v1.0.0
+6. 添加linker_hand_t25_left，版本号v1.0.0
+7. 添加linker_hand_t25_right，版本号v1.0.0
+8. 添加linker_hand_o7_left，版本号v1.0.0
+9. 添加linker_hand_o7_right，版本号v1.0.0
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf
new file mode 100644
index 0000000..9c0c1f1
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf
@@ -0,0 +1,1285 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_l25_left.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_l25_left.urdf
new file mode 120000
index 0000000..b2a4b3b
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_l25_left.urdf
@@ -0,0 +1 @@
+linkerhand_g20_left.urdf
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/hand_base_link.STL
new file mode 100644
index 0000000..9c358cb
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f5fa64265908b92a132e18fff27dc212ec21ba4872806386846374d5c91fc70a
+size 3504384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/index_distal.STL
new file mode 100644
index 0000000..5db670b
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:130639135efbb5cd2773c18b481fcea45afe3bfd5dc8279a28a18a6b518779db
+size 993184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/index_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/index_metacarpals.STL
new file mode 100644
index 0000000..bad2be3
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/index_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:60bc3fc552cbdc4a53fbde3e11a06fc8b0b52a0ee6ea8b730b481a6eb54cfe92
+size 153484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/index_middle.STL
new file mode 100644
index 0000000..34775d6
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b3e46d945996e52bf0758b80f7aa1c2b99bca503b4c5b3c350e6850f78b05152
+size 732584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/index_proximal.STL
new file mode 100644
index 0000000..55a22fa
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5a561d159861170e0bc91d6a4ecb634611bcc497bb2e60440d829e5ae0b9b7f7
+size 881084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/middle_distal.STL
new file mode 100644
index 0000000..3e831da
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a6143d000a38c7b60231b55b9e209161c7726a0410f5a7e5c48c0f41f067a006
+size 993184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/middle_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/middle_metacarpals.STL
new file mode 100644
index 0000000..a576c56
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/middle_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:592ce620ebba1e75ce109cf70654dc8dc2d9bb41315b6dfc902b3ed3c703f43a
+size 153484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/middle_middle.STL
new file mode 100644
index 0000000..0821e32
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:09b1b9a5bf9017d86dc543a214d263acc32c2ed4c3a474116ccdb8ab163cce89
+size 732584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/middle_proximal.STL
new file mode 100644
index 0000000..aa7302f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:115d5c2027a656d6765339a477effc251a6fb9cd26dfc56474e6b63fac64aa43
+size 881084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/pinky_distal.STL
new file mode 100644
index 0000000..d61f5d4
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2d31ac67d0c2573758f0da2de4c3b773494a262845cc63659f2816bfd1fda10a
+size 993184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/pinky_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/pinky_metacarpals.STL
new file mode 100644
index 0000000..977ef69
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/pinky_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:95a7614a7495032fdec6d5bf39df04a5fec85fbe1032ea8ffad5e09e59fe287a
+size 153484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/pinky_middle.STL
new file mode 100644
index 0000000..26bcc40
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5a67a4840294f79a64b143e1cba9131811342df8c735f817708dd106699c6480
+size 732584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..6964857
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5fce82ec63ecba86c656edbba8b17c5b44d51aa78df0ddb9657a465f89872206
+size 881084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/ring_distal.STL
new file mode 100644
index 0000000..fcf4d03
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c82b0efd00e191a019927cf5590135f8b4d9b2c9228abfb68b12f3e0257c42aa
+size 993184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/ring_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/ring_metacarpals.STL
new file mode 100644
index 0000000..d7bc961
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/ring_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c90a028bf7a5da221a92d886a65cde79cf5641ff82eccd65bd6b982e17df912d
+size 153484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/ring_middle.STL
new file mode 100644
index 0000000..0a8cd71
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:191ddee09524ed6f7ebd447cf30fb85f894969ba161fc8c0059b6e97f70319f1
+size 732584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/ring_proximal.STL
new file mode 100644
index 0000000..3d372d5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6a49d3ee8360028022c2ba7435ffb35fb19ffaaa3c73ddba5da16b106d86857c
+size 881084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_distal.STL
new file mode 100644
index 0000000..5921c52
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0812eea8d86179c70f00070e3142595e1b5cea231a5c606c78744041d7acd9ba
+size 1115884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..f26dd54
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7bcb58b906bcff02d8de04f0ac90dbe6bf318cdbda52988787808b50f491d8f7
+size 1074384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..f69e0e3
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fa50f9021ae593bb37b95472a999215574ae56f63fdc5ff6edfd74b298826059
+size 778384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..8d1b5bf
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:316ab10d2dae65040a78f85182635ea3b7b1f57859ef0b568ccee58d88d58129
+size 227384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..03e75a0
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e41ea39ce8cb6673fc33b3e33e0fe2af9e82158006fccf6355ee3e49bfc5db8b
+size 689584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf
new file mode 100644
index 0000000..3d1b6cd
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_g20_right.urdf
@@ -0,0 +1,1285 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_l25_right.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_l25_right.urdf
new file mode 120000
index 0000000..6a0e843
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/linkerhand_l25_right.urdf
@@ -0,0 +1 @@
+linkerhand_g20_right.urdf
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/hand_base_link.STL
new file mode 100644
index 0000000..bad7f8a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:79e36d4c7e9282c79601976d41144557d1928810202589e31bed335f3d308cd1
+size 3433184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/index_distal.STL
new file mode 100644
index 0000000..17066a2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:324aa4b852e0c469bfc0c0bb9b19e0ccffc7dcb401d6a678481340588465fa26
+size 984884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/index_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/index_metacarpals.STL
new file mode 100644
index 0000000..316b088
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/index_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:106ad58d52420c3216ccb87865f55062d4b4b558078dda95210a085a9bbe277c
+size 151784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/index_middle.STL
new file mode 100644
index 0000000..f7d3da0
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:27f9ccd91c5bdb1f6a1c42aeee4ab79e7e0aaf1e124a36999c75715b70d0aee6
+size 805584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/index_proximal.STL
new file mode 100644
index 0000000..8b6beb2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d4f85e69dd94d56bdc4646c945ca8a237c652fd5dffba86c7b9ce1d8067ae8e1
+size 883184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/middle_distal.STL
new file mode 100644
index 0000000..f50ce2b
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:688be3d9dd7fda49a8f9dbba824fedef5cf427c94e2e9e1ea96834eafbf6945a
+size 984884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/middle_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/middle_metacarpals.STL
new file mode 100644
index 0000000..becb768
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/middle_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:65bc65257a01e0ccd284dfdf3b2a66f2a865bf80faeb4da746cd1fd05114fe40
+size 151784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/middle_middle.STL
new file mode 100644
index 0000000..7356bc9
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:600b8e7486abc8a2707a5ecc66d4b5bea1dc0895c7173e911ba994f5cac30c10
+size 805584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/middle_proximal.STL
new file mode 100644
index 0000000..7b3aab7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:edd3af02319cd4a3b4e07a6f5d33d501887d5ab78625489254f289785b6f57e1
+size 883184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/pinky_distal.STL
new file mode 100644
index 0000000..9322965
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3b22871d948ef80447f93866af3643ffa87cb22c1c172750fca9357f3aaa46de
+size 984884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/pinky_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/pinky_metacarpals.STL
new file mode 100644
index 0000000..b635c98
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/pinky_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b63d10a0e18af0169c75b85eb740c1f25123f6c4c5be0c5368afc65e8c73771f
+size 151784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/pinky_middle.STL
new file mode 100644
index 0000000..742f4ba
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c6696bfcd2dbef8f248f86a6f45a90d5ba7b40e11cdb13a86b96347240a7b751
+size 805584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..63a280f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:311ce029c4972530d3b234307f4ea6ca1789577c2786d56f969a5dfac2ef6a57
+size 883184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/ring_distal.STL
new file mode 100644
index 0000000..27f67a3
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fca7bf31fa3f377110d31f7623e27f8c7845faedbdca7c09f98c94a28867bd69
+size 984884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/ring_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/ring_metacarpals.STL
new file mode 100644
index 0000000..c1a329a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/ring_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:11797279afee7f856e472755d3a96593baea659d2ec0014edb02006ae48b5d71
+size 151784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/ring_middle.STL
new file mode 100644
index 0000000..cdf7967
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2b3a0815e8daf95e76c9f54f84c13709a3951aa44ec6bb863facf60ba32c3c16
+size 805584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/ring_proximal.STL
new file mode 100644
index 0000000..5bdd4f0
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:168df91e5a5643a899556f96ca3f3b7fe8a3349436be42dfb5632c0d524f9a47
+size 883184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/thumb_distal.STL
new file mode 100644
index 0000000..0bc2c63
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:026dd2fac43c4461c5a90561ef29039f4a08802c0e47e5c422ecac2f34babe1e
+size 1108484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..6d6da06
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0a82998ece3c784bba7861b850d00ed483d93434f94f72c6298fea0917e328ca
+size 1158784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..9cea770
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8416e5f826f12c4597754fbe3052983635345a76e3d98a4cdbc38ed363a9da78
+size 852984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..a233baa
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a5679a28a612ab6aa62a4f7fa1c67be42f807bba7c2ca00ba062ca2e4ef0070d
+size 227884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..7b67077
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_right/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4cd7d6a283d2e5c3f6dfbd88aa1e18f9f481375d1e6b47f2c373a285184b123d
+size 716984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/linkerhand_l10_left.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/linkerhand_l10_left.urdf
new file mode 100644
index 0000000..fbc114f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/linkerhand_l10_left.urdf
@@ -0,0 +1,1247 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/hand_base_link.STL
new file mode 100644
index 0000000..0c834c9
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b8f4aab969290782bdef41bba895da961e064859cb8ea0c961f73fa9e15244fb
+size 3044984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/index_distal.STL
new file mode 100644
index 0000000..80cc470
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c427d846a9b2b74ac82d02526d74ad5469275104c22d812c565a6e33751c82ba
+size 760984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/index_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/index_metacarpals.STL
new file mode 100644
index 0000000..58a5de6
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/index_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a94c16c53a8d97fec1c38050124a68a3dd344ded225ca5d1ffe05d3ef729ea6f
+size 592384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/index_middle.STL
new file mode 100644
index 0000000..3caff38
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4f1d4210b92f0ded3ca8ec4202c987dca4275342617e5c22c42e5197e1246a45
+size 452884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/index_proximal.STL
new file mode 100644
index 0000000..d9bf812
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1bbdf524df8097956e756c173502405bb660ae3c7d4fbe8f97759c1ffc78efce
+size 712584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/middle_distal.STL
new file mode 100644
index 0000000..1b75f51
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:88f6184d9212dddc63810c2d1981a0c5b328f19daecb2739d9b239d28bc83c23
+size 760984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/middle_middle.STL
new file mode 100644
index 0000000..af6db89
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c09bb5e5cd20c22c98107d1ec6480bec0d1e15ae3ec201ecd82ed06df3ba8be6
+size 452884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/middle_proximal.STL
new file mode 100644
index 0000000..667bede
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5782bde08b82f452ed6630cdf534d9b24c10344fb10c56c945a111be05fa159a
+size 712584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/pinky_distal.STL
new file mode 100644
index 0000000..d02551b
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8aba2dd8bee8befec7e35bcb498446164ee7fe5dac4e5b1f47bbbe1d49014d17
+size 760984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/pinky_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/pinky_metacarpals.STL
new file mode 100644
index 0000000..4f7f2b4
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/pinky_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a6578a0cf7e853f62eb7bca8b491327291177566c104fe96a84e91f3967e32d3
+size 592384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/pinky_middle.STL
new file mode 100644
index 0000000..c658943
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6fa41206aa8e68ba19a8ab105a66a7f88eb29c0c649960149b2cf8fa5e2b265f
+size 452884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..2531781
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a03eee073016a121b4307455c34d348464d933957c574efa06d2046081efc03d
+size 712584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/ring_distal.STL
new file mode 100644
index 0000000..319691b
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3bfefd65ff29f0a58526c7667cf3a7dd28bb16815d7e314947bf111f24856d7d
+size 760984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/ring_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/ring_metacarpals.STL
new file mode 100644
index 0000000..3e40e78
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/ring_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fbb35b3af6b48595dc56329edaf59033c07d497e52589075593bf78e03e1dea3
+size 592384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/ring_middle.STL
new file mode 100644
index 0000000..eea1025
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c8fe3b577a5dd55b8ef661ca9a553d52f5ce20edd837c947c905d2da79e4fe4b
+size 452884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/ring_proximal.STL
new file mode 100644
index 0000000..e40b0f7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8d9e08c8b0913d44344023987633a0436157a24daba4cdbab7e6c3893805ece0
+size 712584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/thumb_distal.STL
new file mode 100644
index 0000000..9236e34
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c0abd49ef4b526d706e23bb8d6bc22e49a45fc2c87c5722730ec4bcd6b29eb73
+size 710984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..6f1e86e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9e66ddb5d88857458ce365e6c178512e4a1c1d8d273696a74455d6979dc73eb3
+size 2396384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..b2b8900
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5e13dd4dacbab84c01fc9eec8f10245d74e79ba084b9039621c0150bd385fef1
+size 609084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..4061a65
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e8c88a0f89332580387018acb71661a5425647821569b2e0a46662413c5e5d20
+size 738984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..0a247e8
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_left/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:bb338e6b09ff6ea3173fa5109bc7d2597c4746d23573655cdd1a038cbeb8b96a
+size 1000884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/linkerhand_l10_right.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/linkerhand_l10_right.urdf
new file mode 100644
index 0000000..85b7010
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/linkerhand_l10_right.urdf
@@ -0,0 +1,1247 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/hand_base_link.STL
new file mode 100644
index 0000000..3535436
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7f4c85cd3f42d528be298842411aa23e076bcbe08f7d3781dbbe63c05bc87e19
+size 3077684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/index_distal.STL
new file mode 100644
index 0000000..639c4b4
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f70beb1e64b11cbe4f3155756d41758003b9cf74c2ffcbe6cbf2a165249c4580
+size 761684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/index_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/index_metacarpals.STL
new file mode 100644
index 0000000..15a9d6e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/index_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4df918b55a4ccee8f20c28144d44e149e5a7092da7381399cc548889a4815b9d
+size 590884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/index_middle.STL
new file mode 100644
index 0000000..8d1a74c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4aaa499e01211455a47520599197e4e99802d4cfe833b2671ff43d6660d4e73f
+size 453084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/index_proximal.STL
new file mode 100644
index 0000000..b4d77ac
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:46ec99aa5f8b75a40073917059211581a6a4b6c330f562c0d16a67400d986dbf
+size 712484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/middle_distal.STL
new file mode 100644
index 0000000..73955d3
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:32e935bab5fc927d034162fd41f912fdd4a63a20212d0120ce9d20d5160f3ad7
+size 761684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/middle_middle.STL
new file mode 100644
index 0000000..40e8dc7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:63ca996e5d77138650262cded44dcbcc08ce56b22067c81ae24eff066e5389a1
+size 453084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/middle_proximal.STL
new file mode 100644
index 0000000..0c88c92
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:11f9012bf1c8419c314cfab146cd9fe5561f4a3fc44215817ffc0cc4ad8e36e3
+size 712484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/pinky_distal.STL
new file mode 100644
index 0000000..af627ac
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d28d017411c0d661d0fc8faddc5b2899eb4835fb6b1d32a730d5b8f59905bb0f
+size 761684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/pinky_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/pinky_metacarpals.STL
new file mode 100644
index 0000000..d93b735
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/pinky_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:03bbde451db4552660b3a6be3a04e4ebc1cfa7665eda3f766f5491693d88b674
+size 590884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/pinky_middle.STL
new file mode 100644
index 0000000..a93dc2d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a419d439dfba53b31fb81f6811986a5baadf92462dcdb14473dc699489c0c5be
+size 453084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..8c13933
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a0b084a3eb8413f2e03c3d6239eb02bfd7797f92599aa27a7f7105f29d3d5f11
+size 712484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/ring_distal.STL
new file mode 100644
index 0000000..d813866
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e5a100437a2d9b4ae84cb352332f7b56f307ac72f29f64814d1634b8a5b39fa3
+size 761684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/ring_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/ring_metacarpals.STL
new file mode 100644
index 0000000..300835a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/ring_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6dbb7cc4f748359a57efbf9eee7c95741811e4d39480aecbf80471bad46f786f
+size 590884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/ring_middle.STL
new file mode 100644
index 0000000..846db32
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0f4d1359769929ff8cb30011a994dadad43c6fd94f40f2ca6c0e4dc0fac4d411
+size 453084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/ring_proximal.STL
new file mode 100644
index 0000000..803948f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3ff7606920b721f58d219a2c18a1638d3e22bc591ed1849593d16ba7e02c8aa6
+size 712484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/thumb_distal.STL
new file mode 100644
index 0000000..e303a5d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8801e5ad9301261f1d20f376ac6655018a126bbc7c9c5d8e103f27b2e66140ea
+size 707984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..884eb6e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b6746407382e281dff3472bc779a5eb25da2edabf15b2d600ec9d9c6dd5f144d
+size 2394084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..3b262f1
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:671daf4e7e1301a7556f20321ee27c517fb66d2cb47996482fdea633a97e03dd
+size 609084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..f5da1bc
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:dc071aeef125f0e63e7fcbc5153979285674f2caa47520167e89fe9186be5f4a
+size 732584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..8f75126
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10_right/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8661742c275f9fdc9d3d8bef554d4bd8103e4e87a40f3957ffc4acd6bb6d50e2
+size 999184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/linkerhand_l10v6_left.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/linkerhand_l10v6_left.urdf
new file mode 100644
index 0000000..eff1aaf
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/linkerhand_l10v6_left.urdf
@@ -0,0 +1,615 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/hand_base_link.STL
new file mode 100644
index 0000000..83c577f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a1f40029aed4e7940033a4d0bb609266639887e7531dbace3f9c596882d47137
+size 2581084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/index_distal.STL
new file mode 100644
index 0000000..c8d2138
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1e1663992b6f4c8f064cbbcfc96b41437ea891772ac311c2185bdb2fdf7d4b90
+size 606184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/index_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/index_metacarpals.STL
new file mode 100644
index 0000000..4d3bb1a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/index_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3795f2e3ea20ea7e118d7d8da6728bef84779c2c8b64607a0b3d415d7c537cae
+size 417584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/index_middle.STL
new file mode 100644
index 0000000..15b6e2f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a49bfaaa26de0b2b13370aee9c032994603a948e762c98778475b1dfd0e34786
+size 513684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/index_proximal.STL
new file mode 100644
index 0000000..cfb2cad
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:13017fc3020bfe84d5fe0bb6bfda17b8010478caf0e2121353774651a2208450
+size 508184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/middle_distal.STL
new file mode 100644
index 0000000..561dda5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1486c93fc1a6318984d16340687267370fef8347fc051e1392a390d6b8151898
+size 606184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/middle_middle.STL
new file mode 100644
index 0000000..1a9c042
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:24443eebca3b5998cd8c349230414e1139ce1b90cf7bec5632838e8ea8865c6c
+size 513684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/middle_proximal.STL
new file mode 100644
index 0000000..c66b3e4
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:32ed328d562107277aefb99f5eafc0006c9e377dfbd0ef53239f0e06d0920cd7
+size 508184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/pinky_distal.STL
new file mode 100644
index 0000000..3558e93
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d899644613a0ee5fc1ca1d099dea0890ba5c9cfa4d90f588d13e88062078e546
+size 606184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/pinky_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/pinky_metacarpals.STL
new file mode 100644
index 0000000..6b79447
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/pinky_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:067393f773b05aaf72ce829141f90227a47f13c753caf1b9e8cb1d20749a1310
+size 417584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/pinky_middle.STL
new file mode 100644
index 0000000..486df78
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b039f0d29004ad5a8f8327dfe431a14567b3f9b284a4fc09adef6697193fc22c
+size 513684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..3efa373
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7f5e8b450e8df141a499c9f17f2c7f6ee5bfba143167bb934b5893d91143608a
+size 508184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/ring_distal.STL
new file mode 100644
index 0000000..c4804e7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1bd1c4b9858b6de4b441c861028b6cf6461b5ea7bc13e513c80c0a8b0c0e4b43
+size 606184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/ring_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/ring_metacarpals.STL
new file mode 100644
index 0000000..cd7ced7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/ring_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:72a66f31ef2b8c13c82079baf403cfbb46fe8953fcd36616384be4f6aff619c2
+size 417584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/ring_middle.STL
new file mode 100644
index 0000000..8330c8e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:01df82680ae5aa262c3bf1467784652b71c67da194e84e173d8c0add7fce39b2
+size 513684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/ring_proximal.STL
new file mode 100644
index 0000000..e5744b9
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7af1fb92710af5ed1053830c5ce8100c90a74b9ff83facdd5964c3df6f3bab42
+size 508184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/thumb_distal.STL
new file mode 100644
index 0000000..1e5e755
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6068aec7ca380dd5da808fd98f9d6b6c34dd2dcde9aff7da7bddbf7c72a660f0
+size 626084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..88483d6
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:98948f8383621736399e27a91645b3b2759959a6cfdd31a70a1c9b18caa95e6a
+size 1263484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..de34a8b
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:bb36d278a95e6c73365cf73e87b341579ff42f2b8836c848f717eada2704b52f
+size 383984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..691f544
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a3b08fc0b7df3dc90124b833c1fe2ab8525e2df04cd608756d493902077aed95
+size 1216684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..d366cfc
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_left/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5e09371264a72cacac13dd2124a7e72c44d5a3fbf11a14f63683cc8061f501fd
+size 689134
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/linkerhand_l10v6_right.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/linkerhand_l10v6_right.urdf
new file mode 100644
index 0000000..e411b5d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/linkerhand_l10v6_right.urdf
@@ -0,0 +1,616 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/hand_base_link.STL
new file mode 100644
index 0000000..5e7a915
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2fd6f75ae68def12f0fb0e76d6aac231cb3f4499c3e69aa7aeee011bf5b23f8d
+size 2583484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/index_distal.STL
new file mode 100644
index 0000000..23ea994
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b21429401840dab48cc14f8500caff1914eb305e576bfa3b68fe8a60d41b7a00
+size 606384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/index_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/index_metacarpals.STL
new file mode 100644
index 0000000..3c00745
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/index_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:daa2a724e03492133b8acec9c4aba3bf7c28c925806a330e771a15063eeabde6
+size 416784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/index_middle.STL
new file mode 100644
index 0000000..4c28ab2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e796a31b2ec62b92aece6dd9281f6d615bed2038fdb3ece6e319e64b6a5682af
+size 514184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/index_proximal.STL
new file mode 100644
index 0000000..f311d7c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:237d4b069cf6947e720de6ee51efb8c3295072d4f323f51361a455275a0be5e3
+size 507584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/middle_distal.STL
new file mode 100644
index 0000000..84049b2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:56cb386b56488726b8d842c923ab170c8e120224372e73f41613d94a9015ce07
+size 606384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/middle_middle.STL
new file mode 100644
index 0000000..764b550
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b5dec0248059db034d488879dce4f9c4da9a0c873a8d7c455495d16028a63b4a
+size 514184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/middle_proximal.STL
new file mode 100644
index 0000000..bbefebc
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e3c1198a55a072a71c0ade4a65ed1d8cd5a4c7f621b8edff4ce2becd36330a45
+size 507584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/pinky_distal.STL
new file mode 100644
index 0000000..ccdf632
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:81aada4caab3ace4abbe9344c0339941fb743f1b674c42b9b38e091e02b064eb
+size 606384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/pinky_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/pinky_metacarpals.STL
new file mode 100644
index 0000000..77071fa
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/pinky_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9ef8c5e38b47a9496d7b1c42de19f2abe8bd1d5cc5877685f0e33879ca1208ea
+size 416784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/pinky_middle.STL
new file mode 100644
index 0000000..68b62cb
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:88c83d9802e81df053fad93c0e469603131a8fc82101635133afa5b513dc945c
+size 514184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..ab62333
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:14e6aff5f396e5255d0e7421b6ac5e06f91e79dbf47317984bc21d2a002ec44a
+size 507584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/ring_distal.STL
new file mode 100644
index 0000000..7812cef
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9278681278f05e5c41461ca0719c9161848ad79001b196e8918bf5e36ffc6cff
+size 606384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/ring_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/ring_metacarpals.STL
new file mode 100644
index 0000000..d9b5fc6
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/ring_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f1d73d0e155880b8409c382718ccb3024655d2b57becf98149c882d973a101c0
+size 416784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/ring_middle.STL
new file mode 100644
index 0000000..a250f2a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:96e95ac7b5ccdbf65f93eaf5a9db1cf6c284c16aa939ef8004c1455f3ea1eb8b
+size 514184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/ring_proximal.STL
new file mode 100644
index 0000000..76aa820
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:aaf1192e1a387b3bd64a8e26d7c45ac1f4620f475b6402029b860e7838483b9b
+size 507584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/thumb_distal.STL
new file mode 100644
index 0000000..e3ed7c4
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:87267c48b907e9d1bd1a1d323bcb7571ac5a078694b1954c4b74894142eedd82
+size 624284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..e7bda81
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:77819a1f80b336c5efd03512ce5760e1172f7cf2734c3fd9e2a2f660599fb680
+size 1263184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..3140d67
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fa6ff0bac6f0846868113f3fa844607ab2c9e2094e80c1fbf19e8e585634af80
+size 384984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..21c372b
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5ceb387ac3e3b2bd6f9f971393c5daa1308d24c7cc6c1a575a2bbfbb8e2a02e0
+size 369084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..821e4f7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v6_right/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:241d5309db0347510ceef365aa2a98e8bd4b4671a92fd87c84ed8e880215b37c
+size 687534
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/linkerhand_l10v7_left.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/linkerhand_l10v7_left.urdf
new file mode 100644
index 0000000..fbc114f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/linkerhand_l10v7_left.urdf
@@ -0,0 +1,1247 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/hand_base_link.STL
new file mode 100644
index 0000000..0c834c9
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b8f4aab969290782bdef41bba895da961e064859cb8ea0c961f73fa9e15244fb
+size 3044984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/index_distal.STL
new file mode 100644
index 0000000..80cc470
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c427d846a9b2b74ac82d02526d74ad5469275104c22d812c565a6e33751c82ba
+size 760984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/index_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/index_metacarpals.STL
new file mode 100644
index 0000000..58a5de6
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/index_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a94c16c53a8d97fec1c38050124a68a3dd344ded225ca5d1ffe05d3ef729ea6f
+size 592384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/index_middle.STL
new file mode 100644
index 0000000..3caff38
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4f1d4210b92f0ded3ca8ec4202c987dca4275342617e5c22c42e5197e1246a45
+size 452884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/index_proximal.STL
new file mode 100644
index 0000000..d9bf812
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1bbdf524df8097956e756c173502405bb660ae3c7d4fbe8f97759c1ffc78efce
+size 712584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/middle_distal.STL
new file mode 100644
index 0000000..1b75f51
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:88f6184d9212dddc63810c2d1981a0c5b328f19daecb2739d9b239d28bc83c23
+size 760984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/middle_middle.STL
new file mode 100644
index 0000000..af6db89
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c09bb5e5cd20c22c98107d1ec6480bec0d1e15ae3ec201ecd82ed06df3ba8be6
+size 452884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/middle_proximal.STL
new file mode 100644
index 0000000..667bede
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5782bde08b82f452ed6630cdf534d9b24c10344fb10c56c945a111be05fa159a
+size 712584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/pinky_distal.STL
new file mode 100644
index 0000000..d02551b
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8aba2dd8bee8befec7e35bcb498446164ee7fe5dac4e5b1f47bbbe1d49014d17
+size 760984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/pinky_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/pinky_metacarpals.STL
new file mode 100644
index 0000000..4f7f2b4
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/pinky_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a6578a0cf7e853f62eb7bca8b491327291177566c104fe96a84e91f3967e32d3
+size 592384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/pinky_middle.STL
new file mode 100644
index 0000000..c658943
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6fa41206aa8e68ba19a8ab105a66a7f88eb29c0c649960149b2cf8fa5e2b265f
+size 452884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..2531781
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a03eee073016a121b4307455c34d348464d933957c574efa06d2046081efc03d
+size 712584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/ring_distal.STL
new file mode 100644
index 0000000..319691b
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3bfefd65ff29f0a58526c7667cf3a7dd28bb16815d7e314947bf111f24856d7d
+size 760984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/ring_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/ring_metacarpals.STL
new file mode 100644
index 0000000..3e40e78
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/ring_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fbb35b3af6b48595dc56329edaf59033c07d497e52589075593bf78e03e1dea3
+size 592384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/ring_middle.STL
new file mode 100644
index 0000000..eea1025
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c8fe3b577a5dd55b8ef661ca9a553d52f5ce20edd837c947c905d2da79e4fe4b
+size 452884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/ring_proximal.STL
new file mode 100644
index 0000000..e40b0f7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8d9e08c8b0913d44344023987633a0436157a24daba4cdbab7e6c3893805ece0
+size 712584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/thumb_distal.STL
new file mode 100644
index 0000000..9236e34
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c0abd49ef4b526d706e23bb8d6bc22e49a45fc2c87c5722730ec4bcd6b29eb73
+size 710984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..6f1e86e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9e66ddb5d88857458ce365e6c178512e4a1c1d8d273696a74455d6979dc73eb3
+size 2396384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..b2b8900
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5e13dd4dacbab84c01fc9eec8f10245d74e79ba084b9039621c0150bd385fef1
+size 609084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..4061a65
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e8c88a0f89332580387018acb71661a5425647821569b2e0a46662413c5e5d20
+size 738984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..0a247e8
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_left/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:bb338e6b09ff6ea3173fa5109bc7d2597c4746d23573655cdd1a038cbeb8b96a
+size 1000884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/linkerhand_l10v7_right.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/linkerhand_l10v7_right.urdf
new file mode 100644
index 0000000..85b7010
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/linkerhand_l10v7_right.urdf
@@ -0,0 +1,1247 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/hand_base_link.STL
new file mode 100644
index 0000000..3535436
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7f4c85cd3f42d528be298842411aa23e076bcbe08f7d3781dbbe63c05bc87e19
+size 3077684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/index_distal.STL
new file mode 100644
index 0000000..639c4b4
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f70beb1e64b11cbe4f3155756d41758003b9cf74c2ffcbe6cbf2a165249c4580
+size 761684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/index_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/index_metacarpals.STL
new file mode 100644
index 0000000..15a9d6e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/index_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4df918b55a4ccee8f20c28144d44e149e5a7092da7381399cc548889a4815b9d
+size 590884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/index_middle.STL
new file mode 100644
index 0000000..8d1a74c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4aaa499e01211455a47520599197e4e99802d4cfe833b2671ff43d6660d4e73f
+size 453084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/index_proximal.STL
new file mode 100644
index 0000000..b4d77ac
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:46ec99aa5f8b75a40073917059211581a6a4b6c330f562c0d16a67400d986dbf
+size 712484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/middle_distal.STL
new file mode 100644
index 0000000..73955d3
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:32e935bab5fc927d034162fd41f912fdd4a63a20212d0120ce9d20d5160f3ad7
+size 761684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/middle_middle.STL
new file mode 100644
index 0000000..40e8dc7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:63ca996e5d77138650262cded44dcbcc08ce56b22067c81ae24eff066e5389a1
+size 453084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/middle_proximal.STL
new file mode 100644
index 0000000..0c88c92
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:11f9012bf1c8419c314cfab146cd9fe5561f4a3fc44215817ffc0cc4ad8e36e3
+size 712484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/pinky_distal.STL
new file mode 100644
index 0000000..af627ac
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d28d017411c0d661d0fc8faddc5b2899eb4835fb6b1d32a730d5b8f59905bb0f
+size 761684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/pinky_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/pinky_metacarpals.STL
new file mode 100644
index 0000000..d93b735
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/pinky_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:03bbde451db4552660b3a6be3a04e4ebc1cfa7665eda3f766f5491693d88b674
+size 590884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/pinky_middle.STL
new file mode 100644
index 0000000..a93dc2d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a419d439dfba53b31fb81f6811986a5baadf92462dcdb14473dc699489c0c5be
+size 453084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..8c13933
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a0b084a3eb8413f2e03c3d6239eb02bfd7797f92599aa27a7f7105f29d3d5f11
+size 712484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/ring_distal.STL
new file mode 100644
index 0000000..d813866
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e5a100437a2d9b4ae84cb352332f7b56f307ac72f29f64814d1634b8a5b39fa3
+size 761684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/ring_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/ring_metacarpals.STL
new file mode 100644
index 0000000..300835a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/ring_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6dbb7cc4f748359a57efbf9eee7c95741811e4d39480aecbf80471bad46f786f
+size 590884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/ring_middle.STL
new file mode 100644
index 0000000..846db32
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0f4d1359769929ff8cb30011a994dadad43c6fd94f40f2ca6c0e4dc0fac4d411
+size 453084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/ring_proximal.STL
new file mode 100644
index 0000000..803948f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3ff7606920b721f58d219a2c18a1638d3e22bc591ed1849593d16ba7e02c8aa6
+size 712484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/thumb_distal.STL
new file mode 100644
index 0000000..e303a5d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8801e5ad9301261f1d20f376ac6655018a126bbc7c9c5d8e103f27b2e66140ea
+size 707984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..884eb6e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b6746407382e281dff3472bc779a5eb25da2edabf15b2d600ec9d9c6dd5f144d
+size 2394084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..3b262f1
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:671daf4e7e1301a7556f20321ee27c517fb66d2cb47996482fdea633a97e03dd
+size 609084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..f5da1bc
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:dc071aeef125f0e63e7fcbc5153979285674f2caa47520167e89fe9186be5f4a
+size 732584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..8f75126
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l10v7_right/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8661742c275f9fdc9d3d8bef554d4bd8103e4e87a40f3957ffc4acd6bb6d50e2
+size 999184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/linkerhand_l20_left.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/linkerhand_l20_left.urdf
new file mode 100644
index 0000000..3105751
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/linkerhand_l20_left.urdf
@@ -0,0 +1,1536 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/base_link.STL
new file mode 100644
index 0000000..113acdf
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:36032fc64c2759d21b3297a59429015e8b8f3de45de2aa53561b1481e63baa7d
+size 1318484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/index_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/index_link0.STL
new file mode 100644
index 0000000..50ab66c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/index_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:34f25900c3fd06d874d44993446fe198bc221d4b84c4c22fcab962482443eec9
+size 69384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/index_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/index_link1.STL
new file mode 100644
index 0000000..797d714
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/index_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d638ad9fa7c2a4a8db33d831fb012251897a9e5592be3d7c9955e29930c0f75f
+size 953184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/index_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/index_link2.STL
new file mode 100644
index 0000000..0a5093e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/index_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4e590cbd0ed18c7c3a575aadda5c8a2414c3a56c17e06536da0fc76b6cdea463
+size 884884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/index_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/index_link3.STL
new file mode 100644
index 0000000..3118860
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/index_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ddfcdd3f2b5a605722b4693c1909c220214f43772c2a72d2bfa5ba8b625c2e03
+size 964984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/index_link4.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/index_link4.STL
new file mode 100644
index 0000000..22d6034
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/index_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:de9f192a5bb842176719a2dd044925b2c25042c89d654d453cd155eff5018603
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/little_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/little_link0.STL
new file mode 100644
index 0000000..5734316
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/little_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:79e2fdef62fcc0b7fbb10038a02bcc271cc92d2ad15fcca5d80db76deaa2557e
+size 69384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/little_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/little_link1.STL
new file mode 100644
index 0000000..1823f7a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/little_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:120c49d3a35b603cfdaedbb42bae03e9a48bff0eaef0f9c80b7ebfc6ed068409
+size 953184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/little_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/little_link2.STL
new file mode 100644
index 0000000..e218dcc
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/little_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:24610b3f72455c7b5b22a9ad8fc31c6062cc5215cea2e56f88a61964335ee0d8
+size 884884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/little_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/little_link3.STL
new file mode 100644
index 0000000..cfdc1cc
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/little_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3e7b3ae67baa9a4b74d92a1cffe26039bc6e02a472ea0077d73803ae675b9e75
+size 964984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/little_link4.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/little_link4.STL
new file mode 100644
index 0000000..820682c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/little_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:88e0f66b7ffe5c6f5f955cdeea3f8532afd65985dc07c3e8823f35412e823b13
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/middle_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/middle_link0.STL
new file mode 100644
index 0000000..ac63ca2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/middle_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b72b0a97bea433e9e7e10128027f34d3eae928792424476bbbfd57d4e084a84d
+size 69384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/middle_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/middle_link1.STL
new file mode 100644
index 0000000..fd7da3c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/middle_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7ecb4635a36d9af4ef5a26be64fd85e447f10eace4ca2426709f0a023cccdd76
+size 953184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/middle_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/middle_link2.STL
new file mode 100644
index 0000000..e5f7616
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/middle_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:44816865bd111ee423d0754efff4247c504c02efee8cbb86fb7c7b54ce3eb3f4
+size 884884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/middle_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/middle_link3.STL
new file mode 100644
index 0000000..94cc874
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/middle_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b17ca9d2a0fe2281f45c6cf46a76d7afab0fd7a72303fe566f82d6418cf2c4cb
+size 964984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/middle_link4.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/middle_link4.STL
new file mode 100644
index 0000000..a7f4175
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/middle_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f4760d7c9d75b30ec198a8604e368aa5c643272d77ebb557f6d86613f999430d
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/ring_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/ring_link0.STL
new file mode 100644
index 0000000..abce9ba
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/ring_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:07b19ee8c4a0b83d22e05b3603dec5ff17bcafb527695a2126b157e93deaf5e1
+size 69384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/ring_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/ring_link1.STL
new file mode 100644
index 0000000..dd50307
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/ring_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2f7a7f80d8d81e78c7bb6b28bae2f53ad35aeceeda638614c84ba73fb25f1159
+size 953184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/ring_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/ring_link2.STL
new file mode 100644
index 0000000..e218dcc
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/ring_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:24610b3f72455c7b5b22a9ad8fc31c6062cc5215cea2e56f88a61964335ee0d8
+size 884884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/ring_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/ring_link3.STL
new file mode 100644
index 0000000..fe2abaa
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/ring_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d13663b39464ccce25d09fa6e09e6b37899010edf35d14c60547e2f49f544989
+size 964984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/ring_link4.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/ring_link4.STL
new file mode 100644
index 0000000..8faae23
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/ring_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:759435162e5690bdc3eddf738ca532e48555972c03eed39c429b26efdad185b7
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/thumb_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/thumb_link0.STL
new file mode 100644
index 0000000..fcff4bd
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/thumb_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c12be7106618f74b18765743c5a8cf4f4bda2d323fdaab4eed8ff1cfec3199cf
+size 129384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/thumb_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/thumb_link1.STL
new file mode 100644
index 0000000..11dbcd5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/thumb_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:584d309e9a1de6d69d346279f6176a924ae0449a34df2398e5a119967e004726
+size 82484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/thumb_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/thumb_link2.STL
new file mode 100644
index 0000000..261a183
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/thumb_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1275fbad6d563aecb647d31407664f1d9cb75d04e2f5e8a037d7b9b338e9a8dc
+size 748684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/thumb_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/thumb_link3.STL
new file mode 100644
index 0000000..77cf70a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/thumb_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e3a7b7da64944f736d927df90b3b58a22612cd883a3ffdb098d6d6dc47e2a963
+size 342484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/thumb_link4.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/thumb_link4.STL
new file mode 100644
index 0000000..44b4a66
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/thumb_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3f8fbfa60d8770c0149530705ab8a8b2578ba48f5705f98b4b08871fbd7b4de2
+size 1101184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/thumb_link5.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/thumb_link5.STL
new file mode 100644
index 0000000..72fe017
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_left/meshes/thumb_link5.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:cbad1b069f357d36d1bffa61301c9fbb3065df68a216c119e3dee98925897289
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/linkerhand_l20_right.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/linkerhand_l20_right.urdf
new file mode 100644
index 0000000..69f3552
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/linkerhand_l20_right.urdf
@@ -0,0 +1,1285 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/base_link.STL
new file mode 100644
index 0000000..56853cd
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0eb91de436f6a8697d5c80c8c01bd8037c0c5c8d1f96d8f1c731bd592cd90c2d
+size 3185884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_distal.STL
new file mode 100644
index 0000000..a29fcb1
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:72f89c70dec87bf8712a17ce33f2f2425d2a3874d92af3c844411e71b8af05a6
+size 631384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_link0.STL
new file mode 100644
index 0000000..525cc39
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ab783c0338c7972d7397d88f29b688596353ad03789facc807ae0613f9a9861c
+size 69284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_link1.STL
new file mode 100644
index 0000000..894a556
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a4e16722dcf166e6005fd80d1faffdbbf9df64b9ae376f7891ae8f703fd19e3e
+size 953284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_link2.STL
new file mode 100644
index 0000000..fd450b3
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7216c7c71d57c9edcbcb01fcdfd249573a58903c172120293db7556b833b61e8
+size 882784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_link3.STL
new file mode 100644
index 0000000..e1ea37f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:447924798632d089a6900540ebae4ab5be7b93b3ad67915eafed511d5131a0c7
+size 963384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_link4.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_link4.STL
new file mode 100644
index 0000000..3c62495
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:37e920c77ee1ca58c90dc3549800c2f20e847e4a78dcd8f8599ea708ee0efbbc
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_metacarpals.STL
new file mode 100644
index 0000000..c0425f8
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:91757b688a6354112fdb84a1d4b952d801a53a4db13647d345f08200a7702bf2
+size 67484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_middle.STL
new file mode 100644
index 0000000..efc219c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ea2fb800981ba9ce1bf04f3315dd665ef89be4512dea91ad118cd8f4135d7e17
+size 660484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_proximal.STL
new file mode 100644
index 0000000..4a5a2ce
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2accc60d0e9d2e937b12103c3a6abd6e2b5d975bb7bc146b5962586e1acbdee6
+size 632484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/little_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/little_link0.STL
new file mode 100644
index 0000000..fa79a75
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/little_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b261557ad5a447cfee6e6c70a209a97b0392d31f175bd2a1afabf03a5acae1e3
+size 69284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/little_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/little_link1.STL
new file mode 100644
index 0000000..f4f2106
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/little_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:03a814cabbc9465734338c685f8c2f4d4abc72be942ec5b2560085335cc593da
+size 953284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/little_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/little_link2.STL
new file mode 100644
index 0000000..bbbe999
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/little_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:940e6cdc2ef76ee21caa2aefda27e228d3d6e7dab7fbaf0e590aaf6ee1b2c0ee
+size 882784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/little_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/little_link3.STL
new file mode 100644
index 0000000..e1ea37f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/little_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:447924798632d089a6900540ebae4ab5be7b93b3ad67915eafed511d5131a0c7
+size 963384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/little_link4.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/little_link4.STL
new file mode 100644
index 0000000..fcd1492
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/little_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b67260805deeaec089c146fb10886ada742d7dec7ecd8c6844a10e20345d39a7
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_distal.STL
new file mode 100644
index 0000000..05e1730
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e8d77445adfcbb549717787478727184f969d1866a6f2eacd66a44e442cb582e
+size 631184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_link0.STL
new file mode 100644
index 0000000..3d7a594
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:11111b64d5158c8fdc66f504691c87066f3d0b75fe5185934893b7a306b730e5
+size 69284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_link1.STL
new file mode 100644
index 0000000..3153337
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f7a2420b05cbe5a6eec8abccb6428922dcec64bf666e12cf2bf56f27419d8375
+size 953284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_link2.STL
new file mode 100644
index 0000000..2ab0e20
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:357f2271df50349b5e25f9629744c9917893f3181021b44087b6e9310a66d0d8
+size 882784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_link3.STL
new file mode 100644
index 0000000..e1ea37f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:447924798632d089a6900540ebae4ab5be7b93b3ad67915eafed511d5131a0c7
+size 963384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_link4.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_link4.STL
new file mode 100644
index 0000000..23e5fa4
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f141b7e2e9ad790316576d7a992e14b446a90d28330e57480e92aac8d609253c
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_metacarpals.STL
new file mode 100644
index 0000000..13f67f1
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:01e4f33ca96dcf965e3649a6bbe69870472fca64b6f01e46737197c406cce41f
+size 67484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_middle.STL
new file mode 100644
index 0000000..1e834b2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f72ccaafa9809a8646d93734f5993b69a3bcdc3fbd91be0685fa2deaf723ef70
+size 659134
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_proximal.STL
new file mode 100644
index 0000000..3419e63
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:37db56ffd4cd64194c569f73d05a38053b74a9ec715b30ed166e33a126f78b6a
+size 632184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/pinky_distal.STL
new file mode 100644
index 0000000..68d59e4
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:15335ff2e102aed4fa1efc92fa51d414fc0d26ca027883131ff970984b329076
+size 636734
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/pinky_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/pinky_metacarpals.STL
new file mode 100644
index 0000000..41832f5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/pinky_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3fabd19f8a6a2ca7438f08e0f1f539a699d43e9fe908c414d7a70466afefa326
+size 67484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/pinky_middle.STL
new file mode 100644
index 0000000..aa3cba7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d1fc985850b5aa43d6a343fae53df6a13a932f756f11b62f5461c2744010dcc3
+size 658934
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..7e7a4c3
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:59a19a36283e0c23b078c44696ba8f76b78f2ae09418108217a3c9e6f82084f3
+size 631584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_distal.STL
new file mode 100644
index 0000000..f828940
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4dafa6c86200b2bd096dc0ce074e057c98648e709e0bc77566b65e74baeb882c
+size 631284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_link0.STL
new file mode 100644
index 0000000..fdb4991
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:067cda8408826e1ae33b914524f1c3add71215d9fbfcc1a3219d034751655e97
+size 69284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_link1.STL
new file mode 100644
index 0000000..726a1dd
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1c1bb48037e12c21eced4de0d816dba3eca7ce4074350551a51df5239bd8d300
+size 953284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_link2.STL
new file mode 100644
index 0000000..a0948d0
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9d524a4300a6bb74ed21ed7dfafe3e3ba078cc36d0e6e7bbec9bf812ccd7edb8
+size 882784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_link3.STL
new file mode 100644
index 0000000..e1ea37f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:447924798632d089a6900540ebae4ab5be7b93b3ad67915eafed511d5131a0c7
+size 963384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_link4.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_link4.STL
new file mode 100644
index 0000000..d0c2c90
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6aa46d40f2f6bc31cbfccbd918d5ae4a2066abde96239dc2e1427c2c2a988b19
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_metacarpals.STL
new file mode 100644
index 0000000..c0cff09
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a00004fa7c11ad685635b3e5b700fd07ab67a92a07a156528ea0599439338339
+size 67484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_middle.STL
new file mode 100644
index 0000000..c0581ae
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:64c45d873432cd44d7e832d04f4e95c4921f11f5e2ff0958893ad20d96170ddd
+size 660034
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_proximal.STL
new file mode 100644
index 0000000..7156737
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:cba62e5c8240648d30795e8bada56721128a660852fdf156d8075c93f10b32e0
+size 632584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_distal.STL
new file mode 100644
index 0000000..1e0777d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b650f164c1972cfc074211b197b59d8b9e21e572e1eff7a3315f557877e1e55a
+size 876934
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_link0.STL
new file mode 100644
index 0000000..d0a5306
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:766b60994e95f859573ba41e76cb1dc2b16f5ddc22d1e1bd3284787aece83bc0
+size 129384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_link1.STL
new file mode 100644
index 0000000..90a9f42
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2216255bbbe159f4546937e97ef9fc71ed10e7826633f214c0817bb09ccad81e
+size 82484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_link2.STL
new file mode 100644
index 0000000..ffa9dfe
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:32b264fe991d7f7c7adbe6e0b9129a99521c45197892f1bc6eddff2c560cfc9e
+size 751084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_link3.STL
new file mode 100644
index 0000000..49bf761
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1470810dfeb1e40acb1f0fe1850db55166f45018239eeb4cfc5db659c75630a5
+size 341284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_link4.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_link4.STL
new file mode 100644
index 0000000..1799c50
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:15878e8c96d192fa9cbaef77bae65a97b21132855698440d1aa0bf3d74cd1efd
+size 1101584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_link5.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_link5.STL
new file mode 100644
index 0000000..2bb674a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_link5.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:73de2617e13616040475356f60851f3a196b25154188696b1628360d37ff2ae5
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..529dc9b
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6cfa06cac194dca87629474d0c8fb2d02d5dcbe61f2e5bc028ac5f726968719c
+size 1083834
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..b87d843
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f11cd741d602cf75adc724565032b241a6113107ec239c10cf6d597bf0696dec
+size 862434
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..6b16091
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9de6affd15cf79f8db7217f04984cd1beefd0f5e16b20f2cabfbafcd53a8e185
+size 265584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..2e1de5f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l20_right/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:790e485a8ab33dd55809eb4c91c299c3cf8ea6e5d7e5689ff50e6a23e10891e2
+size 736434
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/linkerhand_l21_left.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/linkerhand_l21_left.urdf
new file mode 100644
index 0000000..49b7b7e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/linkerhand_l21_left.urdf
@@ -0,0 +1,1033 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/hand_base_link.STL
new file mode 100644
index 0000000..f2b9e05
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0bcc8322d1a83ccbb58a45afe5044231b80447acbdf94d6e35437c67bd76c2e9
+size 1239084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/index_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/index_metacarpals.STL
new file mode 100644
index 0000000..396921a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/index_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7866eb4f650a62ffcd6eab4449d8fb6b53064e955a79e85d09e9e26d27231424
+size 3741334
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/index_middle.STL
new file mode 100644
index 0000000..97dd64d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:49544c49e90b04a083bc90a0be71a31eb7c1e2b7fdaeee4895c1961638d7989a
+size 733984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/index_proximal.STL
new file mode 100644
index 0000000..c97f9ca
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d655b11c5ff1138b7330c8b60472f9b9117c8341a8e7ce07157e2b303be4c4a0
+size 678484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/middle_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/middle_metacarpals.STL
new file mode 100644
index 0000000..4306dc4
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/middle_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:53284a18c1210c3ce13a440817694ec6051451e97695a55f59b012044d324046
+size 3646134
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/middle_middle.STL
new file mode 100644
index 0000000..884c78f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:539f7e4a23d93fc407e846be78171b0a8c73e8663b75b829930029a7c9617e1f
+size 733484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/middle_proximal.STL
new file mode 100644
index 0000000..b24a595
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d2c71f9907c7ac7a03121541d4e379ed5fd818e41c706987d775307919c3f530
+size 664384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/pinky_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/pinky_metacarpals.STL
new file mode 100644
index 0000000..836e209
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/pinky_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9262e21886d43aac7d541eee47ab64d388ace38d1ab4f6854b5456f66feaa0c6
+size 3731484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/pinky_middle.STL
new file mode 100644
index 0000000..c182eab
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3181e60739a780920e84b96d986565fd348cc065705b49eb95612446b07e7052
+size 734884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..17660ae
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b022fd154bbd77d09c1636f92efc842f4a3fbb3017986d863ab04f5876fd2dcb
+size 652184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/ring_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/ring_metacarpals.STL
new file mode 100644
index 0000000..14c1241
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/ring_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7c309c792504d6b2c584aed3f13ebbc4a21d97fee69789e8a793b8b56addcfbc
+size 2975834
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/ring_middle.STL
new file mode 100644
index 0000000..09e8167
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e1e0a3dcf6dbeb90b61c808402eba709da4dadf8684fb430318b7211a283948b
+size 732384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/ring_proximal.STL
new file mode 100644
index 0000000..a49d585
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1a49c3f971492dd559217200f0e09556033efcb3686a3ac2e2f5b651e9f482f7
+size 690484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/thumb_distal.STL
new file mode 100644
index 0000000..c214026
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3587838fd24c9b383784666036563cbff5f905bc7a7bb72335bc5d35c335f0bf
+size 711434
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..b5aa9c8
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0272073e607d8b8f568af13cf90dd7b6b51202de85a8c7ea26ee91d32604453e
+size 1069934
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..208ae72
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:aa7c035a64a77a8c7459cf0eacca79affd1dfc0bf76bc17f151fddc51eb63176
+size 967484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..35c9fc1
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2380fc48a7a097aac08f2953aa1292af765f710939f73d13bcaaa768c85bcd2a
+size 503434
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..ac997a0
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_left/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0ffe01cde5a9810c0235a65caa3aa4e6b9b4b7f2892923c6649ef51856be66fb
+size 999084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/linkerhand_l21_right.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/linkerhand_l21_right.urdf
new file mode 100644
index 0000000..f42dc99
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/linkerhand_l21_right.urdf
@@ -0,0 +1,1033 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/hand_base_link.STL
new file mode 100644
index 0000000..5bac936
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7d029b9477bc2e81e45234553366e5a00628819c512b2525a2de2725ba5d3612
+size 3319484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/index_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/index_metacarpals.STL
new file mode 100644
index 0000000..93ae4a7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/index_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:179b9458833f66a414e7afb17627a7c908f3368db385574eb9d680329b464ab0
+size 2916384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/index_middle.STL
new file mode 100644
index 0000000..5985ab0
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9d29cab9b42587137b2ffaf0007ef3a0e2a738fec09c4f65f63a8e400cb336fa
+size 742084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/index_proximal.STL
new file mode 100644
index 0000000..274459a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:eb4989bcb6548f3789c3bc48908c65829b36e48da3104a7e5eefab1b0f77e730
+size 642384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/middle_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/middle_metacarpals.STL
new file mode 100644
index 0000000..8daa296
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/middle_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b75dc93c77258d8937bd963b63302626d93b6523b5804528acf5051dd1ce43f8
+size 2881384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/middle_middle.STL
new file mode 100644
index 0000000..ab01668
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2c78ce09487e182d3e9bc04e9aeaa3703bde3bfa39b7fa93ef28b3f7360d8c7b
+size 742084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/middle_proximal.STL
new file mode 100644
index 0000000..dd1df3f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5791bdd9a8a15dc83d94a551862727526bc2117f44251019f007d4ecf00c2af1
+size 642384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/pinky_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/pinky_metacarpals.STL
new file mode 100644
index 0000000..e861fdc
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/pinky_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8b2a8769f855fb89458b3bbdd260a3a40e3d0476964cb47a41744cd61980785a
+size 2864084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/pinky_middle.STL
new file mode 100644
index 0000000..e435b03
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7bd0bc14bba67ff470781ac07a3830fd37b37d65c77afd20ff46891706863750
+size 742084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..fc23377
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b59cedeb7dadad0f90995d5155903023f6c7e5ea7d5d78e9ede81c87b0c13b3a
+size 642384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/ring_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/ring_metacarpals.STL
new file mode 100644
index 0000000..fffe865
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/ring_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:99f1c8836676325e7dadb57ee230bb7d6dcd443de663580e93ee674a258397d3
+size 2897784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/ring_middle.STL
new file mode 100644
index 0000000..aef8672
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fb78a0a9d08b3aaa4cedcef721b35529e19bef094c9074b1c21f5201903054fe
+size 742084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/ring_proximal.STL
new file mode 100644
index 0000000..5a8e13a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:662f3984ffddaecbd26f56c7139113d63a57b31bb385285db2fcb3a801565ae2
+size 642384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/thumb_distal.STL
new file mode 100644
index 0000000..3784f66
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c27f90ee7ce7cf0252eb22ad6909fa3c8534387d6a8787f76f1819645c35b6c6
+size 758534
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..cd2ae88
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:17311da4c364f79c3b5ca460e9e80c48f919ae352f7119cb828cd8ad2baffb07
+size 366784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..52e0569
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6f7114551ec68d258c324a28fd52c3d01dd63373b53e482d18f46faed7e829e1
+size 1112384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..e07d3a5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a5fc858752623b8e0cfafd411495ea71ee2b6fe0c04f43d323a638d97b3a713a
+size 520234
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..c3f6c6d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l21_right/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7490a69e5462e5aac717b79c311f9f75c9b0f643165dfe1a72eb5090203368d8
+size 1087384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/linkerhand_l25_left.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/linkerhand_l25_left.urdf
new file mode 100644
index 0000000..9c0c1f1
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/linkerhand_l25_left.urdf
@@ -0,0 +1,1285 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/hand_base_link.STL
new file mode 100644
index 0000000..9c358cb
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f5fa64265908b92a132e18fff27dc212ec21ba4872806386846374d5c91fc70a
+size 3504384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/index_distal.STL
new file mode 100644
index 0000000..5db670b
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:130639135efbb5cd2773c18b481fcea45afe3bfd5dc8279a28a18a6b518779db
+size 993184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/index_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/index_metacarpals.STL
new file mode 100644
index 0000000..bad2be3
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/index_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:60bc3fc552cbdc4a53fbde3e11a06fc8b0b52a0ee6ea8b730b481a6eb54cfe92
+size 153484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/index_middle.STL
new file mode 100644
index 0000000..34775d6
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b3e46d945996e52bf0758b80f7aa1c2b99bca503b4c5b3c350e6850f78b05152
+size 732584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/index_proximal.STL
new file mode 100644
index 0000000..55a22fa
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5a561d159861170e0bc91d6a4ecb634611bcc497bb2e60440d829e5ae0b9b7f7
+size 881084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/middle_distal.STL
new file mode 100644
index 0000000..3e831da
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a6143d000a38c7b60231b55b9e209161c7726a0410f5a7e5c48c0f41f067a006
+size 993184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/middle_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/middle_metacarpals.STL
new file mode 100644
index 0000000..a576c56
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/middle_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:592ce620ebba1e75ce109cf70654dc8dc2d9bb41315b6dfc902b3ed3c703f43a
+size 153484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/middle_middle.STL
new file mode 100644
index 0000000..0821e32
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:09b1b9a5bf9017d86dc543a214d263acc32c2ed4c3a474116ccdb8ab163cce89
+size 732584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/middle_proximal.STL
new file mode 100644
index 0000000..aa7302f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:115d5c2027a656d6765339a477effc251a6fb9cd26dfc56474e6b63fac64aa43
+size 881084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/pinky_distal.STL
new file mode 100644
index 0000000..d61f5d4
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2d31ac67d0c2573758f0da2de4c3b773494a262845cc63659f2816bfd1fda10a
+size 993184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/pinky_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/pinky_metacarpals.STL
new file mode 100644
index 0000000..977ef69
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/pinky_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:95a7614a7495032fdec6d5bf39df04a5fec85fbe1032ea8ffad5e09e59fe287a
+size 153484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/pinky_middle.STL
new file mode 100644
index 0000000..26bcc40
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5a67a4840294f79a64b143e1cba9131811342df8c735f817708dd106699c6480
+size 732584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..6964857
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5fce82ec63ecba86c656edbba8b17c5b44d51aa78df0ddb9657a465f89872206
+size 881084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/ring_distal.STL
new file mode 100644
index 0000000..fcf4d03
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c82b0efd00e191a019927cf5590135f8b4d9b2c9228abfb68b12f3e0257c42aa
+size 993184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/ring_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/ring_metacarpals.STL
new file mode 100644
index 0000000..d7bc961
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/ring_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c90a028bf7a5da221a92d886a65cde79cf5641ff82eccd65bd6b982e17df912d
+size 153484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/ring_middle.STL
new file mode 100644
index 0000000..0a8cd71
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:191ddee09524ed6f7ebd447cf30fb85f894969ba161fc8c0059b6e97f70319f1
+size 732584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/ring_proximal.STL
new file mode 100644
index 0000000..3d372d5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6a49d3ee8360028022c2ba7435ffb35fb19ffaaa3c73ddba5da16b106d86857c
+size 881084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/thumb_distal.STL
new file mode 100644
index 0000000..5921c52
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0812eea8d86179c70f00070e3142595e1b5cea231a5c606c78744041d7acd9ba
+size 1115884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..f26dd54
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7bcb58b906bcff02d8de04f0ac90dbe6bf318cdbda52988787808b50f491d8f7
+size 1074384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..f69e0e3
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fa50f9021ae593bb37b95472a999215574ae56f63fdc5ff6edfd74b298826059
+size 778384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..8d1b5bf
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:316ab10d2dae65040a78f85182635ea3b7b1f57859ef0b568ccee58d88d58129
+size 227384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..03e75a0
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_left/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e41ea39ce8cb6673fc33b3e33e0fe2af9e82158006fccf6355ee3e49bfc5db8b
+size 689584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/linkerhand_l25_right.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/linkerhand_l25_right.urdf
new file mode 100644
index 0000000..3d1b6cd
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/linkerhand_l25_right.urdf
@@ -0,0 +1,1285 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/hand_base_link.STL
new file mode 100644
index 0000000..bad7f8a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:79e36d4c7e9282c79601976d41144557d1928810202589e31bed335f3d308cd1
+size 3433184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/index_distal.STL
new file mode 100644
index 0000000..17066a2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:324aa4b852e0c469bfc0c0bb9b19e0ccffc7dcb401d6a678481340588465fa26
+size 984884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/index_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/index_metacarpals.STL
new file mode 100644
index 0000000..316b088
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/index_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:106ad58d52420c3216ccb87865f55062d4b4b558078dda95210a085a9bbe277c
+size 151784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/index_middle.STL
new file mode 100644
index 0000000..f7d3da0
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:27f9ccd91c5bdb1f6a1c42aeee4ab79e7e0aaf1e124a36999c75715b70d0aee6
+size 805584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/index_proximal.STL
new file mode 100644
index 0000000..8b6beb2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d4f85e69dd94d56bdc4646c945ca8a237c652fd5dffba86c7b9ce1d8067ae8e1
+size 883184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/middle_distal.STL
new file mode 100644
index 0000000..f50ce2b
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:688be3d9dd7fda49a8f9dbba824fedef5cf427c94e2e9e1ea96834eafbf6945a
+size 984884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/middle_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/middle_metacarpals.STL
new file mode 100644
index 0000000..becb768
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/middle_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:65bc65257a01e0ccd284dfdf3b2a66f2a865bf80faeb4da746cd1fd05114fe40
+size 151784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/middle_middle.STL
new file mode 100644
index 0000000..7356bc9
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:600b8e7486abc8a2707a5ecc66d4b5bea1dc0895c7173e911ba994f5cac30c10
+size 805584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/middle_proximal.STL
new file mode 100644
index 0000000..7b3aab7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:edd3af02319cd4a3b4e07a6f5d33d501887d5ab78625489254f289785b6f57e1
+size 883184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/pinky_distal.STL
new file mode 100644
index 0000000..9322965
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3b22871d948ef80447f93866af3643ffa87cb22c1c172750fca9357f3aaa46de
+size 984884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/pinky_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/pinky_metacarpals.STL
new file mode 100644
index 0000000..b635c98
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/pinky_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b63d10a0e18af0169c75b85eb740c1f25123f6c4c5be0c5368afc65e8c73771f
+size 151784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/pinky_middle.STL
new file mode 100644
index 0000000..742f4ba
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c6696bfcd2dbef8f248f86a6f45a90d5ba7b40e11cdb13a86b96347240a7b751
+size 805584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..63a280f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:311ce029c4972530d3b234307f4ea6ca1789577c2786d56f969a5dfac2ef6a57
+size 883184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/ring_distal.STL
new file mode 100644
index 0000000..27f67a3
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fca7bf31fa3f377110d31f7623e27f8c7845faedbdca7c09f98c94a28867bd69
+size 984884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/ring_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/ring_metacarpals.STL
new file mode 100644
index 0000000..c1a329a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/ring_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:11797279afee7f856e472755d3a96593baea659d2ec0014edb02006ae48b5d71
+size 151784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/ring_middle.STL
new file mode 100644
index 0000000..cdf7967
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2b3a0815e8daf95e76c9f54f84c13709a3951aa44ec6bb863facf60ba32c3c16
+size 805584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/ring_proximal.STL
new file mode 100644
index 0000000..5bdd4f0
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:168df91e5a5643a899556f96ca3f3b7fe8a3349436be42dfb5632c0d524f9a47
+size 883184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/thumb_distal.STL
new file mode 100644
index 0000000..0bc2c63
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:026dd2fac43c4461c5a90561ef29039f4a08802c0e47e5c422ecac2f34babe1e
+size 1108484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..6d6da06
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0a82998ece3c784bba7861b850d00ed483d93434f94f72c6298fea0917e328ca
+size 1158784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..9cea770
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8416e5f826f12c4597754fbe3052983635345a76e3d98a4cdbc38ed363a9da78
+size 852984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..a233baa
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a5679a28a612ab6aa62a4f7fa1c67be42f807bba7c2ca00ba062ca2e4ef0070d
+size 227884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..7b67077
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l25_right/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4cd7d6a283d2e5c3f6dfbd88aa1e18f9f481375d1e6b47f2c373a285184b123d
+size 716984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/linkerhand_l6_left.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/linkerhand_l6_left.urdf
new file mode 100644
index 0000000..c858b83
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/linkerhand_l6_left.urdf
@@ -0,0 +1,705 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/hand_base_link.STL
new file mode 100644
index 0000000..3b40d0e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6b93e04aa8268e5838f5fb43448e67d5ed65ec4208fbf2a77efd85885aa51740
+size 2630134
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/index_distal.STL
new file mode 100644
index 0000000..5827278
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ee4c9d0d77785122d453d49facd649f76f009921af4518bc39948e9f3d97454f
+size 505784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/index_proximal.STL
new file mode 100644
index 0000000..68f272b
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:10e4ff31c84027e2f505f38da7adb5e343cdb6014929ba951815cb4b8dd588ab
+size 308284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/middle_distal.STL
new file mode 100644
index 0000000..bd87df0
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0557cb6666b32ee5beb8481e1b0d4561a9e8abb1671b0cbba1a9940c0d0564d4
+size 506384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/middle_proximal.STL
new file mode 100644
index 0000000..cc14450
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:12cce0328157873b4e9ba6cf408bd50ebe07a0ea73b1f4ba146363f1ef974f05
+size 308334
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/pinky_distal.STL
new file mode 100644
index 0000000..43b70eb
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:73b5abadea95b83063d22281170495c6b459412b0f61931b5920f6dff8a1e899
+size 505134
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..f072af8
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e8761720d36e4d8ec4730a2ab0920d82ef0bf09b9d123a737951b698baf7d5e1
+size 301334
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/ring_distal.STL
new file mode 100644
index 0000000..5f66af5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:49c229c7cc23bc104dcf2893055d729997120b5bdc34d38ac0cf7c87258f6c11
+size 516584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/ring_proximal.STL
new file mode 100644
index 0000000..cdd47d2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0c1b492adb1a6bdba7595305f24a6d7cdaf120c65b5460d226705c71c35b1c98
+size 308434
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/thumb_distal.STL
new file mode 100644
index 0000000..267a6c6
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a4b3c468b58998f048075365be8cc414b9c728966a76d8692e1a288488170908
+size 594884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..3d21c16
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6923c2891a207d15829ab6cec1c1aac5c12f9c1d930a9f300fc82ac4a3a6c4cc
+size 537084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..eae5ae8
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_left/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9ca81c74045e5bf0b9627ecb70ec6696c37db1edcc80fde1c2c25d45a74fe24f
+size 56134
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/linkerhand_l6_right.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/linkerhand_l6_right.urdf
new file mode 100644
index 0000000..2b6bfc0
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/linkerhand_l6_right.urdf
@@ -0,0 +1,705 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/hand_base_link.STL
new file mode 100644
index 0000000..4f39c15
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0cb97e7551a3e6ed55b4e8cee73fd022bab6b25fc15b1543f52d645b33daca78
+size 2633084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/index_distal.STL
new file mode 100644
index 0000000..c5734d5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f9ab42b68123d19257c32396348dfa6f7f5d8400f9985ec2a764751455b94844
+size 505784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/index_proximal.STL
new file mode 100644
index 0000000..f01b391
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b8ce66e2f767265e99c9183263b406d6e336d879d51b539dca53b37ebdffe8f3
+size 308284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/middle_distal.STL
new file mode 100644
index 0000000..97ea0fc
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:74e6c0606cc7b14c6fd3b3ee7fe49422d27aeef42700383f4f9fb5a6f8d9c6dc
+size 505784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/middle_proximal.STL
new file mode 100644
index 0000000..b7b3e8a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:27435f0dba77b8d5b56e87134f2832e86aa3c97831148cacca858f35b61ced38
+size 308334
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/pinky_distal.STL
new file mode 100644
index 0000000..fcb2f0c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f4829edc80f999a058c556c8582620b407591b5726b77bb0509b4ff270332320
+size 505784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..cf068f3
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b6071987b9c30de3221d61b45f34fe14ac5feeaf9da22a21152eb221a1f5152c
+size 308334
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/ring_distal.STL
new file mode 100644
index 0000000..898910c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fad0ba5b4863f6b0d196b83cb10ae3042107dfe54868edc0f4b49d879c740320
+size 505784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/ring_proximal.STL
new file mode 100644
index 0000000..e200489
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:636bbb2007e2f176ca2663ce636a704992cb710989a238396bb452fdeab59402
+size 308334
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/thumb_distal.STL
new file mode 100644
index 0000000..3a1cb93
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c676876ddcb6994f33edc2516b8b38713f651d3e669d9de2418285341b97371b
+size 594884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..cd6f41c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e57a7f7772e0e61b46d0f13515e950ae8c83d5ccba1e66c59ec654049ae8bc26
+size 537084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..22e36c6
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l6_right/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6e65dfe08de522a06a175bb594e8a812e958dd0cad51fd3f669a8227904f36ff
+size 59334
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/linkerhand_l7_left.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/linkerhand_l7_left.urdf
new file mode 100644
index 0000000..9b356c9
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/linkerhand_l7_left.urdf
@@ -0,0 +1,1073 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/base_link.STL
new file mode 100644
index 0000000..9b5b829
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:28a98b15c4e1a9efac5845b13d88eb352dcb0a297dd25c69b247a5bc01ce9f3e
+size 1666284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/hand_base_link.STL
new file mode 100644
index 0000000..36a2040
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5de2d561b37421ddb147184f79bb76d119cb7659cfc996790364f69679408f77
+size 4153284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_distal.STL
new file mode 100644
index 0000000..ac1a7b2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6609d37c275758e922320397a5d8c86f9afcef1d9da2de26ebbaf20373df933a
+size 606184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_link0.STL
new file mode 100644
index 0000000..4e3d5ca
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7c3d43706062d2303f84666337eb3c503038d47059a871a817882958a985f9aa
+size 256884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_link1.STL
new file mode 100644
index 0000000..9080edf
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0be74517180e60134c06c8d50d30b96098017585c9b7f607f2c2130e0ae11b0c
+size 519284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_link2.STL
new file mode 100644
index 0000000..1807a0e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:679997180a67ec28a8c2227bea58fed7a540ca7535fcae5393f94db5c43e7919
+size 573484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_link3.STL
new file mode 100644
index 0000000..18adce9
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9cb61e85e4d61a5b63a315348aadd23919115cd54ffa12afced0fc2de929f982
+size 682784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_link4.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_link4.STL
new file mode 100644
index 0000000..aaa7520
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0d3a1584cf566616b84868ffbcf3b82545fbf259e3ed40c2661456799192ceae
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_middle.STL
new file mode 100644
index 0000000..77c7dc2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e4117995119a175a9a76ce4b18e779fab8354161aa12a9a96a701fe7b958012c
+size 513684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_proximal.STL
new file mode 100644
index 0000000..d4955a9
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:38b6a0baeec492d0e1e6e4dd503e3afd67a90d132b23cf30b720c3578685daa7
+size 508684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/little_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/little_link0.STL
new file mode 100644
index 0000000..eb1861a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/little_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7afaaaa27481b0427785070df79e3251a1dfd6fdcd22b2089a117f75bd90c623
+size 256884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/little_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/little_link1.STL
new file mode 100644
index 0000000..cdb5796
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/little_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c8759223942b77ab08666af40c834724920d138e10629de15a45af740c75ba22
+size 519284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/little_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/little_link2.STL
new file mode 100644
index 0000000..dfb76eb
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/little_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:bc49c7f4bf13de34f1e893e993cf1c21f9475bbba837a0dea3157dec1b49c297
+size 573484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/little_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/little_link3.STL
new file mode 100644
index 0000000..9786eae
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/little_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ff45ec96203c305f53724de8aeb9e02c32eb6ffc61497aa2275e128e104bb500
+size 682784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/little_link4.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/little_link4.STL
new file mode 100644
index 0000000..befee60
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/little_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:25c31141f955d73c7bae5f505e90041e1710cff72401704f135fdde3e72c76d9
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_distal.STL
new file mode 100644
index 0000000..7c5edda
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:cf8b43b304a0b1d71d7f7447cf49c7202a9ceec9c1fb8a255429505d2f2a1430
+size 606184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_link0.STL
new file mode 100644
index 0000000..8c30ca4
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:21c5311fef9933ee8b59ea2935a1c407a33d37bf1088167ee4948e80be0a234c
+size 519284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_link1.STL
new file mode 100644
index 0000000..2866f3b
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ce912434165ca8d9cd33643f7251c22fbe12c55cc73d837f655337db234a5d63
+size 573484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_link2.STL
new file mode 100644
index 0000000..63ea564
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:472f53ed3b676f49f0ad160a973ec0830558df31b63d9a1c9220573c7a885555
+size 682784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_link3.STL
new file mode 100644
index 0000000..6ab996d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d1e2a600af4c477829ac553dc8a02ebbaadf42ec3a6e7199dc7d38d0b967687a
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_middle.STL
new file mode 100644
index 0000000..20f906d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e1aa5d91fedda31d3d7aa7cbe87a2332dd29cdc307ca0028db9f064989bd57dc
+size 513684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_proximal.STL
new file mode 100644
index 0000000..ca29f86
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c68a20e35ed628ae71285f9548af705a294d2f80d46ede52da5d3d7b481e46e8
+size 508684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/pinky_distal.STL
new file mode 100644
index 0000000..9cd262a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8515dcb8601ebd317c5211c99793b14cd62b37bb0a6a1d29be06d0482aefade9
+size 606184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/pinky_middle.STL
new file mode 100644
index 0000000..6b728f6
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c924751fba0dc32956db902e7b5de4363b3e1da86a59b438881b9563bf6d3fe7
+size 513684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..e4ad47f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d5b80277509350cafbe52a8129ca1f5d14b4f530bc9df531c86abe2dca0aba4b
+size 508684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_distal.STL
new file mode 100644
index 0000000..72a721a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3579d8b21224124b8aef39b277d23666d1087fa9bd771fb4c968df7d00ad8c77
+size 606184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_link0.STL
new file mode 100644
index 0000000..309640d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ab8e34568c6cae073f1229ea54d2204a3f3c69704fc1446a7c9595987b39c6bb
+size 256884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_link1.STL
new file mode 100644
index 0000000..48851b9
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a379fd662d6337e265e6b2ab957dc861eca7bc44c7a06150dda0fa2ca69f5347
+size 519284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_link2.STL
new file mode 100644
index 0000000..91052f2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8b231d68309ec84c732df40f703f4b3ae7a8f00369f79c488804d05a62dd4182
+size 573484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_link3.STL
new file mode 100644
index 0000000..8addb2f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7801ed795a1b4d4d78da6ab3cd87a481460c2ee6e29d56be162a239a63ff3a40
+size 682784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_link4.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_link4.STL
new file mode 100644
index 0000000..51a99b2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:51d96da28e051c953465ba0eb6867cc0ac5903a1f319c502eb92545fc72c79ca
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_middle.STL
new file mode 100644
index 0000000..7ca1671
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:06b5aad661b2d9226a8f3037d409096ecbf5a09406ce89fa90b5c063dfaffcd3
+size 513684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_proximal.STL
new file mode 100644
index 0000000..ea636ee
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:05d367e06c19c62a73925b3ef87fa1593f608afdb6da0401fdbb0c2f986930fe
+size 508684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_distal.STL
new file mode 100644
index 0000000..bd99bce
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:91c8b973f0c4bbb28d235a0484c2997a50f2abedfe93c9ec5e5351b3ada13e4e
+size 627284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_link0.STL
new file mode 100644
index 0000000..a8426ef
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7ed8c26294c961e3edb4c6fb2e331a2a16b02f9c2c8f9bdbb0dcea2741838ee1
+size 385684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_link1.STL
new file mode 100644
index 0000000..0481409
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c6b07079056fad59c1b232f035fe0a48495ca2d9e6639b02d3635afdfb436bdd
+size 392584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_link2.STL
new file mode 100644
index 0000000..02843ab
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f88d7c7dc37703512b59225d58fb494cde1de2fbb3ca0525cbbdb059b7bd5b0d
+size 986384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_link3.STL
new file mode 100644
index 0000000..a6cf47b
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e0819ace8213c6ebb0a97c5fba5e904e2984aad31ac2b129b61068a2fd9c9c7b
+size 225184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_link4.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_link4.STL
new file mode 100644
index 0000000..12138eb
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:030aad2997c9501f4cad40f074a26228f53501cfd5099c9d33096235c7fb880d
+size 283384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_link5.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_link5.STL
new file mode 100644
index 0000000..a54c2fa
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_link5.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:82513e0a64a64aa0f7f4f3b85a5b8a5f09187d55e7fed5e9aa40fe3407066423
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..0b42222
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9a0edfb9612a9dbb47fdc27b0e671d972f398700ef3acaa9ed51b6b736724a17
+size 1263484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..81b2ac8
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6f36bd7a2203734f87cb9ece03a62a3f38db54cc68b0af4325dc674536d53f9d
+size 383884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..9b35292
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ebc6d7e9122c7612a1c2a98a1587b8eadabcf717c18fe33af5176eff6c6b2b3f
+size 1216684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..5cacac5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_left/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2f4c17b94e199c45c6395ef27448e3d7a200978eac3a5498ae659697194a8321
+size 687534
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/linkerhand_l7_right.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/linkerhand_l7_right.urdf
new file mode 100644
index 0000000..0d39d6f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/linkerhand_l7_right.urdf
@@ -0,0 +1,1073 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/base_link.STL
new file mode 100644
index 0000000..955d770
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:34662efe4b9d4a34cc5066945d2ac95ef66d48926ace3adc9c871808cbde851e
+size 1509984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/hand_base_link.STL
new file mode 100644
index 0000000..60bd476
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:625a4cd9d7739c34205adb962f70e4a4f066af213214e39cbe26dd364a36c5bd
+size 4150484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_distal.STL
new file mode 100644
index 0000000..4110748
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ea22d393399153229608de361ff16280ffbc2be2d49ad8d0ff34a6a40fbb1532
+size 606384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_link0.STL
new file mode 100644
index 0000000..efc95c1
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:0a82724b15185a15534eefccf69127becfff74cc7b43579d228368b9e21a6ec8
+size 257984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_link1.STL
new file mode 100644
index 0000000..2e67d56
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:24dd990af121925ae6836dce055b4ff992209fdb7bf06390cd2797c3e7e7f247
+size 521284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_link2.STL
new file mode 100644
index 0000000..ea9760f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:68b823511f995073faccfcc349d21e66cc0678165e5164867da43958c6c2cabe
+size 572984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_link3.STL
new file mode 100644
index 0000000..73ea2cd
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2d59058d948c3b957043a393dfc6ed4bef5ca6473b34ca978395ebdf371e555c
+size 746784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_link4.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_link4.STL
new file mode 100644
index 0000000..3e13eaf
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6d1359761f91062c97afd95e0fa92b6ea0e3339735dfc859c918e78f090031d1
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_middle.STL
new file mode 100644
index 0000000..08ddfd9
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2c7fc43c889e29ede17a6f575a777077a731412ed92ea8f01d1b3c86eed55bb5
+size 514084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_proximal.STL
new file mode 100644
index 0000000..0fd2989
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a0e8100a742115dc5473ee828e01f80cd6f144062a56d8728b0d5b072d405d60
+size 508084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/little_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/little_link0.STL
new file mode 100644
index 0000000..214afd8
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/little_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:26e8533229f302952431d8a7377b978dfee2442829d3de46636360504ec0dfc1
+size 257984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/little_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/little_link1.STL
new file mode 100644
index 0000000..14ae20e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/little_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:56376eb7b25a0e08ad0f9cc0e2c79d01bc984c99e4ced630eb07a28768dd88fa
+size 521284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/little_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/little_link2.STL
new file mode 100644
index 0000000..28b5f1a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/little_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f0e5a3ca00f8935873096793fb7328879b53f2458268033a7f8c39aa92679610
+size 572984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/little_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/little_link3.STL
new file mode 100644
index 0000000..1b23ac6
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/little_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:43f092a01de5b3b427a49ef5dd1351ccf1b50a662db2f6b5249b5ed4fe918333
+size 746784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/little_link4.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/little_link4.STL
new file mode 100644
index 0000000..1af2f41
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/little_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4336131b69b091b21dbdfbe38c3fb40d1ecb0242fbcb5b8e656c5835559a4eff
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_distal.STL
new file mode 100644
index 0000000..145ad0e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:12c3f994ee69fc0ef73d19422618af16ff2046be85371819cae5c00c4b33399f
+size 606384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_link0.STL
new file mode 100644
index 0000000..9682313
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:85c9267c2d16022d712e7b367ef2aabef9e31e134c3c63fb45b084515b12de10
+size 521284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_link1.STL
new file mode 100644
index 0000000..c982b97
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ab5028794863195998b14659f43a6d1902e55b50d72322232c44345ac47a1e5c
+size 572984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_link2.STL
new file mode 100644
index 0000000..0c37d7f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f818f2791654887012d820178e3e1da18188eef2ddc2803011c4fb1f9ffa03aa
+size 746784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_link3.STL
new file mode 100644
index 0000000..5cc1bb5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2133f86ea956222ef16bfa601f1c2952e4630cba1f3c5cf41a7c40b442e8acdc
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_middle.STL
new file mode 100644
index 0000000..41c4931
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2c70eecdc55820403fe44b6ef953d5397b495bc5e405298f4d42f9c98154fdc4
+size 514084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_proximal.STL
new file mode 100644
index 0000000..9722a13
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d3f3e5c94f6e102e7169e11a72639c20d455589688cbefc4a28b5145c582b6c3
+size 508084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/pinky_distal.STL
new file mode 100644
index 0000000..2a55b66
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:afc5ccf293d8c53488bcbc43c2ec8838fde8b202c2b9ab21eccac7b87c8462f2
+size 606384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/pinky_middle.STL
new file mode 100644
index 0000000..146dc30
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:354a14e69b155a4002ceea6868621e8d08d7c44478451e4a302537ac2cf36245
+size 514084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..9304710
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:008e342f43880ade0d8501e21de64468c03ba26848d0b77fffefa774e17c3f7e
+size 508084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_distal.STL
new file mode 100644
index 0000000..4e7b719
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7e18c138576d4dee8cf9fece453c6488efc1001b82cb536aacebebbe6891f424
+size 606384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_link0.STL
new file mode 100644
index 0000000..e347da4
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b20c5bec23d8b556e3b90cbb0aa451110a7c0fa9cb38db640e3ad0cce129fc47
+size 257984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_link1.STL
new file mode 100644
index 0000000..e8cbf9c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:13a0fdc7302d2e90de835f9331ac1c157239e9de9fbe92b289af06803a2618a0
+size 521284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_link2.STL
new file mode 100644
index 0000000..0d46ec5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7aa1779441cb58ee2a15ef850b813dbeb3cbe089ac4a54af64a101d3e3484902
+size 572984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_link3.STL
new file mode 100644
index 0000000..e8291d5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6afe65c8555fe99eb9e660074fa8e976ae53fed9cebe57a63885798a366d9765
+size 746784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_link4.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_link4.STL
new file mode 100644
index 0000000..f1b6f7b
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:45c8752a716edc970d9263940f2e2ac4df5523f85d5a20bc8cc17b64902f7c2f
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_mcp_pitch.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_mcp_pitch.STL
new file mode 100644
index 0000000..db66349
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_mcp_pitch.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ab8cfa6ea70237052d7a08871e408e2dc5965c07da09945993864ad713ceb811
+size 514084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_proximal.STL
new file mode 100644
index 0000000..c841c33
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b6772ddddac241d335afc3069bca6fc85da7b359fa86ccc7af31d4ce472bd6a1
+size 508084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_distal.STL
new file mode 100644
index 0000000..49d9895
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fd284d798fdba02948d17e33e826376f7140bf619c1199cbbb6a28f1750cbd3c
+size 625784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_link0.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_link0.STL
new file mode 100644
index 0000000..9f95649
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_link0.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3eca4e14a33c4e159a74ff3a60c0234172c8ed58f190d64b4a511c49c1fe829c
+size 386584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_link1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_link1.STL
new file mode 100644
index 0000000..b5d4461
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_link1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b09b4d4301f0d1f1791945952a853b634f2bee75951f02e8704d1106231fcf86
+size 392984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_link2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_link2.STL
new file mode 100644
index 0000000..2b197fc
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_link2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5b5d4b4701098fc64002d88e056bc2ef81295ee535731728efbf0189b85547d2
+size 327684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_link3.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_link3.STL
new file mode 100644
index 0000000..8cc7ee2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_link3.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9b5ef49f1b08859c1f16b6513a49ccab12e235413c541511a528744caa8ccfc3
+size 225084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_link4.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_link4.STL
new file mode 100644
index 0000000..c551d71
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_link4.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d2621267fcdde28faddb52af9518c041ab8d763b57aec6cd917d3e5cd1e13cbd
+size 844784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_link5.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_link5.STL
new file mode 100644
index 0000000..8492f9d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_link5.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ad1a9626c75a421cf3bef28da1bf4a267f20ef4367f16a955c320bbf0e849551
+size 7284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..52eb691
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a23356cbd8061d70c35543dba51dd096759a47cbe6ded8aff06d0806da5e2211
+size 1263184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..865d65c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c3f60c0d411b671f83cfc870e81bf3992acf0923415ef9948ca81cdcf3737a20
+size 384884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..edc63c1
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:56a1ca989fe26e95fd796776cfbf9bb1baef2861353c22c66eab31b489068bd6
+size 1213984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..ebf5324
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/l7_right/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6053aa17c6f2885dfb251149393b9f3ccff9485ae540d36b6c35484e5f0b6566
+size 686134
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/linkerhand_o6_left.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/linkerhand_o6_left.urdf
new file mode 100644
index 0000000..4a712bc
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/linkerhand_o6_left.urdf
@@ -0,0 +1,706 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/hand_base_link.STL
new file mode 100644
index 0000000..874d757
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2dc0d80eb470c0ccd6e338650f383efea75a5b77eee7e51d2b6a1da553afcdfe
+size 6296134
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/index_distal.STL
new file mode 100644
index 0000000..d50b6ee
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:4f2699ea27c60f6c28a3ca0cae64ddab40b24b4929bbc23e112cbe08f7ae311c
+size 581734
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/index_proximal.STL
new file mode 100644
index 0000000..e2e7af5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:dd77da3a5c18d236c46c3adb51e7be8262e64926a0ee9e996741609d5ed0b08d
+size 640384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/middle_distal.STL
new file mode 100644
index 0000000..53916eb
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b92ed1e3af1d6ef3389287a45c27433a1584a1233fcb0825ee48411b59299697
+size 581734
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/middle_proximal.STL
new file mode 100644
index 0000000..cdb518e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ed2daec64e98506a9fdd3e63708f5644c5d0660ef54c7711c83f15351fc8eb85
+size 640384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/pinky_distal.STL
new file mode 100644
index 0000000..f45efac
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:01a8db3f672c232cd3430c2314e1422cc14950c6f2e03606f9f4d20e66c30311
+size 581734
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..d1c3b74
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8d0a3df9d46abe366d27132a49a45552031b57d1684fdf3adf7969a19faf6bf0
+size 640384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/ring_distal.STL
new file mode 100644
index 0000000..dda3885
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a21ba5ccb885f939788ef701d99a94781d0001ffe2b43a3d79a32c376dab988b
+size 581734
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/ring_proximal.STL
new file mode 100644
index 0000000..603eaa5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d17d67b52a5f02d9ff5dbb0b7315fea055f0ee43a0f2443faf7df28942cf3663
+size 640384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/thumb_distal.STL
new file mode 100644
index 0000000..48df9aa
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:baf3c1b16f94925bcd06768af7ec811de1a97d78f253d26adcd502d35067cf75
+size 823084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..77e9619
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c91dc63ccd81ca270d166e9e276665ea3fbe6bb42fbee31d1c7bb545b4238045
+size 1403784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..aae46e8
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9e8bd4b4adce43f74df492c1cd3890457b2ac0151c12582c5a8faaf6cdc9f1d9
+size 686384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/linkerhand_o6_right.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/linkerhand_o6_right.urdf
new file mode 100644
index 0000000..579a8d1
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/linkerhand_o6_right.urdf
@@ -0,0 +1,705 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/hand_base_link.STL
new file mode 100644
index 0000000..a833fde
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f7fc8dae5d375e5a33251593c46f8b882c3a1aaafe89fa9544f0ef57d657f5de
+size 6341484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/index_distal.STL
new file mode 100644
index 0000000..867ab7c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f07c23647d61bbc3e1c195fd94a726fa1a7828d128100ad49176af4c2364645f
+size 580434
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/index_proximal.STL
new file mode 100644
index 0000000..a59ef30
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:38c58f62b9911203069925c3d374703772927ffd3fa98049400fd75e66e7194a
+size 637984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/middle_distal.STL
new file mode 100644
index 0000000..fd557e4
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:15d9e1daab1e75a1ce6808a64d37effd1f7cd179b9d1ecb44f3a4be70d0e186e
+size 580434
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/middle_proximal.STL
new file mode 100644
index 0000000..3411791
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:392519c735b9658d3417d793a00f6a0f655b8e8b0a64fdd096eb2bcb31a4de65
+size 637984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/pinky_distal.STL
new file mode 100644
index 0000000..9e0b71c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:1b4b1c44dadd484bc3564ad622e0b7609c60eb8d76937b2a10c7d262cddc96e0
+size 580434
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..9872e92
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:50a1131fbead7bcf79ed1f435d4d799bd29f637ce81aa5220f31dc8b75a43777
+size 637984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/ring_distal.STL
new file mode 100644
index 0000000..55821c9
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e6488d4c8d8a8aa2d857c51178472f50e398dac1a2552cf90d2cf08561ae69cf
+size 580434
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/ring_proximal.STL
new file mode 100644
index 0000000..0beb3fa
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e7485a837556d6c8a6101617bb113a625d65a878a00ac297b096d59b89629d5b
+size 637984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/thumb_distal.STL
new file mode 100644
index 0000000..0df8a0a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c3642be300b1ae3f7f5af071f5b1cfd4dec99e610e57e7bfe44a3b804886a360
+size 820784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..f1a8785
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6bb54d668aea4f71251d48dc010897e08717b0cd13f63b304357fdc36894b897
+size 1403884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..b1c2022
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_right/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d92665c1cf3304617674cb4bc64cb20bc42ee49238043a427c222e32b4bdfe56
+size 680684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/linkerhand_o7_left.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/linkerhand_o7_left.urdf
new file mode 100644
index 0000000..bcbf0e7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/linkerhand_o7_left.urdf
@@ -0,0 +1,1073 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/hand_base_link.STL
new file mode 100644
index 0000000..773c1f7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c95b3d9ae44f97ea444a26257058fff9d0a67bb79f02a99fd6eaba50e5c02188
+size 5094334
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/index_distal.STL
new file mode 100644
index 0000000..4723f1d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:56c0c09b87d2d414e70e51a4b9125a8fec5a59e53bea0574da715850042de782
+size 760984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/index_middle.STL
new file mode 100644
index 0000000..996643d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e351fd77643d13dc9763d74b54b403af0e9e1ab5951a805042b465c87fb329cc
+size 452884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/index_proximal.STL
new file mode 100644
index 0000000..aab204c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8e4d0c1f409737a073e3b2d89fb256b12c8fac7b89b540aa4c968ee68f0fa4f5
+size 712584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/middle_distal.STL
new file mode 100644
index 0000000..fed1fb5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:84ee1076ceaf77fa9969352d39a6fca66b0287b8f191ce2254331ba7b4348220
+size 760984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/middle_middle.STL
new file mode 100644
index 0000000..fe3f798
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ee8b52d94bda222ada1384c577ccf043d821a523d8099b7be0235cc5a0bcd3e4
+size 452884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/middle_proximal.STL
new file mode 100644
index 0000000..1a1432d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:537fd6ca29c62175eb245e264bd4f6c7c624a200b89d196a6a76b2f2c6b93c7f
+size 712584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/pinky_distal.STL
new file mode 100644
index 0000000..4723f1d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:56c0c09b87d2d414e70e51a4b9125a8fec5a59e53bea0574da715850042de782
+size 760984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/pinky_middle.STL
new file mode 100644
index 0000000..93c7096
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7dc8283fda2abf377622316af2cead160a763292d16222d3912e78e92aaed5b6
+size 452884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..fdcccf5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8486694582c15e034379491231957083909c060ce63c22a78c585339d25d0b4d
+size 712584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/ring_distal.STL
new file mode 100644
index 0000000..ba62cae
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:669ad0a385eab3011b5900484b05a48756deaf0898c89b2826bbf53eddfa3c00
+size 760984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/ring_middle.STL
new file mode 100644
index 0000000..996643d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e351fd77643d13dc9763d74b54b403af0e9e1ab5951a805042b465c87fb329cc
+size 452884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/ring_proximal.STL
new file mode 100644
index 0000000..2ce173a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:751754cdee77367205c65220ac9741337853bfb64a973abe3d0c1b978f428991
+size 712584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/thumb_distal.STL
new file mode 100644
index 0000000..146e3e0
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:37a7c970db35a484328a6b432e5ab48d26030af8997770207bd90f4c9fb1c4dc
+size 710984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..2db6cd7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:16521fb5ba39739ab6b7745225fc8f5b2070cbb802a90dccb8ab5fccf45cc863
+size 2396384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..b3efd68
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f6e6a9b381cfca913c7e94aa37ac2e762c278d1b8e3b73f446cbb9d40a19984a
+size 609084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..1a43d73
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:51a169d2c58e5d0114b4ca6c455a102063482a5630362735d69fd24b8789614f
+size 738984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..575abd5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_left/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5c5f2d2d1f5920dbac54e71dbb0c658bba6d91adee12c18b2dee4bb06169cdc8
+size 1000884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/linkerhand_o7_right.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/linkerhand_o7_right.urdf
new file mode 100644
index 0000000..73df739
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/linkerhand_o7_right.urdf
@@ -0,0 +1,1074 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/hand_base_link.STL
new file mode 100644
index 0000000..c8a21ac
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9b5d88f8ba698c35f8f0b4d5505a71703bcb1d33ed43f6d5817c492b2ae482f6
+size 5061884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/index_distal.STL
new file mode 100644
index 0000000..09db084
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f00a460cdeeb5d98c9c3edb1caa970ddf4b6885d1f43e8453dd8af607a5abe91
+size 761684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/index_middle.STL
new file mode 100644
index 0000000..b8c016d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:719a1372db832690977b35b214235454028ee5ae95aff95ebfe621b03c91df87
+size 453084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/index_proximal.STL
new file mode 100644
index 0000000..6546278
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b611f8611ac1c72f47cd21993ed9e562682bad01f2671d1295b24673048394b0
+size 712484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/middle_distal.STL
new file mode 100644
index 0000000..9f14282
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:cacd51844529306e3161dfe4b9f52eeb826d3170cee0fe45aa02703ce9f4afe4
+size 761684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/middle_middle.STL
new file mode 100644
index 0000000..b88e829
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6d41b5b951f66d266ddebce17828fabd09dab3124412fc73453f34f60ecc6716
+size 453084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/middle_proximal.STL
new file mode 100644
index 0000000..b4fa5af
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:22104f99f287314b5998d3f0b40f6d8549205c3e2cacea9270497922722c2d87
+size 712484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/pinky_distal.STL
new file mode 100644
index 0000000..66bd7c1
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:35c1c63796e929210851b912bbd99faa641be41f3033de60b4f45845a3d40eea
+size 761684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/pinky_middle.STL
new file mode 100644
index 0000000..61c1de8
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:adb45d780fe1e37d2e5841867bfffa5d0e134c7f88fa04f02228bdb8d4e5a886
+size 453084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..bb22ade
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:877fb19054363e5ab1e6f8e23ada8aec597937188460770f699ecd1661517915
+size 712484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/ring_distal.STL
new file mode 100644
index 0000000..e0c23a8
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c43f72fd2e7b460cf1eab535ed0fe7c289d18b0ecf4a5dc0edc1c3b868bf975e
+size 761684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/ring_middle.STL
new file mode 100644
index 0000000..983850a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:cb2fc581de2ccb76d2c8296f20149e22f4f6545e5f504f8d0e9c6d549fb5e238
+size 453084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/ring_proximal.STL
new file mode 100644
index 0000000..581ccd9
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:141d101a58e25ad099335816a00b43ccf712d0ed2409c80f0ea7c27e7032b4cb
+size 712484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/thumb_distal.STL
new file mode 100644
index 0000000..e1c1224
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:67fb0a55afa37fba0c66ec46a2678c4225aad1970a7a5d0c6dd3242b56c3730e
+size 707984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..8b83125
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:bfa89c1d23e7bf15c2da1d0fc11cb8cb556637e72757c34dcd42fe4b014f15f0
+size 2394084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..760a394
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d915e0f99497fb39699d40a700c233fa00620cb27f7cf66ae94f95682fa75770
+size 664984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..d8bc62e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e999147f270c56ec7ccbd599695dbd44f04cb86f7337ed4326d12b3a230cd06a
+size 732584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..68e0baa
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7_right/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5f56677678079ce4e9c6d92a4aa7aa755bbdbd0cab7118144cb82f5329ca09a0
+size 999184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/likerhand_o7v1_left.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/likerhand_o7v1_left.urdf
new file mode 100644
index 0000000..8ad8938
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/likerhand_o7v1_left.urdf
@@ -0,0 +1,1073 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/hand_base_link.STL
new file mode 100644
index 0000000..36a2040
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5de2d561b37421ddb147184f79bb76d119cb7659cfc996790364f69679408f77
+size 4153284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/index_distal.STL
new file mode 100644
index 0000000..ac1a7b2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6609d37c275758e922320397a5d8c86f9afcef1d9da2de26ebbaf20373df933a
+size 606184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/index_middle.STL
new file mode 100644
index 0000000..77c7dc2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e4117995119a175a9a76ce4b18e779fab8354161aa12a9a96a701fe7b958012c
+size 513684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/index_proximal.STL
new file mode 100644
index 0000000..d4955a9
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:38b6a0baeec492d0e1e6e4dd503e3afd67a90d132b23cf30b720c3578685daa7
+size 508684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/middle_distal.STL
new file mode 100644
index 0000000..7c5edda
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:cf8b43b304a0b1d71d7f7447cf49c7202a9ceec9c1fb8a255429505d2f2a1430
+size 606184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/middle_middle.STL
new file mode 100644
index 0000000..20f906d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e1aa5d91fedda31d3d7aa7cbe87a2332dd29cdc307ca0028db9f064989bd57dc
+size 513684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/middle_proximal.STL
new file mode 100644
index 0000000..ca29f86
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c68a20e35ed628ae71285f9548af705a294d2f80d46ede52da5d3d7b481e46e8
+size 508684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/pinky_distal.STL
new file mode 100644
index 0000000..9cd262a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8515dcb8601ebd317c5211c99793b14cd62b37bb0a6a1d29be06d0482aefade9
+size 606184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/pinky_middle.STL
new file mode 100644
index 0000000..6b728f6
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c924751fba0dc32956db902e7b5de4363b3e1da86a59b438881b9563bf6d3fe7
+size 513684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..e4ad47f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d5b80277509350cafbe52a8129ca1f5d14b4f530bc9df531c86abe2dca0aba4b
+size 508684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/ring_distal.STL
new file mode 100644
index 0000000..72a721a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:3579d8b21224124b8aef39b277d23666d1087fa9bd771fb4c968df7d00ad8c77
+size 606184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/ring_middle.STL
new file mode 100644
index 0000000..7ca1671
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:06b5aad661b2d9226a8f3037d409096ecbf5a09406ce89fa90b5c063dfaffcd3
+size 513684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/ring_proximal.STL
new file mode 100644
index 0000000..ea636ee
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:05d367e06c19c62a73925b3ef87fa1593f608afdb6da0401fdbb0c2f986930fe
+size 508684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/thumb_distal.STL
new file mode 100644
index 0000000..bd99bce
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:91c8b973f0c4bbb28d235a0484c2997a50f2abedfe93c9ec5e5351b3ada13e4e
+size 627284
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..0b42222
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9a0edfb9612a9dbb47fdc27b0e671d972f398700ef3acaa9ed51b6b736724a17
+size 1263484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..81b2ac8
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6f36bd7a2203734f87cb9ece03a62a3f38db54cc68b0af4325dc674536d53f9d
+size 383884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..9b35292
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ebc6d7e9122c7612a1c2a98a1587b8eadabcf717c18fe33af5176eff6c6b2b3f
+size 1216684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..5cacac5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_left/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2f4c17b94e199c45c6395ef27448e3d7a200978eac3a5498ae659697194a8321
+size 687534
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/linkerhand_o7v1_right.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/linkerhand_o7v1_right.urdf
new file mode 100644
index 0000000..bc92bec
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/linkerhand_o7v1_right.urdf
@@ -0,0 +1,1073 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/hand_base_link.STL
new file mode 100644
index 0000000..60bd476
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:625a4cd9d7739c34205adb962f70e4a4f066af213214e39cbe26dd364a36c5bd
+size 4150484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/index_distal.STL
new file mode 100644
index 0000000..4110748
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ea22d393399153229608de361ff16280ffbc2be2d49ad8d0ff34a6a40fbb1532
+size 606384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/index_middle.STL
new file mode 100644
index 0000000..08ddfd9
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2c7fc43c889e29ede17a6f575a777077a731412ed92ea8f01d1b3c86eed55bb5
+size 514084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/index_proximal.STL
new file mode 100644
index 0000000..0fd2989
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a0e8100a742115dc5473ee828e01f80cd6f144062a56d8728b0d5b072d405d60
+size 508084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/middle_distal.STL
new file mode 100644
index 0000000..145ad0e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:12c3f994ee69fc0ef73d19422618af16ff2046be85371819cae5c00c4b33399f
+size 606384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/middle_middle.STL
new file mode 100644
index 0000000..41c4931
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:2c70eecdc55820403fe44b6ef953d5397b495bc5e405298f4d42f9c98154fdc4
+size 514084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/middle_proximal.STL
new file mode 100644
index 0000000..9722a13
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d3f3e5c94f6e102e7169e11a72639c20d455589688cbefc4a28b5145c582b6c3
+size 508084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/pinky_distal.STL
new file mode 100644
index 0000000..2a55b66
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:afc5ccf293d8c53488bcbc43c2ec8838fde8b202c2b9ab21eccac7b87c8462f2
+size 606384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/pinky_middle.STL
new file mode 100644
index 0000000..146dc30
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:354a14e69b155a4002ceea6868621e8d08d7c44478451e4a302537ac2cf36245
+size 514084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..9304710
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:008e342f43880ade0d8501e21de64468c03ba26848d0b77fffefa774e17c3f7e
+size 508084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/ring_distal.STL
new file mode 100644
index 0000000..4e7b719
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7e18c138576d4dee8cf9fece453c6488efc1001b82cb536aacebebbe6891f424
+size 606384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/ring_mcp_pitch.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/ring_mcp_pitch.STL
new file mode 100644
index 0000000..db66349
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/ring_mcp_pitch.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ab8cfa6ea70237052d7a08871e408e2dc5965c07da09945993864ad713ceb811
+size 514084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/ring_proximal.STL
new file mode 100644
index 0000000..c841c33
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b6772ddddac241d335afc3069bca6fc85da7b359fa86ccc7af31d4ce472bd6a1
+size 508084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/thumb_distal.STL
new file mode 100644
index 0000000..49d9895
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:fd284d798fdba02948d17e33e826376f7140bf619c1199cbbb6a28f1750cbd3c
+size 625784
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..52eb691
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:a23356cbd8061d70c35543dba51dd096759a47cbe6ded8aff06d0806da5e2211
+size 1263184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..865d65c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c3f60c0d411b671f83cfc870e81bf3992acf0923415ef9948ca81cdcf3737a20
+size 384884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..edc63c1
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:56a1ca989fe26e95fd796776cfbf9bb1baef2861353c22c66eab31b489068bd6
+size 1213984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..ebf5324
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v1_right/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6053aa17c6f2885dfb251149393b9f3ccff9485ae540d36b6c35484e5f0b6566
+size 686134
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/linkerhand_o7v3_left.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/linkerhand_o7v3_left.urdf
new file mode 100644
index 0000000..bcbf0e7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/linkerhand_o7v3_left.urdf
@@ -0,0 +1,1073 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/hand_base_link.STL
new file mode 100644
index 0000000..773c1f7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c95b3d9ae44f97ea444a26257058fff9d0a67bb79f02a99fd6eaba50e5c02188
+size 5094334
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/index_distal.STL
new file mode 100644
index 0000000..4723f1d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:56c0c09b87d2d414e70e51a4b9125a8fec5a59e53bea0574da715850042de782
+size 760984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/index_middle.STL
new file mode 100644
index 0000000..996643d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e351fd77643d13dc9763d74b54b403af0e9e1ab5951a805042b465c87fb329cc
+size 452884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/index_proximal.STL
new file mode 100644
index 0000000..aab204c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8e4d0c1f409737a073e3b2d89fb256b12c8fac7b89b540aa4c968ee68f0fa4f5
+size 712584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/middle_distal.STL
new file mode 100644
index 0000000..fed1fb5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:84ee1076ceaf77fa9969352d39a6fca66b0287b8f191ce2254331ba7b4348220
+size 760984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/middle_middle.STL
new file mode 100644
index 0000000..fe3f798
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:ee8b52d94bda222ada1384c577ccf043d821a523d8099b7be0235cc5a0bcd3e4
+size 452884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/middle_proximal.STL
new file mode 100644
index 0000000..1a1432d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:537fd6ca29c62175eb245e264bd4f6c7c624a200b89d196a6a76b2f2c6b93c7f
+size 712584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/pinky_distal.STL
new file mode 100644
index 0000000..4723f1d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:56c0c09b87d2d414e70e51a4b9125a8fec5a59e53bea0574da715850042de782
+size 760984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/pinky_middle.STL
new file mode 100644
index 0000000..93c7096
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:7dc8283fda2abf377622316af2cead160a763292d16222d3912e78e92aaed5b6
+size 452884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..fdcccf5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:8486694582c15e034379491231957083909c060ce63c22a78c585339d25d0b4d
+size 712584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/ring_distal.STL
new file mode 100644
index 0000000..ba62cae
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:669ad0a385eab3011b5900484b05a48756deaf0898c89b2826bbf53eddfa3c00
+size 760984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/ring_middle.STL
new file mode 100644
index 0000000..996643d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e351fd77643d13dc9763d74b54b403af0e9e1ab5951a805042b465c87fb329cc
+size 452884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/ring_proximal.STL
new file mode 100644
index 0000000..2ce173a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:751754cdee77367205c65220ac9741337853bfb64a973abe3d0c1b978f428991
+size 712584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/thumb_distal.STL
new file mode 100644
index 0000000..146e3e0
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:37a7c970db35a484328a6b432e5ab48d26030af8997770207bd90f4c9fb1c4dc
+size 710984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..2db6cd7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:16521fb5ba39739ab6b7745225fc8f5b2070cbb802a90dccb8ab5fccf45cc863
+size 2396384
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..b3efd68
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f6e6a9b381cfca913c7e94aa37ac2e762c278d1b8e3b73f446cbb9d40a19984a
+size 609084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..1a43d73
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:51a169d2c58e5d0114b4ca6c455a102063482a5630362735d69fd24b8789614f
+size 738984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..575abd5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_left/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5c5f2d2d1f5920dbac54e71dbb0c658bba6d91adee12c18b2dee4bb06169cdc8
+size 1000884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/linkerhand_o7v3_right.urdf b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/linkerhand_o7v3_right.urdf
new file mode 100644
index 0000000..73df739
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/linkerhand_o7v3_right.urdf
@@ -0,0 +1,1074 @@
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/hand_base_link.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/hand_base_link.STL
new file mode 100644
index 0000000..c8a21ac
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/hand_base_link.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:9b5d88f8ba698c35f8f0b4d5505a71703bcb1d33ed43f6d5817c492b2ae482f6
+size 5061884
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/index_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/index_distal.STL
new file mode 100644
index 0000000..09db084
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/index_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:f00a460cdeeb5d98c9c3edb1caa970ddf4b6885d1f43e8453dd8af607a5abe91
+size 761684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/index_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/index_middle.STL
new file mode 100644
index 0000000..b8c016d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/index_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:719a1372db832690977b35b214235454028ee5ae95aff95ebfe621b03c91df87
+size 453084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/index_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/index_proximal.STL
new file mode 100644
index 0000000..6546278
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/index_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:b611f8611ac1c72f47cd21993ed9e562682bad01f2671d1295b24673048394b0
+size 712484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/middle_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/middle_distal.STL
new file mode 100644
index 0000000..9f14282
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/middle_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:cacd51844529306e3161dfe4b9f52eeb826d3170cee0fe45aa02703ce9f4afe4
+size 761684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/middle_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/middle_middle.STL
new file mode 100644
index 0000000..b88e829
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/middle_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:6d41b5b951f66d266ddebce17828fabd09dab3124412fc73453f34f60ecc6716
+size 453084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/middle_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/middle_proximal.STL
new file mode 100644
index 0000000..b4fa5af
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/middle_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:22104f99f287314b5998d3f0b40f6d8549205c3e2cacea9270497922722c2d87
+size 712484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/pinky_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/pinky_distal.STL
new file mode 100644
index 0000000..66bd7c1
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/pinky_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:35c1c63796e929210851b912bbd99faa641be41f3033de60b4f45845a3d40eea
+size 761684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/pinky_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/pinky_middle.STL
new file mode 100644
index 0000000..61c1de8
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/pinky_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:adb45d780fe1e37d2e5841867bfffa5d0e134c7f88fa04f02228bdb8d4e5a886
+size 453084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/pinky_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/pinky_proximal.STL
new file mode 100644
index 0000000..bb22ade
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/pinky_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:877fb19054363e5ab1e6f8e23ada8aec597937188460770f699ecd1661517915
+size 712484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/ring_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/ring_distal.STL
new file mode 100644
index 0000000..e0c23a8
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/ring_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:c43f72fd2e7b460cf1eab535ed0fe7c289d18b0ecf4a5dc0edc1c3b868bf975e
+size 761684
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/ring_middle.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/ring_middle.STL
new file mode 100644
index 0000000..983850a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/ring_middle.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:cb2fc581de2ccb76d2c8296f20149e22f4f6545e5f504f8d0e9c6d549fb5e238
+size 453084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/ring_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/ring_proximal.STL
new file mode 100644
index 0000000..581ccd9
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/ring_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:141d101a58e25ad099335816a00b43ccf712d0ed2409c80f0ea7c27e7032b4cb
+size 712484
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/thumb_distal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/thumb_distal.STL
new file mode 100644
index 0000000..e1c1224
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/thumb_distal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:67fb0a55afa37fba0c66ec46a2678c4225aad1970a7a5d0c6dd3242b56c3730e
+size 707984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/thumb_metacarpals.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/thumb_metacarpals.STL
new file mode 100644
index 0000000..8b83125
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/thumb_metacarpals.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:bfa89c1d23e7bf15c2da1d0fc11cb8cb556637e72757c34dcd42fe4b014f15f0
+size 2394084
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/thumb_metacarpals_base1.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/thumb_metacarpals_base1.STL
new file mode 100644
index 0000000..760a394
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/thumb_metacarpals_base1.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:d915e0f99497fb39699d40a700c233fa00620cb27f7cf66ae94f95682fa75770
+size 664984
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/thumb_metacarpals_base2.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/thumb_metacarpals_base2.STL
new file mode 100644
index 0000000..d8bc62e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/thumb_metacarpals_base2.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:e999147f270c56ec7ccbd599695dbd44f04cb86f7337ed4326d12b3a230cd06a
+size 732584
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/thumb_proximal.STL b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/thumb_proximal.STL
new file mode 100644
index 0000000..68e0baa
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o7v3_right/meshes/thumb_proximal.STL
@@ -0,0 +1,3 @@
+version https://git-lfs.github.com/spec/v1
+oid sha256:5f56677678079ce4e9c6d92a4aa7aa755bbdbd0cab7118144cb82f5329ca09a0
+size 999184
diff --git a/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/version.md b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/version.md
new file mode 100644
index 0000000..80dff26
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/version.md
@@ -0,0 +1,19 @@
+## v1.0.0.1 update，更新日期2025-03-31
+1. 调整linker_hand_l10_left拇指旋转角不当的问题，子版本号变更为1.0.0.1
+2. 调整linker_hand_l10_left无名指偏航角的大小，子版本号变更为1.0.0.2
+3. 调整linker_hand_l10_left小指偏航角的大小，子版本号变更为1.0.0.3
+4. linker_hand_l10_right的拇指旋转角处于异常状态，需结构重新设定，版本封存
+5. 调整linker_hand_l20_right拇指旋转角不当的问题，子版本号变更为1.0.0.1
+6. 调整linker_hand_l20_right拇指偏航角不当的问题，子版本号变更为1.0.0.2
+7. 调整linker_hand_t25_left四指横滚角不当的问题，子版本号变更为1.0.0.1
+
+## v1.0.0.0 create
+1. 版本创建
+2. 添加linker_hand_l10_left，版本号v1.6.7995.38578
+3. 添加linker_hand_l10_right，版本号v1.0.0
+4. 添加linker_hand_l20_left，版本号v1.0.0
+5. 添加linker_hand_l20_right，版本号v1.0.0
+6. 添加linker_hand_t25_left，版本号v1.0.0
+7. 添加linker_hand_t25_right，版本号v1.0.0
+8. 添加linker_hand_o7_left，版本号v1.0.0
+9. 添加linker_hand_o7_right，版本号v1.0.0
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/base_config.yml b/src/linkerhand_retarget/linkerhand_retarget/config/base_config.yml
new file mode 100644
index 0000000..8ae7192
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/base_config.yml
@@ -0,0 +1,56 @@
+calibration:
+ fist_extend_ratio: 0.5
+ show_fist: true
+debug:
+ joint_motor_debug_l: false
+ joint_motor_debug_r: false
+ joint_pub_debug: false
+ mapper_debug: false
+humanset:
+ bodyfile: body_unity_pose
+ targethandfile: human_hand_info
+linkereg:
+ password: i
+ port: null
+netpub:
+ ip: 192.168.11.33
+ port: 20008
+serial:
+ auto_scan: false
+ baudrates:
+ - 2000000
+ - 460800
+ - 1000000
+ - 921600
+ exclude_ports: []
+ left:
+ baudrate: 2000000
+ port: /dev/ttyUSB0
+ right:
+ baudrate: 2000000
+ port: /dev/ttyUSB0
+ serial_debug: false
+system:
+ can:
+ bitrate: 1000000
+ dofs: 25
+ id: 40
+ datasource_type: motion
+ leftpub: true
+ motion_device: eric
+ motion_type: linkerforce
+ retargeting_type: projection
+ rightpub: true
+ robotname_l: l10
+ robotname_r: l10
+ sapientype: left
+ usecan: false
+ usegui: false
+ usenetpub: false
+ usesapien: true
+ useudp: true
+ useudpserver: true
+udp:
+ ip: 0.0.0.0
+ port: 8888
+ serverport: 5551
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/body_custom_pose.yml b/src/linkerhand_retarget/linkerhand_retarget/config/body_custom_pose.yml
new file mode 100644
index 0000000..ccded40
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/body_custom_pose.yml
@@ -0,0 +1,69 @@
+initial_positions:
+ body:
+ - [0, 1, 0]
+ - [0.1, 0.9, 0]
+ - [0.1, 0.5, 0]
+ - [0.1, 0.09, 0]
+ - [0.1, 0.02, 0.07]
+ - [-0.1, 0.9, 0]
+ - [-0.1, 0.5, 0]
+ - [-0.1, 0.09, 0]
+ - [-0.1, 0.02, 0.07]
+ - [0, 1.05, 0]
+ - [0, 1.15, 0]
+ - [0, 1.2, 0]
+ - [0, 1.3, 0]
+ - [0, 1.392, 0]
+ - [0, 1.6, 0]
+ - [0.04, 1.392, 0]
+ - [0.1835, 1.392, 0]
+ - [0.4343, 1.392, 0]
+ - [0.6612, 1.392, 0]
+ - [-0.04, 1.392, 0]
+ - [-0.1835, 1.392, 0]
+ - [-0.4343, 1.392, 0]
+ - [-0.6612, 1.392, 0]
+ right_hand:
+ - [0.5000, 0.0000, 0.1000]
+ # - [0.5740, 0.0534, 0.0974]
+ - [0.5370, 0.0370, 0.0974]
+ - [0.5764, 0.0764, 0.0965]
+ - [0.5901, 0.0901, 0.0959]
+ - [0.5000, 0.0000, 0.1000]
+ - [0.6060, 0.0324, 0.0968]
+ - [0.6378, 0.0325, 0.0959]
+ - [0.6697, 0.0326, 0.0944]
+ - [0.5000, 0.0000, 0.1000]
+ - [0.6061, 0.0111, 0.0970]
+ - [0.6432, 0.0113, 0.0960]
+ - [0.6804, 0.0114, 0.0943]
+ - [0.5000, 0.0000, 0.1000]
+ - [0.6062, -0.0101, 0.0972]
+ - [0.6380, -0.0100, 0.0963]
+ - [0.6699, -0.0098, 0.0949]
+ - [0.5000, 0.0000, 0.1000]
+ - [0.6063, -0.0314, 0.0974]
+ - [0.6328, -0.0312, 0.0967]
+ - [0.6593, -0.0311, 0.0955]
+ left_hand:
+ - [-0.5000, 0.0000, 0.1000]
+ # - [0.5740, 0.0534, 0.0974]
+ - [-0.5370, 0.0370, 0.0974]
+ - [-0.5764, 0.0764, 0.0965]
+ - [-0.5901, 0.0901, 0.0959]
+ - [-0.5000, 0.0000, 0.1000]
+ - [-0.6060, 0.0324, 0.0968]
+ - [-0.6378, 0.0325, 0.0959]
+ - [-0.6697, 0.0326, 0.0944]
+ - [-0.5000, 0.0000, 0.1000]
+ - [-0.6061, 0.0111, 0.0970]
+ - [-0.6432, 0.0113, 0.0960]
+ - [-0.6804, 0.0114, 0.0943]
+ - [-0.5000, 0.0000, 0.1000]
+ - [-0.6062, -0.0101, 0.0972]
+ - [-0.6380, -0.0100, 0.0963]
+ - [-0.6699, -0.0098, 0.0949]
+ - [-0.5000, 0.0000, 0.1000]
+ - [-0.6063, -0.0314, 0.0974]
+ - [-0.6328, -0.0312, 0.0967]
+ - [-0.6593, -0.0311, 0.0955]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/body_unity_pose.yml b/src/linkerhand_retarget/linkerhand_retarget/config/body_unity_pose.yml
new file mode 100644
index 0000000..ed18e66
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/body_unity_pose.yml
@@ -0,0 +1,67 @@
+initial_positions:
+ body:
+ - [0, 1.01417, -0.01136745]
+ - [0.09608424, 0.9339905, -0.01918257]
+ - [0.1070137, 0.5183502, -0.03298733]
+ - [0.1029733, 0.09305052, -0.03527176]
+ - [0.1029734, 0.02005178, 0.07543653]
+ - [-0.09608424, 0.9339905, -0.01918257]
+ - [-0.1070137, 0.5183502, -0.03298733]
+ - [-0.1029733, 0.09305052, -0.03527176]
+ - [-0.1029734, 0.02005178, 0.07543653]
+ - [1.610145E-15, 1.101661, -0.01136744]
+ - [-1.967892E-16, 1.204164, -0.01136744]
+ - [1.722054E-16, 1.313723, -0.01136744]
+ - [3.313691E-17, 1.420459, 2.017152E-09]
+ - [1.412962E-16, 1.530355, -0.01136744]
+ - [-1.426205E-16, 1.62144, -0.01136744]
+ - [0.04465847, 1.45745, -0.01136743]
+ - [0.1871382, 1.45745, -0.01136743]
+ - [0.4227339, 1.45745, -0.01136743]
+ - [0.6813283, 1.45745, -0.01136743]
+ - [-0.04465845, 1.457453, -0.01136744]
+ - [-0.1871379, 1.457453, -0.01136744]
+ - [-0.4227338, 1.457453, -0.01136744]
+ - [-0.6813283, 1.457453, -0.01136744]
+ right_hand:
+ - [0.6813, 1.4575, -0.0114]
+ - [0.7121, 1.4598, 0.0272]
+ - [0.7444, 1.4597, 0.0592]
+ - [0.7669, 1.4598, 0.0815]
+ - [0.6813, 1.4575, -0.0114]
+ - [0.7857, 1.4626, 0.0254]
+ - [0.8304, 1.4604, 0.0255]
+ - [0.8557, 1.4589, 0.0255]
+ - [0.6813, 1.4575, -0.0114]
+ - [0.7871, 1.4628, 0.0019]
+ - [0.8358, 1.4595, 0.0017]
+ - [0.8663, 1.4571, 0.0018]
+ - [0.6813, 1.4575, -0.0114]
+ - [0.7802, 1.4639, -0.0189]
+ - [0.8226, 1.4599, -0.0188]
+ - [0.8520, 1.4573, -0.0189]
+ - [0.6813, 1.4575, -0.0114]
+ - [0.7716, 1.4629, -0.0397]
+ - [0.8056, 1.4611, -0.0398]
+ - [0.8270, 1.4594, -0.0398]
+ left_hand:
+ - [-0.6813, 1.4575, -0.0114]
+ - [-0.7121, 1.4598, 0.0272]
+ - [-0.7444, 1.4599, 0.0593]
+ - [-0.7666, 1.4598, 0.0817]
+ - [-0.6813, 1.4575, -0.0114]
+ - [-0.7857, 1.4626, 0.0254]
+ - [-0.8304, 1.4604, 0.0253]
+ - [-0.8557, 1.4588, 0.0254]
+ - [-0.6813, 1.4575, -0.0114]
+ - [-0.7871, 1.4628, 0.0019]
+ - [-0.8358, 1.4595, 0.0017]
+ - [-0.8663, 1.4571, 0.0019]
+ - [-0.6813, 1.4575, -0.0114]
+ - [-0.7802, 1.4639, -0.0189]
+ - [-0.8226, 1.4606, -0.0189]
+ - [-0.8521, 1.4585, -0.0189]
+ - [-0.6813, 1.4575, -0.0114]
+ - [-0.7716, 1.4629, -0.0397]
+ - [-0.8056, 1.4611, -0.0396]
+ - [-0.8270, 1.4594, -0.0397]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/calibration_sample.yml b/src/linkerhand_retarget/linkerhand_retarget/config/calibration_sample.yml
new file mode 100644
index 0000000..4de87ac
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/calibration_sample.yml
@@ -0,0 +1,133 @@
+jointanglefist_l:
+- 1.2437261653212208
+- 3.175084430630538
+- 1.9181389961835325
+- 3.945504443472827
+- 3.176513677347769
+- 4.15470097472291
+- 1.2220743742244509
+- 5.559884890958649
+- 4.359119825243289
+- 4.092220656591438
+- 1.191495397918279
+- 5.462992890591046
+- 4.491218635943924
+- 4.200856485664777
+- 1.3686122344439422
+- 4.9037060043950795
+- 3.9838837108819094
+- 4.308879495669112
+- 0.9698557684365023
+- 5.547674363135352
+- 4.032751679729586
+jointanglefist_r:
+- 1.1044308306023618
+- 1.5951796058121013
+- 3.6276991375224035
+- 5.47734400510711
+- 5.116904467934635
+- 2.794897842149621
+- 4.45722081173913
+- 2.6553320763879045
+- 5.816282943026772
+- 2.7213461356079076
+- 4.369521194059152
+- 2.451310885672715
+- 7.396954804596076
+- 2.7479907704823434
+- 4.328249766846534
+- 2.4081802807772505
+- 5.57164877596065
+- 2.7861285486955643
+- 4.76350470576917
+- 2.774348560437687
+- 5.953534512596457
+jointangleopose_l:
+- 1.2793574933684364
+- 3.483006318161248
+- 2.331541885879661
+- 3.7824928786437844
+- 2.970712731931335
+- 4.142190635278191
+- 1.7971720165469458
+- 5.247289642697985
+- 3.4463685780825344
+- 4.100680552379041
+- 1.7967017729109331
+- 5.190892714154727
+- 3.604353676476713
+- 4.174859433782973
+- 1.925651520397901
+- 4.825046277195627
+- 3.3016353177490636
+- 4.293594016705458
+- 1.5552237533819402
+- 5.241486378132036
+- 3.3450599532328815
+jointangleopose_r:
+- 1.2290999401142237
+- 1.3007013686522553
+- 3.402433098354701
+- 5.014432913946464
+- 4.331938085144293
+- 2.7775602287644148
+- 4.012928856914155
+- 2.487202411250892
+- 5.0280960910012995
+- 2.723595482905546
+- 3.923280471696764
+- 2.3498730343038363
+- 4.931762287623787
+- 2.734774309055077
+- 3.834016447185085
+- 2.2936636508756503
+- 4.818287076901581
+- 2.743915917282852
+- 4.166702619233486
+- 2.545942118281719
+- 5.167868347521494
+jointangleoriginal_l:
+- 1.3506201494628678
+- 4.098850093222668
+- 3.1583476652719176
+- 3.456469748985699
+- 2.559110841098467
+- 4.1171699563887545
+- 2.9473673011919357
+- 4.622099146176655
+- 1.620866083761026
+- 4.117600343954245
+- 3.0071145228962415
+- 4.646692361282089
+- 1.8306237575422901
+- 4.122865330019365
+- 3.0397300923058186
+- 4.6677268227967215
+- 1.9371385314833716
+- 4.263023058778151
+- 2.725959723272816
+- 4.6291104081254035
+- 1.9696765002394736
+jointangleoriginal_r:
+- 1.4784381591379478
+- 0.7117448943325635
+- 2.9519010200192963
+- 4.088610731625172
+- 2.7620053195636087
+- 2.742885001994002
+- 3.124344947264206
+- 2.1509430809768673
+- 3.451722386950354
+- 2.7280941775008234
+- 3.0307990269719896
+- 2.146997331566079
+- 0.0013772536792077154
+- 2.7083413862005443
+- 2.845549807862187
+- 2.0646303910724497
+- 3.3115636787834424
+- 2.6594906544574264
+- 2.9730984461621164
+- 2.0891292339697816
+- 3.5965360173715664
+timestamp: '2026-03-24T16:59:53.113578'
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/hand_config.yml b/src/linkerhand_retarget/linkerhand_retarget/config/hand_config.yml
new file mode 100644
index 0000000..316bd57
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/hand_config.yml
@@ -0,0 +1,112 @@
+commandlower_right_l10: [255, 255, 255, 255, 255, 255, 255, 0, 0, 255, None, None, None, None, None, None, None, None, None, None]
+commandupper_right_l10: [0, 0, 0, 0, 0, 0, 0, 255, 255, 0, None, None, None, None, None, None, None, None, None, None]
+commandlower_left_l10: [255, 255, 255, 255, 255, 255, 0, 255, 255, 255, None, None, None, None, None, None, None, None, None, None]
+commandupper_left_l10: [0, 0, 0, 0, 0, 0, 255, 0, 0, 0, None, None, None, None, None, None, None, None, None, None]
+commandlower_right_l10v7: [255, 255, 255, 255, 255, 255, 255, 0, 0, 255, None, None, None, None, None, None, None, None, None, None]
+commandupper_right_l10v7: [0, 0, 0, 0, 0, 0, 0, 255, 255, 0, None, None, None, None, None, None, None, None, None, None]
+commandlower_left_l10v7: [255, 255, 255, 255, 255, 255, 0, 255, 255, 255, None, None, None, None, None, None, None, None, None, None]
+commandupper_left_l10v7: [0, 0, 0, 0, 0, 0, 255, 0, 0, 0, None, None, None, None, None, None, None, None, None, None]
+commandlower_right_t25: [255, 255, 255, 255, 255, 0, 255, None, 0, 0, 255, None, None, None, None, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255]
+commandupper_right_t25: [0, 0, 0, 0, 0, 255, 0, None, 255, 255, 0, None, None, None, None, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+commandlower_left_t25: [0, 255, 255, 255, 255, 255, 0, None, 255, 255, 0, None, None, None, None, 0, 255, 255, 255, 255, 0, 255, 255, 255, 255]
+commandupper_left_t25: [255, 0, 0, 0, 0, 0, 255, None, 0, 0, 255, None, None, None, None, 255, 0, 0, 0, 0, 255, 0, 0, 0, 0]
+commandlower_right_l20: [255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 255, None, None, None, None, 255, 255, 255, 255, 255]
+commandupper_right_l20: [0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 0, None, None, None, None, 0, 0, 0, 0, 0]
+commandlower_left_l20: [255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 255, None, None, None, None, 255, 255, 255, 255, 255]
+commandupper_left_l20: [0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 0, None, None, None, None, 0, 0, 0, 0, 0]
+commandlower_right_g20: [255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, None, None, None, None, 255, 255, 255, 255, 255]
+commandupper_right_g20: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, None, None, None, None, 0, 0, 0, 0, 0]
+commandlower_left_g20: [255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 255, None, None, None, None, 255, 255, 255, 255, 255]
+commandupper_left_g20: [0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 0, None, None, None, None, 0, 0, 0, 0, 0]
+commandlower_right_l7: [255, 255, 255, 255, 255, 255, 255, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandupper_right_l7: [0, 0, 0, 0, 0, 0, 0, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandlower_left_l7: [255, 255, 255, 255, 255, 255, 255, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandupper_left_l7: [0, 0, 0, 0, 0, 0, 0, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandlower_right_o7: [255, 255, 255, 255, 255, 255, 255, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandupper_right_o7: [0, 0, 0, 0, 0, 0, 0, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandlower_left_o7: [255, 255, 255, 255, 255, 255, 255, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandupper_left_o7: [0, 0, 0, 0, 0, 0, 0, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandlower_right_o7v1: [255, 255, 255, 255, 255, 255, 255, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandupper_right_o7v1: [0, 0, 0, 0, 0, 0, 0, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandlower_left_o7v1: [255, 255, 255, 255, 255, 255, 255, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandupper_left_o7v1: [0, 0, 0, 0, 0, 0, 0, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandlower_right_o7v3: [255, 255, 255, 255, 255, 255, 255, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandupper_right_o7v3: [0, 0, 0, 0, 0, 0, 0, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandlower_left_o7v3: [255, 255, 255, 255, 255, 255, 255, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandupper_left_o7v3: [0, 0, 0, 0, 0, 0, 0, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandlower_right_l25: [255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, None, None, None, None, 255, 255, 255, 255, 255]
+commandupper_right_l25: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, None, None, None, None, 0, 0, 0, 0, 0]
+commandlower_left_l25: [255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 255, None, None, None, None, 255, 255, 255, 255, 255]
+commandupper_left_l25: [0, 0, 0, 0, 0, 0, 255, 255, 255, 255, 0, None, None, None, None, 0, 0, 0, 0, 0]
+commandlower_right_l21: [255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, None, None, None, None, 255, None, None, None, None, 255, 255, 255, 255, 255]
+commandupper_right_l21: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, None, None, None, None, 0, None, None, None, None, 0, 0, 0, 0, 0]
+commandlower_left_l21: [255, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, None, None, None, None, 255, None, None, None, None, 255, 255, 255, 255, 255]
+commandupper_left_l21: [0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, None, None, None, None, 0, None, None, None, None, 0, 0, 0, 0, 0]
+commandlower_right_l6: [255, 255, 255, 255, 255, 255, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandupper_right_l6: [0, 0, 0, 0, 0, 0, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandlower_left_l6: [255, 255, 255, 255, 255, 255, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandupper_left_l6: [0, 0, 0, 0, 0, 0, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandlower_right_o6: [255, 255, 255, 255, 255, 255, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandupper_right_o6: [0, 0, 0, 0, 0, 0, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandlower_left_o6: [255, 255, 255, 255, 255, 255, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandupper_left_o6: [0, 0, 0, 0, 0, 0, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandlower_right_o6v1: [255, 255, 255, 255, 255, 255, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandupper_right_o6v1: [0, 0, 0, 0, 0, 0, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandlower_left_o6v1: [255, 255, 255, 255, 255, 255, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandupper_left_o6v1: [0, 0, 0, 0, 0, 0, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandsourcedataindex_right_l20: [18, 1, 9, 13, 5, 16, 0, 8, 12, 4, 17, None, None, None, None, 19, 2, 10, 14, 6]
+commandsourcedataindex_left_l20: [18, 1, 9, 13, 5, 16, 0, 8, 12, 4, 17, None, None, None, None, 19, 2, 10, 14, 6]
+commandsourcedataindex_right_g20: [18, 1, 9, 13, 5, 16, 0, 8, 12, 4, 17, None, None, None, None, 19, 2, 10, 14, 6]
+commandsourcedataindex_left_g20: [18, 1, 9, 13, 5, 16, 0, 8, 12, 4, 17, None, None, None, None, 19, 2, 10, 14, 6]
+commandsourcedataindex_right_l10: [20, 17, 1, 9, 13, 5, 0, 12, 4, 16, None, None, None, None, None, None, None, None, None, None]
+commandsourcedataindex_left_l10: [20, 17, 1, 9, 13, 5, 0, 12, 4, 16, None, None, None, None, None, None, None, None, None, None]
+commandsourcedataindex_right_l10v7: [20, 17, 1, 9, 13, 5, 0, 12, 4, 16, None, None, None, None, None, None, None, None, None, None]
+commandsourcedataindex_left_l10v7: [20, 17, 1, 9, 13, 5, 0, 12, 4, 16, None, None, None, None, None, None, None, None, None, None]
+commandsourcedataindex_right_t25: [18, 1, 9, 13, 5, 17, 0, None, 12, 4, 16, None, None, None, None, 19, 2, 10, 14, 6, 20, 3, 11, 15, 7]
+commandsourcedataindex_left_t25: [18, 1, 9, 13, 5, 17, 0, None, 12, 4, 16, None, None, None, None, 19, 2, 10, 14, 6, 20, 3, 11, 15, 7]
+commandsourcedataindex_right_l7: [20, 17, 1, 9, 13, 5, 16, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandsourcedataindex_left_l7: [20, 17, 1, 9, 13, 5, 16, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandsourcedataindex_right_o7: [20, 17, 1, 9, 13, 5, 16, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandsourcedataindex_left_o7: [20, 17, 1, 9, 13, 5, 16, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandsourcedataindex_right_o7v1: [20, 17, 1, 9, 13, 5, 16, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandsourcedataindex_left_o7v1: [20, 17, 1, 9, 13, 5, 16, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandsourcedataindex_right_o7v3: [20, 17, 1, 9, 13, 5, 16, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandsourcedataindex_left_o7v3: [20, 17, 1, 9, 13, 5, 16, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandsourcedataindex_right_l25: [18, 1, 9, 13, 5, 16, 0, 8, 12, 4, 17, None, None, None, None, 19, 2, 10, 14, 6]
+commandsourcedataindex_left_l25: [18, 1, 9, 13, 5, 16, 0, 8, 12, 4, 17, None, None, None, None, 19, 2, 10, 14, 6]
+commandsourcedataindex_right_l21: [18, 1, 9, 13, 5, 17, 0, 8, 12, 4, 16, None, None, None, None, 19, None, None, None, None, 20, 3, 11, 15, 7]
+commandsourcedataindex_left_l21: [18, 1, 9, 13, 5, 17, 0, 8, 12, 4, 16, None, None, None, None, 19, None, None, None, None, 20, 3, 11, 15, 7]
+commandsourcedataindex_right_l6: [20, 17, 1, 9, 13, 5, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandsourcedataindex_left_l6: [20, 17, 1, 9, 13, 5, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandsourcedataindex_right_o6: [20, 17, 1, 9, 13, 5, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandsourcedataindex_left_o6: [20, 17, 1, 9, 13, 5, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandsourcedataindex_right_o6v1: [20, 17, 1, 9, 13, 5, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+commandsourcedataindex_left_o6v1: [20, 17, 1, 9, 13, 5, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+urdfdataindex_right_l20: [2, 6, 10, 14, 18, 0, 5, 9, 13, 17, 1, None, None, None, None, 3, 7, 11, 15, 19]
+urdfdataindex_left_l20: [2, 6, 10, 14, 18, 0, 5, 9, 13, 17, 1, None, None, None, None, 3, 7, 11, 15, 19]
+urdfdataindex_right_g20: [2, 6, 10, 14, 18, 0, 5, 9, 13, 17, 1, None, None, None, None, 3, 7, 11, 15, 19]
+urdfdataindex_left_g20: [2, 6, 10, 14, 18, 0, 5, 9, 13, 17, 1, None, None, None, None, 3, 7, 11, 15, 19]
+urdfdataindex_right_l25: [2, 6, 10, 14, 18, 0, 5, 9, 13, 17, 1, None, None, None, None, 3, 7, 11, 15, 19]
+urdfdataindex_left_l25: [2, 6, 10, 14, 18, 0, 5, 9, 13, 17, 1, None, None, None, None, 3, 7, 11, 15, 19]
+urdfdataindex_right_l7: [4, 1, 5, 8, 11, 14, 0, None, None, None, None, None, None, None, None, None, None, None, None, None]
+urdfdataindex_left_l7: [4, 1, 5, 8, 11, 14, 0, None, None, None, None, None, None, None, None, None, None, None, None, None]
+urdfdataindex_right_o7: [4, 1, 5, 8, 11, 14, 0, None, None, None, None, None, None, None, None, None, None, None, None, None]
+urdfdataindex_left_o7: [4, 1, 5, 8, 11, 14, 0, None, None, None, None, None, None, None, None, None, None, None, None, None]
+urdfdataindex_right_o7v1: [4, 1, 5, 8, 11, 14, 0, None, None, None, None, None, None, None, None, None, None, None, None, None]
+urdfdataindex_left_o7v1: [4, 1, 5, 8, 11, 14, 0, None, None, None, None, None, None, None, None, None, None, None, None, None]
+urdfdataindex_right_o7v3: [4, 1, 5, 8, 11, 14, 0, None, None, None, None, None, None, None, None, None, None, None, None, None]
+urdfdataindex_left_o7v3: [4, 1, 5, 8, 11, 14, 0, None, None, None, None, None, None, None, None, None, None, None, None, None]
+urdfdataindex_right_l10: [4, 1, 6, 9, 13, 17, 5, 12, 16, 0, None, None, None, None, None, None, None, None, None, None]
+urdfdataindex_left_l10: [4, 1, 6, 9, 13, 17, 5, 12, 16, 0, None, None, None, None, None, None, None, None, None, None]
+urdfdataindex_right_l10v7: [4, 1, 6, 9, 13, 17, 5, 12, 16, 0, None, None, None, None, None, None, None, None, None, None]
+urdfdataindex_left_l10v7: [4, 1, 6, 9, 13, 17, 5, 12, 16, 0, None, None, None, None, None, None, None, None, None, None]
+urdfdataindex_right_t25: [2, 6, 9, 13, 17, 1, 5, None, 12, 16, 0, None, None, None, None, 3, 7, 10, 14, 18, 4, 8, 11, 15, 19]
+urdfdataindex_left_t25: [2, 6, 9, 13, 17, 1, 5, None, 12, 16, 0, None, None, None, None, 3, 7, 10, 14, 18, 4, 8, 11, 15, 19]
+urdfdataindex_right_l21: [14, 1, 4, 7, 10, 13, 0, 3, 6, 9, 12, None, None, None, None, 15, None, None, None, None, 16, 2, 5, 8, 11]
+urdfdataindex_left_l21: [14, 1, 4, 7, 10, 13, 0, 3, 6, 9, 12, None, None, None, None, 15, None, None, None, None, 16, 2, 5, 8, 11]
+urdfdataindex_right_l6: [1, 0, 3, 5, 7, 9, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+urdfdataindex_left_l6: [1, 0, 3, 5, 7, 9, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+urdfdataindex_right_o6: [1, 0, 3, 5, 7, 9, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+urdfdataindex_left_o6: [1, 0, 3, 5, 7, 9, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+urdfdataindex_right_o6v1: [1, 0, 3, 5, 7, 9, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
+urdfdataindex_left_o6v1: [1, 0, 3, 5, 7, 9, None, None, None, None, None, None, None, None, None, None, None, None, None, None]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/human_hand_info.yml b/src/linkerhand_retarget/linkerhand_retarget/config/human_hand_info.yml
new file mode 100644
index 0000000..2918085
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/human_hand_info.yml
@@ -0,0 +1,53 @@
+initial_positions:
+ right_hand:
+ - [0.00000, 0.00000, 0.00000]
+ - [0.03380, 0.04230, 0.00000]
+ - [0.06920, 0.07760, 0.00000]
+ - [0.09370, 0.10220, 0.00000]
+ - [0.11820, 0.12680, 0.00000]
+ - [0.04380, 0.02680, 0.00000]
+ - [0.11460, 0.04040, 0.00000]
+ - [0.16370, 0.04040, 0.00000]
+ - [0.19150, 0.04040, 0.00000]
+ - [0.21750, 0.04040, 0.00000]
+ - [0.04590, 0.01030, 0.00000]
+ - [0.11610, 0.01450, 0.00000]
+ - [0.16960, 0.01450, 0.00000]
+ - [0.20310, 0.01450, 0.00000]
+ - [0.22910, 0.01450, 0.00000]
+ - [0.04570, -0.00180, 0.00000]
+ - [0.10860, -0.00830, 0.00000]
+ - [0.15520, -0.00830, 0.00000]
+ - [0.18750, -0.00830, 0.00000]
+ - [0.21350, -0.00830, 0.00000]
+ - [0.04290, -0.01630, 0.00000]
+ - [0.09910, -0.03110, 0.00000]
+ - [0.13650, -0.03110, 0.00000]
+ - [0.16000, -0.03110, 0.00000]
+ - [0.18600, -0.03110, 0.00000]
+ left_hand:
+ - [0.00000, 0.00000, 0.00000]
+ - [-0.03380, 0.04230, 0.00000]
+ - [-0.06920, 0.07760, 0.00000]
+ - [-0.09370, 0.10220, 0.00000]
+ - [-0.11820, 0.12680, 0.00000]
+ - [-0.04380, 0.02680, 0.00000]
+ - [-0.11460, 0.04040, 0.00000]
+ - [-0.16370, 0.04040, 0.00000]
+ - [-0.19150, 0.04040, 0.00000]
+ - [-0.21750, 0.04040, 0.00000]
+ - [-0.04590, 0.01030, 0.00000]
+ - [-0.11610, 0.01450, 0.00000]
+ - [-0.16960, 0.01450, 0.00000]
+ - [-0.20310, 0.01450, 0.00000]
+ - [-0.22910, 0.01450, 0.00000]
+ - [-0.04570, -0.00180, 0.00000]
+ - [-0.10860, -0.00830, 0.00000]
+ - [-0.15520, -0.00830, 0.00000]
+ - [-0.18750, -0.00830, 0.00000]
+ - [-0.21350, -0.00830, 0.00000]
+ - [-0.04290, -0.01630, 0.00000]
+ - [-0.09910, -0.03110, 0.00000]
+ - [-0.13650, -0.03110, 0.00000]
+ - [-0.16000, -0.03110, 0.00000]
+ - [-0.18600, -0.03110, 0.00000]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/linker_hand_info.yml b/src/linkerhand_retarget/linkerhand_retarget/config/linker_hand_info.yml
new file mode 100644
index 0000000..e12a786
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/linker_hand_info.yml
@@ -0,0 +1,289 @@
+initial_positions:
+# right_hand:
+# - [0.00000, 0.00000, 0.00000]
+# - [0.03380, 0.04230, 0.00000]
+# - [0.06920, 0.07760, 0.00000]
+# - [0.09370, 0.10220, 0.00000]
+# - [0.11820, 0.12680, 0.00000]
+# - [0.04380, 0.02680, 0.00000]
+# - [0.11460, 0.04040, 0.00000]
+# - [0.16370, 0.04040, 0.00000]
+# - [0.19150, 0.04040, 0.00000]
+# - [0.21750, 0.04040, 0.00000]
+# - [0.04590, 0.01030, 0.00000]
+# - [0.11610, 0.01450, 0.00000]
+# - [0.16960, 0.01450, 0.00000]
+# - [0.20310, 0.01450, 0.00000]
+# - [0.22910, 0.01450, 0.00000]
+# - [0.04570, -0.00180, 0.00000]
+# - [0.10860, -0.00830, 0.00000]
+# - [0.15520, -0.00830, 0.00000]
+# - [0.18750, -0.00830, 0.00000]
+# - [0.21350, -0.00830, 0.00000]
+# - [0.04290, -0.01630, 0.00000]
+# - [0.09910, -0.03110, 0.00000]
+# - [0.13650, -0.03110, 0.00000]
+# - [0.16000, -0.03110, 0.00000]
+# - [0.18600, -0.03110, 0.00000]
+ right_hand:
+ l16_3: # l20_8_右手
+ - [ 0.0, 0.0, 0.0 ]
+ - [ 0.013, 0.041, 0.008 ]
+ - [ 0.015, 0.058, -0.002 ]
+ - [ 0.048, 0.086, -0.002 ]
+ - [ 0.092, 0.127, 0.008 ]
+ - [ 0.073, 0.032, -0.012 ]
+ - [ 0.082, 0.041, -0.003 ]
+ - [ 0.126, 0.041, 0.0 ]
+ - [ 0.170, 0.041, 0.0 ]
+ - [ 0.209, 0.032, 0.0 ]
+ - [ 0.073, 0.0, -0.012 ]
+ - [ 0.082, 0.009, -0.003 ]
+ - [ 0.126, 0.009, 0.0 ]
+ - [ 0.170, 0.009, 0.0 ]
+ - [ 0.209, 0.0, 0.0 ]
+ - [ 0.073, -0.032, -0.012 ]
+ - [ 0.082, -0.023, -0.003 ]
+ - [ 0.126, -0.023, 0.0 ]
+ - [ 0.170, -0.023, 0.0 ]
+ - [ 0.209, -0.033, 0.0 ]
+ - [ 0.073, -0.064, -0.012 ]
+ - [ 0.082, -0.055, -0.003 ]
+ - [ 0.126, -0.055, 0.0 ]
+ - [ 0.170, -0.055, 0.0 ]
+ - [ 0.209, -0.064, 0.0 ]
+ l20_8: # l20_8_右手
+ - [0.0, 0.0, 0.0]
+ - [0.0490, 0.0080, -0.0220]
+ - [0.1210, 0.0560, -0.0320]
+ - [0.1490, 0.0760, -0.0190]
+ - [0.1690, 0.1000, -0.0230]
+ - [0.1540, 0.0270, 0.0290]
+ - [0.1540, 0.0310, 0.0150]
+ - [0.1990, 0.0290, 0.0180]
+ - [0.2300, 0.0350, 0.0100]
+ - [0.2550, 0.0270, 0.0230]
+ - [0.1590, 0.0050, 0.0330]
+ - [0.1590, 0.0100, 0.0190]
+ - [0.2040, 0.0070, 0.0230]
+ - [0.2340, 0.0130, 0.0150]
+ - [0.2590, 0.0060, 0.0280]
+ - [0.1540, -0.0170, 0.0310]
+ - [0.1540, -0.0120, 0.0170]
+ - [0.1990, -0.0140, 0.0210]
+ - [0.2300, -0.0090, 0.0120]
+ - [0.2550, -0.0160, 0.0260]
+ - [0.1450, -0.0380, 0.0270]
+ - [0.1450, -0.0330, 0.0140]
+ - [0.1900, -0.0360, 0.0170]
+ - [0.2210, -0.0300, 0.0090]
+ - [0.2460, -0.0380, 0.0220]
+ l20_6t: # l20_6t_右手
+ - [0.0, 0.0, 0.0]
+ - [0.0862, 0.0285, -0.0259]
+ - [0.1505, 0.0810, -0.0359]
+ - [0.1722, 0.1079, -0.0338]
+ - [0.1920, 0.1248, -0.0338]
+ - [0.1827, 0.0267, 0.0231]
+ - [0.1827, 0.0219, 0.0127]
+ - [0.2275, 0.0212, 0.0169]
+ - [0.2592, 0.0189, 0.0136]
+ - [0.2827, 0.0189, 0.0331]
+ - [0.1872, 0.0051, 0.0278]
+ - [0.1872, -0.0003, 0.0174]
+ - [0.2320, -0.0004, 0.0216]
+ - [0.2637, -0.0027, 0.0183]
+ - [0.2872, -0.0027, 0.0378]
+ - [0.1827, -0.0165, 0.0255]
+ - [0.1827, -0.0213, 0.0151]
+ - [0.2275, -0.0220, 0.0193]
+ - [0.2592, -0.0243, 0.0160]
+ - [0.2827, -0.0243, 0.0355]
+ - [0.1782, -0.0381, 0.0219]
+ - [0.1782, -0.0429, 0.0115]
+ - [0.2230, -0.0436, 0.0157]
+ - [0.2547, -0.0459, 0.0124]
+ - [0.2782, -0.0459, 0.0319]
+ l10_6: # l10_6右手
+ - [0.0, 0.0, 0.0]
+ - [0.085, 0.047, -0.013]
+ - [0.128, 0.075, -0.014]
+ - [0.154, 0.099, -0.009]
+ - [0.168, 0.123, -0.001]
+ - [0.158, 0.023, -0.012]
+ - [0.176, 0.031, 0.001]
+ - [0.209, 0.028, 0.006]
+ - [0.234, 0.028, 0.016]
+ - [0.256, 0.024, 0.031]
+ - [0.181, 0.012, 0.003]
+ - [0.181, 0.012, 0.003]
+ - [0.214, 0.009, 0.008]
+ - [0.239, 0.009, 0.018]
+ - [0.261, 0.005, 0.033]
+ - [0.158, -0.015, -0.012]
+ - [0.176, -0.007, 0.001]
+ - [0.209, -0.010, 0.006]
+ - [0.234, -0.010, 0.016]
+ - [0.256, -0.014, 0.031]
+ - [0.153, -0.034, -0.014]
+ - [0.171, -0.026, -0.001]
+ - [0.204, -0.029, 0.004]
+ - [0.229, -0.029, 0.014]
+ - [0.251, -0.033, 0.029]
+ t24_1: # t24_1_右手
+ - [0, 0, 0]
+ - [0.0680, 0.0210, -0.0100]
+ - [0.1020, 0.0430, -0.0120]
+ - [0.1360, 0.0700, -0.0220]
+ - [0.1620, 0.1030, -0.0400]
+ - [0.1340, 0.0340, -0.0140]
+ - [0.1510, 0.0360, -0.0080]
+ - [0.1930, 0.0400, -0.0080]
+ - [0.2370, 0.0400, -0.0060]
+ - [0.2820, 0.0340, 0.0050]
+ - [0.1420, 0.0110, -0.0140]
+ - [0.1590, 0.0140, -0.0080]
+ - [0.2010, 0.0170, -0.0080]
+ - [0.2450, 0.0170, -0.0060]
+ - [0.2890, 0.0110, 0.0070]
+ - [0.1320, -0.0110, -0.0140]
+ - [0.1490, -0.0090, -0.0080]
+ - [0.1910, -0.0050, -0.0090]
+ - [0.2360, -0.0060, -0.0070]
+ - [0.2800, -0.0120, 0.0040]
+ - [0.1190, -0.0340, -0.0140]
+ - [0.1360, -0.0320, -0.0080]
+ - [0.1780, -0.0280, -0.0070]
+ - [0.2220, -0.0280, -0.0040]
+ - [0.2670, -0.0340, 0.0070]
+# right_hand: # Human
+# - [0.00000, 0.00000, 0.00000]
+# - [0.03380, 0.04230, 0.00000]
+# - [0.06920, 0.07760, 0.00000]
+# - [0.09370, 0.10220, 0.00000]
+# - [0.11820, 0.12680, 0.00000]
+# - [0.04380, 0.02680, 0.00000]
+# - [0.11460, 0.04040, 0.00000]
+# - [0.16370, 0.04040, 0.00000]
+# - [0.19150, 0.04040, 0.00000]
+# - [0.21750, 0.04040, 0.00000]
+# - [0.04590, 0.01030, 0.00000]
+# - [0.11610, 0.01450, 0.00000]
+# - [0.16960, 0.01450, 0.00000]
+# - [0.20310, 0.01450, 0.00000]
+# - [0.22910, 0.01450, 0.00000]
+# - [0.04570, -0.00180, 0.00000]
+# - [0.10860, -0.00830, 0.00000]
+# - [0.15520, -0.00830, 0.00000]
+# - [0.18750, -0.00830, 0.00000]
+# - [0.21350, -0.00830, 0.00000]
+# - [0.04290, -0.01630, 0.00000]
+# - [0.09910, -0.03110, 0.00000]
+# - [0.13650, -0.03110, 0.00000]
+# - [0.16000, -0.03110, 0.00000]
+# - [0.18600, -0.03110, 0.00000]
+ left_hand: # l18_2_左手
+ l16_3:
+ - [0.0, 0.0, 0.0]
+ - [-0.0130, 0.0520, -0.0360]
+ - [-0.0450, 0.0630, -0.0360]
+ - [-0.0780, 0.0740, -0.0360]
+ - [-0.1200, 0.0810, -0.0270]
+ - [-0.0760, 0.0320, -0.0150]
+ - [-0.0820, 0.0220, -0.0030]
+ - [-0.1170, 0.0220, -0.0030]
+ - [-0.1510, 0.0220, -0.0030]
+ - [-0.1930, 0.0310, -0.0100]
+ - [-0.0760, 0.0000, -0.0150]
+ - [-0.0820, -0.0100, -0.0030]
+ - [-0.1170, -0.0100, -0.0030]
+ - [-0.1510, -0.0100, -0.0030]
+ - [-0.1930, -0.0010, -0.0100]
+ - [-0.0760, -0.0320, -0.0150]
+ - [-0.0820, -0.0420, -0.0030]
+ - [-0.1170, -0.0420, -0.0030]
+ - [-0.1510, -0.0420, -0.0030]
+ - [-0.1930, -0.0330, -0.0100]
+ - [-0.0760, -0.0640, -0.0150]
+ - [-0.0820, -0.0100, -0.0030]
+ - [-0.1170, -0.0100, -0.0030]
+ - [-0.1510, -0.0100, -0.0030]
+ - [-0.1930, -0.0010, -0.0100]
+ l20_8: # l20_8_右手
+ - [0.0, 0.0, 0.0]
+ - [-0.0490, 0.0080, -0.0220]
+ - [-0.1210, 0.0560, -0.0320]
+ - [-0.1490, 0.0760, -0.0190]
+ - [-0.1690, 0.1000, -0.0230]
+ - [-0.1540, 0.0270, 0.0290]
+ - [-0.1540, 0.0310, 0.0150]
+ - [-0.1990, 0.0290, 0.0180]
+ - [-0.2300, 0.0350, 0.0100]
+ - [-0.2550, 0.0270, 0.0230]
+ - [-0.1590, 0.0050, 0.0330]
+ - [-0.1590, 0.0100, 0.0190]
+ - [-0.2040, 0.0070, 0.0230]
+ - [-0.2340, 0.0130, 0.0150]
+ - [-0.2590, 0.0060, 0.0280]
+ - [-0.1540, -0.0170, 0.0310]
+ - [-0.1540, -0.0120, 0.0170]
+ - [-0.1990, -0.0140, 0.0210]
+ - [-0.2300, -0.0090, 0.0120]
+ - [-0.2550, -0.0160, 0.0260]
+ - [-0.1450, -0.0380, 0.0270]
+ - [-0.1450, -0.0330, 0.0140]
+ - [-0.1900, -0.0360, 0.0170]
+ - [-0.2210, -0.0300, 0.0090]
+ - [-0.2460, -0.0380, 0.0220]
+ l10_6: # l10_6右手
+ - [0.0, 0.0, 0.0]
+ - [-0.085, 0.047, -0.013]
+ - [-0.128, 0.075, -0.014]
+ - [-0.154, 0.099, -0.009]
+ - [-0.168, 0.123, -0.001]
+ - [-0.158, 0.023, -0.012]
+ - [-0.176, 0.031, 0.001]
+ - [-0.209, 0.028, 0.006]
+ - [-0.234, 0.028, 0.016]
+ - [-0.256, 0.024, 0.031]
+ - [-0.181, 0.012, 0.003]
+ - [-0.181, 0.012, 0.003]
+ - [-0.214, 0.009, 0.008]
+ - [-0.239, 0.009, 0.018]
+ - [-0.261, 0.005, 0.033]
+ - [-0.158, -0.015, -0.012]
+ - [-0.176, -0.007, 0.001]
+ - [-0.209, -0.010, 0.006]
+ - [-0.234, -0.010, 0.016]
+ - [-0.256, -0.014, 0.031]
+ - [-0.153, -0.034, -0.014]
+ - [-0.171, -0.026, -0.001]
+ - [-0.204, -0.029, 0.004]
+ - [-0.229, -0.029, 0.014]
+ - [-0.251, -0.033, 0.029]
+ t24_1: # t24_1_右手
+ - [0, 0, 0]
+ - [-0.0820, 0.0170, -0.0090]
+ - [-0.1090, 0.0480, -0.0130]
+ - [-0.1320, 0.0840, -0.0250]
+ - [-0.1480, 0.1230, -0.0420]
+ - [-0.1340, 0.0340, -0.0140]
+ - [-0.1510, 0.0360, -0.0080]
+ - [-0.1930, 0.0400, -0.0080]
+ - [-0.2370, 0.0400, -0.0060]
+ - [-0.2820, 0.0340, 0.0050]
+ - [-0.1420, 0.0110, -0.0140]
+ - [-0.1590, 0.0140, -0.0080]
+ - [-0.2010, 0.0170, -0.0080]
+ - [-0.2450, 0.0170, -0.0060]
+ - [-0.2890, 0.0110, 0.0070]
+ - [-0.1320, -0.0110, -0.0140]
+ - [-0.1490, -0.0090, -0.0080]
+ - [-0.1910, -0.0050, -0.0090]
+ - [-0.2360, -0.0060, -0.0070]
+ - [-0.2800, -0.0120, 0.0040]
+ - [-0.1190, -0.0340, -0.0140]
+ - [-0.1360, -0.0320, -0.0080]
+ - [-0.1780, -0.0280, -0.0070]
+ - [-0.2220, -0.0280, -0.0040]
+ - [-0.2670, -0.0340, 0.0070]
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/model_config.yml b/src/linkerhand_retarget/linkerhand_retarget/config/model_config.yml
new file mode 100644
index 0000000..fb1ed63
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/model_config.yml
@@ -0,0 +1,410 @@
+target_position_end:
+ right_hand:
+ l20_8:
+ - [0.005, -0.034, 0.15] #Trumb + littleroot
+ - [0.006, -0.018, 0.149] #Trumb + littleroot
+ - [0.016, 0.002, 0.155] #Trumb + littleroot
+ - [0.023, 0.017, 0.149] #Trumb + littleroot
+ - [0.023, 0.144, 0.112] #Trumb default
+ - [0.023, 0.061, 0.184] #Trumb nearly index
+ - [0.077, 0.018, 0.179] #Trumb + index + print
+ l10_6:
+ - [0.011, 0.045, 0.126] #Trumb + littleroot
+ - [0.012, 0.022, 0.137] #Trumb + littleroot
+ - [0.011, 0.03, 0.141] #Trumb + littleroot
+ - [0.013, -0.013, 0.141] #Trumb + littleroot
+ - [0.032, -0.107, 0.145] #Trumb default
+ - [0.030, -0.051, 0.179] #Trumb nearly index
+ - [0.098, -0.020, 0.167] #Trumb + index + print
+ l16_3:
+ - [ 0.0120, 0.0320, 0.0730]
+ - [ 0.0130, 0.0410, 0.1260]
+ - [ 0.0850, 0.0310, 0.1010]
+ - [-0.0080, 0.1270, 0.0920]
+ - [-0.0009, 0.0610, 0.1180]
+ t24_1:
+ - [0.005, -0.034, 0.15] #Trumb + littleroot
+ - [0.006, -0.018, 0.149] #Trumb + littleroot
+ - [0.016, 0.002, 0.155] #Trumb + littleroot
+ - [0.023, 0.017, 0.149] #Trumb + littleroot
+ - [0.023, 0.144, 0.112] #Trumb default
+ - [0.023, 0.061, 0.184] #Trumb nearly index
+ - [0.077, 0.018, 0.179] #Trumb + index + print
+ left_hand:
+ l18_2:
+ - [-0.5000, 0.0000, 0.1000]
+ - [-0.5270, 0.0270, 0.0974]
+ - [-0.5927, 0.0723, 0.0965]
+ - [-0.6121, 0.0904, 0.0959]
+ - [-0.5000, 0.0000, 0.1000]
+ l20_8:
+ - [0.005, 0.034, 0.15] #Trumb + littleroot
+ - [0.006, 0.018, 0.149] #Trumb + littleroot
+ - [0.016, -0.002, 0.155] #Trumb + littleroot
+ - [0.023, -0.017, 0.149] #Trumb + littleroot
+ - [0.023, -0.144, 0.112] #Trumb default
+ - [0.023, -0.061, 0.184] #Trumb nearly index
+ - [0.077, -0.018, 0.179] #Trumb + index + print
+ l10_6:
+ - [0.078, 0.000, 0.123] #Trumb + littleroot
+ - [0.078, -0.006, 0.126] #Trumb + littleroot
+ - [0.078, -0.012, 0.127] #Trumb + littleroot
+ - [0.078, -0.018, 0.169] #Trumb + littleroot
+ - [0.032, -0.107, 0.145] #Trumb default
+ - [0.030, -0.051, 0.179] #Trumb nearly index
+ - [0.064, -0.017, 0.192] #Trumb + index + print
+ l16_3:
+ - [-0.5000, 0.0000, 0.1000]
+ - [-0.5270, 0.0270, 0.0974]
+ - [-0.5927, 0.0723, 0.0965]
+ - [-0.6121, 0.0904, 0.0959]
+ - [-0.5000, 0.0000, 0.1000]
+ t24_1:
+ - [0.005, -0.034, 0.15] #Trumb + littleroot
+ - [0.006, -0.018, 0.149] #Trumb + littleroot
+ - [0.016, 0.002, 0.155] #Trumb + littleroot
+ - [0.023, 0.017, 0.149] #Trumb + littleroot
+ - [0.023, 0.144, 0.112] #Trumb default
+ - [0.023, 0.061, 0.184] #Trumb nearly index
+ - [0.077, 0.018, 0.179] #Trumb + index + print
+refer_position_end:
+ right_hand:
+ l20_8:
+ - [0.09, -0.05, 0.11]
+ - [0.1, -0.05, 0.14]
+ - [0.085, -0.03, 0.17]
+ - [0.05, -0.02, 0.18]
+ - [0.05, -0.02, 0.18]
+ - [0.05, -0.02, 0.18]
+ - [0.05, -0.02, 0.18]
+ l10_6:
+ - [0.023, 0.034, 0.153] #Trumb + littleroot
+ - [0.023, 0.015, 0.158] #Trumb + littleroot
+ - [0.023, -0.012, 0.181] #Trumb + littleroot
+ - [0.023, -0.023, 0.158] #Trumb + littleroot
+ - [0.096, -0.122, 0.127] #Trumb default
+ - [0.025, -0.043, 0.175] #Trumb nearly index
+ - [0.058, -0.024, 0.184] #Trumb + index + print
+ l16_3:
+ - [ 0.0120, 0.0320, 0.0730]
+ - [ 0.0130, 0.0410, 0.1260]
+ - [ 0.0850, 0.0310, 0.1010]
+ - [-0.0080, 0.1270, 0.0920]
+ - [-0.0009, 0.0610, 0.1180]
+ t24_1:
+ - [0.005, -0.034, 0.15] #Trumb + littleroot
+ - [0.006, -0.018, 0.149] #Trumb + littleroot
+ - [0.016, 0.002, 0.155] #Trumb + littleroot
+ - [0.023, 0.017, 0.149] #Trumb + littleroot
+ - [0.023, 0.144, 0.112] #Trumb default
+ - [0.023, 0.061, 0.184] #Trumb nearly index
+ - [0.077, 0.018, 0.179] #Trumb + index + print
+ left_hand:
+ l18_2:
+ - [-0.5000, 0.0000, 0.1100]
+ - [-0.5270, 0.0270, 0.0974]
+ - [-0.5927, 0.0723, 0.0965]
+ - [-0.6121, 0.0904, 0.0959]
+ l20_8:
+ - [0.09, -0.05, 0.11]
+ - [0.1, -0.05, 0.14]
+ - [0.085, -0.03, 0.17]
+ - [0.05, -0.02, 0.18]
+ - [0.05, -0.02, 0.18]
+ - [0.05, -0.02, 0.18]
+ - [0.05, -0.02, 0.18]
+ l10_6:
+ - [0.011, 0.045, 0.126] #Trumb + littleroot
+ - [0.012, 0.022, 0.137] #Trumb + littleroot
+ - [0.011, 0.03, 0.141] #Trumb + littleroot
+ - [0.013, -0.013, 0.141] #Trumb + littleroot
+ - [0.032, -0.107, 0.145] #Trumb default
+ - [0.030, -0.051, 0.179] #Trumb nearly index
+ - [0.098, -0.020, 0.167] #Trumb + index + print
+ l16_3:
+ - [-0.5000, 0.0000, 0.1000]
+ - [-0.5270, 0.0270, 0.0974]
+ - [-0.5927, 0.0723, 0.0965]
+ - [-0.6121, 0.0904, 0.0959]
+ - [-0.5000, 0.0000, 0.1000]
+ t24_1:
+ - [0.005, -0.034, 0.15] #Trumb + littleroot
+ - [0.006, -0.018, 0.149] #Trumb + littleroot
+ - [0.016, 0.002, 0.155] #Trumb + littleroot
+ - [0.023, 0.017, 0.149] #Trumb + littleroot
+ - [0.023, 0.144, 0.112] #Trumb default
+ - [0.023, 0.061, 0.184] #Trumb nearly index
+ - [0.077, 0.018, 0.179] #Trumb + index + print
+refer_rangerange_end:
+ right_hand:
+ l20_8:
+ - [0.02, 0.01, 0.01]
+ - [0.05, 0.01, 0.01]
+ - [0.01, 0.01, 0.01]
+ - [0.01, 0.01, 0.01]
+ - [0.01, 0.01, 0.01]
+ - [0.01, 0.01, 0.01]
+ - [0.01, 0.01, 0.01 ]
+ l10_6:
+ - [0.011, 0.045, 0.126] #Trumb + littleroot
+ - [0.012, 0.022, 0.137] #Trumb + littleroot
+ - [0.011, 0.03, 0.141] #Trumb + littleroot
+ - [0.013, -0.013, 0.141] #Trumb + littleroot
+ - [0.032, -0.107, 0.145] #Trumb default
+ - [0.030, -0.051, 0.179] #Trumb nearly index
+ - [0.098, -0.020, 0.167] #Trumb + index + print
+ l16_3:
+ - [ 0.0120, 0.0320, 0.0730]
+ - [ 0.0130, 0.0410, 0.1260]
+ - [ 0.0850, 0.0310, 0.1010]
+ - [-0.0080, 0.1270, 0.0920]
+ - [-0.0009, 0.0610, 0.1180]
+ t24_1:
+ - [0.005, -0.034, 0.15] #Trumb + littleroot
+ - [0.006, -0.018, 0.149] #Trumb + littleroot
+ - [0.016, 0.002, 0.155] #Trumb + littleroot
+ - [0.023, 0.017, 0.149] #Trumb + littleroot
+ - [0.023, 0.144, 0.112] #Trumb default
+ - [0.023, 0.061, 0.184] #Trumb nearly index
+ - [0.077, 0.018, 0.179] #Trumb + index + print
+ left_hand:
+ l18_2:
+ - [-0.5000, 0.0000, 0.1000]
+ - [-0.5270, 0.0270, 0.0974]
+ - [-0.5927, 0.0723, 0.0965]
+ - [-0.6121, 0.0904, 0.0959]
+ l20_8:
+ - [0.02, 0.01, 0.01]
+ - [0.05, 0.01, 0.01]
+ - [0.01, 0.01, 0.01]
+ - [0.01, 0.01, 0.01]
+ - [0.01, 0.01, 0.01]
+ - [0.01, 0.01, 0.01]
+ - [0.01, 0.01, 0.01 ]
+ l10_6:
+ - [0.011, 0.045, 0.126] #Trumb + littleroot
+ - [0.012, 0.022, 0.137] #Trumb + littleroot
+ - [0.011, 0.03, 0.141] #Trumb + littleroot
+ - [0.013, -0.013, 0.141] #Trumb + littleroot
+ - [0.032, -0.107, 0.145] #Trumb default
+ - [0.030, -0.051, 0.179] #Trumb nearly index
+ - [0.098, -0.020, 0.167] #Trumb + index + print
+ l16_3:
+ - [-0.5000, 0.0000, 0.1000]
+ - [-0.5270, 0.0270, 0.0974]
+ - [-0.5927, 0.0723, 0.0965]
+ - [-0.6121, 0.0904, 0.0959]
+ - [-0.5000, 0.0000, 0.1000]
+ t24_1:
+ - [0.005, -0.034, 0.15] #Trumb + littleroot
+ - [0.006, -0.018, 0.149] #Trumb + littleroot
+ - [0.016, 0.002, 0.155] #Trumb + littleroot
+ - [0.023, 0.017, 0.149] #Trumb + littleroot
+ - [0.023, 0.144, 0.112] #Trumb default
+ - [0.023, 0.061, 0.184] #Trumb nearly index
+ - [0.077, 0.018, 0.179] #Trumb + index + print
+target_position_secondary:
+ right_hand:
+ l20_8:
+ - [0.051, -0.008, 0.114]
+ - [0.059, -0.012, 0.114]
+ - [0.064, 0.015, 0.114]
+ - [0.032, 0.061, 0.104]
+ - [0.032, 0.081, 0.090]
+ - [0.032, 0.045, 0.128]
+ - [0.064, 0.006, 0.115]
+ l10_6:
+ - [0.011, 0.045, 0.126] #Trumb + littleroot
+ - [0.012, 0.022, 0.137] #Trumb + littleroot
+ - [0.011, 0.03, 0.141] #Trumb + littleroot
+ - [0.013, -0.013, 0.141] #Trumb + littleroot
+ - [0.032, -0.107, 0.145] #Trumb default
+ - [0.030, -0.051, 0.179] #Trumb nearly index
+ - [0.098, -0.020, 0.167] #Trumb + index + print
+ l16_3:
+ - [ 0.0120, 0.0320, 0.0730]
+ - [ 0.0130, 0.0410, 0.1260]
+ - [ 0.0850, 0.0310, 0.1010]
+ - [-0.0080, 0.1270, 0.0920]
+ - [-0.0009, 0.0610, 0.1180]
+ t24_1:
+ - [0.005, -0.034, 0.15] #Trumb + littleroot
+ - [0.006, -0.018, 0.149] #Trumb + littleroot
+ - [0.016, 0.002, 0.155] #Trumb + littleroot
+ - [0.023, 0.017, 0.149] #Trumb + littleroot
+ - [0.023, 0.144, 0.112] #Trumb default
+ - [0.023, 0.061, 0.184] #Trumb nearly index
+ - [0.077, 0.018, 0.179] #Trumb + index + print
+ left_hand:
+ l18_2:
+ - [-0.5000, 0.0000, 0.1000]
+ - [-0.5270, 0.0270, 0.0974]
+ - [-0.5927, 0.0723, 0.0965]
+ - [-0.6121, 0.0904, 0.0959]
+ - [-0.5000, 0.0000, 0.1000]
+ l20_8:
+ - [0.051, 0.008, 0.114]
+ - [0.059, 0.012, 0.114]
+ - [0.064, -0.015, 0.114]
+ - [0.032, -0.061, 0.104]
+ - [0.032, -0.081, 0.090]
+ - [0.032, -0.045, 0.128]
+ - [0.064, -0.006, 0.115]
+ l10_6:
+ - [0.011, 0.045, 0.126] #Trumb + littleroot
+ - [0.012, 0.022, 0.137] #Trumb + littleroot
+ - [0.011, 0.03, 0.141] #Trumb + littleroot
+ - [0.013, -0.013, 0.141] #Trumb + littleroot
+ - [0.032, -0.107, 0.145] #Trumb default
+ - [0.030, -0.051, 0.179] #Trumb nearly index
+ - [0.098, -0.020, 0.167] #Trumb + index + print
+ l16_3:
+ - [-0.5000, 0.0000, 0.1000]
+ - [-0.5270, 0.0270, 0.0974]
+ - [-0.5927, 0.0723, 0.0965]
+ - [-0.6121, 0.0904, 0.0959]
+ - [-0.5000, 0.0000, 0.1000]
+ t24_1:
+ - [0.005, -0.034, 0.15] #Trumb + littleroot
+ - [0.006, -0.018, 0.149] #Trumb + littleroot
+ - [0.016, 0.002, 0.155] #Trumb + littleroot
+ - [0.023, 0.017, 0.149] #Trumb + littleroot
+ - [0.023, 0.144, 0.112] #Trumb default
+ - [0.023, 0.061, 0.184] #Trumb nearly index
+ - [0.077, 0.018, 0.179] #Trumb + index + print
+refer_position_secondary:
+ right_hand:
+ l20_8:
+ - [0.09, 0, 0.1]
+ - [0.07, 0, 0.12]
+ - [0.06, 0, 0.125]
+ - [0.04, 0.04, 0.13]
+ - [0.032, 0.081, 0.090]
+ - [0.032, 0.045, 0.128]
+ - [0.032, 0.045, 0.128]
+ l10_6:
+ - [ 0.011, 0.045, 0.126 ] #Trumb + littleroot
+ - [ 0.012, 0.022, 0.137 ] #Trumb + littleroot
+ - [ 0.011, 0.03, 0.141 ] #Trumb + littleroot
+ - [ 0.013, -0.013, 0.141 ] #Trumb + littleroot
+ - [ 0.032, -0.107, 0.145 ] #Trumb default
+ - [ 0.030, -0.051, 0.179 ] #Trumb nearly index
+ - [ 0.098, -0.020, 0.167 ] #Trumb + index + print
+ l16_3:
+ - [ 0.0120, 0.0320, 0.0730]
+ - [ 0.0130, 0.0410, 0.1260]
+ - [ 0.0850, 0.0310, 0.1010]
+ - [-0.0080, 0.1270, 0.0920]
+ - [-0.0009, 0.0610, 0.1180]
+ t24_1:
+ - [0.005, -0.034, 0.15] #Trumb + littleroot
+ - [0.006, -0.018, 0.149] #Trumb + littleroot
+ - [0.016, 0.002, 0.155] #Trumb + littleroot
+ - [0.023, 0.017, 0.149] #Trumb + littleroot
+ - [0.023, 0.144, 0.112] #Trumb default
+ - [0.023, 0.061, 0.184] #Trumb nearly index
+ - [0.077, 0.018, 0.179] #Trumb + index + print
+ left_hand:
+ l18_2:
+ - [-0.5000, 0.0000, 0.1100]
+ - [-0.5270, 0.0270, 0.0974]
+ - [-0.5927, 0.0723, 0.0965]
+ - [-0.6121, 0.0904, 0.0959]
+ l20_8:
+ - [0.09, 0, 0.1]
+ - [0.07, 0, 0.12]
+ - [0.06, 0, 0.125]
+ - [0.04, 0.04, 0.13]
+ - [0.032, 0.081, 0.090]
+ - [0.032, 0.045, 0.128]
+ - [0.032, 0.045, 0.128]
+ l10_6:
+ - [0.011, 0.045, 0.126] #Trumb + littleroot
+ - [0.012, 0.022, 0.137] #Trumb + littleroot
+ - [0.011, 0.03, 0.141] #Trumb + littleroot
+ - [0.013, -0.013, 0.141] #Trumb + littleroot
+ - [0.032, -0.107, 0.145] #Trumb default
+ - [0.030, -0.051, 0.179] #Trumb nearly index
+ - [0.098, -0.020, 0.167] #Trumb + index + print
+ l16_3:
+ - [-0.5000, 0.0000, 0.1000]
+ - [-0.5270, 0.0270, 0.0974]
+ - [-0.5927, 0.0723, 0.0965]
+ - [-0.6121, 0.0904, 0.0959]
+ - [-0.5000, 0.0000, 0.1000]
+ t24_1:
+ - [0.005, -0.034, 0.15] #Trumb + littleroot
+ - [0.006, -0.018, 0.149] #Trumb + littleroot
+ - [0.016, 0.002, 0.155] #Trumb + littleroot
+ - [0.023, 0.017, 0.149] #Trumb + littleroot
+ - [0.023, 0.144, 0.112] #Trumb default
+ - [0.023, 0.061, 0.184] #Trumb nearly index
+ - [0.077, 0.018, 0.179] #Trumb + index + print
+refer_rangerange_secondary:
+ right_hand:
+ l20_8:
+ - [0.02, 0.01, 0.01]
+ - [0.05, 0.01, 0.01]
+ - [0.01, 0.01, 0.01]
+ - [0.01, 0.01, 0.01]
+ - [0.01, 0.01, 0.01]
+ - [0.01, 0.01, 0.01]
+ - [0.01, 0.01, 0.01]
+ l10_6:
+ - [0.011, 0.045, 0.126] #Trumb + littleroot
+ - [0.012, 0.022, 0.137] #Trumb + littleroot
+ - [0.011, 0.03, 0.141] #Trumb + littleroot
+ - [0.013, -0.013, 0.141] #Trumb + littleroot
+ - [0.032, -0.107, 0.145] #Trumb default
+ - [0.030, -0.051, 0.179] #Trumb nearly index
+ - [0.098, -0.020, 0.167] #Trumb + index + print
+ l16_3:
+ - [-0.5000, 0.0000, 0.1000]
+ - [-0.5270, 0.0270, 0.0974]
+ - [-0.5927, 0.0723, 0.0965]
+ - [-0.6121, 0.0904, 0.0959]
+ - [-0.5000, 0.0000, 0.1000]
+ t24_1:
+ - [0.005, -0.034, 0.15] #Trumb + littleroot
+ - [0.006, -0.018, 0.149] #Trumb + littleroot
+ - [0.016, 0.002, 0.155] #Trumb + littleroot
+ - [0.023, 0.017, 0.149] #Trumb + littleroot
+ - [0.023, 0.144, 0.112] #Trumb default
+ - [0.023, 0.061, 0.184] #Trumb nearly index
+ - [0.077, 0.018, 0.179] #Trumb + index + print
+ left_hand:
+ l18_2:
+ - [-0.5000, 0.0000, 0.1000]
+ - [-0.5270, 0.0270, 0.0974]
+ - [-0.5927, 0.0723, 0.0965]
+ - [-0.6121, 0.0904, 0.0959]
+ l20_8:
+ - [0.02, 0.01, 0.01]
+ - [0.05, 0.01, 0.01]
+ - [0.01, 0.01, 0.01]
+ - [0.01, 0.01, 0.01]
+ - [0.01, 0.01, 0.01]
+ - [0.01, 0.01, 0.01]
+ - [0.01, 0.01, 0.01]
+ l10_6:
+ - [0.011, 0.045, 0.126] #Trumb + littleroot
+ - [0.012, 0.022, 0.137] #Trumb + littleroot
+ - [0.011, 0.03, 0.141] #Trumb + littleroot
+ - [0.013, -0.013, 0.141] #Trumb + littleroot
+ - [0.032, -0.107, 0.145] #Trumb default
+ - [0.030, -0.051, 0.179] #Trumb nearly index
+ - [0.098, -0.020, 0.167] #Trumb + index + print
+ l16_3:
+ - [-0.5000, 0.0000, 0.1000]
+ - [-0.5270, 0.0270, 0.0974]
+ - [-0.5927, 0.0723, 0.0965]
+ - [-0.6121, 0.0904, 0.0959]
+ - [-0.5000, 0.0000, 0.1000]
+ t24_1:
+ - [0.005, -0.034, 0.15] #Trumb + littleroot
+ - [0.006, -0.018, 0.149] #Trumb + littleroot
+ - [0.016, 0.002, 0.155] #Trumb + littleroot
+ - [0.023, 0.017, 0.149] #Trumb + littleroot
+ - [0.023, 0.144, 0.112] #Trumb default
+ - [0.023, 0.061, 0.184] #Trumb nearly index
+ - [0.077, 0.018, 0.179] #Trumb + index + print
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/retarget_config.yml b/src/linkerhand_retarget/linkerhand_retarget/config/retarget_config.yml
new file mode 100644
index 0000000..a521cd7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/retarget_config.yml
@@ -0,0 +1,98 @@
+right_hand:
+ l20:
+ - [ 0, 0, 0.8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0.42, 0.99, 0.36, 0.91, 0, 0, 1.40, 1.08, 0, 0, 1.40, 1.08, 0, 0, 1.40, 1.08, 0, 0, 1.40, 1.08, 0]
+ - [ 0, 0, 0, 0, 0, 0.26, 0, 0, 0, 0.13, 0, 0, 0,-0.13, 0, 0, 0,-0.26, 0, 0, 0]
+ - [ 0, 0.97, 0.31, 0.81, 0, 0, 0.63, 1.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0.16, 0.97, 0.40, 0.81, 0, 0, 0, 0, 0, 0, 0.56, 1.08, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0.31, 1.18, 0.38, 0.81, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.61, 1.08, 0, 0, 0, 0, 0]
+ - [ 0.54, 1.18, 0.28, 0.91, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.61, 1.08, 0]
+ l10:
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ l7:
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ l24:
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ l25:
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ l30:
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+left_hand:
+ l20:
+ - [ 0, 0, 0.8, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0.42, 0.99, 0.36, 0.91, 0, 0, 1.40, 1.08, 0, 0, 1.40, 1.08, 0, 0, 1.40, 1.08, 0, 0, 1.40, 1.08, 0]
+ - [ 0, 0, 0, 0, 0, 0.26, 0, 0, 0, 0.13, 0, 0, 0,-0.13, 0, 0, 0,-0.26, 0, 0, 0]
+ - [ 0, 0.97, 0.31, 0.81, 0, 0, 0.63, 1.06, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0.16, 0.97, 0.40, 0.81, 0, 0, 0, 0, 0, 0, 0.56, 1.08, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0.31, 1.18, 0.38, 0.81, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.61, 1.08, 0, 0, 0, 0, 0]
+ - [ 0.54, 1.18, 0.28, 0.91, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0.61, 1.08, 0]
+ l10:
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ l7:
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ l24:
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ l25:
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ l30:
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
+ - [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0, 0]
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/speed_config.yml b/src/linkerhand_retarget/linkerhand_retarget/config/speed_config.yml
new file mode 100644
index 0000000..631cab2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/speed_config.yml
@@ -0,0 +1 @@
+l20_8:
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_l10v7_left.yml b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_l10v7_left.yml
new file mode 100644
index 0000000..4b033fb
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_l10v7_left.yml
@@ -0,0 +1,21 @@
+retargeting:
+ type: vector
+ urdf_path: linker_hand/l10v7_left/linkerhand_l10v7_left.urdf
+
+ # Target refers to the retargeting target, which is the robot hand
+ target_joint_names: ['index_mcp_pitch', 'index_mcp_roll',
+ 'pinky_mcp_pitch', 'pinky_mcp_roll',
+ 'middle_mcp_pitch',
+ 'ring_mcp_pitch','ring_mcp_roll',
+ 'thumb_cmc_roll', 'thumb_cmc_yaw', 'thumb_cmc_pitch']
+ target_origin_link_names: [ "hand_base_link", "hand_base_link", "hand_base_link", "hand_base_link", "hand_base_link"]
+ target_task_link_names: [ "thumb_distal", "index_distal", "middle_distal", "ring_distal", "pinky_distal"]
+ scaling_factor: 1
+
+ # Source refers to the retargeting input, which usually corresponds to the human hand
+ # The joint indices of human hand joint which corresponds to each link in the target_link_names
+ # target_link_human_indices: [ [ 0, 0, 0, 0, 0], [ 4, 9, 14, 19, 24 ] ]
+ target_link_human_indices: [ [ 0, 0, 0, 0, 0], [ 4, 8, 12, 16, 20 ] ]
+
+ # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency
+ low_pass_alpha: 0.2
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_l10v7_right.yml b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_l10v7_right.yml
new file mode 100644
index 0000000..2fa2561
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_l10v7_right.yml
@@ -0,0 +1,22 @@
+retargeting:
+ type: vector
+ urdf_path: linker_hand/linker_hand_l10_6_right.urdf
+
+ # Target refers to the retargeting target, which is the robot hand
+ target_joint_names: ['index_mcp_pitch', 'index_mcp_roll',
+ 'pinky_mcp_pitch', 'pinky_mcp_roll',
+ 'middle_mcp_pitch',
+ 'ring_mcp_pitch','ring_mcp_roll',
+ 'thumb_cmc_roll', 'thumb_cmc_yaw', 'thumb_cmc_pitch']
+ target_origin_link_names: [ "hand_base_link", "hand_base_link", "hand_base_link", "hand_base_link", "hand_base_link"]
+ target_task_link_names: [ "thumb_distal", "index_distal", "middle_distal", "ring_distal", "pinky_distal"]
+ scaling_factor: 1
+
+ # Source refers to the retargeting input, which usually corresponds to the human hand
+ # The joint indices of human hand joint which corresponds to each link in the target_link_names
+ # target_link_human_indices: [ [ 0, 0, 0, 0, 0], [ 4, 9, 14, 19, 24 ] ]
+ target_link_human_indices: [ [ 0, 0, 0, 0, 0], [ 4, 8, 12, 16, 20 ] ]
+
+ # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency
+ low_pass_alpha: 0.2
+
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_l20_left.yml b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_l20_left.yml
new file mode 100644
index 0000000..7187a9c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_l20_left.yml
@@ -0,0 +1,20 @@
+retargeting:
+ type: vector
+ urdf_path: linker_hand/linker_hand_l20_8_left.urdf
+
+ # Target refers to the retargeting target, which is the robot hand
+ 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"]
+ scaling_factor: 1.0
+
+ # Source refers to the retargeting input, which usually corresponds to the human hand
+ # The joint indices of human hand joint which corresponds to each link in the target_link_names
+ target_link_human_indices: [ [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ], [4, 2, 9, 14, 19, 24, 7, 12, 17, 22 ] ]
+
+ # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency
+ low_pass_alpha: 0.2
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_l20_right.yml b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_l20_right.yml
new file mode 100644
index 0000000..a8a7d12
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_l20_right.yml
@@ -0,0 +1,19 @@
+retargeting:
+ type: vector
+ urdf_path: linker_hand/linker_hand_l20_8_right.urdf
+
+ # Target refers to the retargeting target, which is the robot hand
+ 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"]
+ scaling_factor: 1.0
+
+ # Source refers to the retargeting input, which usually corresponds to the human hand
+ # The joint indices of human hand joint which corresponds to each link in the target_link_names
+ target_link_human_indices: [ [ 0, 0, 0, 0, 0, 0, 0, 0, 0, 0 ], [4, 2, 9, 14, 19, 24, 7, 12, 17, 22 ] ]
+ # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency
+ low_pass_alpha: 0.2
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_l6_left.yml b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_l6_left.yml
new file mode 100644
index 0000000..0161306
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_l6_left.yml
@@ -0,0 +1,21 @@
+retargeting:
+ type: vector
+ urdf_path: linker_hand/l6_left/linkerhand_l6_left.urdf
+
+ # Target refers to the retargeting target, which is the robot hand
+ target_joint_names: ['thumb_cmc_roll', 'thumb_cmc_pitch',
+ 'index_mcp_pitch',
+ 'middle_mcp_pitch',
+ 'ring_mcp_pitch',
+ 'pinky_mcp_pitch']
+ target_origin_link_names: [ "hand_base_link", "hand_base_link", "hand_base_link", "hand_base_link", "hand_base_link"]
+ target_task_link_names: [ "thumb_distal", "index_distal", "middle_distal", "ring_distal", "pinky_distal"]
+ scaling_factor: 1
+
+ # Source refers to the retargeting input, which usually corresponds to the human hand
+ # The joint indices of human hand joint which corresponds to each link in the target_link_names
+ target_link_human_indices: [ [ 0, 0, 0, 0, 0], [ 4, 4, 4, 4, 4 ] ]
+# target_link_human_indices: [ [ 0, 0, 0, 0, 0], [ 4, 8, 12, 16, 20 ] ]
+
+ # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency
+ low_pass_alpha: 0.2
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_l6_right.yml b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_l6_right.yml
new file mode 100644
index 0000000..de5d884
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_l6_right.yml
@@ -0,0 +1,21 @@
+retargeting:
+ type: vector
+ urdf_path: linker_hand/l6_right/linkerhand_l6_right.urdf
+
+ # Target refers to the retargeting target, which is the robot hand
+ target_joint_names: ['thumb_cmc_roll', 'thumb_cmc_pitch',
+ 'index_mcp_pitch',
+ 'middle_mcp_pitch',
+ 'ring_mcp_pitch',
+ 'pinky_mcp_pitch']
+ target_origin_link_names: [ "hand_base_link", "hand_base_link", "hand_base_link", "hand_base_link", "hand_base_link"]
+ target_task_link_names: [ "thumb_distal", "index_distal", "middle_distal", "ring_distal", "pinky_distal"]
+ scaling_factor: 1
+
+ # Source refers to the retargeting input, which usually corresponds to the human hand
+ # The joint indices of human hand joint which corresponds to each link in the target_link_names
+ target_link_human_indices: [ [ 0, 0, 0, 0, 0], [ 4, 4, 4, 4, 4 ] ]
+# target_link_human_indices: [ [ 0, 0, 0, 0, 0], [ 4, 8, 12, 16, 20 ] ]
+
+ # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency
+ low_pass_alpha: 0.2
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_o6_left.yml b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_o6_left.yml
new file mode 100644
index 0000000..5885f58
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_o6_left.yml
@@ -0,0 +1,21 @@
+retargeting:
+ type: vector
+ urdf_path: linker_hand/o6_left/linkerhand_o6_left.urdf
+
+ # Target refers to the retargeting target, which is the robot hand
+ target_joint_names: ['thumb_cmc_yaw', 'thumb_cmc_pitch',
+ 'index_mcp_pitch',
+ 'middle_mcp_pitch',
+ 'ring_mcp_pitch',
+ 'pinky_mcp_pitch']
+ target_origin_link_names: [ "hand_base_link", "hand_base_link", "hand_base_link", "hand_base_link", "hand_base_link"]
+ target_task_link_names: [ "thumb_distal", "index_distal", "middle_distal", "ring_distal", "pinky_distal"]
+ scaling_factor: 1
+
+ # Source refers to the retargeting input, which usually corresponds to the human hand
+ # The joint indices of human hand joint which corresponds to each link in the target_link_names
+ target_link_human_indices: [ [ 0, 0, 0, 0, 0], [ 4, 4, 4, 4, 4 ] ]
+# target_link_human_indices: [ [ 0, 0, 0, 0, 0], [ 4, 8, 12, 16, 20 ] ]
+
+ # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency
+ low_pass_alpha: 0.2
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_o6_right.yml b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_o6_right.yml
new file mode 100644
index 0000000..ecb5170
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_o6_right.yml
@@ -0,0 +1,21 @@
+retargeting:
+ type: vector
+ urdf_path: linker_hand/o6_right/linkerhand_o6_right.urdf
+
+ # Target refers to the retargeting target, which is the robot hand
+ target_joint_names: ['thumb_cmc_yaw', 'thumb_cmc_pitch',
+ 'index_mcp_pitch',
+ 'middle_mcp_pitch',
+ 'ring_mcp_pitch',
+ 'pinky_mcp_pitch']
+ target_origin_link_names: [ "hand_base_link", "hand_base_link", "hand_base_link", "hand_base_link", "hand_base_link"]
+ target_task_link_names: [ "thumb_distal", "index_distal", "middle_distal", "ring_distal", "pinky_distal"]
+ scaling_factor: 1
+
+ # Source refers to the retargeting input, which usually corresponds to the human hand
+ # The joint indices of human hand joint which corresponds to each link in the target_link_names
+ target_link_human_indices: [ [ 0, 0, 0, 0, 0], [ 4, 4, 4, 4, 4 ] ]
+# target_link_human_indices: [ [ 0, 0, 0, 0, 0], [ 4, 8, 12, 16, 20 ] ]
+
+ # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency
+ low_pass_alpha: 0.2
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_o7v3_left.yml b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_o7v3_left.yml
new file mode 100644
index 0000000..a8dbbe2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_o7v3_left.yml
@@ -0,0 +1,21 @@
+retargeting:
+ type: vector
+ urdf_path: linker_hand/o7v3_left/linkerhand_o7v3_left.urdf
+
+ # Target refers to the retargeting target, which is the robot hand
+ target_joint_names: ['index_mcp_pitch',
+ 'pinky_mcp_pitch',
+ 'middle_mcp_pitch',
+ 'ring_mcp_pitch',
+ 'thumb_cmc_roll', 'thumb_cmc_yaw', 'thumb_cmc_pitch']
+ target_origin_link_names: [ "hand_base_link", "hand_base_link", "hand_base_link", "hand_base_link", "hand_base_link"]
+ target_task_link_names: [ "thumb_distal", "index_distal", "middle_distal", "ring_distal", "pinky_distal"]
+ scaling_factor: 1
+
+ # Source refers to the retargeting input, which usually corresponds to the human hand
+ # The joint indices of human hand joint which corresponds to each link in the target_link_names
+ # target_link_human_indices: [ [ 0, 0, 0, 0, 0], [ 4, 9, 14, 19, 24 ] ]
+ target_link_human_indices: [ [ 0, 0, 0, 0, 0], [ 4, 8, 12, 16, 20 ] ]
+
+ # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency
+ low_pass_alpha: 0.2
diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_o7v3_right.yml b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_o7v3_right.yml
new file mode 100644
index 0000000..892cec2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/config/teleop/linker_hand_o7v3_right.yml
@@ -0,0 +1,21 @@
+retargeting:
+ type: vector
+ urdf_path: linker_hand/linker_hand_l10_6_right.urdf
+
+ # Target refers to the retargeting target, which is the robot hand
+ target_joint_names: ['index_joint0', 'index_joint2',
+ 'little_joint0', 'little_joint2',
+ 'middle_joint1',
+ 'ring_joint0', 'ring_joint2',
+ 'thumb_joint0', 'thumb_joint1', 'thumb_joint3']
+ target_origin_link_names: [ "base_link", "base_link", "base_link", "base_link", "base_link"]
+ target_task_link_names: [ "thumb_link5", "index_link4", "middle_link3", "ring_link4", "little_link4"]
+ scaling_factor: 1
+
+ # Source refers to the retargeting input, which usually corresponds to the human hand
+ # The joint indices of human hand joint which corresponds to each link in the target_link_names
+ target_link_human_indices: [ [ 0, 0, 0, 0, 0], [ 4, 9, 14, 19, 24 ] ]
+# target_link_human_indices: [ [ 0, 0, 0, 0, 0], [ 4, 8, 12, 16, 20 ] ]
+
+ # A smaller alpha means stronger filtering, i.e. more smooth but also larger latency
+ low_pass_alpha: 0.2
diff --git a/src/linkerhand_retarget/linkerhand_retarget/handretarget.py b/src/linkerhand_retarget/linkerhand_retarget/handretarget.py
new file mode 100644
index 0000000..9c9f0c0
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/handretarget.py
@@ -0,0 +1,354 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import sys
+import os
+from pathlib import Path
+
+# 强制使用src目录的路径
+def setup_src_paths():
+ """确保使用src目录而不是build目录"""
+ # 获取工作空间的绝对路径
+ current_file = Path(__file__).absolute()
+ workspace_dir = current_file.parent.parent.parent.parent
+
+ # 添加src目录到Python路径
+ src_package_dir = workspace_dir / "src" / "linkerhand_retarget" / "linkerhand_retarget"
+ if src_package_dir.exists():
+ paths_to_add = [
+ src_package_dir,
+ src_package_dir / "linkerhand",
+ ]
+
+ for path in paths_to_add:
+ if path.exists() and str(path) not in sys.path:
+ sys.path.insert(0, str(path))
+
+ return src_package_dir
+
+workspace_dir = setup_src_paths()
+
+
+import time
+from threading import Thread, Event
+from pathlib import Path
+from queue import Empty
+from typing import Optional
+import numpy as np
+import enum
+import signal, sys
+
+
+_script_dir = str(Path(__file__).parent)
+# 使用本地 linkerhand
+if _script_dir not in sys.path:
+ sys.path.insert(0, _script_dir)
+
+from linkerhand.utils import *
+from linkerhand.vtrdyncore import *
+from linkerhand.handcore import HandCore
+from linkerhand.config import HandConfig
+from linkerhand.constants import RetargetingType, DataSource, MotionSource, RobotName
+
+from ament_index_python.packages import get_package_share_directory
+from pathlib import Path
+
+import rclpy
+from rclpy.node import Node
+from rclpy.executors import MultiThreadedExecutor
+from sensor_msgs.msg import JointState
+from geometry_msgs.msg import PoseArray
+from std_msgs.msg import String
+from rcl_interfaces.msg import ParameterDescriptor
+
+import json
+
+
+vr_pose_cache_r = []
+vr_pose_cache_l = []
+video_pose_cache_r = []
+video_pose_cache_l = []
+reangle_r = []
+reangle_l = []
+right_hand_pose_end = []
+left_hand_pose_end = []
+
+
+def signal_handler(sig, frame):
+ rclpy.shutdown()
+
+
+class HandRetargetNode(Node):
+ def __init__(self):
+ super().__init__('handretarget_node')
+ print("Ready Create HandRetargetNode!")
+
+ package_share_dir = workspace_dir
+
+ self.robot_dir = package_share_dir / "assets" / "robots" / "hands"
+ self.base_config = package_share_dir
+
+ self.handconfig = HandConfig(str(self.robot_dir), str(self.base_config))
+ self.handcore = HandCore(self.handconfig)
+
+ self.baseconfig = self.handconfig.baseconfig
+ self.retagetconfig = self.handconfig.retagetconfig
+
+ # 声明参数并提供默认值
+ # 兼容 Foxy (无 dynamic_typing) 和 Jazzy (有 dynamic_typing)
+ try:
+ auto_scan_desc = ParameterDescriptor(dynamic_typing=True)
+ except (TypeError, AttributeError, AssertionError):
+ auto_scan_desc = ParameterDescriptor()
+
+ self.declare_parameters(
+ namespace='',
+ parameters=[
+ ('calibration', False),
+ ('ports', ['']),
+ ('baudrate', 0),
+ ('auto_scan', None, auto_scan_desc),
+ ]
+ )
+ #
+ self.scene, self.retargeting_r, self.retargeting_l, self.config_r, self.config_l = None, None, None, None, None
+ self.robot_name_r, self.robot_name_l = None, None
+ self.retargeting_type = None
+ self.datasource_type = None
+ self.motion_type = None
+ self.udp_ip, self.udp_port, self.use_can, self.motion_device = None, None, None, None
+
+ self.calibration = self.get_parameter('calibration').value
+ print(f"是否启用标定: {self.calibration} ")
+
+ # 读取命令行串口参数（候选列表）
+ cmd_ports = self.get_parameter('ports').value
+ self.cmd_ports = [p for p in cmd_ports if p] if cmd_ports else None
+ self.cmd_baudrate = self.get_parameter('baudrate').value or None
+ self.cmd_auto_scan = self.get_parameter('auto_scan').value
+
+ if self.cmd_ports:
+ print(f"命令行指定候选串口: {self.cmd_ports} @ {self.cmd_baudrate}")
+ if self.cmd_auto_scan:
+ print(f"命令行启用自动扫描")
+
+ self.calibrationopen_r, self.calibrationopen_l, self.calibrationclose_r, self.calibrationclose_l = None, None, None, None
+ self.retarget = None
+ self.datasource_type = DataSource[self.baseconfig["system"]["datasource_type"]]
+ self.retargeting_type = RetargetingType[self.baseconfig["system"]["retargeting_type"]]
+ self.motion_type = MotionSource[self.baseconfig["system"]["motion_type"]]
+ self.robot_name_r = RobotName[self.baseconfig["system"]["robotname_r"]]
+ self.robot_name_l = RobotName[self.baseconfig["system"]["robotname_l"]]
+
+ self.udp_ip = self.baseconfig["udp"]["ip"]
+ self.udp_port = int(self.baseconfig["udp"]["port"])
+ self.use_can = bool(self.baseconfig["system"]["usecan"])
+ self.motion_device = self.baseconfig["system"]["motion_device"]
+
+ # LinkerEG 配置
+ self.linkereg_port = self.baseconfig.get("linkereg", {}).get("port", None)
+ self.linkereg_password = self.baseconfig.get("linkereg", {}).get("password", "i")
+
+ self.righthandprint = bool(self.baseconfig["debug"]["joint_motor_debug_r"])
+ self.lefthandprint = bool(self.baseconfig["debug"]["joint_motor_debug_l"])
+
+ # if self.datasource_type == DataSource.vr:
+ # self.vr_right_sub = self.create_subscription(
+ # JointState,
+ # '/vr_right_hand_pose',
+ # self.vr_right_pose_callback,
+ # 10)
+ # self.vr_left_sub = self.create_subscription(
+ # JointState,
+ # '/vr_left_hand_pose',
+ # self.vr_left_pose_callback,
+ # 10)
+ # elif self.datasource_type == DataSource.video:
+ # self.video_right_sub = self.create_subscription(
+ # JointState,
+ # '/video_right_hand_pose',
+ # self.video_right_pose_callback,
+ # 10)
+ # self.video_left_sub = self.create_subscription(
+ # JointState,
+ # '/video_left_hand_pose',
+ # self.video_left_pose_callback,
+ # 10)
+
+ self.pubprintcount = 0
+
+ # 订阅遥操作参数话题
+ self.teleop_param_sub = self.create_subscription(
+ String,
+ '/hand_teleop_param',
+ self.teleop_param_callback,
+ 10
+ )
+
+ # 发布遥操作状态话题
+ self.teleop_state_pub = self.create_publisher(
+ String,
+ '/hand_teleop_state',
+ 10
+ )
+
+ # 当前模式
+ self.current_mode = 'glove'
+
+ def teleop_param_callback(self, msg):
+ """处理遥操作参数话题回调"""
+ try:
+ param = json.loads(msg.data)
+ mode = param.get('mode') # 可能为 None
+
+ if self.retarget is not None and hasattr(self.retarget, 'set_mode'):
+ self.retarget.set_mode(mode, param)
+ if mode:
+ self.current_mode = mode
+ # 发布状态反馈
+ state_msg = String()
+ state_msg.data = json.dumps({
+ 'mode': mode or self.current_mode,
+ 'status': 'success'
+ })
+ self.teleop_state_pub.publish(state_msg)
+ else:
+ self.get_logger().warn("retarget 未初始化或不支持 set_mode")
+ state_msg = String()
+ state_msg.data = json.dumps({
+ 'mode': mode or 'unknown',
+ 'status': 'failed',
+ 'error': 'retarget not initialized'
+ })
+ self.teleop_state_pub.publish(state_msg)
+ except json.JSONDecodeError as e:
+ self.get_logger().error(f"JSON 解析错误: {e}")
+ state_msg = String()
+ state_msg.data = json.dumps({
+ 'mode': 'unknown',
+ 'status': 'failed',
+ 'error': str(e)
+ })
+ self.teleop_state_pub.publish(state_msg)
+ except Exception as e:
+ self.get_logger().error(f"参数处理错误: {e}")
+
+ def retargetrun(self):
+ if self.motion_type == MotionSource.udexreal:
+ from linkerhand_retarget.motion.udexreal.retarget import Retarget
+ self.retarget = Retarget(
+ self,
+ ip=self.udp_ip,
+ port=self.udp_port,
+ deviceid=self.motion_device,
+ righthand=self.robot_name_r,
+ lefthand=self.robot_name_l,
+ handcore=self.handcore,
+ lefthandpubprint=self.lefthandprint,
+ righthandpubprint=self.righthandprint
+ )
+ elif self.motion_type == MotionSource.udexrealv2t:
+ from linkerhand_retarget.motion.udexrealv2t.retarget import Retarget
+ self.retarget = Retarget(
+ self,
+ ip=self.udp_ip,
+ port=self.udp_port,
+ deviceid=self.motion_device,
+ righthand=self.robot_name_r,
+ lefthand=self.robot_name_l,
+ handcore=self.handcore,
+ lefthandpubprint=self.lefthandprint,
+ righthandpubprint=self.righthandprint,
+ calibration = self.calibration
+ )
+ elif self.motion_type == MotionSource.linkerforce:
+ from linkerhand_retarget.motion.linkerforce.retarget import Retarget
+ self.retarget = Retarget(
+ self,
+ righthand=self.robot_name_r,
+ lefthand=self.robot_name_l,
+ handcore=self.handcore,
+ lefthandpubprint=self.lefthandprint,
+ righthandpubprint=self.righthandprint,
+ calibration = self.calibration,
+ baseconfig = self.baseconfig,
+ cmd_ports=self.cmd_ports,
+ cmd_baudrate=self.cmd_baudrate,
+ cmd_auto_scan=self.cmd_auto_scan
+ )
+ elif self.motion_type == MotionSource.vtrdyn:
+ from linkerhand_retarget.motion.vtrdyn.retarget import Retarget
+ self.retarget = Retarget(
+ self,
+ ip=self.udp_ip,
+ port=self.udp_port,
+ righthand=self.robot_name_r,
+ lefthand=self.robot_name_l,
+ handcore=self.handcore,
+ lefthandpubprint=self.lefthandprint,
+ righthandpubprint=self.righthandprint,
+ calibration = self.calibration
+ )
+ elif self.motion_type == MotionSource.linkermcg:
+ from linkerhand_retarget.motion.linkermcg.retarget import Retarget
+ self.retarget = Retarget(
+ self,
+ ip=self.udp_ip,
+ port=self.udp_port,
+ righthand=self.robot_name_r,
+ lefthand=self.robot_name_l,
+ handcore=self.handcore,
+ lefthandpubprint=self.lefthandprint,
+ righthandpubprint=self.righthandprint
+ )
+ elif self.motion_type == MotionSource.linkereg2:
+ from linkerhand_retarget.motion.linkereg.retarget import Retarget
+ self.retarget = Retarget(
+ self,
+ port=self.linkereg_port,
+ baudrate=921600,
+ password=self.linkereg_password,
+ isdebug=bool(self.baseconfig["debug"]["joint_pub_debug"]),
+ mode='sdk' # SDK控制模式
+ )
+ elif self.motion_type == MotionSource.linkereg1:
+ from linkerhand_retarget.motion.linkereg.retarget import Retarget
+ self.retarget = Retarget(
+ self,
+ port=self.linkereg_port,
+ baudrate=921600,
+ password=self.linkereg_password,
+ isdebug=bool(self.baseconfig["debug"]["joint_pub_debug"]),
+ mode='receiver' # 接收器控制模式 (需要连接灵巧手)
+ )
+ if self.retarget is None:
+ self.get_logger().error("未正确创建应用实例")
+ else:
+ print("启动应用实例")
+ self.retarget.process()
+
+def main(args=None):
+ rclpy.init(args=args)
+ node = None
+
+ try:
+ signal.signal(signal.SIGINT, signal_handler)
+ node = HandRetargetNode()
+ executor = MultiThreadedExecutor()
+ node.retargetrun()
+
+ # Keep the node alive
+ rclpy.spin(node, executor)
+ except KeyboardInterrupt:
+ if node is not None:
+ node.get_logger().info("收到终止信号")
+ finally:
+ if node is not None:
+ # 停止串口线程
+ if hasattr(node, 'retarget') and node.retarget and hasattr(node.retarget, 'stop_serial_threads'):
+ node.retarget.stop_serial_threads()
+ node.destroy_node()
+ rclpy.shutdown()
+
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/launch/linkerhand_retarget.launch.py b/src/linkerhand_retarget/linkerhand_retarget/launch/linkerhand_retarget.launch.py
new file mode 100644
index 0000000..5dbac64
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/launch/linkerhand_retarget.launch.py
@@ -0,0 +1,24 @@
+from launch import LaunchDescription
+from launch_ros.actions import Node
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration
+import os
+
+def generate_launch_description():
+ return LaunchDescription([
+ DeclareLaunchArgument(
+ 'calibration',
+ default_value='True',
+ description='Enable Calibration'
+ ),
+
+ Node(
+ package='linkerhand_retarget',
+ executable='handretarget',
+ name='handretarget',
+ output='screen',
+ parameters=[{
+ 'calibration': LaunchConfiguration('calibration'),
+ }]
+ ),
+ ])
diff --git a/src/linkerhand_retarget/linkerhand_retarget/linkerhand/__init__.py b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/__init__.py
new file mode 100644
index 0000000..5147606
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/__init__.py
@@ -0,0 +1 @@
+__version__ = "2.7.0"
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/linkerhand/config.py b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/config.py
new file mode 100644
index 0000000..26979a5
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/config.py
@@ -0,0 +1,19 @@
+from .utils import *
+
+
+class HandConfig():
+ def __init__(self, robot_dir: str, config_path: str):
+ package_share_dir = config_path
+ self.handconfig = read_yaml(os.path.join(package_share_dir, 'config', 'hand_config.yml'))
+ self.baseconfig = read_yaml(os.path.join(package_share_dir, 'config', 'base_config.yml'))
+ self.retagetconfig = read_yaml(os.path.join(package_share_dir, 'config', 'retarget_config.yml'))
+ self.modelconfig = read_yaml(os.path.join(package_share_dir, 'config', 'model_config.yml'))
+
+ self.robot_dir = robot_dir
+
+ self.bodypose = read_yaml(
+ os.path.join(package_share_dir, 'config', f'{self.baseconfig["humanset"]["bodyfile"]}.yml'))
+ self.targetpose = read_yaml(
+ os.path.join(package_share_dir, 'config', f'{self.baseconfig["humanset"]["targethandfile"]}.yml'))
+ self.retagetconfig = read_yaml(os.path.join(package_share_dir, 'config', 'retarget_config.yml'))
+
diff --git a/src/linkerhand_retarget/linkerhand_retarget/linkerhand/constants.py b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/constants.py
new file mode 100644
index 0000000..7be3311
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/constants.py
@@ -0,0 +1,130 @@
+import enum
+from pathlib import Path
+from typing import Optional
+
+import numpy as np
+
+OPERATOR2MANO_RIGHT = np.array(
+ [
+ [0, 0, -1],
+ [-1, 0, 0],
+ [0, 1, 0],
+ ]
+)
+
+OPERATOR2MANO_LEFT = np.array(
+ [
+ [0, 0, -1],
+ [1, 0, 0],
+ [0, -1, 0],
+ ]
+)
+
+
+class RobotName(enum.Enum):
+ o7 = enum.auto()
+ o7v1 = enum.auto()
+ o7v3 = enum.auto()
+ o6 = enum.auto()
+ l6 = enum.auto()
+ l7 = enum.auto()
+ l10 = enum.auto()
+ l10v7 = enum.auto()
+ l20 = enum.auto()
+ l20lite = enum.auto()
+ l25 = enum.auto()
+ g20 = enum.auto()
+
+
+class RetargetingType(enum.Enum):
+ vector = enum.auto() # For teleoperation, no finger closing prior
+ position = enum.auto() # For offline data processing, especially hand-object interaction data
+ dexpilot = enum.auto() # For teleoperation, with finger closing prior
+ projection = enum.auto()
+
+
+class HandType(enum.Enum):
+ right = enum.auto()
+ left = enum.auto()
+
+
+class DataSource(enum.Enum):
+ motion = enum.auto()
+ video = enum.auto()
+ vr = enum.auto()
+
+
+class MotionSource(enum.Enum):
+ vtrdyn = enum.auto()
+ udexreal = enum.auto()
+ udexrealv2t = enum.auto()
+ linkerforce = enum.auto()
+ sensenova = enum.auto()
+ linkermcg = enum.auto()
+ linkereg1 = enum.auto()
+ linkereg2 = enum.auto()
+
+
+ROBOT_NAME_MAP = {
+ RobotName.o7: "linker_hand_o7",
+ RobotName.l7: "linker_hand_l7",
+ RobotName.o6: "linker_hand_o6",
+ RobotName.l6: "linker_hand_l6",
+ RobotName.o7v1: "linker_hand_o7v1",
+ RobotName.o7v3: "linker_hand_o7v3",
+ RobotName.l10: "linker_hand_l10",
+ RobotName.l10v7: "linker_hand_l10v7",
+ RobotName.l20: "linker_hand_l20",
+ RobotName.l20lite: "linker_hand_l20lite",
+ RobotName.l25: "linker_hand_l25",
+ RobotName.g20: "linker_hand_g20",
+}
+
+ROBOT_NAMES = list(ROBOT_NAME_MAP.keys())
+
+
+ROBOT_LEN_MAP ={
+ RobotName.o7: 7,
+ RobotName.l7: 7,
+ RobotName.o6: 6,
+ RobotName.l6: 6,
+ RobotName.o7v1: 7,
+ RobotName.o7v3: 7,
+ RobotName.l10: 10,
+ RobotName.l10v7: 10,
+ RobotName.l20: 20,
+ RobotName.l20lite: 10,
+ RobotName.l25: 20,
+ RobotName.g20: 20,
+}
+
+ROBOT_LEN = list(ROBOT_LEN_MAP.keys())
+
+
+def get_default_config_path(
+ robot_name: RobotName, retargeting_type: RetargetingType, hand_type: HandType
+) -> Optional[Path]:
+ config_path = Path(__file__).parent.parent / "config"
+ if retargeting_type is RetargetingType.position:
+ config_path = config_path / "offline"
+ else:
+ config_path = config_path / "teleop"
+
+ robot_name_str = ROBOT_NAME_MAP[robot_name]
+ hand_type_str = hand_type.name
+ if "gripper" in robot_name_str: # For gripper robots, only use gripper config file.
+ if retargeting_type == RetargetingType.dexpilot:
+ config_name = f"{robot_name_str}_dexpilot.yml"
+ else:
+ config_name = f"{robot_name_str}.yml"
+ else:
+ if retargeting_type == RetargetingType.dexpilot:
+ config_name = f"{robot_name_str}_{hand_type_str}_dexpilot.yml"
+ else:
+ config_name = f"{robot_name_str}_{hand_type_str}.yml"
+ return config_path / config_name
+
+OPERATOR2MANO = {
+ HandType.right: OPERATOR2MANO_RIGHT,
+ HandType.left: OPERATOR2MANO_LEFT,
+}
diff --git a/src/linkerhand_retarget/linkerhand_retarget/linkerhand/filter.py b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/filter.py
new file mode 100644
index 0000000..7401edd
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/filter.py
@@ -0,0 +1,784 @@
+import numpy as np
+from typing import List, Optional
+from collections import deque
+
+class LCFilter:
+ """
+ LC低通滤波器（一阶低通滤波器）
+ 离散时间实现，常用于信号平滑
+ """
+
+ def __init__(self, alpha: float = 0.1, initial_value: float = 0.0):
+ """
+ 初始化LC滤波器
+
+ 参数：
+ alpha: 滤波系数 (0 < alpha <= 1)
+ alpha越小，滤波效果越强（更平滑）
+ alpha越大，响应越快（更灵敏）
+ initial_value: 初始值
+ """
+ if alpha <= 0 or alpha > 1:
+ raise ValueError("alpha必须在(0, 1]范围内")
+
+ self.alpha = alpha
+ self.filtered_value = initial_value
+ self.previous_raw = initial_value
+ self.previous_filtered = initial_value
+
+ # 历史记录（可选，用于调试）
+ self.history_raw = []
+ self.history_filtered = []
+
+ def update(self, new_value: float) -> float:
+ """
+ 更新滤波器并返回滤波后的值
+
+ 公式：y[n] = α * x[n] + (1-α) * y[n-1]
+ 其中：x[n]是当前输入，y[n-1]是上一次输出
+
+ 参数：
+ new_value: 新的输入值
+ 返回：
+ 滤波后的值
+ """
+ # 保存历史值
+ self.previous_raw = new_value
+ self.previous_filtered = self.filtered_value
+
+ # LC滤波公式
+ self.filtered_value = self.alpha * new_value + (1 - self.alpha) * self.filtered_value
+
+ # 记录历史（可选）
+ self.history_raw.append(new_value)
+ self.history_filtered.append(self.filtered_value)
+
+ return self.filtered_value
+
+ def update_array(self, new_values: List[float]) -> List[float]:
+ """
+ 批量更新数组
+
+ 参数：
+ new_values: 新的输入值列表
+ 返回：
+ 滤波后的值列表
+ """
+ filtered_values = []
+ for value in new_values:
+ filtered = self.update(value)
+ filtered_values.append(filtered)
+ return filtered_values
+
+ def reset(self, initial_value: float = 0.0):
+ """重置滤波器状态"""
+ self.filtered_value = initial_value
+ self.previous_raw = initial_value
+ self.previous_filtered = initial_value
+ self.history_raw = []
+ self.history_filtered = []
+
+ def get_state(self):
+ """获取当前状态"""
+ return {
+ 'filtered_value': self.filtered_value,
+ 'alpha': self.alpha,
+ 'history_length': len(self.history_raw)
+ }
+
+
+class MultiChannelLCFilter:
+ """
+ 多通道LC滤波器
+ 同时对多个信号进行滤波
+ """
+
+ def __init__(self, num_channels: int, alpha: float = 0.1,
+ initial_values: Optional[List[float]] = None):
+ """
+ 初始化多通道滤波器
+
+ 参数：
+ num_channels: 通道数量
+ alpha: 滤波系数
+ initial_values: 初始值列表，长度需等于num_channels
+ """
+ self.num_channels = num_channels
+ self.alpha = alpha
+
+ if initial_values is None:
+ initial_values = [0.0] * num_channels
+ elif len(initial_values) != num_channels:
+ raise ValueError(f"初始值长度必须等于通道数 {num_channels}")
+
+ # 为每个通道创建一个滤波器
+ self.filters = [LCFilter(alpha, initial_values[i]) for i in range(num_channels)]
+
+ def update(self, new_values: List[float]) -> List[float]:
+ """
+ 更新所有通道
+
+ 参数：
+ new_values: 新的输入值列表，长度需等于num_channels
+ 返回：
+ 滤波后的值列表
+ """
+ if len(new_values) != self.num_channels:
+ raise ValueError(f"输入值长度必须等于通道数 {self.num_channels}")
+
+ filtered_values = []
+ for i in range(self.num_channels):
+ filtered = self.filters[i].update(new_values[i])
+ filtered_values.append(filtered)
+
+ return filtered_values
+
+ def update_channel(self, channel_idx: int, new_value: float) -> float:
+ """
+ 更新单个通道
+
+ 参数：
+ channel_idx: 通道索引 (0-based)
+ new_value: 新的输入值
+ 返回：
+ 滤波后的值
+ """
+ if channel_idx < 0 or channel_idx >= self.num_channels:
+ raise ValueError(f"通道索引必须在[0, {self.num_channels-1}]范围内")
+
+ return self.filters[channel_idx].update(new_value)
+
+ def reset(self, initial_values: Optional[List[float]] = None):
+ """重置所有通道"""
+ if initial_values is None:
+ initial_values = [0.0] * self.num_channels
+
+ for i in range(self.num_channels):
+ self.filters[i].reset(initial_values[i])
+
+ def get_state(self):
+ """获取所有通道的状态"""
+ states = []
+ for i, filter_obj in enumerate(self.filters):
+ state = filter_obj.get_state()
+ state['channel'] = i
+ states.append(state)
+ return states
+
+
+class AdaptiveLCFilter(LCFilter):
+ """
+ 自适应LC滤波器
+ 根据信号变化自动调整alpha值
+ """
+
+ def __init__(self, alpha_min: float = 0.05, alpha_max: float = 0.3,
+ change_threshold: float = 0.1, initial_value: float = 0.0):
+ """
+ 初始化自适应滤波器
+
+ 参数：
+ alpha_min: 最小alpha值（信号稳定时使用）
+ alpha_max: 最大alpha值（信号快速变化时使用）
+ change_threshold: 变化阈值，超过此阈值认为信号在快速变化
+ initial_value: 初始值
+ """
+ super().__init__(alpha_max, initial_value) # 初始使用最大alpha
+ self.alpha_min = alpha_min
+ self.alpha_max = alpha_max
+ self.change_threshold = change_threshold
+
+ def update(self, new_value: float) -> float:
+ """
+ 自适应更新滤波器
+
+ 策略：如果信号变化大，使用较大的alpha快速响应
+ 如果信号稳定，使用较小的alpha平滑滤波
+ """
+ # 计算信号变化量
+ change_amount = abs(new_value - self.previous_raw)
+
+ # 自适应调整alpha
+ if change_amount > self.change_threshold:
+ # 信号快速变化，使用大alpha快速响应
+ self.alpha = self.alpha_max
+ else:
+ # 信号稳定，使用小alpha平滑滤波
+ self.alpha = self.alpha_min
+
+ # 调用父类更新方法
+ return super().update(new_value)
+
+
+def apply_lc_filter(data: List[float], alpha: float = 0.1) -> List[float]:
+ """
+ 对数据应用LC滤波（函数式版本）
+
+ 参数：
+ data: 输入数据列表
+ alpha: 滤波系数
+ 返回：
+ 滤波后的数据列表
+ """
+ if alpha <= 0 or alpha > 1:
+ raise ValueError("alpha必须在(0, 1]范围内")
+
+ if not data:
+ return []
+
+ filtered = [data[0]] # 第一个值直接使用
+
+ for i in range(1, len(data)):
+ # LC滤波公式
+ y = alpha * data[i] + (1 - alpha) * filtered[i-1]
+ filtered.append(y)
+
+ return filtered
+
+class KalmanFilter:
+ """
+ 卡尔曼滤波器（简化版）
+ 用于一维信号的滤波
+ """
+
+ def __init__(self,
+ process_variance: float = 1e-5,
+ measurement_variance: float = 0.1,
+ initial_value: float = 0.0,
+ initial_estimate_error: float = 1.0):
+ """
+ 初始化卡尔曼滤波器
+
+ 参数：
+ process_variance: 过程噪声方差（Q，系统不确定性）
+ measurement_variance: 测量噪声方差（R，传感器噪声）
+ initial_value: 初始状态估计值
+ initial_estimate_error: 初始估计误差协方差
+ """
+ # 系统模型（简单的一维模型）
+ self.process_variance = process_variance # Q
+ self.measurement_variance = measurement_variance # R
+
+ # 状态估计
+ self.x_hat = initial_value # 状态估计值
+ self.p = initial_estimate_error # 估计误差协方差
+
+ # 历史记录（可选）
+ self.history_measurement = []
+ self.history_estimate = []
+ self.history_kalman_gain = []
+
+ def update(self, measurement: float) -> float:
+ """
+ 卡尔曼滤波更新步骤
+
+ 参数：
+ measurement: 测量值
+ 返回：
+ 滤波后的估计值
+ """
+ # 1. 预测步骤
+ # 对于简单的一维模型，假设状态不变
+ x_hat_minus = self.x_hat # 先验状态估计
+ p_minus = self.p + self.process_variance # 先验估计误差
+
+ # 2. 更新步骤
+ # 计算卡尔曼增益
+ k = p_minus / (p_minus + self.measurement_variance) # 卡尔曼增益
+
+ # 更新状态估计
+ self.x_hat = x_hat_minus + k * (measurement - x_hat_minus)
+
+ # 更新估计误差协方差
+ self.p = (1 - k) * p_minus
+
+ # 记录历史
+ self.history_measurement.append(measurement)
+ self.history_estimate.append(self.x_hat)
+ self.history_kalman_gain.append(k)
+
+ return self.x_hat
+
+ def update_batch(self, measurements: List[float]) -> List[float]:
+ """
+ 批量更新
+
+ 参数：
+ measurements: 测量值列表
+ 返回：
+ 滤波后的估计值列表
+ """
+ estimates = []
+ for measurement in measurements:
+ estimate = self.update(measurement)
+ estimates.append(estimate)
+ return estimates
+
+ def reset(self,
+ initial_value: float = 0.0,
+ initial_estimate_error: float = 1.0):
+ """
+ 重置滤波器状态
+ """
+ self.x_hat = initial_value
+ self.p = initial_estimate_error
+ self.history_measurement = []
+ self.history_estimate = []
+ self.history_kalman_gain = []
+
+ def get_state(self) -> dict:
+ """
+ 获取当前状态
+ """
+ return {
+ 'estimate': self.x_hat,
+ 'error_covariance': self.p,
+ 'process_variance': self.process_variance,
+ 'measurement_variance': self.measurement_variance
+ }
+
+
+class MultiChannelKalmanFilter:
+ """
+ 多通道卡尔曼滤波器
+ 同时对多个独立信号进行滤波
+ """
+
+ def __init__(self,
+ num_channels: int,
+ process_variance: float = 1e-5,
+ measurement_variance: float = 0.1,
+ initial_values: Optional[List[float]] = None):
+ """
+ 初始化多通道卡尔曼滤波器
+
+ 参数：
+ num_channels: 通道数量
+ process_variance: 过程噪声方差
+ measurement_variance: 测量噪声方差
+ initial_values: 初始值列表
+ """
+ self.num_channels = num_channels
+
+ if initial_values is None:
+ initial_values = [0.0] * num_channels
+ elif len(initial_values) != num_channels:
+ raise ValueError(f"初始值长度必须等于通道数 {num_channels}")
+
+ # 为每个通道创建独立的卡尔曼滤波器
+ self.filters = [
+ KalmanFilter(
+ process_variance=process_variance,
+ measurement_variance=measurement_variance,
+ initial_value=initial_values[i],
+ initial_estimate_error=1.0
+ ) for i in range(num_channels)
+ ]
+
+ def update(self, measurements: List[float]) -> List[float]:
+ """
+ 更新所有通道
+
+ 参数：
+ measurements: 测量值列表，长度需等于num_channels
+ 返回：
+ 滤波后的估计值列表
+ """
+ if len(measurements) != self.num_channels:
+ raise ValueError(f"测量值长度必须等于通道数 {self.num_channels}")
+
+ estimates = []
+ for i in range(self.num_channels):
+ estimate = self.filters[i].update(measurements[i])
+ estimates.append(estimate)
+
+ return estimates
+
+ def update_channel(self, channel_idx: int, measurement: float) -> float:
+ """
+ 更新单个通道
+
+ 参数：
+ channel_idx: 通道索引
+ measurement: 测量值
+ 返回：
+ 滤波后的估计值
+ """
+ if channel_idx < 0 or channel_idx >= self.num_channels:
+ raise ValueError(f"通道索引必须在[0, {self.num_channels-1}]范围内")
+
+ return self.filters[channel_idx].update(measurement)
+
+ def reset(self, initial_values: Optional[List[float]] = None):
+ """
+ 重置所有通道
+ """
+ if initial_values is None:
+ initial_values = [0.0] * self.num_channels
+
+ for i in range(self.num_channels):
+ self.filters[i].reset(
+ initial_value=initial_values[i],
+ initial_estimate_error=1.0
+ )
+
+ def get_state(self, channel_idx: Optional[int] = None) -> dict:
+ """
+ 获取状态信息
+ """
+ if channel_idx is not None:
+ if channel_idx < 0 or channel_idx >= self.num_channels:
+ raise ValueError(f"通道索引必须在[0, {self.num_channels-1}]范围内")
+ return self.filters[channel_idx].get_state()
+ else:
+ states = []
+ for i, filter_obj in enumerate(self.filters):
+ state = filter_obj.get_state()
+ state['channel'] = i
+ states.append(state)
+ return {'channels': states}
+
+
+class AdaptiveKalmanFilter(KalmanFilter):
+ """
+ 自适应卡尔曼滤波器
+ 根据测量噪声自动调整参数
+ """
+
+ def __init__(self,
+ min_process_variance: float = 1e-6,
+ max_process_variance: float = 1e-3,
+ initial_measurement_variance: float = 0.1,
+ adaptation_rate: float = 0.01,
+ initial_value: float = 0.0):
+ """
+ 初始化自适应卡尔曼滤波器
+
+ 参数：
+ min_process_variance: 最小过程噪声方差
+ max_process_variance: 最大过程噪声方差
+ initial_measurement_variance: 初始测量噪声方差
+ adaptation_rate: 自适应调整速率
+ """
+ super().__init__(
+ process_variance=(min_process_variance + max_process_variance) / 2,
+ measurement_variance=initial_measurement_variance,
+ initial_value=initial_value
+ )
+
+ self.min_process_variance = min_process_variance
+ self.max_process_variance = max_process_variance
+ self.adaptation_rate = adaptation_rate
+ self.measurement_history = []
+
+ def update(self, measurement: float) -> float:
+ """
+ 自适应更新
+ """
+ # 保存测量历史
+ self.measurement_history.append(measurement)
+ if len(self.measurement_history) > 10:
+ self.measurement_history.pop(0)
+
+ # 计算最近的测量噪声
+ if len(self.measurement_history) >= 5:
+ recent_std = np.std(self.measurement_history[-5:])
+ # 根据噪声水平调整过程噪声方差
+ if recent_std > 0.1:
+ # 噪声大，增加过程噪声方差
+ self.process_variance = min(
+ self.process_variance * (1 + self.adaptation_rate),
+ self.max_process_variance
+ )
+ else:
+ # 噪声小，减小过程噪声方差
+ self.process_variance = max(
+ self.process_variance * (1 - self.adaptation_rate),
+ self.min_process_variance
+ )
+
+ # 调用父类更新方法
+ return super().update(measurement)
+
+
+class SavitzkyGolayFilter:
+ """
+ Savitzky-Golay滤波器（实时版本）
+ 适合保留波形特征的平滑
+ """
+
+ def __init__(self, window_length: int = 7, polyorder: int = 2,
+ deriv: int = 0, delta: float = 1.0):
+ """
+ 初始化Savitzky-Golay滤波器
+
+ 参数：
+ window_length: 窗口长度（必须为奇数，且大于polyorder）
+ polyorder: 多项式阶数
+ deriv: 微分阶数（0表示平滑，1表示一阶导等）
+ delta: 采样间隔
+ """
+ if window_length % 2 == 0:
+ raise ValueError("window_length必须是奇数")
+ if window_length <= polyorder:
+ raise ValueError("window_length必须大于polyorder")
+
+ self.window_length = window_length
+ self.polyorder = polyorder
+ self.deriv = deriv
+ self.delta = delta
+
+ # 数据缓冲区
+ self.buffer = deque(maxlen=window_length)
+
+ # 计算滤波器系数
+ self.coefficients = self._compute_coefficients()
+
+ # 历史记录
+ self.history_input = []
+ self.history_output = []
+
+ def _compute_coefficients(self) -> np.ndarray:
+ """
+ 计算Savitzky-Golay滤波器系数
+
+ 返回：
+ 滤波器系数数组
+ """
+ # 简单实现：使用滑动窗口多项式拟合
+ # 对于实时应用，我们只需要中心点的系数
+ half_window = self.window_length // 2
+
+ # 构建范德蒙矩阵
+ x = np.arange(-half_window, half_window + 1, dtype=float)
+ A = np.vander(x, self.polyorder + 1, increasing=True)
+
+ # 使用最小二乘法求解系数
+ # 对于Savitzky-Golay，我们只需要中心点的拟合值
+ # 这相当于取A的伪逆的第一行
+ coeff = np.linalg.pinv(A)[self.deriv]
+
+ # 考虑微分和采样间隔
+ if self.deriv > 0:
+ for i in range(self.deriv):
+ coeff = np.polyder(coeff)
+ coeff = coeff / (self.delta ** self.deriv)
+
+ return coeff
+
+ def update(self, new_value: float) -> float:
+ """
+ 更新滤波器并返回滤波后的值
+
+ 参数：
+ new_value: 新的输入值
+ 返回：
+ 滤波后的值
+ """
+ # 添加到缓冲区
+ self.buffer.append(new_value)
+
+ # 如果缓冲区未满，直接返回原值
+ if len(self.buffer) < self.window_length:
+ self.history_input.append(new_value)
+ self.history_output.append(new_value)
+ return new_value
+
+ # 应用Savitzky-Golay滤波
+ # 将缓冲区转换为数组
+ window_data = np.array(self.buffer)
+
+ # 使用预计算的系数进行卷积
+ filtered_value = np.dot(window_data, self.coefficients)
+
+ # 记录历史
+ self.history_input.append(new_value)
+ self.history_output.append(filtered_value)
+
+ # 限制历史长度
+ max_history = 1000
+ if len(self.history_input) > max_history:
+ self.history_input = self.history_input[-max_history:]
+ self.history_output = self.history_output[-max_history:]
+
+ return filtered_value
+
+ def update_batch(self, new_values: List[float]) -> List[float]:
+ """
+ 批量更新
+
+ 参数：
+ new_values: 新的输入值列表
+ 返回：
+ 滤波后的值列表
+ """
+ filtered_values = []
+ for value in new_values:
+ filtered = self.update(value)
+ filtered_values.append(filtered)
+ return filtered_values
+
+ def reset(self):
+ """重置滤波器状态"""
+ self.buffer.clear()
+ self.history_input = []
+ self.history_output = []
+
+ def get_state(self) -> dict:
+ """获取当前状态"""
+ return {
+ 'window_length': self.window_length,
+ 'polyorder': self.polyorder,
+ 'deriv': self.deriv,
+ 'buffer_size': len(self.buffer),
+ 'coefficients': self.coefficients.tolist()
+ }
+
+
+class MultiChannelSavitzkyGolayFilter:
+ """
+ 多通道Savitzky-Golay滤波器
+ """
+
+ def __init__(self, num_channels: int,
+ window_length: int = 7, polyorder: int = 2,
+ initial_values: Optional[List[float]] = None):
+ """
+ 初始化多通道滤波器
+
+ 参数：
+ num_channels: 通道数量
+ window_length: 窗口长度
+ polyorder: 多项式阶数
+ initial_values: 初始值列表
+ """
+ self.num_channels = num_channels
+
+ if initial_values is None:
+ initial_values = [0.0] * num_channels
+ elif len(initial_values) != num_channels:
+ raise ValueError(f"初始值长度必须等于通道数 {num_channels}")
+
+ # 为每个通道创建滤波器
+ self.filters = []
+ for i in range(num_channels):
+ filter_obj = SavitzkyGolayFilter(
+ window_length=window_length,
+ polyorder=polyorder
+ )
+ # 用初始值填充缓冲区
+ for _ in range(window_length // 2):
+ filter_obj.update(initial_values[i])
+ self.filters.append(filter_obj)
+
+ def update(self, new_values: List[float]) -> List[float]:
+ """
+ 更新所有通道
+
+ 参数：
+ new_values: 新的输入值列表
+ 返回：
+ 滤波后的值列表
+ """
+ if len(new_values) != self.num_channels:
+ raise ValueError(f"输入值长度必须等于通道数 {self.num_channels}")
+
+ filtered_values = []
+ for i in range(self.num_channels):
+ filtered = self.filters[i].update(new_values[i])
+ filtered_values.append(filtered)
+
+ return filtered_values
+
+ def reset(self, initial_values: Optional[List[float]] = None):
+ """重置所有通道"""
+ if initial_values is None:
+ initial_values = [0.0] * self.num_channels
+
+ for i in range(self.num_channels):
+ self.filters[i].reset()
+ # 用初始值预热
+ for _ in range(self.filters[i].window_length // 2):
+ self.filters[i].update(initial_values[i])
+
+
+class AdaptiveSavitzkyGolayFilter:
+ """
+ 自适应Savitzky-Golay滤波器
+ 根据信号特性自动调整参数
+ """
+
+ def __init__(self,
+ min_window: int = 5,
+ max_window: int = 15,
+ base_polyorder: int = 2,
+ noise_threshold: float = 0.05,
+ initial_value: float = 0.0):
+ """
+ 初始化自适应滤波器
+
+ 参数：
+ min_window: 最小窗口长度
+ max_window: 最大窗口长度
+ base_polyorder: 基础多项式阶数
+ noise_threshold: 噪声阈值
+ initial_value: 初始值
+ """
+ self.min_window = min_window
+ self.max_window = max_window
+ self.base_polyorder = base_polyorder
+ self.noise_threshold = noise_threshold
+
+ # 当前滤波器
+ self.current_filter = SavitzkyGolayFilter(
+ window_length=(min_window + max_window) // 2,
+ polyorder=base_polyorder
+ )
+
+ # 信号特性跟踪
+ self.signal_buffer = deque(maxlen=20)
+ self.current_noise_level = 0.0
+
+ def update(self, new_value: float) -> float:
+ """
+ 自适应更新
+ """
+ # 更新信号缓冲区
+ self.signal_buffer.append(new_value)
+
+ # 计算信号特性（噪声水平）
+ if len(self.signal_buffer) >= 10:
+ recent_data = np.array(self.signal_buffer)
+ self.current_noise_level = np.std(recent_data)
+
+ # 根据噪声水平调整窗口大小
+ if len(self.signal_buffer) >= 5:
+ if self.current_noise_level > self.noise_threshold * 2:
+ # 高噪声，使用大窗口强滤波
+ new_window = self.max_window
+ elif self.current_noise_level > self.noise_threshold:
+ # 中等噪声，使用中等窗口
+ new_window = (self.min_window + self.max_window) // 2
+ else:
+ # 低噪声，使用小窗口保留细节
+ new_window = self.min_window
+
+ # 如果窗口大小需要改变，创建新滤波器
+ if new_window != self.current_filter.window_length:
+ # 获取当前滤波器的输出作为新滤波器的初始状态
+ current_output = self.current_filter.update(new_value)
+
+ # 创建新滤波器
+ self.current_filter = SavitzkyGolayFilter(
+ window_length=new_window,
+ polyorder=min(self.base_polyorder, new_window - 1)
+ )
+
+ # 用当前输出预热新滤波器
+ for _ in range(new_window // 2):
+ self.current_filter.update(current_output)
+
+ return current_output
+
+ # 使用当前滤波器
+ return self.current_filter.update(new_value)
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/linkerhand/handcore.py b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/handcore.py
new file mode 100644
index 0000000..a0424ab
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/handcore.py
@@ -0,0 +1,338 @@
+from .config import *
+from .yourdfpy import URDF
+from .constants import RobotName,ROBOT_LEN,ROBOT_LEN_MAP
+import threading
+from pathlib import Path
+
+class HandCore():
+ def __init__(self, hand_config: HandConfig):
+ handconfig = hand_config.handconfig
+ self.baseconfig = hand_config.baseconfig
+ self.retagetconfig = hand_config.retagetconfig
+ self.modelconfig = hand_config.modelconfig
+ robot_dir = hand_config.robot_dir
+ targetpose = hand_config.targetpose
+ self.robot_name_str_r = self.baseconfig["system"]["robotname_r"]
+ self.robot_name_str_l = self.baseconfig["system"]["robotname_l"]
+
+ self.righturdfpath = os.path.join(robot_dir, 'linker_hand', f'{self.robot_name_str_r}_right', f'linkerhand_{self.robot_name_str_r}_right.urdf')
+ urdf_path = Path(self.righturdfpath)
+ if not urdf_path.exists():
+ raise ValueError(f"URDF path {urdf_path} does not exist")
+ self.dataminvalue_r = change_list(handconfig[f'commandlower_right_{self.robot_name_str_r}'])
+ self.datamaxvalue_r = change_list(handconfig[f'commandupper_right_{self.robot_name_str_r}'])
+ self.sourcedataindex_r = change_list(handconfig[f'commandsourcedataindex_right_{self.robot_name_str_r}'])
+ self.urdfdataindex_r = change_list(handconfig[f'urdfdataindex_right_{self.robot_name_str_r}'])
+ self.RightHandId = URDF.load(self.righturdfpath)
+
+
+ self.lefturdfpath = os.path.join(robot_dir, 'linker_hand', f'{self.robot_name_str_l}_left', f'linkerhand_{self.robot_name_str_l}_left.urdf')
+ urdf_path = Path(self.lefturdfpath)
+ if not urdf_path.exists():
+ raise ValueError(f"URDF path {urdf_path} does not exist")
+ self.dataminvalue_l = change_list(handconfig[f'commandlower_left_{self.robot_name_str_l}'])
+ self.datamaxvalue_l = change_list(handconfig[f'commandupper_left_{self.robot_name_str_l}'])
+ self.sourcedataindex_l = change_list(handconfig[f'commandsourcedataindex_left_{self.robot_name_str_l}'])
+ self.urdfdataindex_l = change_list(handconfig[f'urdfdataindex_left_{self.robot_name_str_l}'])
+ self.LeftHandId = URDF.load(self.lefturdfpath)
+
+ self.hand_lower_limits_r, self.hand_upper_limits_r, self.hand_joint_ranges_r = self.get_joint_limits(
+ self.RightHandId)
+ self.hand_lower_limits_l, self.hand_upper_limits_l, self.hand_joint_ranges_l = self.get_joint_limits(
+ self.LeftHandId)
+
+ self.hand_numjoints_r = ROBOT_LEN_MAP[RobotName[self.robot_name_str_r]]
+ self.hand_numjoints_l = ROBOT_LEN_MAP[RobotName[self.robot_name_str_l]]
+
+ if "human" in self.baseconfig["humanset"]["targethandfile"]:
+ self.right_hand_targetpose = np.array(targetpose['initial_positions']['right_hand'])
+ self.left_hand_targetpose = np.array(targetpose['initial_positions']['left_hand'])
+ else:
+ self.right_hand_targetpose = np.array(targetpose['initial_positions']['right_hand'][f'{self.robot_name_str_r}'])
+ self.left_hand_targetpose = np.array(targetpose['initial_positions']['left_hand'][f'{self.robot_name_str_l}'])
+ self.debugcount = 0
+ self.multi_target_kf_r = MultiTargetKalman(self.hand_numjoints_r)
+ self.multi_target_kf_l = MultiTargetKalman(self.hand_numjoints_l)
+
+ # 四元数测试用
+ self.angle = 0
+ self.counter = 0
+
+ # 共享数据
+ self.lock = threading.Lock() # 线程锁
+ self.right_joint_angles = [] # 存储关节角度等数据
+ self.left_joint_angles = [] # 存储关节角度等数据
+
+ @staticmethod
+ def get_joint_limits(robot):
+ joint_lower_limits = []
+ joint_upper_limits = []
+ joint_ranges = []
+ # 遍历所有关节
+ for joint_name, joint in robot.joint_map.items():
+ # 跳过固定关节
+ if joint.type == "fixed":
+ continue
+ # 获取关节限位值
+ if joint.limit is not None:
+ lower = joint.limit.lower
+ upper = joint.limit.upper
+ else:
+ # 对于没有明确限位的关节，使用默认值
+ # 连续旋转关节使用 ±π
+ if joint.type == "revolute":
+ lower = -3.1415926535 # -180°
+ upper = 3.1415926535 # +180°
+ # 平移关节使用 ±1m
+ elif joint.type == "prismatic":
+ lower = -1.0
+ upper = 1.0
+ # 其他类型关节使用 ±∞
+ else:
+ lower = float('-inf')
+ upper = float('inf')
+ # 添加到结果列表
+ joint_lower_limits.append(lower)
+ joint_upper_limits.append(upper)
+ joint_ranges.append(upper - lower)
+
+ return joint_lower_limits, joint_upper_limits, joint_ranges
+
+ @staticmethod
+ def projection_process(hand_position):
+ qpos = [0.0] * 30
+ cos_theta = 0
+ # 处理拇指部分,占用5个数据位
+ trumb_a = hand_position[0, :] # 对应MATLAB的 position_rightHand(1,:)
+ trumb_b = hand_position[1, :]
+ trumb_c = hand_position[2, :]
+ trumb_d = hand_position[3, :] # 夹角顶点
+ trumb_e = hand_position[4, :]
+ # 拇指侧摆部分处理成YZ平面
+ A_proj = np.array([trumb_a[1], trumb_a[2]])
+ B_proj = np.array([trumb_b[1], trumb_b[2]])
+ C_proj = np.array([trumb_c[1], trumb_c[2]])
+ vec_BA_proj = A_proj - B_proj
+ vec_BC_proj = C_proj - B_proj
+ dot_product = np.dot(vec_BA_proj, vec_BC_proj)
+ norm_AB = np.linalg.norm(vec_BA_proj)
+ norm_BC = np.linalg.norm(vec_BC_proj)
+ if norm_AB != 0 and norm_BC != 0:
+ cos_theta = np.arccos(dot_product / (norm_AB * norm_BC))
+ angle_deg = np.pi - cos_theta
+ # 拇指旋转
+ qpos[0] = angle_deg
+
+ # 拇指侧摆部分处理成XY平面
+ A_proj = trumb_a[:2] # 提取[X, Y]
+ B_proj = trumb_b[:2]
+ C_proj = trumb_c[:2]
+ vec_BA_proj = A_proj - B_proj
+ vec_BC_proj = C_proj - B_proj
+ dot_product = np.dot(vec_BA_proj, vec_BC_proj)
+ norm_AB = np.linalg.norm(vec_BA_proj)
+ norm_BC = np.linalg.norm(vec_BC_proj)
+ if norm_AB != 0 and norm_BC != 0:
+ cos_theta = np.arccos(dot_product / (norm_AB * norm_BC))
+ angle_deg = np.pi - cos_theta
+ # 拇指侧摆
+ qpos[1] = angle_deg
+
+ vecDC = trumb_c - trumb_d # 对应MATLAB的 vecBA = C - D
+ vecDE = trumb_e - trumb_d # 对应MATLAB的 vecBC = E - D
+ dot_product = np.dot(vecDC, vecDE)
+ norm_AB = np.linalg.norm(vecDC)
+ norm_BC = np.linalg.norm(vecDE)
+ if norm_AB != 0 and norm_BC != 0:
+ cos_theta = np.pi - np.arccos(dot_product / (norm_AB * norm_BC))
+ # 拇指末端夹角
+ qpos[4] = cos_theta
+
+ vecBC = trumb_b - trumb_c # 对应MATLAB的 vecBA = C - D
+ vecDC = trumb_d - trumb_c # 对应MATLAB的 vecBC = E - D
+ dot_product = np.dot(vecDC, vecBC)
+ norm_AB = np.linalg.norm(vecBC)
+ norm_BC = np.linalg.norm(vecDC)
+ if norm_AB != 0 and norm_BC != 0:
+ cos_theta = np.pi - np.arccos(dot_product / (norm_AB * norm_BC))
+ # 拇指中部夹角
+ qpos[3] = cos_theta
+
+ vecAB = trumb_a - trumb_b # 对应MATLAB的 vecBA = C - D
+ vecBC = trumb_c - trumb_b # 对应MATLAB的 vecBC = E - D
+ dot_product = np.dot(vecAB, vecBC)
+ norm_AB = np.linalg.norm(vecAB)
+ norm_BC = np.linalg.norm(vecBC)
+ if norm_AB != 0 and norm_BC != 0:
+ cos_theta = np.pi - np.arccos(dot_product / (norm_AB * norm_BC))
+ # 拇指根部夹角
+ qpos[2] = cos_theta
+
+ # 处理四指部分,占用5个数据位
+ # 食指侧摆部分处理成YZ平面
+ for i in range(4):
+ other_a = hand_position[5 + 5 * i, :]
+ other_b = hand_position[6 + 5 * i, :]
+ other_c = hand_position[7 + 5 * i, :]
+ other_d = hand_position[8 + 5 * i, :] # 夹角顶点
+ other_e = hand_position[9 + 5 * i, :]
+
+ A_proj = np.array([other_b[1], other_b[2] + 0.1])
+ B_proj = np.array([other_b[1], other_b[2]])
+ C_proj = np.array([other_c[1], other_b[2] + 0.1])
+ vecAB = A_proj - B_proj
+ vecBC = C_proj - B_proj
+ dot_product = np.dot(vecAB, vecBC)
+ norm_AB = np.linalg.norm(vecAB)
+ norm_BC = np.linalg.norm(vecBC)
+ if norm_AB != 0 and norm_BC != 0:
+ cos_theta = np.arccos(dot_product / (norm_AB * norm_BC))
+ # 侧摆
+ qpos[5 + 5 * i] = cos_theta
+
+ # 其余四指部分处理成XZ平面
+ # 处理末端CDE3点
+ A_proj = np.array([other_c[0], other_c[2]])
+ B_proj = np.array([other_d[0], other_d[2]])
+ C_proj = np.array([other_e[0], other_e[2]])
+ vecAB = A_proj - B_proj
+ vecBC = C_proj - B_proj
+ dot_product = np.dot(vecAB, vecBC)
+ norm_AB = np.linalg.norm(vecAB)
+ norm_BC = np.linalg.norm(vecBC)
+ if norm_AB != 0 and norm_BC != 0:
+ cos_theta = np.pi - np.arccos(dot_product / (norm_AB * norm_BC))
+ # 末端夹角
+ qpos[9 + 5 * i] = cos_theta
+
+ # 处理中部BCD3点
+ A_proj = np.array([other_b[0], other_b[2]])
+ B_proj = np.array([other_c[0], other_c[2]])
+ C_proj = np.array([other_d[0], other_d[2]])
+ vecAB = A_proj - B_proj
+ vecBC = C_proj - B_proj
+ dot_product = np.dot(vecAB, vecBC)
+ norm_AB = np.linalg.norm(vecAB)
+ norm_BC = np.linalg.norm(vecBC)
+ cos_theta = np.pi - np.arccos(dot_product / (norm_AB * norm_BC))
+ # 中部夹角
+ qpos[8 + 5 * i] = cos_theta
+
+ # 处理根部ABC3点
+ A_proj = np.array([other_a[0], other_a[2]])
+ B_proj = np.array([other_b[0], other_b[2]])
+ C_proj = np.array([other_c[0], other_c[2]])
+ vecAB = A_proj - B_proj
+ vecBC = C_proj - B_proj
+ dot_product = np.dot(vecAB, vecBC)
+ norm_AB = np.linalg.norm(vecAB)
+ norm_BC = np.linalg.norm(vecBC)
+ if norm_AB != 0 and norm_BC != 0:
+ cos_theta = np.pi - np.arccos(dot_product / (norm_AB * norm_BC))
+ # 根部夹角
+ qpos[7 + 5 * i] = cos_theta
+ return qpos
+
+ def trans_to_motor_left(self, temp_l):
+ jointpositions_l = [255.0] * self.hand_numjoints_l
+ for i in range(self.hand_numjoints_l):
+ if self.sourcedataindex_l[i] is not None:
+ val_l = temp_l[self.sourcedataindex_l[i]]
+ val_l = is_within_range(val_l,
+ self.hand_lower_limits_l[self.urdfdataindex_l[i]],
+ self.hand_upper_limits_l[self.urdfdataindex_l[i]])
+ jointpositions_l[i] = int(scale_value(val_l,
+ self.hand_lower_limits_l[self.urdfdataindex_l[i]],
+ self.hand_upper_limits_l[self.urdfdataindex_l[i]],
+ self.dataminvalue_l[i],
+ self.datamaxvalue_l[i]))
+ return jointpositions_l
+
+ def trans_to_motor_right(self, temp_r):
+ jointpositions_r = [255.0] * self.hand_numjoints_r
+ for i in range(self.hand_numjoints_r):
+ if self.sourcedataindex_r[i] is not None:
+ val_r = temp_r[self.sourcedataindex_r[i]]
+ val_r = is_within_range(val_r,
+ self.hand_lower_limits_r[self.urdfdataindex_r[i]],
+ self.hand_upper_limits_r[self.urdfdataindex_r[i]])
+
+ jointpositions_r[i] = int(scale_value(val_r,
+ self.hand_lower_limits_r[self.urdfdataindex_r[i]],
+ self.hand_upper_limits_r[self.urdfdataindex_r[i]],
+ self.dataminvalue_r[i],
+ self.datamaxvalue_r[i]))
+ return jointpositions_r
+
+
+ def generate_position(self, quaternion_r, quaternion_l):
+ rootorin_correct_r = get_quaternion_relative(trans_wxyzori_to_xyzwori(quaternion_r[0]),
+ [0, 0, 0, 1])
+ rootorin_correct_l = get_quaternion_relative(trans_wxyzori_to_xyzwori(quaternion_l[0]),
+ [0, 0, 0, 1])
+ handorin_correct_r = []
+ handorin_correct_l = []
+ for i in range(20):
+ handorin_correct_r.append(
+ trans_xyzwori_to_wxyzori(get_child_quaternion(trans_wxyzori_to_xyzwori(quaternion_r[i]),
+ rootorin_correct_r)))
+ for i in range(20):
+ handorin_correct_l.append(
+ trans_xyzwori_to_wxyzori(get_child_quaternion(trans_wxyzori_to_xyzwori(quaternion_l[i]),
+ rootorin_correct_l)))
+
+ right_hand_pose = quat2handposition(handorin_correct_r, self.right_hand_targetpose)
+ left_hand_pose = quat2handposition(handorin_correct_l, self.left_hand_targetpose)
+
+ # 绕 Y 轴的旋转-90度
+ Ry = rotate_matrix_y(np.radians(-90))
+ right_hand_pose = np.dot(Ry, right_hand_pose.T).T
+ # 先绕 Z 轴的旋转180度再绕Y轴旋转-90度
+ Rz = rotate_matrix_z(np.radians(-180))
+ left_hand_pose = np.dot(Rz, left_hand_pose.T).T
+ Ry = rotate_matrix_y(np.radians(-90))
+ left_hand_pose = np.dot(Ry, left_hand_pose.T).T
+ return right_hand_pose, left_hand_pose
+
+ def update_angles(self, rightangles, leftangle):
+ with self.lock:
+ self.right_joint_angles = rightangles
+ self.left_joint_angles = leftangle
+
+ def get_angles(self):
+ with self.lock:
+ return self.right_joint_angles.copy(), self.left_joint_angles.copy()
+
+class KalmanFilter:
+ def __init__(self, process_variance, measurement_variance, estimated_error, initial_value):
+ self.process_variance = process_variance # 过程噪声
+ self.measurement_variance = measurement_variance # 测量噪声
+ self.estimated_error = estimated_error # 初始估计误差
+ self.current_estimate = initial_value # 初始值
+
+ def update(self, measurement):
+ # 预测更新
+ self.estimated_error += self.process_variance
+
+ # 计算卡尔曼增益
+ kalman_gain = self.estimated_error / (self.estimated_error + self.measurement_variance)
+
+ # 更新估计值
+ self.current_estimate += kalman_gain * (measurement - self.current_estimate)
+ # 更新误差
+ self.estimated_error *= (1 - kalman_gain)
+
+ return self.current_estimate
+
+
+class MultiTargetKalman:
+ def __init__(self, num_targets, process_variance=0.01, measurement_variance=0.1, estimated_error=1,
+ initial_value=255):
+ self.kalman_filters = [KalmanFilter(process_variance, measurement_variance, estimated_error, initial_value) for
+ _ in range(num_targets)]
+ self.num_targets = num_targets
+ self.smoothed_data = [[] for _ in range(num_targets)]
+
+ def update(self, measurements, index):
+ smooth_value = self.kalman_filters[index].update(measurements)
+ return smooth_value
diff --git a/src/linkerhand_retarget/linkerhand_retarget/linkerhand/handcoreex.py b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/handcoreex.py
new file mode 100644
index 0000000..a89eaef
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/handcoreex.py
@@ -0,0 +1,940 @@
+"""
+多态线性映射器
+支持任意数量的状态
+"""
+import numpy as np
+from colorama import Fore, init
+from typing import List, Dict, Tuple
+from .filter import MultiChannelLCFilter, MultiChannelSavitzkyGolayFilter, MultiChannelKalmanFilter
+
+class MultiStateLinearMapper:
+ """
+ 多态线性映射器
+ 支持任意数量的手势状态
+ """
+
+ def __init__(self,FINGER_CONFIGS,MAPPING_ORDER,is_debug = False):
+ self.finger_configs = FINGER_CONFIGS.copy()
+ self.mapping_order = MAPPING_ORDER.copy()
+
+ # 状态存储
+ self.glove_states = {} # {状态名: 手套角度数组}
+ self.robot_states = {} # {状态名: 机械手角度数组}
+ self.state_order = [] # 状态顺序列表
+ self.debug_value = [0.0] * 20 # 长度20的debug缓冲数据
+ self.isdebug = is_debug
+ self.debug_fingers = None # None=全部, []=全部, ["finger_name"]=指定手指
+
+ # self.filters = MultiChannelLCFilter(num_channels=11, alpha=0.1)
+ num_joints = 21
+
+ # 创建多通道Savitzky-Golay滤波器
+ self.filters = MultiChannelKalmanFilter(
+ num_channels=num_joints,
+ process_variance=1e-3,
+ measurement_variance=0.001,
+ initial_values=[0.0] * num_joints
+ )
+
+ # self.filters = MultiChannelSavitzkyGolayFilter(
+ # num_channels=num_joints,
+ # window_length=,
+ # polyorder=3
+ # )
+
+ # 滤波参数
+ # self.filter_params = {
+ # 'window_length': 7,
+ # 'polyorder': 2,
+ # 'filter_type': 'Savitzky-Golay'
+ # }
+
+ # 历史记录（用于调试和可视化）
+ self.raw_history = []
+ self.filtered_history = []
+
+ def add_state(self, state_name: str,
+ glove_angles: List[float],
+ robot_angles: List[float]):
+ """
+ 添加一个手势状态
+
+ 参数：
+ state_name: 状态名称，如 'original', 'opose', 'fist'等
+ glove_angles: 手套角度 (21维)
+ robot_angles: 机械手角度 (11维)
+ """
+ self.glove_states[state_name] = np.array(glove_angles)
+ self.robot_states[state_name] = np.array(robot_angles)
+
+ if state_name not in self.state_order:
+ self.state_order.append(state_name)
+
+ def remove_state(self, state_name: str):
+ """移除一个状态"""
+ if state_name in self.glove_states:
+ del self.glove_states[state_name]
+ del self.robot_states[state_name]
+ if state_name in self.state_order:
+ self.state_order.remove(state_name)
+
+ def set_state_order(self, state_order: List[str]):
+ """
+ 设置状态顺序（从原始到最弯曲）
+
+ 示例：
+ ['original', 'opose', 'fist']
+ """
+ # 验证所有状态都存在
+ for state in state_order:
+ if state not in self.glove_states:
+ raise ValueError(f"状态 '{state}' 未定义")
+
+ self.state_order = state_order
+
+ def map_glove_to_robot(self, glove_current):
+ """
+ 动态权重映射
+ 在映射过程中根据其他手指状态调整权重
+ """
+
+ if isinstance(glove_current, np.ndarray):
+ glove_current = glove_current.tolist()
+ elif isinstance(glove_current, list):
+ glove_current = glove_current
+ else:
+ glove_current = list(glove_current)
+
+ if len(self.state_order) < 2:
+ raise ValueError("请至少设置两个状态")
+
+ if 'original' not in self.glove_states:
+ raise ValueError("必须包含 'original' 状态作为基准")
+
+ glove_current_arr = np.array(glove_current)
+ robot_angles = self.robot_states['original'].copy()
+
+ for config_name in self.mapping_order:
+ config = self.finger_configs[config_name]
+ angle = self._map_finger_multi_state(glove_current_arr, config)
+ robot_angles[config['robot_idx']] = angle
+
+ # self.debug_value[config['robot_idx']] = angle
+
+
+
+ return robot_angles
+
+ def _map_finger_multi_state(self, glove_current: np.ndarray,
+ config: dict) -> float:
+ """
+ 多状态手指映射
+ """
+ joints = config['joints']
+ weights = self._normalize_weights(config['weights'])
+ robot_idx = config['robot_idx']
+
+ # 计算当前融合值
+ current_fused = self._calculate_fused_value(
+ glove_current, joints, weights
+ )
+ # self.debug_value[robot_idx] = current_fused
+
+ # 计算所有状态的融合值
+ state_fused_values = {}
+ for state_name in self.state_order:
+ fused = self._calculate_reference_fused(
+ joints, weights, self.glove_states[state_name]
+ )
+ state_fused_values[state_name] = fused
+
+ # 获取所有状态的角度
+ state_angles = {}
+ for state_name in self.state_order:
+ state_angles[state_name] = self.robot_states[state_name][robot_idx]
+
+ # 分段线性插值
+ result_angle = self._multi_state_interpolation(
+ current_fused, state_fused_values, state_angles
+ )
+
+ # 处理反向运动
+ if config.get('reverse_motion', True):
+ # 找到最小和最大角度
+ min_angle = min(state_angles.values())
+ max_angle = max(state_angles.values())
+
+ result_angle = max_angle - (result_angle - min_angle)
+ # print("触发反向运动")
+
+ return result_angle
+
+ def _calculate_fused_value(self, data: np.ndarray,
+ joints: List[int],
+ weights) -> float:
+ """
+ 完整的融合值计算，处理上下限越界
+ """
+ # 确保有原始状态
+ if 'original' not in self.glove_states:
+ return 0.0
+
+ original = self.glove_states['original']
+ weights = np.array(weights)
+
+ # 归一化权重
+ if np.sum(weights) > 0:
+ weights = weights / np.sum(weights)
+
+ fused = 0.0
+
+ for i, idx in enumerate(joints):
+ # 获取当前值和原始值
+ current = data[idx]
+ orig = original[idx]
+
+ # 步骤1: 找到该关节在所有状态中的最小值和最大值
+ all_vals = [orig]
+ for state_data in self.glove_states.values():
+ all_vals.append(state_data[idx])
+
+ min_val = min(all_vals)
+ max_val = max(all_vals)
+
+ # 步骤2: 截断当前值到[min_val, max_val]范围
+ clamped = np.clip(current, min_val, max_val)
+
+ # 步骤3: 计算归一化位置
+ if abs(max_val - min_val) < 1e-6:
+ normalized_diff = 0.0
+ else:
+ orig_norm = (orig - min_val) / (max_val - min_val)
+ clamped_norm = (clamped - min_val) / (max_val - min_val)
+ normalized_diff = abs(clamped_norm - orig_norm)
+
+ fused += weights[i] * normalized_diff
+ return fused
+
+ def _calculate_reference_fused(self, joints: List[int],
+ weights: np.ndarray,
+ reference_data: np.ndarray) -> float:
+ """
+ 计算参考融合值
+ """
+ return self._calculate_fused_value(reference_data, joints, weights)
+
+ def _multi_state_interpolation(self, current_fused: float,
+ state_fused_values: Dict[str, float],
+ state_angles: Dict[str, float]) -> float:
+ """
+ 多状态分段线性插值
+ """
+ # 确保状态顺序正确
+ if not self.state_order:
+ return 0.0
+
+ # 处理边界情况
+ if current_fused <= state_fused_values[self.state_order[0]]:
+ return state_angles[self.state_order[0]]
+
+ if current_fused >= state_fused_values[self.state_order[-1]]:
+ return state_angles[self.state_order[-1]]
+
+ # 找到当前融合值所在区间
+ for i in range(len(self.state_order) - 1):
+ state1 = self.state_order[i]
+ state2 = self.state_order[i + 1]
+
+ fused1 = state_fused_values[state1]
+ fused2 = state_fused_values[state2]
+
+ # 确保区间有效
+ if fused1 <= current_fused <= fused2:
+ if fused2 - fused1 > 1e-6:
+ t = (current_fused - fused1) / (fused2 - fused1)
+ else:
+ t = 0.0
+
+ angle1 = state_angles[state1]
+ angle2 = state_angles[state2]
+ return angle1 + t * (angle2 - angle1)
+
+ # 如果没有找到区间（理论上不会发生），返回最近状态的角度
+ min_diff = float('inf')
+ nearest_angle = 0.0
+ for state_name in self.state_order:
+ diff = abs(current_fused - state_fused_values[state_name])
+ if diff < min_diff:
+ min_diff = diff
+ nearest_angle = state_angles[state_name]
+
+ return nearest_angle
+
+ def _normalize_weights(self, weights: List[float]) -> List[float]:
+ """
+ 归一化权重
+ """
+ if hasattr(weights, 'tolist'):
+ # 如果是 NumPy 数组
+ weight_list = weights.tolist()
+ elif isinstance(weights, list):
+ # 如果已经是列表
+ weight_list = weights
+ else:
+ # 其他情况，尝试转换
+ weight_list = list(weights)
+ total = np.sum(weight_list)
+ if total > 0:
+ result_array = weight_list / total
+ else:
+ result_array = weight_list
+
+ # 关键：转换回列表
+ return result_array.tolist()
+
+ def get_state_info(self) -> Dict:
+ """
+ 获取状态信息
+ """
+ # 基础信息
+ info = {
+ 'states': list(self.glove_states.keys()),
+ 'state_order': self.state_order,
+ 'has_original': 'original' in self.glove_states
+ }
+
+ return info
+
+
+ def clear_states(self):
+ """清除所有状态"""
+ self.glove_states.clear()
+ self.robot_states.clear()
+ self.state_order.clear()
+
+ def set_debug(self, enabled):
+ """
+ 设置 debug 模式
+
+ Args:
+ enabled: bool 或 list
+ - True: 开启调试，显示全部手指
+ - False: 关闭调试
+ - []: 开启调试，显示全部手指
+ - ["finger_name", ...]: 开启调试，只显示指定手指
+ """
+ if isinstance(enabled, bool):
+ self.isdebug = enabled
+ self.debug_fingers = None
+ elif isinstance(enabled, list):
+ self.isdebug = True
+ self.debug_fingers = enabled if enabled else None
+ else:
+ self.isdebug = bool(enabled)
+ self.debug_fingers = None
+
+ def _should_debug(self, finger_name: str) -> bool:
+ """检查是否应该输出该手指的调试信息"""
+ if not self.isdebug:
+ return False
+ if self.debug_fingers is None:
+ return True
+ return finger_name in self.debug_fingers
+
+
+class DynamicWeightMultiStateLinearMapper(MultiStateLinearMapper):
+ """
+ 动态权重多态线性映射器
+ 继承自MultiStateLinearMapper，增加动态权重调整功能
+ 增加扩展线性映射功能：基于open/opose线性映射，可以继续延伸
+ """
+
+ def __init__(self, FINGER_CONFIGS, MAPPING_ORDER,is_debug=False):
+ super().__init__(FINGER_CONFIGS, MAPPING_ORDER,is_debug)
+
+ # 动态权重配置
+ self.dynamic_weight_configs = {}
+
+ # 扩展映射配置
+ self.extended_mapping_enabled = {}
+ self.scale_factors = {}
+ self.exp_factors = {}
+ # self.isdebug = is_debug
+ # 缓存计算过的关节映射值
+ self.cached_mapped_values = {}
+
+ # 从配置表初始化扩展映射
+ self._init_extended_mapping_from_config()
+
+ def _init_extended_mapping_from_config(self):
+ """从配置表初始化扩展映射设置"""
+ for finger_name, config in self.finger_configs.items():
+ if config.get('dynamic_weight'):
+ self.set_dynamic_weight_config(finger_name, config['dynamic_weight'])
+ ext_config = config.get('extended_mapping')
+ if ext_config and ext_config.get('enabled', False):
+ self.extended_mapping_enabled[finger_name] = True
+
+ # 设置缩放因子
+ scale_factor = ext_config.get('scale_factor', 1.0)
+ if scale_factor != 1.0:
+ self.scale_factors[finger_name] = scale_factor
+ exp_factor = ext_config.get('extended_exp_factor', 1.0)
+ if exp_factor != 1.0:
+ self.exp_factors[finger_name] = exp_factor
+
+ def set_dynamic_weight_config(self, finger_name: str, config: Dict):
+ """
+ 设置动态权重配置
+ """
+ self.dynamic_weight_configs[finger_name] = config
+
+ def set_extended_mapping(self, finger_name: str, enabled: bool = True,
+ scale_factor: float = 1.0):
+ """
+ 手动设置扩展映射
+
+ 参数：
+ finger_name: 手指名称
+ enabled: 是否启用扩展映射
+ scale_factor: 缩放因子，>1加快映射，<1减慢映射
+ """
+ self.extended_mapping_enabled[finger_name] = enabled
+ if scale_factor != 1.0:
+ self.scale_factors[finger_name] = scale_factor
+
+ def fit_exp_factor(self, finger_name: str, current_fused: float,
+ fused_open: float, fused_opose: float,
+ angle_open: float, angle_opose: float, angle_fist: float) -> float:
+ """
+ 根据当前握拳值自动拟合延伸因子
+
+ 目标：使 current_fused 映射到 angle_fist
+
+ 公式：extension = slope * t * (1 + (exp-1) * t)
+ 其中 slope = angle_opose - angle_open, t = normalized - 1
+
+ 参数：
+ finger_name: 手指名称
+ current_fused: 当前握拳时的融合值
+ fused_open: 张开时的融合值
+ fused_opose: O型时的融合值
+ angle_open: 张开时的机械手角度
+ angle_opose: O型时的机械手角度
+ angle_fist: 握拳极限时的机械手角度
+
+ 返回：
+ 计算出的延伸因子
+ """
+ if abs(fused_opose - fused_open) < 1e-6:
+ return 1.0
+
+ normalized = (current_fused - fused_open) / (fused_opose - fused_open)
+
+ if normalized <= 1.0:
+ return 1.0
+
+ t = normalized - 1.0
+
+ slope = angle_opose - angle_open
+ target_extension = angle_fist - angle_opose
+
+ if abs(slope * t) < 1e-6 or abs(target_extension) < 1e-6:
+ return 1.0
+
+ base_extension = slope * t
+ ratio = target_extension / base_extension
+
+ exp_factor = (ratio - 1.0) / t + 1.0
+
+ return max(1.0, min(100.0, exp_factor))
+
+ def _apply_scale_factor(self, fused_value: float,
+ fused_open: float, fused_opose: float,
+ finger_name: str) -> float:
+ """
+ 应用缩放因子，基于归一化的[0,1]范围
+
+ 参数：
+ fused_value: 原始融合值
+ fused_open: open状态的融合值（映射到0）
+ fused_opose: opose状态的融合值（映射到1）
+ finger_name: 手指名称
+ """
+ scale_factor = self.scale_factors.get(finger_name, 1.0)
+
+ if scale_factor == 1.0:
+ return fused_value
+
+ # 将原始融合值归一化到[0,1]范围
+ # 融合值范围 [fused_open, fused_opose] -> [0, 1]
+ if abs(fused_opose - fused_open) < 1e-6:
+ normalized = 0.0
+ else:
+ normalized = (fused_value - fused_open) / (fused_opose - fused_open)
+
+ # 如果已经到达 opose 位置，不应用缩放
+ if abs(normalized - 1.0) < 1e-6:
+ return fused_value
+
+ # 应用缩放因子到归一化的值
+ scaled_normalized = normalized * scale_factor
+
+ # 将缩放后的归一化值转换回原始融合值范围
+ scaled_fused = fused_open + scaled_normalized * (fused_opose - fused_open)
+
+ return scaled_fused
+
+ def _get_max_angle(self, robot_idx: int) -> float:
+ """
+ 获取关节的最大角度
+ 如果有fist状态，使用fist状态的角度作为最大角度
+ 否则使用默认的最大角度
+ """
+ # 如果有fist状态，使用fist状态的角度
+ if 'fist' in self.robot_states:
+ return self.robot_states['fist'][robot_idx]
+
+ # 默认最大角度（可以根据需要调整）
+ return 1.57 # 默认90度
+
+ def map_glove_to_robot(self, source_current):
+ """
+ 动态权重映射
+ 在映射过程中根据其他手指状态调整权重
+ """
+ self.debug_value[3] = source_current[1]
+
+ glove_current = self.filters.update(source_current)
+ # 应用Savitzky-Golay滤波
+ # filtered_angles = self.filters.update(robot_angles)
+
+ # 记录历史（用于调试和分析）
+ self.raw_history.append(source_current.copy())
+ self.filtered_history.append(glove_current.copy())
+
+ # filtered_angles = self.filters.update(robot_angles)
+ # 限制历史记录长度
+ max_history = 100
+ if len(self.raw_history) > max_history:
+ self.raw_history = self.raw_history[-max_history:]
+ self.filtered_history = self.filtered_history[-max_history:]
+
+ self.debug_value[4] = glove_current[1]
+
+ if isinstance(glove_current, np.ndarray):
+ glove_current = glove_current.tolist()
+ elif isinstance(glove_current, list):
+ glove_current = glove_current
+ else:
+ glove_current = list(glove_current)
+
+ if len(self.state_order) < 2:
+ raise ValueError("请至少设置两个状态")
+
+ if 'original' not in self.glove_states:
+ raise ValueError("必须包含 'original' 状态作为基准")
+
+ # 重置缓存
+ self.cached_mapped_values = {}
+
+ glove_current_arr = np.array(glove_current)
+ robot_angles = self.robot_states['original'].copy()
+
+ # 第一遍：计算所有需要用于触发判断的手指映射值
+ for config_name in self.mapping_order:
+ if config_name in self.dynamic_weight_configs:
+ trigger_finger = self.dynamic_weight_configs[config_name]['trigger_finger']
+ # 先计算触发手指的映射值
+ if trigger_finger not in self.cached_mapped_values:
+ trigger_value = self._calculate_trigger_value(
+ glove_current_arr, trigger_finger
+ )
+ self.cached_mapped_values[trigger_finger] = trigger_value
+
+
+ i = 0
+ # 第二遍：使用动态权重进行映射
+ for config_name in self.mapping_order:
+ # 获取动态配置（如果有）
+ dynamic_config = self.dynamic_weight_configs.get(config_name)
+
+ if dynamic_config:
+ # 使用动态权重进行映射
+ config = self.finger_configs[config_name]
+ angle = self._map_finger_dynamic_weight(
+ glove_current_arr, config_name, dynamic_config, config
+ )
+ else:
+ # 使用多状态方法映射（支持扩展映射）
+ config = self.finger_configs[config_name]
+ angle = self._map_finger_multi_state(glove_current_arr, config)
+
+ robot_idx = self.finger_configs[config_name]['robot_idx']
+ robot_angles[robot_idx] = angle
+
+
+ return robot_angles
+
+ def _calculate_trigger_value(self, glove_current: np.ndarray,
+ trigger_finger: str) -> float:
+ """
+ 计算触发手指的归一化映射值（0-1范围）
+
+ 返回：
+ 归一化的映射值，0表示原始状态，1表示最弯曲状态
+ """
+ if trigger_finger not in self.finger_configs:
+ raise ValueError(f"触发手指配置 '{trigger_finger}' 不存在")
+
+ config = self.finger_configs[trigger_finger]
+
+ # 计算当前融合值
+ joints = config['joints']
+ weights = self._normalize_weights(config['weights'])
+
+ current_fused = self._calculate_fused_value(
+ glove_current, joints, weights
+ )
+
+ # 计算所有状态的融合值
+ state_fused_values = {}
+ for state_name in self.state_order:
+ fused = self._calculate_reference_fused(
+ joints, weights, self.glove_states[state_name]
+ )
+ state_fused_values[state_name] = fused
+
+ # 归一化到0-1范围
+ min_fused = min(state_fused_values.values())
+ max_fused = max(state_fused_values.values())
+
+ if abs(max_fused - min_fused) < 1e-6:
+ return 0.0
+
+ normalized = (current_fused - min_fused) / (max_fused - min_fused)
+ return np.clip(normalized, 0.0, 1.0)
+
+ def _map_finger_dynamic_weight(self, glove_current: np.ndarray,
+ finger_name: str,
+ dynamic_config: Dict,
+ base_config: Dict) -> float:
+ """
+ 使用动态权重进行手指映射
+ """
+ # 获取触发值
+ trigger_finger = dynamic_config['trigger_finger']
+ if trigger_finger not in self.cached_mapped_values:
+ trigger_value = self._calculate_trigger_value(
+ glove_current, trigger_finger
+ )
+ self.cached_mapped_values[trigger_finger] = trigger_value
+ else:
+ trigger_value = self.cached_mapped_values[trigger_finger]
+
+ # 根据阈值选择配置
+ threshold = dynamic_config['threshold']
+ temp_config = self.finger_configs[finger_name].copy() # 默认使用基础配置
+
+ if trigger_value < threshold:
+ weight_config = dynamic_config['low_weight_config']
+ # 创建临时配置
+
+ temp_config['joints'] = weight_config['joints']
+ temp_config['weights'] = weight_config['weights']
+ if 'reverse_motion' in weight_config:
+ temp_config['reverse_motion'] = weight_config['reverse_motion']
+ else:
+ temp_config['reverse_motion'] = base_config.get('reverse_motion', False)
+ else:
+ # 使用高权重配置
+ weight_config = dynamic_config.get('high_weight_config', {})
+ # 创建临时配置，合并基础配置和高权重配置
+ if weight_config: # 如果有高权重配置
+ temp_config['joints'] = weight_config.get('joints', temp_config['joints'])
+ temp_config['weights'] = weight_config.get('weights', temp_config['weights'])
+ # 优先使用高权重配置的reverse_motion
+ if 'reverse_motion' in weight_config:
+ temp_config['reverse_motion'] = weight_config['reverse_motion']
+
+ # 使用临时配置进行映射（支持扩展映射）
+ return self._map_finger_multi_state(glove_current, temp_config)
+
+ def _map_finger_multi_state(self, glove_current: np.ndarray,
+ config: dict) -> float:
+ """
+ 手指映射主方法
+ 支持扩展映射和原始多状态映射
+ """
+ # 查找手指名称
+ finger_name = None
+ for name, cfg in self.finger_configs.items():
+ if cfg['robot_idx'] == config['robot_idx']:
+ finger_name = name
+ break
+ # print(self.extended_mapping_enabled)
+ # 检查是否启用扩展映射
+ if (finger_name and finger_name in self.extended_mapping_enabled and
+ self.extended_mapping_enabled[finger_name]):
+ # print("触发线性映射")
+ return self._map_finger_extended(glove_current, config, finger_name)
+ else:
+ # 使用原始的多状态映射
+ return self._map_finger_original(glove_current, config, finger_name)
+
+ def _map_finger_original(self, glove_current: np.ndarray,
+ config: dict, finger_name: str = None) -> float:
+ """
+ 原始的多状态手指映射
+ """
+ joints = config['joints']
+ weights = self._normalize_weights(config['weights'])
+ robot_idx = config['robot_idx']
+
+ # 计算当前融合值
+ current_fused = self._calculate_fused_value(
+ glove_current, joints, weights
+ )
+
+ # 计算所有状态的融合值
+ state_fused_values = {}
+ for state_name in self.state_order:
+ fused = self._calculate_reference_fused(
+ joints, weights, self.glove_states[state_name]
+ )
+ state_fused_values[state_name] = fused
+
+ # 获取所有状态的角度
+ state_angles = {}
+ for state_name in self.state_order:
+ state_angles[state_name] = self.robot_states[state_name][robot_idx]
+
+ if self._should_debug(finger_name):
+ print(f"\n=== {finger_name} 调试信息 (original) ===")
+ print(f"启用状态: {self.state_order}")
+ print(f"权重: {config['weights']}")
+ joints = config['joints']
+ glove_joints_vals = {f"glove[{j}]": glove_current[j] for j in joints}
+ print(f"手套数据: {glove_joints_vals}")
+ print(f"融合值: {current_fused:.6f}")
+ print(f"状态融合值: {state_fused_values}")
+ print(f"状态角度: {state_angles}")
+
+ # 分段线性插值
+ result_angle = self._multi_state_interpolation(
+ current_fused, state_fused_values, state_angles
+ )
+
+ if self._should_debug(finger_name):
+ print(f"插值结果: {result_angle:.6f}")
+
+ # 处理反向运动
+ if config.get('reverse_motion', True):
+ # 找到最小和最大角度
+ min_angle = min(state_angles.values())
+ max_angle = max(state_angles.values())
+
+ result_angle = max_angle - (result_angle - min_angle)
+ if self._should_debug(finger_name):
+ print(f"reverse_motion=True, 反转后: {result_angle:.6f}")
+
+ return result_angle
+
+ def _map_finger_extended(self, glove_current: np.ndarray,
+ config: dict, finger_name: str) -> float:
+ """
+ 多段映射实现
+
+ 根据 state_order 决定映射段数：
+ - ['origin', 'opose', 'fist'] → 三段映射，截断到 fist
+ - ['origin', 'opose'] + extended_mapping.enabled=True → 两段映射，延伸到 fist 截断
+ - ['origin', 'opose'] + extended_mapping.enabled=False → 两段映射，截断到 opose
+ """
+ joints = config['joints']
+ weights = self._normalize_weights(config['weights'])
+ robot_idx = config['robot_idx']
+
+ # 获取启用的状态列表
+ states = self.state_order
+ num_states = len(states)
+
+ if num_states < 2:
+ print(f"警告：状态数量不足，回退到原始映射")
+ return self._map_finger_original(glove_current, config)
+
+ # 计算当前融合值
+ current_fused_raw = self._calculate_fused_value(glove_current, joints, weights)
+
+ # 计算第一个和最后一个状态的融合值
+ fused_first = self._calculate_reference_fused(joints, weights, self.glove_states[states[0]])
+ fused_last = self._calculate_reference_fused(joints, weights, self.glove_states[states[-1]])
+
+ # 应用缩放因子
+ current_fused = self._apply_scale_factor(current_fused_raw, fused_first, fused_last, finger_name)
+
+ # 获取第一个和最后一个状态的角度
+ angle_first = self.robot_states[states[0]][robot_idx]
+ angle_last = self.robot_states[states[-1]][robot_idx]
+
+ # 确保顺序正确
+ if angle_first > angle_last:
+ angle_first, angle_last = angle_last, angle_first
+
+ if self._should_debug(finger_name):
+ print(f"\n=== {finger_name} 调试信息 ===")
+ print(f"启用状态: {states}")
+ print(f"权重: {config['weights']}")
+ print(f"原始融合值: {current_fused_raw:.6f}")
+ print(f"缩放后融合值: {current_fused:.6f}")
+ print(f"融合值范围: [{fused_first:.6f}, {fused_last:.6f}]")
+ print(f"机械手角度范围: [{angle_first:.6f}, {angle_last:.6f}]")
+
+ # 归一化融合值
+ if abs(fused_last - fused_first) < 1e-6:
+ normalized_fused = 0.5
+ else:
+ normalized_fused = (current_fused - fused_first) / (fused_last - fused_first)
+
+ if self._should_debug(finger_name):
+ print(f"归一化融合值: {normalized_fused:.6f}")
+
+ # 判断是否需要延伸（只有 original + opose 两段模式才启用）
+ extrapolation_enabled = self.extended_mapping_enabled.get(finger_name, False)
+ use_extrapolation = extrapolation_enabled and states == ['original', 'opose']
+
+ if num_states >= 3:
+ result_angle = self._multi_state_map(joints, weights, robot_idx, current_fused_raw, finger_name)
+ elif use_extrapolation:
+ result_angle = self._extrapolate_to_fist(
+ current_fused, fused_first, fused_last,
+ angle_first, angle_last, robot_idx, finger_name, joints, weights
+ )
+ else:
+ result_angle = self._two_state_map(
+ current_fused, fused_first, fused_last,
+ angle_first, angle_last, finger_name
+ )
+
+ if config.get('reverse_motion', False):
+ min_angle = min(angle_first, angle_last)
+ max_angle = max(angle_first, angle_last)
+ clamped = np.clip(result_angle, min_angle, max_angle)
+ result_angle = max_angle - (clamped - min_angle)
+
+ return result_angle
+
+ def _multi_state_map(self, joints, weights, robot_idx, current_fused, finger_name):
+ """多段映射：使用所有启用的状态进行分段插值"""
+ state_fused_values = {}
+ state_angles = {}
+
+ for state_name in self.state_order:
+ fused = self._calculate_reference_fused(joints, weights, self.glove_states[state_name])
+ state_fused_values[state_name] = fused
+ state_angles[state_name] = self.robot_states[state_name][robot_idx]
+
+ result_angle = self._multi_state_interpolation(current_fused, state_fused_values, state_angles)
+
+ if self._should_debug(finger_name):
+ print(f"多段映射结果: {result_angle:.6f}")
+
+ return result_angle
+
+ def _extrapolate_to_fist(self, current_fused, fused_first, fused_last,
+ angle_first, angle_last, robot_idx, finger_name, joints, weights):
+ """两段映射 + 延伸映射，截断到 fist 角度"""
+ if abs(fused_last - fused_first) < 1e-6:
+ normalized = 0.5
+ else:
+ normalized = (current_fused - fused_first) / (fused_last - fused_first)
+
+ if self._should_debug(finger_name):
+ print(f"归一化融合值: {normalized:.6f}")
+
+ exp_factor = self.exp_factors.get(finger_name, 1.0)
+ slope = angle_last - angle_first
+
+ if normalized <= 0:
+ result_angle = angle_first
+ if self._should_debug(finger_name):
+ print(f"归一化值<=0: result_angle={result_angle:.6f}")
+ elif normalized <= 1:
+ result_angle = angle_first + normalized * slope
+ if self._should_debug(finger_name):
+ print(f"归一化值在[0,1]: result_angle={result_angle:.6f}")
+ else:
+ t = normalized - 1.0
+ extension = slope * t * (1.0 + (exp_factor - 1.0) * t)
+ result_angle = angle_last + extension
+
+ if self._should_debug(finger_name):
+ print(f"延伸: normalized={normalized:.6f}, t={t:.4f}, exp_factor={exp_factor:.2f}, result={result_angle:.6f}")
+
+ if 'fist' in self.robot_states:
+ angle_fist = self.robot_states['fist'][robot_idx]
+ if slope > 0:
+ result_angle = min(result_angle, angle_fist)
+ else:
+ result_angle = max(result_angle, angle_fist)
+ if self._should_debug(finger_name):
+ print(f"截断到fist: angle_fist={angle_fist:.6f}, result={result_angle:.6f}")
+
+ return result_angle
+
+ def _two_state_map(self, current_fused, fused_first, fused_last,
+ angle_first, angle_last, finger_name):
+ """两段映射：线性插值并截断到最后一个状态"""
+ if abs(fused_last - fused_first) < 1e-6:
+ normalized = 0.5
+ else:
+ normalized = (current_fused - fused_first) / (fused_last - fused_first)
+
+ # 截断到 [0, 1]
+ normalized = max(0.0, min(1.0, normalized))
+
+ result_angle = angle_first + normalized * (angle_last - angle_first)
+
+ if self._should_debug(finger_name):
+ print(f"两段映射截断: normalized={normalized:.6f}, result={result_angle:.6f}")
+
+ return result_angle
+
+ return result_angle
+
+ def get_mapping_info(self, finger_name: str = None) -> Dict:
+ """
+ 获取映射信息
+ """
+ if finger_name:
+ return self._get_finger_info(finger_name)
+ else:
+ return {name: self._get_finger_info(name) for name in self.finger_configs}
+
+ def _get_finger_info(self, finger_name: str) -> Dict:
+ """获取单个手指的信息"""
+ if finger_name not in self.finger_configs:
+ return {}
+
+ robot_idx = self.finger_configs[finger_name]['robot_idx']
+ max_angle = self._get_max_angle(robot_idx)
+
+ info = {
+ 'name': self.finger_configs[finger_name]['name'],
+ 'robot_idx': robot_idx,
+ 'has_dynamic_weight': finger_name in self.dynamic_weight_configs,
+ 'has_extended_mapping': self.extended_mapping_enabled.get(finger_name, False),
+ 'scale_factor': self.scale_factors.get(finger_name, 1.0),
+ 'max_angle': max_angle
+ }
+
+ # 如果有open和opose状态，显示相关信息
+ if 'open' in self.robot_states and 'opose' in self.robot_states:
+ open_angle = self.robot_states['open'][robot_idx]
+ opose_angle = self.robot_states['opose'][robot_idx]
+ info.update({
+ 'open_angle': open_angle,
+ 'opose_angle': opose_angle,
+ 'available_extension': max_angle - opose_angle
+ })
+
+ return info
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/linkerhand/linkerforce.py b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/linkerforce.py
new file mode 100644
index 0000000..c49a659
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/linkerforce.py
@@ -0,0 +1,738 @@
+import array
+import threading
+import numpy as np
+import time
+import re
+import struct
+import serial
+import serial.tools.list_ports
+from threading import Thread, Event
+from enum import Enum
+from typing import List, Dict, Optional, Callable, Any, Set
+from .constants import HandType
+
+
+# ============== 常量定义 ==============
+
+class CommandCode(Enum):
+ VERSION_QUERY = 0x01
+ SET_FLAG = 0x02
+ POSITION_QUERY = 0x03
+ FORCE_FEEDBACK = 0x04
+ A3_POSITION = 0xA3
+ A6_POSITION = 0xA6
+ A7_FORCE = 0xA7
+
+
+# 协议常量
+BUFFER_SIZE = 1024
+MAX_FRAME_DATA_SIZE = 255
+FRAME_HEADER = 0x5D
+
+# 时序常量
+WARMUP_DELAY = 0.15
+RESPONSE_WAIT = 0.3
+FINAL_WAIT = 1.0
+READ_INTERVAL = 0.003
+ERROR_DELAY = 0.01
+RETRY_DELAY = 0.5
+QUERY_INTERVAL = 10
+
+# 超时常量
+CONNECTION_TIMEOUT = 5.0
+CHECK_INTERVAL = 5.0
+
+# USB 设备匹配模式
+USB_PATTERNS = [
+ r'/dev/ttyUSB\d+',
+ r'/dev/ttyACM\d+',
+ r'/dev/ttyXRUSB\d+',
+ r'/dev/ttyOBC\d+',
+]
+
+
+# ============== 环形缓冲区 ==============
+
+class CircularBuffer:
+ def __init__(self):
+ self.data = array.array('B', [0] * BUFFER_SIZE)
+ self.read_pos = 0
+ self.write_pos = 0
+ self.data_len = 0
+
+ def write(self, data):
+ for byte in data:
+ self.data[self.write_pos] = byte
+ self.write_pos = (self.write_pos + 1) % BUFFER_SIZE
+ if self.data_len < BUFFER_SIZE:
+ self.data_len += 1
+ else:
+ self.read_pos = (self.read_pos + 1) % BUFFER_SIZE
+
+ def read_byte(self):
+ if self.data_len == 0:
+ return None
+ byte = self.data[self.read_pos]
+ self.read_pos = (self.read_pos + 1) % BUFFER_SIZE
+ self.data_len -= 1
+ return byte
+
+
+# ============== 帧解析器 ==============
+
+class FrameParseState(Enum):
+ HEADER = 0
+ CMD = 1
+ LENGTH = 2
+ DATA = 3
+ CHECKSUM = 4
+
+
+class FrameParser:
+ def __init__(self):
+ self.state = FrameParseState.HEADER
+ self.frame_buf = array.array('B', [0] * (3 + MAX_FRAME_DATA_SIZE + 1))
+ self.expected_len = 0
+ self.current_pos = 0
+ self.checksum = 0
+
+ def reset(self):
+ self.state = FrameParseState.HEADER
+ self.current_pos = 0
+ self.checksum = 0
+ for i in range(len(self.frame_buf)):
+ self.frame_buf[i] = 0
+
+ def process_byte(self, byte):
+ byte = byte & 0xFF
+ if self.state == FrameParseState.HEADER:
+ if byte == FRAME_HEADER:
+ self.frame_buf[0] = byte
+ self.current_pos = 1
+ self.checksum = 0
+ self.state = FrameParseState.CMD
+ elif self.state == FrameParseState.CMD:
+ self.frame_buf[1] = byte
+ self.current_pos = 2
+ self.state = FrameParseState.LENGTH
+ elif self.state == FrameParseState.LENGTH:
+ self.frame_buf[2] = byte
+ self.expected_len = 3 + byte + 1
+ self.current_pos = 3
+ if 0 < byte <= MAX_FRAME_DATA_SIZE:
+ self.state = FrameParseState.DATA
+ else:
+ self.state = FrameParseState.CHECKSUM
+ elif self.state == FrameParseState.DATA:
+ self.frame_buf[self.current_pos] = byte
+ self.current_pos += 1
+ if self.current_pos >= self.expected_len - 1:
+ self.state = FrameParseState.CHECKSUM
+ elif self.state == FrameParseState.CHECKSUM:
+ if self.checksum == byte:
+ self.frame_buf[self.current_pos] = byte
+ return True
+ else:
+ self.reset()
+
+ if self.state != FrameParseState.HEADER:
+ self.checksum = (self.checksum + byte) & 0xFF
+
+ return False
+
+
+# ============== 日志工具 ==============
+
+class Logger:
+ def __init__(self, logger_func: Optional[Callable[[str, str], None]] = None, isdebug: bool = False):
+ self.logger = logger_func
+ self.isdebug = isdebug
+
+ def log(self, level: str, msg: str) -> None:
+ if self.logger:
+ if self.isdebug and level == 'debug':
+ self.logger('info', msg)
+ else:
+ self.logger(level, msg)
+ else:
+ print(msg)
+
+
+# ============== 串口扫描器 ==============
+
+class SerialScanner:
+ def __init__(self, baudrates: Optional[List[int]] = None,
+ exclude_ports: Optional[List[str]] = None,
+ logger: Optional[Logger] = None):
+ self.baudrates = baudrates or [2000000, 1000000, 921600, 460800]
+ self.exclude_ports = set(exclude_ports) if exclude_ports else set()
+ self.checked_ports: Set[str] = set()
+ self.logger = logger
+
+ def is_usb_device(self, port_name):
+ for pattern in USB_PATTERNS:
+ if re.match(pattern, port_name):
+ return True
+ try:
+ ports = serial.tools.list_ports.comports()
+ for port_info in ports:
+ if port_info.device == port_name:
+ description = (port_info.description or "").lower()
+ if any(kw in description for kw in ['usb', 'serial', 'com']):
+ return True
+ if port_info.hwid and 'USB' in port_info.hwid.upper():
+ return True
+ except:
+ pass
+ return False
+
+ def scan_available_ports(self):
+ ports = serial.tools.list_ports.comports()
+ available = []
+ for port in ports:
+ device = port.device
+ if not self.is_usb_device(device):
+ continue
+ if device in self.exclude_ports:
+ if self.logger:
+ self.logger.log('debug', f"跳过排除的串口: {device}")
+ continue
+ if device not in self.checked_ports:
+ available.append(device)
+ return available
+
+
+# ============== 帧处理器 ==============
+
+class FrameHandler:
+ def __init__(self, handtype: HandType, logger: Optional['Logger'] = None):
+ self._handtype = handtype # 期望的手类型
+ self.logger = logger
+ self._data_lock = threading.Lock()
+ self._poslist: List[float] = [0.0] * 21
+ self._forcelist: List[float] = [0.0] * 5
+ self._realforcelist: List[int] = [0] * 5
+ self.last_update_time: float = 0.0 # poslist 最后更新时间 (perf_counter)
+
+ @property
+ def poslist(self) -> List[float]:
+ with self._data_lock:
+ return self._poslist.copy()
+
+ @poslist.setter
+ def poslist(self, value: List[float]):
+ with self._data_lock:
+ self._poslist = value
+ self.last_update_time = time.perf_counter()
+
+ @property
+ def forcelist(self) -> List[float]:
+ with self._data_lock:
+ return self._forcelist.copy()
+
+ @forcelist.setter
+ def forcelist(self, value: List[float]):
+ with self._data_lock:
+ self._forcelist = value
+
+ @property
+ def realforcelist(self) -> List[int]:
+ with self._data_lock:
+ return self._realforcelist.copy()
+
+ @realforcelist.setter
+ def realforcelist(self, value: List[int]):
+ with self._data_lock:
+ self._realforcelist = value
+
+ def handle_frame(self, frame: array.array) -> Optional[Dict[str, Any]]:
+ cmd = frame[1]
+ data_len = frame[2]
+ frame_data = frame[3:3 + data_len]
+
+ if cmd == CommandCode.VERSION_QUERY.value:
+ return self._handle_version(frame_data)
+ elif cmd == CommandCode.POSITION_QUERY.value:
+ return self._handle_position(frame_data, is_a3=False)
+ elif cmd == CommandCode.FORCE_FEEDBACK.value:
+ return self._handle_force(frame_data)
+ elif cmd == CommandCode.A3_POSITION.value:
+ return self._handle_position(frame_data, is_a3=True)
+ elif cmd == CommandCode.A6_POSITION.value:
+ return self._handle_a6_position(frame_data)
+ else:
+ if self.logger:
+ self.logger.log('warn', f"Unknown command: 0x{cmd:02X}")
+ return None
+
+ def _handle_version(self, frame_data: array.array) -> Dict[str, Any]:
+ value = struct.unpack(' Optional[Dict[str, Any]]:
+ if len(frame_data) % 4 != 0:
+ if self.logger:
+ self.logger.log('warn', f"Invalid position data length: {len(frame_data)}")
+ return None
+ floats: List[float] = []
+ for i in range(len(frame_data) // 4):
+ try:
+ val = struct.unpack(' Optional[Dict[str, Any]]:
+ if len(frame_data) % 2 != 0:
+ if self.logger:
+ self.logger.log('warn', f"Invalid force data length: {len(frame_data)}")
+ return None
+ values: List[int] = []
+ for i in range(len(frame_data) // 2):
+ try:
+ val = struct.unpack('>h', frame_data[i*2:(i+1)*2])[0]
+ values.append(val)
+ except struct.error as e:
+ if self.logger:
+ self.logger.log('warn', f"Unpack error: {e}")
+ self.realforcelist = values
+ return {'realforcelist': values}
+
+ def _handle_a6_position(self, frame_data: array.array) -> Optional[Dict[str, Any]]:
+ if len(frame_data) % 2 != 0:
+ if self.logger:
+ self.logger.log('warn', f"Invalid a6 data length: {len(frame_data)}")
+ return None
+ floats: List[float] = []
+ for i in range(len(frame_data) // 2):
+ try:
+ val = struct.unpack(' Optional[str]:
+ """返回手类型字符串，保持向后兼容"""
+ if status_code == 0 and self._handtype == HandType.left:
+ return "Left"
+ elif status_code == 1 and self._handtype == HandType.right:
+ return "Right"
+ return None
+
+ def detect_hand_type(self, status_code: int) -> Optional[str]:
+ """仅根据 status_code 检测手类型（不验证匹配）"""
+ if status_code == 0:
+ return "Left"
+ elif status_code == 1:
+ return "Right"
+ return None
+
+ @staticmethod
+ def calculate_checksum(data: bytes) -> int:
+ return sum(data) & 0xFF
+
+ @staticmethod
+ def pack_data(cmd: int, payload: bytes = b'') -> bytes:
+ header = struct.pack('BBB', FRAME_HEADER, cmd, len(payload))
+ checksum = FrameHandler.calculate_checksum(header + payload)
+ return header + payload + struct.pack('B', checksum)
+
+ def pack_version_query(self) -> bytes:
+ return self.pack_data(CommandCode.VERSION_QUERY.value)
+
+ def pack_position_query(self) -> bytes:
+ return self.pack_data(CommandCode.POSITION_QUERY.value)
+
+ def pack_force_feedback(self) -> bytes:
+ payload = struct.pack(f'{len(self._forcelist)}f', *self._forcelist)
+ return self.pack_data(CommandCode.FORCE_FEEDBACK.value, payload)
+
+
+# ============== 串口连接管理器 ==============
+
+class SerialConnection:
+ def __init__(self, logger: Optional['Logger'] = None, isdebug: bool = False):
+ self.serial_port: Optional[serial.Serial] = None
+ self.running = Event()
+ self.thread: Optional[Thread] = None
+ self.logger = logger
+ self.isdebug = isdebug
+ self._last_receive_time = time.time()
+ self._last_check_time = time.time()
+ self._disconnect_warned = False
+ self._on_disconnect: Optional[Callable[[], None]] = None
+ self._on_reconnect: Optional[Callable[[], None]] = None
+
+ def open(self, port: str, baudrate: int) -> bool:
+ try:
+ self.serial_port = serial.Serial(
+ port=port,
+ baudrate=baudrate,
+ timeout=0.001,
+ parity=serial.PARITY_NONE,
+ stopbits=serial.STOPBITS_ONE,
+ bytesize=serial.EIGHTBITS
+ )
+ self.running = Event()
+ return True
+ except serial.SerialException as e:
+ if self.logger:
+ self.logger.log('error', f"串口打开失败: {e}")
+ return False
+
+ def close(self) -> None:
+ # 先关闭串口，解除阻塞，让线程快速退出
+ if self.serial_port and self.serial_port.is_open:
+ self.serial_port.close()
+ self.serial_port = None
+
+ self.running.clear()
+ if self.thread and self.thread.is_alive():
+ self.thread.join(timeout=1.0)
+
+ def start(self, data_callback: Callable[[array.array], None],
+ query_callback: Callable[[], Optional[bytes]]) -> None:
+ if self.thread and self.thread.is_alive():
+ return
+ self.running.set()
+ self.thread = Thread(target=self._run, args=(data_callback, query_callback), daemon=True)
+ self.thread.start()
+
+ def stop(self) -> None:
+ self.close()
+
+ def _run(self, data_callback: Callable[[array.array], None],
+ query_callback: Callable[[], Optional[bytes]]) -> None:
+ parser = FrameParser()
+ sendcount = 0
+
+ while self.running.is_set():
+ try:
+ current_time = time.time()
+
+ # 断联检测
+ if current_time - self._last_check_time >= CHECK_INTERVAL:
+ self._last_check_time = current_time
+ elapsed = current_time - self._last_receive_time
+
+ if elapsed > CONNECTION_TIMEOUT:
+ if not self._disconnect_warned:
+ port_name = self.serial_port.port if self.serial_port else 'unknown'
+ if self.logger:
+ self.logger.log('error', f"串口 {port_name} 超过 {CONNECTION_TIMEOUT}秒 无响应，可能已断联")
+ self._disconnect_warned = True
+ if self._on_disconnect:
+ self._on_disconnect()
+
+ # 读取数据
+ if self.serial_port and self.serial_port.in_waiting > 0:
+ data = self.serial_port.read(self.serial_port.in_waiting)
+ if data:
+ if self._disconnect_warned:
+ port_name = self.serial_port.port if self.serial_port else 'unknown'
+ if self.logger:
+ self.logger.log('info', f"串口 {port_name} 已恢复连接")
+ self._disconnect_warned = False
+ if self._on_reconnect:
+ self._on_reconnect()
+
+ self._last_receive_time = current_time
+ for byte in data:
+ if parser.process_byte(byte):
+ if data_callback:
+ data_callback(parser.frame_buf)
+ parser.reset()
+
+ # 发送查询
+ if query_callback:
+ sendcount += 1
+ if sendcount > QUERY_INTERVAL:
+ query_data = query_callback()
+ if query_data and self.serial_port:
+ self.serial_port.write(query_data)
+ sendcount = 0
+
+ time.sleep(READ_INTERVAL)
+
+ except Exception as e:
+ if self.logger and self.isdebug:
+ self.logger.log('error', f"串口读取错误: {e}")
+ time.sleep(ERROR_DELAY)
+
+ def set_disconnect_callback(self, callback: Callable[[], None]) -> None:
+ self._on_disconnect = callback
+
+ def set_reconnect_callback(self, callback: Callable[[], None]) -> None:
+ self._on_reconnect = callback
+
+
+# ============== 主类：整合以上模块 ==============
+
+class ForceSerialReader:
+ def __init__(self, gettype: HandType, excludelist: Optional[List[str]] = None,
+ baudrates: Optional[List[int]] = None, isdebug: bool = False,
+ logger: Optional[Callable[[str, str], None]] = None):
+ self.gettype = gettype
+ self.isdebug = isdebug
+ self.connflag = False
+ self.version: Optional[str] = None
+ self.handtype: Optional[HandType] = None
+
+ # 初始化模块
+ self._logger = Logger(logger, isdebug)
+ self._scanner = SerialScanner(baudrates, excludelist, self._logger)
+ self._handler = FrameHandler(gettype, self._logger)
+ self._connection = SerialConnection(self._logger, isdebug)
+
+ # 串口参数代理
+ self.serial_port: Optional[serial.Serial] = None
+ self.baudrates = self._scanner.baudrates
+ self.checked_ports = self._scanner.checked_ports
+ self.exclude_ports = self._scanner.exclude_ports
+
+ # 数据属性代理
+ @property
+ def poslist(self) -> List[float]:
+ return self._handler.poslist
+
+ @poslist.setter
+ def poslist(self, value: List[float]):
+ self._handler.poslist = value
+
+ @property
+ def forcelist(self) -> List[float]:
+ return self._handler.forcelist
+
+ @forcelist.setter
+ def forcelist(self, value: List[float]):
+ self._handler.forcelist = value
+
+ @property
+ def realforcelist(self) -> List[int]:
+ return self._handler.realforcelist
+
+ @realforcelist.setter
+ def realforcelist(self, value: List[int]):
+ self._handler.realforcelist = value
+
+ @property
+ def last_update_time(self) -> float:
+ return self._handler.last_update_time
+
+ def _log(self, level: str, msg: str) -> None:
+ self._logger.log(level, msg)
+
+ # 扫描方法
+ def is_usb_device(self, port_name: str) -> bool:
+ return self._scanner.is_usb_device(port_name)
+
+ def scan_serial_ports(self) -> List[str]:
+ return self._scanner.scan_available_ports()
+
+ def find_valid_ports(self, timeout: float = 2, scan_interval: float = 2) -> tuple:
+ if self.isdebug:
+ self._log('debug', "开始扫描串口...")
+ self._log('debug', f"排除列表: {list(self.exclude_ports)}")
+ self._log('debug', f"波特率组合: {self.baudrates}")
+
+ available_ports = self.scan_serial_ports()
+ if self.isdebug:
+ self._log('debug', f"发现 {len(available_ports)} 个未检查的串口: {available_ports}")
+
+ for port in available_ports:
+ success, baudrate, errorcode = self.query_serial_port(port, timeout)
+
+ if not success and errorcode != -2:
+ if self.isdebug:
+ self._log('debug', "首次连接失败，尝试重试...")
+ time.sleep(RETRY_DELAY)
+ success, baudrate, errorcode = self.query_serial_port(port, timeout)
+
+ if errorcode == -2:
+ self._log('warn', f"警告: 串口 {port} 权限不足，请手动执行: sudo chmod 666 {port}")
+
+ self.checked_ports.add(port)
+
+ if success:
+ if self.isdebug:
+ self._log('info', f"找到有效串口: {port} (波特率: {baudrate})")
+ return port, baudrate, errorcode
+
+ return None, None, None
+
+ def query_serial_port(self, port_name: str, timeout: float = 1) -> tuple:
+ best_baudrate: Optional[int] = None
+ errorcode: Optional[int] = None
+
+ for baudrate in self.baudrates:
+ ser: Optional[serial.Serial] = None
+ try:
+ ser = serial.Serial(port_name, baudrate, timeout=timeout,
+ bytesize=serial.EIGHTBITS,
+ parity=serial.PARITY_NONE,
+ stopbits=serial.STOPBITS_ONE)
+ self.serial_port = ser
+
+ if self.isdebug:
+ self._log('debug', f"串口 {port_name} 波特率 {baudrate} 预热中...")
+
+ self.handtype = None
+ self.connflag = False
+
+ ser.reset_input_buffer()
+ ser.reset_output_buffer()
+
+ # 预热发送
+ for _ in range(3):
+ ser.write(self.pack_01_data())
+ time.sleep(WARMUP_DELAY)
+
+ # 启动临时读取线程
+ self._connection.running = Event() # 重置 Event
+ self._connection.serial_port = ser
+ self._connection.running.set()
+ self._connection.thread = Thread(target=self._connection._run,
+ args=(self._on_data_received, self._get_query_data),
+ daemon=True)
+ self._connection.thread.start()
+ time.sleep(RESPONSE_WAIT)
+
+ if self.isdebug:
+ self._log('debug', f"侦测串口 {port_name} 波特率 {baudrate} 是否联通...")
+
+ ser.write(self.pack_01_data())
+ time.sleep(FINAL_WAIT)
+
+ # 停止临时线程
+ self._connection.stop()
+ self.serial_port = None
+
+ if self.connflag and self.handtype is not None:
+ best_baudrate = baudrate
+ if self.isdebug:
+ self._log('info', f"串口 {port_name} 在 {baudrate} 波特率下有响应")
+ return True, best_baudrate, errorcode
+
+ except serial.SerialException as e:
+ # 确保清理
+ if self._connection.thread:
+ self._connection.stop()
+ if ser and ser.is_open:
+ ser.close()
+ self.serial_port = None
+
+ error_msg = str(e)
+ if "No such file" in error_msg or "[Errno 2]" in error_msg:
+ errorcode = -1
+ if self.isdebug:
+ self._log('debug', f"串口设备不存在: {port_name}")
+ break
+ elif "Permission denied" in error_msg or "[Errno 13]" in error_msg:
+ errorcode = -2
+ self._log('warn', f"权限被拒绝: {port_name}")
+ break
+ elif "Device or resource busy" in error_msg:
+ errorcode = -3
+ if self.isdebug:
+ self._log('debug', f"设备忙: {port_name}")
+ break
+ else:
+ errorcode = -99
+ if self.isdebug:
+ self._log('debug', f"串口打开失败: {e}")
+ continue
+
+ return False, None, errorcode
+
+ # 连接方法
+ def openserial(self, port: str, baudrate: int = 2000000) -> bool:
+ result = self._connection.open(port, baudrate)
+ if result:
+ self.serial_port = self._connection.serial_port
+ return result
+
+ def start(self) -> None:
+ self._connection.start(self._on_data_received, self._get_query_data)
+
+ def stop(self) -> None:
+ self._connection.stop()
+
+ def _on_data_received(self, frame: array.array) -> None:
+ self.connflag = True
+ result = self._handler.handle_frame(frame)
+
+ if result:
+ if 'version' in result:
+ self.version = result['version']
+ if 'raw_handtype' in result:
+ self.handtype = result['raw_handtype']
+ elif 'handtype' in result:
+ self.handtype = result['handtype']
+ if 'force_response' in result and self.serial_port:
+ self.serial_port.write(self.pack_A7_data(self.forcelist))
+
+ def _get_query_data(self) -> Optional[bytes]:
+ if self.handtype is not None:
+ return self.pack_03_data()
+ return None
+
+ def set_reconnect_callback(self, callback: Callable[[], None]) -> None:
+ self._connection.set_reconnect_callback(callback)
+
+ # 数据打包方法
+ @staticmethod
+ def calculate_checksum(data: bytes) -> int:
+ return FrameHandler.calculate_checksum(data)
+
+ def pack_01_data(self) -> bytes:
+ return self._handler.pack_version_query()
+
+ def pack_02_data(self, mastersendflag: int) -> bytes:
+ payload = struct.pack('BBBBB', mastersendflag, 0, 0, 0, 0)
+ return FrameHandler.pack_data(CommandCode.SET_FLAG.value, payload)
+
+ def pack_03_data(self) -> bytes:
+ return self._handler.pack_position_query()
+
+ def pack_A3_data(self) -> bytes:
+ return FrameHandler.pack_data(CommandCode.A3_POSITION.value)
+
+ def pack_04_data(self):
+ return self._handler.pack_force_feedback()
+
+ def pack_A4_data(self, float_data):
+ payload = struct.pack(f'{len(float_data)}f', *float_data)
+ return FrameHandler.pack_data(CommandCode.A6_POSITION.value, payload)
+
+ def pack_A7_data(self, float_data):
+ payload = struct.pack(f'{len(float_data)}f', *float_data)
+ return FrameHandler.pack_data(CommandCode.A7_FORCE.value, payload)
+
+ # 兼容性方法
+ def hex_dump(self, data):
+ return ' '.join(f'{b:02X}' for b in data)
+
+ def get_current_status(self):
+ return {
+ 'valid_ports': [],
+ 'checked_ports': list(self.checked_ports),
+ 'exclude_ports': list(self.exclude_ports),
+ 'baudrates': self.baudrates
+ }
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/linkerhand/linkermcgcore.py b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/linkermcgcore.py
new file mode 100644
index 0000000..dbff493
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/linkermcgcore.py
@@ -0,0 +1,264 @@
+from datetime import datetime
+import socket
+import time
+from threading import Thread
+import json
+from dataclasses import dataclass
+from typing import List, Dict, Union, Any
+import threading
+import numpy as np
+
+NODES_HAND = 25
+
+LOG_FILE_PATH = "/tmp/a.log"
+
+@dataclass
+class HandData:
+ pitch: List[int] # 5个手指的pitch值 [0-255]
+ side: List[int] # 5个手指的side值 [0-255]
+ roll: List[int] # 5个手指的roll值 [0-255]
+ two_pitch: List[int] # 5个手指的two_pitch值 [0-255]
+ end_pitch: List[int] # 5个手指的end_pitch值 [0-255]
+
+
+class HaoCunData:
+ def __init__(self):
+ self.is_update = False
+ self.frame_index = 0
+ self.frequency = 0
+ self.ns_result = 0
+ # 直接存储25个字节值，不需要转换
+ self.jointangle_rHand = [0.0] * NODES_HAND
+ self.jointangle_lHand = [0.0] * NODES_HAND
+
+
+class HaoCunScoketUdp:
+ def __init__(self, host='127.0.0.1', port=7000, buffer_size=2048):
+ """
+ 初始化UDP客户端
+
+ Args:
+ target_host: 目标服务器地址
+ target_port: 目标服务器端口
+ buffer_size: 缓冲区大小
+ device_id: 设备ID
+ """
+ self.socket_udp = None
+ self.is_use_face_blend_shapes_arkit = False
+ self.udp_thread = None
+ self.udp_running = False
+ self.isconnect = False
+ self.target_host = host
+ self.target_port = port
+ self.target_address = (host, port)
+ self.buffer_size = buffer_size
+ self.realmocapdata = HaoCunData()
+ self.data_lock = threading.Lock()
+ self.frame_counter = 0
+
+ def udp_initial(self) -> bool:
+ """初始化UDP客户端并连接到目标服务器"""
+ try:
+ # 创建UDP socket
+ self.socket_udp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+
+ # 设置超时时间
+ self.socket_udp.settimeout(10)
+
+ # UDP是面向无连接的，这里只是保存目标地址，不会真正建立连接
+ # 但我们可以发送一个测试包来验证连通性
+ try:
+ test_packet = b"CONNECT"
+ self.socket_udp.sendto(test_packet, self.target_address)
+ self.socket_udp.settimeout(2) # 设置较短的超时用于连接测试
+ # 尝试接收响应（如果服务器会响应的话）
+ # 注意：某些UDP服务可能不会响应，这并不代表连接失败
+ try:
+ data, addr = self.socket_udp.recvfrom(self.buffer_size)
+ print(f"成功连接到服务器 {addr}")
+ except socket.timeout:
+ print(f"已发送连接请求到 {self.target_address} (UDP协议，无连接确认)")
+ except Exception as e:
+ print(f"连接测试时出错: {e}")
+
+ # 恢复超时设置
+ self.socket_udp.settimeout(10)
+
+ self.isconnect = True
+ self.udp_running = True
+ self.udp_thread = Thread(target=self.__udp_process)
+ self.udp_thread.start()
+ return True
+ except socket.error as e:
+ self.isconnect = False
+ print(f"UDP客户端初始化错误: {e}")
+ if self.socket_udp:
+ self.socket_udp.close()
+ return False
+
+ def __recv(self) -> tuple:
+ """接收UDP数据"""
+ try:
+ data, addr = self.socket_udp.recvfrom(self.buffer_size)
+ return data, addr
+ except socket.timeout:
+ return None, None
+ except socket.error as e:
+ print(f"接收数据时出错: {e}")
+ return None, None
+
+ def udp_close(self) -> bool:
+ """关闭UDP连接"""
+ self.udp_running = False
+ if self.udp_thread and self.udp_thread.is_alive():
+ self.udp_thread.join(timeout=2)
+ time.sleep(0.1)
+ if self.socket_udp:
+ self.socket_udp.close()
+ self.isconnect = False
+ return True
+
+ def udp_is_connect(self) -> bool:
+ """检查连接状态"""
+ return self.isconnect
+
+ def __udp_process(self):
+ """UDP数据处理线程"""
+ errorprintcount = 0
+ while self.udp_running:
+ try:
+ bytes_data, addr = self.__recv()
+ if bytes_data is not None:
+ try:
+ # 将接收到的数据传递给处理函数
+ self.__process_received_data(bytes_data)
+ except Exception as e:
+ if errorprintcount > 100:
+ print(f"数据处理错误: {e}")
+ errorprintcount = 0
+ # 添加短暂休眠避免CPU占用过高
+ time.sleep(0.001)
+ except Exception as e:
+ if errorprintcount > 100:
+ print(f"UDP处理线程错误: {e}")
+ errorprintcount = 0
+ finally:
+ errorprintcount += 1
+
+ def __process_received_data(self, bytes_data: bytes):
+ """处理接收到的数据"""
+ try:
+ # 解码JSON数据
+ json_str = bytes_data.decode('utf-8', errors='replace')
+ data = json.loads(json_str)
+
+ # 更新帧计数器
+ self.frame_counter += 1
+
+ # 处理数据
+ with self.data_lock:
+ # 处理左手数据
+ if 'leftHand' in data:
+ left_hand = data['leftHand']
+ self.realmocapdata.jointangle_lHand = self.extract_25_bytes(left_hand)
+
+ # 处理右手数据
+ if 'rightHand' in data:
+ right_hand = data['rightHand']
+ self.realmocapdata.jointangle_rHand = self.extract_25_bytes(right_hand)
+
+ # 更新其他状态
+ self.realmocapdata.is_update = True
+ self.realmocapdata.frame_index = self.frame_counter
+
+ except json.JSONDecodeError as e:
+ print(f"JSON解析错误: {e}")
+ print(f"原始数据: {bytes_data.decode('utf-8', errors='replace')}")
+ except Exception as e:
+ print(f"处理数据时出错: {e}")
+
+ def extract_25_bytes(self, hand_data: Dict) -> List[float]:
+ """
+ 从手部数据字典中提取25个字节值，按以下顺序排列：
+ 1. pitch (5个值)
+ 2. side (5个值)
+ 3. roll (5个值)
+ 4. two_pitch (5个值)
+ 5. end_pitch (5个值)
+
+ 总计25个值
+
+ Args:
+ hand_data: 包含pitch, side, roll, two_pitch, end_pitch的字典
+
+ Returns:
+ 长度为25的字节值列表
+ """
+ byte_values = [0.0] * NODES_HAND
+ idx = 0
+
+ try:
+ # 按顺序提取5个数组，每个5个值，共25个值
+ arrays_to_extract = ['pitch', 'side', 'roll', 'two_pitch', 'end_pitch']
+
+ for array_name in arrays_to_extract:
+ if array_name in hand_data:
+ values = hand_data[array_name]
+ # 确保有5个值
+ if len(values) >= 5:
+ for i in range(5):
+ if idx < NODES_HAND:
+ byte_values[idx] = float(values[i])
+ idx += 1
+ else:
+ # 如果数据不足5个，填充0
+ for i in range(5):
+ if idx < NODES_HAND:
+ byte_values[idx] = 0.0
+ idx += 1
+ else:
+ # 如果缺少某个数组，填充5个0
+ for i in range(5):
+ if idx < NODES_HAND:
+ byte_values[idx] = 0.0
+ idx += 1
+
+ except Exception as e:
+ print(f"提取字节值时出错: {e}")
+
+ return byte_values
+
+ def udp_recv_mocap_data(self, mocap_data: HaoCunData) -> bool:
+ """获取最新的动捕数据"""
+ with self.data_lock:
+ mocap_data.frame_index = self.realmocapdata.frame_index
+ mocap_data.is_update = self.realmocapdata.is_update
+ mocap_data.frequency = self.realmocapdata.frequency
+ mocap_data.jointangle_rHand = self.realmocapdata.jointangle_rHand.copy()
+ mocap_data.jointangle_lHand = self.realmocapdata.jointangle_lHand.copy()
+ return True
+
+ def send_data(self, data: bytes) -> bool:
+ """发送数据到目标服务器"""
+ try:
+ if not self.isconnect or not self.socket_udp:
+ print("UDP客户端未连接")
+ return False
+
+ self.socket_udp.sendto(data, self.target_address)
+ return True
+ except Exception as e:
+ print(f"发送数据时出错: {e}")
+ return False
+
+ def get_hand_data_summary(self) -> Dict:
+ """获取手部数据摘要"""
+ with self.data_lock:
+ return {
+ 'frame_index': self.realmocapdata.frame_index,
+ 'is_update': self.realmocapdata.is_update,
+ 'left_hand_first_5': self.realmocapdata.jointangle_lHand[:5],
+ 'right_hand_first_5': self.realmocapdata.jointangle_rHand[:5],
+ 'left_hand_total': len(self.realmocapdata.jointangle_lHand),
+ 'right_hand_total': len(self.realmocapdata.jointangle_rHand)
+ }
diff --git a/src/linkerhand_retarget/linkerhand_retarget/linkerhand/sensenovacore.py b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/sensenovacore.py
new file mode 100644
index 0000000..02a87cc
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/sensenovacore.py
@@ -0,0 +1,270 @@
+from datetime import datetime
+import socket
+import numpy as np
+import time
+from threading import Thread
+import json
+from dataclasses import dataclass
+from typing import List, Dict, Union, Any
+import threading
+
+NODES_HAND = 30
+
+json_send_basic = {
+ "timsstamp": "2025-4-30 22:47:90.123",
+ "datatype": "datarecv",
+ "right": {
+ "thumb": {
+ "normalforce": 0.0,
+ "approachforce": 0.0,
+ "tangentialforce": 0.0
+ },
+ "index": {
+ "normalforce": 0.0,
+ "approachforce": 0.0,
+ "tangentialforce": 0.0
+ },
+ "middle": {
+ "normalforce": 0.0,
+ "approachforce": 0.0,
+ "tangentialforce": 0.0
+ },
+ "ring": {
+ "normalforce": 31.0,
+ "approachforce": 32.0,
+ "tangentialforce": 0.0
+ },
+ "pinky": {
+ "normalforce": 0.0,
+ "approachforce": 0.0,
+ "tangentialforce": 0.0
+ }
+ },
+ "left": {
+ "thumb": {
+ "normalforce": 0.0,
+ "approachforce": 0.0,
+ "tangentialforce": 0.0
+ },
+ "index": {
+ "normalforce": 0.0,
+ "approachforce": 0.0,
+ "tangentialforce": 0.0
+ },
+ "middle": {
+ "normalforce": 0.0,
+ "approachforce": 0.0,
+ "tangentialforce": 0.0
+ },
+ "ring": {
+ "normalforce": 0.0,
+ "approachforce": 0.0,
+ "tangentialforce": 0.0
+ },
+ "pinky": {
+ "normalforce": 0.0,
+ "approachforce": 0.0,
+ "tangentialforce": 0.0
+ }
+ }
+}
+
+
+class SenseNovaData:
+ def __init__(self):
+ self.is_update = False
+ self.frame_index = 0
+ self.frequency = 0
+ self.ns_result = 0
+ self.jointangle_rHand = [0.0] * NODES_HAND
+ self.jointangle_lHand = [0.0] * NODES_HAND
+ self.normalforce_rHand = [0.0] * 5
+ self.normalforce_lHand = [0.0] * 5
+ self.approachforce_rHand = [0.0] * 5
+ self.approachforce_lHand = [0.0] * 5
+
+
+class SenseNovaScoketUdp:
+ def __init__(self, host='0.0.0.0', port=7000, buffer_size=4098):
+ self.socket_udp = None
+ self.is_use_face_blend_shapes_arkit = False
+ self.udp_thread = None
+ self.udp_running = False
+ self.isconnect = False
+ self.host = host
+ self.port = port
+ self.buffer_size = buffer_size
+ self.udp_addr = self.udp_getsockaddr(host, port)
+ self.realmocapdata = SenseNovaData()
+ self.data_lock = threading.Lock()
+
+ def udp_initial(self) -> bool:
+ """初始化 UDP socket"""
+ try:
+ self.socket_udp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ self.socket_udp.bind(('', 8888))
+ self.socket_udp.settimeout(10)
+ self.isconnect = True
+ self.udp_running = True
+ self.udp_thread = Thread(target=self.__udp_process)
+ self.udp_thread.start()
+
+ return True
+ except socket.error as e:
+ self.isconnect = False
+ print(f"发生错误: {e},UDP套接字已关闭!")
+ if self.socket_udp:
+ self.socket_udp.close()
+ return False
+
+ @staticmethod
+ def udp_getsockaddr(ip: str, port: int) -> tuple:
+ """将 IP 地址及端口号转化为能识别的地址格式"""
+ return (ip, port)
+
+ def __send(self):
+ try:
+ json_send_basic["right"]["thumb"]["normalforce"] = self.realmocapdata.normalforce_rHand[0]
+ json_send_basic["right"]["index"]["normalforce"] = self.realmocapdata.normalforce_rHand[1]
+ json_send_basic["right"]["middle"]["normalforce"] = self.realmocapdata.normalforce_rHand[2]
+ json_send_basic["right"]["ring"]["normalforce"] = self.realmocapdata.normalforce_rHand[3]
+ json_send_basic["right"]["pinky"]["normalforce"] = self.realmocapdata.normalforce_rHand[4]
+ json_send_basic["left"]["thumb"]["normalforce"] = self.realmocapdata.normalforce_lHand[0]
+ json_send_basic["left"]["index"]["normalforce"] = self.realmocapdata.normalforce_lHand[1]
+ json_send_basic["left"]["middle"]["normalforce"] = self.realmocapdata.normalforce_lHand[2]
+ json_send_basic["left"]["ring"]["normalforce"] = self.realmocapdata.normalforce_lHand[3]
+ json_send_basic["left"]["pinky"]["normalforce"] = self.realmocapdata.normalforce_lHand[4]
+ json_send_basic["right"]["thumb"]["approachforce"] = self.realmocapdata.approachforce_rHand[0]
+ json_send_basic["right"]["index"]["approachforce"] = self.realmocapdata.approachforce_rHand[1]
+ json_send_basic["right"]["middle"]["approachforce"] = self.realmocapdata.approachforce_rHand[2]
+ json_send_basic["right"]["ring"]["approachforce"] = self.realmocapdata.approachforce_rHand[3]
+ json_send_basic["right"]["pinky"]["approachforce"] = self.realmocapdata.approachforce_rHand[4]
+ json_send_basic["left"]["thumb"]["approachforce"] = self.realmocapdata.approachforce_lHand[0]
+ json_send_basic["left"]["index"]["approachforce"] = self.realmocapdata.approachforce_lHand[1]
+ json_send_basic["left"]["middle"]["approachforce"] = self.realmocapdata.approachforce_lHand[2]
+ json_send_basic["left"]["ring"]["approachforce"] = self.realmocapdata.approachforce_lHand[3]
+ json_send_basic["left"]["pinky"]["approachforce"] = self.realmocapdata.approachforce_lHand[4]
+ json_data = json.dumps(json_send_basic)
+ self.socket_udp.sendto(json_data.encode('utf-8'), self.udp_addr)
+ except Exception as e:
+ print(f"Send未知错误: {e}")
+ finally:
+ pass
+
+ def __recv(self) -> tuple:
+ try:
+ data, addr = self.socket_udp.recvfrom(self.buffer_size)
+ return data, addr
+ except socket.timeout:
+ return None, None
+
+ def udp_close(self) -> bool:
+ self.udp_running = False
+ self.udp_thread.join()
+ time.sleep(0.1)
+ if self.socket_udp:
+ self.socket_udp.close()
+ return True
+
+ def udp_is_onnect(self) -> bool:
+ return self.isconnect
+
+ def __udp_process(self):
+ errorprintcount = 0
+ while self.udp_running:
+ self.__send()
+ try:
+ bytes_data, addr = self.__recv()
+ if bytes_data is not None:
+ try:
+ print(bytes_data)
+ json_data = json.loads(bytes_data.decode('utf-8'))
+ self.realmocapdata.jointangle_rHand[0:6] = [
+ json_data["euler"]["right"]["thumb"]["cmc_roll"],
+ json_data["euler"]["right"]["thumb"]["cmc_yaw"],
+ json_data["euler"]["right"]["thumb"]["cmc_pitch"],
+ json_data["euler"]["right"]["thumb"]["mcp"],
+ json_data["euler"]["right"]["thumb"]["pip"],
+ json_data["euler"]["right"]["thumb"]["dip"]]
+ self.realmocapdata.jointangle_rHand[6:12] = [
+ json_data["euler"]["right"]["index"]["cmc_roll"],
+ json_data["euler"]["right"]["index"]["cmc_yaw"],
+ json_data["euler"]["right"]["index"]["cmc_pitch"],
+ json_data["euler"]["right"]["index"]["mcp"],
+ json_data["euler"]["right"]["index"]["pip"],
+ json_data["euler"]["right"]["index"]["dip"]]
+ self.realmocapdata.jointangle_rHand[12:18] = [
+ json_data["euler"]["right"]["middle"]["cmc_roll"],
+ json_data["euler"]["right"]["middle"]["cmc_yaw"],
+ json_data["euler"]["right"]["middle"]["cmc_pitch"],
+ json_data["euler"]["right"]["middle"]["mcp"],
+ json_data["euler"]["right"]["middle"]["pip"],
+ json_data["euler"]["right"]["middle"]["dip"]]
+ self.realmocapdata.jointangle_rHand[18:24] = [
+ json_data["euler"]["right"]["ring"]["cmc_roll"],
+ json_data["euler"]["right"]["ring"]["cmc_yaw"],
+ json_data["euler"]["right"]["ring"]["cmc_pitch"],
+ json_data["euler"]["right"]["ring"]["mcp"],
+ json_data["euler"]["right"]["ring"]["pip"],
+ json_data["euler"]["right"]["ring"]["dip"]]
+ self.realmocapdata.jointangle_rHand[24:30] = [
+ json_data["euler"]["right"]["pinky"]["cmc_roll"],
+ json_data["euler"]["right"]["pinky"]["cmc_yaw"],
+ json_data["euler"]["right"]["pinky"]["cmc_pitch"],
+ json_data["euler"]["right"]["pinky"]["mcp"],
+ json_data["euler"]["right"]["pinky"]["pip"],
+ json_data["euler"]["right"]["pinky"]["dip"]]
+ self.realmocapdata.jointangle_lHand[0:6] = [
+ json_data["euler"]["left"]["thumb"]["cmc_roll"],
+ json_data["euler"]["left"]["thumb"]["cmc_yaw"],
+ json_data["euler"]["left"]["thumb"]["cmc_pitch"],
+ json_data["euler"]["left"]["thumb"]["mcp"],
+ json_data["euler"]["left"]["thumb"]["pip"],
+ json_data["euler"]["left"]["thumb"]["dip"]]
+ self.realmocapdata.jointangle_lHand[6:12] = [
+ json_data["euler"]["left"]["index"]["cmc_roll"],
+ json_data["euler"]["left"]["index"]["cmc_yaw"],
+ json_data["euler"]["left"]["index"]["cmc_pitch"],
+ json_data["euler"]["left"]["index"]["mcp"],
+ json_data["euler"]["left"]["index"]["pip"],
+ json_data["euler"]["left"]["index"]["dip"]]
+ self.realmocapdata.jointangle_lHand[12:18] = [
+ json_data["euler"]["left"]["middle"]["cmc_roll"],
+ json_data["euler"]["left"]["middle"]["cmc_yaw"],
+ json_data["euler"]["left"]["middle"]["cmc_pitch"],
+ json_data["euler"]["left"]["middle"]["mcp"],
+ json_data["euler"]["left"]["middle"]["pip"],
+ json_data["euler"]["left"]["middle"]["dip"]]
+ self.realmocapdata.jointangle_lHand[18:24] = [
+ json_data["euler"]["left"]["ring"]["cmc_roll"],
+ json_data["euler"]["left"]["ring"]["cmc_yaw"],
+ json_data["euler"]["left"]["ring"]["cmc_pitch"],
+ json_data["euler"]["left"]["ring"]["mcp"],
+ json_data["euler"]["left"]["ring"]["pip"],
+ json_data["euler"]["left"]["ring"]["dip"]]
+ self.realmocapdata.jointangle_lHand[24:30] = [
+ json_data["euler"]["left"]["pinky"]["cmc_roll"],
+ json_data["euler"]["left"]["pinky"]["cmc_yaw"],
+ json_data["euler"]["left"]["pinky"]["cmc_pitch"],
+ json_data["euler"]["left"]["pinky"]["mcp"],
+ json_data["euler"]["left"]["pinky"]["pip"],
+ json_data["euler"]["left"]["pinky"]["dip"]]
+ except json.JSONDecodeError as e:
+ if errorprintcount > 100:
+ print(f"JSON解析错误: {e}")
+ print(f"原始数据: {bytes_data.decode('utf-8', errors='replace')}")
+ errorprintcount = 0
+ except ValueError as e: # 新增：捕获 ValueError
+ if errorprintcount > 100:
+ print(f"[{datetime.now().strftime('%Y-%m-%d %H:%M:%S')}] 设备ID错误: {e}")
+ errorprintcount = 0
+ except Exception as e:
+ if errorprintcount > 100:
+ if e.args[0] == 10054:
+ print("远程设备已经断开!")
+ else:
+ print(f"Recv未知错误: {e}")
+ errorprintcount = 0
+ finally:
+ errorprintcount += 1
+ time.sleep(0.001)
diff --git a/src/linkerhand_retarget/linkerhand_retarget/linkerhand/udexrealcore.py b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/udexrealcore.py
new file mode 100644
index 0000000..88f4f74
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/udexrealcore.py
@@ -0,0 +1,282 @@
+from datetime import datetime
+import socket
+import time
+from threading import Thread
+import json
+from dataclasses import dataclass
+from typing import List, Dict, Union, Any, Optional, Callable
+import threading
+import numpy as np
+
+NODES_HAND = 24
+NO_DATA_TIMEOUT = 1.0 # 无数据超时时间（秒）
+
+@dataclass
+class Bone:
+ Name: str
+ Parent: int
+ Location: List[float]
+ Rotation: List[float]
+ Scale: List[float]
+
+@dataclass
+class Parameter:
+ Name: str
+ Value: Union[float, int, bool]
+
+@dataclass
+class DeviceData:
+ Bones: List[Bone]
+ Parameter: List[Parameter]
+
+class MotionData:
+ def __init__(self, raw_data: Dict[str, Any]):
+ self.devices = {}
+ for device_id, device_content in raw_data.items():
+ bones = [Bone(**bone) for bone in device_content["Bones"]]
+ parameters = [Parameter(**param) for param in device_content["Parameter"]]
+ self.devices[device_id] = DeviceData(Bones=bones, Parameter=parameters)
+
+ def get_device(self, device_id: str) -> DeviceData:
+ return self.devices.get(device_id)
+
+ def list_sequence_params(self, device_id: str, prefix: str) -> Dict[str, Union[float, int, bool]]:
+ device = self.get_device(device_id)
+ if not device:
+ raise ValueError(f"Device {device_id} not found")
+
+ return {
+ param.Name: param.Value
+ for param in device.Parameter
+ if param.Name.startswith(prefix) and param.Name[len(prefix):].isdigit()
+ }
+
+
+class UdexRealData:
+ def __init__(self):
+ self.is_update = False
+ self.frame_index = 0
+ self.frequency = 0
+ self.ns_result = 0
+ self.jointangle_rHand = [0.0] * NODES_HAND
+ self.jointangle_lHand = [0.0] * NODES_HAND
+ # self.jointderict_rHand = [1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1,1]
+ # self.jointderict_lHand = [-1,-1,-1,1,-1,-1,-1,1,-1,-1,-1,1,-1,-1,-1,1,-1,-1,-1,1,1,1,1,1]
+ self.jointderict_rHand = [1] * NODES_HAND
+ self.jointderict_lHand = [1] * NODES_HAND
+ self.last_data_time = 0.0
+ self.is_data_timeout = False
+
+
+@dataclass
+class TimeoutStatus:
+ """超时状态数据结构"""
+ is_timeout: bool # 当前是否超时
+ last_receive_time: float # 上次收到数据的时间戳
+ time_since_last_data: float # 距离上次收到数据的秒数
+ frame_index: int # 当前帧数
+ timeout_threshold: float # 超时阈值
+ consecutive_timeout_checks: int # 连续超时检查次数
+
+
+class UdexRealScoketUdp:
+ def __init__(self, host='0.0.0.0', port=7000, buffer_size=2048, device_id='eric'):
+ self.socket_udp = None
+ self.udp_thread = None
+ self.udp_running = False
+ self.isconnect = False
+ self.host = host
+ self.port = port
+ self.device_id = device_id
+ self.buffer_size = buffer_size
+ self.realmocapdata = UdexRealData()
+ self.data_lock = threading.Lock()
+
+ # 超时检测相关
+ self.no_data_timeout = NO_DATA_TIMEOUT
+ self.last_receive_time = 0.0
+ self.consecutive_timeout_checks = 0 # 连续超时检查次数
+
+ # 回调函数（可选）
+ self.on_timeout_callback: Optional[Callable[[TimeoutStatus], None]] = None
+ self.on_data_recovered_callback: Optional[Callable[[], None]] = None
+
+ def set_timeout_callback(self, callback: Callable[[TimeoutStatus], None]):
+ """设置超时回调函数"""
+ self.on_timeout_callback = callback
+
+ def set_data_recovered_callback(self, callback: Callable[[], None]):
+ """设置数据恢复回调函数"""
+ self.on_data_recovered_callback = callback
+
+ def udp_initial(self) -> bool:
+ try:
+ self.socket_udp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ self.socket_udp.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
+ self.socket_udp.settimeout(1.0)
+ self.socket_udp.bind((self.host, self.port))
+ self.isconnect = True
+ self.udp_running = True
+ self.last_receive_time = time.time()
+ self.consecutive_timeout_checks = 0
+
+ self.udp_thread = Thread(target=self.__udp_process, daemon=True)
+ self.udp_thread.start()
+ return True
+ except socket.error:
+ self.isconnect = False
+ if self.socket_udp:
+ self.socket_udp.close()
+ return False
+
+ def __recv(self) -> tuple:
+ try:
+ data, addr = self.socket_udp.recvfrom(self.buffer_size)
+ return data, addr
+ except socket.timeout:
+ return None, None
+ except socket.error:
+ return None, None
+
+ def udp_close(self) -> bool:
+ self.udp_running = False
+ if self.udp_thread and self.udp_thread.is_alive():
+ self.udp_thread.join(timeout=1.0)
+ time.sleep(0.1)
+ if self.socket_udp:
+ self.socket_udp.close()
+ self.socket_udp = None
+ self.isconnect = False
+ return True
+
+ def udp_is_connect(self) -> bool:
+ return self.isconnect
+
+ def check_timeout(self) -> TimeoutStatus:
+ """
+ 检查超时状态，返回超时状态信息
+ 外部调用此方法来获取超时状态，并决定如何打印
+ """
+ current_time = time.time()
+ time_since_last_data = current_time - self.last_receive_time
+ is_timeout = time_since_last_data > self.no_data_timeout
+
+ # 更新连续超时检查次数
+ if is_timeout:
+ self.consecutive_timeout_checks += 1
+ else:
+ self.consecutive_timeout_checks = 0
+
+ # 更新数据对象的超时状态
+ with self.data_lock:
+ self.realmocapdata.is_data_timeout = is_timeout
+
+ return TimeoutStatus(
+ is_timeout=is_timeout,
+ last_receive_time=self.last_receive_time,
+ time_since_last_data=time_since_last_data,
+ frame_index=self.realmocapdata.frame_index,
+ timeout_threshold=self.no_data_timeout,
+ consecutive_timeout_checks=self.consecutive_timeout_checks
+ )
+
+ def is_data_timeout(self) -> bool:
+ """快速检查是否超时"""
+ current_time = time.time()
+ time_since_last_data = current_time - self.last_receive_time
+ return time_since_last_data > self.no_data_timeout
+
+ def get_connection_status(self) -> Dict[str, Any]:
+ """获取完整的连接状态信息"""
+ current_time = time.time()
+ time_since_last_data = current_time - self.last_receive_time
+
+ with self.data_lock:
+ frame_index = self.realmocapdata.frame_index
+ is_data_timeout = self.realmocapdata.is_data_timeout
+
+ return {
+ 'is_connected': self.isconnect,
+ 'is_running': self.udp_running,
+ 'last_receive_time': self.last_receive_time,
+ 'last_receive_time_str': datetime.fromtimestamp(self.last_receive_time).strftime('%Y-%m-%d %H:%M:%S') if self.last_receive_time > 0 else '从未',
+ 'time_since_last_data': time_since_last_data,
+ 'is_data_timeout': is_data_timeout,
+ 'timeout_threshold': self.no_data_timeout,
+ 'frame_index': frame_index,
+ 'consecutive_timeout_checks': self.consecutive_timeout_checks,
+ 'device_id': self.device_id,
+ 'port': self.port
+ }
+
+ def __udp_process(self):
+ was_timeout = False # 记录上次检查是否超时
+
+ while self.udp_running and self.isconnect:
+ try:
+ bytes_data, addr = self.__recv()
+
+ if bytes_data is not None:
+ # 更新接收时间
+ current_time = time.time()
+ self.last_receive_time = current_time
+
+ # 检查是否从超时状态恢复
+ if was_timeout:
+ was_timeout = False
+ # 调用数据恢复回调
+ if self.on_data_recovered_callback:
+ self.on_data_recovered_callback()
+
+ try:
+ json_data = json.loads(bytes_data.decode('utf-8'))
+ with self.data_lock:
+ motion_data = MotionData(json_data)
+ self.realmocapdata.is_update = True
+ self.realmocapdata.last_data_time = current_time
+ self.realmocapdata.frame_index += 1
+
+ try:
+ l_params = motion_data.list_sequence_params(self.device_id, "l")
+ for name, value in sorted(l_params.items(), key=lambda x: int(x[0][1:])):
+ if int(name[1:]) >= NODES_HAND: break
+ self.realmocapdata.jointangle_lHand[int(name[1:])] = np.deg2rad(value) * self.realmocapdata.jointderict_lHand[int(name[1:])]
+ except Exception:
+ pass
+
+ try:
+ r_params = motion_data.list_sequence_params(self.device_id, "r")
+ for name, value in sorted(r_params.items(), key=lambda x: int(x[0][1:])):
+ if int(name[1:]) >= NODES_HAND: break
+ self.realmocapdata.jointangle_rHand[int(name[1:])] = np.deg2rad(value)* self.realmocapdata.jointderict_rHand[int(name[1:])]
+ except Exception:
+ pass
+
+ except (json.JSONDecodeError, ValueError, Exception):
+ pass
+
+ else:
+ # 没有收到数据，检查是否进入超时状态
+ current_timeout_status = self.check_timeout()
+ if current_timeout_status.is_timeout:
+ was_timeout = True
+ # 调用超时回调
+ if self.on_timeout_callback:
+ self.on_timeout_callback(current_timeout_status)
+
+ except Exception:
+ pass
+
+ def udp_recv_mocap_data(self, mocap_data: UdexRealData) -> bool:
+ with self.data_lock:
+ mocap_data.frame_index = self.realmocapdata.frame_index
+ mocap_data.is_update = self.realmocapdata.is_update
+ mocap_data.frequency = self.realmocapdata.frequency
+ mocap_data.jointangle_rHand = self.realmocapdata.jointangle_rHand.copy()
+ mocap_data.jointangle_lHand = self.realmocapdata.jointangle_lHand.copy()
+ mocap_data.last_data_time = self.realmocapdata.last_data_time
+ mocap_data.is_data_timeout = self.realmocapdata.is_data_timeout
+
+ self.realmocapdata.is_update = False
+
+ return True
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/linkerhand/utils.py b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/utils.py
new file mode 100644
index 0000000..74bbc12
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/utils.py
@@ -0,0 +1,346 @@
+import enum
+import copy
+import yaml
+import math
+import os
+import numpy as np
+from transforms3d.quaternions import axangle2quat, qmult
+from transforms3d.quaternions import mat2quat
+from transforms3d.euler import mat2euler, quat2mat, euler2mat
+from scipy.spatial.transform import Rotation as R
+
+
+class DataSource(enum.Enum):
+ motion = enum.auto()
+ video = enum.auto()
+ vr = enum.auto()
+
+
+def read_yaml(file_path):
+ with open(file_path, 'r') as file:
+ config = yaml.safe_load(file)
+ return config
+
+
+def extract_dataset_folder_last_two_digits(dir_name):
+ # Extract the last two characters, ensure they are digits, and convert to integer
+ last_two = dir_name[-2:] # Get the last two characters
+ if last_two.isdigit():
+ return int(last_two)
+ else:
+ return -1 # Return -1 (or some other value) if there are no digits
+
+
+def _back_project_batch(points, intrinsics):
+ """ Back-project a batch of points from 3D to 2D image space using vectorized operations """
+ points = np.array(points)
+
+ fx, fy, cx, cy = intrinsics[0, 0], intrinsics[1, 1], intrinsics[0, 2], intrinsics[1, 2]
+ x, y, z = points[:, 0], points[:, 1], points[:, 2]
+
+ u = (x * fx / z) + cx
+ v = (y * fy / z) + cy
+
+ projected_points = np.vstack((u, v)).T.astype(int)
+
+ # return projected_points
+
+
+def translate_wrist_to_origin(joint_positions):
+ wrist_position = joint_positions[0]
+ updated_positions = joint_positions - wrist_position
+ return updated_positions
+
+
+def apply_pose_matrix(joint_positions, pose_matrix):
+ homogeneous_joint_positions = np.hstack([joint_positions, np.ones((joint_positions.shape[0], 1))])
+ transformed_positions = np.dot(homogeneous_joint_positions, pose_matrix.T)
+ transformed_positions_3d = transformed_positions[:, :3]
+ return transformed_positions_3d
+
+
+def inverse_transformation(matrix):
+ # Assuming matrix is a 4x4 numpy array
+ R = matrix[:3, :3]
+ T = matrix[:3, 3]
+
+ R_inv = np.linalg.inv(R)
+ T_inv = -np.dot(R_inv, T)
+
+ inverse_matrix = np.eye(4) # Create a 4x4 identity matrix
+ inverse_matrix[:3, :3] = R_inv
+ inverse_matrix[:3, 3] = T_inv
+
+ return inverse_matrix
+
+
+def update_R_delta_init(frame_0_eef_pos, frame_0_eef_quat):
+ global R_delta_init
+
+ frame_0_pose = np.eye(4)
+ frame_0_pose[:3, :3] = quat2mat(frame_0_eef_quat)
+ frame_0_pose[:3, 3] = frame_0_eef_pos
+
+ pose_ori_matirx = frame_0_pose[:3, :3]
+ pose_ori_correction_matrix = np.dot(np.array([[0, -1, 0],
+ [0, 0, 1],
+ [1, 0, 0]]), euler2mat(0, 0, 0))
+ pose_ori_matirx = np.dot(pose_ori_matirx, pose_ori_correction_matrix)
+
+ canonical_t265_ori = np.array([[1, 0, 0],
+ [0, -1, 0],
+ [0, 0, -1]])
+ x_angle, y_angle, z_angle = mat2euler(frame_0_pose[:3, :3])
+ canonical_t265_ori = np.dot(canonical_t265_ori, euler2mat(-z_angle, x_angle + 0.3, y_angle))
+
+ R_delta_init = np.dot(canonical_t265_ori, pose_ori_matirx.T)
+
+
+def switch_axis(quaternion_xyzw, i, j):
+ q1 = np.array(
+ [quaternion_xyzw[3], quaternion_xyzw[0], quaternion_xyzw[1], quaternion_xyzw[2]]
+ )
+ rot_mat = quat2mat(q1)
+ rot_mat_copy = copy.deepcopy(rot_mat)
+ rot_mat[i] = rot_mat_copy[j]
+ rot_mat[j] = rot_mat_copy[i]
+ import pdb
+
+ pdb.set_trace()
+ q2 = mat2quat(rot_mat)
+ q3 = np.array([q2[1], q2[2], q2[3], q2[0]])
+ return q3
+
+
+def swap_quaternion_axes(quaternion, axis1, axis2):
+ """
+ Swap two axes in a quaternion without converting to Euler angles.
+
+ Args:
+ quaternion (list or np.ndarray): The input quaternion [x, y, z, w].
+ axis1 (int): The index of the first axis to swap (0 for X, 1 for Y, 2 for Z).
+ axis2 (int): The index of the second axis to swap (0 for X, 1 for Y, 2 for Z).
+
+ Returns:
+ np.ndarray: The new quaternion with swapped axes [x', y', z', w'].
+ """
+ if axis1 < 0 or axis1 > 2 or axis2 < 0 or axis2 > 2:
+ raise ValueError("Axis indices must be 0, 1, or 2.")
+
+ # Create a copy of the input quaternion
+ new_quaternion = quaternion.copy()
+
+ # Swap the elements corresponding to the specified axes
+ new_quaternion[axis1], new_quaternion[axis2] = quaternion[axis2], quaternion[axis1]
+
+ return new_quaternion
+
+
+def trans_xyzwori_to_wxyzori(ori):
+ return ((ori[3], ori[0], ori[1], ori[2]))
+
+
+def trans_wxyzori_to_xyzwori(ori):
+ return ((ori[1], ori[2], ori[3], ori[0]))
+
+
+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))
+
+
+def extend_line(point1, point2, distance):
+ vector = np.array(point2) - np.array(point1)
+ vector_magnitude = np.linalg.norm(vector)
+ unit_vector = vector / vector_magnitude
+ extended_point = point2 + unit_vector * distance
+ return extended_point
+
+
+def poseture_to_matrix(position, ori):
+ matrix = np.eye(4)
+ rotation_matrix = quat2mat(ori)
+ matrix[:3, :3] = rotation_matrix
+ matrix[:3, 3] = position
+ return matrix
+
+
+def make_reference_matrix(position, ori, lenpose, lenori):
+ rotated_quaternion_wxyz = np.array([ori[3], ori[0], ori[1], ori[2]])
+ rotated_quaternion_wxyz_len = np.array([lenori[3], lenori[0], lenori[1], lenori[2]])
+ base_matrix = poseture_to_matrix(position, rotated_quaternion_wxyz)
+ adder_matrix = poseture_to_matrix(lenpose, rotated_quaternion_wxyz_len)
+ return base_matrix @ adder_matrix
+
+
+def change_orientation(ori):
+ return np.array([ori[3], ori[0], ori[1], ori[2]])
+
+
+def cal_distance(point_s, point_t):
+ vector = [point_s[i] - point_t[i] for i in range(3)]
+ vector_magnitude = math.sqrt(sum(x ** 2 for x in vector))
+ return vector_magnitude
+
+
+def exponential_growth(minvalue, maxvalue, growth_factor, num_points):
+ valuelist = np.linspace(minvalue, maxvalue, num_points)
+ x = valuelist.astype(int)
+ y = num_points - np.exp(growth_factor * x)
+ min_y = np.min(y)
+ y -= min_y
+ y = y / np.max(y) * maxvalue
+ lookup_table = dict(zip(x, y))
+ return lookup_table
+
+
+def change_list(q):
+ converted_list = [None if value == 'None' else value for value in q]
+ return converted_list
+
+
+def quaternion_conjugate(q):
+ x, y, z, w = q
+ return np.array([-x, -y, -z, w])
+
+
+def quaternion_norm_squared(q):
+ return np.dot(q, q)
+
+
+def quaternion_inverse(q):
+ q_conjugate = quaternion_conjugate(q)
+ norm_sq = quaternion_norm_squared(q)
+ return q_conjugate / norm_sq
+
+
+def quaternion_multiply(q1, q2):
+ x1, y1, z1, w1 = q1
+ x2, y2, z2, w2 = q2
+ return np.array([
+ w1 * x2 + x1 * w2 + y1 * z2 - z1 * y2, # x
+ w1 * y2 - x1 * z2 + y1 * w2 + z1 * x2, # y
+ w1 * z2 + x1 * y2 - y1 * x2 + z1 * w2, # z
+ w1 * w2 - x1 * x2 - y1 * y2 - z1 * z2 # w
+ ])
+
+
+def exponential_growth_fun(x_values, c, a_min, a_max):
+ # 计算 k，使得 a(1) = a_max
+ k = a_max / (np.exp(c) - 1)
+
+ # 计算指数增长值
+ a_values = k * (np.exp(c * x_values) - 1)
+
+ # 将 a_values 映射到 [a_min, a_max] 范围
+ a_min_original = 0 # 原有公式的最小值
+ a_max_original = k * (np.exp(c) - 1) # 原有公式的最大值
+ a_values_mapped = a_min + (a_values - a_min_original) * (a_max - a_min) / (a_max_original - a_min_original)
+
+ return a_values_mapped
+
+
+def quaternion_matrixinv(q):
+ qw, qx, qy, qz = q
+ rotation_matrix = np.array([[2 * qw ** 2 + 2 * qx ** 2 - 1, 2 * qx * qy - 2 * qw * qz, 2 * qw * qy + 2 * qx * qz],
+ [2 * qw * qz + 2 * qx * qy, 2 * qw ** 2 + 2 * qy ** 2 - 1, 2 * qy * qz - 2 * qw * qx],
+ [2 * qx * qz - 2 * qw * qy, 2 * qw * qx + 2 * qy * qz, 2 * qw ** 2 + 2 * qz ** 2 - 1]])
+
+ return np.linalg.inv(rotation_matrix)
+
+
+def unitydata_to_worldspacedata(initial_positions):
+ new_positions = []
+ for position in initial_positions:
+ new_positions.append([position[0], position[2], position[1]])
+ return new_positions
+
+
+def get_quaternion_relative(ori, targetori):
+ q_target = R.from_quat(targetori)
+ q_parent = R.from_quat(ori)
+ q_parent_inv = q_parent.inv().as_quat() * -1
+ q_parent_inv = R.from_quat(q_parent_inv)
+ q_relative = q_parent_inv * q_target
+ return q_relative.as_quat()
+
+
+def get_child_quaternion(ori, ori_relative):
+ q_child = R.from_quat(ori)
+ q_relative = R.from_quat(ori_relative)
+ q_result = q_relative * q_child
+ return q_result.as_quat()
+
+
+def quat2handposition(quat, bone):
+ root = bone[0]
+ fn = np.array([0, 0, 1, 2, 3, 0, 5, 6, 7, 8, 0, 10, 11, 12, 13, 0, 15, 16, 17, 18, 0, 20, 21, 22, 23])
+ orin = np.array([0, 0, 1, 2, 3, 0, 4, 5, 6, 7, 0, 8, 9, 10, 11, 0, 12, 13, 14, 15, 0, 16, 17, 18, 19])
+ boneVer = bone[:25] - bone[fn]
+ pos = np.ones((bone.shape[0], 1)) * root
+ for i in range(1, 25):
+ qt = quat[orin[i]]
+ pos[i] = boneVer[i] @ quaternion_matrixinv(qt) + pos[fn[i]]
+ return pos
+
+
+def rotate_matrix_x(radians):
+ return np.array([
+ [1, 0, 0],
+ [0, np.cos(radians), -np.sin(radians)],
+ [0, np.sin(radians), np.cos(radians)]
+ ])
+
+
+def rotate_matrix_y(radians):
+ return np.array([
+ [np.cos(radians), 0, np.sin(radians)],
+ [0, 1, 0],
+ [-np.sin(radians), 0, np.cos(radians)]
+ ])
+
+
+def rotate_matrix_z(radians):
+ return np.array([
+ [np.cos(radians), -np.sin(radians), 0],
+ [np.sin(radians), np.cos(radians), 0],
+ [0, 0, 1]
+ ])
+
+
+def rotate_quaternion(original_quat, roll, pitch, yaw):
+ """应用绕X, Y, Z轴的旋转到原始四元数。
+
+ 参数:
+ original_quat (array_like): 原始四元数 [x, y, z, w] 格式。
+ roll (float): 绕X轴旋转的角度（度）。
+ pitch (float): 绕Y轴旋转的角度（度）。
+ yaw (float): 绕Z轴旋转的角度（度）。
+
+ 返回:
+ np.ndarray: 旋转后的四元数 [x, y, z, w]
+ """
+ # 原始四元数转换为旋转对象
+ original_rotation = R.from_quat(original_quat)
+
+ # 将角度转换为弧度
+ roll_rad = np.radians(roll)
+ pitch_rad = np.radians(pitch)
+ yaw_rad = np.radians(yaw)
+
+ # 创建旋转对象，从给定的欧拉角创建一个新的旋转对象
+ rotation = R.from_euler('xyz', [roll_rad, pitch_rad, yaw_rad])
+
+ # 组合旋转，先应用原始旋转，再应用新旋转
+ new_rotation = original_rotation * rotation
+
+ # 返回结果四元数，转换为 [x, y, z, w] 形式
+ return new_rotation.as_quat()
+
+
+def cubic_model(x, a, b, c, d):
+ """Cubic model for curve fitting"""
+ return a * x ** 3 + b * x ** 2 + c * x + d
diff --git a/src/linkerhand_retarget/linkerhand_retarget/linkerhand/vtrdyncore.py b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/vtrdyncore.py
new file mode 100644
index 0000000..93bf72b
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/vtrdyncore.py
@@ -0,0 +1,208 @@
+import socket
+import struct
+import time
+from threading import Thread
+import threading
+
+NODES_BODY = 23
+NODES_HAND = 20
+NODES_FACEBS_ARKIT = 52
+NODES_FACEBS_AUDIO = 26
+
+DC_QUAT = 1e-4 # short -> double
+DC_POSITION = 1e-3 # short -> double
+DC_POWER = 1e-2 # short -> double
+
+uc_ConnectsendBytes = bytes(
+ [0xfa, 0x00, 0x00, 0x0b, 0x04, 0x03, 0xa2, 0x53, 0x23, 0x52, 0xce, 0x32, 0x99, 0xf4, 0x32, 0xfb, 0x30])
+uc_DisConnectsendBytes = bytes([0xfa, 0x00, 0x00, 0x03, 0x04, 0x0b, 0xa1, 0xfb, 0xa8])
+
+
+class MocapData:
+ def __init__(self):
+ self.is_update = False
+ self.frame_index = 0
+ self.frequency = 0
+ self.ns_result = 0
+
+ self.sensor_state_body = [0] * NODES_BODY
+ self.position_body = [[0.0, 0.0, 0.0] for _ in range(NODES_BODY)]
+ self.quaternion_body = [[0.0, 0.0, 0.0, 0.0] for _ in range(NODES_BODY)]
+ self.gyr_body = [[0.0, 0.0, 0.0] for _ in range(NODES_BODY)]
+ self.acc_body = [[0.0, 0.0, 0.0] for _ in range(NODES_BODY)]
+ self.velocity_body = [[0.0, 0.0, 0.0] for _ in range(NODES_BODY)]
+
+ self.sensor_state_r_hand = [0] * NODES_HAND
+ self.position_rHand = [[0.0, 0.0, 0.0] for _ in range(NODES_HAND)]
+ self.quaternion_rHand = [[0.0, 0.0, 0.0, 0.0] for _ in range(NODES_HAND)]
+ self.gyr_r_hand = [[0.0, 0.0, 0.0] for _ in range(NODES_HAND)]
+ self.acc_r_hand = [[0.0, 0.0, 0.0] for _ in range(NODES_HAND)]
+ self.velocity_r_hand = [[0.0, 0.0, 0.0] for _ in range(NODES_HAND)]
+
+ self.sensor_state_l_hand = [0] * NODES_HAND
+ self.position_lHand = [[0.0, 0.0, 0.0] for _ in range(NODES_HAND)]
+ self.quaternion_lHand = [[0.0, 0.0, 0.0, 0.0] for _ in range(NODES_HAND)]
+ self.gyr_l_hand = [[0.0, 0.0, 0.0] for _ in range(NODES_HAND)]
+ self.acc_l_hand = [[0.0, 0.0, 0.0] for _ in range(NODES_HAND)]
+ self.velocity_l_hand = [[0.0, 0.0, 0.0] for _ in range(NODES_HAND)]
+
+ self.is_use_face_blend_shapes_arkit = False
+ self.is_use_face_blend_shapes_audio = False
+ self.face_blend_shapes_arkit = [0.0] * NODES_FACEBS_ARKIT
+ self.face_blend_shapes_audio = [0.0] * NODES_FACEBS_AUDIO
+ self.local_quat_right_eyeball = [0.0] * 4
+ self.local_quat_left_eyeball = [0.0] * 4
+
+
+class VtrdynSocketUdp:
+ def __init__(self, debug = False):
+ self.socket_udp = None
+ self.is_use_face_blend_shapes_arkit = False
+ self.send_thread = None
+ self.send_running = False
+ self.mocap_data_realtime = MocapData()
+ self.data_lock = threading.Lock()
+ self.isconnect = False
+ self.debug = debug
+
+ def udp_initial(self, local_port: int) -> bool:
+ """初始化 UDP socket"""
+ try:
+ self.socket_udp = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
+ self.socket_udp.bind(('', local_port))
+ self.socket_udp.settimeout(10)
+ self.isconnect = True
+ return True
+ except socket.error as e:
+ self.isconnect = False
+ if self.debug:
+ print(f"Socket initialization failed: {e}")
+ if self.socket_udp:
+ self.socket_udp.close()
+ return False
+
+ @staticmethod
+ def udp_getsockaddr(ip: str, port: int) -> tuple:
+ """将 IP 地址及端口号转化为能识别的地址格式"""
+ return (ip, port)
+
+ def __recv(self, buffer_size: int = 3415) -> tuple:
+ try:
+ data, addr = self.socket_udp.recvfrom(buffer_size)
+ return data, addr
+ except socket.error as e:
+ return None, None
+
+ def udp_close(self, dst_addr: tuple) -> bool:
+ self.socket_udp.sendto(uc_DisConnectsendBytes, dst_addr)
+ self.send_running = False
+ self.send_thread.join()
+ time.sleep(0.1)
+ if self.socket_udp:
+ self.socket_udp.close()
+ return self.send_running
+
+ def udp_send_request_connect(self, dst_addr: tuple) -> bool:
+ connerrflag = False
+ try:
+ self.socket_udp.sendto(uc_ConnectsendBytes, dst_addr)
+ if self.debug:
+ print(f"Initialization packet sent to {dst_addr}")
+ # 等待确认响应
+ bytes_data, addr = self.socket_udp.recvfrom(1024)
+ if addr == dst_addr:
+ connerrflag = True
+ if self.debug:
+ print(f"Connection established with {addr}")
+ except socket.timeout:
+ if self.debug:
+ print("Initialization timeout: No response from target")
+ except socket.error as e:
+ if self.debug:
+ print(f"Initialization error: {str(e)}")
+ self.send_running = True
+ self.send_thread = Thread(target=self.__udp_process)
+ self.send_thread.start()
+ return connerrflag
+
+ def udp_is_onnect(self) -> bool:
+ return self.isconnect
+
+ def __udp_process(self):
+ while self.send_running:
+ try:
+ bytes_data, addr = self.__recv()
+ if bytes_data is None:
+ self.isconnect = False
+ time.sleep(0.01)
+ continue
+ self.isconnect = True
+ if len(bytes_data) < 683 or (bytes_data[2] << 8 | bytes_data[3]) - 3 < (NODES_BODY * 8) or bytes_data[
+ 0] != 250 or bytes_data[681] != 251:
+ return False
+ mocap_temp = MocapData()
+ offset = 1
+ mocap_temp.frame_index = bytes_data[offset]
+ offset = 7 # Move to the next field
+ mocap_temp.is_update = bool(bytes_data[offset])
+ offset = 10 # Move to frequency
+ mocap_temp.frequency = bytes_data[offset]
+ offset = 11 # Move to hips_position
+ for i in range(3):
+ # 每个位置分量
+ mocap_temp.position_body[i] = \
+ struct.unpack('>h', bytes_data[offset + i * 2:offset + i * 2 + 2])[
+ 0] * DC_POSITION
+ offset = offset + 6 + NODES_BODY # 移动到 quaternion_body 开始的位置
+ for i in range(NODES_BODY):
+ for j in range(4):
+ current_offset = offset + i * 8 + j * 2
+ mocap_temp.quaternion_body[i][j] = \
+ struct.unpack('>h', bytes_data[current_offset:current_offset + 2])[
+ 0] * DC_QUAT
+ offset = offset + NODES_BODY * 8 + NODES_HAND # 移动到 quaternion_rightHand 开始的位置
+ for i in range(NODES_HAND):
+ for j in range(4):
+ current_offset = offset + i * 8 + j * 2
+ mocap_temp.quaternion_rHand[i][j] = \
+ struct.unpack('>h', bytes_data[current_offset:current_offset + 2])[
+ 0] * DC_QUAT
+ offset = offset + NODES_HAND * 8 + NODES_HAND # 移动到 quaternion_leftHand 开始的位置
+ for i in range(NODES_HAND):
+ for j in range(4):
+ current_offset = offset + i * 8 + j * 2
+ mocap_temp.quaternion_lHand[i][j] = \
+ struct.unpack('>h', bytes_data[current_offset:current_offset + 2])[
+ 0] * DC_QUAT
+ offset = offset + NODES_HAND * 8 + 1 # 移动到 isUseBlendShapeArkit
+ mocap_temp.is_use_face_blend_shapes_arkit = bool(bytes_data[offset])
+ with self.data_lock:
+ self.mocap_data_realtime.frame_index = mocap_temp.frame_index
+
+ self.mocap_data_realtime.is_update = mocap_temp.is_update
+ self.mocap_data_realtime.frequency = mocap_temp.frequency
+ self.mocap_data_realtime.quaternion_rHand = mocap_temp.quaternion_rHand
+ self.mocap_data_realtime.quaternion_lHand = mocap_temp.quaternion_lHand
+ time.sleep(0.01)
+ except socket.timeout:
+ # 检查连接超时
+ if self.connected:
+ if self.debug:
+ print("Connection timeout detected!")
+ self.connected = False
+ break
+ continue
+
+ except Exception as e:
+ if self.debug:
+ print(f"Receive error: {str(e)}")
+ break
+
+ def udp_recv_mocap_data(self, mocap_data: MocapData) -> bool:
+ with self.data_lock:
+ mocap_data.frame_index = self.mocap_data_realtime.frame_index
+ mocap_data.is_update = self.mocap_data_realtime.is_update
+ mocap_data.frequency = self.mocap_data_realtime.frequency
+ mocap_data.quaternion_rHand = self.mocap_data_realtime.quaternion_rHand
+ mocap_data.quaternion_lHand = self.mocap_data_realtime.quaternion_lHand
+ return True
diff --git a/src/linkerhand_retarget/linkerhand_retarget/linkerhand/yourdfpy.py b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/yourdfpy.py
new file mode 100644
index 0000000..6937c37
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/linkerhand/yourdfpy.py
@@ -0,0 +1,2237 @@
+# Code from yourdfpy with small modification for deprecated warning
+# Source: https://github.com/clemense/yourdfpy/blob/main/src/yourdfpy/urdf.py
+
+import copy
+import logging
+import os
+from dataclasses import dataclass, field, is_dataclass
+from functools import partial
+from typing import Dict, List, Optional, Union
+
+import anytree
+import numpy as np
+import six
+import trimesh
+import trimesh.transformations as tra
+from anytree import Node, LevelOrderIter
+from lxml import etree
+
+_logger = logging.getLogger(__name__)
+
+
+def _array_eq(arr1, arr2):
+ if arr1 is None and arr2 is None:
+ return True
+ return (
+ isinstance(arr1, np.ndarray)
+ and isinstance(arr2, np.ndarray)
+ and arr1.shape == arr2.shape
+ and (arr1 == arr2).all()
+ )
+
+
+@dataclass(eq=False)
+class TransmissionJoint:
+ name: str
+ hardware_interfaces: List[str] = field(default_factory=list)
+
+ def __eq__(self, other):
+ if not isinstance(other, TransmissionJoint):
+ return NotImplemented
+ return (
+ self.name == other.name
+ and all(self_hi in other.hardware_interfaces for self_hi in self.hardware_interfaces)
+ and all(other_hi in self.hardware_interfaces for other_hi in other.hardware_interfaces)
+ )
+
+
+@dataclass(eq=False)
+class Actuator:
+ name: str
+ mechanical_reduction: Optional[float] = None
+ # The follwing is only valid for ROS Indigo and prior versions
+ hardware_interfaces: List[str] = field(default_factory=list)
+
+ def __eq__(self, other):
+ if not isinstance(other, Actuator):
+ return NotImplemented
+ return (
+ self.name == other.name
+ and self.mechanical_reduction == other.mechanical_reduction
+ and all(self_hi in other.hardware_interfaces for self_hi in self.hardware_interfaces)
+ and all(other_hi in self.hardware_interfaces for other_hi in other.hardware_interfaces)
+ )
+
+
+@dataclass(eq=False)
+class Transmission:
+ name: str
+ type: Optional[str] = None
+ joints: List[TransmissionJoint] = field(default_factory=list)
+ actuators: List[Actuator] = field(default_factory=list)
+
+ def __eq__(self, other):
+ if not isinstance(other, Transmission):
+ return NotImplemented
+ return (
+ self.name == other.name
+ and self.type == other.type
+ and all(self_joint in other.joints for self_joint in self.joints)
+ and all(other_joint in self.joints for other_joint in other.joints)
+ and all(self_actuator in other.actuators for self_actuator in self.actuators)
+ and all(other_actuator in self.actuators for other_actuator in other.actuators)
+ )
+
+
+@dataclass
+class Calibration:
+ rising: Optional[float] = None
+ falling: Optional[float] = None
+
+
+@dataclass
+class Mimic:
+ joint: str
+ multiplier: Optional[float] = None
+ offset: Optional[float] = None
+
+
+@dataclass
+class SafetyController:
+ soft_lower_limit: Optional[float] = None
+ soft_upper_limit: Optional[float] = None
+ k_position: Optional[float] = None
+ k_velocity: Optional[float] = None
+
+
+@dataclass
+class Sphere:
+ radius: float
+
+
+@dataclass
+class Cylinder:
+ radius: float
+ length: float
+
+
+@dataclass(eq=False)
+class Box:
+ size: np.ndarray
+
+ def __eq__(self, other):
+ if not isinstance(other, Box):
+ return NotImplemented
+ return _array_eq(self.size, other.size)
+
+
+@dataclass(eq=False)
+class Mesh:
+ filename: str
+ scale: Optional[Union[float, np.ndarray]] = None
+
+ def __eq__(self, other):
+ if not isinstance(other, Mesh):
+ return NotImplemented
+
+ if self.filename != other.filename:
+ return False
+
+ if isinstance(self.scale, float) and isinstance(other.scale, float):
+ return self.scale == other.scale
+
+ return _array_eq(self.scale, other.scale)
+
+
+@dataclass
+class Geometry:
+ box: Optional[Box] = None
+ cylinder: Optional[Cylinder] = None
+ sphere: Optional[Sphere] = None
+ mesh: Optional[Mesh] = None
+
+
+@dataclass(eq=False)
+class Color:
+ rgba: np.ndarray
+
+ def __eq__(self, other):
+ if not isinstance(other, Color):
+ return NotImplemented
+ return _array_eq(self.rgba, other.rgba)
+
+
+@dataclass
+class Texture:
+ filename: str
+
+
+@dataclass
+class Material:
+ name: Optional[str] = None
+ color: Optional[Color] = None
+ texture: Optional[Texture] = None
+
+
+@dataclass(eq=False)
+class Visual:
+ name: Optional[str] = None
+ origin: Optional[np.ndarray] = None
+ geometry: Optional[Geometry] = None # That's not really optional according to ROS
+ material: Optional[Material] = None
+
+ def __eq__(self, other):
+ if not isinstance(other, Visual):
+ return NotImplemented
+ return (
+ self.name == other.name
+ and _array_eq(self.origin, other.origin)
+ and self.geometry == other.geometry
+ and self.material == other.material
+ )
+
+
+@dataclass(eq=False)
+class Collision:
+ name: str
+ origin: Optional[np.ndarray] = None
+ geometry: Geometry = None
+
+ def __eq__(self, other):
+ if not isinstance(other, Collision):
+ return NotImplemented
+ return self.name == other.name and _array_eq(self.origin, other.origin) and self.geometry == other.geometry
+
+
+@dataclass(eq=False)
+class Inertial:
+ origin: Optional[np.ndarray] = None
+ mass: Optional[float] = None
+ inertia: Optional[np.ndarray] = None
+
+ def __eq__(self, other):
+ if not isinstance(other, Inertial):
+ return NotImplemented
+ return (
+ _array_eq(self.origin, other.origin) and self.mass == other.mass and _array_eq(self.inertia, other.inertia)
+ )
+
+
+@dataclass(eq=False)
+class Link:
+ name: str
+ inertial: Optional[Inertial] = None
+ visuals: List[Visual] = field(default_factory=list)
+ collisions: List[Collision] = field(default_factory=list)
+
+ def __eq__(self, other):
+ if not isinstance(other, Link):
+ return NotImplemented
+ return (
+ self.name == other.name
+ and self.inertial == other.inertial
+ and all(self_visual in other.visuals for self_visual in self.visuals)
+ and all(other_visual in self.visuals for other_visual in other.visuals)
+ and all(self_collision in other.collisions for self_collision in self.collisions)
+ and all(other_collision in self.collisions for other_collision in other.collisions)
+ )
+
+
+@dataclass
+class Dynamics:
+ damping: Optional[float] = None
+ friction: Optional[float] = None
+
+
+@dataclass
+class Limit:
+ effort: Optional[float] = None
+ velocity: Optional[float] = None
+ lower: Optional[float] = None
+ upper: Optional[float] = None
+
+
+@dataclass(eq=False)
+class Joint:
+ name: str
+ type: str = None
+ parent: str = None
+ child: str = None
+ origin: np.ndarray = None
+ axis: np.ndarray = None
+ dynamics: Optional[Dynamics] = None
+ limit: Optional[Limit] = None
+ mimic: Optional[Mimic] = None
+ calibration: Optional[Calibration] = None
+ safety_controller: Optional[SafetyController] = None
+
+ def __eq__(self, other):
+ if not isinstance(other, Joint):
+ return NotImplemented
+ return (
+ self.name == other.name
+ and self.type == other.type
+ and self.parent == other.parent
+ and self.child == other.child
+ and _array_eq(self.origin, other.origin)
+ and _array_eq(self.axis, other.axis)
+ and self.dynamics == other.dynamics
+ and self.limit == other.limit
+ and self.mimic == other.mimic
+ and self.calibration == other.calibration
+ and self.safety_controller == other.safety_controller
+ )
+
+
+@dataclass(eq=False)
+class Robot:
+ name: str
+ links: List[Link] = field(default_factory=list)
+ joints: List[Joint] = field(default_factory=list)
+ materials: List[Material] = field(default_factory=list)
+ transmission: List[str] = field(default_factory=list)
+ gazebo: List[str] = field(default_factory=list)
+
+ def __eq__(self, other):
+ if not isinstance(other, Robot):
+ return NotImplemented
+ return (
+ self.name == other.name
+ and all(self_link in other.links for self_link in self.links)
+ and all(other_link in self.links for other_link in other.links)
+ and all(self_joint in other.joints for self_joint in self.joints)
+ and all(other_joint in self.joints for other_joint in other.joints)
+ and all(self_material in other.materials for self_material in self.materials)
+ and all(other_material in self.materials for other_material in other.materials)
+ and all(self_transmission in other.transmission for self_transmission in self.transmission)
+ and all(other_transmission in self.transmission for other_transmission in other.transmission)
+ and all(self_gazebo in other.gazebo for self_gazebo in self.gazebo)
+ and all(other_gazebo in self.gazebo for other_gazebo in other.gazebo)
+ )
+
+
+class URDFError(Exception):
+ """General URDF exception."""
+
+ def __init__(self, msg):
+ super(URDFError, self).__init__()
+ self.msg = msg
+
+ def __str__(self):
+ return type(self).__name__ + ": " + self.msg
+
+ def __repr__(self):
+ return type(self).__name__ + '("' + self.msg + '")'
+
+
+class URDFIncompleteError(URDFError):
+ """Raised when needed data for an object isn't there."""
+
+ pass
+
+
+class URDFAttributeValueError(URDFError):
+ """Raised when attribute value is not contained in the set of allowed values."""
+
+ pass
+
+
+class URDFBrokenRefError(URDFError):
+ """Raised when a referenced object is not found in the scope."""
+
+ pass
+
+
+class URDFMalformedError(URDFError):
+ """Raised when data is found to be corrupted in some way."""
+
+ pass
+
+
+class URDFUnsupportedError(URDFError):
+ """Raised when some unexpectedly unsupported feature is found."""
+
+ pass
+
+
+class URDFSaveValidationError(URDFError):
+ """Raised when XML validation fails when saving."""
+
+ pass
+
+
+def _str2float(s):
+ """Cast string to float if it is not None. Otherwise return None.
+
+ Args:
+ s (str): String to convert or None.
+
+ Returns:
+ str or NoneType: The converted string or None.
+ """
+ return float(s) if s is not None else None
+
+
+def apply_visual_color(
+ geom: trimesh.Trimesh,
+ visual: Visual,
+ material_map: Dict[str, Material],
+) -> None:
+ """Apply the color of the visual material to the mesh.
+
+ Args:
+ geom: Trimesh to color.
+ visual: Visual description from XML.
+ material_map: Dictionary mapping material names to their definitions.
+ """
+ if visual.material is None:
+ return
+
+ if visual.material.color is not None:
+ color = visual.material.color
+ elif visual.material.name is not None and visual.material.name in material_map:
+ color = material_map[visual.material.name].color
+ else:
+ return
+
+ if color is None:
+ return
+ if isinstance(geom.visual, trimesh.visual.ColorVisuals):
+ geom.visual.face_colors[:] = [int(255 * channel) for channel in color.rgba]
+
+
+def filename_handler_null(fname):
+ """A lazy filename handler that simply returns its input.
+
+ Args:
+ fname (str): A file name.
+
+ Returns:
+ str: Same file name.
+ """
+ return fname
+
+
+def filename_handler_ignore_directive(fname):
+ """A filename handler that removes anything before (and including) '://'.
+
+ Args:
+ fname (str): A file name.
+
+ Returns:
+ str: The file name without the prefix.
+ """
+ if "://" in fname or ":\\\\" in fname:
+ return ":".join(fname.split(":")[1:])[2:]
+ return fname
+
+
+def filename_handler_ignore_directive_package(fname):
+ """A filename handler that removes the 'package://' directive and the package it refers to.
+ It subsequently calls filename_handler_ignore_directive, i.e., it removes any other directive.
+
+ Args:
+ fname (str): A file name.
+
+ Returns:
+ str: The file name without 'package://' and the package name.
+ """
+ if fname.startswith("package://"):
+ string_length = len("package://")
+ return os.path.join(*os.path.normpath(fname[string_length:]).split(os.path.sep)[1:])
+ return filename_handler_ignore_directive(fname)
+
+
+def filename_handler_add_prefix(fname, prefix):
+ """A filename handler that adds a prefix.
+
+ Args:
+ fname (str): A file name.
+ prefix (str): A prefix.
+
+ Returns:
+ str: Prefix plus file name.
+ """
+ return prefix + fname
+
+
+def filename_handler_absolute2relative(fname, dir):
+ """A filename handler that turns an absolute file name into a relative one.
+
+ Args:
+ fname (str): A file name.
+ dir (str): A directory.
+
+ Returns:
+ str: The file name relative to the directory.
+ """
+ # TODO: that's not right
+ if fname.startswith(dir):
+ return fname[len(dir) :]
+ return fname
+
+
+def filename_handler_relative(fname, dir):
+ """A filename handler that joins a file name with a directory.
+
+ Args:
+ fname (str): A file name.
+ dir (str): A directory.
+
+ Returns:
+ str: The directory joined with the file name.
+ """
+ return os.path.join(dir, filename_handler_ignore_directive_package(fname))
+
+
+def filename_handler_relative_to_urdf_file(fname, urdf_fname):
+ return filename_handler_relative(fname, os.path.dirname(urdf_fname))
+
+
+def filename_handler_relative_to_urdf_file_recursive(fname, urdf_fname, level=0):
+ if level == 0:
+ return filename_handler_relative_to_urdf_file(fname, urdf_fname)
+ return filename_handler_relative_to_urdf_file_recursive(fname, os.path.split(urdf_fname)[0], level=level - 1)
+
+
+def _create_filename_handlers_to_urdf_file_recursive(urdf_fname):
+ return [
+ partial(
+ filename_handler_relative_to_urdf_file_recursive,
+ urdf_fname=urdf_fname,
+ level=i,
+ )
+ for i in range(len(os.path.normpath(urdf_fname).split(os.path.sep)))
+ ]
+
+
+def filename_handler_meta(fname, filename_handlers):
+ """A filename handler that calls other filename handlers until the resulting file name points to an existing file.
+
+ Args:
+ fname (str): A file name.
+ filename_handlers (list(fn)): A list of function pointers to filename handlers.
+
+ Returns:
+ str: The resolved file name that points to an existing file or the input if none of the files exists.
+ """
+ for fn in filename_handlers:
+ candidate_fname = fn(fname=fname)
+ _logger.debug(f"Checking filename: {candidate_fname}")
+ if os.path.isfile(candidate_fname):
+ return candidate_fname
+ _logger.warning(f"Unable to resolve filename: {fname}")
+ return fname
+
+
+def filename_handler_magic(fname, dir):
+ """A magic filename handler.
+
+ Args:
+ fname (str): A file name.
+ dir (str): A directory.
+
+ Returns:
+ str: The file name that exists or the input if nothing is found.
+ """
+ return filename_handler_meta(
+ fname=fname,
+ filename_handlers=[
+ partial(filename_handler_relative, dir=dir),
+ filename_handler_ignore_directive,
+ ]
+ + _create_filename_handlers_to_urdf_file_recursive(urdf_fname=dir),
+ )
+
+
+def validation_handler_strict(errors):
+ """A validation handler that does not allow any errors.
+
+ Args:
+ errors (list[yourdfpy.URDFError]): List of errors.
+
+ Returns:
+ bool: Whether any errors were found.
+ """
+ return len(errors) == 0
+
+
+class URDF:
+ def __init__(
+ self,
+ robot: Robot = None,
+ build_scene_graph: bool = True,
+ build_collision_scene_graph: bool = False,
+ load_meshes: bool = True,
+ load_collision_meshes: bool = False,
+ filename_handler=None,
+ mesh_dir: str = "",
+ force_mesh: bool = False,
+ force_collision_mesh: bool = True,
+ build_tree: bool = False,
+ ):
+ """A URDF model.
+
+ Args:
+ robot (Robot): The robot model. Defaults to None.
+ build_scene_graph (bool, optional): Wheter to build a scene graph to enable transformation queries and forward kinematics. Defaults to True.
+ build_collision_scene_graph (bool, optional): Wheter to build a scene graph for elements. Defaults to False.
+ load_meshes (bool, optional): Whether to load the meshes referenced in the elements. Defaults to True.
+ load_collision_meshes (bool, optional): Whether to load the collision meshes referenced in the elements. Defaults to False.
+ filename_handler ([type], optional): Any function f(in: str) -> str, that maps filenames in the URDF to actual resources. Can be used to customize treatment of `package://` directives or relative/absolute filenames. Defaults to None.
+ mesh_dir (str, optional): A root directory used for loading meshes. Defaults to "".
+ force_mesh (bool, optional): Each loaded geometry will be concatenated into a single one (instead of being turned into a graph; in case the underlying file contains multiple geometries). This might loose texture information but the resulting scene graph will be smaller. Defaults to False.
+ force_collision_mesh (bool, optional): Same as force_mesh, but for collision scene. Defaults to True.
+ build_tree (bool, optional): Build the tree structure for global kinematics computation
+ """
+ if filename_handler is None:
+ self._filename_handler = partial(filename_handler_magic, dir=mesh_dir)
+ else:
+ self._filename_handler = filename_handler
+
+ self.robot = robot
+ self._create_maps()
+ self._update_actuated_joints()
+
+ self._cfg = self.zero_cfg
+
+ if build_scene_graph or build_collision_scene_graph:
+ self._base_link = self._determine_base_link()
+ else:
+ self._base_link = None
+
+ self._errors = []
+
+ if build_scene_graph:
+ self._scene = self._create_scene(
+ use_collision_geometry=False,
+ load_geometry=load_meshes,
+ force_mesh=force_mesh,
+ force_single_geometry_per_link=force_mesh,
+ )
+ else:
+ self._scene = None
+
+ if build_collision_scene_graph:
+ self._scene_collision = self._create_scene(
+ use_collision_geometry=True,
+ load_geometry=load_collision_meshes,
+ force_mesh=force_collision_mesh,
+ force_single_geometry_per_link=force_collision_mesh,
+ )
+ else:
+ self._scene_collision = None
+
+ if build_tree:
+ self.tree_root = self.build_tree()
+ else:
+ self.tree_root = None
+
+ @property
+ def scene(self) -> trimesh.Scene:
+ """A scene object representing the URDF model.
+
+ Returns:
+ trimesh.Scene: A trimesh scene object.
+ """
+ return self._scene
+
+ @property
+ def collision_scene(self) -> trimesh.Scene:
+ """A scene object representing the elements of the URDF model
+
+ Returns:
+ trimesh.Scene: A trimesh scene object.
+ """
+ return self._scene_collision
+
+ @property
+ def link_map(self) -> dict:
+ """A dictionary mapping link names to link objects.
+
+ Returns:
+ dict: Mapping from link name (str) to Link.
+ """
+ return self._link_map
+
+ @property
+ def joint_map(self) -> dict:
+ """A dictionary mapping joint names to joint objects.
+
+ Returns:
+ dict: Mapping from joint name (str) to Joint.
+ """
+ return self._joint_map
+
+ @property
+ def joint_names(self):
+ """List of joint names.
+
+ Returns:
+ list[str]: List of joint names of the URDF model.
+ """
+ return [j.name for j in self.robot.joints]
+
+ @property
+ def actuated_joints(self):
+ """List of actuated joints. This excludes mimic and fixed joints.
+
+ Returns:
+ list[Joint]: List of actuated joints of the URDF model.
+ """
+ return self._actuated_joints
+
+ @property
+ def actuated_dof_indices(self):
+ """List of DOF indices per actuated joint. Can be used to reference configuration.
+
+ Returns:
+ list[list[int]]: List of DOF indices per actuated joint.
+ """
+ return self._actuated_dof_indices
+
+ @property
+ def actuated_joint_indices(self):
+ """List of indices of all joints that are actuated, i.e., not of type mimic or fixed.
+
+ Returns:
+ list[int]: List of indices of actuated joints.
+ """
+ return self._actuated_joint_indices
+
+ @property
+ def actuated_joint_names(self):
+ """List of names of actuated joints. This excludes mimic and fixed joints.
+
+ Returns:
+ list[str]: List of names of actuated joints of the URDF model.
+ """
+ return [j.name for j in self._actuated_joints]
+
+ @property
+ def num_actuated_joints(self):
+ """Number of actuated joints.
+
+ Returns:
+ int: Number of actuated joints.
+ """
+ return len(self.actuated_joints)
+
+ @property
+ def num_dofs(self):
+ """Number of degrees of freedom of actuated joints. Depending on the type of the joint, the number of DOFs might vary.
+
+ Returns:
+ int: Degrees of freedom.
+ """
+ total_num_dofs = 0
+ for j in self._actuated_joints:
+ if j.type in ["revolute", "prismatic", "continuous"]:
+ total_num_dofs += 1
+ elif j.type == "floating":
+ total_num_dofs += 6
+ elif j.type == "planar":
+ total_num_dofs += 2
+ return total_num_dofs
+
+ @property
+ def zero_cfg(self):
+ """Return the zero configuration.
+
+ Returns:
+ np.ndarray: The zero configuration.
+ """
+ return np.zeros(self.num_dofs)
+
+ @property
+ def center_cfg(self):
+ """Return center configuration of URDF model by using the average of each joint's limits if present, otherwise zero.
+
+ Returns:
+ (n), float: Default configuration of URDF model.
+ """
+ config = []
+ config_names = []
+ for j in self._actuated_joints:
+ if j.type == "revolute" or j.type == "prismatic":
+ if j.limit is not None:
+ cfg = [j.limit.lower + 0.5 * (j.limit.upper - j.limit.lower)]
+ else:
+ cfg = [0.0]
+ elif j.type == "continuous":
+ cfg = [0.0]
+ elif j.type == "floating":
+ cfg = [0.0] * 6
+ elif j.type == "planar":
+ cfg = [0.0] * 2
+
+ config.append(cfg)
+ config_names.append(j.name)
+
+ for i, j in enumerate(self.robot.joints):
+ if j.mimic is not None:
+ index = config_names.index(j.mimic.joint)
+ config[i][0] = config[index][0] * j.mimic.multiplier + j.mimic.offset
+
+ if len(config) == 0:
+ return np.array([], dtype=np.float64)
+ return np.concatenate(config)
+
+ @property
+ def cfg(self):
+ """Current configuration.
+
+ Returns:
+ np.ndarray: Current configuration of URDF model.
+ """
+ return self._cfg
+
+ @property
+ def base_link(self):
+ """Name of URDF base/root link.
+
+ Returns:
+ str: Name of base link of URDF model.
+ """
+ return self._base_link
+
+ @property
+ def errors(self) -> list:
+ """A list with validation errors.
+
+ Returns:
+ list: A list of validation errors.
+ """
+ return self._errors
+
+ def clear_errors(self):
+ """Clear the validation error log."""
+ self._errors = []
+
+ def show(self, collision_geometry=False, callback=None):
+ """Open a simpler viewer displaying the URDF model.
+
+ Args:
+ collision_geometry (bool, optional): Whether to display the or elements. Defaults to False.
+ """
+ if collision_geometry:
+ if self._scene_collision is None:
+ raise ValueError(
+ "No collision scene available. Use build_collision_scene_graph=True and load_collision_meshes=True during loading."
+ )
+ else:
+ self._scene_collision.show(callback=callback)
+ else:
+ if self._scene is None:
+ raise ValueError("No scene available. Use build_scene_graph=True and load_meshes=True during loading.")
+ elif len(self._scene.bounds_corners) < 1:
+ raise ValueError(
+ "Scene is empty, maybe meshes failed to load? Use build_scene_graph=True and load_meshes=True during loading."
+ )
+ else:
+ self._scene.show(callback=callback)
+
+ def validate(self, validation_fn=None) -> bool:
+ """Validate URDF model.
+
+ Args:
+ validation_fn (function, optional): A function f(list[yourdfpy.URDFError]) -> bool. None uses the strict handler (any error leads to False). Defaults to None.
+
+ Returns:
+ bool: Whether the model is valid.
+ """
+ self._errors = []
+ self._validate_robot(self.robot)
+
+ if validation_fn is None:
+ validation_fn = validation_handler_strict
+
+ return validation_fn(self._errors)
+
+ def _create_maps(self):
+ self._material_map = {}
+ for m in self.robot.materials:
+ self._material_map[m.name] = m
+
+ self._joint_map = {}
+ for j in self.robot.joints:
+ self._joint_map[j.name] = j
+
+ self._link_map = {}
+ for l in self.robot.links:
+ self._link_map[l.name] = l
+
+ def _update_actuated_joints(self):
+ self._actuated_joints = []
+ self._actuated_joint_indices = []
+ self._actuated_dof_indices = []
+
+ dof_indices_cnt = 0
+ for i, j in enumerate(self.robot.joints):
+ if j.mimic is None and j.type != "fixed":
+ self._actuated_joints.append(j)
+ self._actuated_joint_indices.append(i)
+
+ if j.type in ["prismatic", "revolute", "continuous"]:
+ self._actuated_dof_indices.append([dof_indices_cnt])
+ dof_indices_cnt += 1
+ elif j.type == "floating":
+ self._actuated_dof_indices.append([dof_indices_cnt, dof_indices_cnt + 1, dof_indices_cnt + 2])
+ dof_indices_cnt += 3
+ elif j.type == "planar":
+ self._actuated_dof_indices.append([dof_indices_cnt, dof_indices_cnt + 1])
+ dof_indices_cnt += 2
+
+ def _validate_required_attribute(self, attribute, error_msg, allowed_values=None):
+ if attribute is None:
+ self._errors.append(URDFIncompleteError(error_msg))
+ elif isinstance(attribute, str) and len(attribute) == 0:
+ self._errors.append(URDFIncompleteError(error_msg))
+
+ if allowed_values is not None and attribute is not None:
+ if attribute not in allowed_values:
+ self._errors.append(URDFAttributeValueError(error_msg))
+
+ @staticmethod
+ def load(fname_or_file, add_dummy_free_joints=False, **kwargs):
+ """Load URDF file from filename or file object.
+
+ Args:
+ fname_or_file (str or file object): A filename or file object, file-like object, stream representing the URDF file.
+ **build_scene_graph (bool, optional): Wheter to build a scene graph to enable transformation queries and forward kinematics. Defaults to True.
+ **build_collision_scene_graph (bool, optional): Wheter to build a scene graph for elements. Defaults to False.
+ **load_meshes (bool, optional): Whether to load the meshes referenced in the elements. Defaults to True.
+ **load_collision_meshes (bool, optional): Whether to load the collision meshes referenced in the elements. Defaults to False.
+ **filename_handler ([type], optional): Any function f(in: str) -> str, that maps filenames in the URDF to actual resources. Can be used to customize treatment of `package://` directives or relative/absolute filenames. Defaults to None.
+ **mesh_dir (str, optional): A root directory used for loading meshes. Defaults to "".
+ **force_mesh (bool, optional): Each loaded geometry will be concatenated into a single one (instead of being turned into a graph; in case the underlying file contains multiple geometries). This might loose texture information but the resulting scene graph will be smaller. Defaults to False.
+ **force_collision_mesh (bool, optional): Same as force_mesh, but for collision scene. Defaults to True.
+
+ Raises:
+ ValueError: If filename does not exist.
+
+ Returns:
+ yourdfpy.URDF: URDF model.
+ """
+ if isinstance(fname_or_file, six.string_types):
+ if not os.path.isfile(fname_or_file):
+ raise ValueError("{} is not a file".format(fname_or_file))
+
+ if not "mesh_dir" in kwargs:
+ kwargs["mesh_dir"] = os.path.dirname(fname_or_file)
+
+ try:
+ parser = etree.XMLParser(remove_blank_text=True)
+ tree = etree.parse(fname_or_file, parser=parser)
+ xml_root = tree.getroot()
+ except Exception as e:
+ _logger.error(e)
+ _logger.error("Using different parsing approach.")
+
+ events = ("start", "end", "start-ns", "end-ns")
+ xml = etree.iterparse(fname_or_file, recover=True, events=events)
+
+ # Iterate through all XML elements
+ for action, elem in xml:
+ # Skip comments and processing instructions,
+ # because they do not have names
+ if not (isinstance(elem, etree._Comment) or isinstance(elem, etree._ProcessingInstruction)):
+ # Remove a namespace URI in the element's name
+ # elem.tag = etree.QName(elem).localname
+ if action == "end" and ":" in elem.tag:
+ elem.getparent().remove(elem)
+
+ xml_root = xml.root
+
+ # Remove comments
+ etree.strip_tags(xml_root, etree.Comment)
+ etree.cleanup_namespaces(xml_root)
+
+ return URDF(
+ robot=URDF._parse_robot(xml_element=xml_root, add_dummy_free_joints=add_dummy_free_joints), **kwargs
+ )
+
+ def contains(self, key, value, element=None) -> bool:
+ """Checks recursively whether the URDF tree contains the provided key-value pair.
+
+ Args:
+ key (str): A key.
+ value (str): A value.
+ element (etree.Element, optional): The XML element from which to start the recursive search. None means URDF root. Defaults to None.
+
+ Returns:
+ bool: Whether the key-value pair was found.
+ """
+ if element is None:
+ element = self.robot
+
+ result = False
+ for field in element.__dataclass_fields__:
+ field_value = getattr(element, field)
+ if is_dataclass(field_value):
+ result = result or self.contains(key=key, value=value, element=field_value)
+ elif isinstance(field_value, list) and len(field_value) > 0 and is_dataclass(field_value[0]):
+ for field_value_element in field_value:
+ result = result or self.contains(key=key, value=value, element=field_value_element)
+ else:
+ if key == field and value == field_value:
+ result = True
+ return result
+
+ def _determine_base_link(self):
+ """Get the base link of the URDF tree by extracting all links without parents.
+ In case multiple links could be root chose the first.
+
+ Returns:
+ str: Name of the base link.
+ """
+ link_names = [l.name for l in self.robot.links]
+
+ for j in self.robot.joints:
+ link_names.remove(j.child)
+
+ if len(link_names) == 0:
+ # raise Error?
+ return None
+
+ return link_names[0]
+
+ def _forward_kinematics_joint(self, joint, q=None):
+ origin = np.eye(4) if joint.origin is None else joint.origin
+
+ if joint.mimic is not None:
+ if joint.mimic.joint in self.actuated_joint_names:
+ mimic_joint_index = self.actuated_joint_names.index(joint.mimic.joint)
+ q = self._cfg[mimic_joint_index] * joint.mimic.multiplier + joint.mimic.offset
+ else:
+ # _logger.warning(
+ # f"Joint '{joint.name}' is supposed to mimic '{joint.mimic.joint}'. But this joint is not actuated - will assume (0.0 + offset)."
+ # )
+ q = 0.0 + joint.mimic.offset
+
+ if joint.type in ["revolute", "prismatic", "continuous"]:
+ if q is None:
+ # Use internal cfg vector for forward kinematics
+ q = float(self.cfg[self.actuated_dof_indices[self.actuated_joint_names.index(joint.name)]])
+
+ if joint.type == "prismatic":
+ matrix = origin @ tra.translation_matrix(q * joint.axis)
+ else:
+ matrix = origin @ tra.rotation_matrix(q, joint.axis)
+ else:
+ # this includes: floating, planar, fixed
+ matrix = origin
+
+ return matrix, q
+
+ def update_cfg(self, configuration):
+ """Update joint configuration of URDF; does forward kinematics.
+
+ Args:
+ configuration (dict, list[float], tuple[float] or np.ndarray): A mapping from joints or joint names to configuration values, or a list containing a value for each actuated joint.
+
+ Raises:
+ ValueError: Raised if dimensionality of configuration does not match number of actuated joints of URDF model.
+ TypeError: Raised if configuration is neither a dict, list, tuple or np.ndarray.
+ """
+ joint_cfg = []
+
+ if isinstance(configuration, dict):
+ for joint in configuration:
+ if isinstance(joint, six.string_types):
+ joint_cfg.append((self._joint_map[joint], configuration[joint]))
+ elif isinstance(joint, Joint):
+ # TODO: Joint is not hashable; so this branch will not succeed
+ joint_cfg.append((joint, configuration[joint]))
+ elif isinstance(configuration, (list, tuple, np.ndarray)):
+ if len(configuration) == len(self.robot.joints):
+ for joint, value in zip(self.robot.joints, configuration):
+ joint_cfg.append((joint, value))
+ elif len(configuration) == self.num_actuated_joints:
+ for joint, value in zip(self._actuated_joints, configuration):
+ joint_cfg.append((joint, value))
+ else:
+ raise ValueError(
+ f"Dimensionality of configuration ({len(configuration)}) doesn't match number of all ({len(self.robot.joints)}) or actuated joints ({self.num_actuated_joints})."
+ )
+ else:
+ raise TypeError("Invalid type for configuration")
+
+ # append all mimic joints in the update
+ for j, q in joint_cfg + [(j, 0.0) for j in self.robot.joints if j.mimic is not None]:
+ matrix, joint_q = self._forward_kinematics_joint(j, q=q)
+
+ # update internal configuration vector - only consider actuated joints
+ if j.name in self.actuated_joint_names:
+ self._cfg[self.actuated_dof_indices[self.actuated_joint_names.index(j.name)]] = joint_q
+
+ if self._scene is not None:
+ self._scene.graph.update(frame_from=j.parent, frame_to=j.child, matrix=matrix)
+ if self._scene_collision is not None:
+ self._scene_collision.graph.update(frame_from=j.parent, frame_to=j.child, matrix=matrix)
+
+ def get_transform(self, frame_to, frame_from=None, collision_geometry=False):
+ """Get the transform from one frame to another.
+
+ Args:
+ frame_to (str): Node name.
+ frame_from (str, optional): Node name. If None it will be set to self.base_frame. Defaults to None.
+ collision_geometry (bool, optional): Whether to use the collision geometry scene graph (instead of the visual geometry). Defaults to False.
+
+ Raises:
+ ValueError: Raised if scene graph wasn't constructed during intialization.
+
+ Returns:
+ (4, 4) float: Homogeneous transformation matrix
+ """
+ if collision_geometry:
+ if self._scene_collision is None:
+ raise ValueError("No collision scene available. Use build_collision_scene_graph=True during loading.")
+ else:
+ return self._scene_collision.graph.get(frame_to=frame_to, frame_from=frame_from)[0]
+ else:
+ if self._scene is None:
+ raise ValueError("No scene available. Use build_scene_graph=True during loading.")
+ else:
+ return self._scene.graph.get(frame_to=frame_to, frame_from=frame_from)[0]
+
+ def _link_mesh(self, link, collision_geometry=True):
+ geometries = link.collisions if collision_geometry else link.visuals
+
+ if len(geometries) == 0:
+ return None
+
+ meshes = []
+ for g in geometries:
+ for m in g.geometry.meshes:
+ m = m.copy()
+ pose = g.origin
+ if g.geometry.mesh is not None:
+ if g.geometry.mesh.scale is not None:
+ S = np.eye(4)
+ S[:3, :3] = np.diag(g.geometry.mesh.scale)
+ pose = pose.dot(S)
+ m.apply_transform(pose)
+ meshes.append(m)
+ if len(meshes) == 0:
+ return None
+ self._collision_mesh = meshes[0] + meshes[1:]
+ return self._collision_mesh
+
+ def _geometry2trimeshscene(self, geometry, load_file, force_mesh, skip_materials):
+ new_s = None
+ if geometry.box is not None:
+ new_s = trimesh.primitives.Box(extents=geometry.box.size).scene()
+ elif geometry.sphere is not None:
+ new_s = trimesh.primitives.Sphere(radius=geometry.sphere.radius).scene()
+ elif geometry.cylinder is not None:
+ new_s = trimesh.primitives.Cylinder(
+ radius=geometry.cylinder.radius, height=geometry.cylinder.length
+ ).scene()
+ elif geometry.mesh is not None and load_file:
+ new_filename = self._filename_handler(fname=geometry.mesh.filename)
+
+ if os.path.isfile(new_filename):
+ _logger.debug(f"Loading {geometry.mesh.filename} as {new_filename}")
+
+ if force_mesh:
+ new_g = trimesh.load(
+ new_filename,
+ ignore_broken=True,
+ force="mesh",
+ skip_materials=skip_materials,
+ )
+
+ # add original filename
+ if "file_path" not in new_g.metadata:
+ new_g.metadata["file_path"] = os.path.abspath(new_filename)
+ new_g.metadata["file_name"] = os.path.basename(new_filename)
+
+ new_s = trimesh.Scene()
+ new_s.add_geometry(new_g)
+ else:
+ new_s = trimesh.load(
+ new_filename,
+ ignore_broken=True,
+ force="scene",
+ skip_materials=skip_materials,
+ )
+
+ if "file_path" in new_s.metadata:
+ for i, (_, geom) in enumerate(new_s.geometry.items()):
+ if "file_path" not in geom.metadata:
+ geom.metadata["file_path"] = new_s.metadata["file_path"]
+ geom.metadata["file_name"] = new_s.metadata["file_name"]
+ geom.metadata["file_element"] = i
+
+ # scale mesh appropriately
+ if geometry.mesh.scale is not None:
+ if isinstance(geometry.mesh.scale, float):
+ new_s = new_s.scaled(geometry.mesh.scale)
+ elif isinstance(geometry.mesh.scale, np.ndarray):
+ new_s = new_s.scaled(geometry.mesh.scale)
+ else:
+ _logger.warning(f"Warning: Can't interpret scale '{geometry.mesh.scale}'")
+ else:
+ _logger.warning(f"Can't find {new_filename}")
+ return new_s
+
+ def _add_geometries_to_scene(
+ self,
+ s,
+ geometries,
+ link_name,
+ load_geometry,
+ force_mesh,
+ force_single_geometry,
+ skip_materials,
+ ):
+ if force_single_geometry:
+ tmp_scene = trimesh.Scene(base_frame=link_name)
+
+ first_geom_name = None
+
+ for v in geometries:
+ if v.geometry is not None:
+ if first_geom_name is None:
+ first_geom_name = v.name
+
+ new_s = self._geometry2trimeshscene(
+ geometry=v.geometry,
+ load_file=load_geometry,
+ force_mesh=force_mesh,
+ skip_materials=skip_materials,
+ )
+ if new_s is not None:
+ origin = v.origin if v.origin is not None else np.eye(4)
+
+ if force_single_geometry:
+ for name, geom in new_s.geometry.items():
+ if isinstance(v, Visual):
+ apply_visual_color(geom, v, self._material_map)
+ tmp_scene.add_geometry(
+ geometry=geom,
+ geom_name=v.name,
+ parent_node_name=link_name,
+ transform=origin @ new_s.graph.get(name)[0],
+ )
+ else:
+ # The following map is used to deal with glb format
+ # when the graph node and geometry have different names
+ geom_name_map = {new_s.graph[node_name][1]: node_name for node_name in new_s.graph.nodes}
+ for name, geom in new_s.geometry.items():
+ if isinstance(v, Visual):
+ apply_visual_color(geom, v, self._material_map)
+ s.add_geometry(
+ geometry=geom,
+ geom_name=v.name,
+ parent_node_name=link_name,
+ transform=origin @ new_s.graph.get(geom_name_map[name])[0],
+ )
+
+ if force_single_geometry and len(tmp_scene.geometry) > 0:
+ s.add_geometry(
+ geometry=tmp_scene.dump(concatenate=True),
+ geom_name=first_geom_name,
+ parent_node_name=link_name,
+ transform=np.eye(4),
+ )
+
+ def _create_scene(
+ self,
+ use_collision_geometry=False,
+ load_geometry=True,
+ force_mesh=False,
+ force_single_geometry_per_link=False,
+ ):
+ s = trimesh.scene.Scene(base_frame=self._base_link)
+
+ for j in self.robot.joints:
+ matrix, _ = self._forward_kinematics_joint(j)
+
+ s.graph.update(frame_from=j.parent, frame_to=j.child, matrix=matrix)
+
+ for l in self.robot.links:
+ if l.name not in s.graph.nodes and l.name != s.graph.base_frame:
+ _logger.warning(f"{l.name} not connected via joints. Will add link to base frame.")
+ s.graph.update(frame_from=s.graph.base_frame, frame_to=l.name)
+
+ meshes = l.collisions if use_collision_geometry else l.visuals
+ self._add_geometries_to_scene(
+ s,
+ geometries=meshes,
+ link_name=l.name,
+ load_geometry=load_geometry,
+ force_mesh=force_mesh,
+ force_single_geometry=force_single_geometry_per_link,
+ skip_materials=use_collision_geometry,
+ )
+
+ return s
+
+ def _successors(self, node):
+ """
+ Get all nodes of the scene that succeeds a specified node.
+
+ Parameters
+ ------------
+ node : any
+ Hashable key in `scene.graph`
+
+ Returns
+ -----------
+ subnodes : set[str]
+ Set of nodes.
+ """
+ # get every node that is a successor to specified node
+ # this includes `node`
+ return self._scene.graph.transforms.successors(node)
+
+ def _create_subrobot(self, robot_name, root_link_name):
+ subrobot = Robot(name=robot_name)
+ subnodes = self._successors(node=root_link_name)
+
+ if len(subnodes) > 0:
+ for node in subnodes:
+ if node in self.link_map:
+ subrobot.links.append(copy.deepcopy(self.link_map[node]))
+ for joint_name, joint in self.joint_map.items():
+ if joint.parent in subnodes and joint.child in subnodes:
+ subrobot.joints.append(copy.deepcopy(self.joint_map[joint_name]))
+
+ return subrobot
+
+ def split_along_joints(self, joint_type="floating", **kwargs):
+ """Split URDF model along a particular joint type.
+ The result is a set of URDF models which together compose the original URDF.
+
+ Args:
+ joint_type (str, or list[str], optional): Type of joint to use for splitting. Defaults to "floating".
+ **kwargs: Arguments delegated to URDF constructor of new URDF models.
+
+ Returns:
+ list[(np.ndarray, yourdfpy.URDF)]: A list of tuples (np.ndarray, yourdfpy.URDF) whereas each homogeneous 4x4 matrix describes the root transformation of the respective URDF model w.r.t. the original URDF.
+ """
+ root_urdf = URDF(robot=copy.deepcopy(self.robot), build_scene_graph=False, load_meshes=False)
+ result = []
+
+ joint_types = joint_type if isinstance(joint_type, list) else [joint_type]
+
+ # find all relevant joints
+ joint_names = [j.name for j in self.robot.joints if j.type in joint_types]
+ for joint_name in joint_names:
+ root_link = self.link_map[self.joint_map[joint_name].child]
+ new_robot = self._create_subrobot(
+ robot_name=root_link.name,
+ root_link_name=root_link.name,
+ )
+
+ result.append(
+ (
+ self._scene.graph.get(root_link.name)[0],
+ URDF(robot=new_robot, **kwargs),
+ )
+ )
+
+ # remove links and joints from root robot
+ for j in new_robot.joints:
+ root_urdf.robot.joints.remove(root_urdf.joint_map[j.name])
+ for l in new_robot.links:
+ root_urdf.robot.links.remove(root_urdf.link_map[l.name])
+
+ # remove joint that connects root urdf to root_link
+ if root_link.name in [j.child for j in root_urdf.robot.joints]:
+ root_urdf.robot.joints.remove(
+ root_urdf.robot.joints[[j.child for j in root_urdf.robot.joints].index(root_link.name)]
+ )
+
+ result.insert(0, (np.eye(4), URDF(robot=root_urdf.robot, **kwargs)))
+
+ return result
+
+ def validate_filenames(self):
+ for l in self.robot.links:
+ meshes = [m.geometry.mesh for m in l.collisions + l.visuals if m.geometry.mesh is not None]
+ for m in meshes:
+ _logger.debug(m.filename, "-->", self._filename_handler(m.filename))
+ if not os.path.isfile(self._filename_handler(m.filename)):
+ return False
+ return True
+
+ def write_xml(self):
+ """Write URDF model to an XML element hierarchy.
+
+ Returns:
+ etree.ElementTree: XML data.
+ """
+ xml_element = self._write_robot(self.robot)
+ return etree.ElementTree(xml_element)
+
+ def write_xml_string(self, **kwargs):
+ """Write URDF model to a string.
+
+ Returns:
+ str: String of the xml representation of the URDF model.
+ """
+ xml_element = self.write_xml()
+ return etree.tostring(xml_element, xml_declaration=True, *kwargs)
+
+ def write_xml_file(self, fname):
+ """Write URDF model to an xml file.
+
+ Args:
+ fname (str): Filename of the file to be written. Usually ends in `.urdf`.
+ """
+ xml_element = self.write_xml()
+ xml_element.write(fname, xml_declaration=True, pretty_print=True)
+
+ def _parse_mimic(xml_element):
+ if xml_element is None:
+ return None
+
+ return Mimic(
+ joint=xml_element.get("joint"),
+ multiplier=_str2float(xml_element.get("multiplier", 1.0)),
+ offset=_str2float(xml_element.get("offset", 0.0)),
+ )
+
+ def _write_mimic(self, xml_parent, mimic):
+ etree.SubElement(
+ xml_parent,
+ "mimic",
+ attrib={
+ "joint": mimic.joint,
+ "multiplier": str(mimic.multiplier),
+ "offset": str(mimic.offset),
+ },
+ )
+
+ def _parse_safety_controller(xml_element):
+ if xml_element is None:
+ return None
+
+ return SafetyController(
+ soft_lower_limit=_str2float(xml_element.get("soft_lower_limit")),
+ soft_upper_limit=_str2float(xml_element.get("soft_upper_limit")),
+ k_position=_str2float(xml_element.get("k_position")),
+ k_velocity=_str2float(xml_element.get("k_velocity")),
+ )
+
+ def _write_safety_controller(self, xml_parent, safety_controller):
+ etree.SubElement(
+ xml_parent,
+ "safety_controller",
+ attrib={
+ "soft_lower_limit": str(safety_controller.soft_lower_limit),
+ "soft_upper_limit": str(safety_controller.soft_upper_limit),
+ "k_position": str(safety_controller.k_position),
+ "k_velocity": str(safety_controller.k_velocity),
+ },
+ )
+
+ def _parse_transmission_joint(xml_element):
+ if xml_element is None:
+ return None
+
+ transmission_joint = TransmissionJoint(name=xml_element.get("name"))
+
+ for h in xml_element.findall("hardware_interface"):
+ transmission_joint.hardware_interfaces.append(h.text)
+
+ return transmission_joint
+
+ def _write_transmission_joint(self, xml_parent, transmission_joint):
+ xml_element = etree.SubElement(
+ xml_parent,
+ "joint",
+ attrib={
+ "name": str(transmission_joint.name),
+ },
+ )
+ for h in transmission_joint.hardware_interfaces:
+ tmp = etree.SubElement(
+ xml_element,
+ "hardwareInterface",
+ )
+ tmp.text = h
+
+ def _parse_actuator(xml_element):
+ if xml_element is None:
+ return None
+
+ actuator = Actuator(name=xml_element.get("name"))
+ if xml_element.find("mechanicalReduction"):
+ actuator.mechanical_reduction = float(xml_element.find("mechanicalReduction").text)
+
+ for h in xml_element.findall("hardwareInterface"):
+ actuator.hardware_interfaces.append(h.text)
+
+ return actuator
+
+ def _write_actuator(self, xml_parent, actuator):
+ xml_element = etree.SubElement(
+ xml_parent,
+ "actuator",
+ attrib={
+ "name": str(actuator.name),
+ },
+ )
+ if actuator.mechanical_reduction is not None:
+ tmp = etree.SubElement("mechanicalReduction")
+ tmp.text = str(actuator.mechanical_reduction)
+
+ for h in actuator.hardware_interfaces:
+ tmp = etree.SubElement(
+ xml_element,
+ "hardwareInterface",
+ )
+ tmp.text = h
+
+ def _parse_transmission(xml_element):
+ if xml_element is None:
+ return None
+
+ transmission = Transmission(name=xml_element.get("name"))
+
+ for j in xml_element.findall("joint"):
+ transmission.joints.append(URDF._parse_transmission_joint(j))
+ for a in xml_element.findall("actuator"):
+ transmission.actuators.append(URDF._parse_actuator(a))
+
+ return transmission
+
+ def _write_transmission(self, xml_parent, transmission):
+ xml_element = etree.SubElement(
+ xml_parent,
+ "transmission",
+ attrib={
+ "name": str(transmission.name),
+ },
+ )
+
+ for j in transmission.joints:
+ self._write_transmission_joint(xml_element, j)
+
+ for a in transmission.actuators:
+ self._write_actuator(xml_element, a)
+
+ def _parse_calibration(xml_element):
+ if xml_element is None:
+ return None
+
+ return Calibration(
+ rising=_str2float(xml_element.get("rising")),
+ falling=_str2float(xml_element.get("falling")),
+ )
+
+ def _write_calibration(self, xml_parent, calibration):
+ etree.SubElement(
+ xml_parent,
+ "calibration",
+ attrib={
+ "rising": str(calibration.rising),
+ "falling": str(calibration.falling),
+ },
+ )
+
+ def _parse_box(xml_element):
+ return Box(size=np.array(xml_element.attrib["size"].split(), dtype=float))
+
+ def _write_box(self, xml_parent, box):
+ etree.SubElement(xml_parent, "box", attrib={"size": " ".join(map(str, box.size))})
+
+ def _parse_cylinder(xml_element):
+ return Cylinder(
+ radius=float(xml_element.attrib["radius"]),
+ length=float(xml_element.attrib["length"]),
+ )
+
+ def _write_cylinder(self, xml_parent, cylinder):
+ etree.SubElement(
+ xml_parent,
+ "cylinder",
+ attrib={"radius": str(cylinder.radius), "length": str(cylinder.length)},
+ )
+
+ def _parse_sphere(xml_element):
+ return Sphere(radius=float(xml_element.attrib["radius"]))
+
+ def _write_sphere(self, xml_parent, sphere):
+ etree.SubElement(xml_parent, "sphere", attrib={"radius": str(sphere.radius)})
+
+ def _parse_scale(xml_element):
+ if "scale" in xml_element.attrib:
+ s = xml_element.get("scale").split()
+ if len(s) == 0:
+ return None
+ elif len(s) == 1:
+ return float(s[0])
+ else:
+ return np.array(list(map(float, s)))
+ return None
+
+ def _write_scale(self, xml_parent, scale):
+ if scale is not None:
+ if isinstance(scale, float) or isinstance(scale, int):
+ xml_parent.set("scale", " ".join([str(scale)] * 3))
+ else:
+ xml_parent.set("scale", " ".join(map(str, scale)))
+
+ def _parse_mesh(xml_element):
+ return Mesh(filename=xml_element.get("filename"), scale=URDF._parse_scale(xml_element))
+
+ def _write_mesh(self, xml_parent, mesh):
+ # TODO: turn into different filename handler
+ xml_element = etree.SubElement(
+ xml_parent,
+ "mesh",
+ attrib={"filename": self._filename_handler(mesh.filename)},
+ )
+
+ self._write_scale(xml_element, mesh.scale)
+
+ def _parse_geometry(xml_element):
+ geometry = Geometry()
+ if xml_element[0].tag == "box":
+ geometry.box = URDF._parse_box(xml_element[0])
+ elif xml_element[0].tag == "cylinder":
+ geometry.cylinder = URDF._parse_cylinder(xml_element[0])
+ elif xml_element[0].tag == "sphere":
+ geometry.sphere = URDF._parse_sphere(xml_element[0])
+ elif xml_element[0].tag == "mesh":
+ geometry.mesh = URDF._parse_mesh(xml_element[0])
+ else:
+ raise ValueError(f"Unknown tag: {xml_element[0].tag}")
+
+ return geometry
+
+ def _validate_geometry(self, geometry):
+ if geometry is None:
+ self._errors.append(URDFIncompleteError(" is missing."))
+
+ num_nones = sum(
+ [
+ x is not None
+ for x in [
+ geometry.box,
+ geometry.cylinder,
+ geometry.sphere,
+ geometry.mesh,
+ ]
+ ]
+ )
+ if num_nones < 1:
+ self._errors.append(
+ URDFIncompleteError(
+ "One of , , , needs to be defined as a child of ."
+ )
+ )
+ elif num_nones > 1:
+ self._errors.append(
+ URDFError(
+ "Too many of , , , defined as a child of . Only one allowed."
+ )
+ )
+
+ def _write_geometry(self, xml_parent, geometry):
+ if geometry is None:
+ return
+
+ xml_element = etree.SubElement(xml_parent, "geometry")
+ if geometry.box is not None:
+ self._write_box(xml_element, geometry.box)
+ elif geometry.cylinder is not None:
+ self._write_cylinder(xml_element, geometry.cylinder)
+ elif geometry.sphere is not None:
+ self._write_sphere(xml_element, geometry.sphere)
+ elif geometry.mesh is not None:
+ self._write_mesh(xml_element, geometry.mesh)
+
+ def _parse_origin(xml_element):
+ if xml_element is None:
+ return None
+
+ xyz = xml_element.get("xyz", default="0 0 0")
+ rpy = xml_element.get("rpy", default="0 0 0")
+
+ return tra.compose_matrix(
+ translate=np.array(list(map(float, xyz.split()))),
+ angles=np.array(list(map(float, rpy.split()))),
+ )
+
+ def _write_origin(self, xml_parent, origin):
+ if origin is None:
+ return
+
+ etree.SubElement(
+ xml_parent,
+ "origin",
+ attrib={
+ "xyz": " ".join(map(str, tra.translation_from_matrix(origin))),
+ "rpy": " ".join(map(str, tra.euler_from_matrix(origin))),
+ },
+ )
+
+ def _parse_color(xml_element):
+ if xml_element is None:
+ return None
+
+ rgba = xml_element.get("rgba", default="1 1 1 1")
+
+ return Color(rgba=np.array(list(map(float, rgba.split()))))
+
+ def _write_color(self, xml_parent, color):
+ if color is None:
+ return
+
+ etree.SubElement(xml_parent, "color", attrib={"rgba": " ".join(map(str, color.rgba))})
+
+ def _parse_texture(xml_element):
+ if xml_element is None:
+ return None
+
+ # TODO: use texture filename handler
+ return Texture(filename=xml_element.get("filename", default=None))
+
+ def _write_texture(self, xml_parent, texture):
+ if texture is None:
+ return
+
+ # TODO: use texture filename handler
+ etree.SubElement(xml_parent, "texture", attrib={"filename": texture.filename})
+
+ def _parse_material(xml_element):
+ if xml_element is None:
+ return None
+
+ material = Material(name=xml_element.get("name"))
+ material.color = URDF._parse_color(xml_element.find("color"))
+ material.texture = URDF._parse_texture(xml_element.find("texture"))
+
+ return material
+
+ def _write_material(self, xml_parent, material):
+ if material is None:
+ return
+
+ attrib = {"name": material.name} if material.name is not None else {}
+ xml_element = etree.SubElement(
+ xml_parent,
+ "material",
+ attrib=attrib,
+ )
+
+ self._write_color(xml_element, material.color)
+ self._write_texture(xml_element, material.texture)
+
+ def _parse_visual(xml_element):
+ visual = Visual(name=xml_element.get("name"))
+
+ visual.geometry = URDF._parse_geometry(xml_element.find("geometry"))
+ visual.origin = URDF._parse_origin(xml_element.find("origin"))
+ visual.material = URDF._parse_material(xml_element.find("material"))
+
+ return visual
+
+ def _validate_visual(self, visual):
+ self._validate_geometry(visual.geometry)
+
+ def _write_visual(self, xml_parent, visual):
+ attrib = {"name": visual.name} if visual.name is not None else {}
+ xml_element = etree.SubElement(
+ xml_parent,
+ "visual",
+ attrib=attrib,
+ )
+
+ self._write_geometry(xml_element, visual.geometry)
+ self._write_origin(xml_element, visual.origin)
+ self._write_material(xml_element, visual.material)
+
+ def _parse_collision(xml_element):
+ collision = Collision(name=xml_element.get("name"))
+
+ collision.geometry = URDF._parse_geometry(xml_element.find("geometry"))
+ collision.origin = URDF._parse_origin(xml_element.find("origin"))
+
+ return collision
+
+ def _validate_collision(self, collision):
+ self._validate_geometry(collision.geometry)
+
+ def _write_collision(self, xml_parent, collision):
+ attrib = {"name": collision.name} if collision.name is not None else {}
+ xml_element = etree.SubElement(
+ xml_parent,
+ "collision",
+ attrib=attrib,
+ )
+
+ self._write_geometry(xml_element, collision.geometry)
+ self._write_origin(xml_element, collision.origin)
+
+ def _parse_inertia(xml_element):
+ if xml_element is None:
+ return None
+
+ x = xml_element
+
+ return np.array(
+ [
+ [
+ x.get("ixx", default=1.0),
+ x.get("ixy", default=0.0),
+ x.get("ixz", default=0.0),
+ ],
+ [
+ x.get("ixy", default=0.0),
+ x.get("iyy", default=1.0),
+ x.get("iyz", default=0.0),
+ ],
+ [
+ x.get("ixz", default=0.0),
+ x.get("iyz", default=0.0),
+ x.get("izz", default=1.0),
+ ],
+ ],
+ dtype=np.float64,
+ )
+
+ def _write_inertia(self, xml_parent, inertia):
+ if inertia is None:
+ return None
+
+ etree.SubElement(
+ xml_parent,
+ "inertia",
+ attrib={
+ "ixx": str(inertia[0, 0]),
+ "ixy": str(inertia[0, 1]),
+ "ixz": str(inertia[0, 2]),
+ "iyy": str(inertia[1, 1]),
+ "iyz": str(inertia[1, 2]),
+ "izz": str(inertia[2, 2]),
+ },
+ )
+
+ def _parse_mass(xml_element):
+ if xml_element is None:
+ return None
+
+ return _str2float(xml_element.get("value", default=0.0))
+
+ def _write_mass(self, xml_parent, mass):
+ if mass is None:
+ return
+
+ etree.SubElement(
+ xml_parent,
+ "mass",
+ attrib={
+ "value": str(mass),
+ },
+ )
+
+ def _parse_inertial(xml_element):
+ if xml_element is None:
+ return None
+
+ inertial = Inertial()
+ inertial.origin = URDF._parse_origin(xml_element.find("origin"))
+ inertial.inertia = URDF._parse_inertia(xml_element.find("inertia"))
+ inertial.mass = URDF._parse_mass(xml_element.find("mass"))
+
+ return inertial
+
+ def _write_inertial(self, xml_parent, inertial):
+ if inertial is None:
+ return
+
+ xml_element = etree.SubElement(xml_parent, "inertial")
+
+ self._write_origin(xml_element, inertial.origin)
+ self._write_mass(xml_element, inertial.mass)
+ self._write_inertia(xml_element, inertial.inertia)
+
+ def _parse_link(xml_element):
+ link = Link(name=xml_element.attrib["name"])
+
+ link.inertial = URDF._parse_inertial(xml_element.find("inertial"))
+
+ for v in xml_element.findall("visual"):
+ link.visuals.append(URDF._parse_visual(v))
+
+ for c in xml_element.findall("collision"):
+ link.collisions.append(URDF._parse_collision(c))
+
+ return link
+
+ def _validate_link(self, link):
+ self._validate_required_attribute(attribute=link.name, error_msg="The tag misses a 'name' attribute.")
+
+ for v in link.visuals:
+ self._validate_visual(v)
+
+ for c in link.collisions:
+ self._validate_collision(c)
+
+ def _write_link(self, xml_parent, link):
+ xml_element = etree.SubElement(
+ xml_parent,
+ "link",
+ attrib={
+ "name": link.name,
+ },
+ )
+
+ self._write_inertial(xml_element, link.inertial)
+ for visual in link.visuals:
+ self._write_visual(xml_element, visual)
+ for collision in link.collisions:
+ self._write_collision(xml_element, collision)
+
+ def _parse_axis(xml_element):
+ if xml_element is None:
+ return np.array([1.0, 0, 0])
+
+ xyz = xml_element.get("xyz", "1 0 0")
+ results = []
+ for x in xyz.split():
+ try:
+ x = float(x)
+ except ValueError:
+ x = 0
+ results.append(x)
+ return np.array(results)
+ # return np.array(list(map(float, xyz.split())))
+
+ def _write_axis(self, xml_parent, axis):
+ if axis is None:
+ return
+
+ etree.SubElement(xml_parent, "axis", attrib={"xyz": " ".join(map(str, axis))})
+
+ def _parse_limit(xml_element):
+ if xml_element is None:
+ return None
+
+ return Limit(
+ effort=_str2float(xml_element.get("effort", default=None)),
+ velocity=_str2float(xml_element.get("velocity", default=None)),
+ lower=_str2float(xml_element.get("lower", default=None)),
+ upper=_str2float(xml_element.get("upper", default=None)),
+ )
+
+ def _validate_limit(self, limit, type):
+ if type in ["revolute", "prismatic"]:
+ self._validate_required_attribute(
+ limit,
+ error_msg="The of a (prismatic, revolute) joint is missing.",
+ )
+
+ if limit is not None:
+ self._validate_required_attribute(
+ limit.upper,
+ error_msg="Tag of joint is missing attribute 'upper'.",
+ )
+ self._validate_required_attribute(
+ limit.lower,
+ error_msg="Tag of joint is missing attribute 'lower'.",
+ )
+
+ if limit is not None:
+ self._validate_required_attribute(
+ limit.effort,
+ error_msg="Tag of joint is missing attribute 'effort'.",
+ )
+
+ self._validate_required_attribute(
+ limit.velocity,
+ error_msg="Tag of joint is missing attribute 'velocity'.",
+ )
+
+ def _write_limit(self, xml_parent, limit):
+ if limit is None:
+ return
+
+ attrib = {}
+ if limit.effort is not None:
+ attrib["effort"] = str(limit.effort)
+ if limit.velocity is not None:
+ attrib["velocity"] = str(limit.velocity)
+ if limit.lower is not None:
+ attrib["lower"] = str(limit.lower)
+ if limit.upper is not None:
+ attrib["upper"] = str(limit.upper)
+
+ etree.SubElement(
+ xml_parent,
+ "limit",
+ attrib=attrib,
+ )
+
+ def _parse_dynamics(xml_element):
+ if xml_element is None:
+ return None
+
+ dynamics = Dynamics()
+ dynamics.damping = xml_element.get("damping", default=None)
+ dynamics.friction = xml_element.get("friction", default=None)
+
+ return dynamics
+
+ def _write_dynamics(self, xml_parent, dynamics):
+ if dynamics is None:
+ return
+
+ attrib = {}
+ if dynamics.damping is not None:
+ attrib["damping"] = str(dynamics.damping)
+ if dynamics.friction is not None:
+ attrib["friction"] = str(dynamics.friction)
+
+ etree.SubElement(
+ xml_parent,
+ "dynamics",
+ attrib=attrib,
+ )
+
+ def _parse_joint(xml_element):
+ joint = Joint(name=xml_element.attrib["name"])
+
+ joint.type = xml_element.get("type", default=None)
+ joint.parent = xml_element.find("parent").get("link")
+ joint.child = xml_element.find("child").get("link")
+ joint.origin = URDF._parse_origin(xml_element.find("origin"))
+ joint.axis = URDF._parse_axis(xml_element.find("axis"))
+ joint.limit = URDF._parse_limit(xml_element.find("limit"))
+ joint.dynamics = URDF._parse_dynamics(xml_element.find("dynamics"))
+ joint.mimic = URDF._parse_mimic(xml_element.find("mimic"))
+ joint.calibration = URDF._parse_calibration(xml_element.find("calibration"))
+ joint.safety_controller = URDF._parse_safety_controller(xml_element.find("safety_controller"))
+
+ return joint
+
+ def _validate_joint(self, joint):
+ self._validate_required_attribute(
+ attribute=joint.name,
+ error_msg="The tag misses a 'name' attribute.",
+ )
+
+ allowed_types = [
+ "revolute",
+ "continuous",
+ "prismatic",
+ "fixed",
+ "floating",
+ "planar",
+ ]
+ self._validate_required_attribute(
+ attribute=joint.type,
+ error_msg=f"The tag misses a 'type' attribute or value is not part of allowed values [{', '.join(allowed_types)}].",
+ allowed_values=allowed_types,
+ )
+
+ self._validate_required_attribute(
+ joint.parent,
+ error_msg=f"The of a is missing.",
+ )
+
+ self._validate_required_attribute(
+ joint.child,
+ error_msg=f"The of a is missing.",
+ )
+
+ self._validate_limit(joint.limit, type=joint.type)
+
+ def _write_joint(self, xml_parent, joint):
+ xml_element = etree.SubElement(
+ xml_parent,
+ "joint",
+ attrib={
+ "name": joint.name,
+ "type": joint.type,
+ },
+ )
+
+ etree.SubElement(xml_element, "parent", attrib={"link": joint.parent})
+ etree.SubElement(xml_element, "child", attrib={"link": joint.child})
+ self._write_origin(xml_element, joint.origin)
+ self._write_axis(xml_element, joint.axis)
+ self._write_limit(xml_element, joint.limit)
+ self._write_dynamics(xml_element, joint.dynamics)
+
+ @staticmethod
+ def _parse_robot(xml_element, add_dummy_free_joints=False):
+ robot = Robot(name=xml_element.attrib["name"])
+
+ for l in xml_element.findall("link"):
+ robot.links.append(URDF._parse_link(l))
+ for j in xml_element.findall("joint"):
+ robot.joints.append(URDF._parse_joint(j))
+ for m in xml_element.findall("material"):
+ robot.materials.append(URDF._parse_material(m))
+
+ if add_dummy_free_joints:
+ # Determine root link
+ link_names = [l.name for l in robot.links]
+ for j in robot.joints:
+ link_names.remove(j.child)
+
+ if len(link_names) == 0:
+ raise RuntimeError(f"No root link found for robot.")
+
+ root_link_name = link_names[0]
+ _add_dummy_joints(robot, root_link_name)
+
+ return robot
+
+ def _validate_robot(self, robot):
+ if robot is not None:
+ self._validate_required_attribute(
+ attribute=robot.name,
+ error_msg="The tag misses a 'name' attribute.",
+ )
+
+ for l in robot.links:
+ self._validate_link(l)
+
+ for j in robot.joints:
+ self._validate_joint(j)
+
+ def _write_robot(self, robot):
+ xml_element = etree.Element("robot", attrib={"name": robot.name})
+ for link in robot.links:
+ self._write_link(xml_element, link)
+ for joint in robot.joints:
+ self._write_joint(xml_element, joint)
+ for material in robot.materials:
+ self._write_material(xml_element, material)
+
+ return xml_element
+
+ def __eq__(self, other):
+ if not isinstance(other, URDF):
+ raise NotImplemented
+ return self.robot == other.robot
+
+ @property
+ def filename_handler(self):
+ return self._filename_handler
+
+ def build_tree(self):
+ parent_child_map: Dict[str, List[str]] = {}
+ for joint in self.robot.joints:
+ if joint.parent in parent_child_map:
+ parent_child_map[joint.parent].append(joint.child)
+ else:
+ parent_child_map[joint.parent] = [joint.child]
+
+ # Sort link with bfs order
+ bfs_link_list = [self.base_link]
+ to_be_handle_list = [self.base_link]
+ while len(to_be_handle_list) > 0:
+ parent = to_be_handle_list.pop(0)
+ if parent not in parent_child_map:
+ continue
+
+ children = parent_child_map[parent]
+ to_be_handle_list.extend(children)
+ bfs_link_list.extend(children)
+ bfs_joint_list = []
+ for link_name in bfs_link_list[1:]:
+ joint_index = [i for i in range(len(self.robot.joints)) if self.robot.joints[i].child == link_name][0]
+ bfs_joint_list.append(self.robot.joints[joint_index])
+
+ # Build tree
+ root = Node(self.base_link, matrix=np.eye(4))
+ for joint in bfs_joint_list:
+ matrix, _ = self._forward_kinematics_joint(joint, 0)
+ parent_node = anytree.search.findall_by_attr(root, value=joint.parent)[0]
+ node = Node(joint.child, parent=parent_node, matrix=matrix)
+ return root
+
+ def update_kinematics(self, configuration):
+ joint_cfg = []
+
+ if isinstance(configuration, dict):
+ for joint in configuration:
+ if isinstance(joint, six.string_types):
+ joint_cfg.append((self._joint_map[joint], configuration[joint]))
+ elif isinstance(joint, Joint):
+ # TODO: Joint is not hashable; so this branch will not succeed
+ joint_cfg.append((joint, configuration[joint]))
+ elif isinstance(configuration, (list, tuple, np.ndarray)):
+ if len(configuration) == len(self.robot.joints):
+ for joint, value in zip(self.robot.joints, configuration):
+ joint_cfg.append((joint, value))
+ elif len(configuration) == self.num_actuated_joints:
+ for joint, value in zip(self._actuated_joints, configuration):
+ joint_cfg.append((joint, value))
+ else:
+ raise ValueError(
+ f"Dimensionality of configuration ({len(configuration)}) doesn't match number of all ({len(self.robot.joints)}) or actuated joints ({self.num_actuated_joints})."
+ )
+ else:
+ raise TypeError("Invalid type for configuration")
+
+ # append all mimic joints in the update
+ for j, q in joint_cfg + [(j, 0.0) for j in self.robot.joints if j.mimic is not None]:
+ matrix, _ = self._forward_kinematics_joint(j, q=q)
+ node = anytree.search.findall_by_attr(self.tree_root, j.child)[0]
+ node.matrix = matrix
+
+ for node in LevelOrderIter(self.tree_root):
+ if node.name == self.base_link:
+ node.global_pose = np.eye(4)
+ else:
+ node.global_pose = node.parent.global_pose @ node.matrix
+
+ def get_link_global_transform(self, link_name):
+ node = anytree.search.findall_by_attr(self.tree_root, link_name)[0]
+
+ return node.global_pose
+
+
+def _add_dummy_joints(robot: Robot, root_link_name: str):
+ # Prepare link and joint properties
+ translation_range = (-5, 5)
+ rotation_range = (-2 * np.pi, 2 * np.pi)
+ joint_types = ["prismatic"] * 3 + ["revolute"] * 3
+ joint_limit = [translation_range] * 3 + [rotation_range] * 3
+ joint_name = DUMMY_JOINT_NAMES.copy()
+ link_name = [f"dummy_{name}_translation_link" for name in "xyz"] + [f"dummy_{name}_rotation_link" for name in "xyz"]
+
+ links = []
+ joints = []
+
+ for i in range(6):
+ inertial = Inertial(
+ mass=0.01, inertia=np.array([[1e-4, 0, 0], [0, 1e-4, 0], [0, 0, 1e-4]]), origin=np.identity(4)
+ )
+ link = Link(name=link_name[i], inertial=inertial)
+ links.append(link)
+
+ joint_axis = np.zeros(3, dtype=int)
+ joint_axis[i % 3] = 1
+ limit = Limit(lower=joint_limit[i][0], upper=joint_limit[i][1], velocity=3.14, effort=10)
+
+ child_name = link_name[i + 1] if i < 5 else root_link_name
+ joint = Joint(
+ name=joint_name[i],
+ type=joint_types[i],
+ parent=link_name[i],
+ child=child_name,
+ origin=np.identity(4),
+ axis=joint_axis,
+ limit=limit,
+ )
+ joints.append(joint)
+
+ robot.joints = joints + robot.joints
+ robot.links = links + robot.links
+
+
+DUMMY_JOINT_NAMES = [f"dummy_{name}_translation_joint" for name in "xyz"] + [
+ f"dummy_{name}_rotation_joint" for name in "xyz"
+]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/__init__.py b/src/linkerhand_retarget/linkerhand_retarget/motion/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkereg/README.md b/src/linkerhand_retarget/linkerhand_retarget/motion/linkereg/README.md
new file mode 100644
index 0000000..3f05225
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkereg/README.md
@@ -0,0 +1,249 @@
+# LinkerEG Teleoperation Glove Module
+
+LinkerEG teleoperation glove ROS2 driver module, receives glove data via serial port and publishes to ROS2 topics.
+
+## Changelog
+
+### 2026-02-04
+- Fixed dead loop bug in initialization
+- Added support for G20 hand
+
+## Features
+
+- **Publish rate**: 50Hz (all topics unified)
+- Auto scan serial port for glove connection
+- Supports left and right hand data
+- Supports sensor raw data publishing (switchable via topic)
+
+## Control Modes
+
+| motion_type | Mode | Description | Receiver needs robot hand connection |
+|-------------|------|-------------|-------------------------------------|
+| `linkereg2` | SDK Control Mode | Glove data publishes to ROS topics directly, SDK controls robot hand | ❌ Not required |
+| `linkereg1` | Receiver Control Mode | Receiver controls robot hand directly, also publishes data to ROS topics | ✅ Required |
+
+## Usage
+
+### 1. Config File
+
+Edit `config/base_config.yml`:
+
+```yaml
+system:
+ # SDK control mode (receiver doesn't need robot hand connection)
+ motion_type: linkereg2
+
+ # Or receiver control mode (receiver must be connected to robot hand)
+ motion_type: linkereg1
+```
+
+#### Serial Port Permission Password
+
+If auto-fixing serial port permissions is needed, configure `password` under `linkereg` (sudo password).
+Note: Values without quotes in YAML are parsed as numbers, use quotes for strings.
+
+```yaml
+linkereg:
+ port: null
+ password: "123456" # sudo password for fixing serial permissions
+```
+
+### 2. Launch
+
+```bash
+ros2 run linkerhand_retarget handretarget
+```
+
+### 3. Enable Debug Print
+
+Edit `config/base_config.yml`:
+
+```yaml
+debug:
+ joint_pub_debug: true
+```
+
+## ROS2 Topics
+
+> **Publish rate**: All topics unified at 50Hz
+
+### Published Topics
+
+| Topic | Message Type | Description | Default State |
+|-------|-------------|-------------|---------------|
+| `/cb_right_hand_control_cmd` | `sensor_msgs/JointState` | Right hand control data | ✅ Enabled |
+| `/cb_left_hand_control_cmd` | `sensor_msgs/JointState` | Left hand control data | ✅ Enabled |
+| `/cb_right_hand_raw_data` | `sensor_msgs/JointState` | Right hand sensor raw data | ❌ Disabled |
+| `/cb_left_hand_raw_data` | `sensor_msgs/JointState` | Left hand sensor raw data | ❌ Disabled |
+
+### Subscribed Topics
+
+| Topic | Message Type | Description |
+|-------|-------------|-------------|
+| `/cb_hand_setting_cmd` | `std_msgs/String` | Settings command (control raw data on/off) |
+
+## Raw Data Feature
+
+Raw data is **disabled** by default, enable via topic command.
+
+### Enable/Disable Raw Data
+
+```bash
+# Enable raw data publishing
+ros2 topic pub --once /cb_hand_setting_cmd std_msgs/String "data: 'on'"
+
+# Disable raw data publishing
+ros2 topic pub --once /cb_hand_setting_cmd std_msgs/String "data: 'off'"
+
+# Listen to raw data
+ros2 topic echo /cb_left_hand_raw_data
+```
+
+## ROS2 Topic Data Format
+
+### Raw Data Format (15 int32 values)
+
+| Index | Joint Name | Index | Joint Name | Index | Joint Name |
+|-------|------------|-------|------------|-------|------------|
+| 0 | Thumb abduction | 1 | Thumb flexion | 2 | Thumb tip |
+| 3 | Index abduction | 4 | Index flexion | 5 | Index tip |
+| 6 | Middle abduction | 7 | Middle flexion | 8 | Middle tip |
+| 9 | Ring abduction | 10 | Ring flexion | 11 | Ring tip |
+| 12 | Pinky abduction | 13 | Pinky flexion | 14 | Pinky tip |
+
+### Control Data Format
+
+#### L21 (25 joint output)
+
+For robot hand models: L21
+
+| Joint | Joint Name | Description |
+|-------|------------|-------------|
+| joint1 | Thumb flexion | |
+| joint2 | Index flexion | |
+| joint3 | Middle flexion | |
+| joint4 | Ring flexion | |
+| joint5 | Pinky flexion | |
+| joint6 | Thumb abduction | |
+| joint7 | Index abduction | |
+| joint8 | Middle abduction | |
+| joint9 | Ring abduction | |
+| joint10 | Pinky abduction | |
+| joint11 | Thumb roll | |
+| joint12 | Reserved | value 0 |
+| joint13 | Reserved | value 0 |
+| joint14 | Reserved | value 0 |
+| joint15 | Reserved | value 0 |
+| joint16 | Thumb middle | value 0 |
+| joint17 | Reserved | value 0 |
+| joint18 | Reserved | value 0 |
+| joint19 | Reserved | value 0 |
+| joint20 | Reserved | value 0 |
+| joint21 | Thumb tip | |
+| joint22 | Index tip | |
+| joint23 | Middle tip | |
+| joint24 | Ring tip | |
+| joint25 | Pinky tip | |
+
+#### L20/G20 (20 joint output)
+
+For robot hand models: L20, Industrial 20, G20
+
+| Joint | Joint Name | Description |
+|-------|------------|-------------|
+| joint1 | Thumb flexion | |
+| joint2 | Index flexion | |
+| joint3 | Middle flexion | |
+| joint4 | Ring flexion | |
+| joint5 | Pinky flexion | |
+| joint6 | Thumb abduction | |
+| joint7 | Index abduction | |
+| joint8 | Middle abduction | |
+| joint9 | Ring abduction | |
+| joint10 | Pinky abduction | |
+| joint11 | Thumb roll | |
+| joint12 | Reserved | value 0 |
+| joint13 | Reserved | value 0 |
+| joint14 | Reserved | value 0 |
+| joint15 | Reserved | value 0 |
+| joint16 | Thumb tip | |
+| joint17 | Index tip | |
+| joint18 | Middle tip | |
+| joint19 | Ring tip | |
+| joint20 | Pinky tip | |
+
+#### L10 (10 joints)
+
+For robot hand models: L10
+
+| Joint | Joint Name |
+|-------|------------|
+| joint1 | Thumb flexion |
+| joint2 | Thumb abduction |
+| joint3 | Index flexion |
+| joint4 | Middle flexion |
+| joint5 | Ring flexion |
+| joint6 | Pinky flexion |
+| joint7 | Index abduction |
+| joint8 | Ring abduction |
+| joint9 | Pinky abduction |
+| joint10 | Thumb roll |
+
+#### L6 (6 joints)
+
+For robot hand models: L6, O6
+
+| Joint | Joint Name |
+|-------|------------|
+| joint1 | Thumb flexion |
+| joint2 | Thumb abduction |
+| joint3 | Index flexion |
+| joint4 | Middle flexion |
+| joint5 | Ring flexion |
+| joint6 | Pinky flexion |
+
+#### O7 (7 joints)
+
+For robot hand models: O7
+
+| Joint | Joint Name |
+|-------|------------|
+| joint1 | Thumb flexion |
+| joint2 | Thumb abduction |
+| joint3 | Index flexion |
+| joint4 | Middle flexion |
+| joint5 | Ring flexion |
+| joint6 | Pinky flexion |
+| joint7 | Thumb roll |
+
+### Message Format
+
+```python
+# sensor_msgs/JointState
+msg.header.stamp # Timestamp
+msg.name # ['joint1', 'joint2', ..., 'jointN']
+msg.position # [0-255, ...] Motor position values
+msg.velocity # [255, ...] Velocity values
+```
+
+## File Structure
+
+```
+motion/linkereg/
+├── __init__.py # Module export
+├── linkeregcore.py # Serial communication core
+├── retarget.py # ROS2 integration
+└── README.md # English documentation
+```
+
+## Notes
+
+1. **Robot hand model**: No need to specify in config file, glove automatically identifies protocol type in data frame
+
+2. **Hardware connection**: Receiver must be connected to host (where the program runs)
+
+3. **Data rate**: Glove pushes data at 50Hz
+
+4. **Mode selection**:
+ - `linkereg2` (SDK control mode): Receiver doesn't need robot hand connection, glove data publishes to ROS topics, upper-layer robot hand SDK subscribes to control robot hand
+ - `linkereg1` (Receiver control mode): Receiver must be connected to real robot hand, receiver directly controls robot hand motion, current SDK is only for collecting glove data.
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkereg/README_zh.md b/src/linkerhand_retarget/linkerhand_retarget/motion/linkereg/README_zh.md
new file mode 100644
index 0000000..97ada4d
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkereg/README_zh.md
@@ -0,0 +1,248 @@
+# LinkerEG 遥操作手套模块
+
+LinkerEG 遥操作手套的 ROS2 驱动模块，通过串口接收手套数据并发布到 ROS2 话题。
+
+## 更新日志
+
+### 2026-02-04
+- 修复了初始化流程中的死循环 bug
+- 增加了对 G20 手的支持
+
+## 特性
+
+- **发布频率**: 50Hz (所有话题统一频率)
+- 自动扫描串口连接手套
+- 支持左右手数据
+- 支持传感器原始数据推送 (可通过话题开关)
+
+## 控制模式
+
+| motion_type | 模式 | 说明 |接收器是否需要连接灵巧手 |
+|-------------|------|------|-------------------|
+| `linkereg2` | SDK控制模式 | 手套数据直接发布到ROS话题，由SDK控制灵巧手 | ❌ 不需要 |
+| `linkereg1` | 接收器控制模式 | 接收器直接控制灵巧手，同时发布数据到ROS话题 | ✅ 必须连接 |
+
+## 使用方法
+
+### 1. 配置文件
+
+修改 `config/base_config.yml`:
+
+```yaml
+system:
+ # SDK控制模式 (接收器不需要连接灵巧手)
+ motion_type: linkereg2
+
+ #或 接收器控制模式 (接收器必须连接灵巧手)
+ motion_type: linkereg1
+```
+
+#### 串口权限密码
+
+如果需要自动修复串口权限，请在 `linkereg` 下配置 `password`（sudo 密码）。
+注意：YAML 里不加引号会被解析成数字，建议加引号以确保是字符串。
+
+```yaml
+linkereg:
+ port: null
+ password: "123456" # sudo密码，用于修复串口权限，如果密码全是数字 需要加引号，比如 "123456"
+```
+
+### 2. 运行
+
+```bash
+ros2 run linkerhand_retarget handretarget
+```
+
+### 3. 启用调试打印
+
+修改 `config/base_config.yml`:
+
+```yaml
+debug:
+ joint_pub_debug: true
+```
+
+## ROS2 话题
+
+> **发布频率**: 所有话题统一 50Hz
+
+### 发布话题
+
+| 话题名 | 消息类型 | 说明 | 默认状态 |
+|-------|---------|------|---------|
+| `/cb_right_hand_control_cmd` | `sensor_msgs/JointState` | 右手控制数据 | ✅ 启用 |
+| `/cb_left_hand_control_cmd` | `sensor_msgs/JointState` | 左手控制数据 | ✅ 启用 |
+| `/cb_right_hand_raw_data` | `sensor_msgs/JointState` | 右手传感器原始数据 | ❌ 禁用 |
+| `/cb_left_hand_raw_data` | `sensor_msgs/JointState` | 左手传感器原始数据 | ❌ 禁用 |
+
+### 订阅话题
+
+| 话题名 | 消息类型 | 说明 |
+|-------|---------|------|
+| `/cb_hand_setting_cmd` | `std_msgs/String` | 设置命令 (控制原始数据开关) |
+
+## 原始数据功能
+
+原始数据默认**禁用**，需要通过话题命令启用。
+
+### 启用/禁用原始数据
+
+```bash
+# 启用原始数据推送
+ros2 topic pub --once /cb_hand_setting_cmd std_msgs/String "data: 'on'"
+
+# 禁用原始数据推送
+ros2 topic pub --once /cb_hand_setting_cmd std_msgs/String "data: 'off'"
+
+# 监听原始数据
+ros2 topic echo /cb_left_hand_raw_data
+```
+
+## ROS2 话题数据格式
+
+### 原始数据格式 (15个int32值)
+
+| 索引 | 关节名称 | 索引 | 关节名称 | 索引 | 关节名称 |
+|------|----------|------|----------|------|----------|
+| 0 | 大拇指横摆 | 1 | 大拇指弯曲 | 2 | 大拇指指尖 |
+| 3 | 食指横摆 | 4 | 食指弯曲 | 5 | 食指指尖 |
+| 6 | 中指横摆 | 7 | 中指弯曲 | 8 | 中指指尖 |
+| 9 | 无名指横摆 | 10 | 无名指弯曲 | 11 | 无名指指尖 |
+| 12 | 小指横摆 | 13 | 小指弯曲 | 14 | 小指指尖 |
+
+### 控制数据格式
+#### L21 (25关节输出)
+
+适用灵巧手型号：L21
+
+| Joint | 关节名称 | 说明 |
+|-------|----------|------|
+| joint1 | 大拇指弯曲 | |
+| joint2 | 食指弯曲 | |
+| joint3 | 中指弯曲 | |
+| joint4 | 无名指弯曲 | |
+| joint5 | 小拇指弯曲 | |
+| joint6 | 大拇指横摆 | |
+| joint7 | 食指横摆 | |
+| joint8 | 中指横摆 | |
+| joint9 | 无名指横摆 | |
+| joint10 | 小拇指横摆 | |
+| joint11 | 大拇指横滚 | |
+| joint12 | 预留 | 值为0 |
+| joint13 | 预留 | 值为0 |
+| joint14 | 预留 | 值为0 |
+| joint15 | 预留 | 值为0 |
+| joint16 | 大拇指中部 | 值为0 |
+| joint17 | 预留 | 值为0 |
+| joint18 | 预留 | 值为0 |
+| joint19 | 预留 | 值为0 |
+| joint20 | 预留 | 值为0 |
+| joint21 | 大拇指指尖 | |
+| joint22 | 食指指尖 | |
+| joint23 | 中指指尖 | |
+| joint24 | 无名指指尖 | |
+| joint25 | 小指指尖 | |
+
+#### L20/G20 (20关节输出)
+
+适用灵巧手型号：L20、工业版20、G20
+
+| Joint | 关节名称 | 说明 |
+|-------|----------|------|
+| joint1 | 拇指弯曲 | |
+| joint2 | 食指弯曲 | |
+| joint3 | 中指弯曲 | |
+| joint4 | 无名指弯曲 | |
+| joint5 | 小指弯曲 | |
+| joint6 | 拇指横摆 | |
+| joint7 | 食指横摆 | |
+| joint8 | 中指横摆 | |
+| joint9 | 无名指横摆 | |
+| joint10 | 小指横摆 | |
+| joint11 | 拇指横滚 | |
+| joint12 | 预留 | 值为0 |
+| joint13 | 预留 | 值为0 |
+| joint14 | 预留 | 值为0 |
+| joint15 | 预留 | 值为0 |
+| joint16 | 拇指指尖 | |
+| joint17 | 食指指尖 | |
+| joint18 | 中指指尖 | |
+| joint19 | 无名指指尖 | |
+| joint20 | 小指指尖 | |
+
+#### L10 (10关节)
+
+适用灵巧手型号：L10
+
+| Joint | 关节名称 |
+|-------|----------|
+| joint1 | 大拇指弯曲 |
+| joint2 | 大拇指横摆 |
+| joint3 | 食指弯曲 |
+| joint4 | 中指弯曲 |
+| joint5 | 无名指弯曲 |
+| joint6 | 小指弯曲 |
+| joint7 | 食指横摆 |
+| joint8 | 无名指横摆 |
+| joint9 | 小指横摆 |
+| joint10 | 大拇指横滚 |
+
+#### L6 (6关节)
+
+适用灵巧手型号：L6、O6
+
+| Joint | 关节名称 |
+|-------|----------|
+| joint1 | 大拇指弯曲 |
+| joint2 | 大拇指横摆 |
+| joint3 | 食指弯曲 |
+| joint4 | 中指弯曲 |
+| joint5 | 无名指弯曲 |
+| joint6 | 小指弯曲 |
+
+#### O7 (7关节)
+
+适用灵巧手型号：O7
+
+| Joint | 关节名称 |
+|-------|----------|
+| joint1 | 大拇指弯曲 |
+| joint2 | 大拇指横摆 |
+| joint3 | 食指弯曲 |
+| joint4 | 中指弯曲 |
+| joint5 | 无名指弯曲 |
+| joint6 | 小指弯曲 |
+| joint7 | 大拇指横滚 |
+
+### 消息格式
+
+```python
+# sensor_msgs/JointState
+msg.header.stamp # 时间戳
+msg.name # ['joint1', 'joint2', ..., 'jointN']
+msg.position # [0-255, ...] 电机位置值
+msg.velocity # [255, ...] 速度值
+```
+
+## 文件结构
+
+```
+motion/linkereg/
+├── __init__.py # 模块导出
+├── linkeregcore.py # 串口通讯核心
+├── retarget.py # ROS2 集成
+└── README.md # 本文档
+```
+
+## 注意事项
+
+1. **灵巧手型号**: 无需在配置文件中指定，手套会在数据帧中自动标识协议类型
+
+2. **硬件连接**：接收器必须连接到主机上（当前程序所在主机）
+
+3. **数据频率**: 手套以 50Hz 频率推送数据
+
+4. **模式选择**:
+ - `linkereg2` (SDK控制模式): 接收器不需要连接灵巧手，手套数据通过ROS话题发布，由上层灵巧手SDK订阅对应话题来控制灵巧手
+ - `linkereg1` (接收器控制模式): 接收器必须连接真实灵巧手，接收器直接控制灵巧手运动，当前SDK只是为了采集手套数据。
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkereg/__init__.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkereg/__init__.py
new file mode 100644
index 0000000..0f70c53
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkereg/__init__.py
@@ -0,0 +1,12 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+LinkerEG 遥操作手套模块
+
+通过串口与 LinkerEG 手套通讯，接收映射数据并发布到 ROS2 话题
+"""
+
+from .linkeregcore import LinkerEGSerial, PROTOCOL_MAP
+from .retarget import Retarget
+
+__all__ = ['LinkerEGSerial', 'PROTOCOL_MAP', 'Retarget']
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkereg/linkeregcore.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkereg/linkeregcore.py
new file mode 100644
index 0000000..d2546fb
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkereg/linkeregcore.py
@@ -0,0 +1,911 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+LinkerEG 遥操作手套串口通讯核心模块
+
+协议特点:
+- 波特率: 115200, 8-n-1
+- 帧格式: [0xAA] [CMD] [LEN] [DATA...] [CHECKSUM] [0x55]
+- 校验和: 补码累加和 checksum = ~(cmd + len + sum(data)) + 1
+- 左右手共用同一串口
+"""
+
+import array
+import struct
+import time
+import re
+import os
+import subprocess
+import serial
+import serial.tools.list_ports
+from enum import Enum
+from threading import Thread, Event
+from typing import Optional, Tuple, List, Callable
+
+# 帧常量
+FRAME_HEADER = 0xAA
+FRAME_TAIL = 0x55
+MAX_DATA_SIZE = 128
+BUFFER_SIZE = 512
+
+# 命令类型
+class CmdType:
+ READ_CONTROL_MODE = 0x0B # 读取控制方式
+ RECEIVER_CONTROL = 0x0D # 接收器控制灵巧手
+ SDK_CONTROL = 0x0E # SDK控制灵巧手
+ ENABLE_RAW_DATA = 0x0F # 启用传感器原始数据推送
+ DISABLE_RAW_DATA = 0x10 # 禁用传感器原始数据推送
+ ENABLE_MAPPED_DATA = 0x11 # 启用映射数据推送
+ DISABLE_MAPPED_DATA = 0x12 # 禁用映射数据推送
+ READ_VERSION = 0x14 # 读取版本号
+ RAW_DATA_PUSH = 0x20 # 传感器原始数据推送 (50Hz)
+ MAPPED_DATA_PUSH = 0x21 # 映射数据推送 (50Hz)
+
+# 协议类型映射
+PROTOCOL_MAP = {
+ 0: {'name': 'L20', 'joints': 16},
+ 1: {'name': 'L10', 'joints': 10},
+ 2: {'name': 'L21', 'joints': 16},
+ 3: {'name': 'L6', 'joints': 6},
+ 4: {'name': 'O7', 'joints': 7},
+ 5: {'name': 'G20', 'joints': 16},
+}
+
+# 结果码
+class ResultCode:
+ SUCCESS = 0x00
+ FAILED = 0x01
+ UNKNOWN_CMD = 0xFD
+ FRAME_MODE_DISABLED = 0xFE
+ CHECKSUM_ERROR = 0xFF
+
+
+class FrameParseState(Enum):
+ """帧解析状态机"""
+ HEADER = 0
+ CMD = 1
+ LENGTH = 2
+ DATA = 3
+ CHECKSUM = 4
+ TAIL = 5
+
+
+class FrameParser:
+ """LinkerEG 帧解析器"""
+
+ def __init__(self):
+ self.state = FrameParseState.HEADER
+ self.frame_buf = array.array('B', [0] * (4 + MAX_DATA_SIZE + 2))
+ self.expected_len = 0
+ self.current_pos = 0
+ self.cmd = 0
+ self.data_len = 0
+
+ def reset(self):
+ """重置解析器状态"""
+ self.state = FrameParseState.HEADER
+ self.current_pos = 0
+ self.cmd = 0
+ self.data_len = 0
+
+ @staticmethod
+ def calculate_checksum(cmd: int, data_len: int, data: bytes) -> int:
+ """
+ 计算校验和 (补码累加和)
+ checksum = ~(cmd + len + sum(data)) + 1
+ """
+ total = cmd + data_len
+ for b in data:
+ total += b
+ checksum = (~total + 1) & 0xFF
+ return checksum
+
+ def process_byte(self, byte: int) -> bool:
+ """
+ 处理单个字节，返回是否接收到完整帧
+ """
+ byte = byte & 0xFF
+
+ if self.state == FrameParseState.HEADER:
+ if byte == FRAME_HEADER:
+ self.frame_buf[0] = byte
+ self.current_pos = 1
+ self.state = FrameParseState.CMD
+
+ elif self.state == FrameParseState.CMD:
+ self.cmd = byte
+ self.frame_buf[1] = byte
+ self.current_pos = 2
+ self.state = FrameParseState.LENGTH
+
+ elif self.state == FrameParseState.LENGTH:
+ self.data_len = byte
+ self.frame_buf[2] = byte
+ self.current_pos = 3
+ if byte <= MAX_DATA_SIZE:
+ if byte == 0:
+ self.state = FrameParseState.CHECKSUM
+ else:
+ self.state = FrameParseState.DATA
+ else:
+ self.reset()
+
+ elif self.state == FrameParseState.DATA:
+ self.frame_buf[self.current_pos] = byte
+ self.current_pos += 1
+ if self.current_pos >= 3 + self.data_len:
+ self.state = FrameParseState.CHECKSUM
+
+ elif self.state == FrameParseState.CHECKSUM:
+ self.frame_buf[self.current_pos] = byte
+ # 验证校验和
+ data = bytes(self.frame_buf[3:3 + self.data_len])
+ expected_checksum = self.calculate_checksum(self.cmd, self.data_len, data)
+ if byte == expected_checksum:
+ self.current_pos += 1
+ self.state = FrameParseState.TAIL
+ else:
+ self.reset()
+
+ elif self.state == FrameParseState.TAIL:
+ if byte == FRAME_TAIL:
+ self.frame_buf[self.current_pos] = byte
+ return True
+ else:
+ self.reset()
+
+ return False
+
+ def get_frame_data(self) -> Tuple[int, bytes]:
+ """获取解析后的帧数据 (cmd, data)"""
+ data = bytes(self.frame_buf[3:3 + self.data_len])
+ return self.cmd, data
+
+
+class LinkerEGSerial:
+ """LinkerEG 串口通讯类"""
+
+ # 控制模式
+ MODE_SDK = 'sdk' # SDK控制模式 (linkereg)
+ MODE_RECEIVER = 'receiver' # 接收器控制模式 (linkereg1)
+
+ def __init__(self, port: str = None, baudrate: int = 115200, password: str = '12345678', isdebug: bool = False, mode: str = 'sdk'):
+ """
+ 初始化 LinkerEG 串口通讯
+
+ Args:
+ port: 串口路径，如果为 None 则自动扫描
+ baudrate: 波特率 (默认 115200)
+ password: sudo 密码，用于修复串口权限
+ isdebug: 是否打印调试信息
+ mode: 控制模式 'sdk'=SDK控制模式, 'receiver'=接收器控制模式
+ """
+ self.port = port
+ self.baudrate = baudrate
+ self.mode = mode
+
+
+ print(f"[LinkerEG] 初始化 LinkerEGSerial (mode={self.mode})...", flush=True)
+ self.serial_port: Optional[serial.Serial] = None
+ self.parser = FrameParser()
+ self.running = Event()
+ self.thread: Optional[Thread] = None
+
+ # 版本信息
+ self.version: Optional[str] = None
+ self.connected = False
+ self.initialized = False
+
+ # 数据存储 (左右手) - 控制数据
+ self.right_hand_data: List[int] = []
+ self.left_hand_data: List[int] = []
+ self.right_hand_protocol: int = -1
+ self.left_hand_protocol: int = -1
+
+ # 回调函数 (control_data, protocol)
+ self.on_right_hand_data: Optional[Callable[[List[int], int], None]] = None
+ self.on_left_hand_data: Optional[Callable[[List[int], int], None]] = None
+
+ # 传感器原始数据存储 (左右手) - 15个int32关节值
+ self.right_hand_raw_data: List[int] = []
+ self.left_hand_raw_data: List[int] = []
+
+ # 原始数据回调函数 (raw_data: List[int])
+ # raw_data 为15个int32值: 大拇指横摆、大拇指弯曲、大拇指指尖、食指横摆、食指弯曲、食指指尖...
+ self.on_right_hand_raw_data: Optional[Callable[[List[int]], None]] = None
+ self.on_left_hand_raw_data: Optional[Callable[[List[int]], None]] = None
+
+ # 原始数据推送状态
+ self.raw_data_enabled = False
+
+ # sudo密码 (用于权限修复，从外部传入)
+ self._sudo_password: str = password
+ self.isdebug = isdebug
+
+ # 串口扫描相关
+ self.checked_ports: set = set()
+ self.exclude_ports: set = set()
+
+
+ # 灵巧手控制状态
+ self.hand_control_mode: Optional[str] = None # 'sdk' 或 'receiver'
+
+ def _is_usb_device(self, port_name: str) -> bool:
+ """
+ 判断是否为USB串口设备
+ 支持: /dev/ttyUSB*, /dev/ttyACM* 等USB转串口设备
+ """
+ import re
+ usb_patterns = [
+ r'/dev/ttyUSB\d+',
+ r'/dev/ttyACM\d+',
+ r'/dev/ttyXRUSB\d+',
+ r'/dev/ttyOBC\d+',
+ ]
+
+ for pattern in usb_patterns:
+ if re.match(pattern, port_name):
+ return True
+
+ try:
+ ports = serial.tools.list_ports.comports()
+ for port_info in ports:
+ if port_info.device == port_name:
+ description = (port_info.description or "").lower()
+ if any(keyword in description for keyword in ['usb', 'serial', 'com']):
+ return True
+ if port_info.hwid and 'USB' in port_info.hwid.upper():
+ return True
+ except:
+ pass
+
+ return False
+
+ def scan_serial_ports(self) -> List[str]:
+ """扫描所有可用的USB串口"""
+ ports = serial.tools.list_ports.comports()
+ available_ports = []
+
+ for port in ports:
+ port_device = port.device
+ if not self._is_usb_device(port_device):
+ continue
+ if port_device in self.exclude_ports:
+ if self.isdebug:
+ print(f"[LinkerEG] 跳过排除的串口: {port_device}", flush=True)
+ continue
+ if port_device not in self.checked_ports:
+ available_ports.append(port_device)
+
+ return available_ports
+
+ def _quick_test_port(self, port_name: str, timeout: float = 3.0) -> Tuple[bool, Optional[int]]:
+ """
+ 快速测试串口是否为 LinkerEG 设备
+ 返回: (success, error_code)
+ error_code: None=成功, -1=不存在, -2=权限问题, -3=设备忙, -99=其他错误
+ """
+ try:
+ with serial.Serial(
+ port_name,
+ baudrate=self.baudrate,
+ timeout=0.2,
+ bytesize=serial.EIGHTBITS,
+ parity=serial.PARITY_NONE,
+ stopbits=serial.STOPBITS_ONE
+ ) as ser:
+ # 清空缓冲区
+ ser.reset_input_buffer()
+ ser.reset_output_buffer()
+ time.sleep(0.2)
+
+ # 发送 frame_enable (尝试三次，间隔更长)
+ for _ in range(3):
+ ser.write(b'frame_enable\n')
+ time.sleep(0.2)
+
+ # 清空可能的回复
+ if ser.in_waiting > 0:
+ ser.read(ser.in_waiting)
+
+ # 发送 SDK 控制命令 (0x0E) - 这个命令响应更可靠
+ cmd = CmdType.SDK_CONTROL
+ data = b''
+ checksum = FrameParser.calculate_checksum(cmd, 0, data)
+ frame = bytes([FRAME_HEADER, cmd, 0, checksum, FRAME_TAIL])
+ if self.isdebug:
+ print(f"[LinkerEG] 发送SDK控制命令: {frame.hex()}", flush=True)
+ ser.write(frame)
+ time.sleep(0.1)
+
+ # 再发送读取版本命令
+ cmd = CmdType.READ_VERSION
+ checksum = FrameParser.calculate_checksum(cmd, 0, data)
+ frame = bytes([FRAME_HEADER, cmd, 0, checksum, FRAME_TAIL])
+ if self.isdebug:
+ print(f"[LinkerEG] 发送版本查询: {frame.hex()}", flush=True)
+ ser.write(frame)
+
+ # 等待响应
+ start_time = time.time()
+ parser = FrameParser()
+
+ while (time.time() - start_time) < timeout:
+ if ser.in_waiting > 0:
+ chunk = ser.read(ser.in_waiting)
+ if self.isdebug:
+ print(f"[LinkerEG] 收到数据: {chunk.hex()}", flush=True)
+ for byte in chunk:
+ if parser.process_byte(byte):
+ cmd_resp, data_resp = parser.get_frame_data()
+ if self.isdebug:
+ print(f"[LinkerEG] 解析到帧: cmd=0x{cmd_resp:02X}, data={data_resp.hex()}", flush=True)
+ # 收到任何有效响应都说明是 LinkerEG 设备
+ if cmd_resp in (CmdType.READ_VERSION, CmdType.SDK_CONTROL, CmdType.RECEIVER_CONTROL,
+ CmdType.MAPPED_DATA_PUSH, CmdType.RAW_DATA_PUSH,
+ CmdType.ENABLE_MAPPED_DATA, CmdType.DISABLE_MAPPED_DATA):
+ if self.isdebug:
+ print(f"[LinkerEG] 串口 {port_name} 检测到 LinkerEG 设备 (cmd=0x{cmd_resp:02X})", flush=True)
+ return True, None
+ parser.reset()
+ time.sleep(0.01)
+
+ if self.isdebug:
+ print(f"[LinkerEG] 串口 {port_name} 无响应", flush=True)
+ return False, None
+
+ except serial.SerialException as e:
+ error_msg = str(e)
+ if "No such file or directory" in error_msg or "[Errno 2]" in error_msg:
+ return False, -1
+ elif "Permission denied" in error_msg or "[Errno 13]" in error_msg:
+ return False, -2
+ elif "Device or resource busy" in error_msg:
+ return False, -3
+ else:
+ if self.isdebug:
+ print(f"[LinkerEG] 串口测试失败: {e}", flush=True)
+ return False, -99
+ except Exception as e:
+ if self.isdebug:
+ print(f"[LinkerEG] 串口测试异常: {e}", flush=True)
+ return False, -99
+
+ def find_valid_port(self) -> Tuple[Optional[str], Optional[int]]:
+ """
+ 自动扫描并查找有效的 LinkerEG 串口
+ 返回: (port_name, error_code)
+ """
+ print("[LinkerEG] 开始扫描串口...", flush=True)
+
+ available_ports = self.scan_serial_ports()
+ if not available_ports:
+ print("[LinkerEG] 未发现可用的 USB 串口设备", flush=True)
+ return None, -1
+
+ print(f"[LinkerEG] 发现 {len(available_ports)} 个串口: {available_ports}", flush=True)
+
+ for port in available_ports:
+ print(f"[LinkerEG] 正在检测 {port}...", flush=True)
+ success, error_code = self._quick_test_port(port)
+
+ if error_code == -2: # 权限问题
+ print(f"[LinkerEG] 检测到权限问题，尝试修复 {port}...", flush=True)
+ if self._fix_serial_permission(port):
+ # 修复后重试
+ success, error_code = self._quick_test_port(port)
+
+ self.checked_ports.add(port)
+
+ if success:
+ print(f"[LinkerEG] ✓ 找到有效串口: {port}", flush=True)
+ return port, None
+
+ print("[LinkerEG] 未找到 LinkerEG 设备", flush=True)
+ return None, -1
+
+
+ def _fix_serial_permission(self, port_name: str) -> bool:
+ """尝试修复串口权限（使用预设密码）"""
+ try:
+ if not os.path.exists(port_name):
+ print(f"[LinkerEG] 串口设备不存在: {port_name}", flush=True)
+ return False
+
+ password = self._sudo_password
+ if not password:
+ print("[LinkerEG] 未设置 sudo 密码，无法修复权限", flush=True)
+ return False
+
+ # 使用echo传递密码执行chmod
+ command = f'echo "{password}" | sudo -S chmod 666 {port_name}'
+ result = subprocess.run(
+ command,
+ shell=True,
+ capture_output=True,
+ text=True,
+ timeout=10
+ )
+
+ if result.returncode == 0:
+ print(f"[LinkerEG] ✓ 成功修复 {port_name} 权限", flush=True)
+ return True
+ else:
+ print(f"[LinkerEG] ✗ 权限修复失败: {result.stderr.strip()}", flush=True)
+ return False
+
+ except subprocess.TimeoutExpired:
+ print(f"[LinkerEG] 修复权限超时", flush=True)
+ except Exception as e:
+ print(f"[LinkerEG] 修复权限时出错: {e}", flush=True)
+
+ return False
+
+ def open(self, auto_scan: bool = True) -> bool:
+ """
+ 打开串口（带权限检测和自动修复）
+
+ Args:
+ auto_scan: 如果 port 为 None，是否自动扫描
+ """
+ # 如果没有指定串口，自动扫描
+ if self.port is None and auto_scan:
+ port, error_code = self.find_valid_port()
+ if port is None:
+ print("[LinkerEG] 自动扫描未找到有效串口", flush=True)
+ return False
+ self.port = port
+
+ if self.port is None:
+ print("[LinkerEG] 未指定串口", flush=True)
+ return False
+
+ max_retry = 2 # 最多尝试2次 (第一次失败后尝试修复权限再试一次)
+
+ for attempt in range(max_retry):
+ try:
+ self.serial_port = serial.Serial(
+ port=self.port,
+ baudrate=self.baudrate,
+ timeout=0.01,
+ bytesize=serial.EIGHTBITS,
+ parity=serial.PARITY_NONE,
+ stopbits=serial.STOPBITS_ONE
+ )
+ self.connected = True
+ print(f"[LinkerEG] 串口 {self.port} 打开成功 (波特率: {self.baudrate})", flush=True)
+ return True
+
+ except serial.SerialException as e:
+ error_msg = str(e)
+
+ if "No such file or directory" in error_msg or "[Errno 2]" in error_msg:
+ print(f"[LinkerEG] 串口设备不存在: {self.port}", flush=True)
+ print("[LinkerEG] 请检查串口名称或设备是否连接", flush=True)
+ return False
+
+ elif "Permission denied" in error_msg or "[Errno 13]" in error_msg:
+ print(f"[LinkerEG] 权限被拒绝: {self.port}", flush=True)
+
+ if attempt == 0: # 第一次失败，尝试修复权限
+ print("[LinkerEG] 检测到权限问题，尝试修复权限...", flush=True)
+ if self._fix_serial_permission(self.port):
+ print("[LinkerEG] 权限修复成功，重新尝试打开串口...", flush=True)
+ continue # 重试
+ else:
+ print("[LinkerEG] 权限修复失败", flush=True)
+ return False
+ else:
+ print("[LinkerEG] 权限修复后仍无法打开串口", flush=True)
+ return False
+
+ elif "Device or resource busy" in error_msg:
+ print(f"[LinkerEG] 设备忙: {self.port} - 串口可能已被其他程序占用", flush=True)
+ return False
+
+ else:
+ print(f"[LinkerEG] 串口打开失败: {e}", flush=True)
+ return False
+
+ return False
+
+ def close(self):
+ """关闭串口"""
+ self.stop()
+ if self.serial_port and self.serial_port.is_open:
+ self.serial_port.close()
+ self.connected = False
+ print("[LinkerEG] 串口已关闭", flush=True)
+
+ def _build_frame(self, cmd: int, data: bytes = b'') -> bytes:
+ """构建发送帧"""
+ data_len = len(data)
+ checksum = FrameParser.calculate_checksum(cmd, data_len, data)
+ frame = bytes([FRAME_HEADER, cmd, data_len]) + data + bytes([checksum, FRAME_TAIL])
+ return frame
+
+ def _send_frame(self, cmd: int, data: bytes = b'') -> bool:
+ """发送帧"""
+ if not self.serial_port or not self.serial_port.is_open:
+ return False
+ frame = self._build_frame(cmd, data)
+ try:
+ self.serial_port.write(frame)
+ return True
+ except Exception as e:
+ print(f"[LinkerEG] 发送失败: {e}", flush=True)
+ return False
+
+ def enable_frame_mode(self) -> bool:
+ """启用数据帧模式 (发送 'frame_enable\n' 多次)"""
+ if not self.serial_port or not self.serial_port.is_open:
+ return False
+ try:
+ # 清空缓冲区
+ self.serial_port.reset_input_buffer()
+ self.serial_port.reset_output_buffer()
+
+ # 发送 ASCII 字符串 "frame_enable\n" 多次确保可靠
+ for _ in range(3):
+ self.serial_port.write(b'frame_enable\n')
+ time.sleep(0.1)
+ print("[LinkerEG] 已发送 frame_enable 命令", flush=True)
+ return True
+ except Exception as e:
+ print(f"[LinkerEG] 发送 frame_enable 失败: {e}", flush=True)
+ return False
+
+ def sdk_control(self) -> bool:
+ """SDK控制灵巧手 (0x0E) - linkereg2模式"""
+ return self._send_frame(CmdType.SDK_CONTROL)
+
+ def receiver_control(self) -> bool:
+ """接收器控制灵巧手 (0x0D) - linkereg1模式"""
+ return self._send_frame(CmdType.RECEIVER_CONTROL)
+
+ def enable_mapped_data(self) -> bool:
+ """启用映射数据推送 (0x11)"""
+ return self._send_frame(CmdType.ENABLE_MAPPED_DATA)
+
+ def disable_mapped_data(self) -> bool:
+ """禁用映射数据推送 (0x12)"""
+ return self._send_frame(CmdType.DISABLE_MAPPED_DATA)
+
+ def enable_raw_data(self) -> bool:
+ """
+ 启用传感器原始数据推送 (0x0F)
+ 发送: AA 0F 00 F1 55
+ 响应: AA 0F 01 00 [校验和] 55
+ 启用后系统以50Hz频率推送原始传感器数据 (命令类型0x20)
+ """
+ return self._send_frame(CmdType.ENABLE_RAW_DATA)
+
+ def disable_raw_data(self) -> bool:
+ """
+ 禁用传感器原始数据推送 (0x10)
+ 发送: AA 10 00 F0 55
+ 响应: AA 10 01 00 [校验和] 55
+ """
+ return self._send_frame(CmdType.DISABLE_RAW_DATA)
+
+ def read_control_mode(self) -> bool:
+ """
+ 读取控制方式 (0x0B)
+ 发送: AA 0B 00 F5 55
+ 响应: AA 0B 02 00 [0x00:SDK控制, 0x01:接收器控制] [校验和] 55
+ """
+ return self._send_frame(CmdType.READ_CONTROL_MODE)
+
+ def read_version(self) -> bool:
+ """读取版本号 (0x14)"""
+ return self._send_frame(CmdType.READ_VERSION)
+
+ def initialize(self) -> bool:
+ """
+ 完整初始化流程:
+ - SDK控制模式: 发送 0x0E 命令
+ - 接收器控制模式: 发送 0x0D 命令 (需要连接灵巧手)
+ """
+ mode_name = "SDK控制模式" if self.mode == self.MODE_SDK else "接收器控制模式"
+ print(f"[LinkerEG] 开始初始化 ({mode_name})...", flush=True)
+
+ # 1. 启用数据帧模式
+ if not self.enable_frame_mode():
+ print("[LinkerEG] 启用数据帧模式失败", flush=True)
+ return False
+ time.sleep(0.2)
+
+ # 2. 根据模式发送控制命令 (多次发送确保生效)
+ while 1:
+ if self.mode == self.MODE_RECEIVER and self.hand_control_mode != self.MODE_RECEIVER:
+ self.receiver_control()
+ elif self.mode == self.MODE_SDK and self.hand_control_mode != self.MODE_SDK:
+ self.sdk_control()
+ else:
+ print(f"[LinkerEG] 当前控制方式: {self.hand_control_mode} (与期望一致)", flush=True)
+ break
+ time.sleep(0.2)
+ self.read_control_mode()
+
+
+
+
+ # 3. 读取版本号
+ if not self.read_version():
+ print("[LinkerEG] 读取版本号失败", flush=True)
+ return False
+ time.sleep(0.3)
+
+ # 4. 读取当前控制方式
+ if not self.read_control_mode():
+ print("[LinkerEG] 读取控制方式失败", flush=True)
+ return False
+ time.sleep(0.3)
+
+ # 5. 启用映射数据推送
+ if not self.enable_mapped_data():
+ print("[LinkerEG] 启用映射数据推送失败", flush=True)
+ return False
+
+ self.initialized = True
+
+ print(f"[LinkerEG] 初始化完成 ({self.hand_control_mode})", flush=True)
+ return True
+
+ def start(self):
+ """启动接收线程"""
+ if self.thread is not None and self.thread.is_alive():
+ return
+ self.running.set()
+ self.thread = Thread(target=self._receive_loop, daemon=True)
+ self.thread.start()
+ print("[LinkerEG] 接收线程已启动", flush=True)
+
+ def stop(self):
+ """停止接收线程"""
+ self.running.clear()
+ if self.thread is not None:
+ self.thread.join(timeout=1.0)
+ self.thread = None
+ print("[LinkerEG] 接收线程已停止", flush=True)
+
+ def _reorder_joints(self, control_data: List[int], protocol: int) -> List[int]:
+ """
+ 重排关节顺序，使输出与 haocun/linkerforce 保持一致
+
+ Args:
+ control_data: 原始控制数据
+ protocol: 协议类型
+
+ Returns:
+ 重排后的控制数据
+ """
+ if protocol == 2: # L21 (16关节 -> 25关节输出)
+ # 原序 (16关节): [大拇指横摆, 食指横摆, 中指横摆, 无名指横摆, 小指横摆,
+ # 大拇指弯曲, 食指弯曲, 中指弯曲, 无名指弯曲, 小指弯曲,
+ # 大拇指横滚, 大拇指中部, 大拇指指尖, 食指指尖, 中指指尖, 无名指指尖, 小指指尖]
+
+ if len(control_data) >= 16:
+ return [
+ control_data[5], # 大拇指弯曲
+ control_data[6], # 食指弯曲
+ control_data[7], # 中指弯曲
+ control_data[8], # 无名指弯曲
+ control_data[9], # 小拇指弯曲
+ control_data[0], # 大拇指横摆
+ control_data[1], # 食指横摆
+ control_data[2], # 中指横摆
+ control_data[3], # 无名指横摆
+ control_data[4], # 小拇指横摆
+ control_data[15], # 大拇指横滚
+ 0, # 预留
+ 0, # 预留
+ 0, # 预留
+ 0, # 预留
+ 0, # 大拇指中部
+ 0, # 预留
+ 0, # 预留
+ 0, # 预留
+ 0, # 预留
+ control_data[10], # 大拇指指尖
+ control_data[11], # 食指指尖
+ control_data[12], # 中指指尖
+ control_data[13], # 无名指指尖
+ control_data[14], # 小指指尖
+
+ ]
+
+ elif protocol == 3: # L6/O6 (6关节)
+ # 原序: [大拇指横摆, 大拇指弯曲, 食指, 中指, 无名指, 小指]
+ # 新序: [大拇指弯曲, 大拇指横摆, 食指, 中指, 无名指, 小指]
+ if len(control_data) >= 6:
+ return [
+ control_data[1], # 大拇指弯曲
+ control_data[0], # 大拇指横摆
+ control_data[2], # 食指弯曲
+ control_data[3], # 中指弯曲
+ control_data[4], # 无名指弯曲
+ control_data[5], # 小指弯曲
+ ]
+
+ elif protocol == 4: # O7 (7关节)
+ # 原序: [大拇指横摆, 大拇指弯曲, 食指, 中指, 无名指, 小指, 大拇指旋转]
+ # 新序: [大拇指弯曲, 大拇指横摆, 食指, 中指, 无名指, 小指, 大拇指旋转]
+ if len(control_data) >= 7:
+ return [
+ control_data[1], # 大拇指弯曲
+ control_data[0], # 大拇指横摆
+ control_data[2], # 食指弯曲
+ control_data[3], # 中指弯曲
+ control_data[4], # 无名指弯曲
+ control_data[5], # 小指弯曲
+ control_data[6], # 大拇指旋转
+ ]
+
+ elif protocol == 1: # L10 (10关节)
+ # 新序: [大拇指弯曲, 大拇指横摆, 食指弯曲, 中指弯曲, 无名指弯曲, 小指弯曲, 食指横摆, 无名指横摆, 小指横摆, 大拇指旋转]
+ # 新序: [大拇指弯曲, 大拇指横摆, 食指弯曲, 中指弯曲, 无名指弯曲, 小指弯曲, 食指横摆, 无名指横摆, 小指横摆, 大拇指旋转]
+ if len(control_data) >= 10:
+ return [
+ control_data[4], # 大拇指弯曲
+ control_data[0], # 大拇指横摆
+ control_data[5], # 食指弯曲
+ control_data[6], # 中指弯曲
+ control_data[7], # 无名指弯曲
+ control_data[8], # 小指弯曲
+ control_data[1], # 食指横摆
+ control_data[2], # 无名指横摆
+ control_data[3], # 小指横摆
+ control_data[9], # 大拇指旋转
+ ]
+
+ elif protocol == 0 or protocol == 5: # L20 G20(16关节 -> 20关节输出)
+ # 原序 (16关节): [大拇指横摆, 食指横摆, 中指横摆, 无名指横摆, 小指横摆,
+ # 大拇指弯曲, 食指弯曲, 中指弯曲, 无名指弯曲, 小指弯曲,
+ # 大拇指指尖, 食指指尖, 中指指尖, 无名指指尖, 小指指尖, 大拇指横滚]
+ # 新序 (20关节): [拇指弯曲, 食指弯曲, 中指弯曲, 无名指弯曲, 小指弯曲,
+ # 拇指横摆, 食指横摆, 中指横摆, 无名指横摆, 小指横摆,
+ # 拇指横滚, 预留, 预留, 预留, 预留,
+ # 拇指指尖, 食指指尖, 中指指尖, 无名指指尖, 小指指尖]
+ if len(control_data) >= 16:
+ return [
+ control_data[5], # 拇指弯曲
+ control_data[6], # 食指弯曲
+ control_data[7], # 中指弯曲
+ control_data[8], # 无名指弯曲
+ control_data[9], # 小指弯曲
+ control_data[0], # 拇指横摆
+ control_data[1], # 食指横摆
+ control_data[2], # 中指横摆
+ control_data[3], # 无名指横摆
+ control_data[4], # 小指横摆
+ control_data[15], # 拇指横滚
+ 0, # 预留
+ 0, # 预留
+ 0, # 预留
+ 0, # 预留
+ control_data[10], # 拇指指尖
+ control_data[11], # 食指指尖
+ control_data[12], # 中指指尖
+ control_data[13], # 无名指指尖
+ control_data[14], # 小指指尖
+ ]
+
+ # 其他协议直接透传
+ return control_data
+
+ def _receive_loop(self):
+ """接收数据循环"""
+ while self.running.is_set():
+ try:
+ if self.serial_port and self.serial_port.in_waiting > 0:
+ data = self.serial_port.read(self.serial_port.in_waiting)
+ for byte in data:
+ if self.parser.process_byte(byte):
+ self._handle_frame()
+ self.parser.reset()
+ time.sleep(0.001)
+ except Exception as e:
+ print(f"[LinkerEG] 接收错误: {e}", flush=True)
+ time.sleep(0.01)
+
+ def _handle_frame(self):
+ """处理接收到的完整帧"""
+ cmd, data = self.parser.get_frame_data()
+
+ if cmd == CmdType.READ_CONTROL_MODE:
+ # 响应格式: AA 0B 02 00 [0x00:SDK控制, 0x01:接收器控制] [校验和] 55
+ if len(data) >= 2 and data[0] == ResultCode.SUCCESS:
+ control_mode = data[1]
+ self.hand_control_mode = "sdk" if control_mode == 0x00 else "receiver"
+ elif cmd == CmdType.READ_VERSION:
+ # 响应格式: [结果码] [版本数据...]
+ if len(data) >= 5 and data[0] == ResultCode.SUCCESS:
+ # 版本号格式: 4字节小端序
+ version_bytes = data[1:5]
+ major = version_bytes[0]
+ minor = version_bytes[1]
+ patch = (version_bytes[2] << 8) | version_bytes[3]
+ self.version = f"{major}.{minor}.{patch}"
+ print(f"[LinkerEG] 版本号: {self.version}", flush=True)
+
+ elif cmd == CmdType.MAPPED_DATA_PUSH:
+ # 映射数据帧 (linkereg2 SDK控制模式): [手侧] [协议] [状态] [N字节控制数据]
+ # SDK模式下只有控制数据
+ if len(data) >= 3:
+ hand_side = data[0] # 0=右手, 1=左手
+ protocol = data[1] # 协议类型
+ # data[2] 是状态字节，忽略
+
+ if protocol in PROTOCOL_MAP:
+ num_joints = PROTOCOL_MAP[protocol]['joints']
+ # SDK模式: 只有控制数据，从第3字节开始
+ if len(data) >= 3 + num_joints:
+ control_data = list(data[3:3 + num_joints])
+
+ # 重排关节顺序，与 haocun/linkerforce 保持一致
+ control_data = self._reorder_joints(control_data, protocol)
+
+ if hand_side == 0: # 右手
+ self.right_hand_data = control_data
+ self.right_hand_protocol = protocol
+ if self.on_right_hand_data:
+ self.on_right_hand_data(control_data, protocol)
+ else: # 左手
+ self.left_hand_data = control_data
+ self.left_hand_protocol = protocol
+ if self.on_left_hand_data:
+ self.on_left_hand_data(control_data, protocol)
+
+ elif cmd == CmdType.SDK_CONTROL:
+ if len(data) >= 1 and data[0] == ResultCode.SUCCESS:
+ print("[LinkerEG] SDK控制模式已启用", flush=True)
+
+ elif cmd == CmdType.RECEIVER_CONTROL:
+ if len(data) >= 1 and data[0] == ResultCode.SUCCESS:
+ print("[LinkerEG] 接收器控制模式已启用", flush=True)
+
+ elif cmd == CmdType.ENABLE_MAPPED_DATA:
+ if len(data) >= 1 and data[0] == ResultCode.SUCCESS:
+ print("[LinkerEG] 映射数据推送已启用", flush=True)
+
+ elif cmd == CmdType.DISABLE_MAPPED_DATA:
+ if len(data) >= 1 and data[0] == ResultCode.SUCCESS:
+ print("[LinkerEG] 映射数据推送已禁用", flush=True)
+
+ elif cmd == CmdType.ENABLE_RAW_DATA:
+ if len(data) >= 1 and data[0] == ResultCode.SUCCESS:
+ self.raw_data_enabled = True
+ print("[LinkerEG] 传感器原始数据推送已启用", flush=True)
+
+ elif cmd == CmdType.DISABLE_RAW_DATA:
+ if len(data) >= 1 and data[0] == ResultCode.SUCCESS:
+ self.raw_data_enabled = False
+ print("[LinkerEG] 传感器原始数据推送已禁用", flush=True)
+
+ elif cmd == CmdType.RAW_DATA_PUSH:
+ # 传感器原始数据帧 (0x20): AA 20 3D [1字节手侧] [60字节传感器数据] [校验和] 55
+ # 数据长度: 0x3D = 61字节 (1字节手侧 + 60字节传感器数据)
+ # 传感器数据: 15个int32_t值 (每个4字节，小端序)
+ # 顺序: 大拇指横摆、大拇指弯曲、大拇指指尖、食指横摆、食指弯曲、食指指尖、
+ # 中指横摆、中指弯曲、中指指尖、无名指横摆、无名指弯曲、无名指指尖、
+ # 小指横摆、小指弯曲、小指指尖
+ if len(data) >= 61: # 1字节手侧 + 60字节传感器数据
+ hand_side = data[0] # 0=右手, 1=左手
+ sensor_bytes = data[1:61] # 60字节传感器数据
+
+ # 解析15个int32_t值 (小端序)
+ raw_values = []
+ for i in range(15):
+ offset = i * 4
+ value = struct.unpack(' 读版本 -> 启用映射数据)
+- 接收左右手数据并通过 ROS2 话题发布
+- 支持传感器原始数据推送
+- 支持通过话题动态启用/禁用原始数据
+- 完全不依赖 HandCore
+
+话题列表:
+ 控制数据 (驱动机械手):
+ /cb_right_hand_control_cmd (sensor_msgs/JointState)
+ /cb_left_hand_control_cmd (sensor_msgs/JointState)
+
+ 传感器原始数据:
+ /cb_right_hand_raw_data (sensor_msgs/JointState) - 15个int32关节值
+ /cb_left_hand_raw_data (sensor_msgs/JointState) - 15个int32关节值
+
+ 控制命令:
+ /cb_hand_setting_cmd (std_msgs/String) - 发送 "on" 或 "off"
+"""
+
+import time
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from std_msgs.msg import String
+
+from .linkeregcore import LinkerEGSerial, PROTOCOL_MAP
+
+
+class Retarget:
+ """LinkerEG Retarget 类"""
+
+ def __init__(self,
+ node: Node,
+ port: str = None,
+ baudrate: int = 115200,
+ password: str = 'i',
+ isdebug: bool = False,
+ mode: str = 'sdk'):
+ """
+ 初始化 LinkerEG Retarget
+
+ Args:
+ node: ROS2 节点
+ port: 串口路径，为 None 时自动扫描 (左右手共用同一个串口)
+ baudrate: 波特率 (默认 115200)
+ password: sudo 密码，用于自动修复串口权限
+ isdebug: 是否打印调试信息
+ mode: 控制模式 'sdk'=SDK控制模式, 'receiver'=接收器控制模式
+ """
+ self.node = node
+ self.port = port
+ self.baudrate = baudrate
+ self.password = password
+ self.isdebug = isdebug
+ self.mode = mode
+ self.raw_data_enabled = False # 原始数据推送状态 (通过话题控制)
+ self.running = True
+ self.pubprintcount = 0
+ self.raw_count = 0
+
+ # 创建 LinkerEG 串口通讯对象
+ self.linkereg = LinkerEGSerial(
+ port=port,
+ baudrate=baudrate,
+ password=password,
+ isdebug=isdebug,
+ mode=mode
+ )
+
+ # 设置数据回调
+ self.linkereg.on_right_hand_data = self._on_right_hand_data
+ self.linkereg.on_left_hand_data = self._on_left_hand_data
+
+ # ROS2 发布器 - 控制数据
+ self.publisher_r = self.node.create_publisher(
+ JointState,
+ '/cb_right_hand_control_cmd',
+ 10
+ )
+ self.publisher_l = self.node.create_publisher(
+ JointState,
+ '/cb_left_hand_control_cmd',
+ 10
+ )
+
+ # ROS2 发布器 - 传感器原始数据 (默认创建，通过话题控制启用/禁用)
+ self.publisher_r_raw = self.node.create_publisher(
+ JointState,
+ '/cb_right_hand_raw_data',
+ 10
+ )
+ self.publisher_l_raw = self.node.create_publisher(
+ JointState,
+ '/cb_left_hand_raw_data',
+ 10
+ )
+
+ # ROS2 订阅器 - 设置命令 (用于动态启用/禁用原始数据)
+ self.setting_sub = self.node.create_subscription(
+ String,
+ '/cb_hand_setting_cmd',
+ self._on_setting_cmd,
+ 10
+ )
+
+ # 定时器 - 检查连接状态
+ self.status_timer = self.node.create_timer(1.0, self._status_callback)
+
+ self.node.get_logger().info("LinkerEG Retarget 模块已创建")
+
+ def _on_right_hand_data(self, control_data: list, protocol: int):
+ """右手数据回调 - 直接发布数据"""
+ if not self.running:
+ return
+
+ now = self.node.get_clock().now().to_msg()
+ protocol_info = PROTOCOL_MAP.get(protocol, {'name': 'Unknown', 'joints': len(control_data)})
+
+ # 调试打印
+ if self.isdebug and self.pubprintcount % 50 == 0:
+ self.node.get_logger().info(
+ f"[LinkerEG] 右手 ({protocol_info['name']}): ctrl={control_data}"
+ )
+
+ msg_ctrl = JointState()
+ msg_ctrl.header.stamp = now
+ msg_ctrl.name = [f'joint{i + 1}' for i in range(len(control_data))]
+ msg_ctrl.position = [float(v) for v in control_data]
+ msg_ctrl.velocity = [255.0] * len(control_data)
+ self.publisher_r.publish(msg_ctrl)
+ self.pubprintcount += 1
+
+ def _on_left_hand_data(self, control_data: list, protocol: int):
+ """左手数据回调 - 直接发布数据"""
+ if not self.running:
+ return
+
+ now = self.node.get_clock().now().to_msg()
+ protocol_info = PROTOCOL_MAP.get(protocol, {'name': 'Unknown', 'joints': len(control_data)})
+
+ # 调试打印
+ if self.isdebug and self.pubprintcount % 50 == 0:
+ self.node.get_logger().info(
+ f"[LinkerEG] 左手 ({protocol_info['name']}): ctrl={control_data}"
+ )
+
+ msg_ctrl = JointState()
+ msg_ctrl.header.stamp = now
+ msg_ctrl.name = [f'joint{i + 1}' for i in range(len(control_data))]
+ msg_ctrl.position = [float(v) for v in control_data]
+ msg_ctrl.velocity = [255.0] * len(control_data)
+ self.publisher_l.publish(msg_ctrl)
+
+ def _on_right_hand_raw_data(self, raw_data: list):
+ """右手传感器原始数据回调 - 直接发布数据"""
+ if not self.running or not self.raw_data_enabled:
+ return
+
+ now = self.node.get_clock().now().to_msg()
+ joint_names = [
+ 'thumb_spread', 'thumb_bend', 'thumb_tip',
+ 'index_spread', 'index_bend', 'index_tip',
+ 'middle_spread', 'middle_bend', 'middle_tip',
+ 'ring_spread', 'ring_bend', 'ring_tip',
+ 'pinky_spread', 'pinky_bend', 'pinky_tip'
+ ]
+ msg = JointState()
+ msg.header.stamp = now
+ msg.name = joint_names
+ msg.position = [float(v) for v in raw_data]
+ self.publisher_r_raw.publish(msg)
+ self.raw_count += 1
+
+ def _on_left_hand_raw_data(self, raw_data: list):
+ """左手传感器原始数据回调 - 直接发布数据"""
+ if not self.running or not self.raw_data_enabled:
+ return
+
+ now = self.node.get_clock().now().to_msg()
+ joint_names = [
+ 'thumb_spread', 'thumb_bend', 'thumb_tip',
+ 'index_spread', 'index_bend', 'index_tip',
+ 'middle_spread', 'middle_bend', 'middle_tip',
+ 'ring_spread', 'ring_bend', 'ring_tip',
+ 'pinky_spread', 'pinky_bend', 'pinky_tip'
+ ]
+ msg = JointState()
+ msg.header.stamp = now
+ msg.name = joint_names
+ msg.position = [float(v) for v in raw_data]
+ self.publisher_l_raw.publish(msg)
+
+ def _on_setting_cmd(self, msg: String):
+ """
+ 处理设置命令
+
+ 支持的命令:
+ on - 启用传感器原始数据推送
+ off - 禁用传感器原始数据推送
+ """
+ cmd = msg.data.strip().lower()
+
+ if cmd == 'on':
+ if self.raw_data_enabled:
+ return # 已启用，静默忽略
+
+ # 设置回调
+ self.linkereg.on_right_hand_raw_data = self._on_right_hand_raw_data
+ self.linkereg.on_left_hand_raw_data = self._on_left_hand_raw_data
+
+ # 发送启用命令
+ self.linkereg.enable_raw_data()
+ self.raw_data_enabled = True
+ self.node.get_logger().info("[LinkerEG] 已启用传感器原始数据推送")
+ self.node.get_logger().info("[LinkerEG] 原始数据话题: /cb_right_hand_raw_data, /cb_left_hand_raw_data")
+
+ elif cmd == 'off':
+ if not self.raw_data_enabled:
+ self.node.get_logger().info("[LinkerEG] 原始数据已经禁用")
+ return
+
+ # 发送禁用命令
+ self.linkereg.disable_raw_data()
+ self.raw_data_enabled = False
+
+ # 清除回调
+ self.linkereg.on_right_hand_raw_data = None
+ self.linkereg.on_left_hand_raw_data = None
+
+ self.node.get_logger().info("[LinkerEG] 已禁用传感器原始数据推送")
+
+ else:
+ self.node.get_logger().warn(f"[LinkerEG] 未知命令: {cmd}")
+ self.node.get_logger().info("[LinkerEG] 支持的命令: on, off")
+
+ def _status_callback(self):
+ """定时状态检查回调"""
+ if not self.running:
+ return
+
+ if self.linkereg.version:
+ # 版本号只打印一次
+ pass
+
+ def initialize(self) -> bool:
+ """初始化串口连接和手套"""
+ if self.port:
+ self.node.get_logger().info(f"[LinkerEG] 正在连接指定串口 {self.port}...")
+ else:
+ self.node.get_logger().info("[LinkerEG] 未指定串口，将自动扫描...")
+
+ # 打开串口（如果 port 为 None，会自动扫描）
+ if not self.linkereg.open():
+ self.node.get_logger().error("[LinkerEG] 无法打开串口")
+ return False
+
+ # 更新实际使用的串口
+ self.port = self.linkereg.port
+ self.node.get_logger().info(f"[LinkerEG] 已连接串口: {self.port}")
+
+ # 启动接收线程
+ self.linkereg.start()
+
+ # 执行初始化流程
+ if not self.linkereg.initialize():
+ self.node.get_logger().error("[LinkerEG] 初始化失败")
+ return False
+
+
+
+ # 等待版本号
+ timeout = 2.0
+ start_time = time.time()
+ while self.linkereg.version is None and (time.time() - start_time) < timeout:
+ time.sleep(0.1)
+
+ if self.linkereg.version:
+ self.node.get_logger().info(f"[LinkerEG] 手套版本: {self.linkereg.version}")
+ else:
+ self.node.get_logger().warn("[LinkerEG] 未能获取版本号，但继续运行")
+
+ self.node.get_logger().info("[LinkerEG] 初始化成功，等待数据...")
+ self.node.get_logger().info("[LinkerEG] 控制数据话题: /cb_right_hand_control_cmd, /cb_left_hand_control_cmd")
+ self.node.get_logger().info("[LinkerEG] 原始数据话题: /cb_right_hand_raw_data, /cb_left_hand_raw_data (默认禁用)")
+ self.node.get_logger().info("[LinkerEG] 设置命令话题: /cb_hand_setting_cmd (on/off)")
+ return True
+
+ def process(self):
+ """主处理函数 (阻塞)"""
+ if not self.initialize():
+ self.node.get_logger().error("[LinkerEG] 初始化失败，无法启动")
+ return
+
+ try:
+ # 保持节点运行
+ rclpy.spin(self.node)
+ except KeyboardInterrupt:
+ self.node.get_logger().info("[LinkerEG] 收到退出信号")
+ finally:
+ self.shutdown()
+
+ def shutdown(self):
+ """关闭模块"""
+ self.running = False
+ # 禁用原始数据推送 (如果启用了)
+ if self.raw_data_enabled:
+ self.linkereg.disable_raw_data()
+ self.linkereg.close()
+ self.node.get_logger().info("[LinkerEG] 模块已关闭")
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/README.md b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/README.md
new file mode 100644
index 0000000..5b5c395
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/README.md
@@ -0,0 +1,420 @@
+# LinkerFFG Robot Hand Driver Module
+
+LinkerFFG (O6/L7/L10/G20/R20/L25) robot hand ROS1/ROS2 driver module, controls robot hands via serial port with real-time data glove mapping.
+
+---
+
+## Quick Start (Step-by-Step Guide)
+
+### Step 1: Install
+
+```bash
+# ROS2
+cd ~/ros2_ws/src
+git clone https://gitee.com/ericbrunt/linkerhand_telop_python.git
+mv linkerhand_telop_python/ros2/src/linkerhand_retarget ./
+rm -rf linkerhand_telop_python
+cd ..
+rosdep install --from-paths src --ignore-src -r -y
+colcon build --symlink-install
+source install/setup.bash
+
+# ROS1
+cd ~/catkin_ws/src
+git clone https://gitee.com/ericbrunt/linkerhand_telop_python.git
+mv linkerhand_telop_python/ros1/src ./
+rm -rf linkerhand_telop_python
+cd ..
+catkin_make install
+source install/setup.bash
+```
+
+### Step 2: Connect Serial Port
+
+Connect LinkerFFG robot hand to your PC via USB, then verify serial port permissions:
+
+```bash
+# Add current user to dialout group (requires re-login)
+sudo usermod -a -G dialout $USER
+
+# List serial devices
+ls -l /dev/ttyUSB*
+```
+
+### Step 3: Configure Robot Hand Model
+
+Edit `config/base_config.yml` to set robot hand model:
+
+```yaml
+system:
+ motion_type: linkerforce # Data glove type: linkerforce (required)
+ robotname_r: l25 # Right hand model: o6 / l7 / l10 / g20 / r20 / l25
+ robotname_l: l25 # Left hand model
+
+serial:
+ auto_scan: false # Enable auto serial scan
+ baudrates: [2000000, 460800, 1000000, 921600] # 2000000 recommended
+ left:
+ port: /dev/ttyUSB1 # Left hand serial port
+ baudrate: 460800 # Wireless: 460800, Wired: 2000000
+ right:
+ port: /dev/ttyUSB0 # Right hand serial port
+ baudrate: 460800 # Wireless: 460800, Wired: 2000000
+```
+
+### Step 4: Launch
+
+**Method 1: Run node directly**
+
+```bash
+ros2 run linkerhand_retarget handretarget
+```
+
+**Method 2: Specify serial port (without modifying config file)**
+
+```bash
+ros2 run linkerhand_retarget handretarget --ros-args \
+ -p ports:='["/dev/ttyUSB0", "/dev/ttyUSB1"]'
+```
+
+### Step 5: Calibration (if needed)
+
+```bash
+ros2 run linkerhand_retarget handretarget --ros-args -p calibration:=auto_calibrate
+```
+
+During calibration, perform three gestures as prompted:
+1. **Open hand** → hold for 5 seconds
+2. **Make fist** → hold for 5 seconds
+3. **O-pose** → hold for 5 seconds
+
+---
+
+## Robot Hand Models
+
+| Model | DOF | Description |
+|-------|-----|-------------|
+| O6 | 6 | 6-DOF basic model |
+| L7 | 7 | 7-DOF (thumb with roll) |
+| L10 | 10 | 10-DOF industrial model |
+| G20 | 20 | 20-DOF industrial model |
+| R20 | 20 | 20-DOF research model |
+| L25 | 25 | 25-DOF full-featured model |
+
+`robotname_r` and `robotname_l` in config must match the actual connected robot hand models.
+
+---
+
+## Serial Connection Details
+
+### Method 1: Command-line Port List (most common)
+
+Suitable when multiple serial ports exist, system auto-detects left/right hand:
+
+```bash
+ros2 run linkerhand_retarget handretarget --ros-args \
+ -p ports:='["/dev/ttyUSB0", "/dev/ttyUSB1"]'
+```
+
+### Serial Parameters
+
+| Parameter | Description | Default |
+|-----------|-------------|---------|
+| `ports` | Candidate port list | empty (use config file) |
+| `baudrate` | Specified baudrate (overrides config) | use config file |
+| `auto_scan` | Auto scan when preset fails | `false` |
+
+---
+
+## Calibration Details
+
+### Calibration Config
+
+In `config/base_config.yml`:
+
+```yaml
+calibration:
+ show_fist: true # Whether to show fist calibration step
+ fist_extend_ratio: 0.5 # Fist extend ratio (only effective when show_fist=false)
+```
+
+### Start Calibration
+
+```bash
+ros2 run linkerhand_retarget handretarget --ros-args -p calibration:=auto_calibrate
+```
+
+### Calibration Process
+
+1. **Open hand** → hold for 5 seconds (motor value 255)
+2. **O-pose** → hold for 5 seconds (motor middle value)
+
+If `show_fist: true`, a third step appears:
+
+3. **Make fist** → hold for 5 seconds (motor value 0)
+
+### Difference between show_fist=true and show_fist=false
+
+| Item | `show_fist: true` | `show_fist: false` |
+|------|-------------------|-------------------|
+| Calibration steps | Open → O-pose → Fist (3 steps) | Open → O-pose (2 steps) |
+| Fist data source | User actually performs fist gesture | Calculated from O-pose by ratio |
+| Fist formula | N/A | `fist = opose + (opose - original) × fist_extend_ratio` |
+| Mapping precision | Three-segment linear interpolation, most accurate | Two-segment interpolation, relies on extension |
+| fist_extend_ratio | Not used | Controls extension ratio (default 0.5) |
+
+### fist_extend_ratio Details
+
+Only effective when `show_fist: false`, used to calculate fist value from O-pose:
+
+- `fist_extend_ratio = 0.5` (default): O-pose extends 50% toward fist
+- `fist_extend_ratio = 0.0`: fist value = O-pose value (no extension)
+- `fist_extend_ratio = 1.0`: fist value = O-pose + full (O-pose - Open) extension
+- Recommended range: `0.3 ~ 0.7`, adjust based on actual results
+
+### Stability Detection
+
+- Auto-detects gesture stability (variance < 0.03)
+- Requires **5 seconds continuous stability** to complete
+- Resets on instability, no timeout limit
+- Progress bar shows real-time stability duration
+
+### Calibration Data Storage
+
+- Location: `motion/linkerforce/tmp/jointangle_data.tmp`
+- Format: JSON
+- Auto-loads preset sample data on first use
+- Re-calibration overwrites old data
+
+---
+
+## Topic Parameter Control (Runtime Dynamic Adjustment)
+
+Dynamically adjust parameters via `/hand_teleop_param` topic without restarting the node.
+
+### Adjustable Parameters
+
+| Parameter | Description | Example |
+|-----------|-------------|---------|
+| `mapper_debug` | Mapper debug switch | `true` / `false` / `["thumb_rotate"]` |
+| `mapper_exp_factor` | Extrapolation factor | `2.0` (global) or `{"thumb_rotate": 2.0}` (per finger) |
+| `mapper_scale_factor` | Scale factor | `1.5` (global) or `{"index_root_flexion": 1.5}` (per finger) |
+| `force_glove_pose` | Force glove data source | `open` / `fist` / `opose` / `none` |
+
+### Usage Examples
+
+```bash
+# Enable mapper debug for all fingers
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"mapper_debug\": true}"}'
+
+# Debug specific thumb fingers
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"mapper_debug\": [\"thumb_rotate\", \"thumb_abduction\"]}"}'
+
+# Disable debug
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"mapper_debug\": false}"}'
+
+# Adjust global extrapolation factor (higher = faster to target)
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"mapper_exp_factor\": 2.0}"}'
+
+# Adjust per-finger extrapolation factor
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"mapper_exp_factor\": {\"thumb_rotate\": 2.0, \"index_root_flexion\": 1.5}}"}'
+
+# Adjust global scale factor
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"mapper_scale_factor\": 1.5}"}'
+
+# Use calibration data instead of glove data (for testing)
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"force_glove_pose\": \"opose\"}"}'
+
+# Restore real-time glove data
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"force_glove_pose\": \"none\"}"}'
+```
+
+### Available Finger Names
+
+| Finger | Names |
+|--------|-------|
+| Thumb | `thumb_rotate`, `thumb_abduction`, `thumb_root_flexion`, `thumb_end_flexion` |
+| Index | `index_roll`, `index_root_flexion`, `index_end_flexion` |
+| Middle | `middle_roll`, `middle_root_flexion`, `middle_end_flexion` |
+| Ring | `ring_roll`, `ring_root_flexion`, `ring_end_flexion` |
+| Pinky | `pinky_roll`, `pinky_root_flexion`, `pinky_end_flexion` |
+
+---
+
+## Mapping Parameters Details
+
+### Extrapolation Factor (exp_factor)
+
+Controls mapping extrapolation speed to target pose:
+- `= 1.0`: Linear extrapolation
+- `> 1.0`: Accelerated (faster to target)
+- `< 1.0`: Decelerated (smoother)
+
+### Scale Factor (scale_factor)
+
+Controls input-to-output mapping ratio:
+- `= 1.0`: 1:1 mapping
+- `> 1.0`: Amplified output range
+- `< 1.0`: Reduced output range
+
+### Force Glove Data Source (force_glove_pose)
+
+Use calibration data instead of real-time glove data for testing:
+
+| Value | Description |
+|-------|-------------|
+| `open` | Use open hand calibration data |
+| `fist` | Use fist calibration data |
+| `opose` | Use O-pose calibration data |
+| `none` | Use real-time glove data (default) |
+
+---
+
+## Configuration Details
+
+See the "Configuration" section in the main README for full config reference. LinkerFFG-specific configs:
+
+### system System Config
+
+| Config | Description | Options |
+|--------|-------------|---------|
+| `motion_type` | Data glove type | `linkerforce` (required) |
+| `robotname_r` | Right hand robot model | `o6`, `l7`, `l10`, `g20`, `r20`, `l25` |
+| `robotname_l` | Left hand robot model | same as above |
+| `retargeting_type` | Retargeting type | `projection` |
+
+### calibration Calibration Config
+
+| Config | Description | Default |
+|--------|-------------|---------|
+| `show_fist` | Show fist calibration step | `true` |
+| `fist_extend_ratio` | Fist extend ratio | `0.5` |
+
+### debug Debug Config
+
+| Config | Description | Default |
+|--------|-------------|---------|
+| `mapper_debug` | Mapper debug switch | `false` |
+| `joint_motor_debug_r` | Right hand joint motor debug | `false` |
+| `joint_motor_debug_l` | Left hand joint motor debug | `false` |
+
+---
+
+## Troubleshooting
+
+### Cannot open serial port
+
+```bash
+# Check serial permissions
+ls -l /dev/ttyUSB*
+# Grant permissions
+sudo chmod 666 /dev/ttyUSB0
+```
+
+### Robot hand not responding (Key Topic Monitoring)
+
+Follow these steps in order:
+
+**Step 1: Check if topics are published**
+
+```bash
+# List all related topics
+ros2 topic list | grep cb_
+
+# Check if data is being output (is the node publishing normally)
+ros2 topic echo /cb_right_hand_control_cmd --once
+ros2 topic echo /cb_left_hand_control_cmd --once
+```
+
+**Step 2: Check glove data topics**
+
+```bash
+# LinkerFFG driver does not publish glove data topics - this step can be skipped
+# To verify data source, check if /cb_right_hand_control_cmd has data output
+ros2 topic echo /cb_right_hand_control_cmd --once
+```
+
+**Step 3: Check LinkerFFG joint control topics**
+
+```bash
+# Check if joint control commands have output
+ros2 topic echo /cb_right_hand_control_cmd --once
+ros2 topic echo /cb_left_hand_control_cmd --once
+
+# Check topic frequency (should be around 50Hz)
+ros2 topic hz /cb_right_hand_control_cmd
+```
+
+**Step 4: Enable debug output**
+
+```bash
+# Enable all debug
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"mapper_debug\": true}"}'
+
+# Observe terminal output, check if glove data is changing
+# If data doesn't change, glove is not connected or topic is not published
+```
+
+**Step 5: Check calibration data**
+
+```bash
+# Check current calibration data in use
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"force_glove_pose\": \"opose\"}"}'
+# Observe if robot hand responds
+
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"force_glove_pose\": \"fist\"}"}'
+# Observe if robot hand grips
+
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"force_glove_pose\": \"none\"}"}'
+# Restore normal data source
+```
+
+**Step 6: Check serial connection**
+
+1. Verify baudrate matches robot hand settings (wireless: 460800, wired: 2000000)
+2. Try `auto_scan: true` for auto detection
+3. Check robot hand power supply
+
+**Step 7: Check logs**
+
+```bash
+# View node logs
+ros2 run linkerhand_retarget handretarget
+# Observe debug info in terminal output
+```
+
+**Quick Problem Identification**
+
+| Symptom | Possible Cause | Solution |
+|---------|---------------|----------|
+| No topic data | Glove not connected or topic name wrong | Check glove connection, verify topic name |
+| Data always 0 or 255 | Calibration data abnormal | Re-calibrate or delete `tmp/jointangle_data.tmp` |
+| Data fluctuates wildly | Serial signal interference | Check serial cable, use shielded cable |
+| No terminal output | Node didn't start successfully | Check for errors, verify dependencies installed |
+| Topic has data but robot hand doesn't move | Robot hand SDK not receiving commands | Check SDK connection, verify topic is subscribed |
+
+---
+
+## File Structure
+
+```
+motion/linkerforce/
+├── config/ # Hand model configurations
+│ ├── o6_config.py # O6 config
+│ ├── l6_config.py # L6 config
+│ ├── l7_config.py # L7 config
+│ ├── l10_config.py # L10 config
+│ ├── l20_config.py # L20 config
+│ ├── g20_config.py # G20 config
+│ └── o7_config.py # O7 config
+├── hand/ # Robot hand drivers
+│ ├── linkerforce_o6.py
+│ ├── linkerforce_l6.py
+│ ├── linkerforce_l7.py
+│ ├── linkerforce_l10.py
+│ ├── linkerforce_l20.py
+│ └── linkerforce_g20.py
+├── tmp/ # Temp files (calibration data, etc.)
+├── retarget.py # ROS integration
+└── README.md # English documentation
+```
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/README_zh.md b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/README_zh.md
new file mode 100644
index 0000000..29bc911
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/README_zh.md
@@ -0,0 +1,407 @@
+# LinkerFFG 机械手驱动模块
+
+LinkerFFG (O6/L7/L10/G20/R20/L25) 机械手的 ROS1/ROS2 驱动模块，通过串口控制机械手，支持数据手套实时映射。
+
+---
+
+## 快速入门
+### 步骤 1：安装
+
+```bash
+# ROS2
+cd ~/ros2_ws/src
+git clone https://gitee.com/ericbrunt/linkerhand_telop_python.git
+mv linkerhand_telop_python/ros2/src/linkerhand_retarget ./
+rm -rf linkerhand_telop_python
+cd ..
+rosdep install --from-paths src --ignore-src -r -y
+colcon build --symlink-install
+source install/setup.bash
+
+# ROS1
+cd ~/catkin_ws/src
+git clone https://gitee.com/ericbrunt/linkerhand_telop_python.git
+mv linkerhand_telop_python/ros1/src ./
+rm -rf linkerhand_telop_python
+cd ..
+catkin_make install
+source install/setup.bash
+```
+
+### 步骤 2：连接串口
+
+将 LinkerFFG 机械手通过 USB 连接到电脑，确认串口权限：
+
+```bash
+# 添加当前用户到 dialout 组（需要重新登录生效）
+sudo usermod -a -G dialout $USER
+
+# 查看串口设备
+ls -l /dev/ttyUSB*
+```
+
+### 步骤 3：配置机械手型号
+
+编辑 `config/base_config.yml`，设置机械手型号：
+
+```yaml
+system:
+ motion_type: linkerforce # 数据手套类型：linkerforce（必须）
+ robotname_r: l25 # 右手机械手型号：o6 / l7 / l10 / g20 / r20 / l25
+ robotname_l: l25 # 左手机械手型号
+
+serial:
+ auto_scan: false # 是否自动扫描串口
+ baudrates: [2000000, 460800, 1000000, 921600] # 波特率列表，2000000 优先
+ left:
+ port: /dev/ttyUSB1 # 左手套接的串口
+ baudrate: 460800 # 无线460800 有线2000000
+ right:
+ port: /dev/ttyUSB0 # 右手套接的串口
+ baudrate: 460800 # 无线460800 有线2000000
+```
+
+### 步骤 4：启动
+
+**方式一：直接运行节点**
+
+```bash
+ros2 run linkerhand_retarget handretarget
+```
+
+**方式二：指定串口启动（不修改配置文件）**
+
+```bash
+ros2 run linkerhand_retarget handretarget --ros-args \
+ -p ports:='["/dev/ttyUSB0", "/dev/ttyUSB1"]'
+```
+
+### 步骤 5：标定（如需要）
+
+```bash
+ros2 run linkerhand_retarget handretarget --ros-args -p calibration:=auto_calibrate
+```
+
+标定时按照提示做三个动作：
+1. **五指张开** → 保持 5 秒
+2. **握紧拳头** → 保持 5 秒
+3. **O 型手势** → 保持 5 秒
+
+---
+
+## 机械手型号说明
+
+| 型号 | 关节数 | 说明 |
+|------|--------|------|
+| O6 | 6 | 6 自由度基础款 |
+| L7 | 7 | 7 自由度（拇指增加横滚） |
+| L10 | 10 | 10 自由度工业款 |
+| G20 | 20 | 20 自由度工业款 |
+| L25 | 25 | 25 自由度全功能款 |
+
+配置中的 `robotname_r` 和 `robotname_l` 必须与实际连接的机械手型号匹配。
+
+---
+
+## 串口参数说明
+
+| 参数 | 说明 | 默认值 |
+|------|------|--------|
+| `ports` | 候选串口列表 | 空（使用配置文件） |
+| `baudrate` | 指定波特率（优先级高于配置） | 使用配置文件 |
+| `auto_scan` | 预设失败后是否自动扫描 | `false` |
+
+---
+
+## 自动标定详解
+
+### 标定配置
+
+在 `config/base_config.yml` 中：
+
+```yaml
+calibration:
+ show_fist: true # 是否显示握拳标定步骤
+ fist_extend_ratio: 0.5 # 握拳延伸比例（仅 show_fist=false 时生效）
+```
+
+### 启动标定
+
+```bash
+ros2 run linkerhand_retarget handretarget --ros-args -p calibration:=auto_calibrate
+```
+
+### 标定流程
+
+1. **五指张开** → 保持 5 秒（电机值 255）
+2. **O 型手势** → 保持 5 秒（电机中间值）
+
+如果 `show_fist: true`，还会出现第三步：
+
+3. **握紧拳头** → 保持 5 秒（电机值 0）
+
+### show_fist=true 与 show_fist=false 的区别
+
+| 对比项 | `show_fist: true` | `show_fist: false` |
+|--------|-------------------|-------------------|
+| 标定步骤 | 张开 → O 型 → 握拳（共 3 步） | 张开 → O 型（共 2 步） |
+| 握拳数据来源 | 用户实际做握拳动作采集 | 从 O 型按比例延伸计算 |
+| 握拳延伸公式 | 无 | `fist = opose + (opose - original) × fist_extend_ratio` |
+| 映射精度 | 三段线性插值，最精确 | 两段插值，依赖延伸估算 |
+| fist_extend_ratio | 不生效 | 控制延伸比例（默认 0.5） |
+
+### fist_extend_ratio 详解
+
+仅在 `show_fist: false` 时生效，用于从 O 型自动计算握拳值：
+
+- `fist_extend_ratio = 0.5`（默认）：O 型向握拳方向延伸 50%
+- `fist_extend_ratio = 0.0`：握拳值 = O 型值（无延伸）
+- `fist_extend_ratio = 1.0`：握拳值 = O 型 + (O 型 - 张开) 的全量延伸
+- 推荐值范围：`0.3 ~ 0.7`，需要根据实际效果调整
+
+### 稳定性检测
+
+- 标定时自动检测手势稳定性（方差 < 0.03）
+- 需**连续稳定 5 秒**才完成采集
+- 不稳定时清空重来，无超时限制
+- 终端显示进度条，实时反馈稳定时长
+
+### 标定数据存储
+
+- 存储位置：`motion/linkerforce/tmp/jointangle_data.tmp`
+- 格式：JSON
+- 首次使用自动加载内置样本数据
+- 重新标定会覆盖旧数据
+
+---
+
+## 话题参数控制（运行时动态调整）
+
+通过 `/hand_teleop_param` 话题动态调整运行参数，无需重启节点。
+
+### 可调参数
+
+| 参数 | 说明 | 示例值 |
+|------|------|--------|
+| `mapper_debug` | 映射器调试开关 | `true` / `false` / `["thumb_rotate"]` |
+| `mapper_exp_factor` | 延伸指数因子 | `2.0`（全局）或 `{"thumb_rotate": 2.0}`（单指） |
+| `mapper_scale_factor` | 缩放因子 | `1.5`（全局）或 `{"index_root_flexion": 1.5}`（单指） |
+| `force_glove_pose` | 强制手套数据源 | `open` / `fist` / `opose` / `none` |
+
+### 使用示例
+
+```bash
+# 开启全部手指的映射器调试
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"mapper_debug\": true}"}'
+
+# 只调试拇指相关手指
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"mapper_debug\": [\"thumb_rotate\", \"thumb_abduction\"]}"}'
+
+# 关闭调试
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"mapper_debug\": false}"}'
+
+# 调整全局延伸指数（值越大到达目标越快）
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"mapper_exp_factor\": 2.0}"}'
+
+# 调整单指延伸指数
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"mapper_exp_factor\": {\"thumb_rotate\": 2.0, \"index_root_flexion\": 1.5}}"}'
+
+# 调整全局缩放因子
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"mapper_scale_factor\": 1.5}"}'
+
+# 用标定数据替代手套数据（用于测试）
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"force_glove_pose\": \"opose\"}"}'
+
+# 恢复实时手套数据
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"force_glove_pose\": \"none\"}"}'
+```
+
+### 可用手指名称
+
+| 手指 | 参数名称 |
+|------|----------|
+| 拇指 | `thumb_rotate`, `thumb_abduction`, `thumb_root_flexion`, `thumb_end_flexion` |
+| 食指 | `index_roll`, `index_root_flexion`, `index_end_flexion` |
+| 中指 | `middle_roll`, `middle_root_flexion`, `middle_end_flexion` |
+| 无名指 | `ring_roll`, `ring_root_flexion`, `ring_end_flexion` |
+| 小指 | `pinky_roll`, `pinky_root_flexion`, `pinky_end_flexion` |
+
+---
+
+## 映射参数详解
+
+### 延伸指数因子 (exp_factor)
+
+控制映射延伸到目标姿态的速度：
+- `= 1.0`：线性延伸
+- `> 1.0`：加速延伸（更快到达目标）
+- `< 1.0`：减速延伸（更平滑）
+
+### 缩放因子 (scale_factor)
+
+控制输入到输出的映射比例：
+- `= 1.0`：1:1 映射
+- `> 1.0`：放大输出范围
+- `< 1.0`：缩小输出范围
+
+### 强制手套数据源 (force_glove_pose)
+
+用标定数据替代实时手套数据，用于调试和测试：
+
+| 值 | 说明 |
+|-----|------|
+| `open` | 使用五指张开标定数据 |
+| `fist` | 使用握拳标定数据 |
+| `opose` | 使用 O 型手势标定数据 |
+| `none` | 使用实时手套数据（默认） |
+
+---
+
+## 配置文件详解
+
+完整配置项见主 README 的「配置说明」章节。LinkerFFG 驱动专用配置：
+
+### system 系统配置
+
+| 配置项 | 说明 | 可选值 |
+|--------|------|--------|
+| `motion_type` | 数据手套类型 | `linkerforce`（必须） |
+| `robotname_r` | 右手机械手型号 | `o6`, `l7`, `l10`, `g20`, `r20`, `l25` |
+| `robotname_l` | 左手机械手型号 | 同上 |
+| `retargeting_type` | 重定向类型 | `projection` |
+
+### calibration 标定配置
+
+| 配置项 | 说明 | 默认值 |
+|--------|------|--------|
+| `show_fist` | 是否显示握拳标定步骤 | `true` |
+| `fist_extend_ratio` | 握拳延伸比例 | `0.5` |
+
+### debug 调试配置
+
+| 配置项 | 说明 | 默认值 |
+|--------|------|--------|
+| `mapper_debug` | 映射器调试开关 | `false` |
+| `joint_motor_debug_r` | 右手关节电机调试 | `false` |
+| `joint_motor_debug_l` | 左手关节电机调试 | `false` |
+
+---
+
+## 故障排除
+
+### 串口无法打开
+
+```bash
+# 检查串口权限
+ls -l /dev/ttyUSB*
+# 添加权限
+sudo chmod 666 /dev/ttyUSB0
+```
+
+### 机械手无反应（重点监测话题）
+
+按以下顺序逐项检查：
+
+**步骤 1：检查 LinkerFFG 输出话题**
+
+```bash
+# 查看 LinkerFFG 驱动发布的话题
+ros2 topic list | grep cb_
+
+# 查看是否有数据输出（机械手节点是否正常发布数据）
+ros2 topic echo /cb_right_hand_control_cmd --once
+ros2 topic echo /cb_left_hand_control_cmd --once
+```
+
+**步骤 2：检查手套数据话题**
+
+```bash
+# 查看手套数据是否到达（LinkerFFG 驱动不发布此话题，此步骤可跳过）
+# 如需验证数据源，请检查 /cb_right_hand_control_cmd 是否有数据输出
+ros2 topic echo /cb_right_hand_control_cmd --once
+```
+
+**步骤 3：检查 LinkerFFG 关节控制话题**
+
+```bash
+# 查看关节控制指令是否有输出
+ros2 topic echo /cb_right_hand_control_cmd --once
+ros2 topic echo /cb_left_hand_control_cmd --once
+
+# 查看话题频率是否正常（应该 50Hz 左右）
+ros2 topic hz /cb_right_hand_control_cmd
+```
+
+**步骤 4：启用调试打印**
+
+```bash
+# 开启全部调试
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"mapper_debug\": true}"}'
+
+# 查看终端输出，是否有手套数据变化
+# 如果数据不变，说明手套未连接或话题未发布
+```
+
+**步骤 5：检查标定数据**
+
+```bash
+# 查看当前使用的标定数据
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"force_glove_pose\": \"opose\"}"}'
+# 观察机械手是否有反应
+
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"force_glove_pose\": \"fist\"}"}'
+# 观察机械手是否握紧
+
+ros2 topic pub --once /hand_teleop_param std_msgs/msg/String '{"data": "{\"force_glove_pose\": \"none\"}"}'
+# 恢复正常数据源
+```
+
+**步骤 6：检查串口连接**
+
+1. 确认波特率配置与机械手一致（无线 460800，有线 2000000）
+2. 尝试使用 `auto_scan: true` 自动检测
+3. 检查机械手电源是否正常
+
+**步骤 7：检查日志**
+
+```bash
+# 查看节点日志
+ros2 run linkerhand_retarget handretarget
+# 观察终端输出的 debug 信息
+```
+
+**常见问题快速定位**
+
+| 现象 | 可能原因 | 解决方法 |
+|------|----------|----------|
+| 话题无数据 | 手套未连接或话题名错误 | 检查手套连接，确认话题名 |
+| 数据一直是 0 或 255 | 标定数据异常 | 重新标定或删除 `tmp/jointangle_data.tmp` |
+| 数据跳变剧烈 | 串口信号干扰 | 检查串口线，使用屏蔽线 |
+| 终端无输出 | 节点未启动成功 | 检查是否报错，检查依赖是否安装 |
+| 话题有数据但机械手不动 | 机械手SDK未收到指令 | 检查机械手SDK连接，确认话题被正确订阅 |
+
+---
+
+## 文件结构
+
+```
+motion/linkerforce/
+├── config/ # 手型配置文件
+│ ├── o6_config.py # O6 手型配置
+│ ├── l6_config.py # L6 手型配置
+│ ├── l7_config.py # L7 手型配置
+│ ├── l10_config.py # L10 手型配置
+│ ├── l20_config.py # L20 手型配置
+│ ├── g20_config.py # G20 手型配置
+│ └── o7_config.py # O7 手型配置
+├── hand/ # 机械手驱动
+│ ├── linkerforce_o6.py
+│ ├── linkerforce_l6.py
+│ ├── linkerforce_l7.py
+│ ├── linkerforce_l10.py
+│ ├── linkerforce_l20.py
+│ └── linkerforce_g20.py
+├── tmp/ # 临时文件（标定数据等）
+├── retarget.py # ROS 集成层
+└── README.md # 本文档
+```
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/g20_config.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/g20_config.py
new file mode 100644
index 0000000..4941166
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/g20_config.py
@@ -0,0 +1,475 @@
+# 手指配置常量
+FINGER_CONFIGS = {
+ # 含义解释：
+ # robot_idx：URDF关节序列
+
+ # 拇指旋转3个关节的加权系数，人手的0/1/2序列，对应URDF的第1关节(下标0)
+ 'thumb_rotate': {
+ 'name': '拇指旋转',
+ 'joints': [1, 2],
+ 'weights': {
+ 'v1': [1, 0],
+ 'v2': [1, 0]
+ },
+ 'robot_idx': 0,
+ 'type': 'thumb',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 1.0
+ }
+ },
+ # 拇指侧摆3个关节的加权系数，人手的0/1/2序列，对应URDF的第2关节(下标1)
+ 'thumb_abduction': {
+ 'name': '拇指侧摆',
+ 'joints': [0, 1, 2],
+ 'weights': {
+ 'v1': [0.7, 0.3, 0],
+ 'v2': [0.7, 0.3, 0]
+ },
+ 'robot_idx': 1,
+ 'type': 'thumb',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.2,
+ 'extended_exp_factor': 1.0
+ }
+ },
+ # 拇指根部弯曲3个关节的加权系数，人手的2/3/4序列，对应URDF的第3关节(下标2)
+ 'thumb_root_flexion': {
+ 'name': '拇指根部弯曲',
+ 'joints': [2, 3, 4],
+ 'weights': {
+ 'v1': [1, 0, 0],
+ 'v2': [1, 0, 0]
+ },
+ 'robot_idx': 2,
+ 'type': 'thumb',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ # {
+ # 'trigger_finger': 'thumb_abduction',
+ # 'threshold': 0.3,
+ # 'low_weight_config': {
+ # 'joints': [2, 3, 4],
+ # 'weights': [1, 0, 0],
+ # 'reverse_motion': False
+ # },
+ # 'high_weight_config': {
+ # 'joints': [2, 3, 4],
+ # 'weights': [0.3, 0.0, 0.7],
+ # 'reverse_motion': False
+ # }
+ # },
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.2,
+ 'extended_exp_factor': 30
+ }
+ },
+ # 拇指指尖弯曲3个关节的加权系数，人手的2/3/4序列，对应URDF的第4关节(下标3)
+ 'thumb_end_flexion': {
+ 'name': '拇指指尖弯曲',
+ 'joints': [2, 3, 4],
+ 'weights': {
+ 'v1': [0, 0.0, 1],
+ 'v2': [0, 0.0, 1]
+ },
+ 'robot_idx': 3,
+ 'type': 'thumb',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1,
+ 'extended_exp_factor': 50
+ }
+ },
+ # 食指ROLL旋转（侧摆）关节的加权系数，人手的5序列，对应URDF的第4关节(下标3)
+ 'index_roll': {
+ 'name': '食指',
+ 'joints': [5],
+ 'weights': {
+ 'v1': [1],
+ 'v2': [1]
+ },
+ 'robot_idx': 5,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': False,
+ 'scale_factor': 1.0,
+ }
+ },
+ # 食指弯曲（根部弯曲）的加权系数，人身的6/7/8序列，对应URDF的第4关节(下标3)
+ 'index_root_flexion': {
+ 'name': '食指',
+ 'joints': [6, 7, 8],
+ 'weights': {
+ 'v1': [1, 0.0, 0],
+ 'v2': [1, 0.0, 0]
+ },
+ 'robot_idx': 6,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.2,
+ 'extended_exp_factor': 5
+ }
+ },
+ # 食指弯曲（末端弯曲）的加权系数，人手的6/7/8序列，对应URDF的第4关节(下标3)
+ 'index_end_flexion': {
+ 'name': '食指',
+ 'joints': [6, 7, 8],
+ 'weights': {
+ 'v1': [0, 0.0, 1],
+ 'v2': [0, 0.0, 1]
+ },
+ 'robot_idx': 7,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1,
+ 'extended_exp_factor': 30
+ }
+ },
+ # 中指ROLL旋转（侧摆）关节的加权系数，人手的5序列，对应URDF的第4关节(下标3)
+ 'middle_roll': {
+ 'name': '中指',
+ 'joints': [9],
+ 'weights': {
+ 'v1': [1],
+ 'v2': [1]
+ },
+ 'robot_idx': 9,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 1.0
+ }
+ },
+ # 中指弯曲（根部弯曲）的加权系数，人手的10/11/12序列，对应URDF的第6关节(下标5)
+ 'middle_root_flexion': {
+ 'name': '中指',
+ 'joints': [10, 11, 12],
+ 'weights': {
+ 'v1': [1, 0.0, 0],
+ 'v2': [1, 0.0, 0]
+ },
+ 'robot_idx': 10,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.2,
+ 'extended_exp_factor': 5
+ }
+ },
+ # 中指弯曲（末端弯曲）的加权系数，人手的10/11/12序列，对应URDF的第6关节(下标5)
+ 'middle_end_flexion': {
+ 'name': '中指',
+ 'joints': [10, 11, 12],
+ 'weights': {
+ 'v1': [0, 0.0, 1],
+ 'v2': [0, 0.0, 1]
+ },
+ 'robot_idx': 11,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1,
+ 'extended_exp_factor': 30
+ }
+ },
+ # 无名指ROLL旋转（侧摆）关节的加权系数，人手的5序列，对应URDF的第4关节(下标3)
+ 'ring_roll': {
+ 'name': '无名指',
+ 'joints': [13],
+ 'weights': {
+ 'v1': [1],
+ 'v2': [1]
+ },
+ 'robot_idx': 13,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': False,
+ 'scale_factor': 1.0,
+ }
+ },
+ # 无名指弯曲（根部弯曲）的加权系数，人手的14/15/16序列，对应URDF的第8关节(下标7)
+ 'ring_root_flexion': {
+ 'name': '无名指',
+ 'joints': [14, 15, 16],
+ 'weights': {
+ 'v1': [1, 0.0, 0],
+ 'v2': [1, 0.0, 0]
+ },
+ 'robot_idx': 14,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.2,
+ 'extended_exp_factor': 5
+ }
+ },
+ # 无名指弯曲（末端弯曲）的加权系数，人手的14/15/16序列，对应URDF的第8关节(下标7)
+ 'ring_end_flexion': {
+ 'name': '无名指',
+ 'joints': [14, 15, 16],
+ 'weights': {
+ 'v1': [0, 0.0, 1],
+ 'v2': [0, 0.0, 1]
+ },
+ 'robot_idx': 15,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1,
+ 'extended_exp_factor': 30
+ }
+ },
+ # 小指ROLL旋转（侧摆）关节的加权系数，人手的5序列，对应URDF的第4关节(下标3)
+ 'pinky_roll': {
+ 'name': '小指',
+ 'joints': [17],
+ 'weights': {
+ 'v1': [1],
+ 'v2': [1]
+ },
+ 'robot_idx': 17,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': False,
+ 'scale_factor': 1.0,
+ }
+ },
+ # 小指弯曲（根部弯曲）的加权系数，人手的18/19/20序列，对应URDF的第10关节(下标9)
+ 'pinky_root_flexion': {
+ 'name': '小指',
+ 'joints': [18, 19, 20],
+ 'weights': {
+ 'v1': [1, 0.0, 0],
+ 'v2': [1, 0.0, 0]
+ },
+ 'robot_idx': 18,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.2,
+ 'extended_exp_factor': 5
+ }
+ },
+ # 小指弯曲（末端弯曲）的加权系数，人手的18/19/20序列，对应URDF的第10关节(下标9)
+ 'pinky_end_flexion': {
+ 'name': '小指',
+ 'joints': [18, 19, 20],
+ 'weights': {
+ 'v1': [0, 0.0, 1],
+ 'v2': [0, 0.0, 1]
+ },
+ 'robot_idx': 19,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1,
+ 'extended_exp_factor': 30
+ }
+ }
+}
+
+# 映射顺序
+MAPPING_ORDER = [
+ 'thumb_rotate', 'thumb_abduction', 'thumb_root_flexion', 'thumb_end_flexion',
+ 'index_roll', 'index_root_flexion', 'index_end_flexion',
+ 'middle_roll', 'middle_root_flexion', 'middle_end_flexion',
+ 'ring_roll', 'ring_root_flexion','ring_end_flexion',
+ 'pinky_roll', 'pinky_root_flexion', 'pinky_end_flexion'
+]
+
+# 三态默认配置
+MULTI_SEGMENT_CONFIG = {
+ 'states': [
+ 'original',
+ 'opose',
+ # 'fist' # 取消注释启用三段映射
+ ],
+ 'state_names': {
+ 'original': '张手',
+ 'opose': 'O手势',
+ # 'fist': '握拳'
+ }
+}
+MULTI_SEGMENT_CONFIG_FROZEN = tuple(MULTI_SEGMENT_CONFIG['states'])
+
+ROBOT_ORIGINAL_LEFT = [
+ 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.2, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ -0.2, 0.0, 0.0, 0.0,
+ -0.2, 0.0, 0.0, 0.0
+]
+
+ROBOT_ORIGINAL_RIGHT = [
+ 0.0, 0.0, 0.0, 0.0, 0.0,
+ -0.2, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0, 0.0,
+ 0.2, 0.0, 0.0, 0.0,
+ 0.2, 0.0, 0.0, 0.0
+]
+
+ROBOT_OPOSE_LEFT = [
+ 0.6, 1.2, 0.5, 0.6, 0.0,
+ 0.0, 0.7, 1.08, 0.00,
+ 0.0, 0.7, 1.08, 0.00 ,
+ 0.0, 0.7, 1.08, 0.00,
+ 0.0, 0.7, 1.08, 0.00
+]
+
+ROBOT_OPOSE_RIGHT = [
+ 0.6, 1.2, 0.5, 0.6, 0.0,
+ 0.0, 0.7, 1.08, 0.00,
+ 0.0, 0.7, 1.08, 0.00,
+ 0.0, 0.7, 1.08, 0.00,
+ 0.0, 0.7, 1.08, 0.00
+]
+
+ROBOT_FIST_RIGHT = [
+ 1.39, 1.57, 0.83, 1.25, 1.29,
+ 0, 1.22, 1.75, 1.55,
+ 0, 1.22, 1.75, 1.55,
+ 0, 1.22, 1.75, 1.55,
+ 0, 1.22, 1.75, 1.55
+]
+
+ROBOT_FIST_LEFT = [
+ 1.39, 1.57, 0.83, 1.25, 1.29,
+ 0, 1.22, 1.75, 1.55,
+ 0, 1.22, 1.75, 1.55,
+ 0, 1.22, 1.75, 1.55,
+ 0, 1.22, 1.75, 1.55
+]
+
+# 电机输出约束配置 (20电机)
+MOTOR_CONSTRAINTS = {
+ 'left': [
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ # Left G20 thumb abduction is enabled by hand_config.yml. Keep the
+ # physically verified command away from the mechanical end stop.
+ {'min': 20, 'max': 255, 'enabled': True},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 80, 'max': 255, 'enabled': True},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ ],
+ 'right': [
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 80, 'max': 255, 'enabled': True},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ ]
+}
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/l10_config.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/l10_config.py
new file mode 100644
index 0000000..05936b2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/l10_config.py
@@ -0,0 +1,297 @@
+FINGER_CONFIGS = {
+ 'thumb_rotate': {
+ 'name': '拇指旋转',
+ 'joints': [1, 2],
+ 'weights': {
+ 'v1': [1, 0],
+ 'v2': [1, 0]
+ },
+ 'robot_idx': 0,
+ 'type': 'thumb',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 1
+ }
+ },
+ 'thumb_abduction': {
+ 'name': '拇指侧摆',
+ 'joints': [0, 1, 2],
+ 'weights': {
+ 'v1': [0, 0, 1],
+ 'v2': [0, 1, 0]
+ },
+ 'robot_idx': 1,
+ 'type': 'thumb',
+ 'reverse_motion': False,
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.3,
+ 'extended_exp_factor': 10
+ }
+ },
+ 'thumb_root_flexion': {
+ 'name': '拇指弯曲',
+ 'joints': [2, 3, 4],
+ 'weights': {
+ 'v1': [0.2, 0, 0.8],
+ 'v2': [0.6, 0, 0.4]
+ },
+ 'robot_idx': 2,
+ 'type': 'thumb',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 2
+ }
+ },
+ 'index_roll': {
+ 'name': '食指',
+ 'joints': [5],
+ 'weights': {
+ 'v1': [1],
+ 'v2': [1]
+ },
+ 'robot_idx': 5,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': False,
+ 'scale_factor': 1.0,
+ }
+ },
+ 'index_root_flexion': {
+ 'name': '食指',
+ 'joints': [6, 7, 8],
+ 'weights': {
+ 'v1': [1, 0, 0],
+ 'v2': [1, 0, 0]
+ },
+ 'robot_idx': 6,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 25
+ }
+ },
+ 'middle_root_flexion': {
+ 'name': '中指',
+ 'joints': [10, 11, 12],
+ 'weights': {
+ 'v1': [1, 0, 0],
+ 'v2': [1, 0, 0]
+ },
+ 'robot_idx': 9,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 25
+ }
+ },
+ 'ring_roll': {
+ 'name': '无名指',
+ 'joints': [13],
+ 'weights': {
+ 'v1': [1],
+ 'v2': [1]
+ },
+ 'robot_idx': 12,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': True
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': False,
+ 'scale_factor': 1.0,
+ }
+ },
+ 'ring_root_flexion': {
+ 'name': '无名指',
+ 'joints': [14, 15, 16],
+ 'weights': {
+ 'v1': [1, 0, 0],
+ 'v2': [1, 0, 0]
+ },
+ 'robot_idx': 13,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 25
+ }
+ },
+ 'pinky_roll': {
+ 'name': '小指',
+ 'joints': [17],
+ 'weights': {
+ 'v1': [1],
+ 'v2': [1]
+ },
+ 'robot_idx': 16,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': False,
+ 'scale_factor': 1.0,
+ }
+ },
+ 'pinky_root_flexion': {
+ 'name': '小指',
+ 'joints': [18, 19, 20],
+ 'weights': {
+ 'v1': [1, 0, 0],
+ 'v2': [1, 0, 0]
+ },
+ 'robot_idx': 17,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.2,
+ 'extended_exp_factor': 30
+ }
+ }
+}
+
+MAPPING_ORDER = [
+ 'thumb_rotate', 'thumb_abduction', 'thumb_root_flexion',
+ 'index_roll','index_root_flexion',
+ 'middle_root_flexion',
+ 'ring_roll', 'ring_root_flexion',
+ 'pinky_roll', 'pinky_root_flexion',
+
+]
+
+MULTI_SEGMENT_CONFIG = {
+ 'states': [
+ 'original',
+ 'opose',
+ # 'fist'
+ ],
+ 'state_names': {
+ 'original': '张手',
+ 'opose': 'O手势',
+ # 'fist': '握拳'
+ }
+}
+MULTI_SEGMENT_CONFIG_FROZEN = tuple(MULTI_SEGMENT_CONFIG['states'])
+
+ROBOT_OPOSE_LEFT = [
+ 0.13, 1.13, 0.28, 0.0, 0.0,
+ 0.0, 0.73, 0.0, 0.0,
+ 0.73, 0.0, 0.0,
+ 0.0, 0.73, 0.0, 0.0,
+ 0.0, 0.73, 0.0, 0.0
+]
+
+ROBOT_OPOSE_RIGHT = [
+ 0.13, 1.13, 0.28, 0.0, 0.0,
+ 0.0, 0.73, 0.0, 0.0,
+ 0.73, 0.0, 0.0,
+ 0.0, 0.73, 0.0, 0.0,
+ 0.0, 0.73, 0.0, 0.0
+]
+
+ROBOT_ORIGINAL_LEFT = [
+ 0.0, 0.0, 0.0, 0.0, 0.0,
+ 0.22, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0,
+ -0.22, 0.0, 0.0, 0.0,
+ -0.22, 0.0, 0.0, 0.0
+]
+
+ROBOT_ORIGINAL_RIGHT = [
+ 0.0, 0.0, 0.0, 0.0, 0.0,
+ -0.22, 0.0, 0.0, 0.0,
+ 0.0, 0.0, 0.0,
+ 0.22, 0.0, 0.0, 0.0,
+ 0.22, 0.0, 0.0, 0.0
+]
+
+ROBOT_FIST_LEFT = [
+ 1.1339, 1.9189, 0.5146, 0.7152, 0.7763,
+ 0, 1.3607, 1.8317, 1.8317,
+ 1.3607, 1.8317, 0.628,
+ 0, 1.3607, 1.8317, 0.628,
+ 0, 1.3607, 1.8317, 0.628
+]
+
+ROBOT_FIST_RIGHT = [
+ 1.1339, 1.9189, 0.5146, 0.7152, 0.7763,
+ 0, 1.3607, 1.8317, 1.8317,
+ 1.3607, 1.8317, 0.628,
+ 0, 1.3607, 1.8317, 0.628,
+ 0, 1.3607, 1.8317, 0.628
+]
+
+# 电机输出约束配置
+# 格式: {'min': 最小值, 'max': 最大值, 'enabled': 是否启用}
+# None 表示不约束该电机
+MOTOR_CONSTRAINTS = {
+ 'left': [
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 0: 拇指弯曲
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 1: 拇指侧摆
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 2: 食指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 3: 中指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 4: 无名指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 5: 小指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 6: 食指侧摆
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 7: 无名指侧摆
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 8: 小指侧摆
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 9: 拇指旋转
+ ],
+ 'right': [
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 0: 拇指弯曲
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 1: 拇指侧摆
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 2: 食指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 3: 中指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 4: 无名指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 5: 小指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 6: 食指侧摆
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 7: 无名指侧摆
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 8: 小指侧摆
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 9: 拇指旋转
+ ]
+}
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/l20_config.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/l20_config.py
new file mode 100644
index 0000000..414e2a0
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/l20_config.py
@@ -0,0 +1,434 @@
+# 手指配置常量
+FINGER_CONFIGS = {
+ 'thumb_rotate': {
+ 'name': '拇指旋转',
+ 'joints': [1, 2],
+ 'weights': {
+ 'v1': [1, 0],
+ 'v2': [1, 0]
+ },
+ 'robot_idx': 0,
+ 'type': 'thumb',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 1.5
+ }
+ },
+ 'thumb_abduction': {
+ 'name': '拇指侧摆',
+ 'joints': [0, 1, 2],
+ 'weights': {
+ 'v1': [0.7, 0.3, 0],
+ 'v2': [0.7, 0.3, 0]
+ },
+ 'robot_idx': 1,
+ 'type': 'thumb',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.2,
+ 'extended_exp_factor': 5
+ }
+ },
+ 'thumb_root_flexion': {
+ 'name': '拇指根部弯曲',
+ 'joints': [2, 3, 4],
+ 'weights': {
+ 'v1': [1, 0, 0],
+ 'v2': [1, 0, 0]
+ },
+ 'robot_idx': 2,
+ 'type': 'thumb',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': {
+ 'trigger_finger': 'thumb_abduction',
+ 'threshold': 0.3,
+ 'low_weight_config': {
+ 'joints': [2, 3, 4],
+ 'weights': {'v1': [1, 0, 0], 'v2': [1, 0, 0]},
+ 'reverse_motion': {'v1': False, 'v2': False}
+ },
+ 'high_weight_config': {
+ 'joints': [2, 3, 4],
+ 'weights': {'v1': [0.3, 0.0, 0.7], 'v2': [0.3, 0.0, 0.7]},
+ 'reverse_motion': {'v1': False, 'v2': False}
+ }
+ },
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.2,
+ 'extended_exp_factor': 5
+ }
+ },
+ 'thumb_end_flexion': {
+ 'name': '拇指指尖弯曲',
+ 'joints': [2, 3, 4],
+ 'weights': {
+ 'v1': [0, 0.0, 1],
+ 'v2': [0, 0.0, 1]
+ },
+ 'robot_idx': 3,
+ 'type': 'thumb',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 0.9,
+ 'extended_exp_factor': 1.5
+ }
+ },
+ 'index_roll': {
+ 'name': '食指',
+ 'joints': [5],
+ 'weights': {
+ 'v1': [1],
+ 'v2': [1]
+ },
+ 'robot_idx': 5,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': False,
+ 'scale_factor': 1.0,
+ }
+ },
+ 'index_root_flexion': {
+ 'name': '食指',
+ 'joints': [6, 7, 8],
+ 'weights': {
+ 'v1': [1, 0.0, 0],
+ 'v2': [1, 0.0, 0]
+ },
+ 'robot_idx': 6,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.2,
+ 'extended_exp_factor': 5
+ }
+ },
+ 'index_end_flexion': {
+ 'name': '食指',
+ 'joints': [6, 7, 8],
+ 'weights': {
+ 'v1': [0, 0.0, 1],
+ 'v2': [0, 0.0, 1]
+ },
+ 'robot_idx': 7,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 0.9,
+ 'extended_exp_factor': 1.5
+ }
+ },
+ 'middle_roll': {
+ 'name': '中指',
+ 'joints': [9],
+ 'weights': {
+ 'v1': [1],
+ 'v2': [1]
+ },
+ 'robot_idx': 9,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 5
+ }
+ },
+ 'middle_root_flexion': {
+ 'name': '中指',
+ 'joints': [10, 11, 12],
+ 'weights': {
+ 'v1': [1, 0.0, 0],
+ 'v2': [1, 0.0, 0]
+ },
+ 'robot_idx': 10,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.2,
+ 'extended_exp_factor': 5
+ }
+ },
+ 'middle_end_flexion': {
+ 'name': '中指',
+ 'joints': [10, 11, 12],
+ 'weights': {
+ 'v1': [0, 0.0, 1],
+ 'v2': [0, 0.0, 1]
+ },
+ 'robot_idx': 11,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 0.9,
+ 'extended_exp_factor': 1.5
+ }
+ },
+ 'ring_roll': {
+ 'name': '无名指',
+ 'joints': [13],
+ 'weights': {
+ 'v1': [1],
+ 'v2': [1]
+ },
+ 'robot_idx': 13,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': False,
+ 'scale_factor': 1.0,
+ }
+ },
+ 'ring_root_flexion': {
+ 'name': '无名指',
+ 'joints': [14, 15, 16],
+ 'weights': {
+ 'v1': [1, 0.0, 0],
+ 'v2': [1, 0.0, 0]
+ },
+ 'robot_idx': 14,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.2,
+ 'extended_exp_factor': 5
+ }
+ },
+ 'ring_end_flexion': {
+ 'name': '无名指',
+ 'joints': [14, 15, 16],
+ 'weights': {
+ 'v1': [0, 0.0, 1],
+ 'v2': [0, 0.0, 1]
+ },
+ 'robot_idx': 15,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 0.9,
+ 'extended_exp_factor': 1.5
+ }
+ },
+ 'pinky_roll': {
+ 'name': '小指',
+ 'joints': [17],
+ 'weights': {
+ 'v1': [1],
+ 'v2': [1]
+ },
+ 'robot_idx': 17,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': False,
+ 'scale_factor': 1.0,
+ }
+ },
+ 'pinky_root_flexion': {
+ 'name': '小指',
+ 'joints': [18, 19, 20],
+ 'weights': {
+ 'v1': [1, 0.0, 0],
+ 'v2': [1, 0.0, 0]
+ },
+ 'robot_idx': 18,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.2,
+ 'extended_exp_factor': 5
+ }
+ },
+ 'pinky_end_flexion': {
+ 'name': '小指',
+ 'joints': [18, 19, 20],
+ 'weights': {
+ 'v1': [0, 0.0, 1],
+ 'v2': [0, 0.0, 1]
+ },
+ 'robot_idx': 19,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 0.9,
+ 'extended_exp_factor': 1.5
+ }
+ }
+}
+
+MAPPING_ORDER = [
+ 'thumb_rotate', 'thumb_abduction', 'thumb_root_flexion', 'thumb_end_flexion',
+ 'index_roll', 'index_root_flexion', 'index_end_flexion',
+ 'middle_roll', 'middle_root_flexion', 'middle_end_flexion',
+ 'ring_roll', 'ring_root_flexion','ring_end_flexion',
+ 'pinky_roll', 'pinky_root_flexion', 'pinky_end_flexion'
+]
+
+MULTI_SEGMENT_CONFIG = {
+ 'states': [
+ 'original',
+ 'opose',
+ 'fist'
+ ],
+ 'state_names': {
+ 'original': '张手',
+ 'opose': 'O手势',
+ 'fist': '握拳'
+ }
+}
+MULTI_SEGMENT_CONFIG_FROZEN = tuple(MULTI_SEGMENT_CONFIG['states'])
+
+ROBOT_ORIGINAL_LEFT = [
+ 0.0, 0.0, 0.0, 0.0, 0.0, -0.15, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.15, 0.0, 0.0, 0.0, -0.15, 0.0, 0.0, 0.0
+]
+
+ROBOT_ORIGINAL_RIGHT = [
+ 0.0, 0.0, 0.0, 0.0, 0.0, 0.15, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.15, 0.0, 0.0, 0.0, 0.15, 0.0, 0.0, 0.0
+]
+
+ROBOT_FIST_RIGHT = [
+ 0.5, 1.57, 0.6, 1.2, 1.2,
+ 0.18, 1.33, 1.77, 1.43,
+ 0.18, 1.33, 1.77, 1.43,
+ 0.18, 1.33, 1.77, 1.43,
+ 0.18, 1.33, 1.77, 1.43
+]
+
+ROBOT_FIST_LEFT = [
+ 0.5, 1.57, 0.6, 1.2, 1.2,
+ 0.18, 1.33, 1.77, 1.43,
+ 0.18, 1.33, 1.77, 1.43,
+ 0.18, 1.33, 1.77, 1.43,
+ 0.18, 1.33, 1.77, 1.43
+]
+
+ROBOT_OPOSE_LEFT = [
+ 0.0, 1.2, 0.3, 0.8, 0.0, 0.0, 0.65, 1.0, 0.0, 0.0, 0.65, 1.0, 0.0, 0.0, 0.65, 1.0, 0.0, 0.0, 0.65, 1.0, 0.0
+]
+
+ROBOT_OPOSE_RIGHT = [
+ 0.0, 1.2, 0.3, 0.8, 0.0, 0.0, 0.65, 1.0, 0.0, 0.0, 0.65, 1.0, 0.0, 0.0, 0.65, 1.0, 0.0, 0.0, 0.65, 1.0, 0.0
+]
+
+MOTOR_CONSTRAINTS = {
+ 'left': [
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': True},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': True},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ ],
+ 'right': [
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': True},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': True},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ {'min': 0, 'max': 255, 'enabled': False},
+ ]
+}
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/l6_config.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/l6_config.py
new file mode 100644
index 0000000..9409707
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/l6_config.py
@@ -0,0 +1,209 @@
+FINGER_CONFIGS = {
+ 'thumb_abduction': {
+ 'name': '拇指侧摆',
+ 'joints': [0, 1, 2],
+ 'weights': {
+ 'v1': [0, 0, 1],
+ 'v2': [0, 1, 0]
+ },
+ 'robot_idx': 0,
+ 'type': 'thumb',
+ 'reverse_motion': False,
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.3,
+ 'extended_exp_factor': 10
+ }
+ },
+ 'thumb_root_flexion': {
+ 'name': '拇指弯曲',
+ 'joints': [2, 3, 4],
+ 'weights': {
+ 'v1': [0.2, 0, 0.8],
+ 'v2': [0.6, 0, 0.4]
+ },
+ 'robot_idx': 1,
+ 'type': 'thumb',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 2
+ }
+ },
+ 'index_root_flexion': {
+ 'name': '食指',
+ 'joints': [6, 7, 8],
+ 'weights': {
+ 'v1': [1, 0, 0],
+ 'v2': [1, 0, 0]
+ },
+ 'robot_idx': 3,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 5
+ }
+ },
+ 'middle_root_flexion': {
+ 'name': '中指',
+ 'joints': [10, 11, 12],
+ 'weights': {
+ 'v1': [1, 0, 0],
+ 'v2': [1, 0, 0]
+ },
+ 'robot_idx': 5,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 5
+ }
+ },
+ 'ring_root_flexion': {
+ 'name': '无名指',
+ 'joints': [14, 15, 16],
+ 'weights': {
+ 'v1': [1, 0, 0],
+ 'v2': [1, 0, 0]
+ },
+ 'robot_idx': 7,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 5
+ }
+ },
+ 'pinky_root_flexion': {
+ 'name': '小指',
+ 'joints': [18, 19, 20],
+ 'weights': {
+ 'v1': [1, 0, 0],
+ 'v2': [1, 0, 0]
+ },
+ 'robot_idx': 9,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.2,
+ 'extended_exp_factor': 5
+ }
+ }
+}
+
+MAPPING_ORDER = [
+ 'thumb_abduction', 'thumb_root_flexion',
+ 'index_root_flexion', 'middle_root_flexion', 'ring_root_flexion', 'pinky_root_flexion'
+]
+
+MULTI_SEGMENT_CONFIG = {
+ 'states': [
+ 'original',
+ # 'opose',
+ 'fist'
+ ],
+ 'state_names': {
+ 'original': '张手',
+ # 'opose': 'O手势',
+ 'fist': '握拳'
+ }
+}
+MULTI_SEGMENT_CONFIG_FROZEN = tuple(MULTI_SEGMENT_CONFIG['states'])
+
+ROBOT_OPOSE_LEFT = [
+ 1.4, 0.5, 0.0,
+ 0.48, 0.0,
+ 0.48, 0.0,
+ 0.48, 0.0,
+ 0.48, 0.0
+]
+
+ROBOT_OPOSE_RIGHT = [
+ 1.4, 0.5, 0.0,
+ 0.48, 0.0,
+ 0.48, 0.0,
+ 0.48, 0.0,
+ 0.48, 0.0
+]
+
+ROBOT_ORIGINAL_LEFT = [
+ 0.0, 0.0, 0.0,
+ 0.0, 0.0,
+ 0.0, 0.0,
+ 0.0, 0.0,
+ 0.0, 0.0
+]
+
+ROBOT_ORIGINAL_RIGHT = [
+ 0.0, 0.0, 0.0,
+ 0.0, 0.0,
+ 0.0, 0.0,
+ 0.0, 0.0,
+ 0.0, 0.0
+]
+
+# 握拳姿态 (使用 URDF upper limit)
+ROBOT_FIST_LEFT = [
+ 1.53, 0.73, 0.66,
+ 1.22, 1.08,
+ 1.22, 1.08,
+ 1.22, 1.08,
+ 1.22, 1.08
+]
+
+ROBOT_FIST_RIGHT = [
+ 1.53, 0.73, 0.66,
+ 1.22, 1.08,
+ 1.22, 1.08,
+ 1.22, 1.08,
+ 1.22, 1.08
+]
+
+# 电机输出约束配置
+# 格式: {'min': 最小值, 'max': 最大值, 'enabled': 是否启用}
+# None 表示不约束该电机
+MOTOR_CONSTRAINTS = {
+ 'left': [
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 0: 拇指弯曲
+ {'min': 15, 'max': 255, 'enabled': True}, # motor 1: 拇指侧摆
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 2: 食指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 3: 中指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 4: 无名指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 5: 小指
+ ],
+ 'right': [
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 0: 拇指弯曲
+ {'min': 18, 'max': 255, 'enabled': True}, # motor 1: 拇指侧摆
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 2: 食指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 3: 中指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 4: 无名指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 5: 小指
+ ]
+}
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/o6_config.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/o6_config.py
new file mode 100644
index 0000000..0ee2f65
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/o6_config.py
@@ -0,0 +1,187 @@
+# 手指配置常量
+FINGER_CONFIGS = {
+ 'thumb_abduction': {
+ 'name': '拇指侧摆',
+ 'joints': [0, 1, 2],
+ 'weights': {
+ 'v1': [0, 0, 1],
+ 'v2': [0, 1, 0]
+ },
+ 'robot_idx': 0,
+ 'type': 'thumb',
+ 'reverse_motion': False,
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.2,
+ 'extended_exp_factor': 10
+ }
+ },
+ 'thumb_root_flexion': {
+ 'name': '拇指弯曲',
+ 'joints': [2, 3, 4],
+ 'weights': {
+ 'v1': [0, 0, 1],
+ 'v2': [0.2, 0, 0.8]
+ },
+ 'robot_idx': 1,
+ 'type': 'thumb',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 10
+ }
+ },
+ 'index_root_flexion': {
+ 'name': '食指',
+ 'joints': [6, 7, 8],
+ 'weights': {
+ 'v1': [1.0, 0.0, 0.0],
+ 'v2': [1.0, 0.0, 0.0]
+ },
+ 'robot_idx': 3,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 10
+ }
+ },
+ 'middle_root_flexion': {
+ 'name': '中指',
+ 'joints': [10, 11, 12],
+ 'weights': {
+ 'v1': [1.0, 0.0, 0.0],
+ 'v2': [1.0, 0.0, 0.0]
+ },
+ 'robot_idx': 5,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 10
+ }
+ },
+ 'ring_root_flexion': {
+ 'name': '无名指',
+ 'joints': [14, 15, 16],
+ 'weights': {
+ 'v1': [1.0, 0.0, 0.0],
+ 'v2': [1.0, 0.0, 0.0]
+ },
+ 'robot_idx': 7,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 10
+ }
+ },
+ 'pinky_root_flexion': {
+ 'name': '小指',
+ 'joints': [18, 19, 20],
+ 'weights': {
+ 'v1': [1.0, 0.0, 0.0],
+ 'v2': [1.0, 0.0, 0.0]
+ },
+ 'robot_idx': 9,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 10
+ }
+ }
+}
+
+MAPPING_ORDER = [
+ 'thumb_abduction', 'thumb_root_flexion',
+ 'index_root_flexion', 'middle_root_flexion', 'ring_root_flexion', 'pinky_root_flexion'
+]
+
+MULTI_SEGMENT_CONFIG = {
+ 'states': [
+ 'original',
+ # 'opose',
+ 'fist'
+ ],
+ 'state_names': {
+ 'original': '张手',
+ # 'opose': 'O手势',
+ 'fist': '握拳'
+ }
+}
+MULTI_SEGMENT_CONFIG_FROZEN = tuple(MULTI_SEGMENT_CONFIG['states'])
+
+ROBOT_OPOSE_LEFT = [
+ 1.1, 0.33, 0.0, 0.84, 0.0, 0.84, 0.0, 0.84, 0.0, 0.84, 0.0
+]
+
+ROBOT_OPOSE_RIGHT = [
+ 1.1, 0.33, 0.0, 0.84, 0.0, 0.84, 0.0, 0.84, 0.0, 0.84, 0.0
+]
+
+ROBOT_ORIGINAL_LEFT = [
+ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+]
+
+ROBOT_ORIGINAL_RIGHT = [
+ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+]
+
+ROBOT_FIST_LEFT = [
+ 1.54, 0.52, 0.96, 1.57, 1.4, 1.57, 1.4, 1.57, 1.4, 1.57, 1.4
+]
+
+ROBOT_FIST_RIGHT = [
+ 1.54, 0.52, 0.96, 1.57, 1.4, 1.57, 1.4, 1.57, 1.4, 1.57, 1.4
+]
+
+PLOTGUI_ROBOT_ID = [
+ 0, 1, 2
+]
+
+# 电机输出约束配置 (6电机)
+MOTOR_CONSTRAINTS = {
+ 'left': [
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 0: 拇指侧摆
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 1: 拇指根部
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 2: 食指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 3: 中指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 4: 无名指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 5: 小指
+ ],
+ 'right': [
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 0: 拇指侧摆
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 1: 拇指根部
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 2: 食指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 3: 中指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 4: 无名指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 5: 小指
+ ]
+}
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/o7_config.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/o7_config.py
new file mode 100644
index 0000000..d177aa4
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/o7_config.py
@@ -0,0 +1,217 @@
+# 手指配置常量
+FINGER_CONFIGS = {
+ 'thumb_rotate': {
+ 'name': '拇指旋转',
+ 'joints': [1, 2],
+ 'weights': {
+ 'v1': [0.3, 0.7],
+ 'v2': [0.3, 0.7]
+ },
+ 'robot_idx': 0,
+ 'type': 'thumb',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 5
+ }
+ },
+ 'thumb_abduction': {
+ 'name': '拇指侧摆',
+ 'joints': [0, 1, 2],
+ 'weights': {
+ 'v1': [0, 0, 1],
+ 'v2': [0, 1, 0]
+ },
+ 'robot_idx': 1,
+ 'type': 'thumb',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.3,
+ 'extended_exp_factor': 10
+ }
+ },
+ 'thumb_root_flexion': {
+ 'name': '拇指弯曲',
+ 'joints': [2, 3, 4],
+ 'weights': {
+ 'v1': [0.2, 0, 0.8],
+ 'v2': [0.6, 0, 0.4]
+ },
+ 'robot_idx': 2,
+ 'type': 'thumb',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 2
+ }
+ },
+ 'index_root_flexion': {
+ 'name': '食指',
+ 'joints': [6, 7, 8],
+ 'weights': {
+ 'v1': [1, 0, 0],
+ 'v2': [1, 0, 0]
+ },
+ 'robot_idx': 5,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 25
+ }
+ },
+ 'middle_root_flexion': {
+ 'name': '中指',
+ 'joints': [10, 11, 12],
+ 'weights': {
+ 'v1': [1, 0, 0],
+ 'v2': [1, 0, 0]
+ },
+ 'robot_idx': 8,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 25
+ }
+ },
+ 'ring_root_flexion': {
+ 'name': '无名指',
+ 'joints': [14, 15, 16],
+ 'weights': {
+ 'v1': [1, 0, 0],
+ 'v2': [1, 0, 0]
+ },
+ 'robot_idx': 11,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.0,
+ 'extended_exp_factor': 25
+ }
+ },
+ 'pinky_root_flexion': {
+ 'name': '小指',
+ 'joints': [18, 19, 20],
+ 'weights': {
+ 'v1': [1, 0, 0],
+ 'v2': [1, 0, 0]
+ },
+ 'robot_idx': 14,
+ 'type': 'finger',
+ 'reverse_motion': {
+ 'v1': False,
+ 'v2': False
+ },
+ 'dynamic_weight': None,
+ 'extended_mapping': {
+ 'enabled': True,
+ 'scale_factor': 1.2,
+ 'extended_exp_factor': 30
+ }
+ }
+}
+
+MAPPING_ORDER = [
+ 'thumb_rotate', 'thumb_abduction', 'thumb_root_flexion',
+ 'index_root_flexion', 'middle_root_flexion', 'ring_root_flexion', 'pinky_root_flexion'
+]
+
+MULTI_SEGMENT_CONFIG = {
+ 'states': [
+ 'original',
+ 'opose',
+ # 'fist'
+ ],
+ 'state_names': {
+ 'original': '张手',
+ 'opose': 'O手势',
+ # 'fist': '握拳'
+ }
+}
+MULTI_SEGMENT_CONFIG_FROZEN = tuple(MULTI_SEGMENT_CONFIG['states'])
+
+ROBOT_OPOSE_LEFT = [
+ 0.0, 0.8, 0.5, 0.0, 0.0, 0.7, 0.0, 0.0, 0.7, 0.0, 0.0, 0.7, 0.0, 0.0, 0.7
+]
+
+ROBOT_OPOSE_RIGHT = [
+ 0.0, 0.8, 0.5, 0.0, 0.0, 0.7, 0.0, 0.0, 0.7, 0.0, 0.0, 0.7, 0.0, 0.0, 0.7
+]
+
+ROBOT_ORIGINAL_LEFT = [
+ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+]
+
+ROBOT_ORIGINAL_RIGHT = [
+ 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0
+]
+
+ROBOT_FIST_LEFT = [
+ 1.0467, 1.2037, 0.4867, 0.6699, 0.6611,
+ 1.3275, 1.7915, 0.6053,
+ 1.3275, 1.7915, 0.6053,
+ 1.3275, 1.7915, 0.6053,
+ 1.3275, 1.7915, 0.6053
+]
+
+ROBOT_FIST_RIGHT = [
+ 1.0467, 1.2037, 0.4867, 0.6699, 0.6611,
+ 1.3275, 1.7915, 0.6053,
+ 1.3275, 1.7915, 0.6053,
+ 1.3275, 1.7915, 0.6053,
+ 1.3275, 1.7915, 0.6053
+]
+
+# 电机输出约束配置
+# 格式: {'min': 最小值, 'max': 最大值, 'enabled': 是否启用}
+MOTOR_CONSTRAINTS = {
+ 'left': [
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 0: 拇指旋转
+ {'min': 30, 'max': 255, 'enabled': True}, # motor 1: 拇指侧摆
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 2: 拇指弯曲
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 3: 食指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 4: 中指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 5: 无名指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 6: 小指
+ ],
+ 'right': [
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 0: 拇指旋转
+ {'min': 30, 'max': 255, 'enabled': True}, # motor 1: 拇指侧摆
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 2: 拇指弯曲
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 3: 食指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 4: 中指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 5: 无名指
+ {'min': 0, 'max': 255, 'enabled': False}, # motor 6: 小指
+ ]
+}
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/__init__.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_g20.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_g20.py
new file mode 100644
index 0000000..8fcd91c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_g20.py
@@ -0,0 +1,534 @@
+"""
+LinkerForce G20 手型映射模块 - ROS2版本
+支持基于标定数据的精确映射
+v2.8.0升级了映射器算法
+"""
+import numpy as np
+import copy
+from linkerhand.handcore import HandCore
+from ..config.g20_config import FINGER_CONFIGS, MAPPING_ORDER, ROBOT_OPOSE_RIGHT, ROBOT_OPOSE_LEFT, ROBOT_ORIGINAL_LEFT, ROBOT_ORIGINAL_RIGHT, ROBOT_FIST_LEFT, ROBOT_FIST_RIGHT, MULTI_SEGMENT_CONFIG, MULTI_SEGMENT_CONFIG_FROZEN, MOTOR_CONSTRAINTS
+from typing import List
+from linkerhand.handcoreex import DynamicWeightMultiStateLinearMapper,MultiStateLinearMapper
+
+
+# The left G20 ring abduction motor runs opposite to the glove's corresponding
+# roll sensor. Keep this override local to LeftHand so the pinky and right-hand
+# mappings retain their native directions.
+LEFT_REVERSED_SIDE_SWINGS = ('ring_roll',)
+
+
+def _resolve_version_config(configs: dict, version: str) -> dict:
+ """
+ 解析版本配置，将字典格式的 weights/reverse_motion 转换为具体值
+ """
+ resolved = copy.deepcopy(configs)
+ for finger_name, config in resolved.items():
+ if 'weights' in config and isinstance(config['weights'], dict):
+ config['weights'] = config['weights'].get(version, config['weights'].get('v2', [0.5, 0, 0.5]))
+ if 'reverse_motion' in config and isinstance(config['reverse_motion'], dict):
+ config['reverse_motion'] = config['reverse_motion'].get(version, config['reverse_motion'].get('v2', False))
+ return resolved
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=20, is_debug: bool = False):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions_arc = [0] * length
+ self.g_jointvelocity_arc = [0] * length
+ self.handstate = [0] * length
+ self.calibrationoriginal = None # 五指张开标定值 (对应255)
+ self.calibrationfistpose = None # 握拳标定值 (对应0)
+ self.calibrationopose = None # O型标定值 (对应中间值)
+ self.glove_version = 'v2'
+
+ # ========== 平滑滤波参数 ==========
+ self.smooth_enabled = True
+ self.smooth_alpha = 0.5 # 平滑系数：越小越平滑，范围 0.05-0.3
+ self.smooth_positions = [255.0] * length # 平滑后的位置（浮点）
+ self.max_step = 20 # 每帧最大变化量，防止跳变
+
+ # 目标机械手预设姿势，数值从URDF获取数据集，
+ # 张开手的时候对应最小角度，
+ # 握拳的时候对应最大角度
+ # O型手势的时候，用工具驱动URDF去驱动目标机械手达到期望姿势，也可以调整这些参数使得实物更加达到期望角度
+ # 其他手势也类似，也可以增加多个手势来实现多模态的映射器（后期陆续开发）
+ self.robot_original = ROBOT_ORIGINAL_RIGHT
+ self.robot_opose = ROBOT_OPOSE_RIGHT
+ self.robot_fist = ROBOT_FIST_RIGHT
+
+ finger_configs = _resolve_version_config(FINGER_CONFIGS, self.glove_version)
+ self.multi_state_mapper = DynamicWeightMultiStateLinearMapper(finger_configs, MAPPING_ORDER, is_debug=is_debug)
+
+ for config_name, config in FINGER_CONFIGS.items():
+ if config.get('dynamic_weight'):
+ self.multi_state_mapper.set_dynamic_weight_config(config_name, config['dynamic_weight'])
+
+ self.motor_constraints = MOTOR_CONSTRAINTS['right']
+
+ def _apply_motor_constraints(self):
+ for i, constraint in enumerate(self.motor_constraints):
+ if constraint.get('enabled', False):
+ min_val = constraint.get('min', 0)
+ max_val = constraint.get('max', 255)
+ self.g_jointpositions[i] = int(max(min_val, min(max_val, self.g_jointpositions[i])))
+
+ def set_glove_version(self, version: str):
+ if not version:
+ return
+
+ major_version = version.split('.')[0]
+ version_key = f'v{major_version}'
+
+ if version_key == self.glove_version:
+ return
+
+ self.glove_version = version_key
+
+ for finger_name, config in FINGER_CONFIGS.items():
+ if 'weights' in config and isinstance(config['weights'], dict):
+ if version_key in config['weights']:
+ self.multi_state_mapper.finger_configs[finger_name]['weights'] = config['weights'][version_key]
+
+ if 'reverse_motion' in config and isinstance(config['reverse_motion'], dict):
+ if version_key in config['reverse_motion']:
+ self.multi_state_mapper.finger_configs[finger_name]['reverse_motion'] = config['reverse_motion'][version_key]
+
+ def initialize_mapper(self) -> bool:
+ """
+ 初始化映射器
+
+ 将三种人手标定数据和三种机械手标定数据加载到映射器中
+ 分别是original,opose,fist
+
+ 人手是glove_前缀,机械手是robot_前缀
+ """
+
+ glove_original = self._to_list(self.calibrationoriginal)
+ glove_fist = self._to_list(self.calibrationfistpose)
+ glove_opose = self._to_list(self.calibrationopose)
+
+ self.multi_state_mapper.add_state('original', glove_original, self.robot_original)
+ self.multi_state_mapper.add_state('opose', glove_opose, self.robot_opose)
+ self.multi_state_mapper.add_state('fist', glove_fist, self.robot_fist)
+
+ self.multi_state_mapper.set_state_order(list(MULTI_SEGMENT_CONFIG_FROZEN))
+
+ state_info = self.multi_state_mapper.get_state_info()
+
+ def _to_list(self, data):
+ """转换为列表"""
+ if hasattr(data, 'tolist'):
+ return data.tolist()
+ elif isinstance(data, np.ndarray):
+ # print(111)
+ return data.tolist()
+ else:
+ return list(data)
+
+ # V2.8.0 本函数作废
+ # def _linear_map_diff(self, current_diff, fist_diff, extend_ratio=1.2):
+ # """
+ # 基于差值的线性映射到0-255
+
+ # 注意: 传入joint_update的是差值 (当前值 - 张开值)
+
+ # 参数:
+ # current_diff: 当前传感器差值 (当前值 - 张开值)
+ # fist_diff: 握拳时的差值 (握拳值 - 张开值)
+ # extend_ratio: 缩放比例，>1.0 使映射更容易到达0/255边界
+
+ # 映射逻辑：
+ # - 差值为0（张开）→ 255
+ # - 差值为fist_diff（握拳）→ 0
+ # """
+ # if abs(fist_diff) < 0.01:
+ # return 128 # 变化太小，返回中值
+
+ # # 缩小fist_diff使得更容易到达0边界
+ # effective_fist_diff = fist_diff / extend_ratio
+
+ # # 计算比例: 差值0→比例0, 差值fist_diff→比例1
+ # ratio = current_diff / effective_fist_diff
+ # ratio = max(0.0, min(1.0, ratio)) # 限制在0-1之间
+
+ # # 映射: 比例0→255, 比例1→0
+ # return int((1 - ratio) * 255)
+
+ def _apply_smooth(self, raw_positions):
+ """
+ 对电机输出应用平滑滤波，防止跳变
+
+ 使用指数移动平均(EMA) + 最大步长限制
+ """
+ if not self.smooth_enabled:
+ return raw_positions
+
+ smoothed = []
+ for i, raw in enumerate(raw_positions):
+ # 指数移动平均
+ target = self.smooth_alpha * raw + (1 - self.smooth_alpha) * self.smooth_positions[i]
+
+ # 最大步长限制，防止大幅跳变
+ diff = target - self.smooth_positions[i]
+ if abs(diff) > self.max_step:
+ target = self.smooth_positions[i] + (self.max_step if diff > 0 else -self.max_step)
+
+ self.smooth_positions[i] = target
+ smoothed.append(int(round(target)))
+
+ return smoothed
+
+ def joint_update(self, joint_arc):
+ """
+ 右手映射 - 基于标定数据和预期机械手动作的映射器完成
+ """
+ qpos = np.zeros(25)
+ # ========== 使用映射器进行精确映射 ==========
+ if self.calibrationoriginal is not None and self.calibrationfistpose is not None and self.calibrationopose is not None:
+ arc_value = self.multi_state_mapper.map_glove_to_robot(joint_arc)
+ # ========== 没有标定数据时使用手动映射 ==========
+ else:
+ arc_value = None
+
+ if arc_value is not None:
+ qpos[16] = self.g_jointpositions_arc[0] = arc_value[0]
+ qpos[17] = self.g_jointpositions_arc[1] = arc_value[1]
+ qpos[18] = self.g_jointpositions_arc[2] = arc_value[2]
+ qpos[19] = self.g_jointpositions_arc[3] = arc_value[3]
+
+ qpos[0] = self.g_jointpositions_arc[5] = arc_value[5]
+ qpos[1] = self.g_jointpositions_arc[6] = arc_value[6]
+ qpos[2] = self.g_jointpositions_arc[7] = arc_value[7]
+ qpos[3] = self.g_jointpositions_arc[8] = arc_value[8]
+
+ qpos[4] = self.g_jointpositions_arc[17] = arc_value[17]
+ qpos[5] = self.g_jointpositions_arc[18] = arc_value[18]
+ qpos[6] = self.g_jointpositions_arc[19] = arc_value[19]
+ qpos[7] = self.g_jointpositions_arc[4] = arc_value[20]
+
+ qpos[8] = self.g_jointpositions_arc[9] = arc_value[9]
+ qpos[9] = self.g_jointpositions_arc[10] = arc_value[10]
+ qpos[10] = self.g_jointpositions_arc[11] = arc_value[11]
+ qpos[11] = self.g_jointpositions_arc[12] = arc_value[12]
+
+ qpos[12] = self.g_jointpositions_arc[13] = arc_value[13]
+ qpos[13] = self.g_jointpositions_arc[14] = arc_value[14]
+ qpos[14] = self.g_jointpositions_arc[15] = arc_value[15]
+ qpos[15] = self.g_jointpositions_arc[16] = arc_value[16]
+ else:
+ # 手动映射备用
+ qpos[20] = joint_arc[4] * 2.2
+ qpos[17] = joint_arc[2] * -2.5
+ qpos[1] = joint_arc[6] * 0.1 + joint_arc[8] * 0.7
+ qpos[9] = joint_arc[10] * 0.1 + joint_arc[12] * 0.7
+ qpos[13] = joint_arc[14] * 0.1 + joint_arc[16] * 0.7
+ qpos[5] = joint_arc[18] * 0.1 + joint_arc[20] * 0.7
+
+ # ========== 应用平滑滤波 ==========
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+ self._apply_motor_constraints()
+ self.g_jointpositions = self._apply_smooth(self.g_jointpositions)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=20, is_debug: bool = False):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions_arc = [0] * length
+ self.g_jointvelocity_arc = [0] * length
+ self.handstate = [0] * length
+ self.calibrationoriginal = None
+ self.calibrationfistpose = None
+ self.calibrationopose = None
+ self.glove_version = 'v2'
+
+ self.smooth_enabled = True
+ self.smooth_alpha = 0.5
+ self.smooth_positions = [255.0] * length
+ self.max_step = 20
+
+ self.robot_original = ROBOT_ORIGINAL_LEFT
+ self.robot_opose = ROBOT_OPOSE_LEFT
+ self.robot_fist = ROBOT_FIST_LEFT
+
+ finger_configs = _resolve_version_config(FINGER_CONFIGS, self.glove_version)
+ self.multi_state_mapper = DynamicWeightMultiStateLinearMapper(finger_configs, MAPPING_ORDER, is_debug=is_debug)
+ self._apply_side_direction_overrides()
+
+ for config_name, config in FINGER_CONFIGS.items():
+ if config.get('dynamic_weight'):
+ self.multi_state_mapper.set_dynamic_weight_config(config_name, config['dynamic_weight'])
+
+ self.motor_constraints = MOTOR_CONSTRAINTS['left']
+
+ def _apply_side_direction_overrides(self):
+ for finger_name in LEFT_REVERSED_SIDE_SWINGS:
+ self.multi_state_mapper.finger_configs[finger_name]['reverse_motion'] = True
+
+ def _apply_motor_constraints(self):
+ for i, constraint in enumerate(self.motor_constraints):
+ if constraint.get('enabled', False):
+ min_val = constraint.get('min', 0)
+ max_val = constraint.get('max', 255)
+ self.g_jointpositions[i] = int(max(min_val, min(max_val, self.g_jointpositions[i])))
+
+ def set_glove_version(self, version: str):
+ if not version:
+ return
+
+ major_version = version.split('.')[0]
+ version_key = f'v{major_version}'
+
+ if version_key == self.glove_version:
+ return
+
+ self.glove_version = version_key
+
+ for finger_name, config in FINGER_CONFIGS.items():
+ if 'weights' in config and isinstance(config['weights'], dict):
+ if version_key in config['weights']:
+ self.multi_state_mapper.finger_configs[finger_name]['weights'] = config['weights'][version_key]
+
+ if 'reverse_motion' in config and isinstance(config['reverse_motion'], dict):
+ if version_key in config['reverse_motion']:
+ self.multi_state_mapper.finger_configs[finger_name]['reverse_motion'] = config['reverse_motion'][version_key]
+
+ self._apply_side_direction_overrides()
+
+ def initialize_mapper(self) -> bool:
+ """
+ 初始化映射器
+
+ 将三种人手标定数据和三种机械手标定数据加载到映射器中
+ 分别是original,opose,fist
+
+ 人手是glove_前缀,机械手是robot_前缀
+ """
+
+ glove_original = self._to_list(self.calibrationoriginal)
+ glove_fist = self._to_list(self.calibrationfistpose)
+ glove_opose = self._to_list(self.calibrationopose)
+
+ self.multi_state_mapper.add_state('original', glove_original, self.robot_original)
+ self.multi_state_mapper.add_state('opose', glove_opose, self.robot_opose)
+ self.multi_state_mapper.add_state('fist', glove_fist, self.robot_fist)
+
+ self.multi_state_mapper.set_state_order(list(MULTI_SEGMENT_CONFIG_FROZEN))
+
+ state_info = self.multi_state_mapper.get_state_info()
+
+ def _to_list(self, data):
+ """转换为列表"""
+ if hasattr(data, 'tolist'):
+ return data.tolist()
+ elif isinstance(data, np.ndarray):
+ # print(111)
+ return data.tolist()
+ else:
+ return list(data)
+
+ # V2.8.0 本函数作废
+ # def _linear_map_diff(self, current_diff, fist_diff, extend_ratio=1.2):
+ # """
+ # 基于差值的线性映射到0-255
+
+ # 注意: 传入joint_update的是差值 (当前值 - 张开值)
+
+ # 参数:
+ # current_diff: 当前传感器差值 (当前值 - 张开值)
+ # fist_diff: 握拳时的差值 (握拳值 - 张开值)
+ # extend_ratio: 缩放比例，>1.0 使映射更容易到达0/255边界
+
+ # 映射逻辑：
+ # - 差值为0（张开）→ 255
+ # - 差值为fist_diff（握拳）→ 0
+ # """
+ # if abs(fist_diff) < 0.01:
+ # return 128 # 变化太小，返回中值
+
+ # # 缩小fist_diff使得更容易到达0边界
+ # effective_fist_diff = fist_diff / extend_ratio
+
+ # # 计算比例: 差值0→比例0, 差值fist_diff→比例1
+ # ratio = current_diff / effective_fist_diff
+ # ratio = max(0.0, min(1.0, ratio)) # 限制在0-1之间
+
+ # # 映射: 比例0→255, 比例1→0
+ # return int((1 - ratio) * 255)
+
+ def _apply_smooth(self, raw_positions):
+ """
+ 对电机输出应用平滑滤波，防止跳变
+
+ 使用指数移动平均(EMA) + 最大步长限制
+ """
+ if not self.smooth_enabled:
+ return raw_positions
+
+ smoothed = []
+ for i, raw in enumerate(raw_positions):
+ # 指数移动平均
+ target = self.smooth_alpha * raw + (1 - self.smooth_alpha) * self.smooth_positions[i]
+
+ # 最大步长限制，防止大幅跳变
+ diff = target - self.smooth_positions[i]
+ if abs(diff) > self.max_step:
+ target = self.smooth_positions[i] + (self.max_step if diff > 0 else -self.max_step)
+
+ self.smooth_positions[i] = target
+ smoothed.append(int(round(target)))
+
+ return smoothed
+
+ def joint_update(self, joint_arc):
+ """
+ 右手映射 - 基于标定数据和预期机械手动作的映射器完成
+ """
+ qpos = np.zeros(25)
+ # ========== 使用映射器进行精确映射 ==========
+ if self.calibrationoriginal is not None and self.calibrationfistpose is not None and self.calibrationopose is not None:
+ arc_value = self.multi_state_mapper.map_glove_to_robot(joint_arc)
+ # ========== 没有标定数据时使用手动映射 ==========
+ else:
+ arc_value = None
+
+ if arc_value is not None:
+ qpos[16] = self.g_jointpositions_arc[0] = arc_value[0]
+ qpos[17] = self.g_jointpositions_arc[1] = arc_value[1]
+ qpos[18] = self.g_jointpositions_arc[2] = arc_value[2]
+ qpos[19] = self.g_jointpositions_arc[3] = arc_value[3]
+
+ qpos[0] = self.g_jointpositions_arc[5] = arc_value[5]
+ qpos[1] = self.g_jointpositions_arc[6] = arc_value[6]
+ qpos[2] = self.g_jointpositions_arc[7] = arc_value[7]
+ qpos[3] = self.g_jointpositions_arc[8] = arc_value[8]
+
+ qpos[4] = self.g_jointpositions_arc[17] = arc_value[17]
+ qpos[5] = self.g_jointpositions_arc[18] = arc_value[18]
+ qpos[6] = self.g_jointpositions_arc[19] = arc_value[19]
+ qpos[7] = self.g_jointpositions_arc[4] = arc_value[20]
+
+ qpos[8] = self.g_jointpositions_arc[9] = arc_value[9]
+ qpos[9] = self.g_jointpositions_arc[10] = arc_value[10]
+ qpos[10] = self.g_jointpositions_arc[11] = arc_value[11]
+ qpos[11] = self.g_jointpositions_arc[12] = arc_value[12]
+
+ qpos[12] = self.g_jointpositions_arc[13] = arc_value[13]
+ qpos[13] = self.g_jointpositions_arc[14] = arc_value[14]
+ qpos[14] = self.g_jointpositions_arc[15] = arc_value[15]
+ qpos[15] = self.g_jointpositions_arc[16] = arc_value[16]
+ else:
+ # 手动映射备用
+ qpos[20] = joint_arc[4] * 2.2
+ qpos[17] = joint_arc[2] * -2.5
+ qpos[1] = joint_arc[6] * 0.1 + joint_arc[8] * 0.7
+ qpos[9] = joint_arc[10] * 0.1 + joint_arc[12] * 0.7
+ qpos[13] = joint_arc[14] * 0.1 + joint_arc[16] * 0.7
+ qpos[5] = joint_arc[18] * 0.1 + joint_arc[20] * 0.7
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ # print(qpos[4],arc_value[17],self.g_jointpositions[9])
+ self._apply_motor_constraints()
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_l10.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_l10.py
new file mode 100644
index 0000000..af36ca9
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_l10.py
@@ -0,0 +1,387 @@
+"""
+LinkerForce L10 手型映射模块 - ROS2版本
+"""
+import numpy as np
+import copy
+from linkerhand.handcore import HandCore
+from ..config.l10_config import FINGER_CONFIGS, MAPPING_ORDER, ROBOT_OPOSE_RIGHT, ROBOT_OPOSE_LEFT, ROBOT_ORIGINAL_RIGHT, ROBOT_ORIGINAL_LEFT, ROBOT_FIST_RIGHT, ROBOT_FIST_LEFT, MULTI_SEGMENT_CONFIG, MULTI_SEGMENT_CONFIG_FROZEN, MOTOR_CONSTRAINTS
+from typing import List
+from linkerhand.handcoreex import DynamicWeightMultiStateLinearMapper,MultiStateLinearMapper
+
+def _resolve_version_config(configs: dict, version: str) -> dict:
+ """
+ 解析版本配置，将字典格式的 weights/reverse_motion 转换为具体值
+ """
+ resolved = copy.deepcopy(configs)
+ for finger_name, config in resolved.items():
+ if 'weights' in config and isinstance(config['weights'], dict):
+ config['weights'] = config['weights'].get(version, config['weights'].get('v2', [0.5, 0, 0.5]))
+ if 'reverse_motion' in config and isinstance(config['reverse_motion'], dict):
+ config['reverse_motion'] = config['reverse_motion'].get(version, config['reverse_motion'].get('v2', False))
+ return resolved
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=10, is_debug: bool = False):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions_arc = [0] * length
+ self.g_jointvelocity_arc = [0] * length
+ self.handstate = [0] * length
+ self.calibrationoriginal = None
+ self.calibrationfistpose = None
+ self.calibrationopose = None
+ self.glove_version = 'v2'
+
+ self.smooth_enabled = True
+ self.smooth_alpha = 0.8
+ self.smooth_positions = [255.0] * length
+ self.max_step = 40
+
+ self.robot_original = ROBOT_ORIGINAL_RIGHT
+ self.robot_opose = ROBOT_OPOSE_RIGHT
+ self.robot_fist = ROBOT_FIST_RIGHT
+
+ finger_configs = _resolve_version_config(FINGER_CONFIGS, self.glove_version)
+ self.multi_state_mapper = DynamicWeightMultiStateLinearMapper(finger_configs, MAPPING_ORDER, is_debug=is_debug)
+
+ for config_name, config in FINGER_CONFIGS.items():
+ if config.get('dynamic_weight'):
+ self.multi_state_mapper.set_dynamic_weight_config(config_name, config['dynamic_weight'])
+
+ self.motor_constraints = MOTOR_CONSTRAINTS['right']
+
+ def _apply_motor_constraints(self):
+ for i, constraint in enumerate(self.motor_constraints):
+ if constraint.get('enabled', False):
+ min_val = constraint.get('min', 0)
+ max_val = constraint.get('max', 255)
+ self.g_jointpositions[i] = int(max(min_val, min(max_val, self.g_jointpositions[i])))
+
+ def set_glove_version(self, version: str):
+ if not version:
+ return
+
+ major_version = version.split('.')[0]
+ version_key = f'v{major_version}'
+
+ if version_key == self.glove_version:
+ return
+
+ self.glove_version = version_key
+
+ for finger_name, config in FINGER_CONFIGS.items():
+ if 'weights' in config and isinstance(config['weights'], dict):
+ if version_key in config['weights']:
+ self.multi_state_mapper.finger_configs[finger_name]['weights'] = config['weights'][version_key]
+
+ if 'reverse_motion' in config and isinstance(config['reverse_motion'], dict):
+ if version_key in config['reverse_motion']:
+ self.multi_state_mapper.finger_configs[finger_name]['reverse_motion'] = config['reverse_motion'][version_key]
+
+ def initialize_mapper(self) -> bool:
+ glove_original = self._to_list(self.calibrationoriginal)
+ glove_fist = self._to_list(self.calibrationfistpose)
+ glove_opose = self._to_list(self.calibrationopose)
+
+ self.multi_state_mapper.add_state('original', glove_original, self.robot_original)
+ self.multi_state_mapper.add_state('opose', glove_opose, self.robot_opose)
+ self.multi_state_mapper.add_state('fist', glove_fist, self.robot_fist)
+ self.multi_state_mapper.set_state_order(list(MULTI_SEGMENT_CONFIG_FROZEN))
+
+ def _to_list(self, data):
+ """转换为列表"""
+ if hasattr(data, 'tolist'):
+ return data.tolist()
+ elif isinstance(data, np.ndarray):
+ return data.tolist()
+ else:
+ return list(data)
+
+ def _apply_smooth(self, raw_positions):
+ """
+ 对电机输出应用平滑滤波，防止跳变
+
+ 使用指数移动平均(EMA) + 最大步长限制
+ """
+ if not self.smooth_enabled:
+ return raw_positions
+
+ smoothed = []
+ for i, raw in enumerate(raw_positions):
+ # 指数移动平均
+ target = self.smooth_alpha * raw + (1 - self.smooth_alpha) * self.smooth_positions[i]
+
+ # 最大步长限制，防止大幅跳变
+ diff = target - self.smooth_positions[i]
+ if abs(diff) > self.max_step:
+ target = self.smooth_positions[i] + (self.max_step if diff > 0 else -self.max_step)
+
+ self.smooth_positions[i] = target
+ smoothed.append(int(round(target)))
+
+ return smoothed
+
+ def joint_update(self, joint_arc):
+ """
+ 右手映射 - 基于标定数据和预期机械手动作的映射器完成
+ """
+ qpos = np.zeros(25)
+ # ========== 使用映射器进行精确映射 ==========
+ if self.calibrationoriginal is not None and self.calibrationfistpose is not None and self.calibrationopose is not None:
+
+ arc_value = self.multi_state_mapper.map_glove_to_robot(joint_arc)
+ # arc_value = ROBOT_OPOSE_RIGHT
+ qpos[20] = self.g_jointpositions_arc[0] = arc_value[2]
+ qpos[17] = self.g_jointpositions_arc[1] = arc_value[1]
+ qpos[1] = self.g_jointpositions_arc[2] = arc_value[6]
+ qpos[9] = self.g_jointpositions_arc[3] = arc_value[9]
+ qpos[13] = self.g_jointpositions_arc[4] = arc_value[13]
+ qpos[5] = self.g_jointpositions_arc[5] = arc_value[17]
+ qpos[0] = self.g_jointpositions_arc[6] = arc_value[5]
+ qpos[12] = self.g_jointpositions_arc[7] = arc_value[12]
+ qpos[4] = self.g_jointpositions_arc[8] = arc_value[16]
+ qpos[16] = self.g_jointpositions_arc[9] = arc_value[0]
+ else:
+ # 拇指处理
+ qpos[16] = joint_arc[1] * 1.2
+ qpos[17] = joint_arc[0] * -2 + joint_arc[1] * 1.2
+ qpos[20] = joint_arc[4] * 0.5 + joint_arc[2] * 0.8
+
+ qpos[1] = joint_arc[6] * 0.1 + joint_arc[8] * 0.7
+ qpos[9] = joint_arc[10] * 0.1 + joint_arc[12] * 0.7
+ qpos[13] = joint_arc[14] * 0.1 + joint_arc[16] * 0.7
+ qpos[4] = joint_arc[18] * 0.1 + joint_arc[20] * 0.7
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ self._apply_motor_constraints()
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=10, is_debug: bool = False):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions_arc = [0] * length
+ self.g_jointvelocity_arc = [0] * length
+ self.handstate = [0] * length
+ self.calibrationoriginal = None
+ self.calibrationfistpose = None
+ self.calibrationopose = None
+ self.glove_version = 'v2'
+
+ self.smooth_enabled = True
+ self.smooth_alpha = 0.5
+ self.smooth_positions = [255.0] * length
+ self.max_step = 20
+
+ self.robot_original = ROBOT_ORIGINAL_LEFT
+ self.robot_opose = ROBOT_OPOSE_LEFT
+ self.robot_fist = ROBOT_FIST_LEFT
+
+ finger_configs = _resolve_version_config(FINGER_CONFIGS, self.glove_version)
+ self.multi_state_mapper = DynamicWeightMultiStateLinearMapper(finger_configs, MAPPING_ORDER, is_debug=is_debug)
+
+ for config_name, config in FINGER_CONFIGS.items():
+ if config.get('dynamic_weight'):
+ self.multi_state_mapper.set_dynamic_weight_config(config_name, config['dynamic_weight'])
+
+ self.motor_constraints = MOTOR_CONSTRAINTS['left']
+
+ def _apply_motor_constraints(self):
+ for i, constraint in enumerate(self.motor_constraints):
+ if constraint.get('enabled', False):
+ min_val = constraint.get('min', 0)
+ max_val = constraint.get('max', 255)
+ self.g_jointpositions[i] = int(max(min_val, min(max_val, self.g_jointpositions[i])))
+
+ def set_glove_version(self, version: str):
+ if not version:
+ return
+
+ major_version = version.split('.')[0]
+ version_key = f'v{major_version}'
+
+ if version_key == self.glove_version:
+ return
+
+ self.glove_version = version_key
+
+ for finger_name, config in FINGER_CONFIGS.items():
+ if 'weights' in config and isinstance(config['weights'], dict):
+ if version_key in config['weights']:
+ self.multi_state_mapper.finger_configs[finger_name]['weights'] = config['weights'][version_key]
+
+ if 'reverse_motion' in config and isinstance(config['reverse_motion'], dict):
+ if version_key in config['reverse_motion']:
+ self.multi_state_mapper.finger_configs[finger_name]['reverse_motion'] = config['reverse_motion'][version_key]
+
+ def initialize_mapper(self) -> bool:
+ glove_original = self._to_list(self.calibrationoriginal)
+ glove_fist = self._to_list(self.calibrationfistpose)
+ glove_opose = self._to_list(self.calibrationopose)
+
+ self.multi_state_mapper.add_state('original', glove_original, self.robot_original)
+ self.multi_state_mapper.add_state('opose', glove_opose, self.robot_opose)
+ self.multi_state_mapper.add_state('fist', glove_fist, self.robot_fist)
+ self.multi_state_mapper.set_state_order(list(MULTI_SEGMENT_CONFIG_FROZEN))
+
+ def _to_list(self, data):
+ """转换为列表"""
+ if hasattr(data, 'tolist'):
+ return data.tolist()
+ elif isinstance(data, np.ndarray):
+ return data.tolist()
+ else:
+ return list(data)
+
+ def _apply_smooth(self, raw_positions):
+ """
+ 对电机输出应用平滑滤波，防止跳变
+
+ 使用指数移动平均(EMA) + 最大步长限制
+ """
+ if not self.smooth_enabled:
+ return raw_positions
+
+ smoothed = []
+ for i, raw in enumerate(raw_positions):
+ # 指数移动平均
+ target = self.smooth_alpha * raw + (1 - self.smooth_alpha) * self.smooth_positions[i]
+
+ # 最大步长限制，防止大幅跳变
+ diff = target - self.smooth_positions[i]
+ if abs(diff) > self.max_step:
+ target = self.smooth_positions[i] + (self.max_step if diff > 0 else -self.max_step)
+
+ self.smooth_positions[i] = target
+ smoothed.append(int(round(target)))
+
+ return smoothed
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ if self.calibrationoriginal is not None and self.calibrationfistpose is not None and self.calibrationopose is not None:
+ arc_value = self.multi_state_mapper.map_glove_to_robot(joint_arc)
+ qpos[20] = self.g_jointpositions_arc[0] = arc_value[2]
+ qpos[17] = self.g_jointpositions_arc[1] = arc_value[1]
+ qpos[1] = self.g_jointpositions_arc[2] = arc_value[6]
+ qpos[9] = self.g_jointpositions_arc[3] = arc_value[9]
+ qpos[13] = self.g_jointpositions_arc[4] = arc_value[13]
+ qpos[5] = self.g_jointpositions_arc[5] = arc_value[17]
+ qpos[0] = self.g_jointpositions_arc[6] = arc_value[5]
+ qpos[12] = self.g_jointpositions_arc[7] = arc_value[12]
+ qpos[4] = self.g_jointpositions_arc[8] = arc_value[16]
+ qpos[16] = self.g_jointpositions_arc[9] = arc_value[0]
+ else:
+ qpos[16] = joint_arc[1] * 1.2
+ qpos[17] = joint_arc[0] * -2 + joint_arc[1] * 1.2
+ qpos[20] = joint_arc[4] * 0.5 + joint_arc[2] * 0.8
+
+ qpos[1] = joint_arc[6] * 0.1 + joint_arc[8] * 0.7
+ qpos[9] = joint_arc[10] * 0.1 + joint_arc[12] * 0.7
+ qpos[13] = joint_arc[14] * 0.1 + joint_arc[16] * 0.7
+ qpos[5] = joint_arc[18] * 0.1 + joint_arc[20] * 0.7
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+ self._apply_motor_constraints()
+
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_l20.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_l20.py
new file mode 100644
index 0000000..f27162a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_l20.py
@@ -0,0 +1,558 @@
+"""
+LinkerForce L20 手型映射模块 - ROS2版本
+"""
+
+import numpy as np
+import copy
+from linkerhand.handcore import HandCore
+from ..config.l20_config import (
+ FINGER_CONFIGS, MAPPING_ORDER, MULTI_SEGMENT_CONFIG,
+ ROBOT_ORIGINAL_LEFT, ROBOT_ORIGINAL_RIGHT,
+ ROBOT_FIST_LEFT, ROBOT_FIST_RIGHT,
+ ROBOT_OPOSE_LEFT, ROBOT_OPOSE_RIGHT,
+ MOTOR_CONSTRAINTS
+)
+from typing import List
+from linkerhand.handcoreex import DynamicWeightMultiStateLinearMapper,MultiStateLinearMapper
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=20, is_debug: bool = False):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions_arc = [0] * length
+ self.g_jointvelocity_arc = [0] * length
+ self.handstate = [0] * length
+ self.calibrationoriginal = None # 五指张开标定值 (对应255)
+ self.calibrationfistpose = None # 握拳标定值 (对应0)
+ self.calibrationopose = None # O型标定值 (对应中间值)
+ self.glove_version = 'v2'
+
+ # ========== 平滑滤波参数 ==========
+ self.smooth_enabled = True
+ self.smooth_alpha = 0.5 # 平滑系数：越小越平滑，范围 0.05-0.3
+ self.smooth_positions = [255.0] * length # 平滑后的位置（浮点）
+ self.max_step = 20 # 每帧最大变化量，防止跳变
+
+ # 目标机械手预设姿势，数值从URDF获取数据集，
+ # 张开手的时候对应最小角度，
+ # 握拳的时候对应最大角度
+ # O型手势的时候，用工具驱动URDF去驱动目标机械手达到期望姿势，也可以调整这些参数使得实物更加达到期望角度
+ # 其他手势也类似，也可以增加多个手势来实现多模态的映射器（后期陆续开发）
+ self.robot_original = ROBOT_ORIGINAL_RIGHT
+ self.robot_opose = ROBOT_OPOSE_RIGHT
+ self.robot_fist = ROBOT_FIST_RIGHT
+
+ # self.robot_fist[0] = -0.2 # 拇指旋转锁死在最大0.2，高于0.2的属于无用区间
+ # self.robot_fist[1] = 1.4 # 拇指侧摆锁死在最大1.2，高于1.2的属于无用区间
+
+ # 这里可以额外对self.robot_fist的非期望值进行修正，
+ # 由于机械手达到最大值，存在非期望值的区域，在这里可以进行修正，
+ # 同样也需要URDF驱动工具包去做这个事情
+
+
+ # 映射器（v2.8.0专属），具体介绍参考l6_config.py文件
+ self.multi_state_mapper = DynamicWeightMultiStateLinearMapper(FINGER_CONFIGS, MAPPING_ORDER, is_debug=is_debug)
+
+ # 设置动态权重配置（v2.8.2新增）
+ for config_name, config in FINGER_CONFIGS.items():
+ if config.get('dynamic_weight'):
+ self.multi_state_mapper.set_dynamic_weight_config(config_name, config['dynamic_weight'])
+
+ # 电机输出约束
+ self.motor_constraints = MOTOR_CONSTRAINTS['right']
+
+ def set_glove_version(self, version: str):
+ if not version:
+ return
+
+ major_version = version.split('.')[0]
+ version_key = f'v{major_version}'
+
+ if version_key == self.glove_version:
+ return
+
+ self.glove_version = version_key
+
+ for finger_name, config in FINGER_CONFIGS.items():
+ if 'weights' in config and isinstance(config['weights'], dict):
+ if version_key in config['weights']:
+ self.multi_state_mapper.finger_configs[finger_name]['weights'] = config['weights'][version_key]
+
+ if 'reverse_motion' in config and isinstance(config['reverse_motion'], dict):
+ if version_key in config['reverse_motion']:
+ self.multi_state_mapper.finger_configs[finger_name]['reverse_motion'] = config['reverse_motion'][version_key]
+
+ def initialize_mapper(self) -> bool:
+ """
+ 初始化映射器
+
+ 将三种人手标定数据和三种机械手标定数据加载到映射器中
+ 分别是original,opose,fist
+
+ 人手是glove_前缀,机械手是robot_前缀
+ """
+ # 侧摆部分预处理
+ for i in [5, 9, 13, 17]:
+ self.calibrationoriginal[i] = self.calibrationopose[i] + 0.1
+ self.calibrationfistpose[i] = self.calibrationopose[i] - 0.1
+
+ glove_original = self._to_list(self.calibrationoriginal)
+ glove_fist = self._to_list(self.calibrationfistpose)
+ glove_opose = self._to_list(self.calibrationopose)
+
+ self.multi_state_mapper.add_state('original', glove_original, self.robot_original)
+ self.multi_state_mapper.add_state('opose', glove_opose, self.robot_opose)
+ self.multi_state_mapper.add_state('fist', glove_fist, self.robot_fist)
+
+ self.multi_state_mapper.set_state_order(MULTI_SEGMENT_CONFIG['states'])
+
+ state_info = self.multi_state_mapper.get_state_info()
+
+ def _to_list(self, data):
+ """转换为列表"""
+ if hasattr(data, 'tolist'):
+ return data.tolist()
+ elif isinstance(data, np.ndarray):
+ print(111)
+ return data.tolist()
+ else:
+ return list(data)
+
+ def _apply_motor_constraints(self, positions):
+ """应用电机输出约束"""
+ if not hasattr(self, 'motor_constraints') or self.motor_constraints is None:
+ return positions
+ result = []
+ for i, pos in enumerate(positions):
+ if i < len(self.motor_constraints) and self.motor_constraints[i].get('enabled', False):
+ result.append(max(self.motor_constraints[i]['min'], min(pos, self.motor_constraints[i]['max'])))
+ else:
+ result.append(pos)
+ return result
+
+ # V2.8.0 本函数作废
+ # def _linear_map_diff(self, current_diff, fist_diff, extend_ratio=1.2):
+ # """
+ # 基于差值的线性映射到0-255
+
+ # 注意: 传入joint_update的是差值 (当前值 - 张开值)
+
+ # 参数:
+ # current_diff: 当前传感器差值 (当前值 - 张开值)
+ # fist_diff: 握拳时的差值 (握拳值 - 张开值)
+ # extend_ratio: 缩放比例，>1.0 使映射更容易到达0/255边界
+
+ # 映射逻辑：
+ # - 差值为0（张开）→ 255
+ # - 差值为fist_diff（握拳）→ 0
+ # """
+ # if abs(fist_diff) < 0.01:
+ # return 128 # 变化太小，返回中值
+
+ # # 缩小fist_diff使得更容易到达0边界
+ # effective_fist_diff = fist_diff / extend_ratio
+
+ # # 计算比例: 差值0→比例0, 差值fist_diff→比例1
+ # ratio = current_diff / effective_fist_diff
+ # ratio = max(0.0, min(1.0, ratio)) # 限制在0-1之间
+
+ # # 映射: 比例0→255, 比例1→0
+ # return int((1 - ratio) * 255)
+
+ def _apply_smooth(self, raw_positions):
+ """
+ 对电机输出应用平滑滤波，防止跳变
+
+ 使用指数移动平均(EMA) + 最大步长限制
+ """
+ if not self.smooth_enabled:
+ return raw_positions
+
+ smoothed = []
+ for i, raw in enumerate(raw_positions):
+ # 指数移动平均
+ target = self.smooth_alpha * raw + (1 - self.smooth_alpha) * self.smooth_positions[i]
+
+ # 最大步长限制，防止大幅跳变
+ diff = target - self.smooth_positions[i]
+ if abs(diff) > self.max_step:
+ target = self.smooth_positions[i] + (self.max_step if diff > 0 else -self.max_step)
+
+ self.smooth_positions[i] = target
+ smoothed.append(int(round(target)))
+
+ return smoothed
+
+ def joint_update(self, joint_arc):
+ """
+ 右手映射 - 基于标定数据和预期机械手动作的映射器完成
+ """
+ qpos = np.zeros(25)
+ # ========== 使用映射器进行精确映射 ==========
+ if self.calibrationoriginal is not None and self.calibrationfistpose is not None and self.calibrationopose is not None:
+ arc_value = self.multi_state_mapper.map_glove_to_robot(joint_arc)
+ # arc_value = ROBOT_OPOSE_RIGHT
+ qpos[16] = self.g_jointpositions_arc[0] = arc_value[0]
+ qpos[17] = self.g_jointpositions_arc[1] = arc_value[1]
+ qpos[18] = self.g_jointpositions_arc[2] = arc_value[2]
+ qpos[19] = self.g_jointpositions_arc[3] = arc_value[3]
+
+ qpos[0] = self.g_jointpositions_arc[5] = arc_value[5]
+ qpos[1] = self.g_jointpositions_arc[6] = arc_value[6]
+
+ qpos[2] = self.g_jointpositions_arc[7] = arc_value[7]
+ qpos[3] = self.g_jointpositions_arc[8] = arc_value[8]
+
+ qpos[4] = self.g_jointpositions_arc[17] = arc_value[17]
+ qpos[5] = self.g_jointpositions_arc[18] = arc_value[18]
+ qpos[6] = self.g_jointpositions_arc[19] = arc_value[19]
+ qpos[7] = self.g_jointpositions_arc[4] = arc_value[20]
+
+ qpos[8] = self.g_jointpositions_arc[9] = arc_value[9]
+ qpos[9] = self.g_jointpositions_arc[10] = arc_value[10]
+ qpos[10] = self.g_jointpositions_arc[11] = arc_value[11]
+ qpos[11] = self.g_jointpositions_arc[12] = arc_value[12]
+
+ qpos[12] = self.g_jointpositions_arc[13] = arc_value[13]
+ qpos[13] = self.g_jointpositions_arc[14] = arc_value[14]
+ qpos[14] = self.g_jointpositions_arc[15] = arc_value[15]
+ qpos[15] = self.g_jointpositions_arc[16] = arc_value[16]
+ # ========== 没有标定数据时使用手动映射 ==========
+ else:
+ # 拇指处理 (与O6相同)
+ qpos[20] = joint_arc[4] * 2.2 # 拇指弯曲
+ qpos[17] = joint_arc[2] * -2.5 # 拇指侧摆
+ # 四指处理
+ qpos[1] = joint_arc[6] * 0.1 + joint_arc[8] * 0.7
+ qpos[9] = joint_arc[10] * 0.1 + joint_arc[12] * 0.7
+ qpos[13] = joint_arc[14] * 0.1 + joint_arc[16] * 0.7
+ qpos[5] = joint_arc[18] * 0.1 + joint_arc[20] * 0.7
+ # self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ # ========== 应用电机约束 ==========
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+ self.g_jointpositions = self._apply_motor_constraints(self.g_jointpositions)
+ # ========== 应用平滑滤波 ==========
+ self.g_jointpositions = self._apply_smooth(self.g_jointpositions)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=20, is_debug: bool = False):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions_arc = [0] * length
+ self.g_jointvelocity_arc = [0] * length
+ self.handstate = [0] * length
+ self.calibrationoriginal = None # 五指张开标定值 (对应255)
+ self.calibrationfistpose = None # 握拳标定值 (对应0)
+ self.calibrationopose = None # O型标定值 (对应中间值)
+ self.glove_version = 'v2'
+
+ # ========== 平滑滤波参数 ==========
+ self.smooth_enabled = True
+ self.smooth_alpha = 0.5 # 平滑系数：越小越平滑，范围 0.05-0.3
+ self.smooth_positions = [255.0] * length # 平滑后的位置（浮点）
+ self.max_step = 20 # 每帧最大变化量，防止跳变
+
+ # 目标机械手预设姿势，数值从URDF获取数据集，
+ # 张开手的时候对应最小角度，
+ # 握拳的时候对应最大角度
+ # O型手势的时候，用工具驱动URDF去驱动目标机械手达到期望姿势，也可以调整这些参数使得实物更加达到期望角度
+ # 其他手势也类似，也可以增加多个手势来实现多模态的映射器（后期陆续开发）
+ self.robot_original = ROBOT_ORIGINAL_LEFT
+ self.robot_opose = ROBOT_OPOSE_LEFT
+ self.robot_fist = ROBOT_FIST_LEFT
+
+ self.robot_fist[0] = 0.3 # 拇指旋转锁死在最大0.2，高于0.2的属于无用区间
+ # self.robot_fist[1] = 1.5 # 拇指侧摆锁死在最大1.2，高于1.2的属于无用区间
+
+ # 这里可以额外对self.robot_fist的非期望值进行修正，
+ # 由于机械手达到最大值，存在非期望值的区域，在这里可以进行修正，
+ # 同样也需要URDF驱动工具包去做这个事情
+
+
+ # 映射器（v2.8.0专属），具体介绍参考l6_config.py文件
+ self.multi_state_mapper = DynamicWeightMultiStateLinearMapper(FINGER_CONFIGS, MAPPING_ORDER, is_debug=is_debug)
+
+ # 设置动态权重配置（v2.8.2新增）
+ for config_name, config in FINGER_CONFIGS.items():
+ if config.get('dynamic_weight'):
+ self.multi_state_mapper.set_dynamic_weight_config(config_name, config['dynamic_weight'])
+
+ # 电机输出约束
+ self.motor_constraints = MOTOR_CONSTRAINTS['left']
+
+ def set_glove_version(self, version: str):
+ if not version:
+ return
+
+ major_version = version.split('.')[0]
+ version_key = f'v{major_version}'
+
+ if version_key == self.glove_version:
+ return
+
+ self.glove_version = version_key
+
+ for finger_name, config in FINGER_CONFIGS.items():
+ if 'weights' in config and isinstance(config['weights'], dict):
+ if version_key in config['weights']:
+ self.multi_state_mapper.finger_configs[finger_name]['weights'] = config['weights'][version_key]
+
+ if 'reverse_motion' in config and isinstance(config['reverse_motion'], dict):
+ if version_key in config['reverse_motion']:
+ self.multi_state_mapper.finger_configs[finger_name]['reverse_motion'] = config['reverse_motion'][version_key]
+
+ def initialize_mapper(self) -> bool:
+ """
+ 初始化映射器
+
+ 将三种人手标定数据和三种机械手标定数据加载到映射器中
+ 分别是original,opose,fist
+
+ 人手是glove_前缀,机械手是robot_前缀
+ """
+ # 侧摆部分预处理
+ for i in [5, 9, 13, 17]:
+ self.calibrationoriginal[i] = self.calibrationopose[i] - 0.1
+ self.calibrationfistpose[i] = self.calibrationopose[i] + 0.1
+
+ glove_original = self._to_list(self.calibrationoriginal)
+ glove_fist = self._to_list(self.calibrationfistpose)
+ glove_opose = self._to_list(self.calibrationopose)
+
+ self.multi_state_mapper.add_state('original', glove_original, self.robot_original)
+ self.multi_state_mapper.add_state('opose', glove_opose, self.robot_opose)
+ self.multi_state_mapper.add_state('fist', glove_fist, self.robot_fist)
+
+ self.multi_state_mapper.set_state_order(MULTI_SEGMENT_CONFIG['states'])
+
+ state_info = self.multi_state_mapper.get_state_info()
+
+ def _to_list(self, data):
+ """转换为列表"""
+ if hasattr(data, 'tolist'):
+ return data.tolist()
+ elif isinstance(data, np.ndarray):
+ return data.tolist()
+ else:
+ return list(data)
+
+ def _apply_motor_constraints(self, positions):
+ """应用电机输出约束"""
+ if not hasattr(self, 'motor_constraints') or self.motor_constraints is None:
+ return positions
+ result = []
+ for i, pos in enumerate(positions):
+ if i < len(self.motor_constraints) and self.motor_constraints[i].get('enabled', False):
+ result.append(max(self.motor_constraints[i]['min'], min(pos, self.motor_constraints[i]['max'])))
+ else:
+ result.append(pos)
+ return result
+
+ # V2.8.0 本函数作废
+ # def _linear_map_diff(self, current_diff, fist_diff, extend_ratio=1.2):
+ # """
+ # 基于差值的线性映射到0-255
+
+ # 注意: 传入joint_update的是差值 (当前值 - 张开值)
+
+ # 参数:
+ # current_diff: 当前传感器差值 (当前值 - 张开值)
+ # fist_diff: 握拳时的差值 (握拳值 - 张开值)
+ # extend_ratio: 缩放比例，>1.0 使映射更容易到达0/255边界
+
+ # 映射逻辑：
+ # - 差值为0（张开）→ 255
+ # - 差值为fist_diff（握拳）→ 0
+ # """
+ # if abs(fist_diff) < 0.01:
+ # return 128 # 变化太小，返回中值
+
+ # # 缩小fist_diff使得更容易到达0边界
+ # effective_fist_diff = fist_diff / extend_ratio
+
+ # # 计算比例: 差值0→比例0, 差值fist_diff→比例1
+ # ratio = current_diff / effective_fist_diff
+ # ratio = max(0.0, min(1.0, ratio)) # 限制在0-1之间
+
+ # # 映射: 比例0→255, 比例1→0
+ # return int((1 - ratio) * 255)
+
+ def _apply_smooth(self, raw_positions):
+ """
+ 对电机输出应用平滑滤波，防止跳变
+
+ 使用指数移动平均(EMA) + 最大步长限制
+ """
+ if not self.smooth_enabled:
+ return raw_positions
+
+ smoothed = []
+ for i, raw in enumerate(raw_positions):
+ # 指数移动平均
+ target = self.smooth_alpha * raw + (1 - self.smooth_alpha) * self.smooth_positions[i]
+
+ # 最大步长限制，防止大幅跳变
+ diff = target - self.smooth_positions[i]
+ if abs(diff) > self.max_step:
+ target = self.smooth_positions[i] + (self.max_step if diff > 0 else -self.max_step)
+
+ self.smooth_positions[i] = target
+ smoothed.append(int(round(target)))
+
+ return smoothed
+
+ def joint_update(self, joint_arc):
+ """
+ 右手映射 - 基于标定数据和预期机械手动作的映射器完成
+ """
+ qpos = np.zeros(25)
+ # ========== 使用映射器进行精确映射 ==========
+ if self.calibrationoriginal is not None and self.calibrationfistpose is not None and self.calibrationopose is not None:
+ arc_value = self.multi_state_mapper.map_glove_to_robot(joint_arc)
+ # arc_value = ROBOT_OPOSE_LEFT
+ qpos[16] = self.g_jointpositions_arc[0] = arc_value[0]
+ qpos[17] = self.g_jointpositions_arc[1] = arc_value[1]
+ qpos[18] = self.g_jointpositions_arc[2] = arc_value[2]
+ qpos[19] = self.g_jointpositions_arc[3] = arc_value[3]
+
+ qpos[0] = self.g_jointpositions_arc[5] = arc_value[5] * 3
+ qpos[1] = self.g_jointpositions_arc[6] = arc_value[6]
+ qpos[2] = self.g_jointpositions_arc[7] = arc_value[7]
+ qpos[3] = self.g_jointpositions_arc[8] = arc_value[8]
+
+ qpos[4] = self.g_jointpositions_arc[17] = arc_value[17]
+ qpos[5] = self.g_jointpositions_arc[18] = arc_value[18]
+ qpos[6] = self.g_jointpositions_arc[19] = arc_value[19]
+ qpos[7] = self.g_jointpositions_arc[4] = arc_value[20]
+
+ qpos[8] = self.g_jointpositions_arc[9] = arc_value[9]
+ qpos[9] = self.g_jointpositions_arc[10] = arc_value[10]
+ qpos[10] = self.g_jointpositions_arc[11] = arc_value[11]
+ qpos[11] = self.g_jointpositions_arc[12] = arc_value[12]
+
+ qpos[12] = self.g_jointpositions_arc[13] = arc_value[13]
+ qpos[13] = self.g_jointpositions_arc[14] = arc_value[14]
+ qpos[14] = self.g_jointpositions_arc[15] = arc_value[15]
+ qpos[15] = self.g_jointpositions_arc[16] = arc_value[16]
+ # ========== 没有标定数据时使用手动映射 ==========
+ else:
+ # 拇指处理 (与O6相同)
+ qpos[20] = joint_arc[4] * 2.2 # 拇指弯曲
+ qpos[17] = joint_arc[2] * -2.5 # 拇指侧摆
+ # 四指处理
+ qpos[1] = joint_arc[6] * 0.1 + joint_arc[8] * 0.7
+ qpos[9] = joint_arc[10] * 0.1 + joint_arc[12] * 0.7
+ qpos[13] = joint_arc[14] * 0.1 + joint_arc[16] * 0.7
+ qpos[5] = joint_arc[18] * 0.1 + joint_arc[20] * 0.7
+ # self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ # ========== 应用电机约束 ==========
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+ self.g_jointpositions = self._apply_motor_constraints(self.g_jointpositions)
+ # ========== 应用平滑滤波 ==========
+ self.g_jointpositions = self._apply_smooth(self.g_jointpositions)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_l6.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_l6.py
new file mode 100644
index 0000000..866d5a7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_l6.py
@@ -0,0 +1,495 @@
+"""
+LinkerForce L6 手型映射模块 - ROS2版本
+与ROS1 L6版本保持一致，支持基于标定数据的精确映射
+v2.8.0升级了映射器算法
+"""
+import numpy as np
+import copy
+from linkerhand.handcore import HandCore
+from ..config.l6_config import FINGER_CONFIGS, MAPPING_ORDER, ROBOT_OPOSE_RIGHT, ROBOT_OPOSE_LEFT, ROBOT_ORIGINAL_RIGHT, ROBOT_ORIGINAL_LEFT, ROBOT_FIST_RIGHT, ROBOT_FIST_LEFT, MULTI_SEGMENT_CONFIG, MULTI_SEGMENT_CONFIG_FROZEN, MOTOR_CONSTRAINTS
+from typing import List
+from linkerhand.handcoreex import DynamicWeightMultiStateLinearMapper,MultiStateLinearMapper
+
+
+def _resolve_version_config(configs: dict, version: str) -> dict:
+ """
+ 解析版本配置，将字典格式的 weights/reverse_motion 转换为具体值
+ """
+ resolved = copy.deepcopy(configs)
+ for finger_name, config in resolved.items():
+ if 'weights' in config and isinstance(config['weights'], dict):
+ config['weights'] = config['weights'].get(version, config['weights'].get('v2', [0.5, 0, 0.5]))
+ if 'reverse_motion' in config and isinstance(config['reverse_motion'], dict):
+ config['reverse_motion'] = config['reverse_motion'].get(version, config['reverse_motion'].get('v2', False))
+ return resolved
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=6, is_debug: bool = False):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions_arc = [0] * length
+ self.g_jointvelocity_arc = [0] * length
+ self.handstate = [0] * length
+ self.calibrationoriginal = None
+ self.calibrationfistpose = None
+ self.calibrationopose = None
+ self.glove_version = 'v2'
+
+ # ========== 平滑滤波参数 ==========
+ self.smooth_enabled = True
+ self.smooth_alpha = 0.5 # 平滑系数：越小越平滑，范围 0.05-0.3
+ self.smooth_positions = [255.0] * length # 平滑后的位置（浮点）
+ self.max_step = 20 # 每帧最大变化量，防止跳变
+
+ # 目标机械手预设姿势，数值从URDF获取数据集，
+ # 张开手的时候对应最小角度，
+ # 握拳的时候对应最大角度
+ # O型手势的时候，用工具驱动URDF去驱动目标机械手达到期望姿势，也可以调整这些参数使得实物更加达到期望角度
+ # 其他手势也类似，也可以增加多个手势来实现多模态的映射器（后期陆续开发）
+ self.robot_original = ROBOT_ORIGINAL_RIGHT
+ self.robot_opose = ROBOT_OPOSE_RIGHT
+ self.robot_fist = ROBOT_FIST_RIGHT
+
+ # 映射器（v2.8.0专属），具体介绍参考l6_config.py文件
+ finger_configs = _resolve_version_config(FINGER_CONFIGS, self.glove_version)
+ self.multi_state_mapper = DynamicWeightMultiStateLinearMapper(finger_configs, MAPPING_ORDER, is_debug=is_debug)
+
+ # 电机约束配置
+ self.motor_constraints = MOTOR_CONSTRAINTS['right']
+
+ def initialize_mapper(self) -> bool:
+ """
+ 初始化映射器
+
+ 将三种人手标定数据和三种机械手标定数据加载到映射器中
+ 分别是original,opose,fist
+
+ 人手是glove_前缀,机械手是robot_前缀
+ """
+ glove_original = self._to_list(self.calibrationoriginal)
+ glove_fist = self._to_list(self.calibrationfistpose)
+ glove_opose = self._to_list(self.calibrationopose)
+
+ self.multi_state_mapper.add_state('original', glove_original, self.robot_original)
+ self.multi_state_mapper.add_state('opose', glove_opose, self.robot_opose)
+ self.multi_state_mapper.add_state('fist', glove_fist, self.robot_fist)
+ self.multi_state_mapper.set_state_order(list(MULTI_SEGMENT_CONFIG_FROZEN))
+
+ def set_glove_version(self, version: str):
+ if not version:
+ return
+
+ major_version = version.split('.')[0]
+ version_key = f'v{major_version}'
+
+ if version_key == self.glove_version:
+ return
+
+ self.glove_version = version_key
+
+ for finger_name, config in FINGER_CONFIGS.items():
+ if 'weights' in config and isinstance(config['weights'], dict):
+ if version_key in config['weights']:
+ self.multi_state_mapper.finger_configs[finger_name]['weights'] = config['weights'][version_key]
+
+ if 'reverse_motion' in config and isinstance(config['reverse_motion'], dict):
+ if version_key in config['reverse_motion']:
+ self.multi_state_mapper.finger_configs[finger_name]['reverse_motion'] = config['reverse_motion'][version_key]
+
+ def _to_list(self, data):
+ """转换为列表"""
+ if hasattr(data, 'tolist'):
+ return data.tolist()
+ elif isinstance(data, np.ndarray):
+ print(111)
+ return data.tolist()
+ else:
+ return list(data)
+
+ # V2.8.0 本函数作废
+ # def _linear_map_diff(self, current_diff, fist_diff, extend_ratio=1.2):
+ # """
+ # 基于差值的线性映射到0-255
+
+ # 注意: 传入joint_update的是差值 (当前值 - 张开值)
+
+ # 参数:
+ # current_diff: 当前传感器差值 (当前值 - 张开值)
+ # fist_diff: 握拳时的差值 (握拳值 - 张开值)
+ # extend_ratio: 缩放比例，>1.0 使映射更容易到达0/255边界
+
+ # 映射逻辑：
+ # - 差值为0（张开）→ 255
+ # - 差值为fist_diff（握拳）→ 0
+ # """
+ # if abs(fist_diff) < 0.01:
+ # return 128 # 变化太小，返回中值
+
+ # # 缩小fist_diff使得更容易到达0边界
+ # effective_fist_diff = fist_diff / extend_ratio
+
+ # # 计算比例: 差值0→比例0, 差值fist_diff→比例1
+ # ratio = current_diff / effective_fist_diff
+ # ratio = max(0.0, min(1.0, ratio)) # 限制在0-1之间
+
+ # # 映射: 比例0→255, 比例1→0
+ # return int((1 - ratio) * 255)
+
+ def _apply_smooth(self, raw_positions):
+ """
+ 对电机输出应用平滑滤波，防止跳变
+
+ 使用指数移动平均(EMA) + 最大步长限制
+ """
+ if not self.smooth_enabled:
+ return raw_positions
+
+ smoothed = []
+ for i, raw in enumerate(raw_positions):
+ # 指数移动平均
+ target = self.smooth_alpha * raw + (1 - self.smooth_alpha) * self.smooth_positions[i]
+
+ # 最大步长限制，防止大幅跳变
+ diff = target - self.smooth_positions[i]
+ if abs(diff) > self.max_step:
+ target = self.smooth_positions[i] + (self.max_step if diff > 0 else -self.max_step)
+
+ self.smooth_positions[i] = target
+ smoothed.append(int(round(target)))
+
+ return smoothed
+
+ def joint_update(self, joint_arc):
+ """
+ 右手映射 - 基于标定数据和预期机械手动作的映射器完成
+ """
+ qpos = np.zeros(25)
+ # info = self.multi_state_mapper.get_mapping_info()
+ # for finger, data in info.items():
+ # print(f"{finger}: 扩展映射={data['has_extended_mapping']}, "
+ # f"缩放={data['scale_factor']:.1f}, "
+ # f"最大角度={data['max_angle']:.3f}rad")
+ # ========== 使用映射器进行精确映射 ==========
+ 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]
+ # ========== 没有标定数据时使用手动映射 ==========
+ else:
+ # 拇指处理 (与O6相同)
+ qpos[20] = joint_arc[4] * 2.2 # 拇指弯曲
+ qpos[17] = joint_arc[2] * -2.5 # 拇指侧摆
+ # 四指处理
+ qpos[1] = joint_arc[6] * 0.1 + joint_arc[8] * 0.7
+ qpos[9] = joint_arc[10] * 0.1 + joint_arc[12] * 0.7
+ qpos[13] = joint_arc[14] * 0.1 + joint_arc[16] * 0.7
+ qpos[5] = joint_arc[18] * 0.1 + joint_arc[20] * 0.7
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ # ========== 应用平滑滤波 ==========
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+ self._apply_motor_constraints()
+ self.g_jointpositions = self._apply_smooth(self.g_jointpositions)
+
+ def _apply_motor_constraints(self):
+ """对 g_jointpositions (电机值) 应用约束"""
+ for i, constraint in enumerate(self.motor_constraints):
+ if constraint.get('enabled', False):
+ min_val = constraint.get('min', 0)
+ max_val = constraint.get('max', 255)
+ self.g_jointpositions[i] = int(max(min_val, min(max_val, self.g_jointpositions[i])))
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=6, is_debug: bool = False):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions_arc = [0] * length
+ self.g_jointvelocity_arc = [0] * length
+ self.handstate = [0] * length
+ self.calibrationoriginal = None
+ self.calibrationfistpose = None
+ self.calibrationopose = None
+ self.glove_version = 'v2'
+
+ # ========== 平滑滤波参数 ==========
+ self.smooth_enabled = True
+ self.smooth_alpha = 0.5 # 平滑系数：越小越平滑，范围 0.05-0.3
+ self.smooth_positions = [255.0] * length # 平滑后的位置（浮点）
+ self.max_step = 20 # 每帧最大变化量，防止跳变
+
+ # 目标机械手预设姿势，数值从URDF获取数据集，
+ # 张开手的时候对应最小角度，
+ # 握拳的时候对应最大角度
+ # O型手势的时候，用工具驱动URDF去驱动目标机械手达到期望姿势，也可以调整这些参数使得实物更加达到期望角度
+ # 其他手势也类似，也可以增加多个手势来实现多模态的映射器（后期陆续开发）
+ self.robot_original = ROBOT_ORIGINAL_LEFT
+ self.robot_opose = ROBOT_OPOSE_LEFT
+ self.robot_fist = ROBOT_FIST_LEFT
+
+ # 映射器（v2.8.0专属），具体介绍参考l6_config.py文件
+ finger_configs = _resolve_version_config(FINGER_CONFIGS, self.glove_version)
+ self.multi_state_mapper = DynamicWeightMultiStateLinearMapper(finger_configs, MAPPING_ORDER, is_debug=is_debug)
+
+ # 电机约束配置
+ self.motor_constraints = MOTOR_CONSTRAINTS['left']
+
+ def initialize_mapper(self) -> bool:
+ """
+ 初始化映射器
+
+ 将三种人手标定数据和三种机械手标定数据加载到映射器中
+ 分别是original,opose,fist
+
+ 人手是glove_前缀,机械手是robot_前缀
+ """
+ glove_original = self._to_list(self.calibrationoriginal)
+ glove_fist = self._to_list(self.calibrationfistpose)
+ glove_opose = self._to_list(self.calibrationopose)
+
+ self.multi_state_mapper.add_state('original', glove_original, self.robot_original)
+ self.multi_state_mapper.add_state('opose', glove_opose, self.robot_opose)
+ self.multi_state_mapper.add_state('fist', glove_fist, self.robot_fist)
+ self.multi_state_mapper.set_state_order(list(MULTI_SEGMENT_CONFIG_FROZEN))
+
+ def set_glove_version(self, version: str):
+ if not version:
+ return
+
+ major_version = version.split('.')[0]
+ version_key = f'v{major_version}'
+
+ if version_key == self.glove_version:
+ return
+
+ self.glove_version = version_key
+
+ for finger_name, config in FINGER_CONFIGS.items():
+ if 'weights' in config and isinstance(config['weights'], dict):
+ if version_key in config['weights']:
+ self.multi_state_mapper.finger_configs[finger_name]['weights'] = config['weights'][version_key]
+
+ if 'reverse_motion' in config and isinstance(config['reverse_motion'], dict):
+ if version_key in config['reverse_motion']:
+ self.multi_state_mapper.finger_configs[finger_name]['reverse_motion'] = config['reverse_motion'][version_key]
+
+ def _to_list(self, data):
+ if hasattr(data, 'tolist'):
+ return data.tolist()
+ elif isinstance(data, np.ndarray):
+ print(111)
+ return data.tolist()
+ else:
+ return list(data)
+
+ # V2.8.0 本函数作废
+ # def _linear_map_diff(self, current_diff, fist_diff, extend_ratio=1.2):
+ # """
+ # 基于差值的线性映射到0-255
+
+ # 注意: 传入joint_update的是差值 (当前值 - 张开值)
+
+ # 参数:
+ # current_diff: 当前传感器差值 (当前值 - 张开值)
+ # fist_diff: 握拳时的差值 (握拳值 - 张开值)
+ # extend_ratio: 缩放比例，>1.0 使映射更容易到达0/255边界
+
+ # 映射逻辑：
+ # - 差值为0（张开）→ 255
+ # - 差值为fist_diff（握拳）→ 0
+ # """
+ # if abs(fist_diff) < 0.01:
+ # return 128 # 变化太小，返回中值
+
+ # # 缩小fist_diff使得更容易到达0边界
+ # effective_fist_diff = fist_diff / extend_ratio
+
+ # # 计算比例: 差值0→比例0, 差值fist_diff→比例1
+ # ratio = current_diff / effective_fist_diff
+ # ratio = max(0.0, min(1.0, ratio)) # 限制在0-1之间
+
+ # # 映射: 比例0→255, 比例1→0
+ # return int((1 - ratio) * 255)
+
+ def _apply_smooth(self, raw_positions):
+ """
+ 对电机输出应用平滑滤波，防止跳变
+
+ 使用指数移动平均(EMA) + 最大步长限制
+ """
+ if not self.smooth_enabled:
+ return raw_positions
+
+ smoothed = []
+ for i, raw in enumerate(raw_positions):
+ # 指数移动平均
+ target = self.smooth_alpha * raw + (1 - self.smooth_alpha) * self.smooth_positions[i]
+
+ # 最大步长限制，防止大幅跳变
+ diff = target - self.smooth_positions[i]
+ if abs(diff) > self.max_step:
+ target = self.smooth_positions[i] + (self.max_step if diff > 0 else -self.max_step)
+
+ self.smooth_positions[i] = target
+ smoothed.append(int(round(target)))
+
+ return smoothed
+
+ def joint_update(self, joint_arc):
+ """
+ 左手映射 - 基于标定数据和预期机械手动作的映射器完成
+ """
+ qpos = np.zeros(25)
+
+ # ========== 使用映射器进行精确映射 ==========
+ # for finger, data in info.items():
+ # print(f"{finger}: 扩展映射={data['has_extended_mapping']}, "
+ # f"缩放={data['scale_factor']:.1f}, "
+ # f"最大角度={data['max_angle']:.3f}rad")
+ # ========== 使用映射器进行精确映射 ==========
+ 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)
+ 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]
+ # ========== 没有标定数据时使用手动映射 ==========
+ else:
+ # 拇指处理 (与O6相同)
+ qpos[20] = joint_arc[4] * 2.2 # 拇指弯曲
+ qpos[17] = joint_arc[2] * -2.5 # 拇指侧摆
+ # 四指处理
+ qpos[1] = joint_arc[6] * 0.1 + joint_arc[8] * 0.7
+ qpos[9] = joint_arc[10] * 0.1 + joint_arc[12] * 0.7
+ qpos[13] = joint_arc[14] * 0.1 + joint_arc[16] * 0.7
+ qpos[5] = joint_arc[18] * 0.1 + joint_arc[20] * 0.7
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ # ========== 应用平滑滤波 ==========
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+ self._apply_motor_constraints()
+ # self.g_jointpositions = self._apply_smooth(self.g_jointpositions)
+
+ def _apply_motor_constraints(self):
+ """对 g_jointpositions (电机值) 应用约束"""
+ for i, constraint in enumerate(self.motor_constraints):
+ if constraint.get('enabled', False):
+ min_val = constraint.get('min', 0)
+ max_val = constraint.get('max', 255)
+ self.g_jointpositions[i] = int(max(min_val, min(max_val, self.g_jointpositions[i])))
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_l7.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_l7.py
new file mode 100644
index 0000000..a3710c0
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_l7.py
@@ -0,0 +1,434 @@
+"""
+LinkerForce L7 手型映射模块 - ROS2版本
+"""
+import numpy as np
+import copy
+from linkerhand.handcore import HandCore
+from ..config.o7_config import FINGER_CONFIGS, MAPPING_ORDER, ROBOT_OPOSE_RIGHT, ROBOT_OPOSE_LEFT, ROBOT_ORIGINAL_RIGHT, ROBOT_ORIGINAL_LEFT, ROBOT_FIST_RIGHT, ROBOT_FIST_LEFT, MULTI_SEGMENT_CONFIG, MULTI_SEGMENT_CONFIG_FROZEN, MOTOR_CONSTRAINTS
+from typing import List
+from linkerhand.handcoreex import DynamicWeightMultiStateLinearMapper,MultiStateLinearMapper
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=7, is_debug: bool = False):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions_arc = [0] * length
+ self.g_jointvelocity_arc = [0] * length
+ self.handstate = [0] * length
+ self.calibrationoriginal = None # 五指张开标定值 (对应255)
+ self.calibrationfistpose = None # 握拳标定值 (对应0)
+ self.calibrationopose = None # O型标定值 (对应中间值)
+ self.glove_version = 'v2'
+
+ # ========== 平滑滤波参数 ==========
+ self.smooth_enabled = True
+ self.smooth_alpha = 0.5 # 平滑系数：越小越平滑，范围 0.05-0.3
+ self.smooth_positions = [255.0] * length # 平滑后的位置（浮点）
+ self.max_step = 20 # 每帧最大变化量，防止跳变
+
+ # 目标机械手预设姿势，数值从URDF获取数据集，
+ # 张开手的时候对应最小角度，
+ # 握拳的时候对应最大角度
+ # O型手势的时候，用工具驱动URDF去驱动目标机械手达到期望姿势，也可以调整这些参数使得实物更加达到期望角度
+ # 其他手势也类似，也可以增加多个手势来实现多模态的映射器（后期陆续开发）
+ self.robot_original = ROBOT_ORIGINAL_RIGHT
+ self.robot_opose = ROBOT_OPOSE_RIGHT
+ self.robot_fist = ROBOT_FIST_RIGHT
+ self.robot_fist[0] = 0.8
+
+ # 这里可以额外对self.robot_fist的非期望值进行修正，
+ # 由于机械手达到最大值，存在非期望值的区域，在这里可以进行修正，
+ # 同样也需要URDF驱动工具包去做这个事情
+
+
+ # 映射器（v2.8.0专属），具体介绍参考l6_config.py文件
+ self.multi_state_mapper = DynamicWeightMultiStateLinearMapper(FINGER_CONFIGS, MAPPING_ORDER, is_debug=is_debug)
+
+ # 设置动态权重配置（v2.8.2新增）
+ for config_name, config in FINGER_CONFIGS.items():
+ if config.get('dynamic_weight'):
+ self.multi_state_mapper.set_dynamic_weight_config(config_name, config['dynamic_weight'])
+
+ # 电机输出约束
+ self.motor_constraints = MOTOR_CONSTRAINTS['right']
+
+ def set_glove_version(self, version: str):
+ if not version:
+ return
+
+ major_version = version.split('.')[0]
+ version_key = f'v{major_version}'
+
+ if version_key == self.glove_version:
+ return
+
+ self.glove_version = version_key
+
+ for finger_name, config in FINGER_CONFIGS.items():
+ if 'weights' in config and isinstance(config['weights'], dict):
+ if version_key in config['weights']:
+ self.multi_state_mapper.finger_configs[finger_name]['weights'] = config['weights'][version_key]
+
+ if 'reverse_motion' in config and isinstance(config['reverse_motion'], dict):
+ if version_key in config['reverse_motion']:
+ self.multi_state_mapper.finger_configs[finger_name]['reverse_motion'] = config['reverse_motion'][version_key]
+
+
+ def initialize_mapper(self) -> bool:
+ """初始化映射器"""
+ glove_original = self._to_list(self.calibrationoriginal)
+ glove_fist = self._to_list(self.calibrationfistpose)
+ glove_opose = self._to_list(self.calibrationopose)
+
+ self.multi_state_mapper.add_state('original', glove_original, self.robot_original)
+ self.multi_state_mapper.add_state('opose', glove_opose, self.robot_opose)
+ self.multi_state_mapper.add_state('fist', glove_fist, self.robot_fist)
+
+ self.multi_state_mapper.set_state_order(list(MULTI_SEGMENT_CONFIG_FROZEN))
+
+ def _to_list(self, data):
+ """转换为列表"""
+ if hasattr(data, 'tolist'):
+ return data.tolist()
+ elif isinstance(data, np.ndarray):
+ return data.tolist()
+ else:
+ return list(data)
+
+ def _apply_motor_constraints(self, positions):
+ """应用电机输出约束"""
+ if not hasattr(self, 'motor_constraints') or self.motor_constraints is None:
+ return positions
+ result = []
+ for i, pos in enumerate(positions):
+ if i < len(self.motor_constraints) and self.motor_constraints[i].get('enabled', False):
+ result.append(max(self.motor_constraints[i]['min'], min(pos, self.motor_constraints[i]['max'])))
+ else:
+ result.append(pos)
+ return result
+
+ def _apply_smooth(self, raw_positions):
+ """
+ 对电机输出应用平滑滤波，防止跳变
+
+ 使用指数移动平均(EMA) + 最大步长限制
+ """
+ if not self.smooth_enabled:
+ return raw_positions
+
+ smoothed = []
+ for i, raw in enumerate(raw_positions):
+ # 指数移动平均
+ target = self.smooth_alpha * raw + (1 - self.smooth_alpha) * self.smooth_positions[i]
+
+ # 最大步长限制，防止大幅跳变
+ diff = target - self.smooth_positions[i]
+ if abs(diff) > self.max_step:
+ target = self.smooth_positions[i] + (self.max_step if diff > 0 else -self.max_step)
+
+ self.smooth_positions[i] = target
+ smoothed.append(int(round(target)))
+
+ return smoothed
+
+ def joint_update(self, joint_arc):
+ """
+ 右手映射 - 基于标定数据和预期机械手动作的映射器完成
+ """
+ qpos = np.zeros(25)
+ # ========== 使用映射器进行精确映射 ==========
+ if self.calibrationoriginal is not None and self.calibrationfistpose is not None and self.calibrationopose is not None:
+
+ 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[1]
+ qpos[20] = self.g_jointpositions_arc[0] = arc_value[2]
+ qpos[16] = self.g_jointpositions_arc[6] = arc_value[0]
+
+ qpos[1] = self.g_jointpositions_arc[2] = arc_value[3]
+
+ qpos[9] = self.g_jointpositions_arc[3] = arc_value[4]
+
+ qpos[13] = self.g_jointpositions_arc[4] = arc_value[5]
+
+ qpos[5] = self.g_jointpositions_arc[5] = arc_value[6]
+ else:
+ # 拇指处理
+ qpos[16] = joint_arc[1] * 1.2
+ qpos[17] = joint_arc[0] * -2 + joint_arc[1] * 1.2
+ qpos[20] = joint_arc[4] * 0.5 + joint_arc[2] * 0.8
+
+ qpos[1] = joint_arc[6] * 0.1 + joint_arc[8] * 0.7
+ qpos[9] = joint_arc[10] * 0.1 + joint_arc[12] * 0.7
+ qpos[13] = joint_arc[14] * 0.1 + joint_arc[16] * 0.7
+ qpos[5] = joint_arc[18] * 0.1 + joint_arc[20] * 0.7
+ # ========== 应用电机约束 ==========
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+ self.g_jointpositions = self._apply_motor_constraints(self.g_jointpositions)
+ # ========== 应用平滑滤波 ==========
+ self.g_jointpositions = self._apply_smooth(self.g_jointpositions)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+# LeftHand 类类似修正
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=7, is_debug: bool = False):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions_arc = [0] * length
+ self.g_jointvelocity_arc = [0] * length
+ self.handstate = [0] * length
+ self.calibrationoriginal = None # 五指张开标定值 (对应255)
+ self.calibrationfistpose = None # 握拳标定值 (对应0)
+ self.calibrationopose = None # O型标定值 (对应中间值)
+ self.glove_version = 'v2'
+
+ # ========== 平滑滤波参数 ==========
+ self.smooth_enabled = True
+ self.smooth_alpha = 0.5 # 平滑系数：越小越平滑，范围 0.05-0.3
+ self.smooth_positions = [255.0] * length # 平滑后的位置（浮点）
+ self.max_step = 20 # 每帧最大变化量，防止跳变
+
+ # 目标机械手预设姿势，数值从URDF获取数据集，
+ # 张开手的时候对应最小角度，
+ # 握拳的时候对应最大角度
+ # O型手势的时候，用工具驱动URDF去驱动目标机械手达到期望姿势，也可以调整这些参数使得实物更加达到期望角度
+ # 其他手势也类似，也可以增加多个手势来实现多模态的映射器（后期陆续开发）
+ self.robot_original = ROBOT_ORIGINAL_LEFT
+ self.robot_opose = ROBOT_OPOSE_LEFT
+ self.robot_fist = ROBOT_FIST_LEFT
+ self.robot_fist[0] = 0.8
+
+ # 这里可以额外对self.robot_fist的非期望值进行修正，
+ # 由于机械手达到最大值，存在非期望值的区域，在这里可以进行修正，
+ # 同样也需要URDF驱动工具包去做这个事情
+
+
+ # 映射器（v2.8.0专属），具体介绍参考l6_config.py文件
+ self.multi_state_mapper = DynamicWeightMultiStateLinearMapper(FINGER_CONFIGS, MAPPING_ORDER, is_debug=is_debug)
+
+ # 设置动态权重配置（v2.8.2新增）
+ for config_name, config in FINGER_CONFIGS.items():
+ if config.get('dynamic_weight'):
+ self.multi_state_mapper.set_dynamic_weight_config(config_name, config['dynamic_weight'])
+
+ # 电机输出约束
+ self.motor_constraints = MOTOR_CONSTRAINTS['left']
+
+ def set_glove_version(self, version: str):
+ if not version:
+ return
+
+ major_version = version.split('.')[0]
+ version_key = f'v{major_version}'
+
+ if version_key == self.glove_version:
+ return
+
+ self.glove_version = version_key
+
+ for finger_name, config in FINGER_CONFIGS.items():
+ if 'weights' in config and isinstance(config['weights'], dict):
+ if version_key in config['weights']:
+ self.multi_state_mapper.finger_configs[finger_name]['weights'] = config['weights'][version_key]
+
+ if 'reverse_motion' in config and isinstance(config['reverse_motion'], dict):
+ if version_key in config['reverse_motion']:
+ self.multi_state_mapper.finger_configs[finger_name]['reverse_motion'] = config['reverse_motion'][version_key]
+
+ def initialize_mapper(self) -> bool:
+ """初始化映射器"""
+ glove_original = self._to_list(self.calibrationoriginal)
+ glove_fist = self._to_list(self.calibrationfistpose)
+ glove_opose = self._to_list(self.calibrationopose)
+
+ self.multi_state_mapper.add_state('original', glove_original, self.robot_original)
+ self.multi_state_mapper.add_state('opose', glove_opose, self.robot_opose)
+ self.multi_state_mapper.add_state('fist', glove_fist, self.robot_fist)
+
+ self.multi_state_mapper.set_state_order(list(MULTI_SEGMENT_CONFIG_FROZEN))
+
+ def _to_list(self, data):
+ """转换为列表"""
+ if hasattr(data, 'tolist'):
+ return data.tolist()
+ elif isinstance(data, np.ndarray):
+ return data.tolist()
+ else:
+ return list(data)
+
+ def _apply_motor_constraints(self, positions):
+ """应用电机输出约束"""
+ if not hasattr(self, 'motor_constraints') or self.motor_constraints is None:
+ return positions
+ result = []
+ for i, pos in enumerate(positions):
+ if i < len(self.motor_constraints) and self.motor_constraints[i].get('enabled', False):
+ result.append(max(self.motor_constraints[i]['min'], min(pos, self.motor_constraints[i]['max'])))
+ else:
+ result.append(pos)
+ return result
+
+ def _apply_smooth(self, raw_positions):
+ """
+ 对电机输出应用平滑滤波，防止跳变
+
+ 使用指数移动平均(EMA) + 最大步长限制
+ """
+ if not self.smooth_enabled:
+ return raw_positions
+
+ smoothed = []
+ for i, raw in enumerate(raw_positions):
+ # 指数移动平均
+ target = self.smooth_alpha * raw + (1 - self.smooth_alpha) * self.smooth_positions[i]
+
+ # 最大步长限制，防止大幅跳变
+ diff = target - self.smooth_positions[i]
+ if abs(diff) > self.max_step:
+ target = self.smooth_positions[i] + (self.max_step if diff > 0 else -self.max_step)
+
+ self.smooth_positions[i] = target
+ smoothed.append(int(round(target)))
+
+ return smoothed
+
+ def joint_update(self, joint_arc):
+ """
+ 右手映射 - 基于标定数据和预期机械手动作的映射器完成
+ """
+ qpos = np.zeros(25)
+ # ========== 使用映射器进行精确映射 ==========
+ if self.calibrationoriginal is not None and self.calibrationfistpose is not None and self.calibrationopose is not None:
+
+ 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[1]
+ qpos[20] = self.g_jointpositions_arc[0] = arc_value[2]
+ qpos[16] = self.g_jointpositions_arc[6] = arc_value[0]
+
+ qpos[1] = self.g_jointpositions_arc[2] = arc_value[3]
+
+ qpos[9] = self.g_jointpositions_arc[3] = arc_value[4]
+
+ qpos[13] = self.g_jointpositions_arc[4] = arc_value[5]
+
+ qpos[5] = self.g_jointpositions_arc[5] = arc_value[6]
+ else:
+ # 拇指处理
+ qpos[16] = joint_arc[1] * 1.2
+ qpos[17] = joint_arc[0] * -2 + joint_arc[1] * 1.2
+ qpos[20] = joint_arc[4] * 0.5 + joint_arc[2] * 0.8
+
+ qpos[1] = joint_arc[6] * 0.1 + joint_arc[8] * 0.7
+ qpos[9] = joint_arc[10] * 0.1 + joint_arc[12] * 0.7
+ qpos[13] = joint_arc[14] * 0.1 + joint_arc[16] * 0.7
+ qpos[5] = joint_arc[18] * 0.1 + joint_arc[20] * 0.7
+ # ========== 应用电机约束 ==========
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+ self.g_jointpositions = self._apply_motor_constraints(self.g_jointpositions)
+ # ========== 应用平滑滤波 ==========
+ self.g_jointpositions = self._apply_smooth(self.g_jointpositions)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_o6.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_o6.py
new file mode 100644
index 0000000..042d84a
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_o6.py
@@ -0,0 +1,468 @@
+"""
+LinkerForce O6 手型映射模块 - ROS2版本
+与ROS1 O6版本保持一致，支持基于标定数据的精确映射
+v2.8.0升级了映射器算法
+"""
+import numpy as np
+import copy
+from linkerhand.handcore import HandCore
+from ..config.o6_config import FINGER_CONFIGS, MAPPING_ORDER, ROBOT_OPOSE_RIGHT, ROBOT_OPOSE_LEFT, ROBOT_ORIGINAL_RIGHT, ROBOT_ORIGINAL_LEFT, ROBOT_FIST_RIGHT, ROBOT_FIST_LEFT, MULTI_SEGMENT_CONFIG, MOTOR_CONSTRAINTS
+from typing import List
+from linkerhand.handcoreex import DynamicWeightMultiStateLinearMapper,MultiStateLinearMapper
+
+
+def _resolve_version_config(configs: dict, version: str) -> dict:
+ """
+ 解析版本配置，将字典格式的 weights/reverse_motion 转换为具体值
+ """
+ resolved = copy.deepcopy(configs)
+ for finger_name, config in resolved.items():
+ if 'weights' in config and isinstance(config['weights'], dict):
+ config['weights'] = config['weights'].get(version, config['weights'].get('v2', [0.5, 0, 0.5]))
+ if 'reverse_motion' in config and isinstance(config['reverse_motion'], dict):
+ config['reverse_motion'] = config['reverse_motion'].get(version, config['reverse_motion'].get('v2', False))
+ return resolved
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=6, is_debug: bool = False):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions_arc = [0] * length
+ self.g_jointvelocity_arc = [0] * length
+ self.handstate = [0] * length
+ self.calibrationoriginal = None
+ self.calibrationfistpose = None
+ self.calibrationopose = None
+ self.glove_version = 'v2'
+
+ # ========== 平滑滤波参数 ==========
+ self.smooth_enabled = True
+ self.smooth_alpha = 0.5 # 平滑系数：越小越平滑，范围 0.05-0.3
+ self.smooth_positions = [255.0] * length # 平滑后的位置（浮点）
+ self.max_step = 20 # 每帧最大变化量，防止跳变
+
+ # 目标机械手预设姿势，数值从URDF获取数据集，
+ # 张开手的时候对应最小角度，
+ # 握拳的时候对应最大角度
+ # O型手势的时候，用工具驱动URDF去驱动目标机械手达到期望姿势，也可以调整这些参数使得实物更加达到期望角度
+ # 其他手势也类似，也可以增加多个手势来实现多模态的映射器（后期陆续开发）
+ self.robot_original = ROBOT_ORIGINAL_RIGHT
+ self.robot_opose = ROBOT_OPOSE_RIGHT
+ self.robot_fist = ROBOT_FIST_RIGHT
+
+ # self.robot_fist[0] = 1.5 # 拇指旋转锁死在最大0.2，高于0.2的属于无用区间
+ self.robot_fist[0] = 1.1 # 拇指侧摆锁死在最大1.2，高于1.2的属于无用区间
+ # 这里可以额外对self.robot_fist的非期望值进行修正，
+ # 由于机械手达到最大值，存在非期望值的区域，在这里可以进行修正，
+ # 同样也需要URDF驱动工具包去做这个事情
+
+# 映射器（v2.8.0专属），具体介绍参考l6_config.py文件
+ finger_configs = _resolve_version_config(FINGER_CONFIGS, self.glove_version)
+ self.multi_state_mapper = DynamicWeightMultiStateLinearMapper(finger_configs, MAPPING_ORDER, is_debug=is_debug)
+
+ # 电机约束配置
+ self.motor_constraints = MOTOR_CONSTRAINTS['right']
+
+ def _apply_motor_constraints(self):
+ """对 g_jointpositions (电机值) 应用约束"""
+ for i, constraint in enumerate(self.motor_constraints):
+ if constraint.get('enabled', False):
+ min_val = constraint.get('min', 0)
+ max_val = constraint.get('max', 255)
+ self.g_jointpositions[i] = int(max(min_val, min(max_val, self.g_jointpositions[i])))
+
+ def initialize_mapper(self) -> bool:
+ """
+ 初始化映射器
+
+ 将三种人手标定数据和三种机械手标定数据加载到映射器中
+ 分别是original,opose,fist
+
+ 人手是glove_前缀,机械手是robot_前缀
+ """
+ glove_original = self._to_list(self.calibrationoriginal)
+ glove_fist = self._to_list(self.calibrationfistpose)
+ glove_opose = self._to_list(self.calibrationopose)
+
+ self.multi_state_mapper.add_state('original', glove_original, self.robot_original)
+ self.multi_state_mapper.add_state('opose', glove_opose, self.robot_opose)
+ self.multi_state_mapper.add_state('fist', glove_fist, self.robot_fist)
+ self.multi_state_mapper.set_state_order(MULTI_SEGMENT_CONFIG['states'])
+
+ def set_glove_version(self, version: str):
+ if not version:
+ return
+
+ major_version = version.split('.')[0]
+ version_key = f'v{major_version}'
+
+ if version_key == self.glove_version:
+ return
+
+ self.glove_version = version_key
+
+ for finger_name, config in FINGER_CONFIGS.items():
+ if 'weights' in config and isinstance(config['weights'], dict):
+ if version_key in config['weights']:
+ self.multi_state_mapper.finger_configs[finger_name]['weights'] = config['weights'][version_key]
+
+ if 'reverse_motion' in config and isinstance(config['reverse_motion'], dict):
+ if version_key in config['reverse_motion']:
+ self.multi_state_mapper.finger_configs[finger_name]['reverse_motion'] = config['reverse_motion'][version_key]
+
+ def _to_list(self, data):
+ """转换为列表"""
+ if hasattr(data, 'tolist'):
+ return data.tolist()
+ elif isinstance(data, np.ndarray):
+ print(111)
+ return data.tolist()
+ else:
+ return list(data)
+
+ # V2.8.0 本函数作废
+ # def _linear_map_diff(self, current_diff, fist_diff, extend_ratio=1.2):
+ # """
+ # 基于差值的线性映射到0-255
+
+ # 注意: 传入joint_update的是差值 (当前值 - 张开值)
+
+ # 参数:
+ # current_diff: 当前传感器差值 (当前值 - 张开值)
+ # fist_diff: 握拳时的差值 (握拳值 - 张开值)
+ # extend_ratio: 缩放比例，>1.0 使映射更容易到达0/255边界
+
+ # 映射逻辑：
+ # - 差值为0（张开）→ 255
+ # - 差值为fist_diff（握拳）→ 0
+ # """
+ # if abs(fist_diff) < 0.01:
+ # return 128 # 变化太小，返回中值
+
+ # # 缩小fist_diff使得更容易到达0边界
+ # effective_fist_diff = fist_diff / extend_ratio
+
+ # # 计算比例: 差值0→比例0, 差值fist_diff→比例1
+ # ratio = current_diff / effective_fist_diff
+ # ratio = max(0.0, min(1.0, ratio)) # 限制在0-1之间
+
+ # # 映射: 比例0→255, 比例1→0
+ # return int((1 - ratio) * 255)
+
+ def _apply_smooth(self, raw_positions):
+ """
+ 对电机输出应用平滑滤波，防止跳变
+
+ 使用指数移动平均(EMA) + 最大步长限制
+ """
+ if not self.smooth_enabled:
+ return raw_positions
+
+ smoothed = []
+ for i, raw in enumerate(raw_positions):
+ # 指数移动平均
+ target = self.smooth_alpha * raw + (1 - self.smooth_alpha) * self.smooth_positions[i]
+
+ # 最大步长限制，防止大幅跳变
+ diff = target - self.smooth_positions[i]
+ if abs(diff) > self.max_step:
+ target = self.smooth_positions[i] + (self.max_step if diff > 0 else -self.max_step)
+
+ self.smooth_positions[i] = target
+ smoothed.append(int(round(target)))
+
+ return smoothed
+
+ def joint_update(self, joint_arc):
+ """
+ 右手映射 - 基于标定数据和预期机械手动作的映射器完成
+ """
+ qpos = np.zeros(25)
+ # info = self.multi_state_mapper.get_mapping_info()
+ # for finger, data in info.items():
+ # print(f"{finger}: 扩展映射={data['has_extended_mapping']}, "
+ # f"缩放={data['scale_factor']:.1f}, "
+ # f"最大角度={data['max_angle']:.3f}rad")
+ # ========== 使用映射器进行精确映射 ==========
+ 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]
+ # ========== 没有标定数据时使用手动映射 ==========
+ else:
+ # 拇指处理 (与O6相同)
+ qpos[20] = joint_arc[4] * 2.2 # 拇指弯曲
+ qpos[17] = joint_arc[2] * -2.5 # 拇指侧摆
+ # 四指处理
+ qpos[1] = joint_arc[6] * 0.1 + joint_arc[8] * 0.7
+ qpos[9] = joint_arc[10] * 0.1 + joint_arc[12] * 0.7
+ qpos[13] = joint_arc[14] * 0.1 + joint_arc[16] * 0.7
+ qpos[5] = joint_arc[18] * 0.1 + joint_arc[20] * 0.7
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ # ========== 应用平滑滤波 ==========
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+ self._apply_motor_constraints()
+ self.g_jointpositions = self._apply_smooth(self.g_jointpositions)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=6, is_debug: bool = False):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions_arc = [0] * length
+ self.g_jointvelocity_arc = [0] * length
+ self.handstate = [0] * length
+ self.calibrationoriginal = None
+ self.calibrationfistpose = None
+ self.calibrationopose = None
+ self.glove_version = 'v2'
+
+ self.smooth_enabled = True
+ self.smooth_alpha = 0.5
+ self.smooth_positions = [255.0] * length
+ self.max_step = 20
+
+ self.robot_original = ROBOT_ORIGINAL_LEFT
+ self.robot_opose = ROBOT_OPOSE_LEFT
+ self.robot_fist = ROBOT_FIST_LEFT
+
+ finger_configs = _resolve_version_config(FINGER_CONFIGS, self.glove_version)
+ self.multi_state_mapper = DynamicWeightMultiStateLinearMapper(finger_configs, MAPPING_ORDER, is_debug=is_debug)
+
+ # 电机约束配置
+ self.motor_constraints = MOTOR_CONSTRAINTS['left']
+
+ def _apply_motor_constraints(self):
+ """对 g_jointpositions (电机值) 应用约束"""
+ for i, constraint in enumerate(self.motor_constraints):
+ if constraint.get('enabled', False):
+ min_val = constraint.get('min', 0)
+ max_val = constraint.get('max', 255)
+ self.g_jointpositions[i] = int(max(min_val, min(max_val, self.g_jointpositions[i])))
+
+ def initialize_mapper(self) -> bool:
+ glove_original = self._to_list(self.calibrationoriginal)
+ glove_fist = self._to_list(self.calibrationfistpose)
+ glove_opose = self._to_list(self.calibrationopose)
+
+ self.multi_state_mapper.add_state('original', glove_original, self.robot_original)
+ self.multi_state_mapper.add_state('opose', glove_opose, self.robot_opose)
+ self.multi_state_mapper.add_state('fist', glove_fist, self.robot_fist)
+
+ self.multi_state_mapper.set_state_order(MULTI_SEGMENT_CONFIG['states'])
+
+ def set_glove_version(self, version: str):
+ if not version:
+ return
+
+ major_version = version.split('.')[0]
+ version_key = f'v{major_version}'
+
+ if version_key == self.glove_version:
+ return
+
+ self.glove_version = version_key
+
+ for finger_name, config in FINGER_CONFIGS.items():
+ if 'weights' in config and isinstance(config['weights'], dict):
+ if version_key in config['weights']:
+ self.multi_state_mapper.finger_configs[finger_name]['weights'] = config['weights'][version_key]
+
+ if 'reverse_motion' in config and isinstance(config['reverse_motion'], dict):
+ if version_key in config['reverse_motion']:
+ self.multi_state_mapper.finger_configs[finger_name]['reverse_motion'] = config['reverse_motion'][version_key]
+
+ def _to_list(self, data):
+ if hasattr(data, 'tolist'):
+ return data.tolist()
+ elif isinstance(data, np.ndarray):
+ print(111)
+ return data.tolist()
+ else:
+ return list(data)
+
+ # V2.8.0 本函数作废
+ # def _linear_map_diff(self, current_diff, fist_diff, extend_ratio=1.2):
+ # """
+ # 基于差值的线性映射到0-255
+
+ # 注意: 传入joint_update的是差值 (当前值 - 张开值)
+
+ # 参数:
+ # current_diff: 当前传感器差值 (当前值 - 张开值)
+ # fist_diff: 握拳时的差值 (握拳值 - 张开值)
+ # extend_ratio: 缩放比例，>1.0 使映射更容易到达0/255边界
+
+ # 映射逻辑：
+ # - 差值为0（张开）→ 255
+ # - 差值为fist_diff（握拳）→ 0
+ # """
+ # if abs(fist_diff) < 0.01:
+ # return 128 # 变化太小，返回中值
+
+ # # 缩小fist_diff使得更容易到达0边界
+ # effective_fist_diff = fist_diff / extend_ratio
+
+ # # 计算比例: 差值0→比例0, 差值fist_diff→比例1
+ # ratio = current_diff / effective_fist_diff
+ # ratio = max(0.0, min(1.0, ratio)) # 限制在0-1之间
+
+ # # 映射: 比例0→255, 比例1→0
+ # return int((1 - ratio) * 255)
+
+ def _apply_smooth(self, raw_positions):
+ """
+ 对电机输出应用平滑滤波，防止跳变
+
+ 使用指数移动平均(EMA) + 最大步长限制
+ """
+ if not self.smooth_enabled:
+ return raw_positions
+
+ smoothed = []
+ for i, raw in enumerate(raw_positions):
+ # 指数移动平均
+ target = self.smooth_alpha * raw + (1 - self.smooth_alpha) * self.smooth_positions[i]
+
+ # 最大步长限制，防止大幅跳变
+ diff = target - self.smooth_positions[i]
+ if abs(diff) > self.max_step:
+ target = self.smooth_positions[i] + (self.max_step if diff > 0 else -self.max_step)
+
+ self.smooth_positions[i] = target
+ smoothed.append(int(round(target)))
+
+ return smoothed
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ if self.calibrationoriginal is not None and self.calibrationfistpose is not None and self.calibrationopose is not None:
+ arc_value = self.multi_state_mapper.map_glove_to_robot(joint_arc)
+ 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]
+ else:
+ qpos[20] = joint_arc[4] * 2.2
+ qpos[17] = joint_arc[2] * -2.5
+ qpos[1] = joint_arc[6] * 0.1 + joint_arc[8] * 0.7
+ qpos[9] = joint_arc[10] * 0.1 + joint_arc[12] * 0.7
+ qpos[13] = joint_arc[14] * 0.1 + joint_arc[16] * 0.7
+ qpos[5] = joint_arc[18] * 0.1 + joint_arc[20] * 0.7
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+ self._apply_motor_constraints()
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/retarget.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/retarget.py
new file mode 100644
index 0000000..3b6a7da
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/retarget.py
@@ -0,0 +1,1417 @@
+import time
+import sys
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from std_msgs.msg import String, Int32MultiArray, Header, Float32MultiArray, Float64MultiArray, MultiArrayLayout, MultiArrayDimension
+from pathlib import Path
+
+# 将项目根目录放在最前面
+# 强制使用项目本地的 linkerhand 模块
+_project_root = Path(__file__).absolute().parent.parent.parent
+_project_root_str = str(_project_root)
+
+if _project_root_str in sys.path:
+ sys.path.remove(_project_root_str)
+sys.path.insert(0, _project_root_str)
+
+from linkerhand.linkerforce import ForceSerialReader
+from linkerhand.constants import RobotName, ROBOT_LEN_MAP, HandType
+from linkerhand.handcore import HandCore
+from tqdm import tqdm
+from pathlib import Path
+from colorama import Fore, init
+from datetime import datetime, timedelta
+import threading
+import copy
+import pickle
+import os
+import json
+import sys
+import numpy as np
+import math
+import yaml
+
+
+TMP_FILE_PATH = Path(__file__).parent / "tmp" / "jointangle_data.tmp"
+SAMPLE_FILE_PATH = Path(__file__).parent.parent.parent / "config" / "calibration_sample.yml"
+
+
+class Retarget():
+ def __init__(self,
+ node,
+ righthand: RobotName,
+ lefthand: RobotName,
+ handcore: HandCore,
+ lefthandpubprint: bool,
+ righthandpubprint: bool,
+ calibration: bool = False,
+ auto_detect: bool = True,
+ isgetdebug: bool = True,
+ baseconfig: dict = None,
+ cmd_ports: list = None,
+ cmd_baudrate: int = None,
+ cmd_auto_scan: bool = None):
+ """
+ 初始化 LinkerForce Retarget 模块 (ROS1 版本)
+
+ Args:
+ leftport: 左手串口路径（如 '/dev/ttyUSB0'），auto_detect=True 时可忽略
+ leftbaudrate: 左手波特率
+ rightport: 右手串口路径（如 '/dev/ttyUSB1'），auto_detect=True 时可忽略
+ rightbaudrate: 右手波特率
+ lefthand: 左手机器人类型
+ righthand: 右手机器人类型
+ handcore: HandCore 实例
+ lefthandpubprint: 是否打印左手调试信息
+ righthandpubprint: 是否打印右手调试信息
+ calibration: True=强制标定, False=尝试加载缓存
+ auto_detect: 是否自动检测串口（默认 True）
+ isgetdebug: 是否发布debug测试数据话题（默认False）
+ baseconfig: 基础配置字典
+ """
+
+ self.node = node
+ self.lefthandtype = lefthand
+ self.righthandtype = righthand
+ self.handcore = handcore
+ self.runing = True
+ self.lefthandpubprint = lefthandpubprint
+ self.righthandpubprint = righthandpubprint
+ self.isdebugpub = isgetdebug
+ self.baseconfig = baseconfig or {}
+
+ self.show_fist_calibration = self.baseconfig.get('calibration', {}).get('show_fist', True)
+ self.fist_extend_ratio = self.baseconfig.get('calibration', {}).get('fist_extend_ratio', 0.5)
+
+ # 命令行串口参数（候选列表，系统自动识别左右手）
+ self.cmd_ports = cmd_ports
+ self.cmd_baudrate = cmd_baudrate
+ self.cmd_auto_scan = cmd_auto_scan
+
+ # 根据右手类型初始化
+ mapper_debug = self.baseconfig.get('debug', {}).get('mapper_debug', False)
+
+ if self.righthandtype == RobotName.o7 \
+ or self.righthandtype == RobotName.l7 \
+ or self.righthandtype == RobotName.o7v1 \
+ or self.righthandtype == RobotName.o7v3:
+ from .hand.linkerforce_l7 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand], is_debug=mapper_debug)
+
+ elif self.righthandtype == RobotName.o6:
+ from .hand.linkerforce_o6 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand], is_debug=mapper_debug)
+ elif self.righthandtype == RobotName.l6:
+ from .hand.linkerforce_l6 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand], is_debug=mapper_debug)
+ elif self.righthandtype == RobotName.l25 \
+ or self.righthandtype == RobotName.g20:
+ from .hand.linkerforce_g20 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand], is_debug=mapper_debug)
+ elif self.righthandtype == RobotName.l20:
+ from .hand.linkerforce_l20 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand], is_debug=mapper_debug)
+ elif self.righthandtype == RobotName.l10 \
+ or self.righthandtype == RobotName.l10v7 \
+ or self.righthandtype == RobotName.l20lite:
+ from .hand.linkerforce_l10 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand], is_debug=mapper_debug)
+ else:
+ print("未正确定义机械左手对象，请检查支持清单列表!")
+
+ # 根据左手类型初始化
+ if self.lefthandtype == RobotName.o7 \
+ or self.lefthandtype == RobotName.l7 \
+ or self.lefthandtype == RobotName.o7v1 \
+ or self.lefthandtype == RobotName.o7v3:
+ from .hand.linkerforce_l7 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand], is_debug=mapper_debug)
+ elif self.lefthandtype == RobotName.o6:
+ from .hand.linkerforce_o6 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand], is_debug=mapper_debug)
+ elif self.lefthandtype == RobotName.l6:
+ from .hand.linkerforce_l6 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand], is_debug=mapper_debug)
+ elif self.lefthandtype == RobotName.l25 \
+ or self.lefthandtype == RobotName.g20:
+ from .hand.linkerforce_g20 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand], is_debug=mapper_debug)
+ elif self.lefthandtype == RobotName.l20:
+ from .hand.linkerforce_l20 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand], is_debug=mapper_debug)
+ elif self.lefthandtype == RobotName.l10 \
+ or self.lefthandtype == RobotName.l10v7 \
+ or self.lefthandtype == RobotName.l20lite:
+ from .hand.linkerforce_l10 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand], is_debug=mapper_debug)
+ else:
+ print("未正确定义机械右手对象，请检查支持清单列表!")
+
+ self.node.get_logger().info(f"[机械手] 左手型号: {self.lefthandtype.name}, 右手型号: {self.righthandtype.name}")
+
+ # ROS2 发布器
+ self.publisher_r = self.node.create_publisher(
+ JointState,
+ '/cb_right_hand_control_cmd',
+ self.handcore.hand_numjoints_r)
+
+ self.publisher_l = self.node.create_publisher(
+ JointState,
+ '/cb_left_hand_control_cmd',
+ self.handcore.hand_numjoints_l)
+
+ self.publisher_angle_r = self.node.create_publisher(
+ JointState,
+ '/cb_right_hand_control_angle_cmd',
+ self.handcore.hand_numjoints_r)
+
+ self.publisher_angle_l = self.node.create_publisher(
+ JointState,
+ '/cb_left_hand_control_angle_cmd',
+ self.handcore.hand_numjoints_l)
+
+ # 创建订阅者，订阅/cb_left_hand_matrix_touch话题
+ self.left_touch_subscription = self.node.create_subscription(
+ String,
+ '/cb_left_hand_matrix_touch',
+ self.touch_left_callback,
+ 10 # QoS 队列深度
+ )
+ self.right_touch_subscription = self.node.create_subscription(
+ String,
+ '/cb_right_hand_matrix_touch',
+ self.touch_right_callback,
+ 10 # QoS 队列深度
+ )
+
+
+ if self.isdebugpub:
+ # # ROS1 发布器，触感矩阵转换相关
+ # self.publisher_hand_matrix2int_r = self.node.create_publisher(
+ # '/cb_right_hand_matrix2int',
+ # Int32MultiArray,
+ # self.handcore.hand_numjoints_r)
+
+ # self.publisher_hand_matrix2int_l = self.node.create_publisher(
+ # '/cb_left_hand_matrix2int',
+ # Int32MultiArray,
+ # self.handcore.hand_numjoints_l)
+
+
+ self.publisher_hand_debugdata_r = self.node.create_publisher(
+ Float32MultiArray,
+ '/cb_right_hand_debug',
+ self.handcore.hand_numjoints_r)
+
+
+ self.publisher_hand_debugdata_l = self.node.create_publisher(
+ Float32MultiArray,
+ '/cb_left_hand_debug',
+ self.handcore.hand_numjoints_l)
+
+ # 延时测量发布器
+ self.latency_pub = self.node.create_publisher(
+ Float64MultiArray,
+ '/latency_stamp',
+ 100
+ )
+ self._latency_seq = 0 # 帧序号，嵌入 frame_id 用于跨节点匹配
+
+ # 初始化统计结果
+ self.results = {
+ 'left':{},
+ 'right':{}
+ }
+ self.leftforcesendcount = -1
+ self.rightforcesendcount = -1
+
+ # 状态变量
+ self.pubprintcount = 0
+ self.force_reader_left = None
+ self.force_reader_right = None
+ self.calibration = calibration
+ self.leftport = None
+ self.leftbaudrate = None
+ self.rightport = None
+ self.rightbaudrate = None
+
+ # 强制手套数据源 (none/open/fist/opose)
+ self.force_glove_pose = None
+ self.calibration_cache = None # 缓存标定数据
+
+ # 力数据线程锁
+ self.forcelock = threading.Lock()
+
+ # 自动标定相关变量
+ self.calibration_data_left = []
+ self.calibration_data_right = []
+ self.calibration_in_progress = False
+
+ # ========== 调试：映射层跳变检测 ==========
+ self.debug_enabled = True # 设为 False 关闭调试
+ self.debug_motor_jump_threshold = 20 # 电机值跳变阈值
+ self.debug_last_motor_l = [255] * 6
+ self.debug_last_motor_r = [255] * 6
+ self.debug_last_raw_l = [0.0] * 21
+ self.debug_last_raw_r = [0.0] * 21
+
+
+
+ def touch_left_callback(self, msg):
+ self.process_touch_data(msg.data,'left')
+
+ def touch_right_callback(self, msg):
+ self.process_touch_data(msg.data,'right')
+
+ def process_touch_data(self, json_str, hand_type):
+ with self.forcelock:
+ try:
+ data = json.loads(json_str)
+ self.results[hand_type] = {}
+ # 处理每个手指的矩阵
+ for finger in ['thumb_matrix', 'index_matrix', 'middle_matrix', 'ring_matrix', 'little_matrix']:
+ matrix = np.array(data[finger])
+ # 计算接触面积（非零元素数量）
+ contact_area = np.count_nonzero(matrix)
+ # 计算总接触力
+ total_force = np.sum(matrix)
+ # 计算平均接触力（避免除以零）
+ avg_force = total_force / contact_area if contact_area > 0 else 0
+ max_force = np.max(matrix) * 4 if contact_area > 0 else 0
+ if max_force > 500:
+ max_force = 500
+ self.results[hand_type][finger] = {
+ 'contact_area': contact_area,
+ 'total_force': total_force,
+ 'avg_force': avg_force,
+ 'max_force': max_force
+ }
+
+ except Exception as e:
+ self.node.get_logger().error("Error processing touch data: %s" % str(e))
+ return None
+
+ def linkerforce_init(self):
+ # 从配置读取串口参数
+ serial_config = self.baseconfig.get('serial', {})
+ baudrates = serial_config.get('baudrates', [2000000, 1000000, 921600, 460800])
+ exclude_ports = serial_config.get('exclude_ports', [])
+ serial_debug = serial_config.get('serial_debug', False)
+ config_auto_scan = serial_config.get('auto_scan', False)
+
+ # 自动扫描开关：命令行参数 > 配置文件 > 默认false
+ auto_scan = self.cmd_auto_scan if self.cmd_auto_scan is not None else config_auto_scan
+
+ saved_left = serial_config.get('left', {})
+ saved_right = serial_config.get('right', {})
+
+ # 确定候选端口列表
+ if self.cmd_ports:
+ candidate_ports = self.cmd_ports
+ self.node.get_logger().info(f"使用命令行候选串口: {candidate_ports}")
+ else:
+ candidate_ports = None
+ self.node.get_logger().info(f"使用配置文件串口: 左手={saved_left.get('port')}, 右手={saved_right.get('port')}")
+
+ # 确定波特率
+ if self.cmd_baudrate:
+ baudrates = [self.cmd_baudrate]
+
+ self.node.get_logger().info(f"波特率组合: {baudrates}, 自动扫描={auto_scan}, 调试={serial_debug}")
+
+ # 日志回调函数
+ def serial_logger(level, msg):
+ if level == 'error':
+ self.node.get_logger().error(msg)
+ elif level == 'warn':
+ self.node.get_logger().warn(msg)
+ elif level == 'debug':
+ self.node.get_logger().debug(msg)
+ else:
+ self.node.get_logger().info(msg)
+
+ # 如果提供了候选端口列表，从中自动检测左右手
+ if candidate_ports:
+ left_found, right_found = self._init_from_candidates(
+ candidate_ports, baudrates, exclude_ports, serial_debug, serial_logger
+ )
+ else:
+ # 使用配置文件的预设端口
+ self.force_reader_left = ForceSerialReader(
+ HandType.left,
+ excludelist=exclude_ports,
+ baudrates=baudrates,
+ isdebug=serial_debug,
+ logger=serial_logger
+ )
+ left_found = self._init_hand(
+ self.force_reader_left,
+ 'Left',
+ saved_left.get('port'),
+ saved_left.get('baudrate'),
+ '左手',
+ auto_scan
+ )
+
+ exclude_right = exclude_ports + ([self.leftport] if left_found else [])
+ self.force_reader_right = ForceSerialReader(
+ HandType.right,
+ excludelist=exclude_right,
+ baudrates=baudrates,
+ isdebug=serial_debug,
+ logger=serial_logger
+ )
+ right_found = self._init_hand(
+ self.force_reader_right,
+ 'Right',
+ saved_right.get('port') if saved_right.get('port') not in exclude_right else None,
+ saved_right.get('baudrate'),
+ '右手',
+ auto_scan
+ )
+
+ # 保存检测到的串口配置
+ if left_found or right_found:
+ self._save_serial_to_config(left_found, right_found)
+
+ time.sleep(1)
+
+ # 标定流程
+ if self.calibration is True:
+ self.calibration = "auto_calibrate"
+ self.node.get_logger().info("强制标定模式：将进行自动标定")
+ else:
+ if self._load_from_tmp() is True:
+ self.node.get_logger().info("已加载缓存标定数据，跳过标定流程")
+ self.calibration = -1
+ self.righthand.initialize_mapper()
+ self.lefthand.initialize_mapper()
+ else:
+ self.calibration = "auto_calibrate"
+ self.node.get_logger().info("未找到有效缓存，将进行自动标定")
+
+ def _init_from_candidates(self, candidate_ports, baudrates, exclude_ports, serial_debug, serial_logger):
+ """从候选端口列表中自动检测并初始化左右手"""
+ left_found = False
+ right_found = False
+ detected_ports = {}
+
+ for port in candidate_ports:
+ if port in exclude_ports:
+ continue
+
+ # 检查端口是否存在
+ if not os.path.exists(port):
+ self.node.get_logger().warn(f"端口不存在: {port}")
+ continue
+
+ self.node.get_logger().info(f"检测候选端口: {port}")
+
+ temp_reader = ForceSerialReader(
+ HandType.left,
+ excludelist=[],
+ baudrates=baudrates,
+ isdebug=serial_debug,
+ logger=serial_logger
+ )
+
+ detected = False
+ for baudrate in baudrates:
+ try:
+ if temp_reader.openserial(port=port, baudrate=baudrate):
+ temp_reader.start()
+ time.sleep(0.1)
+ temp_reader.serial_port.write(temp_reader.pack_01_data())
+
+ for _ in range(10):
+ time.sleep(0.1)
+ if temp_reader.handtype:
+ detected_ports[port] = (temp_reader.handtype, baudrate, temp_reader.version)
+ self.node.get_logger().info(f"检测到 {port}: {temp_reader.handtype} @ {baudrate}")
+ detected = True
+ break
+
+ if detected:
+ break
+ except Exception as e:
+ self.node.get_logger().debug(f"端口 {port} @ {baudrate} 检测失败: {e}")
+ finally:
+ temp_reader.stop()
+
+ if not detected:
+ self.node.get_logger().warn(f"端口 {port} 未能识别设备类型")
+
+ # 根据检测到的手型初始化
+ for port, (handtype, baudrate, version) in detected_ports.items():
+ if handtype == 'Left' and not left_found:
+ self.force_reader_left = ForceSerialReader(
+ HandType.left,
+ excludelist=exclude_ports,
+ baudrates=baudrates,
+ isdebug=serial_debug,
+ logger=serial_logger
+ )
+ if self.force_reader_left.openserial(port=port, baudrate=baudrate):
+ self.force_reader_left.start()
+ self.force_reader_left.serial_port.write(self.force_reader_left.pack_01_data())
+ self.leftport = port
+ self.leftbaudrate = baudrate
+ left_found = True
+ self.node.get_logger().info(f"左手已连接: {port} @ {baudrate}, 版本 {version}")
+ if version:
+ self.lefthand.set_glove_version(version)
+ self.node.get_logger().info(f"[手套版本] 左手: v{version.split('.')[0]} ({version})")
+
+ elif handtype == 'Right' and not right_found:
+ self.force_reader_right = ForceSerialReader(
+ HandType.right,
+ excludelist=exclude_ports + ([self.leftport] if left_found else []),
+ baudrates=baudrates,
+ isdebug=serial_debug,
+ logger=serial_logger
+ )
+ if self.force_reader_right.openserial(port=port, baudrate=baudrate):
+ self.force_reader_right.start()
+ self.force_reader_right.serial_port.write(self.force_reader_right.pack_01_data())
+ self.rightport = port
+ self.rightbaudrate = baudrate
+ right_found = True
+ self.node.get_logger().info(f"右手已连接: {port} @ {baudrate}, 版本 {version}")
+ if version:
+ self.righthand.set_glove_version(version)
+ self.node.get_logger().info(f"[手套版本] 右手: v{version.split('.')[0]} ({version})")
+
+ # 初始化未找到的 reader（占位）
+ if not left_found:
+ self.node.get_logger().error("未找到左手力反馈手套")
+ self.force_reader_left = None
+ if not right_found:
+ self.node.get_logger().error("未找到右手力反馈手套")
+ self.force_reader_right = None
+
+ return left_found, right_found
+
+ def _init_hand(self, reader, hand_type, saved_port, saved_baudrate, hand_name, auto_scan=False):
+ """初始化单个手的串口连接"""
+ found = False
+
+ # 尝试预设串口
+ if saved_port and saved_baudrate:
+ self.node.get_logger().info(f"尝试预设{hand_name}串口: {saved_port}")
+ try:
+ if reader.openserial(port=saved_port, baudrate=int(saved_baudrate)):
+ time.sleep(0.3)
+ reader.start()
+ time.sleep(0.3)
+ reader.serial_port.write(reader.pack_01_data())
+ time.sleep(0.3)
+ if reader.handtype == hand_type:
+ self.node.get_logger().info(f"预设{hand_name}串口有效, 版本{reader.version}")
+ if hand_type == 'Left':
+ self.leftport = saved_port
+ self.leftbaudrate = int(saved_baudrate)
+ if reader.version:
+ self.lefthand.set_glove_version(reader.version)
+ self.node.get_logger().info(f"[手套版本] 左手: v{reader.version.split('.')[0]} ({reader.version})")
+ else:
+ self.rightport = saved_port
+ self.rightbaudrate = int(saved_baudrate)
+ if reader.version:
+ self.righthand.set_glove_version(reader.version)
+ self.node.get_logger().info(f"[手套版本] 右手: v{reader.version.split('.')[0]} ({reader.version})")
+ found = True
+ else:
+ reader.stop()
+ self.node.get_logger().warn(f"预设{hand_name}串口类型不匹配")
+ except Exception as e:
+ self.node.get_logger().warn(f"预设{hand_name}串口无效: {e}")
+
+ # 预设无效，根据 auto_scan 决定是否搜索设备
+ if not found:
+ if auto_scan:
+ self.node.get_logger().info(f"搜索{hand_name}力反馈手套...")
+ port, baudrate, errorcode = reader.find_valid_ports(timeout=0.001)
+ if port:
+ if reader.openserial(port=port, baudrate=baudrate):
+ time.sleep(0.3)
+ reader.start()
+ time.sleep(0.3)
+ reader.serial_port.write(reader.pack_01_data())
+ time.sleep(0.3)
+ if reader.handtype == hand_type:
+ self.node.get_logger().info(f"已搜索到{hand_name}力反馈手套, 版本{reader.version}")
+ if hand_type == 'Left':
+ self.leftport = port
+ self.leftbaudrate = baudrate
+ if reader.version:
+ self.lefthand.set_glove_version(reader.version)
+ self.node.get_logger().info(f"[手套版本] 左手: v{reader.version.split('.')[0]} ({reader.version})")
+ else:
+ self.rightport = port
+ self.rightbaudrate = baudrate
+ if reader.version:
+ self.righthand.set_glove_version(reader.version)
+ self.node.get_logger().info(f"[手套版本] 右手: v{reader.version.split('.')[0]} ({reader.version})")
+ found = True
+ else:
+ self.node.get_logger().warn(f"{hand_name}无法正常识别")
+ else:
+ self.node.get_logger().warn(f"未搜索到{hand_name}力反馈手套")
+ else:
+ self.node.get_logger().warn(f"{hand_name}串口未连接（自动扫描已禁用）")
+
+ return found
+
+ def _save_serial_to_config(self, left_found, right_found):
+ """保存检测到的串口配置到 base_config.yml"""
+ config_path = Path(__file__).parent.parent.parent / "config" / "base_config.yml"
+ try:
+ with open(config_path, 'r') as f:
+ config = yaml.safe_load(f)
+
+ if 'serial' not in config:
+ config['serial'] = {}
+ if 'left' not in config['serial']:
+ config['serial']['left'] = {}
+ if 'right' not in config['serial']:
+ config['serial']['right'] = {}
+
+ if left_found:
+ config['serial']['left']['port'] = self.leftport
+ config['serial']['left']['baudrate'] = self.leftbaudrate
+ if right_found:
+ config['serial']['right']['port'] = self.rightport
+ config['serial']['right']['baudrate'] = self.rightbaudrate
+
+ with open(config_path, 'w') as f:
+ yaml.dump(config, f, default_flow_style=False)
+
+ self.node.get_logger().info(f"串口配置已保存: 左手={self.leftport}, 右手={self.rightport}")
+ except Exception as e:
+ self.node.get_logger().error(f"保存串口配置失败: {e}")
+
+ def process_callback(self):
+ if not self.runing:
+ return
+
+ self.pubprintcount += 1
+
+ left_valid = self.force_reader_left and self.force_reader_left.handtype == 'Left'
+ right_valid = self.force_reader_right and self.force_reader_right.handtype == 'Right'
+
+ warn_interval = 150
+
+ if not left_valid and not right_valid and not self.force_glove_pose:
+ if self.pubprintcount % warn_interval == 1:
+ self.node.get_logger().warn("设备未连接: 左手套(未连接), 右手套(未连接)")
+ return
+
+ if left_valid and not right_valid and not self.force_glove_pose:
+ if self.pubprintcount % warn_interval == 1:
+ self.node.get_logger().warn("设备未连接: 右手套(未连接)")
+ elif right_valid and not left_valid and not self.force_glove_pose:
+ if self.pubprintcount % warn_interval == 1:
+ self.node.get_logger().warn("设备未连接: 左手套(未连接)")
+
+
+ if left_valid or self.force_glove_pose:
+ t0 = time.perf_counter() # ---- 延时: 手套数据到达 ----
+ if self.force_glove_pose:
+ left_positions = self._get_forced_positions(self.force_glove_pose, 'left')
+ if left_positions is None:
+ if left_valid:
+ left_positions = copy.deepcopy(self.force_reader_left.poslist)
+ self.node.get_logger().warn(f"强制姿态 {self.force_glove_pose} 数据不存在，使用实际数据")
+ else:
+ self.node.get_logger().warn(f"强制姿态 {self.force_glove_pose} 数据不存在且无设备")
+ return
+ else:
+ left_positions = copy.deepcopy(self.force_reader_left.poslist)
+
+ t1 = time.perf_counter() # ---- 延时: 重映射开始 ----
+ self.lefthand.joint_update(left_positions)
+ t2 = time.perf_counter() # ---- 延时: 重映射结束 ----
+ self.lefthand.speed_update()
+ if left_valid:
+ with self.forcelock:
+ if self.results['left']:
+ self.force_reader_left.forcelist = [
+ self.results['left']['thumb_matrix']['max_force'],
+ self.results['left']['index_matrix']['max_force'],
+ self.results['left']['middle_matrix']['max_force'],
+ self.results['left']['ring_matrix']['max_force'],
+ self.results['left']['little_matrix']['max_force']
+ ]
+ self.force_reader_left.serial_port.write(self.force_reader_left.pack_04_data())
+ if self.lefthandpubprint and self.pubprintcount % 5 == 0:
+ print(f"左手位置: {self.lefthand.g_jointpositions}")
+
+ t3 = time.perf_counter() # ---- 延时: ROS2发布前 ----
+ self._latency_seq += 1
+ msg_l = JointState()
+ msg_l.header.stamp = self.node.get_clock().now().to_msg()
+ msg_l.header.frame_id = f"lat_{self._latency_seq}" # 跨节点匹配用
+ msg_l.name = [f'joint{i + 1}' for i in range(len(self.lefthand.g_jointpositions))]
+ msg_l.position = [float(num) for num in self.lefthand.g_jointpositions]
+ self.publisher_l.publish(msg_l)
+
+ # 发布延时时间戳 (retarget 端: t0-t3)
+ latency_msg = Float64MultiArray()
+ latency_msg.data = [float(self._latency_seq), 0.0, t0, t1, t2, t3]
+ self.latency_pub.publish(latency_msg)
+
+ if self.isdebugpub:
+ msg_debug_l = Float32MultiArray()
+ msg_debug_l.data = [float(num) for num in self.lefthand.multi_state_mapper.debug_value]
+ self.publisher_hand_debugdata_l.publish(msg_debug_l)
+
+ if right_valid or self.force_glove_pose:
+ t0 = time.perf_counter() # ---- 延时: 手套数据到达 ----
+ if self.force_glove_pose:
+ right_positions = self._get_forced_positions(self.force_glove_pose, 'right')
+ if right_positions is None:
+ if right_valid:
+ right_positions = copy.deepcopy(self.force_reader_right.poslist)
+ self.node.get_logger().warn(f"强制姿态 {self.force_glove_pose} 数据不存在，使用实际数据")
+ else:
+ self.node.get_logger().warn(f"强制姿态 {self.force_glove_pose} 数据不存在且无设备")
+ return
+ else:
+ right_positions = copy.deepcopy(self.force_reader_right.poslist)
+
+ t1 = time.perf_counter() # ---- 延时: 重映射开始 ----
+ self.righthand.joint_update(right_positions)
+ t2 = time.perf_counter() # ---- 延时: 重映射结束 ----
+ self.righthand.speed_update()
+ if right_valid:
+ with self.forcelock:
+ if self.results['right']:
+ self.force_reader_right.forcelist = [
+ self.results['right']['thumb_matrix']['max_force'],
+ self.results['right']['index_matrix']['max_force'],
+ self.results['right']['middle_matrix']['max_force'],
+ self.results['right']['ring_matrix']['max_force'],
+ self.results['right']['little_matrix']['max_force']
+ ]
+ self.force_reader_right.serial_port.write(self.force_reader_right.pack_04_data())
+ if self.righthandpubprint and self.pubprintcount % 5 == 0:
+ print(f"右手位置: {self.righthand.g_jointpositions}")
+
+ t3 = time.perf_counter() # ---- 延时: ROS2发布前 ----
+ self._latency_seq += 1
+ msg_r = JointState()
+ msg_r.header.stamp = self.node.get_clock().now().to_msg()
+ msg_r.header.frame_id = f"lat_{self._latency_seq}" # 跨节点匹配用
+ msg_r.name = [f'joint{i + 1}' for i in range(len(self.righthand.g_jointpositions))]
+ msg_r.position = [float(num) for num in self.righthand.g_jointpositions]
+ msg_r.velocity = [float(num) for num in self.righthand.g_jointvelocity]
+ self.publisher_r.publish(msg_r)
+
+ # 发布延时时间戳 (retarget 端: t0-t3)
+ latency_msg = Float64MultiArray()
+ latency_msg.data = [float(self._latency_seq), 0.0, t0, t1, t2, t3]
+ self.latency_pub.publish(latency_msg)
+
+ if self.isdebugpub:
+ msg_debug_r = Float32MultiArray()
+ msg_debug_r.data = [float(num) for num in self.righthand.multi_state_mapper.debug_value]
+
+ self.publisher_hand_debugdata_r.publish(msg_debug_r)
+
+ self.pubprintcount += 1
+
+ def _calculate_weighted_average(self, data_list):
+ """
+ 计算加权平均值，后面的数据权重更高
+
+ Args:
+ data_list: 包含多帧数据的列表，每帧是21个关节值的列表
+
+ Returns:
+ 加权平均后的21个关节值列表
+ """
+ if not data_list:
+ return [0.0] * 21
+
+ n = len(data_list)
+ if n == 1:
+ return data_list[0]
+
+ # 生成权重：后面的数据权重更高
+ weights = np.array([i + 1 for i in range(n)], dtype=float)
+ weights = weights / weights.sum()
+
+ # 转换为numpy数组进行计算
+ data_array = np.array(data_list)
+
+ # 加权平均
+ weighted_avg = np.average(data_array, axis=0, weights=weights)
+
+ return weighted_avg.tolist()
+
+ def _check_stability(self, window_size=20, threshold=0.05):
+ """
+ 检测手势稳定性
+
+ Args:
+ window_size: 检测窗口大小（帧数）
+ threshold: 稳定性阈值（关节角度方差）
+
+ Returns:
+ (is_stable, variance): 是否稳定，当前方差
+ """
+ if len(self.calibration_data_left) < window_size:
+ return False, 1.0
+
+ recent_left = self.calibration_data_left[-window_size:]
+ recent_right = self.calibration_data_right[-window_size:]
+
+ var_left = np.var(recent_left, axis=0).mean()
+ var_right = np.var(recent_right, axis=0).mean()
+
+ is_stable = var_left < threshold and var_right < threshold
+ return is_stable, max(var_left, var_right)
+
+ def _is_left_connected(self):
+ return self.force_reader_left is not None and self.force_reader_left.handtype == 'Left'
+
+ def _is_right_connected(self):
+ return self.force_reader_right is not None and self.force_reader_right.handtype == 'Right'
+
+ def _connected_hand_labels(self):
+ labels = []
+ if self._is_left_connected():
+ labels.append("左手")
+ if self._is_right_connected():
+ labels.append("右手")
+ return labels
+
+ def _calibration_with_progress(self, stability_window=30, stability_threshold=0.03):
+ """
+ 带稳定性检测的标定数据采集
+
+ Args:
+ stability_window: 稳定性检测窗口（帧数）
+ stability_threshold: 稳定性阈值
+ """
+ left_valid = self._is_left_connected()
+ right_valid = self._is_right_connected()
+ if not left_valid and not right_valid:
+ return False
+
+ self.calibration_data_left = []
+ self.calibration_data_right = []
+
+ temp_buffer_left = []
+ temp_buffer_right = []
+
+ stability_samples = []
+ stable_start = None
+ collected_duration = 0
+ target_stable_duration = 5.0
+
+ with tqdm(total=100, desc=f"{Fore.CYAN}标定进度{Fore.RESET}",
+ bar_format="{l_bar}{bar}| {n_fmt}/{total_fmt} [{postfix}]",
+ postfix="") as pbar:
+ while collected_duration < target_stable_duration:
+ left_pos = copy.deepcopy(self.force_reader_left.poslist) if left_valid else None
+ right_pos = copy.deepcopy(self.force_reader_right.poslist) if right_valid else None
+
+ if left_valid:
+ temp_buffer_left.append(left_pos)
+ if right_valid:
+ temp_buffer_right.append(right_pos)
+
+ if left_valid and len(temp_buffer_left) > stability_window:
+ temp_buffer_left.pop(0)
+ if right_valid and len(temp_buffer_right) > stability_window:
+ temp_buffer_right.pop(0)
+
+ buffer_lengths = []
+ if left_valid:
+ buffer_lengths.append(len(temp_buffer_left))
+ if right_valid:
+ buffer_lengths.append(len(temp_buffer_right))
+ min_buffer_len = min(buffer_lengths)
+ if min_buffer_len < stability_window:
+ pbar.set_postfix_str(f"{Fore.YELLOW}等待数据 {min_buffer_len}/{stability_window}{Fore.RESET}")
+ pbar.refresh()
+ time.sleep(1.0 / 30)
+ continue
+
+ variances = []
+ drifts = []
+ if left_valid:
+ var_left = np.var(temp_buffer_left, axis=0).mean()
+ drift_left = np.abs(np.array(temp_buffer_left[-1]) - np.array(temp_buffer_left[0])).mean()
+ variances.append(var_left)
+ drifts.append(drift_left)
+ if right_valid:
+ var_right = np.var(temp_buffer_right, axis=0).mean()
+ drift_right = np.abs(np.array(temp_buffer_right[-1]) - np.array(temp_buffer_right[0])).mean()
+ variances.append(var_right)
+ drifts.append(drift_right)
+
+ variance = max(variances)
+ drift = max(drifts)
+
+ is_stable = (variance < stability_threshold and drift < stability_threshold)
+
+ if is_stable:
+ if stable_start is None:
+ stable_start = time.time()
+
+ stability_samples.append((left_pos, right_pos))
+ collected_duration = time.time() - stable_start
+
+ progress = min(100, int(collected_duration / target_stable_duration * 100))
+ pbar.n = progress
+ pbar.last_print_n = progress
+ pbar.set_postfix_str(f"{Fore.GREEN}稳定 {collected_duration:.1f}s var={variance:.3f} drift={drift:.3f}{Fore.RESET}")
+ else:
+ stable_start = None
+ stability_samples = []
+ collected_duration = 0
+ pbar.n = 0
+ pbar.last_print_n = 0
+ pbar.set_postfix_str(f"{Fore.YELLOW}等待稳定 var={variance:.3f} drift={drift:.3f}{Fore.RESET}")
+
+ pbar.refresh()
+ time.sleep(1.0 / 30)
+
+ if len(stability_samples) > 0:
+ self.calibration_data_left = [s[0] for s in stability_samples if s[0] is not None]
+ self.calibration_data_right = [s[1] for s in stability_samples if s[1] is not None]
+ print(f"{Fore.GREEN}采集完成，有效样本: {len(stability_samples)} 帧{Fore.RESET}")
+ return True
+ return False
+
+ def run_calibration(self):
+ """
+ 执行自动标定流程
+ """
+ # 检查手套连接状态
+ left_valid = self._is_left_connected()
+ right_valid = self._is_right_connected()
+
+ if not left_valid and not right_valid:
+ print(f"\n{Fore.RED}【标定失败】左右手套均未连接，请检查设备连接后重试{Fore.RESET}\n")
+ self.calibration_in_progress = False
+ return False
+ print(f"\n{Fore.CYAN}本次标定设备: {', '.join(self._connected_hand_labels())}{Fore.RESET}")
+ if not left_valid:
+ print(f"{Fore.YELLOW}左手未连接，本次跳过左手采样，并保留已有/样例标定数据{Fore.RESET}")
+ if not right_valid:
+ print(f"{Fore.YELLOW}右手未连接，本次跳过右手采样，并保留已有/样例标定数据{Fore.RESET}")
+
+ self.calibration_in_progress = True
+
+ # ===== 第一步：五指张开标定 (对应255) =====
+ self.calibration_data_left = []
+ self.calibration_data_right = []
+ total_steps = 2 if not self.show_fist_calibration else 3
+ print(f"\n{Fore.GREEN}{'='*50}{Fore.RESET}")
+ print(f"{Fore.GREEN}【标定 1/{total_steps}】请保持五指张开姿势 (对应电机值255){Fore.RESET}")
+ print(f"{Fore.GREEN}{'='*50}{Fore.RESET}\n")
+
+ self._calibration_with_progress(10)
+
+ open_ok = ((not left_valid or len(self.calibration_data_left) > 0) and
+ (not right_valid or len(self.calibration_data_right) > 0))
+ if open_ok:
+ if left_valid:
+ avg_left_open = self._calculate_weighted_average(self.calibration_data_left)
+ self.lefthand.calibrationoriginal = avg_left_open
+ if right_valid:
+ avg_right_open = self._calculate_weighted_average(self.calibration_data_right)
+ self.righthand.calibrationoriginal = avg_right_open
+ else:
+ print(f"\n{Fore.RED}【标定失败】五指张开数据采集失败{Fore.RESET}\n")
+ self.calibration_in_progress = False
+ return False
+
+ # ===== 第二步：O型标定 (对应中间值) =====
+ self.calibration_data_left = []
+ self.calibration_data_right = []
+ print(f"\n{Fore.MAGENTA}{'='*50}{Fore.RESET}")
+ print(f"{Fore.MAGENTA}【标定 2/{total_steps}】请保持O型手势 (对应电机中间值){Fore.RESET}")
+ print(f"{Fore.MAGENTA}{'='*50}{Fore.RESET}\n")
+
+ self._calibration_with_progress(10)
+
+ opose_ok = ((not left_valid or len(self.calibration_data_left) > 0) and
+ (not right_valid or len(self.calibration_data_right) > 0))
+ if opose_ok:
+ if left_valid:
+ avg_left_opose = self._calculate_weighted_average(self.calibration_data_left)
+ self.lefthand.calibrationopose = avg_left_opose
+ if right_valid:
+ avg_right_opose = self._calculate_weighted_average(self.calibration_data_right)
+ self.righthand.calibrationopose = avg_right_opose
+ else:
+ print(f"\n{Fore.RED}【标定失败】O型手势数据采集失败{Fore.RESET}\n")
+ self.calibration_in_progress = False
+ return False
+
+ # ===== 第三步：握拳标定 (可选) =====
+ if self.show_fist_calibration:
+ self.calibration_data_left = []
+ self.calibration_data_right = []
+ print(f"\n{Fore.YELLOW}{'='*50}{Fore.RESET}")
+ print(f"{Fore.YELLOW}【标定 3/{total_steps}】请握紧拳头 (对应电机值0){Fore.RESET}")
+ print(f"{Fore.YELLOW}{'='*50}{Fore.RESET}\n")
+
+ self._calibration_with_progress(10)
+
+ fist_ok = ((not left_valid or len(self.calibration_data_left) > 0) and
+ (not right_valid or len(self.calibration_data_right) > 0))
+ if fist_ok:
+ if left_valid:
+ avg_left_fist = self._calculate_weighted_average(self.calibration_data_left)
+ self.lefthand.calibrationfistpose = avg_left_fist
+ if right_valid:
+ avg_right_fist = self._calculate_weighted_average(self.calibration_data_right)
+ self.righthand.calibrationfistpose = avg_right_fist
+ else:
+ print(f"\n{Fore.RED}【标定失败】握拳数据采集失败{Fore.RESET}\n")
+ self.calibration_in_progress = False
+ return False
+ else:
+ self._calculate_fist_from_extension()
+
+ # ===== 保存标定数据 =====
+ if not self._save_to_tmp():
+ self.calibration_in_progress = False
+ return False
+ print(f"\n{Fore.GREEN}{'='*50}{Fore.RESET}")
+ print(f"{Fore.GREEN}【标定完成】{'三个' if self.show_fist_calibration else '两个'}姿势数据已保存{Fore.RESET}")
+ print(f"{Fore.GREEN}{'='*50}{Fore.RESET}\n")
+
+ self.calibration_in_progress = False
+ if right_valid:
+ self.righthand.initialize_mapper()
+ if left_valid:
+ self.lefthand.initialize_mapper()
+ return True
+
+ def _calculate_fist_from_extension(self):
+ """
+ 从 original 和 opose 延伸计算 fist 值
+ fist = opose + (opose - original) * extend_ratio
+ """
+ original_l = self.lefthand.calibrationoriginal
+ original_r = self.righthand.calibrationoriginal
+ opose_l = self.lefthand.calibrationopose
+ opose_r = self.righthand.calibrationopose
+
+ ratio = self.fist_extend_ratio
+ generated = []
+
+ if original_l is not None and opose_l is not None:
+ fist_l = []
+ for i in range(len(original_l)):
+ fist_l.append(opose_l[i] + (opose_l[i] - original_l[i]) * ratio)
+ self.lefthand.calibrationfistpose = fist_l
+ generated.append("左手")
+
+ if original_r is not None and opose_r is not None:
+ fist_r = []
+ for i in range(len(original_r)):
+ fist_r.append(opose_r[i] + (opose_r[i] - original_r[i]) * ratio)
+ self.righthand.calibrationfistpose = fist_r
+ generated.append("右手")
+
+ if generated:
+ self.node.get_logger().info(f"[自动计算] {', '.join(generated)}握拳值已从 O型延伸 {ratio*100:.0f}% 生成")
+
+ def _load_calibration_fallback_data(self):
+ """读取历史标定数据；没有历史数据时使用样例标定数据。"""
+ fallback_data = {}
+
+ if TMP_FILE_PATH.exists():
+ try:
+ fallback_data.update(json.loads(TMP_FILE_PATH.read_text()) or {})
+ except Exception as e:
+ self.node.get_logger().warn(f"读取历史标定数据失败，将尝试样例数据: {e}")
+
+ if SAMPLE_FILE_PATH.exists():
+ try:
+ with open(SAMPLE_FILE_PATH, 'r') as f:
+ sample_data = yaml.safe_load(f) or {}
+ for key, value in sample_data.items():
+ if fallback_data.get(key) is None:
+ fallback_data[key] = value
+ except Exception as e:
+ self.node.get_logger().warn(f"读取样例标定数据失败: {e}")
+
+ return fallback_data
+
+ def _save_to_tmp(self):
+ """
+ 保存标定数据到临时文件 (JSON格式，与ROS2一致)
+ - jointangleoriginal: 五指张开 (对应电机255)
+ - jointanglefist: 握拳 (对应电机0)
+ """
+ # v2.8.6 版本添加标定差异检测
+ def calculate_vector_difference(vec1, vec2):
+ """计算两个向量之间的差异"""
+ if vec1 is None or vec2 is None:
+ return 0
+
+ min_len = min(len(vec1), len(vec2))
+ if min_len == 0:
+ return 0
+
+ squared_diff = 0
+ for i in range(min_len):
+ squared_diff += (vec1[i] - vec2[i]) ** 2
+ return math.sqrt(squared_diff)
+
+ # 设置阈值，根据实际情况调整
+ # 这个阈值表示两个向量之间的最小可接受差异
+ MIN_DIFFERENCE_THRESHOLD = 3.0
+
+ right_connected = self._is_right_connected()
+ left_connected = self._is_left_connected()
+
+ # 检查右手数据是否有效
+ right_valid = False
+ if right_connected:
+ right_diff_original_fist = calculate_vector_difference(
+ self.righthand.calibrationoriginal,
+ self.righthand.calibrationfistpose
+ )
+ if right_diff_original_fist > MIN_DIFFERENCE_THRESHOLD:
+ right_valid = True
+ else:
+ self.node.get_logger().error("右手张手和握拳标定数据差异过小，可能未正确标定")
+
+ # 检查左手数据是否有效
+ left_valid = False
+ if left_connected:
+ left_diff_original_fist = calculate_vector_difference(
+ self.lefthand.calibrationoriginal,
+ self.lefthand.calibrationfistpose
+ )
+ if left_diff_original_fist > MIN_DIFFERENCE_THRESHOLD:
+ left_valid = True
+ else:
+ self.node.get_logger().error("左手张手和握拳标定数据差异过小，可能未正确标定")
+
+ # 如果没有有效的新数据，直接返回不保存
+ if not right_valid and not left_valid:
+ self.node.get_logger().error("没有有效的标定数据差异，取消保存")
+ return False
+
+ fallback_data = self._load_calibration_fallback_data()
+
+ data = {
+ "timestamp": datetime.now().isoformat(),
+ "jointangleoriginal_r": self.righthand.calibrationoriginal,
+ "jointangleoriginal_l": self.lefthand.calibrationoriginal,
+ "jointanglefist_r": self.righthand.calibrationfistpose,
+ "jointanglefist_l": self.lefthand.calibrationfistpose,
+ "jointangleopose_r": self.righthand.calibrationopose,
+ "jointangleopose_l": self.lefthand.calibrationopose
+ }
+
+ def apply_fallback(hand_label, hand_obj, suffix):
+ keys_and_attrs = [
+ (f"jointangleoriginal_{suffix}", "calibrationoriginal"),
+ (f"jointanglefist_{suffix}", "calibrationfistpose"),
+ (f"jointangleopose_{suffix}", "calibrationopose"),
+ ]
+ missing_keys = []
+ for key, attr in keys_and_attrs:
+ value = fallback_data.get(key)
+ if value is None:
+ missing_keys.append(key)
+ continue
+ data[key] = value
+ setattr(hand_obj, attr, value)
+
+ if missing_keys:
+ self.node.get_logger().warn(
+ f"{hand_label}未参与本次有效标定，且缺少历史/样例数据: {missing_keys}"
+ )
+ return False
+
+ self.node.get_logger().warning(f"{hand_label}未参与本次有效标定，采用历史/样例标定内容。")
+ return True
+
+ if not right_valid:
+ right_ready = apply_fallback("右手", self.righthand, "r")
+ else:
+ right_ready = True
+
+ if not left_valid:
+ left_ready = apply_fallback("左手", self.lefthand, "l")
+ else:
+ left_ready = True
+
+ if right_connected and not right_ready:
+ self.node.get_logger().error("右手缺少可用标定数据，取消保存")
+ return False
+ if left_connected and not left_ready:
+ self.node.get_logger().error("左手缺少可用标定数据，取消保存")
+ return False
+
+ try:
+ TMP_FILE_PATH.parent.mkdir(parents=True, exist_ok=True)
+ json_str = json.dumps(data, indent=2)
+ TMP_FILE_PATH.write_text(json_str)
+ self.node.get_logger().info("标定数据保存成功")
+ return True
+ except Exception as e:
+ self.node.get_logger().error(f"保存失败: {e}")
+ return False
+
+ def _load_from_tmp(self):
+ """
+ 从临时文件读取数据 (JSON格式)
+ 如果文件不存在，自动从样本数据(YAML)加载并保存
+ """
+ data = None
+ from_sample = False
+
+ if TMP_FILE_PATH.exists():
+ try:
+ content = TMP_FILE_PATH.read_text()
+ data = json.loads(content)
+ self.node.get_logger().info("加载用户标定数据")
+ except Exception as e:
+ self.node.get_logger().error(f"读取标定数据失败: {e}")
+
+ if data is None and SAMPLE_FILE_PATH.exists():
+ try:
+ with open(SAMPLE_FILE_PATH, 'r') as f:
+ data = yaml.safe_load(f)
+ from_sample = True
+ self.node.get_logger().info("首次使用，加载样本标定数据")
+ except Exception as e:
+ self.node.get_logger().error(f"读取样本数据失败: {e}")
+
+ if data is None:
+ self.node.get_logger().warn("标定数据不存在")
+ return False
+
+ if 'timestamp' not in data or not data['timestamp']:
+ self.node.get_logger().warn("无效的时间戳...")
+ return False
+
+ try:
+ saved_time = datetime.fromisoformat(str(data['timestamp']))
+ current_time = datetime.now()
+ time_diff = current_time - saved_time
+ if time_diff > timedelta(days=30):
+ self.node.get_logger().warn("标定数据已超过30天有效期，建议重新标定...")
+ except:
+ pass
+
+ self.righthand.calibrationoriginal = data.get('jointangleoriginal_r')
+ self.lefthand.calibrationoriginal = data.get('jointangleoriginal_l')
+
+ if data.get('jointanglefist_r'):
+ self.righthand.calibrationfistpose = data['jointanglefist_r']
+ if data.get('jointanglefist_l'):
+ self.lefthand.calibrationfistpose = data['jointanglefist_l']
+
+ if data.get('jointangleopose_r'):
+ self.righthand.calibrationopose = data['jointangleopose_r']
+ if data.get('jointangleopose_l'):
+ self.lefthand.calibrationopose = data['jointangleopose_l']
+
+ if self.lefthand.calibrationfistpose is None or self.righthand.calibrationfistpose is None:
+ if self.lefthand.calibrationoriginal and self.lefthand.calibrationopose:
+ self._calculate_fist_from_extension()
+
+ if from_sample:
+ TMP_FILE_PATH.parent.mkdir(parents=True, exist_ok=True)
+ with open(TMP_FILE_PATH, 'w') as f:
+ json.dump(data, f, indent=2)
+ self.node.get_logger().info("样本标定数据已保存到用户标定文件")
+
+ self.node.get_logger().info("标定数据加载成功")
+ return True
+
+ def _get_forced_positions(self, pose_type: str, hand_type: str):
+ """
+ 获取强制姿态的标定数据
+
+ Args:
+ pose_type: 姿态类型 (open/fist/opose)
+ hand_type: 手类型 (left/right)
+
+ Returns:
+ 位置数据列表，如果不存在返回 None
+ """
+ if not TMP_FILE_PATH.exists():
+ return None
+
+ if self.calibration_cache is None:
+ try:
+ content = TMP_FILE_PATH.read_text()
+ self.calibration_cache = json.loads(content)
+ except:
+ return None
+
+ hand = hand_type.lower()
+ pose_map = {
+ 'open': f'jointangleoriginal_{hand[0]}',
+ 'fist': f'jointanglefist_{hand[0]}',
+ 'opose': f'jointangleopose_{hand[0]}'
+ }
+
+ key = pose_map.get(pose_type)
+ if key and self.calibration_cache:
+ return self.calibration_cache.get(key)
+ return None
+
+ def process(self):
+ """主处理函数"""
+ # 初始化串口连接
+ self.linkerforce_init()
+ # 执行标定（如果需要）
+ if self.calibration == "auto_calibrate":
+ if not self.run_calibration():
+ self.node.get_logger().error("标定失败，退出程序")
+ return
+ self.calibration = -1
+ self.node.create_timer(1.0/100, self.process_callback) # 100Hz
+
+ def stop_serial_threads(self):
+ """停止串口线程，在 destroy_node 时调用"""
+ self.runing = False
+ if self.force_reader_left:
+ self.force_reader_left.stop()
+ if self.force_reader_right:
+ self.force_reader_right.stop()
+ self.node.get_logger().info("串口线程已停止")
+
+ def set_mode(self, mode, param=None):
+ """
+ 设置遥操作模式
+
+ Args:
+ mode: 运行模式
+ - 'glove': 使用手套数据
+ - 'fixed_opose': 使用固定O型姿态
+ - 'fixed_fist': 使用固定握拳姿态
+ param: 额外参数 (dict)
+ - serial_debug: bool, 开启串口调试
+ - mapper_debug: bool, 开启映射器调试
+ """
+ if param is None:
+ param = {}
+
+ # 处理串口调试开关
+ if 'serial_debug' in param:
+ debug_enabled = param['serial_debug']
+ if hasattr(self.force_reader_left, 'isdebug'):
+ self.force_reader_left.isdebug = debug_enabled
+ if hasattr(self.force_reader_right, 'isdebug'):
+ self.force_reader_right.isdebug = debug_enabled
+ self.node.get_logger().info(f"串口调试: {'开启' if debug_enabled else '关闭'}")
+
+ # 处理映射器调试开关
+ if 'mapper_debug' in param:
+ mapper_debug_enabled = param['mapper_debug']
+ if hasattr(self.righthand, 'multi_state_mapper') and hasattr(self.righthand.multi_state_mapper, 'set_debug'):
+ self.righthand.multi_state_mapper.set_debug(mapper_debug_enabled)
+ if hasattr(self.lefthand, 'multi_state_mapper') and hasattr(self.lefthand.multi_state_mapper, 'set_debug'):
+ self.lefthand.multi_state_mapper.set_debug(mapper_debug_enabled)
+ if isinstance(mapper_debug_enabled, list):
+ fingers_str = ', '.join(mapper_debug_enabled) if mapper_debug_enabled else '全部'
+ self.node.get_logger().info(f"映射器调试: 开启 (手指: {fingers_str})")
+ else:
+ self.node.get_logger().info(f"映射器调试: {'开启' if mapper_debug_enabled else '关闭'}")
+
+ # 处理强制手套数据源
+ if 'force_glove_pose' in param:
+ pose = param['force_glove_pose']
+ if pose in ['open', 'fist', 'opose', 'none', None]:
+ self.force_glove_pose = pose if pose != 'none' else None
+ if self.force_glove_pose:
+ self.node.get_logger().info(f"强制手套数据源: {self.force_glove_pose}")
+ else:
+ self.node.get_logger().info("强制手套数据源: 关闭，使用实际数据")
+ else:
+ self.node.get_logger().warn(f"无效的强制姿态: {pose}，可选: open/fist/opose/none")
+
+ # 处理延伸指数因子
+ if 'mapper_exp_factor' in param:
+ exp_factor = param['mapper_exp_factor']
+ if isinstance(exp_factor, dict):
+ # 指定手指设置
+ for finger, value in exp_factor.items():
+ if hasattr(self.righthand, 'multi_state_mapper') and hasattr(self.righthand.multi_state_mapper, 'exp_factors'):
+ if finger in self.righthand.multi_state_mapper.exp_factors:
+ self.righthand.multi_state_mapper.exp_factors[finger] = value
+ if hasattr(self.lefthand, 'multi_state_mapper') and hasattr(self.lefthand.multi_state_mapper, 'exp_factors'):
+ if finger in self.lefthand.multi_state_mapper.exp_factors:
+ self.lefthand.multi_state_mapper.exp_factors[finger] = value
+ fingers_str = ', '.join([f"{k}:{v}" for k, v in exp_factor.items()])
+ self.node.get_logger().info(f"延伸指数因子(指定): {fingers_str}")
+ else:
+ # 全部手指设置
+ if hasattr(self.righthand, 'multi_state_mapper') and hasattr(self.righthand.multi_state_mapper, 'exp_factors'):
+ for finger in self.righthand.multi_state_mapper.exp_factors:
+ self.righthand.multi_state_mapper.exp_factors[finger] = exp_factor
+ if hasattr(self.lefthand, 'multi_state_mapper') and hasattr(self.lefthand.multi_state_mapper, 'exp_factors'):
+ for finger in self.lefthand.multi_state_mapper.exp_factors:
+ self.lefthand.multi_state_mapper.exp_factors[finger] = exp_factor
+ self.node.get_logger().info(f"延伸指数因子(全部): {exp_factor}")
+
+ # 处理缩放因子
+ if 'mapper_scale_factor' in param:
+ scale_factor = param['mapper_scale_factor']
+ if isinstance(scale_factor, dict):
+ # 指定手指设置
+ for finger, value in scale_factor.items():
+ if hasattr(self.righthand, 'multi_state_mapper') and hasattr(self.righthand.multi_state_mapper, 'scale_factors'):
+ if finger in self.righthand.multi_state_mapper.scale_factors:
+ self.righthand.multi_state_mapper.scale_factors[finger] = value
+ if hasattr(self.lefthand, 'multi_state_mapper') and hasattr(self.lefthand.multi_state_mapper, 'scale_factors'):
+ if finger in self.lefthand.multi_state_mapper.scale_factors:
+ self.lefthand.multi_state_mapper.scale_factors[finger] = value
+ fingers_str = ', '.join([f"{k}:{v}" for k, v in scale_factor.items()])
+ self.node.get_logger().info(f"缩放因子(指定): {fingers_str}")
+ else:
+ # 全部手指设置
+ if hasattr(self.righthand, 'multi_state_mapper') and hasattr(self.righthand.multi_state_mapper, 'scale_factors'):
+ for finger in self.righthand.multi_state_mapper.scale_factors:
+ self.righthand.multi_state_mapper.scale_factors[finger] = scale_factor
+ if hasattr(self.lefthand, 'multi_state_mapper') and hasattr(self.lefthand.multi_state_mapper, 'scale_factors'):
+ for finger in self.lefthand.multi_state_mapper.scale_factors:
+ self.lefthand.multi_state_mapper.scale_factors[finger] = scale_factor
+ self.node.get_logger().info(f"缩放因子(全部): {scale_factor}")
+
+ # 模式切换（仅当 mode 有明确值时）
+ if mode == 'glove':
+ if hasattr(self.righthand, 'use_fixed_pose'):
+ self.righthand.use_fixed_pose = False
+ if hasattr(self.lefthand, 'use_fixed_pose'):
+ self.lefthand.use_fixed_pose = False
+ self.node.get_logger().info("模式切换: 手套数据")
+
+ elif mode == 'fixed_opose':
+ if hasattr(self.righthand, 'use_fixed_pose'):
+ self.righthand.use_fixed_pose = True
+ self.righthand.fixed_pose = self.righthand.robot_opose
+ if hasattr(self.lefthand, 'use_fixed_pose'):
+ self.lefthand.use_fixed_pose = True
+ self.lefthand.fixed_pose = self.lefthand.robot_opose
+ self.node.get_logger().info("模式切换: 固定O型姿态")
+
+ elif mode == 'fixed_fist':
+ if hasattr(self.righthand, 'use_fixed_pose'):
+ self.righthand.use_fixed_pose = True
+ self.righthand.fixed_pose = self.righthand.robot_fist
+ if hasattr(self.lefthand, 'use_fixed_pose'):
+ self.lefthand.use_fixed_pose = True
+ self.lefthand.fixed_pose = self.lefthand.robot_fist
+ self.node.get_logger().info("模式切换: 固定握拳姿态")
+
+ elif mode is not None:
+ self.node.get_logger().warn(f"未知模式: {mode}")
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/tmp/.gitkeep b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/tmp/.gitkeep
new file mode 100644
index 0000000..e69de29
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/README.md b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/README.md
new file mode 100644
index 0000000..52e63d9
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/README.md
@@ -0,0 +1,62 @@
+# LinkerMCG Data Glove Module
+
+LinkerMCG data glove ROS2 driver module, receives glove data via UDP and publishes to ROS2 topics.
+
+## Features
+
+- Receives glove data via UDP protocol
+- Supports left and right hand data
+- 50Hz publish rate
+
+## Usage
+
+### 1. Config File
+
+Edit `config/base_config.yml`:
+
+```yaml
+system:
+ motion_type: linkermcg
+
+udp:
+ ip: "0.0.0.0"
+ port: 8888
+```
+
+### 2. Launch
+
+```bash
+ros2 run linkerhand_retarget handretarget
+```
+
+## ROS2 Topics
+
+### Published Topics
+
+| Topic | Message Type | Description |
+|-------|-------------|-------------|
+| `/cb_right_hand_control_cmd` | `sensor_msgs/JointState` | Right hand control data |
+| `/cb_left_hand_control_cmd` | `sensor_msgs/JointState` | Left hand control data |
+
+### Message Format
+
+```python
+# sensor_msgs/JointState
+msg.header.stamp # Timestamp
+msg.name # Joint name list
+msg.position # Joint position values
+```
+
+## File Structure
+
+```
+motion/linkermcg/
+├── __init__.py
+├── retarget.py # ROS2 integration
+└── README.md # English documentation
+```
+
+## Notes
+
+1. Ensure UDP port is not occupied
+2. Glove and host must be on the same network
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/README_zh.md b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/README_zh.md
new file mode 100644
index 0000000..cf4c2af
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/README_zh.md
@@ -0,0 +1,62 @@
+# LinkerMCG 数据手套模块
+
+LinkerMCG 数据手套的 ROS2 驱动模块，通过 UDP 接收手套数据并发布到 ROS2 话题。
+
+## 特性
+
+- 通过 UDP 协议接收手套数据
+- 支持左右手数据
+- 发布频率 50Hz
+
+## 使用方法
+
+### 1. 配置文件
+
+修改 `config/base_config.yml`:
+
+```yaml
+system:
+ motion_type: linkermcg
+
+udp:
+ ip: "0.0.0.0"
+ port: 8888
+```
+
+### 2. 运行
+
+```bash
+ros2 run linkerhand_retarget handretarget
+```
+
+## ROS2 话题
+
+### 发布话题
+
+| 话题名 | 消息类型 | 说明 |
+|-------|---------|------|
+| `/cb_right_hand_control_cmd` | `sensor_msgs/JointState` | 右手控制数据 |
+| `/cb_left_hand_control_cmd` | `sensor_msgs/JointState` | 左手控制数据 |
+
+### 消息格式
+
+```python
+# sensor_msgs/JointState
+msg.header.stamp # 时间戳
+msg.name # 关节名称列表
+msg.position # 关节位置值
+```
+
+## 文件结构
+
+```
+motion/linkermcg/
+├── __init__.py
+├── retarget.py # ROS2 集成
+└── README.md # 本文档
+```
+
+## 注意事项
+
+1. 确保 UDP 端口未被占用
+2. 手套与主机需在同一网络
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/__init__.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/__init__.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_l10v7.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_l10v7.py
new file mode 100644
index 0000000..0a349c1
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_l10v7.py
@@ -0,0 +1,143 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=10):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ self.g_jointpositions[0] = joint_arc[0] # 根部关节
+ self.g_jointpositions[1] = joint_arc[5] # 侧摆
+ self.g_jointpositions[2] = joint_arc[1] # 食指根部关节
+ self.g_jointpositions[3] = joint_arc[2] # 中指根部关节
+ self.g_jointpositions[4] = joint_arc[3] # 无名指根部关节
+ self.g_jointpositions[5] = joint_arc[4] # 小指根部关节
+ self.g_jointpositions[6] = joint_arc[6] # 食指侧摆
+ self.g_jointpositions[7] = joint_arc[8] # 无名指侧摆
+ self.g_jointpositions[8] = joint_arc[9] # 小指侧摆
+ self.g_jointpositions[9] = joint_arc[10] # 旋转
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ self.g_jointvelocity[i] = 255
+ # lastpos = self.last_jointpositions[i]
+ # position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ # position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ # slow_limit = 4
+ # fast_limit = 10
+ # max_vel = int(self.last_jointvelocity[i] * 2)
+ # mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ # min_vel = int(self.last_jointvelocity[i] * 0.5)
+ # target_vel = self.last_jointvelocity[i]
+ # if self.handstate[i] == 0: # stop
+ # if 0 < position_error:
+ # target_vel = position_error * 5 + 30
+ # self.handstate[i] = 1
+ # elif self.handstate[i] == 1: # slow
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 50
+ # if target_vel > mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 2
+ # elif position_error == 0:
+ # self.handstate[i] = 0
+ # target_vel = position_error * 5 + 100
+ # else:
+ # target_vel = position_error * 5 + 100
+ # else: # fast
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 90
+ # if target_vel > max_vel:
+ # target_vel = max_vel
+ # elif slow_limit < position_error < fast_limit:
+ # target_vel = position_error * 5 + 60
+ # if target_vel < mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 3
+ # elif 0 < position_error <= slow_limit:
+ # target_vel = position_error * 5 + 40
+ # if target_vel < min_vel:
+ # target_vel = min_vel
+ # self.handstate[i] = 1
+ # self.g_jointvelocity[i] = int(target_vel * 1)
+ # if self.g_jointvelocity[i] > 255:
+ # self.g_jointvelocity[i] = 255
+
+ # self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ # self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=10):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ self.g_jointpositions[0] = joint_arc[0] # 根部关节
+ self.g_jointpositions[1] = joint_arc[5] # 侧摆
+ self.g_jointpositions[2] = joint_arc[1] # 食指根部关节
+ self.g_jointpositions[3] = joint_arc[2] # 中指根部关节
+ self.g_jointpositions[4] = joint_arc[3] # 无名指根部关节
+ self.g_jointpositions[5] = joint_arc[4] # 小指根部关节
+ self.g_jointpositions[6] = joint_arc[6] # 食指侧摆
+ self.g_jointpositions[7] = joint_arc[8] # 无名指侧摆
+ self.g_jointpositions[8] = joint_arc[9] # 小指侧摆
+ self.g_jointpositions[9] = joint_arc[10] # 旋转
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ self.g_jointvelocity[i] = 255
+ # lastpos = self.last_jointpositions[i]
+ # position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ # position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ # slow_limit = 4
+ # fast_limit = 10
+ # max_vel = int(self.last_jointvelocity[i] * 2)
+ # mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ # min_vel = int(self.last_jointvelocity[i] * 0.5)
+ # target_vel = self.last_jointvelocity[i]
+ # if self.handstate[i] == 0: # stop
+ # if 0 < position_error:
+ # target_vel = position_error * 5 + 30
+ # self.handstate[i] = 1
+ # elif self.handstate[i] == 1: # slow
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 50
+ # if target_vel > mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 2
+ # elif position_error == 0:
+ # self.handstate[i] = 0
+ # target_vel = position_error * 5 + 100
+ # else:
+ # target_vel = position_error * 5 + 100
+ # else: # fast
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 90
+ # if target_vel > max_vel:
+ # target_vel = max_vel
+ # elif slow_limit < position_error < fast_limit:
+ # target_vel = position_error * 5 + 60
+ # if target_vel < mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 3
+ # elif 0 < position_error <= slow_limit:
+ # target_vel = position_error * 5 + 40
+ # if target_vel < min_vel:
+ # target_vel = min_vel
+ # self.handstate[i] = 1
+ # self.g_jointvelocity[i] = int(target_vel * 1)
+ # if self.g_jointvelocity[i] > 255:
+ # self.g_jointvelocity[i] = 255
+ # self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ # self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_l20.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_l20.py
new file mode 100644
index 0000000..53785fa
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_l20.py
@@ -0,0 +1,123 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=20):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ self.g_jointpositions = joint_arc[:20] # 完整复制
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_rimit = 2
+ fast_rimit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.8)
+ min_vel = int(self.last_jointvelocity[i] * 0.6)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 10 + 5
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_rimit:
+ target_vel = position_error * 10 + 30
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 10 + 10
+ else: # fast
+ if position_error >= fast_rimit:
+ target_vel = position_error * 10 + 50
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_rimit < position_error < fast_rimit:
+ target_vel = position_error * 10 + 30
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_rimit:
+ target_vel = position_error * 10 + 10
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity [i] = int(target_vel)
+
+ if self.g_jointvelocity [i] > 255:
+ self.g_jointvelocity [i] = 255
+ self.g_jointvelocity [i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity [i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=20):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ self.g_jointpositions = joint_arc[:20] # 完整复制
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 2
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.8)
+ min_vel = int(self.last_jointvelocity[i] * 0.6)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 10 + 5
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 10 + 30
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 10 + 10
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 10 + 50
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 10 + 30
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 10 + 10
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity [i] = int(target_vel)
+
+ if self.g_jointvelocity [i] > 255:
+ self.g_jointvelocity [i] = 255
+ self.g_jointvelocity [i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity [i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_l21.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_l21.py
new file mode 100644
index 0000000..b2244e6
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_l21.py
@@ -0,0 +1,123 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=25):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ self.g_jointpositions = joint_arc # 完整复制
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 1
+ fast_limit = 5
+ max_vel = int(self.last_jointvelocity[i] * 1.5)
+ min_vel = int(self.last_jointvelocity[i] * 0.995)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 3 + 50
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 3 + 100
+ if target_vel > max_vel:
+ target_vel = max_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 3 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 200
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 3 + 200
+ if target_vel < min_vel:
+ target_vel = min_vel
+ else:
+ target_vel = min_vel
+ if min_vel < 150:
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel)
+ # if self.g_jointvelocity[i] > 255:
+ # self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+ self.g_jointvelocity[6] = 255
+ self.g_jointvelocity[7] = 255
+ self.g_jointvelocity[8] = 255
+ self.g_jointvelocity[9] = 255
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=25):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ self.g_jointpositions = joint_arc # 完整复制
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 1
+ fast_limit = 5
+ max_vel = int(self.last_jointvelocity[i] * 1.5)
+ min_vel = int(self.last_jointvelocity[i] * 0.995)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 3 + 50
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 3 + 100
+ if target_vel > max_vel:
+ target_vel = max_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 3 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 200
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 3 + 200
+ if target_vel < min_vel:
+ target_vel = min_vel
+ else:
+ target_vel = min_vel
+ if min_vel < 150:
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+ self.g_jointvelocity[6] = 255
+ self.g_jointvelocity[7] = 255
+ self.g_jointvelocity[8] = 255
+ self.g_jointvelocity[9] = 255
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_l25.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_l25.py
new file mode 100644
index 0000000..53785fa
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_l25.py
@@ -0,0 +1,123 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=20):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ self.g_jointpositions = joint_arc[:20] # 完整复制
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_rimit = 2
+ fast_rimit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.8)
+ min_vel = int(self.last_jointvelocity[i] * 0.6)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 10 + 5
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_rimit:
+ target_vel = position_error * 10 + 30
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 10 + 10
+ else: # fast
+ if position_error >= fast_rimit:
+ target_vel = position_error * 10 + 50
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_rimit < position_error < fast_rimit:
+ target_vel = position_error * 10 + 30
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_rimit:
+ target_vel = position_error * 10 + 10
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity [i] = int(target_vel)
+
+ if self.g_jointvelocity [i] > 255:
+ self.g_jointvelocity [i] = 255
+ self.g_jointvelocity [i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity [i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=20):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ self.g_jointpositions = joint_arc[:20] # 完整复制
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 2
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.8)
+ min_vel = int(self.last_jointvelocity[i] * 0.6)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 10 + 5
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 10 + 30
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 10 + 10
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 10 + 50
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 10 + 30
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 10 + 10
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity [i] = int(target_vel)
+
+ if self.g_jointvelocity [i] > 255:
+ self.g_jointvelocity [i] = 255
+ self.g_jointvelocity [i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity [i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_l6.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_l6.py
new file mode 100644
index 0000000..02105bf
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_l6.py
@@ -0,0 +1,161 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ self.g_jointpositions[0] = joint_arc[0] # 根部关节
+ self.g_jointpositions[1] = joint_arc[5] # 侧摆
+ self.g_jointpositions[2] = joint_arc[1] # 食指根部关节
+ self.g_jointpositions[3] = joint_arc[2] # 中指根部关节
+ self.g_jointpositions[4] = joint_arc[3] # 无名指根部关节
+ self.g_jointpositions[5] = joint_arc[4] # 小指根部关节
+
+
+ # qpos[17] = joint_arc[10] # 旋转
+
+ # qpos[19] = joint_arc[15] # 中部关节
+ # qpos[20] = joint_arc[20] # 远端关节
+
+ # qpos[0] = joint_arc[6] # 食指侧摆
+ # qpos[1] = joint_arc[1] # 食指根部关节
+ # qpos[2] = joint_arc[16] # 食指中部关节
+ # qpos[3] = joint_arc[21] # 食指远端关节
+
+ # qpos[4] = joint_arc[9] # 小指侧摆
+ # qpos[5] = joint_arc[4] # 小指根部关节
+ # qpos[6] = joint_arc[19] # 小指中部关节
+ # qpos[7] = joint_arc[24] # 小指远端关节
+
+ # qpos[8] = joint_arc[7] # 中指侧摆
+ # qpos[9] = joint_arc[2] # 中指根部关节
+ # qpos[10] = joint_arc[17] # 中指中部关节
+ # qpos[11] = joint_arc[22] # 中指远端关节
+
+ # qpos[12] = joint_arc[8] # 无名指侧摆
+ # qpos[13] = joint_arc[3] # 无名指根部关节
+ # qpos[14] = joint_arc[18] # 无名指中部关节
+ # qpos[15] = joint_arc[23] # 无名指远端关节
+ # self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ self.g_jointpositions[0] = joint_arc[0] # 根部关节
+ self.g_jointpositions[1] = joint_arc[5] # 侧摆
+ self.g_jointpositions[2] = joint_arc[1] # 食指根部关节
+ self.g_jointpositions[3] = joint_arc[2] # 中指根部关节
+ self.g_jointpositions[4] = joint_arc[3] # 无名指根部关节
+ self.g_jointpositions[5] = joint_arc[4] # 小指根部关节
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_l7.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_l7.py
new file mode 100644
index 0000000..9261653
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_l7.py
@@ -0,0 +1,136 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=7):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ self.g_jointpositions[0] = joint_arc[0] # 根部关节
+ self.g_jointpositions[1] = joint_arc[5] # 侧摆
+ self.g_jointpositions[2] = joint_arc[1] # 食指根部关节
+ self.g_jointpositions[3] = joint_arc[2] # 中指根部关节
+ self.g_jointpositions[4] = joint_arc[3] # 无名指根部关节
+ self.g_jointpositions[5] = joint_arc[4] # 小指根部关节
+ self.g_jointpositions[6] = joint_arc[10] # 旋转
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=7):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ self.g_jointpositions[0] = joint_arc[0] # 根部关节
+ self.g_jointpositions[1] = joint_arc[5] # 侧摆
+ self.g_jointpositions[2] = joint_arc[1] # 食指根部关节
+ self.g_jointpositions[3] = joint_arc[2] # 中指根部关节
+ self.g_jointpositions[4] = joint_arc[3] # 无名指根部关节
+ self.g_jointpositions[5] = joint_arc[4] # 小指根部关节
+ self.g_jointpositions[6] = joint_arc[10] # 旋转
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_o6.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_o6.py
new file mode 100644
index 0000000..5ea5083
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/hand/linkermcg_o6.py
@@ -0,0 +1,134 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ self.g_jointpositions[0] = joint_arc[0] # 根部关节
+ self.g_jointpositions[1] = joint_arc[5] # 侧摆
+ self.g_jointpositions[2] = joint_arc[1] # 食指根部关节
+ self.g_jointpositions[3] = joint_arc[2] # 中指根部关节
+ self.g_jointpositions[4] = joint_arc[3] # 无名指根部关节
+ self.g_jointpositions[5] = joint_arc[4] # 小指根部关节
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ self.g_jointpositions[0] = joint_arc[0] # 根部关节
+ self.g_jointpositions[1] = joint_arc[5] # 侧摆
+ self.g_jointpositions[2] = joint_arc[1] # 食指根部关节
+ self.g_jointpositions[3] = joint_arc[2] # 中指根部关节
+ self.g_jointpositions[4] = joint_arc[3] # 无名指根部关节
+ self.g_jointpositions[5] = joint_arc[4] # 小指根部关节
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/retarget.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/retarget.py
new file mode 100644
index 0000000..bbfb120
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkermcg/retarget.py
@@ -0,0 +1,174 @@
+import time
+import rclpy
+import sys
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from pathlib import Path
+
+_project_root = Path(__file__).absolute().parent.parent.parent
+_project_root_str = str(_project_root)
+
+if _project_root_str in sys.path:
+ sys.path.remove(_project_root_str)
+sys.path.insert(0, _project_root_str)
+
+
+from linkerhand.linkermcgcore import HaoCunScoketUdp
+from linkerhand.handcore import HandCore
+from linkerhand.constants import RobotName, ROBOT_LEN_MAP
+
+
+
+class Retarget():
+ def __init__(self,node, ip, port, lefthand: RobotName, righthand: RobotName, handcore: HandCore,
+ lefthandpubprint: bool, righthandpubprint: bool):
+ self.node = node
+ self.udp_ip = ip
+ self.udp_port = port
+ self.lefthandtype = lefthand
+ self.righthandtype = righthand
+ self.handcore = handcore
+ self.running = True
+ self.lefthandpubprint = lefthandpubprint
+ self.righthandpubprint = righthandpubprint
+
+ # 根据右手类型初始化
+ if self.righthandtype == RobotName.o7 \
+ or self.righthandtype == RobotName.l7 \
+ or self.righthandtype == RobotName.o7v1 \
+ or self.righthandtype == RobotName.o7v3:
+ from .hand.linkermcg_l7 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.o6:
+ from .hand.linkermcg_o6 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l6:
+ from .hand.linkermcg_l6 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l25 \
+ or self.righthandtype == RobotName.g20:
+ from .hand.linkermcg_l25 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ # elif self.righthandtype == RobotName.t25:
+ # from .hand.linkermcg_t25 import RightHand
+ # self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l20:
+ from .hand.linkermcg_l20 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l10 \
+ or self.righthandtype == RobotName.l10v7 :
+ from .hand.linkermcg_l10v7 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l21:
+ from .hand.linkermcg_l21 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+
+ # 根据LEFT手类型初始化
+ if self.lefthandtype == RobotName.o7 \
+ or self.lefthandtype == RobotName.l7 \
+ or self.lefthandtype == RobotName.o7v1 \
+ or self.lefthandtype == RobotName.o7v3:
+ from .hand.linkermcg_l7 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.o6:
+ from .hand.linkermcg_o6 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l6:
+ from .hand.linkermcg_l6 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l25 \
+ or self.lefthandtype == RobotName.g20:
+ from .hand.linkermcg_l25 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ # elif self.lefthandtype == RobotName.t25:
+ # from .hand.linkermcg_t25 import LeftHand
+ # self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l20:
+ from .hand.linkermcg_l20 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l10 \
+ or self.lefthandtype == RobotName.l10v7 :
+ from .hand.linkermcg_l10v7 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l21:
+ from .hand.linkermcg_l21 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+
+ self.publisher_r = self.node.create_publisher(
+ JointState,
+ '/cb_right_hand_control_cmd',
+ self.handcore.hand_numjoints_r)
+
+ self.publisher_l = self.node.create_publisher(
+ JointState,
+ '/cb_left_hand_control_cmd',
+ self.handcore.hand_numjoints_l)
+
+ # 创建ROS定时器，以固定频率处理数据
+ # 参数1: period 周期(秒)
+ # 参数2: callback 回调函数
+ # 参数3: oneshot 是否只执行一次
+ self.timer = self.node.create_timer(1.0/120, self.process_callback) # 120Hz
+ self.pubprintcount = 0
+ self.pubprintcount = 0
+
+ self.pubprintcount = 0
+ self.udp_datacapture = None
+
+ def initialize_udp(self):
+ """初始化UDP连接"""
+ self.udp_datacapture = HaoCunScoketUdp(
+ host=self.udp_ip,
+ port=self.udp_port)
+ if self.udp_datacapture.udp_initial():
+ self.node.get_logger().info("UDP连接初始化成功")
+ self.running = True
+ else:
+ self.node.get_logger().error("UDP连接初始化失败")
+
+ def process_callback(self):
+ if not self.running:
+ return
+
+ mocapdata = self.udp_datacapture.realmocapdata
+ if not mocapdata.is_update:
+ return
+
+ # 处理左右手原始数据
+ self.lefthand.joint_update(mocapdata.jointangle_lHand)
+ self.righthand.joint_update(mocapdata.jointangle_rHand)
+
+ # 速度环节处理
+ self.lefthand.speed_update()
+ self.righthand.speed_update()
+
+ # 调试打印
+ if self.lefthandpubprint and self.pubprintcount % 1 == 0:
+ self.node.get_logger().info(f"左手位置: {self.lefthand.g_jointpositions}")
+ if self.righthandpubprint and self.pubprintcount % 1 == 0:
+ self.node.get_logger().info(f"右手位置: {self.righthand.g_jointpositions}")
+
+ # 发布右手数据
+ msg_r = JointState()
+ msg_r.header.stamp = self.node.get_clock().now().to_msg()
+ msg_r.name = [f'joint{i + 1}' for i in range(len(self.righthand.g_jointpositions))]
+ msg_r.position = [float(num) for num in self.righthand.g_jointpositions]
+ msg_r.velocity = [float(num) for num in self.righthand.g_jointvelocity]
+ self.publisher_r.publish(msg_r)
+
+ # 发布左手数据
+ msg_l = JointState()
+ msg_l.header.stamp = self.node.get_clock().now().to_msg()
+ msg_l.name = [f'joint{i + 1}' for i in range(len(self.lefthand.g_jointpositions))]
+ msg_l.position = [float(num) for num in self.lefthand.g_jointpositions]
+ msg_l.velocity = [float(num) for num in self.lefthand.g_jointvelocity]
+ self.publisher_l.publish(msg_l)
+
+ def process(self):
+ """主处理函数"""
+ self.initialize_udp()
+ try:
+ while rclpy.ok():
+ rclpy.spin_once(self.node, timeout_sec=0.1)
+ except rclpy.ROSInterruptException:
+ pass
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/__init__.py b/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/__init__.py b/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_l10v7.py b/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_l10v7.py
new file mode 100644
index 0000000..fcb7de0
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_l10v7.py
@@ -0,0 +1,180 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=10):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1.3 # 旋转
+ qpos[17] = joint_arc[20] * 2.6 # 侧摆
+ # qpos[18] = joint_arc[2] * 0 # 根部关节
+ # qpos[19] = joint_arc[1] * 0 # 中部关节
+ qpos[20] = joint_arc[0] * -0.6 # 远端关节
+ # print(qpos[16],qpos[17])
+
+
+ qpos[0] = joint_arc[7]
+ qpos[1] = joint_arc[6] * -0.7
+ # qpos[2] = joint_arc[5] * -1
+ # qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19]
+ qpos[5] = joint_arc[18] * -0.7
+ # qpos[6] = joint_arc[17] * -1
+ # qpos[7] = joint_arc[16] * -1
+
+ # qpos[8] = joint_arc[11]
+ qpos[9] = joint_arc[10] * -0.7
+ # qpos[10] = joint_arc[9] * -1
+ # qpos[11] = joint_arc[8] * -1
+
+ qpos[12] = joint_arc[15]
+ qpos[13] = joint_arc[14] * -0.7
+ # qpos[14] = joint_arc[13] * -1
+ # qpos[15] = joint_arc[12] * -1
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ self.g_jointvelocity[i] = 255
+ # lastpos = self.last_jointpositions[i]
+ # position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ # position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ # slow_limit = 4
+ # fast_limit = 10
+ # max_vel = int(self.last_jointvelocity[i] * 2)
+ # mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ # min_vel = int(self.last_jointvelocity[i] * 0.5)
+ # target_vel = self.last_jointvelocity[i]
+ # if self.handstate[i] == 0: # stop
+ # if 0 < position_error:
+ # target_vel = position_error * 5 + 30
+ # self.handstate[i] = 1
+ # elif self.handstate[i] == 1: # slow
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 50
+ # if target_vel > mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 2
+ # elif position_error == 0:
+ # self.handstate[i] = 0
+ # target_vel = position_error * 5 + 100
+ # else:
+ # target_vel = position_error * 5 + 100
+ # else: # fast
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 90
+ # if target_vel > max_vel:
+ # target_vel = max_vel
+ # elif slow_limit < position_error < fast_limit:
+ # target_vel = position_error * 5 + 60
+ # if target_vel < mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 3
+ # elif 0 < position_error <= slow_limit:
+ # target_vel = position_error * 5 + 40
+ # if target_vel < min_vel:
+ # target_vel = min_vel
+ # self.handstate[i] = 1
+ # self.g_jointvelocity[i] = int(target_vel * 1)
+ # if self.g_jointvelocity[i] > 255:
+ # self.g_jointvelocity[i] = 255
+
+ # self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ # self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=10):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1 # 旋转
+ qpos[17] = joint_arc[2] * -1 # 侧摆
+ qpos[18] = joint_arc[2] * 0 # 根部关节
+ qpos[19] = joint_arc[1] * 0 # 中部关节
+ qpos[20] = joint_arc[1] * -1 # 远端关节
+ # print(qpos[16],qpos[17])
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ qpos[8] = joint_arc[11] * -1
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -1
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ self.g_jointvelocity[i] = 255
+ # lastpos = self.last_jointpositions[i]
+ # position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ # position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ # slow_limit = 4
+ # fast_limit = 10
+ # max_vel = int(self.last_jointvelocity[i] * 2)
+ # mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ # min_vel = int(self.last_jointvelocity[i] * 0.5)
+ # target_vel = self.last_jointvelocity[i]
+ # if self.handstate[i] == 0: # stop
+ # if 0 < position_error:
+ # target_vel = position_error * 5 + 30
+ # self.handstate[i] = 1
+ # elif self.handstate[i] == 1: # slow
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 50
+ # if target_vel > mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 2
+ # elif position_error == 0:
+ # self.handstate[i] = 0
+ # target_vel = position_error * 5 + 100
+ # else:
+ # target_vel = position_error * 5 + 100
+ # else: # fast
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 90
+ # if target_vel > max_vel:
+ # target_vel = max_vel
+ # elif slow_limit < position_error < fast_limit:
+ # target_vel = position_error * 5 + 60
+ # if target_vel < mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 3
+ # elif 0 < position_error <= slow_limit:
+ # target_vel = position_error * 5 + 40
+ # if target_vel < min_vel:
+ # target_vel = min_vel
+ # self.handstate[i] = 1
+ # self.g_jointvelocity[i] = int(target_vel * 1)
+ # if self.g_jointvelocity[i] > 255:
+ # self.g_jointvelocity[i] = 255
+ # self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ # self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_l20.py b/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_l20.py
new file mode 100644
index 0000000..2e8790c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_l20.py
@@ -0,0 +1,176 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=20):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1
+ qpos[17] = joint_arc[20] * 1
+ qpos[18] = joint_arc[1] * -1
+ qpos[19] = joint_arc[0] * -1
+
+ qpos[0] = joint_arc[7] * 1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19] * 1
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ qpos[8] = joint_arc[11] * -1
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ qpos[12] = joint_arc[15] * 1
+ qpos[13] = joint_arc[14] * -1
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_rimit = 2
+ fast_rimit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.8)
+ min_vel = int(self.last_jointvelocity[i] * 0.6)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 10 + 5
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_rimit:
+ target_vel = position_error * 10 + 30
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 10 + 10
+ else: # fast
+ if position_error >= fast_rimit:
+ target_vel = position_error * 10 + 50
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_rimit < position_error < fast_rimit:
+ target_vel = position_error * 10 + 30
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_rimit:
+ target_vel = position_error * 10 + 10
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity [i] = int(target_vel)
+
+ if self.g_jointvelocity [i] > 255:
+ self.g_jointvelocity [i] = 255
+ self.g_jointvelocity [i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity [i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=20):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = (joint_arc[2] * -1 - 0.3)
+ qpos[17] = joint_arc[20] * 1
+ qpos[18] = joint_arc[1] * -1
+ qpos[19] = joint_arc[0] * 0
+
+ qpos[0] = joint_arc[7] * 1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19] * 1
+
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ qpos[8] = joint_arc[11] * 1
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+
+ qpos[12] = joint_arc[15] * 1
+ qpos[13] = joint_arc[14] * -1
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 2
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.8)
+ min_vel = int(self.last_jointvelocity[i] * 0.6)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 10 + 5
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 10 + 30
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 10 + 10
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 10 + 50
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 10 + 30
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 10 + 10
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity [i] = int(target_vel)
+
+ if self.g_jointvelocity [i] > 255:
+ self.g_jointvelocity [i] = 255
+ self.g_jointvelocity [i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity [i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_l21.py b/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_l21.py
new file mode 100644
index 0000000..d9ee68e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_l21.py
@@ -0,0 +1,192 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=25):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1 # 侧摆
+ qpos[17] = joint_arc[20] * 2 # 旋转
+ qpos[18] = joint_arc[2] * -0.5 # 根部关节
+ qpos[19] = joint_arc[1] * -1 # 中部关节
+ qpos[20] = joint_arc[0] * -1 # 远端关节
+
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -0.9
+ qpos[2] = joint_arc[5] * -0.9
+ qpos[3] = joint_arc[4] * -1
+ # if joint_arc[6] > -80 * 3.14 / 180: qpos[2] = 0
+ # if joint_arc[6] > -80 * 3.14 / 180: qpos[3] = 0
+
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -0.9
+ qpos[6] = joint_arc[17] * -0.9
+ qpos[7] = joint_arc[16] * -1
+ # if joint_arc[18] > -75 * 3.14 / 180: qpos[6] = 0
+ # if joint_arc[18] > -75 * 3.14 / 180: qpos[7] = 0
+
+ qpos[8] = joint_arc[11] * -1
+ qpos[9] = joint_arc[10] * -0.9
+ qpos[10] = joint_arc[9] * -0.9
+ qpos[11] = joint_arc[8] * -1
+ # if joint_arc[10] > -80 * 3.14 / 180: qpos[10] = 0
+ # if joint_arc[10] > -80 * 3.14 / 180: qpos[11] = 0
+
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -0.9
+ qpos[14] = joint_arc[13] * -0.9
+ qpos[15] = joint_arc[12] * -1
+ # if joint_arc[14] > -80 * 3.14 / 180: qpos[14] = 0
+ # if joint_arc[14] > -80 * 3.14 / 180: qpos[15] = 0
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 1
+ fast_limit = 5
+ max_vel = int(self.last_jointvelocity[i] * 1.5)
+ min_vel = int(self.last_jointvelocity[i] * 0.995)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 3 + 50
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 3 + 100
+ if target_vel > max_vel:
+ target_vel = max_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 3 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 200
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 3 + 200
+ if target_vel < min_vel:
+ target_vel = min_vel
+ else:
+ target_vel = min_vel
+ if min_vel < 150:
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel)
+ # if self.g_jointvelocity[i] > 255:
+ # self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+ self.g_jointvelocity[6] = 255
+ self.g_jointvelocity[7] = 255
+ self.g_jointvelocity[8] = 255
+ self.g_jointvelocity[9] = 255
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=25):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1.5 # 侧摆
+ qpos[17] = joint_arc[20] * 3 # 旋转
+ qpos[18] = joint_arc[2] * -0.5 # 根部关节
+ qpos[19] = joint_arc[1] * -1 # 中部关节
+ qpos[20] = joint_arc[0] * -1 # 远端关节
+
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -0.9
+ qpos[2] = joint_arc[5] * -0.9
+ qpos[3] = joint_arc[4] * -1
+ # if joint_arc[6] > -80 * 3.14 / 180: qpos[2] = 0
+ # if joint_arc[6] > -80 * 3.14 / 180: qpos[3] = 0
+
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -0.9
+ qpos[6] = joint_arc[17] * -0.9
+ qpos[7] = joint_arc[16] * -1
+ # if joint_arc[18] > -75 * 3.14 / 180: qpos[6] = 0
+ # if joint_arc[18] > -75 * 3.14 / 180: qpos[7] = 0
+
+ qpos[8] = joint_arc[11] * -1
+ qpos[9] = joint_arc[10] * -0.9
+ qpos[10] = joint_arc[9] * -0.9
+ qpos[11] = joint_arc[8] * -1
+ # if joint_arc[10] > -80 * 3.14 / 180: qpos[10] = 0
+ # if joint_arc[10] > -80 * 3.14 / 180: qpos[11] = 0
+
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -0.9
+ qpos[14] = joint_arc[13] * -0.9
+ qpos[15] = joint_arc[12] * -1
+ # if joint_arc[14] > -80 * 3.14 / 180: qpos[14] = 0
+ # if joint_arc[14] > -80 * 3.14 / 180: qpos[15] = 0
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 1
+ fast_limit = 5
+ max_vel = int(self.last_jointvelocity[i] * 1.5)
+ min_vel = int(self.last_jointvelocity[i] * 0.995)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 3 + 50
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 3 + 100
+ if target_vel > max_vel:
+ target_vel = max_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 3 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 200
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 3 + 200
+ if target_vel < min_vel:
+ target_vel = min_vel
+ else:
+ target_vel = min_vel
+ if min_vel < 150:
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+ self.g_jointvelocity[6] = 255
+ self.g_jointvelocity[7] = 255
+ self.g_jointvelocity[8] = 255
+ self.g_jointvelocity[9] = 255
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_l25.py b/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_l25.py
new file mode 100644
index 0000000..bc32533
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_l25.py
@@ -0,0 +1,202 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=25):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1 # 侧摆
+ qpos[17] = joint_arc[20] * 2.6 # 旋转
+ qpos[18] = joint_arc[2] * -0.5 # 根部关节
+ qpos[19] = joint_arc[1] * -1 # 中部关节
+ qpos[20] = joint_arc[0] * -1.5 # 远端关节
+ if joint_arc[0] > -40 * 3.14 / 180:
+ qpos[18] = 0
+ qpos[19] = 0
+ qpos[20] = 0
+
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+ if joint_arc[6] > -80 * 3.14 / 180: qpos[2] = 0
+ if joint_arc[6] > -80 * 3.14 / 180: qpos[3] = 0
+
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+ if joint_arc[18] > -70 * 3.14 / 180: qpos[6] = 0
+ if joint_arc[18] > -70 * 3.14 / 180: qpos[7] = 0
+
+ qpos[8] = joint_arc[11] * -1
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+ if joint_arc[10] > -80 * 3.14 / 180: qpos[10] = 0
+ if joint_arc[10] > -80 * 3.14 / 180: qpos[11] = 0
+
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -1
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+ if joint_arc[14] > -80 * 3.14 / 180: qpos[14] = 0
+ if joint_arc[14] > -80 * 3.14 / 180: qpos[15] = 0
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 1
+ fast_limit = 5
+ max_vel = int(self.last_jointvelocity[i] * 1.5)
+ min_vel = int(self.last_jointvelocity[i] * 0.995)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 3 + 50
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 3 + 100
+ if target_vel > max_vel:
+ target_vel = max_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 3 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 200
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 3 + 200
+ if target_vel < min_vel:
+ target_vel = min_vel
+ else:
+ target_vel = min_vel
+ if min_vel < 150:
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+ self.g_jointvelocity[6] = 255
+ self.g_jointvelocity[7] = 255
+ self.g_jointvelocity[8] = 255
+ self.g_jointvelocity[9] = 255
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=25):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1 - 0.4 # 侧摆
+ qpos[17] = joint_arc[20] * 2 - 0.2 # 旋转
+ qpos[18] = joint_arc[2] * -0.2 # 根部关节
+ qpos[19] = joint_arc[1] * -1 # 中部关节
+ qpos[20] = joint_arc[0] * -1 # 远端关节
+ if joint_arc[0] > -50 * 3.14 / 180:
+ qpos[18] = 0.2
+ qpos[19] = 0
+ qpos[20] = 0
+
+ # 食指 index
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+ if joint_arc[6] > -80 * 3.14 / 180: qpos[2] = 0
+ if joint_arc[6] > -80 * 3.14 / 180: qpos[3] = 0
+
+ # 小指 little
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+ if joint_arc[18] > -75 * 3.14 / 180: qpos[6] = 0
+ if joint_arc[18] > -75 * 3.14 / 180: qpos[7] = 0
+
+ # 中指 middle
+ qpos[8] = joint_arc[11] * 0
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+ if joint_arc[10] > -80 * 3.14 / 180: qpos[10] = 0
+ if joint_arc[10] > -80 * 3.14 / 180: qpos[11] = 0
+
+ # 无名指 ring
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -1 # gen
+ qpos[14] = joint_arc[13] * -1 # zhong
+ qpos[15] = joint_arc[12] * -1
+ if joint_arc[14] > -80 * 3.14 / 180: qpos[14] = 0
+ if joint_arc[14] > -80 * 3.14 / 180: qpos[15] = 0
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 1
+ fast_limit = 5
+ max_vel = int(self.last_jointvelocity[i] * 1.5)
+ min_vel = int(self.last_jointvelocity[i] * 0.995)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 3 + 50
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 3 + 100
+ if target_vel > max_vel:
+ target_vel = max_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 3 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 200
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 3 + 200
+ if target_vel < min_vel:
+ target_vel = min_vel
+ else:
+ target_vel = min_vel
+ if min_vel < 150:
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+ self.g_jointvelocity[6] = 255
+ self.g_jointvelocity[7] = 255
+ self.g_jointvelocity[8] = 255
+ self.g_jointvelocity[9] = 255
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_l6.py b/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_l6.py
new file mode 100644
index 0000000..81715cd
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_l6.py
@@ -0,0 +1,181 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1.2 # 侧摆
+ qpos[17] = joint_arc[20] * 2.4 # 旋转
+ qpos[18] = joint_arc[2] * -0.3878 # 根部关节
+ qpos[19] = joint_arc[1] * -0.66845 # 中部关节
+ qpos[20] = joint_arc[0] * -0.8 # 远端关节
+
+ qpos[0] = joint_arc[7]
+ qpos[1] = joint_arc[6] * -0.7
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19]
+ qpos[5] = joint_arc[18] * -0.7
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ qpos[8] = joint_arc[11]
+ qpos[9] = joint_arc[10] * -1.0098
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ qpos[12] = joint_arc[15]
+ qpos[13] = joint_arc[14] * -0.7
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1 # 侧摆
+ qpos[17] = joint_arc[20] * 1.5 # 旋转
+ qpos[18] = joint_arc[2] * -0.9 # 根部关节
+ qpos[19] = joint_arc[1] * -0.8 # 中部关节
+ qpos[20] = joint_arc[0] * -0.8 # 远端关节
+
+ # 食指 index
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -0.8
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ # 小指 little
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -0.8
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ # 中指 middle
+ qpos[8] = joint_arc[11] * -1.0098
+ qpos[9] = joint_arc[10] * -0.8
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ # 无名指 ring
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -0.8 # gen
+ qpos[14] = joint_arc[13] * -1 # zhong
+ qpos[15] = joint_arc[12] * -1
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_l7.py b/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_l7.py
new file mode 100644
index 0000000..7707080
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_l7.py
@@ -0,0 +1,181 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=7):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1.2 # 侧摆
+ qpos[17] = joint_arc[20] * 2.4 # 旋转
+ qpos[18] = joint_arc[2] * -0.3878 # 根部关节
+ qpos[19] = joint_arc[1] * -0.66845 # 中部关节
+ qpos[20] = joint_arc[0] * -0.8 # 远端关节
+
+ qpos[0] = joint_arc[7]
+ qpos[1] = joint_arc[6] * -0.7
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19]
+ qpos[5] = joint_arc[18] * -0.7
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ qpos[8] = joint_arc[11]
+ qpos[9] = joint_arc[10] * -1.0098
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ qpos[12] = joint_arc[15]
+ qpos[13] = joint_arc[14] * -0.7
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=7):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[2] * -0.5 # 旋转
+ qpos[17] = joint_arc[20] * 0.8 # 侧摆
+ qpos[18] = joint_arc[2] * -0.9 # 根部关节
+ qpos[19] = joint_arc[1] * -0.8 # 中部关节
+ qpos[20] = joint_arc[0] * -0.5 # 远端关节
+
+ # 食指 index
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ # 小指 little
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ # 中指 middle
+ qpos[8] = joint_arc[11] * -1.0098
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ # 无名指 ring
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -1 # gen
+ qpos[14] = joint_arc[13] * -1 # zhong
+ qpos[15] = joint_arc[12] * -1
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_o6.py b/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_o6.py
new file mode 100644
index 0000000..a6cf885
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/hand/simulator_o6.py
@@ -0,0 +1,181 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1.2 # 侧摆
+ qpos[17] = joint_arc[20] * 2.4 # 旋转
+ qpos[18] = joint_arc[2] * -0.3878 # 根部关节
+ qpos[19] = joint_arc[1] * -0.66845 # 中部关节
+ qpos[20] = joint_arc[0] * -0.1 # 远端关节
+
+ qpos[0] = joint_arc[7]
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19]
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ qpos[8] = joint_arc[11]
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ qpos[12] = joint_arc[15]
+ qpos[13] = joint_arc[14] * -1
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1 # 侧摆
+ qpos[17] = joint_arc[20] * 2 # 旋转
+ qpos[18] = joint_arc[2] * -0.9 # 根部关节
+ qpos[19] = joint_arc[1] * -0.8 # 中部关节
+ qpos[20] = joint_arc[0] * -0.3 # 远端关节
+
+ # 食指 index
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ # 小指 little
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -11
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ # 中指 middle
+ qpos[8] = joint_arc[11] * -1
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ # 无名指 ring
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -1 # gen
+ qpos[14] = joint_arc[13] * -1 # zhong
+ qpos[15] = joint_arc[12] * -1
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/retarget.py b/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/retarget.py
new file mode 100644
index 0000000..0664155
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/simulator/retarget.py
@@ -0,0 +1,232 @@
+
+import time
+import sapien
+import tyro
+import rclpy
+import numpy as np
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from datetime import datetime
+from pathlib import Path
+from loguru import logger
+from sapien.asset import create_dome_envmap
+from sapien.utils import Viewer
+
+from ...linkerhand.handcore import HandCore
+from ...linkerhand.constants import (
+ RetargetingType,
+ DataSource,
+ ROBOT_LEN_MAP,
+ MotionSource,
+ RobotName,
+ HandType,
+ get_default_config_path,
+)
+from ...linkerhand.retargeting_config import RetargetingConfig
+
+LOG_FILE_PATH = "/tmp/b.log"
+
+class Retarget():
+ def __init__(self,node, ip, port, deviceid, lefthand: RobotName, righthand: RobotName, handcore: HandCore,
+ lefthandpubprint: bool, righthandpubprint: bool):
+ self.node = node
+ self.udp_ip = ip
+ self.udp_port = port
+ self.motion_device = deviceid
+ self.lefthandtype = lefthand
+ self.righthandtype = righthand
+ self.handcore = handcore
+ self.runing = True
+ self.lefthandpubprint = lefthandpubprint
+ self.righthandpubprint = righthandpubprint
+
+ # 根据右手类型初始化
+ if self.righthandtype == RobotName.o7 \
+ or self.righthandtype == RobotName.l7 \
+ or self.righthandtype == RobotName.o7v1 \
+ or self.righthandtype == RobotName.o7v2:
+ from .hand.simulator_l7 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.o6:
+ from .hand.simulator_o6 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l6:
+ from .hand.simulator_l6 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ # elif self.righthandtype == RobotName.l25:
+ # from .hand.simulator_l25 import RightHand
+ # self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ # elif self.righthandtype == RobotName.t25:
+ # from .hand.simulator_t25 import RightHand
+ # self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l20:
+ from .hand.simulator_l20 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l10 \
+ or self.righthandtype == RobotName.l10v7 :
+ from .hand.simulator_l10v7 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l21:
+ from .hand.simulator_l21 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+
+ # 根据LEFT手类型初始化
+ if self.lefthandtype == RobotName.o7 \
+ or self.lefthandtype == RobotName.l7 \
+ or self.lefthandtype == RobotName.o7v1 \
+ or self.lefthandtype == RobotName.o7v2:
+ from .hand.simulator_l7 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.o6:
+ from .hand.simulator_o6 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l6:
+ from .hand.simulator_l6 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l25:
+ from .hand.simulator_l25 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ # elif self.lefthandtype == RobotName.t25:
+ # from .hand.simulator_t25 import LeftHand
+ # self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l20:
+ from .hand.simulator_l20 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l10 \
+ or self.lefthandtype == RobotName.l10v7 :
+ from .hand.simulator_l10v7 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l21:
+ from .hand.simulator_l21 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+
+ # ROS2 发布器
+ self.publisher_r = self.node.create_publisher(
+ JointState,
+ '/cb_right_hand_control_cmd',
+ self.handcore.hand_numjoints_r)
+
+ self.publisher_l = self.node.create_publisher(
+ JointState,
+ '/cb_left_hand_control_cmd',
+ self.handcore.hand_numjoints_l)
+
+ self.timer = self.node.create_timer(1.0/120, self.process_callback) # 120Hz
+ self.pubprintcount = 0
+ self.udp_datacapture = None
+
+ def sapien_init(self):
+ sapien.render.set_viewer_shader_dir("default")
+ sapien.render.set_camera_shader_dir("default")
+
+ config = RetargetingConfig.load_from_file(self.config_path)
+
+ # Setup
+ scene = sapien.Scene()
+ render_mat = sapien.render.RenderMaterial()
+ render_mat.base_color = [0.06, 0.08, 0.12, 1]
+ render_mat.metallic = 0.0
+ render_mat.roughness = 0.9
+ render_mat.specular = 0.8
+ scene.add_ground(-0.2, render_material=render_mat, render_half_size=[1000, 1000])
+
+ # Lighting
+ scene.add_directional_light(np.array([1, 1, -1]), np.array([3, 3, 3]))
+ scene.add_point_light(np.array([2, 2, 2]), np.array([2, 2, 2]), shadow=False)
+ scene.add_point_light(np.array([2, -2, 2]), np.array([2, 2, 2]), shadow=False)
+ scene.set_environment_map(
+ create_dome_envmap(sky_color=[0.2, 0.2, 0.2], ground_color=[0.2, 0.2, 0.2])
+ )
+ scene.add_area_light_for_ray_tracing(
+ sapien.Pose([2, 1, 2], [0.707, 0, 0.707, 0]), np.array([1, 1, 1]), 5, 5
+ )
+
+ # Camera
+ cam = scene.add_camera(
+ name="Cheese!", width=600, height=600, fovy=1, near=0.1, far=10
+ )
+ cam.set_local_pose(sapien.Pose([0.50, 0, 0.0], [0, 0, 0, -1]))
+
+ self.viewer = Viewer()
+ self.viewer.set_scene(scene)
+ self.viewer.control_window.show_origin_frame = False
+ self.viewer.control_window.move_speed = 0.01
+ self.viewer.control_window.toggle_camera_lines(False)
+ self.viewer.set_camera_pose(cam.get_local_pose())
+
+ # Load robot and set it to a good pose to take picture
+ loader = scene.create_urdf_loader()
+ filepath = Path(config.urdf_path)
+ robot_name = filepath.stem
+ loader.load_multiple_collisions_from_file = True
+ loader.scale = 1.5
+
+ filepath = str(filepath)
+ self.robot = loader.load(filepath)
+ self.robot.set_pose(sapien.Pose([0, 0, -0.13]))
+
+ # Different robot loader may have different orders for joints
+ self.sapien_joint_names = [joint.get_name() for joint in self.robot.get_active_joints()]
+ retargeting_joint_names = self.retargeting.joint_names
+ retargeting_to_sapien = np.array(
+ [retargeting_joint_names.index(name) for name in self.sapien_joint_names]
+ ).astype(int)
+
+ self.qpos = [0] * len(self.sapien_joint_names)
+
+ def process_callback(self):
+ self.viewer.render()
+
+ """定时器回调函数，处理数据并发布"""
+ if not self.udp_datacapture or not self.udp_datacapture.udp_is_onnect():
+ self.node.get_logger().warning("侦测到UDP断开状态，正在重连！")
+ if not self.initialize_udp():
+ self.node.get_logger().error("UDP重连失败")
+ return
+ time.sleep(2)
+ return
+
+ mocapdata = self.udp_datacapture.realmocapdata
+ if not mocapdata.is_update:
+ return
+
+ # 处理左右手原始数据
+ self.lefthand.joint_update(mocapdata.jointangle_lHand)
+ self.righthand.joint_update(mocapdata.jointangle_rHand)
+
+ # 速度环节处理
+ self.lefthand.speed_update()
+ self.righthand.speed_update()
+
+ # self.quick_log(self.lefthand.g_jointpositions[1],LOG_FILE_PATH,"SEND")
+
+ # 调试打印
+ if self.lefthandpubprint and self.pubprintcount % 1 == 0:
+ self.node.get_logger().info(f"左手位置: {self.lefthand.g_jointpositions}")
+ if self.righthandpubprint and self.pubprintcount % 1 == 0:
+ self.node.get_logger().info(f"右手位置: {self.righthand.g_jointpositions}")
+
+ # 发布右手数据
+ msg_r = JointState()
+ msg_r.header.stamp = self.node.get_clock().now().to_msg()
+ msg_r.name = [f'joint{i + 1}' for i in range(len(self.righthand.g_jointpositions))]
+ msg_r.position = [float(num) for num in self.righthand.g_jointpositions]
+ msg_r.velocity = [float(num) for num in self.righthand.g_jointvelocity]
+ self.publisher_r.publish(msg_r)
+
+ # 发布左手数据
+ msg_l = JointState()
+ msg_l.header.stamp = self.node.get_clock().now().to_msg()
+ msg_l.name = [f'joint{i + 1}' for i in range(len(self.lefthand.g_jointpositions))]
+ msg_l.position = [float(num) for num in self.lefthand.g_jointpositions]
+ msg_l.velocity = [float(num) for num in self.lefthand.g_jointvelocity]
+ self.publisher_l.publish(msg_l)
+
+ self.pubprintcount += 1
+
+ def process(self):
+ """主处理函数"""
+ if not self.initialize_udp():
+ self.node.get_logger().error("初始化配置网络失败")
+ return
+
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/README.md b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/README.md
new file mode 100644
index 0000000..28259bb
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/README.md
@@ -0,0 +1,62 @@
+# UdexReal (LinkerTG) Data Glove Module
+
+UdexReal / LinkerTG data glove ROS2 driver module, receives glove data via UDP and publishes to ROS2 topics.
+
+## Features
+
+- Receives glove data via UDP protocol
+- Supports left and right hand data
+- 50Hz publish rate
+
+## Usage
+
+### 1. Config File
+
+Edit `config/base_config.yml`:
+
+```yaml
+system:
+ motion_type: udexreal
+
+udp:
+ ip: "0.0.0.0"
+ port: 8888
+```
+
+### 2. Launch
+
+```bash
+ros2 run linkerhand_retarget handretarget
+```
+
+## ROS2 Topics
+
+### Published Topics
+
+| Topic | Message Type | Description |
+|-------|-------------|-------------|
+| `/cb_right_hand_control_cmd` | `sensor_msgs/JointState` | Right hand control data |
+| `/cb_left_hand_control_cmd` | `sensor_msgs/JointState` | Left hand control data |
+
+### Message Format
+
+```python
+# sensor_msgs/JointState
+msg.header.stamp # Timestamp
+msg.name # Joint name list
+msg.position # Joint position values
+```
+
+## File Structure
+
+```
+motion/udexreal/
+├── __init__.py
+├── retarget.py # ROS2 integration
+└── README.md # English documentation
+```
+
+## Notes
+
+1. Ensure UDP port is not occupied
+2. Glove and host must be on the same network
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/README_zh.md b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/README_zh.md
new file mode 100644
index 0000000..9c2ecd8
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/README_zh.md
@@ -0,0 +1,62 @@
+# UdexReal (LinkerTG) 数据手套模块
+
+UdexReal / LinkerTG 数据手套的 ROS2 驱动模块，通过 UDP 接收手套数据并发布到 ROS2 话题。
+
+## 特性
+
+- 通过 UDP 协议接收手套数据
+- 支持左右手数据
+- 发布频率 50Hz
+
+## 使用方法
+
+### 1. 配置文件
+
+修改 `config/base_config.yml`:
+
+```yaml
+system:
+ motion_type: udexreal
+
+udp:
+ ip: "0.0.0.0"
+ port: 8888
+```
+
+### 2. 运行
+
+```bash
+ros2 run linkerhand_retarget handretarget
+```
+
+## ROS2 话题
+
+### 发布话题
+
+| 话题名 | 消息类型 | 说明 |
+|-------|---------|------|
+| `/cb_right_hand_control_cmd` | `sensor_msgs/JointState` | 右手控制数据 |
+| `/cb_left_hand_control_cmd` | `sensor_msgs/JointState` | 左手控制数据 |
+
+### 消息格式
+
+```python
+# sensor_msgs/JointState
+msg.header.stamp # 时间戳
+msg.name # 关节名称列表
+msg.position # 关节位置值
+```
+
+## 文件结构
+
+```
+motion/udexreal/
+├── __init__.py
+├── retarget.py # ROS2 集成
+└── README.md # 本文档
+```
+
+## 注意事项
+
+1. 确保 UDP 端口未被占用
+2. 手套与主机需在同一网络
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/__init__.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/__init__.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_l10v7.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_l10v7.py
new file mode 100644
index 0000000..832eac6
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_l10v7.py
@@ -0,0 +1,180 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=10):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1.3 # 旋转
+ qpos[17] = joint_arc[20] * 2.6 # 侧摆
+ # qpos[18] = joint_arc[2] * 0 # 根部关节
+ # qpos[19] = joint_arc[1] * 0 # 中部关节
+ qpos[20] = joint_arc[0] * -0.6 # 远端关节
+ # print(qpos[16],qpos[17])
+
+
+ qpos[0] = joint_arc[7]
+ qpos[1] = joint_arc[6] * -0.7
+ # qpos[2] = joint_arc[5] * -1
+ # qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19]
+ qpos[5] = joint_arc[18] * -0.7
+ # qpos[6] = joint_arc[17] * -1
+ # qpos[7] = joint_arc[16] * -1
+
+ # qpos[8] = joint_arc[11]
+ qpos[9] = joint_arc[10] * -0.7
+ # qpos[10] = joint_arc[9] * -1
+ # qpos[11] = joint_arc[8] * -1
+
+ qpos[12] = joint_arc[15]
+ qpos[13] = joint_arc[14] * -0.7
+ # qpos[14] = joint_arc[13] * -1
+ # qpos[15] = joint_arc[12] * -1
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ self.g_jointvelocity[i] = 255
+ # lastpos = self.last_jointpositions[i]
+ # position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ # position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ # slow_limit = 4
+ # fast_limit = 10
+ # max_vel = int(self.last_jointvelocity[i] * 2)
+ # mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ # min_vel = int(self.last_jointvelocity[i] * 0.5)
+ # target_vel = self.last_jointvelocity[i]
+ # if self.handstate[i] == 0: # stop
+ # if 0 < position_error:
+ # target_vel = position_error * 5 + 30
+ # self.handstate[i] = 1
+ # elif self.handstate[i] == 1: # slow
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 50
+ # if target_vel > mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 2
+ # elif position_error == 0:
+ # self.handstate[i] = 0
+ # target_vel = position_error * 5 + 100
+ # else:
+ # target_vel = position_error * 5 + 100
+ # else: # fast
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 90
+ # if target_vel > max_vel:
+ # target_vel = max_vel
+ # elif slow_limit < position_error < fast_limit:
+ # target_vel = position_error * 5 + 60
+ # if target_vel < mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 3
+ # elif 0 < position_error <= slow_limit:
+ # target_vel = position_error * 5 + 40
+ # if target_vel < min_vel:
+ # target_vel = min_vel
+ # self.handstate[i] = 1
+ # self.g_jointvelocity[i] = int(target_vel * 1)
+ # if self.g_jointvelocity[i] > 255:
+ # self.g_jointvelocity[i] = 255
+
+ # self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ # self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=10):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1 # 旋转
+ qpos[17] = joint_arc[2] * -1 # 侧摆
+ qpos[18] = joint_arc[2] * 0 # 根部关节
+ qpos[19] = joint_arc[1] * 0 # 中部关节
+ qpos[20] = joint_arc[1] * -1 # 远端关节
+ # print(qpos[16],qpos[17])
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ qpos[8] = joint_arc[11] * -1
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -1
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ self.g_jointvelocity[i] = 255
+ # lastpos = self.last_jointpositions[i]
+ # position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ # position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ # slow_limit = 4
+ # fast_limit = 10
+ # max_vel = int(self.last_jointvelocity[i] * 2)
+ # mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ # min_vel = int(self.last_jointvelocity[i] * 0.5)
+ # target_vel = self.last_jointvelocity[i]
+ # if self.handstate[i] == 0: # stop
+ # if 0 < position_error:
+ # target_vel = position_error * 5 + 30
+ # self.handstate[i] = 1
+ # elif self.handstate[i] == 1: # slow
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 50
+ # if target_vel > mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 2
+ # elif position_error == 0:
+ # self.handstate[i] = 0
+ # target_vel = position_error * 5 + 100
+ # else:
+ # target_vel = position_error * 5 + 100
+ # else: # fast
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 90
+ # if target_vel > max_vel:
+ # target_vel = max_vel
+ # elif slow_limit < position_error < fast_limit:
+ # target_vel = position_error * 5 + 60
+ # if target_vel < mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 3
+ # elif 0 < position_error <= slow_limit:
+ # target_vel = position_error * 5 + 40
+ # if target_vel < min_vel:
+ # target_vel = min_vel
+ # self.handstate[i] = 1
+ # self.g_jointvelocity[i] = int(target_vel * 1)
+ # if self.g_jointvelocity[i] > 255:
+ # self.g_jointvelocity[i] = 255
+ # self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ # self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_l20.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_l20.py
new file mode 100644
index 0000000..2e8790c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_l20.py
@@ -0,0 +1,176 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=20):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1
+ qpos[17] = joint_arc[20] * 1
+ qpos[18] = joint_arc[1] * -1
+ qpos[19] = joint_arc[0] * -1
+
+ qpos[0] = joint_arc[7] * 1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19] * 1
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ qpos[8] = joint_arc[11] * -1
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ qpos[12] = joint_arc[15] * 1
+ qpos[13] = joint_arc[14] * -1
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_rimit = 2
+ fast_rimit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.8)
+ min_vel = int(self.last_jointvelocity[i] * 0.6)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 10 + 5
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_rimit:
+ target_vel = position_error * 10 + 30
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 10 + 10
+ else: # fast
+ if position_error >= fast_rimit:
+ target_vel = position_error * 10 + 50
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_rimit < position_error < fast_rimit:
+ target_vel = position_error * 10 + 30
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_rimit:
+ target_vel = position_error * 10 + 10
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity [i] = int(target_vel)
+
+ if self.g_jointvelocity [i] > 255:
+ self.g_jointvelocity [i] = 255
+ self.g_jointvelocity [i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity [i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=20):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = (joint_arc[2] * -1 - 0.3)
+ qpos[17] = joint_arc[20] * 1
+ qpos[18] = joint_arc[1] * -1
+ qpos[19] = joint_arc[0] * 0
+
+ qpos[0] = joint_arc[7] * 1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19] * 1
+
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ qpos[8] = joint_arc[11] * 1
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+
+ qpos[12] = joint_arc[15] * 1
+ qpos[13] = joint_arc[14] * -1
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 2
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.8)
+ min_vel = int(self.last_jointvelocity[i] * 0.6)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 10 + 5
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 10 + 30
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 10 + 10
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 10 + 50
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 10 + 30
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 10 + 10
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity [i] = int(target_vel)
+
+ if self.g_jointvelocity [i] > 255:
+ self.g_jointvelocity [i] = 255
+ self.g_jointvelocity [i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity [i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_l21.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_l21.py
new file mode 100644
index 0000000..d9ee68e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_l21.py
@@ -0,0 +1,192 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=25):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1 # 侧摆
+ qpos[17] = joint_arc[20] * 2 # 旋转
+ qpos[18] = joint_arc[2] * -0.5 # 根部关节
+ qpos[19] = joint_arc[1] * -1 # 中部关节
+ qpos[20] = joint_arc[0] * -1 # 远端关节
+
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -0.9
+ qpos[2] = joint_arc[5] * -0.9
+ qpos[3] = joint_arc[4] * -1
+ # if joint_arc[6] > -80 * 3.14 / 180: qpos[2] = 0
+ # if joint_arc[6] > -80 * 3.14 / 180: qpos[3] = 0
+
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -0.9
+ qpos[6] = joint_arc[17] * -0.9
+ qpos[7] = joint_arc[16] * -1
+ # if joint_arc[18] > -75 * 3.14 / 180: qpos[6] = 0
+ # if joint_arc[18] > -75 * 3.14 / 180: qpos[7] = 0
+
+ qpos[8] = joint_arc[11] * -1
+ qpos[9] = joint_arc[10] * -0.9
+ qpos[10] = joint_arc[9] * -0.9
+ qpos[11] = joint_arc[8] * -1
+ # if joint_arc[10] > -80 * 3.14 / 180: qpos[10] = 0
+ # if joint_arc[10] > -80 * 3.14 / 180: qpos[11] = 0
+
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -0.9
+ qpos[14] = joint_arc[13] * -0.9
+ qpos[15] = joint_arc[12] * -1
+ # if joint_arc[14] > -80 * 3.14 / 180: qpos[14] = 0
+ # if joint_arc[14] > -80 * 3.14 / 180: qpos[15] = 0
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 1
+ fast_limit = 5
+ max_vel = int(self.last_jointvelocity[i] * 1.5)
+ min_vel = int(self.last_jointvelocity[i] * 0.995)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 3 + 50
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 3 + 100
+ if target_vel > max_vel:
+ target_vel = max_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 3 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 200
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 3 + 200
+ if target_vel < min_vel:
+ target_vel = min_vel
+ else:
+ target_vel = min_vel
+ if min_vel < 150:
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel)
+ # if self.g_jointvelocity[i] > 255:
+ # self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+ self.g_jointvelocity[6] = 255
+ self.g_jointvelocity[7] = 255
+ self.g_jointvelocity[8] = 255
+ self.g_jointvelocity[9] = 255
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=25):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1.5 # 侧摆
+ qpos[17] = joint_arc[20] * 3 # 旋转
+ qpos[18] = joint_arc[2] * -0.5 # 根部关节
+ qpos[19] = joint_arc[1] * -1 # 中部关节
+ qpos[20] = joint_arc[0] * -1 # 远端关节
+
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -0.9
+ qpos[2] = joint_arc[5] * -0.9
+ qpos[3] = joint_arc[4] * -1
+ # if joint_arc[6] > -80 * 3.14 / 180: qpos[2] = 0
+ # if joint_arc[6] > -80 * 3.14 / 180: qpos[3] = 0
+
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -0.9
+ qpos[6] = joint_arc[17] * -0.9
+ qpos[7] = joint_arc[16] * -1
+ # if joint_arc[18] > -75 * 3.14 / 180: qpos[6] = 0
+ # if joint_arc[18] > -75 * 3.14 / 180: qpos[7] = 0
+
+ qpos[8] = joint_arc[11] * -1
+ qpos[9] = joint_arc[10] * -0.9
+ qpos[10] = joint_arc[9] * -0.9
+ qpos[11] = joint_arc[8] * -1
+ # if joint_arc[10] > -80 * 3.14 / 180: qpos[10] = 0
+ # if joint_arc[10] > -80 * 3.14 / 180: qpos[11] = 0
+
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -0.9
+ qpos[14] = joint_arc[13] * -0.9
+ qpos[15] = joint_arc[12] * -1
+ # if joint_arc[14] > -80 * 3.14 / 180: qpos[14] = 0
+ # if joint_arc[14] > -80 * 3.14 / 180: qpos[15] = 0
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 1
+ fast_limit = 5
+ max_vel = int(self.last_jointvelocity[i] * 1.5)
+ min_vel = int(self.last_jointvelocity[i] * 0.995)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 3 + 50
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 3 + 100
+ if target_vel > max_vel:
+ target_vel = max_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 3 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 200
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 3 + 200
+ if target_vel < min_vel:
+ target_vel = min_vel
+ else:
+ target_vel = min_vel
+ if min_vel < 150:
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+ self.g_jointvelocity[6] = 255
+ self.g_jointvelocity[7] = 255
+ self.g_jointvelocity[8] = 255
+ self.g_jointvelocity[9] = 255
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_l25.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_l25.py
new file mode 100644
index 0000000..bc32533
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_l25.py
@@ -0,0 +1,202 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=25):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1 # 侧摆
+ qpos[17] = joint_arc[20] * 2.6 # 旋转
+ qpos[18] = joint_arc[2] * -0.5 # 根部关节
+ qpos[19] = joint_arc[1] * -1 # 中部关节
+ qpos[20] = joint_arc[0] * -1.5 # 远端关节
+ if joint_arc[0] > -40 * 3.14 / 180:
+ qpos[18] = 0
+ qpos[19] = 0
+ qpos[20] = 0
+
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+ if joint_arc[6] > -80 * 3.14 / 180: qpos[2] = 0
+ if joint_arc[6] > -80 * 3.14 / 180: qpos[3] = 0
+
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+ if joint_arc[18] > -70 * 3.14 / 180: qpos[6] = 0
+ if joint_arc[18] > -70 * 3.14 / 180: qpos[7] = 0
+
+ qpos[8] = joint_arc[11] * -1
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+ if joint_arc[10] > -80 * 3.14 / 180: qpos[10] = 0
+ if joint_arc[10] > -80 * 3.14 / 180: qpos[11] = 0
+
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -1
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+ if joint_arc[14] > -80 * 3.14 / 180: qpos[14] = 0
+ if joint_arc[14] > -80 * 3.14 / 180: qpos[15] = 0
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 1
+ fast_limit = 5
+ max_vel = int(self.last_jointvelocity[i] * 1.5)
+ min_vel = int(self.last_jointvelocity[i] * 0.995)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 3 + 50
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 3 + 100
+ if target_vel > max_vel:
+ target_vel = max_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 3 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 200
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 3 + 200
+ if target_vel < min_vel:
+ target_vel = min_vel
+ else:
+ target_vel = min_vel
+ if min_vel < 150:
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+ self.g_jointvelocity[6] = 255
+ self.g_jointvelocity[7] = 255
+ self.g_jointvelocity[8] = 255
+ self.g_jointvelocity[9] = 255
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=25):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1 - 0.4 # 侧摆
+ qpos[17] = joint_arc[20] * 2 - 0.2 # 旋转
+ qpos[18] = joint_arc[2] * -0.2 # 根部关节
+ qpos[19] = joint_arc[1] * -1 # 中部关节
+ qpos[20] = joint_arc[0] * -1 # 远端关节
+ if joint_arc[0] > -50 * 3.14 / 180:
+ qpos[18] = 0.2
+ qpos[19] = 0
+ qpos[20] = 0
+
+ # 食指 index
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+ if joint_arc[6] > -80 * 3.14 / 180: qpos[2] = 0
+ if joint_arc[6] > -80 * 3.14 / 180: qpos[3] = 0
+
+ # 小指 little
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+ if joint_arc[18] > -75 * 3.14 / 180: qpos[6] = 0
+ if joint_arc[18] > -75 * 3.14 / 180: qpos[7] = 0
+
+ # 中指 middle
+ qpos[8] = joint_arc[11] * 0
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+ if joint_arc[10] > -80 * 3.14 / 180: qpos[10] = 0
+ if joint_arc[10] > -80 * 3.14 / 180: qpos[11] = 0
+
+ # 无名指 ring
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -1 # gen
+ qpos[14] = joint_arc[13] * -1 # zhong
+ qpos[15] = joint_arc[12] * -1
+ if joint_arc[14] > -80 * 3.14 / 180: qpos[14] = 0
+ if joint_arc[14] > -80 * 3.14 / 180: qpos[15] = 0
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 1
+ fast_limit = 5
+ max_vel = int(self.last_jointvelocity[i] * 1.5)
+ min_vel = int(self.last_jointvelocity[i] * 0.995)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 3 + 50
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 3 + 100
+ if target_vel > max_vel:
+ target_vel = max_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 3 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 200
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 3 + 200
+ if target_vel < min_vel:
+ target_vel = min_vel
+ else:
+ target_vel = min_vel
+ if min_vel < 150:
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+ self.g_jointvelocity[6] = 255
+ self.g_jointvelocity[7] = 255
+ self.g_jointvelocity[8] = 255
+ self.g_jointvelocity[9] = 255
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_l6.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_l6.py
new file mode 100644
index 0000000..81715cd
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_l6.py
@@ -0,0 +1,181 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1.2 # 侧摆
+ qpos[17] = joint_arc[20] * 2.4 # 旋转
+ qpos[18] = joint_arc[2] * -0.3878 # 根部关节
+ qpos[19] = joint_arc[1] * -0.66845 # 中部关节
+ qpos[20] = joint_arc[0] * -0.8 # 远端关节
+
+ qpos[0] = joint_arc[7]
+ qpos[1] = joint_arc[6] * -0.7
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19]
+ qpos[5] = joint_arc[18] * -0.7
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ qpos[8] = joint_arc[11]
+ qpos[9] = joint_arc[10] * -1.0098
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ qpos[12] = joint_arc[15]
+ qpos[13] = joint_arc[14] * -0.7
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1 # 侧摆
+ qpos[17] = joint_arc[20] * 1.5 # 旋转
+ qpos[18] = joint_arc[2] * -0.9 # 根部关节
+ qpos[19] = joint_arc[1] * -0.8 # 中部关节
+ qpos[20] = joint_arc[0] * -0.8 # 远端关节
+
+ # 食指 index
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -0.8
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ # 小指 little
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -0.8
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ # 中指 middle
+ qpos[8] = joint_arc[11] * -1.0098
+ qpos[9] = joint_arc[10] * -0.8
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ # 无名指 ring
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -0.8 # gen
+ qpos[14] = joint_arc[13] * -1 # zhong
+ qpos[15] = joint_arc[12] * -1
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_l7.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_l7.py
new file mode 100644
index 0000000..7707080
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_l7.py
@@ -0,0 +1,181 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=7):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1.2 # 侧摆
+ qpos[17] = joint_arc[20] * 2.4 # 旋转
+ qpos[18] = joint_arc[2] * -0.3878 # 根部关节
+ qpos[19] = joint_arc[1] * -0.66845 # 中部关节
+ qpos[20] = joint_arc[0] * -0.8 # 远端关节
+
+ qpos[0] = joint_arc[7]
+ qpos[1] = joint_arc[6] * -0.7
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19]
+ qpos[5] = joint_arc[18] * -0.7
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ qpos[8] = joint_arc[11]
+ qpos[9] = joint_arc[10] * -1.0098
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ qpos[12] = joint_arc[15]
+ qpos[13] = joint_arc[14] * -0.7
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=7):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[2] * -0.5 # 旋转
+ qpos[17] = joint_arc[20] * 0.8 # 侧摆
+ qpos[18] = joint_arc[2] * -0.9 # 根部关节
+ qpos[19] = joint_arc[1] * -0.8 # 中部关节
+ qpos[20] = joint_arc[0] * -0.5 # 远端关节
+
+ # 食指 index
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ # 小指 little
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ # 中指 middle
+ qpos[8] = joint_arc[11] * -1.0098
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ # 无名指 ring
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -1 # gen
+ qpos[14] = joint_arc[13] * -1 # zhong
+ qpos[15] = joint_arc[12] * -1
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_o6.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_o6.py
new file mode 100644
index 0000000..a6cf885
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/hand/udexreal_o6.py
@@ -0,0 +1,181 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1.2 # 侧摆
+ qpos[17] = joint_arc[20] * 2.4 # 旋转
+ qpos[18] = joint_arc[2] * -0.3878 # 根部关节
+ qpos[19] = joint_arc[1] * -0.66845 # 中部关节
+ qpos[20] = joint_arc[0] * -0.1 # 远端关节
+
+ qpos[0] = joint_arc[7]
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19]
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ qpos[8] = joint_arc[11]
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ qpos[12] = joint_arc[15]
+ qpos[13] = joint_arc[14] * -1
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1 # 侧摆
+ qpos[17] = joint_arc[20] * 2 # 旋转
+ qpos[18] = joint_arc[2] * -0.9 # 根部关节
+ qpos[19] = joint_arc[1] * -0.8 # 中部关节
+ qpos[20] = joint_arc[0] * -0.3 # 远端关节
+
+ # 食指 index
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ # 小指 little
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -11
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ # 中指 middle
+ qpos[8] = joint_arc[11] * -1
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ # 无名指 ring
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -1 # gen
+ qpos[14] = joint_arc[13] * -1 # zhong
+ qpos[15] = joint_arc[12] * -1
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/retarget.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/retarget.py
new file mode 100644
index 0000000..76a1080
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexreal/retarget.py
@@ -0,0 +1,189 @@
+
+import time
+import sys
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from datetime import datetime
+from pathlib import Path
+
+# 将项目根目录放在最前面
+# 强制使用项目本地的 linkerhand 模块
+_project_root = Path(__file__).absolute().parent.parent.parent
+_project_root_str = str(_project_root)
+
+if _project_root_str in sys.path:
+ sys.path.remove(_project_root_str)
+sys.path.insert(0, _project_root_str)
+from linkerhand.udexrealcore import UdexRealScoketUdp, TimeoutStatus, UdexRealData
+from linkerhand.handcore import HandCore
+
+from linkerhand.constants import RobotName, ROBOT_LEN_MAP
+
+
+LOG_FILE_PATH = "/tmp/b.log"
+
+class Retarget():
+ def __init__(self,node, ip, port, deviceid, lefthand: RobotName, righthand: RobotName, handcore: HandCore,
+ lefthandpubprint: bool, righthandpubprint: bool):
+ self.node = node
+ self.udp_ip = ip
+ self.udp_port = port
+ self.motion_device = deviceid
+ self.lefthandtype = lefthand
+ self.righthandtype = righthand
+ self.handcore = handcore
+ self.runing = True
+ self.lefthandpubprint = lefthandpubprint
+ self.righthandpubprint = righthandpubprint
+
+ # 根据右手类型初始化
+ if self.righthandtype == RobotName.o7 \
+ or self.righthandtype == RobotName.l7 \
+ or self.righthandtype == RobotName.o7v1 \
+ or self.righthandtype == RobotName.o7v3:
+ from .hand.udexreal_l7 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.o6:
+ from .hand.udexreal_o6 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l6:
+ from .hand.udexreal_l6 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l25 \
+ or self.righthandtype == RobotName.g20:
+ from .hand.udexreal_l25 import RightHand
+ # self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ # elif self.righthandtype == RobotName.t25:
+ # from .hand.udexreal_t25 import RightHand
+ # self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l20:
+ from .hand.udexreal_l20 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l10 \
+ or self.righthandtype == RobotName.l10v7 :
+ from .hand.udexreal_l10v7 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+
+ else:
+ print("未正确定义机械左手对象，请检查支持清单列表!")
+
+ # 根据LEFT手类型初始化
+ if self.lefthandtype == RobotName.o7 \
+ or self.lefthandtype == RobotName.l7 \
+ or self.lefthandtype == RobotName.o7v1 \
+ or self.lefthandtype == RobotName.o7v3:
+ from .hand.udexreal_l7 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.o6:
+ from .hand.udexreal_o6 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l6:
+ from .hand.udexreal_l6 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l25 \
+ or self.lefthandtype == RobotName.g20:
+ from .hand.udexreal_l25 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ # elif self.lefthandtype == RobotName.t25:
+ # from .hand.udexreal_t25 import LeftHand
+ # self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l20:
+ from .hand.udexreal_l20 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l10 \
+ or self.lefthandtype == RobotName.l10v7 :
+ from .hand.udexreal_l10v7 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ else:
+ print("未正确定义机械右手对象，请检查支持清单列表!")
+
+ # ROS2 发布器
+ self.publisher_r = self.node.create_publisher(
+ JointState,
+ '/cb_right_hand_control_cmd',
+ self.handcore.hand_numjoints_r)
+
+ self.publisher_l = self.node.create_publisher(
+ JointState,
+ '/cb_left_hand_control_cmd',
+ self.handcore.hand_numjoints_l)
+
+ self.timer = self.node.create_timer(1.0/120, self.process_callback) # 120Hz
+ self.pubprintcount = 0
+ self.udp_datacapture = None
+
+ def initialize_udp(self):
+ """初始化UDP连接"""
+ self.udp_datacapture = UdexRealScoketUdp(
+ host=self.udp_ip,
+ port=self.udp_port,
+ device_id=self.motion_device)
+ self.udp_datacapture.set_timeout_callback(self.on_timeout_callback)
+ self.udp_datacapture.set_data_recovered_callback(self.on_data_recovered_callback)
+ return self.udp_datacapture.udp_initial()
+
+ def on_timeout_callback(self, status: TimeoutStatus):
+ """超时回调函数"""
+ # if status.consecutive_timeout_checks == 1: # 第一次超时
+ # print(f"[{datetime.now().strftime('%Y-%m-%d %H:%M:%S')}] "
+ # f"警告: 数据接收超时，{status.time_since_last_data:.1f}秒未收到数据")
+ if status.consecutive_timeout_checks % 1 == 0: # 每10次检查打印一次
+ print(f"[{datetime.now().strftime('%Y-%m-%d %H:%M:%S')}] "
+ f"警告: 数据接收超时，{status.time_since_last_data:.1f}秒未收到数据")
+
+ def on_data_recovered_callback(self):
+ """数据恢复回调函数"""
+ print(f"[{datetime.now().strftime('%Y-%m-%d %H:%M:%S')}] 数据连接已恢复")
+
+
+ def process_callback(self):
+ if not self.runing:
+ return
+
+ mocapdata = self.udp_datacapture.realmocapdata
+
+ # 检查数据是否更新
+ if not mocapdata.is_update:
+ return
+
+ # 处理左右手原始数据
+ self.lefthand.joint_update(mocapdata.jointangle_lHand)
+ self.righthand.joint_update(mocapdata.jointangle_rHand)
+
+ # 速度环节处理
+ self.lefthand.speed_update()
+ self.righthand.speed_update()
+
+ # self.quick_log(self.lefthand.g_jointpositions[1],LOG_FILE_PATH,"SEND")
+
+ # 调试打印
+ if self.lefthandpubprint and self.pubprintcount % 1 == 0:
+ self.node.get_logger().info(f"左手位置: {self.lefthand.g_jointpositions}")
+ if self.righthandpubprint and self.pubprintcount % 1 == 0:
+ self.node.get_logger().info(f"右手位置: {self.righthand.g_jointpositions}")
+
+ # 发布右手数据
+ msg_r = JointState()
+ msg_r.header.stamp = self.node.get_clock().now().to_msg()
+ msg_r.name = [f'joint{i + 1}' for i in range(len(self.righthand.g_jointpositions))]
+ msg_r.position = [float(num) for num in self.righthand.g_jointpositions]
+ msg_r.velocity = [float(num) for num in self.righthand.g_jointvelocity]
+ self.publisher_r.publish(msg_r)
+
+ # 发布左手数据
+ msg_l = JointState()
+ msg_l.header.stamp = self.node.get_clock().now().to_msg()
+ msg_l.name = [f'joint{i + 1}' for i in range(len(self.lefthand.g_jointpositions))]
+ msg_l.position = [float(num) for num in self.lefthand.g_jointpositions]
+ msg_l.velocity = [float(num) for num in self.lefthand.g_jointvelocity]
+ self.publisher_l.publish(msg_l)
+
+ self.pubprintcount += 1
+
+ def process(self):
+ """主处理函数"""
+ if not self.initialize_udp():
+ self.node.get_logger().error("初始化配置网络失败")
+ return
+
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/__init__.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/config/l6_config.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/config/l6_config.py
new file mode 100644
index 0000000..d896fc9
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/config/l6_config.py
@@ -0,0 +1,61 @@
+# 手指配置常量
+FINGER_CONFIGS = {
+ 'thumb_abduction': {
+ 'name': '拇指侧摆',
+ 'joints': [0, 1, 2],
+ 'weights': [0.3, 0.4, 0.3],
+ 'robot_idx': 0,
+ 'type': 'thumb'
+ },
+ 'thumb_flexion': {
+ 'name': '拇指弯曲',
+ 'joints': [2, 3, 4],
+ 'weights': [0.3, 0.0, 0.7],
+ 'robot_idx': 1,
+ 'type': 'thumb'
+ },
+ 'index': {
+ 'name': '食指',
+ 'joints': [6, 7, 8],
+ 'weights': [0.5, 0.3, 0.2],
+ 'robot_idx': 3,
+ 'type': 'finger'
+ },
+ 'middle': {
+ 'name': '中指',
+ 'joints': [10, 11, 12],
+ 'weights': [0.5, 0.1, 0.4],
+ 'robot_idx': 5,
+ 'type': 'finger'
+ },
+ 'ring': {
+ 'name': '无名指',
+ 'joints': [14, 15, 16],
+ 'weights': [0.5, 0.1, 0.4],
+ 'robot_idx': 7,
+ 'type': 'finger'
+ },
+ 'pinky': {
+ 'name': '小指',
+ 'joints': [18, 19, 20],
+ 'weights': [0.5, 0.1, 0.4],
+ 'robot_idx': 9,
+ 'type': 'finger'
+ }
+}
+
+# 映射顺序
+MAPPING_ORDER = [
+ 'thumb_abduction', 'thumb_flexion',
+ 'index', 'middle', 'ring', 'pinky'
+]
+
+# 三态默认配置
+MULTI_SEGMENT_CONFIG = {
+ 'states': ['original', 'opose', 'fist'],
+ 'state_names': {
+ 'original': '张手',
+ 'opose': 'O手势',
+ 'fist': '握拳'
+ }
+}
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/config/o6_config.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/config/o6_config.py
new file mode 100644
index 0000000..bdd9a92
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/config/o6_config.py
@@ -0,0 +1,61 @@
+# 手指配置常量
+FINGER_CONFIGS = {
+ 'thumb_abduction': {
+ 'name': '拇指侧摆',
+ 'joints': [20],
+ 'weights': [1],
+ 'robot_idx': 0,
+ 'type': 'thumb'
+ },
+ 'thumb_flexion': {
+ 'name': '拇指弯曲',
+ 'joints': [0, 1, 2],
+ 'weights': [0.6, 0.3, 0.1],
+ 'robot_idx': 1,
+ 'type': 'thumb'
+ },
+ 'index': {
+ 'name': '食指',
+ 'joints': [4, 5, 6],
+ 'weights': [0.6, 0.3, 0.1],
+ 'robot_idx': 3,
+ 'type': 'finger'
+ },
+ 'middle': {
+ 'name': '中指',
+ 'joints': [8, 9, 10],
+ 'weights': [0.6, 0.3, 0.1],
+ 'robot_idx': 5,
+ 'type': 'finger'
+ },
+ 'ring': {
+ 'name': '无名指',
+ 'joints': [12, 13, 14],
+ 'weights': [0.6, 0.3, 0.1],
+ 'robot_idx': 7,
+ 'type': 'finger'
+ },
+ 'pinky': {
+ 'name': '小指',
+ 'joints': [16, 17, 18],
+ 'weights': [0.6, 0.3, 0.1],
+ 'robot_idx': 9,
+ 'type': 'finger'
+ }
+}
+
+# 映射顺序
+MAPPING_ORDER = [
+ 'thumb_abduction', 'thumb_flexion',
+ 'index', 'middle', 'ring', 'pinky'
+]
+
+# 三态默认配置
+MULTI_SEGMENT_CONFIG = {
+ 'states': ['original', 'opose', 'fist'],
+ 'state_names': {
+ 'original': '张手',
+ 'opose': 'O手势',
+ 'fist': '握拳'
+ }
+}
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/config/o7_config.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/config/o7_config.py
new file mode 100644
index 0000000..2e0b543
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/config/o7_config.py
@@ -0,0 +1,68 @@
+# 手指配置常量
+FINGER_CONFIGS = {
+ 'thumb_rotate': {
+ 'name': '拇指旋转',
+ 'joints': [1, 2],
+ 'weights': [0.3, 0.7],
+ 'robot_idx': 0,
+ 'type': 'thumb'
+ },
+ 'thumb_abduction': {
+ 'name': '拇指侧摆',
+ 'joints': [0, 1, 2],
+ 'weights': [0.6, 0.1, 0.3],
+ 'robot_idx': 1,
+ 'type': 'thumb'
+ },
+ 'thumb_flexion': {
+ 'name': '拇指弯曲',
+ 'joints': [2, 3, 4],
+ 'weights': [0.3, 0.1, 0.6],
+ 'robot_idx': 2,
+ 'type': 'thumb'
+ },
+ 'index': {
+ 'name': '食指',
+ 'joints': [6, 7, 8],
+ 'weights': [0.5, 0.3, 0.2],
+ 'robot_idx': 5,
+ 'type': 'finger'
+ },
+ 'middle': {
+ 'name': '中指',
+ 'joints': [10, 11, 12],
+ 'weights': [0.5, 0.3, 0.2],
+ 'robot_idx': 8,
+ 'type': 'finger'
+ },
+ 'ring': {
+ 'name': '无名指',
+ 'joints': [14, 15, 16],
+ 'weights': [0.5, 0.3, 0.2],
+ 'robot_idx': 11,
+ 'type': 'finger'
+ },
+ 'pinky': {
+ 'name': '小指',
+ 'joints': [18, 19, 20],
+ 'weights': [0.5, 0.3, 0.2],
+ 'robot_idx': 14,
+ 'type': 'finger'
+ }
+}
+
+# 映射顺序
+MAPPING_ORDER = [
+ 'thumb_rotate', 'thumb_abduction', 'thumb_flexion',
+ 'index', 'middle', 'ring', 'pinky'
+]
+
+# 三态默认配置
+MULTI_SEGMENT_CONFIG = {
+ 'states': ['original', 'opose', 'fist'],
+ 'state_names': {
+ 'original': '张手',
+ 'opose': 'O手势',
+ 'fist': '握拳'
+ }
+}
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/__init__.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_l10v7.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_l10v7.py
new file mode 100644
index 0000000..832eac6
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_l10v7.py
@@ -0,0 +1,180 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=10):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1.3 # 旋转
+ qpos[17] = joint_arc[20] * 2.6 # 侧摆
+ # qpos[18] = joint_arc[2] * 0 # 根部关节
+ # qpos[19] = joint_arc[1] * 0 # 中部关节
+ qpos[20] = joint_arc[0] * -0.6 # 远端关节
+ # print(qpos[16],qpos[17])
+
+
+ qpos[0] = joint_arc[7]
+ qpos[1] = joint_arc[6] * -0.7
+ # qpos[2] = joint_arc[5] * -1
+ # qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19]
+ qpos[5] = joint_arc[18] * -0.7
+ # qpos[6] = joint_arc[17] * -1
+ # qpos[7] = joint_arc[16] * -1
+
+ # qpos[8] = joint_arc[11]
+ qpos[9] = joint_arc[10] * -0.7
+ # qpos[10] = joint_arc[9] * -1
+ # qpos[11] = joint_arc[8] * -1
+
+ qpos[12] = joint_arc[15]
+ qpos[13] = joint_arc[14] * -0.7
+ # qpos[14] = joint_arc[13] * -1
+ # qpos[15] = joint_arc[12] * -1
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ self.g_jointvelocity[i] = 255
+ # lastpos = self.last_jointpositions[i]
+ # position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ # position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ # slow_limit = 4
+ # fast_limit = 10
+ # max_vel = int(self.last_jointvelocity[i] * 2)
+ # mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ # min_vel = int(self.last_jointvelocity[i] * 0.5)
+ # target_vel = self.last_jointvelocity[i]
+ # if self.handstate[i] == 0: # stop
+ # if 0 < position_error:
+ # target_vel = position_error * 5 + 30
+ # self.handstate[i] = 1
+ # elif self.handstate[i] == 1: # slow
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 50
+ # if target_vel > mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 2
+ # elif position_error == 0:
+ # self.handstate[i] = 0
+ # target_vel = position_error * 5 + 100
+ # else:
+ # target_vel = position_error * 5 + 100
+ # else: # fast
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 90
+ # if target_vel > max_vel:
+ # target_vel = max_vel
+ # elif slow_limit < position_error < fast_limit:
+ # target_vel = position_error * 5 + 60
+ # if target_vel < mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 3
+ # elif 0 < position_error <= slow_limit:
+ # target_vel = position_error * 5 + 40
+ # if target_vel < min_vel:
+ # target_vel = min_vel
+ # self.handstate[i] = 1
+ # self.g_jointvelocity[i] = int(target_vel * 1)
+ # if self.g_jointvelocity[i] > 255:
+ # self.g_jointvelocity[i] = 255
+
+ # self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ # self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=10):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1 # 旋转
+ qpos[17] = joint_arc[2] * -1 # 侧摆
+ qpos[18] = joint_arc[2] * 0 # 根部关节
+ qpos[19] = joint_arc[1] * 0 # 中部关节
+ qpos[20] = joint_arc[1] * -1 # 远端关节
+ # print(qpos[16],qpos[17])
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ qpos[8] = joint_arc[11] * -1
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -1
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ self.g_jointvelocity[i] = 255
+ # lastpos = self.last_jointpositions[i]
+ # position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ # position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ # slow_limit = 4
+ # fast_limit = 10
+ # max_vel = int(self.last_jointvelocity[i] * 2)
+ # mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ # min_vel = int(self.last_jointvelocity[i] * 0.5)
+ # target_vel = self.last_jointvelocity[i]
+ # if self.handstate[i] == 0: # stop
+ # if 0 < position_error:
+ # target_vel = position_error * 5 + 30
+ # self.handstate[i] = 1
+ # elif self.handstate[i] == 1: # slow
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 50
+ # if target_vel > mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 2
+ # elif position_error == 0:
+ # self.handstate[i] = 0
+ # target_vel = position_error * 5 + 100
+ # else:
+ # target_vel = position_error * 5 + 100
+ # else: # fast
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 90
+ # if target_vel > max_vel:
+ # target_vel = max_vel
+ # elif slow_limit < position_error < fast_limit:
+ # target_vel = position_error * 5 + 60
+ # if target_vel < mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 3
+ # elif 0 < position_error <= slow_limit:
+ # target_vel = position_error * 5 + 40
+ # if target_vel < min_vel:
+ # target_vel = min_vel
+ # self.handstate[i] = 1
+ # self.g_jointvelocity[i] = int(target_vel * 1)
+ # if self.g_jointvelocity[i] > 255:
+ # self.g_jointvelocity[i] = 255
+ # self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ # self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_l20.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_l20.py
new file mode 100644
index 0000000..2e8790c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_l20.py
@@ -0,0 +1,176 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=20):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1
+ qpos[17] = joint_arc[20] * 1
+ qpos[18] = joint_arc[1] * -1
+ qpos[19] = joint_arc[0] * -1
+
+ qpos[0] = joint_arc[7] * 1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19] * 1
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ qpos[8] = joint_arc[11] * -1
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ qpos[12] = joint_arc[15] * 1
+ qpos[13] = joint_arc[14] * -1
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_rimit = 2
+ fast_rimit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.8)
+ min_vel = int(self.last_jointvelocity[i] * 0.6)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 10 + 5
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_rimit:
+ target_vel = position_error * 10 + 30
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 10 + 10
+ else: # fast
+ if position_error >= fast_rimit:
+ target_vel = position_error * 10 + 50
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_rimit < position_error < fast_rimit:
+ target_vel = position_error * 10 + 30
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_rimit:
+ target_vel = position_error * 10 + 10
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity [i] = int(target_vel)
+
+ if self.g_jointvelocity [i] > 255:
+ self.g_jointvelocity [i] = 255
+ self.g_jointvelocity [i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity [i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=20):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = (joint_arc[2] * -1 - 0.3)
+ qpos[17] = joint_arc[20] * 1
+ qpos[18] = joint_arc[1] * -1
+ qpos[19] = joint_arc[0] * 0
+
+ qpos[0] = joint_arc[7] * 1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19] * 1
+
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ qpos[8] = joint_arc[11] * 1
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+
+ qpos[12] = joint_arc[15] * 1
+ qpos[13] = joint_arc[14] * -1
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 2
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.8)
+ min_vel = int(self.last_jointvelocity[i] * 0.6)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 10 + 5
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 10 + 30
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 10 + 10
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 10 + 50
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 10 + 30
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 10 + 10
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity [i] = int(target_vel)
+
+ if self.g_jointvelocity [i] > 255:
+ self.g_jointvelocity [i] = 255
+ self.g_jointvelocity [i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity [i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_l21.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_l21.py
new file mode 100644
index 0000000..d9ee68e
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_l21.py
@@ -0,0 +1,192 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=25):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1 # 侧摆
+ qpos[17] = joint_arc[20] * 2 # 旋转
+ qpos[18] = joint_arc[2] * -0.5 # 根部关节
+ qpos[19] = joint_arc[1] * -1 # 中部关节
+ qpos[20] = joint_arc[0] * -1 # 远端关节
+
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -0.9
+ qpos[2] = joint_arc[5] * -0.9
+ qpos[3] = joint_arc[4] * -1
+ # if joint_arc[6] > -80 * 3.14 / 180: qpos[2] = 0
+ # if joint_arc[6] > -80 * 3.14 / 180: qpos[3] = 0
+
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -0.9
+ qpos[6] = joint_arc[17] * -0.9
+ qpos[7] = joint_arc[16] * -1
+ # if joint_arc[18] > -75 * 3.14 / 180: qpos[6] = 0
+ # if joint_arc[18] > -75 * 3.14 / 180: qpos[7] = 0
+
+ qpos[8] = joint_arc[11] * -1
+ qpos[9] = joint_arc[10] * -0.9
+ qpos[10] = joint_arc[9] * -0.9
+ qpos[11] = joint_arc[8] * -1
+ # if joint_arc[10] > -80 * 3.14 / 180: qpos[10] = 0
+ # if joint_arc[10] > -80 * 3.14 / 180: qpos[11] = 0
+
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -0.9
+ qpos[14] = joint_arc[13] * -0.9
+ qpos[15] = joint_arc[12] * -1
+ # if joint_arc[14] > -80 * 3.14 / 180: qpos[14] = 0
+ # if joint_arc[14] > -80 * 3.14 / 180: qpos[15] = 0
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 1
+ fast_limit = 5
+ max_vel = int(self.last_jointvelocity[i] * 1.5)
+ min_vel = int(self.last_jointvelocity[i] * 0.995)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 3 + 50
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 3 + 100
+ if target_vel > max_vel:
+ target_vel = max_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 3 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 200
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 3 + 200
+ if target_vel < min_vel:
+ target_vel = min_vel
+ else:
+ target_vel = min_vel
+ if min_vel < 150:
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel)
+ # if self.g_jointvelocity[i] > 255:
+ # self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+ self.g_jointvelocity[6] = 255
+ self.g_jointvelocity[7] = 255
+ self.g_jointvelocity[8] = 255
+ self.g_jointvelocity[9] = 255
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=25):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1.5 # 侧摆
+ qpos[17] = joint_arc[20] * 3 # 旋转
+ qpos[18] = joint_arc[2] * -0.5 # 根部关节
+ qpos[19] = joint_arc[1] * -1 # 中部关节
+ qpos[20] = joint_arc[0] * -1 # 远端关节
+
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -0.9
+ qpos[2] = joint_arc[5] * -0.9
+ qpos[3] = joint_arc[4] * -1
+ # if joint_arc[6] > -80 * 3.14 / 180: qpos[2] = 0
+ # if joint_arc[6] > -80 * 3.14 / 180: qpos[3] = 0
+
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -0.9
+ qpos[6] = joint_arc[17] * -0.9
+ qpos[7] = joint_arc[16] * -1
+ # if joint_arc[18] > -75 * 3.14 / 180: qpos[6] = 0
+ # if joint_arc[18] > -75 * 3.14 / 180: qpos[7] = 0
+
+ qpos[8] = joint_arc[11] * -1
+ qpos[9] = joint_arc[10] * -0.9
+ qpos[10] = joint_arc[9] * -0.9
+ qpos[11] = joint_arc[8] * -1
+ # if joint_arc[10] > -80 * 3.14 / 180: qpos[10] = 0
+ # if joint_arc[10] > -80 * 3.14 / 180: qpos[11] = 0
+
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -0.9
+ qpos[14] = joint_arc[13] * -0.9
+ qpos[15] = joint_arc[12] * -1
+ # if joint_arc[14] > -80 * 3.14 / 180: qpos[14] = 0
+ # if joint_arc[14] > -80 * 3.14 / 180: qpos[15] = 0
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 1
+ fast_limit = 5
+ max_vel = int(self.last_jointvelocity[i] * 1.5)
+ min_vel = int(self.last_jointvelocity[i] * 0.995)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 3 + 50
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 3 + 100
+ if target_vel > max_vel:
+ target_vel = max_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 3 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 200
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 3 + 200
+ if target_vel < min_vel:
+ target_vel = min_vel
+ else:
+ target_vel = min_vel
+ if min_vel < 150:
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+ self.g_jointvelocity[6] = 255
+ self.g_jointvelocity[7] = 255
+ self.g_jointvelocity[8] = 255
+ self.g_jointvelocity[9] = 255
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_l25.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_l25.py
new file mode 100644
index 0000000..bc32533
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_l25.py
@@ -0,0 +1,202 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=25):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1 # 侧摆
+ qpos[17] = joint_arc[20] * 2.6 # 旋转
+ qpos[18] = joint_arc[2] * -0.5 # 根部关节
+ qpos[19] = joint_arc[1] * -1 # 中部关节
+ qpos[20] = joint_arc[0] * -1.5 # 远端关节
+ if joint_arc[0] > -40 * 3.14 / 180:
+ qpos[18] = 0
+ qpos[19] = 0
+ qpos[20] = 0
+
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+ if joint_arc[6] > -80 * 3.14 / 180: qpos[2] = 0
+ if joint_arc[6] > -80 * 3.14 / 180: qpos[3] = 0
+
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+ if joint_arc[18] > -70 * 3.14 / 180: qpos[6] = 0
+ if joint_arc[18] > -70 * 3.14 / 180: qpos[7] = 0
+
+ qpos[8] = joint_arc[11] * -1
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+ if joint_arc[10] > -80 * 3.14 / 180: qpos[10] = 0
+ if joint_arc[10] > -80 * 3.14 / 180: qpos[11] = 0
+
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -1
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+ if joint_arc[14] > -80 * 3.14 / 180: qpos[14] = 0
+ if joint_arc[14] > -80 * 3.14 / 180: qpos[15] = 0
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 1
+ fast_limit = 5
+ max_vel = int(self.last_jointvelocity[i] * 1.5)
+ min_vel = int(self.last_jointvelocity[i] * 0.995)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 3 + 50
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 3 + 100
+ if target_vel > max_vel:
+ target_vel = max_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 3 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 200
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 3 + 200
+ if target_vel < min_vel:
+ target_vel = min_vel
+ else:
+ target_vel = min_vel
+ if min_vel < 150:
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+ self.g_jointvelocity[6] = 255
+ self.g_jointvelocity[7] = 255
+ self.g_jointvelocity[8] = 255
+ self.g_jointvelocity[9] = 255
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=25):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions[6:4] = [128, 128, 128, 128]
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1 - 0.4 # 侧摆
+ qpos[17] = joint_arc[20] * 2 - 0.2 # 旋转
+ qpos[18] = joint_arc[2] * -0.2 # 根部关节
+ qpos[19] = joint_arc[1] * -1 # 中部关节
+ qpos[20] = joint_arc[0] * -1 # 远端关节
+ if joint_arc[0] > -50 * 3.14 / 180:
+ qpos[18] = 0.2
+ qpos[19] = 0
+ qpos[20] = 0
+
+ # 食指 index
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+ if joint_arc[6] > -80 * 3.14 / 180: qpos[2] = 0
+ if joint_arc[6] > -80 * 3.14 / 180: qpos[3] = 0
+
+ # 小指 little
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+ if joint_arc[18] > -75 * 3.14 / 180: qpos[6] = 0
+ if joint_arc[18] > -75 * 3.14 / 180: qpos[7] = 0
+
+ # 中指 middle
+ qpos[8] = joint_arc[11] * 0
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+ if joint_arc[10] > -80 * 3.14 / 180: qpos[10] = 0
+ if joint_arc[10] > -80 * 3.14 / 180: qpos[11] = 0
+
+ # 无名指 ring
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -1 # gen
+ qpos[14] = joint_arc[13] * -1 # zhong
+ qpos[15] = joint_arc[12] * -1
+ if joint_arc[14] > -80 * 3.14 / 180: qpos[14] = 0
+ if joint_arc[14] > -80 * 3.14 / 180: qpos[15] = 0
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 1
+ fast_limit = 5
+ max_vel = int(self.last_jointvelocity[i] * 1.5)
+ min_vel = int(self.last_jointvelocity[i] * 0.995)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 3 + 50
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 3 + 100
+ if target_vel > max_vel:
+ target_vel = max_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 3 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 200
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 3 + 200
+ if target_vel < min_vel:
+ target_vel = min_vel
+ else:
+ target_vel = min_vel
+ if min_vel < 150:
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+ self.g_jointvelocity[6] = 255
+ self.g_jointvelocity[7] = 255
+ self.g_jointvelocity[8] = 255
+ self.g_jointvelocity[9] = 255
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_l6.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_l6.py
new file mode 100644
index 0000000..81715cd
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_l6.py
@@ -0,0 +1,181 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1.2 # 侧摆
+ qpos[17] = joint_arc[20] * 2.4 # 旋转
+ qpos[18] = joint_arc[2] * -0.3878 # 根部关节
+ qpos[19] = joint_arc[1] * -0.66845 # 中部关节
+ qpos[20] = joint_arc[0] * -0.8 # 远端关节
+
+ qpos[0] = joint_arc[7]
+ qpos[1] = joint_arc[6] * -0.7
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19]
+ qpos[5] = joint_arc[18] * -0.7
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ qpos[8] = joint_arc[11]
+ qpos[9] = joint_arc[10] * -1.0098
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ qpos[12] = joint_arc[15]
+ qpos[13] = joint_arc[14] * -0.7
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1 # 侧摆
+ qpos[17] = joint_arc[20] * 1.5 # 旋转
+ qpos[18] = joint_arc[2] * -0.9 # 根部关节
+ qpos[19] = joint_arc[1] * -0.8 # 中部关节
+ qpos[20] = joint_arc[0] * -0.8 # 远端关节
+
+ # 食指 index
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -0.8
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ # 小指 little
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -0.8
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ # 中指 middle
+ qpos[8] = joint_arc[11] * -1.0098
+ qpos[9] = joint_arc[10] * -0.8
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ # 无名指 ring
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -0.8 # gen
+ qpos[14] = joint_arc[13] * -1 # zhong
+ qpos[15] = joint_arc[12] * -1
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_l7.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_l7.py
new file mode 100644
index 0000000..7707080
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_l7.py
@@ -0,0 +1,181 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=7):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[20] * 1.2 # 侧摆
+ qpos[17] = joint_arc[20] * 2.4 # 旋转
+ qpos[18] = joint_arc[2] * -0.3878 # 根部关节
+ qpos[19] = joint_arc[1] * -0.66845 # 中部关节
+ qpos[20] = joint_arc[0] * -0.8 # 远端关节
+
+ qpos[0] = joint_arc[7]
+ qpos[1] = joint_arc[6] * -0.7
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ qpos[4] = joint_arc[19]
+ qpos[5] = joint_arc[18] * -0.7
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ qpos[8] = joint_arc[11]
+ qpos[9] = joint_arc[10] * -1.0098
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ qpos[12] = joint_arc[15]
+ qpos[13] = joint_arc[14] * -0.7
+ qpos[14] = joint_arc[13] * -1
+ qpos[15] = joint_arc[12] * -1
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=7):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[2] * -0.5 # 旋转
+ qpos[17] = joint_arc[20] * 0.8 # 侧摆
+ qpos[18] = joint_arc[2] * -0.9 # 根部关节
+ qpos[19] = joint_arc[1] * -0.8 # 中部关节
+ qpos[20] = joint_arc[0] * -0.5 # 远端关节
+
+ # 食指 index
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+
+ # 小指 little
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+
+ # 中指 middle
+ qpos[8] = joint_arc[11] * -1.0098
+ qpos[9] = joint_arc[10] * -1
+ qpos[10] = joint_arc[9] * -1
+ qpos[11] = joint_arc[8] * -1
+
+ # 无名指 ring
+ qpos[12] = joint_arc[15] * -1
+ qpos[13] = joint_arc[14] * -1 # gen
+ qpos[14] = joint_arc[13] * -1 # zhong
+ qpos[15] = joint_arc[12] * -1
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_o6.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_o6.py
new file mode 100644
index 0000000..6cc6628
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/hand/udexreal_o6.py
@@ -0,0 +1,256 @@
+import numpy as np
+import copy
+from linkerhand.handcore import HandCore
+from ..config.o6_config import FINGER_CONFIGS, MAPPING_ORDER
+from typing import List
+from linkerhand.handcoreex import MultiStateLinearMapper
+
+
+# 修正 RightHand 类
+class RightHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions_arc = [0] * length
+ self.g_jointvelocity_arc = [0] * length
+ self.handstate = [0] * length
+ self.calibrationopose = None
+ self.calibrationfistpose = None
+ self.calibrationoriginal = None
+
+ # 机械手预设姿势
+ self.robot_original = handcore.hand_lower_limits_l
+ self.robot_opose = [1.1, 0.37, 0.0, 0.82, 0.0, 0.82, 0.0, 0.82, 0.0, 0.82, 0.0]
+ self.robot_fist = handcore.hand_upper_limits_l
+
+ # 映射器
+ self.multi_state_mapper = MultiStateLinearMapper(FINGER_CONFIGS, MAPPING_ORDER)
+
+ def initialize_mapper(self) -> bool:
+ """初始化映射器"""
+ glove_original = self._to_list(self.calibrationoriginal)
+ glove_fist = self._to_list(self.calibrationfistpose)
+ glove_opose = self._to_list(self.calibrationopose)
+
+ self.multi_state_mapper.add_state('original', glove_original, self.robot_original)
+ self.multi_state_mapper.add_state('opose', glove_opose, self.robot_opose)
+ self.multi_state_mapper.add_state('fist', glove_fist, self.robot_fist)
+
+ # 设置状态顺序
+ self.multi_state_mapper.set_state_order(['original', 'opose', 'fist'])
+
+
+ def _to_list(self, data):
+ """转换为列表"""
+ if hasattr(data, 'tolist'):
+ return data.tolist()
+ elif isinstance(data, np.ndarray):
+ return data.tolist()
+ else:
+ return list(data)
+
+ def joint_arc_update(self, joint_arc: List[float]):
+ """映射手套数据到机械手"""
+ qpos = np.zeros(25)
+ arc_value = self.multi_state_mapper.map_glove_to_robot(joint_arc)
+ 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]
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ # 拇指处理
+ qpos[16] = joint_arc[1] * 1.2
+ qpos[17] = joint_arc[0] * -2 + joint_arc[1] * 1.2
+ qpos[20] = joint_arc[4] * 0.5 + joint_arc[2] * 0.8
+
+ qpos[1] = joint_arc[6] * 0.1 + joint_arc[8] * 0.7
+ qpos[9] = joint_arc[10] * 0.1 + joint_arc[12] * 0.7
+ qpos[13] = joint_arc[14] * 0.1 + joint_arc[16] * 0.7
+ qpos[5] = joint_arc[18] * 0.1 + joint_arc[20] * 0.7
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+# LeftHand 类类似修正
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.g_jointpositions_arc = [0] * length
+ self.g_jointvelocity_arc = [0] * length
+ self.handstate = [0] * length
+ self.calibrationopose = None
+ self.calibrationfistpose = None
+ self.calibrationoriginal = None
+
+ # 机械手预设姿势
+ self.robot_original = handcore.hand_lower_limits_l
+ self.robot_opose = [1.1, 0.37, 0.0, 0.82, 0.0, 0.82, 0.0, 0.82, 0.0, 0.82, 0.0]
+ self.robot_fist = handcore.hand_upper_limits_l
+
+ # 使用多态映射器（支持更多手势）
+ self.multi_state_mapper = MultiStateLinearMapper(FINGER_CONFIGS, MAPPING_ORDER)
+
+
+ def initialize_mapper(self) -> bool:
+ """初始化映射器"""
+ glove_original = self._to_list(self.calibrationoriginal)
+ glove_fist = self._to_list(self.calibrationfistpose)
+ glove_opose = self._to_list(self.calibrationopose)
+
+ self.multi_state_mapper.add_state('original', glove_original, self.robot_original)
+ self.multi_state_mapper.add_state('opose', glove_opose, self.robot_opose)
+ self.multi_state_mapper.add_state('fist', glove_fist, self.robot_fist)
+
+ glove_pinch = glove_original.copy()
+ glove_pinch[2:5] = [1.5, 1.2, 1.0] # 拇指弯曲明显
+ glove_pinch[0:2] = glove_original[0:2] # 拇指侧摆保持原始
+ robot_pinch = self.robot_original.copy()
+ robot_pinch[1] = 0.8 # 拇指弯曲加大
+ robot_pinch[3] = 0.6 # 食指轻微弯曲
+
+ self.multi_state_mapper.add_state('pinch', glove_pinch, robot_pinch)
+
+ # 设置状态顺序
+ self.multi_state_mapper.set_state_order(['original', 'opose', 'fist'])
+
+
+ def _to_list(self, data):
+ """转换为列表"""
+ if hasattr(data, 'tolist'):
+ return data.tolist()
+ elif isinstance(data, np.ndarray):
+ return data.tolist()
+ else:
+ return list(data)
+
+ def joint_arc_update(self, joint_arc: List[float]):
+ """映射手套数据到机械手"""
+ qpos = np.zeros(25)
+ arc_value = self.multi_state_mapper.map_glove_to_robot(joint_arc)
+ 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]
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ # 拇指处理
+ qpos[16] = joint_arc[1] * 1.2
+ qpos[17] = joint_arc[0] * -2 + joint_arc[1] * 1.2
+ qpos[20] = joint_arc[4] * 0.5 + joint_arc[2] * 0.8
+
+ qpos[1] = joint_arc[6] * 0.1 + joint_arc[8] * 0.7
+ qpos[9] = joint_arc[10] * 0.1 + joint_arc[12] * 0.7
+ qpos[13] = joint_arc[14] * 0.1 + joint_arc[16] * 0.7
+ qpos[5] = joint_arc[18] * 0.1 + joint_arc[20] * 0.7
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/retarget.py b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/retarget.py
new file mode 100644
index 0000000..24a653c
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/udexrealv2t/retarget.py
@@ -0,0 +1,442 @@
+
+import time
+import json
+import copy
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from datetime import datetime, timedelta
+from tqdm import tqdm
+from pathlib import Path
+from colorama import Fore, init
+from typing import Optional
+
+from ...linkerhand.udexrealcore import UdexRealScoketUdp, TimeoutStatus, UdexRealData
+from ...linkerhand.handcore import HandCore
+
+from ...linkerhand.constants import RobotName, ROBOT_LEN_MAP
+
+
+tmp_file_path = Path(__file__).parent / "tmp" / "jointangle_data.tmp"
+
+
+
+class CalibrationProgress:
+ """校准进度管理器"""
+ def __init__(self, duration: float = 3.0):
+ self.step_started = False
+ self.message = ""
+ self.start_time = 0
+ self.duration = duration
+ self.last_progress = -1
+
+ # 进度条配置
+ self.bar_length = 60 # 增加到60，占更多屏幕
+ self.filled_char = '█'
+ self.empty_char = '░'
+
+ # 状态跟踪
+ self.message_printed = False
+
+ def start_step(self, message: str, duration: Optional[float] = None):
+ """开始新的校准步骤"""
+ # 重置状态
+ self.step_started = True
+ self.message = message
+ self.start_time = time.time()
+ self.last_progress = -1
+ self.message_printed = False
+
+ if duration is not None:
+ self.duration = duration
+
+ # 只在开始时打印一次消息
+ if not self.message_printed:
+ self.message_printed = True
+
+ def update_progress(self) -> tuple[bool, int]:
+ """
+ 更新并显示进度
+
+ Returns:
+ tuple[bool, int]: (是否完成, 当前进度百分比)
+ """
+ if not self.step_started:
+ return False, 0
+
+ elapsed = time.time() - self.start_time
+ progress = min(100, int((elapsed / self.duration) * 100))
+
+ # 只在进度有变化时更新显示
+ if progress != self.last_progress:
+ self.last_progress = progress
+
+ # 计算填充长度
+ filled = int(self.bar_length * progress // 100)
+
+ # 构建进度条
+ if progress < 100:
+ bar = self.filled_char * filled + self.empty_char * (self.bar_length - filled)
+ else:
+ # 100%时显示完整条
+ bar = self.filled_char * self.bar_length
+
+ # 使用 \r 和 \033[K 确保完全覆盖
+ print(f'\r\033[K{self.message} |{bar}| {progress:3d}%', end='', flush=True)
+
+ # 检查是否完成
+ is_completed = elapsed >= self.duration
+ if is_completed:
+ self.step_started = False
+ # 完成后不在这里换行，让调用者控制
+
+ return is_completed, progress
+
+ def get_progress_info(self) -> dict:
+ """获取进度信息"""
+ if not self.step_started:
+ return {"elapsed": 0, "progress": 0, "remaining": 0}
+
+ elapsed = time.time() - self.start_time
+ progress = min(1.0, elapsed / self.duration)
+
+ return {
+ "elapsed": elapsed,
+ "progress": progress,
+ "remaining": max(0, self.duration - elapsed),
+ "progress_percent": int(progress * 100)
+ }
+
+ def reset(self):
+ """重置状态"""
+ self.step_started = False
+ self.message = ""
+ self.start_time = 0
+ self.last_progress = -1
+ self.message_printed = False
+
+ def is_active(self) -> bool:
+ """检查是否正在运行"""
+ return self.step_started
+
+ def stop(self, success: bool = True, final_message: Optional[str] = None):
+ """停止进度条"""
+ if not self.step_started:
+ return
+
+ self.step_started = False
+
+ if success:
+ if final_message:
+ print(f"\r\033[K✅ {final_message}")
+ else:
+ print(f"\r\033[K✅ {self.message} - 完成")
+ else:
+ if final_message:
+ print(f"\r\033[K❌ {final_message}")
+ else:
+ print(f"\r\033[K❌ {self.message} - 失败")
+
+
+class Retarget():
+ def __init__(self,node, ip, port, deviceid, lefthand: RobotName, righthand: RobotName, handcore: HandCore,
+ lefthandpubprint: bool, righthandpubprint: bool,calibration :bool = False):
+ self.node = node
+ self.udp_ip = ip
+ self.udp_port = port
+ self.motion_device = deviceid
+ self.lefthandtype = lefthand
+ self.righthandtype = righthand
+ self.handcore = handcore
+ self.runing = True
+ self.lefthandpubprint = lefthandpubprint
+ self.righthandpubprint = righthandpubprint
+ self.calibration = calibration
+
+ # 根据右手类型初始化
+ if self.righthandtype == RobotName.o7 \
+ or self.righthandtype == RobotName.l7 \
+ or self.righthandtype == RobotName.o7v1 \
+ or self.righthandtype == RobotName.o7v3:
+ from .hand.udexreal_l7 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.o6:
+ from .hand.udexreal_o6 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l6:
+ from .hand.udexreal_l6 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ # elif self.righthandtype == RobotName.l25:
+ # from .hand.udexreal_l25 import RightHand
+ # self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ # elif self.righthandtype == RobotName.t25:
+ # from .hand.udexreal_t25 import RightHand
+ # self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l20:
+ from .hand.udexreal_l20 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l10 \
+ or self.righthandtype == RobotName.l10v7 :
+ from .hand.udexreal_l10v7 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l21:
+ from .hand.udexreal_l21 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+
+ # 根据LEFT手类型初始化
+ if self.lefthandtype == RobotName.o7 \
+ or self.lefthandtype == RobotName.l7 \
+ or self.lefthandtype == RobotName.o7v1 \
+ or self.lefthandtype == RobotName.o7v3:
+ from .hand.udexreal_l7 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.o6:
+ from .hand.udexreal_o6 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l6:
+ from .hand.udexreal_l6 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l25:
+ from .hand.udexreal_l25 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ # elif self.lefthandtype == RobotName.t25:
+ # from .hand.udexreal_t25 import LeftHand
+ # self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l20:
+ from .hand.udexreal_l20 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l10 \
+ or self.lefthandtype == RobotName.l10v7 :
+ from .hand.udexreal_l10v7 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l21:
+ from .hand.udexreal_l21 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+
+ # ROS2 发布器
+ self.publisher_r = self.node.create_publisher(
+ JointState,
+ '/cb_right_hand_control_cmd',
+ self.handcore.hand_numjoints_r)
+
+ self.publisher_l = self.node.create_publisher(
+ JointState,
+ '/cb_left_hand_control_cmd',
+ self.handcore.hand_numjoints_l)
+
+ self.timer = self.node.create_timer(1.0/120, self.process_callback) # 120Hz
+ self.pubprintcount = 0
+ self.udp_datacapture = None
+
+ self.calibration_progress = CalibrationProgress(duration=3.0)
+
+ def initialize_udp(self):
+ """初始化UDP连接"""
+ self.udp_datacapture = UdexRealScoketUdp(
+ host=self.udp_ip,
+ port=self.udp_port,
+ device_id=self.motion_device)
+ self.udp_datacapture.set_timeout_callback(self.on_timeout_callback)
+ self.udp_datacapture.set_data_recovered_callback(self.on_data_recovered_callback)
+ return self.udp_datacapture.udp_initial()
+
+ def on_timeout_callback(self, status: TimeoutStatus):
+ """超时回调函数"""
+ # if status.consecutive_timeout_checks == 1: # 第一次超时
+ # print(f"[{datetime.now().strftime('%Y-%m-%d %H:%M:%S')}] "
+ # f"警告: 数据接收超时，{status.time_since_last_data:.1f}秒未收到数据")
+ if status.consecutive_timeout_checks % 1 == 0: # 每10次检查打印一次
+ print(f"[{datetime.now().strftime('%Y-%m-%d %H:%M:%S')}] "
+ f"警告: 数据接收超时，{status.time_since_last_data:.1f}秒未收到数据")
+
+ def on_data_recovered_callback(self):
+ """数据恢复回调函数"""
+ print(f"[{datetime.now().strftime('%Y-%m-%d %H:%M:%S')}] 数据连接已恢复")
+
+
+ def process_callback(self):
+ if not self.runing:
+ return
+
+ mocapdata = self.udp_datacapture.realmocapdata
+
+ # 检查数据是否更新
+ if not mocapdata.is_update:
+ return
+
+ if self.calibration != -1:
+ step_info = {
+ None: ("请并拢五指...", 0),
+ 0: ("请做握拳姿势...", 1),
+ 1: ("请做O型姿势...", -1)
+ }.get(self.calibration)
+
+ if not step_info:
+ return
+
+ message, next_step = step_info
+
+ # 启动或更新进度条
+ if not self.calibration_progress.is_active():
+ self.calibration_progress.start_step(message)
+
+ # 更新显示
+ is_completed, _ = self.calibration_progress.update_progress()
+
+ if is_completed:
+ print() # 换行
+ if self.calibration is None:
+ # 第一次校准：并拢五指姿势
+ self.righthand.calibrationoriginal = mocapdata.jointangle_rHand.copy()
+ self.lefthand.calibrationoriginal = mocapdata.jointangle_lHand.copy()
+ self.calibration = 0
+
+ elif self.calibration == 0:
+ # 第二次校准：握拳姿势
+ self.righthand.calibrationfistpose = mocapdata.jointangle_rHand.copy()
+ self.lefthand.calibrationfistpose = mocapdata.jointangle_lHand.copy()
+ self.calibration = 1
+
+ elif self.calibration == 1:
+ # 第三次校准：O型姿势
+ self.righthand.calibrationopose = mocapdata.jointangle_rHand.copy()
+ self.lefthand.calibrationopose = mocapdata.jointangle_lHand.copy()
+ self.calibration = -1
+
+ # 完成所有校准步骤
+ self._save_to_tmp()
+ self.righthand.initialize_mapper()
+ self.lefthand.initialize_mapper()
+ print("✅ 校准完成！")
+
+ # 重置进度条，准备下一步或结束
+ self.calibration_progress.reset()
+
+ return
+
+ # 处理左右手原始数据
+ self.lefthand.joint_arc_update(mocapdata.jointangle_lHand)
+ self.righthand.joint_arc_update(mocapdata.jointangle_rHand)
+
+ # 速度环节处理
+ self.lefthand.speed_update()
+ self.righthand.speed_update()
+
+ # 调试打印
+ if self.lefthandpubprint and self.pubprintcount % 1 == 0:
+ self.node.get_logger().info(f"左手位置: {self.lefthand.g_jointpositions}")
+ if self.righthandpubprint and self.pubprintcount % 1 == 0:
+ self.node.get_logger().info(f"右手位置: {self.righthand.g_jointpositions}")
+
+ # 发布右手数据
+ msg_r = JointState()
+ msg_r.header.stamp = self.node.get_clock().now().to_msg()
+ msg_r.name = [f'joint{i + 1}' for i in range(len(self.righthand.g_jointpositions))]
+ msg_r.position = [float(num) for num in self.righthand.g_jointpositions]
+ msg_r.velocity = [float(num) for num in self.righthand.g_jointvelocity]
+ self.publisher_r.publish(msg_r)
+
+ # 发布左手数据
+ msg_l = JointState()
+ msg_l.header.stamp = self.node.get_clock().now().to_msg()
+ msg_l.name = [f'joint{i + 1}' for i in range(len(self.lefthand.g_jointpositions))]
+ msg_l.position = [float(num) for num in self.lefthand.g_jointpositions]
+ msg_l.velocity = [float(num) for num in self.lefthand.g_jointvelocity]
+ self.publisher_l.publish(msg_l)
+
+ self.pubprintcount += 1
+
+ def _save_to_tmp(self):
+ """
+ 保存 jointangle_r 数据和时间戳到临时文件
+ """
+ newdata = {
+ "timestamp": datetime.now().isoformat(), # 当前时间
+ "jointangleoriginal_r": self.righthand.calibrationoriginal,
+ "jointangleoriginal_l": self.lefthand.calibrationoriginal,
+ "jointanglefist_r": self.righthand.calibrationfistpose,
+ "jointanglefist_l": self.lefthand.calibrationfistpose,
+ "jointangleopose_r": self.righthand.calibrationopose,
+ "jointangleopose_l": self.lefthand.calibrationopose
+ }
+ try:
+ tmp_file_path.parent.mkdir(parents=True, exist_ok=True)
+ if tmp_file_path.exists():
+ content = tmp_file_path.read_text()
+ historydata = json.loads(content)
+ # if self.force_reader_left.handtype != 'Left':
+ # # 左手数据不存在
+ # newdata["jointangleoriginal_l"] = historydata["jointangleoriginal_l"]
+ # newdata["jointanglefist_l"] = historydata["jointanglefist_l"]
+ # newdata["jointangleopose_l"] = historydata["jointangleopose_l"]
+ # if self.force_reader_right.handtype != 'Right':
+ # newdata["jointangleoriginal_r"] = historydata["jointangleoriginal_r"]
+ # newdata["jointanglefist_r"] = historydata["jointanglefist_r"]
+ # newdata["jointangleopose_r"] = historydata["jointangleopose_r"]
+ json_str = json.dumps(newdata, indent=2)
+ tmp_file_path.write_text(json_str)
+ print("保存成功")
+ return True
+ except Exception as e:
+ print(f"保存失败: {e}")
+ return False
+
+ def _load_from_tmp(self):
+ """
+ 从临时文件读取数据，检查时间戳有效性
+ - 如果文件不存在、时间戳为空或超过1小时，返回 False
+ - 否则返回 jointangle_r 数据
+ """
+ if not tmp_file_path.exists():
+ print("文件不存在")
+ return False
+ try:
+ content = tmp_file_path.read_text()
+ data = json.loads(content)
+ except Exception as e:
+ print(f"读取失败: {e}")
+ return False
+
+ # 检查时间戳
+ if 'timestamp' not in data or not data['timestamp']:
+ print("无效的时间戳...")
+ return False
+
+ # 检查是否超过8小时
+ saved_time = datetime.fromisoformat(data['timestamp'])
+ current_time = datetime.now()
+
+ # 计算时间差
+ time_diff = current_time - saved_time
+ if time_diff > timedelta(hours=8):
+ print("标定数据有效期不足8小时,暂不进行标定流程...")
+ try:
+ self.righthand.calibrationoriginal = data['jointangleoriginal_r']
+ self.lefthand.calibrationoriginal = data['jointangleoriginal_l']
+ self.righthand.calibrationfistpose = data['jointanglefist_r']
+ self.lefthand.calibrationfistpose = data['jointanglefist_l']
+ self.righthand.calibrationopose = data['jointangleopose_r']
+ self.lefthand.calibrationopose = data['jointangleopose_l']
+ except Exception as e:
+ print(f"标定数据异常: {e}")
+ return False
+ return True
+
+
+ def process(self):
+ """主处理函数"""
+ if not self.initialize_udp():
+ self.node.get_logger().error("初始化配置网络失败")
+ return
+
+ if self.calibration is True:
+ self.calibration = None
+ else:
+ if self._load_from_tmp() is True:
+ print("跳过标定流程")
+ self.calibration = -1
+ self.righthand.initialize_mapper()
+ self.lefthand.initialize_mapper()
+ else:
+ self.calibration = None
+
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/README.md b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/README.md
new file mode 100644
index 0000000..c3ade4f
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/README.md
@@ -0,0 +1,63 @@
+# VTR-DYN Data Glove Module
+
+VTR-DYN data glove ROS2 driver module, receives glove data via UDP and publishes to ROS2 topics.
+
+## Features
+
+- Receives glove data via UDP protocol
+- Supports left and right hand data
+- 50Hz publish rate
+
+## Usage
+
+### 1. Config File
+
+Edit `config/base_config.yml`:
+
+```yaml
+system:
+ motion_type: vtrdyn
+
+udp:
+ ip: "0.0.0.0"
+ port: 8888
+```
+
+### 2. Launch
+
+```bash
+ros2 run linkerhand_retarget handretarget
+```
+
+## ROS2 Topics
+
+### Published Topics
+
+| Topic | Message Type | Description |
+|-------|-------------|-------------|
+| `/cb_right_hand_control_cmd` | `sensor_msgs/JointState` | Right hand control data |
+| `/cb_left_hand_control_cmd` | `sensor_msgs/JointState` | Left hand control data |
+
+### Message Format
+
+```python
+# sensor_msgs/JointState
+msg.header.stamp # Timestamp
+msg.name # Joint name list
+msg.position # Joint position values
+```
+
+## File Structure
+
+```
+motion/vtrdyn/
+├── __init__.py
+├── vtrdyncore.py # UDP communication core
+├── retarget.py # ROS2 integration
+└── README.md # English documentation
+```
+
+## Notes
+
+1. Ensure UDP port is not occupied
+2. Glove and host must be on the same network
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/README_zh.md b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/README_zh.md
new file mode 100644
index 0000000..541d383
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/README_zh.md
@@ -0,0 +1,63 @@
+# VTR-DYN 数据手套模块
+
+VTR-DYN 数据手套的 ROS2 驱动模块，通过 UDP 接收手套数据并发布到 ROS2 话题。
+
+## 特性
+
+- 通过 UDP 协议接收手套数据
+- 支持左右手数据
+- 发布频率 50Hz
+
+## 使用方法
+
+### 1. 配置文件
+
+修改 `config/base_config.yml`:
+
+```yaml
+system:
+ motion_type: vtrdyn
+
+udp:
+ ip: "0.0.0.0"
+ port: 8888
+```
+
+### 2. 运行
+
+```bash
+ros2 run linkerhand_retarget handretarget
+```
+
+## ROS2 话题
+
+### 发布话题
+
+| 话题名 | 消息类型 | 说明 |
+|-------|---------|------|
+| `/cb_right_hand_control_cmd` | `sensor_msgs/JointState` | 右手控制数据 |
+| `/cb_left_hand_control_cmd` | `sensor_msgs/JointState` | 左手控制数据 |
+
+### 消息格式
+
+```python
+# sensor_msgs/JointState
+msg.header.stamp # 时间戳
+msg.name # 关节名称列表
+msg.position # 关节位置值
+```
+
+## 文件结构
+
+```
+motion/vtrdyn/
+├── __init__.py
+├── vtrdyncore.py # UDP 通讯核心
+├── retarget.py # ROS2 集成
+└── README.md # 本文档
+```
+
+## 注意事项
+
+1. 确保 UDP 端口未被占用
+2. 手套与主机需在同一网络
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/__init__.py b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/__init__.py b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_l10v7.py b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_l10v7.py
new file mode 100644
index 0000000..05deae2
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_l10v7.py
@@ -0,0 +1,179 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=10):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ # if joint_arc[0] * 1.2 > 0.5:
+ # qpos[15] = joint_arc[0] * 0.8
+ # else:
+ qpos[16] = joint_arc[0] * 0.4
+ qpos[17] = joint_arc[1] * 0.8
+ qpos[18] = joint_arc[2]
+ qpos[19] = joint_arc[3]
+ qpos[20] = joint_arc[4]
+
+ qpos[0] = joint_arc[5] * -1.2
+ qpos[1] = joint_arc[7]
+ qpos[2] = joint_arc[8]
+ qpos[3] = joint_arc[9]
+
+ qpos[4] = joint_arc[20] * 2
+ qpos[5] = joint_arc[22]
+ qpos[6] = joint_arc[23]
+ qpos[7] = joint_arc[24]
+
+ qpos[8] = joint_arc[10]
+ qpos[9] = joint_arc[12]
+ qpos[10] = joint_arc[13]
+ qpos[11] = joint_arc[14]
+
+ qpos[12] = joint_arc[15] * 1.5
+ qpos[13] = joint_arc[17]
+ qpos[14] = joint_arc[18]
+ qpos[15] = joint_arc[19]
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ self.g_jointvelocity[i] = 255
+ # lastpos = self.last_jointpositions[i]
+ # position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ # position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ # slow_limit = 4
+ # fast_limit = 10
+ # max_vel = int(self.last_jointvelocity[i] * 2)
+ # mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ # min_vel = int(self.last_jointvelocity[i] * 0.5)
+ # target_vel = self.last_jointvelocity[i]
+ # if self.handstate[i] == 0: # stop
+ # if 0 < position_error:
+ # target_vel = position_error * 5 + 30
+ # self.handstate[i] = 1
+ # elif self.handstate[i] == 1: # slow
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 50
+ # if target_vel > mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 2
+ # elif position_error == 0:
+ # self.handstate[i] = 0
+ # target_vel = position_error * 5 + 100
+ # else:
+ # target_vel = position_error * 5 + 100
+ # else: # fast
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 90
+ # if target_vel > max_vel:
+ # target_vel = max_vel
+ # elif slow_limit < position_error < fast_limit:
+ # target_vel = position_error * 5 + 60
+ # if target_vel < mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 3
+ # elif 0 < position_error <= slow_limit:
+ # target_vel = position_error * 5 + 40
+ # if target_vel < min_vel:
+ # target_vel = min_vel
+ # self.handstate[i] = 1
+ # self.g_jointvelocity[i] = int(target_vel * 1)
+ # if self.g_jointvelocity[i] > 255:
+ # self.g_jointvelocity[i] = 255
+ # self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ # self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=10):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[0] * 0.6
+ qpos[17] = joint_arc[1] * 0.6
+ qpos[18] = joint_arc[2]
+ qpos[19] = joint_arc[3]
+ qpos[20] = joint_arc[4]
+
+ qpos[0] = joint_arc[5] * 1.2
+ qpos[1] = joint_arc[7]
+ qpos[2] = joint_arc[8]
+ qpos[3] = joint_arc[9]
+
+ qpos[4] = joint_arc[20] * -2
+ qpos[5] = joint_arc[22]
+ qpos[6] = joint_arc[23]
+ qpos[7] = joint_arc[24]
+
+ qpos[8] = joint_arc[10]
+ qpos[9] = joint_arc[12]
+ qpos[10] = joint_arc[13]
+ qpos[11] = joint_arc[14]
+
+ qpos[12] = joint_arc[15] * -1.5
+ qpos[13] = joint_arc[17]
+ qpos[14] = joint_arc[18]
+ qpos[15] = joint_arc[19]
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ self.g_jointvelocity[i] = 255
+ # lastpos = self.last_jointpositions[i]
+ # position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ # position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ # slow_limit = 4
+ # fast_limit = 10
+ # max_vel = int(self.last_jointvelocity[i] * 2)
+ # mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ # min_vel = int(self.last_jointvelocity[i] * 0.5)
+ # target_vel = self.last_jointvelocity[i]
+ # if self.handstate[i] == 0: # stop
+ # if 0 < position_error:
+ # target_vel = position_error * 5 + 30
+ # self.handstate[i] = 1
+ # elif self.handstate[i] == 1: # slow
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 50
+ # if target_vel > mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 2
+ # elif position_error == 0:
+ # self.handstate[i] = 0
+ # target_vel = position_error * 5 + 100
+ # else:
+ # target_vel = position_error * 5 + 100
+ # else: # fast
+ # if position_error >= fast_limit:
+ # target_vel = position_error * 5 + 90
+ # if target_vel > max_vel:
+ # target_vel = max_vel
+ # elif slow_limit < position_error < fast_limit:
+ # target_vel = position_error * 5 + 60
+ # if target_vel < mid_vel:
+ # target_vel = mid_vel
+ # self.handstate[i] = 3
+ # elif 0 < position_error <= slow_limit:
+ # target_vel = position_error * 5 + 40
+ # if target_vel < min_vel:
+ # target_vel = min_vel
+ # self.handstate[i] = 1
+ # self.g_jointvelocity[i] = int(target_vel * 1)
+ # if self.g_jointvelocity[i] > 255:
+ # self.g_jointvelocity[i] = 255
+ # self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ # self.last_jointpositions[i] = self.g_jointpositions[i]
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_l20.py b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_l20.py
new file mode 100644
index 0000000..6fd6e82
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_l20.py
@@ -0,0 +1,177 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=20):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[0] * 0.5
+ qpos[17] = joint_arc[1] * 0.4
+ qpos[18] = joint_arc[2] * 1.5
+ qpos[19] = joint_arc[3]
+ qpos[20] = joint_arc[4]
+
+ qpos[0] = joint_arc[5] * -1.2
+ qpos[1] = joint_arc[7]
+ qpos[2] = joint_arc[8]
+ qpos[3] = joint_arc[9]
+
+ qpos[4] = joint_arc[20] * 2
+ qpos[5] = joint_arc[22]
+ qpos[6] = joint_arc[23]
+ qpos[7] = joint_arc[24]
+
+ qpos[8] = joint_arc[10]
+ qpos[9] = joint_arc[12]
+ qpos[10] = joint_arc[13]
+ qpos[11] = joint_arc[14]
+
+ qpos[12] = joint_arc[15] * 1.5
+ qpos[13] = joint_arc[17]
+ qpos[14] = joint_arc[18]
+ qpos[15] = joint_arc[19]
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=20):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[0] * 0.3
+ qpos[17] = joint_arc[1] * 0.8
+ qpos[18] = joint_arc[2] * 0.5
+ qpos[19] = joint_arc[4] * 1
+ qpos[20] = joint_arc[4]
+
+ qpos[0] = joint_arc[5] * -1.2
+ qpos[1] = joint_arc[7] * 0.7
+ qpos[2] = joint_arc[8] * 0.6
+ qpos[3] = joint_arc[9]
+
+ qpos[4] = joint_arc[20] * 2
+ qpos[5] = joint_arc[22] * 0.7
+ qpos[6] = joint_arc[23] * 0.6
+ qpos[7] = joint_arc[24]
+
+ qpos[8] = joint_arc[10]
+ qpos[9] = joint_arc[12] * 0.7
+ qpos[10] = joint_arc[13] * 0.6
+ qpos[11] = joint_arc[14]
+
+ qpos[12] = joint_arc[15] * 1.5
+ qpos[13] = joint_arc[17] * 0.7
+ qpos[14] = joint_arc[18] * 0.6
+ qpos[15] = joint_arc[19]
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_l21.py b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_l21.py
new file mode 100644
index 0000000..c857659
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_l21.py
@@ -0,0 +1,173 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=25):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+
+ qpos[16] = joint_arc[0]
+ qpos[17] = joint_arc[1] * 0.5
+ qpos[18] = joint_arc[2] * 0.5
+ qpos[19] = joint_arc[3]
+ qpos[20] = joint_arc[4]
+
+ qpos[0] = joint_arc[5] * 1.2
+ qpos[1] = joint_arc[7]
+ qpos[2] = joint_arc[8]
+ qpos[3] = joint_arc[9]
+
+ qpos[4] = joint_arc[20] * -1.2
+ qpos[5] = joint_arc[22]
+ qpos[6] = joint_arc[23]
+ qpos[7] = joint_arc[24]
+
+ qpos[8] = joint_arc[10]
+ qpos[9] = joint_arc[12]
+ qpos[10] = joint_arc[13]
+ qpos[11] = joint_arc[14]
+
+ qpos[12] = joint_arc[15] * -1.5
+ qpos[13] = joint_arc[17]
+ qpos[14] = joint_arc[18]
+ qpos[15] = joint_arc[19]
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 1
+ fast_limit = 5
+ max_vel = int(self.last_jointvelocity[i] * 1.5)
+ min_vel = int(self.last_jointvelocity[i] * 0.995)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 3 + 50
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 3 + 100
+ if target_vel > max_vel:
+ target_vel = max_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 3 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 200
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 3 + 200
+ if target_vel < min_vel:
+ target_vel = min_vel
+ else:
+ target_vel = min_vel
+ if min_vel < 150:
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+ self.g_jointvelocity[6] = 255
+ self.g_jointvelocity[7] = 255
+ self.g_jointvelocity[8] = 255
+ self.g_jointvelocity[9] = 255
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=25):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[0]
+ qpos[17] = joint_arc[1] * 0.6
+ qpos[18] = joint_arc[2] * 0.5
+ qpos[19] = joint_arc[4]
+ qpos[20] = joint_arc[4]
+
+ qpos[0] = joint_arc[5] * 1.2
+ qpos[1] = joint_arc[7] *1.1
+ qpos[2] = joint_arc[8] *1.1
+ qpos[3] = joint_arc[9]
+
+ qpos[4] = joint_arc[20] * -1.2
+ qpos[5] = joint_arc[22] *1.1
+ qpos[6] = joint_arc[23] *1.1
+ qpos[7] = joint_arc[24]
+
+ qpos[8] = joint_arc[10]
+ qpos[9] = joint_arc[12] *1.1
+ qpos[10] = joint_arc[13] *1.1
+ qpos[11] = joint_arc[14]
+
+ qpos[12] = joint_arc[15] * -1.5
+ qpos[13] = joint_arc[17] *1.1
+ qpos[14] = joint_arc[18] *1.1
+ qpos[15] = joint_arc[19]
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 1
+ fast_limit = 5
+ max_vel = int(self.last_jointvelocity[i] * 1.5)
+ min_vel = int(self.last_jointvelocity[i] * 0.995)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 3 + 50
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 3 + 100
+ if target_vel > max_vel:
+ target_vel = max_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 3 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 200
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 3 + 200
+ if target_vel < min_vel:
+ target_vel = min_vel
+ else:
+ target_vel = min_vel
+ if min_vel < 150:
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+ self.g_jointvelocity[6] = 255
+ self.g_jointvelocity[7] = 255
+ self.g_jointvelocity[8] = 255
+ self.g_jointvelocity[9] = 255
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_l25.py b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_l25.py
new file mode 100644
index 0000000..c857659
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_l25.py
@@ -0,0 +1,173 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=25):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+
+ qpos[16] = joint_arc[0]
+ qpos[17] = joint_arc[1] * 0.5
+ qpos[18] = joint_arc[2] * 0.5
+ qpos[19] = joint_arc[3]
+ qpos[20] = joint_arc[4]
+
+ qpos[0] = joint_arc[5] * 1.2
+ qpos[1] = joint_arc[7]
+ qpos[2] = joint_arc[8]
+ qpos[3] = joint_arc[9]
+
+ qpos[4] = joint_arc[20] * -1.2
+ qpos[5] = joint_arc[22]
+ qpos[6] = joint_arc[23]
+ qpos[7] = joint_arc[24]
+
+ qpos[8] = joint_arc[10]
+ qpos[9] = joint_arc[12]
+ qpos[10] = joint_arc[13]
+ qpos[11] = joint_arc[14]
+
+ qpos[12] = joint_arc[15] * -1.5
+ qpos[13] = joint_arc[17]
+ qpos[14] = joint_arc[18]
+ qpos[15] = joint_arc[19]
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 1
+ fast_limit = 5
+ max_vel = int(self.last_jointvelocity[i] * 1.5)
+ min_vel = int(self.last_jointvelocity[i] * 0.995)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 3 + 50
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 3 + 100
+ if target_vel > max_vel:
+ target_vel = max_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 3 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 200
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 3 + 200
+ if target_vel < min_vel:
+ target_vel = min_vel
+ else:
+ target_vel = min_vel
+ if min_vel < 150:
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+ self.g_jointvelocity[6] = 255
+ self.g_jointvelocity[7] = 255
+ self.g_jointvelocity[8] = 255
+ self.g_jointvelocity[9] = 255
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=25):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[0]
+ qpos[17] = joint_arc[1] * 0.6
+ qpos[18] = joint_arc[2] * 0.5
+ qpos[19] = joint_arc[4]
+ qpos[20] = joint_arc[4]
+
+ qpos[0] = joint_arc[5] * 1.2
+ qpos[1] = joint_arc[7] *1.1
+ qpos[2] = joint_arc[8] *1.1
+ qpos[3] = joint_arc[9]
+
+ qpos[4] = joint_arc[20] * -1.2
+ qpos[5] = joint_arc[22] *1.1
+ qpos[6] = joint_arc[23] *1.1
+ qpos[7] = joint_arc[24]
+
+ qpos[8] = joint_arc[10]
+ qpos[9] = joint_arc[12] *1.1
+ qpos[10] = joint_arc[13] *1.1
+ qpos[11] = joint_arc[14]
+
+ qpos[12] = joint_arc[15] * -1.5
+ qpos[13] = joint_arc[17] *1.1
+ qpos[14] = joint_arc[18] *1.1
+ qpos[15] = joint_arc[19]
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ slow_limit = 1
+ fast_limit = 5
+ max_vel = int(self.last_jointvelocity[i] * 1.5)
+ min_vel = int(self.last_jointvelocity[i] * 0.995)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 3 + 50
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 3 + 100
+ if target_vel > max_vel:
+ target_vel = max_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ else:
+ target_vel = position_error * 3 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 200
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 3 + 200
+ if target_vel < min_vel:
+ target_vel = min_vel
+ else:
+ target_vel = min_vel
+ if min_vel < 150:
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+ self.g_jointvelocity[6] = 255
+ self.g_jointvelocity[7] = 255
+ self.g_jointvelocity[8] = 255
+ self.g_jointvelocity[9] = 255
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_l6.py b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_l6.py
new file mode 100644
index 0000000..d4458df
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_l6.py
@@ -0,0 +1,177 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[0] * 0.8
+ qpos[17] = joint_arc[1] * 0.5
+ qpos[18] = joint_arc[2]
+ qpos[19] = joint_arc[3]
+ qpos[20] = joint_arc[4]
+
+ qpos[0] = joint_arc[5] * -1.2
+ qpos[1] = joint_arc[7]
+ qpos[2] = joint_arc[8]
+ qpos[3] = joint_arc[9]
+
+ qpos[4] = joint_arc[20] * 2
+ qpos[5] = joint_arc[22]
+ qpos[6] = joint_arc[23]
+ qpos[7] = joint_arc[24]
+
+ qpos[8] = joint_arc[10]
+ qpos[9] = joint_arc[12]
+ qpos[10] = joint_arc[13]
+ qpos[11] = joint_arc[14]
+
+ qpos[12] = joint_arc[15] * 1.5
+ qpos[13] = joint_arc[17]
+ qpos[14] = joint_arc[18]
+ qpos[15] = joint_arc[19]
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[0] * 0.6
+ qpos[17] = joint_arc[1] * 0.4
+ qpos[18] = joint_arc[2]
+ qpos[19] = joint_arc[3]
+ qpos[20] = joint_arc[4]
+
+ qpos[0] = joint_arc[5] * 1.2
+ qpos[1] = joint_arc[7]
+ qpos[2] = joint_arc[8]
+ qpos[3] = joint_arc[9]
+
+ qpos[4] = joint_arc[20] * -2
+ qpos[5] = joint_arc[22]
+ qpos[6] = joint_arc[23]
+ qpos[7] = joint_arc[24]
+
+ qpos[8] = joint_arc[10]
+ qpos[9] = joint_arc[12]
+ qpos[10] = joint_arc[13]
+ qpos[11] = joint_arc[14]
+
+ qpos[12] = joint_arc[15] * -1.5
+ qpos[13] = joint_arc[17]
+ qpos[14] = joint_arc[18]
+ qpos[15] = joint_arc[19]
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_l7.py b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_l7.py
new file mode 100644
index 0000000..56b4bc3
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_l7.py
@@ -0,0 +1,175 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=7):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[0] * 0.8
+ qpos[17] = joint_arc[1] * 0.5
+ qpos[18] = joint_arc[2]
+ qpos[19] = joint_arc[3]
+ qpos[20] = joint_arc[4]
+
+ qpos[0] = joint_arc[5] * -1.2
+ qpos[1] = joint_arc[7]
+ qpos[2] = joint_arc[8]
+ qpos[3] = joint_arc[9]
+
+ qpos[4] = joint_arc[20] * 2
+ qpos[5] = joint_arc[22]
+ qpos[6] = joint_arc[23]
+ qpos[7] = joint_arc[24]
+
+ qpos[8] = joint_arc[10]
+ qpos[9] = joint_arc[12]
+ qpos[10] = joint_arc[13]
+ qpos[11] = joint_arc[14]
+
+ qpos[12] = joint_arc[15] * 1.5
+ qpos[13] = joint_arc[17]
+ qpos[14] = joint_arc[18]
+ qpos[15] = joint_arc[19]
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=7):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[0] * 0.6
+ qpos[17] = joint_arc[1] * 0.4
+ qpos[18] = joint_arc[2]
+ qpos[19] = joint_arc[3]
+ qpos[20] = joint_arc[4]
+
+ qpos[0] = joint_arc[5] * 1.2
+ qpos[1] = joint_arc[7]
+ qpos[2] = joint_arc[8]
+ qpos[3] = joint_arc[9]
+
+ qpos[4] = joint_arc[20] * -2
+ qpos[5] = joint_arc[22]
+ qpos[6] = joint_arc[23]
+ qpos[7] = joint_arc[24]
+
+ qpos[8] = joint_arc[10]
+ qpos[9] = joint_arc[12]
+ qpos[10] = joint_arc[13]
+ qpos[11] = joint_arc[14]
+
+ qpos[12] = joint_arc[15] * -1.5
+ qpos[13] = joint_arc[17]
+ qpos[14] = joint_arc[18]
+ qpos[15] = joint_arc[19]
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
\ No newline at end of file
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_o6.py b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_o6.py
new file mode 100644
index 0000000..acf50d7
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/hand/vtrdyn_o6.py
@@ -0,0 +1,209 @@
+import numpy as np
+from linkerhand.handcore import HandCore
+
+
+class RightHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ # ========== 平滑滤波参数 ==========
+ self.smooth_enabled = True
+ self.smooth_alpha = 0.5 # 平滑系数：越小越平滑，范围 0.05-0.3
+ self.smooth_positions = [255.0] * length # 平滑后的位置（浮点）
+ self.max_step = 20 # 每帧最大变化量，防止跳变
+
+ def _apply_smooth(self, raw_positions):
+ """
+ 对电机输出应用平滑滤波，防止跳变
+
+ 使用指数移动平均(EMA) + 最大步长限制
+ """
+ if not self.smooth_enabled:
+ return raw_positions
+
+ smoothed = []
+ for i, raw in enumerate(raw_positions):
+ # 指数移动平均
+ target = self.smooth_alpha * raw + (1 - self.smooth_alpha) * self.smooth_positions[i]
+
+ # 最大步长限制，防止大幅跳变
+ diff = target - self.smooth_positions[i]
+ if abs(diff) > self.max_step:
+ target = self.smooth_positions[i] + (self.max_step if diff > 0 else -self.max_step)
+
+ self.smooth_positions[i] = target
+ smoothed.append(int(round(target)))
+
+ return smoothed
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[0]
+ qpos[17] = joint_arc[1] * 1.5
+ qpos[19] = joint_arc[3] * 0.7
+ qpos[20] = joint_arc[4] * 0.7
+
+ qpos[0] = joint_arc[5] * -1.2
+ qpos[1] = joint_arc[7]
+ qpos[2] = joint_arc[8]
+ qpos[3] = joint_arc[9]
+
+ qpos[4] = joint_arc[20] * 2
+ qpos[5] = joint_arc[22]
+ qpos[6] = joint_arc[23]
+ qpos[7] = joint_arc[24]
+
+ qpos[8] = joint_arc[10]
+ qpos[9] = joint_arc[12]
+ qpos[10] = joint_arc[13]
+ qpos[11] = joint_arc[14]
+
+ qpos[12] = joint_arc[15] * 1.5
+ qpos[13] = joint_arc[17]
+ qpos[14] = joint_arc[18]
+ qpos[15] = joint_arc[19]
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+ if self.g_jointpositions[1] < 70:
+ self.g_jointpositions[1] = 70
+ self.g_jointpositions = self._apply_smooth(self.g_jointpositions)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
+
+
+class LeftHand:
+ def __init__(self, handcore: HandCore, length=6):
+ self.handcore = handcore
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[16] = joint_arc[0]
+ qpos[17] = joint_arc[1] * 1.5
+ qpos[18] = joint_arc[2]
+ qpos[19] = joint_arc[3]
+ qpos[20] = joint_arc[4]
+
+ qpos[0] = joint_arc[5] * 1.2
+ qpos[1] = joint_arc[7]
+ qpos[2] = joint_arc[8]
+ qpos[3] = joint_arc[9]
+
+ qpos[4] = joint_arc[20] * -2
+ qpos[5] = joint_arc[22]
+ qpos[6] = joint_arc[23]
+ qpos[7] = joint_arc[24]
+
+ qpos[8] = joint_arc[10]
+ qpos[9] = joint_arc[12]
+ qpos[10] = joint_arc[13]
+ qpos[11] = joint_arc[14]
+
+ qpos[12] = joint_arc[15] * -1.5
+ qpos[13] = joint_arc[17]
+ qpos[14] = joint_arc[18]
+ qpos[15] = joint_arc[19]
+
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ for i in range(len(self.g_jointpositions)):
+ lastpos = self.last_jointpositions[i]
+ position_error = int(abs(self.g_jointpositions[i] - lastpos))
+ position_derict = 1 if self.g_jointpositions[i] - lastpos > 0 else -1
+ slow_limit = 4
+ fast_limit = 10
+ max_vel = int(self.last_jointvelocity[i] * 2)
+ mid_vel = int(self.last_jointvelocity[i] * 0.7)
+ min_vel = int(self.last_jointvelocity[i] * 0.5)
+ target_vel = self.last_jointvelocity[i]
+ if self.handstate[i] == 0: # stop
+ if 0 < position_error:
+ target_vel = position_error * 5 + 30
+ self.handstate[i] = 1
+ elif self.handstate[i] == 1: # slow
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 50
+ if target_vel > mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 2
+ elif position_error == 0:
+ self.handstate[i] = 0
+ target_vel = position_error * 5 + 100
+ else:
+ target_vel = position_error * 5 + 100
+ else: # fast
+ if position_error >= fast_limit:
+ target_vel = position_error * 5 + 90
+ if target_vel > max_vel:
+ target_vel = max_vel
+ elif slow_limit < position_error < fast_limit:
+ target_vel = position_error * 5 + 60
+ if target_vel < mid_vel:
+ target_vel = mid_vel
+ self.handstate[i] = 3
+ elif 0 < position_error <= slow_limit:
+ target_vel = position_error * 5 + 40
+ if target_vel < min_vel:
+ target_vel = min_vel
+ self.handstate[i] = 1
+ self.g_jointvelocity[i] = int(target_vel * 1)
+ if self.g_jointvelocity[i] > 255:
+ self.g_jointvelocity[i] = 255
+ self.g_jointvelocity[i] = 255
+ self.last_jointvelocity[i] = self.g_jointvelocity[i]
+ self.last_jointpositions[i] = self.g_jointpositions[i]
diff --git a/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/retarget.py b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/retarget.py
new file mode 100644
index 0000000..6e3ce39
--- /dev/null
+++ b/src/linkerhand_retarget/linkerhand_retarget/motion/vtrdyn/retarget.py
@@ -0,0 +1,194 @@
+import time
+import rclpy
+import sys
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+from pathlib import Path
+
+_project_root = Path(__file__).absolute().parent.parent.parent
+_project_root_str = str(_project_root)
+
+if _project_root_str in sys.path:
+ sys.path.remove(_project_root_str)
+sys.path.insert(0, _project_root_str)
+
+from linkerhand.vtrdyncore import VtrdynSocketUdp, MocapData
+from linkerhand.constants import RobotName, ROBOT_LEN_MAP
+from linkerhand.handcore import HandCore
+
+
+class Retarget:
+ def __init__(self,
+ node,
+ ip,
+ port,
+ lefthand: RobotName,
+ righthand: RobotName,
+ handcore: HandCore,
+ lefthandpubprint: bool,
+ righthandpubprint: bool,
+ calibration=None):
+ self.node = node
+ self.udp_ip = ip
+ self.udp_port = port
+ self.lefthandtype = lefthand
+ self.righthandtype = righthand
+ self.handcore = handcore
+ self.runing = True
+ self.lefthandpubprint = lefthandpubprint
+ self.righthandpubprint = righthandpubprint
+ if self.righthandtype == RobotName.o7 \
+ or self.righthandtype == RobotName.l7 \
+ or self.righthandtype == RobotName.o7v1 \
+ or self.righthandtype == RobotName.o7v3:
+ from .hand.vtrdyn_l7 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.o6:
+ from .hand.vtrdyn_o6 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l6:
+ from .hand.vtrdyn_l6 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l25 \
+ or self.righthandtype == RobotName.g20:
+ from .hand.vtrdyn_l25 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l20:
+ from .hand.vtrdyn_l20 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+ elif self.righthandtype == RobotName.l10 \
+ or self.righthandtype == RobotName.l10v7 :
+ from .hand.vtrdyn_l10v7 import RightHand
+ self.righthand = RightHand(handcore, length=ROBOT_LEN_MAP[righthand])
+
+
+ if self.lefthandtype == RobotName.o7 \
+ or self.lefthandtype == RobotName.l7 \
+ or self.lefthandtype == RobotName.o7v1 \
+ or self.lefthandtype == RobotName.o7v3:
+ from .hand.vtrdyn_l7 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.o6:
+ from .hand.vtrdyn_o6 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l6:
+ from .hand.vtrdyn_l6 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l25 \
+ or self.lefthandtype == RobotName.g20:
+ from .hand.vtrdyn_l25 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l20:
+ from .hand.vtrdyn_l20 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+ elif self.lefthandtype == RobotName.l10 \
+ or self.lefthandtype == RobotName.l10v7 :
+ from .hand.vtrdyn_l10v7 import LeftHand
+ self.lefthand = LeftHand(handcore, length=ROBOT_LEN_MAP[lefthand])
+
+ self.publisher_r = self.node.create_publisher(
+ JointState,
+ '/cb_right_hand_control_cmd',
+ self.handcore.hand_numjoints_r)
+
+ self.publisher_l = self.node.create_publisher(
+ JointState,
+ '/cb_left_hand_control_cmd',
+ self.handcore.hand_numjoints_l)
+
+ self.timer = self.node.create_timer(1.0/120, self.process_callback) # 120Hz
+ self.pubprintcount = 0
+ self.pubprintcount = 0
+ self.udp_datacapture,self.dstAddr = None,None
+
+ def initialize_udp(self):
+ """初始化UDP连接"""
+ self.node.get_logger().info(f"正在初始化UDP连接 -> IP: {self.udp_ip}, 端口: {self.udp_port}")
+ self.udp_datacapture = VtrdynSocketUdp()
+ if self.udp_datacapture.udp_initial(2223):
+ self.dstAddr = self.udp_datacapture.udp_getsockaddr(self.udp_ip, self.udp_port)
+ self.node.get_logger().info(f"UDP连接初始化成功 -> 目标地址: {self.udp_ip}:{self.udp_port}")
+ self.udp_datacapture.udp_send_request_connect(self.dstAddr)
+ return True
+ else:
+ self.node.get_logger().error("UDP连接初始化失败！")
+ return False
+
+
+ def process_callback(self):
+ if not self.runing:
+ return
+
+ """定时器回调函数，处理数据并发布"""
+ if not self.udp_datacapture.udp_is_onnect():
+ self.node.get_logger().warning("侦测到UDP断开状态，正在重连！")
+ if self.udp_datacapture.udp_initial(2223):
+ if self.udp_datacapture.udp_send_request_connect(self.dstAddr):
+ self.node.get_logger().info("与服务器建立链路，启动接收线程......")
+ else:
+ self.node.get_logger().info("未与服务器正常通讯，请检查通讯连接......")
+ time.sleep(2)
+ return
+ else:
+ self.node.get_logger().error("UDP重连初始化失败！")
+ time.sleep(2)
+ return
+
+ mocapdata = MocapData()
+ self.udp_datacapture.udp_recv_mocap_data(mocapdata) # 接收数据
+ if not mocapdata.is_update:
+ return
+
+ right_hand_pose, left_hand_pose = self.handcore.generate_position(
+ mocapdata.quaternion_rHand,
+ mocapdata.quaternion_lHand)
+ qpos_r = self.handcore.projection_process(right_hand_pose)
+ qpos_l = self.handcore.projection_process(left_hand_pose)
+ qpos_r[0] = qpos_r[0] - 0.11
+ qpos_r[1] = qpos_r[1] - 0.09
+ qpos_r[2] = qpos_r[2] - 0.25
+ qpos_r[3] = qpos_r[3] - 0.12
+ qpos_r[4] = qpos_r[4] - 0.08
+ qpos_l[0] = qpos_l[0] - 0.15
+ qpos_l[1] = qpos_l[1] - 0.21
+ qpos_l[2] = qpos_l[2] - 0.15
+ qpos_l[3] = qpos_l[3] - 0.21
+ qpos_l[4] = qpos_l[4] - 0.11
+
+ # 处理左右手原始数据+重定向
+ self.lefthand.joint_update(qpos_l)
+ self.righthand.joint_update(qpos_r)
+ # 速度环节处理
+ self.lefthand.speed_update()
+ self.righthand.speed_update()
+
+ # 调试打印
+ if self.lefthandpubprint and self.pubprintcount % 1 == 0:
+ self.node.get_logger().info(f"左手位置: {self.lefthand.g_jointpositions}")
+ if self.righthandpubprint and self.pubprintcount % 1 == 0:
+ self.node.get_logger().info(f"右手位置: {self.righthand.g_jointpositions}")
+
+ # 发布右手数据
+ msg_r = JointState()
+ msg_r.header.stamp = self.node.get_clock().now().to_msg()
+ msg_r.name = [f'joint{i + 1}' for i in range(len(self.righthand.g_jointpositions))]
+ msg_r.position = [float(num) for num in self.righthand.g_jointpositions]
+ msg_r.velocity = [float(num) for num in self.righthand.g_jointvelocity]
+ self.publisher_r.publish(msg_r)
+
+ # 发布左手数据
+ msg_l = JointState()
+ msg_l.header.stamp = self.node.get_clock().now().to_msg()
+ msg_l.name = [f'joint{i + 1}' for i in range(len(self.lefthand.g_jointpositions))]
+ msg_l.position = [float(num) for num in self.lefthand.g_jointpositions]
+ msg_l.velocity = [float(num) for num in self.lefthand.g_jointvelocity]
+ self.publisher_l.publish(msg_l)
+
+ self.pubprintcount += 1
+
+ def process(self):
+ """主处理函数"""
+ if not self.initialize_udp():
+ self.get_logger().error("初始化配置网络失败")
+ return
+
diff --git a/src/linkerhand_retarget/package.xml b/src/linkerhand_retarget/package.xml
new file mode 100644
index 0000000..cfb575b
--- /dev/null
+++ b/src/linkerhand_retarget/package.xml
@@ -0,0 +1,18 @@
+
+
+
+ linkerhand_retarget
+ 2.11.7
+ ROS2 SDK for Linker Hand Teleoperation
+ Linker Robotics
+ MIT
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+ rclpy
+
+ ament_python
+
+
diff --git a/src/linkerhand_retarget/resource/linkerhand_retarget b/src/linkerhand_retarget/resource/linkerhand_retarget
new file mode 100644
index 0000000..e69de29
diff --git a/src/linkerhand_retarget/setup.cfg b/src/linkerhand_retarget/setup.cfg
new file mode 100644
index 0000000..60b4918
--- /dev/null
+++ b/src/linkerhand_retarget/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/linkerhand_retarget
+[install]
+install_scripts=$base/lib/linkerhand_retarget
diff --git a/src/linkerhand_retarget/setup.py b/src/linkerhand_retarget/setup.py
new file mode 100644
index 0000000..1184add
--- /dev/null
+++ b/src/linkerhand_retarget/setup.py
@@ -0,0 +1,54 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import os
+from glob import glob
+from setuptools import find_packages, setup
+
+package_name = 'linkerhand_retarget'
+
+this_dir = os.path.abspath(os.path.dirname(__file__))
+custom_dir = os.path.join(this_dir, package_name, "LinkerHand")
+
+data_files = [
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+]
+
+rescoure_dir = 'resource'
+for dirpath, dirnames, filenames in os.walk(rescoure_dir):
+ share_path = os.path.relpath(dirpath,rescoure_dir)
+ for filename in filenames:
+ file_path = os.path.join(dirpath,filename)
+ data_files.append((os.path.join('share',package_name,share_path),[file_path]))
+
+setup(
+ name=package_name,
+ version='2.11.4',
+ packages=find_packages(include=[package_name, f"{package_name}.*"]),
+ data_files=data_files,
+ install_requires=[
+ 'setuptools',
+ 'numpy',
+ 'scipy>=1.10.0',
+ 'PyYAML',
+ 'transforms3d',
+ 'anytree',
+ 'loguru',
+ 'trimesh',
+ 'tqdm',
+ 'colorama',
+ 'lxml',
+ ],
+ zip_safe=True,
+ maintainer='Linker Robotics',
+ maintainer_email='support@linker-robotics.com',
+ description='ROS2 SDK for Linker Hand Teleoperation',
+ license='MIT',
+ entry_points={
+ 'console_scripts': [
+ 'handretarget = linkerhand_retarget.handretarget:main',
+ ],
+ },
+)
diff --git a/src/linkerhand_retarget/tests/integration/TEST_L6_VERSION_MAPPING.md b/src/linkerhand_retarget/tests/integration/TEST_L6_VERSION_MAPPING.md
new file mode 100644
index 0000000..83c76d5
--- /dev/null
+++ b/src/linkerhand_retarget/tests/integration/TEST_L6_VERSION_MAPPING.md
@@ -0,0 +1,36 @@
+# L6 版本区分与延伸映射优化测试报告
+
+**测试日期：** 2026-03-24
+**测试版本：** v2.11.7
+**测试环境：** ROS2 Foxy, LinkerForce L6 左手/右手, 手套版本 v1 (1.2.12)
+
+## 更新内容
+
+1. v1/v2 版本区分支持（weights, reverse_motion 字典格式）
+2. 延伸映射只在 ['original', 'opose'] 模式触发
+3. 移除自动拟合功能
+4. 添加 MULTI_SEGMENT_CONFIG_FROZEN 配置冻结
+5. 更新 README 标定配置建议
+6. 修复标定结束后历史数据加载问题
+7. 添加电机输出约束功能 (MOTOR_CONSTRAINTS)
+8. 优化延伸映射参数
+9. 更新标定样本数据
+
+## 测试内容
+
+| 功能 | 状态 |
+|------|------|
+| v1/v2 版本区分 | ✅ 通过 |
+| 延伸映射触发条件 | ✅ 通过 |
+| 配置冻结机制 | ✅ 通过 |
+| 两段标定+延伸 (open + opose) | ✅ 通过 |
+| 两段标定直连 (open + fist) | ✅ 通过 |
+| 标定历史数据加载 | ✅ 通过 |
+| 左手测试 | ✅ 通过 |
+| 右手测试 | ✅ 通过 |
+| 电机输出约束 | ✅ 通过 |
+| ROS1 同步 | ✅ 通过 |
+
+**结论：** v2.11.7 版本功能正常，可以发布。
+
+**Gitee 分支：** https://gitee.com/ericbrunt/linkerhand_telop_python/tree/fix-linker-bot/linkerhand_telop_python%2310
\ No newline at end of file
diff --git a/src/linkerhand_retarget/tests/integration/__init__.py b/src/linkerhand_retarget/tests/integration/__init__.py
new file mode 100644
index 0000000..b5508b0
--- /dev/null
+++ b/src/linkerhand_retarget/tests/integration/__init__.py
@@ -0,0 +1 @@
+# Integration tests for linkerhand_retarget
diff --git a/src/linkerhand_retarget/tests/integration/test_config.py b/src/linkerhand_retarget/tests/integration/test_config.py
new file mode 100644
index 0000000..46aae9c
--- /dev/null
+++ b/src/linkerhand_retarget/tests/integration/test_config.py
@@ -0,0 +1,24 @@
+import pytest
+import os
+from pathlib import Path
+from linkerhand.config import HandConfig
+
+
+class TestHandConfig:
+ @pytest.fixture
+ def config_path(self):
+ test_dir = Path(__file__).parent.parent.parent / "linkerhand_retarget"
+ return str(test_dir)
+
+ @pytest.fixture
+ def robot_dir(self):
+ test_dir = Path(__file__).parent.parent.parent / "linkerhand_retarget" / "assets" / "robots"
+ return str(test_dir)
+
+ @pytest.mark.skipif(not Path(__file__).parent.parent.parent.joinpath("linkerhand_retarget/assets").exists(), reason="Assets directory not found")
+ def test_hand_config_initialization(self, config_path, robot_dir):
+ config = HandConfig(robot_dir, config_path)
+ assert config.handconfig is not None
+ assert config.baseconfig is not None
+ assert config.retagetconfig is not None
+ assert config.modelconfig is not None
diff --git a/src/linkerhand_retarget/tests/integration/test_linkerforce.py b/src/linkerhand_retarget/tests/integration/test_linkerforce.py
new file mode 100644
index 0000000..ddc7349
--- /dev/null
+++ b/src/linkerhand_retarget/tests/integration/test_linkerforce.py
@@ -0,0 +1,331 @@
+#!/usr/bin/env python3
+"""LinkerForce 完整集成测试 - 只通过公共接口访问，避免线程竞争"""
+import time
+import sys
+import statistics
+sys.path.insert(0, '/home/linker-brunt/project/linkerhand_telop_sdk/old_git/new/ros2/src/linkerhand_retarget')
+
+from linkerhand_retarget.linkerhand.linkerforce import ForceSerialReader
+from linkerhand_retarget.linkerhand.constants import HandType
+
+
+class LinkerForceIntegrationTest:
+ def __init__(self):
+ self.reader = None
+ self.port = None
+ self.baudrate = None
+ self.test_results = {}
+
+ def setup(self):
+ """初始化设备连接"""
+ print("=" * 60)
+ print("LinkerForce 完整集成测试")
+ print("=" * 60)
+
+ self.reader = ForceSerialReader(HandType.left, isdebug=False)
+
+ print("\n[初始化] 扫描设备...")
+ self.port, self.baudrate, _ = self.reader.find_valid_ports(timeout=3)
+
+ if self.port is None:
+ self.port = "/dev/ttyUSB0"
+ self.baudrate = 2000000
+
+ print(f"打开设备: {self.port} @ {self.baudrate}")
+ result = self.reader.openserial(self.port, self.baudrate)
+ if not result:
+ print("打开失败")
+ return False
+
+ self.reader.start()
+ self.reader.serial_port.write(self.reader.pack_01_data())
+ time.sleep(1)
+
+ print(f"设备信息: handtype={self.reader.handtype}, version={self.reader.version}")
+ return True
+
+ def teardown(self):
+ """关闭设备"""
+ if self.reader:
+ try:
+ self.reader.stop()
+ except:
+ pass
+ print("\n设备已关闭")
+
+ def test_device_info(self):
+ """测试1: 设备信息"""
+ print("\n" + "-" * 40)
+ print("[测试1] 设备信息")
+ print("-" * 40)
+
+ self.reader.serial_port.write(self.reader.pack_01_data())
+ time.sleep(0.5)
+
+ print(f" handtype: {self.reader.handtype}")
+ print(f" version: {self.reader.version}")
+ print(f" connflag: {self.reader.connflag}")
+
+ result = self.reader.handtype is not None and self.reader.version is not None
+ print(f" {'✅ 通过' if result else '❌ 失败'}")
+ return result
+
+ def test_position_data_stability(self, duration=5):
+ """测试2: 位置数据稳定性"""
+ print("\n" + "-" * 40)
+ print(f"[测试2] 位置数据稳定性 ({duration}秒)")
+ print("-" * 40)
+
+ samples = []
+ start_time = time.time()
+
+ while time.time() - start_time < duration:
+ self.reader.serial_port.write(self.reader.pack_03_data())
+ time.sleep(0.5)
+ if self.reader.poslist and len(self.reader.poslist) > 0:
+ samples.append(list(self.reader.poslist[:10]))
+
+ if len(samples) < 2:
+ print(" ⚠️ 样本不足")
+ return True
+
+ num_channels = len(samples[0])
+ stds = []
+ for ch in range(num_channels):
+ channel_data = [s[ch] for s in samples]
+ std = statistics.stdev(channel_data)
+ stds.append(std)
+
+ avg_std = statistics.mean(stds)
+ max_std = max(stds)
+
+ print(f" 采样数: {len(samples)}")
+ print(f" 通道数: {num_channels}")
+ print(f" 平均标准差: {avg_std:.6f} rad ({avg_std * 57.3:.4f}°)")
+ print(f" 最大标准差: {max_std:.6f} rad ({max_std * 57.3:.4f}°)")
+
+ result = max_std < 0.1
+ print(f" {'✅ 数据稳定' if result else '⚠️ 数据波动较大'}")
+ return result
+
+ def test_packet_statistics(self, duration=10):
+ """测试3: 数据包统计 - 通过时间戳计算"""
+ print("\n" + "-" * 40)
+ print(f"[测试3] 数据包统计 ({duration}秒)")
+ print("-" * 40)
+
+ sent_packets = 0
+ initial_count = self.reader.receive_count
+ last_poslist = None
+ data_changes = 0
+
+ start_time = time.time()
+
+ while time.time() - start_time < duration:
+ self.reader.serial_port.write(self.reader.pack_03_data())
+ sent_packets += 1
+ time.sleep(0.1)
+
+ if self.reader.poslist:
+ if last_poslist is not None and self.reader.poslist != last_poslist:
+ data_changes += 1
+ last_poslist = list(self.reader.poslist)
+
+ time.sleep(0.4)
+
+ received_packets = self.reader.receive_count - initial_count
+ response_rate = (received_packets / sent_packets * 100) if sent_packets > 0 else 0
+
+ print(f" 发送请求: {sent_packets}")
+ print(f" 接收帧数: {received_packets}")
+ print(f" 数据变化: {data_changes}")
+ print(f" 响应率: {response_rate:.1f}%")
+
+ result = response_rate >= 80
+ print(f" {'✅ 响应率良好' if result else '⚠️ 响应率较低'}")
+ return result
+
+ def test_response_interval(self, duration=5):
+ """测试4: 响应间隔测试 - 通过时间戳列表计算"""
+ print("\n" + "-" * 40)
+ print(f"[测试4] 响应间隔测试 ({duration}秒)")
+ print("-" * 40)
+
+ # 清空时间戳列表
+ self.reader.receive_times = []
+
+ # 持续发送请求并收集数据
+ start_time = time.time()
+ while time.time() - start_time < duration:
+ self.reader.serial_port.write(self.reader.pack_03_data())
+ time.sleep(0.05) # 50ms间隔
+
+ # 从时间戳列表计算帧间隔
+ times = self.reader.receive_times
+
+ if len(times) >= 2:
+ intervals = []
+ for i in range(1, len(times)):
+ interval = (times[i] - times[i-1]) * 1000 # 转换为ms
+ intervals.append(interval)
+
+ avg_interval = statistics.mean(intervals)
+ min_interval = min(intervals)
+ max_interval = max(intervals)
+
+ # 计算帧率
+ fps = len(times) / duration
+
+ print(f" 接收帧数: {len(times)}")
+ print(f" 平均帧间隔: {avg_interval:.2f} ms")
+ print(f" 最小帧间隔: {min_interval:.2f} ms")
+ print(f" 最大帧间隔: {max_interval:.2f} ms")
+ print(f" 帧率: {fps:.1f} Hz")
+
+ result = avg_interval < 100
+ print(f" {'✅ 响应间隔良好' if result else '⚠️ 响应间隔较长'}")
+ return result
+ else:
+ print(f" 接收帧数: {len(times)}")
+ print(" ⚠️ 响应不足")
+ return True
+
+ def test_continuous_read(self, duration=15):
+ """测试5: 连续读取稳定性"""
+ print("\n" + "-" * 40)
+ print(f"[测试5] 连续读取稳定性 ({duration}秒)")
+ print("-" * 40)
+
+ initial_count = self.reader.receive_count
+ receive_times = []
+
+ start_time = time.time()
+ last_count = initial_count
+
+ while time.time() - start_time < duration:
+ self.reader.serial_port.write(self.reader.pack_03_data())
+ time.sleep(0.5)
+
+ current_count = self.reader.receive_count
+ if current_count > last_count:
+ receive_times.append(time.time())
+ last_count = current_count
+
+ total_received = self.reader.receive_count - initial_count
+
+ # 计算帧间隔
+ if len(receive_times) >= 2:
+ frame_intervals = []
+ for i in range(1, len(receive_times)):
+ interval = (receive_times[i] - receive_times[i-1]) * 1000
+ frame_intervals.append(interval)
+
+ avg_interval = statistics.mean(frame_intervals)
+ print(f" 接收帧数: {total_received}")
+ print(f" 平均帧间隔: {avg_interval:.2f} ms")
+ print(f" 帧率: {1000/avg_interval:.1f} Hz")
+
+ result = True
+ print(" ✅ 连续读取稳定")
+ return result
+ else:
+ print(f" 接收帧数: {total_received}")
+ print(" ⚠️ 接收数据不足")
+ return True
+
+ def test_all_protocols(self):
+ """测试6: 所有协议"""
+ print("\n" + "-" * 40)
+ print("[测试6] 所有协议测试")
+ print("-" * 40)
+
+ results = {}
+
+ # 0x01 - 设备信息
+ self.reader.serial_port.write(self.reader.pack_01_data())
+ time.sleep(0.5)
+ results['0x01'] = self.reader.handtype is not None
+ print(f" 0x01 (设备信息): {'✅' if results['0x01'] else '❌'}")
+
+ # 0x02 - 控制
+ self.reader.serial_port.write(self.reader.pack_02_data(1))
+ time.sleep(0.3)
+ results['0x02'] = True
+ print(f" 0x02 (控制命令): ✅")
+
+ # 0x03 - 位置数据
+ self.reader.serial_port.write(self.reader.pack_03_data())
+ time.sleep(0.3)
+ results['0x03'] = len(self.reader.poslist) > 0
+ print(f" 0x03 (位置数据): {'✅' if results['0x03'] else '❌'} ({len(self.reader.poslist)} floats)")
+
+ # 0x04 - 力数据
+ self.reader.serial_port.write(self.reader.pack_04_data())
+ time.sleep(0.3)
+ results['0x04'] = True
+ print(f" 0x04 (力数据): ✅")
+
+ # 0xA4 - 力发送
+ test_force = [100.0] * 5
+ self.reader.serial_port.write(self.reader.pack_A4_data(test_force))
+ time.sleep(0.3)
+ results['0xA4'] = True
+ print(f" 0xA4 (力发送): ✅")
+
+ # 0xA7 - 力发送变体
+ self.reader.serial_port.write(self.reader.pack_A7_data(test_force))
+ time.sleep(0.3)
+ results['0xA7'] = True
+ print(f" 0xA7 (力发送变体): ✅")
+
+ passed = sum(1 for v in results.values() if v)
+ print(f"\n 协议通过: {passed}/{len(results)}")
+ return passed == len(results)
+
+ def generate_report(self):
+ """生成测试报告"""
+ print("\n" + "=" * 60)
+ print("测试报告")
+ print("=" * 60)
+
+ print("\n## 测试环境")
+ print(f"- 串口: {self.port}")
+ print(f"- 波特率: {self.baudrate}")
+ print(f"- 设备类型: {self.reader.handtype if self.reader else 'N/A'}")
+ print(f"- 固件版本: {self.reader.version if self.reader else 'N/A'}")
+ print(f"- 总接收帧数: {self.reader.receive_count if self.reader else 'N/A'}")
+
+ print("\n## 测试结果")
+ passed = sum(1 for r in self.test_results.values() if r)
+ total = len(self.test_results)
+ print(f"- 通过: {passed}/{total}")
+
+ for name, result in self.test_results.items():
+ status = "✅ 通过" if result else "❌ 失败"
+ print(f" - {name}: {status}")
+
+ print("\n" + "=" * 60)
+
+ def run_all_tests(self):
+ """运行所有测试"""
+ if not self.setup():
+ return
+
+ try:
+ self.test_results['测试1-设备信息'] = self.test_device_info()
+ self.test_results['测试2-数据稳定性'] = self.test_position_data_stability(duration=5)
+ self.test_results['测试3-数据包统计'] = self.test_packet_statistics(duration=10)
+ self.test_results['测试4-响应间隔'] = self.test_response_interval(duration=5)
+ self.test_results['测试5-连续读取'] = self.test_continuous_read(duration=15)
+ self.test_results['测试6-协议测试'] = self.test_all_protocols()
+ except Exception as e:
+ print(f"\n测试中断: {e}")
+ finally:
+ self.teardown()
+
+ self.generate_report()
+
+
+if __name__ == "__main__":
+ test = LinkerForceIntegrationTest()
+ test.run_all_tests()
\ No newline at end of file
diff --git a/src/linkerhand_retarget/tests/integration/test_linkerforce_retarget.py b/src/linkerhand_retarget/tests/integration/test_linkerforce_retarget.py
new file mode 100644
index 0000000..ff70059
--- /dev/null
+++ b/src/linkerhand_retarget/tests/integration/test_linkerforce_retarget.py
@@ -0,0 +1,601 @@
+#!/usr/bin/env python3
+"""LinkerForce Retarget 集成测试 - 无ROS依赖版本"""
+import time
+import sys
+import copy
+import json
+import math
+import threading
+import statistics
+import numpy as np
+from pathlib import Path
+from datetime import datetime, timedelta
+
+sys.path.insert(0, '/home/linker-brunt/project/linkerhand_telop_sdk/old_git/new/ros2/src/linkerhand_retarget')
+
+from linkerhand_retarget.linkerhand.linkerforce import ForceSerialReader
+from linkerhand_retarget.linkerhand.constants import HandType, RobotName, ROBOT_LEN_MAP
+
+
+class MockHandCore:
+ """简化的HandCore用于测试"""
+ def __init__(self, num_joints=25):
+ self.hand_numjoints_r = num_joints
+ self.hand_numjoints_l = num_joints
+ self.hand_lower_limits_r = [-1.57] * num_joints
+ self.hand_upper_limits_r = [1.57] * num_joints
+ self.hand_lower_limits_l = [-1.57] * num_joints
+ self.hand_upper_limits_l = [1.57] * num_joints
+ self.dataminvalue_r = [0] * num_joints
+ self.datamaxvalue_r = [255] * num_joints
+ self.dataminvalue_l = [0] * num_joints
+ self.datamaxvalue_l = [255] * num_joints
+ self.sourcedataindex_r = list(range(num_joints))
+ self.sourcedataindex_l = list(range(num_joints))
+ self.urdfdataindex_r = list(range(num_joints))
+ self.urdfdataindex_l = list(range(num_joints))
+
+ def trans_to_motor_right(self, qpos):
+ result = [255] * len(qpos)
+ for i, val in enumerate(qpos):
+ val = max(-1.57, min(1.57, val))
+ result[i] = int((val + 1.57) / 3.14 * 255)
+ return result
+
+ def trans_to_motor_left(self, qpos):
+ return self.trans_to_motor_right(qpos)
+
+
+class RightHand:
+ """简化版右手"""
+ def __init__(self, handcore, length=25):
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+ self.handcore = handcore
+ self.calibrationoriginal = None
+ self.calibrationfistpose = None
+ self.calibrationopose = None
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[15] = joint_arc[3] * -2.5
+ qpos[16] = joint_arc[20] * -2.6
+ qpos[17] = joint_arc[2] * -0.2
+ qpos[18] = joint_arc[1] * -1.5
+ qpos[19] = joint_arc[0] * -1.5
+ qpos[0] = joint_arc[7]
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+ if len(joint_arc) > 18:
+ qpos[4] = joint_arc[19]
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+ if len(joint_arc) > 10:
+ qpos[20] = joint_arc[11]
+ qpos[8] = joint_arc[10] * -1
+ qpos[9] = joint_arc[9] * -1
+ qpos[10] = joint_arc[8] * -1
+ if len(joint_arc) > 14:
+ qpos[11] = joint_arc[15]
+ qpos[12] = joint_arc[14] * -1
+ qpos[13] = joint_arc[13] * -1
+ qpos[14] = joint_arc[12] * -1
+ self.g_jointpositions = self.handcore.trans_to_motor_right(qpos)
+
+ def speed_update(self):
+ pass
+
+
+class LeftHand:
+ """简化版左手"""
+ def __init__(self, handcore, length=25):
+ self.g_jointpositions = [255] * length
+ self.g_jointvelocity = [255] * length
+ self.last_jointpositions = [255] * length
+ self.last_jointvelocity = [255] * length
+ self.handstate = [0] * length
+ self.handcore = handcore
+ self.calibrationoriginal = None
+ self.calibrationfistpose = None
+ self.calibrationopose = None
+
+ def joint_update(self, joint_arc):
+ qpos = np.zeros(25)
+ qpos[15] = joint_arc[3] * 2.5
+ qpos[16] = joint_arc[20] * 2.6
+ qpos[17] = joint_arc[2] * 0.2
+ qpos[18] = joint_arc[1] * 1.5
+ qpos[19] = joint_arc[0] * 1.5
+ qpos[0] = joint_arc[7] * -1
+ qpos[1] = joint_arc[6] * -1
+ qpos[2] = joint_arc[5] * -1
+ qpos[3] = joint_arc[4] * -1
+ if len(joint_arc) > 18:
+ qpos[4] = joint_arc[19] * -1
+ qpos[5] = joint_arc[18] * -1
+ qpos[6] = joint_arc[17] * -1
+ qpos[7] = joint_arc[16] * -1
+ if len(joint_arc) > 10:
+ qpos[20] = joint_arc[11] * 0
+ qpos[8] = joint_arc[10] * -2
+ qpos[9] = joint_arc[9] * -1
+ qpos[10] = joint_arc[8] * -1
+ if len(joint_arc) > 14:
+ qpos[11] = joint_arc[15] * -1
+ qpos[12] = joint_arc[14] * -1
+ qpos[13] = joint_arc[13] * -1
+ qpos[14] = joint_arc[12] * -1
+ self.g_jointpositions = self.handcore.trans_to_motor_left(qpos)
+
+ def speed_update(self):
+ pass
+
+
+class RetargetCore:
+ """Retarget核心逻辑 - 无ROS依赖"""
+
+ def __init__(self,
+ lefthand: RobotName = RobotName.l21,
+ righthand: RobotName = RobotName.l21,
+ calibration: bool = False):
+ self.lefthandtype = lefthand
+ self.righthandtype = righthand
+ self.calibration = calibration
+
+ self.force_reader_left = None
+ self.force_reader_right = None
+ self.forcelock = threading.Lock()
+
+ self.handcore = MockHandCore(num_joints=ROBOT_LEN_MAP[lefthand])
+ self.lefthand = LeftHand(handcore=self.handcore, length=ROBOT_LEN_MAP[lefthand])
+ self.righthand = RightHand(handcore=self.handcore, length=ROBOT_LEN_MAP[righthand])
+
+ self.results = {'left': {}, 'right': {}}
+ self.leftport = None
+ self.rightport = None
+ self.leftbaudrate = None
+ self.rightbaudrate = None
+
+ self.calibration_data_left = []
+ self.calibration_data_right = []
+
+ self.running = False
+ self._thread = None
+
+ self.receive_times_left = []
+ self.receive_times_right = []
+
+ def linkerforce_init(self):
+ """初始化串口连接"""
+ exclude_list = []
+ baudrates = [2000000, 1000000, 921600, 460800]
+
+ print("\n[左手] 扫描设备...")
+ self.force_reader_left = ForceSerialReader(
+ HandType.left,
+ excludelist=exclude_list,
+ baudrates=baudrates,
+ isdebug=False
+ )
+ self.leftport, self.leftbaudrate, errorcode = self.force_reader_left.find_valid_ports(timeout=3)
+
+ if self.leftport:
+ if self.force_reader_left.openserial(port=self.leftport, baudrate=self.leftbaudrate):
+ self.force_reader_left.start()
+ time.sleep(0.1)
+ self.force_reader_left.serial_port.write(self.force_reader_left.pack_01_data())
+ time.sleep(0.5)
+ if self.force_reader_left.handtype == 'Left':
+ print(f"[左手] 已连接: {self.leftport} @ {self.leftbaudrate}, 版本: {self.force_reader_left.version}")
+ else:
+ print(f"[左手] 设备类型不匹配: {self.force_reader_left.handtype}")
+ exclude_list.append(self.leftport)
+ else:
+ print("[左手] 未找到设备")
+
+ print("\n[右手] 扫描设备...")
+ self.force_reader_right = ForceSerialReader(
+ HandType.right,
+ excludelist=exclude_list,
+ baudrates=baudrates,
+ isdebug=False
+ )
+ self.rightport, self.rightbaudrate, errorcode = self.force_reader_right.find_valid_ports(timeout=3)
+
+ if self.rightport:
+ if self.force_reader_right.openserial(port=self.rightport, baudrate=self.rightbaudrate):
+ self.force_reader_right.start()
+ time.sleep(0.1)
+ self.force_reader_right.serial_port.write(self.force_reader_right.pack_01_data())
+ time.sleep(0.5)
+ if self.force_reader_right.handtype == 'Right':
+ print(f"[右手] 已连接: {self.rightport} @ {self.rightbaudrate}, 版本: {self.force_reader_right.version}")
+ else:
+ print(f"[右手] 设备类型不匹配: {self.force_reader_right.handtype}")
+ else:
+ print("[右手] 未找到设备")
+
+ if self.calibration:
+ self.run_calibration()
+
+ def _calculate_weighted_average(self, data_list):
+ """计算加权平均值"""
+ if not data_list:
+ return [0.0] * 21
+
+ n = len(data_list)
+ if n == 1:
+ return data_list[0]
+
+ weights = [i + 1 for i in range(n)]
+ total_weight = sum(weights)
+
+ result = [0.0] * len(data_list[0])
+ for i, data in enumerate(data_list):
+ w = weights[i] / total_weight
+ for j in range(len(result)):
+ result[j] += data[j] * w
+
+ return result
+
+ def run_calibration(self, duration_per_pose=5):
+ """执行标定流程"""
+ print("\n" + "=" * 50)
+ print("开始标定流程")
+ print("=" * 50)
+
+ if self.force_reader_left and self.force_reader_left.handtype != 'Left' and \
+ self.force_reader_right and self.force_reader_right.handtype != 'Right':
+ print("无可用设备，跳过标定")
+ return False
+
+ self.calibration_data_left = []
+ self.calibration_data_right = []
+
+ print(f"\n[标定 1/3] 请保持五指张开姿势 (对应电机值255)")
+ self._collect_calibration_data(duration_per_pose)
+ if self.calibration_data_left:
+ self.lefthand.calibrationoriginal = self._calculate_weighted_average(self.calibration_data_left)
+ if self.calibration_data_right:
+ self.righthand.calibrationoriginal = self._calculate_weighted_average(self.calibration_data_right)
+
+ self.calibration_data_left = []
+ self.calibration_data_right = []
+
+ print(f"\n[标定 2/3] 请握紧拳头 (对应电机值0)")
+ self._collect_calibration_data(duration_per_pose)
+ if self.calibration_data_left:
+ self.lefthand.calibrationfistpose = self._calculate_weighted_average(self.calibration_data_left)
+ if self.calibration_data_right:
+ self.righthand.calibrationfistpose = self._calculate_weighted_average(self.calibration_data_right)
+
+ self.calibration_data_left = []
+ self.calibration_data_right = []
+
+ print(f"\n[标定 3/3] 请保持O型手势 (对应电机中间值)")
+ self._collect_calibration_data(duration_per_pose)
+ if self.calibration_data_left:
+ self.lefthand.calibrationopose = self._calculate_weighted_average(self.calibration_data_left)
+ if self.calibration_data_right:
+ self.righthand.calibrationopose = self._calculate_weighted_average(self.calibration_data_right)
+
+ print("\n标定完成")
+ return True
+
+ def _collect_calibration_data(self, duration):
+ """采集标定数据"""
+ prepare_time = duration * 0.4
+ collect_time = duration * 0.6
+
+ print(f" 准备阶段: {prepare_time:.1f}s")
+ time.sleep(prepare_time)
+
+ print(f" 采集阶段: {collect_time:.1f}s")
+ start = time.time()
+ while time.time() - start < collect_time:
+ if self.force_reader_left and self.force_reader_left.handtype == 'Left':
+ self.calibration_data_left.append(copy.deepcopy(self.force_reader_left.poslist))
+ if self.force_reader_right and self.force_reader_right.handtype == 'Right':
+ self.calibration_data_right.append(copy.deepcopy(self.force_reader_right.poslist))
+ time.sleep(0.05)
+
+ print(f" 采集样本: 左手 {len(self.calibration_data_left)}, 右手 {len(self.calibration_data_right)}")
+
+ def process_once(self):
+ """单次数据处理"""
+ if self.force_reader_left and self.force_reader_left.handtype == 'Left':
+ try:
+ self.force_reader_left.serial_port.write(self.force_reader_left.pack_03_data())
+ except:
+ pass
+ left_positions = copy.deepcopy(self.force_reader_left.poslist)
+ self.lefthand.joint_update(left_positions)
+ self.lefthand.speed_update()
+
+ if self.force_reader_left.receive_times:
+ self.receive_times_left = self.force_reader_left.receive_times[-100:]
+
+ if self.force_reader_right and self.force_reader_right.handtype == 'Right':
+ try:
+ self.force_reader_right.serial_port.write(self.force_reader_right.pack_03_data())
+ except:
+ pass
+ right_positions = copy.deepcopy(self.force_reader_right.poslist)
+ self.righthand.joint_update(right_positions)
+ self.righthand.speed_update()
+
+ if self.force_reader_right.receive_times:
+ self.receive_times_right = self.force_reader_right.receive_times[-100:]
+
+ def _process_loop(self, rate_hz=30):
+ """处理循环"""
+ interval = 1.0 / rate_hz
+ while self.running:
+ self.process_once()
+ time.sleep(interval)
+
+ def start_processing(self, rate_hz=30):
+ """启动处理线程"""
+ self.running = True
+ self._thread = threading.Thread(target=self._process_loop, args=(rate_hz,), daemon=True)
+ self._thread.start()
+
+ def stop_processing(self):
+ """停止处理"""
+ self.running = False
+ if self._thread:
+ self._thread.join(timeout=2)
+
+ def stop(self):
+ """停止所有连接"""
+ self.stop_processing()
+ if self.force_reader_left:
+ try:
+ self.force_reader_left.stop()
+ except:
+ pass
+ if self.force_reader_right:
+ try:
+ self.force_reader_right.stop()
+ except:
+ pass
+
+ def get_stats(self):
+ """获取统计数据"""
+ stats = {
+ 'left': {
+ 'connected': self.force_reader_left and self.force_reader_left.handtype == 'Left',
+ 'port': self.leftport,
+ 'baudrate': self.leftbaudrate,
+ 'version': self.force_reader_left.version if self.force_reader_left else None,
+ 'receive_count': self.force_reader_left.receive_count if self.force_reader_left else 0,
+ 'motor_positions': self.lefthand.g_jointpositions[:6] if self.lefthand else []
+ },
+ 'right': {
+ 'connected': self.force_reader_right and self.force_reader_right.handtype == 'Right',
+ 'port': self.rightport,
+ 'baudrate': self.rightbaudrate,
+ 'version': self.force_reader_right.version if self.force_reader_right else None,
+ 'receive_count': self.force_reader_right.receive_count if self.force_reader_right else 0,
+ 'motor_positions': self.righthand.g_jointpositions[:6] if self.righthand else []
+ }
+ }
+ return stats
+
+
+class RetargetIntegrationTest:
+ """Retarget集成测试"""
+
+ def __init__(self):
+ self.retarget = None
+ self.test_results = {}
+
+ def setup(self):
+ """初始化"""
+ print("=" * 60)
+ print("LinkerForce Retarget 集成测试 (无ROS)")
+ print("=" * 60)
+
+ self.retarget = RetargetCore(
+ lefthand=RobotName.l21,
+ righthand=RobotName.l21,
+ calibration=False
+ )
+ self.retarget.linkerforce_init()
+ return True
+
+ def teardown(self):
+ """清理"""
+ if self.retarget:
+ self.retarget.stop()
+ print("\n设备已关闭")
+
+ def test_device_connection(self):
+ """测试1: 设备连接"""
+ print("\n" + "-" * 40)
+ print("[测试1] 设备连接")
+ print("-" * 40)
+
+ stats = self.retarget.get_stats()
+
+ left_ok = stats['left']['connected']
+ right_ok = stats['right']['connected']
+
+ print(f" 左手: {'✅ 已连接' if left_ok else '❌ 未连接'}")
+ if left_ok:
+ print(f" 端口: {stats['left']['port']} @ {stats['left']['baudrate']}")
+ print(f" 版本: {stats['left']['version']}")
+
+ print(f" 右手: {'✅ 已连接' if right_ok else '❌ 未连接'}")
+ if right_ok:
+ print(f" 端口: {stats['right']['port']} @ {stats['right']['baudrate']}")
+ print(f" 版本: {stats['right']['version']}")
+
+ result = left_ok or right_ok
+ print(f" {'✅ 通过' if result else '❌ 失败'}")
+ return result
+
+ def test_joint_mapping(self, duration=5):
+ """测试2: 关节映射"""
+ print("\n" + "-" * 40)
+ print(f"[测试2] 关节映射 ({duration}s)")
+ print("-" * 40)
+
+ self.retarget.start_processing(rate_hz=30)
+ time.sleep(duration)
+ self.retarget.stop_processing()
+
+ stats = self.retarget.get_stats()
+
+ if stats['left']['connected']:
+ motor_pos = stats['left']['motor_positions']
+ print(f" 左手电机位置: {motor_pos}")
+ valid = all(0 <= p <= 255 for p in motor_pos)
+ print(f" 左手映射有效性: {'✅' if valid else '❌'}")
+
+ if stats['right']['connected']:
+ motor_pos = stats['right']['motor_positions']
+ print(f" 右手电机位置: {motor_pos}")
+ valid = all(0 <= p <= 255 for p in motor_pos)
+ print(f" 右手映射有效性: {'✅' if valid else '❌'}")
+
+ result = True
+ print(f" {'✅ 通过' if result else '❌ 失败'}")
+ return result
+
+ def test_data_rate(self, duration=10):
+ """测试3: 数据帧率"""
+ print("\n" + "-" * 40)
+ print(f"[测试3] 数据帧率 ({duration}s)")
+ print("-" * 40)
+
+ self.retarget.start_processing(rate_hz=30)
+
+ initial_left = self.retarget.force_reader_left.receive_count if self.retarget.force_reader_left else 0
+ initial_right = self.retarget.force_reader_right.receive_count if self.retarget.force_reader_right else 0
+
+ time.sleep(duration)
+
+ self.retarget.stop_processing()
+
+ final_left = self.retarget.force_reader_left.receive_count if self.retarget.force_reader_left else 0
+ final_right = self.retarget.force_reader_right.receive_count if self.retarget.force_reader_right else 0
+
+ left_frames = final_left - initial_left
+ right_frames = final_right - initial_right
+
+ left_fps = left_frames / duration if left_frames > 0 else 0
+ right_fps = right_frames / duration if right_frames > 0 else 0
+
+ if left_frames > 0:
+ print(f" 左手: {left_frames} 帧, {left_fps:.1f} Hz")
+ if right_frames > 0:
+ print(f" 右手: {right_frames} 帧, {right_fps:.1f} Hz")
+
+ result = left_fps >= 5 or right_fps >= 5
+ print(f" {'✅ 通过' if result else '❌ 帧率过低'}")
+ return result
+
+ def test_frame_interval(self, duration=5):
+ """测试4: 帧间隔分析"""
+ print("\n" + "-" * 40)
+ print(f"[测试4] 帧间隔分析 ({duration}s)")
+ print("-" * 40)
+
+ self.retarget.start_processing(rate_hz=30)
+ time.sleep(duration)
+ self.retarget.stop_processing()
+
+ stats = self.retarget.get_stats()
+
+ if self.retarget.receive_times_left and len(self.retarget.receive_times_left) >= 2:
+ times = self.retarget.receive_times_left
+ intervals = [(times[i] - times[i-1]) * 1000 for i in range(1, len(times))]
+ avg = statistics.mean(intervals)
+ std = statistics.stdev(intervals) if len(intervals) > 1 else 0
+ print(f" 左手帧间隔: 平均 {avg:.2f}ms, 标准差 {std:.2f}ms")
+
+ if self.retarget.receive_times_right and len(self.retarget.receive_times_right) >= 2:
+ times = self.retarget.receive_times_right
+ intervals = [(times[i] - times[i-1]) * 1000 for i in range(1, len(times))]
+ avg = statistics.mean(intervals)
+ std = statistics.stdev(intervals) if len(intervals) > 1 else 0
+ print(f" 右手帧间隔: 平均 {avg:.2f}ms, 标准差 {std:.2f}ms")
+
+ result = True
+ print(f" {'✅ 通过' if result else '❌ 失败'}")
+ return result
+
+ def test_calibration(self, duration_per_pose=3):
+ """测试5: 标定流程"""
+ print("\n" + "-" * 40)
+ print("[测试5] 标定流程")
+ print("-" * 40)
+
+ if not self.retarget.force_reader_left and not self.retarget.force_reader_right:
+ print(" ⚠️ 无设备，跳过")
+ return True
+
+ print(" 开始标定 (每个姿势 3 秒)...")
+ result = self.retarget.run_calibration(duration_per_pose=duration_per_pose)
+
+ if result:
+ print(f" 左手张开数据: {self.retarget.lefthand.calibrationoriginal[:5] if self.retarget.lefthand.calibrationoriginal else 'N/A'}...")
+ print(f" 左手握拳数据: {self.retarget.lefthand.calibrationfistpose[:5] if self.retarget.lefthand.calibrationfistpose else 'N/A'}...")
+
+ print(f" {'✅ 标定完成' if result else '❌ 标定失败'}")
+ return result
+
+ def generate_report(self):
+ """生成报告"""
+ print("\n" + "=" * 60)
+ print("测试报告")
+ print("=" * 60)
+
+ stats = self.retarget.get_stats() if self.retarget else {}
+
+ print("\n## 设备状态")
+ if stats.get('left', {}).get('connected'):
+ print(f"- 左手: {stats['left']['port']} @ {stats['left']['baudrate']}, 版本 {stats['left']['version']}")
+ if stats.get('right', {}).get('connected'):
+ print(f"- 右手: {stats['right']['port']} @ {stats['right']['baudrate']}, 版本 {stats['right']['version']}")
+
+ print("\n## 测试结果")
+ passed = sum(1 for r in self.test_results.values() if r)
+ total = len(self.test_results)
+ print(f"- 通过: {passed}/{total}")
+
+ for name, result in self.test_results.items():
+ print(f" - {name}: {'✅' if result else '❌'}")
+
+ print("\n" + "=" * 60)
+
+ def run_all_tests(self):
+ """运行所有测试"""
+ if not self.setup():
+ return
+
+ try:
+ self.test_results['测试1-设备连接'] = self.test_device_connection()
+ self.test_results['测试2-关节映射'] = self.test_joint_mapping(duration=5)
+ self.test_results['测试3-数据帧率'] = self.test_data_rate(duration=10)
+ self.test_results['测试4-帧间隔'] = self.test_frame_interval(duration=5)
+ # self.test_results['测试5-标定流程'] = self.test_calibration(duration_per_pose=3)
+ except Exception as e:
+ print(f"\n测试中断: {e}")
+ import traceback
+ traceback.print_exc()
+ finally:
+ self.teardown()
+
+ self.generate_report()
+
+
+if __name__ == "__main__":
+ test = RetargetIntegrationTest()
+ test.run_all_tests()
\ No newline at end of file
diff --git a/src/linkerhand_retarget/tests/integration/test_linkermcg.py b/src/linkerhand_retarget/tests/integration/test_linkermcg.py
new file mode 100644
index 0000000..3af0758
--- /dev/null
+++ b/src/linkerhand_retarget/tests/integration/test_linkermcg.py
@@ -0,0 +1,159 @@
+#!/usr/bin/env python3
+"""LinkerMCG 集成测试 - UDP 客户端"""
+import time
+import sys
+sys.path.insert(0, '/home/linker-brunt/project/linkerhand_telop_sdk/old_git/new/ros2/src/linkerhand_retarget')
+
+from linkerhand_retarget.linkerhand.linkermcgcore import HaoCunScoketUdp, HaoCunData
+
+
+class LinkerMCGIntegrationTest:
+ def __init__(self):
+ self.client = None
+ self.test_results = {}
+
+ def setup(self, host='192.168.1.23', port=8888):
+ print("=" * 60)
+ print("LinkerMCG 集成测试")
+ print("=" * 60)
+
+ print(f"\n[初始化] 连接 UDP {host}:{port}...")
+
+ self.client = HaoCunScoketUdp(host=host, port=port)
+ result = self.client.udp_initial()
+
+ if result:
+ print(f"✅ UDP 初始化成功")
+ return True
+ else:
+ print(f"❌ UDP 初始化失败")
+ return False
+
+ def teardown(self):
+ if self.client:
+ self.client.udp_close()
+ print("\n设备已关闭")
+
+ def test_connection(self):
+ print("\n" + "-" * 40)
+ print("[测试1] 连接状态")
+ print("-" * 40)
+
+ is_connect = self.client.udp_is_connect()
+ print(f" is_connected: {is_connect}")
+
+ print(f" {'✅ 通过' if is_connect else '❌ 失败'}")
+ return is_connect
+
+ def test_receive_data(self, duration=5):
+ print("\n" + "-" * 40)
+ print(f"[测试2] 数据接收 ({duration}秒)")
+ print("-" * 40)
+
+ frame_count = 0
+ last_frame = 0
+ start_time = time.time()
+
+ while time.time() - start_time < duration:
+ time.sleep(0.5)
+ current_frame = self.client.realmocapdata.frame_index
+ if current_frame > last_frame:
+ frame_count += current_frame - last_frame
+ last_frame = current_frame
+ print(f" 接收帧: {current_frame}")
+
+ print(f" 总帧数: {frame_count}")
+ print(f" 帧率: {frame_count / duration:.1f} Hz")
+
+ result = frame_count > 0
+ print(f" {'✅ 通过' if result else '❌ 无数据'}")
+ return result
+
+ def test_data_content(self):
+ print("\n" + "-" * 40)
+ print("[测试3] 数据内容")
+ print("-" * 40)
+
+ time.sleep(1)
+
+ data = self.client.realmocapdata
+ print(f" frame_index: {data.frame_index}")
+ print(f" is_update: {data.is_update}")
+
+ r_hand = data.jointangle_rHand
+ l_hand = data.jointangle_lHand
+
+ print(f" 右手关节数: {len(r_hand)}")
+ print(f" 左手关节数: {len(l_hand)}")
+
+ r_nonzero = any(v != 0.0 for v in r_hand)
+ l_nonzero = any(v != 0.0 for v in l_hand)
+
+ if r_nonzero:
+ print(f" 右手关节示例: {[f'{v:.2f}' for v in r_hand[:5]]}")
+ if l_nonzero:
+ print(f" 左手关节示例: {[f'{v:.2f}' for v in l_hand[:5]]}")
+
+ result = len(r_hand) == 25 and len(l_hand) == 25
+ print(f" {'✅ 通过' if result else '❌ 失败'}")
+ return result
+
+ def test_continuous_read(self, duration=10):
+ print("\n" + "-" * 40)
+ print(f"[测试4] 连续读取 ({duration}秒)")
+ print("-" * 40)
+
+ frames = []
+ start_time = time.time()
+
+ while time.time() - start_time < duration:
+ time.sleep(1)
+ frame = self.client.realmocapdata.frame_index
+ frames.append(frame)
+ print(f" {int(time.time() - start_time)}s: frame={frame}")
+
+ if len(frames) >= 2:
+ frame_diff = frames[-1] - frames[0]
+ avg_fps = frame_diff / (duration - 1) if duration > 1 else 0
+ print(f" 平均帧率: {avg_fps:.1f} Hz")
+
+ result = len(frames) > 0
+ print(f" {'✅ 通过' if result else '❌ 失败'}")
+ return result
+
+ def generate_report(self):
+ print("\n" + "=" * 60)
+ print("测试报告")
+ print("=" * 60)
+
+ print("\n## 测试结果")
+ passed = sum(1 for r in self.test_results.values() if r)
+ total = len(self.test_results)
+ print(f"- 通过: {passed}/{total}")
+
+ for name, result in self.test_results.items():
+ status = "✅ 通过" if result else "❌ 失败"
+ print(f" - {name}: {status}")
+
+ print("\n" + "=" * 60)
+
+ def run_all_tests(self, host='192.168.1.23', port=8888):
+ if not self.setup(host, port):
+ return
+
+ try:
+ self.test_results['测试1-连接状态'] = self.test_connection()
+ self.test_results['测试2-数据接收'] = self.test_receive_data(duration=5)
+ self.test_results['测试3-数据内容'] = self.test_data_content()
+ self.test_results['测试4-连续读取'] = self.test_continuous_read(duration=10)
+ except Exception as e:
+ print(f"\n测试中断: {e}")
+ finally:
+ self.teardown()
+
+ self.generate_report()
+
+
+if __name__ == "__main__":
+ test = LinkerMCGIntegrationTest()
+ test.run_all_tests(host='192.168.11.88', port=9000)
\ No newline at end of file
diff --git a/src/linkerhand_retarget/tests/integration/test_udexreal.py b/src/linkerhand_retarget/tests/integration/test_udexreal.py
new file mode 100644
index 0000000..9984234
--- /dev/null
+++ b/src/linkerhand_retarget/tests/integration/test_udexreal.py
@@ -0,0 +1,191 @@
+#!/usr/bin/env python3
+"""UdexReal 集成测试 - UDP 设备连接"""
+import time
+import sys
+sys.path.insert(0, '/home/linker-brunt/project/linkerhand_telop_sdk/old_git/new/ros2/src/linkerhand_retarget')
+
+from linkerhand_retarget.linkerhand.udexrealcore import UdexRealScoketUdp, UdexRealData
+
+
+class UdexRealIntegrationTest:
+ def __init__(self):
+ self.client = None
+ self.test_results = {}
+
+ def setup(self, host='0.0.0.0', port=8888):
+ """初始化设备连接"""
+ print("=" * 60)
+ print("UdexReal 集成测试")
+ print("=" * 60)
+
+ print(f"\n[初始化] 连接 UDP {host}:{port}...")
+
+ self.client = UdexRealScoketUdp(host=host, port=port)
+ result = self.client.udp_initial()
+
+ if result:
+ print(f"✅ UDP 初始化成功")
+ return True
+ else:
+ print(f"❌ UDP 初始化失败")
+ return False
+
+ def teardown(self):
+ """关闭设备"""
+ if self.client:
+ self.client.udp_close()
+ print("\n设备已关闭")
+
+ def test_connection(self):
+ """测试1: 连接状态"""
+ print("\n" + "-" * 40)
+ print("[测试1] 连接状态")
+ print("-" * 40)
+
+ status = self.client.get_connection_status()
+ print(f" is_connected: {status['is_connected']}")
+ print(f" is_data_timeout: {status['is_data_timeout']}")
+
+ result = status['is_connected']
+ print(f" {'✅ 通过' if result else '❌ 失败'}")
+ return result
+
+ def test_receive_data(self, duration=5):
+ """测试2: 数据接收"""
+ print("\n" + "-" * 40)
+ print(f"[测试2] 数据接收 ({duration}秒)")
+ print("-" * 40)
+
+ frame_count = 0
+ last_frame = 0
+ start_time = time.time()
+
+ while time.time() - start_time < duration:
+ time.sleep(0.5)
+ current_frame = self.client.realmocapdata.frame_index
+ if current_frame > last_frame:
+ frame_count += current_frame - last_frame
+ last_frame = current_frame
+ print(f" 接收帧: {current_frame}")
+
+ print(f" 总帧数: {frame_count}")
+ print(f" 帧率: {frame_count / duration:.1f} Hz")
+
+ result = frame_count > 0
+ print(f" {'✅ 通过' if result else '❌ 无数据'}")
+ return result
+
+ def test_data_content(self):
+ """测试3: 数据内容"""
+ print("\n" + "-" * 40)
+ print("[测试3] 数据内容")
+ print("-" * 40)
+
+ time.sleep(1)
+
+ data = self.client.realmocapdata
+ print(f" frame_index: {data.frame_index}")
+ print(f" frequency: {data.frequency}")
+ print(f" is_update: {data.is_update}")
+
+ # 检查关节数据
+ r_hand = data.jointangle_rHand
+ l_hand = data.jointangle_lHand
+
+ print(f" 右手关节数: {len(r_hand)}")
+ print(f" 左手关节数: {len(l_hand)}")
+
+ # 检查是否有非零数据
+ r_nonzero = any(v != 0.0 for v in r_hand)
+ l_nonzero = any(v != 0.0 for v in l_hand)
+
+ if r_nonzero:
+ print(f" 右手关节示例: {r_hand[:5]}")
+ if l_nonzero:
+ print(f" 左手关节示例: {l_hand[:5]}")
+
+ result = len(r_hand) == 24 and len(l_hand) == 24
+ print(f" {'✅ 通过' if result else '❌ 失败'}")
+ return result
+
+ def test_timeout_detection(self, timeout=2):
+ """测试4: 超时检测"""
+ print("\n" + "-" * 40)
+ print(f"[测试4] 超时检测")
+ print("-" * 40)
+
+ timeout_status = self.client.check_timeout()
+ print(f" is_timeout: {timeout_status.is_timeout}")
+ print(f" time_since_last_data: {timeout_status.time_since_last_data:.3f}s")
+ print(f" timeout_threshold: {timeout_status.timeout_threshold}s")
+
+ result = True # 功能存在即通过
+ print(f" ✅ 超时检测功能正常")
+ return result
+
+ def test_continuous_read(self, duration=10):
+ """测试5: 连续读取"""
+ print("\n" + "-" * 40)
+ print(f"[测试5] 连续读取 ({duration}秒)")
+ print("-" * 40)
+
+ frames = []
+ start_time = time.time()
+
+ while time.time() - start_time < duration:
+ time.sleep(1)
+ frame = self.client.realmocapdata.frame_index
+ frames.append(frame)
+ timeout_status = self.client.check_timeout()
+ status = "⏰ 超时" if timeout_status.is_timeout else "✓"
+ print(f" {int(time.time() - start_time)}s: frame={frame} {status}")
+
+ # 计算帧率
+ if len(frames) >= 2:
+ frame_diff = frames[-1] - frames[0]
+ avg_fps = frame_diff / (duration - 1) if duration > 1 else 0
+ print(f" 平均帧率: {avg_fps:.1f} Hz")
+
+ result = len(frames) > 0
+ print(f" {'✅ 通过' if result else '❌ 失败'}")
+ return result
+
+ def generate_report(self):
+ """生成测试报告"""
+ print("\n" + "=" * 60)
+ print("测试报告")
+ print("=" * 60)
+
+ print("\n## 测试结果")
+ passed = sum(1 for r in self.test_results.values() if r)
+ total = len(self.test_results)
+ print(f"- 通过: {passed}/{total}")
+
+ for name, result in self.test_results.items():
+ status = "✅ 通过" if result else "❌ 失败"
+ print(f" - {name}: {status}")
+
+ print("\n" + "=" * 60)
+
+ def run_all_tests(self, host='0.0.0.0', port=8888):
+ """运行所有测试"""
+ if not self.setup(host, port):
+ return
+
+ try:
+ self.test_results['测试1-连接状态'] = self.test_connection()
+ self.test_results['测试2-数据接收'] = self.test_receive_data(duration=5)
+ self.test_results['测试3-数据内容'] = self.test_data_content()
+ self.test_results['测试4-超时检测'] = self.test_timeout_detection()
+ self.test_results['测试5-连续读取'] = self.test_continuous_read(duration=10)
+ except Exception as e:
+ print(f"\n测试中断: {e}")
+ finally:
+ self.teardown()
+
+ self.generate_report()
+
+
+if __name__ == "__main__":
+ test = UdexRealIntegrationTest()
+ test.run_all_tests(host='0.0.0.0', port=8888)
\ No newline at end of file
diff --git a/src/linkerhand_retarget/tests/integration/test_vtrdyn.py b/src/linkerhand_retarget/tests/integration/test_vtrdyn.py
new file mode 100644
index 0000000..dc38491
--- /dev/null
+++ b/src/linkerhand_retarget/tests/integration/test_vtrdyn.py
@@ -0,0 +1,162 @@
+#!/usr/bin/env python3
+"""VtrDyn 集成测试 - UDP 设备"""
+import time
+import sys
+sys.path.insert(0, '/home/linker-brunt/project/linkerhand_telop_sdk/old_git/new/ros2/src/linkerhand_retarget')
+
+from linkerhand_retarget.linkerhand.vtrdyncore import VtrdynSocketUdp, MocapData
+
+
+class VtrDynIntegrationTest:
+ def __init__(self):
+ self.client = None
+ self.test_results = {}
+
+ def setup(self, local_port=7000, remote_ip='192.168.11.88', remote_port=7000):
+ print("=" * 60)
+ print("VtrDyn 集成测试")
+ print("=" * 60)
+
+ print(f"\n[初始化] 本地端口 {local_port}, 远程 {remote_ip}:{remote_port}...")
+
+ self.client = VtrdynSocketUdp(debug=True)
+ result = self.client.udp_initial(local_port)
+
+ if not result:
+ print(f"❌ UDP 初始化失败")
+ return False
+
+ print(f"✅ UDP 初始化成功")
+
+ dst_addr = (remote_ip, remote_port)
+ conn_result = self.client.udp_send_request_connect(dst_addr)
+
+ if conn_result:
+ print(f"✅ 连接成功")
+ else:
+ print(f"⚠️ 发送连接请求，等待数据...")
+
+ return True
+
+ def teardown(self):
+ if self.client:
+ self.client.udp_close(('192.168.11.88', 7000))
+ print("\n设备已关闭")
+
+ def test_connection(self):
+ print("\n" + "-" * 40)
+ print("[测试1] 连接状态")
+ print("-" * 40)
+
+ is_connect = self.client.udp_is_onnect()
+ print(f" is_connected: {is_connect}")
+
+ print(f" {'✅ 通过' if is_connect else '❌ 失败'}")
+ return is_connect
+
+ def test_receive_data(self, duration=5):
+ print("\n" + "-" * 40)
+ print(f"[测试2] 数据接收 ({duration}秒)")
+ print("-" * 40)
+
+ frame_count = 0
+ last_frame = 0
+ start_time = time.time()
+
+ while time.time() - start_time < duration:
+ time.sleep(0.5)
+ current_frame = self.client.mocap_data_realtime.frame_index
+ if current_frame > last_frame:
+ frame_count += current_frame - last_frame
+ last_frame = current_frame
+ print(f" 接收帧: {current_frame}")
+
+ print(f" 总帧数: {frame_count}")
+ print(f" 帧率: {frame_count / duration:.1f} Hz")
+
+ result = frame_count > 0
+ print(f" {'✅ 通过' if result else '❌ 无数据'}")
+ return result
+
+ def test_data_content(self):
+ print("\n" + "-" * 40)
+ print("[测试3] 数据内容")
+ print("-" * 40)
+
+ time.sleep(1)
+
+ data = self.client.mocap_data_realtime
+ print(f" frame_index: {data.frame_index}")
+ print(f" frequency: {data.frequency}")
+ print(f" is_update: {data.is_update}")
+
+ print(f" 身体节点数: {len(data.position_body)}")
+ print(f" 右手节点数: {len(data.position_rHand)}")
+ print(f" 左手节点数: {len(data.position_lHand)}")
+
+ body_pos = data.position_body[0] if data.position_body else [0,0,0]
+ print(f" 身体位置示例: {[f'{v:.3f}' for v in body_pos]}")
+
+ result = len(data.position_body) == 23
+ print(f" {'✅ 通过' if result else '❌ 失败'}")
+ return result
+
+ def test_continuous_read(self, duration=10):
+ print("\n" + "-" * 40)
+ print(f"[测试4] 连续读取 ({duration}秒)")
+ print("-" * 40)
+
+ frames = []
+ start_time = time.time()
+
+ while time.time() - start_time < duration:
+ time.sleep(1)
+ frame = self.client.mocap_data_realtime.frame_index
+ frames.append(frame)
+ print(f" {int(time.time() - start_time)}s: frame={frame}")
+
+ if len(frames) >= 2:
+ frame_diff = frames[-1] - frames[0]
+ avg_fps = frame_diff / (duration - 1) if duration > 1 else 0
+ print(f" 平均帧率: {avg_fps:.1f} Hz")
+
+ result = len(frames) > 0
+ print(f" {'✅ 通过' if result else '❌ 失败'}")
+ return result
+
+ def generate_report(self):
+ print("\n" + "=" * 60)
+ print("测试报告")
+ print("=" * 60)
+
+ print("\n## 测试结果")
+ passed = sum(1 for r in self.test_results.values() if r)
+ total = len(self.test_results)
+ print(f"- 通过: {passed}/{total}")
+
+ for name, result in self.test_results.items():
+ status = "✅ 通过" if result else "❌ 失败"
+ print(f" - {name}: {status}")
+
+ print("\n" + "=" * 60)
+
+ def run_all_tests(self, local_port=7000, remote_ip='192.168.11.88', remote_port=7000):
+ if not self.setup(local_port, remote_ip, remote_port):
+ return
+
+ try:
+ self.test_results['测试1-连接状态'] = self.test_connection()
+ self.test_results['测试2-数据接收'] = self.test_receive_data(duration=5)
+ self.test_results['测试3-数据内容'] = self.test_data_content()
+ self.test_results['测试4-连续读取'] = self.test_continuous_read(duration=10)
+ except Exception as e:
+ print(f"\n测试中断: {e}")
+ finally:
+ self.teardown()
+
+ self.generate_report()
+
+
+if __name__ == "__main__":
+ test = VtrDynIntegrationTest()
+ test.run_all_tests(local_port=7000, remote_ip='192.168.11.88', remote_port=7000)
\ No newline at end of file
diff --git a/src/linkerhand_retarget/tests/unit/TEST_LINKERFORCE.md b/src/linkerhand_retarget/tests/unit/TEST_LINKERFORCE.md
new file mode 100644
index 0000000..39ba7b4
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/TEST_LINKERFORCE.md
@@ -0,0 +1,188 @@
+# LinkerForce 集成测试报告
+
+测试日期: 2026-03-03
+测试设备: LinkerForce 左手数据手套
+测试结果: 6/6 通过
+
+---
+
+## 启动命令
+
+```bash
+cd /home/linker-brunt/project/linkerhand_telop_sdk/old_git/new/ros2/src/linkerhand_retarget
+python3 tests/integration/test_linkerforce.py
+```
+
+前提条件:
+- LinkerForce 设备已连接到 `/dev/ttyUSB0`
+- 波特率: 2000000
+
+---
+
+## 测试环境
+
+- 串口: /dev/ttyUSB0
+- 波特率: 2000000
+- 设备类型: Left (左手)
+- 固件版本: 1.2.12
+- 总接收帧数: 1266
+
+---
+
+## 测试结果
+
+| 测试项 | 结果 | 关键指标 |
+|--------|------|----------|
+| 设备信息 | 通过 | handtype=Left, version=1.2.12 |
+| 数据稳定性 | 通过 | 标准差 0.00007 rad (0.004°) |
+| 数据包统计 | 通过 | 315帧/10秒, 数据变化率 95% |
+| 响应间隔 | 通过 | 平均 30.82ms, 帧率 20Hz |
+| 连续读取 | 通过 | 485帧/15秒, 稳定无断开 |
+| 协议测试 | 通过 | 6/6 协议通过 |
+
+---
+
+## 详细数据
+
+### 1. 设备信息测试
+
+```
+handtype: Left
+version: 1.2.12
+connflag: True
+```
+
+### 2. 位置数据稳定性测试 (5秒)
+
+- 采样数: 10
+- 通道数: 10
+- 平均标准差: 0.000071 rad (0.0041°)
+- 最大标准差: 0.000254 rad (0.0145°)
+
+数据稳定性良好。
+
+### 3. 数据包统计测试 (10秒)
+
+- 发送请求: 20
+- 接收帧数: 315
+- 数据变化: 19
+- 帧率: ~31.5 Hz
+
+设备持续输出数据。
+
+### 4. 响应间隔测试 (5秒)
+
+- 接收帧数: 100
+- 平均帧间隔: 30.82 ms
+- 最小帧间隔: 12.11 ms
+- 最大帧间隔: 42.87 ms
+- 帧率: 20.0 Hz
+
+响应及时稳定。
+
+### 5. 连续读取稳定性测试 (15秒)
+
+- 接收帧数: 485
+- 平均帧间隔: 500.68 ms
+- 帧率: 2.0 Hz (受测试间隔限制)
+- 设备断开: 0 次
+
+稳定运行，无断开无错误。
+
+### 6. 协议测试
+
+| 协议 | 功能 | 状态 |
+|------|------|------|
+| 0x01 | 设备信息 | 通过 |
+| 0x02 | 控制命令 | 通过 |
+| 0x03 | 位置数据 | 通过 (21 floats) |
+| 0x04 | 力数据 | 通过 |
+| 0xA4 | 力发送 | 通过 |
+| 0xA7 | 力发送变体 | 通过 |
+
+---
+
+## 协议说明
+
+### 支持的协议命令
+
+| 命令码 | 功能 | 方向 | 说明 |
+|--------|------|------|------|
+| 0x01 | 设备信息 | 主机→设备 | 查询设备类型和版本 |
+| 0x02 | 控制命令 | 主机→设备 | 发送控制参数 |
+| 0x03 | 位置数据 | 设备→主机 | 返回21个关节角度(float) |
+| 0x04 | 力数据 | 设备→主机 | 返回力传感器数据(int16) |
+| 0xA4 | 力发送 | 主机→设备 | 发送力反馈数据 |
+| 0xA7 | 力发送变体 | 主机→设备 | 发送力反馈数据(备用) |
+
+### 数据格式
+
+位置数据 (0x03):
+- 格式: 21个 float (小端序)
+- 单位: 弧度
+- 更新频率: ~20-30 Hz
+
+力数据 (0x04):
+- 格式: 5个 int16
+- 单位: 原始ADC值
+
+---
+
+## 测试结论
+
+设备工作正常:
+- 通信稳定: 无断开、无错误
+- 数据精确: 角度标准差 < 0.015°
+- 响应及时: 平均帧间隔 30ms, 帧率 20Hz
+- 协议完整: 6个协议全部通过
+
+---
+
+## 测试文件结构
+
+```
+tests/
+├── TEST_LINKERFORCE.md # 本测试报告
+├── unit/
+│ ├── test_linkerforce.py # LinkerForce 单元测试
+│ ├── test_filter.py # 滤波器测试
+│ ├── test_constants.py # 常量测试
+│ ├── test_handcore.py # HandCore测试
+│ ├── test_handcoreex.py # HandCoreEx测试
+│ ├── test_linkermcgcore.py # LinkerMCG测试
+│ ├── test_sensenovacore.py # SenseNova测试
+│ ├── test_udexrealcore.py # UdexReal测试
+│ ├── test_utils.py # 工具函数测试
+│ └── test_vtrdyncore.py # VtrDyn测试
+└── integration/
+ ├── test_linkerforce.py # LinkerForce 集成测试
+ └── test_config.py # 测试配置
+```
+
+### LinkerForce 测试文件
+
+单元测试 `tests/unit/test_linkerforce.py`:
+- CircularBuffer 测试 (7项)
+- FrameParser 测试 (5项)
+- 常量测试 (4项)
+
+集成测试 `tests/integration/test_linkerforce.py`:
+- 设备信息测试
+- 数据稳定性测试
+- 数据包统计测试
+- 响应间隔测试
+- 连续读取测试
+- 协议测试
+
+## 运行测试
+
+```bash
+# 进入测试目录
+cd ros2/src/linkerhand_retarget
+
+# 运行单元测试
+PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest tests/unit/ -v
+
+# 运行集成测试 (需连接设备)
+python3 tests/integration/test_linkerforce.py
+```
\ No newline at end of file
diff --git a/src/linkerhand_retarget/tests/unit/TEST_LINKERMCG.md b/src/linkerhand_retarget/tests/unit/TEST_LINKERMCG.md
new file mode 100644
index 0000000..addd793
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/TEST_LINKERMCG.md
@@ -0,0 +1,76 @@
+# LinkerMCG 集成测试报告
+
+**测试日期**: 2026-03-04
+**测试设备**: LinkerMCG 数据手套
+**测试结果**: ✅ 3/4 通过
+
+---
+
+## 测试环境
+
+| 项目 | 值 |
+|------|-----|
+| 协议 | UDP 客户端 |
+| 目标地址 | 192.168.11.88 |
+| 端口 | 9000 |
+| 关节数 | 25/手 |
+
+---
+
+## 测试结果总览
+
+| 测试项 | 结果 | 说明 |
+|--------|------|------|
+| 连接状态 | ✅ | UDP 连接成功 |
+| 数据接收 | ⚠️ | 启动延迟约6秒 | UDP软件手动启动
+| 数据内容 | ✅ | 数据结构正确 |
+| 连续读取 | ✅ | 帧率 14.8 Hz |
+
+---
+
+## 详细测试数据
+
+### 1. 连接状态测试
+
+```
+is_connected: True
+```
+
+### 2. 数据接收测试 (5秒)
+
+前5秒无数据，数据在第6秒后开始到达。
+
+### 3. 数据内容测试
+
+| 指标 | 值 |
+|------|-----|
+| 右手关节数 | 25 |
+| 左手关节数 | 25 |
+
+### 4. 连续读取测试 (10秒)
+
+| 指标 | 值 |
+|------|-----|
+| 最终帧数 | 133 |
+| 平均帧率 | 14.8 Hz |
+
+---
+
+## 测试结论
+
+1. **网络连接**: 正常
+2. **数据传输**: 正常，启动有延迟
+3. **帧率**: 14.8 Hz
+
+---
+
+## 运行测试
+
+```bash
+cd ros2/src/linkerhand_retarget
+python3 tests/integration/test_linkermcg.py
+```
+
+---
+
+**报告生成时间**: 2026-03-04
\ No newline at end of file
diff --git a/src/linkerhand_retarget/tests/unit/TEST_UDEXREAL.md b/src/linkerhand_retarget/tests/unit/TEST_UDEXREAL.md
new file mode 100644
index 0000000..82b5054
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/TEST_UDEXREAL.md
@@ -0,0 +1,121 @@
+# UdexReal 集成测试报告
+
+**测试日期**: 2026-03-03
+**测试设备**: UdexReal 动捕设备
+**测试结果**: ✅ 5/5 通过
+
+---
+
+## 测试环境
+
+| 项目 | 值 |
+|------|-----|
+| 协议 | UDP |
+| 本地地址 | 0.0.0.0 |
+| 端口 | 8888 |
+| 帧率 | ~124 Hz |
+
+---
+
+## 测试结果总览
+
+| 测试项 | 结果 | 关键指标 |
+|--------|------|----------|
+| 连接状态 | ✅ | UDP 连接成功 |
+| 数据接收 | ✅ | 619帧/5秒, 123.8 Hz |
+| 数据内容 | ✅ | 24个关节/手 |
+| 超时检测 | ✅ | 功能正常 |
+| 连续读取 | ✅ | 123.7 Hz, 无断开 |
+
+---
+
+## 详细测试数据
+
+### 1. 连接状态测试
+
+```
+is_connected: True
+is_data_timeout: False
+```
+
+### 2. 数据接收测试 (5秒)
+
+| 指标 | 值 |
+|------|-----|
+| 接收帧数 | 619 |
+| 帧率 | 123.8 Hz |
+
+### 3. 数据内容测试
+
+| 指标 | 值 |
+|------|-----|
+| frame_index | 741 |
+| 右手关节数 | 24 |
+| 左手关节数 | 24 |
+| 左手关节示例 | [0.0, -0.82, 0.17, 0.44, -0.23] |
+
+### 4. 超时检测测试
+
+| 指标 | 值 |
+|------|-----|
+| is_timeout | False |
+| time_since_last_data | 0.008s |
+| timeout_threshold | 1.0s |
+
+### 5. 连续读取测试 (10秒)
+
+| 指标 | 值 |
+|------|-----|
+| 平均帧率 | 123.7 Hz |
+| 超时次数 | 0 |
+
+---
+
+## 数据质量评估
+
+### 通信性能
+
+| 指标 | 值 | 评估 |
+|------|-----|------|
+| 帧率 | 123.7 Hz | 优秀 |
+| 连接稳定性 | 无断开 | ✅ |
+| 超时次数 | 0 | ✅ |
+
+### 数据格式
+
+**关节数据:**
+- 关节数: 24/手
+- 数据类型: float (弧度)
+- 更新频率: ~124 Hz
+
+---
+
+## 测试结论
+
+### 总体评价
+
+✅ **设备工作正常**
+
+1. **连接稳定**: UDP 连接正常
+2. **帧率高**: 124 Hz
+3. **数据完整**: 24个关节数据
+4. **超时检测**: 功能正常
+
+---
+
+## 运行测试
+
+```bash
+# 进入测试目录
+cd ros2/src/linkerhand_retarget
+
+# 运行单元测试
+PYTEST_DISABLE_PLUGIN_AUTOLOAD=1 python3 -m pytest tests/unit/test_udexrealcore.py -v
+
+# 运行集成测试 (需连接设备)
+python3 tests/integration/test_udexreal.py
+```
+
+---
+
+**报告生成时间**: 2026-03-03
\ No newline at end of file
diff --git a/src/linkerhand_retarget/tests/unit/TEST_VTRDYN.md b/src/linkerhand_retarget/tests/unit/TEST_VTRDYN.md
new file mode 100644
index 0000000..4aa946a
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/TEST_VTRDYN.md
@@ -0,0 +1,97 @@
+# VtrDyn 集成测试报告
+
+**测试日期**: 2026-03-04
+**测试设备**: VtrDyn 动捕设备
+**测试结果**: ✅ 4/4 通过
+
+---
+
+## 测试环境
+
+| 项目 | 值 |
+|------|-----|
+| 协议 | UDP |
+| 本地端口 | 7000 |
+| 远程地址 | 192.168.11.88:7000 |
+
+---
+
+## 测试结果总览
+
+| 测试项 | 结果 | 说明 |
+|--------|------|------|
+| 连接状态 | ✅ | UDP 连接成功 |
+| 数据接收 | ✅ | 帧率 27.8 Hz |
+| 数据内容 | ✅ | 数据结构正确 |
+| 连续读取 | ✅ | 持续接收数据 |
+
+---
+
+## 详细测试数据
+
+### 1. 连接状态测试
+
+```
+is_connected: True
+Connection established
+```
+
+### 2. 数据接收测试 (5秒)
+
+| 指标 | 值 |
+|------|-----|
+| 接收帧数 | 139 |
+| 帧率 | 27.8 Hz |
+
+### 3. 数据内容测试
+
+| 指标 | 值 |
+|------|-----|
+| frame_index | 166 |
+| frequency | 60 |
+| 身体节点数 | 23 |
+| 右手节点数 | 20 |
+| 左手节点数 | 20 |
+
+### 4. 连续读取测试 (10秒)
+
+| 指标 | 值 |
+|------|-----|
+| 最终帧数 | 221 |
+
+---
+
+## 数据格式
+
+**节点配置:**
+- 身体节点: 23
+- 右手节点: 20
+- 左手节点: 20
+
+**数据类型:**
+- 位置 (position)
+- 四元数 (quaternion)
+- 陀螺仪 (gyr)
+- 加速度计 (acc)
+- 速度 (velocity)
+
+---
+
+## 测试结论
+
+1. 连接正常
+2. 数据接收正常
+3. 帧率稳定
+
+---
+
+## 运行测试
+
+```bash
+cd ros2/src/linkerhand_retarget
+python3 tests/integration/test_vtrdyn.py
+```
+
+---
+
+**报告生成时间**: 2026-03-04
\ No newline at end of file
diff --git a/src/linkerhand_retarget/tests/unit/__init__.py b/src/linkerhand_retarget/tests/unit/__init__.py
new file mode 100644
index 0000000..322f1e0
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/__init__.py
@@ -0,0 +1 @@
+# Unit tests for linkerhand_retarget
diff --git a/src/linkerhand_retarget/tests/unit/plot_mapping_curve.py b/src/linkerhand_retarget/tests/unit/plot_mapping_curve.py
new file mode 100644
index 0000000..427c7a0
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/plot_mapping_curve.py
@@ -0,0 +1,236 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Mapping Curve Visualization Script
+Plot open → opose → fist sensor and motor value curves for a single DOF
+
+Usage:
+ python3 plot_mapping_curve.py
+
+ joint_index: 0-19 (robot_idx)
+
+ Joint mapping:
+ 0: Thumb Rotate
+ 1: Thumb Abduction
+ 2: Thumb Root Flexion
+ 3: Thumb End Flexion
+ 5: Index Roll
+ 6: Index Root Flexion
+ 7: Index End Flexion
+ 9: Middle Roll
+ 10: Middle Root Flexion
+ 11: Middle End Flexion
+ 13: Ring Roll
+ 14: Ring Root Flexion
+ 15: Ring End Flexion
+ 17: Pinky Roll
+ 18: Pinky Root Flexion
+ 19: Pinky End Flexion
+
+Example:
+ python3 plot_mapping_curve.py 6 # Index Root Flexion
+"""
+
+import sys
+import json
+import numpy as np
+import matplotlib.pyplot as plt
+from pathlib import Path
+
+# Joint name mapping
+JOINT_NAMES = {
+ 0: 'Thumb Rotate',
+ 1: 'Thumb Abduction',
+ 2: 'Thumb Root Flexion',
+ 3: 'Thumb End Flexion',
+ 5: 'Index Roll',
+ 6: 'Index Root Flexion',
+ 7: 'Index End Flexion',
+ 9: 'Middle Roll',
+ 10: 'Middle Root Flexion',
+ 11: 'Middle End Flexion',
+ 13: 'Ring Roll',
+ 14: 'Ring Root Flexion',
+ 15: 'Ring End Flexion',
+ 17: 'Pinky Roll',
+ 18: 'Pinky Root Flexion',
+ 19: 'Pinky End Flexion',
+}
+
+# Sensor index mapping (sensor array index for each robot_idx)
+SENSOR_MAP = {
+ 0: 1, # Thumb Rotate -> sensor[1]
+ 1: 0, # Thumb Abduction -> sensor[0]
+ 2: 2, # Thumb Root Flexion -> sensor[2]
+ 3: 4, # Thumb End Flexion -> sensor[4]
+ 5: 5, # Index Roll -> sensor[5]
+ 6: 6, # Index Root Flexion -> sensor[6]
+ 7: 8, # Index End Flexion -> sensor[8]
+ 9: 9, # Middle Roll -> sensor[9]
+ 10: 10, # Middle Root Flexion -> sensor[10]
+ 11: 12, # Middle End Flexion -> sensor[12]
+ 13: 12, # Ring Roll -> sensor[12] (shared with middle)
+ 14: 14, # Ring Root Flexion -> sensor[14]
+ 15: 16, # Ring End Flexion -> sensor[16]
+ 17: 17, # Pinky Roll -> sensor[17]
+ 18: 18, # Pinky Root Flexion -> sensor[18]
+ 19: 20, # Pinky End Flexion -> sensor[20]
+}
+
+# exp_factor for each joint
+EXP_FACTORS = {
+ 0: 1, # Thumb Rotate
+ 1: 1, # Thumb Abduction
+ 2: 5, # Thumb Root Flexion
+ 3: 7, # Thumb End Flexion
+ 5: 1, # Index Roll
+ 6: 4, # Index Root Flexion
+ 7: 3, # Index End Flexion
+ 9: 1, # Middle Roll
+ 10: 5, # Middle Root Flexion
+ 11: 10, # Middle End Flexion
+ 13: 1, # Ring Roll
+ 14: 5, # Ring Root Flexion
+ 15: 18, # Ring End Flexion
+ 17: 1, # Pinky Roll
+ 18: 5, # Pinky Root Flexion
+ 19: 8, # Pinky End Flexion
+}
+
+TMP_FILE = Path(__file__).resolve().parent.parent.parent / "linkerhand_retarget" / "motion" / "linkerforce" / "tmp" / "jointangle_data.tmp"
+
+MOTOR_OPEN = 255
+MOTOR_OPOSE = 128
+MOTOR_FIST = 0
+
+def map_value(sensor_val, sensor_open, sensor_opose, sensor_fist, exp_factor, debug=False):
+ if abs(sensor_opose - sensor_open) < 1e-6:
+ normalized = 0.5
+ else:
+ normalized = (sensor_val - sensor_open) / (sensor_opose - sensor_open)
+
+ if normalized <= 0:
+ return MOTOR_OPEN, normalized
+ elif normalized <= 1:
+ return MOTOR_OPEN + normalized * (MOTOR_OPOSE - MOTOR_OPEN), normalized
+ else:
+ normalized_fist = (sensor_fist - sensor_open) / (sensor_opose - sensor_open) if abs(sensor_opose - sensor_open) > 1e-6 else 1.5
+ t_max = normalized_fist - 1.0
+ t = min(normalized - 1.0, t_max)
+ slope = MOTOR_FIST - MOTOR_OPOSE
+ S1 = MOTOR_OPOSE - MOTOR_OPEN
+ k = slope / (t_max * S1) - 1
+ ratio = t / t_max
+ extension = slope * (ratio + k * ratio ** exp_factor) / (1 + k)
+ result = MOTOR_OPOSE + extension
+ if debug:
+ print(f"normalized_fist={normalized_fist:.4f}, t_max={t_max:.4f}, slope={slope}, S1={S1}, k={k:.4f}, exp_factor={exp_factor}")
+ return max(MOTOR_FIST, result), normalized
+
+def generate_curve_data(sensor_open, sensor_opose, sensor_fist, exp_factor, steps=200):
+ sensor_min = min(sensor_open, sensor_fist) - 0.1
+ sensor_max = max(sensor_open, sensor_opose, sensor_fist) + 0.2
+
+ sensor_values = np.linspace(sensor_min, sensor_max, steps)
+ motor_values = []
+ normalized_values = []
+
+ for s in sensor_values:
+ motor, normalized = map_value(s, sensor_open, sensor_opose, sensor_fist, exp_factor)
+ motor_values.append(motor)
+ normalized_values.append(normalized)
+
+ return sensor_values, motor_values, normalized_values
+
+def main():
+ if len(sys.argv) < 2:
+ print(__doc__)
+ return
+
+ try:
+ joint_idx = int(sys.argv[1])
+ except ValueError:
+ print(f"Error: joint_index must be an integer")
+ print(__doc__)
+ return
+
+ if joint_idx not in JOINT_NAMES:
+ print(f"Error: joint_index {joint_idx} not found")
+ print("Valid indices:", sorted(JOINT_NAMES.keys()))
+ return
+
+ sensor_idx = SENSOR_MAP.get(joint_idx, joint_idx)
+ exp_factor = EXP_FACTORS.get(joint_idx, 1)
+ joint_name = JOINT_NAMES[joint_idx]
+
+ # Load calibration data
+ with open(TMP_FILE) as f:
+ data = json.load(f)
+
+ open_r = data['jointangleoriginal_r']
+ opose_r = data['jointangleopose_r']
+ fist_r = data['jointanglefist_r']
+
+ if sensor_idx >= len(open_r):
+ print(f"Error: sensor_idx {sensor_idx} out of range")
+ return
+
+ sensor_open = open_r[sensor_idx]
+ sensor_opose = opose_r[sensor_idx]
+ sensor_fist = fist_r[sensor_idx]
+
+ # Generate curve data
+ sensor_vals, motor_vals, normalized_vals = generate_curve_data(sensor_open, sensor_opose, sensor_fist, exp_factor)
+
+ # Print parameters
+ map_value(sensor_opose + 0.01, sensor_open, sensor_opose, sensor_fist, exp_factor, debug=True)
+
+ # Verify motor at normalized=1.2
+ sensor_1_2 = sensor_open + 1.2 * (sensor_opose - sensor_open)
+ motor_1_2, _ = map_value(sensor_1_2, sensor_open, sensor_opose, sensor_fist, exp_factor)
+ print(f"motor at normalized=1.2: {motor_1_2:.1f}")
+
+ # Create plot
+ fig, ax = plt.subplots(figsize=(10, 6))
+ ax2 = ax.twinx()
+
+ line1, = ax.plot(normalized_vals, motor_vals, color='#4ECDC4', linewidth=2.5, label='Motor Value')
+ line2, = ax2.plot(normalized_vals, sensor_vals, color='gray', linewidth=1.5, linestyle='--', label='Sensor Value')
+
+ ax.axvline(x=0, color='green', linestyle=':', alpha=0.7, linewidth=1.5, label='open (normalized=0)')
+ ax.axvline(x=1, color='orange', linestyle=':', alpha=0.7, linewidth=1.5, label='opose (normalized=1)')
+
+ normalized_fist = (sensor_fist - sensor_open) / (sensor_opose - sensor_open) if abs(sensor_opose - sensor_open) > 1e-6 else 0.5
+ if normalized_fist > 1:
+ ax.axvline(x=normalized_fist, color='red', linestyle=':', alpha=0.7, linewidth=1.5, label=f'fist (normalized={normalized_fist:.2f})')
+
+ ax.axhline(y=MOTOR_OPEN, color='green', linestyle=':', alpha=0.3)
+ ax.axhline(y=MOTOR_OPOSE, color='orange', linestyle=':', alpha=0.3)
+ ax.axhline(y=MOTOR_FIST, color='red', linestyle=':', alpha=0.3)
+
+ ax.scatter([0, 1], [MOTOR_OPEN, MOTOR_OPOSE], color='black', s=80, zorder=5)
+ if normalized_fist > 1:
+ motor_at_fist, _ = map_value(sensor_fist, sensor_open, sensor_opose, sensor_fist, exp_factor)
+ ax.scatter([normalized_fist], [motor_at_fist], color='red', s=100, zorder=5, marker='*')
+
+ ax.set_xlabel('Normalized Sensor Value', fontsize=11)
+ ax.set_ylabel('Motor Value', fontsize=11)
+ ax2.set_ylabel('Sensor Raw Value', fontsize=11)
+ ax.set_title(f'{joint_name} (robot_idx={joint_idx}, exp_factor={exp_factor})', fontsize=13, fontweight='bold')
+
+ lines = [line1, line2]
+ labels = [l.get_label() for l in lines]
+ ax.legend(lines, labels, loc='upper right', fontsize=9)
+
+ ax.grid(True, alpha=0.3)
+ ax.set_ylim(-20, 280)
+
+ plt.tight_layout()
+
+ output_path = Path(__file__).parent / "images" / f"mapping_curve_joint_{joint_idx}.png"
+ plt.savefig(output_path, dpi=150, bbox_inches='tight')
+ print(f"Chart saved to: {output_path}")
+ plt.close()
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/linkerhand_retarget/tests/unit/test_constants.py b/src/linkerhand_retarget/tests/unit/test_constants.py
new file mode 100644
index 0000000..f6cc48c
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/test_constants.py
@@ -0,0 +1,99 @@
+import pytest
+from linkerhand_retarget.linkerhand.constants import (
+ RobotName,
+ RetargetingType,
+ HandType,
+ DataSource,
+ MotionSource,
+ ROBOT_NAME_MAP,
+ ROBOT_LEN_MAP,
+ OPERATOR2MANO,
+ OPERATOR2MANO_RIGHT,
+ OPERATOR2MANO_LEFT,
+ get_default_config_path,
+)
+
+
+class TestRobotName:
+ def test_enum_values(self):
+ assert RobotName.o7.value is not None
+ assert RobotName.l6.value is not None
+ assert RobotName.l20.value is not None
+
+ def test_robot_names_list(self):
+ from linkerhand_retarget.linkerhand.constants import ROBOT_NAMES
+ assert len(ROBOT_NAMES) > 0
+ assert RobotName.o7 in ROBOT_NAMES
+
+
+class TestRetargetingType:
+ def test_enum_values(self):
+ assert RetargetingType.vector is not None
+ assert RetargetingType.position is not None
+ assert RetargetingType.dexpilot is not None
+ assert RetargetingType.projection is not None
+
+
+class TestHandType:
+ def test_enum_values(self):
+ assert HandType.right is not None
+ assert HandType.left is not None
+
+
+class TestDataSource:
+ def test_enum_values(self):
+ assert DataSource.motion is not None
+ assert DataSource.video is not None
+ assert DataSource.vr is not None
+
+
+class TestMotionSource:
+ def test_enum_values(self):
+ assert MotionSource.vtrdyn is not None
+ assert MotionSource.udexreal is not None
+ assert MotionSource.linkerforce is not None
+
+
+class TestRobotNameMap:
+ def test_robot_name_map(self):
+ assert ROBOT_NAME_MAP[RobotName.o7] == "linker_hand_o7"
+ assert ROBOT_NAME_MAP[RobotName.l6] == "linker_hand_l6"
+ assert ROBOT_NAME_MAP[RobotName.l20] == "linker_hand_l20"
+ assert ROBOT_NAME_MAP[RobotName.l25] == "linker_hand_l25"
+
+ def test_robot_len_map(self):
+ assert ROBOT_LEN_MAP[RobotName.o7] == 7
+ assert ROBOT_LEN_MAP[RobotName.l6] == 6
+ assert ROBOT_LEN_MAP[RobotName.l20] == 20
+ assert ROBOT_LEN_MAP[RobotName.l25] == 25
+
+
+class TestOperatorToMano:
+ def test_operator2mano_right(self):
+ assert OPERATOR2MANO[HandType.right].shape == (3, 3)
+ assert (OPERATOR2MANO[HandType.right] == OPERATOR2MANO_RIGHT).all()
+
+ def test_operator2mano_left(self):
+ assert OPERATOR2MANO[HandType.left].shape == (3, 3)
+ assert (OPERATOR2MANO[HandType.left] == OPERATOR2MANO_LEFT).all()
+
+ def test_operator2mano_right_values(self):
+ expected = [
+ [0, 0, -1],
+ [-1, 0, 0],
+ [0, 1, 0],
+ ]
+ assert (OPERATOR2MANO_RIGHT == expected).all()
+
+
+class TestGetDefaultConfigPath:
+ def test_get_config_path_teleop(self):
+ path = get_default_config_path(RobotName.l6, RetargetingType.vector, HandType.right)
+ assert path is not None
+ assert "teleop" in str(path)
+ assert "l6" in str(path).lower()
+
+ def test_get_config_path_offline(self):
+ path = get_default_config_path(RobotName.l6, RetargetingType.position, HandType.right)
+ assert path is not None
+ assert "offline" in str(path)
diff --git a/src/linkerhand_retarget/tests/unit/test_filter.py b/src/linkerhand_retarget/tests/unit/test_filter.py
new file mode 100644
index 0000000..e7b3f27
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/test_filter.py
@@ -0,0 +1,224 @@
+import pytest
+import numpy as np
+from linkerhand_retarget.linkerhand.filter import (
+ LCFilter,
+ MultiChannelLCFilter,
+ AdaptiveLCFilter,
+ KalmanFilter,
+ MultiChannelKalmanFilter,
+ AdaptiveKalmanFilter,
+ SavitzkyGolayFilter,
+ MultiChannelSavitzkyGolayFilter,
+ AdaptiveSavitzkyGolayFilter,
+ apply_lc_filter,
+)
+
+
+class TestLCFilter:
+ def test_initialization(self):
+ f = LCFilter(alpha=0.5, initial_value=1.0)
+ assert f.alpha == 0.5
+ assert f.filtered_value == 1.0
+
+ def test_invalid_alpha(self):
+ with pytest.raises(ValueError):
+ LCFilter(alpha=0)
+ with pytest.raises(ValueError):
+ LCFilter(alpha=1.5)
+
+ def test_update(self):
+ f = LCFilter(alpha=0.5)
+ result = f.update(10.0)
+ assert result == 5.0 # 0.5 * 10 + 0.5 * 0
+
+ def test_update_chain(self):
+ f = LCFilter(alpha=0.5)
+ f.update(10.0) # 5.0
+ result = f.update(20.0) # 0.5 * 20 + 0.5 * 5 = 12.5
+ assert result == 12.5
+
+ def test_update_array(self):
+ f = LCFilter(alpha=0.5)
+ result = f.update_array([10.0, 20.0, 30.0])
+ assert len(result) == 3
+
+ def test_reset(self):
+ f = LCFilter(alpha=0.5, initial_value=5.0)
+ f.update(10.0)
+ f.reset(initial_value=0.0)
+ assert f.filtered_value == 0.0
+ assert len(f.history_raw) == 0
+
+
+class TestMultiChannelLCFilter:
+ def test_initialization(self):
+ f = MultiChannelLCFilter(num_channels=3, alpha=0.5)
+ assert f.num_channels == 3
+ assert len(f.filters) == 3
+
+ def test_invalid_channels(self):
+ with pytest.raises(ValueError):
+ MultiChannelLCFilter(num_channels=3, initial_values=[1.0, 2.0])
+
+ def test_update(self):
+ f = MultiChannelLCFilter(num_channels=3, alpha=0.5)
+ result = f.update([10.0, 20.0, 30.0])
+ assert result == [5.0, 10.0, 15.0]
+
+ def test_update_channel(self):
+ f = MultiChannelLCFilter(num_channels=3, alpha=0.5)
+ result = f.update_channel(1, 20.0)
+ assert result == 10.0
+
+ def test_invalid_channel_index(self):
+ f = MultiChannelLCFilter(num_channels=3)
+ with pytest.raises(ValueError):
+ f.update_channel(5, 10.0)
+
+
+class TestAdaptiveLCFilter:
+ def test_initialization(self):
+ f = AdaptiveLCFilter(alpha_min=0.05, alpha_max=0.3)
+ assert f.alpha_min == 0.05
+ assert f.alpha_max == 0.3
+
+ def test_adaptive_update_fast_change(self):
+ f = AdaptiveLCFilter(alpha_min=0.05, alpha_max=0.3, change_threshold=0.1)
+ f.update(0.0) # initial
+ result = f.update(10.0) # large change, should use alpha_max
+ assert f.alpha == 0.3
+
+ def test_adaptive_update_slow_change(self):
+ f = AdaptiveLCFilter(alpha_min=0.05, alpha_max=0.3, change_threshold=0.1)
+ f.update(0.0) # initial
+ f.update(0.01) # small change
+ result = f.update(0.02) # small change
+ assert f.alpha == 0.05
+
+
+class TestKalmanFilter:
+ def test_initialization(self):
+ kf = KalmanFilter(process_variance=1e-5, measurement_variance=0.1)
+ assert kf.process_variance == 1e-5
+ assert kf.measurement_variance == 0.1
+
+ def test_update(self):
+ kf = KalmanFilter(process_variance=1e-5, measurement_variance=0.1)
+ result = kf.update(10.0)
+ assert result > 0 and result < 10.0
+
+ def test_update_batch(self):
+ kf = KalmanFilter(process_variance=1e-5, measurement_variance=0.1)
+ result = kf.update_batch([10.0, 20.0, 30.0])
+ assert len(result) == 3
+
+ def test_reset(self):
+ kf = KalmanFilter(process_variance=1e-5, measurement_variance=0.1)
+ kf.update(10.0)
+ kf.reset(initial_value=0.0)
+ assert kf.x_hat == 0.0
+
+
+class TestMultiChannelKalmanFilter:
+ def test_initialization(self):
+ mkf = MultiChannelKalmanFilter(num_channels=3)
+ assert mkf.num_channels == 3
+ assert len(mkf.filters) == 3
+
+ def test_update(self):
+ mkf = MultiChannelKalmanFilter(num_channels=3)
+ result = mkf.update([10.0, 20.0, 30.0])
+ assert len(result) == 3
+
+ def test_update_channel(self):
+ mkf = MultiChannelKalmanFilter(num_channels=3)
+ result = mkf.update_channel(1, 20.0)
+ assert result > 0
+
+ def test_invalid_channel(self):
+ mkf = MultiChannelKalmanFilter(num_channels=3)
+ with pytest.raises(ValueError):
+ mkf.update_channel(5, 10.0)
+
+
+class TestAdaptiveKalmanFilter:
+ def test_initialization(self):
+ akf = AdaptiveKalmanFilter(
+ min_process_variance=1e-6,
+ max_process_variance=1e-3,
+ )
+ assert akf.min_process_variance == 1e-6
+ assert akf.max_process_variance == 1e-3
+
+
+class TestSavitzkyGolayFilter:
+ def test_initialization(self):
+ sgf = SavitzkyGolayFilter(window_length=7, polyorder=2)
+ assert sgf.window_length == 7
+ assert sgf.polyorder == 2
+
+ def test_invalid_window_length(self):
+ with pytest.raises(ValueError):
+ SavitzkyGolayFilter(window_length=6) # even number
+
+ def test_window_less_than_polyorder(self):
+ with pytest.raises(ValueError):
+ SavitzkyGolayFilter(window_length=3, polyorder=4)
+
+ def test_update(self):
+ sgf = SavitzkyGolayFilter(window_length=7, polyorder=2)
+ result = sgf.update(10.0)
+ assert isinstance(result, float)
+
+ def test_buffer_not_full(self):
+ sgf = SavitzkyGolayFilter(window_length=7, polyorder=2)
+ for i in range(3):
+ result = sgf.update(float(i))
+ assert result == float(i) # returns original when buffer not full
+
+ def test_reset(self):
+ sgf = SavitzkyGolayFilter(window_length=7, polyorder=2)
+ sgf.update(10.0)
+ sgf.reset()
+ assert len(sgf.buffer) == 0
+
+
+class TestMultiChannelSavitzkyGolayFilter:
+ def test_initialization(self):
+ msgf = MultiChannelSavitzkyGolayFilter(num_channels=3)
+ assert msgf.num_channels == 3
+
+ def test_update(self):
+ msgf = MultiChannelSavitzkyGolayFilter(num_channels=3)
+ result = msgf.update([10.0, 20.0, 30.0])
+ assert len(result) == 3
+
+
+class TestAdaptiveSavitzkyGolayFilter:
+ def test_initialization(self):
+ asgf = AdaptiveSavitzkyGolayFilter(min_window=5, max_window=13)
+ assert asgf.min_window == 5
+ assert asgf.max_window == 13
+
+ def test_update(self):
+ asgf = AdaptiveSavitzkyGolayFilter(min_window=5, max_window=13)
+ result = asgf.update(10.0)
+ assert isinstance(result, float)
+
+
+class TestApplyLCFilter:
+ def test_empty_list(self):
+ result = apply_lc_filter([])
+ assert result == []
+
+ def test_single_value(self):
+ result = apply_lc_filter([5.0], alpha=0.5)
+ assert result == [5.0]
+
+ def test_multiple_values(self):
+ result = apply_lc_filter([10.0, 20.0, 30.0], alpha=0.5)
+ assert len(result) == 3
+
+ def test_invalid_alpha(self):
+ with pytest.raises(ValueError):
+ apply_lc_filter([1.0, 2.0], alpha=0)
diff --git a/src/linkerhand_retarget/tests/unit/test_handcore.py b/src/linkerhand_retarget/tests/unit/test_handcore.py
new file mode 100644
index 0000000..4fc479a
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/test_handcore.py
@@ -0,0 +1,157 @@
+import pytest
+import numpy as np
+from unittest.mock import Mock, MagicMock
+from linkerhand_retarget.linkerhand.handcore import HandCore, KalmanFilter, MultiTargetKalman
+
+
+class MockJoint:
+ def __init__(self, joint_type="revolute", lower=-1.0, upper=1.0):
+ self.type = joint_type
+ self.limit = MagicMock()
+ self.limit.lower = lower
+ self.limit.upper = upper
+
+
+class MockRobot:
+ def __init__(self, joints):
+ self.joint_map = joints
+
+
+class TestHandCoreGetJointLimits:
+ def test_revolute_joints(self):
+ joints = {
+ 'joint1': MockJoint("revolute", -1.57, 1.57),
+ 'joint2': MockJoint("revolute", -0.5, 0.5),
+ }
+ robot = MockRobot(joints)
+ lower, upper, ranges = HandCore.get_joint_limits(robot)
+
+ assert len(lower) == 2
+
+ def test_prismatic_joint(self):
+ joints = {
+ 'prismatic_joint': MockJoint("prismatic", -0.5, 0.5),
+ }
+ robot = MockRobot(joints)
+ lower, upper, ranges = HandCore.get_joint_limits(robot)
+
+ assert lower[0] == -0.5
+ assert upper[0] == 0.5
+ assert ranges[0] == 1.0
+
+ def test_fixed_joint_skipped(self):
+ joints = {
+ 'fixed_joint': MockJoint("fixed", -1.0, 1.0),
+ 'revolute_joint': MockJoint("revolute", -1.0, 1.0),
+ }
+ robot = MockRobot(joints)
+ lower, upper, ranges = HandCore.get_joint_limits(robot)
+
+ assert len(lower) == 1
+
+ def test_joint_without_limit(self):
+ joints = {
+ 'revolute_no_limit': Mock(),
+ }
+ joints['revolute_no_limit'].type = "revolute"
+ joints['revolute_no_limit'].limit = None
+
+ robot = MockRobot(joints)
+ lower, upper, ranges = HandCore.get_joint_limits(robot)
+
+ assert lower[0] == -3.1415926535
+ assert upper[0] == 3.1415926535
+
+
+class TestHandCoreProjectionProcess:
+ def test_projection_process_returns_30_values(self):
+ hand_position = np.random.rand(25, 3)
+ result = HandCore.projection_process(hand_position)
+
+ assert len(result) == 30
+
+ def test_projection_process_returns_list(self):
+ hand_position = np.ones((25, 3)) * 0.1
+ result = HandCore.projection_process(hand_position)
+
+ assert len(result) == 30
+ assert isinstance(result[0], float)
+
+
+class TestKalmanFilter:
+ def test_initialization(self):
+ kf = KalmanFilter(process_variance=0.01, measurement_variance=0.1, estimated_error=1.0, initial_value=0.0)
+ assert kf.process_variance == 0.01
+ assert kf.measurement_variance == 0.1
+ assert kf.estimated_error == 1.0
+ assert kf.current_estimate == 0.0
+
+ def test_update_first_measurement(self):
+ kf = KalmanFilter(process_variance=0.01, measurement_variance=0.1, estimated_error=1.0, initial_value=0.0)
+ result = kf.update(10.0)
+
+ assert 0.0 < result < 10.0
+
+ def test_update_convergence(self):
+ kf = KalmanFilter(process_variance=0.001, measurement_variance=0.01, estimated_error=1.0, initial_value=0.0)
+
+ results = []
+ for _ in range(100):
+ results.append(kf.update(10.0))
+
+ assert abs(results[-1] - 10.0) < 0.5
+
+ def test_update_with_known_measurement(self):
+ kf = KalmanFilter(process_variance=0.01, measurement_variance=0.1, estimated_error=1.0, initial_value=5.0)
+ result = kf.update(5.0)
+
+ assert result == 5.0
+
+ def test_estimated_error_decreases(self):
+ kf = KalmanFilter(process_variance=0.01, measurement_variance=0.1, estimated_error=1.0, initial_value=0.0)
+
+ initial_error = kf.estimated_error
+ kf.update(10.0)
+
+ assert kf.estimated_error < initial_error
+
+
+class TestMultiTargetKalman:
+ def test_initialization(self):
+ mtkf = MultiTargetKalman(num_targets=5)
+
+ assert mtkf.num_targets == 5
+ assert len(mtkf.kalman_filters) == 5
+ assert len(mtkf.smoothed_data) == 5
+
+ def test_initialization_custom_params(self):
+ mtkf = MultiTargetKalman(
+ num_targets=3,
+ process_variance=0.001,
+ measurement_variance=0.01,
+ estimated_error=0.5,
+ initial_value=100.0
+ )
+
+ assert mtkf.num_targets == 3
+ assert len(mtkf.kalman_filters) == 3
+
+ def test_update_single_target(self):
+ mtkf = MultiTargetKalman(num_targets=5)
+
+ result = mtkf.update(10.0, index=2)
+
+ assert isinstance(result, float)
+
+ def test_update_all_targets(self):
+ mtkf = MultiTargetKalman(num_targets=3)
+
+ for i in range(3):
+ result = mtkf.update(float(i * 10), index=i)
+ assert isinstance(result, float)
+
+ def test_smoothed_data_initialized(self):
+ mtkf = MultiTargetKalman(num_targets=2)
+
+ assert len(mtkf.smoothed_data) == 2
+ assert isinstance(mtkf.smoothed_data, list)
diff --git a/src/linkerhand_retarget/tests/unit/test_handcoreex.py b/src/linkerhand_retarget/tests/unit/test_handcoreex.py
new file mode 100644
index 0000000..d68af96
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/test_handcoreex.py
@@ -0,0 +1,158 @@
+import pytest
+import numpy as np
+from linkerhand_retarget.linkerhand.handcoreex import MultiStateLinearMapper
+
+
+FINGER_CONFIGS_TEST = {
+ 'thumb': {
+ 'name': 'thumb',
+ 'joints': [0, 1, 2],
+ 'weights': [0.2, 0.3, 0.5],
+ 'robot_idx': 0,
+ 'reverse_motion': False,
+ },
+ 'index': {
+ 'name': 'index',
+ 'joints': [3, 4, 5],
+ 'weights': [0.3, 0.3, 0.4],
+ 'robot_idx': 1,
+ 'reverse_motion': False,
+ },
+}
+
+MAPPING_ORDER_TEST = ['thumb', 'index']
+
+
+class TestMultiStateLinearMapper:
+ def test_initialization(self):
+ mapper = MultiStateLinearMapper(FINGER_CONFIGS_TEST, MAPPING_ORDER_TEST)
+ assert mapper.finger_configs == FINGER_CONFIGS_TEST
+ assert mapper.mapping_order == MAPPING_ORDER_TEST
+ assert len(mapper.glove_states) == 0
+ assert len(mapper.robot_states) == 0
+
+ def test_add_state(self):
+ mapper = MultiStateLinearMapper(FINGER_CONFIGS_TEST, MAPPING_ORDER_TEST)
+ glove_angles = [0.0] * 21
+ robot_angles = [0.0] * 6
+
+ mapper.add_state('original', glove_angles, robot_angles)
+
+ assert 'original' in mapper.glove_states
+ assert 'original' in mapper.robot_states
+ assert np.array_equal(mapper.glove_states['original'], glove_angles)
+
+ def test_add_state_with_list(self):
+ mapper = MultiStateLinearMapper(FINGER_CONFIGS_TEST, MAPPING_ORDER_TEST)
+ glove_angles = [1.0] * 21
+ robot_angles = [0.5] * 6
+
+ mapper.add_state('fist', glove_angles, robot_angles)
+
+ assert 'fist' in mapper.glove_states
+
+ def test_remove_state(self):
+ mapper = MultiStateLinearMapper(FINGER_CONFIGS_TEST, MAPPING_ORDER_TEST)
+ glove_angles = [0.0] * 21
+ robot_angles = [0.0] * 6
+
+ mapper.add_state('original', glove_angles, robot_angles)
+ mapper.remove_state('original')
+
+ assert 'original' not in mapper.glove_states
+
+ def test_set_state_order(self):
+ mapper = MultiStateLinearMapper(FINGER_CONFIGS_TEST, MAPPING_ORDER_TEST)
+ mapper.add_state('original', [0.0] * 21, [0.0] * 6)
+ mapper.add_state('fist', [1.0] * 21, [1.0] * 6)
+
+ mapper.set_state_order(['original', 'fist'])
+
+ assert mapper.state_order == ['original', 'fist']
+
+ def test_set_state_order_invalid_state(self):
+ mapper = MultiStateLinearMapper(FINGER_CONFIGS_TEST, MAPPING_ORDER_TEST)
+ mapper.add_state('original', [0.0] * 21, [0.0] * 6)
+
+ with pytest.raises(ValueError):
+ mapper.set_state_order(['original', 'nonexistent'])
+
+ def test_map_glove_to_robot_requires_two_states(self):
+ mapper = MultiStateLinearMapper(FINGER_CONFIGS_TEST, MAPPING_ORDER_TEST)
+
+ with pytest.raises(ValueError, match="请至少设置两个状态"):
+ mapper.map_glove_to_robot([0.0] * 21)
+
+ def test_map_glove_to_robot_with_original(self):
+ mapper = MultiStateLinearMapper(FINGER_CONFIGS_TEST, MAPPING_ORDER_TEST)
+ mapper.add_state('original', [0.0] * 21, [0.0] * 6)
+ mapper.add_state('fist', [1.0] * 21, [1.0] * 6)
+ mapper.set_state_order(['original', 'fist'])
+
+ result = mapper.map_glove_to_robot([0.5] * 21)
+
+ assert isinstance(result, np.ndarray)
+
+ def test_map_glove_to_robot_returns_array(self):
+ mapper = MultiStateLinearMapper(FINGER_CONFIGS_TEST, MAPPING_ORDER_TEST)
+ mapper.add_state('original', [0.0] * 21, [0.0] * 6)
+ mapper.add_state('fist', [1.0] * 21, [1.0] * 6)
+ mapper.set_state_order(['original', 'fist'])
+
+ result = mapper.map_glove_to_robot([0.5] * 21)
+
+ assert isinstance(result, np.ndarray)
+ assert len(result) == 6
+
+ def test_map_glove_to_robot_with_numpy_array(self):
+ mapper = MultiStateLinearMapper(FINGER_CONFIGS_TEST, MAPPING_ORDER_TEST)
+ mapper.add_state('original', [0.0] * 21, [0.0] * 6)
+ mapper.add_state('fist', [1.0] * 21, [1.0] * 6)
+ mapper.set_state_order(['original', 'fist'])
+
+ result = mapper.map_glove_to_robot(np.array([0.5] * 21))
+
+ assert isinstance(result, np.ndarray)
+
+ def test_map_glove_to_robot_full_extension(self):
+ mapper = MultiStateLinearMapper(FINGER_CONFIGS_TEST, MAPPING_ORDER_TEST)
+ mapper.add_state('original', [0.0] * 21, [0.0] * 6)
+ mapper.add_state('fist', [1.0] * 21, [1.0] * 6)
+ mapper.set_state_order(['original', 'fist'])
+
+ result = mapper.map_glove_to_robot([0.0] * 21)
+
+ assert isinstance(result, np.ndarray)
+
+ def test_map_glove_to_robot_full_flexion(self):
+ mapper = MultiStateLinearMapper(FINGER_CONFIGS_TEST, MAPPING_ORDER_TEST)
+ mapper.add_state('original', [0.0] * 21, [0.0] * 6)
+ mapper.add_state('fist', [1.0] * 21, [1.0] * 6)
+ mapper.set_state_order(['original', 'fist'])
+
+ result = mapper.map_glove_to_robot([1.0] * 21)
+
+ assert isinstance(result, np.ndarray)
+
+
+class TestMultiStateLinearMapperEdgeCases:
+ def test_empty_glove_states(self):
+ mapper = MultiStateLinearMapper(FINGER_CONFIGS_TEST, MAPPING_ORDER_TEST)
+ assert len(mapper.glove_states) == 0
+
+ def test_debug_value_initialized(self):
+ mapper = MultiStateLinearMapper(FINGER_CONFIGS_TEST, MAPPING_ORDER_TEST)
+ assert len(mapper.debug_value) == 20
+
+ def test_history_initialized(self):
+ mapper = MultiStateLinearMapper(FINGER_CONFIGS_TEST, MAPPING_ORDER_TEST)
+ assert len(mapper.raw_history) == 0
+ assert len(mapper.filtered_history) == 0
+
+ def test_multiple_states(self):
+ mapper = MultiStateLinearMapper(FINGER_CONFIGS_TEST, MAPPING_ORDER_TEST)
+ mapper.add_state('original', [0.0] * 21, [0.0] * 6)
+ mapper.add_state('opose', [0.5] * 21, [0.5] * 6)
+ mapper.add_state('fist', [1.0] * 21, [1.0] * 6)
+
+ assert len(mapper.glove_states) == 3
diff --git a/src/linkerhand_retarget/tests/unit/test_linkerforce.py b/src/linkerhand_retarget/tests/unit/test_linkerforce.py
new file mode 100644
index 0000000..00382bb
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/test_linkerforce.py
@@ -0,0 +1,119 @@
+import pytest
+import array
+from linkerhand_retarget.linkerhand.linkerforce import CircularBuffer, FrameParser, FrameParseState, BUFFER_SIZE, FRAME_HEADER
+
+
+class TestCircularBuffer:
+ def test_initialization(self):
+ buf = CircularBuffer()
+ assert buf.data_len == 0
+ assert buf.read_pos == 0
+ assert buf.write_pos == 0
+
+ def test_write_single_byte(self):
+ buf = CircularBuffer()
+ buf.write([0x5D])
+ assert buf.data_len == 1
+ assert buf.read_pos == 0
+ assert buf.write_pos == 1
+
+ def test_read_byte(self):
+ buf = CircularBuffer()
+ buf.write([0x5D, 0x01])
+ byte = buf.read_byte()
+ assert byte == 0x5D
+ assert buf.data_len == 1
+
+ def test_read_empty_buffer(self):
+ buf = CircularBuffer()
+ byte = buf.read_byte()
+ assert byte is None
+
+ def test_write_multiple_bytes(self):
+ buf = CircularBuffer()
+ data = [0x01, 0x02, 0x03, 0x04, 0x05]
+ buf.write(data)
+ assert buf.data_len == 5
+
+ def test_read_write_sequence(self):
+ buf = CircularBuffer()
+ buf.write([10, 20, 30])
+ assert buf.read_byte() == 10
+ assert buf.read_byte() == 20
+ assert buf.read_byte() == 30
+ assert buf.read_byte() is None
+
+ def test_buffer_wrap_around(self):
+ buf = CircularBuffer()
+ for i in range(BUFFER_SIZE + 10):
+ buf.write([i % 256])
+ assert buf.data_len == BUFFER_SIZE
+
+
+class TestFrameParser:
+ def test_initialization(self):
+ parser = FrameParser()
+ assert parser.state == FrameParseState.HEADER
+ assert parser.expected_len == 0
+ assert parser.current_pos == 0
+
+ def test_reset(self):
+ parser = FrameParser()
+ parser.state = FrameParseState.DATA
+ parser.current_pos = 5
+ parser.reset()
+ assert parser.state == FrameParseState.HEADER
+ assert parser.current_pos == 0
+
+ def test_process_byte_finds_header(self):
+ parser = FrameParser()
+ result = parser.process_byte(FRAME_HEADER)
+ assert parser.state == FrameParseState.CMD
+
+ def test_process_byte_accumulates_data(self):
+ parser = FrameParser()
+ parser.process_byte(FRAME_HEADER)
+ parser.process_byte(0x01)
+ parser.process_byte(0x03)
+ for i in range(3):
+ parser.process_byte(i)
+ parser.process_byte(0)
+
+ def test_state_transitions(self):
+ parser = FrameParser()
+ assert parser.state == FrameParseState.HEADER
+
+ parser.process_byte(FRAME_HEADER)
+ assert parser.state == FrameParseState.CMD
+
+ parser.process_byte(0x01)
+ assert parser.state == FrameParseState.LENGTH
+
+ parser.process_byte(0x02)
+ assert parser.state == FrameParseState.DATA
+
+ def test_process_multiple_frames(self):
+ parser = FrameParser()
+ frame1 = [FRAME_HEADER, 0x01, 0x02, 0xAA, 0xBB]
+ for byte in frame1:
+ parser.process_byte(byte)
+
+ parser.reset()
+ frame2 = [FRAME_HEADER, 0x02, 0x01, 0xCC]
+ for byte in frame2:
+ parser.process_byte(byte)
+
+
+class TestConstants:
+ def test_buffer_size(self):
+ assert BUFFER_SIZE == 1024
+
+ def test_frame_header(self):
+ assert FRAME_HEADER == 0x5D
+
+ def test_frame_parse_states(self):
+ assert FrameParseState.HEADER.value == 0
+ assert FrameParseState.CMD.value == 1
+ assert FrameParseState.LENGTH.value == 2
+ assert FrameParseState.DATA.value == 3
+ assert FrameParseState.CHECKSUM.value == 4
diff --git a/src/linkerhand_retarget/tests/unit/test_linkerforce_g20.py b/src/linkerhand_retarget/tests/unit/test_linkerforce_g20.py
new file mode 100644
index 0000000..46c0669
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/test_linkerforce_g20.py
@@ -0,0 +1,51 @@
+from pathlib import Path
+
+import yaml
+
+from linkerhand_retarget.motion.linkerforce.hand.linkerforce_g20 import (
+ LeftHand,
+ RightHand,
+)
+from linkerhand_retarget.motion.linkerforce.config.g20_config import MOTOR_CONSTRAINTS
+
+
+class _HandCoreStub:
+ pass
+
+
+def test_left_g20_thumb_side_swing_has_a_safe_command_range():
+ package_dir = Path(__file__).resolve().parents[2] / 'linkerhand_retarget'
+ config = yaml.safe_load((package_dir / 'config' / 'hand_config.yml').read_text())
+
+ assert config['commandlower_left_g20'][5] == 255
+ assert config['commandupper_left_g20'][5] == 0
+ assert MOTOR_CONSTRAINTS['left'][5] == {
+ 'min': 20,
+ 'max': 255,
+ 'enabled': True,
+ }
+
+
+def test_left_g20_reverses_only_ring_side_swing():
+ hand = LeftHand(_HandCoreStub())
+
+ assert hand.multi_state_mapper.finger_configs['index_roll']['reverse_motion'] is False
+ assert hand.multi_state_mapper.finger_configs['middle_roll']['reverse_motion'] is False
+ assert hand.multi_state_mapper.finger_configs['ring_roll']['reverse_motion'] is True
+ assert hand.multi_state_mapper.finger_configs['pinky_roll']['reverse_motion'] is False
+
+
+def test_left_g20_side_direction_overrides_survive_version_change():
+ hand = LeftHand(_HandCoreStub())
+
+ hand.set_glove_version('1.2.12')
+
+ assert hand.multi_state_mapper.finger_configs['ring_roll']['reverse_motion'] is True
+ assert hand.multi_state_mapper.finger_configs['pinky_roll']['reverse_motion'] is False
+
+
+def test_right_g20_side_directions_are_unchanged():
+ hand = RightHand(_HandCoreStub())
+
+ assert hand.multi_state_mapper.finger_configs['ring_roll']['reverse_motion'] is False
+ assert hand.multi_state_mapper.finger_configs['pinky_roll']['reverse_motion'] is False
diff --git a/src/linkerhand_retarget/tests/unit/test_linkerforce_improved.py b/src/linkerhand_retarget/tests/unit/test_linkerforce_improved.py
new file mode 100644
index 0000000..1ffcc94
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/test_linkerforce_improved.py
@@ -0,0 +1,387 @@
+"""
+LinkerForce 改进版单元测试
+"""
+import unittest
+import threading
+import time
+import queue
+from unittest.mock import Mock, MagicMock, patch
+import serial
+
+from linkerhand.linkerforce_improved import (
+ ForceSerialReader,
+ SerialConfig,
+ DeviceInfo,
+ FrameParser,
+ FrameParseState
+)
+from linkerhand.constants import HandType
+
+
+class TestSerialConfig(unittest.TestCase):
+ """测试配置类"""
+
+ def test_default_config(self):
+ """测试默认配置"""
+ config = SerialConfig()
+ self.assertEqual(config.baudrates, [2000000, 1000000, 921600, 460800])
+ self.assertEqual(config.timeout, 0.001)
+ self.assertTrue(config.auto_reconnect)
+ self.assertEqual(config.max_reconnect_attempts, 5)
+
+ def test_custom_config(self):
+ """测试自定义配置"""
+ config = SerialConfig(
+ baudrates=[921600],
+ auto_reconnect=False,
+ max_reconnect_attempts=10
+ )
+ self.assertEqual(config.baudrates, [921600])
+ self.assertFalse(config.auto_reconnect)
+ self.assertEqual(config.max_reconnect_attempts, 10)
+
+
+class TestDeviceInfo(unittest.TestCase):
+ """测试设备信息类"""
+
+ def test_device_info_creation(self):
+ """测试设备信息创建"""
+ info = DeviceInfo(
+ port="/dev/ttyUSB0",
+ baudrate=2000000,
+ handtype="Right",
+ version="1.0.0"
+ )
+ self.assertEqual(info.port, "/dev/ttyUSB0")
+ self.assertEqual(info.baudrate, 2000000)
+ self.assertEqual(info.handtype, "Right")
+ self.assertEqual(info.version, "1.0.0")
+
+ def test_device_info_optional_fields(self):
+ """测试可选字段"""
+ info = DeviceInfo(port="/dev/ttyUSB0", baudrate=2000000)
+ self.assertIsNone(info.handtype)
+ self.assertIsNone(info.version)
+
+
+class TestFrameParser(unittest.TestCase):
+ """测试帧解析器"""
+
+ def setUp(self):
+ self.parser = FrameParser()
+
+ def test_initial_state(self):
+ """测试初始状态"""
+ self.assertEqual(self.parser.state, FrameParseState.HEADER)
+ self.assertEqual(self.parser.current_pos, 0)
+ self.assertEqual(self.parser.checksum, 0)
+
+ def test_reset(self):
+ """测试重置"""
+ self.parser.state = FrameParseState.DATA
+ self.parser.current_pos = 10
+ self.parser.checksum = 100
+
+ self.parser.reset()
+
+ self.assertEqual(self.parser.state, FrameParseState.HEADER)
+ self.assertEqual(self.parser.current_pos, 0)
+ self.assertEqual(self.parser.checksum, 0)
+
+ def test_parse_valid_frame(self):
+ """测试解析有效帧"""
+ # 构造一个有效帧: 0x5D, 0x01, 0x00, checksum
+ frame_data = bytes([0x5D, 0x01, 0x00, 0x5E]) # checksum = 0x5D + 0x01 + 0x00 = 0x5E
+
+ result = False
+ for byte in frame_data:
+ if self.parser.process_byte(byte):
+ result = True
+
+ self.assertTrue(result)
+ self.assertEqual(self.parser.frame_buf[0], 0x5D)
+ self.assertEqual(self.parser.frame_buf[1], 0x01)
+ self.assertEqual(self.parser.frame_buf[2], 0x00)
+
+
+class TestForceSerialReader(unittest.TestCase):
+ """测试主类"""
+
+ def setUp(self):
+ self.config = SerialConfig(
+ baudrates=[2000000],
+ auto_reconnect=False
+ )
+ self.reader = ForceSerialReader(
+ hand_type=HandType.right,
+ config=self.config,
+ debug=False
+ )
+
+ def tearDown(self):
+ if self.reader.is_connected:
+ self.reader.stop()
+
+ def test_initial_state(self):
+ """测试初始状态"""
+ self.assertEqual(self.reader.poslist, [0.0] * 21)
+ self.assertEqual(self.reader.forcelist, [0.0] * 5)
+ self.assertIsNone(self.reader.handtype)
+ self.assertIsNone(self.reader.version)
+ self.assertFalse(self.reader.is_connected)
+
+ def test_thread_safe_access(self):
+ """测试线程安全访问"""
+ results = []
+ errors = []
+
+ def write_data():
+ for i in range(100):
+ with self.reader._lock:
+ self.reader._poslist = [float(i)] * 21
+ time.sleep(0.001)
+
+ def read_data():
+ for _ in range(100):
+ try:
+ data = self.reader.poslist
+ results.append(len(data))
+ except Exception as e:
+ errors.append(e)
+ time.sleep(0.001)
+
+ writer = threading.Thread(target=write_data)
+ readers = [threading.Thread(target=read_data) for _ in range(5)]
+
+ writer.start()
+ for r in readers:
+ r.start()
+
+ writer.join()
+ for r in readers:
+ r.join()
+
+ self.assertEqual(len(errors), 0)
+ self.assertEqual(len(results), 500)
+ for result in results:
+ self.assertEqual(result, 21)
+
+ def test_callback_registration(self):
+ """测试回调注册"""
+ disconnect_called = []
+ reconnect_called = []
+
+ self.reader.set_disconnect_callback(lambda: disconnect_called.append(True))
+ self.reader.set_reconnect_callback(lambda: reconnect_called.append(True))
+
+ self.assertIsNotNone(self.reader._on_disconnect)
+ self.assertIsNotNone(self.reader._on_reconnect)
+
+ def test_context_manager(self):
+ """测试上下文管理器"""
+ with patch.object(self.reader, 'start') as mock_start, \
+ patch.object(self.reader, 'stop') as mock_stop:
+
+ with self.reader:
+ mock_start.assert_called_once()
+
+ mock_stop.assert_called_once()
+
+ def test_pack_data(self):
+ """测试数据打包"""
+ pack_01 = self.reader._pack_01_data()
+ self.assertEqual(pack_01[0], 0x5D) # header
+ self.assertEqual(pack_01[1], 0x01) # cmd
+ self.assertEqual(pack_01[2], 0x00) # length
+
+ pack_03 = self.reader._pack_03_data()
+ self.assertEqual(pack_03[0], 0x5D)
+ self.assertEqual(pack_03[1], 0x03)
+
+ float_data = [1.0, 2.0, 3.0]
+ pack_A7 = self.reader._pack_A7_data(float_data)
+ self.assertEqual(pack_A7[0], 0x5D)
+ self.assertEqual(pack_A7[1], 0xA7)
+
+ def test_handle_version_frame(self):
+ """测试版本帧处理"""
+ # 模拟版本帧数据: value=10001, status_code=1
+ import struct
+ value = 10001 # version 1.0.1
+ status_code = 1 # right hand
+ frame_data = struct.pack('h', force) for force in forces)
+
+ self.reader._handle_force_frame(frame_data)
+
+ realforcelist = self.reader.realforcelist
+ self.assertEqual(len(realforcelist), 3)
+ self.assertEqual(realforcelist, forces)
+
+ def test_disconnect_handler(self):
+ """测试断开连接处理"""
+ disconnect_called = []
+ self.reader.set_disconnect_callback(lambda: disconnect_called.append(True))
+
+ self.reader._connected = True
+ self.reader._handle_disconnect()
+
+ self.assertFalse(self.reader._connected)
+ self.assertEqual(len(disconnect_called), 1)
+
+ def test_reconnect_disabled(self):
+ """测试重连禁用"""
+ config = SerialConfig(auto_reconnect=False)
+ reader = ForceSerialReader(HandType.right, config=config)
+
+ reader._device_info = DeviceInfo(port="/dev/ttyUSB0", baudrate=2000000)
+
+ # 不应该重连
+ reader._attempt_reconnect()
+
+ self.assertFalse(reader._connected)
+
+
+class TestForceSerialReaderIntegration(unittest.TestCase):
+ """集成测试"""
+
+ def setUp(self):
+ self.config = SerialConfig(
+ baudrates=[2000000],
+ auto_reconnect=False
+ )
+
+ @patch('serial.Serial')
+ def test_scan_and_connect(self, mock_serial):
+ """测试扫描和连接流程"""
+ # 模拟串口设备
+ mock_port = Mock()
+ mock_port.device = "/dev/ttyUSB0"
+ mock_port.description = "USB Serial"
+ mock_port.hwid = "USB VID:PID"
+
+ with patch('serial.tools.list_ports.comports', return_value=[mock_port]):
+ reader = ForceSerialReader(
+ hand_type=HandType.right,
+ config=self.config,
+ debug=True
+ )
+
+ # 检查是否识别为USB设备
+ is_usb = reader._is_usb_device("/dev/ttyUSB0")
+ self.assertTrue(is_usb)
+
+ def test_concurrent_read_write(self):
+ """测试并发读写"""
+ reader = ForceSerialReader(HandType.right, config=self.config)
+
+ read_count = [0]
+ write_count = [0]
+
+ def writer():
+ for i in range(100):
+ with reader._lock:
+ reader._poslist = [float(i)] * 21
+ write_count[0] += 1
+ time.sleep(0.0001)
+
+ def reader_thread():
+ for _ in range(100):
+ data = reader.poslist
+ self.assertEqual(len(data), 21)
+ read_count[0] += 1
+ time.sleep(0.0001)
+
+ threads = [
+ threading.Thread(target=writer),
+ threading.Thread(target=reader_thread),
+ threading.Thread(target=reader_thread)
+ ]
+
+ for t in threads:
+ t.start()
+ for t in threads:
+ t.join()
+
+ self.assertEqual(write_count[0], 100)
+ self.assertEqual(read_count[0], 200)
+
+
+class TestForceSerialReaderMockSerial(unittest.TestCase):
+ """模拟串口测试"""
+
+ def setUp(self):
+ self.config = SerialConfig(
+ baudrates=[2000000],
+ auto_reconnect=False
+ )
+
+ @patch('serial.Serial')
+ def test_open_serial_success(self, mock_serial_class):
+ """测试成功打开串口"""
+ mock_serial_instance = Mock()
+ mock_serial_instance.is_open = True
+ mock_serial_class.return_value = mock_serial_instance
+
+ reader = ForceSerialReader(HandType.right, config=self.config)
+ result = reader.open_serial("/dev/ttyUSB0", 2000000)
+
+ self.assertTrue(result)
+ mock_serial_class.assert_called_once()
+
+ @patch('serial.Serial')
+ def test_open_serial_failure(self, mock_serial_class):
+ """测试打开串口失败"""
+ mock_serial_class.side_effect = serial.SerialException("Permission denied")
+
+ reader = ForceSerialReader(HandType.right, config=self.config)
+ result = reader.open_serial("/dev/ttyUSB0", 2000000)
+
+ self.assertFalse(result)
+
+ @patch('serial.Serial')
+ def test_close_serial(self, mock_serial_class):
+ """测试关闭串口"""
+ mock_serial_instance = Mock()
+ mock_serial_instance.is_open = True
+ mock_serial_class.return_value = mock_serial_instance
+
+ reader = ForceSerialReader(HandType.right, config=self.config)
+ reader.open_serial("/dev/ttyUSB0", 2000000)
+ reader.close_serial()
+
+ mock_serial_instance.close.assert_called_once()
+
+
+if __name__ == '__main__':
+ unittest.main(verbosity=2)
\ No newline at end of file
diff --git a/src/linkerhand_retarget/tests/unit/test_linkerforce_improved_standalone.py b/src/linkerhand_retarget/tests/unit/test_linkerforce_improved_standalone.py
new file mode 100644
index 0000000..d6ff3ab
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/test_linkerforce_improved_standalone.py
@@ -0,0 +1,361 @@
+#!/usr/bin/env python3
+"""
+LinkerForce 改进版验证测试
+独立运行，不依赖pytest
+"""
+import sys
+import os
+import threading
+import time
+import struct
+
+# 添加路径 - 从当前文件位置向上查找
+current_dir = os.path.dirname(os.path.abspath(__file__))
+linkerhand_retarget_dir = os.path.dirname(os.path.dirname(current_dir))
+sys.path.insert(0, linkerhand_retarget_dir)
+
+# 直接从文件导入
+import importlib.util
+spec = importlib.util.spec_from_file_location(
+ "linkerforce_improved",
+ os.path.join(linkerhand_retarget_dir, "linkerhand_retarget/linkerhand/linkerforce_improved.py")
+)
+linkerforce_improved = importlib.util.module_from_spec(spec)
+spec.loader.exec_module(linkerforce_improved)
+
+ForceSerialReader = linkerforce_improved.ForceSerialReader
+SerialConfig = linkerforce_improved.SerialConfig
+DeviceInfo = linkerforce_improved.DeviceInfo
+FrameParser = linkerforce_improved.FrameParser
+FrameParseState = linkerforce_improved.FrameParseState
+
+# 导入常量
+spec2 = importlib.util.spec_from_file_location(
+ "constants",
+ os.path.join(linkerhand_retarget_dir, "linkerhand_retarget/linkerhand/constants.py")
+)
+constants = importlib.util.module_from_spec(spec2)
+spec2.loader.exec_module(constants)
+
+HandType = constants.HandType
+import numpy as np
+
+
+class TestRunner:
+ def __init__(self):
+ self.tests_passed = 0
+ self.tests_failed = 0
+ self.errors = []
+
+ def test(self, name, func):
+ try:
+ func()
+ print(f"✓ {name}")
+ self.tests_passed += 1
+ except Exception as e:
+ print(f"✗ {name}: {e}")
+ self.tests_failed += 1
+ self.errors.append((name, str(e)))
+
+ def report(self):
+ print(f"\n{'='*60}")
+ print(f"测试结果: {self.tests_passed} 通过, {self.tests_failed} 失败")
+ if self.errors:
+ print("\n失败详情:")
+ for name, error in self.errors:
+ print(f" - {name}: {error}")
+ print(f"{'='*60}\n")
+
+
+def test_serial_config():
+ runner = TestRunner()
+
+ def test_default():
+ config = SerialConfig()
+ assert config.baudrates == [2000000, 1000000, 921600, 460800]
+ assert config.timeout == 0.001
+ assert config.auto_reconnect == True
+
+ def test_custom():
+ config = SerialConfig(
+ baudrates=[921600],
+ auto_reconnect=False,
+ max_reconnect_attempts=10
+ )
+ assert config.baudrates == [921600]
+ assert config.auto_reconnect == False
+ assert config.max_reconnect_attempts == 10
+
+ runner.test("默认配置", test_default)
+ runner.test("自定义配置", test_custom)
+ return runner
+
+
+def test_device_info():
+ runner = TestRunner()
+
+ def test_creation():
+ info = DeviceInfo(
+ port="/dev/ttyUSB0",
+ baudrate=2000000,
+ handtype="Right",
+ version="1.0.0"
+ )
+ assert info.port == "/dev/ttyUSB0"
+ assert info.baudrate == 2000000
+ assert info.handtype == "Right"
+ assert info.version == "1.0.0"
+
+ def test_optional():
+ info = DeviceInfo(port="/dev/ttyUSB0", baudrate=2000000)
+ assert info.handtype is None
+ assert info.version is None
+
+ runner.test("设备信息创建", test_creation)
+ runner.test("设备信息可选字段", test_optional)
+ return runner
+
+
+def test_frame_parser():
+ runner = TestRunner()
+
+ def test_initial():
+ parser = FrameParser()
+ assert parser.state == FrameParseState.HEADER
+ assert parser.current_pos == 0
+ assert parser.checksum == 0
+
+ def test_reset():
+ parser = FrameParser()
+ parser.state = FrameParseState.DATA
+ parser.current_pos = 10
+ parser.checksum = 100
+ parser.reset()
+ assert parser.state == FrameParseState.HEADER
+ assert parser.current_pos == 0
+ assert parser.checksum == 0
+
+ def test_parse_valid():
+ parser = FrameParser()
+ # 构造帧: 0x5D, 0x01, 0x00, checksum
+ frame_data = bytes([0x5D, 0x01, 0x00, 0x5E])
+ result = False
+ for byte in frame_data:
+ if parser.process_byte(byte):
+ result = True
+ assert result == True
+ assert parser.frame_buf[0] == 0x5D
+ assert parser.frame_buf[1] == 0x01
+
+ runner.test("帧解析器初始状态", test_initial)
+ runner.test("帧解析器重置", test_reset)
+ runner.test("帧解析器解析有效帧", test_parse_valid)
+ return runner
+
+
+def test_force_reader():
+ runner = TestRunner()
+
+ def test_initial():
+ config = SerialConfig(auto_reconnect=False)
+ reader = ForceSerialReader(hand_type=HandType.right, config=config)
+ assert reader.poslist == [0.0] * 21
+ assert reader.forcelist == [0.0] * 5
+ assert reader.handtype is None
+ assert reader.version is None
+ assert reader.is_connected == False
+
+ def test_thread_safe():
+ config = SerialConfig(auto_reconnect=False)
+ reader = ForceSerialReader(hand_type=HandType.right, config=config)
+
+ errors = []
+
+ def write_data():
+ for i in range(100):
+ with reader._lock:
+ reader._poslist = [float(i)] * 21
+ time.sleep(0.001)
+
+ def read_data():
+ for _ in range(100):
+ try:
+ data = reader.poslist
+ assert len(data) == 21
+ except Exception as e:
+ errors.append(e)
+ time.sleep(0.001)
+
+ writer = threading.Thread(target=write_data)
+ readers = [threading.Thread(target=read_data) for _ in range(3)]
+
+ writer.start()
+ for r in readers:
+ r.start()
+
+ writer.join()
+ for r in readers:
+ r.join()
+
+ assert len(errors) == 0
+
+ def test_version_frame():
+ config = SerialConfig(auto_reconnect=False)
+ reader = ForceSerialReader(hand_type=HandType.right, config=config)
+
+ # 模拟版本帧
+ value = 10001
+ status_code = 1
+ frame_data = struct.pack('h', force) for force in forces)
+
+ reader._handle_force_frame(frame_data)
+
+ realforcelist = reader.realforcelist
+ assert len(realforcelist) == 3
+ assert realforcelist == forces
+
+ def test_pack_data():
+ config = SerialConfig(auto_reconnect=False)
+ reader = ForceSerialReader(hand_type=HandType.right, config=config)
+
+ pack_01 = reader._pack_01_data()
+ assert pack_01[0] == 0x5D
+ assert pack_01[1] == 0x01
+ assert pack_01[2] == 0x00
+
+ pack_03 = reader._pack_03_data()
+ assert pack_03[0] == 0x5D
+ assert pack_03[1] == 0x03
+
+ runner.test("ForceReader初始状态", test_initial)
+ runner.test("ForceReader线程安全", test_thread_safe)
+ runner.test("ForceReader版本帧处理", test_version_frame)
+ runner.test("ForceReader位置帧处理", test_position_frame)
+ runner.test("ForceReader力数据帧处理", test_force_frame)
+ runner.test("ForceReader数据打包", test_pack_data)
+ return runner
+
+
+def test_concurrent_access():
+ """并发访问压力测试"""
+ runner = TestRunner()
+
+ def test_concurrent():
+ config = SerialConfig(auto_reconnect=False)
+ reader = ForceSerialReader(hand_type=HandType.right, config=config)
+
+ read_count = [0]
+ write_count = [0]
+
+ def writer():
+ for i in range(100):
+ with reader._lock:
+ reader._poslist = [float(i)] * 21
+ write_count[0] += 1
+ time.sleep(0.0001)
+
+ def reader_thread():
+ for _ in range(100):
+ data = reader.poslist
+ assert len(data) == 21
+ read_count[0] += 1
+ time.sleep(0.0001)
+
+ threads = [
+ threading.Thread(target=writer),
+ threading.Thread(target=reader_thread),
+ threading.Thread(target=reader_thread)
+ ]
+
+ for t in threads:
+ t.start()
+ for t in threads:
+ t.join()
+
+ assert write_count[0] == 100
+ assert read_count[0] == 200
+
+ runner.test("并发访问压力测试", test_concurrent)
+ return runner
+
+
+def main():
+ print("=" * 60)
+ print("LinkerForce 改进版验证测试")
+ print("=" * 60)
+ print()
+
+ # 运行所有测试
+ results = []
+
+ print("【配置类测试】")
+ results.append(test_serial_config())
+ print()
+
+ print("【设备信息测试】")
+ results.append(test_device_info())
+ print()
+
+ print("【帧解析器测试】")
+ results.append(test_frame_parser())
+ print()
+
+ print("【ForceReader测试】")
+ results.append(test_force_reader())
+ print()
+
+ print("【并发访问测试】")
+ results.append(test_concurrent_access())
+ print()
+
+ # 汇总结果
+ print("=" * 60)
+ print("总体测试报告")
+ print("=" * 60)
+
+ total_passed = sum(r.tests_passed for r in results)
+ total_failed = sum(r.tests_failed for r in results)
+
+ print(f"总通过: {total_passed}")
+ print(f"总失败: {total_failed}")
+ print(f"成功率: {total_passed / (total_passed + total_failed) * 100:.1f}%")
+
+ if total_failed == 0:
+ print("\n✓ 所有测试通过！")
+ return 0
+ else:
+ print("\n✗ 存在失败的测试")
+ return 1
+
+
+if __name__ == '__main__':
+ sys.exit(main())
\ No newline at end of file
diff --git a/src/linkerhand_retarget/tests/unit/test_linkermcgcore.py b/src/linkerhand_retarget/tests/unit/test_linkermcgcore.py
new file mode 100644
index 0000000..671ecab
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/test_linkermcgcore.py
@@ -0,0 +1,52 @@
+import pytest
+from linkerhand_retarget.linkerhand.linkermcgcore import HaoCunData, HaoCunScoketUdp, HandData, NODES_HAND, LOG_FILE_PATH
+
+
+class TestHaoCunData:
+ def test_initialization(self):
+ data = HaoCunData()
+ assert data.is_update == False
+ assert data.frame_index == 0
+ assert data.frequency == 0
+
+ def test_jointangle_arrays_length(self):
+ data = HaoCunData()
+ assert len(data.jointangle_rHand) == NODES_HAND
+ assert len(data.jointangle_lHand) == NODES_HAND
+
+
+class TestHaoCunScoketUdp:
+ def test_initialization_default(self):
+ udp = HaoCunScoketUdp()
+ assert udp.socket_udp is None
+ assert udp.isconnect == False
+
+ def test_initialization_custom_params(self):
+ udp = HaoCunScoketUdp(host='192.168.1.1', port=8000, buffer_size=4096)
+ assert udp.udp_thread is None
+ assert udp.udp_running == False
+
+ def test_is_use_face_blendshapes_default_false(self):
+ udp = HaoCunScoketUdp()
+ assert udp.is_use_face_blend_shapes_arkit == False
+
+
+class TestHandData:
+ def test_hand_data_creation(self):
+ data = HandData(
+ pitch=[0]*5,
+ side=[0]*5,
+ roll=[0]*5,
+ two_pitch=[0]*5,
+ end_pitch=[0]*5
+ )
+ assert len(data.pitch) == 5
+ assert len(data.side) == 5
+
+
+class TestConstants:
+ def test_nodes_hand_value(self):
+ assert NODES_HAND == 25
+
+ def test_log_file_path(self):
+ assert LOG_FILE_PATH == "/tmp/a.log"
diff --git a/src/linkerhand_retarget/tests/unit/test_mapping_curve.py b/src/linkerhand_retarget/tests/unit/test_mapping_curve.py
new file mode 100644
index 0000000..5bcc822
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/test_mapping_curve.py
@@ -0,0 +1,148 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+"""
+Mapping Curve Test Script
+Test open → opose → fist interpolation for specified joints
+
+Usage:
+ python3 test_mapping_curve.py [joint_index]
+
+ joint_index: 0-20 (sensor index), default: all end joints
+
+ Joint mapping:
+ 2: Thumb Root Flexion
+ 4: Thumb End Flexion
+ 6: Index Root Flexion
+ 8: Index End Flexion
+ 10: Middle Root Flexion
+ 12: Middle End Flexion
+ 14: Ring Root Flexion
+ 16: Ring End Flexion
+ 18: Pinky Root Flexion
+ 20: Pinky End Flexion
+
+Example:
+ python3 test_mapping_curve.py 6 # Index Root Flexion
+ python3 test_mapping_curve.py # All end joints
+"""
+
+import sys
+import json
+import numpy as np
+from pathlib import Path
+
+TMP_FILE = Path(__file__).resolve().parent.parent.parent / "linkerhand_retarget" / "motion" / "linkerforce" / "tmp" / "jointangle_data.tmp"
+with open(TMP_FILE) as f:
+ data = json.load(f)
+
+open_r = data['jointangleoriginal_r']
+opose_r = data['jointangleopose_r']
+fist_r = data['jointanglefist_r']
+
+JOINT_NAMES = {
+ 2: '拇指根部', 4: '拇指末端',
+ 6: '食指根部', 8: '食指末端',
+ 10: '中指根部', 12: '中指末端',
+ 14: '无名指根部', 16: '无名指末端',
+ 18: '小指根部', 20: '小指末端',
+}
+
+EXP_FACTORS = {
+ 2: 5, 4: 7,
+ 6: 10, 8: 3,
+ 10: 5, 12: 10,
+ 14: 5, 16: 18,
+ 18: 5, 20: 8,
+}
+
+MOTOR_OPEN = 255
+MOTOR_OPOSE = 128
+MOTOR_FIST = 0
+
+def interpolate(a, b, t):
+ return a + (b - a) * t
+
+def map_value(sensor_val, sensor_open, sensor_opose, sensor_fist, exp_factor):
+ if abs(sensor_opose - sensor_open) < 1e-6:
+ normalized = 0.5
+ else:
+ normalized = (sensor_val - sensor_open) / (sensor_opose - sensor_open)
+
+ if normalized <= 0:
+ return MOTOR_OPEN, normalized
+ elif normalized <= 1:
+ return MOTOR_OPEN + normalized * (MOTOR_OPOSE - MOTOR_OPEN), normalized
+ else:
+ exceed_amount = normalized - 1.0
+ slope = MOTOR_FIST - MOTOR_OPOSE
+ if exp_factor == 1.0:
+ extension = exceed_amount * slope
+ else:
+ linear_extension = exceed_amount * slope
+ exp_multiplier = 1.0 + (exp_factor - 1.0) * exceed_amount
+ extension = linear_extension * exp_multiplier
+ result = MOTOR_OPOSE + extension
+ return max(MOTOR_FIST, result), normalized
+
+def test_joint(idx):
+ name = JOINT_NAMES.get(idx, f'关节{idx}')
+ exp_factor = EXP_FACTORS.get(idx, 5)
+
+ sensor_open = open_r[idx]
+ sensor_opose = opose_r[idx]
+ sensor_fist = fist_r[idx]
+
+ normalized_fist = (sensor_fist - sensor_open) / (sensor_opose - sensor_open) if abs(sensor_opose - sensor_open) > 1e-6 else 0.5
+
+ print(f"【{name}】 (传感器索引 {idx}, exp_factor={exp_factor})")
+ print("-" * 75)
+ print(f"传感器值: open={sensor_open:.4f}, opose={sensor_opose:.4f}, fist={sensor_fist:.4f}")
+ print(f"normalized_fist={normalized_fist:.4f}")
+ print()
+ print(f"{'阶段':<12} {'插值t':<8} {'传感器值':<12} {'normalized':<12} {'电机值':<10} {'说明'}")
+ print("-" * 75)
+
+ for t in np.arange(0, 1.1, 0.1):
+ sensor_val = interpolate(sensor_open, sensor_opose, t)
+ motor_val, normalized = map_value(sensor_val, sensor_open, sensor_opose, sensor_fist, exp_factor)
+ print(f"open→opose {t:<8.1f} {sensor_val:<12.4f} {normalized:<12.4f} {motor_val:<10.1f}")
+
+ print()
+
+ for t in np.arange(0, 1.1, 0.1):
+ sensor_val = interpolate(sensor_opose, sensor_fist, t)
+ motor_val, normalized = map_value(sensor_val, sensor_open, sensor_opose, sensor_fist, exp_factor)
+ phase = "延伸" if t > 0 else "opose"
+ print(f"opose→fist {t:<8.1f} {sensor_val:<12.4f} {normalized:<12.4f} {motor_val:<10.1f} {phase}")
+
+ print()
+
+def main():
+ if len(sys.argv) > 1:
+ try:
+ idx = int(sys.argv[1])
+ if idx not in JOINT_NAMES:
+ print(f"错误: 不支持关节索引 {idx}")
+ print("有效索引:", sorted(JOINT_NAMES.keys()))
+ return
+ print("=" * 80)
+ print(f"映射曲线测试 - 关节 {idx}")
+ print("=" * 80)
+ print()
+ test_joint(idx)
+ except ValueError:
+ print(__doc__)
+ else:
+ print("=" * 80)
+ print("映射曲线测试 - 所有末端关节")
+ print("=" * 80)
+ print()
+ for idx in [4, 8, 12, 16, 20]:
+ test_joint(idx)
+
+ print("=" * 80)
+ print("测试完成")
+ print("=" * 80)
+
+if __name__ == "__main__":
+ main()
\ No newline at end of file
diff --git a/src/linkerhand_retarget/tests/unit/test_sensenovacore.py b/src/linkerhand_retarget/tests/unit/test_sensenovacore.py
new file mode 100644
index 0000000..b2b6e5f
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/test_sensenovacore.py
@@ -0,0 +1,7 @@
+import pytest
+from linkerhand_retarget.linkerhand.sensenovacore import NODES_HAND
+
+
+class TestSensenovaConstants:
+ def test_nodes_hand_value(self):
+ assert NODES_HAND == 30
diff --git a/src/linkerhand_retarget/tests/unit/test_udexrealcore.py b/src/linkerhand_retarget/tests/unit/test_udexrealcore.py
new file mode 100644
index 0000000..34f0f61
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/test_udexrealcore.py
@@ -0,0 +1,96 @@
+import pytest
+from linkerhand_retarget.linkerhand.udexrealcore import (
+ UdexRealData, MotionData, Bone, Parameter, DeviceData,
+ NODES_HAND, NO_DATA_TIMEOUT
+)
+
+
+class TestUdexRealData:
+ def test_initialization(self):
+ data = UdexRealData()
+ assert data.is_update == False
+ assert data.frame_index == 0
+ assert data.frequency == 0
+
+ def test_jointangle_arrays_length(self):
+ data = UdexRealData()
+ assert len(data.jointangle_rHand) == NODES_HAND
+ assert len(data.jointangle_lHand) == NODES_HAND
+
+ def test_jointderict_arrays_length(self):
+ data = UdexRealData()
+ assert len(data.jointderict_rHand) == NODES_HAND
+ assert len(data.jointderict_lHand) == NODES_HAND
+
+ def test_jointderict_default_values(self):
+ data = UdexRealData()
+ assert all(v == 1 for v in data.jointderict_rHand)
+ assert all(v == 1 for v in data.jointderict_lHand)
+
+ def test_timeout_attributes(self):
+ data = UdexRealData()
+ assert data.last_data_time == 0.0
+ assert data.is_data_timeout == False
+
+
+class TestBone:
+ def test_bone_creation(self):
+ bone = Bone(
+ Name="test_bone",
+ Parent=1,
+ Location=[0.0, 0.0, 0.0],
+ Rotation=[0.0, 0.0, 0.0, 1.0],
+ Scale=[1.0, 1.0, 1.0]
+ )
+ assert bone.Name == "test_bone"
+ assert bone.Parent == 1
+ assert bone.Location == [0.0, 0.0, 0.0]
+
+
+class TestParameter:
+ def test_parameter_creation(self):
+ param = Parameter(Name="test_param", Value=1.0)
+ assert param.Name == "test_param"
+ assert param.Value == 1.0
+
+ def test_parameter_int_value(self):
+ param = Parameter(Name="int_param", Value=10)
+ assert param.Value == 10
+
+ def test_parameter_bool_value(self):
+ param = Parameter(Name="bool_param", Value=True)
+ assert param.Value == True
+
+
+class TestDeviceData:
+ def test_device_data_creation(self):
+ bone = Bone(Name="bone1", Parent=0, Location=[0,0,0], Rotation=[0,0,0,1], Scale=[1,1,1])
+ param = Parameter(Name="param1", Value=1.0)
+ device = DeviceData(Bones=[bone], Parameter=[param])
+
+ assert len(device.Bones) == 1
+ assert len(device.Parameter) == 1
+
+
+class TestMotionData:
+ def test_initialization_empty(self):
+ motion = MotionData({})
+ assert motion.devices == {}
+
+ def test_get_device_not_found(self):
+ motion = MotionData({})
+ result = motion.get_device("nonexistent")
+ assert result is None
+
+ def test_list_sequence_params_empty(self):
+ motion = MotionData({})
+ with pytest.raises(ValueError):
+ motion.list_sequence_params("nonexistent", "prefix")
+
+
+class TestConstants:
+ def test_nodes_hand_value(self):
+ assert NODES_HAND == 24
+
+ def test_no_data_timeout_value(self):
+ assert NO_DATA_TIMEOUT == 1.0
diff --git a/src/linkerhand_retarget/tests/unit/test_utils.py b/src/linkerhand_retarget/tests/unit/test_utils.py
new file mode 100644
index 0000000..7c5f8e0
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/test_utils.py
@@ -0,0 +1,331 @@
+import pytest
+import numpy as np
+import tempfile
+import os
+import yaml
+from linkerhand_retarget.linkerhand.utils import (
+ DataSource,
+ read_yaml,
+ extract_dataset_folder_last_two_digits,
+ translate_wrist_to_origin,
+ apply_pose_matrix,
+ inverse_transformation,
+ trans_xyzwori_to_wxyzori,
+ trans_wxyzori_to_xyzwori,
+ scale_value,
+ is_within_range,
+ extend_line,
+ poseture_to_matrix,
+ cal_distance,
+ change_list,
+ quaternion_conjugate,
+ quaternion_norm_squared,
+ quaternion_inverse,
+ quaternion_multiply,
+ unitydata_to_worldspacedata,
+ get_quaternion_relative,
+ get_child_quaternion,
+ rotate_matrix_x,
+ rotate_matrix_y,
+ rotate_matrix_z,
+ rotate_quaternion,
+ cubic_model,
+)
+
+
+class TestDataSource:
+ def test_enum_values(self):
+ assert DataSource.motion.value == 1
+ assert DataSource.video.value == 2
+ assert DataSource.vr.value == 3
+
+ def test_enum_names(self):
+ assert DataSource.motion.name == "motion"
+ assert DataSource.video.name == "video"
+ assert DataSource.vr.name == "vr"
+
+
+class TestReadYaml:
+ def test_read_valid_yaml(self):
+ with tempfile.NamedTemporaryFile(mode='w', suffix='.yaml', delete=False) as f:
+ yaml.dump({'key': 'value', 'number': 42}, f)
+ f.flush()
+ config = read_yaml(f.name)
+ assert config['key'] == 'value'
+ assert config['number'] == 42
+ os.unlink(f.name)
+
+ def test_read_nested_yaml(self):
+ data = {'a': {'b': {'c': 1}}}
+ with tempfile.NamedTemporaryFile(mode='w', suffix='.yaml', delete=False) as f:
+ yaml.dump(data, f)
+ f.flush()
+ config = read_yaml(f.name)
+ assert config['a']['b']['c'] == 1
+ os.unlink(f.name)
+
+
+class TestExtractDatasetFolderLastTwoDigits:
+ def test_valid_two_digits(self):
+ assert extract_dataset_folder_last_two_digits("folder23") == 23
+ assert extract_dataset_folder_last_two_digits("data99") == 99
+
+ def test_single_digit(self):
+ assert extract_dataset_folder_last_two_digits("folder05") == 5
+
+ def test_no_digits(self):
+ assert extract_dataset_folder_last_two_digits("folder") == -1
+ assert extract_dataset_folder_last_two_digits("abc") == -1
+
+
+class TestTranslateWristToOrigin:
+ def test_basic_translation(self):
+ joint_positions = np.array([[1.0, 2.0, 3.0], [4.0, 5.0, 6.0], [7.0, 8.0, 9.0]])
+ result = translate_wrist_to_origin(joint_positions)
+ assert np.allclose(result[0], [0.0, 0.0, 0.0])
+ assert np.allclose(result[1], [3.0, 3.0, 3.0])
+ assert np.allclose(result[2], [6.0, 6.0, 6.0])
+
+ def test_single_point(self):
+ joint_positions = np.array([[1.0, 2.0, 3.0]])
+ result = translate_wrist_to_origin(joint_positions)
+ assert np.allclose(result, [[0.0, 0.0, 0.0]])
+
+
+class TestApplyPoseMatrix:
+ def test_identity_matrix(self):
+ joint_positions = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]])
+ pose_matrix = np.eye(4)
+ result = apply_pose_matrix(joint_positions, pose_matrix)
+ assert np.allclose(result, joint_positions)
+
+ def test_translation_matrix(self):
+ joint_positions = np.array([[1.0, 0.0, 0.0], [0.0, 1.0, 0.0]])
+ pose_matrix = np.eye(4)
+ pose_matrix[:3, 3] = [10.0, 20.0, 30.0]
+ result = apply_pose_matrix(joint_positions, pose_matrix)
+ assert np.allclose(result[0], [11.0, 20.0, 30.0])
+ assert np.allclose(result[1], [10.0, 21.0, 30.0])
+
+
+class TestInverseTransformation:
+ def test_identity(self):
+ matrix = np.eye(4)
+ result = inverse_transformation(matrix)
+ assert np.allclose(result, np.eye(4))
+
+ def test_translation_only(self):
+ matrix = np.eye(4)
+ matrix[:3, 3] = [1.0, 2.0, 3.0]
+ result = inverse_transformation(matrix)
+ assert np.allclose(result[:3, 3], [-1.0, -2.0, -3.0])
+
+ def test_rotation_only(self):
+ matrix = np.eye(4)
+ matrix[:3, :3] = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
+ result = inverse_transformation(matrix)
+ assert np.allclose(result[:3, :3], matrix[:3, :3].T)
+
+
+class TestQuaternionConversions:
+ def test_trans_xyzwori_to_wxyzori(self):
+ ori_xyzw = [0.1, 0.2, 0.3, 0.4]
+ result = trans_xyzwori_to_wxyzori(ori_xyzw)
+ assert result == (0.4, 0.1, 0.2, 0.3)
+
+ def test_trans_wxyzori_to_xyzwori(self):
+ ori_wxyz = [0.4, 0.1, 0.2, 0.3]
+ result = trans_wxyzori_to_xyzwori(ori_wxyz)
+ assert result == (0.1, 0.2, 0.3, 0.4)
+
+ def test_quaternion_roundtrip(self):
+ original = [0.1, 0.2, 0.3, 0.4]
+ wxyz = trans_xyzwori_to_wxyzori(original)
+ back = trans_wxyzori_to_xyzwori(wxyz)
+ assert np.allclose(back, original)
+
+
+class TestScaleValue:
+ def test_identity_scale(self):
+ result = scale_value(5.0, 0.0, 10.0, 0.0, 10.0)
+ assert result == 5.0
+
+ def test_range_conversion(self):
+ result = scale_value(5.0, 0.0, 10.0, 0.0, 100.0)
+ assert result == 50.0
+
+ def test_negative_range(self):
+ result = scale_value(5.0, 0.0, 10.0, -100.0, 0.0)
+ assert result == -50.0
+
+ def test_out_of_bounds(self):
+ result = scale_value(15.0, 0.0, 10.0, 0.0, 100.0)
+ assert result == 150.0
+
+
+class TestIsWithinRange:
+ def test_within_bounds(self):
+ assert is_within_range(5.0, 0.0, 10.0) == 5.0
+
+ def test_above_max(self):
+ assert is_within_range(15.0, 0.0, 10.0) == 10.0
+
+ def test_below_min(self):
+ assert is_within_range(-5.0, 0.0, 10.0) == 0.0
+
+
+class TestExtendLine:
+ def test_extend_positive(self):
+ point1 = [0.0, 0.0, 0.0]
+ point2 = [1.0, 0.0, 0.0]
+ result = extend_line(point1, point2, 1.0)
+ assert np.allclose(result, [2.0, 0.0, 0.0])
+
+ def test_extend_negative(self):
+ point1 = [0.0, 0.0, 0.0]
+ point2 = [1.0, 0.0, 0.0]
+ result = extend_line(point1, point2, -0.5)
+ assert np.allclose(result, [0.5, 0.0, 0.0])
+
+
+class TestPosetureToMatrix:
+ def test_identity_rotation(self):
+ position = [1.0, 2.0, 3.0]
+ ori = [0.0, 0.0, 0.0, 1.0]
+ matrix = poseture_to_matrix(position, ori)
+ assert np.allclose(matrix[:3, 3], position)
+
+ def test_180_degree_rotation(self):
+ position = [0.0, 0.0, 0.0]
+ ori = [1.0, 0.0, 0.0, 0.0]
+ matrix = poseture_to_matrix(position, ori)
+ assert np.allclose(matrix[:3, 3], position)
+
+
+class TestCalDistance:
+ def test_same_point(self):
+ assert cal_distance([1.0, 2.0, 3.0], [1.0, 2.0, 3.0]) == 0.0
+
+ def test_unit_distance(self):
+ assert cal_distance([0.0, 0.0, 0.0], [1.0, 0.0, 0.0]) == 1.0
+
+ def test_3d_distance(self):
+ result = cal_distance([0.0, 0.0, 0.0], [1.0, 2.0, 2.0])
+ assert np.isclose(result, 3.0)
+
+
+class TestChangeList:
+ def test_none_conversion(self):
+ input_list = ['None', '1', '2']
+ result = change_list(input_list)
+ assert result == [None, '1', '2']
+
+ def test_no_none(self):
+ input_list = ['1', '2', '3']
+ result = change_list(input_list)
+ assert result == ['1', '2', '3']
+
+ def test_all_none(self):
+ input_list = ['None', 'None']
+ result = change_list(input_list)
+ assert result == [None, None]
+
+
+class TestQuaternionOperations:
+ def test_quaternion_conjugate(self):
+ q = [1.0, 2.0, 3.0, 4.0]
+ result = quaternion_conjugate(q)
+ assert np.allclose(result, [-1.0, -2.0, -3.0, 4.0])
+
+ def test_quaternion_norm_squared(self):
+ q = [1.0, 2.0, 2.0, 2.0]
+ result = quaternion_norm_squared(q)
+ assert result == 13.0
+
+ def test_quaternion_inverse(self):
+ q = [0.0, 0.0, 0.0, 1.0]
+ result = quaternion_inverse(q)
+ assert np.allclose(result, [0.0, 0.0, 0.0, 1.0])
+
+ def test_quaternion_multiply_identity(self):
+ q = [0.0, 0.0, 0.0, 1.0]
+ result = quaternion_multiply(q, q)
+ assert np.allclose(result, q)
+
+ def test_quaternion_multiply_rotation(self):
+ q1 = [0.0, 0.0, 0.0, 1.0]
+ q2 = [0.0, 0.0, 0.707, 0.707]
+ result = quaternion_multiply(q1, q2)
+ assert np.allclose(result, q2, atol=0.01)
+
+
+class TestUnitydataToWorldspacedata:
+ def test_basic_conversion(self):
+ positions = [[1.0, 2.0, 3.0], [4.0, 5.0, 6.0]]
+ result = unitydata_to_worldspacedata(positions)
+ assert result == [[1.0, 3.0, 2.0], [4.0, 6.0, 5.0]]
+
+
+class TestQuaternionRelative:
+ def test_identity_orientation(self):
+ ori = [0.0, 0.0, 0.0, 1.0]
+ targetori = [0.0, 0.0, 0.0, 1.0]
+ result = get_quaternion_relative(ori, targetori)
+ assert np.allclose(result, [0.0, 0.0, 0.0, 1.0], atol=0.01)
+
+
+class TestGetChildQuaternion:
+ def test_identity_combination(self):
+ ori = [0.0, 0.0, 0.0, 1.0]
+ ori_relative = [0.0, 0.0, 0.0, 1.0]
+ result = get_child_quaternion(ori, ori_relative)
+ assert np.allclose(result, [0.0, 0.0, 0.0, 1.0], atol=0.01)
+
+
+class TestRotateMatrix:
+ def test_rotate_matrix_x_90(self):
+ result = rotate_matrix_x(np.pi / 2)
+ expected = np.array([[1, 0, 0], [0, 0, -1], [0, 1, 0]])
+ assert np.allclose(result, expected)
+
+ def test_rotate_matrix_y_90(self):
+ result = rotate_matrix_y(np.pi / 2)
+ expected = np.array([[0, 0, 1], [0, 1, 0], [-1, 0, 0]])
+ assert np.allclose(result, expected)
+
+ def test_rotate_matrix_z_90(self):
+ result = rotate_matrix_z(np.pi / 2)
+ expected = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]])
+ assert np.allclose(result, expected)
+
+ def test_rotate_matrix_identity(self):
+ assert np.allclose(rotate_matrix_x(0), np.eye(3))
+ assert np.allclose(rotate_matrix_y(0), np.eye(3))
+ assert np.allclose(rotate_matrix_z(0), np.eye(3))
+
+
+class TestRotateQuaternion:
+ def test_no_rotation(self):
+ original_quat = [0.0, 0.0, 0.0, 1.0]
+ result = rotate_quaternion(original_quat, 0, 0, 0)
+ assert np.allclose(result, original_quat, atol=0.01)
+
+ def test_180_degree_roll(self):
+ original_quat = [0.0, 0.0, 0.0, 1.0]
+ result = rotate_quaternion(original_quat, 180, 0, 0)
+ assert np.allclose(result, [1.0, 0.0, 0.0, 0.0], atol=0.01)
+
+
+class TestCubicModel:
+ def test_basic_evaluation(self):
+ result = cubic_model(1.0, 1.0, 1.0, 1.0, 1.0)
+ assert result == 4.0
+
+ def test_zero_coefficients(self):
+ result = cubic_model(2.0, 0.0, 0.0, 0.0, 5.0)
+ assert result == 5.0
+
+ def test_array_input(self):
+ x = np.array([0.0, 1.0, 2.0])
+ result = cubic_model(x, 1.0, 0.0, 0.0, 0.0)
+ assert np.allclose(result, [0.0, 1.0, 8.0])
diff --git a/src/linkerhand_retarget/tests/unit/test_vtrdyncore.py b/src/linkerhand_retarget/tests/unit/test_vtrdyncore.py
new file mode 100644
index 0000000..3cdc6e2
--- /dev/null
+++ b/src/linkerhand_retarget/tests/unit/test_vtrdyncore.py
@@ -0,0 +1,76 @@
+import pytest
+from linkerhand_retarget.linkerhand.vtrdyncore import MocapData, VtrdynSocketUdp, NODES_BODY, NODES_HAND, NODES_FACEBS_ARKIT, NODES_FACEBS_AUDIO
+
+
+class TestMocapData:
+ def test_initialization(self):
+ mocap = MocapData()
+ assert mocap.is_update == False
+ assert mocap.frame_index == 0
+ assert mocap.frequency == 0
+
+ def test_body_arrays_length(self):
+ mocap = MocapData()
+ assert len(mocap.sensor_state_body) == NODES_BODY
+ assert len(mocap.position_body) == NODES_BODY
+ assert len(mocap.quaternion_body) == NODES_BODY
+ assert len(mocap.gyr_body) == NODES_BODY
+ assert len(mocap.acc_body) == NODES_BODY
+ assert len(mocap.velocity_body) == NODES_BODY
+
+ def test_hand_arrays_length(self):
+ mocap = MocapData()
+ assert len(mocap.sensor_state_r_hand) == NODES_HAND
+ assert len(mocap.position_rHand) == NODES_HAND
+ assert len(mocap.quaternion_rHand) == NODES_HAND
+
+ def test_face_blendshapes_length(self):
+ mocap = MocapData()
+ assert len(mocap.face_blend_shapes_arkit) == NODES_FACEBS_ARKIT
+ assert len(mocap.face_blend_shapes_audio) == NODES_FACEBS_AUDIO
+
+ def test_eyeball_quaternion_length(self):
+ mocap = MocapData()
+ assert len(mocap.local_quat_right_eyeball) == 4
+ assert len(mocap.local_quat_left_eyeball) == 4
+
+
+class TestVtrdynSocketUdp:
+ def test_initialization(self):
+ udp = VtrdynSocketUdp()
+ assert udp.socket_udp is None
+ assert udp.isconnect == False
+
+ def test_initialization_with_debug(self):
+ udp = VtrdynSocketUdp(debug=True)
+ assert udp.debug == True
+
+ def test_mocap_data_initialized(self):
+ udp = VtrdynSocketUdp()
+ assert udp.mocap_data_realtime is not None
+
+ def test_data_lock_initialized(self):
+ udp = VtrdynSocketUdp()
+ assert udp.data_lock is not None
+
+ def test_send_running_default_false(self):
+ udp = VtrdynSocketUdp()
+ assert udp.send_running == False
+
+ def test_thread_initialized(self):
+ udp = VtrdynSocketUdp()
+ assert udp.send_thread is None
+
+
+class TestConstants:
+ def test_nodes_body_value(self):
+ assert NODES_BODY == 23
+
+ def test_nodes_hand_value(self):
+ assert NODES_HAND == 20
+
+ def test_nodes_facebs_arkit_value(self):
+ assert NODES_FACEBS_ARKIT == 52
+
+ def test_nodes_facebs_audio_value(self):
+ assert NODES_FACEBS_AUDIO == 26
diff --git a/src/pressure_diagram/launch/pressure_diagram.launch.py b/src/pressure_diagram/launch/pressure_diagram.launch.py
new file mode 100644
index 0000000..34a5800
--- /dev/null
+++ b/src/pressure_diagram/launch/pressure_diagram.launch.py
@@ -0,0 +1,17 @@
+#!/usr/bin/env python3
+from launch import LaunchDescription
+from launch_ros.actions import Node
+
+def generate_launch_description():
+ return LaunchDescription([
+ Node(
+ package='pressure_diagram',
+ executable='pressure_diagram',
+ name='pressure_diagram_node',
+ output='screen',
+ parameters=[{
+
+ }],
+ ),
+
+ ])
\ No newline at end of file
diff --git a/src/pressure_diagram/package.xml b/src/pressure_diagram/package.xml
new file mode 100644
index 0000000..15650cc
--- /dev/null
+++ b/src/pressure_diagram/package.xml
@@ -0,0 +1,28 @@
+
+
+
+ pressure_diagram
+ 0.0.0
+ ROS2 Pressure Diagram - Real-time pressure sensor visualization for Linker Hand
+ linkerhand
+ Apache-2.0
+
+
+ rclpy
+ std_msgs
+ ament_index_python
+
+
+ python3-pyqt5
+ python3-pyqtgraph
+ python3-numpy
+
+ ament_copyright
+ ament_flake8
+ ament_pep257
+ python3-pytest
+
+
+ ament_python
+
+
diff --git a/src/pressure_diagram/pressure_diagram/__init__.py b/src/pressure_diagram/pressure_diagram/__init__.py
new file mode 100644
index 0000000..e69de29
diff --git a/src/pressure_diagram/pressure_diagram/pressure_diagram.py b/src/pressure_diagram/pressure_diagram/pressure_diagram.py
new file mode 100644
index 0000000..9e78244
--- /dev/null
+++ b/src/pressure_diagram/pressure_diagram/pressure_diagram.py
@@ -0,0 +1,421 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+
+import rclpy
+from rclpy.node import Node
+from rclpy.qos import QoSProfile, ReliabilityPolicy, HistoryPolicy
+import signal
+import sys
+import json
+import numpy as np
+from std_msgs.msg import String
+from PyQt5 import QtWidgets, QtCore, QtGui
+import pyqtgraph as pg
+
+pg.setConfigOption('imageAxisOrder', 'row-major')
+pg.setConfigOption('useOpenGL', False)
+
+class PressureDiagram(Node, QtWidgets.QMainWindow):
+ def __init__(self):
+ # 先调用 Node 的 __init__，避免 super() 歧义
+ Node.__init__(self, 'pressure_diagram')
+ QtWidgets.QMainWindow.__init__(self)
+
+ # 键名与ROS数据一致
+ self.fingers_matrix = ['thumb_matrix', 'index_matrix', 'middle_matrix', 'ring_matrix', 'little_matrix']
+ self.fingers_mass = ['thumb_mass', 'index_mass', 'middle_mass', 'ring_mass', 'little_mass']
+
+ self.finger_colors = [
+ (255, 75, 75), # Thumb - Red
+ (50, 205, 50), # Index - Green
+ (65, 105, 255), # Middle - Blue
+ (255, 215, 0), # Ring - Gold
+ (218, 112, 214) # Pinky - Purple
+ ]
+
+ # 波形图数据
+ self.wave_data = {f: np.zeros(100) for f in self.fingers_mass}
+ self.current_wave_values = {f: 0.0 for f in self.fingers_mass}
+
+ # 热力图数据 - 12行×6列 (高大于宽)
+ self.matrix_data = {f: np.zeros((12, 6)) for f in self.fingers_matrix}
+ self.matrix_shapes = {f: (12, 6) for f in self.fingers_matrix} # (rows, cols)
+ self.data_received = {f: False for f in self.fingers_matrix}
+
+ self.wave_sub = None
+ self.matrix_sub = None
+
+ # ROS2 节流计时器
+ self.last_matrix_log_time = {f: None for f in self.fingers_matrix}
+
+ # QoS 配置
+ qos_profile = QoSProfile(
+ reliability=ReliabilityPolicy.BEST_EFFORT,
+ history=HistoryPolicy.KEEP_LAST,
+ depth=100
+ )
+ self.qos_profile = qos_profile
+
+ self.init_ui()
+ self.setup_ros()
+
+ def init_ui(self):
+ self.setWindowTitle("Robotic Hand Sensor Fusion Interface")
+ self.resize(1600, 900)
+ self.setStyleSheet("background-color: #0f172a;")
+
+ central_widget = QtWidgets.QWidget()
+ self.setCentralWidget(central_widget)
+
+ main_layout = QtWidgets.QHBoxLayout(central_widget)
+ main_layout.setSpacing(15)
+ main_layout.setContentsMargins(10, 10, 10, 10)
+
+ # ============== 左侧：波形图 ==============
+ left_widget = self.create_waveform_panel()
+ main_layout.addWidget(left_widget, stretch=1)
+
+ line = QtWidgets.QFrame()
+ line.setFrameShape(QtWidgets.QFrame.VLine)
+ line.setStyleSheet("QFrame { background-color: #334155; max-width: 2px; }")
+ main_layout.addWidget(line)
+
+ # ============== 右侧：热力图 ==============
+ right_widget = self.create_heatmap_panel()
+ main_layout.addWidget(right_widget, stretch=1)
+
+ self.timer = QtCore.QTimer()
+ self.timer.timeout.connect(self.update_display)
+ self.timer.start(33)
+
+ def create_waveform_panel(self):
+ panel = QtWidgets.QWidget()
+ layout = QtWidgets.QVBoxLayout(panel)
+ layout.setSpacing(8)
+
+ wave_ctrl = QtWidgets.QFrame()
+ wave_ctrl.setStyleSheet("""QFrame { background-color: #1e293b; border: 2px solid #3b82f6; border-radius: 4px; }""")
+ wave_layout = QtWidgets.QHBoxLayout(wave_ctrl)
+ wave_layout.setContentsMargins(12, 6, 12, 6)
+
+ title = QtWidgets.QLabel("WAVEFORM MONITOR (30Hz)")
+ title.setStyleSheet("color: #60a5fa; font-weight: bold; font-size: 13px;")
+
+ self.wave_combo = QtWidgets.QComboBox()
+ self.wave_combo.addItems([
+ "/cb_left_hand_matrix_touch_mass",
+ "/cb_right_hand_matrix_touch_mass"
+ ])
+ self.wave_combo.setStyleSheet("""
+ QComboBox { color: white; background-color: #334155; border: 1px solid #475569; border-radius: 4px; padding: 4px 8px; min-width: 260px; }
+ QComboBox QAbstractItemView { background-color: #1e293b; color: white; selection-background-color: #3b82f6; }
+ """)
+ self.wave_combo.currentTextChanged.connect(self.switch_wave_topic)
+
+ wave_layout.addWidget(title)
+ wave_layout.addWidget(self.wave_combo)
+ wave_layout.addStretch()
+ layout.addWidget(wave_ctrl)
+
+ self.wave_widget = pg.GraphicsLayoutWidget()
+ self.wave_widget.setBackground('#0f172a')
+ layout.addWidget(self.wave_widget, stretch=1)
+
+ self.wave_plots = {}
+ self.wave_curves = {}
+
+ for i, finger in enumerate(self.fingers_mass):
+ p = self.wave_widget.addPlot(row=i, col=0)
+ p.setMenuEnabled(False)
+ p.setMouseEnabled(x=False, y=False)
+ p.setYRange(0, 4500)
+ p.setXRange(0, 100)
+ p.showGrid(x=True, y=True, alpha=0.3)
+ p.getAxis('left').setTextPen('#64748b')
+ p.getAxis('bottom').setTextPen('#64748b')
+
+ color = QtGui.QColor(*self.finger_colors[i])
+ display_name = finger.replace('_mass', '').upper()
+ p.setTitle(f"[ {display_name} ]", color=color, size='11pt')
+
+ if i == 4:
+ p.getAxis('bottom').setLabel('Time', color='#64748b')
+ else:
+ p.getAxis('bottom').setStyle(showValues=False)
+
+ pen = pg.mkPen(color=self.finger_colors[i], width=2.5)
+ curve = p.plot(pen=pen)
+ self.wave_curves[finger] = curve
+
+ text = pg.TextItem(text="0", color=(255,255,255), anchor=(1, 0.5))
+ text.setFont(QtGui.QFont("Arial", 10, QtGui.QFont.Bold))
+ p.addItem(text)
+ self.wave_curves[finger + '_text'] = text
+
+ return panel
+
+ def create_heatmap_panel(self):
+ panel = QtWidgets.QWidget()
+ layout = QtWidgets.QVBoxLayout(panel)
+ layout.setSpacing(8)
+
+ heat_ctrl = QtWidgets.QFrame()
+ heat_ctrl.setStyleSheet("""QFrame { background-color: #1e293b; border: 2px solid #f43f5e; border-radius: 4px; }""")
+ heat_layout = QtWidgets.QHBoxLayout(heat_ctrl)
+ heat_layout.setContentsMargins(12, 6, 12, 6)
+
+ title = QtWidgets.QLabel("PRESSURE MATRIX (12×6)")
+ title.setStyleSheet("color: #f43f5e; font-weight: bold; font-size: 13px;")
+
+ # 单位标签
+ unit_label = QtWidgets.QLabel("Max Value (g)")
+ unit_label.setStyleSheet("color: #94a3b8; font-size: 11px; border: none; background: transparent;")
+ heat_layout.addWidget(unit_label)
+ heat_layout.addSpacing(10)
+
+ self.matrix_combo = QtWidgets.QComboBox()
+ self.matrix_combo.addItems([
+ "/cb_left_hand_matrix_touch",
+ "/cb_right_hand_matrix_touch"
+ ])
+ self.matrix_combo.setStyleSheet("""
+ QComboBox { color: white; background-color: #334155; border: 1px solid #475569; border-radius: 4px; padding: 4px 8px; min-width: 260px; }
+ QComboBox QAbstractItemView { background-color: #1e293b; color: white; selection-background-color: #f43f5e; }
+ """)
+ self.matrix_combo.currentTextChanged.connect(self.switch_matrix_topic)
+
+ heat_layout.addWidget(title)
+ heat_layout.addWidget(self.matrix_combo)
+ heat_layout.addStretch()
+ layout.addWidget(heat_ctrl)
+
+ heat_container = QtWidgets.QWidget()
+ heat_grid = QtWidgets.QGridLayout(heat_container)
+ heat_grid.setSpacing(10)
+ heat_grid.setContentsMargins(5, 5, 5, 5)
+
+ self.matrix_plots = {}
+ self.matrix_images = {}
+ self.matrix_peaks = {}
+
+ positions = [
+ ('thumb_matrix', 0, 0), ('index_matrix', 0, 1), ('middle_matrix', 0, 2),
+ ('ring_matrix', 1, 0), ('little_matrix', 1, 1)
+ ]
+
+ self.colormap = pg.colormap.get('plasma')
+
+ for idx, (finger, row, col) in enumerate(positions):
+ container = QtWidgets.QWidget()
+ vbox = QtWidgets.QVBoxLayout(container)
+ vbox.setSpacing(2)
+ vbox.setContentsMargins(0, 0, 0, 0)
+
+ display_name = finger.replace('_matrix', '').upper()
+ name_label = QtWidgets.QLabel(display_name)
+ name_label.setAlignment(QtCore.Qt.AlignCenter)
+ color_hex = '#{:02x}{:02x}{:02x}'.format(*self.finger_colors[idx])
+ name_label.setStyleSheet(f"color: {color_hex}; font-weight: bold; font-size: 12px;")
+ vbox.addWidget(name_label)
+
+ # 固定比例显示 12×6 (高:宽 = 2:1)
+ view = pg.PlotWidget()
+ view.setMenuEnabled(False)
+ view.setMouseEnabled(x=False, y=False)
+ view.setAspectLocked(True, ratio=6/12) # X:Y = 1:2
+ view.setMaximumSize(240, 280)
+ view.setMinimumSize(120, 160)
+ view.setBackground('#0f172a')
+ view.hideAxis('left')
+ view.hideAxis('bottom')
+
+ img = pg.ImageItem()
+ img.setLookupTable(self.colormap.getLookupTable())
+ img.setImage(np.zeros((12, 6)), levels=[0, 100])
+ view.addItem(img)
+
+ peak_text = pg.TextItem(text="", color=(255,255,255), anchor=(0.5, 0.5))
+ peak_text.setFont(QtGui.QFont("Arial", 10, QtGui.QFont.Bold))
+ view.addItem(peak_text)
+
+ vbox.addWidget(view, stretch=1)
+ heat_grid.addWidget(container, row, col)
+
+ self.matrix_plots[finger] = view
+ self.matrix_images[finger] = img
+ self.matrix_peaks[finger] = peak_text
+
+ # 颜色条
+ legend_widget = QtWidgets.QWidget()
+ legend_layout = QtWidgets.QVBoxLayout(legend_widget)
+ legend_layout.setAlignment(QtCore.Qt.AlignCenter)
+
+ lbl_max = QtWidgets.QLabel("MAX")
+ lbl_max.setStyleSheet("color: #fbbf24; font-size: 9px;")
+ lbl_max.setAlignment(QtCore.Qt.AlignCenter)
+
+ gradient = pg.GradientWidget(orientation='right')
+ gradient.setMaximumWidth(25)
+ gradient.setMaximumHeight(180)
+ gradient.setColorMap(self.colormap)
+
+ lbl_min = QtWidgets.QLabel("0")
+ lbl_min.setStyleSheet("color: #3b82f6; font-size: 9px;")
+ lbl_min.setAlignment(QtCore.Qt.AlignCenter)
+
+ legend_layout.addWidget(lbl_max)
+ legend_layout.addWidget(gradient, stretch=1)
+ legend_layout.addWidget(lbl_min)
+
+ heat_grid.addWidget(legend_widget, 1, 2)
+
+ layout.addWidget(heat_container, stretch=1)
+ return panel
+
+ def setup_ros(self):
+ self.get_logger().info("Setting up ROS...")
+ self.subscribe_waveform("/cb_left_hand_matrix_touch_mass")
+ self.subscribe_matrix("/cb_left_hand_matrix_touch")
+
+ def subscribe_waveform(self, topic):
+ if self.wave_sub:
+ self.destroy_subscription(self.wave_sub)
+ self.wave_sub = self.create_subscription(String, topic, self.wave_callback, self.qos_profile)
+ self.get_logger().info(f"WAVEFORM: {topic}")
+
+ def subscribe_matrix(self, topic):
+ if self.matrix_sub:
+ self.destroy_subscription(self.matrix_sub)
+ self.matrix_sub = self.create_subscription(String, topic, self.matrix_callback, self.qos_profile)
+ self.get_logger().info(f"MATRIX: {topic}")
+
+ def switch_wave_topic(self, topic):
+ self.subscribe_waveform(topic)
+ for f in self.fingers_mass:
+ self.wave_data[f] = np.zeros(100)
+ self.current_wave_values[f] = 0.0
+
+ def switch_matrix_topic(self, topic):
+ self.subscribe_matrix(topic)
+ for f in self.fingers_matrix:
+ self.matrix_data[f] = np.zeros((12, 6))
+ self.matrix_shapes[f] = (12, 6)
+ self.data_received[f] = False
+
+ def wave_callback(self, msg):
+ try:
+ data = json.loads(msg.data)
+ for finger in self.fingers_mass:
+ if finger in data:
+ self.current_wave_values[finger] = float(data[finger])
+ except Exception as e:
+ pass
+
+ def matrix_callback(self, msg):
+ """处理 12×6 矩阵数据"""
+ try:
+ data = json.loads(msg.data)
+
+ for finger in self.fingers_matrix:
+ if finger not in data:
+ continue
+
+ arr = np.array(data[finger], dtype=np.float32)
+
+ if arr.ndim == 2:
+ rows, cols = arr.shape
+ self.data_received[finger] = True
+ self.matrix_data[finger] = arr
+ self.matrix_shapes[finger] = (rows, cols)
+
+ # ROS2 节流日志
+ current_time = self.get_clock().now()
+ if self.last_matrix_log_time[finger] is None or \
+ (current_time - self.last_matrix_log_time[finger]).nanoseconds > 3e9:
+ self.last_matrix_log_time[finger] = current_time
+ self.get_logger().info(f"{finger}: {rows}×{cols}, max={arr.max():.1f}")
+
+ except Exception as e:
+ self.get_logger().error(f"Matrix error: {str(e)}")
+
+ def update_display(self):
+ # 更新波形图
+ for finger in self.fingers_mass:
+ self.wave_data[finger] = np.roll(self.wave_data[finger], -1)
+ self.wave_data[finger][-1] = self.current_wave_values[finger]
+ self.wave_curves[finger].setData(self.wave_data[finger])
+
+ val = self.current_wave_values[finger]
+ self.wave_curves[finger + '_text'].setText(f"{val:.0f}")
+ self.wave_curves[finger + '_text'].setPos(99, val)
+
+ # 更新热力图 - 修复翻转问题
+ for finger in self.fingers_matrix:
+ data = self.matrix_data[finger]
+ rows, cols = self.matrix_shapes[finger] # 12×6
+
+ if not self.data_received[finger]:
+ continue
+
+ img = self.matrix_images[finger]
+ view = self.matrix_plots[finger]
+ peak_text = self.matrix_peaks[finger]
+
+ max_val = data.max()
+
+ if max_val > 0:
+ display_data = data / max_val if max_val > 1 else data
+ else:
+ display_data = data
+
+ # **关键修复**: 垂直翻转，使第0行(ROS数据第1行)显示在顶部
+ display_data_flipped = np.flipud(display_data)
+
+ # 显示翻转后的数据 (12×6)
+ img.setImage(display_data_flipped, autoLevels=False, levels=[0, 1])
+
+ # 设置视图范围
+ view.setRange(xRange=(-0.5, cols-0.5), yRange=(-0.5, rows-0.5))
+
+ # 显示数值：矩阵中的最大压力值，单位g(克)
+ if max_val > 0:
+ # 找到原始数据中的最大值位置
+ max_idx = np.unravel_index(np.argmax(data), data.shape)
+
+ # 峰值显示格式: 最大值 + 单位
+ peak_text.setText(f"{max_val:.0f}")
+
+ # 坐标转换：因为图像翻转了，y坐标也要翻转
+ # 原始行号row，在翻转后的图像中是 (rows-1-row)
+ flipped_row = (rows - 1) - max_idx[0]
+ peak_text.setPos(max_idx[1], flipped_row)
+ else:
+ peak_text.setText("0")
+
+def signal_handler(sig, frame):
+ QtWidgets.QApplication.quit()
+ sys.exit(0)
+
+def main(args=None):
+ rclpy.init(args=args)
+
+ app = QtWidgets.QApplication(sys.argv)
+ app.setStyle('Fusion')
+
+ signal.signal(signal.SIGINT, signal_handler)
+
+ gui = PressureDiagram()
+ gui.show()
+
+ # ROS2 spin 在 Qt 定时器中处理
+ def ros_spin():
+ rclpy.spin_once(gui, timeout_sec=0)
+
+ ros_timer = QtCore.QTimer()
+ ros_timer.timeout.connect(ros_spin)
+ ros_timer.start(1) # ~1000Hz
+
+ sys.exit(app.exec_())
+
+if __name__ == '__main__':
+ main()
\ No newline at end of file
diff --git a/src/pressure_diagram/resource/pressure_diagram b/src/pressure_diagram/resource/pressure_diagram
new file mode 100644
index 0000000..e69de29
diff --git a/src/pressure_diagram/setup.cfg b/src/pressure_diagram/setup.cfg
new file mode 100644
index 0000000..0a18512
--- /dev/null
+++ b/src/pressure_diagram/setup.cfg
@@ -0,0 +1,4 @@
+[develop]
+script_dir=$base/lib/pressure_diagram
+[install]
+install_scripts=$base/lib/pressure_diagram
diff --git a/src/pressure_diagram/setup.py b/src/pressure_diagram/setup.py
new file mode 100644
index 0000000..7c61879
--- /dev/null
+++ b/src/pressure_diagram/setup.py
@@ -0,0 +1,34 @@
+#!/usr/bin/env python3
+# -*- coding: utf-8 -*-
+import os
+from glob import glob
+from setuptools import find_packages, setup
+package_name = 'pressure_diagram'
+
+setup(
+ name=package_name,
+ version='0.0.0',
+ packages=find_packages(exclude=['test']),
+ data_files=[
+ ('share/ament_index/resource_index/packages',
+ ['resource/' + package_name]),
+ ('share/' + package_name, ['package.xml']),
+ (os.path.join('share', package_name, 'launch'), glob('launch/*.launch.py')),
+ ],
+ install_requires=['setuptools'],
+ zip_safe=True,
+ maintainer='linkerhand',
+ maintainer_email='linkerhand@todo.todo',
+ description='ROS2 Pressure Diagram - Real-time pressure sensor visualization for Linker Hand',
+ license='Apache-2.0',
+ extras_require={
+ 'test': [
+ 'pytest',
+ ],
+ },
+ entry_points={
+ 'console_scripts': [
+ 'pressure_diagram = pressure_diagram.pressure_diagram:main',
+ ],
+ },
+)
diff --git a/src/pressure_diagram/test/test_copyright.py b/src/pressure_diagram/test/test_copyright.py
new file mode 100644
index 0000000..97a3919
--- /dev/null
+++ b/src/pressure_diagram/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'
diff --git a/src/pressure_diagram/test/test_flake8.py b/src/pressure_diagram/test/test_flake8.py
new file mode 100644
index 0000000..27ee107
--- /dev/null
+++ b/src/pressure_diagram/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)
diff --git a/src/pressure_diagram/test/test_pep257.py b/src/pressure_diagram/test/test_pep257.py
new file mode 100644
index 0000000..b234a38
--- /dev/null
+++ b/src/pressure_diagram/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'
diff --git a/src/requirements.txt b/src/requirements.txt
new file mode 100644
index 0000000..4e77d45
--- /dev/null
+++ b/src/requirements.txt
@@ -0,0 +1,14 @@
+anytree
+colorama
+loguru
+lxml
+numpy
+pyqtgraph
+pyserial
+PyYAML
+scipy>=1.10.0
+six
+tqdm
+transforms3d
+trimesh
+tyro
\ No newline at end of file
diff --git a/startup_linkerforce.sh b/startup_linkerforce.sh
new file mode 100644
index 0000000..7ecbe48
--- /dev/null
+++ b/startup_linkerforce.sh
@@ -0,0 +1,15 @@
+#!/bin/bash
+# LinkerForce 手套遥操作启动脚本
+
+# 设置串口权限
+sudo chmod 666 /dev/ttyUSB0
+sudo chmod 666 /dev/ttyUSB1
+
+# source ROS 工作空间(请输入正确的目录)
+source /home/linker-brunt/project/linkerhand_telop_sdk/v2/ros2/install/setup.bash
+
+# 启动程序（强制标定）
+# ros2 run linkerhand_retarget handretarget --ros-args -p calibration:=True
+
+# 无标定启动
+ros2 run linkerhand_retarget handretarget
\ No newline at end of file