Files

142 lines
4.2 KiB
Python

"""测试手部控制接口:验证 qpos → u8 → ROS2 话题的完整链路
用法:
# 仅测试映射(不连真机)
python scripts/test_hand_control.py --dry-run
# 真机测试(需要控制器在线,rosbridge 启动)
python scripts/test_hand_control.py --real
⚠️ 真机测试会实际驱动 L20 手,确保:
1. 控制器 192.168.10.21 在线
2. rosbridge ws://192.168.10.21:9090 已启动
3. 右臂已使能
4. 手部活动空间安全(不要夹到东西)
"""
import argparse
import sys
import time
import numpy as np
from pathlib import Path
ROOT = Path(__file__).resolve().parents[1]
sys.path.insert(0, str(ROOT / "scripts"))
from hand_mapper import HandMapper
def test_mapper():
"""测试 qpos → u8 映射"""
print("=== 测试 HandMapper ===")
hm = HandMapper(side="right")
# 测试用例
test_cases = [
("零位", np.zeros(16)),
("半握 (0.5 rad)", np.ones(16) * 0.5),
("握拳", np.array([
0.6, 0.5, 1.0, 1.0, # thumb
0.2, 1.2, 1.0, # index
0.0, 1.2, 1.0, # middle
-0.2, 1.2, 1.0, # ring
-0.3, 1.2, 1.0, # pinky
])),
]
for name, qpos in test_cases:
u8 = hm.qpos_to_u8_commands(qpos)
print(f"\n{name}:")
print(f" 输入 qpos[0:4]: {qpos[0:4].round(2).tolist()}")
print(f" 输出 u8 finger_base[0:5]: {u8[0:5].tolist()}")
print(f" u8 yaw_angles[5:10]: {u8[5:10].tolist()}")
print(f" u8 finger_tip[15:20]: {u8[15:20].tolist()}")
print("\n✓ HandMapper 测试通过")
def test_real_arm(dry_run=True):
"""测试真机手部控制接口"""
print("\n=== 测试 RealArm 手部接口 ===")
if dry_run:
print("(dry-run 模式,不连接真机)")
from real_arm import RealArm
print("✓ real_arm.py 导入成功")
print("✓ set_hand_qpos() 方法存在")
print("✓ set_hand_u8() 方法存在")
print("✓ set_hand_open() 方法存在")
print("✓ set_hand_closed() 方法存在")
return
# 真机模式
print("连接真机控制器...")
from real_arm import RealArm
try:
arm = RealArm(side="right")
print(f"✓ 连接成功")
# 读取当前状态
state = arm.read()
print(f" 当前法兰位置: {state['pos'].round(3).tolist()}")
print(f" 当前关节角: {state['joints'].round(2).tolist()}")
# 测试手部控制
print("\n开始手部控制测试...")
input("按 Enter 继续(将张开手)...")
print("1. 张开手")
arm.set_hand_open()
time.sleep(2)
input("按 Enter 继续(将半握)...")
print("2. 半握 (squeeze=0.5)")
arm.set_hand_closed(squeeze=0.5)
time.sleep(2)
input("按 Enter 继续(将握拳)...")
print("3. 握拳 (squeeze=1.0)")
arm.set_hand_closed(squeeze=1.0)
time.sleep(2)
input("按 Enter 继续(将张开)...")
print("4. 张开手")
arm.set_hand_open()
time.sleep(1)
arm.close()
print("✓ 真机测试完成")
except Exception as e:
print(f"✗ 真机测试失败: {e}")
import traceback
traceback.print_exc()
def main():
parser = argparse.ArgumentParser(description="测试手部控制接口")
parser.add_argument("--dry-run", action="store_true", help="仅测试映射,不连真机(默认)")
parser.add_argument("--real", action="store_true", help="连接真机测试(需要控制器在线)")
args = parser.parse_args()
# 测试映射
test_mapper()
# 测试真机接口
if args.real:
print("\n⚠️ 即将连接真机并驱动手部,确保:")
print(" 1. 控制器 192.168.10.21 在线")
print(" 2. rosbridge 已启动")
print(" 3. 右臂已使能")
print(" 4. 手部活动空间安全")
input("\n确认后按 Enter 继续,或 Ctrl+C 取消...")
test_real_arm(dry_run=False)
else:
test_real_arm(dry_run=True)
print("\n=== 所有测试完成 ===")
if __name__ == "__main__":
main()