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_sin_leg.py
+++ b/examples/example_sin_leg.py
@@ -0,0 +1,111 @@
+#!/usr/bin/env python3
+"""安全单腿正弦测试 (保守参数, 小幅摆动).
+
+跟 example_position.py 的区别:
+  - 默认振幅小 (±0.3 rad), 频率慢 (0.5 Hz)
+  - sin 中点保持当前实际关节角, 不强行移到 -2.0
+  - 适合首次试电机控制
+
+⚠️ 狗悬空, Ctrl+C 紧急停止
+"""
+import argparse
+import math
+import signal
+import sys
+import time
+
+from go1_pro_sdk import (
+    MCUClient, LowCmd, MotorCmd, MotorMode,
+    apply_safety, PowerProtectViolation,
+)
+
+DT = 0.002
+running = True
+
+
+def sigint(s, f):
+    global running
+    running = False
+
+
+def main():
+    p = argparse.ArgumentParser()
+    p.add_argument('--state', default=None)
+    p.add_argument('--amplitude', type=float, default=0.3, help='sin 振幅 (rad)')
+    p.add_argument('--freq', type=float, default=0.5, help='sin 频率 (Hz)')
+    p.add_argument('--duration', type=float, default=10)
+    p.add_argument('--power-factor', type=int, default=1)
+    args = p.parse_args()
+    signal.signal(signal.SIGINT, sigint)
+
+    print(f'🦿 单腿 sin 测试 — 振幅 {args.amplitude}, 频率 {args.freq}Hz, '
+          f'{args.duration}s, power={args.power_factor}')
+
+    with MCUClient(state_path=args.state) as client:
+        if client.wake_mcu(50) == 0:
+            print('❌ 无回包')
+            return 1
+        state = client.last_state
+
+        # 用实际值作为 sin 中点
+        mid = [state.motorState[i].q for i in range(3)]
+        print(f'  sin 中点 (= 当前角): {[f"{x:+.3f}" for x in mid]}')
+
+        # Phase 1: 软启动 (Kp/Kd 从 0 慢慢升)
+        ramp_steps = 500
+        print(f'  软启动 {ramp_steps*DT:.1f}s...')
+
+        try:
+            n_steps = int(args.duration / DT)
+            for step in range(n_steps + ramp_steps):
+                if not running:
+                    break
+                t0 = time.time()
+                state = client.recv_latest() or state
+
+                if step < ramp_steps:
+                    # 软启动: Kp/Kd 从 0 到目标
+                    ramp = step / ramp_steps
+                    Kp = 3 * ramp
+                    Kd = 0.5 * ramp
+                    qDes = list(mid)
+                else:
+                    Kp = 3
+                    Kd = 0.5
+                    t = (step - ramp_steps) * DT
+                    sin_v = args.amplitude * math.sin(2 * math.pi * args.freq * t)
+                    qDes = [mid[0], mid[1] + sin_v, mid[2] - sin_v * 1.5]
+
+                cmd = LowCmd()
+                for i in range(3):
+                    cmd.motorCmd[i] = MotorCmd(
+                        mode=MotorMode.Servo,
+                        q=qDes[i], dq=0, tau=0,
+                        Kp=Kp, Kd=Kd,
+                    )
+
+                try:
+                    apply_safety(cmd, state, power_factor=args.power_factor,
+                                 position_protect_limit=0.5)
+                except PowerProtectViolation as e:
+                    print(f'⚠️ {e}')
+                    break
+
+                client.send(cmd)
+
+                if step % 250 == 0 and step > 0:
+                    actual = [state.motorState[i].q for i in range(3)]
+                    print(f'  t={step*DT:5.2f}s des={[f"{x:+.3f}" for x in qDes]} '
+                          f'act={[f"{x:+.3f}" for x in actual]}')
+
+                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)