import grpc import numpy as np from typing import Tuple from typing import Dict from .enums import CMVRErrorCode, FingerType from .models import DexHandState, FreedomState, HandTactileSensors class DexHandClient: """灵巧手客户端""" def __init__(self, device_id: str, 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, dexhand_command_pb2 self.generated = type('GeneratedModules', (), { 'common_pb2': common_pb2, 'dexhand_command_pb2': dexhand_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 get_status(self) -> Tuple[CMVRErrorCode, DexHandState]: """获取灵巧手状态""" try: generated = self._import_generated() request = generated.dexhand_command_pb2.GetDexHandStateCommand.Request() request.header.CopyFrom(self._create_command_header()) response = self.stub.GetStatus(request) if not response.header.success: return CMVRErrorCode.CMVR_RPC_FAILED, DexHandState() state = DexHandState( is_initialized=response.state.is_initialized ) # 填充手指状态 for i, hand_state in enumerate(response.state.hands): if i < len(state.hands): state.hands[i] = FreedomState( angle=hand_state.angle, speed=hand_state.speed, force=hand_state.force, position=hand_state.position, current=hand_state.current, temperature=hand_state.temperature, error=hand_state.error, error_message=list(hand_state.error_message) ) return CMVRErrorCode.CMVR_SUCCESS, state except grpc.RpcError as e: print(f"获取灵巧手状态失败: {e}") return CMVRErrorCode.CMVR_RPC_FAILED, DexHandState() def set_angle(self, angle_map: Dict[str, float]) -> CMVRErrorCode: """ 设置DexHand的角度 参数: angle_map: 字典，键为自由度ID（字符串），值为角度百分比 (0-1) 有效字符串ID："little_finger"（小拇指）、"ring_finger"（无名指）、 "middle_finger"（中指）、"index_finger"（食指）、 "thumb_bend"（大拇指弯曲）、"thumb_rotate"（大拇指旋转） 对应proto中的FreedomValue (id和value) 返回: CMVRErrorCode: 操作结果状态码 """ try: generated = self._import_generated() # 有效自由度字符串ID列表（与proto定义一致） valid_dof_ids = { "little_finger", "ring_finger", "middle_finger", "index_finger", "thumb_bend", "thumb_rotate" } # 创建角度设置请求对象 request = generated.dexhand_command_pb2.SetDexHandAnglesCommand.Request() request.header.CopyFrom(self._create_command_header()) # 将字典转换为protobuf的FreedomValue列表 for dof_id, angle_value in angle_map.items(): # 验证自由度ID有效性（字符串） if dof_id not in valid_dof_ids: print(f"警告: 无效的自由度ID '{dof_id}'，有效ID为：{valid_dof_ids}") continue # 验证角度值范围（0-1） if not (0 <= angle_value <= 1): print(f"警告: 角度值 {angle_value} 超出范围，必须在0-1之间") angle_value = max(0, min(1, angle_value)) # 限制在有效范围 # 添加自由度角度设置（直接使用字符串ID） freedom_value = request.values.add() freedom_value.id = dof_id freedom_value.value = angle_value # 调用RPC接口 response = self.stub.SetDexHandAngle(request) # 返回操作结果 return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED except grpc.RpcError as e: print(f"设置DexHand角度失败: {e}") return CMVRErrorCode.CMVR_RPC_FAILED def get_sensor_data(self) -> Tuple[CMVRErrorCode, HandTactileSensors]: """ 获取灵巧手所有触觉传感器数据 将Protobuf的SensorData转换为HandTactileSensors对象 返回: Tuple[CMVRErrorCode, HandTactileSensors]: 错误码和传感器数据对象 """ try: generated = self._import_generated() # 创建传感器数据请求 request = generated.dexhand_command_pb2.GetSensorDataCommand.Request() request.header.CopyFrom(self._create_command_header()) # 调用RPC接口获取数据 response = self.stub.GetSensorData(request) if not response.header.success: return CMVRErrorCode.CMVR_RPC_FAILED, HandTactileSensors() # 初始化传感器数据容器 tactile_sensors = HandTactileSensors() # 映射表：FingerType + PartType -> HandTactileSensors属性 sensor_mapping = { # 小拇指 (FingerType.PINKY, generated.dexhand_command_pb2.SensorData.PartType.TIP): 'pinky_tip', (FingerType.PINKY, generated.dexhand_command_pb2.SensorData.PartType.FINGER): 'pinky_finger', (FingerType.PINKY, generated.dexhand_command_pb2.SensorData.PartType.PAD): 'pinky_pad', # 无名指 (FingerType.RING, generated.dexhand_command_pb2.SensorData.PartType.TIP): 'ring_tip', (FingerType.RING, generated.dexhand_command_pb2.SensorData.PartType.FINGER): 'ring_finger', (FingerType.RING, generated.dexhand_command_pb2.SensorData.PartType.PAD): 'ring_pad', # 中指 (FingerType.MIDDLE, generated.dexhand_command_pb2.SensorData.PartType.TIP): 'middle_tip', (FingerType.MIDDLE, generated.dexhand_command_pb2.SensorData.PartType.FINGER): 'middle_finger', (FingerType.MIDDLE, generated.dexhand_command_pb2.SensorData.PartType.PAD): 'middle_pad', # 食指 (FingerType.INDEX, generated.dexhand_command_pb2.SensorData.PartType.TIP): 'index_tip', (FingerType.INDEX, generated.dexhand_command_pb2.SensorData.PartType.FINGER): 'index_finger', (FingerType.INDEX, generated.dexhand_command_pb2.SensorData.PartType.PAD): 'index_pad', # 大拇指 (FingerType.THUMB, generated.dexhand_command_pb2.SensorData.PartType.TIP): 'thumb_tip', (FingerType.THUMB, generated.dexhand_command_pb2.SensorData.PartType.FINGER): 'thumb_finger', (FingerType.THUMB, generated.dexhand_command_pb2.SensorData.PartType.THUMB_MIDDLE): 'thumb_middle', (FingerType.THUMB, generated.dexhand_command_pb2.SensorData.PartType.PAD): 'thumb_pad', # 掌心 (FingerType.PALM, generated.dexhand_command_pb2.SensorData.PartType.PALM_PAD): 'palm' } # 遍历所有传感器数据并填充到对应的结构中 for sensor in response.sensor: # 获取映射的属性名 key = (FingerType(sensor.finger_type), sensor.part_type) attr_name = sensor_mapping.get(key) if not attr_name: # 跳过未定义的传感器类型 print(f"警告: 未定义的传感器类型 - 手指: {sensor.finger_type}, 部位: {sensor.part_type}") continue # 获取对应的传感器对象 sensor_obj = getattr(tactile_sensors, attr_name) # 更新传感器基本信息 sensor_obj.name = sensor.sensor_name sensor_obj.rows = sensor.rows sensor_obj.cols = sensor.cols # 计算字节大小 (每个int32占2字节) sensor_obj.byteSize = sensor.rows * sensor.cols * 2 # 转换数据为numpy数组 if sensor.rows > 0 and sensor.cols > 0 and len(sensor.data) > 0: # 初始化数据数组 data_array = np.zeros((sensor.rows, sensor.cols), dtype=np.uint16) # 填充数据 for row_idx, row_data in enumerate(sensor.data): if row_idx < sensor.rows: # 防止数组越界 # 截取有效长度并转换 values = row_data.values[:sensor.cols] data_array[row_idx, :len(values)] = values sensor_obj.data = data_array return CMVRErrorCode.CMVR_SUCCESS, tactile_sensors except grpc.RpcError as e: print(f"获取灵巧手传感器数据失败: {e}") return CMVRErrorCode.CMVR_RPC_FAILED, HandTactileSensors() def create_stream_request(self): """创建流请求（无参数）""" generated = self._import_generated() request = generated.dexhand_command_pb2.GetSensorDataStreamCommand.Request() request.header.CopyFrom(self._create_command_header()) # 不需要设置其他参数 return request def get_sensor_data_stream(self, request_generator): """ 双向流获取传感器数据 参数: request_generator: 请求生成器 返回: 传感器数据流迭代器 """ generated = self._import_generated() return self.stub.GetSensorDataStream(request_generator) def parse_sensor_stream_data(self, feedback) -> Tuple[CMVRErrorCode, HandTactileSensors]: """ 解析流反馈中的传感器数据 复用之前的传感器数据转换逻辑 """ try: generated = self._import_generated() SensorData = generated.dexhand_command_pb2.SensorData pb_finger_type = SensorData.FingerType pb_part_type = SensorData.PartType # 初始化传感器容器 tactile_sensors = HandTactileSensors() # 传感器映射表（与get_sensor_data保持一致） sensor_mapping = { # 小拇指 (pb_finger_type.PINKY, pb_part_type.TIP): 'pinky_tip', (pb_finger_type.PINKY, pb_part_type.FINGER): 'pinky_finger', (pb_finger_type.PINKY, pb_part_type.PAD): 'pinky_pad', # 无名指 (pb_finger_type.RING, pb_part_type.TIP): 'ring_tip', (pb_finger_type.RING, pb_part_type.FINGER): 'ring_finger', (pb_finger_type.RING, pb_part_type.PAD): 'ring_pad', # 中指 (pb_finger_type.MIDDLE_FINGER, pb_part_type.TIP): 'middle_tip', (pb_finger_type.MIDDLE_FINGER, pb_part_type.FINGER): 'middle_finger', (pb_finger_type.MIDDLE_FINGER, pb_part_type.PAD): 'middle_pad', # 食指 (pb_finger_type.INDEX, pb_part_type.TIP): 'index_tip', (pb_finger_type.INDEX, pb_part_type.FINGER): 'index_finger', (pb_finger_type.INDEX, pb_part_type.PAD): 'index_pad', # 大拇指 (pb_finger_type.THUMB, pb_part_type.TIP): 'thumb_tip', (pb_finger_type.THUMB, pb_part_type.FINGER): 'thumb_finger', (pb_finger_type.THUMB, pb_part_type.THUMB_MIDDLE): 'thumb_middle', (pb_finger_type.THUMB, pb_part_type.PAD): 'thumb_pad', # 掌心 (pb_finger_type.PALM, pb_part_type.PALM_PAD): 'palm' } # 解析反馈中的传感器数据 for sensor in feedback.sensor: # 假设反馈中的传感器数据字段是sensor_data key = (sensor.finger_type, sensor.part_type) attr_name = sensor_mapping.get(key) if not attr_name: finger_name = pb_finger_type.Name(sensor.finger_type) part_name = pb_part_type.Name(sensor.part_type) print(f"警告: 未定义的传感器类型 - 手指: {finger_name}, 部位: {part_name}") continue # 填充传感器数据 sensor_obj = getattr(tactile_sensors, attr_name) sensor_obj.name = sensor.sensor_name sensor_obj.rows = sensor.rows sensor_obj.cols = sensor.cols sensor_obj.byteSize = sensor.rows * sensor.cols * 2 # 转换数据为numpy数组 if sensor.rows > 0 and sensor.cols > 0 and len(sensor.data) > 0: data_array = np.zeros((sensor.rows, sensor.cols), dtype=np.uint16) for row_idx, row_data in enumerate(sensor.data): if row_idx < sensor.rows: values = row_data.values[:sensor.cols] data_array[row_idx, :len(values)] = values sensor_obj.data = data_array return CMVRErrorCode.CMVR_SUCCESS, tactile_sensors except Exception as e: print(f"解析流数据失败: {e}") return CMVRErrorCode.CMVR_INTERNAL_ERROR, HandTactileSensors()