init pro_sdk

examples/README.md

@@ -0,0 +1,61 @@
+# Examples
+
+每个示例都假定:
+- 你已经从狗上提取了 `blowfish_state.bin` (或用包内置的)
+- 已经停掉 Pi 上的 keep_sport_alive + Legged_sport + appTransit
+- 狗悬空 (脚架/吊带), 除非明确说不需要
+
+## 只读类 (无风险)
+
+### monitor_state.py — 实时监听状态
+
+```bash
+python examples/monitor_state.py --duration 30 --verbose
+```
+
+不触发任何电机, 持续发 damping LowCmd 维持 MCU 回包流, 解码后打印 12 关节角 + IMU + 电量.
+
+### monitor_remote.py — 实时监听遥控器
+
+```bash
+python examples/monitor_remote.py --duration 60
+```
+
+按下按键/拨动摇杆都会实时输出. 包括 L2 模拟值.
+
+## 控制类 (需要狗悬空!)
+
+### example_sin_leg.py — 单腿小幅 sin
+
+```bash
+python examples/example_sin_leg.py --amplitude 0.3 --freq 0.5 --duration 10
+```
+
+**保守参数**, 适合首次电机控制. sin 中点 = 当前关节角, 不强行移动到指定位置.
+
+### example_position.py — 复刻原版 example_position
+
+```bash
+python examples/example_position.py --max-steps 5000
+```
+
+跟 free-dog-sdk `example_position(lowlevel).py` **逻辑一模一样**:
+- 0..10: 记录 qInit
+- 10..400: 插值到 sin_mid_q = [0, 1.2, -2.0]
+- 400+: 1Hz sin 摆动 (FR_1 ±0.6, FR_2 ±0.9)
+
+Mac 上实测 480 Hz.
+
+### example_remote_control.py — 摇杆控制 FR 腿
+
+```bash
+python examples/example_remote_control.py
+```
+
+- 左摇杆 X → 髋外摆
+- 左摇杆 Y → 大腿前后
+- L2 按下 → 退出
+
+## 安全
+
+所有控制类示例都内置 `apply_safety(cmd, state, power_factor=1)` (10% 力矩限制), 出问题会立即停机. 见 `docs/SAFETY.md`.

examples/example_position.py

@@ -0,0 +1,164 @@
+#!/usr/bin/env python3
+"""复刻 free-dog-sdk example_position(lowlevel).py 的 PRO 版本.
+
+跟原版 EDU 例程逻辑一致:
+  0..10 步:    记录初始关节角 qInit
+  10..400 步:  Kp=5 Kd=1 插值到 sin_mid_q = [0.0, 1.2, -2.0]
+  400+ 步:     1Hz sin 摆动 (FR_1 += 0.6*sin, FR_2 += -0.9*sin)
+
+差异:
+  - 用 Go1 PRO SDK (Blowfish 加密 / PRO 格式 / 安全保护)
+  - 集成 safety 层 (power_factor=1, 位置硬限位)
+
+⚠️ 狗必须悬空 (脚架/吊带). Ctrl+C 紧急停止.
+"""
+import argparse
+import math
+import signal
+import sys
+import time
+
+from go1_pro_sdk import (
+    MCUClient, LowCmd, MotorCmd, MotorMode,
+    apply_safety, PowerProtectViolation,
+    JOINT_NAMES,
+)
+
+# 跟原版一致的常量
+SIN_MID = {'hip': 0.0, 'thigh': 1.2, 'knee': -2.0}
+DT = 0.002
+
+running = True
+
+
+def sigint(s, f):
+    global running
+    running = False
+
+
+def interp(a, b, t):
+    t = max(0.0, min(1.0, t))
+    return a * (1 - t) + b * t
+
+
+def main():
+    p = argparse.ArgumentParser()
+    p.add_argument('--state', default=None)
+    p.add_argument('--max-steps', type=int, default=5000)
+    p.add_argument('--freq-hz', type=float, default=1.0)
+    p.add_argument('--power-factor', type=int, default=1, choices=range(1, 11),
+                   help='PowerProtect 1-10, 越大越宽松')
+    args = p.parse_args()
+    signal.signal(signal.SIGINT, sigint)
+
+    print('🐕 PRO example_position (Go1 PRO SDK)')
+    print(f'  控制周期 {DT*1000:.0f}ms ({1/DT:.0f} Hz)')
+    print(f'  sin 中点: {SIN_MID}')
+    print(f'  sin 频率: {args.freq_hz} Hz')
+    print(f'  power_factor: {args.power_factor} (限 {args.power_factor*10}% 力矩)')
+    print(f'  最大步数: {args.max_steps} (≈{args.max_steps*DT:.1f}s)')
+
+    with MCUClient(state_path=args.state) as client:
+        print(f'\n[Phase 0] 唤醒 MCU...')
+        recv = client.wake_mcu(50)
+        if recv == 0:
+            print('❌ 无回包. 先停 Pi 上的 keep_sport_alive/Legged_sport/appTransit')
+            return 1
+        state = client.last_state
+        print(f'  当前 FR_0={state.motorState[0].q:.4f} '
+              f'FR_1={state.motorState[1].q:.4f} '
+              f'FR_2={state.motorState[2].q:.4f}')
+
+        # 主循环
+        qInit = [0.0, 0.0, 0.0]
+        qDes = [0.0, 0.0, 0.0]
+        Kp = [0.0, 0.0, 0.0]
+        Kd = [0.0, 0.0, 0.0]
+        sin_count = 0
+        rate_count = 0
+        motiontime = 0
+        freq_rad = args.freq_hz * 2 * math.pi
+
+        print(f'\n[主循环]')
+        loop_start_t = time.time()
+        loop_times = []
+        last_print = time.time()
+
+        try:
+            while running and motiontime < args.max_steps:
+                t0 = time.time()
+                motiontime += 1
+                state = client.recv_latest() or state
+
+                if motiontime < 10:
+                    for i in range(3):
+                        qInit[i] = state.motorState[i].q
+
+                if 10 <= motiontime < 400:
+                    rate_count += 1
+                    rate = rate_count / 200.0
+                    Kp = [5, 5, 5]
+                    Kd = [1, 1, 1]
+                    qDes[0] = interp(qInit[0], SIN_MID['hip'], rate)
+                    qDes[1] = interp(qInit[1], SIN_MID['thigh'], rate)
+                    qDes[2] = interp(qInit[2], SIN_MID['knee'], rate)
+
+                if motiontime >= 400:
+                    sin_count += 1
+                    t = DT * sin_count
+                    sin_j1 = 0.6 * math.sin(t * freq_rad)
+                    sin_j2 = -0.9 * math.sin(t * freq_rad)
+                    qDes[0] = SIN_MID['hip']
+                    qDes[1] = SIN_MID['thigh'] + sin_j1
+                    qDes[2] = SIN_MID['knee'] + sin_j2
+
+                # 构造命令: 只控 FR (索引 0,1,2)
+                cmd = LowCmd()
+                for i in range(3):
+                    cmd.motorCmd[i] = MotorCmd(
+                        mode=MotorMode.Servo,
+                        q=qDes[i], dq=0,
+                        tau=-0.65 if i == 0 else 0.0,   # FR_0 预紧
+                        Kp=Kp[i], Kd=Kd[i],
+                    )
+
+                # 安全保护
+                try:
+                    apply_safety(cmd, state, power_factor=args.power_factor,
+                                 position_limit_on=True,
+                                 position_protect_limit=0.5)
+                except PowerProtectViolation as e:
+                    print(f'\n⚠️ {e}')
+                    break
+
+                client.send(cmd)
+                loop_times.append(time.time() - t0)
+
+                now = time.time()
+                if now - last_print >= 0.5:
+                    phase = 'qInit' if motiontime < 10 else 'ramp' if motiontime < 400 else 'sin '
+                    print(f'  t={motiontime*DT:5.2f}s [{phase}] '
+                          f'des=[{qDes[0]:+.3f} {qDes[1]:+.3f} {qDes[2]:+.3f}] '
+                          f'act=[{state.motorState[0].q:+.3f} '
+                          f'{state.motorState[1].q:+.3f} {state.motorState[2].q:+.3f}]')
+                    last_print = now
+
+                # 维持 DT
+                sleep_t = DT - (time.time() - t0)
+                if sleep_t > 0:
+                    time.sleep(sleep_t)
+
+            # 时序统计
+            if loop_times:
+                total = time.time() - loop_start_t
+                avg_ms = sum(loop_times) / len(loop_times) * 1000
+                print(f'\n实际频率: {len(loop_times)/total:.1f} Hz, '
+                      f'每步均值 {avg_ms:.2f}ms')
+        finally:
+            print('\n安全停机...')
+            client.safe_stop()
+            print('✅ 退出')
+
+
+if __name__ == '__main__':
+    sys.exit(main() or 0)

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)

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)

examples/monitor_remote.py

@@ -0,0 +1,78 @@
+#!/usr/bin/env python3
+"""实时监听遥控器状态 (按键 + 摇杆 + L2).
+
+用法: python examples/monitor_remote.py --duration 60
+"""
+import argparse
+import signal
+import sys
+import time
+
+from go1_pro_sdk import MCUClient, LowCmd
+
+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('--raw', action='store_true')
+    args = p.parse_args()
+    signal.signal(signal.SIGINT, sigint)
+
+    with MCUClient(state_path=args.state) as client:
+        recv = client.wake_mcu(50)
+        if recv == 0:
+            print('❌ 无回包')
+            return 1
+
+        damping = LowCmd().all_damping()
+        last_btn = 0
+        last_print = 0
+        start = time.time()
+
+        print(f'📡 监听遥控器 {args.duration}s, Ctrl+C 退出\n')
+
+        while running and time.time() - start < args.duration:
+            client.send(damping)
+            time.sleep(0.005)
+            state = client.recv_latest()
+            if state is None:
+                continue
+
+            r = state.remote
+            now = time.time()
+
+            # 按键事件
+            if r.btn != last_btn:
+                pressed_now = set(r.pressed)
+                pressed_before = set(n for m, n in __import__('go1_pro_sdk').BUTTON_NAMES if last_btn & m)
+                just_p = pressed_now - pressed_before
+                just_r = pressed_before - pressed_now
+                if just_p:
+                    print(f"[{now-start:6.2f}s] ⬇ {'+'.join(sorted(just_p))}")
+                if just_r:
+                    print(f"[{now-start:6.2f}s] ⬆ {'+'.join(sorted(just_r))}")
+                last_btn = r.btn
+
+            # 摇杆 (0.2s 一次)
+            if now - last_print > 0.2:
+                active = (abs(r.lx) > 0.02 or abs(r.ly) > 0.02 or
+                          abs(r.rx) > 0.02 or abs(r.ry) > 0.02 or r.L2 > 0.02)
+                if active:
+                    print(f'[{now-start:6.2f}s] L=({r.lx:+.2f},{r.ly:+.2f}) '
+                          f'R=({r.rx:+.2f},{r.ry:+.2f}) L2={r.L2:+.2f}'
+                          + (f'  raw={state.wirelessRemote.hex()}' if args.raw else ''))
+                    last_print = now
+
+        print('\n✅ 退出')
+
+
+if __name__ == '__main__':
+    sys.exit(main() or 0)

examples/monitor_state.py

@@ -0,0 +1,79 @@
+#!/usr/bin/env python3
+"""实时监听 LowState (只读, 不控制电机).
+
+用法:
+    python examples/monitor_state.py --duration 30 --verbose
+"""
+import argparse
+import math
+import signal
+import sys
+import time
+
+from go1_pro_sdk import MCUClient, JOINT_NAMES, decode_sn
+
+running = True
+
+
+def sigint(s, f):
+    global running
+    running = False
+
+
+def main():
+    p = argparse.ArgumentParser()
+    p.add_argument('--state', default=None, help='Blowfish state 文件 (默认用包内置)')
+    p.add_argument('--duration', type=float, default=30)
+    p.add_argument('--rate', type=float, default=5, help='打印频率 Hz')
+    p.add_argument('--verbose', '-v', action='store_true')
+    args = p.parse_args()
+    signal.signal(signal.SIGINT, sigint)
+
+    print('📡 监听 LowState')
+    print(f'  时长 {args.duration}s, 打印 {args.rate}Hz')
+
+    with MCUClient(state_path=args.state) as client:
+        recv_count = client.wake_mcu(50)
+        print(f'唤醒: 收到 {recv_count}/50 帧')
+        if recv_count == 0:
+            print('❌ 无回包. 检查 Pi 抢占源或 state 是否正确')
+            return 1
+
+        start = time.time()
+        last_print = 0
+        n_decoded = 0
+        damping = __import__('go1_pro_sdk').LowCmd().all_damping()
+
+        print(f"\n{'时间':>6} {'电量':>5} {'FR_0':>8} {'FR_1':>8} {'FR_2':>8} {'rpy[°]':>20}")
+        print('-' * 70)
+
+        while running and time.time() - start < args.duration:
+            client.send(damping)
+            time.sleep(0.005)
+            state = client.recv_latest()
+            if state is None:
+                continue
+            n_decoded += 1
+
+            now = time.time()
+            if now - last_print >= 1.0 / args.rate:
+                t = now - start
+                rpy_str = (f"{math.degrees(state.imu.rpy[0]):+5.1f},"
+                           f"{math.degrees(state.imu.rpy[1]):+5.1f},"
+                           f"{math.degrees(state.imu.rpy[2]):+5.1f}")
+                print(f'{t:6.1f} {state.bms.SOC:4d}% '
+                      f'{state.motorState[0].q:+8.3f} '
+                      f'{state.motorState[1].q:+8.3f} '
+                      f'{state.motorState[2].q:+8.3f} '
+                      f'{rpy_str:>20}')
+                if args.verbose:
+                    print(f'    SN={decode_sn(state.SN)} '
+                          f'acc_z={state.imu.accelerometer[2]:.2f} m/s²  '
+                          f'BMS current={state.bms.current_a:.2f}A')
+                last_print = now
+
+        print(f'\n解码 {n_decoded} 帧, 退出.')
+
+
+if __name__ == '__main__':
+    sys.exit(main() or 0)