101 lines
3.2 KiB
Python
101 lines
3.2 KiB
Python
#!/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)
|