Initialize FFG left-glove L20 teleoperation workspace with guarded trial modes

tools/check_ffg_readonly.py

@@ -0,0 +1,57 @@
+"""Query FFG identity/angles only. No force commands, ROS publication or CAN access."""
+import sys
+import time
+from pathlib import Path
+import json
+import numpy as np
+import serial
+
+sys.path.insert(0, str(Path(__file__).resolve().parents[1] / 'src/linkerhand_retarget/linkerhand_retarget'))
+from linkerhand.linkerforce import FrameParser, FrameHandler
+from linkerhand.constants import HandType
+
+handler = FrameHandler(HandType.left)
+parser = FrameParser()
+samples = []
+identity = None
+print('准备开始：请戴好手套，接下来按提示缓慢张手、握拳，不要强掰关节。', file=sys.stderr, flush=True)
+for remaining in (3, 2, 1):
+    print(f'倒计时 {remaining}', file=sys.stderr, flush=True)
+    time.sleep(1)
+with serial.Serial('/dev/ttyUSB0', 2000000, timeout=0.1, exclusive=True) as port:
+    port.write(handler.pack_version_query())
+    start = time.monotonic()
+    end = start + 20
+    next_query = 0
+    next_identity_query = start + 1
+    last_phase = -1
+    while time.monotonic() < end:
+        if identity is None and time.monotonic() >= next_identity_query:
+            port.write(handler.pack_version_query())
+            next_identity_query = time.monotonic() + 1
+        phase = min(int((time.monotonic() - start) // 5), 3)
+        if phase != last_phase:
+            gesture = ('张开手并保持', '缓慢握拳并保持', '再次张开手并保持', '再次缓慢握拳并保持')[phase]
+            print(f'{gesture}（5秒）；已收到 {len(samples)} 帧', file=sys.stderr, flush=True)
+            last_phase = phase
+        if time.monotonic() >= next_query:
+            port.write(handler.pack_position_query())
+            next_query = time.monotonic() + 0.1
+        for byte in port.read(port.in_waiting or 1):
+            if parser.process_byte(byte):
+                result = handler.handle_frame(parser.frame_buf)
+                parser.reset()
+                if result and 'version' in result:
+                    identity = result
+                if result and 'poslist' in result:
+                    samples.append(result['poslist'])
+report = {'identity': identity, 'frames': len(samples), 'baudrate': 2000000,
+          'writes': ['version query 0x01', 'position query 0x03']}
+if samples:
+    a = np.asarray(samples)
+    report.update(dimension=int(a.shape[1]), finite=bool(np.isfinite(a).all()),
+                  last_degrees=np.rad2deg(a[-1]).round(2).tolist(),
+                  range_degrees=np.rad2deg(np.ptp(a, axis=0)).round(2).tolist())
+print(json.dumps(report, ensure_ascii=False, indent=2))
+if not identity or identity.get('raw_handtype') != 'Left' or not samples:
+    sys.exit(1)

tools/check_hand_touch_readonly.py

@@ -0,0 +1,78 @@
+#!/usr/bin/env python3
+"""Type-2 G20-protocol matrix queries only. No driver/ROS/serial/motor writes."""
+import argparse
+import json
+import subprocess
+import time
+from pathlib import Path
+import can
+
+NAMES = ['thumb', 'index', 'middle', 'ring', 'little']
+ROW_IDS = {i * 16: i for i in range(12)}
+
+
+def query(bus, command, payload=(), timeout=0.15):
+    # Drain old traffic so incomplete responses never reuse cached matrices.
+    deadline = time.monotonic() + 0.05
+    while bus.recv(0) is not None:
+        if time.monotonic() > deadline:
+            raise RuntimeError('Unexpected continuous CAN traffic; stop other CAN clients')
+    bus.send(can.Message(arbitration_id=0x28, is_extended_id=False,
+                         data=[command, *payload]), timeout=0.2)
+    frames = []
+    end = time.monotonic() + timeout
+    while time.monotonic() < end:
+        msg = bus.recv(max(0, end-time.monotonic()))
+        if msg is None:
+            break
+        if msg.is_error_frame:
+            raise RuntimeError('CAN error frame')
+        if (msg.is_rx and not msg.is_extended_id and msg.arbitration_id == 0x28
+                and msg.data and msg.data[0] == command):
+            frames.append(list(msg.data))
+    return frames
+
+
+def main():
+    parser = argparse.ArgumentParser(description=__doc__)
+    parser.add_argument('--duration', type=float, default=25)
+    args = parser.parse_args()
+    if not 1 <= args.duration <= 120:
+        parser.error('duration must be 1..120 seconds')
+    processes = subprocess.check_output(['ps', '-eo', 'args='], text=True)
+    if any('linker_hand_sdk' in line and 'python' in line for line in processes.splitlines()):
+        raise SystemExit('Stop linker_hand_sdk first. This test requires no motion controller.')
+    records = []
+    print('READ ONLY: type-2 touch raw intensity, NOT calibrated N or grams.', flush=True)
+    with can.Bus(channel='can0', interface='socketcan', receive_own_messages=False) as bus:
+        frames = query(bus, 0xB0)
+        if not any(len(f) >= 2 and f[1] == 2 for f in frames):
+            raise RuntimeError(f'Expected touch type 2, got {frames}')
+        start = time.monotonic()
+        try:
+            while time.monotonic()-start < args.duration:
+                sample = {'elapsed': round(time.monotonic()-start, 2), 'fingers': {}}
+                for i, name in enumerate(NAMES):
+                    frames = query(bus, 0xB1+i, [0xC6], timeout=0.08)
+                    rows = {ROW_IDS[f[1]]: f[2:] for f in frames
+                            if len(f) == 8 and f[1] in ROW_IDS}
+                    complete = len(rows) == 12
+                    values = [v for row in rows.values() for v in row]
+                    sample['fingers'][name] = {
+                        'complete': complete, 'rows': len(rows),
+                        'max': max(values) if complete else None,
+                        'sum_raw': sum(values) if complete else None,
+                        'matrix': [rows.get(r) for r in range(12)],
+                    }
+                records.append(sample)
+                print(' '.join(f"{name}:max={v['max']} rows={v['rows']}/12"
+                               for name,v in sample['fingers'].items()), flush=True)
+        finally:
+            destination = Path(__file__).resolve().parents[1]/'diagnostics'/f'touch-readonly-{time.time_ns()}.json'
+            destination.parent.mkdir(exist_ok=True)
+            destination.write_text(json.dumps(records, indent=2))
+            print(f'Saved: {destination}', flush=True)
+
+
+if __name__ == '__main__':
+    main()

tools/ffg_trial.sh

@@ -0,0 +1,40 @@
+#!/usr/bin/env bash
+set -eo pipefail
+cd "$(dirname "$0")/.."
+source /opt/ros/jazzy/setup.bash
+source install/setup.bash
+export ROS_DOMAIN_ID=74
+export ROS_AUTOMATIC_DISCOVERY_RANGE=LOCALHOST
+case "${1:-}" in
+  glove)
+    exec ros2 run linkerhand_retarget handretarget --ros-args \
+      -p calibration:=false -p ports:='["/dev/ttyUSB0"]' -p baudrate:=2000000 \
+      -r /cb_left_hand_control_cmd:=/ffg_trial/left_cmd \
+      -r /cb_left_hand_matrix_touch:=/ffg_trial/unused_left_touch \
+      -r /cb_right_hand_matrix_touch:=/ffg_trial/unused_right_touch \
+      -r /hand_teleop_param:=/ffg_trial/unused_params
+    ;;
+  hand|half|full|follow)
+    scale=0.0
+    slew=10.0
+    speed=30
+    confirmation=ARM
+    case "$1" in
+      hand) echo '相对当前位置 +/-10 SDK单位小幅测试。' ;;
+      half) scale=0.5; confirmation=ARM50; echo '50%位置映射：目标=启动时机械手位置+0.5*(手套映射位置-启动时机械手位置)。' ;;
+      full) scale=1.0; confirmation=ARM100; echo '100%标定位置映射。必须先完成50%空载检查；不是满速/满力。' ;;
+      follow) scale=1.0; slew=60.0; speed=60; confirmation=FOLLOW; echo '完整位置跟随档：仅在慢速全范围已验证正常后使用。' ;;
+    esac
+    echo "空载测试，速度${speed}、目标缓变${slew}单位/秒。收到手套数据后可能立即开始运动。不是急停保护。"
+    read -r -p "确认周围无人、无夹持物且可立即断电，输入 $confirmation：" answer
+    [[ "$answer" == "$confirmation" ]] || exit 1
+    exec ros2 run linker_hand_ros2_sdk linker_hand_sdk --ros-args \
+      -p hand_type:=left -p hand_joint:=G20 -p can:=can0 \
+      -p is_touch:=false -p modbus:=None -p safe_trial:=true \
+      -p trial_motion_scale:="$scale" \
+      -p trial_slew_rate:="$slew" -p trial_motor_speed:="$speed" \
+      -p state_poll_rate:=10.0 -p velocity_poll_rate:=2.0 \
+      -r /cb_left_hand_control_cmd:=/ffg_trial/left_cmd
+    ;;
+  *) echo "用法：bash tools/ffg_trial.sh glove|hand|half|full|follow"; exit 2 ;;
+esac

tools/test_trial_guard.py

@@ -0,0 +1,95 @@
+import sys
+import unittest
+from pathlib import Path
+sys.path.insert(0, str(Path(__file__).resolve().parents[1] / 'src/linker_hand_ros2_sdk'))
+from linker_hand_ros2_sdk.trial_guard import TrialGuard
+
+class GuardTests(unittest.TestCase):
+    def test_no_input_no_command(self):
+        self.assertIsNone(TrialGuard([100]*20).step(100))
+
+    def test_anchor_no_jump(self):
+        g = TrialGuard([100]*20)
+        g.accept([255]*20, 0)
+        self.assertEqual(g.step(0.05), [100]*20)
+
+    def test_slew_and_envelope(self):
+        g = TrialGuard([100]*20)
+        g.accept([100]*20, 0)
+        for i in range(1, 101):
+            now = i * 0.05
+            g.accept([255]*20, now)
+            out = g.step(now)
+            self.assertLessEqual(max(out), min(110, 100 + i*0.5 + 0.5))
+        self.assertEqual(out, [110]*20)
+
+    def test_timeout_latches_even_if_input_resumes(self):
+        g = TrialGuard([100]*20)
+        g.accept([100]*20, 0)
+        self.assertIsNone(g.step(0.31))
+        g.accept([120]*20, 0.32)
+        self.assertIsNone(g.step(0.33))
+        self.assertTrue(g.locked)
+
+    def test_callback_gap_latches(self):
+        g = TrialGuard([100]*20)
+        g.accept([100]*20, 0)
+        g.accept([120]*20, 0.31)
+        self.assertTrue(g.locked)
+
+    def test_half_mapping(self):
+        g = TrialGuard([100]*20, 0.5)
+        g.accept([200]*20, 0)
+        self.assertEqual(g.target, [150]*20)
+        self.assertEqual(g.step(0), [100]*20)
+        self.assertEqual(g.step(0.1), [100]*20)  # 0.5-unit capped step
+
+    def test_full_mapping_reaches_target_slowly(self):
+        g = TrialGuard([100]*20, 1.0)
+        g.accept([200]*20, 0)
+        self.assertEqual(g.target, [200]*20)
+        self.assertEqual(g.step(0), [100]*20)
+        for i in range(1, 241):
+            now = i * 0.05
+            g.accept([200]*20, now)
+            out = g.step(now)
+            self.assertLessEqual(max(out), min(200, 100 + i*0.5 + 0.5))
+        self.assertEqual(out, [200]*20)
+
+    def test_scaled_modes_bounds_and_timeout(self):
+        for scale in (0.5, 1.0):
+            for initial, target in ((0,255),(255,0)):
+                g = TrialGuard([initial]*20, scale)
+                g.accept([target]*20, 0)
+                self.assertTrue(all(0 <= x <= 255 for x in g.target))
+                self.assertIsNone(g.step(0.31))
+                self.assertTrue(g.locked)
+
+    def test_follow_slew_and_stall_cap(self):
+        g = TrialGuard([100]*20, 1.0, 60.0)
+        g.accept([200]*20, 0)
+        self.assertEqual(g.step(0.05), [103]*20)
+        g.accept([200]*20, 0.2)
+        self.assertEqual(g.step(0.2), [106]*20)
+        self.assertIsNone(g.step(0.51))
+        self.assertTrue(g.locked)
+
+    def test_bad_slew(self):
+        for rate in (0, -1, 61, float('inf'), float('nan')):
+            with self.assertRaises(ValueError):
+                TrialGuard([100]*20, 1.0, rate)
+
+    def test_bad_scale(self):
+        for scale in (-1, 0.75, 2, float('nan')):
+            with self.assertRaises(ValueError):
+                TrialGuard([100]*20, scale)
+
+    def test_invalid_input_latches(self):
+        for values in ([1]*19, [float('nan')]*20, [256]*20, [-1]*20):
+            g = TrialGuard([100]*20)
+            g.accept(values, 0)
+            self.assertTrue(g.locked)
+            self.assertIsNone(g.step(0))
+
+if __name__ == '__main__':
+    unittest.main()