60 lines
1.9 KiB
Python
60 lines
1.9 KiB
Python
|
#!/usr/bin/env python3
|
|||
|
|
|
|||
|
|
import sys
|
|||
|
|
import os
|
|||
|
|
import time
|
|||
|
|
import keyboard
|
|||
|
|
import grpc
|
|||
|
|
import numpy as np
|
|||
|
|
|
|||
|
|
sys.path.insert(0, os.path.dirname(os.path.abspath(__file__)))
|
|||
|
|
|
|||
|
|
from generated.cmvr.api import biohead_command_pb2
|
|||
|
|
from cmvr import CMVRGrpcClient, CMVRErrorCode, FacialExpressionState, JointCmd, Pose3D, RobotCartesian, RobotJointIndexDirection
|
|||
|
|
|
|||
|
|
def test_humanoid_robot():
|
|||
|
|
"""循环获取所有电机状态，按q退出，每个电机信息单独一行并按指定格式打印"""
|
|||
|
|
print("电机状态监控 (按q退出)\n")
|
|||
|
|
client = CMVRGrpcClient("192.168.0.222:50052")
|
|||
|
|
|
|||
|
|
if not client.is_connected():
|
|||
|
|
print("连接服务器失败")
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
humanoidRobot = client.get_humanoid_robot("hc01")
|
|||
|
|
if humanoidRobot is None:
|
|||
|
|
print("获取机器人失败")
|
|||
|
|
return
|
|||
|
|
|
|||
|
|
try:
|
|||
|
|
while True:
|
|||
|
|
if keyboard.is_pressed('q'):
|
|||
|
|
print("\n退出程序...")
|
|||
|
|
break
|
|||
|
|
|
|||
|
|
# 获取所有关节状态
|
|||
|
|
result, joint_states = humanoidRobot.get_joint_states()
|
|||
|
|
if result == CMVRErrorCode.CMVR_SUCCESS and joint_states:
|
|||
|
|
# 遍历所有状态组和电机
|
|||
|
|
for status in joint_states:
|
|||
|
|
for j, name in enumerate(status.name):
|
|||
|
|
pos = status.position[j] if j < len(status.position) else 0.0
|
|||
|
|
# 每个电机信息单独一行，按指定格式打印并以逗号结尾
|
|||
|
|
print(f'{{"{name}", {pos:.6f}}},')
|
|||
|
|
|
|||
|
|
time.sleep(0.5)
|
|||
|
|
# 如需每次刷新屏幕可取消下面一行注释
|
|||
|
|
# os.system('cls' if os.name == 'nt' else 'clear')
|
|||
|
|
|
|||
|
|
except Exception as e:
|
|||
|
|
print(f"\n错误: {e}")
|
|||
|
|
import traceback
|
|||
|
|
traceback.print_exc()
|
|||
|
|
|
|||
|
|
finally:
|
|||
|
|
print("监控结束")
|
|||
|
|
|
|||
|
|
|
|||
|
|
if __name__ == "__main__":
|
|||
|
|
test_humanoid_robot()
|