278 lines
9.6 KiB
Python
278 lines
9.6 KiB
Python
from dataclasses import dataclass, field
|
||
from typing import List
|
||
import numpy as np
|
||
|
||
@dataclass
|
||
class MicState:
|
||
"""麦克风状态"""
|
||
is_initialized: bool = False
|
||
is_running: bool = False
|
||
is_recording: bool = False
|
||
volume: int = 0
|
||
error_message: str = ""
|
||
|
||
@dataclass
|
||
class SpeakerState:
|
||
"""扬声器状态"""
|
||
is_initialized: bool = False
|
||
is_running: bool = False
|
||
is_decoding: bool = False
|
||
is_paused: bool = False
|
||
volume: int = 0
|
||
|
||
@dataclass
|
||
class CameraState:
|
||
"""相机状态"""
|
||
is_initialized: bool = False
|
||
is_opened: bool = False
|
||
is_streaming: bool = False
|
||
is_recording: bool = False
|
||
width: int = 0
|
||
height: int = 0
|
||
fps: int = 0
|
||
@dataclass
|
||
class CameraIntrinsics:
|
||
"""相机内参（与Protobuf CameraIntrinsics对齐）"""
|
||
cx: float = 0.0 # 对应Protobuf tag=1
|
||
cy: float = 0.0 # 对应Protobuf tag=2
|
||
fx: float = 0.0 # 对应Protobuf tag=3
|
||
fy: float = 0.0 # 对应Protobuf tag=4
|
||
# 畸变系数：固定5个元素（k1, k2, p1, p2, k3），与Protobuf tag=5对齐
|
||
coeffs: List[float] = field(default_factory=lambda: [0.0]*5)
|
||
|
||
@dataclass
|
||
class FreedomState:
|
||
"""灵巧手自由度状态"""
|
||
dof_id: int = 0
|
||
angle: int = 0
|
||
speed: int = 0
|
||
force: int = 0
|
||
position: int = 0
|
||
current: int = 0
|
||
temperature: int = 0
|
||
error: int = 0
|
||
error_message: List[str] = field(default_factory=list)
|
||
|
||
@dataclass
|
||
class DexHandState:
|
||
"""灵巧手整体状态"""
|
||
is_initialized: bool = False
|
||
hands: List[FreedomState] = field(default_factory=lambda: [FreedomState() for _ in range(6)])
|
||
|
||
@dataclass
|
||
class FacialExpressionState:
|
||
"""面部表情状态"""
|
||
# 眉毛
|
||
left_eyebrow_outside_y: float = 0.0
|
||
left_eyebrow_inside_y: float = 0.0
|
||
right_eyebrow_outside_y: float = 0.0
|
||
right_eyebrow_inside_y: float = 0.0
|
||
|
||
# 眼睑
|
||
left_eye_upper_lid_y: float = 0.0
|
||
left_eye_lower_lid_y: float = 0.0
|
||
right_eye_upper_lid_y: float = 0.0
|
||
right_eye_lower_lid_y: float = 0.0
|
||
|
||
# 眼球
|
||
left_eye_ball_x: float = 0.0
|
||
left_eye_ball_y: float = 0.0
|
||
right_eye_ball_x: float = 0.0
|
||
right_eye_ball_y: float = 0.0
|
||
|
||
# 鼻子
|
||
left_nose_y: float = 0.0
|
||
right_nose_y: float = 0.0
|
||
|
||
# 嘴巴
|
||
upper_lip_y: float = 0.0
|
||
upper_lip_z: float = 0.0
|
||
lower_lip_y: float = 0.0
|
||
lower_lip_z: float = 0.0
|
||
|
||
# 左唇角
|
||
upper_left_lip_x: float = 0.0
|
||
upper_left_lip_y: float = 0.0
|
||
left_corner_lip_x: float = 0.0
|
||
left_corner_lip_y: float = 0.0
|
||
lower_left_lip_x: float = 0.0
|
||
lower_left_lip_y: float = 0.0
|
||
|
||
# 右唇角
|
||
upper_right_lip_x: float = 0.0
|
||
upper_right_lip_y: float = 0.0
|
||
right_corner_lip_x: float = 0.0
|
||
right_corner_lip_y: float = 0.0
|
||
lower_right_lip_x: float = 0.0
|
||
lower_right_lip_y: float = 0.0
|
||
|
||
# 下巴
|
||
jaw_x: float = 0.0
|
||
jaw_y: float = 0.0
|
||
|
||
@dataclass
|
||
class FingerTactileData:
|
||
"""手指触觉数据"""
|
||
data: np.ndarray = None
|
||
rows: int = 0
|
||
cols: int = 0
|
||
byteSize: int = 0
|
||
name: str = ""
|
||
|
||
def __post_init__(self):
|
||
if self.data is None and self.rows > 0 and self.cols > 0:
|
||
self.data = np.zeros((self.rows, self.cols), dtype=np.uint16)
|
||
|
||
@dataclass
|
||
class PalmTactileData:
|
||
"""手掌触觉数据"""
|
||
data: np.ndarray = None
|
||
rows: int = 8
|
||
cols: int = 14
|
||
byteSize: int = 224
|
||
name: str = "掌心"
|
||
|
||
def __post_init__(self):
|
||
if self.data is None:
|
||
self.data = np.zeros((self.rows, self.cols), dtype=np.uint16)
|
||
|
||
@dataclass
|
||
class HandTactileSensors:
|
||
"""整只手的触觉传感器数据"""
|
||
pinky_tip: FingerTactileData = None
|
||
pinky_finger: FingerTactileData = None
|
||
pinky_pad: FingerTactileData = None
|
||
|
||
ring_tip: FingerTactileData = None
|
||
ring_finger: FingerTactileData = None
|
||
ring_pad: FingerTactileData = None
|
||
|
||
middle_tip: FingerTactileData = None
|
||
middle_finger: FingerTactileData = None
|
||
middle_pad: FingerTactileData = None
|
||
|
||
index_tip: FingerTactileData = None
|
||
index_finger: FingerTactileData = None
|
||
index_pad: FingerTactileData = None
|
||
|
||
thumb_tip: FingerTactileData = None
|
||
thumb_finger: FingerTactileData = None
|
||
thumb_middle: FingerTactileData = None
|
||
thumb_pad: FingerTactileData = None
|
||
|
||
palm: PalmTactileData = None
|
||
|
||
def __post_init__(self):
|
||
"""初始化所有传感器数据"""
|
||
# 小拇指
|
||
self.pinky_tip = FingerTactileData(rows=3, cols=3, byteSize=18, name="小拇指指端")
|
||
self.pinky_finger = FingerTactileData(rows=12, cols=8, byteSize=192, name="小拇指指尖")
|
||
self.pinky_pad = FingerTactileData(rows=10, cols=8, byteSize=160, name="小拇指指腹")
|
||
|
||
# 无名指
|
||
self.ring_tip = FingerTactileData(rows=3, cols=3, byteSize=18, name="无名指指端")
|
||
self.ring_finger = FingerTactileData(rows=12, cols=8, byteSize=192, name="无名指指尖")
|
||
self.ring_pad = FingerTactileData(rows=10, cols=8, byteSize=160, name="无名指指腹")
|
||
|
||
# 中指
|
||
self.middle_tip = FingerTactileData(rows=3, cols=3, byteSize=18, name="中指指端")
|
||
self.middle_finger = FingerTactileData(rows=12, cols=8, byteSize=192, name="中指指尖")
|
||
self.middle_pad = FingerTactileData(rows=10, cols=8, byteSize=160, name="中指指腹")
|
||
|
||
# 食指
|
||
self.index_tip = FingerTactileData(rows=3, cols=3, byteSize=18, name="食指指端")
|
||
self.index_finger = FingerTactileData(rows=12, cols=8, byteSize=192, name="食指指尖")
|
||
self.index_pad = FingerTactileData(rows=10, cols=8, byteSize=160, name="食指指腹")
|
||
|
||
# 大拇指
|
||
self.thumb_tip = FingerTactileData(rows=3, cols=3, byteSize=18, name="大拇指指端")
|
||
self.thumb_finger = FingerTactileData(rows=12, cols=8, byteSize=192, name="大拇指尖")
|
||
self.thumb_middle = FingerTactileData(rows=3, cols=3, byteSize=18, name="大拇指指中")
|
||
self.thumb_pad = FingerTactileData(rows=12, cols=8, byteSize=192, name="大拇指指腹")
|
||
|
||
# 掌心
|
||
self.palm = PalmTactileData()
|
||
|
||
def get_finger_name(self, finger_type):
|
||
"""获取手指名称"""
|
||
from .enums import FingerType
|
||
|
||
names = {
|
||
FingerType.PINKY: "小拇指",
|
||
FingerType.RING: "无名指",
|
||
FingerType.MIDDLE: "中指",
|
||
FingerType.INDEX: "食指",
|
||
FingerType.THUMB: "大拇指"
|
||
}
|
||
return names.get(finger_type, "未知")
|
||
def print_summary(self):
|
||
"""打印传感器数据摘要信息"""
|
||
print(f" 小拇指指端: {self.pinky_tip.rows}x{self.pinky_tip.cols} (数据大小: {self.pinky_tip.byteSize} bytes)")
|
||
print(f" 无名指指端: {self.ring_tip.rows}x{self.ring_tip.cols} (数据大小: {self.ring_tip.byteSize} bytes)")
|
||
print(f" 中指指端: {self.middle_tip.rows}x{self.middle_tip.cols} (数据大小: {self.middle_tip.byteSize} bytes)")
|
||
print(f" 食指指端: {self.index_tip.rows}x{self.index_tip.cols} (数据大小: {self.index_tip.byteSize} bytes)")
|
||
print(f" 大拇指指端: {self.thumb_tip.rows}x{self.thumb_tip.cols} (数据大小: {self.thumb_tip.byteSize} bytes)")
|
||
print(f" 大拇指指中: {self.thumb_middle.rows}x{self.thumb_middle.cols} (数据大小: {self.thumb_middle.byteSize} bytes)")
|
||
print(f" 掌心: {self.palm.rows}x{self.palm.cols} (数据大小: {self.palm.byteSize} bytes)")
|
||
|
||
# 机器人相关数据结构
|
||
@dataclass
|
||
class JointCmd:
|
||
"""关节控制命令"""
|
||
joint_name: str = "" # 关节名称
|
||
rad: float = 0.0 # 弧度
|
||
vel: float = 0.0 # 角速度，rad/s
|
||
|
||
@dataclass
|
||
class Pose3D:
|
||
"""3D位姿"""
|
||
x: float = 0.0 # X坐标
|
||
y: float = 0.0 # Y坐标
|
||
z: float = 0.0 # Z坐标
|
||
rx: float = 0.0 # 绕X轴旋转（弧度）
|
||
ry: float = 0.0 # 绕Y轴旋转（弧度）
|
||
rz: float = 0.0 # 绕Z轴旋转（弧度）
|
||
|
||
def to_array(self) -> np.ndarray:
|
||
"""转换为numpy数组"""
|
||
return np.array([self.x, self.y, self.z, self.rx, self.ry, self.rz])
|
||
|
||
@classmethod
|
||
def from_array(cls, arr: np.ndarray) -> 'Pose3D':
|
||
"""从numpy数组创建Pose3D"""
|
||
return cls(arr[0], arr[1], arr[2], arr[3], arr[4], arr[5])
|
||
|
||
@dataclass
|
||
class JointState:
|
||
"""关节状态"""
|
||
name: List[str] = field(default_factory=list) # 关节名称列表
|
||
position: List[float] = field(default_factory=list) # 关节位置列表（弧度）
|
||
velocity: List[float] = field(default_factory=list) # 关节速度列表（rad/s）
|
||
effort: List[float] = field(default_factory=list) # 关节力矩列表
|
||
timestamp: float = 0.0 # 时间戳
|
||
|
||
def __post_init__(self):
|
||
"""确保列表长度一致"""
|
||
if len(self.name) > 0:
|
||
n_joints = len(self.name)
|
||
self.position = self.position[:n_joints] + [0.0] * (n_joints - len(self.position))
|
||
self.velocity = self.velocity[:n_joints] + [0.0] * (n_joints - len(self.velocity))
|
||
self.effort = self.effort[:n_joints] + [0.0] * (n_joints - len(self.effort))
|
||
|
||
def get_joint_by_name(self, joint_name: str) -> tuple:
|
||
"""根据关节名称获取状态"""
|
||
if joint_name in self.name:
|
||
idx = self.name.index(joint_name)
|
||
return (self.position[idx], self.velocity[idx], self.effort[idx])
|
||
return (0.0, 0.0, 0.0)
|
||
|
||
def to_dict(self) -> dict:
|
||
"""转换为字典格式"""
|
||
return {
|
||
'name': self.name,
|
||
'positions': self.position,
|
||
'velocities': self.velocity,
|
||
'efforts': self.effort,
|
||
'timestamp': self.timestamp
|
||
}
|