多手势标定

.gitignore

@@ -52,10 +52,31 @@ Thumbs.db
 /logs/
 *.tmp
 *.log
+*.bak
+*.orig
+*.rej
+
+# Operator/device-specific calibration artifacts
+# Reproducible seed profiles remain under
+# src/linkerhand_retarget/resource/linkerforce_v2/profiles/.
+/profiles/
+*.wear_check.json
+*.checkpoint.json
+*.verification.json
+*_mapping_quality.json
 
 # ROS bag / MCAP recordings and CAN captures
 rosbag2_*/
+/bags/
+/recordings/
+/captures/
+/sessions/
+/reports/
 *.db3
 *.mcap
 candump-*
-l10_*_state_*/
+*_state_*/
+
+# Local Codex/agent workspace metadata
+/.agents/
+/.codex/

src/gui_control/gui_control/gui_control.py

@@ -19,6 +19,33 @@ from .utils.mapping import *
 
 from .config.constants import _HAND_CONFIGS
 LOOP_TIME = 1000 # 循环动作间隔时间 毫秒
+
+_CANONICAL_COMMAND_NAMES = {
+    "G20": [
+        "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", "reserved_11",
+        "reserved_12", "reserved_13", "reserved_14", "thumb_mcp",
+        "index_pip", "middle_pip", "ring_pip", "pinky_pip",
+    ],
+    "O6": [
+        "thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch",
+        "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch",
+    ],
+}
+
+_CANONICAL_COMMAND_BOUNDS = {
+    "G20": [
+        *[(0, 255)] * 10,
+        (80, 255),
+        *[(255, 255)] * 4,
+        *[(0, 255)] * 5,
+    ],
+    "O6": [(0, 255)] * 6,
+}
+
+
 class ROS2NodeManager(QObject):
     """ROS2节点管理器，处理ROS通信"""
     status_updated = pyqtSignal(str, str)  # 状态类型, 消息内容
@@ -45,28 +72,43 @@ class ROS2NodeManager(QObject):
             self.node.declare_parameter('hand_joint', 'L10')
             self.node.declare_parameter('topic_hz', 30)
             self.node.declare_parameter('is_arc', False)
+            self.node.declare_parameter('topic_prefix', '')
             
             # 获取参数
             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
+            self.topic_prefix = self.normalize_topic_prefix(
+                self.node.get_parameter('topic_prefix').value
+            )
             
             if self.is_arc == True:
                 # 创建发布者
                 self.publisher_arc = self.node.create_publisher(
-                    JointState, f'/cb_{self.hand_type}_hand_control_cmd_arc', 10
+                    JointState,
+                    self.topic(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
+                JointState,
+                self.topic(f'/cb_{self.hand_type}_hand_control_cmd'),
+                10
+            )
+            self.snapshot_publisher = self.node.create_publisher(
+                JointState, self.topic('/calibration_pose_snapshot'), 10
             )
                     # 新增 speed / torque 发布者
             self.speed_pub = self.node.create_publisher(
-                String, f'/cb_hand_setting_cmd', 10)
+                String, self.topic('/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}")
+                String, self.topic('/cb_hand_setting_cmd'), 10)
+            self.status_updated.emit(
+                "info",
+                f"ROS2节点初始化成功: {self.topic_prefix or '/'} "
+                f"{self.hand_type} {self.hand_joint}"
+            )
             
             # 启动ROS2自旋线程
             self.spin_thread = threading.Thread(target=self.spin_node, daemon=True)
@@ -75,11 +117,34 @@ class ROS2NodeManager(QObject):
             self.status_updated.emit("error", f"ROS2初始化失败: {str(e)}")
             raise
 
+    @staticmethod
+    def normalize_topic_prefix(prefix: str) -> str:
+        prefix = str(prefix).strip()
+        if not prefix or prefix == '/':
+            return ''
+        if not prefix.startswith('/'):
+            prefix = '/' + prefix
+        return prefix.rstrip('/')
+
+    def topic(self, absolute_topic: str) -> str:
+        if not absolute_topic.startswith('/'):
+            raise ValueError('base topic must be absolute')
+        return self.topic_prefix + absolute_topic
+
     def spin_node(self):
         """运行ROS2节点自旋循环"""
         while rclpy.ok() and self.node:
             rclpy.spin_once(self.node, timeout_sec=0.1)
 
+    def bound_positions(self, positions: List[int]) -> List[int]:
+        bounds = _CANONICAL_COMMAND_BOUNDS.get(self.hand_joint)
+        if not bounds or len(bounds) != len(positions):
+            return list(positions)
+        return [
+            max(minimum, min(maximum, int(value)))
+            for value, (minimum, maximum) in zip(positions, bounds)
+        ]
+
     def publish_joint_state(self, positions: List[int]):
         """发布关节状态消息"""
         if not self.publisher or not self.node:
@@ -87,6 +152,7 @@ class ROS2NodeManager(QObject):
             return
             
         try:
+            positions = self.bound_positions(positions)
             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)
@@ -94,7 +160,10 @@ class ROS2NodeManager(QObject):
             # 如果有关节名称，添加到消息中
             #hand_config = HandConfig.from_hand_type(self.hand_joint)
             hand_config = _HAND_CONFIGS[self.hand_joint]
-            if len(hand_config.joint_names) == len(positions):
+            canonical_names = _CANONICAL_COMMAND_NAMES.get(self.hand_joint)
+            if canonical_names and len(canonical_names) == len(positions):
+                self.joint_state.name = canonical_names
+            elif len(hand_config.joint_names) == len(positions):
                 if hand_config.joint_names_en != None:
                     self.joint_state.name = hand_config.joint_names_en
                 else:
@@ -131,9 +200,20 @@ class ROS2NodeManager(QObject):
         except Exception as e:
             self.status_updated.emit("error", f"发布失败: {str(e)}")
 
+    def publish_pose_snapshot(self, positions: List[int]):
+        """发布带名称的当前标定姿势快照。"""
+        positions = self.bound_positions(positions)
+        self.publish_joint_state(positions)
+        self.joint_state.header.stamp = self.node.get_clock().now().to_msg()
+        self.joint_state.position = [float(pos) for pos in positions]
+        canonical_names = _CANONICAL_COMMAND_NAMES.get(self.hand_joint)
+        if canonical_names:
+            self.joint_state.name = canonical_names
+        self.snapshot_publisher.publish(self.joint_state)
+        self.status_updated.emit("info", "当前标定姿势快照已发布")
+
     def publish_speed(self, val: int):
-        joint_len = 0
-        if (self.hand_joint.upper() == "O6" or self.hand_joint.upper() == "L6"):
+        if self.hand_joint.upper() in ("O6", "L6"):
             joint_len = 6
         elif self.hand_joint == "L7":
             joint_len = 7
@@ -152,8 +232,7 @@ class ROS2NodeManager(QObject):
         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"):
+        if self.hand_joint.upper() in ("O6", "L6"):
             joint_len = 6
         elif self.hand_joint == "L7":
             joint_len = 7
@@ -212,7 +291,10 @@ class HandControlGUI(QWidget):
     def init_ui(self):
         """初始化用户界面"""
         # 设置窗口属性
-        self.setWindowTitle(f'灵巧手控制界面 - {self.hand_type} {self.hand_joint}')
+        self.setWindowTitle(
+            f'灵巧手控制界面 - {self.ros_manager.topic_prefix or "/"} '
+            f'{self.hand_type} {self.hand_joint}'
+        )
         self.setMinimumSize(1200, 900)
         
         # 设置样式
@@ -383,13 +465,18 @@ class HandControlGUI(QWidget):
         for i, (name, value) in enumerate(zip(
             self.hand_config.joint_names, self.hand_config.init_pos
         )):
+            bounds = _CANONICAL_COMMAND_BOUNDS.get(self.hand_joint)
+            minimum, maximum = (
+                bounds[i] if bounds and i < len(bounds) else (0, 255)
+            )
+            value = max(minimum, min(maximum, int(value)))
             # 创建标签
             label = QLabel(f"{name}: {value}")
             label.setMinimumWidth(120)
             
             # 创建滑动条
             slider = QSlider(Qt.Horizontal)
-            slider.setRange(0, 255)
+            slider.setRange(minimum, maximum)
             slider.setValue(value)
             slider.valueChanged.connect(
                 lambda val, idx=i: self.on_slider_value_changed(idx, val)
@@ -435,6 +522,11 @@ class HandControlGUI(QWidget):
         self.stop_button.setProperty("category", "danger")
         self.stop_button.clicked.connect(self.on_stop_clicked)
         actions_layout.addWidget(self.stop_button)
+
+        self.save_pose_button = QPushButton("保存当前标定姿势")
+        self.save_pose_button.setProperty("category", "action")
+        self.save_pose_button.clicked.connect(self.on_save_pose_clicked)
+        actions_layout.addWidget(self.save_pose_button)
         
         layout.addLayout(actions_layout)
         
@@ -652,6 +744,11 @@ class HandControlGUI(QWidget):
             
         self.status_updated.emit("warning", "已停止所有动作")
 
+    def on_save_pose_clicked(self):
+        """发布当前滑块姿势，供hand_pose_capture写入profile。"""
+        positions = [slider.value() for slider in self.sliders]
+        self.ros_manager.publish_pose_snapshot(positions)
+
     def on_cycle_clicked(self):
         """循环运行预设动作按钮点击事件处理"""
         if not self.hand_config.preset_actions:

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/core/can/linker_hand_g20_can.py

@@ -1030,6 +1030,16 @@ class LinkerHandG20Can:
         cmd_state = self.joint_state_to_cmd_state(state=s)
         return cmd_state
 
+    def get_cached_current_status(self):
+        """Return the latest received five-finger state without CAN queries."""
+        state = [self.x41, self.x42, self.x43, self.x44, self.x45]
+        if not all(
+            isinstance(finger, (list, tuple)) and len(finger) == 6
+            for finger in state
+        ):
+            return None
+        return self.joint_state_to_cmd_state(state=state)
+
     def get_current_pub_status(self):
         """API接口:获取手指当前状态"""
         self.get_current_status()
@@ -1266,4 +1276,25 @@ class LinkerHandG20Can:
         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"]
+        return [
+            "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",
+            "reserved_11",
+            "reserved_12",
+            "reserved_13",
+            "reserved_14",
+            "thumb_mcp",
+            "index_pip",
+            "middle_pip",
+            "ring_pip",
+            "pinky_pip",
+        ]

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/LinkerHand/linker_hand_api.py

@@ -202,6 +202,11 @@ class LinkerHandApi:
         '''Get current joint state'''
         return self.hand.get_current_status()
 
+    def get_state_cached(self):
+        """Get the latest received state without transmitting new queries."""
+        getter = getattr(self.hand, "get_cached_current_status", None)
+        return getter() if getter is not None else None
+
     
     def get_state_for_pub(self):
         return self.hand.get_current_pub_status()

src/linker_hand_ros2_sdk/linker_hand_ros2_sdk/linker_hand.py

@@ -37,11 +37,30 @@ def command_changed(previous, current):
     return any(float(old) != float(new) for old, new in zip(previous, values))
 
 
+def position_command_should_queue(previous, current, repeat=True):
+    """Whether the newest position target should be written on this heartbeat."""
+    values = list(current)
+    return bool(values) and (
+        bool(repeat) or command_changed(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
 
 
+def state_reads_deferred(
+    last_command_time, now, quiet_period, enabled=True
+):
+    """Return whether blocking state reads must yield to active commands."""
+    return (
+        enabled
+        and last_command_time is not None
+        and now < last_command_time + quiet_period
+    )
+
+
 class LinkerHand(Node):
     def __init__(self, name):
         super().__init__(name)
@@ -54,6 +73,12 @@ class LinkerHand(Node):
         # -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)
+        # -1 keeps the model's original startup torque.  The retarget v2
+        # launch uses a conservative value for calibration and preview.
+        self.declare_parameter('startup_torque', -1)
+        # Preserve the legacy behaviour by default.  Safety-critical launch
+        # files can configure limits without moving to a startup pose.
+        self.declare_parameter('move_on_startup', True)
         # 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', '')
@@ -63,6 +88,17 @@ class LinkerHand(Node):
         # incoming position commands.
         self.declare_parameter('state_poll_rate', 60.0)
         self.declare_parameter('velocity_poll_rate', 60.0)
+        # G20 state and velocity reads each transmit five synchronous CAN
+        # queries. Defer them while teleoperation commands are arriving.
+        self.declare_parameter('defer_state_reads_while_commanding', True)
+        self.declare_parameter('command_quiet_period', 0.2)
+        # Legacy teleoperation sent the latest target on every 30 Hz callback,
+        # including an unchanged target.  Some firmware revisions track that
+        # cadence more smoothly than sparse change-only updates.
+        self.declare_parameter('repeat_position_commands', True)
+        # Faults stay manually clearable through cb_hand_setting_cmd. Repeated
+        # automatic clears add periodic CAN traffic to the command stream.
+        self.declare_parameter('auto_clear_faults', False)
 
         # ros时间获取
         self.stamp_clock = Clock()
@@ -75,6 +111,12 @@ class LinkerHand(Node):
         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.startup_torque = int(self.get_parameter('startup_torque').value)
+        if self.startup_torque < -1 or self.startup_torque > 255:
+            raise ValueError('startup_torque must be -1 or in the range [0, 255]')
+        self.move_on_startup = bool(
+            self.get_parameter('move_on_startup').value
+        )
         self.topic_prefix = self.normalize_topic_prefix(
             self.get_parameter('topic_prefix').value
         )
@@ -92,6 +134,22 @@ class LinkerHand(Node):
             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
+        self.defer_state_reads_while_commanding = bool(
+            self.get_parameter(
+                'defer_state_reads_while_commanding'
+            ).value
+        )
+        self.command_quiet_period = float(
+            self.get_parameter('command_quiet_period').value
+        )
+        if self.command_quiet_period < 0.0:
+            raise ValueError('command_quiet_period must not be negative')
+        self.auto_clear_faults = bool(
+            self.get_parameter('auto_clear_faults').value
+        )
+        self.repeat_position_commands = bool(
+            self.get_parameter('repeat_position_commands').value
+        )
         configured_startup_pose = self.get_parameter_or(
             'startup_pose',
             Parameter('startup_pose', Parameter.Type.INTEGER_ARRAY, []),
@@ -107,6 +165,7 @@ class LinkerHand(Node):
         self.last_hand_eff_cmd = None # 最新手指力矩命令
         self.applied_hand_post_cmd = None
         self.applied_hand_vel_cmd = None
+        self.last_position_command_time = None
 
         self.last_hand_state = [-1] * 10
         self.last_hand_vel = [-1] * 10
@@ -244,26 +303,38 @@ class LinkerHand(Node):
             pose = list(self.startup_pose)
         if self.startup_speed >= 0:
             speed = [self.startup_speed] * len(speed)
+        if self.startup_torque >= 0:
+            torque = [self.startup_torque] * len(torque)
         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)
+                if self.move_on_startup:
+                    self.api.finger_move(pose=pose)
+                    time.sleep(0.1)
 
     def hand_control_cb(self, msg):
-        # 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.
+        # The hardware can be slower than the publisher, so a pending target is
+        # always replaced by the newest sample.  By default the newest target
+        # is also resent at the publisher cadence, matching the legacy driver.
         position = list(msg.position)
         if position:
-            self.last_hand_post_cmd = (
-                position
-                if command_changed(self.applied_hand_post_cmd, position)
-                else None
-            )
+            # Treat every valid sample as an active teleoperation heartbeat,
+            # even if integer quantisation made it identical to the previous
+            # target. This keeps all synchronous CAN diagnostics off the bus
+            # for the entire control session, matching the legacy execution
+            # path that had no state subscriber.
+            self.last_position_command_time = time.monotonic()
+            if position_command_should_queue(
+                self.applied_hand_post_cmd,
+                position,
+                self.repeat_position_commands,
+            ):
+                self.last_hand_post_cmd = position
+            else:
+                self.last_hand_post_cmd = None
 
         velocity = list(msg.velocity)
         if velocity:
@@ -286,6 +357,9 @@ class LinkerHand(Node):
             self.api.finger_move(pose=pose)
             self.applied_hand_post_cmd = pose
             self.last_hand_post_cmd = None
+            cached_state = self.api.get_state_cached()
+            if cached_state is not None:
+                self.last_hand_state = cached_state
 
         if self.last_hand_vel_cmd is not None:
             vel = list(self.last_hand_vel_cmd)
@@ -320,6 +394,15 @@ class LinkerHand(Node):
         if self.hand_state_pub.get_subscription_count() < 1:
             return
         now = time.monotonic()
+        if state_reads_deferred(
+            self.last_position_command_time,
+            now,
+            self.command_quiet_period,
+            self.defer_state_reads_while_commanding,
+        ):
+            # pub_state continues to publish the last completed state as a
+            # heartbeat. A fresh blocking read is made after motion settles.
+            return
         if not state_poll_due(
             self.last_state_poll_time, now, self.state_poll_period
         ):
@@ -342,6 +425,12 @@ class LinkerHand(Node):
         # Position commands have priority over synchronous state reads.
         self._apply_pending_commands()
         self._poll_state_if_due()
+        diagnostics_deferred = state_reads_deferred(
+            self.last_position_command_time,
+            time.monotonic(),
+            self.command_quiet_period,
+            self.defer_state_reads_while_commanding,
+        )
         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:
@@ -360,7 +449,11 @@ class LinkerHand(Node):
                 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:
+            if (
+                self.run_count == 8
+                and self.hand_info_pub.get_subscription_count() > 0
+                and not diagnostics_deferred
+            ):
                 """手部信息"""
                 self.last_hand_info = {
                     "version": self.embedded_version, # Dexterous hand version number
@@ -375,8 +468,9 @@ class LinkerHand(Node):
                     "finger_order": self.api.get_finger_order() # Finger motor order
                 }
                 
-            if self.run_count == 9:
+            if self.run_count == 9 and self.auto_clear_faults:
                 self.api.clear_faults() # 自动清除错误编码
+            if self.run_count == 9:
                 self.run_count = 0
             self.run_count += 1
             time.sleep(0.003)

src/linker_hand_ros2_sdk/test/test_topic_prefix.py

@@ -4,7 +4,9 @@ from linker_hand_ros2_sdk.linker_hand import (
     COMMAND_QOS,
     LinkerHand,
     command_changed,
+    position_command_should_queue,
     state_poll_due,
+    state_reads_deferred,
 )
 
 
@@ -30,7 +32,25 @@ def test_identical_commands_are_not_reapplied():
     assert not command_changed([60, 60], [])
 
 
+def test_legacy_heartbeat_resends_unchanged_position_target():
+    assert position_command_should_queue([60, 60], [60, 60], repeat=True)
+    assert not position_command_should_queue(
+        [60, 60], [60, 60], repeat=False
+    )
+    assert position_command_should_queue(
+        [60, 60], [60, 61], repeat=False
+    )
+    assert not position_command_should_queue([60, 60], [], repeat=True)
+
+
 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)
+
+
+def test_state_reads_yield_to_recent_motion_then_resume():
+    assert state_reads_deferred(10.0, 10.1, 0.2)
+    assert not state_reads_deferred(10.0, 10.2, 0.2)
+    assert not state_reads_deferred(None, 10.1, 0.2)
+    assert not state_reads_deferred(10.0, 10.1, 0.2, enabled=False)

src/linkerhand_retarget/launch/ffg_dual_g20_o6.launch.py

@@ -0,0 +1,230 @@
+"""One left FFG, G20 on can0 and O6 on can1, fail-closed by default."""
+
+from launch import LaunchDescription
+from launch.actions import DeclareLaunchArgument
+from launch.substitutions import LaunchConfiguration, PathJoinSubstitution
+from launch_ros.actions import Node
+from launch_ros.parameter_descriptions import ParameterValue
+from launch_ros.substitutions import FindPackageShare
+
+from linkerhand_retarget.motion.linkerforce_v2.constants import (
+    DEFAULT_COMMAND_FILTER_CONFIG,
+    DEFAULT_INPUT_FILTER_ENABLED,
+)
+
+
+G20_OPEN_POSE = [
+    255, 255, 255, 255, 255,
+    255, 193, 148, 105, 42,
+    255, 255, 255, 255, 255,
+    255, 255, 255, 255, 255,
+]
+O6_OPEN_POSE = [255, 179, 255, 255, 255, 255]
+
+
+def generate_launch_description():
+    package_share = FindPackageShare("linkerhand_retarget")
+    g20_seed = PathJoinSubstitution(
+        [package_share, "linkerforce_v2", "profiles", "g20_seed_profile.json"]
+    )
+    o6_seed = PathJoinSubstitution(
+        [package_share, "linkerforce_v2", "profiles", "o6_seed_profile.json"]
+    )
+    arguments = [
+        DeclareLaunchArgument("input_mode", default_value="serial"),
+        DeclareLaunchArgument(
+            "raw_input_topic", default_value="/ffg/left/raw_joint_state"
+        ),
+        DeclareLaunchArgument("serial_port", default_value=""),
+        DeclareLaunchArgument("baudrate", default_value="0"),
+        DeclareLaunchArgument("auto_scan", default_value="true"),
+        DeclareLaunchArgument("publish_rate", default_value="30.0"),
+        DeclareLaunchArgument(
+            "input_filter_enabled",
+            default_value=str(DEFAULT_INPUT_FILTER_ENABLED).lower(),
+        ),
+        DeclareLaunchArgument(
+            "input_filter_process_variance", default_value="0.00001"
+        ),
+        DeclareLaunchArgument(
+            "input_filter_measurement_variance", default_value="0.0005"
+        ),
+        DeclareLaunchArgument(
+            "input_filter_reset_gap", default_value="0.35"
+        ),
+        DeclareLaunchArgument(
+            "command_filter_mode",
+            default_value=str(DEFAULT_COMMAND_FILTER_CONFIG["mode"]),
+        ),
+        DeclareLaunchArgument(
+            "command_filter_ema_alpha",
+            default_value=str(
+                DEFAULT_COMMAND_FILTER_CONFIG["ema_alpha"]
+            ),
+        ),
+        DeclareLaunchArgument(
+            "command_filter_max_step_u8",
+            default_value=str(
+                DEFAULT_COMMAND_FILTER_CONFIG["max_step_u8"]
+            ),
+        ),
+        DeclareLaunchArgument(
+            "command_filter_deadband_u8",
+            default_value=str(
+                DEFAULT_COMMAND_FILTER_CONFIG["deadband_u8"]
+            ),
+        ),
+        DeclareLaunchArgument("glove_profile", default_value=""),
+        DeclareLaunchArgument("wear_check", default_value=""),
+        DeclareLaunchArgument("g20_profile", default_value=g20_seed),
+        DeclareLaunchArgument("o6_profile", default_value=o6_seed),
+        DeclareLaunchArgument("g20_serial_number", default_value=""),
+        DeclareLaunchArgument("o6_serial_number", default_value=""),
+        DeclareLaunchArgument("g20_can", default_value="can0"),
+        DeclareLaunchArgument("o6_can", default_value="can1"),
+        DeclareLaunchArgument("startup_speed", default_value="30"),
+        DeclareLaunchArgument("startup_torque", default_value="80"),
+        DeclareLaunchArgument("move_on_startup", default_value="false"),
+        DeclareLaunchArgument("g20_state_poll_rate", default_value="10.0"),
+        DeclareLaunchArgument("o6_state_poll_rate", default_value="30.0"),
+        DeclareLaunchArgument(
+            "defer_state_reads_while_commanding", default_value="true"
+        ),
+        DeclareLaunchArgument("command_quiet_period", default_value="0.2"),
+        DeclareLaunchArgument("auto_clear_faults", default_value="false"),
+    ]
+
+    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("startup_speed"), value_type=int
+                ),
+                "startup_torque": ParameterValue(
+                    LaunchConfiguration("startup_torque"), value_type=int
+                ),
+                "move_on_startup": ParameterValue(
+                    LaunchConfiguration("move_on_startup"), value_type=bool
+                ),
+                "state_poll_rate": ParameterValue(
+                    LaunchConfiguration("g20_state_poll_rate"), value_type=float
+                ),
+                "velocity_poll_rate": ParameterValue(
+                    LaunchConfiguration("g20_state_poll_rate"), value_type=float
+                ),
+                "defer_state_reads_while_commanding": ParameterValue(
+                    LaunchConfiguration("defer_state_reads_while_commanding"),
+                    value_type=bool,
+                ),
+                "command_quiet_period": ParameterValue(
+                    LaunchConfiguration("command_quiet_period"),
+                    value_type=float,
+                ),
+                "auto_clear_faults": ParameterValue(
+                    LaunchConfiguration("auto_clear_faults"), value_type=bool
+                ),
+            }
+        ],
+    )
+    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("startup_speed"), value_type=int
+                ),
+                "startup_torque": ParameterValue(
+                    LaunchConfiguration("startup_torque"), value_type=int
+                ),
+                "move_on_startup": ParameterValue(
+                    LaunchConfiguration("move_on_startup"), value_type=bool
+                ),
+                "state_poll_rate": ParameterValue(
+                    LaunchConfiguration("o6_state_poll_rate"), value_type=float
+                ),
+            }
+        ],
+    )
+    retarget = Node(
+        package="linkerhand_retarget",
+        executable="ffg_dual_retarget",
+        name="ffg_dual_retarget",
+        output="screen",
+        parameters=[
+            {
+                "input_mode": LaunchConfiguration("input_mode"),
+                "raw_input_topic": LaunchConfiguration("raw_input_topic"),
+                "serial_port": LaunchConfiguration("serial_port"),
+                "baudrate": ParameterValue(
+                    LaunchConfiguration("baudrate"), value_type=int
+                ),
+                "auto_scan": ParameterValue(
+                    LaunchConfiguration("auto_scan"), value_type=bool
+                ),
+                "publish_rate": ParameterValue(
+                    LaunchConfiguration("publish_rate"), value_type=float
+                ),
+                "input_filter_enabled": ParameterValue(
+                    LaunchConfiguration("input_filter_enabled"),
+                    value_type=bool,
+                ),
+                "input_filter_process_variance": ParameterValue(
+                    LaunchConfiguration("input_filter_process_variance"),
+                    value_type=float,
+                ),
+                "input_filter_measurement_variance": ParameterValue(
+                    LaunchConfiguration("input_filter_measurement_variance"),
+                    value_type=float,
+                ),
+                "input_filter_reset_gap": ParameterValue(
+                    LaunchConfiguration("input_filter_reset_gap"),
+                    value_type=float,
+                ),
+                "command_filter_mode": LaunchConfiguration(
+                    "command_filter_mode"
+                ),
+                "command_filter_ema_alpha": ParameterValue(
+                    LaunchConfiguration("command_filter_ema_alpha"),
+                    value_type=float,
+                ),
+                "command_filter_max_step_u8": ParameterValue(
+                    LaunchConfiguration("command_filter_max_step_u8"),
+                    value_type=float,
+                ),
+                "command_filter_deadband_u8": ParameterValue(
+                    LaunchConfiguration("command_filter_deadband_u8"),
+                    value_type=float,
+                ),
+                "glove_profile": LaunchConfiguration("glove_profile"),
+                "wear_check": LaunchConfiguration("wear_check"),
+                "g20_profile": LaunchConfiguration("g20_profile"),
+                "o6_profile": LaunchConfiguration("o6_profile"),
+                "g20_serial_number": LaunchConfiguration("g20_serial_number"),
+                "o6_serial_number": LaunchConfiguration("o6_serial_number"),
+                "g20_can_interface": LaunchConfiguration("g20_can"),
+                "o6_can_interface": LaunchConfiguration("o6_can"),
+            }
+        ],
+    )
+    return LaunchDescription(arguments + [g20_driver, o6_driver, retarget])

src/linkerhand_retarget/linkerhand_retarget/linkerhand/linkerforce.py

@@ -210,6 +210,8 @@ class FrameHandler:
         self.logger = logger
         self._data_lock = threading.Lock()
         self._poslist: List[float] = [0.0] * 21
+        self._position_sequence = 0
+        self._last_position_monotonic = 0.0
         self._forcelist: List[float] = [0.0] * 5
         self._realforcelist: List[int] = [0] * 5
 
@@ -221,7 +223,20 @@ class FrameHandler:
     @poslist.setter
     def poslist(self, value: List[float]):
         with self._data_lock:
-            self._poslist = value
+            self._poslist = value.copy()
+            self._position_sequence += 1
+            self._last_position_monotonic = time.monotonic()
+
+    @property
+    def position_snapshot(self) -> tuple[List[float], int, float]:
+        """Return values, sequence and monotonic receive time atomically."""
+
+        with self._data_lock:
+            return (
+                self._poslist.copy(),
+                self._position_sequence,
+                self._last_position_monotonic,
+            )
 
     @property
     def forcelist(self) -> List[float]:
@@ -514,6 +529,10 @@ class ForceSerialReader:
     def poslist(self, value: List[float]):
         self._handler.poslist = value
 
+    @property
+    def position_snapshot(self) -> tuple[List[float], int, float]:
+        return self._handler.position_snapshot
+
     @property
     def forcelist(self) -> List[float]:
         return self._handler.forcelist
@@ -729,4 +748,4 @@ class ForceSerialReader:
             'checked_ports': list(self.checked_ports),
             'exclude_ports': list(self.exclude_ports),
             'baudrates': self.baudrates
-        }
+        }

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/__init__.py

@@ -0,0 +1,18 @@
+"""Profile-driven FFG retargeting for heterogeneous Linker Hands."""
+
+from .mapping import HandIntentExtractor, RobotMapper
+from .profiles import (
+    ProfileError,
+    load_glove_profile,
+    load_robot_profile,
+    profile_sha256,
+)
+
+__all__ = [
+    "HandIntentExtractor",
+    "ProfileError",
+    "RobotMapper",
+    "load_glove_profile",
+    "load_robot_profile",
+    "profile_sha256",
+]

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/calibrate_glove.py

@@ -0,0 +1,244 @@
+"""Interactive multi-gesture FFG calibration CLI."""
+
+from __future__ import annotations
+
+import argparse
+from datetime import datetime, timezone
+from pathlib import Path
+import time
+from typing import Sequence
+
+import numpy as np
+import rclpy
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+
+from .calibration import build_glove_profile, robust_summary
+from .constants import (
+    FFG_JOINT_NAMES,
+    GLOVE_DYNAMIC_GESTURES,
+    GLOVE_STATIC_GESTURES,
+)
+from .profiles import load_glove_profile, save_profile
+
+
+GESTURE_LABELS = {
+    "open_spread": "五指自然张开、自然分开",
+    "fingers_together": "五指伸直并拢",
+    "tabletop": "桌面手势（根部弯曲、末端伸直）",
+    "hook": "钩拳（根部伸直、末端弯曲）",
+    "fist": "自然握拳",
+    "thumb_max_out": "拇指最大外展",
+    "thumb_across": "拇指横跨掌心",
+    "pinch_index": "拇指—食指捏合",
+    "pinch_middle": "拇指—中指捏合",
+    "pinch_ring": "拇指—无名指捏合",
+    "pinch_pinky": "拇指—小指捏合",
+    "index_flex_sweep": "食指独立弯曲往返",
+    "middle_flex_sweep": "中指独立弯曲往返",
+    "ring_flex_sweep": "无名指独立弯曲往返",
+    "pinky_flex_sweep": "小指独立弯曲往返",
+    "thumb_flex_sweep": "拇指弯曲往返",
+    "thumb_opposition_sweep": "拇指对掌往返",
+    "finger_splay_sweep": "四指开合往返",
+}
+
+
+class CaptureNode(Node):
+    def __init__(self, topic: str) -> None:
+        super().__init__("ffg_calibration_capture")
+        self.capture_active = False
+        self.frames: list[list[float]] = []
+        self.invalid_frames = 0
+        self.subscription = self.create_subscription(
+            JointState, topic, self._callback, 50
+        )
+
+    def _callback(self, message: JointState) -> None:
+        if not self.capture_active:
+            return
+        if list(message.name) != list(FFG_JOINT_NAMES):
+            self.invalid_frames += 1
+            return
+        values = [float(value) for value in message.position]
+        if len(values) != 21 or not all(np.isfinite(values)):
+            self.invalid_frames += 1
+            return
+        self.frames.append(values)
+
+    def collect(self, duration: float) -> list[list[float]]:
+        self.frames = []
+        self.invalid_frames = 0
+        self.capture_active = True
+        deadline = time.monotonic() + duration
+        while rclpy.ok() and time.monotonic() < deadline:
+            rclpy.spin_once(self, timeout_sec=0.02)
+        self.capture_active = False
+        return [frame.copy() for frame in self.frames]
+
+
+def _capture_repeats(
+    node: CaptureNode,
+    gesture: str,
+    *,
+    duration: float,
+    repeats: int,
+    minimum: int,
+) -> tuple[list[list[float]], list[dict[str, object]]]:
+    all_frames: list[list[float]] = []
+    summaries: list[dict[str, object]] = []
+    for repeat in range(1, repeats + 1):
+        input(
+            f"\n[{gesture}] {GESTURE_LABELS[gesture]}，"
+            f"第{repeat}/{repeats}次。摆好后按Enter开始采集："
+        )
+        frames = node.collect(duration)
+        if len(frames) < minimum:
+            raise RuntimeError(
+                f"{gesture} 第{repeat}次只有{len(frames)}个有效帧，"
+                f"要求至少{minimum}；无效命名/数据帧={node.invalid_frames}"
+            )
+        summary = robust_summary(frames, 21)
+        summary["repeat"] = repeat
+        summaries.append(summary)
+        all_frames.extend(frames)
+        print(f"  已保存 {len(frames)} 帧")
+    return all_frames, summaries
+
+
+def _quick_check(node: CaptureNode, profile_path: str, args: argparse.Namespace) -> int:
+    profile = load_glove_profile(profile_path)
+    failures: list[str] = []
+    errors: dict[str, float] = {}
+    for gesture in ("open_spread", "fist", "pinch_index"):
+        frames, _ = _capture_repeats(
+            node,
+            gesture,
+            duration=args.duration,
+            repeats=1,
+            minimum=args.min_samples,
+        )
+        observed = np.median(np.asarray(frames, dtype=float), axis=0)
+        reference = np.asarray(profile["poses"][gesture]["median"], dtype=float)
+        mad = np.asarray(profile["poses"][gesture]["mad"], dtype=float)
+        normalized_error = float(
+            np.sqrt(np.mean(np.square((observed - reference) / np.maximum(6 * mad, 0.05))))
+        )
+        print(f"  {gesture}: normalized_error={normalized_error:.3f}")
+        errors[gesture] = normalized_error
+        if normalized_error > args.quick_threshold:
+            failures.append(gesture)
+    attestation = {
+        "schema_version": 1,
+        "kind": "ffg_wear_check",
+        "checked_at": datetime.now(timezone.utc).isoformat(),
+        "glove_profile_sha256": profile["_profile_sha256"],
+        "threshold": args.quick_threshold,
+        "normalized_errors": errors,
+        "passed": not failures,
+    }
+    output = args.quick_output or str(
+        Path(profile_path).expanduser().resolve().with_suffix(".wear_check.json")
+    )
+    save_profile(attestation, output)
+    if failures:
+        print(
+            f"快速佩戴检查失败: {', '.join(failures)}；"
+            f"请重新佩戴或完整标定。结果已保存: {output}"
+        )
+        return 2
+    print(f"快速佩戴检查通过，凭据已保存: {output}")
+    return 0
+
+
+def _parser() -> argparse.ArgumentParser:
+    parser = argparse.ArgumentParser(description="FFG左手套多手势标定")
+    parser.add_argument("--topic", default="/ffg/left/raw_joint_state")
+    parser.add_argument("--output")
+    parser.add_argument("--glove-id")
+    parser.add_argument("--operator")
+    parser.add_argument("--firmware", default="unknown")
+    parser.add_argument("--duration", type=float, default=2.0)
+    parser.add_argument("--repeats", type=int, default=3)
+    parser.add_argument("--min-samples", type=int, default=50)
+    parser.add_argument("--dynamic-duration", type=float, default=3.0)
+    parser.add_argument("--skip-dynamic", action="store_true")
+    parser.add_argument("--quick-check", metavar="PROFILE")
+    parser.add_argument("--quick-output")
+    parser.add_argument("--quick-threshold", type=float, default=4.0)
+    return parser
+
+
+def main(argv: Sequence[str] | None = None) -> int:
+    args, ros_args = _parser().parse_known_args(argv)
+    if not args.quick_check and not (args.output and args.glove_id and args.operator):
+        raise SystemExit(
+            "完整标定必须提供 --output、--glove-id 和 --operator"
+        )
+    if args.duration <= 0 or args.repeats < 1 or args.min_samples < 1:
+        raise SystemExit("duration/repeats/min-samples参数无效")
+    rclpy.init(args=ros_args)
+    node = CaptureNode(args.topic)
+    try:
+        if args.quick_check:
+            return _quick_check(node, args.quick_check, args)
+
+        static_captures: dict[str, list[list[float]]] = {}
+        repeat_summaries: dict[str, list[dict[str, object]]] = {}
+        print(
+            "开始FFG完整标定。每个静态姿势保持"
+            f"{args.duration:.1f}s、重复{args.repeats}次；"
+            "采集期间保持姿势稳定。"
+        )
+        for gesture in GLOVE_STATIC_GESTURES:
+            frames, summaries = _capture_repeats(
+                node,
+                gesture,
+                duration=args.duration,
+                repeats=args.repeats,
+                minimum=args.min_samples,
+            )
+            static_captures[gesture] = frames
+            repeat_summaries[gesture] = summaries
+
+        dynamic_captures: dict[str, list[list[float]]] = {}
+        dynamic_repeat_summaries: dict[str, list[dict[str, object]]] = {}
+        if not args.skip_dynamic:
+            print("\n开始短时往返轨迹，每种动作重复3次。")
+            for gesture in GLOVE_DYNAMIC_GESTURES:
+                frames, summaries = _capture_repeats(
+                    node,
+                    gesture,
+                    duration=args.dynamic_duration,
+                    repeats=3,
+                    minimum=args.min_samples,
+                )
+                dynamic_captures[gesture] = frames
+                dynamic_repeat_summaries[gesture] = summaries
+
+        profile = build_glove_profile(
+            static_captures,
+            glove_id=args.glove_id,
+            operator=args.operator,
+            firmware=args.firmware,
+            dynamic_captures=dynamic_captures,
+            repeat_summaries=repeat_summaries,
+            dynamic_repeat_summaries=dynamic_repeat_summaries,
+        )
+        output = save_profile(profile, args.output)
+        validated = load_glove_profile(output)
+        print(
+            f"\n标定完成: {output}\n"
+            f"approved_for_runtime={validated['approved_for_runtime']}\n"
+            f"sha256={validated['_profile_sha256']}\n"
+            "启动实机前请再执行 --quick-check。"
+        )
+        return 0
+    finally:
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/calibrate_robot.py

@@ -0,0 +1,519 @@
+"""Interactive G20/O6 pose-profile capture from namespaced ROS topics."""
+
+from __future__ import annotations
+
+import argparse
+from datetime import datetime, timezone
+import json
+from pathlib import Path
+import threading
+import time
+from typing import Any, Mapping, Sequence
+
+import numpy as np
+import rclpy
+from rclpy.executors import SingleThreadedExecutor
+from rclpy.node import Node
+from sensor_msgs.msg import JointState
+
+from .calibration import build_robot_profile
+from .constants import MODEL_COMMAND_LENGTHS
+from .profiles import load_robot_profile, save_profile
+
+
+POSE_LABELS = {
+    "open_spread": "张手",
+    "fingers_together": "五指并拢",
+    "tabletop": "桌面手势",
+    "hook": "钩拳",
+    "fist": "握拳",
+    "thumb_max_out": "拇指最大外展",
+    "thumb_across": "拇指横跨掌心",
+    "pinch_index": "拇指—食指捏合",
+    "pinch_middle": "拇指—中指捏合",
+    "pinch_ring": "拇指—无名指捏合",
+    "pinch_pinky": "拇指—小指捏合",
+}
+
+
+class HandPoseCaptureNode(Node):
+    def __init__(
+        self,
+        model: str,
+        command_topic: str,
+        state_topic: str,
+        snapshot_topic: str,
+        command_names: Sequence[str],
+    ) -> None:
+        super().__init__(f"{model.lower()}_pose_capture")
+        self.model = model
+        self.length = MODEL_COMMAND_LENGTHS[model]
+        self.command_names = [str(name) for name in command_names]
+        self.latest_command: list[float] | None = None
+        self.latest_state: list[float] | None = None
+        self.latest_state_names: list[str] | None = None
+        self.latest_snapshot: list[float] | None = None
+        self.snapshot_sequence = 0
+        self.command_subscription = self.create_subscription(
+            JointState, command_topic, self._command_callback, 20
+        )
+        self.state_subscription = self.create_subscription(
+            JointState, state_topic, self._state_callback, 20
+        )
+        self.snapshot_subscription = self.create_subscription(
+            JointState, snapshot_topic, self._snapshot_callback, 20
+        )
+
+    def _valid(self, values: Sequence[float]) -> bool:
+        return (
+            len(values) == self.length
+            and all(np.isfinite(values))
+            and all(0.0 <= float(value) <= 255.0 for value in values)
+        )
+
+    def _command_callback(self, message: JointState) -> None:
+        if not self._valid(message.position):
+            return
+        incoming_names = [str(name) for name in message.name]
+        if (
+            len(incoming_names) != self.length
+            or set(incoming_names) != set(self.command_names)
+        ):
+            return
+        lookup = {
+            name: float(value)
+            for name, value in zip(incoming_names, message.position)
+        }
+        self.latest_command = [lookup[name] for name in self.command_names]
+
+    def _snapshot_callback(self, message: JointState) -> None:
+        if not self._valid(message.position):
+            return
+        names = [str(name) for name in message.name]
+        if len(names) != self.length or set(names) != set(self.command_names):
+            return
+        lookup = {
+            name: float(value)
+            for name, value in zip(names, message.position)
+        }
+        self.latest_snapshot = [lookup[name] for name in self.command_names]
+        self.snapshot_sequence += 1
+
+    def _state_callback(self, message: JointState) -> None:
+        names = [str(name) for name in message.name]
+        if (
+            self._valid(message.position)
+            and len(names) == self.length
+            and len(set(names)) == self.length
+            and set(names) == set(self.command_names)
+        ):
+            lookup = {
+                name: float(value)
+                for name, value in zip(names, message.position)
+            }
+            self.latest_state = [lookup[name] for name in self.command_names]
+            self.latest_state_names = list(self.command_names)
+
+
+def _default_status(model: str, pose: str) -> str:
+    if model == "O6" and pose in {
+        "tabletop",
+        "hook",
+        "pinch_middle",
+        "pinch_ring",
+        "pinch_pinky",
+    }:
+        return "approximate"
+    return "exact"
+
+
+def _checkpoint_path(output: str, configured: str | None) -> Path:
+    if configured:
+        return Path(configured).expanduser().resolve()
+    output_path = Path(output).expanduser().resolve()
+    return output_path.with_suffix(".checkpoint.json")
+
+
+def _checkpoint_document(
+    *,
+    model: str,
+    seed: Mapping[str, Any],
+    output: str,
+    serial_number: str,
+    firmware: str,
+    can_interface: str,
+    operator: str,
+    captures: Mapping[str, Mapping[str, Any]],
+    completed: bool = False,
+    final_profile_sha256: str | None = None,
+) -> dict[str, Any]:
+    return {
+        "schema_version": 1,
+        "kind": "robot_pose_capture_checkpoint",
+        "updated_at": datetime.now(timezone.utc).isoformat(),
+        "model": model,
+        "side": "left",
+        "seed_profile_sha256": seed["_profile_sha256"],
+        "output": str(Path(output).expanduser().resolve()),
+        "device": {
+            "serial_number": serial_number,
+            "firmware": firmware,
+            "can_interface": can_interface,
+        },
+        "operator": operator,
+        "command_names": list(seed["command_names"]),
+        "required_poses": list(seed["required_poses"]),
+        "captures": dict(captures),
+        "completed": bool(completed),
+        "final_profile_sha256": final_profile_sha256,
+    }
+
+
+def _command_bound_violations(
+    profile: Mapping[str, Any],
+    command: Sequence[float],
+) -> tuple[str, ...]:
+    try:
+        values = np.asarray(command, dtype=float)
+        bounds = np.asarray(profile["command_bounds"], dtype=float)
+    except (KeyError, TypeError, ValueError):
+        return ("命令或安全范围格式无效",)
+    names = list(profile.get("command_names", ()))
+    if values.shape != (len(names),) or bounds.shape != (len(names), 2):
+        return ("命令长度或安全范围长度无效",)
+    violations = []
+    for index, (value, (minimum, maximum)) in enumerate(zip(values, bounds)):
+        if (
+            not np.isfinite(value)
+            or value < minimum
+            or value > maximum
+        ):
+            violations.append(
+                f"{names[index]}={value:g}，允许[{minimum:g}, {maximum:g}]"
+            )
+    return tuple(violations)
+
+
+def _load_checkpoint(
+    path: Path,
+    *,
+    model: str,
+    seed: Mapping[str, Any],
+    output: str,
+    serial_number: str,
+    firmware: str,
+    can_interface: str,
+    operator: str,
+) -> dict[str, dict[str, Any]]:
+    try:
+        checkpoint = json.loads(path.read_text(encoding="utf-8"))
+    except (OSError, json.JSONDecodeError) as exc:
+        raise RuntimeError(f"无法读取姿势检查点 {path}: {exc}") from exc
+    expected = {
+        "kind": "robot_pose_capture_checkpoint",
+        "model": model,
+        "side": "left",
+        "seed_profile_sha256": seed["_profile_sha256"],
+        "output": str(Path(output).expanduser().resolve()),
+        "operator": operator,
+        "command_names": list(seed["command_names"]),
+        "required_poses": list(seed["required_poses"]),
+    }
+    for field, value in expected.items():
+        if checkpoint.get(field) != value:
+            raise RuntimeError(
+                f"检查点字段{field}与本次标定不匹配；"
+                "确认命令参数，或使用--fresh明确重新开始"
+            )
+    expected_device = {
+        "serial_number": serial_number,
+        "firmware": firmware,
+        "can_interface": can_interface,
+    }
+    if checkpoint.get("device") != expected_device:
+        raise RuntimeError(
+            "检查点的SN/固件/CAN与本次标定不匹配；"
+            "确认命令参数，或使用--fresh明确重新开始"
+        )
+    captures = checkpoint.get("captures")
+    if not isinstance(captures, dict):
+        raise RuntimeError("姿势检查点中的captures无效")
+    required = set(seed["required_poses"])
+    names = list(seed["command_names"])
+    length = MODEL_COMMAND_LENGTHS[model]
+    validated: dict[str, dict[str, Any]] = {}
+    for pose, capture in captures.items():
+        if pose not in required or not isinstance(capture, dict):
+            raise RuntimeError(f"姿势检查点包含无效动作: {pose}")
+        command = capture.get("cmd_u8")
+        state = capture.get("state_u8")
+        try:
+            command_values = np.asarray(command, dtype=float)
+            state_values = np.asarray(state, dtype=float)
+        except (TypeError, ValueError):
+            command_values = np.asarray([], dtype=float)
+            state_values = np.asarray([], dtype=float)
+        if (
+            not isinstance(command, list)
+            or not isinstance(state, list)
+            or command_values.shape != (length,)
+            or state_values.shape != (length,)
+            or not np.all(np.isfinite(command_values))
+            or not np.all(np.isfinite(state_values))
+            or np.any(command_values < 0.0)
+            or np.any(command_values > 255.0)
+            or np.any(state_values < 0.0)
+            or np.any(state_values > 255.0)
+            or capture.get("command_names") != names
+            or capture.get("state_names") != names
+            or capture.get("status")
+            not in {"exact", "approximate", "unsupported"}
+            or capture.get("confirmed") is not True
+        ):
+            raise RuntimeError(f"姿势检查点中的{pose}数据无效")
+        validated[pose] = dict(capture)
+    return validated
+
+
+def _parser() -> argparse.ArgumentParser:
+    parser = argparse.ArgumentParser(description="G20/O6实机姿势profile捕获")
+    parser.add_argument("--model", required=True, choices=("G20", "O6", "g20", "o6"))
+    parser.add_argument("--seed", required=True)
+    parser.add_argument("--output", required=True)
+    parser.add_argument("--serial-number", required=True)
+    parser.add_argument("--operator", required=True)
+    parser.add_argument("--firmware", default="unknown")
+    parser.add_argument("--can-interface")
+    parser.add_argument("--command-topic")
+    parser.add_argument("--state-topic")
+    parser.add_argument("--snapshot-topic")
+    parser.add_argument(
+        "--checkpoint",
+        help="检查点文件；默认使用输出文件名加.checkpoint.json",
+    )
+    parser.add_argument(
+        "--fresh",
+        action="store_true",
+        help="忽略并覆盖已有检查点，从第一个姿势重新开始",
+    )
+    return parser
+
+
+def main(argv: Sequence[str] | None = None) -> int:
+    args, ros_args = _parser().parse_known_args(argv)
+    model = args.model.upper()
+    lower = model.lower()
+    can_interface = args.can_interface or ("can0" if model == "G20" else "can1")
+    command_topic = args.command_topic or f"/{lower}/cb_left_hand_control_cmd"
+    state_topic = args.state_topic or f"/{lower}/cb_left_hand_state"
+    snapshot_topic = (
+        args.snapshot_topic or f"/{lower}/calibration_pose_snapshot"
+    )
+    seed = load_robot_profile(args.seed, model)
+    required_poses = list(seed.get("required_poses", ()))
+    if not required_poses:
+        raise SystemExit("seed profile不包含required_poses，不能用于姿势采集")
+    checkpoint_path = _checkpoint_path(args.output, args.checkpoint)
+    captures: dict[str, dict[str, Any]] = {}
+    if checkpoint_path.is_file() and not args.fresh:
+        captures = _load_checkpoint(
+            checkpoint_path,
+            model=model,
+            seed=seed,
+            output=args.output,
+            serial_number=args.serial_number,
+            firmware=args.firmware,
+            can_interface=can_interface,
+            operator=args.operator,
+        )
+        unsafe_poses = {
+            pose: _command_bound_violations(seed, capture["cmd_u8"])
+            for pose, capture in captures.items()
+            if _command_bound_violations(seed, capture["cmd_u8"])
+        }
+        if unsafe_poses:
+            print("检查点中发现超出profile安全范围的姿势，将只重拍这些姿势：")
+            for pose, violations in unsafe_poses.items():
+                print(
+                    f"  {POSE_LABELS.get(pose, pose)}: "
+                    + "; ".join(violations)
+                )
+                captures.pop(pose)
+    save_profile(
+        _checkpoint_document(
+            model=model,
+            seed=seed,
+            output=args.output,
+            serial_number=args.serial_number,
+            firmware=args.firmware,
+            can_interface=can_interface,
+            operator=args.operator,
+            captures=captures,
+        ),
+        checkpoint_path,
+    )
+
+    rclpy.init(args=ros_args)
+    node = HandPoseCaptureNode(
+        model,
+        command_topic,
+        state_topic,
+        snapshot_topic,
+        seed["command_names"],
+    )
+    executor = SingleThreadedExecutor()
+    executor.add_node(node)
+    spin_thread = threading.Thread(target=executor.spin, daemon=True)
+    spin_thread.start()
+    try:
+        print(
+            f"{model}姿势标定开始。请使用低速、低力矩的命名空间GUI独立调节；"
+            "不要用旧手套映射，也不要带电强掰。\n"
+            f"监听命令: {command_topic}\n监听状态: {state_topic}\n"
+            f"检查点: {checkpoint_path}"
+        )
+        if captures:
+            print(
+                "已从检查点恢复 "
+                f"{len(captures)}/{len(required_poses)} 个姿势: "
+                + ", ".join(captures)
+            )
+        for pose in required_poses:
+            if pose in captures:
+                print(
+                    f"\n跳过已保存姿势：{POSE_LABELS.get(pose, pose)}"
+                )
+                continue
+            while pose not in captures:
+                default = _default_status(model, pose)
+                snapshot_sequence = node.snapshot_sequence
+                print(
+                    f"\n将{model}调到“{POSE_LABELS.get(pose, pose)}”。"
+                    "确认通道、方向和姿势正确且实机稳定后，"
+                    "点击GUI的“保存当前标定姿势”；终端正在自动等待快照……",
+                    flush=True,
+                )
+                while (
+                    rclpy.ok()
+                    and node.snapshot_sequence <= snapshot_sequence
+                ):
+                    time.sleep(0.05)
+                if not rclpy.ok():
+                    raise RuntimeError("ROS已停止，无法继续姿势标定")
+                command = node.latest_snapshot
+                state = node.latest_state
+                state_names = node.latest_state_names
+                if command is None or node.latest_command is None:
+                    print(
+                        f"未从{command_topic}收到有效{model}命令；"
+                        "请检查GUI命名空间后重新保存当前姿势。"
+                    )
+                    continue
+                if state is None or state_names is None:
+                    print(
+                        f"未从{state_topic}收到有效{model}状态；"
+                        "请检查驱动后重新保存当前姿势。"
+                    )
+                    continue
+                violations = _command_bound_violations(seed, command)
+                if violations:
+                    print(
+                        "当前GUI快照超出profile安全范围，不会保存："
+                        + "; ".join(violations)
+                        + "。请在安全范围内重新调整并保存。"
+                    )
+                    continue
+                redo = False
+                while True:
+                    entered = input(
+                        "手势状态 exact/approximate/unsupported "
+                        f"[{default}]（输入r重拍）："
+                    ).strip().lower()
+                    if entered == "r":
+                        redo = True
+                        break
+                    status = entered or default
+                    if status in {"exact", "approximate", "unsupported"}:
+                        break
+                    print(f"无效手势状态: {status}，请重新输入。")
+                if redo:
+                    print("当前快照已放弃，请调整后重新点击GUI保存。")
+                    continue
+                captures[pose] = {
+                    "cmd_u8": command,
+                    "command_names": list(seed["command_names"]),
+                    "state_u8": state,
+                    "state_names": state_names,
+                    "status": status,
+                    "confirmed": True,
+                    "confirmation_method": "gui_snapshot_and_status",
+                    "captured_at": datetime.now(timezone.utc).isoformat(),
+                }
+                save_profile(
+                    _checkpoint_document(
+                        model=model,
+                        seed=seed,
+                        output=args.output,
+                        serial_number=args.serial_number,
+                        firmware=args.firmware,
+                        can_interface=can_interface,
+                        operator=args.operator,
+                        captures=captures,
+                    ),
+                    checkpoint_path,
+                )
+                print(
+                    f"已写入检查点：{len(captures)}/{len(required_poses)}"
+                )
+
+        approve = (
+            input(
+                "\n是否批准该profile用于低速实机控制？只有全部姿势已确认才会生效。[y/N]："
+            )
+            .strip()
+            .lower()
+            == "y"
+        )
+        profile = build_robot_profile(
+            seed,
+            captures,
+            serial_number=args.serial_number,
+            firmware=args.firmware,
+            can_interface=can_interface,
+            operator=args.operator,
+            approved_for_control=approve,
+        )
+        output = save_profile(profile, args.output)
+        validated = load_robot_profile(output, model)
+        save_profile(
+            _checkpoint_document(
+                model=model,
+                seed=seed,
+                output=args.output,
+                serial_number=args.serial_number,
+                firmware=args.firmware,
+                can_interface=can_interface,
+                operator=args.operator,
+                captures=captures,
+                completed=True,
+                final_profile_sha256=validated["_profile_sha256"],
+            ),
+            checkpoint_path,
+        )
+        print(
+            f"\nprofile已保存: {output}\n"
+            f"approved_for_control={validated['approved_for_control']}\n"
+            f"sha256={validated['_profile_sha256']}\n"
+            f"检查点已标记完成并保留: {checkpoint_path}"
+        )
+        return 0
+    finally:
+        executor.shutdown()
+        spin_thread.join(timeout=1.0)
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/calibration.py

@@ -0,0 +1,475 @@
+"""Profile generation from robust multi-gesture calibration captures."""
+
+from __future__ import annotations
+
+import copy
+from datetime import datetime, timezone
+from typing import Any, Mapping, Sequence
+
+import numpy as np
+
+from .constants import (
+    BASE_INTENT_NAMES,
+    FFG_JOINT_NAMES,
+    GLOVE_DYNAMIC_GESTURES,
+    GLOVE_STATIC_GESTURES,
+    MODEL_COMMAND_LENGTHS,
+    PINCH_ROLES,
+    SCHEMA_VERSION,
+    gesture_intent_value,
+)
+
+
+def robust_summary(samples: Sequence[Sequence[float]], length: int) -> dict[str, Any]:
+    """Return raw frames and robust statistics for a calibration capture."""
+
+    values = np.asarray(samples, dtype=float)
+    if values.ndim != 2 or values.shape[1] != length:
+        raise ValueError(f"capture must have shape (N, {length})")
+    if values.shape[0] < 1 or not np.all(np.isfinite(values)):
+        raise ValueError("capture contains no valid finite samples")
+    median = np.median(values, axis=0)
+    mad = np.median(np.abs(values - median), axis=0)
+    return {
+        "sample_count": int(values.shape[0]),
+        "median": median.astype(float).tolist(),
+        "mad": mad.astype(float).tolist(),
+        "raw_frames": values.astype(float).tolist(),
+    }
+
+
+_FEATURE_POSE_LABELS: dict[str, tuple[list[int], dict[str, float]]] = {
+    "thumb_rotate": (
+        [1, 2],
+        {"open_spread": 0.5, "thumb_max_out": 0.0, "thumb_across": 1.0},
+    ),
+    "thumb_abduction": (
+        [0, 1, 2],
+        {"open_spread": 0.5, "thumb_max_out": 0.0, "thumb_across": 1.0},
+    ),
+    "thumb_opposition": (
+        [0, 1, 2, 3, 4],
+        {
+            "open_spread": 0.0,
+            "thumb_max_out": 0.0,
+            "thumb_across": 1.0,
+            "pinch_index": 1.0,
+            "pinch_middle": 1.0,
+            "pinch_ring": 1.0,
+            "pinch_pinky": 1.0,
+        },
+    ),
+    "thumb_root": (
+        [2, 3],
+        {
+            "open_spread": 0.0,
+            "thumb_max_out": 0.0,
+            "fist": 1.0,
+            "thumb_across": 0.75,
+        },
+    ),
+    "thumb_tip": (
+        [3, 4],
+        {
+            "open_spread": 0.0,
+            "thumb_max_out": 0.0,
+            "fist": 1.0,
+            "thumb_across": 0.75,
+        },
+    ),
+}
+
+for _finger, _base in (("index", 5), ("middle", 9), ("ring", 13), ("pinky", 17)):
+    _FEATURE_POSE_LABELS[f"{_finger}_root"] = (
+        [_base + 1, _base + 2, _base + 3],
+        {
+            "open_spread": 0.0,
+            "fingers_together": 0.0,
+            "hook": 0.0,
+            "tabletop": 1.0,
+            "fist": 1.0,
+        },
+    )
+    _FEATURE_POSE_LABELS[f"{_finger}_tip"] = (
+        [_base + 1, _base + 2, _base + 3],
+        {
+            "open_spread": 0.0,
+            "fingers_together": 0.0,
+            "tabletop": 0.0,
+            "hook": 1.0,
+            "fist": 1.0,
+        },
+    )
+    _FEATURE_POSE_LABELS[f"{_finger}_splay"] = (
+        [_base],
+        {"fingers_together": 0.0, "open_spread": 1.0},
+    )
+
+
+def _fit_feature(
+    pose_medians: Mapping[str, np.ndarray],
+    indices: Sequence[int],
+    labels: Mapping[str, float],
+) -> dict[str, Any]:
+    available = [(pose, float(label)) for pose, label in labels.items() if pose in pose_medians]
+    zeros = [pose_medians[pose][indices] for pose, label in available if label <= 0.05]
+    ones = [pose_medians[pose][indices] for pose, label in available if label >= 0.95]
+    if not zeros or not ones:
+        raise ValueError("feature calibration requires both zero and one endpoint poses")
+    low = np.median(np.vstack(zeros), axis=0)
+    high = np.median(np.vstack(ones), axis=0)
+    span = high - low
+    x_rows: list[np.ndarray] = []
+    y: list[float] = []
+    for pose, label in available:
+        row = np.zeros(len(indices), dtype=float)
+        valid = np.abs(span) > 1e-6
+        row[valid] = (pose_medians[pose][indices][valid] - low[valid]) / span[valid]
+        x_rows.append(np.clip(row, 0.0, 1.0))
+        y.append(label)
+    weights, *_ = np.linalg.lstsq(np.vstack(x_rows), np.asarray(y), rcond=None)
+    weights = np.clip(weights, 0.0, None)
+    if float(np.sum(weights)) <= 1e-9:
+        weights = np.ones(len(indices), dtype=float)
+    weights /= np.sum(weights)
+    return {
+        "indices": [int(index) for index in indices],
+        "weights": weights.astype(float).tolist(),
+        "low": low.astype(float).tolist(),
+        "high": high.astype(float).tolist(),
+    }
+
+
+def build_glove_profile(
+    captures: Mapping[str, Sequence[Sequence[float]]],
+    *,
+    glove_id: str,
+    operator: str,
+    firmware: str = "",
+    dynamic_captures: Mapping[str, Sequence[Sequence[float]]] | None = None,
+    repeat_summaries: Mapping[str, Sequence[Mapping[str, Any]]] | None = None,
+    dynamic_repeat_summaries:
+        Mapping[str, Sequence[Mapping[str, Any]]] | None = None,
+) -> dict[str, Any]:
+    """Build a complete left FFG glove profile from eleven pose captures."""
+
+    missing = set(GLOVE_STATIC_GESTURES) - set(captures)
+    if missing:
+        raise ValueError(f"missing glove calibration gestures: {sorted(missing)}")
+    summaries = {
+        gesture: robust_summary(captures[gesture], 21)
+        for gesture in GLOVE_STATIC_GESTURES
+    }
+    for gesture, repeats in (repeat_summaries or {}).items():
+        if gesture in summaries:
+            summaries[gesture]["repeats"] = copy.deepcopy(list(repeats))
+    pose_medians = {
+        gesture: np.asarray(summary["median"], dtype=float)
+        for gesture, summary in summaries.items()
+    }
+    features = {
+        name: _fit_feature(pose_medians, *_FEATURE_POSE_LABELS[name])
+        for name in BASE_INTENT_NAMES
+    }
+
+    open_pose = pose_medians["open_spread"]
+    pinch_anchors: dict[str, Any] = {}
+    finger_indices = {
+        "index": list(range(5, 9)),
+        "middle": list(range(9, 13)),
+        "ring": list(range(13, 17)),
+        "pinky": list(range(17, 21)),
+    }
+    for role in PINCH_ROLES:
+        indices = list(range(5)) + finger_indices[role]
+        center = pose_medians[f"pinch_{role}"][indices]
+        open_values = open_pose[indices]
+        pose_mad = np.asarray(summaries[f"pinch_{role}"]["mad"], dtype=float)[indices]
+        scale = np.maximum.reduce(
+            [np.abs(center - open_values), pose_mad * 6.0, np.full(len(indices), 0.02)]
+        )
+        open_distance = float(
+            np.sqrt(np.mean(np.square((open_values - center) / scale)))
+        )
+        pinch_anchors[role] = {
+            "indices": indices,
+            "center": center.astype(float).tolist(),
+            "scale": scale.astype(float).tolist(),
+            "open_distance": max(open_distance, 1e-3),
+        }
+
+    dynamic = {
+        name: robust_summary(samples, 21)
+        for name, samples in (dynamic_captures or {}).items()
+    }
+    for gesture, repeats in (dynamic_repeat_summaries or {}).items():
+        if gesture in dynamic:
+            dynamic[gesture]["repeats"] = copy.deepcopy(list(repeats))
+
+    def complete_repeats(summary: Mapping[str, Any]) -> bool:
+        repeats = summary.get("repeats")
+        return (
+            isinstance(repeats, list)
+            and len(repeats) == 3
+            and all(
+                isinstance(repeat, Mapping)
+                and int(repeat.get("sample_count", 0)) >= 50
+                for repeat in repeats
+            )
+            and sum(int(repeat["sample_count"]) for repeat in repeats)
+            == int(summary.get("sample_count", 0))
+        )
+
+    approved_for_runtime = (
+        all(complete_repeats(summary) for summary in summaries.values())
+        and set(GLOVE_DYNAMIC_GESTURES).issubset(dynamic)
+        and all(
+            complete_repeats(dynamic[name])
+            for name in GLOVE_DYNAMIC_GESTURES
+        )
+    )
+    return {
+        "schema_version": SCHEMA_VERSION,
+        "profile_kind": "ffg_glove",
+        "calibration": "provisional",
+        "approved_for_runtime": approved_for_runtime,
+        "created_at": datetime.now(timezone.utc).isoformat(),
+        "glove": {
+            "id": str(glove_id),
+            "side": "left",
+            "operator": str(operator),
+            "firmware": str(firmware),
+        },
+        "joint_names": list(FFG_JOINT_NAMES),
+        "poses": summaries,
+        "dynamic_captures": dynamic,
+        "features": features,
+        "pinch_anchors": pinch_anchors,
+    }
+
+
+def _fit_source_weights(
+    model_profile: Mapping[str, Any],
+    channel: Mapping[str, Any],
+    pose_commands: Mapping[str, Sequence[float]],
+) -> dict[str, float]:
+    sources = list(
+        model_profile.get("fit_sources", {}).get(str(channel["name"]), [])
+    )
+    if not sources:
+        return {
+            str(name): float(weight)
+            for name, weight in channel["sources"].items()
+        }
+    command_index = int(channel["index"])
+    open_command = float(pose_commands["open_spread"][command_index])
+    fist_command = float(pose_commands["fist"][command_index])
+    span = fist_command - open_command
+    if abs(span) < 1e-6:
+        return {source: 1.0 / len(sources) for source in sources}
+    gestures = [
+        pose for pose in ("open_spread", "tabletop", "hook", "fist") if pose in pose_commands
+    ]
+    x = np.asarray(
+        [
+            [gesture_intent_value(gesture, source) for source in sources]
+            for gesture in gestures
+        ],
+        dtype=float,
+    )
+    y = np.asarray(
+        [
+            (float(pose_commands[gesture][command_index]) - open_command) / span
+            for gesture in gestures
+        ],
+        dtype=float,
+    )
+    weights, *_ = np.linalg.lstsq(x, y, rcond=None)
+    weights = np.clip(weights, 0.0, None)
+    if float(np.sum(weights)) <= 1e-9:
+        weights = np.ones(len(sources), dtype=float)
+    weights /= np.sum(weights)
+    return {source: float(weight) for source, weight in zip(sources, weights)}
+
+
+def _aggregate_points(points: Sequence[Sequence[float]]) -> list[list[float]]:
+    buckets: dict[float, list[float]] = {}
+    for raw_x, raw_y in points:
+        buckets.setdefault(float(raw_x), []).append(float(raw_y))
+    return [
+        [float(x), float(np.median(values))]
+        for x, values in sorted(buckets.items())
+    ]
+
+
+def _monotonic_points(points: Sequence[Sequence[float]]) -> list[list[float]]:
+    """Aggregate duplicate activations and fit monotonic command endpoints."""
+
+    ordered = [
+        (point[0], point[1])
+        for point in _aggregate_points(points)
+    ]
+    if len(ordered) < 2:
+        raise ValueError("calibration produced fewer than two activation points")
+    direction = 1.0 if ordered[-1][1] >= ordered[0][1] else -1.0
+    blocks: list[dict[str, Any]] = []
+    for index, (_, value) in enumerate(ordered):
+        blocks.append(
+            {"start": index, "end": index, "sum": direction * value, "count": 1}
+        )
+        while len(blocks) >= 2:
+            left = blocks[-2]
+            right = blocks[-1]
+            left_mean = float(left["sum"]) / int(left["count"])
+            right_mean = float(right["sum"]) / int(right["count"])
+            if left_mean <= right_mean:
+                break
+            blocks[-2:] = [
+                {
+                    "start": left["start"],
+                    "end": right["end"],
+                    "sum": float(left["sum"]) + float(right["sum"]),
+                    "count": int(left["count"]) + int(right["count"]),
+                }
+            ]
+    fitted = np.zeros(len(ordered), dtype=float)
+    for block in blocks:
+        mean = direction * float(block["sum"]) / int(block["count"])
+        fitted[int(block["start"]) : int(block["end"]) + 1] = mean
+    return [
+        [float(point[0]), float(value)]
+        for point, value in zip(ordered, fitted)
+    ]
+
+
+def build_robot_profile(
+    seed_profile: Mapping[str, Any],
+    captures: Mapping[str, Mapping[str, Any]],
+    *,
+    serial_number: str,
+    firmware: str = "",
+    can_interface: str = "",
+    operator: str = "",
+    approved_for_control: bool = False,
+) -> dict[str, Any]:
+    """Turn visually confirmed robot gesture captures into a runtime profile."""
+
+    profile = copy.deepcopy(
+        {
+            key: value
+            for key, value in seed_profile.items()
+            if not str(key).startswith("_profile_")
+        }
+    )
+    model = str(profile["model"]).upper()
+    length = MODEL_COMMAND_LENGTHS[model]
+    required = set(profile["required_poses"])
+    missing = required - set(captures)
+    if missing:
+        raise ValueError(f"missing {model} robot calibration poses: {sorted(missing)}")
+    pose_commands: dict[str, list[float]] = {}
+    poses: dict[str, Any] = {}
+    command_bounds = np.asarray(profile["command_bounds"], dtype=float)
+    for pose_name in profile["required_poses"]:
+        capture = captures[pose_name]
+        command = np.asarray(capture["cmd_u8"], dtype=float)
+        state = np.asarray(capture.get("state_u8", command), dtype=float)
+        if command.shape != (length,) or state.shape != (length,):
+            raise ValueError(f"{pose_name} must contain {length} command/state values")
+        if (
+            not np.all(np.isfinite(command))
+            or np.any(command < 0)
+            or np.any(command > 255)
+            or not np.all(np.isfinite(state))
+            or np.any(state < 0)
+            or np.any(state > 255)
+        ):
+            raise ValueError(f"{pose_name} contains an invalid command")
+        if np.any(command < command_bounds[:, 0]) or np.any(
+            command > command_bounds[:, 1]
+        ):
+            raise ValueError(
+                f"{pose_name} command exceeds the profile safety bounds"
+            )
+        pose_commands[pose_name] = command.astype(float).tolist()
+        status = str(capture.get("status", ""))
+        if status not in {"exact", "approximate", "unsupported"}:
+            raise ValueError(f"{pose_name} has an invalid status")
+        state_names = [str(name) for name in capture.get("state_names", ())]
+        if (
+            len(state_names) != length
+            or len(set(state_names)) != length
+            or state_names != list(profile["command_names"])
+        ):
+            raise ValueError(f"{pose_name} has invalid state names")
+        poses[pose_name] = {
+            "cmd_u8": command.astype(float).tolist(),
+            "command_names": list(profile["command_names"]),
+            "state_u8": state.astype(float).tolist(),
+            "state_names": state_names,
+            "status": status,
+            "confirmed": bool(capture.get("confirmed", False)),
+            "captured_at": str(capture.get("captured_at", "")),
+        }
+
+    for channel in profile["channels"]:
+        channel["sources"] = _fit_source_weights(
+            profile, channel, pose_commands
+        )
+        points = []
+        point_poses = channel.get("point_poses", ("open_spread", "fist"))
+        for pose_name in point_poses:
+            if pose_name in pose_commands:
+                sources = channel["sources"]
+                activation = sum(
+                    float(weight) * gesture_intent_value(pose_name, source)
+                    for source, weight in sources.items()
+                ) / max(sum(float(value) for value in sources.values()), 1e-9)
+                points.append(
+                    [
+                        float(activation),
+                        float(pose_commands[pose_name][int(channel["index"])]),
+                    ]
+                )
+        if channel.get("mapping") == "monotonic_piecewise":
+            channel["points"] = _monotonic_points(points)
+        else:
+            channel["points"] = _aggregate_points(points)
+        channel.pop("point_poses", None)
+
+    # Older seeds carried a discrete pinch selector configuration.  Preserve
+    # compatibility when reading those seeds, but never copy that mechanism
+    # into a newly calibrated continuous profile.  The named pinch captures
+    # already live in ``poses`` and are paired with the glove at runtime.
+    profile.pop("pinch", None)
+    profile["mapping_strategy"] = "paired_continuous_v1"
+    profile["poses"] = poses
+    profile["approved_for_control"] = (
+        bool(approved_for_control)
+        and bool(str(serial_number).strip())
+        and bool(str(can_interface).strip())
+        and bool(str(operator).strip())
+        and all(pose["confirmed"] for pose in poses.values())
+    )
+    profile["calibration"] = "provisional"
+    profile["created_at"] = datetime.now(timezone.utc).isoformat()
+    profile["device"] = {
+        "serial_number": str(serial_number),
+        "firmware": str(firmware),
+        "can_interface": str(can_interface),
+    }
+    profile.setdefault("metadata", {})
+    profile["metadata"]["operator_confirmed"] = bool(
+        profile["approved_for_control"]
+    )
+    profile["metadata"]["description"] = (
+        f"{model}左手机身多手势标定profile；无真实角度GT"
+    )
+    profile["metadata"]["operator"] = str(operator)
+    profile.pop("required_poses", None)
+    profile.pop("fit_sources", None)
+    profile["schema_version"] = SCHEMA_VERSION
+    profile.pop("kind", None)
+    profile["profile_kind"] = "robot_hand"
+    profile["side"] = "left"
+    return profile

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/constants.py

@@ -0,0 +1,162 @@
+"""Stable names and calibration pose definitions for linkerforce_v2."""
+
+from __future__ import annotations
+
+SCHEMA_VERSION = 1
+
+# Runtime defaults intentionally match the effective legacy left-G20 path:
+# direct latest-frame mapping and firmware-side interpolation.
+DEFAULT_INPUT_FILTER_ENABLED = False
+DEFAULT_COMMAND_FILTER_CONFIG = {
+    "mode": "passthrough",
+    "ema_alpha": 1.0,
+    "max_step_u8": 255.0,
+    "max_acceleration_u8_per_frame2": 255.0,
+    "deadband_u8": 0.0,
+}
+
+FFG_JOINT_NAMES = tuple(
+    [f"thumb_{index}" for index in range(5)]
+    + [f"index_{index}" for index in range(4)]
+    + [f"middle_{index}" for index in range(4)]
+    + [f"ring_{index}" for index in range(4)]
+    + [f"pinky_{index}" for index in range(4)]
+)
+
+BASE_INTENT_NAMES = (
+    "thumb_rotate",
+    "thumb_abduction",
+    "thumb_opposition",
+    "thumb_root",
+    "thumb_tip",
+    "index_root",
+    "index_tip",
+    "middle_root",
+    "middle_tip",
+    "ring_root",
+    "ring_tip",
+    "pinky_root",
+    "pinky_tip",
+    "index_splay",
+    "middle_splay",
+    "ring_splay",
+    "pinky_splay",
+)
+
+PINCH_ROLES = ("index", "middle", "ring", "pinky")
+PINCH_INTENT_NAMES = tuple(f"pinch_{role}" for role in PINCH_ROLES)
+INTENT_NAMES = BASE_INTENT_NAMES + PINCH_INTENT_NAMES + ("power_grasp",)
+
+GLOVE_STATIC_GESTURES = (
+    "open_spread",
+    "fingers_together",
+    "tabletop",
+    "hook",
+    "fist",
+    "thumb_max_out",
+    "thumb_across",
+    "pinch_index",
+    "pinch_middle",
+    "pinch_ring",
+    "pinch_pinky",
+)
+
+GLOVE_DYNAMIC_GESTURES = (
+    "index_flex_sweep",
+    "middle_flex_sweep",
+    "ring_flex_sweep",
+    "pinky_flex_sweep",
+    "thumb_flex_sweep",
+    "thumb_opposition_sweep",
+    "finger_splay_sweep",
+)
+
+G20_GESTURES = GLOVE_STATIC_GESTURES
+O6_GESTURES = (
+    "open_spread",
+    "tabletop",
+    "hook",
+    "fist",
+    "thumb_max_out",
+    "thumb_across",
+    "pinch_index",
+    "pinch_middle",
+    "pinch_ring",
+    "pinch_pinky",
+)
+
+GESTURE_STATUS_VALUES = ("exact", "approximate", "unsupported")
+
+MODEL_COMMAND_LENGTHS = {"G20": 20, "O6": 6}
+
+# Expected semantic endpoints are used only while generating a profile. Runtime
+# mapping consumes generated coefficients and does not contain gesture magic.
+GESTURE_INTENT_TARGETS = {
+    "open_spread": {
+        **{name: 0.0 for name in BASE_INTENT_NAMES},
+        "thumb_rotate": 0.5,
+        "thumb_abduction": 0.5,
+        "index_splay": 1.0,
+        "middle_splay": 1.0,
+        "ring_splay": 1.0,
+        "pinky_splay": 1.0,
+    },
+    "fingers_together": {
+        **{name: 0.0 for name in BASE_INTENT_NAMES},
+    },
+    "tabletop": {
+        "index_root": 1.0,
+        "middle_root": 1.0,
+        "ring_root": 1.0,
+        "pinky_root": 1.0,
+    },
+    "hook": {
+        "index_tip": 1.0,
+        "middle_tip": 1.0,
+        "ring_tip": 1.0,
+        "pinky_tip": 1.0,
+    },
+    "fist": {
+        "thumb_root": 1.0,
+        "thumb_tip": 1.0,
+        "thumb_opposition": 1.0,
+        "index_root": 1.0,
+        "index_tip": 1.0,
+        "middle_root": 1.0,
+        "middle_tip": 1.0,
+        "ring_root": 1.0,
+        "ring_tip": 1.0,
+        "pinky_root": 1.0,
+        "pinky_tip": 1.0,
+    },
+    "thumb_max_out": {
+        "thumb_rotate": 0.0,
+        "thumb_abduction": 0.0,
+        "thumb_opposition": 0.0,
+    },
+    "thumb_across": {
+        "thumb_rotate": 1.0,
+        "thumb_abduction": 1.0,
+        "thumb_opposition": 1.0,
+        "thumb_root": 0.75,
+        "thumb_tip": 0.75,
+    },
+}
+
+for _role in PINCH_ROLES:
+    GESTURE_INTENT_TARGETS[f"pinch_{_role}"] = {
+        "thumb_rotate": 0.8,
+        "thumb_abduction": 0.8,
+        "thumb_opposition": 1.0,
+        "thumb_root": 0.75,
+        "thumb_tip": 0.85,
+        f"{_role}_root": 0.55,
+        f"{_role}_tip": 0.65,
+        f"pinch_{_role}": 1.0,
+    }
+
+
+def gesture_intent_value(gesture: str, feature: str) -> float:
+    """Return the canonical calibration target for a gesture/feature pair."""
+
+    return float(GESTURE_INTENT_TARGETS.get(gesture, {}).get(feature, 0.0))

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/node.py

@@ -0,0 +1,866 @@
+"""ROS 2 node for one left FFG driving G20 and O6 through profiles."""
+
+from __future__ import annotations
+
+from collections import deque
+from datetime import datetime, timezone
+import json
+import math
+from pathlib import Path
+import time
+from typing import Any
+
+import numpy as np
+
+import rclpy
+from rclpy.node import Node
+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 ...linkerhand.constants import HandType
+from ...linkerhand.linkerforce import ForceSerialReader
+from .constants import (
+    DEFAULT_COMMAND_FILTER_CONFIG,
+    DEFAULT_INPUT_FILTER_ENABLED,
+    FFG_JOINT_NAMES,
+    INTENT_NAMES,
+    MODEL_COMMAND_LENGTHS,
+)
+from .mapping import (
+    HandIntentExtractor,
+    MappingResult,
+    RobotMapper,
+    VectorKalmanFilter,
+)
+from .profiles import ProfileError, load_glove_profile, load_robot_profile
+from .safety import evaluate_timeouts
+
+
+COMMAND_QOS = QoSProfile(
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,
+    reliability=ReliabilityPolicy.RELIABLE,
+)
+
+
+class FFGDualRetargetNode(Node):
+    """Profile-driven, fail-closed retargeting for a single left FFG."""
+
+    def __init__(self) -> None:
+        super().__init__("ffg_dual_retarget")
+        self._declare_parameters()
+
+        self.publish_rate = float(self.get_parameter("publish_rate").value)
+        self.glove_timeout = float(self.get_parameter("glove_timeout").value)
+        self.driver_timeout = float(self.get_parameter("driver_timeout").value)
+        if (
+            self.publish_rate <= 0.0
+            or self.glove_timeout <= 0.0
+            or self.driver_timeout <= 0.0
+        ):
+            raise ValueError("publish_rate and timeout parameters must be positive")
+        self.input_mode = str(self.get_parameter("input_mode").value).strip().lower()
+        self.input_filter_enabled = bool(
+            self.get_parameter("input_filter_enabled").value
+        )
+        self.input_filter_process_variance = float(
+            self.get_parameter("input_filter_process_variance").value
+        )
+        self.input_filter_measurement_variance = float(
+            self.get_parameter("input_filter_measurement_variance").value
+        )
+        self.input_filter_reset_gap = float(
+            self.get_parameter("input_filter_reset_gap").value
+        )
+        self.command_filter_config = {
+            "mode": str(
+                self.get_parameter("command_filter_mode").value
+            ).strip().lower(),
+            "ema_alpha": float(
+                self.get_parameter("command_filter_ema_alpha").value
+            ),
+            "max_step_u8": float(
+                self.get_parameter("command_filter_max_step_u8").value
+            ),
+            "max_acceleration_u8_per_frame2": float(
+                self.get_parameter("command_filter_max_step_u8").value
+            ),
+            "deadband_u8": float(
+                self.get_parameter("command_filter_deadband_u8").value
+            ),
+        }
+        self.sequence = 0
+        self.last_glove_time = 0.0
+        self.last_processed_sequence = -1
+        self.latest_raw: list[float] | None = None
+        self.latest_intent: dict[str, float] | None = None
+        self.latencies_ms: deque[float] = deque(maxlen=600)
+        self.last_status_publish = 0.0
+        self.last_fault = ""
+
+        self.glove_profile: dict[str, Any] | None = None
+        self.wear_check_valid = False
+        self.intent_extractor: HandIntentExtractor | None = None
+        self.profile_errors: dict[str, str] = {}
+        self.robot_profiles: dict[str, dict[str, Any]] = {}
+        self.mappers: dict[str, RobotMapper] = {}
+        self.last_anchor_weights: dict[
+            str,
+            tuple[tuple[str, float], ...],
+        ] = {
+            "G20": (),
+            "O6": (),
+        }
+        self.enabled = {"G20": False, "O6": False}
+        self.driver_state_time = {"G20": 0.0, "O6": 0.0}
+        self.driver_state_valid = {"G20": False, "O6": False}
+        self.latest_driver_state: dict[str, tuple[float, ...] | None] = {
+            "G20": None,
+            "O6": None,
+        }
+        self.last_hardware_command: dict[str, tuple[int, ...] | None] = {
+            "G20": None,
+            "O6": None,
+        }
+        self.serial_reader: ForceSerialReader | None = None
+        self.input_filter = VectorKalmanFilter(
+            len(FFG_JOINT_NAMES),
+            process_variance=self.input_filter_process_variance,
+            measurement_variance=self.input_filter_measurement_variance,
+            reset_gap_s=self.input_filter_reset_gap,
+        )
+
+        self.raw_publisher = self.create_publisher(
+            JointState, "/ffg/left/raw_joint_state", 10
+        )
+        self.filtered_raw_publisher = self.create_publisher(
+            JointState, "/ffg/left/filtered_joint_state", 10
+        )
+        self.intent_publisher = self.create_publisher(
+            JointState, "/retarget/left/hand_intent", 10
+        )
+        self.frame_meta_publisher = self.create_publisher(
+            String, "/retarget/left/frame_meta", 10
+        )
+        self.status_publisher = self.create_publisher(String, "~/status", 10)
+
+        self.actuation_publishers: dict[str, Any] = {}
+        self.nominal_publishers: dict[str, Any] = {}
+        self.preview_publishers: dict[str, Any] = {}
+        self.command_publishers: dict[str, Any] = {}
+        self.state_subscriptions: list[Any] = []
+        for model in ("G20", "O6"):
+            lower = model.lower()
+            self.actuation_publishers[model] = self.create_publisher(
+                JointState, f"/retarget/{lower}/left/actuation_target", 10
+            )
+            self.nominal_publishers[model] = self.create_publisher(
+                JointState, f"/retarget/{lower}/left/joint_target_nominal", 10
+            )
+            self.preview_publishers[model] = self.create_publisher(
+                JointState, f"/retarget/{lower}/left/cmd_u8_preview", 10
+            )
+            self.command_publishers[model] = self.create_publisher(
+                JointState, f"/{lower}/cb_left_hand_control_cmd", COMMAND_QOS
+            )
+            self.state_subscriptions.append(
+                self.create_subscription(
+                    JointState,
+                    f"/{lower}/cb_left_hand_state",
+                    lambda message, selected=model: self._state_callback(
+                        selected, message
+                    ),
+                    10,
+                )
+            )
+
+        self.create_service(
+            SetBool,
+            "~/enable_g20",
+            lambda request, response: self._enable_callback(
+                "G20", request, response
+            ),
+        )
+        self.create_service(
+            SetBool,
+            "~/enable_o6",
+            lambda request, response: self._enable_callback(
+                "O6", request, response
+            ),
+        )
+        self.create_service(SetBool, "~/enable_all", self._enable_all_callback)
+        self.create_service(Trigger, "~/emergency_stop", self._estop_callback)
+
+        self._load_profiles()
+        if self.input_mode == "topic":
+            raw_topic = str(self.get_parameter("raw_input_topic").value)
+            self.raw_subscription = self.create_subscription(
+                JointState, raw_topic, self._raw_topic_callback, 10
+            )
+            self.get_logger().info(f"FFG输入使用ROS话题: {raw_topic}")
+        elif self.input_mode == "serial":
+            self._connect_left_glove()
+        else:
+            raise ValueError("input_mode must be 'serial' or 'topic'")
+
+        self.process_timer = self.create_timer(
+            1.0 / max(self.publish_rate, 1.0), self._process_latest
+        )
+        self.watchdog_timer = self.create_timer(0.05, self._watchdog)
+        self.get_logger().info(
+            "linkerforce_v2已启动：默认PREVIEW，单只左手套即可运行；"
+            "G20/O6必须分别加载匹配且已批准的实机profile后才能使能"
+        )
+
+    def _declare_parameters(self) -> None:
+        self.declare_parameter("input_mode", "serial")
+        self.declare_parameter("raw_input_topic", "/ffg/left/raw_joint_state")
+        self.declare_parameter("serial_port", "")
+        self.declare_parameter("baudrate", 0)
+        self.declare_parameter("baudrates", [2000000, 460800, 1000000, 921600])
+        self.declare_parameter("auto_scan", True)
+        self.declare_parameter("serial_debug", False)
+        self.declare_parameter("glove_profile", "")
+        self.declare_parameter("wear_check", "")
+        self.declare_parameter("wear_check_max_age_hours", 12.0)
+        self.declare_parameter("g20_profile", "")
+        self.declare_parameter("o6_profile", "")
+        self.declare_parameter("g20_serial_number", "")
+        self.declare_parameter("o6_serial_number", "")
+        self.declare_parameter("g20_can_interface", "can0")
+        self.declare_parameter("o6_can_interface", "can1")
+        self.declare_parameter("publish_rate", 30.0)
+        self.declare_parameter("glove_timeout", 0.35)
+        self.declare_parameter("driver_timeout", 1.0)
+        # The legacy left-G20 runtime consumes the latest 21-value glove frame
+        # directly.  Keep that timing by default; Kalman remains opt-in for a
+        # particularly noisy glove.
+        self.declare_parameter(
+            "input_filter_enabled", DEFAULT_INPUT_FILTER_ENABLED
+        )
+        self.declare_parameter("input_filter_process_variance", 1e-5)
+        self.declare_parameter("input_filter_measurement_variance", 5e-4)
+        self.declare_parameter("input_filter_reset_gap", 0.35)
+        # LeftHand.joint_update() in the legacy G20 mapper did not call its
+        # declared _apply_smooth() helper.  Pass the calibrated target straight
+        # to the G20 firmware as it did, avoiding a second sample-and-hold layer.
+        self.declare_parameter(
+            "command_filter_mode",
+            DEFAULT_COMMAND_FILTER_CONFIG["mode"],
+        )
+        self.declare_parameter(
+            "command_filter_ema_alpha",
+            DEFAULT_COMMAND_FILTER_CONFIG["ema_alpha"],
+        )
+        self.declare_parameter(
+            "command_filter_max_step_u8",
+            DEFAULT_COMMAND_FILTER_CONFIG["max_step_u8"],
+        )
+        self.declare_parameter(
+            "command_filter_deadband_u8",
+            DEFAULT_COMMAND_FILTER_CONFIG["deadband_u8"],
+        )
+
+    def _load_profiles(self) -> None:
+        glove_path = str(self.get_parameter("glove_profile").value).strip()
+        if glove_path:
+            try:
+                self.glove_profile = load_glove_profile(glove_path)
+                self.intent_extractor = HandIntentExtractor(self.glove_profile)
+                self.get_logger().info(
+                    "FFG profile已加载: "
+                    f"{Path(glove_path).name} "
+                    f"sha256={self.glove_profile['_profile_sha256'][:12]}"
+                )
+                self._load_wear_check()
+            except (ProfileError, TypeError, ValueError) as exc:
+                self.profile_errors["FFG"] = str(exc)
+                self.get_logger().error(f"FFG profile无效，仅保留raw预览: {exc}")
+        else:
+            self.profile_errors["FFG"] = "glove_profile未配置"
+            self.get_logger().warning("未配置FFG profile，仅发布原始手套数据")
+
+        for model in ("G20", "O6"):
+            parameter = f"{model.lower()}_profile"
+            path = str(self.get_parameter(parameter).value).strip()
+            if not path:
+                self.profile_errors[model] = f"{parameter}未配置"
+                self.get_logger().warning(
+                    f"未配置{model} profile，不生成该型号目标"
+                )
+                continue
+            try:
+                profile = load_robot_profile(path, model)
+                self.robot_profiles[model] = profile
+                self.mappers[model] = RobotMapper(
+                    profile,
+                    self.glove_profile,
+                    command_filter_config=self.command_filter_config,
+                )
+                mode = (
+                    "可申请实机使能"
+                    if profile.get("approved_for_control")
+                    else "仅预览"
+                )
+                self.get_logger().info(
+                    f"{model} profile已加载({mode}): {Path(path).name} "
+                    f"sha256={profile['_profile_sha256'][:12]}"
+                )
+                mapper = self.mappers[model]
+                self.get_logger().info(
+                    f"{model}映射={mapper.mapping_mode}, "
+                    "执行滤波="
+                    f"{mapper.command_filter_config['mode']} "
+                    f"alpha={mapper.command_filter_config['ema_alpha']}, "
+                    "max_step="
+                    f"{mapper.command_filter_config['max_step_u8']}"
+                )
+            except (ProfileError, TypeError, ValueError) as exc:
+                self.profile_errors[model] = str(exc)
+                self.get_logger().error(f"{model} profile无效: {exc}")
+
+    def _load_wear_check(self) -> None:
+        path = str(self.get_parameter("wear_check").value).strip()
+        if not path:
+            self.profile_errors["wear_check"] = "wear_check未配置"
+            self.get_logger().warning(
+                "未配置三姿势快速佩戴检查凭据，实机使能将被拒绝"
+            )
+            return
+        try:
+            data = json.loads(Path(path).expanduser().resolve().read_text("utf-8"))
+            checked_at = datetime.fromisoformat(str(data["checked_at"]))
+            if checked_at.tzinfo is None:
+                checked_at = checked_at.replace(tzinfo=timezone.utc)
+            age_hours = (
+                datetime.now(timezone.utc) - checked_at.astimezone(timezone.utc)
+            ).total_seconds() / 3600.0
+            expected_hash = (
+                self.glove_profile.get("_profile_sha256")
+                if self.glove_profile
+                else None
+            )
+            if data.get("kind") != "ffg_wear_check" or not data.get("passed"):
+                raise ValueError("快速佩戴检查未通过")
+            if data.get("glove_profile_sha256") != expected_hash:
+                raise ValueError("快速佩戴检查与当前FFG profile哈希不匹配")
+            maximum = float(
+                self.get_parameter("wear_check_max_age_hours").value
+            )
+            if age_hours < 0.0 or age_hours > maximum:
+                raise ValueError(
+                    f"快速佩戴检查已过期: {age_hours:.1f}h > {maximum:.1f}h"
+                )
+            self.wear_check_valid = True
+            self.get_logger().info(
+                f"三姿势快速佩戴检查有效，凭据年龄{age_hours:.2f}h"
+            )
+        except (OSError, KeyError, TypeError, ValueError, json.JSONDecodeError) as exc:
+            self.profile_errors["wear_check"] = str(exc)
+            self.get_logger().error(f"快速佩戴检查凭据无效: {exc}")
+
+    def _log_from_reader(self, level: str, message: str) -> None:
+        logger = self.get_logger()
+        method = getattr(logger, "warning" if level == "warn" else level, logger.info)
+        method(message)
+
+    def _connect_left_glove(self) -> None:
+        configured_rates = [
+            int(value) for value in self.get_parameter("baudrates").value
+        ]
+        requested_rate = int(self.get_parameter("baudrate").value)
+        if requested_rate > 0:
+            configured_rates = [requested_rate] + [
+                value for value in configured_rates if value != requested_rate
+            ]
+        reader = ForceSerialReader(
+            HandType.left,
+            baudrates=configured_rates,
+            isdebug=bool(self.get_parameter("serial_debug").value),
+            logger=self._log_from_reader,
+        )
+        configured_port = str(self.get_parameter("serial_port").value).strip()
+        auto_scan = bool(self.get_parameter("auto_scan").value)
+        port: str | None = None
+        baud: int | None = None
+        if configured_port:
+            success, detected_baud, error = reader.query_serial_port(
+                configured_port, timeout=1.0
+            )
+            if success:
+                port, baud = configured_port, int(detected_baud)
+            else:
+                self.get_logger().error(
+                    f"左手套端口检测失败: {configured_port}, error={error}"
+                )
+        if port is None and auto_scan:
+            detected_port, detected_baud, _ = reader.find_valid_ports(timeout=1.0)
+            if detected_port:
+                port, baud = str(detected_port), int(detected_baud)
+        if port is None or baud is None:
+            self.get_logger().error(
+                "未找到左手FFG；节点保持运行并禁止实机控制，不会寻找右手套"
+            )
+            return
+        if not reader.openserial(port, baud):
+            self.get_logger().error(f"无法打开左手FFG: {port} @ {baud}")
+            return
+        reader.start()
+        self.serial_reader = reader
+        self.get_logger().info(
+            f"左手FFG已连接: {port} @ {baud}, version={reader.version or 'unknown'}"
+        )
+
+    def _raw_topic_callback(self, message: JointState) -> None:
+        if len(message.position) != len(FFG_JOINT_NAMES):
+            self.last_fault = (
+                f"FFG topic长度错误: {len(message.position)}, 应为21"
+            )
+            return
+        names = [str(name) for name in message.name]
+        if len(names) != 21 or set(names) != set(FFG_JOINT_NAMES):
+            self.last_fault = "FFG topic关节名称缺失、重复或不匹配"
+            return
+        lookup = {
+            name: float(value)
+            for name, value in zip(names, message.position)
+        }
+        raw = [lookup[name] for name in FFG_JOINT_NAMES]
+        if not all(math.isfinite(value) for value in raw):
+            self.last_fault = "FFG topic包含非有限值"
+            return
+        self.sequence += 1
+        self.latest_raw = raw
+        self.last_glove_time = time.monotonic()
+
+    def _state_callback(self, model: str, message: JointState) -> None:
+        values = [float(value) for value in message.position]
+        names = [str(name) for name in message.name]
+        expected_names = list(
+            self.robot_profiles.get(model, {}).get("command_names", ())
+        )
+        valid = (
+            len(values) == MODEL_COMMAND_LENGTHS[model]
+            and all(math.isfinite(value) for value in values)
+            and all(0.0 <= value <= 255.0 for value in values)
+            and names == expected_names
+        )
+        self.driver_state_valid[model] = valid
+        if valid:
+            self.driver_state_time[model] = time.monotonic()
+            self.latest_driver_state[model] = tuple(values)
+
+    def _update_serial_frame(self) -> None:
+        if self.serial_reader is None:
+            return
+        raw, frame_sequence, received_at = self.serial_reader.position_snapshot
+        if frame_sequence == self.last_processed_sequence or frame_sequence <= 0:
+            return
+        self.last_processed_sequence = frame_sequence
+        if len(raw) != len(FFG_JOINT_NAMES) or not np.all(np.isfinite(raw)):
+            self.last_fault = "串口FFG帧不是21维有限值"
+            return
+        self.sequence += 1
+        self.latest_raw = [float(value) for value in raw]
+        self.last_glove_time = float(received_at)
+
+    def _joint_state(
+        self,
+        names: list[str] | tuple[str, ...],
+        values: Any,
+        stamp: Any | None = None,
+    ) -> JointState:
+        message = JointState()
+        message.header.stamp = stamp or self.get_clock().now().to_msg()
+        message.name = list(names)
+        message.position = [float(value) for value in values]
+        return message
+
+    def _publish_mapping(
+        self,
+        model: str,
+        result: MappingResult,
+        stamp: Any,
+    ) -> None:
+        self.actuation_publishers[model].publish(
+            self._joint_state(result.actuation_names, result.actuation, stamp)
+        )
+        self.nominal_publishers[model].publish(
+            self._joint_state(result.sim_joint_names, result.q_nominal, stamp)
+        )
+        names = self.robot_profiles[model]["command_names"]
+        command_message = self._joint_state(names, result.command, stamp)
+        self.preview_publishers[model].publish(command_message)
+        self.last_anchor_weights[model] = result.anchor_weights
+        if self.enabled[model]:
+            self.command_publishers[model].publish(command_message)
+            self.last_hardware_command[model] = result.command
+
+    def _publish_frame_meta(self, stamp: Any) -> None:
+        message = String()
+        message.data = json.dumps(
+            {
+                "timestamp_ns": int(stamp.sec) * 1_000_000_000
+                + int(stamp.nanosec),
+                "sequence": self.sequence,
+                "calibration": "provisional",
+                "q_gt": None,
+            },
+            ensure_ascii=False,
+            sort_keys=True,
+        )
+        self.frame_meta_publisher.publish(message)
+
+    def _process_latest(self) -> None:
+        if self.input_mode == "serial":
+            self._update_serial_frame()
+        raw = self.latest_raw
+        if raw is None:
+            return
+        new_frame = self.sequence != getattr(self, "_mapped_sequence", -1)
+        frame_stamp = self.get_clock().now().to_msg()
+        try:
+            if new_frame:
+                self._mapped_sequence = self.sequence
+                if self.input_mode == "serial":
+                    self.raw_publisher.publish(
+                        self._joint_state(
+                            FFG_JOINT_NAMES,
+                            raw,
+                            frame_stamp,
+                        )
+                    )
+                if self.intent_extractor is None:
+                    return
+                filtered_raw = (
+                    self.input_filter.apply(
+                        raw,
+                        timestamp_s=self.last_glove_time,
+                    )
+                    if self.input_filter_enabled
+                    else np.asarray(raw, dtype=float)
+                )
+                self.filtered_raw_publisher.publish(
+                    self._joint_state(
+                        FFG_JOINT_NAMES,
+                        filtered_raw,
+                        frame_stamp,
+                    )
+                )
+                self.latest_intent = self.intent_extractor.extract(
+                    filtered_raw
+                )
+                self.intent_publisher.publish(
+                    self._joint_state(
+                        INTENT_NAMES,
+                        [
+                            self.latest_intent[name]
+                            for name in INTENT_NAMES
+                        ],
+                        frame_stamp,
+                    )
+                )
+                self._publish_frame_meta(frame_stamp)
+                if self.last_glove_time > 0.0:
+                    self.latencies_ms.append(
+                        max(
+                            0.0,
+                            (
+                                time.monotonic()
+                                - self.last_glove_time
+                            )
+                            * 1000.0,
+                        )
+                    )
+
+            # Advance and publish the motor trajectory on every timer tick.
+            # Reusing the most recent valid intent is intentional: the legacy
+            # path also ran its controller at a fixed 30 Hz even between
+            # serial updates.  The watchdog still revokes real control when
+            # the source becomes stale.
+            if self.latest_intent is None:
+                return
+            for model, mapper in self.mappers.items():
+                self._publish_mapping(
+                    model,
+                    mapper.map(self.latest_intent),
+                    frame_stamp,
+                )
+        except (ValueError, FloatingPointError) as exc:
+            self.last_fault = f"映射失败: {exc}"
+            self._disable_all(self.last_fault)
+
+    def _identity_error(self, model: str) -> str | None:
+        profile = self.robot_profiles.get(model)
+        if profile is None:
+            return "profile未加载"
+        if not profile.get("approved_for_control", False):
+            return "profile未获实机控制批准"
+        device = profile.get("device")
+        if not isinstance(device, dict):
+            return "profile缺少设备SN/CAN元数据"
+        expected_sn = str(
+            self.get_parameter(f"{model.lower()}_serial_number").value
+        ).strip()
+        profile_sn = str(device.get("serial_number", "")).strip()
+        if not expected_sn:
+            return f"未设置{model.lower()}_serial_number，无法核对单机profile"
+        if profile_sn != expected_sn:
+            return f"profile SN={profile_sn} 与期望SN={expected_sn}不匹配"
+        expected_can = str(
+            self.get_parameter(f"{model.lower()}_can_interface").value
+        ).strip()
+        if str(device.get("can_interface", "")).strip() != expected_can:
+            return "profile CAN接口与启动配置不匹配"
+        return None
+
+    def _can_enable(self, model: str) -> str | None:
+        if (
+            self.glove_profile is None
+            or not self.glove_profile.get("approved_for_runtime", False)
+        ):
+            return "FFG profile采样或动态轨迹不完整，仅允许预览"
+        if not self.wear_check_valid:
+            return "三姿势快速佩戴检查缺失、失败或过期"
+        identity_error = self._identity_error(model)
+        if identity_error:
+            return identity_error
+        if self.mappers[model].mapping_mode != "factorized_paired_v2":
+            return "未建立FFG与机械手分解式姿势映射，仅允许预览"
+        now = time.monotonic()
+        if self.last_glove_time <= 0.0 or now - self.last_glove_time > self.glove_timeout:
+            return "FFG无新帧或已超时"
+        if (
+            not self.driver_state_valid[model]
+            or now - self.driver_state_time[model] > self.driver_timeout
+        ):
+            return f"{model}驱动状态无效或已超时"
+        return None
+
+    def _enable_callback(
+        self, model: str, request: SetBool.Request, response: SetBool.Response
+    ) -> SetBool.Response:
+        if not request.data:
+            self.enabled[model] = False
+            response.success = True
+            response.message = f"{model}已停用"
+            return response
+        error = self._can_enable(model)
+        if error:
+            response.success = False
+            response.message = f"{model}拒绝使能: {error}"
+            return response
+        self.mappers[model].reset(self.latest_driver_state[model])
+        self.enabled[model] = True
+        response.success = True
+        response.message = f"{model}已使能"
+        return response
+
+    def _enable_all_callback(
+        self, request: SetBool.Request, response: SetBool.Response
+    ) -> SetBool.Response:
+        if not request.data:
+            self._disable_all("用户停用全部")
+            response.success = True
+            response.message = "G20/O6均已停用"
+            return response
+        errors = {
+            model: error
+            for model in ("G20", "O6")
+            if (error := self._can_enable(model)) is not None
+        }
+        if errors:
+            response.success = False
+            response.message = "拒绝全部使能: " + "; ".join(
+                f"{model}={error}" for model, error in errors.items()
+            )
+            return response
+        for model in ("G20", "O6"):
+            self.mappers[model].reset(self.latest_driver_state[model])
+            self.enabled[model] = True
+        response.success = True
+        response.message = "G20/O6均已使能"
+        return response
+
+    def _disable_all(self, reason: str) -> None:
+        was_enabled = any(self.enabled.values())
+        self.enabled = {"G20": False, "O6": False}
+        self.last_fault = reason
+        if was_enabled:
+            self.get_logger().error(f"实机控制已全部撤销: {reason}")
+
+    def _estop_callback(
+        self, request: Trigger.Request, response: Trigger.Response
+    ) -> Trigger.Response:
+        del request
+        self._disable_all("软件急停")
+        response.success = True
+        response.message = "软件急停已触发；命令保持在最后值，需重新显式使能"
+        return response
+
+    def _watchdog(self) -> None:
+        now = time.monotonic()
+        disable_all, timed_out_models = evaluate_timeouts(
+            now=now,
+            enabled=self.enabled,
+            last_glove_time=self.last_glove_time,
+            driver_state_time=self.driver_state_time,
+            driver_state_valid=self.driver_state_valid,
+            glove_timeout=self.glove_timeout,
+            driver_timeout=self.driver_timeout,
+        )
+        if disable_all:
+            self._disable_all(f"FFG超过{self.glove_timeout:.2f}s无新帧")
+        for model in timed_out_models:
+            self.enabled[model] = False
+            self.last_fault = f"{model}驱动状态超过{self.driver_timeout:.2f}s未更新"
+            self.get_logger().error(
+                f"{model}实机控制已撤销，另一型号不受影响: {self.last_fault}"
+            )
+        if now - self.last_status_publish >= 1.0:
+            self._publish_status(now)
+            self.last_status_publish = now
+
+    def _publish_status(self, now: float) -> None:
+        latency_p95 = (
+            float(np.percentile(np.asarray(self.latencies_ms), 95))
+            if self.latencies_ms
+            else None
+        )
+        profile_hashes = {
+            "glove": (
+                self.glove_profile.get("_profile_sha256")
+                if self.glove_profile
+                else None
+            ),
+            **{
+                model.lower(): (
+                    self.robot_profiles[model].get("_profile_sha256")
+                    if model in self.robot_profiles
+                    else None
+                )
+                for model in ("G20", "O6")
+            },
+        }
+        payload = {
+            "timestamp_ns": int(self.get_clock().now().nanoseconds),
+            "sequence": self.sequence,
+            "calibration": "provisional",
+            "q_gt": None,
+            "input_mode": self.input_mode,
+            "glove_age_s": (
+                None
+                if self.last_glove_time <= 0.0
+                else round(now - self.last_glove_time, 4)
+            ),
+            "enabled": {key.lower(): value for key, value in self.enabled.items()},
+            "profile_approved": {
+                "glove": bool(
+                    self.glove_profile
+                    and self.glove_profile.get("approved_for_runtime", False)
+                ),
+                **{
+                    model.lower(): bool(
+                        model in self.robot_profiles
+                        and self.robot_profiles[model].get(
+                            "approved_for_control", False
+                        )
+                    )
+                    for model in ("G20", "O6")
+                },
+            },
+            "mapping_mode": {
+                model.lower(): (
+                    self.mappers[model].mapping_mode
+                    if model in self.mappers
+                    else None
+                )
+                for model in ("G20", "O6")
+            },
+            "input_filter": {
+                "enabled": self.input_filter_enabled,
+                "type": "vector_kalman_v1",
+                "process_variance": self.input_filter_process_variance,
+                "measurement_variance": (
+                    self.input_filter_measurement_variance
+                ),
+                "reset_gap_s": self.input_filter_reset_gap,
+            },
+            "command_filter": {
+                model.lower(): (
+                    self.mappers[model].command_filter_config
+                    if model in self.mappers
+                    else None
+                )
+                for model in ("G20", "O6")
+            },
+            "simulation_mapping_mode": {
+                model.lower(): (
+                    self.mappers[model].simulation_mapping_mode
+                    if model in self.mappers
+                    else None
+                )
+                for model in ("G20", "O6")
+            },
+            "anchor_weights": {
+                model.lower(): [
+                    {
+                        "pose": pose,
+                        "weight": round(weight, 6),
+                    }
+                    for pose, weight in self.last_anchor_weights[model][:5]
+                ]
+                for model in ("G20", "O6")
+            },
+            "wear_check_valid": self.wear_check_valid,
+            "driver_state_age_s": {
+                model.lower(): (
+                    None
+                    if self.driver_state_time[model] <= 0.0
+                    else round(now - self.driver_state_time[model], 4)
+                )
+                for model in ("G20", "O6")
+            },
+            "profile_sha256": profile_hashes,
+            "urdf_sha256": {
+                model.lower(): (
+                    self.robot_profiles[model]["simulation"]["urdf_sha256"]
+                    if model in self.robot_profiles
+                    else None
+                )
+                for model in ("G20", "O6")
+            },
+            "profile_errors": self.profile_errors,
+            "latency_p95_ms": latency_p95,
+            "last_fault": self.last_fault or None,
+        }
+        message = String()
+        message.data = json.dumps(payload, ensure_ascii=False, sort_keys=True)
+        self.status_publisher.publish(message)
+
+    def destroy_node(self) -> bool:
+        if self.serial_reader is not None:
+            self.serial_reader.stop()
+            self.serial_reader = None
+        return super().destroy_node()
+
+
+def main(args: list[str] | None = None) -> None:
+    rclpy.init(args=args)
+    node: FFGDualRetargetNode | None = None
+    try:
+        node = FFGDualRetargetNode()
+        rclpy.spin(node)
+    except KeyboardInterrupt:
+        pass
+    finally:
+        if node is not None:
+            node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    main()

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/profiles.py

@@ -0,0 +1,571 @@
+"""Strict JSON profile loading and validation for linkerforce_v2."""
+
+from __future__ import annotations
+
+import hashlib
+import json
+import math
+from pathlib import Path
+from typing import Any, Mapping, Sequence
+
+from .constants import (
+    BASE_INTENT_NAMES,
+    FFG_JOINT_NAMES,
+    G20_GESTURES,
+    GLOVE_DYNAMIC_GESTURES,
+    GLOVE_STATIC_GESTURES,
+    MODEL_COMMAND_LENGTHS,
+    O6_GESTURES,
+    PINCH_ROLES,
+    SCHEMA_VERSION,
+)
+
+
+class ProfileError(ValueError):
+    """Raised when a calibration profile is missing or internally inconsistent."""
+
+
+def _finite_scalar(value: Any, *, field: str) -> float:
+    try:
+        result = float(value)
+    except (TypeError, ValueError) as exc:
+        raise ProfileError(f"{field} must be numeric") from exc
+    if not math.isfinite(result):
+        raise ProfileError(f"{field} must be finite")
+    return result
+
+
+def _finite_vector(
+    value: Any,
+    *,
+    length: int,
+    field: str,
+    minimum: float | None = None,
+    maximum: float | None = None,
+) -> list[float]:
+    if not isinstance(value, Sequence) or isinstance(value, (str, bytes)):
+        raise ProfileError(f"{field} must be an array")
+    if len(value) != length:
+        raise ProfileError(f"{field} must contain {length} values, got {len(value)}")
+    try:
+        result = [float(item) for item in value]
+    except (TypeError, ValueError) as exc:
+        raise ProfileError(f"{field} contains a non-numeric value") from exc
+    if not all(math.isfinite(item) for item in result):
+        raise ProfileError(f"{field} contains a non-finite value")
+    if minimum is not None and any(item < minimum for item in result):
+        raise ProfileError(f"{field} contains a value below {minimum}")
+    if maximum is not None and any(item > maximum for item in result):
+        raise ProfileError(f"{field} contains a value above {maximum}")
+    return result
+
+
+def _load_json(path: str | Path) -> dict[str, Any]:
+    resolved = Path(path).expanduser().resolve()
+    if not resolved.is_file():
+        raise ProfileError(f"profile does not exist: {resolved}")
+    try:
+        data = json.loads(resolved.read_text(encoding="utf-8"))
+    except (OSError, json.JSONDecodeError) as exc:
+        raise ProfileError(f"cannot read profile {resolved}: {exc}") from exc
+    if not isinstance(data, dict):
+        raise ProfileError(f"profile root must be an object: {resolved}")
+    data["_profile_path"] = str(resolved)
+    data["_profile_sha256"] = profile_sha256(data)
+    return data
+
+
+def profile_sha256(profile: Mapping[str, Any]) -> str:
+    """Hash only persistent profile content, excluding loader metadata."""
+
+    persistent = {
+        key: value for key, value in profile.items() if not key.startswith("_profile_")
+    }
+    payload = json.dumps(
+        persistent, ensure_ascii=False, sort_keys=True, separators=(",", ":")
+    ).encode("utf-8")
+    return hashlib.sha256(payload).hexdigest()
+
+
+def save_profile(profile: Mapping[str, Any], path: str | Path) -> Path:
+    """Atomically save a profile as deterministic UTF-8 JSON."""
+
+    resolved = Path(path).expanduser().resolve()
+    resolved.parent.mkdir(parents=True, exist_ok=True)
+    persistent = {
+        key: value for key, value in profile.items() if not key.startswith("_profile_")
+    }
+    temporary = resolved.with_suffix(resolved.suffix + ".tmp")
+    temporary.write_text(
+        json.dumps(persistent, ensure_ascii=False, indent=2, sort_keys=True) + "\n",
+        encoding="utf-8",
+    )
+    temporary.replace(resolved)
+    return resolved
+
+
+def load_glove_profile(path: str | Path) -> dict[str, Any]:
+    """Load and validate an FFG glove calibration profile."""
+
+    profile = _load_json(path)
+    if profile.get("schema_version") != SCHEMA_VERSION:
+        raise ProfileError("unsupported glove profile schema_version")
+    if profile.get("profile_kind", profile.get("kind")) != "ffg_glove":
+        raise ProfileError("profile_kind must be ffg_glove")
+    if profile.get("calibration") != "provisional":
+        raise ProfileError("glove profile calibration must be provisional")
+    glove = profile.get("glove")
+    if not isinstance(glove, dict) or glove.get("side") != "left":
+        raise ProfileError("glove profile must describe a left glove")
+    joint_names = profile.get("joint_names")
+    if list(joint_names or []) != list(FFG_JOINT_NAMES):
+        raise ProfileError("glove profile joint_names do not match the FFG v2 layout")
+    poses = profile.get("poses")
+    if not isinstance(poses, dict):
+        raise ProfileError("glove profile poses must be an object")
+    missing_poses = set(GLOVE_STATIC_GESTURES) - set(poses)
+    if missing_poses:
+        raise ProfileError(
+            f"glove profile is missing poses: {sorted(missing_poses)}"
+        )
+    for name in GLOVE_STATIC_GESTURES:
+        summary = poses[name]
+        if not isinstance(summary, dict):
+            raise ProfileError(f"poses.{name} must be an object")
+        try:
+            count = int(summary.get("sample_count", 0))
+        except (TypeError, ValueError) as exc:
+            raise ProfileError(f"poses.{name}.sample_count is invalid") from exc
+        raw_frames = summary.get("raw_frames")
+        if not isinstance(raw_frames, list) or len(raw_frames) != count:
+            raise ProfileError(f"poses.{name} raw frame count is inconsistent")
+        for frame_index, frame in enumerate(raw_frames):
+            _finite_vector(
+                frame,
+                length=21,
+                field=f"poses.{name}.raw_frames[{frame_index}]",
+            )
+        _finite_vector(summary.get("median"), length=21, field=f"{name}.median")
+        _finite_vector(summary.get("mad"), length=21, field=f"{name}.mad")
+
+    features = profile.get("features")
+    if not isinstance(features, dict):
+        raise ProfileError("glove profile features must be an object")
+    missing = set(BASE_INTENT_NAMES) - set(features)
+    if missing:
+        raise ProfileError(f"glove profile is missing features: {sorted(missing)}")
+    for name in BASE_INTENT_NAMES:
+        spec = features[name]
+        if not isinstance(spec, dict):
+            raise ProfileError(f"features.{name} must be an object")
+        indices = spec.get("indices")
+        if not isinstance(indices, list) or not indices:
+            raise ProfileError(f"features.{name}.indices must be non-empty")
+        if any(not isinstance(index, int) or index < 0 or index >= 21 for index in indices):
+            raise ProfileError(f"features.{name}.indices contains an invalid index")
+        weights = _finite_vector(
+            spec.get("weights"), length=len(indices), field=f"{name}.weights"
+        )
+        if any(weight < 0.0 for weight in weights) or not math.isclose(
+            sum(weights), 1.0, abs_tol=1e-6
+        ):
+            raise ProfileError(f"features.{name}.weights must be normalized")
+        _finite_vector(spec.get("low"), length=len(indices), field=f"{name}.low")
+        _finite_vector(spec.get("high"), length=len(indices), field=f"{name}.high")
+
+    pinch = profile.get("pinch_anchors")
+    if not isinstance(pinch, dict) or set(pinch) != set(PINCH_ROLES):
+        raise ProfileError("glove profile must contain four pinch_anchors")
+    for role, spec in pinch.items():
+        if not isinstance(spec, dict):
+            raise ProfileError(f"pinch_anchors.{role} must be an object")
+        indices = spec.get("indices")
+        if not isinstance(indices, list) or not indices:
+            raise ProfileError(f"pinch_anchors.{role}.indices must be non-empty")
+        _finite_vector(spec.get("center"), length=len(indices), field=f"{role}.center")
+        scale = _finite_vector(
+            spec.get("scale"), length=len(indices), field=f"{role}.scale"
+        )
+        if any(item <= 0.0 for item in scale):
+            raise ProfileError(f"pinch_anchors.{role}.scale must be positive")
+        if _finite_scalar(
+            spec.get("open_distance", 0.0),
+            field=f"pinch_anchors.{role}.open_distance",
+        ) <= 0.0:
+            raise ProfileError(f"pinch_anchors.{role}.open_distance must be positive")
+    if not isinstance(profile.get("approved_for_runtime"), bool):
+        raise ProfileError("approved_for_runtime must be boolean")
+    if profile["approved_for_runtime"]:
+        def validate_repeats(summary: Mapping[str, Any], field: str) -> None:
+            repeats = summary.get("repeats")
+            if not isinstance(repeats, list) or len(repeats) != 3:
+                raise ProfileError(
+                    f"approved glove profile requires exactly three repeats for {field}"
+                )
+            repeat_total = 0
+            for repeat_index, repeat in enumerate(repeats):
+                if not isinstance(repeat, dict):
+                    raise ProfileError(f"{field}.repeats must contain objects")
+                try:
+                    repeat_count = int(repeat.get("sample_count", 0))
+                except (TypeError, ValueError) as exc:
+                    raise ProfileError(
+                        f"{field}.repeats[{repeat_index}] count is invalid"
+                    ) from exc
+                if repeat_count < 50:
+                    raise ProfileError(
+                        f"{field}.repeats[{repeat_index}] requires 50 frames"
+                    )
+                repeat_total += repeat_count
+                repeat_frames = repeat.get("raw_frames")
+                if (
+                    not isinstance(repeat_frames, list)
+                    or len(repeat_frames) != repeat_count
+                ):
+                    raise ProfileError(
+                        f"{field}.repeats[{repeat_index}] frames are inconsistent"
+                    )
+                for frame_index, frame in enumerate(repeat_frames):
+                    _finite_vector(
+                        frame,
+                        length=21,
+                        field=(
+                            f"{field}.repeats[{repeat_index}]"
+                            f".raw_frames[{frame_index}]"
+                        ),
+                    )
+                _finite_vector(
+                    repeat.get("median"),
+                    length=21,
+                    field=f"{field}.repeats[{repeat_index}].median",
+                )
+                _finite_vector(
+                    repeat.get("mad"),
+                    length=21,
+                    field=f"{field}.repeats[{repeat_index}].mad",
+                )
+            if repeat_total != int(summary.get("sample_count", 0)):
+                raise ProfileError(
+                    f"{field} aggregate count does not match its repeats"
+                )
+
+        for name in GLOVE_STATIC_GESTURES:
+            validate_repeats(poses[name], f"poses.{name}")
+        dynamic = profile.get("dynamic_captures")
+        if not isinstance(dynamic, dict) or not set(
+            GLOVE_DYNAMIC_GESTURES
+        ).issubset(dynamic):
+            raise ProfileError(
+                "approved glove profile requires every dynamic sweep"
+            )
+        for name in GLOVE_DYNAMIC_GESTURES:
+            validate_repeats(
+                dynamic[name],
+                f"dynamic_captures.{name}",
+            )
+    return profile
+
+
+def load_robot_profile(
+    path: str | Path, expected_model: str | None = None
+) -> dict[str, Any]:
+    """Load and validate a G20 or O6 single-device profile."""
+
+    profile = _load_json(path)
+    if profile.get("schema_version") != SCHEMA_VERSION:
+        raise ProfileError("unsupported robot profile schema_version")
+    if profile.get("profile_kind", profile.get("kind")) != "robot_hand":
+        raise ProfileError("profile_kind must be robot_hand")
+    model = str(profile.get("model", "")).upper()
+    if model not in MODEL_COMMAND_LENGTHS:
+        raise ProfileError(f"unsupported robot model: {model}")
+    if expected_model and model != expected_model.upper():
+        raise ProfileError(
+            f"profile model {model} does not match expected {expected_model.upper()}"
+        )
+    if profile.get("side") != "left":
+        raise ProfileError("robot profile must describe a left hand")
+    if profile.get("calibration") != "provisional":
+        raise ProfileError("robot profile calibration must be provisional")
+    if not isinstance(profile.get("approved_for_control"), bool):
+        raise ProfileError("approved_for_control must be boolean")
+    mapping_strategy = profile.get(
+        "mapping_strategy",
+        "paired_continuous_v1",
+    )
+    if mapping_strategy != "paired_continuous_v1":
+        raise ProfileError(
+            f"unsupported robot mapping_strategy: {mapping_strategy}"
+        )
+    profile["mapping_strategy"] = mapping_strategy
+
+    length = MODEL_COMMAND_LENGTHS[model]
+    if profile.get("command_length") != length:
+        raise ProfileError(f"{model}.command_length must be {length}")
+    names = profile.get("command_names")
+    if not isinstance(names, list) or len(names) != length or len(set(names)) != length:
+        raise ProfileError(f"{model}.command_names must contain {length} unique names")
+    bounds = profile.get("command_bounds")
+    if not isinstance(bounds, list) or len(bounds) != length:
+        raise ProfileError(f"{model}.command_bounds must contain {length} pairs")
+    for index, pair in enumerate(bounds):
+        values = _finite_vector(pair, length=2, field=f"command_bounds[{index}]")
+        if values[0] < 0 or values[1] > 255 or values[0] > values[1]:
+            raise ProfileError(f"invalid command bounds at index {index}")
+        bounds[index] = values
+
+    poses = profile.get("poses")
+    if not isinstance(poses, dict) or "open_spread" not in poses or "fist" not in poses:
+        raise ProfileError("robot profile requires open_spread and fist poses")
+    required_gestures = set(G20_GESTURES if model == "G20" else O6_GESTURES)
+    missing_poses = required_gestures - set(poses)
+    if missing_poses:
+        raise ProfileError(f"{model} profile is missing poses: {sorted(missing_poses)}")
+    for pose_name, pose in poses.items():
+        if not isinstance(pose, dict):
+            raise ProfileError(f"poses.{pose_name} must be an object")
+        pose["cmd_u8"] = _finite_vector(
+            pose.get("cmd_u8"),
+            length=length,
+            field=f"poses.{pose_name}.cmd_u8",
+            minimum=0,
+            maximum=255,
+        )
+        if any(
+            value < bounds[index][0] or value > bounds[index][1]
+            for index, value in enumerate(pose["cmd_u8"])
+        ):
+            raise ProfileError(
+                f"poses.{pose_name}.cmd_u8 exceeds command_bounds"
+            )
+        pose_command_names = pose.get("command_names")
+        if pose_command_names is not None and list(pose_command_names) != list(names):
+            raise ProfileError(
+                f"poses.{pose_name}.command_names do not match the profile"
+            )
+        state = pose.get("state_u8", [])
+        state_names = pose.get("state_names", [])
+        if state:
+            pose["state_u8"] = _finite_vector(
+                state,
+                length=length,
+                field=f"poses.{pose_name}.state_u8",
+                minimum=0,
+                maximum=255,
+            )
+            if (
+                not isinstance(state_names, list)
+                or len(state_names) != length
+                or len(set(state_names)) != length
+            ):
+                raise ProfileError(
+                    f"poses.{pose_name}.state_names are invalid"
+                )
+        status = str(pose.get("status", ""))
+        if status not in {"exact", "approximate", "unsupported"}:
+            raise ProfileError(f"poses.{pose_name}.status is invalid")
+
+    channels = profile.get("channels")
+    if not isinstance(channels, list) or not channels:
+        raise ProfileError("robot profile channels must be non-empty")
+    seen_command_indices: set[int] = set()
+    seen_channel_names: set[str] = set()
+    for channel in channels:
+        if not isinstance(channel, dict):
+            raise ProfileError("each channel must be an object")
+        name = str(channel.get("name", ""))
+        index = channel.get("index")
+        if not name or name in seen_channel_names:
+            raise ProfileError(f"duplicate or empty channel name: {name}")
+        if not isinstance(index, int) or index < 0 or index >= length:
+            raise ProfileError(f"invalid command index for channel {name}")
+        if index in seen_command_indices:
+            raise ProfileError(f"duplicate command_index {index}")
+        if str(names[index]) != name:
+            raise ProfileError(
+                f"channel {name} does not match command_names[{index}]"
+            )
+        seen_channel_names.add(name)
+        seen_command_indices.add(index)
+        sources = channel.get("sources")
+        if not isinstance(sources, dict) or not sources:
+            raise ProfileError(f"channel {name} has no intent_sources")
+        if any(source not in BASE_INTENT_NAMES for source in sources):
+            raise ProfileError(f"channel {name} references an unknown intent")
+        try:
+            weights = [float(weight) for weight in sources.values()]
+        except (TypeError, ValueError) as exc:
+            raise ProfileError(
+                f"channel {name} has non-numeric source weights"
+            ) from exc
+        if not all(math.isfinite(weight) and weight >= 0.0 for weight in weights):
+            raise ProfileError(f"channel {name} has invalid source weights")
+        if not math.isclose(sum(weights), 1.0, abs_tol=1e-6):
+            raise ProfileError(f"channel {name} source weights must sum to one")
+        channel["sources"] = {
+            str(source): float(weight)
+            for source, weight in zip(sources, weights)
+        }
+        mapping_kind = channel.get("mapping")
+        if mapping_kind not in {"monotonic_piecewise", "piecewise"}:
+            raise ProfileError(f"channel {name} has an invalid mapping kind")
+        points = channel.get("points")
+        if not isinstance(points, list) or len(points) < 2:
+            raise ProfileError(f"channel {name} requires at least two command_points")
+        normalized_points = []
+        for point in points:
+            values = _finite_vector(point, length=2, field=f"{name}.command_point")
+            if values[0] < 0.0 or values[0] > 1.0 or values[1] < 0 or values[1] > 255:
+                raise ProfileError(f"channel {name} has an invalid command point")
+            if values[1] < bounds[index][0] or values[1] > bounds[index][1]:
+                raise ProfileError(f"channel {name} command point exceeds bounds")
+            normalized_points.append(values)
+        channel["points"] = normalized_points
+        points = normalized_points
+        ordered_points = sorted((float(point[0]), float(point[1])) for point in points)
+        distinct_x = {point[0] for point in ordered_points}
+        if len(distinct_x) < 2:
+            raise ProfileError(f"channel {name} needs two distinct activations")
+        deltas = [
+            right[1] - left[1]
+            for left, right in zip(ordered_points, ordered_points[1:])
+            if right[0] > left[0]
+        ]
+        if mapping_kind == "monotonic_piecewise" and deltas and not (
+            all(delta >= -1e-9 for delta in deltas)
+            or all(delta <= 1e-9 for delta in deltas)
+        ):
+            raise ProfileError(f"channel {name} command_points are not monotonic")
+    simulation = profile.get("simulation")
+    if not isinstance(simulation, dict):
+        raise ProfileError("simulation must be an object")
+    simulation_mapping_strategy = simulation.get(
+        "mapping_strategy",
+        "semantic_urdf_v1",
+    )
+    if simulation_mapping_strategy != "semantic_urdf_v1":
+        raise ProfileError(
+            "simulation.mapping_strategy must be semantic_urdf_v1"
+        )
+    simulation["mapping_strategy"] = simulation_mapping_strategy
+    urdf_hash = str(simulation.get("urdf_sha256", ""))
+    if len(urdf_hash) != 64 or any(
+        character not in "0123456789abcdef" for character in urdf_hash.lower()
+    ):
+        raise ProfileError("simulation.urdf_sha256 must be a SHA-256 hex digest")
+    sim_names = simulation.get("joint_names")
+    if (
+        not isinstance(sim_names, list)
+        or len(sim_names) != len(channels)
+        or len(set(sim_names)) != len(sim_names)
+    ):
+        raise ProfileError("simulation.joint_names must match the active channels")
+    if list(sim_names) != [str(channel["name"]) for channel in channels]:
+        raise ProfileError("simulation.joint_names must use channel order")
+    for key in ("open", "closed", "lower", "upper"):
+        simulation[key] = _finite_vector(
+            simulation.get(key), length=len(channels), field=f"simulation.{key}"
+        )
+    if any(
+        low > high
+        for low, high in zip(simulation["lower"], simulation["upper"])
+    ):
+        raise ProfileError("simulation lower limit exceeds upper limit")
+    for endpoint in ("open", "closed"):
+        if any(
+            value < low or value > high
+            for value, low, high in zip(
+                simulation[endpoint],
+                simulation["lower"],
+                simulation["upper"],
+            )
+        ):
+            raise ProfileError(f"simulation.{endpoint} exceeds URDF limits")
+
+    reserved = profile.get("reserved_channels", {})
+    if not isinstance(reserved, dict):
+        raise ProfileError("reserved_channels must be an object")
+    for raw_index, raw_value in reserved.items():
+        try:
+            index = int(raw_index)
+        except (TypeError, ValueError) as exc:
+            raise ProfileError("reserved channel index must be an integer") from exc
+        value = _finite_scalar(raw_value, field=f"reserved_channels.{raw_index}")
+        if index < 0 or index >= length or value < 0 or value > 255:
+            raise ProfileError("reserved_channels contains an invalid entry")
+        if float(bounds[index][0]) != value or float(bounds[index][1]) != value:
+            raise ProfileError("reserved channel bounds must be fixed to its value")
+    if seen_command_indices | {int(index) for index in reserved} != set(range(length)):
+        raise ProfileError("active and reserved channels do not cover every command")
+    filters = profile.get("filter")
+    if not isinstance(filters, dict):
+        raise ProfileError("filter must be an object")
+    mode = str(filters.get("mode", "acceleration_limited")).strip().lower()
+    if mode not in {"passthrough", "ema", "acceleration_limited"}:
+        raise ProfileError(
+            "filter.mode must be passthrough, ema or acceleration_limited"
+        )
+    alpha = _finite_scalar(
+        filters.get("ema_alpha", -1.0), field="filter.ema_alpha"
+    )
+    max_step = _finite_scalar(
+        filters.get("max_step_u8", -1.0), field="filter.max_step_u8"
+    )
+    max_acceleration = _finite_scalar(
+        filters.get(
+            "max_acceleration_u8_per_frame2",
+            max_step,
+        ),
+        field="filter.max_acceleration_u8_per_frame2",
+    )
+    deadband = _finite_scalar(
+        filters.get("deadband_u8", -1.0), field="filter.deadband_u8"
+    )
+    if (
+        not 0.0 < alpha <= 1.0
+        or max_step <= 0.0
+        or not 0.0 < max_acceleration <= max_step
+        or deadband < 0.0
+    ):
+        raise ProfileError("filter parameters are invalid")
+    filters["mode"] = mode
+    filters["ema_alpha"] = alpha
+    filters["max_step_u8"] = max_step
+    filters["max_acceleration_u8_per_frame2"] = max_acceleration
+    filters["deadband_u8"] = deadband
+    if profile.get("approved_for_control", False):
+        device = profile.get("device")
+        if not isinstance(device, dict):
+            raise ProfileError("approved profile requires device metadata")
+        if not str(device.get("serial_number", "")).strip():
+            raise ProfileError("approved profile requires a serial_number")
+        if not str(device.get("can_interface", "")).strip():
+            raise ProfileError("approved profile requires a CAN interface")
+        metadata = profile.get("metadata")
+        if (
+            not isinstance(metadata, dict)
+            or not metadata.get("operator_confirmed")
+            or not str(metadata.get("operator", "")).strip()
+        ):
+            raise ProfileError(
+                "approved profile requires a named human confirmation"
+            )
+        for pose_name in required_gestures:
+            pose = poses[pose_name]
+            if not pose.get("confirmed", False):
+                raise ProfileError(
+                    f"approved profile pose {pose_name} is not confirmed"
+                )
+            if list(pose.get("command_names", ())) != list(names):
+                raise ProfileError(
+                    f"approved profile pose {pose_name} lacks command names"
+                )
+            if len(pose.get("state_u8", ())) != length:
+                raise ProfileError(
+                    f"approved profile pose {pose_name} lacks device state"
+                )
+            if list(pose.get("state_names", ())) != list(names):
+                raise ProfileError(
+                    f"approved profile pose {pose_name} lacks named device state"
+                )
+    return profile

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/quality.py

@@ -0,0 +1,475 @@
+"""Offline accuracy and smoothness checks for paired retarget profiles."""
+
+from __future__ import annotations
+
+import argparse
+import json
+from pathlib import Path
+from typing import Any, Mapping, Sequence
+
+import numpy as np
+
+from .constants import PINCH_ROLES
+from .mapping import (
+    CommandFilter,
+    HandIntentExtractor,
+    RobotMapper,
+)
+from .profiles import load_glove_profile, load_robot_profile
+
+
+def _active_indices(robot_profile: Mapping[str, Any]) -> tuple[int, ...]:
+    return tuple(int(channel["index"]) for channel in robot_profile["channels"])
+
+
+def _pose_indices(
+    robot_profile: Mapping[str, Any],
+    pose_name: str,
+) -> tuple[int, ...]:
+    channels = robot_profile["channels"]
+    if pose_name.startswith("pinch_"):
+        role = pose_name.removeprefix("pinch_")
+        prefixes = ("thumb_", f"{role}_")
+        return tuple(
+            int(channel["index"])
+            for channel in channels
+            if str(channel["name"]).startswith(prefixes)
+        )
+    if pose_name in {"thumb_max_out", "thumb_across"}:
+        return tuple(
+            int(channel["index"])
+            for channel in channels
+            if str(channel["name"]).startswith("thumb_")
+        )
+    if pose_name == "fingers_together":
+        return tuple(
+            int(channel["index"])
+            for channel in channels
+            if str(channel["name"]).endswith("_mcp_roll")
+        )
+    if pose_name in {"tabletop", "hook"}:
+        return tuple(
+            int(channel["index"])
+            for channel in channels
+            if not str(channel["name"]).startswith("thumb_")
+            and not str(channel["name"]).endswith("_mcp_roll")
+        )
+    if pose_name == "fist":
+        return tuple(
+            int(channel["index"])
+            for channel in channels
+            if not str(channel["name"]).endswith("_mcp_roll")
+        )
+    return _active_indices(robot_profile)
+
+
+def _dynamic_target_indices(
+    robot_profile: Mapping[str, Any],
+    capture_name: str,
+) -> tuple[int, ...]:
+    channels = robot_profile["channels"]
+    if capture_name == "finger_splay_sweep":
+        return tuple(
+            int(channel["index"])
+            for channel in channels
+            if str(channel["name"]).endswith("_mcp_roll")
+        )
+    if capture_name.startswith("thumb_"):
+        return tuple(
+            int(channel["index"])
+            for channel in channels
+            if str(channel["name"]).startswith("thumb_")
+        )
+    role = capture_name.removesuffix("_flex_sweep")
+    return tuple(
+        int(channel["index"])
+        for channel in channels
+        if str(channel["name"]).startswith(f"{role}_")
+        and not str(channel["name"]).endswith("_mcp_roll")
+    )
+
+
+def _repeat_groups(summary: Mapping[str, Any]) -> list[Sequence[Sequence[float]]]:
+    repeats = summary.get("repeats")
+    if isinstance(repeats, list) and repeats:
+        return [
+            repeat.get("raw_frames", [])
+            for repeat in repeats
+            if isinstance(repeat, Mapping)
+        ]
+    return [summary.get("raw_frames", [])]
+
+
+def evaluate_mapping_quality(
+    glove_profile: Mapping[str, Any],
+    robot_profile: Mapping[str, Any],
+) -> dict[str, Any]:
+    """Replay profile captures without touching ROS or physical hardware."""
+
+    extractor = HandIntentExtractor(glove_profile)
+    mapper = RobotMapper(
+        robot_profile,
+        glove_profile,
+        command_filter_config={
+            "mode": "passthrough",
+            "ema_alpha": 1.0,
+            "max_step_u8": 255.0,
+            "max_acceleration_u8_per_frame2": 255.0,
+            "deadband_u8": 0.0,
+        },
+    )
+    static: dict[str, Any] = {}
+    hard_failures: list[str] = []
+    warnings: list[str] = []
+    for pose_name, glove_pose in glove_profile["poses"].items():
+        if pose_name not in robot_profile["poses"]:
+            continue
+        indices = _pose_indices(robot_profile, str(pose_name))
+        if not indices:
+            continue
+        intent = extractor.extract(glove_pose["median"])
+        command, _, gates = mapper._raw_map(intent)
+        target = np.asarray(
+            robot_profile["poses"][pose_name]["cmd_u8"],
+            dtype=float,
+        )
+        errors = np.abs(command[list(indices)] - target[list(indices)])
+        maximum = float(np.max(errors))
+        static[str(pose_name)] = {
+            "relevant_channel_count": len(indices),
+            "max_abs_error_u8": maximum,
+            "mean_abs_error_u8": float(np.mean(errors)),
+            "active_branches": {
+                str(name): float(value)
+                for name, value in gates.items()
+                if value > 0.0
+            },
+        }
+        if maximum > 5.0:
+            hard_failures.append(
+                f"{pose_name}有效通道复现误差{maximum:.1f} > 5"
+            )
+
+    pinch_confusion: dict[str, Any] = {}
+    for target_role in PINCH_ROLES:
+        pose_name = f"pinch_{target_role}"
+        intent = extractor.extract(
+            glove_profile["poses"][pose_name]["median"]
+        )
+        scores = {
+            role: float(intent[f"pinch_{role}"])
+            for role in PINCH_ROLES
+        }
+        gates = mapper._pinch_gates(intent)
+        winner = max(scores, key=scores.get)
+        pinch_confusion[target_role] = {
+            "winner": winner,
+            "scores": scores,
+            "gates": gates,
+        }
+        if winner != target_role or gates[target_role] < 0.95:
+            hard_failures.append(
+                f"{pose_name}竞争识别失败: winner={winner}, "
+                f"gate={gates[target_role]:.3f}"
+            )
+
+    active = _active_indices(robot_profile)
+    dynamic: dict[str, Any] = {}
+    all_steps: list[np.ndarray] = []
+    all_unchanged: list[np.ndarray] = []
+    all_execution_steps: list[np.ndarray] = []
+    all_execution_unchanged: list[np.ndarray] = []
+    execution_filter_config = dict(robot_profile["filter"])
+    for capture_name, summary in glove_profile.get(
+        "dynamic_captures", {}
+    ).items():
+        if not isinstance(summary, Mapping):
+            continue
+        target_indices = _dynamic_target_indices(
+            robot_profile,
+            str(capture_name),
+        )
+        off_indices = tuple(
+            index
+            for index in active
+            if index not in set(target_indices)
+        )
+        repeats: list[dict[str, float]] = []
+        for frames in _repeat_groups(summary):
+            values = np.asarray(frames, dtype=float)
+            if values.ndim != 2 or values.shape[0] < 2:
+                continue
+            commands = []
+            semantic_rows = []
+            for frame in values:
+                # Runtime defaults to the legacy left-G20 timing path: use
+                # the latest glove frame directly and let the motor firmware
+                # interpolate the calibrated target.
+                intent = extractor.extract(frame)
+                semantic_rows.append(intent)
+                commands.append(mapper._raw_map(intent)[0])
+            command_array = np.asarray(commands, dtype=float)
+            active_command = command_array[:, list(active)]
+            steps = np.abs(np.diff(active_command, axis=0))
+            unchanged = (
+                np.max(
+                    np.abs(
+                        np.diff(np.rint(active_command), axis=0)
+                    ),
+                    axis=1,
+                )
+                == 0.0
+            )
+            all_steps.append(steps.reshape(-1))
+            all_unchanged.append(unchanged)
+            execution_filter = CommandFilter(
+                command_array[0],
+                execution_filter_config,
+            )
+            execution_command = np.asarray(
+                [
+                    execution_filter.apply(command)
+                    for command in command_array
+                ],
+                dtype=float,
+            )[:, list(active)]
+            execution_steps = np.abs(
+                np.diff(execution_command, axis=0)
+            )
+            execution_unchanged = (
+                np.max(
+                    np.abs(
+                        np.diff(
+                            np.rint(execution_command),
+                            axis=0,
+                        )
+                    ),
+                    axis=1,
+                )
+                == 0.0
+            )
+            all_execution_steps.append(execution_steps.reshape(-1))
+            all_execution_unchanged.append(execution_unchanged)
+            target_span = (
+                float(
+                    np.max(
+                        np.ptp(
+                            command_array[:, list(target_indices)],
+                            axis=0,
+                        )
+                    )
+                )
+                if target_indices
+                else 0.0
+            )
+            off_span = (
+                float(
+                    np.max(
+                        np.ptp(
+                            command_array[:, list(off_indices)],
+                            axis=0,
+                        )
+                    )
+                )
+                if off_indices
+                else 0.0
+            )
+            repeats.append(
+                {
+                    "target_span_u8": target_span,
+                    "off_target_span_u8": off_span,
+                    "step_p95_u8": float(np.percentile(steps, 95)),
+                    "step_max_u8": float(np.max(steps)),
+                    "integer_unchanged_fraction": float(
+                        np.mean(unchanged)
+                    ),
+                    "execution_step_p95_u8": float(
+                        np.percentile(execution_steps, 95)
+                    ),
+                    "execution_step_max_u8": float(
+                        np.max(execution_steps)
+                    ),
+                    "execution_integer_unchanged_fraction": float(
+                        np.mean(execution_unchanged)
+                    ),
+                }
+            )
+        dynamic[str(capture_name)] = {"repeats": repeats}
+        if str(capture_name).endswith("_flex_sweep") and repeats:
+            median_target = float(
+                np.median([item["target_span_u8"] for item in repeats])
+            )
+            median_off = float(
+                np.median(
+                    [item["off_target_span_u8"] for item in repeats]
+                )
+            )
+            if median_off > max(15.0, 0.2 * median_target):
+                warnings.append(
+                    f"{capture_name}非目标通道跨度{median_off:.1f}偏大"
+                )
+        if capture_name == "finger_splay_sweep":
+            flex_names = (
+                "index_root",
+                "index_tip",
+                "middle_root",
+                "middle_tip",
+                "ring_root",
+                "ring_tip",
+                "pinky_root",
+                "pinky_tip",
+            )
+            flex_ranges = []
+            for frames in _repeat_groups(summary):
+                rows = [
+                    extractor.extract(frame)
+                    for frame in frames
+                ]
+                if rows:
+                    flex_ranges.append(
+                        max(
+                            float(
+                                np.ptp(
+                                    [row[name] for row in rows]
+                                )
+                            )
+                            for name in flex_names
+                        )
+                    )
+            if flex_ranges and float(np.median(flex_ranges)) > 0.5:
+                warnings.append(
+                    "finger_splay_sweep中检测到明显屈伸，建议重采四指开合轨迹"
+                )
+
+    flattened_steps = (
+        np.concatenate(all_steps) if all_steps else np.asarray([], dtype=float)
+    )
+    flattened_unchanged = (
+        np.concatenate(all_unchanged)
+        if all_unchanged
+        else np.asarray([], dtype=bool)
+    )
+    flattened_execution_steps = (
+        np.concatenate(all_execution_steps)
+        if all_execution_steps
+        else np.asarray([], dtype=float)
+    )
+    flattened_execution_unchanged = (
+        np.concatenate(all_execution_unchanged)
+        if all_execution_unchanged
+        else np.asarray([], dtype=bool)
+    )
+    return {
+        "kind": "linkerforce_v2_mapping_quality",
+        "mapping_mode": mapper.mapping_mode,
+        "model": str(robot_profile["model"]).upper(),
+        "static": static,
+        "pinch_confusion": pinch_confusion,
+        "dynamic": dynamic,
+        "summary": {
+            "static_max_abs_error_u8": max(
+                (
+                    float(item["max_abs_error_u8"])
+                    for item in static.values()
+                ),
+                default=0.0,
+            ),
+            "dynamic_step_p95_u8": (
+                float(np.percentile(flattened_steps, 95))
+                if flattened_steps.size
+                else None
+            ),
+            "dynamic_step_max_u8": (
+                float(np.max(flattened_steps))
+                if flattened_steps.size
+                else None
+            ),
+            "integer_unchanged_fraction": (
+                float(np.mean(flattened_unchanged))
+                if flattened_unchanged.size
+                else None
+            ),
+            "execution_filter": {
+                "mode": str(execution_filter_config["mode"]),
+                "ema_alpha": float(
+                    execution_filter_config["ema_alpha"]
+                ),
+                "max_step_u8": float(
+                    execution_filter_config["max_step_u8"]
+                ),
+                "deadband_u8": float(
+                    execution_filter_config["deadband_u8"]
+                ),
+            },
+            "execution_step_p95_u8": (
+                float(np.percentile(flattened_execution_steps, 95))
+                if flattened_execution_steps.size
+                else None
+            ),
+            "execution_step_max_u8": (
+                float(np.max(flattened_execution_steps))
+                if flattened_execution_steps.size
+                else None
+            ),
+            "execution_integer_unchanged_fraction": (
+                float(np.mean(flattened_execution_unchanged))
+                if flattened_execution_unchanged.size
+                else None
+            ),
+            "splay_compensation": {
+                name: list(coefficients)
+                for name, coefficients
+                in extractor.splay_compensation.items()
+            },
+            "hard_failures": hard_failures,
+            "warnings": warnings,
+            "passed": not hard_failures,
+        },
+    }
+
+
+def main(argv: list[str] | None = None) -> None:
+    parser = argparse.ArgumentParser(
+        description="离线检查FFG与机械手profile的准确性、串指和目标连续性"
+    )
+    parser.add_argument("--glove", required=True, help="FFG profile JSON")
+    parser.add_argument("--robot", required=True, help="G20/O6 profile JSON")
+    parser.add_argument("--model", choices=("G20", "O6"), required=True)
+    parser.add_argument("--output", help="可选的完整JSON报告路径")
+    parser.add_argument(
+        "--strict",
+        action="store_true",
+        help="硬错误存在时以退出码2结束",
+    )
+    args = parser.parse_args(argv)
+    glove = load_glove_profile(args.glove)
+    robot = load_robot_profile(args.robot, args.model)
+    report = evaluate_mapping_quality(glove, robot)
+    if args.output:
+        output = Path(args.output).expanduser().resolve()
+        output.parent.mkdir(parents=True, exist_ok=True)
+        temporary = output.with_suffix(output.suffix + ".tmp")
+        temporary.write_text(
+            json.dumps(report, ensure_ascii=False, indent=2, sort_keys=True)
+            + "\n",
+            encoding="utf-8",
+        )
+        temporary.replace(output)
+    summary = report["summary"]
+    print(
+        json.dumps(
+            {
+                "mapping_mode": report["mapping_mode"],
+                **summary,
+            },
+            ensure_ascii=False,
+            indent=2,
+            sort_keys=True,
+        )
+    )
+    if args.strict and not summary["passed"]:
+        raise SystemExit(2)
+
+
+if __name__ == "__main__":
+    main()

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/safety.py

@@ -0,0 +1,33 @@
+"""Pure safety decisions shared by runtime and tests."""
+
+from __future__ import annotations
+
+from typing import Mapping
+
+
+def evaluate_timeouts(
+    *,
+    now: float,
+    enabled: Mapping[str, bool],
+    last_glove_time: float,
+    driver_state_time: Mapping[str, float],
+    driver_state_valid: Mapping[str, bool],
+    glove_timeout: float,
+    driver_timeout: float,
+) -> tuple[bool, tuple[str, ...]]:
+    """Return (disable_all, individually_timed_out_models)."""
+
+    if any(enabled.values()) and (
+        last_glove_time <= 0.0 or now - last_glove_time > glove_timeout
+    ):
+        return True, ()
+    timed_out = tuple(
+        model
+        for model, is_enabled in enabled.items()
+        if is_enabled
+        and (
+            not bool(driver_state_valid.get(model, False))
+            or now - float(driver_state_time.get(model, 0.0)) > driver_timeout
+        )
+    )
+    return False, timed_out

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/session_manifest.py

@@ -0,0 +1,181 @@
+"""Create a traceable manifest for provisional rosbag2 data collection."""
+
+from __future__ import annotations
+
+import argparse
+from datetime import datetime, timezone
+import hashlib
+import json
+from pathlib import Path
+import shlex
+from typing import Sequence
+
+from .profiles import load_glove_profile, load_robot_profile, save_profile
+
+
+TOPICS = (
+    "/ffg/left/raw_joint_state",
+    "/retarget/left/hand_intent",
+    "/retarget/left/frame_meta",
+    "/retarget/g20/left/actuation_target",
+    "/retarget/o6/left/actuation_target",
+    "/retarget/g20/left/joint_target_nominal",
+    "/retarget/o6/left/joint_target_nominal",
+    "/retarget/g20/left/cmd_u8_preview",
+    "/retarget/o6/left/cmd_u8_preview",
+    "/g20/cb_left_hand_control_cmd",
+    "/o6/cb_left_hand_control_cmd",
+    "/g20/cb_left_hand_state",
+    "/o6/cb_left_hand_state",
+    "/sim/mujoco/g20/left/joint_state",
+    "/sim/mujoco/o6/left/joint_state",
+    "/sim/isaac/g20/left/joint_state",
+    "/sim/isaac/o6/left/joint_state",
+    "/ffg_dual_retarget/status",
+)
+
+
+def _sha256(path: str) -> str:
+    source = Path(path).expanduser().resolve()
+    digest = hashlib.sha256()
+    with source.open("rb") as handle:
+        for block in iter(lambda: handle.read(1024 * 1024), b""):
+            digest.update(block)
+    return digest.hexdigest()
+
+
+def _parser() -> argparse.ArgumentParser:
+    parser = argparse.ArgumentParser(description="生成provisional数采manifest")
+    parser.add_argument("--output", required=True)
+    parser.add_argument("--session-id", required=True)
+    parser.add_argument("--operator", required=True)
+    parser.add_argument("--glove-profile", required=True)
+    parser.add_argument("--wear-check", required=True)
+    parser.add_argument("--wear-check-max-age-hours", type=float, default=12.0)
+    parser.add_argument("--g20-profile", required=True)
+    parser.add_argument("--o6-profile", required=True)
+    parser.add_argument("--g20-urdf", required=True)
+    parser.add_argument("--o6-urdf", required=True)
+    parser.add_argument("--g20-firmware", default="unknown")
+    parser.add_argument("--o6-firmware", default="unknown")
+    parser.add_argument("--g20-serial-number", required=True)
+    parser.add_argument("--o6-serial-number", required=True)
+    parser.add_argument("--bag-output")
+    return parser
+
+
+def main(argv: Sequence[str] | None = None) -> int:
+    args = _parser().parse_args(argv)
+    bag_output = args.bag_output or f"bags/{args.session_id}"
+    glove_profile = load_glove_profile(args.glove_profile)
+    g20_profile = load_robot_profile(args.g20_profile, "G20")
+    o6_profile = load_robot_profile(args.o6_profile, "O6")
+    if not glove_profile["approved_for_runtime"]:
+        raise SystemExit("FFG profile不完整，禁止生成数采session")
+    if not g20_profile["approved_for_control"] or not o6_profile[
+        "approved_for_control"
+    ]:
+        raise SystemExit("G20/O6 profile未全部获批，禁止生成数采session")
+    wear_path = Path(args.wear_check).expanduser().resolve()
+    wear_check = json.loads(wear_path.read_text(encoding="utf-8"))
+    if (
+        wear_check.get("kind") != "ffg_wear_check"
+        or not wear_check.get("passed")
+        or wear_check.get("glove_profile_sha256")
+        != glove_profile["_profile_sha256"]
+    ):
+        raise SystemExit("快速佩戴检查失败或与FFG profile不匹配")
+    checked_at = datetime.fromisoformat(str(wear_check["checked_at"]))
+    if checked_at.tzinfo is None:
+        checked_at = checked_at.replace(tzinfo=timezone.utc)
+    wear_age_hours = (
+        datetime.now(timezone.utc) - checked_at.astimezone(timezone.utc)
+    ).total_seconds() / 3600.0
+    if (
+        wear_age_hours < 0.0
+        or wear_age_hours > args.wear_check_max_age_hours
+    ):
+        raise SystemExit("快速佩戴检查已过期")
+    expected_devices = {
+        "g20": (g20_profile, args.g20_serial_number, "can0"),
+        "o6": (o6_profile, args.o6_serial_number, "can1"),
+    }
+    for model, (profile, serial_number, can_interface) in expected_devices.items():
+        device = profile["device"]
+        if (
+            device["serial_number"] != serial_number
+            or device["can_interface"] != can_interface
+        ):
+            raise SystemExit(f"{model.upper()} profile的SN/CAN与session不匹配")
+        requested_firmware = getattr(args, f"{model}_firmware")
+        if (
+            requested_firmware != "unknown"
+            and requested_firmware != device["firmware"]
+        ):
+            raise SystemExit(f"{model.upper()}固件版本与profile不匹配")
+    urdf_paths = {"g20": args.g20_urdf, "o6": args.o6_urdf}
+    for model, path in urdf_paths.items():
+        if _sha256(path) != expected_devices[model][0]["simulation"]["urdf_sha256"]:
+            raise SystemExit(f"{model.upper()} URDF哈希与profile不匹配")
+    manifest = {
+        "schema_version": 1,
+        "session_id": args.session_id,
+        "created_at": datetime.now(timezone.utc).isoformat(),
+        "operator": args.operator,
+        "calibration": "provisional",
+        "q_gt": None,
+        "angle_accuracy_claim": False,
+        "profiles": {
+            "glove": {
+                "path": str(Path(args.glove_profile).expanduser().resolve()),
+                "sha256": glove_profile["_profile_sha256"],
+            },
+            "wear_check": {
+                "path": str(wear_path),
+                "sha256": _sha256(args.wear_check),
+                "checked_at": wear_check["checked_at"],
+            },
+            "g20": {
+                "path": str(Path(args.g20_profile).expanduser().resolve()),
+                "sha256": g20_profile["_profile_sha256"],
+            },
+            "o6": {
+                "path": str(Path(args.o6_profile).expanduser().resolve()),
+                "sha256": o6_profile["_profile_sha256"],
+            },
+        },
+        "urdf": {
+            "g20": {
+                "path": str(Path(args.g20_urdf).expanduser().resolve()),
+                "sha256": _sha256(args.g20_urdf),
+            },
+            "o6": {
+                "path": str(Path(args.o6_urdf).expanduser().resolve()),
+                "sha256": _sha256(args.o6_urdf),
+            },
+        },
+        "devices": {
+            "g20": {
+                "serial_number": args.g20_serial_number,
+                "firmware": g20_profile["device"]["firmware"],
+                "can_interface": "can0",
+            },
+            "o6": {
+                "serial_number": args.o6_serial_number,
+                "firmware": o6_profile["device"]["firmware"],
+                "can_interface": "can1",
+            },
+        },
+        "topics": list(TOPICS),
+        "bag_output": bag_output,
+    }
+    output = save_profile(manifest, args.output)
+    command = ["ros2", "bag", "record", "-o", bag_output, *TOPICS]
+    print(f"manifest已保存: {output}")
+    print("录制命令：")
+    print(shlex.join(command))
+    return 0
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/simulation.py

@@ -0,0 +1,60 @@
+"""Dependency-free joint-name contract used by MuJoCo and Isaac bridges."""
+
+from __future__ import annotations
+
+from typing import Mapping, Sequence
+
+import numpy as np
+
+
+class JointNameMismatch(ValueError):
+    """Raised before a simulator receives an ambiguously ordered target."""
+
+
+def build_name_mapping(
+    incoming_names: Sequence[str], simulator_joint_names: Sequence[str]
+) -> tuple[int, ...]:
+    """Return indices that reorder a named ROS target into simulator order."""
+
+    incoming = [str(name) for name in incoming_names]
+    expected = [str(name) for name in simulator_joint_names]
+    if not incoming or len(set(incoming)) != len(incoming):
+        raise JointNameMismatch("incoming target has empty or duplicate joint names")
+    if not expected or len(set(expected)) != len(expected):
+        raise JointNameMismatch("simulator model has empty or duplicate joint names")
+    incoming_set = set(incoming)
+    expected_set = set(expected)
+    missing = sorted(expected_set - incoming_set)
+    extra = sorted(incoming_set - expected_set)
+    if missing or extra:
+        raise JointNameMismatch(
+            f"joint-name mismatch: missing={missing}, extra={extra}"
+        )
+    lookup = {name: index for index, name in enumerate(incoming)}
+    return tuple(lookup[name] for name in expected)
+
+
+def reorder_named_target(
+    incoming_names: Sequence[str],
+    incoming_positions: Sequence[float],
+    simulator_joint_names: Sequence[str],
+    limits: Mapping[str, tuple[float, float]] | None = None,
+) -> np.ndarray:
+    """Validate, reorder and limit q_nominal before simulator position control."""
+
+    if len(incoming_names) != len(incoming_positions):
+        raise JointNameMismatch("target name/position lengths differ")
+    values = np.asarray(incoming_positions, dtype=float)
+    if not np.all(np.isfinite(values)):
+        raise JointNameMismatch("target contains a non-finite position")
+    mapping = build_name_mapping(incoming_names, simulator_joint_names)
+    ordered = values[list(mapping)].copy()
+    if limits:
+        for index, name in enumerate(simulator_joint_names):
+            if name not in limits:
+                raise JointNameMismatch(f"missing simulator limit for joint {name}")
+            lower, upper = limits[name]
+            if lower > upper:
+                raise JointNameMismatch(f"invalid simulator limit for joint {name}")
+            ordered[index] = np.clip(ordered[index], lower, upper)
+    return ordered

src/linkerhand_retarget/linkerhand_retarget/motion/linkerforce_v2/verify_robot_profile.py

@@ -0,0 +1,518 @@
+"""Safely replay and manually verify calibrated robot-hand profile poses."""
+
+from __future__ import annotations
+
+import argparse
+from datetime import datetime, timezone
+import math
+from pathlib import Path
+import threading
+import time
+from typing import Any, Mapping, Sequence
+
+import numpy as np
+import rclpy
+from rclpy.executors import SingleThreadedExecutor
+from rclpy.node import Node
+from rclpy.qos import HistoryPolicy, QoSProfile, ReliabilityPolicy
+from sensor_msgs.msg import JointState
+
+from .calibrate_robot import POSE_LABELS
+from .constants import MODEL_COMMAND_LENGTHS
+from .profiles import load_robot_profile, save_profile
+
+
+COMMAND_QOS = QoSProfile(
+    history=HistoryPolicy.KEEP_LAST,
+    depth=1,
+    reliability=ReliabilityPolicy.RELIABLE,
+)
+
+POSE_EXPECTATIONS = {
+    "open_spread": "五指伸直并自然分开；拇指处于自然张开位置",
+    "fingers_together": "四指伸直并拢；该姿势不评价拇指",
+    "tabletop": "四指根部弯曲、末端伸直；该姿势不评价拇指",
+    "hook": "四指根部基本伸直、末端弯曲；该姿势不评价拇指",
+    "fist": "四指根部和末端闭合，拇指自然横跨",
+    "thumb_max_out": "拇指最大外展，四指保持伸直",
+    "thumb_across": "拇指横跨掌心，四指保持伸直",
+    "pinch_index": "拇指只与食指捏合，其他三指不串动",
+    "pinch_middle": "拇指只与中指捏合，其他三指不串动",
+    "pinch_ring": "拇指只与无名指捏合，其他三指不串动",
+    "pinch_pinky": "拇指只与小指捏合，其他三指不串动",
+}
+
+
+def transition_commands(
+    start: Sequence[float],
+    target: Sequence[float],
+    *,
+    max_step_u8: float,
+) -> list[list[float]]:
+    """Return a linear transition whose per-channel step is bounded."""
+
+    start_values = np.asarray(start, dtype=float)
+    target_values = np.asarray(target, dtype=float)
+    if (
+        start_values.ndim != 1
+        or target_values.shape != start_values.shape
+        or not np.all(np.isfinite(start_values))
+        or not np.all(np.isfinite(target_values))
+    ):
+        raise ValueError("start and target must be equal-length finite vectors")
+    if not math.isfinite(max_step_u8) or max_step_u8 <= 0.0:
+        raise ValueError("max_step_u8 must be positive")
+    maximum_delta = float(np.max(np.abs(target_values - start_values)))
+    steps = max(1, int(math.ceil(maximum_delta / max_step_u8)))
+    return [
+        (
+            start_values
+            + (target_values - start_values) * (step / steps)
+        ).astype(float).tolist()
+        for step in range(1, steps + 1)
+    ]
+
+
+def state_error_summary(
+    command_names: Sequence[str],
+    target: Sequence[float],
+    state: Sequence[float],
+    *,
+    active_indices: Sequence[int],
+) -> dict[str, Any]:
+    """Summarize command/state disagreement without treating it as angle GT."""
+
+    target_values = np.asarray(target, dtype=float)
+    state_values = np.asarray(state, dtype=float)
+    indices = np.asarray(active_indices, dtype=int)
+    if (
+        target_values.shape != state_values.shape
+        or target_values.shape != (len(command_names),)
+        or indices.ndim != 1
+        or len(indices) < 1
+    ):
+        raise ValueError("invalid target/state vectors")
+    error = np.abs(target_values - state_values)
+    worst_indices = sorted(
+        (int(index) for index in indices),
+        key=lambda index: (-float(error[index]), str(command_names[index])),
+    )[:5]
+    return {
+        "mean_abs_error_u8": float(np.mean(error[indices])),
+        "max_abs_error_u8": float(np.max(error[indices])),
+        "worst_channels": [
+            {
+                "name": str(command_names[index]),
+                "target_u8": float(target_values[index]),
+                "state_u8": float(state_values[index]),
+                "abs_error_u8": float(error[index]),
+            }
+            for index in worst_indices
+        ],
+    }
+
+
+def verification_document(
+    profile: Mapping[str, Any],
+    *,
+    operator: str,
+    command_topic: str,
+    state_topic: str,
+    results: Mapping[str, Mapping[str, Any]],
+) -> dict[str, Any]:
+    """Build a traceable report separate from the calibration profile."""
+
+    return {
+        "schema_version": 1,
+        "kind": "robot_pose_verification",
+        "updated_at": datetime.now(timezone.utc).isoformat(),
+        "profile_path": str(profile["_profile_path"]),
+        "profile_sha256": str(profile["_profile_sha256"]),
+        "model": str(profile["model"]),
+        "side": str(profile["side"]),
+        "device": dict(profile.get("device", {})),
+        "operator": str(operator),
+        "command_topic": str(command_topic),
+        "state_topic": str(state_topic),
+        "state_is_angle_ground_truth": False,
+        "results": dict(results),
+    }
+
+
+class HandPoseVerifyNode(Node):
+    """Publish bounded pose transitions and receive named device state."""
+
+    def __init__(
+        self,
+        *,
+        model: str,
+        command_topic: str,
+        state_topic: str,
+        command_names: Sequence[str],
+    ) -> None:
+        super().__init__(f"{model.lower()}_pose_verify")
+        self.model = model
+        self.command_topic = command_topic
+        self.state_topic = state_topic
+        self.command_names = tuple(str(name) for name in command_names)
+        self.length = MODEL_COMMAND_LENGTHS[model]
+        self.latest_state: list[float] | None = None
+        self.latest_state_time = 0.0
+        self.command_publisher: Any | None = None
+        self.state_subscription = self.create_subscription(
+            JointState,
+            state_topic,
+            self._state_callback,
+            20,
+        )
+
+    def _state_callback(self, message: JointState) -> None:
+        names = [str(name) for name in message.name]
+        values = [float(value) for value in message.position]
+        if (
+            len(names) != self.length
+            or len(set(names)) != self.length
+            or set(names) != set(self.command_names)
+            or len(values) != self.length
+            or not np.all(np.isfinite(values))
+            or any(value < 0.0 or value > 255.0 for value in values)
+        ):
+            return
+        lookup = dict(zip(names, values))
+        self.latest_state = [lookup[name] for name in self.command_names]
+        self.latest_state_time = time.monotonic()
+
+    def competing_publishers(self) -> tuple[str, ...]:
+        competitors = set()
+        for info in self.get_publishers_info_by_topic(self.command_topic):
+            if info.node_name == self.get_name():
+                continue
+            namespace = str(info.node_namespace).rstrip("/")
+            competitors.add(f"{namespace}/{info.node_name}" or info.node_name)
+        return tuple(sorted(competitors))
+
+    def enable_command_publisher(self) -> None:
+        if self.command_publisher is None:
+            self.command_publisher = self.create_publisher(
+                JointState,
+                self.command_topic,
+                COMMAND_QOS,
+            )
+
+    def publish_command(self, values: Sequence[float]) -> None:
+        if self.command_publisher is None:
+            raise RuntimeError("command publisher is not enabled")
+        message = JointState()
+        message.header.stamp = self.get_clock().now().to_msg()
+        message.name = list(self.command_names)
+        message.position = [float(value) for value in values]
+        self.command_publisher.publish(message)
+
+
+def _parser() -> argparse.ArgumentParser:
+    parser = argparse.ArgumentParser(
+        description="低速平滑回放G20/O6 profile姿势并记录人工复核结果"
+    )
+    parser.add_argument("--profile", required=True)
+    parser.add_argument("--operator", required=True)
+    parser.add_argument("--topic-prefix")
+    parser.add_argument("--command-topic")
+    parser.add_argument("--state-topic")
+    parser.add_argument("--pose", help="只复核一个姿势；默认显示交互菜单")
+    parser.add_argument("--report")
+    parser.add_argument("--rate", type=float, default=30.0)
+    parser.add_argument("--max-step-u8", type=float, default=4.0)
+    parser.add_argument("--state-timeout", type=float, default=3.0)
+    parser.add_argument("--settle-seconds", type=float, default=0.7)
+    return parser
+
+
+def _report_path(profile_path: str, configured: str | None) -> Path:
+    if configured:
+        return Path(configured).expanduser().resolve()
+    path = Path(profile_path).expanduser().resolve()
+    return path.with_suffix(".verification.json")
+
+
+def _load_existing_results(
+    report_path: Path,
+    profile: Mapping[str, Any],
+) -> dict[str, dict[str, Any]]:
+    if not report_path.is_file():
+        return {}
+    try:
+        import json
+
+        report = json.loads(report_path.read_text(encoding="utf-8"))
+    except (OSError, ValueError):
+        return {}
+    if (
+        report.get("kind") != "robot_pose_verification"
+        or report.get("profile_sha256") != profile["_profile_sha256"]
+        or not isinstance(report.get("results"), dict)
+    ):
+        return {}
+    return {
+        str(name): dict(result)
+        for name, result in report["results"].items()
+        if isinstance(result, dict)
+    }
+
+
+def _wait_for_fresh_state(
+    node: HandPoseVerifyNode,
+    *,
+    newer_than: float,
+    timeout: float,
+) -> list[float]:
+    deadline = time.monotonic() + timeout
+    while rclpy.ok() and time.monotonic() < deadline:
+        if (
+            node.latest_state is not None
+            and node.latest_state_time > newer_than
+        ):
+            return list(node.latest_state)
+        time.sleep(0.02)
+    raise RuntimeError(
+        f"未在{timeout:g}秒内从{node.state_topic}收到新的有效状态"
+    )
+
+
+def _print_menu(
+    pose_names: Sequence[str],
+    results: Mapping[str, Mapping[str, Any]],
+) -> None:
+    print("\n可复核姿势：")
+    for index, pose_name in enumerate(pose_names, start=1):
+        result = results.get(pose_name)
+        if result is None:
+            marker = "未检查"
+        else:
+            marker = "通过" if result.get("visual_passed") else "未通过"
+        print(
+            f"  {index:2d}. {POSE_LABELS.get(pose_name, pose_name):12s}"
+            f" [{marker}]"
+        )
+    print("输入编号或姿势英文名；输入q退出。")
+
+
+def _select_pose(
+    entered: str,
+    pose_names: Sequence[str],
+) -> str | None:
+    value = entered.strip()
+    if not value:
+        return None
+    if value.isdigit():
+        index = int(value)
+        if 1 <= index <= len(pose_names):
+            return pose_names[index - 1]
+        return None
+    return value if value in pose_names else None
+
+
+def main(argv: Sequence[str] | None = None) -> int:
+    args, ros_args = _parser().parse_known_args(argv)
+    profile = load_robot_profile(args.profile)
+    model = str(profile["model"]).upper()
+    if not profile.get("approved_for_control", False):
+        raise SystemExit("profile未获实机控制批准，拒绝带电回放")
+    if args.rate <= 0.0 or args.rate > 60.0:
+        raise SystemExit("--rate必须在(0, 60]范围")
+    if args.max_step_u8 <= 0.0 or args.max_step_u8 > 8.0:
+        raise SystemExit("--max-step-u8必须在(0, 8]范围")
+    if args.state_timeout <= 0.0 or args.settle_seconds < 0.0:
+        raise SystemExit("状态超时必须为正数，稳定等待时间不能为负数")
+
+    lower = model.lower()
+    prefix = (args.topic_prefix or f"/{lower}").strip().rstrip("/")
+    if not prefix.startswith("/"):
+        prefix = "/" + prefix
+    command_topic = (
+        args.command_topic or f"{prefix}/cb_left_hand_control_cmd"
+    )
+    state_topic = args.state_topic or f"{prefix}/cb_left_hand_state"
+    report_path = _report_path(args.profile, args.report)
+    pose_names = [
+        name for name in POSE_LABELS if name in profile["poses"]
+    ]
+    if args.pose and args.pose not in pose_names:
+        raise SystemExit(
+            f"未知姿势{args.pose}；可选: {', '.join(pose_names)}"
+        )
+    active_indices = sorted(
+        int(channel["index"]) for channel in profile["channels"]
+    )
+    results = _load_existing_results(report_path, profile)
+
+    rclpy.init(args=ros_args)
+    node = HandPoseVerifyNode(
+        model=model,
+        command_topic=command_topic,
+        state_topic=state_topic,
+        command_names=profile["command_names"],
+    )
+    executor = SingleThreadedExecutor()
+    executor.add_node(node)
+    spin_thread = threading.Thread(target=executor.spin, daemon=True)
+    spin_thread.start()
+    try:
+        print(
+            f"{model} profile姿势复核\n"
+            f"profile: {profile['_profile_path']}\n"
+            f"sha256: {profile['_profile_sha256']}\n"
+            f"设备: {profile.get('device', {})}\n"
+            f"命令: {command_topic}\n状态: {state_topic}\n"
+            f"报告: {report_path}\n\n"
+            "安全要求：遥操节点和GUI必须停止或保持未启动；"
+            "驱动应使用低速、低力矩；带电时禁止手掰。"
+        )
+        time.sleep(0.7)
+        competitors = node.competing_publishers()
+        if competitors:
+            raise RuntimeError(
+                f"{command_topic}已有其他发布者: {', '.join(competitors)}；"
+                "请停止遥操和GUI后重试"
+            )
+        confirmation = input(
+            "确认周围安全、急停可用，并已设置低速低力矩。输入VERIFY继续："
+        ).strip()
+        if confirmation != "VERIFY":
+            print("未确认安全条件，已退出且未发送命令。")
+            return 1
+
+        node.enable_command_publisher()
+        _wait_for_fresh_state(
+            node,
+            newer_than=0.0,
+            timeout=args.state_timeout,
+        )
+        one_shot_pose = args.pose
+        while rclpy.ok():
+            if one_shot_pose:
+                pose_name = one_shot_pose
+            else:
+                _print_menu(pose_names, results)
+                entered = input("选择：").strip()
+                if entered.lower() == "q":
+                    break
+                pose_name = _select_pose(entered, pose_names)
+                if pose_name is None:
+                    print("无效选择。")
+                    continue
+
+            pose = profile["poses"][pose_name]
+            target = [float(value) for value in pose["cmd_u8"]]
+            print(
+                f"\n准备复核“{POSE_LABELS.get(pose_name, pose_name)}”"
+                f"（标定状态={pose['status']}）\n"
+                f"期望：{POSE_EXPECTATIONS.get(pose_name, '按标定定义目视检查')}"
+            )
+            move = input("输入MOVE开始平滑运动；输入其他内容取消：").strip()
+            if move != "MOVE":
+                print("已取消，未发送该姿势。")
+                if one_shot_pose:
+                    break
+                continue
+
+            competitors = node.competing_publishers()
+            if competitors:
+                raise RuntimeError(
+                    f"检测到新的命令发布者: {', '.join(competitors)}；立即停止复核"
+                )
+            state_request_time = time.monotonic()
+            start = _wait_for_fresh_state(
+                node,
+                newer_than=state_request_time,
+                timeout=args.state_timeout,
+            )
+            commands = transition_commands(
+                start,
+                target,
+                max_step_u8=args.max_step_u8,
+            )
+            print(
+                f"从当前状态分{len(commands)}步运动，"
+                f"预计{len(commands) / args.rate:.2f}秒……"
+            )
+            for index, command in enumerate(commands):
+                if index % 10 == 0:
+                    competitors = node.competing_publishers()
+                    if competitors:
+                        raise RuntimeError(
+                            "运动过程中检测到其他命令发布者，已停止发送"
+                        )
+                node.publish_command(command)
+                time.sleep(1.0 / args.rate)
+
+            last_publish_time = time.monotonic()
+            node.publish_command(target)
+            time.sleep(args.settle_seconds)
+            state = _wait_for_fresh_state(
+                node,
+                newer_than=last_publish_time,
+                timeout=args.state_timeout,
+            )
+            summary = state_error_summary(
+                profile["command_names"],
+                target,
+                state,
+                active_indices=active_indices,
+            )
+            print(
+                "状态回读（仅电机状态，不是真实角度GT）："
+                f"平均误差={summary['mean_abs_error_u8']:.2f}，"
+                f"最大误差={summary['max_abs_error_u8']:.2f}"
+            )
+            for item in summary["worst_channels"]:
+                print(
+                    f"  {item['name']}: 目标{item['target_u8']:.1f}，"
+                    f"状态{item['state_u8']:.1f}，"
+                    f"差{item['abs_error_u8']:.1f}"
+                )
+
+            while True:
+                verdict = input(
+                    "目视结果：p=通过，f=未通过，r=重放，s=跳过："
+                ).strip().lower()
+                if verdict in {"p", "f", "r", "s"}:
+                    break
+                print("请输入p/f/r/s。")
+            if verdict == "r":
+                continue
+            if verdict != "s":
+                note = input("备注（可直接回车）：").strip()
+                results[pose_name] = {
+                    "pose_label": POSE_LABELS.get(pose_name, pose_name),
+                    "profile_status": pose["status"],
+                    "verified_at": datetime.now(timezone.utc).isoformat(),
+                    "visual_passed": verdict == "p",
+                    "operator_note": note,
+                    "target_cmd_u8": target,
+                    "observed_state_u8": state,
+                    **summary,
+                }
+                save_profile(
+                    verification_document(
+                        profile,
+                        operator=args.operator,
+                        command_topic=command_topic,
+                        state_topic=state_topic,
+                        results=results,
+                    ),
+                    report_path,
+                )
+                print(f"复核结果已保存：{report_path}")
+            if one_shot_pose:
+                break
+        return 0
+    finally:
+        executor.shutdown()
+        spin_thread.join(timeout=1.0)
+        node.destroy_node()
+        if rclpy.ok():
+            rclpy.shutdown()
+
+
+if __name__ == "__main__":
+    raise SystemExit(main())

src/linkerhand_retarget/package.xml

@@ -12,6 +12,12 @@
   <test_depend>python3-pytest</test_depend>
   
   <exec_depend>rclpy</exec_depend>
+  <exec_depend>ament_index_python</exec_depend>
+  <exec_depend>sensor_msgs</exec_depend>
+  <exec_depend>std_msgs</exec_depend>
+  <exec_depend>std_srvs</exec_depend>
+  <exec_depend>python3-numpy</exec_depend>
+  <exec_depend>python3-serial</exec_depend>
   <export>
     <build_type>ament_python</build_type>
   </export>

src/linkerhand_retarget/resource/linkerforce_v2/README_zh.md

@@ -0,0 +1,374 @@
+# linkerforce_v2：FFG左手套同时控制G20和O6
+
+该链路与旧 `handretarget` 并存。它只使用一只左手FFG，不创建右手套读取器；运行时链路为：
+
+```text
+FFG 21维原始弧度
+  → hand_intent（模型无关、0～1）
+  → G20/O6 actuation_target（按名称）
+       ├─→ 解剖通道曲线＋局部捏合/握持校正 → cmd_u8（实机）
+       └─→ 独立语义—URDF映射 → q_nominal（仿真）
+```
+
+种子profile只给标定GUI提供安全的滑块初值，均为
+`approved_for_control=false`。它们不能使能实机，也不会成为运行时静默回退。
+
+### 分解式连续映射原则
+
+运行时不会把整只手吸附或锁定到“最相似手势”。新版将普通关节曲线、握持协调和
+捏合修正拆开：
+
+- 每根普通手指的根部、末端和侧摆只读取该手指的解剖语义，拇指运动不会再改变
+  中指、无名指或小指命令；
+- 四指屈伸使用张手、桌面、钩拳和握拳构成的标定面连续解耦根部与末端传感器串扰；
+  这些锚点定义语义坐标，不会在命令曲线中形成“先不动、随后突变”的平台区；
+- 张手、并拢、桌面、钩拳和握拳只约束其实际定义的通道，GUI中非目标滑块的残留值
+  不再污染映射；
+- 四种捏合先按当前手套profile做竞争判别，只有证据充分且领先其他候选的分支生效；
+- 捏合分支只连续修正拇指和目标手指，竞争不明确时平滑退回普通逐关节映射；
+- 握拳只对拇指增加局部协调，且必须同时满足四指握持和拇指折叠，普通拇指动作不会
+  被强制变成握拳模板；
+- 独立屈伸往返轨迹用于拟合FFG屈伸对侧摆语义的串扰补偿；
+- 默认执行链路与旧版左手G20一致：直接使用最新手套帧并把连续目标交给电机固件，
+  不叠加输入Kalman和输出EMA；两种滤波仍作为显式可选项保留。
+
+状态话题中的 `mapping_mode=factorized_paired_v2` 表示分解式配对已建立；
+`anchor_weights` 只显示当前局部捏合或握持修正强度，不代表整手姿势切换。
+新生成的机械手profile标记为 `mapping_strategy=paired_continuous_v1`，不再保存
+捏合进入/退出阈值或确认帧数；旧profile中的这些字段只为文件兼容而保留，运行时忽略。
+
+实机命令与仿真目标使用两条独立标尺。`cmd_u8` 是电机空间目标，不能反解为真实关节
+弧度；`q_nominal` 由 `actuation_target` 和profile中的URDF名义端点直接计算并限制在
+URDF范围内。状态中的 `simulation_mapping_mode=semantic_urdf_v1` 表示该隔离生效。
+因此，机械手姿势捕获中某个未定义关节的偶然电机值不会改变仿真姿势。
+
+## 1. 构建
+
+```bash
+cd /home/lxp/projects/linkerhand_retarget_ros2
+source /opt/ros/jazzy/setup.bash
+colcon build --symlink-install --packages-select \
+  linker_hand_ros2_sdk gui_control linkerhand_retarget
+source install/setup.bash
+```
+
+## 2. 完整标定FFG
+
+先启动只读FFG节点。它只连接左手套；没有右手套不会退出：
+
+```bash
+ros2 run linkerhand_retarget ffg_dual_retarget --ros-args \
+  -p serial_port:=/dev/ttyUSB0 \
+  -p auto_scan:=true
+```
+
+另开终端执行完整标定：
+
+```bash
+source /opt/ros/jazzy/setup.bash
+source /home/lxp/projects/linkerhand_retarget_ros2/install/setup.bash
+
+ros2 run linkerhand_retarget ffg_calibrate -- \
+  --glove-id FFG_LEFT_SN \
+  --operator lxp \
+  --output profiles/glove_FFG_LEFT_SN_left_lxp.json
+```
+
+工具依次采集11个静态姿势，每姿势2秒、重复3次且每次至少50帧；随后采集7组短时往返轨迹。profile保存全部原始帧、每次重复统计、总中位数、MAD和有效帧数。
+
+每次正式启动前做张手、握拳、食指捏合快速佩戴检查：
+
+```bash
+ros2 run linkerhand_retarget ffg_calibrate -- \
+  --quick-check profiles/glove_FFG_LEFT_SN_left_lxp.json \
+  --quick-output profiles/glove_FFG_LEFT_SN_left_lxp.wear_check.json
+```
+
+检查凭据默认12小时有效，并绑定FFG profile的SHA-256。缺失、失败、过期或哈希不匹配时，节点仍发布预览，但拒绝实机使能。
+
+## 3. 建立回放基线与Shadow验证
+
+完整标定前后都建议录制一段包含全部标定手势和5个独立验收手势的FFG基线：
+
+```bash
+ros2 bag record -o bags/ffg_baseline \
+  /ffg/left/raw_joint_state
+```
+
+回放时不要占用FFG串口，使用topic输入运行新链路：
+
+```bash
+ros2 run linkerhand_retarget ffg_dual_retarget --ros-args \
+  -p input_mode:=topic \
+  -p raw_input_topic:=/ffg/left/raw_joint_state \
+  -p glove_profile:=/绝对路径/profiles/glove_FFG_LEFT_SN_left_lxp.json \
+  -p g20_profile:=/绝对路径/g20_profile.json \
+  -p o6_profile:=/绝对路径/o6_profile.json
+```
+
+随后执行 `ros2 bag play bags/ffg_baseline`。节点默认不使能，因此只发布两套模型目标，适合逐帧检查长度、名称、范围、连续性、手指局部性和保留通道。
+
+## 4. 标定G20和O6
+
+标定时使用低速、低力矩；不得使用旧手套映射把机械手带到姿势，带电时不得强行手掰。下面的启动会把两台驱动限制为速度30、力矩80，但默认
+`move_on_startup:=false`，不会主动发送张手初始位：
+
+```bash
+ros2 launch linkerhand_retarget ffg_dual_g20_o6.launch.py \
+  auto_scan:=false startup_speed:=30 startup_torque:=80 \
+  move_on_startup:=false
+```
+
+### G20（can0）
+
+终端A启动带命名空间的GUI：
+
+```bash
+ros2 run gui_control gui_control --ros-args \
+  -r __node:=g20_calibration_gui \
+  -p hand_type:=left -p hand_joint:=G20 \
+  -p topic_prefix:=/g20
+```
+
+终端B按提示捕获11个姿势：
+
+```bash
+ros2 run linkerhand_retarget hand_pose_capture -- \
+  --model G20 \
+  --seed install/linkerhand_retarget/share/linkerhand_retarget/linkerforce_v2/profiles/g20_seed_profile.json \
+  --serial-number G20_LEFT_SN \
+  --operator lxp \
+  --firmware G20_FW \
+  --can-interface can0 \
+  --output profiles/hand_G20_left_G20_LEFT_SN_provisional.json
+```
+
+### O6（can1）
+
+```bash
+ros2 run gui_control gui_control --ros-args \
+  -r __node:=o6_calibration_gui \
+  -p hand_type:=left -p hand_joint:=O6 \
+  -p topic_prefix:=/o6
+```
+
+```bash
+ros2 run linkerhand_retarget hand_pose_capture -- \
+  --model O6 \
+  --seed install/linkerhand_retarget/share/linkerhand_retarget/linkerforce_v2/profiles/o6_seed_profile.json \
+  --serial-number O6_LEFT_SN \
+  --operator lxp \
+  --firmware O6_FW \
+  --can-interface can1 \
+  --output profiles/hand_O6_left_O6_LEFT_SN_provisional.json
+```
+
+两个型号的捕获流程都应先把滑块调好、检查通道和方向、等待实机稳定，
+再点击GUI的“保存当前标定姿势”。CLI会自动等待快照，不需要再回终端按Enter；
+快照到达后只需接受默认手势状态或输入实际状态，输入 `r` 可放弃快照并重拍。
+快照和SDK状态都带规范且唯一的通道名称，CLI会按名称重排；缺名、重名或
+型号通道不匹配时拒绝保存。
+
+CLI将GUI保存动作和状态选择共同视为人工确认，并立即写入同目录的
+`*.checkpoint.json`。驱动状态暂时不可用、输入无效状态或选择重拍时，只会重新
+提示当前姿势，不会退出。程序被关闭或异常中断后，使用完全相同的命令
+会校验型号、seed哈希、SN、固件、CAN、操作者和输出路径，并自动跳过已保存姿势。
+若旧检查点中的某些命令超出profile安全范围，CLI会保留其他有效姿势，只移除并
+重拍超限姿势。G20 GUI同时把 `thumb_cmc_yaw` 限制在80～255，并将4个保留通道
+固定为255；不得为了复现超限手势而放宽这些安全范围。
+只有明确希望放弃旧进度时才在原命令末尾增加 `--fresh`；该选项会覆盖旧检查点，
+从第一个姿势重新开始。
+
+O6的桌面、钩拳和不能准确实现的捏合应标为 `approximate`。工具根据张手、桌面、钩拳、握拳捕获值拟合每个主动通道的根部—末端融合权重。只有所有姿势都人工确认，并在最后明确批准，生成的profile才允许申请实机使能。
+
+### 不抄JSON的实机姿势复核
+
+`hand_pose_verify`会直接读取已批准的机械手profile，显示姿势菜单，从SDK当前状态
+按每帧最多4个命令单位平滑过渡到所选姿势，并回读命名状态。每个姿势由操作者选择
+通过、未通过、重放或跳过，结果写入独立的 `*.verification.json`，不会修改标定profile。
+
+复核前必须停止遥操节点和GUI，只保留使用低速、低力矩启动的对应SDK驱动。工具还会
+查询命令话题；发现其他发布者时拒绝开始，运动过程中出现新发布者时停止发送。
+
+```bash
+ros2 run linkerhand_retarget hand_pose_verify -- \
+  --profile /绝对路径/profiles/hand_G20_left_G20_LEFT_SN_provisional.json \
+  --operator lxp \
+  --topic-prefix /g20
+```
+
+只复核一个姿势：
+
+```bash
+ros2 run linkerhand_retarget hand_pose_verify -- \
+  --profile /绝对路径/profiles/hand_G20_left_G20_LEFT_SN_provisional.json \
+  --operator lxp \
+  --topic-prefix /g20 \
+  --pose pinch_middle
+```
+
+默认报告与profile同目录、文件名后缀为 `.verification.json`。SDK的 `state_u8`
+只用于判断电机是否接近命令，不是真实关节角GT；最终姿势仍由操作者目视确认。
+
+## 5. 预览、启动和使能
+
+```bash
+ros2 launch linkerhand_retarget ffg_dual_g20_o6.launch.py \
+  serial_port:=/dev/ttyUSB0 auto_scan:=true \
+  glove_profile:=/绝对路径/profiles/glove_FFG_LEFT_SN_left_lxp.json \
+  wear_check:=/绝对路径/profiles/glove_FFG_LEFT_SN_left_lxp.wear_check.json \
+  g20_profile:=/绝对路径/profiles/hand_G20_left_G20_LEFT_SN_provisional.json \
+  o6_profile:=/绝对路径/profiles/hand_O6_left_O6_LEFT_SN_provisional.json \
+  g20_serial_number:=G20_LEFT_SN \
+  o6_serial_number:=O6_LEFT_SN \
+  g20_can:=can0 o6_can:=can1
+```
+
+节点始终默认PREVIEW。确认以下预览话题正确后再使能：
+
+```text
+/ffg/left/raw_joint_state
+/ffg/left/filtered_joint_state
+/retarget/left/hand_intent
+/retarget/left/frame_meta
+/retarget/g20/left/actuation_target
+/retarget/o6/left/actuation_target
+/retarget/g20/left/joint_target_nominal
+/retarget/o6/left/joint_target_nominal
+/retarget/g20/left/cmd_u8_preview
+/retarget/o6/left/cmd_u8_preview
+```
+
+还应确认状态中的对应型号为 `mapping_mode=factorized_paired_v2`。做食指捏合时，
+应主要看到拇指和食指目标变化；缓慢从食指捏合过渡到中指、无名指或小指时，
+目标应连续变化，且不会保持在先前的捏合模板。若行为不符合这两点，不要使能实机。
+
+`raw_joint_state`始终保留未滤波原始数据用于数采；`filtered_joint_state`表示映射器
+实际使用的输入。旧版左手G20直接使用最新手套帧，所以默认关闭可选Kalman：
+
+```text
+input_filter_enabled=false
+input_filter_process_variance=1e-5
+input_filter_measurement_variance=5e-4
+input_filter_reset_gap=0.35
+```
+
+映射器以固定30Hz推进最新目标，即使两次串口更新之间没有新帧也保持连续输出。
+默认把目标直接交给G20固件插值，可由ROS参数修改，不会写回或改变标定profile哈希：
+
+```text
+command_filter_mode=passthrough
+command_filter_ema_alpha=1.0
+command_filter_max_step_u8=255
+command_filter_deadband_u8=0
+```
+
+SDK默认 `repeat_position_commands=true`，因此会像旧版一样在每个30Hz控制心跳重发最新
+位置目标；待发送队列深度仍为1，来不及发送时只保留最新目标，不会累积延迟。
+
+需要处理异常噪声时可显式启用 `input_filter_enabled:=true`，或将
+`command_filter_mode`设为 `ema`；不要同时启用两层滤波，否则小幅连续运动经过取整后
+容易表现为停顿—跳变。
+
+可在不启动ROS和实机的情况下回放profile自带的静态/动态数据：
+
+```bash
+ros2 run linkerhand_retarget retarget_profile_check -- \
+  --glove /绝对路径/glove_profile.json \
+  --robot /绝对路径/hand_G20_profile.json \
+  --model G20 \
+  --output /绝对路径/g20_mapping_quality.json
+```
+
+报告包含有效通道静态复现误差、四种捏合混淆、动态非目标跨度、帧间变化和侧摆串扰
+补偿。它是离线质量诊断，不会移动机械手。
+
+G20驱动默认把每一帧位置命令都视为遥操心跳：只要30Hz控制流仍在，就完全暂停
+同步状态和速度查询；停用遥操且0.2秒没有命令后才恢复查询。运动期间的状态由CAN
+命令响应缓存更新。GUI订阅触发的速度、故障、温度和扭矩诊断读取也遵守同一规则。
+周期性自动清故障默认关闭，需要时仍可通过设置话题手动清除。这样新版需要的状态
+心跳和GUI监控不会用同步CAN查询打断位置控制。启动参数为：
+
+```text
+defer_state_reads_while_commanding=true
+command_quiet_period=0.2
+auto_clear_faults=false
+```
+
+分别或同时使能：
+
+```bash
+ros2 service call /ffg_dual_retarget/enable_g20 std_srvs/srv/SetBool "{data: true}"
+ros2 service call /ffg_dual_retarget/enable_o6 std_srvs/srv/SetBool "{data: true}"
+ros2 service call /ffg_dual_retarget/enable_all std_srvs/srv/SetBool "{data: true}"
+```
+
+软件急停：
+
+```bash
+ros2 service call /ffg_dual_retarget/emergency_stop std_srvs/srv/Trigger "{}"
+```
+
+FFG超过0.35秒没有新帧时，节点停止发布新实机命令并撤销全部使能；任一驱动状态超过1秒时，只撤销对应型号。恢复后必须重新显式使能。
+
+## 6. MuJoCo和Isaac Sim桥
+
+四个仿真桥分别订阅：
+
+```text
+/retarget/g20/left/joint_target_nominal
+/retarget/o6/left/joint_target_nominal
+```
+
+并分别发布：
+
+```text
+/sim/mujoco/g20/left/joint_state
+/sim/mujoco/o6/left/joint_state
+/sim/isaac/g20/left/joint_state
+/sim/isaac/o6/left/joint_state
+```
+
+桥必须使用 `JointState.name` 匹配模型关节，不得按裸下标映射。可在各仿真环境中复用
+`linkerhand_retarget.motion.linkerforce_v2.simulation.reorder_named_target`；
+目标缺少、多出或重复关节名时它会立即抛出 `JointNameMismatch`。本仓库不引入MuJoCo或Isaac的重型依赖，位置控制器仍放在各自仿真环境中。
+
+`q_nominal` 使用 `semantic_urdf_v1`：根部/末端弯曲、侧摆和拇指各语义通道分别在
+URDF名义端点间连续插值，不经过 `cmd_u8` 反解。该目标用于统一驱动和检查仿真模型，
+不是实机测量角度。
+
+仿真验收只比较仿真主动关节与 `q_nominal`，稳定后误差目标为0.02 rad。当前没有Marker或独立角度传感器，因此不得把该误差解释成仿真—实机关节角误差。
+
+## 7. provisional数采
+
+先生成session manifest：
+
+```bash
+ros2 run linkerhand_retarget provisional_manifest -- \
+  --output sessions/demo_001.json \
+  --session-id demo_001 --operator lxp \
+  --glove-profile profiles/glove_FFG_LEFT_SN_left_lxp.json \
+  --wear-check profiles/glove_FFG_LEFT_SN_left_lxp.wear_check.json \
+  --g20-profile profiles/hand_G20_left_G20_LEFT_SN_provisional.json \
+  --o6-profile profiles/hand_O6_left_O6_LEFT_SN_provisional.json \
+  --g20-urdf src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/g20_left/linkerhand_g20_left.urdf \
+  --o6-urdf src/linkerhand_retarget/linkerhand_retarget/assets/robots/hands/linker_hand/o6_left/linkerhand_o6_left.urdf \
+  --g20-serial-number G20_LEFT_SN \
+  --o6-serial-number O6_LEFT_SN
+```
+
+工具保存profile/URDF哈希、SN、固件、CAN、话题清单，并打印完整的 `ros2 bag record` 命令。manifest固定写入：
+
+```json
+{
+  "calibration": "provisional",
+  "q_gt": null,
+  "angle_accuracy_claim": false
+}
+```
+
+SDK返回的 `state_u8` 仅作为设备状态记录，不是真实物理角度。
+`/retarget/left/frame_meta` 为每个已映射手套帧记录统一时间戳、
+`sequence`、`calibration=provisional` 和 `q_gt=null`；同一帧产生的
+`hand_intent`、两套 `actuation_target`、`q_nominal` 与 `cmd_u8_preview`
+共享该时间戳。

src/linkerhand_retarget/resource/linkerforce_v2/profiles/g20_seed_profile.json

@@ -0,0 +1,156 @@
+{
+  "schema_version": 1,
+  "profile_kind": "robot_hand",
+  "model": "G20",
+  "side": "left",
+  "mapping_strategy": "paired_continuous_v1",
+  "serial_number": "UNSET",
+  "calibration": "provisional",
+  "approved_for_control": false,
+  "metadata": {
+    "description": "G20标定滑块初值，仅用于预览和姿势捕获，禁止实机控制",
+    "firmware": "unknown",
+    "can_interface": "can0",
+    "operator_confirmed": false
+  },
+  "command_length": 20,
+  "command_names": [
+    "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", "reserved_11", "reserved_12", "reserved_13", "reserved_14",
+    "thumb_mcp", "index_pip", "middle_pip", "ring_pip", "pinky_pip"
+  ],
+  "required_poses": [
+    "open_spread",
+    "fingers_together",
+    "tabletop",
+    "hook",
+    "fist",
+    "thumb_max_out",
+    "thumb_across",
+    "pinch_index",
+    "pinch_middle",
+    "pinch_ring",
+    "pinch_pinky"
+  ],
+  "poses": {
+    "open_spread": {
+      "cmd_u8": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "fingers_together": {
+      "cmd_u8": [255, 255, 255, 255, 255, 255, 128, 128, 128, 128, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "tabletop": {
+      "cmd_u8": [255, 0, 0, 0, 0, 255, 128, 128, 128, 128, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "hook": {
+      "cmd_u8": [255, 255, 255, 255, 255, 255, 193, 148, 105, 42, 255, 255, 255, 255, 255, 255, 0, 0, 0, 0],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "fist": {
+      "cmd_u8": [50, 0, 0, 0, 0, 78, 193, 158, 128, 91, 112, 255, 255, 255, 255, 144, 0, 0, 0, 0],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "thumb_max_out": {
+      "cmd_u8": [255, 255, 255, 255, 255, 20, 193, 148, 105, 42, 255, 255, 255, 255, 255, 255, 255, 255, 255, 255],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "thumb_across": {
+      "cmd_u8": [150, 255, 255, 255, 255, 78, 193, 148, 105, 42, 112, 255, 255, 255, 255, 200, 255, 255, 255, 255],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "pinch_index": {
+      "cmd_u8": [81, 89, 255, 255, 255, 167, 193, 148, 105, 42, 184, 255, 255, 255, 255, 161, 89, 255, 255, 255],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "pinch_middle": {
+      "cmd_u8": [81, 255, 89, 255, 255, 122, 193, 148, 105, 42, 148, 255, 255, 255, 255, 161, 255, 89, 255, 255],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "pinch_ring": {
+      "cmd_u8": [81, 255, 255, 89, 255, 78, 193, 148, 105, 42, 112, 255, 255, 255, 255, 161, 255, 255, 89, 255],
+      "state_u8": [],
+      "status": "approximate"
+    },
+    "pinch_pinky": {
+      "cmd_u8": [81, 255, 255, 255, 89, 50, 193, 148, 105, 42, 90, 255, 255, 255, 255, 161, 255, 255, 255, 89],
+      "state_u8": [],
+      "status": "approximate"
+    }
+  },
+  "command_bounds": [
+    [0, 255], [0, 255], [0, 255], [0, 255], [0, 255],
+    [0, 255], [0, 255], [0, 255], [0, 255], [0, 255],
+    [80, 255], [255, 255], [255, 255], [255, 255], [255, 255],
+    [0, 255], [0, 255], [0, 255], [0, 255], [0, 255]
+  ],
+  "reserved_channels": {
+    "11": 255,
+    "12": 255,
+    "13": 255,
+    "14": 255
+  },
+  "fit_sources": {
+    "thumb_cmc_pitch": ["thumb_root"],
+    "thumb_mcp": ["thumb_tip"],
+    "index_mcp_pitch": ["index_root"],
+    "index_pip": ["index_tip"],
+    "middle_mcp_pitch": ["middle_root"],
+    "middle_pip": ["middle_tip"],
+    "ring_mcp_pitch": ["ring_root"],
+    "ring_pip": ["ring_tip"],
+    "pinky_mcp_pitch": ["pinky_root"],
+    "pinky_pip": ["pinky_tip"]
+  },
+  "channels": [
+    {"index": 0, "name": "thumb_cmc_pitch", "mapping": "monotonic_piecewise", "sources": {"thumb_root": 1.0}, "point_poses": ["open_spread", "thumb_across", "fist"], "points": [[0.0, 255.0], [1.0, 50.0]]},
+    {"index": 1, "name": "index_mcp_pitch", "mapping": "monotonic_piecewise", "sources": {"index_root": 1.0}, "point_poses": ["open_spread", "hook", "tabletop", "fist"], "points": [[0.0, 255.0], [1.0, 0.0]]},
+    {"index": 2, "name": "middle_mcp_pitch", "mapping": "monotonic_piecewise", "sources": {"middle_root": 1.0}, "point_poses": ["open_spread", "hook", "tabletop", "fist"], "points": [[0.0, 255.0], [1.0, 0.0]]},
+    {"index": 3, "name": "ring_mcp_pitch", "mapping": "monotonic_piecewise", "sources": {"ring_root": 1.0}, "point_poses": ["open_spread", "hook", "tabletop", "fist"], "points": [[0.0, 255.0], [1.0, 0.0]]},
+    {"index": 4, "name": "pinky_mcp_pitch", "mapping": "monotonic_piecewise", "sources": {"pinky_root": 1.0}, "point_poses": ["open_spread", "hook", "tabletop", "fist"], "points": [[0.0, 255.0], [1.0, 0.0]]},
+    {"index": 5, "name": "thumb_cmc_roll", "mapping": "piecewise", "sources": {"thumb_abduction": 1.0}, "point_poses": ["thumb_max_out", "open_spread", "thumb_across"], "points": [[0.0, 20.0], [0.5, 255.0], [1.0, 78.0]]},
+    {"index": 6, "name": "index_mcp_roll", "mapping": "monotonic_piecewise", "sources": {"index_splay": 1.0}, "point_poses": ["fingers_together", "open_spread"], "points": [[0.0, 128.0], [1.0, 193.0]]},
+    {"index": 7, "name": "middle_mcp_roll", "mapping": "monotonic_piecewise", "sources": {"middle_splay": 1.0}, "point_poses": ["fingers_together", "open_spread"], "points": [[0.0, 128.0], [1.0, 148.0]]},
+    {"index": 8, "name": "ring_mcp_roll", "mapping": "monotonic_piecewise", "sources": {"ring_splay": 1.0}, "point_poses": ["fingers_together", "open_spread"], "points": [[0.0, 128.0], [1.0, 105.0]]},
+    {"index": 9, "name": "pinky_mcp_roll", "mapping": "monotonic_piecewise", "sources": {"pinky_splay": 1.0}, "point_poses": ["fingers_together", "open_spread"], "points": [[0.0, 128.0], [1.0, 42.0]]},
+    {"index": 10, "name": "thumb_cmc_yaw", "mapping": "piecewise", "sources": {"thumb_rotate": 1.0}, "point_poses": ["thumb_max_out", "open_spread", "thumb_across"], "points": [[0.0, 255.0], [0.5, 255.0], [1.0, 112.0]]},
+    {"index": 15, "name": "thumb_mcp", "mapping": "monotonic_piecewise", "sources": {"thumb_tip": 1.0}, "point_poses": ["open_spread", "thumb_across", "fist"], "points": [[0.0, 255.0], [1.0, 144.0]]},
+    {"index": 16, "name": "index_pip", "mapping": "monotonic_piecewise", "sources": {"index_tip": 1.0}, "point_poses": ["open_spread", "tabletop", "hook", "fist"], "points": [[0.0, 255.0], [1.0, 0.0]]},
+    {"index": 17, "name": "middle_pip", "mapping": "monotonic_piecewise", "sources": {"middle_tip": 1.0}, "point_poses": ["open_spread", "tabletop", "hook", "fist"], "points": [[0.0, 255.0], [1.0, 0.0]]},
+    {"index": 18, "name": "ring_pip", "mapping": "monotonic_piecewise", "sources": {"ring_tip": 1.0}, "point_poses": ["open_spread", "tabletop", "hook", "fist"], "points": [[0.0, 255.0], [1.0, 0.0]]},
+    {"index": 19, "name": "pinky_pip", "mapping": "monotonic_piecewise", "sources": {"pinky_tip": 1.0}, "point_poses": ["open_spread", "tabletop", "hook", "fist"], "points": [[0.0, 255.0], [1.0, 0.0]]}
+  ],
+  "simulation": {
+    "mapping_strategy": "semantic_urdf_v1",
+    "urdf_file": "linkerhand_g20_left.urdf",
+    "urdf_sha256": "51578cb1ece676324983d4076aae6de8d9c623cf437df786f4322134d0c7d305",
+    "joint_names": [
+      "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", "thumb_mcp", "index_pip", "middle_pip", "ring_pip", "pinky_pip"
+    ],
+    "open": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    "closed": [0.84, 1.22, 1.22, 1.22, 1.22, 1.4, 0.2, 0.0, -0.2, -0.2, 1.57, 1.26, 1.74, 1.74, 1.74, 1.74],
+    "lower": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0, -0.23, -0.23, -0.23, -0.23, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    "upper": [0.84, 1.22, 1.22, 1.22, 1.22, 1.4, 0.23, 0.23, 0.23, 0.23, 1.57, 1.26, 1.74, 1.74, 1.74, 1.74]
+  },
+  "filter": {
+    "ema_alpha": 1.0,
+    "max_step_u8": 255.0,
+    "max_acceleration_u8_per_frame2": 255.0,
+    "deadband_u8": 0.0,
+    "mode": "passthrough"
+  }
+}

src/linkerhand_retarget/resource/linkerforce_v2/profiles/o6_seed_profile.json

@@ -0,0 +1,70 @@
+{
+  "schema_version": 1,
+  "profile_kind": "robot_hand",
+  "model": "O6",
+  "side": "left",
+  "mapping_strategy": "paired_continuous_v1",
+  "serial_number": "UNSET",
+  "calibration": "provisional",
+  "approved_for_control": false,
+  "metadata": {
+    "description": "O6标定滑块初值，仅用于预览和姿势捕获，禁止实机控制",
+    "firmware": "unknown",
+    "can_interface": "can1",
+    "operator_confirmed": false
+  },
+  "command_length": 6,
+  "command_names": ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"],
+  "required_poses": [
+    "open_spread",
+    "tabletop",
+    "hook",
+    "fist",
+    "thumb_max_out",
+    "thumb_across",
+    "pinch_index",
+    "pinch_middle",
+    "pinch_ring",
+    "pinch_pinky"
+  ],
+  "poses": {
+    "open_spread": {"cmd_u8": [255, 179, 255, 255, 255, 255], "state_u8": [], "status": "approximate"},
+    "tabletop": {"cmd_u8": [255, 179, 64, 64, 64, 64], "state_u8": [], "status": "approximate"},
+    "hook": {"cmd_u8": [255, 179, 96, 96, 96, 96], "state_u8": [], "status": "approximate"},
+    "fist": {"cmd_u8": [0, 151, 0, 0, 0, 0], "state_u8": [], "status": "approximate"},
+    "thumb_max_out": {"cmd_u8": [255, 207, 255, 255, 255, 255], "state_u8": [], "status": "approximate"},
+    "thumb_across": {"cmd_u8": [64, 80, 255, 255, 255, 255], "state_u8": [], "status": "approximate"},
+    "pinch_index": {"cmd_u8": [38, 80, 89, 255, 255, 255], "state_u8": [], "status": "approximate"},
+    "pinch_middle": {"cmd_u8": [38, 0, 255, 89, 255, 255], "state_u8": [], "status": "approximate"},
+    "pinch_ring": {"cmd_u8": [38, 0, 255, 255, 89, 255], "state_u8": [], "status": "approximate"},
+    "pinch_pinky": {"cmd_u8": [38, 0, 255, 255, 255, 89], "state_u8": [], "status": "approximate"}
+  },
+  "command_bounds": [[0, 255], [0, 255], [0, 255], [0, 255], [0, 255], [0, 255]],
+  "reserved_channels": {},
+  "fit_sources": {
+    "thumb_cmc_pitch": ["thumb_root", "thumb_tip"],
+    "index_mcp_pitch": ["index_root", "index_tip"],
+    "middle_mcp_pitch": ["middle_root", "middle_tip"],
+    "ring_mcp_pitch": ["ring_root", "ring_tip"],
+    "pinky_mcp_pitch": ["pinky_root", "pinky_tip"]
+  },
+  "channels": [
+    {"index": 0, "name": "thumb_cmc_pitch", "mapping": "monotonic_piecewise", "sources": {"thumb_root": 0.5, "thumb_tip": 0.5}, "point_poses": ["open_spread", "tabletop", "hook", "fist"], "points": [[0.0, 255.0], [0.33, 255.0], [0.66, 255.0], [1.0, 0.0]]},
+    {"index": 1, "name": "thumb_cmc_yaw", "mapping": "monotonic_piecewise", "sources": {"thumb_opposition": 1.0}, "point_poses": ["open_spread", "thumb_across"], "points": [[0.0, 179.0], [1.0, 80.0]]},
+    {"index": 2, "name": "index_mcp_pitch", "mapping": "monotonic_piecewise", "sources": {"index_root": 0.5, "index_tip": 0.5}, "point_poses": ["open_spread", "tabletop", "hook", "fist"], "points": [[0.0, 255.0], [0.33, 96.0], [0.66, 64.0], [1.0, 0.0]]},
+    {"index": 3, "name": "middle_mcp_pitch", "mapping": "monotonic_piecewise", "sources": {"middle_root": 0.5, "middle_tip": 0.5}, "point_poses": ["open_spread", "tabletop", "hook", "fist"], "points": [[0.0, 255.0], [0.33, 96.0], [0.66, 64.0], [1.0, 0.0]]},
+    {"index": 4, "name": "ring_mcp_pitch", "mapping": "monotonic_piecewise", "sources": {"ring_root": 0.5, "ring_tip": 0.5}, "point_poses": ["open_spread", "tabletop", "hook", "fist"], "points": [[0.0, 255.0], [0.33, 96.0], [0.66, 64.0], [1.0, 0.0]]},
+    {"index": 5, "name": "pinky_mcp_pitch", "mapping": "monotonic_piecewise", "sources": {"pinky_root": 0.5, "pinky_tip": 0.5}, "point_poses": ["open_spread", "tabletop", "hook", "fist"], "points": [[0.0, 255.0], [0.33, 96.0], [0.66, 64.0], [1.0, 0.0]]}
+  ],
+  "simulation": {
+    "mapping_strategy": "semantic_urdf_v1",
+    "urdf_file": "linkerhand_o6_left.urdf",
+    "urdf_sha256": "0aa6cb5fe296389e019c4b575f6559554bb88a7e72c6c3435970434c7f9c6f8e",
+    "joint_names": ["thumb_cmc_pitch", "thumb_cmc_yaw", "index_mcp_pitch", "middle_mcp_pitch", "ring_mcp_pitch", "pinky_mcp_pitch"],
+    "open": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    "closed": [0.58, 1.36, 1.6, 1.6, 1.6, 1.6],
+    "lower": [0.0, 0.0, 0.0, 0.0, 0.0, 0.0],
+    "upper": [0.58, 1.36, 1.6, 1.6, 1.6, 1.6]
+  },
+  "filter": {"ema_alpha": 0.55, "max_step_u8": 16.0, "deadband_u8": 0.0}
+}

src/linkerhand_retarget/setup.py

@@ -49,6 +49,12 @@ setup(
     entry_points={
         'console_scripts': [
             'handretarget = linkerhand_retarget.handretarget:main',
+            'ffg_dual_retarget = linkerhand_retarget.motion.linkerforce_v2.node:main',
+            'ffg_calibrate = linkerhand_retarget.motion.linkerforce_v2.calibrate_glove:main',
+            'hand_pose_capture = linkerhand_retarget.motion.linkerforce_v2.calibrate_robot:main',
+            'hand_pose_verify = linkerhand_retarget.motion.linkerforce_v2.verify_robot_profile:main',
+            'retarget_profile_check = linkerhand_retarget.motion.linkerforce_v2.quality:main',
+            'provisional_manifest = linkerhand_retarget.motion.linkerforce_v2.session_manifest:main',
         ],
     },
 )

src/linkerhand_retarget/test/test_linkerforce_v2.py

@@ -0,0 +1,917 @@
+from __future__ import annotations
+
+import ast
+import copy
+import hashlib
+import json
+from pathlib import Path
+import xml.etree.ElementTree as ET
+
+import numpy as np
+import pytest
+
+from linkerhand_retarget.motion.linkerforce_v2.calibration import (
+    build_glove_profile,
+    build_robot_profile,
+    robust_summary,
+)
+from linkerhand_retarget.motion.linkerforce_v2.calibrate_robot import (
+    _checkpoint_document,
+    _checkpoint_path,
+    _command_bound_violations,
+    _load_checkpoint,
+)
+from linkerhand_retarget.motion.linkerforce_v2.constants import (
+    BASE_INTENT_NAMES,
+    DEFAULT_COMMAND_FILTER_CONFIG,
+    DEFAULT_INPUT_FILTER_ENABLED,
+    GLOVE_DYNAMIC_GESTURES,
+    GLOVE_STATIC_GESTURES,
+    PINCH_ROLES,
+)
+from linkerhand_retarget.motion.linkerforce_v2.mapping import (
+    CommandFilter,
+    HandIntentExtractor,
+    RobotMapper,
+    VectorKalmanFilter,
+    piecewise_linear,
+)
+from linkerhand_retarget.motion.linkerforce_v2.profiles import (
+    ProfileError,
+    load_glove_profile,
+    load_robot_profile,
+    save_profile,
+)
+from linkerhand_retarget.motion.linkerforce_v2.quality import (
+    evaluate_mapping_quality,
+)
+from linkerhand_retarget.motion.linkerforce_v2.safety import evaluate_timeouts
+from linkerhand_retarget.motion.linkerforce_v2.simulation import (
+    JointNameMismatch,
+    reorder_named_target,
+)
+from linkerhand_retarget.motion.linkerforce_v2.verify_robot_profile import (
+    state_error_summary,
+    transition_commands,
+    verification_document,
+)
+
+
+PACKAGE = Path(__file__).resolve().parents[1]
+PROFILE_DIR = PACKAGE / "resource" / "linkerforce_v2" / "profiles"
+
+
+def _pose_frame(gesture: str) -> np.ndarray:
+    frame = np.zeros(21, dtype=float)
+    if gesture == "open_spread":
+        frame[[5, 9, 13, 17]] = 1.0
+    elif gesture == "thumb_max_out":
+        frame[0] = -0.5
+    elif gesture == "thumb_across":
+        frame[:5] = 1.0
+    elif gesture == "tabletop":
+        for base in (5, 9, 13, 17):
+            frame[base + 1] = 1.0
+    elif gesture == "hook":
+        for base in (5, 9, 13, 17):
+            frame[base + 2 : base + 4] = 1.0
+    elif gesture == "fist":
+        frame[2:5] = 1.0
+        for base in (5, 9, 13, 17):
+            frame[base + 1 : base + 4] = 1.0
+    elif gesture.startswith("pinch_"):
+        frame[:5] = 0.9
+        role = gesture.removeprefix("pinch_")
+        base = {"index": 5, "middle": 9, "ring": 13, "pinky": 17}[role]
+        frame[base : base + 4] = 0.8
+    return frame
+
+
+def _glove_captures() -> dict[str, list[list[float]]]:
+    captures = {}
+    noise = np.linspace(-0.002, 0.002, 60)
+    for gesture in GLOVE_STATIC_GESTURES:
+        base = _pose_frame(gesture)
+        captures[gesture] = [
+            (base + offset).astype(float).tolist() for offset in noise
+        ]
+    return captures
+
+
+def test_robust_summary_keeps_raw_median_mad_and_count():
+    summary = robust_summary([[0.0] * 21, [1.0] * 21, [100.0] * 21], 21)
+    assert summary["sample_count"] == 3
+    assert summary["median"] == [1.0] * 21
+    assert summary["mad"] == [1.0] * 21
+    assert len(summary["raw_frames"]) == 3
+
+
+def test_glove_profile_and_semantic_extraction(tmp_path):
+    profile = build_glove_profile(
+        _glove_captures(), glove_id="FFG-TEST", operator="tester"
+    )
+    path = save_profile(profile, tmp_path / "glove.json")
+    loaded = load_glove_profile(path)
+    assert loaded["approved_for_runtime"] is False
+    extractor = HandIntentExtractor(loaded)
+    fist = extractor.extract(_pose_frame("fist"))
+    assert set(BASE_INTENT_NAMES).issubset(fist)
+    assert fist["index_root"] > 0.8
+    assert fist["index_tip"] > 0.8
+    pinch = extractor.extract(_pose_frame("pinch_index"))
+    assert pinch["pinch_index"] > 0.95
+    assert pinch["pinch_index"] > pinch["pinch_pinky"]
+
+
+def test_four_finger_flex_calibration_decouples_root_and_tip_anchors():
+    profile = build_glove_profile(
+        _glove_captures(),
+        glove_id="FFG-FLEX-DECOUPLING",
+        operator="tester",
+    )
+    # Reproduce realistic sensor crosstalk: tabletop also moves the raw tip
+    # features, while hook also moves the raw root features.
+    for role_index, base in enumerate((5, 9, 13, 17)):
+        profile["poses"]["tabletop"]["median"][base + 2] = (
+            0.18 + 0.05 * role_index
+        )
+        profile["poses"]["tabletop"]["median"][base + 3] = (
+            0.12 + 0.04 * role_index
+        )
+        profile["poses"]["hook"]["median"][base + 1] = (
+            0.22 + 0.04 * role_index
+        )
+
+    extractor = HandIntentExtractor(profile)
+    assert set(extractor.flex_decoders) == set(PINCH_ROLES)
+    expected = {
+        "open_spread": (0.0, 0.0),
+        "tabletop": (1.0, 0.0),
+        "hook": (0.0, 1.0),
+        "fist": (1.0, 1.0),
+    }
+    for pose_name, (expected_root, expected_tip) in expected.items():
+        intent = extractor.extract(profile["poses"][pose_name]["median"])
+        for role in PINCH_ROLES:
+            assert intent[f"{role}_root"] == pytest.approx(
+                expected_root, abs=1e-6
+            )
+            assert intent[f"{role}_tip"] == pytest.approx(
+                expected_tip, abs=1e-6
+            )
+
+
+def test_four_finger_flex_curves_have_no_internal_command_plateau():
+    glove_profile = build_glove_profile(
+        _glove_captures(),
+        glove_id="FFG-FLEX-CONTINUITY",
+        operator="tester",
+    )
+    robot_profile = load_robot_profile(
+        PROFILE_DIR / "g20_seed_profile.json", "G20"
+    )
+    mapper = RobotMapper(robot_profile, glove_profile)
+
+    for channel, curve in zip(robot_profile["channels"], mapper.paired_curves):
+        name = str(channel["name"])
+        if not (
+            name.startswith(("index_", "middle_", "ring_", "pinky_"))
+            and name.endswith(("mcp_pitch", "pip"))
+        ):
+            continue
+        points = np.asarray(curve.points, dtype=float)
+        assert points[:, 0].tolist() == pytest.approx([0.0, 1.0])
+        assert abs(float(points[1, 1] - points[0, 1])) > 200.0
+
+
+def test_glove_runtime_approval_requires_three_traceable_repeats(tmp_path):
+    source = _glove_captures()
+    captures = {
+        gesture: frames * 3
+        for gesture, frames in source.items()
+    }
+    repeats = {
+        gesture: [robust_summary(frames, 21) for _ in range(3)]
+        for gesture, frames in source.items()
+    }
+    dynamic_captures = {
+        gesture: source["open_spread"] * 3
+        for gesture in GLOVE_DYNAMIC_GESTURES
+    }
+    dynamic_repeats = {
+        gesture: [
+            robust_summary(source["open_spread"], 21)
+            for _ in range(3)
+        ]
+        for gesture in GLOVE_DYNAMIC_GESTURES
+    }
+    profile = build_glove_profile(
+        captures,
+        glove_id="FFG-TEST",
+        operator="tester",
+        dynamic_captures=dynamic_captures,
+        repeat_summaries=repeats,
+        dynamic_repeat_summaries=dynamic_repeats,
+    )
+    assert profile["approved_for_runtime"] is True
+    path = save_profile(profile, tmp_path / "approved_glove.json")
+    assert load_glove_profile(path)["approved_for_runtime"] is True
+
+    profile["poses"]["fist"]["repeats"][0]["raw_frames"].pop()
+    broken = save_profile(profile, tmp_path / "broken_glove.json")
+    with pytest.raises(ProfileError):
+        load_glove_profile(broken)
+
+
+@pytest.mark.parametrize("model,length", [("G20", 20), ("O6", 6)])
+def test_seed_profiles_map_named_bounded_commands(model, length):
+    profile = load_robot_profile(
+        PROFILE_DIR / f"{model.lower()}_seed_profile.json", model
+    )
+    mapper = RobotMapper(profile)
+    intent = {name: 0.5 for name in BASE_INTENT_NAMES}
+    intent.update(
+        {
+            "pinch_index": 0.0,
+            "pinch_middle": 0.0,
+            "pinch_ring": 0.0,
+            "pinch_pinky": 0.0,
+            "power_grasp": 0.5,
+        }
+    )
+    result = mapper.map(intent)
+    assert len(result.command) == length
+    assert len(result.actuation_names) == len(result.actuation)
+    assert len(result.sim_joint_names) == len(result.q_nominal)
+    assert all(0 <= value <= 255 for value in result.command)
+    if model == "G20":
+        assert result.command[11:15] == (255, 255, 255, 255)
+
+
+def test_paired_pinch_anchors_only_constrain_thumb_and_target_finger():
+    glove_profile = build_glove_profile(
+        _glove_captures(),
+        glove_id="FFG-PAIRED",
+        operator="tester",
+    )
+    robot_profile = load_robot_profile(
+        PROFILE_DIR / "g20_seed_profile.json", "G20"
+    )
+    extractor = HandIntentExtractor(glove_profile)
+    mapper = RobotMapper(robot_profile, glove_profile)
+
+    for role in ("index", "middle", "ring", "pinky"):
+        pose_name = f"pinch_{role}"
+        result = mapper.map(extractor.extract(_pose_frame(pose_name)))
+        expected = np.asarray(
+            robot_profile["poses"][pose_name]["cmd_u8"],
+            dtype=float,
+        )
+        relevant = [
+            int(channel["index"])
+            for channel in robot_profile["channels"]
+            if str(channel["name"]).startswith(("thumb_", f"{role}_"))
+        ]
+        assert not hasattr(result, "pinch_role")
+        assert not hasattr(result, "pinch_strength")
+        assert np.allclose(
+            np.asarray(result.raw_command)[relevant],
+            expected[relevant],
+        )
+
+
+def test_paired_mapping_is_continuous_and_has_no_pose_latch():
+    glove_profile = build_glove_profile(
+        _glove_captures(),
+        glove_id="FFG-CONTINUOUS",
+        operator="tester",
+    )
+    robot_profile = load_robot_profile(
+        PROFILE_DIR / "g20_seed_profile.json", "G20"
+    )
+    extractor = HandIntentExtractor(glove_profile)
+    index_intent = extractor.extract(_pose_frame("pinch_index"))
+    ring_intent = extractor.extract(_pose_frame("pinch_ring"))
+    mapper = RobotMapper(robot_profile, glove_profile)
+
+    raw_commands = []
+    for ratio in np.linspace(0.0, 1.0, 101):
+        intent = {
+            name: (1.0 - ratio) * index_intent[name] + ratio * ring_intent[name]
+            for name in index_intent
+        }
+        raw_commands.append(np.asarray(mapper.map(intent).raw_command))
+    step_sizes = np.max(np.abs(np.diff(raw_commands, axis=0)), axis=1)
+    assert np.max(step_sizes) < 8.0
+
+    target = {
+        name: 0.4 * index_intent[name] + 0.6 * ring_intent[name]
+        for name in index_intent
+    }
+    before = np.asarray(mapper.map(target).raw_command)
+    mapper.map(index_intent)
+    mapper.map(ring_intent)
+    after = np.asarray(mapper.map(target).raw_command)
+    assert np.allclose(before, after)
+
+
+def test_regular_finger_commands_are_independent_from_thumb_motion():
+    glove_profile = build_glove_profile(
+        _glove_captures(),
+        glove_id="FFG-ANATOMICAL",
+        operator="tester",
+    )
+    robot_profile = load_robot_profile(
+        PROFILE_DIR / "g20_seed_profile.json", "G20"
+    )
+    mapper = RobotMapper(robot_profile, glove_profile)
+    intent = HandIntentExtractor(glove_profile).extract(
+        _pose_frame("open_spread")
+    )
+    intent.update({f"pinch_{role}": 0.0 for role in PINCH_ROLES})
+    reference = np.asarray(mapper.map(intent).raw_command)
+    changed = dict(intent)
+    for name in (
+        "thumb_rotate",
+        "thumb_abduction",
+        "thumb_opposition",
+        "thumb_root",
+        "thumb_tip",
+    ):
+        changed[name] = 1.0 - changed[name]
+    actual = np.asarray(mapper.map(changed).raw_command)
+    regular = [
+        int(channel["index"])
+        for channel in robot_profile["channels"]
+        if not str(channel["name"]).startswith("thumb_")
+    ]
+    assert np.allclose(actual[regular], reference[regular])
+
+
+def test_ambiguous_pinch_evidence_falls_back_to_continuous_base_mapping():
+    glove_profile = build_glove_profile(
+        _glove_captures(),
+        glove_id="FFG-PINCH-COMPETITION",
+        operator="tester",
+    )
+    robot_profile = load_robot_profile(
+        PROFILE_DIR / "g20_seed_profile.json", "G20"
+    )
+    mapper = RobotMapper(robot_profile, glove_profile)
+    intent = HandIntentExtractor(glove_profile).extract(
+        _pose_frame("pinch_index")
+    )
+    base = dict(intent)
+    base.update({f"pinch_{role}": 0.0 for role in PINCH_ROLES})
+    ambiguous = dict(base)
+    ambiguous["pinch_index"] = 1.0
+    ambiguous["pinch_middle"] = 1.0
+    assert mapper._pinch_gates(ambiguous) == {
+        role: 0.0 for role in PINCH_ROLES
+    }
+    assert np.allclose(
+        mapper.map(ambiguous).raw_command,
+        mapper.map(base).raw_command,
+    )
+
+
+def test_dynamic_flex_capture_learns_same_finger_splay_compensation():
+    glove_profile = build_glove_profile(
+        _glove_captures(),
+        glove_id="FFG-DYNAMIC-CROSSTALK",
+        operator="tester",
+    )
+    without_dynamic = HandIntentExtractor(glove_profile)
+    frames = []
+    for ratio in np.linspace(0.0, 1.0, 80):
+        frame = np.zeros(21, dtype=float)
+        frame[5] = 0.4 * ratio
+        frame[6] = ratio
+        frame[8] = ratio
+        frames.append(frame.tolist())
+    glove_profile["dynamic_captures"] = {
+        "index_flex_sweep": {"raw_frames": frames}
+    }
+    with_dynamic = HandIntentExtractor(glove_profile)
+    raw_splay = without_dynamic.extract(frames[-1])["index_splay"]
+    corrected_splay = with_dynamic.extract(frames[-1])["index_splay"]
+    assert "index_splay" in with_dynamic.splay_compensation
+    assert corrected_splay < raw_splay - 0.2
+
+
+def test_quality_replay_reports_factorized_static_and_pinch_accuracy():
+    glove_profile = build_glove_profile(
+        _glove_captures(),
+        glove_id="FFG-QUALITY",
+        operator="tester",
+    )
+    robot_profile = load_robot_profile(
+        PROFILE_DIR / "g20_seed_profile.json", "G20"
+    )
+    report = evaluate_mapping_quality(glove_profile, robot_profile)
+    assert report["mapping_mode"] == "factorized_paired_v2"
+    assert report["summary"]["static_max_abs_error_u8"] < 1e-6
+    assert report["summary"]["passed"] is True
+    assert report["summary"]["execution_step_max_u8"] is None
+    for role in PINCH_ROLES:
+        assert report["pinch_confusion"][role]["winner"] == role
+
+
+def test_thumb_mapping_ignores_four_finger_only_pose_commands():
+    glove_profile = build_glove_profile(
+        _glove_captures(),
+        glove_id="FFG-THUMB-SCOPE",
+        operator="tester",
+    )
+    robot_profile = load_robot_profile(
+        PROFILE_DIR / "g20_seed_profile.json", "G20"
+    )
+    altered_profile = copy.deepcopy(robot_profile)
+    for pose_name in ("fingers_together", "tabletop", "hook"):
+        for command_index in (0, 5, 10, 15):
+            altered_profile["poses"][pose_name]["cmd_u8"][command_index] = (
+                80.0 if command_index == 10 else 0.0
+            )
+
+    extractor = HandIntentExtractor(glove_profile)
+    reference_mapper = RobotMapper(robot_profile, glove_profile)
+    altered_mapper = RobotMapper(altered_profile, glove_profile)
+    thumb_channels = [
+        (channel, model)
+        for channel, model in zip(
+            robot_profile["channels"],
+            altered_mapper.continuous_models,
+        )
+        if str(channel["name"]).startswith("thumb_")
+    ]
+    for _, model in thumb_channels:
+        assert not {
+            "fingers_together",
+            "tabletop",
+            "hook",
+        }.intersection(model.pose_names)
+
+    for pose_name in ("fingers_together", "tabletop", "hook"):
+        intent = extractor.extract(_pose_frame(pose_name))
+        reference = np.asarray(reference_mapper.map(intent).raw_command)
+        altered = np.asarray(altered_mapper.map(intent).raw_command)
+        assert np.allclose(reference[[0, 5, 10, 15]], altered[[0, 5, 10, 15]])
+
+
+def test_nominal_simulation_is_independent_from_motor_pose_corrections():
+    glove_profile = build_glove_profile(
+        _glove_captures(),
+        glove_id="FFG-SIM-INDEPENDENT",
+        operator="tester",
+    )
+    robot_profile = load_robot_profile(
+        PROFILE_DIR / "g20_seed_profile.json", "G20"
+    )
+    altered_profile = copy.deepcopy(robot_profile)
+    altered_profile["poses"]["pinch_index"]["cmd_u8"][0] = 180.0
+    extractor = HandIntentExtractor(glove_profile)
+    intent = extractor.extract(_pose_frame("pinch_index"))
+    reference = RobotMapper(robot_profile, glove_profile).map(intent)
+    altered = RobotMapper(altered_profile, glove_profile).map(intent)
+
+    assert reference.raw_command[0] != altered.raw_command[0]
+    assert reference.actuation == altered.actuation
+    assert reference.q_nominal == altered.q_nominal
+    simulation = robot_profile["simulation"]
+    expected_q = (
+        np.asarray(simulation["open"], dtype=float)
+        + np.asarray(reference.actuation)
+        * (
+            np.asarray(simulation["closed"], dtype=float)
+            - np.asarray(simulation["open"], dtype=float)
+        )
+    )
+    assert np.allclose(reference.q_nominal, expected_q)
+
+
+def test_recorded_frame_style_replay_produces_both_models_every_frame():
+    glove_profile = build_glove_profile(
+        _glove_captures(),
+        glove_id="FFG-REPLAY",
+        operator="tester",
+    )
+    extractor = HandIntentExtractor(glove_profile)
+    profiles = {
+        model: load_robot_profile(
+            PROFILE_DIR / f"{model.lower()}_seed_profile.json", model
+        )
+        for model in ("G20", "O6")
+    }
+    mappers = {
+        model: RobotMapper(profile, glove_profile)
+        for model, profile in profiles.items()
+    }
+    previous = {
+        model: np.asarray(profile["poses"]["open_spread"]["cmd_u8"], dtype=float)
+        for model, profile in profiles.items()
+    }
+    open_frame = _pose_frame("open_spread")
+    fist_frame = _pose_frame("fist")
+    replay = [
+        (
+            (1.0 - ratio) * open_frame
+            + ratio * fist_frame
+        )
+        for ratio in np.concatenate(
+            [np.linspace(0.0, 1.0, 60), np.linspace(1.0, 0.0, 60)]
+        )
+    ]
+    for raw_frame in replay:
+        intent = extractor.extract(raw_frame)
+        outputs = {model: mapper.map(intent) for model, mapper in mappers.items()}
+        assert len(outputs["G20"].command) == 20
+        assert len(outputs["O6"].command) == 6
+        for model, output in outputs.items():
+            command = np.asarray(output.command, dtype=float)
+            active = sorted(int(channel["index"]) for channel in profiles[model]["channels"])
+            assert np.all(np.abs(command[active] - previous[model][active]) <= 16.0)
+            assert all(0.0 <= value <= 1.0 for value in output.actuation)
+            assert len(output.sim_joint_names) == len(set(output.sim_joint_names))
+            previous[model] = command
+
+
+def test_robot_calibration_fits_o6_fusion_and_requires_complete_capture(tmp_path):
+    seed = load_robot_profile(PROFILE_DIR / "o6_seed_profile.json", "O6")
+    captures = {
+        name: {
+            "cmd_u8": pose["cmd_u8"],
+            "state_u8": pose["cmd_u8"],
+            "state_names": seed["command_names"],
+            "status": pose["status"],
+            "confirmed": True,
+        }
+        for name, pose in seed["poses"].items()
+    }
+    with pytest.raises(ValueError):
+        build_robot_profile(
+            seed,
+            {key: value for key, value in captures.items() if key != "hook"},
+            serial_number="O6-TEST",
+            can_interface="can1",
+        )
+    profile = build_robot_profile(
+        seed,
+        captures,
+        serial_number="O6-TEST",
+        can_interface="can1",
+        operator="tester",
+        approved_for_control=True,
+    )
+    path = save_profile(profile, tmp_path / "o6.json")
+    loaded = load_robot_profile(path, "O6")
+    assert loaded["approved_for_control"] is True
+    assert loaded["mapping_strategy"] == "paired_continuous_v1"
+    assert loaded["simulation"]["mapping_strategy"] == "semantic_urdf_v1"
+    assert "pinch" not in loaded
+    for channel in loaded["channels"][2:]:
+        assert pytest.approx(sum(channel["sources"].values())) == 1.0
+        assert list(channel["sources"].values()) != [0.5, 0.5]
+
+
+def test_robot_pose_checkpoint_resumes_and_rejects_identity_mismatch(tmp_path):
+    seed = load_robot_profile(PROFILE_DIR / "g20_seed_profile.json", "G20")
+    names = seed["command_names"]
+    command = seed["poses"]["open_spread"]["cmd_u8"]
+    captures = {
+        "open_spread": {
+            "cmd_u8": command,
+            "command_names": names,
+            "state_u8": command,
+            "state_names": names,
+            "status": "exact",
+            "confirmed": True,
+            "captured_at": "2026-07-23T00:00:00+00:00",
+        }
+    }
+    output = tmp_path / "hand_g20.json"
+    checkpoint = _checkpoint_path(str(output), None)
+    document = _checkpoint_document(
+        model="G20",
+        seed=seed,
+        output=str(output),
+        serial_number="G20-TEST",
+        firmware="unknown",
+        can_interface="can0",
+        operator="tester",
+        captures=captures,
+    )
+    save_profile(document, checkpoint)
+    loaded = _load_checkpoint(
+        checkpoint,
+        model="G20",
+        seed=seed,
+        output=str(output),
+        serial_number="G20-TEST",
+        firmware="unknown",
+        can_interface="can0",
+        operator="tester",
+    )
+    assert loaded == captures
+    assert checkpoint.name == "hand_g20.checkpoint.json"
+    with pytest.raises(RuntimeError, match="SN/固件/CAN"):
+        _load_checkpoint(
+            checkpoint,
+            model="G20",
+            seed=seed,
+            output=str(output),
+            serial_number="OTHER-HAND",
+            firmware="unknown",
+            can_interface="can0",
+            operator="tester",
+        )
+
+
+def test_robot_capture_rejects_commands_outside_profile_safety_bounds():
+    seed = load_robot_profile(PROFILE_DIR / "g20_seed_profile.json", "G20")
+    command = list(seed["poses"]["open_spread"]["cmd_u8"])
+    command[10] = 79.0
+    assert _command_bound_violations(seed, command) == (
+        "thumb_cmc_yaw=79，允许[80, 255]",
+    )
+
+
+def test_profile_rejects_incomplete_and_wrong_model(tmp_path):
+    source = json.loads((PROFILE_DIR / "g20_seed_profile.json").read_text())
+    source["command_names"] = source["command_names"][:-1]
+    path = tmp_path / "bad.json"
+    path.write_text(json.dumps(source))
+    with pytest.raises(ProfileError):
+        load_robot_profile(path, "G20")
+    with pytest.raises(ProfileError):
+        load_robot_profile(PROFILE_DIR / "g20_seed_profile.json", "O6")
+
+
+def test_monotonic_interpolation_filter_and_limits():
+    assert piecewise_linear(0.5, [[0.0, 255.0], [1.0, 0.0]]) == 127.5
+    command_filter = CommandFilter(
+        [0.0, 0.0],
+        {"ema_alpha": 1.0, "max_step_u8": 16.0, "deadband_u8": 2.0},
+    )
+    assert command_filter.apply([255.0, 1.0]).tolist() == [16.0, 0.0]
+
+
+def test_zero_deadband_tracks_a_slow_ramp_without_staircase_jumps():
+    command_filter = CommandFilter(
+        [255.0],
+        {
+            "ema_alpha": 0.55,
+            "max_step_u8": 16.0,
+            "deadband_u8": 0.0,
+        },
+    )
+    output = np.asarray(
+        [
+            round(float(command_filter.apply([target])[0]))
+            for target in np.linspace(255.0, 0.0, 256)
+        ]
+    )
+    delta = np.abs(np.diff(output))
+    assert np.max(delta) <= 1.0
+    assert np.count_nonzero(delta == 1.0) >= 250
+
+
+def test_vector_kalman_filter_reduces_glove_noise_and_resets_after_gap():
+    input_filter = VectorKalmanFilter(
+        2,
+        process_variance=1e-5,
+        measurement_variance=5e-4,
+        reset_gap_s=0.35,
+    )
+    first = input_filter.apply([1.0, 2.0], timestamp_s=1.0)
+    assert first.tolist() == [1.0, 2.0]
+
+    # Let covariance reach its steady operating range, then compare an
+    # alternating sensor disturbance with the filtered output.
+    timestamp = 1.0
+    for _ in range(60):
+        timestamp += 1.0 / 30.0
+        input_filter.apply([1.0, 2.0], timestamp_s=timestamp)
+    raw = []
+    filtered = []
+    for offset in ([0.08, -0.08] * 40):
+        timestamp += 1.0 / 30.0
+        sample = [1.0 + offset, 2.0 - offset]
+        raw.append(sample)
+        filtered.append(
+            input_filter.apply(sample, timestamp_s=timestamp)
+        )
+    assert np.var(np.asarray(filtered), axis=0).max() < (
+        0.1 * np.var(np.asarray(raw), axis=0).min()
+    )
+
+    # A reconnection must not interpolate from stale pre-disconnect state.
+    reset = input_filter.apply([4.0, 5.0], timestamp_s=timestamp + 0.5)
+    assert reset.tolist() == [4.0, 5.0]
+
+
+def test_acceleration_limited_filter_has_bounded_velocity_and_acceleration():
+    command_filter = CommandFilter(
+        [255.0],
+        {
+            "ema_alpha": 0.55,
+            "max_step_u8": 10.0,
+            "max_acceleration_u8_per_frame2": 2.5,
+            "deadband_u8": 0.0,
+        },
+    )
+    trajectory = np.asarray(
+        [255.0]
+        + [
+            float(command_filter.apply([0.0])[0])
+            for _ in range(80)
+        ]
+    )
+    velocity = np.diff(trajectory)
+    acceleration = np.diff(velocity)
+    assert np.max(np.abs(velocity)) <= 10.0
+    assert np.max(np.abs(acceleration)) <= 2.5 + 1e-9
+    assert trajectory[-1] == 0.0
+
+
+def test_optional_ema_filter_keeps_its_configured_step_limit():
+    command_filter = CommandFilter(
+        [255.0],
+        {
+            "mode": "ema",
+            "ema_alpha": 0.5,
+            "max_step_u8": 20.0,
+            "max_acceleration_u8_per_frame2": 20.0,
+            "deadband_u8": 0.0,
+        },
+    )
+    trajectory = [
+        float(command_filter.apply([0.0])[0])
+        for _ in range(4)
+    ]
+    assert trajectory == [235.0, 215.0, 195.0, 175.0]
+    assert command_filter.velocity.tolist() == [-20.0]
+
+
+def test_passthrough_filter_leaves_interpolation_to_motor_firmware():
+    command_filter = CommandFilter(
+        [255.0, 10.0],
+        {
+            "mode": "passthrough",
+            "ema_alpha": 1.0,
+            "max_step_u8": 255.0,
+            "max_acceleration_u8_per_frame2": 255.0,
+            "deadband_u8": 0.0,
+        },
+    )
+    assert command_filter.apply([0.0, 200.0]).tolist() == [0.0, 200.0]
+    assert command_filter.velocity.tolist() == [-255.0, 190.0]
+
+
+def test_runtime_defaults_match_effective_legacy_left_g20_path():
+    assert DEFAULT_INPUT_FILTER_ENABLED is False
+    assert DEFAULT_COMMAND_FILTER_CONFIG == {
+        "mode": "passthrough",
+        "ema_alpha": 1.0,
+        "max_step_u8": 255.0,
+        "max_acceleration_u8_per_frame2": 255.0,
+        "deadband_u8": 0.0,
+    }
+
+
+def test_pose_verifier_transition_is_bounded_and_reaches_exact_target():
+    transition = np.asarray(
+        transition_commands(
+            [0.0, 255.0, 100.0],
+            [10.0, 0.0, 101.0],
+            max_step_u8=4.0,
+        )
+    )
+    complete = np.vstack(([0.0, 255.0, 100.0], transition))
+    assert np.max(np.abs(np.diff(complete, axis=0))) <= 4.0
+    assert transition[-1].tolist() == [10.0, 0.0, 101.0]
+
+
+def test_pose_verifier_report_is_traceable_and_state_is_not_angle_gt():
+    profile = load_robot_profile(
+        PROFILE_DIR / "g20_seed_profile.json",
+        "G20",
+    )
+    summary = state_error_summary(
+        profile["command_names"],
+        [100.0] * 20,
+        [99.0] * 19 + [80.0],
+        active_indices=[0, 19],
+    )
+    assert summary["max_abs_error_u8"] == 20.0
+    assert summary["worst_channels"][0]["name"] == "pinky_pip"
+    report = verification_document(
+        profile,
+        operator="tester",
+        command_topic="/g20/cb_left_hand_control_cmd",
+        state_topic="/g20/cb_left_hand_state",
+        results={"open_spread": {"visual_passed": True, **summary}},
+    )
+    assert report["profile_sha256"] == profile["_profile_sha256"]
+    assert report["state_is_angle_ground_truth"] is False
+    assert report["results"]["open_spread"]["visual_passed"] is True
+
+
+def test_mapper_reenable_rebases_filter_to_current_driver_state():
+    profile = load_robot_profile(PROFILE_DIR / "o6_seed_profile.json", "O6")
+    mapper = RobotMapper(profile)
+    current_state = np.asarray([120.0] * 6)
+    mapper.reset(current_state)
+    intent = {name: 0.0 for name in BASE_INTENT_NAMES}
+    intent.update(
+        {f"pinch_{role}": 0.0 for role in ("index", "middle", "ring", "pinky")}
+    )
+    intent["power_grasp"] = 0.0
+    command = np.asarray(mapper.map(intent).command, dtype=float)
+    assert np.all(np.abs(command - current_state) <= 16.0)
+
+
+def test_timeout_policy_disables_all_for_glove_and_one_for_driver():
+    disable_all, models = evaluate_timeouts(
+        now=10.0,
+        enabled={"G20": True, "O6": True},
+        last_glove_time=9.0,
+        driver_state_time={"G20": 9.9, "O6": 9.9},
+        driver_state_valid={"G20": True, "O6": True},
+        glove_timeout=0.35,
+        driver_timeout=1.0,
+    )
+    assert disable_all and not models
+    disable_all, models = evaluate_timeouts(
+        now=10.0,
+        enabled={"G20": True, "O6": True},
+        last_glove_time=9.9,
+        driver_state_time={"G20": 8.0, "O6": 9.9},
+        driver_state_valid={"G20": True, "O6": True},
+        glove_timeout=0.35,
+        driver_timeout=1.0,
+    )
+    assert not disable_all and models == ("G20",)
+
+
+def test_simulator_bridge_maps_by_name_and_fails_fast():
+    ordered = reorder_named_target(
+        ["joint_b", "joint_a"], [2.0, 1.0], ["joint_a", "joint_b"]
+    )
+    assert ordered.tolist() == [1.0, 2.0]
+    with pytest.raises(JointNameMismatch):
+        reorder_named_target(["joint_a"], [1.0], ["joint_a", "joint_b"])
+
+
+@pytest.mark.parametrize("model", ["G20", "O6"])
+def test_nominal_joint_names_match_independent_urdf_joints(model):
+    profile = load_robot_profile(
+        PROFILE_DIR / f"{model.lower()}_seed_profile.json", model
+    )
+    urdf = (
+        PACKAGE
+        / "linkerhand_retarget"
+        / "assets"
+        / "robots"
+        / "hands"
+        / "linker_hand"
+        / f"{model.lower()}_left"
+        / f"linkerhand_{model.lower()}_left.urdf"
+    )
+    root = ET.parse(urdf).getroot()
+    independent = {
+        joint.attrib["name"]
+        for joint in root.findall("joint")
+        if joint.attrib.get("type") != "fixed" and joint.find("mimic") is None
+    }
+    assert set(profile["simulation"]["joint_names"]) == independent
+    assert hashlib.sha256(urdf.read_bytes()).hexdigest() == (
+        profile["simulation"]["urdf_sha256"]
+    )
+
+
+def test_runtime_mapping_has_no_legacy_fallback_magic():
+    mapping_path = (
+        PACKAGE
+        / "linkerhand_retarget"
+        / "motion"
+        / "linkerforce_v2"
+        / "mapping.py"
+    )
+    tree = ast.parse(mapping_path.read_text())
+    numbers = {
+        float(node.value)
+        for node in ast.walk(tree)
+        if isinstance(node, ast.Constant)
+        and isinstance(node.value, (int, float))
+    }
+    assert not ({2.2, -2.5, 0.1, 0.7} & numbers)
+    source = mapping_path.read_text()
+    assert "PinchSelector" not in source
+    assert "piecewise_inverse" not in source
+    assert "enter_threshold" not in source
+    assert "release_threshold" not in source
+    for seed in ("g20_seed_profile.json", "o6_seed_profile.json"):
+        seed_source = (PROFILE_DIR / seed).read_text()
+        assert "enter_threshold" not in seed_source
+        assert "exit_threshold" not in seed_source
+        assert "confirm_frames" not in seed_source