cmvr-es-cli/cmvr/dexhand_client.py

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()