279 lines
11 KiB
Python
279 lines
11 KiB
Python
import grpc
|
||
import numpy as np
|
||
from typing import List, Optional, Tuple
|
||
from typing import Dict
|
||
|
||
from .enums import CMVRErrorCode, RobotCartesian, RobotJointIndexDirection
|
||
from .models import JointCmd, Pose3D, JointState
|
||
|
||
class HumanoidRobotClient:
|
||
"""人形机器人客户端"""
|
||
|
||
def __init__(self, device_id: str, stub):
|
||
"""
|
||
初始化机器人客户端
|
||
|
||
参数:
|
||
device_id: 设备ID
|
||
stub: gRPC 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, humanoid_robot_command_pb2
|
||
self.generated = type('GeneratedModules', (), {
|
||
'common_pb2': common_pb2,
|
||
'humanoid_robot_command_pb2': humanoid_robot_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 torqueOn(self) -> CMVRErrorCode:
|
||
try:
|
||
generated = self._import_generated()
|
||
request = generated.common_pb2.CommandHeader.Request()
|
||
request.CopyFrom(self._create_command_header())
|
||
|
||
response = self.stub.torqueOn(request)
|
||
|
||
return CMVRErrorCode.CMVR_SUCCESS if response.success else CMVRErrorCode.CMVR_RPC_FAILED
|
||
|
||
except grpc.RpcError as e:
|
||
print(f"开启使能失败: {e}")
|
||
return CMVRErrorCode.CMVR_RPC_FAILED
|
||
|
||
def torqueOff(self) -> CMVRErrorCode:
|
||
try:
|
||
generated = self._import_generated()
|
||
request = generated.common_pb2.CommandHeader.Request()
|
||
request.CopyFrom(self._create_command_header())
|
||
|
||
response = self.stub.torqueOff(request)
|
||
|
||
return CMVRErrorCode.CMVR_SUCCESS if response.success else CMVRErrorCode.CMVR_RPC_FAILED
|
||
|
||
except grpc.RpcError as e:
|
||
print(f"关闭使能失败: {e}")
|
||
return CMVRErrorCode.CMVR_RPC_FAILED
|
||
|
||
# ==================== 关节空间运动 ====================
|
||
|
||
def moveJ(self, joint_commands: List[JointCmd],
|
||
overall_vel: float = 0.1, overall_acc: float = 0.5) -> Tuple[CMVRErrorCode, Optional[str]]:
|
||
try:
|
||
generated = self._import_generated()
|
||
# 创建MoveJ请求
|
||
request = generated.humanoid_robot_command_pb2.MoveJ.Request()
|
||
request.header.CopyFrom(self._create_command_header())
|
||
request.vel = overall_vel
|
||
request.acc = overall_acc
|
||
|
||
# 添加关节命令
|
||
for cmd in joint_commands:
|
||
joint_cmd = request.cmds.add()
|
||
joint_cmd.joint_name = cmd.joint_name
|
||
joint_cmd.rad = cmd.rad
|
||
joint_cmd.vel = cmd.vel
|
||
|
||
# 调用RPC接口
|
||
response = self.stub.moveJ(request)
|
||
|
||
return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED
|
||
|
||
except grpc.RpcError as e:
|
||
return CMVRErrorCode.CMVR_RPC_FAILED
|
||
|
||
# ==================== 直线空间运动 ====================
|
||
|
||
def moveL(self, ee_link: str, target_pose: Pose3D,
|
||
vel: float = 0.1, acc: float = 0.5) -> Tuple[CMVRErrorCode, Optional[str]]:
|
||
"""
|
||
直线空间运动 (MoveL)
|
||
|
||
参数:
|
||
ee_link: 末端执行器连杆名称
|
||
target_pose: 目标位姿 (Pose3D)
|
||
vel: 速度 (m/s)
|
||
acc: 加速度 (m/s²)
|
||
|
||
返回:
|
||
Tuple[CMVRErrorCode, Optional[str]]: 错误码和错误消息
|
||
"""
|
||
try:
|
||
generated = self._import_generated()
|
||
# 创建MoveL请求
|
||
request = generated.humanoid_robot_command_pb2.MoveL.Request()
|
||
request.header.CopyFrom(self._create_command_header())
|
||
request.ee_link = ee_link
|
||
request.vel = vel
|
||
request.acc = acc
|
||
|
||
# 设置目标位姿
|
||
request.target_pose.x = target_pose.x
|
||
request.target_pose.y = target_pose.y
|
||
request.target_pose.z = target_pose.z
|
||
request.target_pose.rx = target_pose.rx
|
||
request.target_pose.ry = target_pose.ry
|
||
request.target_pose.rz = target_pose.rz
|
||
|
||
# 调用RPC接口
|
||
response = self.stub.moveL(request)
|
||
return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED
|
||
|
||
except grpc.RpcError as e:
|
||
print(f"直线空间运动失败: {e}")
|
||
return CMVRErrorCode.CMVR_RPC_FAILED
|
||
|
||
# ==================== 关节速度控制 ====================
|
||
|
||
def speedJ(self, joint_name: str, velocity: float,
|
||
direction: RobotJointIndexDirection = RobotJointIndexDirection.FORWARD,
|
||
acc: float = 0.5) -> Tuple[CMVRErrorCode, Optional[str]]:
|
||
"""
|
||
关节速度控制 (SpeedJ)
|
||
|
||
参数:
|
||
joint_name: 关节名称
|
||
velocity: 速度 (rad/s)
|
||
direction: 运动方向
|
||
acc: 加速度 (rad/s²)
|
||
|
||
返回:
|
||
Tuple[CMVRErrorCode, Optional[str]]: 错误码和错误消息
|
||
"""
|
||
try:
|
||
generated = self._import_generated()
|
||
# 创建SpeedJ请求
|
||
request = generated.humanoid_robot_command_pb2.SpeedJ.Request()
|
||
request.header.CopyFrom(self._create_command_header())
|
||
request.joint_name = joint_name
|
||
request.vel = abs(velocity) # 确保速度为正值
|
||
request.acc = acc
|
||
request.direction = direction.value
|
||
|
||
# 调用RPC接口
|
||
response = self.stub.speedJ(request)
|
||
return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED
|
||
|
||
except grpc.RpcError as e:
|
||
print(f"关节速度控制失败: {e}")
|
||
return CMVRErrorCode.CMVR_RPC_FAILED, str(e)
|
||
|
||
# ==================== 笛卡尔速度控制 ====================
|
||
|
||
def speedL(self, ee_link: str, velocity: float,
|
||
cartesian: RobotCartesian = RobotCartesian.X,
|
||
direction: RobotJointIndexDirection = RobotJointIndexDirection.FORWARD,
|
||
acc: float = 0.5) -> Tuple[CMVRErrorCode, Optional[str]]:
|
||
"""
|
||
笛卡尔速度控制 (SpeedL)
|
||
|
||
参数:
|
||
ee_link: 末端执行器连杆名称
|
||
velocity: 速度 (m/s 或 rad/s，取决于cartesian类型)
|
||
cartesian: 笛卡尔坐标轴
|
||
direction: 运动方向
|
||
acc: 加速度 (m/s² 或 rad/s²)
|
||
|
||
返回:
|
||
Tuple[CMVRErrorCode, Optional[str]]: 错误码和错误消息
|
||
"""
|
||
try:
|
||
generated = self._import_generated()
|
||
# 创建SpeedL请求
|
||
request = generated.humanoid_robot_command_pb2.SpeedL.Request()
|
||
request.header.CopyFrom(self._create_command_header())
|
||
request.ee_link = ee_link
|
||
request.vel = abs(velocity)
|
||
request.acc = acc
|
||
request.cartesian = cartesian.value
|
||
request.direction = direction.value
|
||
|
||
# 调用RPC接口
|
||
response = self.stub.speedL(request)
|
||
return CMVRErrorCode.CMVR_SUCCESS if response.header.success else CMVRErrorCode.CMVR_RPC_FAILED
|
||
|
||
except grpc.RpcError as e:
|
||
print(f"笛卡尔速度控制失败: {e}")
|
||
return CMVRErrorCode.CMVR_RPC_FAILED, str(e)
|
||
|
||
# ==================== 状态获取 ====================
|
||
|
||
def get_joint_states(self) -> Tuple[CMVRErrorCode, List[JointState]]:
|
||
"""
|
||
获取机器人所有关节状态
|
||
|
||
返回:
|
||
Tuple[CMVRErrorCode, List[JointState]]: 错误码和关节状态对象列表
|
||
"""
|
||
try:
|
||
generated = self._import_generated()
|
||
# 创建关节状态请求
|
||
request = generated.humanoid_robot_command_pb2.JointRequest()
|
||
request.header.CopyFrom(self._create_command_header())
|
||
|
||
# 调用RPC接口
|
||
response = self.stub.getJointState(request)
|
||
|
||
if not response.header.success:
|
||
print(f"RPC调用失败: {response.header}")
|
||
return CMVRErrorCode.CMVR_RPC_FAILED, []
|
||
|
||
# 检查是否有状态数据
|
||
if not response.state:
|
||
print("响应中没有关节状态数据")
|
||
return CMVRErrorCode.CMVR_NO_DATA, []
|
||
|
||
# 解析所有JointState
|
||
joint_states = []
|
||
for proto_joint_state in response.state:
|
||
# 解析响应数据到JointState对象
|
||
# 注意proto字段名和Python dataclass字段名的映射
|
||
joint_state = JointState(
|
||
name=list(proto_joint_state.name), # proto: name -> Python: name
|
||
position=list(proto_joint_state.position), # proto: position -> Python: position
|
||
velocity=list(proto_joint_state.velocity), # proto: velocity -> Python: velocity
|
||
effort=list(proto_joint_state.effort), # proto: effort -> Python: effort
|
||
timestamp=proto_joint_state.timestamp # proto: timestamp -> Python: timestamp
|
||
)
|
||
joint_states.append(joint_state)
|
||
|
||
if not joint_states:
|
||
print("解析后没有有效的关节状态数据")
|
||
return CMVRErrorCode.CMVR_NO_DATA, []
|
||
|
||
return CMVRErrorCode.CMVR_SUCCESS, joint_states
|
||
|
||
except grpc.RpcError as e:
|
||
print(f"gRPC错误 - 获取关节状态失败: {e}")
|
||
if hasattr(e, 'code'):
|
||
if e.code() == grpc.StatusCode.NOT_FOUND:
|
||
return CMVRErrorCode.CMVR_NOT_FOUND, []
|
||
elif e.code() == grpc.StatusCode.UNAVAILABLE:
|
||
return CMVRErrorCode.CMVR_CONNECTION_FAILED, []
|
||
return CMVRErrorCode.CMVR_RPC_FAILED, []
|
||
|
||
except Exception as e:
|
||
print(f"未知错误 - 获取关节状态失败: {e}")
|
||
import traceback
|
||
traceback.print_exc()
|
||
return CMVRErrorCode.CMVR_UNKNOWN_ERROR, [] |