init pro_sdk

tools/calibrate_joints.py

@@ -0,0 +1,99 @@
+#!/usr/bin/env python3
+"""关节活动范围校准 - 手动转动法.
+
+流程:
+  1. 狗悬空 + L2+B damping
+  2. 跑这个脚本
+  3. 慢慢把每条腿的每个关节转到极限两端 (软力, 听到阻力就停)
+  4. 输出每个关节实测的 min/max/range
+"""
+import argparse
+import os
+import signal
+import sys
+import time
+
+sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))
+from go1_pro_sdk import MCUClient, LowCmd, JOINT_NAMES
+from go1_pro_sdk.utils.constants import JOINT_LIMITS_URDF
+
+
+running = True
+
+
+def sigint(s, f):
+    global running
+    running = False
+
+
+def main():
+    parser = argparse.ArgumentParser()
+    parser.add_argument('--state', default=None)
+    parser.add_argument('--duration', type=float, default=60)
+    args = parser.parse_args()
+    signal.signal(signal.SIGINT, sigint)
+
+    print('🦾 关节活动范围校准')
+    print(f'  采样时长: {args.duration}s')
+    print()
+    print('操作:')
+    print('  1. 狗悬空, L2+B damping (电机失能)')
+    print('  2. 按顺序逐条腿/逐个关节转到极限')
+    print('  3. Ctrl+C 提前结束')
+    input('回车开始采样...')
+
+    with MCUClient(state_path=args.state) as client:
+        recv = client.wake_mcu(50)
+        if recv == 0:
+            print('❌ 无回包')
+            return 1
+
+        damping = LowCmd().all_damping()
+        q_min = [float('inf')] * 12
+        q_max = [float('-inf')] * 12
+
+        start = time.time()
+        last_print = 0
+        n_decoded = 0
+
+        while running and time.time() - start < args.duration:
+            client.send(damping)
+            time.sleep(0.01)
+            state = client.recv_latest()
+            if state is None:
+                continue
+            n_decoded += 1
+            for i in range(12):
+                q = state.motorState[i].q
+                if q < q_min[i]: q_min[i] = q
+                if q > q_max[i]: q_max[i] = q
+
+            now = time.time()
+            if now - last_print >= 2.0:
+                print(f'\r⏱  {now-start:5.1f}/{args.duration:.0f}s  解码 {n_decoded} 帧',
+                      end='', flush=True)
+                last_print = now
+
+    print('\n\n实测范围 vs URDF 标称:')
+    print('-' * 80)
+    print(f'{"关节":8} {"实测min":>9} {"实测max":>9} {"范围":>8}   '
+          f'{"URDF min":>9} {"URDF max":>9}')
+    for i, name in enumerate(JOINT_NAMES):
+        jt = ('hip' if i % 3 == 0 else 'thigh' if i % 3 == 1 else 'knee')
+        urdf_lo, urdf_hi = JOINT_LIMITS_URDF[jt]
+        lo, hi = q_min[i], q_max[i]
+        if lo == float('inf'):
+            print(f'{name:8} {"未采到":>9}')
+            continue
+        print(f'{name:8} {lo:+9.4f} {hi:+9.4f} {hi-lo:8.4f}   '
+              f'{urdf_lo:+9.4f} {urdf_hi:+9.4f}')
+
+    print('\nJOINT_LIMITS_MEASURED = {')
+    for i, name in enumerate(JOINT_NAMES):
+        if q_min[i] == float('inf'): continue
+        print(f"    '{name}': ({q_min[i]:+.4f}, {q_max[i]:+.4f}),")
+    print('}')
+
+
+if __name__ == '__main__':
+    sys.exit(main() or 0)