SmartBench/cmvr-es-cli
6
0
Fork 0
Code Issues Pull Requests Actions Packages Projects Releases Wiki Activity

update

Browse Source
This commit is contained in:
linbo 2025-10-21 10:38:31 +08:00
parent 882a9d9133
commit b428db1372
3 changed files with 76 additions and 58 deletions
Show Stats Download Patch File Download Diff File Expand all files Collapse all files

27
cmvr/dexhand_client.py
View File

@ -74,35 +74,44 @@ class DexHandClient:
print(f"获取灵巧手状态失败: {e}") print(f"获取灵巧手状态失败: {e}")
return CMVRErrorCode.CMVR_RPC_FAILED, DexHandState() 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的角度 设置DexHand的角度
参数: 参数:
angle_map: 字典，键为自由度ID (0-6)，值为角度百分比 (0-1) angle_map: 字典，键为自由度ID（字符串），值为角度百分比 (0-1)
对应proto中的FreedomValue (id和value) 有效字符串ID："little_finger"（小拇指）、"ring_finger"（无名指）、
"middle_finger"（中指）、"index_finger"（食指）、
"thumb_bend"（大拇指弯曲）、"thumb_rotate"（大拇指旋转）
对应proto中的FreedomValue (id和value)
返回: 返回:
CMVRErrorCode: 操作结果状态码 CMVRErrorCode: 操作结果状态码
""" """
try: try:
generated = self._import_generated() 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 = generated.dexhand_command_pb2.SetDexHandAnglesCommand.Request()
request.header.CopyFrom(self._create_command_header()) request.header.CopyFrom(self._create_command_header())
# 将字典转换为protobuf的FreedomValue列表 # 将字典转换为protobuf的FreedomValue列表
for dof_id, angle_value in angle_map.items(): for dof_id, angle_value in angle_map.items():
# 验证输入有效性 # 验证自由度ID有效性（字符串）
if not (0 <= dof_id <= 6): if dof_id not in valid_dof_ids:
print(f"警告: 无效的自由度ID {dof_id}，必须在0-6范围内") print(f"警告: 无效的自由度ID '{dof_id}'，有效ID为：{valid_dof_ids}")
continue continue
# 验证角度值范围（0-1）
if not (0 <= angle_value <= 1): if not (0 <= angle_value <= 1):
print(f"警告: 角度值 {angle_value} 超出范围，必须在0-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 = request.values.add()
freedom_value.id = dof_id freedom_value.id = dof_id
freedom_value.value = angle_value freedom_value.value = angle_value

2
cmvr/models.py
View File

@ -43,7 +43,7 @@ class CameraIntrinsics:
@dataclass @dataclass
class FreedomState: class FreedomState:
"""灵巧手自由度状态""" """灵巧手自由度状态"""
dof_id: int = 0 dof_id: str = ""
angle: int = 0 angle: int = 0
speed: int = 0 speed: int = 0
force: int = 0 force: int = 0

105
example_usage.py
View File

@ -313,7 +313,7 @@ def test_dexhand():
print("=== 测试DexHand功能 ===") print("=== 测试DexHand功能 ===")
# 初始化GRPC客户端（使用实际服务器地址） # 初始化GRPC客户端（使用实际服务器地址）
client = CMVRGrpcClient("192.168.1.222:50060") client = CMVRGrpcClient("192.168.0.222:50052")
if not client.is_connected(): if not client.is_connected():
print("连接服务器失败") print("连接服务器失败")
return return
@ -325,21 +325,35 @@ def test_dexhand():
client.close() client.close()
return 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. 获取初始状态 # 1. 获取初始状态
print("\n--- 获取初始状态 ---") # print("\n--- 获取初始状态 ---")
error_code, state = dexhand.get_status() # error_code, state = dexhand.get_status()
if error_code == CMVRErrorCode.CMVR_SUCCESS: # if error_code == CMVRErrorCode.CMVR_SUCCESS:
print(f"初始化状态: 已初始化={state.is_initialized}") # print(f"初始化状态: 已初始化={state.is_initialized}")
print("各自由度状态:") # print("各自由度状态:")
for hand in state.hands: # for hand in state.hands:
print(f" 自由度ID: {hand.dof_id}, 角度: {hand.angle}, 速度: {hand.speed}, " # # 匹配字符串ID对应的手指名称，未知ID显示原始值
f"受力: {hand.force}, 位置: {hand.position}, 温度: {hand.temperature}°C") # finger_name = dof_id_to_name.get(hand.dof_id, f"未知自由度({hand.dof_id})")
if hand.error != 0: # print(f" {finger_name} (ID:{hand.dof_id}): 角度={hand.angle}, 速度={hand.speed}, "
print(f" 警告: 自由度ID {hand.dof_id} 存在故障: {hand.error_message}") # f"受力={hand.force}, 位置={hand.position}, 温度={hand.temperature}°C")
else: # if hand.error != 0:
print(f"获取初始状态失败，错误码: {error_code}") # print(f" 警告: {finger_name} 存在故障: {hand.error_message}")
client.close() # else:
return # print(f"获取初始状态失败，错误码: {error_code}")
# client.close()
# return
# 1.1 获取初始传感器数据 # 1.1 获取初始传感器数据
print("\n--- 获取初始传感器数据 ---") print("\n--- 获取初始传感器数据 ---")
@ -353,34 +367,25 @@ def test_dexhand():
else: else:
print(f"获取初始传感器数据失败，错误码: {error_code}") print(f"获取初始传感器数据失败，错误码: {error_code}")
# 2. 设置测试角度（小拇指、食指、大拇指弯曲） # 2. 设置测试角度（小拇指、食指、大拇指弯曲等）
print("\n--- 设置测试角度 ---") print("\n--- 设置测试角度 ---")
# 角度字典: 键为自由度ID (0-6)，值为0-1百分比 # 角度字典: 键为字符串类型自由度ID，值为0-1百分比（与set_angle函数参数要求一致）
test_angles = { test_angles = {
0: 0.9, # 小拇指 "little_finger": 0.9, # 小拇指
1: 0.9, # 无名指 "ring_finger": 0.9, # 无名指
2: 0.9, # 中指 "middle_finger": 0.9, # 中指
3: 0.9, # 食指 "index_finger": 0.9, # 食指
4: 0.9, # 大拇指弯曲 "thumb_bend": 0.9, # 大拇指弯曲
5: 0.9 # 大拇指旋转 "thumb_rotate": 0.9 # 大拇指旋转
} }
# 打印设置信息（映射自由度ID到手指名称） # 打印设置信息（通过映射表显示中文名称）
finger_map = {
0: "小拇指",
1: "无名指",
2: "中指",
3: "食指",
4: "大拇指弯曲",
5: "大拇指旋转",
6: "预留自由度"
}
print("准备设置角度:") print("准备设置角度:")
for dof_id, value in test_angles.items(): 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}%") print(f" {finger_name} (ID:{dof_id}): {value*100}%")
# 执行角度设置 # 执行角度设置（直接传入字符串ID的字典）
set_result = dexhand.set_angle(test_angles) set_result = dexhand.set_angle(test_angles)
if set_result == CMVRErrorCode.CMVR_SUCCESS: if set_result == CMVRErrorCode.CMVR_SUCCESS:
print("角度设置命令发送成功，等待执行器动作...") print("角度设置命令发送成功，等待执行器动作...")
@ -402,7 +407,7 @@ def test_dexhand():
print(f" 食指指腹平均压力变化: {avg_before:.2f} → {avg_after:.2f}") print(f" 食指指腹平均压力变化: {avg_before:.2f} → {avg_after:.2f}")
# 打印大拇指指端数据示例（前3个值） # 打印大拇指指端数据示例（前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]}...") print(f" 大拇指指端部分数据: {sensors_after.thumb_tip.data.flatten()[:3]}...")
else: else:
print(f"获取角度变化后传感器数据失败，错误码: {error_code}") print(f"获取角度变化后传感器数据失败，错误码: {error_code}")
@ -411,23 +416,25 @@ def test_dexhand():
print("\n--- 获取设置后状态 ---") print("\n--- 获取设置后状态 ---")
error_code, state = dexhand.get_status() error_code, state = dexhand.get_status()
if error_code == CMVRErrorCode.CMVR_SUCCESS: if error_code == CMVRErrorCode.CMVR_SUCCESS:
print("设置后各自由度状态:") print("设置后各自由度状态（仅显示已设置的自由度）:")
for hand in state.hands: for hand in state.hands:
# 只打印我们设置过的自由度 # 只打印我们设置过的自由度（字符串ID对比）
if hand.dof_id in test_angles: 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: else:
print(f"获取设置后状态失败，错误码: {error_code}") print(f"获取设置后状态失败，错误码: {error_code}")
# 4. 恢复默认角度（复位操作） # 4. 恢复默认角度（复位操作）
print("\n--- 恢复默认角度 ---") print("\n--- 恢复默认角度 ---")
# 复位字典：键为字符串ID，值为默认百分比
default_angles = { default_angles = {
0: 0.1, "little_finger": 0.1,
1: 0.1, "ring_finger": 0.1,
2: 0.1, "middle_finger": 0.1,
3: 0.1, "index_finger": 0.1,
4: 0.1, "thumb_bend": 0.1,
5: 0.8 "thumb_rotate": 0.8
} }
set_result = dexhand.set_angle(default_angles) set_result = dexhand.set_angle(default_angles)
@ -456,10 +463,12 @@ def test_dexhand():
if error_code == CMVRErrorCode.CMVR_SUCCESS: if error_code == CMVRErrorCode.CMVR_SUCCESS:
print(f"最终初始化状态: {state.is_initialized}") print(f"最终初始化状态: {state.is_initialized}")
print("关键自由度最终位置:") print("关键自由度最终位置:")
# 遍历需要确认的自由度（字符串ID）
for dof_id in test_angles.keys(): for dof_id in test_angles.keys():
for hand in state.hands: for hand in state.hands:
if hand.dof_id == dof_id: 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: else:
print(f"获取最终状态失败，错误码: {error_code}") print(f"获取最终状态失败，错误码: {error_code}")
@ -863,8 +872,8 @@ if __name__ == "__main__":
# test_biohead() # test_biohead()
# test_camera() # test_camera()
# test_rgb_image_stream() # test_rgb_image_stream()
# test_dexhand() test_dexhand()
# test_sensor_data_stream() # test_sensor_data_stream()
# test_speaker() # test_speaker()
# test_microphone() # test_microphone()
test_humanoid_robot() # test_humanoid_robot()