112 lines
3.5 KiB
Python
112 lines
3.5 KiB
Python
#!/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)
|