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

2026-09-15 15:14:32 +08:00
commit a9013afdc8
816 changed files with 71500 additions and 0 deletions
@@ -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()