diff --git a/biohead.py b/biohead.py new file mode 100644 index 0000000..42363ff --- /dev/null +++ b/biohead.py @@ -0,0 +1,557 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +biohead.py — 基于gRPC的仿生头控制脚本 +功能： + 1. 控制单个表情参数 + 2. 控制多个表情参数 + 3. 退出 + 4. 自动增减模式：从0到1再到0循环变化 + 5. 设置自动模式参数数量 + 6. 眨眼模式：执行预设的眨眼动作 + 7. 打哈欠模式：模拟打哈欠动作 + 8. 说话模式：模拟嘴巴开合动作 +""" +import sys +import time +import random +import os +import grpc +from typing import Generator + +# 添加项目根目录到Python路径 +sys.path.insert(0, os.path.dirname(os.path.abspath(__file__))) + +# 导入gRPC相关模块 +from generated.cmvr.api import biohead_command_pb2 +from cmvr import CMVRGrpcClient, CMVRErrorCode, FacialExpressionState + +def angle_to_normalized(angle): + """将角度值(0-180)转换为归一化值(0-1)""" + return max(0.0, min(1.0, angle / 180.0)) + +def normalized_to_angle(normalized): + """将归一化值(0-1)转换为角度值(0-180)""" + return int(normalized * 180) + +def init_biohead_client(server_addr="192.168.1.222:50051"): + """初始化仿生头gRPC客户端""" + client = CMVRGrpcClient(server_addr) + if not client.is_connected(): + print("连接服务器失败") + return None + + biohead = client.get_biohead("bio_head") + if not biohead: + print("获取仿生头客户端失败") + client.close() + return None + + return client, biohead + +def set_single_expression(biohead): + """设置单个表情参数""" + print("\n--- 设置单个表情参数 ---") + # 表情参数映射表 + param_map = { + 1: "left_eyebrow_outside_y", + 2: "left_eyebrow_inside_y", + 3: "right_eyebrow_outside_y", + 4: "right_eyebrow_inside_y", + 5: "left_eye_upper_lid_y", + 6: "left_eye_lower_lid_y", + 7: "right_eye_upper_lid_y", + 8: "right_eye_lower_lid_y", + 9: "left_eye_ball_x", + 10: "left_eye_ball_y", + 11: "right_eye_ball_x", + 12: "right_eye_ball_y", + 13: "upper_lip_y", + 14: "lower_lip_y", + 15: "jaw_x", + 16: "jaw_y" + } + + print("可用表情参数:") + for idx, name in param_map.items(): + print(f"{idx}: {name}") + + try: + param_id = int(input("选择参数ID: ")) + if param_id not in param_map: + print("无效的参数ID") + return + + angle = int(input("输入角度(0-180): ")) + if angle < 0 or angle > 180: + print("角度必须在0-180之间") + return + + # 转换为归一化值 + value = angle_to_normalized(angle) + + # 创建表情对象 + expression = FacialExpressionState() + # 设置对应的参数 + setattr(expression, param_map[param_id], value) + + # 发送表情设置 + result = biohead.set_expression(expression) + if result == CMVRErrorCode.CMVR_SUCCESS: + print(f"设置 {param_map[param_id]} 成功，角度: {angle}°，归一化值: {value:.2f}") + else: + print(f"设置失败，错误码: {result}") + + except ValueError: + print("输入无效，请输入数字") + +def set_multiple_expressions(biohead): + """设置多个表情参数""" + print("\n--- 设置多个表情参数 ---") + # 表情参数映射表 + param_map = { + 1: "left_eyebrow_outside_y", + 2: "left_eyebrow_inside_y", + 3: "right_eyebrow_outside_y", + 4: "right_eyebrow_inside_y", + 5: "left_eye_upper_lid_y", + 6: "left_eye_lower_lid_y", + 7: "right_eye_upper_lid_y", + 8: "right_eye_lower_lid_y", + 9: "left_eye_ball_x", + 10: "left_eye_ball_y", + 11: "right_eye_ball_x", + 12: "right_eye_ball_y", + 13: "upper_lip_y", + 14: "lower_lip_y", + 15: "jaw_x", + 16: "jaw_y" + } + + print("可用表情参数:") + for idx, name in param_map.items(): + print(f"{idx}: {name}") + + try: + count = int(input("输入要设置的参数数量: ")) + if count <= 0: + print("数量必须大于0") + return + + # 创建表情对象 + expression = FacialExpressionState() + + for i in range(count): + print(f"\n第{i+1}个参数:") + param_id = int(input("选择参数ID: ")) + if param_id not in param_map: + print("无效的参数ID，跳过该参数") + continue + + angle = int(input("输入角度(0-180): ")) + if angle < 0 or angle > 180: + print("角度必须在0-180之间，跳过该参数") + continue + + # 转换为归一化值 + value = angle_to_normalized(angle) + # 设置对应的参数 + setattr(expression, param_map[param_id], value) + print(f"已添加: {param_map[param_id]} = {value:.2f} (对应角度: {angle}°)") + + # 发送表情设置 + result = biohead.set_expression(expression) + if result == CMVRErrorCode.CMVR_SUCCESS: + print("设置多个表情参数成功") + else: + print(f"设置失败，错误码: {result}") + + except ValueError: + print("输入无效，请输入数字") + +def auto_mode(biohead, params): + """自动增减模式：0→1→0，循环变化""" + print("\n--- 自动增减模式 ---") + try: + # 表情参数映射表 + param_map = { + 1: "left_eyebrow_outside_y", + 2: "left_eyebrow_inside_y", + 3: "right_eyebrow_outside_y", + 4: "right_eyebrow_inside_y", + 5: "left_eye_upper_lid_y", + 6: "left_eye_lower_lid_y", + 7: "right_eye_upper_lid_y", + 8: "right_eye_lower_lid_y", + 9: "left_eye_ball_x", + 10: "left_eye_ball_y", + 11: "right_eye_ball_x", + 12: "right_eye_ball_y", + 13: "upper_lip_y", + 14: "lower_lip_y", + 15: "jaw_x", + 16: "jaw_y" + } + + # 过滤有效的参数ID + valid_params = [p for p in params if p in param_map] + if not valid_params: + print("没有有效的参数，返回菜单") + return + + print(f"将自动控制以下参数: {[param_map[p] for p in valid_params]}") + + frequency = 50.0 # 50Hz + interval = 1.0 / frequency + value = 0.0 + direction = 0.02 # 步长 + + # 启动流式控制 + result = biohead.start_stream() + if result != CMVRErrorCode.CMVR_SUCCESS: + print(f"开始流式会话失败，错误码: {result}") + return + + print("启动自动模式，按Ctrl+C停止") + while True: + # 创建protobuf消息 + proto_expr = biohead_command_pb2.FacialExpression() + + # 设置所有参数 + for param_id in valid_params: + param_name = param_map[param_id] + + # 根据参数类型设置到不同的字段 + if param_name in ["left_eyebrow_outside_y", "left_eyebrow_inside_y", + "right_eyebrow_outside_y", "right_eyebrow_inside_y"]: + setattr(proto_expr.eyebrow, param_name, value) + elif param_name in ["left_eye_upper_lid_y", "left_eye_lower_lid_y", + "right_eye_upper_lid_y", "right_eye_lower_lid_y"]: + setattr(proto_expr.eyelid, param_name, value) + elif param_name in ["left_eye_ball_x", "left_eye_ball_y", + "right_eye_ball_x", "right_eye_ball_y"]: + setattr(proto_expr.eye_ball, param_name, value) + elif param_name in ["upper_lip_y", "lower_lip_y"]: + setattr(proto_expr.mouth, param_name, value) + elif param_name in ["jaw_x", "jaw_y"]: + setattr(proto_expr.jaw, param_name, value) + + # 发送流式表情 + result = biohead.stream_expression(proto_expr) + if result != CMVRErrorCode.CMVR_SUCCESS: + print(f"流式控制失败，错误码: {result}") + break + + # 更新值 + value += direction + if value >= 1.0: + value = 1.0 + direction = -direction + elif value <= 0.0: + value = 0.0 + direction = -direction + + time.sleep(interval) + + except KeyboardInterrupt: + print("\n自动模式已停止") + finally: + # 结束流式会话 + biohead.end_stream() + +def set_auto_params(): + """设置自动模式的参数列表""" + print("\n--- 设置自动模式参数 ---") + # 表情参数映射表 + param_map = { + 1: "left_eyebrow_outside_y", + 2: "left_eyebrow_inside_y", + 3: "right_eyebrow_outside_y", + 4: "right_eyebrow_inside_y", + 5: "left_eye_upper_lid_y", + 6: "left_eye_lower_lid_y", + 7: "right_eye_upper_lid_y", + 8: "right_eye_lower_lid_y", + 9: "left_eye_ball_x", + 10: "left_eye_ball_y", + 11: "right_eye_ball_x", + 12: "right_eye_ball_y", + 13: "upper_lip_y", + 14: "lower_lip_y", + 15: "jaw_x", + 16: "jaw_y" + } + + print("可用表情参数:") + for idx, name in param_map.items(): + print(f"{idx}: {name}") + + try: + count = int(input("输入要自动控制的参数数量: ")) + if count <= 0: + print("数量必须大于0") + return None + + params = [] + for i in range(count): + param_id = int(input(f"第{i+1}个参数ID: ")) + if param_id not in param_map: + print("无效的参数ID，跳过") + continue + params.append(param_id) + + if not params: + print("未设置有效的参数") + return None + + print(f"已设置自动模式参数: {[param_map[p] for p in params]}") + return params + + except ValueError: + print("输入无效，请输入数字") + return None + +def blink_mode(biohead): + """眨眼模式：执行预设的眨眼动作""" + print("\n--- 眨眼模式 ---") + try: + blink_count = int(input("输入眨眼次数(默认10次): ") or "10") + interval = float(input("输入间隔时间(秒，默认0.1): ") or "0.1") + + # 启动流式控制 + result = biohead.start_stream() + if result != CMVRErrorCode.CMVR_SUCCESS: + print(f"开始流式会话失败，错误码: {result}") + return + + print(f"启动眨眼模式，共{blink_count}次，间隔{interval}秒") + + for i in range(blink_count): + print(f"第{i+1}次眨眼...") + + # 闭眼动作 + proto_expr = biohead_command_pb2.FacialExpression() + proto_expr.eyelid.left_upper_lid_y = angle_to_normalized(90) + proto_expr.eyelid.left_lower_lid_y = angle_to_normalized(90) + proto_expr.eyelid.right_upper_lid_y = angle_to_normalized(90) + proto_expr.eyelid.right_lower_lid_y = angle_to_normalized(90) + + result = biohead.stream_expression(proto_expr) + if result != CMVRErrorCode.CMVR_SUCCESS: + print(f"闭眼动作失败，错误码: {result}") + break + + time.sleep(interval) + + # 睁眼动作 + proto_expr = biohead_command_pb2.FacialExpression() + proto_expr.eyelid.left_upper_lid_y = angle_to_normalized(50) + proto_expr.eyelid.left_lower_lid_y = angle_to_normalized(130) + proto_expr.eyelid.right_upper_lid_y = angle_to_normalized(130) + proto_expr.eyelid.right_lower_lid_y = angle_to_normalized(60) + + result = biohead.stream_expression(proto_expr) + if result != CMVRErrorCode.CMVR_SUCCESS: + print(f"睁眼动作失败，错误码: {result}") + break + + time.sleep(interval) + + print("眨眼模式结束") + + except KeyboardInterrupt: + print("\n眨眼模式已停止") + except ValueError: + print("输入无效，请输入数字") + finally: + # 结束流式会话 + biohead.end_stream() + +def yawn_mode(biohead): + """打哈欠模式：模拟打哈欠动作""" + print("\n--- 打哈欠模式 ---") + try: + yawn_count = int(input("输入打哈欠次数(默认5次): ") or "5") + interval = float(input("输入间隔时间(秒，默认1): ") or "1") + + # 启动流式控制 + result = biohead.start_stream() + if result != CMVRErrorCode.CMVR_SUCCESS: + print(f"开始流式会话失败，错误码: {result}") + return + + print(f"启动打哈欠模式，共{yawn_count}次，间隔{interval}秒") + + for i in range(yawn_count): + print(f"第{i+1}次打哈欠...") + + # 打哈欠动作 + proto_expr = biohead_command_pb2.FacialExpression() + # 眉毛 + proto_expr.eyebrow.left_eyebrow_outside_y = angle_to_normalized(70) + proto_expr.eyebrow.right_eyebrow_outside_y = angle_to_normalized(110) + # 眼睑 + proto_expr.eyelid.left_upper_lid_y = angle_to_normalized(60) + proto_expr.eyelid.left_lower_lid_y = angle_to_normalized(130) + proto_expr.eyelid.right_upper_lid_y = angle_to_normalized(120) + proto_expr.eyelid.right_lower_lid_y = angle_to_normalized(80) + # 嘴巴和下巴 + proto_expr.mouth.upper_lip_y = angle_to_normalized(120) + proto_expr.mouth.lower_lip_y = angle_to_normalized(45) + proto_expr.jaw.x = angle_to_normalized(60) + proto_expr.jaw.y = angle_to_normalized(120) + + result = biohead.stream_expression(proto_expr) + if result != CMVRErrorCode.CMVR_SUCCESS: + print(f"打哈欠动作失败，错误码: {result}") + break + + time.sleep(interval) + + # 恢复动作 + proto_expr = biohead_command_pb2.FacialExpression() + # 恢复到正常状态 + proto_expr.eyebrow.left_eyebrow_outside_y = angle_to_normalized(90) + proto_expr.eyebrow.right_eyebrow_outside_y = angle_to_normalized(90) + proto_expr.eyelid.left_upper_lid_y = angle_to_normalized(90) + proto_expr.eyelid.left_lower_lid_y = angle_to_normalized(90) + proto_expr.eyelid.right_upper_lid_y = angle_to_normalized(90) + proto_expr.eyelid.right_lower_lid_y = angle_to_normalized(90) + proto_expr.mouth.upper_lip_y = angle_to_normalized(90) + proto_expr.mouth.lower_lip_y = angle_to_normalized(90) + proto_expr.jaw.x = angle_to_normalized(90) + proto_expr.jaw.y = angle_to_normalized(90) + + result = biohead.stream_expression(proto_expr) + if result != CMVRErrorCode.CMVR_SUCCESS: + print(f"恢复动作失败，错误码: {result}") + break + + time.sleep(interval) + + print("打哈欠模式结束") + + except KeyboardInterrupt: + print("\n打哈欠模式已停止") + except ValueError: + print("输入无效，请输入数字") + finally: + # 结束流式会话 + biohead.end_stream() + +def talk_mode(biohead): + """说话模式：模拟嘴巴开合动作""" + print("\n--- 说话模式 ---") + try: + talk_count = int(input("输入说话循环次数(默认50次): ") or "50") + + # 启动流式控制 + result = biohead.start_stream() + if result != CMVRErrorCode.CMVR_SUCCESS: + print(f"开始流式会话失败，错误码: {result}") + return + + print(f"启动说话模式，共{talk_count}次") + + for i in range(talk_count): + print(f"第{i+1}次说话...") + + # 开口动作 + proto_expr = biohead_command_pb2.FacialExpression() + proto_expr.mouth.upper_lip_y = angle_to_normalized(120) + proto_expr.mouth.lower_lip_y = angle_to_normalized(45) + proto_expr.jaw.x = angle_to_normalized(80) + proto_expr.jaw.y = angle_to_normalized(110) + + result = biohead.stream_expression(proto_expr) + if result != CMVRErrorCode.CMVR_SUCCESS: + print(f"开口动作失败，错误码: {result}") + break + + # 随机间隔 + interval = random.uniform(0, 0.3) + time.sleep(interval) + + # 闭口动作 + proto_expr = biohead_command_pb2.FacialExpression() + proto_expr.mouth.upper_lip_y = angle_to_normalized(120) + proto_expr.mouth.lower_lip_y = angle_to_normalized(45) + proto_expr.jaw.x = angle_to_normalized(90) + proto_expr.jaw.y = angle_to_normalized(90) + + result = biohead.stream_expression(proto_expr) + if result != CMVRErrorCode.CMVR_SUCCESS: + print(f"闭口动作失败，错误码: {result}") + break + + # 随机间隔 + interval = random.uniform(0, 0.3) + time.sleep(interval) + + print("说话模式结束") + + except KeyboardInterrupt: + print("\n说话模式已停止") + except ValueError: + print("输入无效，请输入数字") + finally: + # 结束流式会话 + biohead.end_stream() + +def main(): + # 初始化gRPC客户端 + server_addr = "192.168.1.222:50051" + if len(sys.argv) > 1: + server_addr = sys.argv[1] + + print(f"连接到仿生头服务器: {server_addr}") + client, biohead = init_biohead_client(server_addr) + if not client or not biohead: + sys.exit(1) + + # 默认自动模式参数 + auto_params = [5, 6, 7, 8] # 默认控制眼睑参数 + + try: + while True: + print("\n=== 仿生头控制菜单 ===") + print("1. 控制单个表情参数") + print("2. 控制多个表情参数") + print("3. 退出") + print("4. 自动增减模式(50Hz)") + print("5. 设置自动模式参数") + print("6. 眨眼模式") + print("7. 打哈欠模式") + print("8. 说话模式") + opt = input("选择操作: ").strip() + + if opt == '1': + set_single_expression(biohead) + elif opt == '2': + set_multiple_expressions(biohead) + elif opt == '3': + print("退出程序") + break + elif opt == '4': + auto_mode(biohead, auto_params) + elif opt == '5': + new_params = set_auto_params() + if new_params: + auto_params = new_params + elif opt == '6': + blink_mode(biohead) + elif opt == '7': + yawn_mode(biohead) + elif opt == '8': + talk_mode(biohead) + else: + print("无效选项，请重试") + + except KeyboardInterrupt: + print("\n用户中断，退出程序") + finally: + client.close() + print("已关闭连接") + +if __name__ == '__main__': + main() diff --git a/biohead_test.py b/biohead_test.py new file mode 100644 index 0000000..627f7cf --- /dev/null +++ b/biohead_test.py @@ -0,0 +1,344 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +interactive_servo_test.py — 基于 Python 的 ESP32 舵机控制交互脚本 +依赖：pip install pyserial +用法： + python3 interactive_servo_test.py /dev/ttyUSB0 [baudrate] +功能： + 1. 控制单个舵机 + 2. 控制多个舵机 + 3. 退出 + 4. 自动增减模式：从 0 到 180° 每步 +6° → -6° 循环，50Hz 频率发送 + 5. 设置自动模式舵机数量（通道 1~N） + 6. 眨眼模式：让五个舵机执行预设角度循环，模拟眨眼，8号舵机保持120度 + 7. 打哈欠模式：模拟打哈欠动作 + 8. 说话模式：模拟嘴巴开合动作 +""" +import sys +import time +import serial +import random + +FRAME_HEADER = 0xAA # 帧头 + +def wakeup_esp32(port, baud=115200): + """通过 RTS/DTR 线复位并唤醒 ESP32""" + try: + ser = serial.Serial(port, baud, timeout=1) + ser.setDTR(False) + ser.setRTS(False) + time.sleep(0.01) + ser.setDTR(True) + ser.setRTS(True) + ser.write(b"\r\n") + time.sleep(1) + except Exception as e: + print(f"ESP32 唤醒失败: {e}") + finally: + try: + ser.close() + except: + pass + + +def open_serial(port, baud): + """打开并配置串口""" + try: + ser = serial.Serial(port, baud, timeout=1) + print(f"已打开串口: {port} @ {baud}bps") + return ser + except Exception as e: + print(f"打开串口失败: {e}") + sys.exit(1) + + +def calc_checksum(data: bytearray) -> int: + """计算 XOR 校验和""" + cs = 0 + for b in data: + cs ^= b + return cs + + +def pack_commands(cmds): + """ + cmds: list of (addr, channel, angle, duration_ms) + 返回：完整帧字节数组 + 格式： [FRAME_HEADER][count] [addr, ch, ang, durL, durH]... [checksum] + """ + pkt = bytearray([FRAME_HEADER, len(cmds)]) + for addr, ch, ang, dur in cmds: + pkt.extend([addr, ch, ang, dur & 0xFF, (dur >> 8) & 0xFF]) + pkt.append(calc_checksum(pkt)) + return pkt + + +def print_packet(pkt): + """打印十六进制数据包""" + print("发送数据包:", ' '.join(f"0x{b:02X}" for b in pkt)) + + +def input_hex(prompt): + """输入 0x 开头或十进制整型""" + val = input(prompt).strip() + try: + return int(val, 0) + except ValueError: + print(f"无效数字: {val}") + return input_hex(prompt) + + +def input_cmd_single(): + """交互输入单条命令""" + addr = input_hex("输入 addr (hex or dec): ") + ch = input_hex("输入 channel (hex or dec): ") + ang = input_hex("输入 angle (0-180): ") + dur = input_hex("输入 duration(ms): ") + return [(addr, ch, ang, dur)] + + +def input_cmd_multiple(): + """交互输入多条命令""" + cnt = input_hex("输入命令数量: ") + cmds = [] + for i in range(cnt): + print(f"第 {i+1} 条:") + cmds.extend(input_cmd_single()) + return cmds + + +def auto_mode(ser, channels): + """自动增减模式：0→180→0，步长6，50Hz，多舵机""" + addr = input_hex("自动模式: 输入 addr (hex或dec): ") + interval = 1.0 / 50.0 + dur_ms = int(interval * 1000) + angle = 0 + direction = 1 + step = 6 + + print(f"启动自动模式 @50Hz, addr=0x{addr:02X}, channels={channels}, step={step}°") + try: + while True: + cmds = [(addr, ch, angle, dur_ms) for ch in channels] + pkt = pack_commands(cmds) + print_packet(pkt) + ser.write(pkt) + # 更新角度 + angle += direction * step + if angle >= 180: + angle = 180 + direction = -1 + elif angle <= 0: + angle = 0 + direction = 1 + time.sleep(interval) + except KeyboardInterrupt: + print("\n自动模式已停止，返回菜单") + + +def set_channels(): + """自定义舵机数量，返回通道列表 1~N""" + cnt = input_hex("输入自动模式舵机数量 N: ") + if cnt < 1: + print("数量必须 >= 1") + return None + channels = list(range(1, cnt+1)) + print(f"已设置通道列表: {channels}") + return channels + + +def blink_mode(ser): + """眨眼模式：根据输入的角度进行指定舵机的眨眼操作""" + addr = input_hex("眨眼模式: 输入 addr (hex或dec): ") + blink_count = 10 # 循环10次 + interval = 0.1 # 每秒眨眼一次 + + # 定义五个舵机的通道 + servo_channels = [4, 5, 6, 7, 8] # 控制五个舵机 + blink_angles = [90, 90, 90, 90, 90] # 眨眼时的角度 + open_angles = [50, 130, 130, 60, 90] # 睁开时的角度（4-7号舵机回到90度，8号保持120度） + + print(f"启动眨眼模式，10次眨眼，每次间隔 0.1 秒") + + try: + for i in range(blink_count): + print(f"第 {i+1} 次眨眼...") + + # 眨眼动作：同步发送所有舵机到眨眼角度 + cmds = [(addr, ch, angle, 0) for ch, angle in zip(servo_channels, blink_angles)] + pkt = pack_commands(cmds) + print_packet(pkt) + ser.write(pkt) + time.sleep(interval) + + # 睁开动作：同步发送4-7号舵机回到90度，8号保持120度 + print("睁开动作：舵机4-7回到90度") + cmds = [(addr, ch, angle, 0) for ch, angle in zip(servo_channels, open_angles)] + pkt = pack_commands(cmds) + print_packet(pkt) + ser.write(pkt) + time.sleep(interval) + + print("眨眼模式结束，返回菜单") + except KeyboardInterrupt: + print("\n眨眼模式已停止，返回菜单") + + +def yawn_mode(ser): + """打哈欠模式：控制多个舵机模拟打哈欠动作""" + addr = input_hex("打哈欠模式: 输入 addr (hex或dec): ") + yawn_count = 5 # 循环5次 + interval = 1 # 每次打哈欠动作间隔1秒 + + # 定义控制的舵机通道 + # 打哈欠时使用舵机1, 3, 4, 5, 6, 7, 8（通道1~8） + # 闭嘴时使用舵机8、9（65号舵机） + yawning_channels = [1, 3, 4, 5, 6, 7, 8] # 控制通道1~8 + yawning_angles = [70, 110, 60, 130, 120, 80, 90] # 打哈欠时的角度（根据需要调整） + closing_angles = [90, 90, 90, 90, 90, 90, 90] # 闭嘴时的角度（闭嘴动作） + + # 65号舵机（8、9号通道）设置不同的角度，模拟控制头部动作 + yawning_65_angles = [120, 45, 60, 120] # 65号舵机（8、9号通道）在打哈欠时的角度 + closing_65_angles = [120, 45, 90, 90] # 闭嘴时65号舵机（8、9号通道）的角度 + + print(f"启动打哈欠模式，循环 {yawn_count} 次，每次间隔 {interval} 秒") + + try: + for i in range(yawn_count): + print(f"第 {i+1} 次打哈欠...") + + # 打哈欠动作：同步发送所有舵机到打哈欠角度 + cmds = [(addr, ch, angle, 3000) for ch, angle in zip(yawning_channels, yawning_angles)] + cmds_65 = [(65, ch, angle, 3000) for ch, angle in zip([0,1,8, 9], yawning_65_angles)] # 控制65号舵机（8、9号通道） + cmds.extend(cmds_65) # 合并65号舵机的指令 + + pkt = pack_commands(cmds) + print_packet(pkt) + ser.write(pkt) # 一次性发送所有指令 + time.sleep(interval) + + # 闭嘴动作：同步发送舵机回到闭嘴角度 + print("闭嘴动作：所有舵机回到闭嘴角度") + cmds = [(addr, ch, angle, 3000) for ch, angle in zip(yawning_channels, closing_angles)] + cmds_65 = [(65, ch, angle, 3000) for ch, angle in zip([0,1,8, 9], closing_65_angles)] # 65号舵机（8、9号通道） + cmds.extend(cmds_65) # 合并65号舵机的指令 + + pkt = pack_commands(cmds) + print_packet(pkt) + ser.write(pkt) # 一次性发送所有指令 + time.sleep(interval) + + print("打哈欠模式结束，返回菜单") + except KeyboardInterrupt: + print("\n打哈欠模式已停止，返回菜单") + + +def talk_mode(ser): + """说话模式：模拟嘴巴开合动作""" + addr = input_hex("说话模式: 输入 addr (hex或dec): ") + talk_count = 50 # 循环5次 + + # 定义控制的舵机通道 + mouth_channels = [0, 1, 8, 9] # 控制3个舵机（口型） + mouth_open_angles = [120, 45, 80, 110] # 嘴巴打开时的角度 + mouth_close_angles = [120, 45, 90, 90] # 嘴巴关闭时的角度（正常闭合） + + print(f"启动说话模式，循环 {talk_count} 次，每次间隔随机生成在0~1秒之间") + + try: + for i in range(talk_count): + print(f"第 {i + 1} 次说话...") + + # 开口动作：同步发送所有舵机到嘴巴打开的角度 + cmds = [(addr, ch, angle, 0) for ch, angle in zip(mouth_channels, mouth_open_angles)] + pkt = pack_commands(cmds) + print_packet(pkt) + ser.write(pkt) # 一次性发送所有指令 + + # 生成一个 0 到 1 之间的随机间隔时间 + interval = random.uniform(0, 0.3) + print(f"随机间隔时间：{interval:.2f}秒") + time.sleep(interval) + + # 闭口动作：同步发送所有舵机回到嘴巴关闭的角度 + print("闭口动作：舵机回到闭口角度") + cmds = [(addr, ch, angle, 0) for ch, angle in zip(mouth_channels, mouth_close_angles)] + pkt = pack_commands(cmds) + print_packet(pkt) + ser.write(pkt) # 一次性发送所有指令 + + # 生成一个 0 到 1 之间的随机间隔时间 + interval = random.uniform(0, 0.3) + print(f"随机间隔时间：{interval:.2f}秒") + time.sleep(interval) + + print("说话模式结束，返回菜单") + except KeyboardInterrupt: + print("\n说话模式已停止，返回菜单") + + +def main(): + if len(sys.argv) < 2: + print(f"用法: {sys.argv[0]} /dev/ttyUSB0 [baudrate]") + sys.exit(1) + port = sys.argv[1] + baud = int(sys.argv[2]) if len(sys.argv) > 2 else 115200 + + print("唤醒 ESP32...") + wakeup_esp32(port, baud) + ser = open_serial(port, baud) + channels = list(range(1, 10)) # 默认通道 1-9 + + try: + while True: + print("\n=== 操作菜单 ===") + print("1. 控制单个舵机") + print("2. 控制多个舵机") + print("3. 退出") + print("4. 自动增减模式(50Hz)") + print("5. 设置自动模式舵机数量") + print("6. 眨眼模式") + print("7. 打哈欠模式") + print("8. 说话模式") # 新增选项 + opt = input("选择: ").strip() + if opt == '1': + cmds = input_cmd_single() + elif opt == '2': + cmds = input_cmd_multiple() + elif opt == '4': + auto_mode(ser, channels) + continue + elif opt == '5': + new_ch = set_channels() + if new_ch: + channels = new_ch + continue + elif opt == '6': + blink_mode(ser) + continue + elif opt == '7': # 调用打哈欠模式 + yawn_mode(ser) + continue + elif opt == '8': # 调用说话模式 + talk_mode(ser) + continue + elif opt == '3': + break + else: + print("无效选项，重试") + continue + + pkt = pack_commands(cmds) + print_packet(pkt) + ser.write(pkt) + print("✅ 指令已发送") + except KeyboardInterrupt: + print("\n用户中断，退出") + finally: + ser.close() + print("串口已关闭") + + +if __name__ == '__main__': + main() \ No newline at end of file diff --git a/cmvr/__init__.py b/cmvr/__init__.py index 6f31244..cc97382 100644 --- a/cmvr/__init__.py +++ b/cmvr/__init__.py @@ -1,15 +1,15 @@ from .client import CMVRGrpcClient -from .enums import CMVRErrorCode, DeviceState, FingerType +from .enums import CMVRErrorCode, DeviceState, FingerType, RobotCartesian, RobotJointIndexDirection from .models import ( MicState, SpeakerState, CameraState, FreedomState, DexHandState, FacialExpressionState, - FingerTactileData, PalmTactileData, HandTactileSensors + FingerTactileData, PalmTactileData, HandTactileSensors,JointCmd,Pose3D,JointState ) __all__ = [ 'CMVRGrpcClient', - 'CMVRErrorCode', 'DeviceState', 'FingerType', + 'CMVRErrorCode', 'DeviceState', 'FingerType',"RobotCartesian", "RobotJointIndexDirection", 'MicState', 'SpeakerState', 'CameraState', 'FreedomState', 'DexHandState', 'FacialExpressionState', - 'FingerTactileData', 'PalmTactileData', 'HandTactileSensors' + 'FingerTactileData', 'PalmTactileData', 'HandTactileSensors',"JointCmd","Pose3D","JointState" ] \ No newline at end of file diff --git a/cmvr/camera_client.py b/cmvr/camera_client.py index 76f51df..c0768f3 100644 --- a/cmvr/camera_client.py +++ b/cmvr/camera_client.py @@ -147,6 +147,213 @@ class CameraClient: print(f"获取图像失败: {e}") return CMVRErrorCode.CMVR_RPC_FAILED, np.array([]), 0, 0 + + def get_depth_image(self) -> Tuple[CMVRErrorCode, np.ndarray, int, int, CameraIntrinsics]: + """ + 获取深度图像及内参（修复返回值类型） + """ + # 创建默认内参实例（空值）用于错误返回 + default_intrinsics = CameraIntrinsics(cx=0, cy=0, fx=0, fy=0, coeffs=[]) + # 错误返回模板（类型与声明完全匹配） + error_return = ( + CMVRErrorCode.CMVR_RPC_FAILED, + np.array([], dtype=np.uint16), + 0, + 0, + default_intrinsics + ) + + try: + generated = self._import_generated() + if not generated: + print("无法导入生成的protobuf模块") + return error_return + + # 创建请求 + request = generated.camera_command_pb2.GetDepthImageComand.Request() + request.header.CopyFrom(self._create_command_header()) + + # 调用RPC + response = self.stub.GetDepthImage(request) + + if not response.header.success: + print(f"深度图像请求失败，错误码: {response.header.error_code}") + return ( + CMVRErrorCode.CMVR_RPC_FAILED, + np.array([], dtype=np.uint16), + 0, + 0, + default_intrinsics + ) + + # 提取图像信息 + width = response.depth_frame.width + height = response.depth_frame.height + depth_data = response.depth_frame.data + expected_size = width * height * 2 + + if len(depth_data) != expected_size: + print(f"深度图像数据尺寸不匹配: 预期 {expected_size}, 实际 {len(depth_data)}") + return ( + CMVRErrorCode.CMVR_INTERNAL_ERROR, + np.array([], dtype=np.uint16), + 0, + 0, + default_intrinsics + ) + + # 转换为数组 + depth_array = np.frombuffer(depth_data, dtype=np.uint16).reshape((height, width)) + + # 解析内参（确保始终返回有效实例） + intrinsics = CameraIntrinsics( + cx=response.intrinsics.cx, + cy=response.intrinsics.cy, + fx=response.intrinsics.fx, + fy=response.intrinsics.fy, + coeffs=list(response.intrinsics.coeffs) if hasattr(response.intrinsics, 'coeffs') else [] + ) + + return ( + CMVRErrorCode.CMVR_SUCCESS, + depth_array, + width, + height, + intrinsics + ) + + except grpc.RpcError as e: + print(f"深度图像RPC调用失败: {str(e)}") + return error_return + except Exception as e: + print(f"深度图像处理失败: {str(e)}") + return ( + CMVRErrorCode.CMVR_INTERNAL_ERROR, + np.array([], dtype=np.uint16), + 0, + 0, + default_intrinsics + ) + + + def get_rgbd_images(self) -> Tuple[CMVRErrorCode, np.ndarray, np.ndarray, int, int, CameraIntrinsics]: + """ + 获取同步RGBD图像及内参（修复返回值类型） + """ + # 创建默认内参实例 + default_intrinsics = CameraIntrinsics(cx=0, cy=0, fx=0, fy=0, coeffs=[]) + # 错误返回模板 + error_return = ( + CMVRErrorCode.CMVR_RPC_FAILED, + np.array([], dtype=np.uint8), + np.array([], dtype=np.uint16), + 0, + 0, + default_intrinsics + ) + + try: + generated = self._import_generated() + if not generated: + print("无法导入生成的protobuf模块") + return error_return + + # 创建请求 + request = generated.camera_command_pb2.GetRGBDImagesCommand.Request() + request.header.CopyFrom(self._create_command_header()) + + # 调用RPC + response = self.stub.GetRGBDImages(request) + + if not response.header.success: + print(f"RGBD图像请求失败，错误码: {response.header.error_code}") + return ( + CMVRErrorCode.CMVR_RPC_FAILED, + np.array([], dtype=np.uint8), + np.array([], dtype=np.uint16), + 0, + 0, + default_intrinsics + ) + + # 提取尺寸信息 + rgb_width, rgb_height = response.color_frame.width, response.color_frame.height + depth_width, depth_height = response.depth_frame.width, response.depth_frame.height + + if rgb_width != depth_width or rgb_height != depth_height: + print(f"RGBD尺寸不匹配: RGB({rgb_width}x{rgb_height}), Depth({depth_width}x{depth_height})") + return ( + CMVRErrorCode.CMVR_INTERNAL_ERROR, + np.array([], dtype=np.uint8), + np.array([], dtype=np.uint16), + 0, + 0, + default_intrinsics + ) + + # 处理RGB图像 + rgb_data = response.color_frame.data + rgb_expected = rgb_width * rgb_height * 3 + if len(rgb_data) != rgb_expected: + print(f"RGB数据尺寸不匹配: 预期 {rgb_expected}, 实际 {len(rgb_data)}") + return ( + CMVRErrorCode.CMVR_INTERNAL_ERROR, + np.array([], dtype=np.uint8), + np.array([], dtype=np.uint16), + 0, + 0, + default_intrinsics + ) + rgb_array = np.frombuffer(rgb_data, dtype=np.uint8).reshape((rgb_height, rgb_width, 3)) + + # 处理深度图像 + depth_data = response.depth_frame.data + depth_expected = depth_width * depth_height * 2 + if len(depth_data) != depth_expected: + print(f"深度数据尺寸不匹配: 预期 {depth_expected}, 实际 {len(depth_data)}") + return ( + CMVRErrorCode.CMVR_INTERNAL_ERROR, + np.array([], dtype=np.uint8), + np.array([], dtype=np.uint16), + 0, + 0, + default_intrinsics + ) + depth_array = np.frombuffer(depth_data, dtype=np.uint16).reshape((depth_height, depth_width)) + + # 解析内参 + intrinsics = CameraIntrinsics( + cx=response.intrinsics.cx, + cy=response.intrinsics.cy, + fx=response.intrinsics.fx, + fy=response.intrinsics.fy, + coeffs=list(response.intrinsics.coeffs) if hasattr(response.intrinsics, 'coeffs') else [] + ) + + return ( + CMVRErrorCode.CMVR_SUCCESS, + rgb_array, + depth_array, + rgb_width, + rgb_height, + intrinsics + ) + + except grpc.RpcError as e: + print(f"RGBD RPC调用失败: {str(e)}") + return error_return + except Exception as e: + print(f"RGBD图像处理失败: {str(e)}") + return ( + CMVRErrorCode.CMVR_INTERNAL_ERROR, + np.array([], dtype=np.uint8), + np.array([], dtype=np.uint16), + 0, + 0, + default_intrinsics + ) + + def start_record(self, video_path: str) -> CMVRErrorCode: """开始录像""" try: diff --git a/cmvr/client.py b/cmvr/client.py index 26c6015..d4d7282 100644 --- a/cmvr/client.py +++ b/cmvr/client.py @@ -7,6 +7,7 @@ from .camera_client import CameraClient from .dexhand_client import DexHandClient from .micphone_client import MicphoneClient from .speaker_client import SpeakerClient +from .humanoid_robot import HumanoidRobotClient class CMVRGrpcClient: """CMVR gRPC 主客户端""" @@ -15,7 +16,13 @@ class CMVRGrpcClient: self.connected = False # 创建 gRPC 通道 - self.channel = grpc.insecure_channel(server_address) + # 设置通道选项，增加最大接收和发送消息大小 + max_message_size: int = 20 * 1024 * 1024 + options = [ + ('grpc.max_send_message_length', max_message_size), + ('grpc.max_receive_message_length', max_message_size), + ] + self.channel = grpc.insecure_channel(server_address,options=options) # 初始化各服务的存根 self.generated = None # 推迟导入 @@ -34,6 +41,9 @@ class CMVRGrpcClient: self.speaker_stub = None self.speaker_map: Dict[str, SpeakerClient] = {} + self.humanoid_robot_stub = None + self.humanoid_robot_map: Dict[str, HumanoidRobotClient] = {} + # 检查连接状态 try: grpc.channel_ready_future(self.channel).result(timeout=5) @@ -49,6 +59,8 @@ class CMVRGrpcClient: self.micphone_stub = self.generated.microphone_service_pb2_grpc.MicPhoneServiceStub(self.channel) + self.humanoid_robot_stub = self.generated.humanoid_robot_sevice_pb2_grpc.HumanoidRobotServiceStub(self.channel) + except grpc.FutureTimeoutError: print(f"连接服务器超时: {server_address}") self.connected = False @@ -69,6 +81,9 @@ class CMVRGrpcClient: from generated.cmvr.api import microphone_command_pb2 from generated.cmvr.api import microphone_service_pb2_grpc + from generated.cmvr.api import humanoid_robot_command_pb2 + from generated.cmvr.api import humanoid_robot_service_pb2_grpc + self.generated = type('GeneratedModules', (), { 'biohead_service_pb2_grpc': biohead_service_pb2_grpc, 'common_pb2': common_pb2, @@ -80,7 +95,9 @@ class CMVRGrpcClient: 'speaker_service_pb2_grpc': speaker_service_pb2_grpc, 'speaker_command_pb2': speaker_command_pb2, 'microphone_service_pb2_grpc': microphone_service_pb2_grpc, - 'microphone_command_pb2': microphone_command_pb2 + 'microphone_command_pb2': microphone_command_pb2, + 'humanoid_robot_sevice_pb2_grpc': humanoid_robot_service_pb2_grpc, + 'humanoid_robot_command_pb2': humanoid_robot_command_pb2 }) except ImportError as e: @@ -118,6 +135,11 @@ class CMVRGrpcClient: if device_id not in self.micphone_map: self.micphone_map[device_id] = MicphoneClient(device_id, self.micphone_stub) return self.micphone_map[device_id] + + def get_humanoid_robot(self, device_id: str) -> HumanoidRobotClient: + if device_id not in self.micphone_map: + self.humanoid_robot_map[device_id] = HumanoidRobotClient(device_id, self.humanoid_robot_stub) + return self.humanoid_robot_map[device_id] def close(self): """关闭所有连接""" diff --git a/cmvr/enums.py b/cmvr/enums.py index 9401979..141310f 100644 --- a/cmvr/enums.py +++ b/cmvr/enums.py @@ -25,4 +25,28 @@ class FingerType(Enum): MIDDLE = 2 # 中指 INDEX = 3 # 食指 THUMB = 4 # 大拇指 - PALM = 5 # 掌心 \ No newline at end of file + PALM = 5 # 掌心 + + # 机器人相关枚举定义 +class RobotCartesian(Enum): + """机器人笛卡尔坐标系枚举""" + X = 0 + Y = 1 + Z = 2 + RX = 3 # 绕X轴旋转 + RY = 4 # 绕Y轴旋转 + RZ = 5 # 绕Z轴旋转 + +class RobotJointIndexDirection(Enum): + """机器人关节索引方向枚举""" + FORWARD = 0 + BACKWARD = 1 + X_POSITIVE = 2 # X轴正向 + X_NEGATIVE = 3 # X轴负向 + Y_POSITIVE = 4 # Y轴正向 + Y_NEGATIVE = 5 # Y轴负向 + Z_POSITIVE = 6 # Z轴正向 + Z_NEGATIVE = 7 # Z轴负向 + ROTATE_X = 8 # 绕X轴旋转 + ROTATE_Y = 9 # 绕Y轴旋转 + ROTATE_Z = 10 # 绕Z轴旋转 \ No newline at end of file diff --git a/cmvr/humanoid_robot.py b/cmvr/humanoid_robot.py new file mode 100644 index 0000000..c0f3405 --- /dev/null +++ b/cmvr/humanoid_robot.py @@ -0,0 +1,279 @@ +import grpc +import numpy as np +from typing import List, Optional, Tuple +from typing import Dict + +from .enums import CMVRErrorCode, RobotCartesian, RobotJointIndexDirection +from .models import JointCmd, Pose3D, JointState + +class HumanoidRobotClient: + """人形机器人客户端""" + + def __init__(self, device_id: str, stub): + """ + 初始化机器人客户端 + + 参数: + device_id: 设备ID + stub: gRPC stub实例 + """ + self.device_id = device_id + self.stub = stub + self.generated = None # 推迟导入 + + def _import_generated(self): + """推迟导入 generated 模块""" + if self.generated is None: + try: + # 从正确的路径导入生成的模块 + from generated.cmvr.api import common_pb2, humanoid_robot_command_pb2 + self.generated = type('GeneratedModules', (), { + 'common_pb2': common_pb2, + 'humanoid_robot_command_pb2': humanoid_robot_command_pb2 + }) + except ImportError as e: + print(f"导入生成的模块失败: {e}") + print("请确保已生成 protobuf 代码") + raise + return self.generated + + def _create_command_header(self): + """创建命令头""" + generated = self._import_generated() + header = generated.common_pb2.CommandHeader.Request() + header.device_id = self.device_id + header.timestamp.GetCurrentTime() + return header + + # ==================== 基本控制接口 ==================== + + def torqueOn(self) -> CMVRErrorCode: + try: + generated = self._import_generated() + request = generated.common_pb2.CommandHeader.Request() + request.CopyFrom(self._create_command_header()) + + response = self.stub.torqueOn(request) + + return CMVRErrorCode.CMVR_SUCCESS if response.success else CMVRErrorCode.CMVR_RPC_FAILED + + except grpc.RpcError as e: + print(f"开启使能失败: {e}") + return CMVRErrorCode.CMVR_RPC_FAILED + + def torqueOff(self) -> CMVRErrorCode: + try: + generated = self._import_generated() + request = generated.common_pb2.CommandHeader.Request() + request.CopyFrom(self._create_command_header()) + + response = self.stub.torqueOff(request) + + return CMVRErrorCode.CMVR_SUCCESS if response.success else CMVRErrorCode.CMVR_RPC_FAILED + + except grpc.RpcError as e: + print(f"关闭使能失败: {e}") + return CMVRErrorCode.CMVR_RPC_FAILED + + # ==================== 关节空间运动 ==================== + + def moveJ(self, joint_commands: List[JointCmd], + overall_vel: float = 0.1, overall_acc: float = 0.5) -> Tuple[CMVRErrorCode, Optional[str]]: + try: + generated = self._import_generated() + # 创建MoveJ请求 + request = generated.humanoid_robot_command_pb2.MoveJ.Request() + request.header.CopyFrom(self._create_command_header()) + request.vel = overall_vel + request.acc = overall_acc + + # 添加关节命令 + for cmd in joint_commands: + joint_cmd = request.cmds.add() + joint_cmd.joint_name = cmd.joint_name + joint_cmd.rad = cmd.rad + joint_cmd.vel = cmd.vel + + # 调用RPC接口 + response = self.stub.moveJ(request) + + return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED + + except grpc.RpcError as e: + return CMVRErrorCode.CMVR_RPC_FAILED + + # ==================== 直线空间运动 ==================== + + def moveL(self, ee_link: str, target_pose: Pose3D, + vel: float = 0.1, acc: float = 0.5) -> Tuple[CMVRErrorCode, Optional[str]]: + """ + 直线空间运动 (MoveL) + + 参数: + ee_link: 末端执行器连杆名称 + target_pose: 目标位姿 (Pose3D) + vel: 速度 (m/s) + acc: 加速度 (m/s²) + + 返回: + Tuple[CMVRErrorCode, Optional[str]]: 错误码和错误消息 + """ + try: + generated = self._import_generated() + # 创建MoveL请求 + request = generated.humanoid_robot_command_pb2.MoveL.Request() + request.header.CopyFrom(self._create_command_header()) + request.ee_link = ee_link + request.vel = vel + request.acc = acc + + # 设置目标位姿 + request.target_pose.x = target_pose.x + request.target_pose.y = target_pose.y + request.target_pose.z = target_pose.z + request.target_pose.rx = target_pose.rx + request.target_pose.ry = target_pose.ry + request.target_pose.rz = target_pose.rz + + # 调用RPC接口 + response = self.stub.moveL(request) + return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED + + except grpc.RpcError as e: + print(f"直线空间运动失败: {e}") + return CMVRErrorCode.CMVR_RPC_FAILED + + # ==================== 关节速度控制 ==================== + + def speedJ(self, joint_name: str, velocity: float, + direction: RobotJointIndexDirection = RobotJointIndexDirection.FORWARD, + acc: float = 0.5) -> Tuple[CMVRErrorCode, Optional[str]]: + """ + 关节速度控制 (SpeedJ) + + 参数: + joint_name: 关节名称 + velocity: 速度 (rad/s) + direction: 运动方向 + acc: 加速度 (rad/s²) + + 返回: + Tuple[CMVRErrorCode, Optional[str]]: 错误码和错误消息 + """ + try: + generated = self._import_generated() + # 创建SpeedJ请求 + request = generated.humanoid_robot_command_pb2.SpeedJ.Request() + request.header.CopyFrom(self._create_command_header()) + request.joint_name = joint_name + request.vel = abs(velocity) # 确保速度为正值 + request.acc = acc + request.direction = direction.value + + # 调用RPC接口 + response = self.stub.speedJ(request) + return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED + + except grpc.RpcError as e: + print(f"关节速度控制失败: {e}") + return CMVRErrorCode.CMVR_RPC_FAILED, str(e) + + # ==================== 笛卡尔速度控制 ==================== + + def speedL(self, ee_link: str, velocity: float, + cartesian: RobotCartesian = RobotCartesian.X, + direction: RobotJointIndexDirection = RobotJointIndexDirection.FORWARD, + acc: float = 0.5) -> Tuple[CMVRErrorCode, Optional[str]]: + """ + 笛卡尔速度控制 (SpeedL) + + 参数: + ee_link: 末端执行器连杆名称 + velocity: 速度 (m/s 或 rad/s，取决于cartesian类型) + cartesian: 笛卡尔坐标轴 + direction: 运动方向 + acc: 加速度 (m/s² 或 rad/s²) + + 返回: + Tuple[CMVRErrorCode, Optional[str]]: 错误码和错误消息 + """ + try: + generated = self._import_generated() + # 创建SpeedL请求 + request = generated.humanoid_robot_command_pb2.SpeedL.Request() + request.header.CopyFrom(self._create_command_header()) + request.ee_link = ee_link + request.vel = abs(velocity) + request.acc = acc + request.cartesian = cartesian.value + request.direction = direction.value + + # 调用RPC接口 + response = self.stub.speedL(request) + return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED + + except grpc.RpcError as e: + print(f"笛卡尔速度控制失败: {e}") + return CMVRErrorCode.CMVR_RPC_FAILED, str(e) + + # ==================== 状态获取 ==================== + + def get_joint_states(self) -> Tuple[CMVRErrorCode, List[JointState]]: + """ + 获取机器人所有关节状态 + + 返回: + Tuple[CMVRErrorCode, List[JointState]]: 错误码和关节状态对象列表 + """ + try: + generated = self._import_generated() + # 创建关节状态请求 + request = generated.humanoid_robot_command_pb2.JointRequest() + request.header.CopyFrom(self._create_command_header()) + + # 调用RPC接口 + response = self.stub.getJointState(request) + + if not response.header.success: + print(f"RPC调用失败: {response.header}") + return CMVRErrorCode.CMVR_RPC_FAILED, [] + + # 检查是否有状态数据 + if not response.state: + print("响应中没有关节状态数据") + return CMVRErrorCode.CMVR_NO_DATA, [] + + # 解析所有JointState + joint_states = [] + for proto_joint_state in response.state: + # 解析响应数据到JointState对象 + # 注意proto字段名和Python dataclass字段名的映射 + joint_state = JointState( + name=list(proto_joint_state.name), # proto: name -> Python: name + position=list(proto_joint_state.position), # proto: position -> Python: position + velocity=list(proto_joint_state.velocity), # proto: velocity -> Python: velocity + effort=list(proto_joint_state.effort), # proto: effort -> Python: effort + timestamp=proto_joint_state.timestamp # proto: timestamp -> Python: timestamp + ) + joint_states.append(joint_state) + + if not joint_states: + print("解析后没有有效的关节状态数据") + return CMVRErrorCode.CMVR_NO_DATA, [] + + return CMVRErrorCode.CMVR_SUCCESS, joint_states + + except grpc.RpcError as e: + print(f"gRPC错误 - 获取关节状态失败: {e}") + if hasattr(e, 'code'): + if e.code() == grpc.StatusCode.NOT_FOUND: + return CMVRErrorCode.CMVR_NOT_FOUND, [] + elif e.code() == grpc.StatusCode.UNAVAILABLE: + return CMVRErrorCode.CMVR_CONNECTION_FAILED, [] + return CMVRErrorCode.CMVR_RPC_FAILED, [] + + except Exception as e: + print(f"未知错误 - 获取关节状态失败: {e}") + import traceback + traceback.print_exc() + return CMVRErrorCode.CMVR_UNKNOWN_ERROR, [] \ No newline at end of file diff --git a/cmvr/models.py b/cmvr/models.py index 8bb938d..8238886 100644 --- a/cmvr/models.py +++ b/cmvr/models.py @@ -213,4 +213,65 @@ class HandTactileSensors: print(f" 食指指端: {self.index_tip.rows}x{self.index_tip.cols} (数据大小: {self.index_tip.byteSize} bytes)") print(f" 大拇指指端: {self.thumb_tip.rows}x{self.thumb_tip.cols} (数据大小: {self.thumb_tip.byteSize} bytes)") print(f" 大拇指指中: {self.thumb_middle.rows}x{self.thumb_middle.cols} (数据大小: {self.thumb_middle.byteSize} bytes)") - print(f" 掌心: {self.palm.rows}x{self.palm.cols} (数据大小: {self.palm.byteSize} bytes)") \ No newline at end of file + print(f" 掌心: {self.palm.rows}x{self.palm.cols} (数据大小: {self.palm.byteSize} bytes)") + +# 机器人相关数据结构 +@dataclass +class JointCmd: + """关节控制命令""" + joint_name: str = "" # 关节名称 + rad: float = 0.0 # 弧度 + vel: float = 0.0 # 角速度，rad/s + +@dataclass +class Pose3D: + """3D位姿""" + x: float = 0.0 # X坐标 + y: float = 0.0 # Y坐标 + z: float = 0.0 # Z坐标 + rx: float = 0.0 # 绕X轴旋转（弧度） + ry: float = 0.0 # 绕Y轴旋转（弧度） + rz: float = 0.0 # 绕Z轴旋转（弧度） + + def to_array(self) -> np.ndarray: + """转换为numpy数组""" + return np.array([self.x, self.y, self.z, self.rx, self.ry, self.rz]) + + @classmethod + def from_array(cls, arr: np.ndarray) -> 'Pose3D': + """从numpy数组创建Pose3D""" + return cls(arr[0], arr[1], arr[2], arr[3], arr[4], arr[5]) + +@dataclass +class JointState: + """关节状态""" + name: List[str] = field(default_factory=list) # 关节名称列表 + position: List[float] = field(default_factory=list) # 关节位置列表（弧度） + velocity: List[float] = field(default_factory=list) # 关节速度列表（rad/s） + effort: List[float] = field(default_factory=list) # 关节力矩列表 + timestamp: float = 0.0 # 时间戳 + + def __post_init__(self): + """确保列表长度一致""" + if len(self.name) > 0: + n_joints = len(self.name) + self.position = self.position[:n_joints] + [0.0] * (n_joints - len(self.position)) + self.velocity = self.velocity[:n_joints] + [0.0] * (n_joints - len(self.velocity)) + self.effort = self.effort[:n_joints] + [0.0] * (n_joints - len(self.effort)) + + def get_joint_by_name(self, joint_name: str) -> tuple: + """根据关节名称获取状态""" + if joint_name in self.name: + idx = self.name.index(joint_name) + return (self.position[idx], self.velocity[idx], self.effort[idx]) + return (0.0, 0.0, 0.0) + + def to_dict(self) -> dict: + """转换为字典格式""" + return { + 'name': self.name, + 'positions': self.position, + 'velocities': self.velocity, + 'efforts': self.effort, + 'timestamp': self.timestamp + } diff --git a/example_usage.py b/example_usage.py new file mode 100644 index 0000000..c9c7d08 --- /dev/null +++ b/example_usage.py @@ -0,0 +1,870 @@ +#!/usr/bin/env python3 + +import sys +import os +import time +import random +import grpc # 新增grpc导入 +import cv2 +from typing import Generator # 新增Generator类型导入 +import numpy as np # 确保导入numpy（传感器数据处理需要） + +# 添加项目根目录到 Python 路径 +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_biohead(): + """测试仿生头功能""" + print("=== 测试仿生头功能 ===") + + # 初始化客户端 + client = CMVRGrpcClient("192.168.1.222:50051") + if not client.is_connected(): + print("连接服务器失败") + return + + # 获取仿生头 + biohead = client.get_biohead("bio_head") + + # 创建表情 + expression = FacialExpressionState() + expression.left_eyebrow_outside_y = random.uniform(0, 1) + expression.jaw_x = 0.5 + expression.jaw_y = 0.5 + # # 设置眉毛，使用随机数并打印 + # expression.left_eyebrow_outside_y = random.uniform(0, 1) + # expression.left_eyebrow_inside_y = random.uniform(0, 1) + # expression.right_eyebrow_outside_y = random.uniform(0, 1) + # expression.right_eyebrow_inside_y = random.uniform(0, 1) + # print(f"眉毛设置: 左外 {expression.left_eyebrow_outside_y}, 左内 {expression.left_eyebrow_inside_y}, 右外 {expression.right_eyebrow_outside_y}, 右内 {expression.right_eyebrow_inside_y}") + # + # # 设置眼睑，使用随机数并打印 + # expression.left_eye_upper_lid_y = random.uniform(0, 1) + # expression.left_eye_lower_lid_y = random.uniform(0, 1) + # expression.right_eye_upper_lid_y = random.uniform(0, 1) + # expression.right_eye_lower_lid_y = random.uniform(0, 1) + # print(f"眼睑设置: 左上 {expression.left_eye_upper_lid_y}, 左下 {expression.left_eye_lower_lid_y}, 右上 {expression.right_eye_upper_lid_y}, 右下 {expression.right_eye_lower_lid_y}") + # + # # 设置眼球，使用随机数并打印 + # expression.left_eye_ball_x = random.uniform(0, 1) + # expression.left_eye_ball_y = random.uniform(0, 1) + # expression.right_eye_ball_x = random.uniform(0, 1) + # expression.right_eye_ball_y = random.uniform(0, 1) + # print(f"眼球设置: 左X {expression.left_eye_ball_x}, 左Y {expression.left_eye_ball_y}, 右X {expression.right_eye_ball_x}, 右Y {expression.right_eye_ball_y}") + # + # # 设置嘴巴，使用随机数并打印 + # expression.upper_lip_y = random.uniform(0, 1) + # expression.lower_lip_y = random.uniform(0, 1) + # print(f"嘴巴设置: 上唇 {expression.upper_lip_y}, 下唇 {expression.lower_lip_y}") + # + # # 设置下巴，使用随机数并打印 + # expression.jaw_x = random.uniform(0, 1) + # expression.jaw_y = random.uniform(0, 1) + # print(f"下巴设置: X {expression.jaw_x}, Y {expression.jaw_y}") + + # 设置表情 + result = biohead.set_expression(expression) + if result == CMVRErrorCode.CMVR_SUCCESS: + print("设置表情成功") + else: + print(f"设置表情失败，错误码: {result}") + + # 测试流式控制 + print("开始流式控制表情 (5次眨眼)...") + result = biohead.start_stream() + if result != CMVRErrorCode.CMVR_SUCCESS: + print(f"开始流式会话失败，错误码: {result}") + else: + for i in range(5): + # 创建 protobuf 消息用于流式传输 + proto_expr = biohead_command_pb2.FacialExpression() + + # 睁眼，使用随机数并打印 + proto_expr.eyelid.left_upper_y = random.uniform(0, 1) + proto_expr.eyelid.left_lower_y = random.uniform(0, 1) + proto_expr.eyelid.right_upper_y = random.uniform(0, 1) + proto_expr.eyelid.right_lower_y = random.uniform(0, 1) + print(f"第{i+1}次流式睁眼设置: 左上 {proto_expr.eyelid.left_upper_y}, 左下 {proto_expr.eyelid.left_lower_y}, 右上 {proto_expr.eyelid.right_upper_y}, 右下 {proto_expr.eyelid.right_lower_y}") + + result = biohead.stream_expression(proto_expr) + if result == CMVRErrorCode.CMVR_SUCCESS: + print(f"第{i+1}次流式睁眼成功") + else: + print(f"第{i+1}次流式睁眼失败，错误码: {result}") + + + + # 闭眼，使用随机数并打印 + proto_expr.eyelid.left_upper_y = random.uniform(0, 1) + proto_expr.eyelid.left_lower_y = random.uniform(0, 1) + proto_expr.eyelid.right_upper_y = random.uniform(0, 1) + proto_expr.eyelid.right_lower_y = random.uniform(0, 1) + print(f"第{i+1}次流式闭眼设置: 左上 {proto_expr.eyelid.left_upper_y}, 左下 {proto_expr.eyelid.left_lower_y}, 右上 {proto_expr.eyelid.right_upper_y}, 右下 {proto_expr.eyelid.right_lower_y}") + + result = biohead.stream_expression(proto_expr) + if result == CMVRErrorCode.CMVR_SUCCESS: + print(f"第{i+1}次流式闭眼成功") + else: + print(f"第{i+1}次流式闭眼失败，错误码: {result}") + + + + # 结束流式会话 + result = biohead.end_stream() + if result == CMVRErrorCode.CMVR_SUCCESS: + print("结束流式会话成功") + else: + print(f"结束流式会话失败，错误码: {result}") + + print("流式控制完成") + + # 获取系统状态 + result = biohead.get_system_status() + if result == CMVRErrorCode.CMVR_SUCCESS: + print("获取系统状态成功") + else: + print(f"获取系统状态失败，错误码: {result}") + + # 测试紧急停止 + result = biohead.emergency_stop() + if result == CMVRErrorCode.CMVR_SUCCESS: + print("紧急停止成功") + else: + print(f"紧急停止失败，错误码: {result}") + + client.close() + +def test_camera(): + """测试相机功能""" + print("=== 测试相机功能 ===") + + # 初始化客户端 + client = CMVRGrpcClient("192.168.1.222:50060") + if not client.is_connected(): + print("连接服务器失败") + return + + # 获取相机客户端 + camera = client.get_camera("cam4") # 假设设备ID为"cam4" + + # 获取相机初始状态 + error_code, state = camera.get_status() + if error_code == CMVRErrorCode.CMVR_SUCCESS: + print(f"相机初始状态: 已初始化={state.is_initialized}, 已打开={state.is_opened}, " + f"已流传输={state.is_streaming}, 已录制={state.is_recording}, " + f"分辨率={state.width}x{state.height}, FPS={state.fps}") + else: + print(f"获取相机状态失败，错误码: {error_code}") + client.close() + return + + # 启动相机 + start_result = camera.start_camera() + if start_result == CMVRErrorCode.CMVR_SUCCESS: + print("相机启动成功") + else: + print(f"相机启动失败，错误码: {start_result}") + client.close() + return + + # 等待相机启动 + time.sleep(1) + + # 再次获取相机状态确认 + error_code, state = camera.get_status() + if error_code == CMVRErrorCode.CMVR_SUCCESS: + print(f"启动后状态: 已打开={state.is_opened}, 已流传输={state.is_streaming}") + else: + print(f"获取相机状态失败，错误码: {error_code}") + + # 测试获取RGB图像并保存 + print("开始获取图像并保存 (3次)...") + for i in range(3): + error_code, img_array, width, height = camera.get_rgb_image() + if error_code == CMVRErrorCode.CMVR_SUCCESS and img_array.size > 0: + # 生成保存路径（当前目录） + img_filename = f"camera_test_image_{i+1}_{int(time.time())}.jpg" + # 转换颜色格式（如果需要） + if len(img_array.shape) == 3: + cv2_img = cv2.cvtColor(img_array, cv2.COLOR_RGB2BGR) + else: + cv2_img = img_array + # 保存图像 + cv2.imwrite(img_filename, cv2_img) + print(f"第{i+1}次获取图像成功，已保存至: {img_filename}，分辨率: {width}x{height}") + else: + print(f"第{i+1}次获取图像失败，错误码: {error_code}") + time.sleep(1) + + # 测试录像功能 + video_path = f"test_recording_{int(time.time())}.mp4" + print(f"开始录像，保存路径: {os.path.abspath(video_path)}") # 显示绝对路径 + record_start_result = camera.start_record(video_path) + if record_start_result == CMVRErrorCode.CMVR_SUCCESS: + print("录像开始成功，录制5秒...") + time.sleep(5) + + # 停止录像 + record_stop_result = camera.stop_record() + if record_stop_result == CMVRErrorCode.CMVR_SUCCESS: + print("录像停止成功") + else: + print(f"录像停止失败，错误码: {record_stop_result}") + else: + print(f"录像开始失败，错误码: {record_start_result}") + + # 停止相机 + stop_result = camera.stop_camera() + if stop_result == CMVRErrorCode.CMVR_SUCCESS: + print("相机停止成功") + else: + print(f"相机停止失败，错误码: {stop_result}") + + # 最终状态确认 + error_code, state = camera.get_status() + if error_code == CMVRErrorCode.CMVR_SUCCESS: + print(f"最终状态: 已打开={state.is_opened}, 已流传输={state.is_streaming}") + else: + print(f"获取相机状态失败，错误码: {error_code}") + + client.close() + +def test_rgb_image_stream(): + """测试双向流视频帧数据接口""" + print("=== 测试双向流视频帧数据接口 ===") + + # 初始化GRPC客户端 + client = CMVRGrpcClient("192.168.1.222:50060") + if not client.is_connected(): + print("连接服务器失败") + return + + # 获取camera客户端 + camera = client.get_camera("cam4") + if not camera: + print("获取camera客户端失败") + client.close() + return + # 启动相机 + start_result = camera.start_camera() + if start_result == CMVRErrorCode.CMVR_SUCCESS: + print("相机启动成功") + else: + print(f"相机启动失败，错误码: {start_result}") + client.close() + return + # 等待相机启动 + time.sleep(1) + try: + # 1. 启动传感器数据流 + print("\n--- 启动rgb数据流 ---") + # 创建请求生成器（双向流不需要参数，发送空请求） + def request_generator() -> Generator: + # 发送初始请求启动流 + yield camera.create_rgb_stream_request() + # 保持流连接，每5秒发送一次心跳（可选） + try: + while True: + time.sleep(5) + # 发送空请求保持连接 + yield camera.create_rgb_stream_request() + except GeneratorExit: + print("请求生成器已关闭") + return + + # 2. 接收并处理流数据 + print("开始接收rgb流数据（5秒后自动停止）...") + start_time = time.time() + stream_duration = 15 # 接收15秒数据 + + keyframe_count = 0 # 关键帧计数器 + total_frame_count = 0 # 总帧数计数器 + # 存储流迭代器的变量 + stream_iterator = camera.get_rgb_stream(request_generator()) + # 调用双向流接口 + for feedback in stream_iterator: + # 检查是否超时 + if time.time() - start_time > stream_duration: + print("\n达到预设接收时间，停止接收") + break + total_frame_count += 1 # 累加总帧数 + # 检查是否为关键帧并计数 + if feedback.color_frame.is_key_frame: + keyframe_count += 1 + print(f"[关键帧 #{keyframe_count}] 收到关键帧 (总帧数: {total_frame_count})") + stream_iterator.cancel() + print(f"\n流接收结束 - 总帧数: {total_frame_count}, 关键帧数: {keyframe_count}, 关键帧占比: {keyframe_count/total_frame_count:.2%}") + + + except grpc.RpcError as e: + print(f"双向流通信错误: {e}") + except Exception as e: + print(f"处理流数据时发生错误: {e}") + finally: + # 关闭连接 + client.close() + print("\n=== 双向流传感器数据测试完成 ===") + +def test_dexhand(): + """测试DexHand功能（获取状态、设置角度和传感器数据）""" + print("=== 测试DexHand功能 ===") + + # 初始化GRPC客户端（使用实际服务器地址） + client = CMVRGrpcClient("192.168.1.222:50060") + if not client.is_connected(): + print("连接服务器失败") + return + + # 获取DexHand客户端（假设设备ID为"hand1"，根据实际设备ID修改） + dexhand = client.get_dexhand("hand1") + if not dexhand: + print("获取DexHand客户端失败") + client.close() + return + + # 1. 获取初始状态 + print("\n--- 获取初始状态 ---") + error_code, state = dexhand.get_status() + if error_code == CMVRErrorCode.CMVR_SUCCESS: + print(f"初始化状态: 已初始化={state.is_initialized}") + print("各自由度状态:") + for hand in state.hands: + print(f" 自由度ID: {hand.dof_id}, 角度: {hand.angle}, 速度: {hand.speed}, " + f"受力: {hand.force}, 位置: {hand.position}, 温度: {hand.temperature}°C") + if hand.error != 0: + print(f" 警告: 自由度ID {hand.dof_id} 存在故障: {hand.error_message}") + else: + print(f"获取初始状态失败，错误码: {error_code}") + client.close() + return + + # 1.1 获取初始传感器数据 + print("\n--- 获取初始传感器数据 ---") + error_code, sensors = dexhand.get_sensor_data() + if error_code == CMVRErrorCode.CMVR_SUCCESS: + print("初始传感器数据摘要:") + sensors.print_summary() # 调用传感器数据摘要打印方法 + # 打印掌心和食指指端的传感器数据示例 + print(f" 掌心传感器数据形状: {sensors.palm.data.shape}") + print(f" 食指指端传感器数据形状: {sensors.index_tip.data.shape}") + else: + print(f"获取初始传感器数据失败，错误码: {error_code}") + + # 2. 设置测试角度（小拇指、食指、大拇指弯曲） + print("\n--- 设置测试角度 ---") + # 角度字典: 键为自由度ID (0-6)，值为0-1百分比 + test_angles = { + 0: 0.9, # 小拇指 + 1: 0.9, # 无名指 + 2: 0.9, # 中指 + 3: 0.9, # 食指 + 4: 0.9, # 大拇指弯曲 + 5: 0.9 # 大拇指旋转 + } + + # 打印设置信息（映射自由度ID到手指名称） + finger_map = { + 0: "小拇指", + 1: "无名指", + 2: "中指", + 3: "食指", + 4: "大拇指弯曲", + 5: "大拇指旋转", + 6: "预留自由度" + } + print("准备设置角度:") + for dof_id, value in test_angles.items(): + finger_name = finger_map.get(dof_id, f"自由度{dof_id}") + print(f" {finger_name} (ID:{dof_id}): {value*100}%") + + # 执行角度设置 + set_result = dexhand.set_angle(test_angles) + if set_result == CMVRErrorCode.CMVR_SUCCESS: + print("角度设置命令发送成功，等待执行器动作...") + time.sleep(2) # 等待执行器完成动作 + else: + print(f"角度设置失败，错误码: {set_result}") + client.close() + return + + # 2.1 获取角度变化后的传感器数据 + print("\n--- 获取角度变化后传感器数据 ---") + error_code, sensors_after = dexhand.get_sensor_data() + if error_code == CMVRErrorCode.CMVR_SUCCESS: + print("角度变化后传感器数据摘要:") + # 对比食指指腹在动作前后的平均压力变化 + if hasattr(sensors, 'index_pad') and hasattr(sensors_after, 'index_pad'): + avg_before = sensors.index_pad.data.mean() if sensors.index_pad.data.size > 0 else 0 + avg_after = sensors_after.index_pad.data.mean() if sensors_after.index_pad.data.size > 0 else 0 + print(f" 食指指腹平均压力变化: {avg_before:.2f} → {avg_after:.2f}") + + # 打印大拇指指端数据示例（前3个值） + if sensors_after.thumb_tip.data.size > 0: + print(f" 大拇指指端部分数据: {sensors_after.thumb_tip.data.flatten()[:3]}...") + else: + print(f"获取角度变化后传感器数据失败，错误码: {error_code}") + + # 3. 获取设置后的状态 + print("\n--- 获取设置后状态 ---") + error_code, state = dexhand.get_status() + if error_code == CMVRErrorCode.CMVR_SUCCESS: + print("设置后各自由度状态:") + for hand in state.hands: + # 只打印我们设置过的自由度 + if hand.dof_id in test_angles: + print(f" 自由度ID: {hand.dof_id}, 新角度: {hand.angle}, 当前位置: {hand.position}") + else: + print(f"获取设置后状态失败，错误码: {error_code}") + + # 4. 恢复默认角度（复位操作） + print("\n--- 恢复默认角度 ---") + default_angles = { + 0: 0.1, + 1: 0.1, + 2: 0.1, + 3: 0.1, + 4: 0.1, + 5: 0.8 + } + + set_result = dexhand.set_angle(default_angles) + if set_result == CMVRErrorCode.CMVR_SUCCESS: + print("默认角度恢复成功，等待复位完成...") + time.sleep(2) + else: + print(f"默认角度恢复失败，错误码: {set_result}") + + # 4.1 获取复位后的传感器数据 + print("\n--- 获取复位后传感器数据 ---") + error_code, sensors_reset = dexhand.get_sensor_data() + if error_code == CMVRErrorCode.CMVR_SUCCESS: + print("复位后传感器数据摘要:") + # 对比掌心压力在复位前后的变化 + if hasattr(sensors_after, 'palm') and hasattr(sensors_reset, 'palm'): + avg_after = sensors_after.palm.data.mean() if sensors_after.palm.data.size > 0 else 0 + avg_reset = sensors_reset.palm.data.mean() if sensors_reset.palm.data.size > 0 else 0 + print(f" 掌心平均压力变化: {avg_after:.2f} → {avg_reset:.2f}") + else: + print(f"获取复位后传感器数据失败，错误码: {error_code}") + + # 5. 最终状态确认 + print("\n--- 最终状态确认 ---") + error_code, state = dexhand.get_status() + if error_code == CMVRErrorCode.CMVR_SUCCESS: + print(f"最终初始化状态: {state.is_initialized}") + print("关键自由度最终位置:") + for dof_id in test_angles.keys(): + for hand in state.hands: + if hand.dof_id == dof_id: + print(f" 自由度ID {dof_id}: 角度={hand.angle}, 位置={hand.position}") + else: + print(f"获取最终状态失败，错误码: {error_code}") + + # 关闭连接 + client.close() + print("\n=== DexHand测试完成 ===") + +def test_sensor_data_stream(): + """测试双向流传感器数据接口""" + print("=== 测试双向流传感器数据接口 ===") + + # 初始化GRPC客户端 + client = CMVRGrpcClient("192.168.1.222:50052") + if not client.is_connected(): + print("连接服务器失败") + return + + # 获取DexHand客户端 + dexhand = client.get_dexhand("hand1") + if not dexhand: + print("获取DexHand客户端失败") + client.close() + return + + try: + # 1. 启动传感器数据流 + print("\n--- 启动传感器数据流 ---") + # 创建请求生成器（双向流不需要参数，发送空请求） + def request_generator() -> Generator: + # 发送初始请求启动流 + yield dexhand.create_stream_request() + # 保持流连接，每5秒发送一次心跳（可选） + try: + while True: + time.sleep(5) + # 发送空请求保持连接 + yield dexhand.create_stream_request() + except GeneratorExit: + print("请求生成器已关闭") + return + + # 2. 接收并处理流数据 + print("开始接收传感器流数据（5秒后自动停止）...") + start_time = time.time() + stream_duration = 15 # 接收15秒数据 + + # 调用双向流接口 + for feedback in dexhand.get_sensor_data_stream(request_generator()): + # 检查是否超时 + if time.time() - start_time > stream_duration: + print("\n达到预设接收时间，停止接收") + break + + # # 检查反馈状态 + # if not feedback.header.success: + # print(f"数据流接收失败，错误码: {feedback.header.error_code}") + # continue + + # 处理传感器数据（转换为HandTactileSensors对象） + error_code, sensors = dexhand.parse_sensor_stream_data(feedback) + if error_code != CMVRErrorCode.CMVR_SUCCESS: + print("传感器数据解析失败") + continue + + # 打印传感器数据摘要（每秒打印一次） + if int(time.time()) % 1 == 0: # 控制打印频率 + print(f"\n--- 传感器数据 (时间: {time.time() - start_time:.2f}s) ---") + sensors.print_summary() + + # 打印食指指端的实时数据示例 + if sensors.index_tip.data is not None and sensors.index_tip.data.size > 0: + print(f"食指指端平均压力: {sensors.index_tip.data.mean():.2f}") + if sensors.palm.data is not None and sensors.palm.data.size > 0: + print(f"掌心平均压力: {sensors.palm.data.mean():.2f}") + + except grpc.RpcError as e: + print(f"双向流通信错误: {e}") + except Exception as e: + print(f"处理流数据时发生错误: {e}") + finally: + # 关闭连接 + client.close() + print("\n=== 双向流传感器数据测试完成 ===") + +def test_speaker(): + """测试扬声器功能""" + print("\n=== 测试扬声器功能 ===") + + # 初始化GRPC客户端 + client = CMVRGrpcClient("192.168.1.222:50060") + if not client.is_connected(): + print("连接服务器失败") + return + + speaker = client.get_speaker("spk1") + if speaker is None: + print("获取扬声器失败") + return + + # 获取状态 + result, status = speaker.get_status() + if result == CMVRErrorCode.CMVR_SUCCESS: + print(f"扬声器状态: 初始化={status.is_initialized}, 运行中={status.is_running}") + else: + print(f"获取扬声器状态失败，错误码: {result}") + + # 设置音量 + result = speaker.set_volume(80) # 80%音量 + if result == CMVRErrorCode.CMVR_SUCCESS: + print("设置音量成功") + else: + print(f"设置音量失败，错误码: {result}") + + # 获取音量 + result, volume = speaker.get_volume() + if result == CMVRErrorCode.CMVR_SUCCESS: + print(f"当前音量: {volume}%") + else: + print(f"获取音量失败，错误码: {result}") + + # 播放音频 (需要替换为实际存在的音频文件路径) + audio_path = "/home/share/assets/upload/员工女_车内温度有点低，是否需要调高空调？.wav" + print(f"尝试播放音频: {audio_path}") + result = speaker.play_audio(audio_path) + if result == CMVRErrorCode.CMVR_SUCCESS: + print("开始播放音频") + + # 等待3秒 + time.sleep(3) + + # 暂停播放 + result = speaker.pause_audio() + if result == CMVRErrorCode.CMVR_SUCCESS: + print("暂停播放") + + # 等待1秒 + time.sleep(1) + + # 继续播放 + result = speaker.resume_audio() + if result == CMVRErrorCode.CMVR_SUCCESS: + print("继续播放") + + # 等待2秒 + time.sleep(2) + + # 停止播放 + result = speaker.stop_audio() + if result == CMVRErrorCode.CMVR_SUCCESS: + print("停止播放") + else: + print(f"停止播放失败，错误码: {result}") + else: + print(f"继续播放失败，错误码: {result}") + else: + print(f"暂停播放失败，错误码: {result}") + else: + print(f"播放音频失败，错误码: {result}") + +def test_microphone(): + """测试麦克风功能""" + print("\n=== 测试麦克风功能 ===") + # 初始化GRPC客户端 + client = CMVRGrpcClient("192.168.1.222:50060") + if not client.is_connected(): + print("连接服务器失败") + return + + microphone = client.get_micphone("mic2") + if microphone is None: + print("获取麦克风失败") + return + + # 获取状态 + result, status = microphone.get_status() + if result == CMVRErrorCode.CMVR_SUCCESS: + print(f"麦克风状态: 初始化={status.is_initialized}, 录音中={status.is_recording}") + else: + print(f"获取麦克风状态失败，错误码: {result}") + + # 开始录音 + audio_path = "/home/linbo/Data/Audio/recorded_audio.mp3" + result = microphone.start_record(audio_path) + if result == CMVRErrorCode.CMVR_SUCCESS: + print(f"开始录音，保存到: {audio_path}") + + # 录音5秒 + time.sleep(5) + + # 停止录音 + result = microphone.stop_record() + if result == CMVRErrorCode.CMVR_SUCCESS: + print("停止录音") + else: + print(f"停止录音失败，错误码: {result}") + else: + print(f"开始录音失败，错误码: {result}") + +def test_humanoid_robot(): + """测试机器人功能""" + print("\n=== 测试机器人功能 ===") + # 初始化GRPC客户端 + 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 + + + + # 使能 + # print("\n1. 开启机器人使能...") + # result = humanoidRobot.torqueOn() + # if result == CMVRErrorCode.CMVR_SUCCESS: + # print("✅ 使能开启成功") + # else: + # print(f"❌ 使能开启失败，错误码: {result}") + # return + + # 等待使能生效 + import time + time.sleep(1) + + try: + # 获取初始状态 + print("\n2. 获取初始关节状态...") + # 获取状态 + result, joint_states = humanoidRobot.get_joint_states() + if result == CMVRErrorCode.CMVR_SUCCESS: + print(f"成功获取 {len(joint_states)} 个关节状态组") + + for i, status in enumerate(joint_states): + print(f"\n=== 关节状态组 {i+1} ===") + print(f"时间戳: {status.timestamp:.3f}s") + print(f"关节数量: {len(status.name)}") + + if len(status.name) > 0: + print("\n关节详情:") + print(f"{'关节名称':<20} {'位置(rad)':<12} {'速度(rad/s)':<12} {'力矩':<10}") + print("-" * 55) + + for j, name in enumerate(status.name): + pos = status.position[j] if j < len(status.position) else 0.0 + vel = status.velocity[j] if j < len(status.velocity) else 0.0 + eff = status.effort[j] if j < len(status.effort) else 0.0 + + print(f"{name:<20} {pos:<12.4f} {vel:<12.4f} {eff:<10.4f}") + else: + print("该状态组没有关节数据") + + # 打印所有状态的汇总信息 + print(f"\n=== 汇总信息 ===") + total_joints = sum(len(state.name) for state in joint_states) + timestamps = [state.timestamp for state in joint_states] + print(f"总关节数: {total_joints}") + print(f"时间戳范围: {min(timestamps):.3f}s - {max(timestamps):.3f}s") + + else: + print(f"获取关节状态失败，错误码: {result}") + + return + # 测试moveJ - 关节空间运动 + print("\n3. 测试关节空间运动 (moveJ)...") + + # 创建关节命令列表,回到零位 + joint_commands = [ + JointCmd(joint_name="L_SHOULDER_P", rad=0, vel=0.8), + JointCmd(joint_name="L_SHOULDER_R", rad=0, vel=0.8), + JointCmd(joint_name="L_SHOULDER_Y", rad=0, vel=0.8), + JointCmd(joint_name="L_ELBOW_R", rad=0, vel=0.8), + JointCmd(joint_name="L_WRIST_P", rad=0, vel=0.8), + JointCmd(joint_name="L_WRIST_Y", rad=0, vel=0.8), + JointCmd(joint_name="L_WRIST_R", rad=0, vel=0.8), + + JointCmd(joint_name="R_SHOULDER_P", rad=0, vel=0.8), + JointCmd(joint_name="R_SHOULDER_R", rad=0, vel=0.8), + JointCmd(joint_name="R_SHOULDER_Y", rad=0, vel=0.8), + JointCmd(joint_name="R_ELBOW_R", rad=0, vel=0.8), + JointCmd(joint_name="R_WRIST_P", rad=0, vel=0.8), + JointCmd(joint_name="R_WRIST_Y", rad=0, vel=0.8), + JointCmd(joint_name="R_WRIST_R", rad=0, vel=0.8), + + JointCmd(joint_name="WAIST_P", rad=0, vel=0.8), + JointCmd(joint_name="WAIST_Y", rad=0, vel=0.8) + ] + + result, _ = humanoidRobot.moveJ(joint_commands, overall_vel=0.8, overall_acc=0.4) + if result == CMVRErrorCode.CMVR_SUCCESS: + print("✅ moveJ 命令发送成功") + time.sleep(3) # 等待运动完成 + else: + print(f"❌ moveJ 命令发送失败，错误码: {result}") + + # 获取运动后的状态 + print("\n 获取moveJ后的关节状态...") + result, status = humanoidRobot.get_joint_state() + if result == CMVRErrorCode.CMVR_SUCCESS: + print(f" 运动后时间戳: {status.timestamp:.3f}s") + print(f" 关节数量: {len(status.name)}") + + # 测试moveL - 直线空间运动 + print("\n4. 测试直线空间运动 (moveL)...") + # 创建目标位姿 (相对于当前位姿的小幅移动) + target_pose = Pose3D(x=0.05, y=0.0, z=0.0, rx=0.0, ry=0.0, rz=0.0) + + result, _ = humanoidRobot.moveL(ee_link="R_WRIST_R", target_pose=target_pose, + vel=0.05, acc=0.2) + if result == CMVRErrorCode.CMVR_SUCCESS: + print("✅ moveL 命令发送成功") + time.sleep(3) # 等待运动完成 + else: + print(f"❌ moveL 命令发送失败，错误码: {result}") + + # 测试speedJ - 关节速度控制 + print("\n5. 测试关节速度控制 (speedJ)...") + result, error_msg = humanoidRobot.speedJ(joint_name="R_SHOULDER_P", + velocity=0.1, + direction=RobotJointIndexDirection.FORWARD, + acc=0.3) + if result == CMVRErrorCode.CMVR_SUCCESS: + print("✅ speedJ 命令发送成功") + time.sleep(2) # 运行2秒 + # 停止运动 + stop_result, _ = humanoidRobot.speedJ(joint_name="R_SHOULDER_P", + velocity=0.0, + direction=RobotJointIndexDirection.FORWARD, + acc=0.3) + print(" 停止speedJ运动") + else: + print(f"❌ speedJ 命令发送失败，错误码: {result}") + if error_msg: + print(f" 错误信息: {error_msg}") + + # 测试speedL - 笛卡尔速度控制 + print("\n6. 测试笛卡尔速度控制 (speedL)...") + result, error_msg = humanoidRobot.speedL(ee_link="R_WRIST_R", + velocity=0.02, + cartesian=RobotCartesian.X, + direction=RobotJointIndexDirection.FORWARD, + acc=0.1) + if result == CMVRErrorCode.CMVR_SUCCESS: + print("✅ speedL 命令发送成功") + time.sleep(2) # 运行2秒 + # 停止运动 + stop_result, _ = humanoidRobot.speedL(ee_link="R_WRIST_R", + velocity=0.0, + cartesian=RobotCartesian.X, + direction=RobotJointIndexDirection.FORWARD, + acc=0.1) + print(" 停止speedL运动") + else: + print(f"❌ speedL 命令发送失败，错误码: {result}") + if error_msg: + print(f" 错误信息: {error_msg}") + + # 获取最终状态 + print("\n7. 获取最终关节状态...") + result, status = humanoidRobot.get_joint_state() + if result == CMVRErrorCode.CMVR_SUCCESS: + print("\n=== 最终关节状态信息 ===") + print(f"时间戳: {status.timestamp:.3f}s") + print(f"关节数量: {len(status.name)}") + + if len(status.name) > 0: + print("\n关节详情:") + print(f"{'关节名称':<15} {'位置(rad)':<12} {'速度(rad/s)':<12} {'力矩':<10}") + print("-" * 50) + + for i, name in enumerate(status.name): + pos = status.position[i] if i < len(status.position) else 0.0 + vel = status.velocity[i] if i < len(status.velocity) else 0.0 + eff = status.effort[i] if i < len(status.effort) else 0.0 + + print(f"{name:<15} {pos:<12.4f} {vel:<12.4f} {eff:<10.4f}") + else: + print("没有关节数据") + else: + print(f"获取最终状态失败，错误码: {result}") + + except Exception as e: + print(f"\n❌ 测试过程中发生异常: {e}") + import traceback + traceback.print_exc() + + finally: + # 去使能 + print("\n8. 关闭机器人使能...") + result = humanoidRobot.torqueOff() + if result == CMVRErrorCode.CMVR_SUCCESS: + print("✅ 使能关闭成功") + else: + print(f"❌ 使能关闭失败，错误码: {result}") + + print("\n=== 机器人功能测试完成 ===") + +if __name__ == "__main__": + # test_biohead() + # test_camera() + # test_rgb_image_stream() + # test_dexhand() + # test_sensor_data_stream() + # test_speaker() + # test_microphone() + test_humanoid_robot() diff --git a/examples/example_usage.py b/examples/example_usage.py index 1fd4846..4b359dc 100644 --- a/examples/example_usage.py +++ b/examples/example_usage.py @@ -14,7 +14,7 @@ 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 +from cmvr import CMVRGrpcClient, CMVRErrorCode, FacialExpressionState,JointCmd, Pose3D,RobotCartesian, RobotJointIndexDirection def test_biohead(): """测试仿生头功能""" @@ -658,11 +658,183 @@ def test_microphone(): else: print(f"开始录音失败，错误码: {result}") +def test_humanoid_robot(): + """测试机器人功能""" + print("\n=== 测试机器人功能 ===") + # 初始化GRPC客户端 + 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 + + # 获取初始状态 + print("\n1. 获取初始关节状态...") + result, status = humanoidRobot.get_joint_state() + if result == CMVRErrorCode.CMVR_SUCCESS: + # 打印关节状态详细信息 + print("\n=== 初始关节状态信息 ===") + print(f"时间戳: {status.timestamp:.3f}s") + print(f"关节数量: {len(status.name)}") + + if len(status.name) > 0: + print("\n关节详情:") + print(f"{'关节名称':<15} {'位置(rad)':<12} {'速度(rad/s)':<12} {'力矩':<10}") + print("-" * 50) + + for i, name in enumerate(status.name): + pos = status.position[i] if i < len(status.position) else 0.0 + vel = status.velocity[i] if i < len(status.velocity) else 0.0 + eff = status.effort[i] if i < len(status.effort) else 0.0 + + print(f"{name:<15} {pos:<12.4f} {vel:<12.4f} {eff:<10.4f}") + else: + print("没有关节数据") + else: + print(f"获取关节状态失败，错误码: {result}") + + # 使能 + print("\n2. 开启机器人使能...") + result = humanoidRobot.torqueOn() + if result == CMVRErrorCode.CMVR_SUCCESS: + print("✅ 使能开启成功") + else: + print(f"❌ 使能开启失败，错误码: {result}") + return + + # 等待使能生效 + import time + time.sleep(1) + + try: + # 测试moveJ - 关节空间运动 + print("\n3. 测试关节空间运动 (moveJ)...") + # 创建关节命令列表 + joint_commands = [ + JointCmd(joint_name="right_shoulder_pitch", rad=0.5, vel=0.3), + JointCmd(joint_name="right_shoulder_roll", rad=0.3, vel=0.3), + JointCmd(joint_name="right_elbow_pitch", rad=0.2, vel=0.3), + ] + + result, _ = humanoidRobot.moveJ(joint_commands, overall_vel=0.2, overall_acc=0.4) + if result == CMVRErrorCode.CMVR_SUCCESS: + print("✅ moveJ 命令发送成功") + time.sleep(3) # 等待运动完成 + else: + print(f"❌ moveJ 命令发送失败，错误码: {result}") + + # 获取运动后的状态 + print("\n 获取moveJ后的关节状态...") + result, status = humanoidRobot.get_joint_state() + if result == CMVRErrorCode.CMVR_SUCCESS: + print(f" 运动后时间戳: {status.timestamp:.3f}s") + print(f" 关节数量: {len(status.name)}") + + # 测试moveL - 直线空间运动 + print("\n4. 测试直线空间运动 (moveL)...") + # 创建目标位姿 (相对于当前位姿的小幅移动) + target_pose = Pose3D(x=0.1, y=0.0, z=0.0, rx=0.0, ry=0.0, rz=0.0) + + result, _ = humanoidRobot.moveL(ee_link="right_hand", target_pose=target_pose, + vel=0.05, acc=0.2) + if result == CMVRErrorCode.CMVR_SUCCESS: + print("✅ moveL 命令发送成功") + time.sleep(3) # 等待运动完成 + else: + print(f"❌ moveL 命令发送失败，错误码: {result}") + + # 测试speedJ - 关节速度控制 + print("\n5. 测试关节速度控制 (speedJ)...") + result, error_msg = humanoidRobot.speedJ(joint_name="right_shoulder_pitch", + velocity=0.1, + direction=RobotJointIndexDirection.FORWARD, + acc=0.3) + if result == CMVRErrorCode.CMVR_SUCCESS: + print("✅ speedJ 命令发送成功") + time.sleep(2) # 运行2秒 + # 停止运动 + stop_result, _ = humanoidRobot.speedJ(joint_name="right_shoulder_pitch", + velocity=0.0, + direction=RobotJointIndexDirection.FORWARD, + acc=0.3) + print(" 停止speedJ运动") + else: + print(f"❌ speedJ 命令发送失败，错误码: {result}") + if error_msg: + print(f" 错误信息: {error_msg}") + + # 测试speedL - 笛卡尔速度控制 + print("\n6. 测试笛卡尔速度控制 (speedL)...") + result, error_msg = humanoidRobot.speedL(ee_link="right_hand", + velocity=0.02, + cartesian=RobotCartesian.X, + direction=RobotJointIndexDirection.FORWARD, + acc=0.1) + if result == CMVRErrorCode.CMVR_SUCCESS: + print("✅ speedL 命令发送成功") + time.sleep(2) # 运行2秒 + # 停止运动 + stop_result, _ = humanoidRobot.speedL(ee_link="right_hand", + velocity=0.0, + cartesian=RobotCartesian.X, + direction=RobotJointIndexDirection.FORWARD, + acc=0.1) + print(" 停止speedL运动") + else: + print(f"❌ speedL 命令发送失败，错误码: {result}") + if error_msg: + print(f" 错误信息: {error_msg}") + + # 获取最终状态 + print("\n7. 获取最终关节状态...") + result, status = humanoidRobot.get_joint_state() + if result == CMVRErrorCode.CMVR_SUCCESS: + print("\n=== 最终关节状态信息 ===") + print(f"时间戳: {status.timestamp:.3f}s") + print(f"关节数量: {len(status.name)}") + + if len(status.name) > 0: + print("\n关节详情:") + print(f"{'关节名称':<15} {'位置(rad)':<12} {'速度(rad/s)':<12} {'力矩':<10}") + print("-" * 50) + + for i, name in enumerate(status.name): + pos = status.position[i] if i < len(status.position) else 0.0 + vel = status.velocity[i] if i < len(status.velocity) else 0.0 + eff = status.effort[i] if i < len(status.effort) else 0.0 + + print(f"{name:<15} {pos:<12.4f} {vel:<12.4f} {eff:<10.4f}") + else: + print("没有关节数据") + else: + print(f"获取最终状态失败，错误码: {result}") + + except Exception as e: + print(f"\n❌ 测试过程中发生异常: {e}") + import traceback + traceback.print_exc() + + finally: + # 去使能 + print("\n8. 关闭机器人使能...") + result = humanoidRobot.torqueOff() + if result == CMVRErrorCode.CMVR_SUCCESS: + print("✅ 使能关闭成功") + else: + print(f"❌ 使能关闭失败，错误码: {result}") + + print("\n=== 机器人功能测试完成 ===") + if __name__ == "__main__": # test_biohead() - test_camera() + # test_camera() # test_rgb_image_stream() # test_dexhand() # test_sensor_data_stream() # test_speaker() # test_microphone() + test_humanoid_robot() diff --git a/generate_proto.py b/generate_proto.py new file mode 100644 index 0000000..8a81a76 --- /dev/null +++ b/generate_proto.py @@ -0,0 +1,74 @@ +import os +import subprocess +from pathlib import Path + +def generate_proto(): + # 获取当前目录 + current_dir = Path(__file__).parent + proto_dir = current_dir / "protos" + generated_dir = current_dir / "generated" + + # 创建生成的目录 + generated_dir.mkdir(parents=True, exist_ok=True) + + # 编译所有 .proto 文件 + proto_files = [] + for root, _, files in os.walk(proto_dir): + for file in files: + if file.endswith(".proto"): + proto_files.append(os.path.join(root, file)) + + for proto_path in proto_files: + # 计算相对路径 + rel_path = os.path.relpath(proto_path, proto_dir) + output_dir = generated_dir + + cmd = [ + "python", "-m", "grpc_tools.protoc", + f"-I{proto_dir}", + f"--python_out={output_dir}", + f"--grpc_python_out={output_dir}", + proto_path + ] + + print(f"Generating code for {rel_path}...") + result = subprocess.run(cmd, capture_output=True, text=True) + + if result.returncode != 0: + print(f"Error generating {rel_path}:") + print(result.stderr) + return False + + print("Proto files generated successfully!") + + # 修复生成的代码中的导入路径 + fix_imports(generated_dir) + + return True + +def fix_imports(generated_dir): + """修复生成的代码中的导入路径""" + for root, _, files in os.walk(generated_dir): + for file in files: + if file.endswith(".py"): + file_path = os.path.join(root, file) + with open(file_path, 'r', encoding='utf-8') as f: + content = f.read() + + # 替换导入路径 + content = content.replace( + "from cmvr.api import", + "from generated.cmvr.api import" + ) + content = content.replace( + "import cmvr.api.", + "import generated.cmvr.api." + ) + + with open(file_path, 'w', encoding='utf-8') as f: + f.write(content) + + print(f"Fixed imports in {file_path}") + +if __name__ == "__main__": + generate_proto() \ No newline at end of file diff --git a/get_joint_state.py b/get_joint_state.py new file mode 100644 index 0000000..4f2d031 --- /dev/null +++ b/get_joint_state.py @@ -0,0 +1,59 @@ +#!/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() diff --git a/protos/cmvr/api/common.proto b/protos/cmvr/api/common.proto index 0bea15c..accb936 100644 --- a/protos/cmvr/api/common.proto +++ b/protos/cmvr/api/common.proto @@ -40,4 +40,4 @@ message ConfigParam { bool bool_value = 5; // 布尔类型 bytes bytes_value = 6; // 二进制数据类型 } -} \ No newline at end of file +} diff --git a/protos/cmvr/api/hlc_command.proto b/protos/cmvr/api/hlc_command.proto new file mode 100644 index 0000000..7c39ced --- /dev/null +++ b/protos/cmvr/api/hlc_command.proto @@ -0,0 +1,24 @@ +syntax = "proto3"; + +import "cmvr/api/common.proto"; + +package cmvr.api; + + +message Touch{ + message Request{ + CommandHeader.Request header = 1; + // 触点坐标，unit: ms + int32 u = 2; // 水平坐标 + int32 v = 3; // 垂直坐标 + + // 触控时允许的最大作用力，unit: N + double max_force = 4; + } + + message Response{ + CommandHeader.Feedback header= 1; + } + +} + diff --git a/protos/cmvr/api/hlc_service.proto b/protos/cmvr/api/hlc_service.proto new file mode 100644 index 0000000..7e14032 --- /dev/null +++ b/protos/cmvr/api/hlc_service.proto @@ -0,0 +1,9 @@ +syntax = "proto3"; + +import "cmvr/api/hlc_command.proto"; + +package cmvr.api; + +service HlcService{ + rpc touch(Touch.Request) returns (Touch.Response); +} \ No newline at end of file diff --git a/protos/cmvr/api/humanoid_robot_command.proto b/protos/cmvr/api/humanoid_robot_command.proto new file mode 100644 index 0000000..a9c909a --- /dev/null +++ b/protos/cmvr/api/humanoid_robot_command.proto @@ -0,0 +1,122 @@ + + +syntax = "proto3"; +package cmvr.api; +import "cmvr/api/common.proto"; + +message JointCmd { + string joint_name = 1; // 关节名称 + double rad = 2; // 弧度 + double vel = 3; // 角速度，rad/s +} + +message Pose3D{ + double x = 1; + double y = 2; + double z = 3; + double rx = 4; + double ry = 5; + double rz = 6; +} + +enum RobotCartesian{ + X = 0; + Y = 1; + Z = 2; + RX = 3; + RY = 4; + RZ = 5; +} ; +enum RobotJointIndexDirection{ + FORWARD = 0; + BACKWARD = 1; + X_POSITIVE = 2; // X轴正向 + X_NEGATIVE = 3; // X轴负向 + Y_POSITIVE = 4; // Y轴正向 + Y_NEGATIVE = 5; // Y轴负向 + Z_POSITIVE = 6; // Z轴正向 + Z_NEGATIVE = 7; // Z轴负向 + ROTATE_X = 8; // 绕X轴旋转 + ROTATE_Y = 9; // 绕Y轴旋转 + ROTATE_Z = 10; // 绕Z轴旋转 +} +message MoveJ{ + message Request{ + CommandHeader.Request header = 1; + repeated JointCmd cmds = 2; + double vel = 3; + double acc = 4; + } + + message Response{ + CommandHeader.Feedback header= 1; + } + +} + +message MoveL{ + + message Request{ + CommandHeader.Request header = 1; + string ee_link = 2; //连杆名称 + Pose3D targetPose = 3; + double vel = 4; + double acc = 5; + } + + message Response{ + CommandHeader.Feedback header= 1; + } + +} + +message SpeedJ{ + + message Request{ + CommandHeader.Request header = 1; + string joint_name = 2; + double vel = 3; + double acc = 4; + RobotJointIndexDirection dir = 5; + } + + message Response{ + CommandHeader.Feedback header= 1; + } +} + +message SpeedL{ + message Request{ + CommandHeader.Request header = 1; + string ee_link = 2; + double vel = 3; + double acc = 4; + RobotJointIndexDirection dir = 5; + RobotCartesian cart = 6; + } + + message Response{ + CommandHeader.Feedback header= 1; + } +} + +// 关节状态消息 +message JointState { + repeated string name = 1; // 关节名称 + repeated double position = 2; // 关节位置 + repeated double velocity = 3; // 可选：关节速度 + repeated double effort = 4; // 可选：关节力矩 + double timestamp = 5; // 可选：时间戳 +} + +// 返回消息 +message JointResponse { + CommandHeader.Feedback header= 1; + repeated JointState state = 2; +} + +// 请求消息（可选） +message JointRequest { + CommandHeader.Request header = 1; +} + diff --git a/protos/cmvr/api/humanoid_robot_service.proto b/protos/cmvr/api/humanoid_robot_service.proto new file mode 100644 index 0000000..fafb672 --- /dev/null +++ b/protos/cmvr/api/humanoid_robot_service.proto @@ -0,0 +1,16 @@ + + +syntax = "proto3"; +package cmvr.api; +import "cmvr/api/common.proto"; +import "cmvr/api/humanoid_robot_command.proto"; + +service HumanoidRobotService{ + rpc torqueOff(CommandHeader.Request) returns (CommandHeader.Feedback); + rpc torqueOn(CommandHeader.Request) returns (CommandHeader.Feedback); + rpc moveJ(MoveJ.Request) returns (MoveJ.Response); + rpc moveL(MoveL.Request) returns (MoveL.Response); + rpc speedJ(SpeedJ.Request) returns (SpeedJ.Response); + rpc speedL(SpeedL.Request) returns (SpeedL.Response); + rpc getJointState(JointRequest) returns (JointResponse); +} diff --git a/protos/cmvr/api/system_command.proto b/protos/cmvr/api/system_command.proto index 668bc9d..ea6f21d 100644 --- a/protos/cmvr/api/system_command.proto +++ b/protos/cmvr/api/system_command.proto @@ -50,10 +50,12 @@ message GetSystemStatusCommand { } message UpdateParamsCommand { - message Request {} + message Request { + CommandHeader.Request header = 1; + repeated ConfigParam params = 2; + } message Feedback { CommandHeader.Feedback header = 1; - repeated ConfigParam params = 2; } } diff --git a/protos/cmvr/api/system_service.proto b/protos/cmvr/api/system_service.proto index e46a728..f96ea81 100644 --- a/protos/cmvr/api/system_service.proto +++ b/protos/cmvr/api/system_service.proto @@ -10,5 +10,4 @@ service SystemService { rpc GetSystemStatus(GetSystemStatusCommand.Request) returns (GetSystemStatusCommand.Feedback) {} rpc UpdateParams(UpdateParamsCommand.Request) returns (UpdateParamsCommand.Feedback) {} - } \ No newline at end of file