100 lines
3.0 KiB
Python
100 lines
3.0 KiB
Python
#!/usr/bin/env python3
|
|
"""关节活动范围校准 - 手动转动法.
|
|
|
|
流程:
|
|
1. 狗悬空 + L2+B damping
|
|
2. 跑这个脚本
|
|
3. 慢慢把每条腿的每个关节转到极限两端 (软力, 听到阻力就停)
|
|
4. 输出每个关节实测的 min/max/range
|
|
"""
|
|
import argparse
|
|
import os
|
|
import signal
|
|
import sys
|
|
import time
|
|
|
|
sys.path.insert(0, os.path.join(os.path.dirname(__file__), '..'))
|
|
from go1_pro_sdk import MCUClient, LowCmd, JOINT_NAMES
|
|
from go1_pro_sdk.utils.constants import JOINT_LIMITS_URDF
|
|
|
|
|
|
running = True
|
|
|
|
|
|
def sigint(s, f):
|
|
global running
|
|
running = False
|
|
|
|
|
|
def main():
|
|
parser = argparse.ArgumentParser()
|
|
parser.add_argument('--state', default=None)
|
|
parser.add_argument('--duration', type=float, default=60)
|
|
args = parser.parse_args()
|
|
signal.signal(signal.SIGINT, sigint)
|
|
|
|
print('🦾 关节活动范围校准')
|
|
print(f' 采样时长: {args.duration}s')
|
|
print()
|
|
print('操作:')
|
|
print(' 1. 狗悬空, L2+B damping (电机失能)')
|
|
print(' 2. 按顺序逐条腿/逐个关节转到极限')
|
|
print(' 3. Ctrl+C 提前结束')
|
|
input('回车开始采样...')
|
|
|
|
with MCUClient(state_path=args.state) as client:
|
|
recv = client.wake_mcu(50)
|
|
if recv == 0:
|
|
print('❌ 无回包')
|
|
return 1
|
|
|
|
damping = LowCmd().all_damping()
|
|
q_min = [float('inf')] * 12
|
|
q_max = [float('-inf')] * 12
|
|
|
|
start = time.time()
|
|
last_print = 0
|
|
n_decoded = 0
|
|
|
|
while running and time.time() - start < args.duration:
|
|
client.send(damping)
|
|
time.sleep(0.01)
|
|
state = client.recv_latest()
|
|
if state is None:
|
|
continue
|
|
n_decoded += 1
|
|
for i in range(12):
|
|
q = state.motorState[i].q
|
|
if q < q_min[i]: q_min[i] = q
|
|
if q > q_max[i]: q_max[i] = q
|
|
|
|
now = time.time()
|
|
if now - last_print >= 2.0:
|
|
print(f'\r⏱ {now-start:5.1f}/{args.duration:.0f}s 解码 {n_decoded} 帧',
|
|
end='', flush=True)
|
|
last_print = now
|
|
|
|
print('\n\n实测范围 vs URDF 标称:')
|
|
print('-' * 80)
|
|
print(f'{"关节":8} {"实测min":>9} {"实测max":>9} {"范围":>8} '
|
|
f'{"URDF min":>9} {"URDF max":>9}')
|
|
for i, name in enumerate(JOINT_NAMES):
|
|
jt = ('hip' if i % 3 == 0 else 'thigh' if i % 3 == 1 else 'knee')
|
|
urdf_lo, urdf_hi = JOINT_LIMITS_URDF[jt]
|
|
lo, hi = q_min[i], q_max[i]
|
|
if lo == float('inf'):
|
|
print(f'{name:8} {"未采到":>9}')
|
|
continue
|
|
print(f'{name:8} {lo:+9.4f} {hi:+9.4f} {hi-lo:8.4f} '
|
|
f'{urdf_lo:+9.4f} {urdf_hi:+9.4f}')
|
|
|
|
print('\nJOINT_LIMITS_MEASURED = {')
|
|
for i, name in enumerate(JOINT_NAMES):
|
|
if q_min[i] == float('inf'): continue
|
|
print(f" '{name}': ({q_min[i]:+.4f}, {q_max[i]:+.4f}),")
|
|
print('}')
|
|
|
|
|
|
if __name__ == '__main__':
|
|
sys.exit(main() or 0)
|