331 lines
14 KiB
Python
331 lines
14 KiB
Python
import grpc
|
||
import numpy as np
|
||
from typing import Tuple
|
||
from typing import Dict
|
||
|
||
from .enums import CMVRErrorCode, FingerType
|
||
from .models import DexHandState, RH56DFTPDexHand, 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] = RH56DFTPDexHand(
|
||
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[int, float]) -> CMVRErrorCode:
|
||
"""
|
||
设置DexHand的角度
|
||
|
||
参数:
|
||
angle_map: 字典，键为自由度ID (0-6)，值为角度百分比 (0-1)
|
||
对应proto中的FreedomValue (id和value)
|
||
|
||
返回:
|
||
CMVRErrorCode: 操作结果状态码
|
||
"""
|
||
try:
|
||
generated = self._import_generated()
|
||
# 创建角度设置请求对象
|
||
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():
|
||
# 验证输入有效性
|
||
if not (0 <= dof_id <= 6):
|
||
print(f"警告: 无效的自由度ID {dof_id}，必须在0-6范围内")
|
||
continue
|
||
if not (0 <= angle_value <= 1):
|
||
print(f"警告: 角度值 {angle_value} 超出范围，必须在0-1之间")
|
||
# 可选：将值限制在有效范围内
|
||
angle_value = max(0, min(1, angle_value))
|
||
|
||
# 添加自由度角度设置
|
||
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()
|