admin/linkerhand_retarget_ros2
1
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

chore: save current code

Browse Source
This commit is contained in:
lxp
2026-07-23 11:10:00 +08:00
parent 7953e50ca7
commit 1bec806c6e
8 changed files with 484 additions and 77 deletions
Download Patch File Download Diff File Expand all files Collapse all files
src/linkerhand_retarget/linkerhand_retarget/config/base_config.yml
+5 -5
View File
@@ -27,8 +27,8 @@ serial:
baudrate: 2000000
port: /dev/ttyUSB0
right:
baudrate: 2000000
port: /dev/ttyUSB0
baudrate: null
port: null
serial_debug: false
system:
can:
@@ -40,9 +40,9 @@ system:
motion_device: eric
motion_type: linkerforce
retargeting_type: projection
rightpub: true
rightpub: false
robotname_l: g20
robotname_r: l10
robotname_r: g20
sapientype: left
usecan: false
usegui: false
@@ -51,6 +51,6 @@ system:
useudp: true
useudpserver: true
udp:
ip: 0.0.0.0
ip: 192.168.11.88
port: 8888
serverport: 5551
src/linkerhand_retarget/linkerhand_retarget/config/hand_config.yml
+1 -1
View File
@@ -16,7 +16,7 @@ commandlower_left_l20: [255, 255, 255, 255, 255, 255, 0, 0, 0, 0, 255, None, Non
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]
commandlower_left_g20: [255, 255, 255, 255, 255, 0, 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]
src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/g20_config copy.py
+473
View File
@@ -0,0 +1,473 @@
# 手指配置常量
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, 1, 0],
'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.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': 10
}
},
# 拇指指尖弯曲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': 20
}
},
# 食指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},
{'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},
],
'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},
]
}
src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/g20_config.py
+2 -4
View File
@@ -432,9 +432,7 @@ MOTOR_CONSTRAINTS = {
{'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},
@@ -472,4 +470,4 @@ MOTOR_CONSTRAINTS = {
{'min': 0, 'max': 255, 'enabled': False},
{'min': 0, 'max': 255, 'enabled': False},
]
}
}
src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/l10_config.py
+1 -1
View File
@@ -126,7 +126,7 @@ FINGER_CONFIGS = {
'type': 'finger',
'reverse_motion': {
'v1': False,
'v2': True
'v2': False
},
'dynamic_weight': None,
'extended_mapping': {
src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_g20.py
+1 -14
View File
@@ -10,13 +10,6 @@ from ..config.g20_config import FINGER_CONFIGS, MAPPING_ORDER, ROBOT_OPOSE_RIGHT
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 转换为具体值
@@ -307,7 +300,6 @@ class LeftHand:
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'):
@@ -315,10 +307,6 @@ class LeftHand:
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):
@@ -347,8 +335,6 @@ class LeftHand:
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:
"""
初始化映射器
@@ -532,3 +518,4 @@ class LeftHand:
self.g_jointvelocity[i] = 255
self.last_jointvelocity[i] = self.g_jointvelocity[i]
self.last_jointpositions[i] = self.g_jointpositions[i]
src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/retarget.py
+1 -1
View File
@@ -1247,7 +1247,7 @@ class Retarget():
self.node.get_logger().error("标定失败，退出程序")
return
self.calibration = -1
self.node.create_timer(1.0/100, self.process_callback) # 100Hz
self.node.create_timer(1.0/30, self.process_callback) # 30Hz
def stop_serial_threads(self):
"""停止串口线程，在 destroy_node 时调用"""
src/linkerhand_retarget/tests/unit/test_linkerforce_g20.py
-51
View File
@@ -1,51 +0,0 @@
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