From b428db137275b9dc2d0681394514c05d0c9b3665 Mon Sep 17 00:00:00 2001 From: linbo <1034003879@qq.com> Date: Tue, 21 Oct 2025 10:38:31 +0800 Subject: [PATCH] update --- cmvr/dexhand_client.py | 27 +++++++---- cmvr/models.py | 2 +- example_usage.py | 105 ++++++++++++++++++++++------------------- 3 files changed, 76 insertions(+), 58 deletions(-) diff --git a/cmvr/dexhand_client.py b/cmvr/dexhand_client.py index d15fb34..c21659c 100644 --- a/cmvr/dexhand_client.py +++ b/cmvr/dexhand_client.py @@ -74,35 +74,44 @@ class DexHandClient: print(f"获取灵巧手状态失败: {e}") return CMVRErrorCode.CMVR_RPC_FAILED, DexHandState() - def set_angle(self, angle_map: Dict[int, float]) -> CMVRErrorCode: + def set_angle(self, angle_map: Dict[str, float]) -> CMVRErrorCode: """ 设置DexHand的角度 参数: - angle_map: 字典，键为自由度ID (0-6)，值为角度百分比 (0-1) - 对应proto中的FreedomValue (id和value) + 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(): - # 验证输入有效性 - if not (0 <= dof_id <= 6): - print(f"警告: 无效的自由度ID {dof_id}，必须在0-6范围内") + # 验证自由度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)) + angle_value = max(0, min(1, angle_value)) # 限制在有效范围 - # 添加自由度角度设置 + # 添加自由度角度设置（直接使用字符串ID） freedom_value = request.values.add() freedom_value.id = dof_id freedom_value.value = angle_value diff --git a/cmvr/models.py b/cmvr/models.py index 8238886..5b6bb28 100644 --- a/cmvr/models.py +++ b/cmvr/models.py @@ -43,7 +43,7 @@ class CameraIntrinsics: @dataclass class FreedomState: """灵巧手自由度状态""" - dof_id: int = 0 + dof_id: str = "" angle: int = 0 speed: int = 0 force: int = 0 diff --git a/example_usage.py b/example_usage.py index c9c7d08..41f57fa 100644 --- a/example_usage.py +++ b/example_usage.py @@ -313,7 +313,7 @@ def test_dexhand(): print("=== 测试DexHand功能 ===") # 初始化GRPC客户端（使用实际服务器地址） - client = CMVRGrpcClient("192.168.1.222:50060") + client = CMVRGrpcClient("192.168.0.222:50052") if not client.is_connected(): print("连接服务器失败") return @@ -325,21 +325,35 @@ def test_dexhand(): client.close() return + # 定义：字符串ID与手指名称的映射（与proto/服务端一致） + dof_id_to_name = { + "little_finger": "小拇指", + "ring_finger": "无名指", + "middle_finger": "中指", + "index_finger": "食指", + "thumb_bend": "大拇指弯曲", + "thumb_rotate": "大拇指旋转" + } + # 有效字符串ID列表（用于后续判断） + valid_dof_ids = set(dof_id_to_name.keys()) + # 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 + # 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: + # # 匹配字符串ID对应的手指名称，未知ID显示原始值 + # finger_name = dof_id_to_name.get(hand.dof_id, f"未知自由度({hand.dof_id})") + # print(f" {finger_name} (ID:{hand.dof_id}): 角度={hand.angle}, 速度={hand.speed}, " + # f"受力={hand.force}, 位置={hand.position}, 温度={hand.temperature}°C") + # if hand.error != 0: + # print(f" 警告: {finger_name} 存在故障: {hand.error_message}") + # else: + # print(f"获取初始状态失败，错误码: {error_code}") + # client.close() + # return # 1.1 获取初始传感器数据 print("\n--- 获取初始传感器数据 ---") @@ -353,34 +367,25 @@ def test_dexhand(): else: print(f"获取初始传感器数据失败，错误码: {error_code}") - # 2. 设置测试角度（小拇指、食指、大拇指弯曲） + # 2. 设置测试角度（小拇指、食指、大拇指弯曲等） print("\n--- 设置测试角度 ---") - # 角度字典: 键为自由度ID (0-6)，值为0-1百分比 + # 角度字典: 键为字符串类型自由度ID，值为0-1百分比（与set_angle函数参数要求一致） test_angles = { - 0: 0.9, # 小拇指 - 1: 0.9, # 无名指 - 2: 0.9, # 中指 - 3: 0.9, # 食指 - 4: 0.9, # 大拇指弯曲 - 5: 0.9 # 大拇指旋转 + "little_finger": 0.9, # 小拇指 + "ring_finger": 0.9, # 无名指 + "middle_finger": 0.9, # 中指 + "index_finger": 0.9, # 食指 + "thumb_bend": 0.9, # 大拇指弯曲 + "thumb_rotate": 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}") + finger_name = dof_id_to_name[dof_id] # 已确保ID有效，直接获取名称 print(f" {finger_name} (ID:{dof_id}): {value*100}%") - # 执行角度设置 + # 执行角度设置（直接传入字符串ID的字典） set_result = dexhand.set_angle(test_angles) if set_result == CMVRErrorCode.CMVR_SUCCESS: print("角度设置命令发送成功，等待执行器动作...") @@ -402,7 +407,7 @@ def test_dexhand(): print(f" 食指指腹平均压力变化: {avg_before:.2f} → {avg_after:.2f}") # 打印大拇指指端数据示例（前3个值） - if sensors_after.thumb_tip.data.size > 0: + if hasattr(sensors_after, 'thumb_tip') and sensors_after.thumb_tip.data.size > 0: print(f" 大拇指指端部分数据: {sensors_after.thumb_tip.data.flatten()[:3]}...") else: print(f"获取角度变化后传感器数据失败，错误码: {error_code}") @@ -411,23 +416,25 @@ def test_dexhand(): print("\n--- 获取设置后状态 ---") error_code, state = dexhand.get_status() if error_code == CMVRErrorCode.CMVR_SUCCESS: - print("设置后各自由度状态:") + print("设置后各自由度状态（仅显示已设置的自由度）:") for hand in state.hands: - # 只打印我们设置过的自由度 + # 只打印我们设置过的自由度（字符串ID对比） if hand.dof_id in test_angles: - print(f" 自由度ID: {hand.dof_id}, 新角度: {hand.angle}, 当前位置: {hand.position}") + finger_name = dof_id_to_name[hand.dof_id] + print(f" {finger_name} (ID:{hand.dof_id}): 新角度={hand.angle}, 当前位置={hand.position}") else: print(f"获取设置后状态失败，错误码: {error_code}") # 4. 恢复默认角度（复位操作） print("\n--- 恢复默认角度 ---") + # 复位字典：键为字符串ID，值为默认百分比 default_angles = { - 0: 0.1, - 1: 0.1, - 2: 0.1, - 3: 0.1, - 4: 0.1, - 5: 0.8 + "little_finger": 0.1, + "ring_finger": 0.1, + "middle_finger": 0.1, + "index_finger": 0.1, + "thumb_bend": 0.1, + "thumb_rotate": 0.8 } set_result = dexhand.set_angle(default_angles) @@ -456,10 +463,12 @@ def test_dexhand(): if error_code == CMVRErrorCode.CMVR_SUCCESS: print(f"最终初始化状态: {state.is_initialized}") print("关键自由度最终位置:") + # 遍历需要确认的自由度（字符串ID） 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}") + finger_name = dof_id_to_name[dof_id] + print(f" {finger_name} (ID:{dof_id}): 角度={hand.angle}, 位置={hand.position}") else: print(f"获取最终状态失败，错误码: {error_code}") @@ -863,8 +872,8 @@ if __name__ == "__main__": # test_biohead() # test_camera() # test_rgb_image_stream() - # test_dexhand() + test_dexhand() # test_sensor_data_stream() # test_speaker() # test_microphone() - test_humanoid_robot() + # test_humanoid_robot()