init pro_sdk

2026-06-20 20:02:36 +08:00
commit 5ecd422a59
49 changed files with 4226 additions and 0 deletions
--- a/examples/example_remote_control.py
+++ b/examples/example_remote_control.py
@@ -0,0 +1,100 @@
+#!/usr/bin/env python3
+"""用遥控器摇杆控制 FR 腿 (Demo: 摇杆 X → 髋外摆, 摇杆 Y → 大腿).
+
+按键映射:
+  左摇杆 X  → FR_0 hip (±0.4 rad)
+  左摇杆 Y  → FR_1 thigh (mid ± 0.5 rad)
+  L2 按下   → 立即 damping 退出 (软急停)
+  其他腿    → Damping
+
+⚠️ 狗必须悬空 (脚架/吊带)
+"""
+import argparse
+import signal
+import sys
+import time
+
+from go1_pro_sdk import (
+    MCUClient, LowCmd, MotorCmd, MotorMode,
+    apply_safety, PowerProtectViolation,
+)
+
+DT = 0.005    # 200 Hz 即可, 摇杆响应慢
+running = True
+
+
+def sigint(s, f):
+    global running
+    running = False
+
+
+def main():
+    p = argparse.ArgumentParser()
+    p.add_argument('--state', default=None)
+    p.add_argument('--duration', type=float, default=60)
+    p.add_argument('--hip-range', type=float, default=0.4, help='hip 摆幅 (rad)')
+    p.add_argument('--thigh-range', type=float, default=0.5, help='thigh 摆幅 (rad)')
+    args = p.parse_args()
+    signal.signal(signal.SIGINT, sigint)
+
+    print('🎮 摇杆遥控 FR 腿')
+    print(f'  左 X → hip ±{args.hip_range}, 左 Y → thigh ±{args.thigh_range}')
+    print(f'  L2 按下 → 退出')
+
+    with MCUClient(state_path=args.state) as client:
+        if client.wake_mcu(50) == 0:
+            print('❌ 无回包')
+            return 1
+        state = client.last_state
+        mid = [state.motorState[i].q for i in range(3)]
+        print(f'  初始角: hip={mid[0]:+.3f} thigh={mid[1]:+.3f} knee={mid[2]:+.3f}')
+
+        start = time.time()
+        try:
+            while running and time.time() - start < args.duration:
+                t0 = time.time()
+                state = client.recv_latest() or state
+                r = state.remote
+
+                # L2 按下 → 退出
+                if r.is_pressed('L2'):
+                    print('\nL2 → 退出')
+                    break
+
+                # 摇杆 → 目标 q
+                q_hip = mid[0] + r.lx * args.hip_range
+                q_thigh = mid[1] + r.ly * args.thigh_range
+                q_knee = mid[2]   # 膝盖保持不变
+
+                cmd = LowCmd()
+                for i, q in enumerate([q_hip, q_thigh, q_knee]):
+                    cmd.motorCmd[i] = MotorCmd(
+                        mode=MotorMode.Servo, q=q, dq=0, tau=0, Kp=3, Kd=0.5)
+
+                try:
+                    apply_safety(cmd, state, power_factor=1,
+                                 position_protect_limit=0.5)
+                except PowerProtectViolation as e:
+                    print(f'\n⚠️ {e}')
+                    break
+
+                client.send(cmd)
+
+                # 每秒打印一次
+                if int(time.time() * 2) != int((time.time() - DT) * 2):
+                    print(f'  L=({r.lx:+.2f},{r.ly:+.2f})  '
+                          f'des=({q_hip:+.3f},{q_thigh:+.3f}) '
+                          f'act=({state.motorState[0].q:+.3f},'
+                          f'{state.motorState[1].q:+.3f})')
+
+                sleep_t = DT - (time.time() - t0)
+                if sleep_t > 0:
+                    time.sleep(sleep_t)
+        finally:
+            print('\n安全停机...')
+            client.safe_stop()
+            print('✅ 退出')
+
+
+if __name__ == '__main__':
+    sys.exit(main() or 0)