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,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)