142 lines
4.2 KiB
Python
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()
|