From 7953e50ca7d4a38a364ab7d06a2859f0e53b8166 Mon Sep 17 00:00:00 2001 From: lxp <2770281812@qq.com> Date: Tue, 21 Jul 2026 15:25:16 +0800 Subject: [PATCH] =?UTF-8?q?=E5=B7=A6=E6=89=8BG20=E5=A4=A7=E6=8B=87?= =?UTF-8?q?=E6=8C=87=E4=BE=A7=E6=91=86=E6=98=A0=E5=B0=84?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- .../camera_retarget_l20/g20_mapping.py | 12 +++-- .../test/test_g20_mapping.py | 7 +-- .../config/base_config.yml | 6 +-- .../config/hand_config.yml | 2 +- .../motion/linkerforce/config/g20_config.py | 6 ++- .../linkerforce/hand/linkerforce_g20.py | 15 +++++- .../motion/linkerforce/retarget.py | 2 +- .../tests/unit/test_linkerforce_g20.py | 51 +++++++++++++++++++ 8 files changed, 85 insertions(+), 16 deletions(-) create mode 100644 src/linkerhand_retarget/tests/unit/test_linkerforce_g20.py diff --git a/src/camera_retarget_l20/camera_retarget_l20/g20_mapping.py b/src/camera_retarget_l20/camera_retarget_l20/g20_mapping.py index 84970d3..a260749 100644 --- a/src/camera_retarget_l20/camera_retarget_l20/g20_mapping.py +++ b/src/camera_retarget_l20/camera_retarget_l20/g20_mapping.py @@ -65,12 +65,12 @@ G20_COMMAND_NAMES: tuple[str, ...] = ( FLEXION_MOTOR_INDICES = frozenset((*range(0, 5), *range(15, 20))) RESERVED_MOTOR_INDICES = frozenset(range(11, 15)) -# Left-G20 endpoints from linkerhand_retarget/config/hand_config.yml. Motor 5 -# is fixed at zero by the vendor mapping; motors 6..9 run in the opposite +# 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, - 0, 0, 0, 0, 0, + 255, 0, 0, 0, 0, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255, ) @@ -118,8 +118,9 @@ POWER_GRASP_THUMB_OPPOSITION_COMMANDS = np.asarray( (78.0, 112.0), dtype=float ) -# The vendor G20 mapping constrains thumb horizontal swing (motor 10) to this -# range to avoid commanding the mechanical end stop. +# 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. @@ -294,6 +295,7 @@ class G20CommandMapper: 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) diff --git a/src/camera_retarget_l20/test/test_g20_mapping.py b/src/camera_retarget_l20/test/test_g20_mapping.py index 514a7a4..b720713 100644 --- a/src/camera_retarget_l20/test/test_g20_mapping.py +++ b/src/camera_retarget_l20/test/test_g20_mapping.py @@ -6,6 +6,7 @@ from camera_retarget_l20.g20_mapping import ( FLEXION_MOTOR_INDICES, MOTOR_JOINT_NAMES, RESERVED_MOTOR_INDICES, + THUMB_ABDUCTION_MIN_COMMAND, THUMB_YAW_MIN_COMMAND, G20CommandMapper, G20MediaPipeMapper, @@ -52,7 +53,7 @@ def qpos_at(limit_index): def test_left_g20_lower_limit_endpoints(): command = make_mapper().raw_command(qpos_at(0)) assert command[:5].tolist() == [255.0] * 5 - assert command[5:10].tolist() == [0.0] * 5 + assert command[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 @@ -61,7 +62,7 @@ def test_left_g20_lower_limit_endpoints(): def test_left_g20_upper_limit_endpoints_and_thumb_constraint(): command = make_mapper().raw_command(qpos_at(1)) assert command[:5].tolist() == [0.0] * 5 - assert command[5] == 0.0 + assert command[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 @@ -71,7 +72,7 @@ def test_max_closure_only_limits_flexion_motors(): command = make_mapper(max_closure=0.60).raw_command(qpos_at(1)) for index in FLEXION_MOTOR_INDICES: assert command[index] == pytest.approx(102.0) - assert command[5] == 0.0 + assert command[5] == THUMB_ABDUCTION_MIN_COMMAND assert command[6:10].tolist() == [255.0] * 4 assert command[10] == THUMB_YAW_MIN_COMMAND diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/base_config.yml b/src/linkerhand_retarget/linkerhand_retarget/config/base_config.yml index ed90974..aeb6f6b 100644 --- a/src/linkerhand_retarget/linkerhand_retarget/config/base_config.yml +++ b/src/linkerhand_retarget/linkerhand_retarget/config/base_config.yml @@ -24,8 +24,8 @@ serial: - 921600 exclude_ports: [] left: - baudrate: 460800 - port: /dev/ttyUSB1 + baudrate: 2000000 + port: /dev/ttyUSB0 right: baudrate: 2000000 port: /dev/ttyUSB0 @@ -41,7 +41,7 @@ system: motion_type: linkerforce retargeting_type: projection rightpub: true - robotname_l: l10 + robotname_l: g20 robotname_r: l10 sapientype: left usecan: false diff --git a/src/linkerhand_retarget/linkerhand_retarget/config/hand_config.yml b/src/linkerhand_retarget/linkerhand_retarget/config/hand_config.yml index 8e32e01..316bd57 100644 --- a/src/linkerhand_retarget/linkerhand_retarget/config/hand_config.yml +++ b/src/linkerhand_retarget/linkerhand_retarget/config/hand_config.yml @@ -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, 0, 0, 0, 0, 0, 255, None, None, None, None, 255, 255, 255, 255, 255] +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] 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 index 02a52be..4941166 100644 --- a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/g20_config.py +++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/config/g20_config.py @@ -432,7 +432,9 @@ MOTOR_CONSTRAINTS = { {'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}, @@ -470,4 +472,4 @@ MOTOR_CONSTRAINTS = { {'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/hand/linkerforce_g20.py b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_g20.py index 1eec528..8fcd91c 100644 --- a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_g20.py +++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/hand/linkerforce_g20.py @@ -10,6 +10,13 @@ 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 转换为具体值 @@ -300,6 +307,7 @@ 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'): @@ -307,6 +315,10 @@ 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): @@ -335,6 +347,8 @@ 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: """ 初始化映射器 @@ -518,4 +532,3 @@ class LeftHand: 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 index 5747e3d..9ef7df7 100644 --- a/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/retarget.py +++ b/src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce/retarget.py @@ -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) # 60Hz + self.node.create_timer(1.0/100, self.process_callback) # 100Hz def stop_serial_threads(self): """停止串口线程，在 destroy_node 时调用""" 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