跳转至

Service Registry

controller.shared.config.service_registry

服务注册配置 - Shared Config 注册所有应用服务到DI容器 - 简化版

SerialApplicationService 

SerialApplicationService(message_display: MessageDisplay, serial_domain_service: SerialDomainService)

Bases: BaseService

串口应用服务。

只负责串口连接管理和状态协调，不处理数据收发。 数据收发由各个Domain服务直接使用SerialDomainService。

属性：

名称 类型 描述
connection_status_changed pyqtSignal

串口连接状态变化信号。
port_list_updated pyqtSignal

端口列表更新信号。

初始化串口应用服务。

源代码位于： src/controller/controller/application/services/serial_application_service.py 
27
28
29
30
31
32
33
34
def __init__(self, message_display: MessageDisplay, serial_domain_service: SerialDomainService):
    """初始化串口应用服务。"""
    super().__init__(message_display)

    self.serial_domain_service = serial_domain_service

    # 连接Domain层信号到Application层
    self._connect_domain_signals()

refresh_ports 

refresh_ports() -> List[str]

刷新可用端口列表。

返回：

类型 描述
List[str]

List[str]: 可用端口名称列表。
源代码位于： src/controller/controller/application/services/serial_application_service.py 
36
37
38
39
40
41
42
43
44
45
46
47
48
49
def refresh_ports(self) -> List[str]:
    """刷新可用端口列表。

    Returns:
        List[str]: 可用端口名称列表。
    """
    try:
        ports = self.serial_domain_service.scan_available_ports()
        self.port_list_updated.emit(ports)
        self._display_message(f"发现 {len(ports)} 个可用端口", "系统")
        return ports
    except Exception as e:
        self._display_message(f"刷新端口失败: {str(e)}", "错误")
        return []

get_port_info 

get_port_info(port_name: str) -> Optional[Dict[str, str]]

获取指定端口信息。

参数：

名称 类型 描述 默认
port_name str

端口名称。

 必需

返回：

类型 描述
Optional[Dict[str, str]]

Optional[Dict[str, str]]: 端口信息字典，失败时返回None。
源代码位于： src/controller/controller/application/services/serial_application_service.py 
51
52
53
54
55
56
57
58
59
60
61
62
63
64
def get_port_info(self, port_name: str) -> Optional[Dict[str, str]]:
    """获取指定端口信息。

    Args:
        port_name (str): 端口名称。

    Returns:
        Optional[Dict[str, str]]: 端口信息字典，失败时返回None。
    """
    try:
        return self.serial_domain_service.get_port_info(port_name)
    except Exception as e:
        self._display_message(f"获取端口信息失败: {str(e)}", "错误")
        return None

get_available_ports 

get_available_ports() -> List[str]

获取当前可用端口列表（不刷新）。

返回：

类型 描述
List[str]

List[str]: 可用端口名称列表。
源代码位于： src/controller/controller/application/services/serial_application_service.py 
66
67
68
69
70
71
72
73
74
75
76
def get_available_ports(self) -> List[str]:
    """获取当前可用端口列表（不刷新）。

    Returns:
        List[str]: 可用端口名称列表。
    """
    try:
        return self.serial_domain_service.scan_available_ports()
    except Exception as e:
        self._display_message(f"获取端口列表失败: {str(e)}", "错误")
        return []

get_current_port 

get_current_port() -> Optional[str]

获取当前连接的端口名称。

返回：

类型 描述
Optional[str]

Optional[str]: 端口名称，未连接时返回None。
源代码位于： src/controller/controller/application/services/serial_application_service.py 
78
79
80
81
82
83
84
def get_current_port(self) -> Optional[str]:
    """获取当前连接的端口名称。

    Returns:
        Optional[str]: 端口名称，未连接时返回None。
    """
    return self.serial_domain_service.get_current_port()

connect_serial 

connect_serial(port: str, config: Dict[str, Any]) -> bool

连接串口。

参数：

名称 类型 描述 默认
port str

串口名称。

 必需
config Dict[str, Any]

串口配置参数。

 必需

返回：

名称 类型 描述
bool bool

连接是否成功。
源代码位于： src/controller/controller/application/services/serial_application_service.py 
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
def connect_serial(self, port: str, config: Dict[str, Any]) -> bool:
    """连接串口。

    Args:
        port (str): 串口名称。
        config (Dict[str, Any]): 串口配置参数。

    Returns:
        bool: 连接是否成功。
    """
    self._display_message(f"连接串口: {port}, 配置: {config}", "系统")
    try:
        success = self.serial_domain_service.connect_port(port, config)
        if success:
            self._display_message(f"成功连接到端口 {port}", "系统")
        return success
    except Exception as e:
        self._display_message(f"连接失败: {str(e)}", "错误")
        return False

disconnect_serial 

disconnect_serial() -> bool

断开串口连接。

返回：

名称 类型 描述
bool bool

断开是否成功。
源代码位于： src/controller/controller/application/services/serial_application_service.py 
106
107
108
109
110
111
112
113
114
115
116
117
118
119
def disconnect_serial(self) -> bool:
    """断开串口连接。

    Returns:
        bool: 断开是否成功。
    """
    try:
        success = self.serial_domain_service.disconnect_port()
        if success:
            self._display_message("已断开串口连接", "系统")
        return success
    except Exception as e:
        self._display_message(f"断开连接失败: {str(e)}", "错误")
        return False

is_connected 

is_connected() -> bool

检查串口是否已连接。

返回：

名称 类型 描述
bool bool

是否已连接。
源代码位于： src/controller/controller/application/services/serial_application_service.py 
121
122
123
124
125
126
127
def is_connected(self) -> bool:
    """检查串口是否已连接。

    Returns:
        bool: 是否已连接。
    """
    return self.serial_domain_service.is_connected()

cleanup 

cleanup() -> None

清理资源。

源代码位于： src/controller/controller/application/services/serial_application_service.py 
145
146
147
148
149
150
151
def cleanup(self) -> None:
    """清理资源。"""
    try:
        self._disconnect_domain_signals()
        self.serial_domain_service.cleanup()
    except Exception:
        pass  # 忽略清理时的异常

CommandHubService 

CommandHubService(message_domain_service: MessageDomainService, motion_runner: MotionRunner, serial_domain_service: SerialDomainService, message_display: MessageDisplay, motion_constructor: MotionConstructor)

Bases: BaseService

命令中心服务 - Application层。

职责： 1. 分发用户命令到不同的处理逻辑 2. 协调运动任务的准备和执行 3. 管理消息显示

属性：

名称 类型 描述
message_domain_service

消息领域服务。
motion_runner

运动执行器。
serial_domain_service

串口领域服务。
motion_constructor

运动构造器。

初始化命令中心服务。

源代码位于： src/controller/controller/application/services/command_hub_service.py 
28
29
30
31
32
33
34
35
36
37
38
39
def __init__(self, 
message_domain_service: MessageDomainService, 
motion_runner: MotionRunner,
serial_domain_service: SerialDomainService, 
message_display: MessageDisplay,
motion_constructor: MotionConstructor):
    """初始化命令中心服务。"""
    super().__init__(message_display)
    self.message_domain_service = message_domain_service
    self.motion_runner = motion_runner
    self.serial_domain_service = serial_domain_service
    self.motion_constructor = motion_constructor

single_send_command 

single_send_command(**kwargs: Any) -> bool

发送单个命令。

参数：

名称 类型 描述 默认
**kwargs Any

命令参数，将被编码为消息帧。

 {}

返回：

名称 类型 描述
bool bool

是否发送成功。
源代码位于： src/controller/controller/application/services/command_hub_service.py 
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
def single_send_command(self, **kwargs: Any) -> bool:
    """发送单个命令。

    Args:
        **kwargs: 命令参数，将被编码为消息帧。

    Returns:
        bool: 是否发送成功。
    """
    try:
        msg = self.message_domain_service.encode_message(**kwargs)
        self._display_message(msg, "发送")
        success = self.serial_domain_service.send_data(msg)
        if not success:
            self._display_message("发送失败：串口未连接", "错误")
        return success
    except Exception as e:
        self._display_message(f"发送命令失败: {str(e)}", "错误")
        return False

command_distribution 

command_distribution(config_dict: dict)

分发用户命令到对应的处理逻辑。

参数：

名称 类型 描述 默认
config_dict dict

配置字典，包含 control, mode 等参数。

 必需
源代码位于： src/controller/controller/application/services/command_hub_service.py 
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
def command_distribution(self, config_dict: dict):
    """分发用户命令到对应的处理逻辑。

    Args:
        config_dict (dict): 配置字典，包含 control, mode 等参数。
    """
    control = config_dict.get('control')
    mode = config_dict.get('mode')

    if control in [0x00, 0x01, 0x02, 0x03, 0x04]:
        self.single_send_command(**config_dict)
        self.get_current_position()

    elif control == 0x05:
        if mode == 0x0A:
            self._display_message("关闭示教模式", "动力学")
        self.single_send_command(**config_dict)
        self.motion_runner.stop_motion()

    elif control == 0x07:
        if mode == 0x0A:
            self._display_message("开启示教模式", "动力学")
        self.single_send_command(**config_dict)
        if mode != 0x0A:
            self.get_current_position()

    elif control == 0x06:
        if mode == 0x0A:
            torque_values = config_dict.get('torque')
            self._display_message(f"发送力矩: {torque_values}", "动力学")
            self.single_send_command(**config_dict)
        else:
            # 角度控制：默认使用 S 曲线和 0.01 频率
            target_angles = config_dict.get('target_angles')
            task = {
                "type": "motion",
                "target_angles": target_angles,
                "curve_type": "s_curve",
                "frequency": 0.01
            }
            self.motion_constructor.prepare_operation(
                MotionOperationMode.EXECUTE,
                [task]
            )
            self._display_message(f"准备运动到目标位置: {target_angles}", "控制")
            self.get_current_position()

    elif control == 0x08:
        self.single_send_command(**config_dict)
        self.motion_runner.stop_motion()

get_current_position 

get_current_position()

请求获取当前位置。

源代码位于： src/controller/controller/application/services/command_hub_service.py 
113
114
115
116
117
def get_current_position(self):
    """请求获取当前位置。"""
    self.message_display.clear_messages()
    self._display_message("正在获取当前位置...", "控制")
    self.single_send_command(control=0x07)

run_teach_record 

run_teach_record(angles_list: list)

运行示教记录（播放示教轨迹）。

流程： 1. 准备示教运动任务 2. 获取当前位置 3. 异步回调时自动构建平滑轨迹并执行

参数：

名称 类型 描述 默认
angles_list list[list[float]]

示教记录的角度列表 [[θ1,...,θ6], ...]。

 必需
源代码位于： src/controller/controller/application/services/command_hub_service.py 
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
def run_teach_record(self, angles_list: list):
    """运行示教记录（播放示教轨迹）。

    流程：
    1. 准备示教运动任务
    2. 获取当前位置
    3. 异步回调时自动构建平滑轨迹并执行

    Args:
        angles_list (list[list[float]]): 示教记录的角度列表 [[θ1,...,θ6], ...]。
    """
    task = {
        "type": "teach",
        "teach_data": angles_list
    }
    self.motion_constructor.prepare_operation(
        MotionOperationMode.EXECUTE,
        [task]
    )
    self._display_message(f"准备播放示教轨迹，共 {len(angles_list)} 个点", "示教")
    self.get_current_position()

run_motion_sequence 

run_motion_sequence()

开始执行运动序列（用于运动规划方案）。

流程： 1. 查询当前位置 2. 收到位置反馈后，MotionConstructor 自动规划所有任务

源代码位于： src/controller/controller/application/services/command_hub_service.py 
141
142
143
144
145
146
147
148
149
def run_motion_sequence(self):
    """开始执行运动序列（用于运动规划方案）。

    流程：
    1. 查询当前位置
    2. 收到位置反馈后，MotionConstructor 自动规划所有任务
    """
    self._display_message("开始执行运动规划方案...", "运动规划")
    self.get_current_position()

MessageResponseService 

MessageResponseService(message_display: MessageDisplay, serial_domain_service: SerialDomainService, message_domain_service: MessageDomainService, robot_state_service: RobotStateDomainService, motion_runner: MotionRunner, motion_constructor: MotionConstructor, trajectory_planner: TrajectoryPlanningService, trajectory_repository: TrajectoryRepository)

Bases: BaseService

消息响应服务 - Application层。

处理串口接收的数据，解码并更新状态服务。

职责： 1. 接收串口数据 2. 拼接缓冲，检测完整帧 3. 解码消息 4. 更新状态服务（统一入口） 5. 根据操作模式分发处理（执行/保存/预览）

属性：

名称 类型 描述
get_current_position_signal pyqtSignal

获取当前位置信号（用于UI显示）。
trajectory_preview_signal pyqtSignal

轨迹预览数据信号。

初始化消息响应服务。

源代码位于： src/controller/controller/application/services/message_response_service.py 
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
def __init__(
    self, 
    message_display: MessageDisplay,
    serial_domain_service: SerialDomainService,
    message_domain_service: MessageDomainService,
    robot_state_service: RobotStateDomainService,
    motion_runner: MotionRunner,
    motion_constructor: MotionConstructor,
    trajectory_planner: TrajectoryPlanningService,
    trajectory_repository: TrajectoryRepository
):
    """初始化消息响应服务。"""
    super().__init__(message_display)
    self.serial_domain_service = serial_domain_service
    self.message_domain_service = message_domain_service
    self.robot_state_service = robot_state_service
    self.motion_runner = motion_runner
    self.motion_constructor = motion_constructor
    self.trajectory_planner = trajectory_planner
    self.trajectory_repository = trajectory_repository

    self._connect_signals()
    self.message_buffer = ""

    # 操作处理器映射（策略模式）
    self._operation_handlers = {
        MotionOperationMode.EXECUTE: self._handle_execute,
        MotionOperationMode.SAVE: self._handle_save,
        MotionOperationMode.PREVIEW: self._handle_preview,
    }

handle_message 

handle_message(message_in: str)

处理接收到的消息。

流程： 1. 拼接缓冲 2. 检测完整帧（AA55...0D0A） 3. 解码消息 4. 更新状态服务 5. 处理运动消息

参数：

名称 类型 描述 默认
message_in str

输入的原始串口数据。

 必需
源代码位于： src/controller/controller/application/services/message_response_service.py 
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
def handle_message(self, message_in: str):
    """处理接收到的消息。

    流程：
    1. 拼接缓冲
    2. 检测完整帧（AA55...0D0A）
    3. 解码消息
    4. 更新状态服务
    5. 处理运动消息

    Args:
        message_in (str): 输入的原始串口数据。
    """
    self.message_buffer += message_in
    if "0D0A" in self.message_buffer:
        if self.message_buffer.startswith("AA55") and len(self.message_buffer) < 120:
            return
        lines = self.message_buffer.rsplit("0D0A", 1)            
        self.message_buffer = lines[-1]
        command_line = lines[0]
        self._display_message(command_line + "0D0A", "接收")

        if command_line.startswith("AA55"):
            try:
                # 解码消息
                decoded_message = self.message_domain_service.decode_message(
                    command_line + "0D0A"
                )
                # 更新统一的状态服务（单一入口）
                self.robot_state_service.update_state(decoded_message)

                # 处理运动消息
                if decoded_message.control == 0x07 and decoded_message.mode == 0x08:
                    self.handle_motion_message(decoded_message.positions)

            except Exception as e:
                self._display_message(f"解码消息失败: {str(e)}", "错误")
        else:
            try:
                ascii_text = bytes.fromhex(command_line).decode('ascii', errors='replace')
                printable_text = ''.join(
                    c if c.isprintable() or c in '\n\r\t' 
                    else f'\\x{ord(c):02x}' 
                    for c in ascii_text
                )
                if printable_text.strip():
                    self._display_message(f"ASCII: {printable_text}", "接收")
            except Exception:
                pass

handle_motion_message 

handle_motion_message(current_position)

处理运动消息（获取当前位置的回复）。

根据状态决定行为： 1. 有待处理的操作 → 调用对应的处理器 2. 无操作 → 仅发射信号供UI显示

参数：

名称 类型 描述 默认
current_position list[float]

当前关节位置（编码器值）。

 必需
源代码位于： src/controller/controller/application/services/message_response_service.py 
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
def handle_motion_message(self, current_position):
    """处理运动消息（获取当前位置的回复）。

    根据状态决定行为：
    1. 有待处理的操作 → 调用对应的处理器
    2. 无操作 → 仅发射信号供UI显示

    Args:
        current_position (list[float]): 当前关节位置（编码器值）。
    """
    if self.motion_constructor.has_pending_operation():
        # 获取操作模式
        mode = self.motion_constructor.get_operation_mode()

        # 从字典中获取对应的处理函数
        handler = self._operation_handlers.get(mode)

        if handler:
            # 调用处理函数
            handler(current_position)
        else:
            self._display_message(f"未知操作模式: {mode}", "错误")
            self.motion_constructor.clear_operation()
    else:
        # 无操作：仅发射信号供UI显示
        self.get_current_position_signal.emit(current_position)

MotionPlanningApplicationService 

MotionPlanningApplicationService(domain_service: MotionPlanningDomainService, repository: MotionPlanRepository, motion_constructor: MotionConstructor, command_hub: CommandHubService, message_response: MessageResponseService, trajectory_repository: TrajectoryRepository)

Bases: QObject

运动规划应用服务。

职责： 1. 协调Domain和Infrastructure 2. 自动保存 3. 发送UI更新信号 4. 执行运动规划（单点和整体方案） 5. 处理"获取位置"功能

属性：

名称 类型 描述
plan_list_changed pyqtSignal

方案列表变化信号。
current_plan_changed pyqtSignal

当前方案切换信号。
point_list_changed pyqtSignal

节点列表变化信号。
current_position_received pyqtSignal

当前位置数据信号（弧度值）。
trajectory_preview_signal pyqtSignal

轨迹预览数据信号（轨迹数据，上下文）。

初始化运动规划应用服务。

参数：

名称 类型 描述 默认
domain_service MotionPlanningDomainService

运动规划领域服务。

 必需
repository MotionPlanRepository

运动方案仓储。

 必需
motion_constructor MotionConstructor

运动构造器。

 必需
command_hub CommandHubService

命令中心服务。

 必需
message_response MessageResponseService

消息响应服务。

 必需
trajectory_repository TrajectoryRepository

轨迹仓储。

 必需
源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
def __init__(
    self,
    domain_service: MotionPlanningDomainService,
    repository: MotionPlanRepository,
    motion_constructor: MotionConstructor,
    command_hub: CommandHubService,
    message_response: MessageResponseService,
    trajectory_repository: TrajectoryRepository
):
    """初始化运动规划应用服务。

    Args:
        domain_service: 运动规划领域服务。
        repository: 运动方案仓储。
        motion_constructor: 运动构造器。
        command_hub: 命令中心服务。
        message_response: 消息响应服务。
        trajectory_repository: 轨迹仓储。
    """
    super().__init__()
    self.domain_service = domain_service
    self.repository = repository
    self.motion_constructor = motion_constructor
    self.command_hub = command_hub
    self.message_response = message_response
    self.trajectory_repository = trajectory_repository

    # 连接MessageResponse的位置数据信号
    self.message_response.get_current_position_signal.connect(
        self.current_position_received.emit
    )

    # 连接MessageResponse的轨迹预览信号
    self.message_response.trajectory_preview_signal.connect(
        self.trajectory_preview_signal.emit
    )

    self._load_data()

create_plan 

create_plan(name: str)

创建方案。

参数：

名称 类型 描述 默认
name str

方案名称。

 必需
源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
115
116
117
118
119
120
121
122
123
124
125
def create_plan(self, name: str):
    """创建方案。

    Args:
        name (str): 方案名称。
    """
    new_index = self.domain_service.create_plan(name)
    self._save_data()
    self.plan_list_changed.emit()
    self.current_plan_changed.emit(new_index)
    self.point_list_changed.emit()

delete_plan 

delete_plan(index: int) -> bool

删除方案。

参数：

名称 类型 描述 默认
index int

方案索引。

 必需

返回：

名称 类型 描述
bool bool

True=删除成功, False=删除失败（违反业务规则）。
源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
def delete_plan(self, index: int) -> bool:
    """删除方案。

    Args:
        index (int): 方案索引。

    Returns:
        bool: True=删除成功, False=删除失败（违反业务规则）。
    """
    if self.domain_service.delete_plan(index):
        self._save_data()
        self.plan_list_changed.emit()
        self.current_plan_changed.emit(self.domain_service.get_current_index())
        self.point_list_changed.emit()
        return True
    return False

switch_plan 

switch_plan(index: int)

切换方案。

参数：

名称 类型 描述 默认
index int

目标方案索引。

 必需
源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
144
145
146
147
148
149
150
151
152
153
def switch_plan(self, index: int):
    """切换方案。

    Args:
        index (int): 目标方案索引。
    """
    if self.domain_service.set_current_index(index):
        self._save_data()
        self.current_plan_changed.emit(index)
        self.point_list_changed.emit()

rename_plan 

rename_plan(index: int, new_name: str)

重命名方案。

参数：

名称 类型 描述 默认
index int

方案索引。

 必需
new_name str

新名称。

 必需
源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
155
156
157
158
159
160
161
162
163
164
def rename_plan(self, index: int, new_name: str):
    """重命名方案。

    Args:
        index (int): 方案索引。
        new_name (str): 新名称。
    """
    if self.domain_service.rename_plan(index, new_name):
        self._save_data()
        self.plan_list_changed.emit()

add_point 

add_point(point_data: dict)

添加节点

源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
168
169
170
171
172
def add_point(self, point_data: dict):
    """添加节点"""
    self.domain_service.add_point(point_data)
    self._save_data()
    self.point_list_changed.emit()

delete_point 

delete_point(index: int)

删除节点

源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
174
175
176
177
178
def delete_point(self, index: int):
    """删除节点"""
    self.domain_service.remove_point(index)
    self._save_data()
    self.point_list_changed.emit()

move_point_up 

move_point_up(index: int)

上移节点

源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
180
181
182
183
184
def move_point_up(self, index: int):
    """上移节点"""
    if self.domain_service.move_point_up(index):
        self._save_data()
        self.point_list_changed.emit()

move_point_down 

move_point_down(index: int)

下移节点

源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
186
187
188
189
190
def move_point_down(self, index: int):
    """下移节点"""
    if self.domain_service.move_point_down(index):
        self._save_data()
        self.point_list_changed.emit()

update_point 

update_point(index: int, point_data: dict)

更新节点

源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
192
193
194
195
196
def update_point(self, index: int, point_data: dict):
    """更新节点"""
    self.domain_service.update_point(index, point_data)
    self._save_data()
    self.point_list_changed.emit()

get_single_point 

get_single_point(index: int) -> dict

获取单个节点数据。

参数：

名称 类型 描述 默认
index int

节点索引。

 必需

返回：

名称 类型 描述
dict dict

节点数据字典，如果索引无效则返回None。
源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
198
199
200
201
202
203
204
205
206
207
208
209
210
def get_single_point(self, index: int) -> dict:
    """获取单个节点数据。

    Args:
        index (int): 节点索引。

    Returns:
        dict: 节点数据字典，如果索引无效则返回None。
    """
    points = self.domain_service.get_all_points()
    if 0 <= index < len(points):
        return points[index]
    return None

execute_single_point 

execute_single_point(index: int)

执行单个节点。

流程： 1. 获取节点数据 2. 解析为任务列表 3. 准备执行操作 4. 查询当前位置

参数：

名称 类型 描述 默认
index int

节点索引。

 必需
源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
def execute_single_point(self, index: int):
    """执行单个节点。

    流程：
    1. 获取节点数据
    2. 解析为任务列表
    3. 准备执行操作
    4. 查询当前位置

    Args:
        index (int): 节点索引。
    """
    tasks = self._get_node_tasks(index)
    if not tasks:
        return

    # 准备执行操作
    self.motion_constructor.prepare_operation(
        MotionOperationMode.EXECUTE,
        tasks
    )

    # 查询当前位置，触发执行
    self.command_hub.get_current_position()

execute_motion_plan 

execute_motion_plan()

执行整个运动规划方案。

流程： 1. 获取所有节点的任务 2. 准备执行操作 3. 查询当前位置，触发执行

源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
def execute_motion_plan(self):
    """执行整个运动规划方案。

    流程：
    1. 获取所有节点的任务
    2. 准备执行操作
    3. 查询当前位置，触发执行
    """
    tasks = self._get_plan_tasks()
    if not tasks:
        return

    # 准备执行操作
    self.motion_constructor.prepare_operation(
        MotionOperationMode.EXECUTE,
        tasks
    )

    # 查询当前位置，触发执行
    self.command_hub.run_motion_sequence()

request_current_position 

request_current_position()

请求获取当前位置（仅用于UI显示）。

流程： 1. 发送 0x07 命令查询位置 2. 串口返回数据后，MessageResponseService 检查 has_pending_operation() 3. 因为没有调用 prepare_operation，所以返回 False 4. MessageResponseService emit get_current_position_signal 5. 本Service转发信号给ViewModel

源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
389
390
391
392
393
394
395
396
397
398
399
def request_current_position(self):
    """请求获取当前位置（仅用于UI显示）。

    流程：
    1. 发送 0x07 命令查询位置
    2. 串口返回数据后，MessageResponseService 检查 has_pending_operation()
    3. 因为没有调用 prepare_operation，所以返回 False
    4. MessageResponseService emit get_current_position_signal
    5. 本Service转发信号给ViewModel
    """
    self.command_hub.single_send_command(control=0x07)

save_node_trajectory 

save_node_trajectory(node_index: int) -> bool

保存单个节点的轨迹。

流程： 1. 获取节点任务 2. 准备保存操作 3. 查询当前位置 4. MessageResponseService 自动完成保存

参数：

名称 类型 描述 默认
node_index int

节点索引。

 必需

返回：

名称 类型 描述
bool bool

True: 准备成功, False: 准备失败。
源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
403
404
405
406
407
408
409
410
411
412
413
414
415
416
417
418
419
420
421
422
423
424
425
426
427
428
429
430
431
432
433
434
435
436
437
438
439
440
441
def save_node_trajectory(self, node_index: int) -> bool:
    """保存单个节点的轨迹。

    流程：
    1. 获取节点任务
    2. 准备保存操作
    3. 查询当前位置
    4. MessageResponseService 自动完成保存

    Args:
        node_index (int): 节点索引。

    Returns:
        bool: True: 准备成功, False: 准备失败。
    """
    try:
        tasks = self._get_node_tasks(node_index)
        if not tasks:
            return False

        current_plan = self.domain_service.get_current_plan()
        if not current_plan:
            return False

        context = {
            "filename": f"{current_plan.name}-{node_index}",
            "type": "node"
        }

        self.motion_constructor.prepare_operation(
            MotionOperationMode.SAVE,
            tasks,
            context
        )

        self.command_hub.get_current_position()
        return True
    except Exception:
        return False

save_plan_trajectory 

save_plan_trajectory() -> bool

保存整个方案的轨迹

流程： 1. 获取方案所有任务 2. 准备保存操作 3. 查询当前位置 4. MessageResponseService 自动完成保存

返回：

名称 类型 描述
True bool

准备成功
False bool

准备失败
源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
443
444
445
446
447
448
449
450
451
452
453
454
455
456
457
458
459
460
461
462
463
464
465
466
467
468
469
470
471
472
473
474
475
476
477
478
479
480
def save_plan_trajectory(self) -> bool:
    """
    保存整个方案的轨迹

    流程：
    1. 获取方案所有任务
    2. 准备保存操作
    3. 查询当前位置
    4. MessageResponseService 自动完成保存

    Returns:
        True: 准备成功
        False: 准备失败
    """
    try:
        tasks = self._get_plan_tasks()
        if not tasks:
            return False

        current_plan = self.domain_service.get_current_plan()
        if not current_plan:
            return False

        context = {
            "filename": current_plan.name,
            "type": "plan"
        }

        self.motion_constructor.prepare_operation(
            MotionOperationMode.SAVE,
            tasks,
            context
        )

        self.command_hub.get_current_position()
        return True
    except Exception:
        return False

preview_node_trajectory 

preview_node_trajectory(node_index: int) -> bool

预览单个节点的轨迹曲线

流程： 1. 获取节点任务 2. 准备预览操作 3. 查询当前位置 4. MessageResponseService 自动发射预览信号

参数：

名称 类型 描述 默认
node_index int

节点索引

 必需

返回：

名称 类型 描述
True bool

准备成功
False bool

准备失败
源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
484
485
486
487
488
489
490
491
492
493
494
495
496
497
498
499
500
501
502
503
504
505
506
507
508
509
510
511
512
513
514
515
516
517
518
519
520
def preview_node_trajectory(self, node_index: int) -> bool:
    """
    预览单个节点的轨迹曲线

    流程：
    1. 获取节点任务
    2. 准备预览操作
    3. 查询当前位置
    4. MessageResponseService 自动发射预览信号

    Args:
        node_index: 节点索引

    Returns:
        True: 准备成功
        False: 准备失败
    """
    try:
        tasks = self._get_node_tasks(node_index)
        if not tasks:
            return False

        context = {
            "type": "node",
            "node_index": node_index
        }

        self.motion_constructor.prepare_operation(
            MotionOperationMode.PREVIEW,
            tasks,
            context
        )

        self.command_hub.get_current_position()
        return True
    except Exception:
        return False

preview_plan_trajectory 

preview_plan_trajectory() -> bool

预览整个方案的轨迹曲线

流程： 1. 获取方案所有任务 2. 准备预览操作 3. 查询当前位置 4. MessageResponseService 自动发射预览信号

返回：

名称 类型 描述
True bool

准备成功
False bool

准备失败
源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
522
523
524
525
526
527
528
529
530
531
532
533
534
535
536
537
538
539
540
541
542
543
544
545
546
547
548
549
550
551
552
def preview_plan_trajectory(self) -> bool:
    """
    预览整个方案的轨迹曲线

    流程：
    1. 获取方案所有任务
    2. 准备预览操作
    3. 查询当前位置
    4. MessageResponseService 自动发射预览信号

    Returns:
        True: 准备成功
        False: 准备失败
    """
    try:
        tasks = self._get_plan_tasks()
        if not tasks:
            return False

        context = {"type": "plan"}

        self.motion_constructor.prepare_operation(
            MotionOperationMode.PREVIEW,
            tasks,
            context
        )

        self.command_hub.get_current_position()
        return True
    except Exception:
        return False

load_local_trajectory 

load_local_trajectory(file_path: str) -> bool

从 plans 目录加载轨迹文件，作为示教节点添加

参数：

名称 类型 描述 默认
file_path str

轨迹文件的完整路径或文件名

 必需

返回：

名称 类型 描述
True bool

加载成功
False bool

加载失败
源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
591
592
593
594
595
596
597
598
599
600
601
602
603
604
605
606
607
608
609
610
611
612
613
614
615
616
617
618
619
620
621
622
623
624
625
626
627
628
629
630
631
632
633
634
635
636
637
638
639
def load_local_trajectory(self, file_path: str) -> bool:
    """
    从 plans 目录加载轨迹文件，作为示教节点添加

    Args:
        file_path: 轨迹文件的完整路径或文件名

    Returns:
        True: 加载成功
        False: 加载失败
    """
    try:
        # 1. 提取文件名（不带路径和扩展名）
        file_stem = Path(file_path).stem

        # 2. 从 TrajectoryRepository 加载
        trajectory_data = self.trajectory_repository.load_trajectory(file_stem)

        # 3. 验证数据
        if not self._validate_trajectory_data(trajectory_data):
            return False

        # 4. 创建示教节点数据（复用示教模式的格式）
        point_data = {
            "mode": f"示教-{file_stem}",
            "joint1": 0.0,
            "joint2": 0.0,
            "joint3": 0.0,
            "joint4": 0.0,
            "joint5": 0.0,
            "joint6": 0.0,
            "frequency": 0.01,
            "curve_type": "S曲线",
            "gripper_command": "00: 不进行任何操作",
            "gripper_param": 0.0,
            "note": f"本地轨迹，共{len(trajectory_data)}个点",
            "teach_record_name": file_stem,
            "teach_data": trajectory_data,
            "source": "local_trajectory"  # 标记来源
        }

        # 5. 添加节点
        self.domain_service.add_point(point_data)
        self._save_data()  # 持久化保存
        self.point_list_changed.emit()
        return True

    except Exception:
        return False

get_local_trajectory_files 

get_local_trajectory_files() -> List[str]

获取所有可用的本地轨迹文件

返回：

类型 描述
List[str]

文件名列表（不带扩展名）
源代码位于： src/controller/controller/application/services/motion_planning_application_service.py 
641
642
643
644
645
646
647
648
def get_local_trajectory_files(self) -> List[str]:
    """
    获取所有可用的本地轨迹文件

    Returns:
        文件名列表（不带扩展名）
    """
    return self.trajectory_repository.list_trajectory_files()

CameraApplicationService 

CameraApplicationService(camera_service: CameraDomainService, recognition_service: RecognitionDomainService, hand_eye_service: HandEyeTransformDomainService, kinematic_service: KinematicDomainService, robot_state_service: RobotStateDomainService, motion_constructor: MotionConstructor, command_hub: CommandHubService, message_display: MessageDisplay)

Bases: QObject

摄像头应用服务。

协调摄像头Domain服务、检测服务和消息显示，提供统一的查询接口，避免ViewModel直接访问Domain层。

属性：

名称 类型 描述
connection_status_changed pyqtSignal

摄像头连接状态变化信号。
detection_status_changed pyqtSignal

检测状态变化信号。

初始化摄像头应用服务。

源代码位于： src/controller/controller/application/services/camera_application_service.py 
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
def __init__(
    self,
    camera_service: CameraDomainService,
    recognition_service: RecognitionDomainService,
    hand_eye_service: HandEyeTransformDomainService,
    kinematic_service: KinematicDomainService,
    robot_state_service: RobotStateDomainService,
    motion_constructor: MotionConstructor,
    command_hub: CommandHubService,
    message_display: MessageDisplay
):
    """初始化摄像头应用服务。"""
    super().__init__()
    self.camera_service = camera_service
    self.recognition_service = recognition_service
    self.hand_eye_service = hand_eye_service
    self.kinematic_service = kinematic_service
    self.robot_state_service = robot_state_service
    self.motion_constructor = motion_constructor
    self.command_hub = command_hub
    self.message_display = message_display

    # 连接Domain Service信号
    self._connect_signals()

connect_camera 

connect_camera()

连接摄像头。

源代码位于： src/controller/controller/application/services/camera_application_service.py 
64
65
66
67
68
69
70
71
72
73
74
75
76
def connect_camera(self):
    """连接摄像头。"""
    self.message_display.clear_messages()
    self._display_message("正在连接摄像头...", "摄像头")

    success = self.camera_service.connect()

    if success:
        self._display_message("摄像头连接成功", "摄像头")
        self.connection_status_changed.emit(True)
    else:
        self._display_message("摄像头连接失败", "错误")
        self.connection_status_changed.emit(False)

disconnect_camera 

disconnect_camera()

断开摄像头。

源代码位于： src/controller/controller/application/services/camera_application_service.py 
78
79
80
81
82
83
84
85
86
87
88
89
def disconnect_camera(self):
    """断开摄像头。"""
    self.message_display.clear_messages()
    self._display_message("正在断开摄像头...", "摄像头")

    success = self.camera_service.disconnect()

    if success:
        self._display_message("摄像头已断开", "摄像头")
        self.connection_status_changed.emit(False)
    else:
        self._display_message("断开摄像头失败", "错误")

start_detection 

start_detection()

开始检测。

源代码位于： src/controller/controller/application/services/camera_application_service.py 
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
def start_detection(self):
    """开始检测。"""
    self.message_display.clear_messages()
    self._display_message("正在启动检测...", "检测")

    success = self.recognition_service.start_detection()

    if success:
        self._display_message("开始零件识别", "检测")
        self.detection_status_changed.emit(True)
    else:
        self._display_message("启动检测失败", "错误")
        self.detection_status_changed.emit(False)

stop_detection 

stop_detection()

停止检测。

源代码位于： src/controller/controller/application/services/camera_application_service.py 
105
106
107
108
109
110
111
112
113
114
115
116
def stop_detection(self):
    """停止检测。"""
    self.message_display.clear_messages()
    self._display_message("正在停止检测...", "检测")

    success = self.recognition_service.stop_detection()

    if success:
        self._display_message("停止零件识别", "检测")
        self.detection_status_changed.emit(False)
    else:
        self._display_message("停止检测失败", "错误")

move_to_detected_part 

move_to_detected_part()

运动到检测到的零件位置。

完整流程： 1. 检查检测状态 2. 获取检测结果 3. 获取当前关节角度 4. 手眼标定计算目标关节角度 5. 构建运动任务 6. 触发运动执行 7. 停止检测（可选）

源代码位于： src/controller/controller/application/services/camera_application_service.py 
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
def move_to_detected_part(self):
    """运动到检测到的零件位置。

    完整流程：
    1. 检查检测状态
    2. 获取检测结果
    3. 获取当前关节角度
    4. 手眼标定计算目标关节角度
    5. 构建运动任务
    6. 触发运动执行
    7. 停止检测（可选）
    """
    # 1. 检查检测是否运行
    if not self.recognition_service.is_detection_running():
        self._display_message("检测未运行，无法执行运动", "错误")
        return

    # 2. 获取最新检测结果
    detection_result = self.recognition_service.get_latest_result()
    if not detection_result:
        self._display_message("未检测到零件，无法执行运动", "警告")
        return

    # 3. 获取当前关节角度
    current_state = self.robot_state_service.get_current_state()
    if not current_state:
        self._display_message("无法获取当前机器人状态", "错误")
        return

    current_joint_angles = current_state.joint_angles  # ✅ 修复：正确的属性名（弧度）

    # 4. 手眼标定计算目标关节角度
    try:
        target_angles = self.hand_eye_service.calculate_target_joint_angles(
            central_center=detection_result['central_center'],
            depth=detection_result['depth'],
            real_center=detection_result['real_center'],
            real_depth=detection_result['real_depth'],
            angle=detection_result['angle'],
            current_joint_angles=current_joint_angles
        )

        if target_angles is None:
            self._display_message("逆运动学无解，无法到达目标位置", "错误")
            return

    except Exception as e:
        self._display_message(f"计算目标位姿失败: {str(e)}", "错误")
        return

    # 5. 构建运动任务
    motion_task = {
        'type': 'motion',
        'target_angles': target_angles,
        'curve_type': 's_curve',  # 's_curve' 或 'linear'（笛卡尔直线）
        'frequency': 0.01
    }

    # 6. 准备运动并触发执行
    try:
        # 使用统一的 prepare_operation 接口
        self.motion_constructor.prepare_operation(
            MotionOperationMode.EXECUTE,
            [motion_task]  # 需要传入任务列表
        )
        self.command_hub.get_current_position()  # 触发运动执行

        self._display_message(
            f"开始运动到零件位置 "
            f"(中心: {detection_result['central_center']}, "
            f"深度: {detection_result['depth']:.2f}mm, "
            f"角度: {np.degrees(detection_result['angle']):.1f}°)",
            "运动"
        )

    except Exception as e:
        self._display_message(f"启动运动失败: {str(e)}", "错误")
        return

is_camera_connected 

is_camera_connected() -> bool

检查摄像头是否连接。

返回：

名称 类型 描述
bool bool

True=已连接, False=未连接。
源代码位于： src/controller/controller/application/services/camera_application_service.py 
272
273
274
275
276
277
278
def is_camera_connected(self) -> bool:
    """检查摄像头是否连接。

    Returns:
        bool: True=已连接, False=未连接。
    """
    return self.camera_service.is_connected

is_color_available 

is_color_available() -> bool

检查彩色图像是否可用。

返回：

名称 类型 描述
bool bool

True=有数据, False=无数据。
源代码位于： src/controller/controller/application/services/camera_application_service.py 
280
281
282
283
284
285
286
def is_color_available(self) -> bool:
    """检查彩色图像是否可用。

    Returns:
        bool: True=有数据, False=无数据。
    """
    return self.camera_service.is_color_available()

is_depth_available 

is_depth_available() -> bool

检查深度图像是否可用。

返回：

名称 类型 描述
bool bool

True=有数据, False=无数据。
源代码位于： src/controller/controller/application/services/camera_application_service.py 
288
289
290
291
292
293
294
def is_depth_available(self) -> bool:
    """检查深度图像是否可用。

    Returns:
        bool: True=有数据, False=无数据。
    """
    return self.camera_service.is_depth_available()

get_latest_color_image 

get_latest_color_image() -> Optional[np.ndarray]

获取最新彩色图像。

返回：

类型 描述
Optional[ndarray]

Optional[np.ndarray]: 彩色图像（BGR格式），如果无数据则返回None。
源代码位于： src/controller/controller/application/services/camera_application_service.py 
296
297
298
299
300
301
302
def get_latest_color_image(self) -> Optional[np.ndarray]:
    """获取最新彩色图像。

    Returns:
        Optional[np.ndarray]: 彩色图像（BGR格式），如果无数据则返回None。
    """
    return self.camera_service.get_latest_color_image()

get_latest_depth_image 

get_latest_depth_image() -> Optional[np.ndarray]

获取最新深度图像。

返回：

类型 描述
Optional[ndarray]

Optional[np.ndarray]: 深度图像（16位或32位浮点），如果无数据则返回None。
源代码位于： src/controller/controller/application/services/camera_application_service.py 
304
305
306
307
308
309
310
def get_latest_depth_image(self) -> Optional[np.ndarray]:
    """获取最新深度图像。

    Returns:
        Optional[np.ndarray]: 深度图像（16位或32位浮点），如果无数据则返回None。
    """
    return self.camera_service.get_latest_depth_image()

visualize_depth_image 

visualize_depth_image(depth_image: ndarray) -> np.ndarray

深度图可视化为伪彩色图。

参数：

名称 类型 描述 默认
depth_image ndarray

原始深度图（16位或32位浮点）。

 必需

返回：

类型 描述
ndarray

np.ndarray: 伪彩色深度图（BGR, uint8）。
源代码位于： src/controller/controller/application/services/camera_application_service.py 
312
313
314
315
316
317
318
319
320
321
def visualize_depth_image(self, depth_image: np.ndarray) -> np.ndarray:
    """深度图可视化为伪彩色图。

    Args:
        depth_image (np.ndarray): 原始深度图（16位或32位浮点）。

    Returns:
        np.ndarray: 伪彩色深度图（BGR, uint8）。
    """
    return self.camera_service.visualize_depth_image(depth_image)

is_detection_running 

is_detection_running() -> bool

检查检测是否正在运行。

返回：

名称 类型 描述
bool bool

True=运行中, False=未运行。
源代码位于： src/controller/controller/application/services/camera_application_service.py 
323
324
325
326
327
328
329
def is_detection_running(self) -> bool:
    """检查检测是否正在运行。

    Returns:
        bool: True=运行中, False=未运行。
    """
    return self.recognition_service.is_detection_running()

get_latest_detection_result 

get_latest_detection_result() -> Optional[Dict]

获取最新检测结果。

返回：

类型 描述
Optional[Dict]

Optional[Dict]: 检测结果字典，如果无数据则返回None。 包含字段：head_center, central_center, real_center, angle, depth, real_depth。
源代码位于： src/controller/controller/application/services/camera_application_service.py 
331
332
333
334
335
336
337
338
def get_latest_detection_result(self) -> Optional[Dict]:
    """获取最新检测结果。

    Returns:
        Optional[Dict]: 检测结果字典，如果无数据则返回None。
            包含字段：head_center, central_center, real_center, angle, depth, real_depth。
    """
    return self.recognition_service.get_latest_result()

get_image_with_detection 

get_image_with_detection(image: ndarray) -> np.ndarray

获取叠加了检测结果的图像。

封装了检测状态检查和图像绘制逻辑，提供高层业务接口。

参数：

名称 类型 描述 默认
image ndarray

原始图像（BGR格式）。

 必需

返回：

类型 描述
ndarray

np.ndarray: 如果检测正在运行且有结果，返回叠加了检测标注的图像； 否则返回原图像。
源代码位于： src/controller/controller/application/services/camera_application_service.py 
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
def get_image_with_detection(self, image: np.ndarray) -> np.ndarray:
    """获取叠加了检测结果的图像。

    封装了检测状态检查和图像绘制逻辑，提供高层业务接口。

    Args:
        image (np.ndarray): 原始图像（BGR格式）。

    Returns:
        np.ndarray: 如果检测正在运行且有结果，返回叠加了检测标注的图像；
                   否则返回原图像。
    """
    # 检查检测是否运行
    if self.recognition_service.is_detection_running():
        # 获取最新检测结果
        detection = self.recognition_service.get_latest_result()
        if detection:
            # 绘制检测结果到图像上
            return ImageDrawingUtils.draw_detection_result(image, detection)

    # 无检测或无结果，返回原图
    return image

ToolsApplicationService 

ToolsApplicationService(kinematic_service: KinematicDomainService, robot_state_service: RobotStateDomainService)

Bases: QObject

工具应用服务。

职责： 1. 接收 6 个关节角度 2. 调用 KinematicDomainService 计算正运动学 3. 格式化结果并发射信号 4. 获取当前机械臂关节角度 5. 计算逆运动学

属性：

名称 类型 描述
calculation_result_signal pyqtSignal

发送正运动学计算结果。
current_angles_signal pyqtSignal

发送当前关节角度。
inverse_result_signal pyqtSignal

发送逆运动学计算结果。

初始化工具应用服务。

参数：

名称 类型 描述 默认
kinematic_service KinematicDomainService

运动学服务。

 必需
robot_state_service RobotStateDomainService

机械臂状态服务。

 必需
源代码位于： src/controller/controller/application/services/tools_application_service.py 
31
32
33
34
35
36
37
38
39
40
def __init__(self, kinematic_service: KinematicDomainService, robot_state_service: RobotStateDomainService):
    """初始化工具应用服务。

    Args:
        kinematic_service (KinematicDomainService): 运动学服务。
        robot_state_service (RobotStateDomainService): 机械臂状态服务。
    """
    super().__init__()
    self.kinematic_service = kinematic_service
    self.robot_state_service = robot_state_service

calculate_forward_kinematics 

calculate_forward_kinematics(joint_angles: List[float])

计算正运动学。

参数：

名称 类型 描述 默认
joint_angles List[float]

6个关节角度（弧度）。

 必需
Emits

calculation_result_signal 包含： { "quaternion": [x, y, z, w], "position": [x, y, z], "rotation_matrix": [[...], [...], [...]] # 3x3 } 或错误信息： { "error": "错误信息" }

源代码位于： src/controller/controller/application/services/tools_application_service.py 
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
def calculate_forward_kinematics(self, joint_angles: List[float]):
    """计算正运动学。

    Args:
        joint_angles (List[float]): 6个关节角度（弧度）。

    Emits:
        calculation_result_signal 包含：
        {
            "quaternion": [x, y, z, w],
            "position": [x, y, z],
            "rotation_matrix": [[...], [...], [...]]  # 3x3
        }
        或错误信息：
        {
            "error": "错误信息"
        }
    """
    try:
        # 获取四元数和位置
        quat, pos = self.kinematic_service.get_gripper2base(joint_angles)

        # 获取完整变换矩阵
        transform_matrix = self.kinematic_service.get_gripper2base_rm(joint_angles)
        rotation_matrix = transform_matrix[:3, :3]  # 提取旋转矩阵部分

        # 构建结果字典
        result = {
            "quaternion": quat.tolist(),  # [x, y, z, w]
            "position": pos.tolist(),     # [x, y, z]
            "rotation_matrix": rotation_matrix.tolist()  # 3x3 list
        }

        # 发射信号
        self.calculation_result_signal.emit(result)

    except Exception as e:
        # 错误处理
        error_result = {
            "error": f"计算失败: {str(e)}"
        }
        self.calculation_result_signal.emit(error_result)

get_current_joint_angles 

get_current_joint_angles()

获取当前机械臂关节角度。

Emits

current_angles_signal: List[float] - 6个关节角度（弧度）。

源代码位于： src/controller/controller/application/services/tools_application_service.py 
85
86
87
88
89
90
91
92
93
94
95
96
97
98
def get_current_joint_angles(self):
    """获取当前机械臂关节角度。

    Emits:
        current_angles_signal: List[float] - 6个关节角度（弧度）。
    """
    try:
        # 从机械臂状态服务获取当前角度
        current_angles = self.robot_state_service.get_current_angles()
        # 发射信号
        self.current_angles_signal.emit(current_angles)
    except Exception as e:
        # 如果获取失败，发送空列表
        self.current_angles_signal.emit([0.0] * 6)

calculate_inverse_kinematics 

calculate_inverse_kinematics(rotation_matrix: List[List[float]], position: List[float], initial_theta: List[float] = None)

计算逆运动学。

参数：

名称 类型 描述 默认
rotation_matrix List[List[float]]

3x3 旋转矩阵。

 必需
position List[float]

[x, y, z] 位置（米）。

 必需
initial_theta List[float]

初始关节角度（用于选择最接近的解）. Defaults to None.

 None
Emits

inverse_result_signal 包含： { "joint_angles": [θ1, θ2, θ3, θ4, θ5, θ6], # 弧度 "joint_angles_deg": [θ1, θ2, θ3, θ4, θ5, θ6] # 度 } 或错误信息： { "error": "错误信息" }

源代码位于： src/controller/controller/application/services/tools_application_service.py 
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
def calculate_inverse_kinematics(self, rotation_matrix: List[List[float]], position: List[float], initial_theta: List[float] = None):
    """计算逆运动学。

    Args:
        rotation_matrix (List[List[float]]): 3x3 旋转矩阵。
        position (List[float]): [x, y, z] 位置（米）。
        initial_theta (List[float], optional): 初始关节角度（用于选择最接近的解）. Defaults to None.

    Emits:
        inverse_result_signal 包含：
        {
            "joint_angles": [θ1, θ2, θ3, θ4, θ5, θ6],  # 弧度
            "joint_angles_deg": [θ1, θ2, θ3, θ4, θ5, θ6]  # 度
        }
        或错误信息：
        {
            "error": "错误信息"
        }
    """
    try:
        # 转换为 numpy 数组
        rm = np.array(rotation_matrix)
        pos = np.array(position)

        # 如果没有提供初始角度，使用当前角度
        if initial_theta is None:
            initial_theta = self.robot_state_service.get_current_angles()

        # 调用逆运动学求解
        solution = self.kinematic_service.inverse_kinematic(rm, pos, initial_theta)

        # 构建结果字典
        result = {
            "joint_angles": solution,  # 弧度
            "joint_angles_deg": [np.rad2deg(angle) for angle in solution]  # 度
        }

        # 发射信号
        self.inverse_result_signal.emit(result)

    except Exception as e:
        # 错误处理
        error_result = {
            "error": f"逆解计算失败: {str(e)}"
        }
        self.inverse_result_signal.emit(error_result)

MessageDisplay 

MessageDisplay()

Bases: QObject

消息显示服务。

负责在界面上显示日志和状态消息，通过信号分发给 UI 组件。

属性：

名称 类型 描述
message_display_signal pyqtSignal

消息显示信号，携带 (消息内容, 消息类型)。
clear_requested pyqtSignal

清除消息请求信号。

初始化消息显示服务。

源代码位于： src/controller/controller/application/commands/message_display.py 
17
18
19
def __init__(self):
    """初始化消息显示服务。"""
    super().__init__()

display_message 

display_message(message: str, message_type: str) -> None

显示一条消息。

参数：

名称 类型 描述 默认
message str

消息内容。

 必需
message_type str

消息类型（如 "info", "error", "warning"）。

 必需
源代码位于： src/controller/controller/application/commands/message_display.py 
21
22
23
24
25
26
27
28
def display_message(self, message: str, message_type: str) -> None:
    """显示一条消息。

    Args:
        message (str): 消息内容。
        message_type (str): 消息类型（如 "info", "error", "warning"）。
    """
    self.message_display_signal.emit(message, message_type)

clear_messages 

clear_messages() -> None

清除所有消息。

源代码位于： src/controller/controller/application/commands/message_display.py 
30
31
32
def clear_messages(self) -> None:
    """清除所有消息。"""
    self.clear_requested.emit()

MotionListener 

MotionListener(motion_runner: MotionRunner, message_display: MessageDisplay)

运动消息监听器。

负责监听运动执行器发送的消息，解码位置数据并显示。

属性：

名称 类型 描述
motion_runner MotionRunner

运动执行器。
message_display MessageDisplay

消息显示服务。
robot_utils RobotUtils

机器人工具类，用于坐标转换。

初始化运动监听器。

参数：

名称 类型 描述 默认
motion_runner MotionRunner

运动执行器。

 必需
message_display MessageDisplay

消息显示服务。

 必需
源代码位于： src/controller/controller/application/listener/motion_listener.py 
16
17
18
19
20
21
22
23
24
25
26
def __init__(self, motion_runner: MotionRunner, message_display: MessageDisplay):
    """初始化运动监听器。

    Args:
        motion_runner (MotionRunner): 运动执行器。
        message_display (MessageDisplay): 消息显示服务。
    """
    self.motion_runner = motion_runner
    self.message_display = message_display
    self.robot_utils = RobotUtils()
    self._connect_signals()

handle_motion_msg 

handle_motion_msg(message, message_type)

处理运动消息，手动解析位置数据。

消息格式：AA55 + control(1) + mode(1) + joint_angles(24) + ... + CRC(2) + 0D0A 编码过程：弧度 -> radian2position -> 位置值(32位) -> 大端序16进制

参数：

名称 类型 描述 默认
message str

原始16进制消息字符串。

 必需
message_type str

消息类型。

 必需
源代码位于： src/controller/controller/application/listener/motion_listener.py 
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
def handle_motion_msg(self, message, message_type):
    """处理运动消息，手动解析位置数据。

    消息格式：AA55 + control(1) + mode(1) + joint_angles(24) + ... + CRC(2) + 0D0A
    编码过程：弧度 -> radian2position -> 位置值(32位) -> 大端序16进制

    Args:
        message (str): 原始16进制消息字符串。
        message_type (str): 消息类型。
    """
    try:
        # 手动解析位置数据
        joint_angles = self._extract_joint_angles(message)

        if joint_angles:
            # 格式化显示弧度值
            angles_str = ", ".join([f"{angle:.4f}" for angle in joint_angles])
            display_text = f"[{message_type}] 位置(弧度): [{angles_str}]"
            self.message_display.display_message(display_text, message_type)

            # 同时显示原始16进制数据
            raw_text = f"[{message_type}] 原始数据: {message}"
            self.message_display.display_message(raw_text, message_type)
        else:
            # 无法解析，只显示原始消息
            self.message_display.display_message(str(message), message_type)

    except Exception as e:
        # 解析失败，显示原始消息
        self.message_display.display_message(str(message), message_type)

BaseViewModel 

BaseViewModel(parent=None)

Bases: QObject

基础视图模型类。

所有视图模型的基类，提供通用的信号和清理接口。

属性：

名称 类型 描述
connection_status_changed pyqtSignal

连接状态变更信号，携带 bool 状态。

初始化基础视图模型。

参数：

名称 类型 描述 默认
parent QObject

父对象. Defaults to None.

 None
源代码位于： src/controller/controller/presentation/view_models/base_view_model.py 
17
18
19
20
21
22
23
def __init__(self, parent=None):
    """初始化基础视图模型。

    Args:
        parent (QObject, optional): 父对象. Defaults to None.
    """
    super().__init__(parent)

cleanup 

cleanup()

清理资源。

子类应重写此方法以释放资源（如关闭连接、停止定时器等）。

源代码位于： src/controller/controller/presentation/view_models/base_view_model.py 
25
26
27
28
29
30
def cleanup(self):
    """清理资源。

    子类应重写此方法以释放资源（如关闭连接、停止定时器等）。
    """
    pass

MainViewModel 

MainViewModel(serial_vm: SerialViewModel, display_vm: DisplayViewModel, control_vm: ControlViewModel, status_vm: StatusViewModel, effector_vm: EffectorViewModel, trajectory_vm: TrajectoryViewModel, dynamics_vm: DynamicsViewModel, camera_vm: CameraViewModel, motion_planning_vm: MotionPlanningViewModel, tools_vm: ToolsViewModel, parent=None)

Bases: BaseViewModel

主视图模型。

整合所有子视图模型，并管理它们之间的信号交互。

属性：

名称 类型 描述
connection_status_changed pyqtSignal

串口连接状态信号。
status_message_changed pyqtSignal

状态消息变更信号。
progress_changed pyqtSignal

进度变更信号。
serial_vm SerialViewModel

串口视图模型。
display_vm DisplayViewModel

显示视图模型。
control_vm ControlViewModel

控制视图模型。
status_vm StatusViewModel

状态视图模型。
effector_vm EffectorViewModel

末端执行器视图模型。
trajectory_vm TrajectoryViewModel

轨迹视图模型。
dynamics_vm DynamicsViewModel

动力学视图模型。
camera_vm CameraViewModel

相机视图模型。
motion_planning_vm MotionPlanningViewModel

运动规划视图模型。
tools_vm ToolsViewModel

工具视图模型。

初始化主视图模型。

参数：

名称 类型 描述 默认
serial_vm SerialViewModel

注入的串口 VM。

 必需
display_vm DisplayViewModel

注入的显示 VM。

 必需
control_vm ControlViewModel

注入的控制 VM。

 必需
status_vm StatusViewModel

注入的状态 VM。

 必需
effector_vm EffectorViewModel

注入的末端 VM。

 必需
trajectory_vm TrajectoryViewModel

注入的轨迹 VM。

 必需
dynamics_vm DynamicsViewModel

注入的动力学 VM。

 必需
camera_vm CameraViewModel

注入的相机 VM。

 必需
motion_planning_vm MotionPlanningViewModel

注入的规划 VM。

 必需
tools_vm ToolsViewModel

注入的工具 VM。

 必需
parent QObject

父对象. Defaults to None.

 None
源代码位于： src/controller/controller/presentation/view_models/main_view_model.py 
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
def __init__(
    self, 
    serial_vm: SerialViewModel, 
    display_vm: DisplayViewModel, 
    control_vm: ControlViewModel, 
    status_vm: StatusViewModel,
    effector_vm: EffectorViewModel,
    trajectory_vm: TrajectoryViewModel,
    dynamics_vm: DynamicsViewModel,
    camera_vm: CameraViewModel,
    motion_planning_vm: MotionPlanningViewModel,
    tools_vm: ToolsViewModel,
    parent=None
):
    """初始化主视图模型。

    Args:
        serial_vm (SerialViewModel): 注入的串口 VM。
        display_vm (DisplayViewModel): 注入的显示 VM。
        control_vm (ControlViewModel): 注入的控制 VM。
        status_vm (StatusViewModel): 注入的状态 VM。
        effector_vm (EffectorViewModel): 注入的末端 VM。
        trajectory_vm (TrajectoryViewModel): 注入的轨迹 VM。
        dynamics_vm (DynamicsViewModel): 注入的动力学 VM。
        camera_vm (CameraViewModel): 注入的相机 VM。
        motion_planning_vm (MotionPlanningViewModel): 注入的规划 VM。
        tools_vm (ToolsViewModel): 注入的工具 VM。
        parent (QObject, optional): 父对象. Defaults to None.
    """
    super().__init__(parent)
    self.serial_vm = serial_vm
    self.display_vm = display_vm
    self.control_vm = control_vm
    self.status_vm = status_vm
    self.effector_vm = effector_vm
    self.trajectory_vm = trajectory_vm
    self.dynamics_vm = dynamics_vm
    self.camera_vm = camera_vm
    self.motion_planning_vm = motion_planning_vm
    self.tools_vm = tools_vm

    # 连接连接状态信号
    self._connect_status_update_signals()

cleanup 

cleanup()

清理资源。

调用所有子 ViewModel 的 cleanup 方法，确保资源释放。

源代码位于： src/controller/controller/presentation/view_models/main_view_model.py 
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
def cleanup(self):
    """清理资源。

    调用所有子 ViewModel 的 cleanup 方法，确保资源释放。
    """
    # 清理所有子ViewModels
    for vm_attr in ['serial_vm', 'display_vm', 'control_vm', 'status_vm',
                   'effector_vm', 'trajectory_vm', 'dynamics_vm', 'camera_vm',
                   'motion_planning_vm', 'tools_vm']:
        vm = getattr(self, vm_attr, None)
        if vm and hasattr(vm, 'cleanup'):
            try:
                vm.cleanup()
            except Exception as e:
                pass

    super().cleanup()

SerialViewModel 

SerialViewModel(serial_service: SerialApplicationService, command_hub_service: CommandHubService, parent=None)

Bases: BaseViewModel

串口视图模型。

纯信号转发层，不包含业务逻辑，负责 UI 与串口服务的交互。

属性：

名称 类型 描述
port_list_updated pyqtSignal

端口列表更新信号，携带端口列表。
serial_service SerialApplicationService

串口应用服务。
command_hub_service CommandHubService

命令中心服务。

初始化视图模型。

参数：

名称 类型 描述 默认
serial_service SerialApplicationService

串口应用服务。

 必需
command_hub_service CommandHubService

命令中心服务。

 必需
parent QObject

父对象. Defaults to None.

 None
源代码位于： src/controller/controller/presentation/view_models/serial_view_model.py 
24
25
26
27
28
29
30
31
32
33
34
35
36
def __init__(self, serial_service: SerialApplicationService, command_hub_service: CommandHubService, parent=None):
    """初始化视图模型。

    Args:
        serial_service (SerialApplicationService): 串口应用服务。
        command_hub_service (CommandHubService): 命令中心服务。
        parent (QObject, optional): 父对象. Defaults to None.
    """
    super().__init__(parent)

    self.serial_service = serial_service
    self.command_hub_service = command_hub_service
    self._connect_service_signals()

refresh_ports 

refresh_ports() -> None

刷新端口列表命令 - 直接转发到Service。

源代码位于： src/controller/controller/presentation/view_models/serial_view_model.py 
40
41
42
def refresh_ports(self) -> None:
    """刷新端口列表命令 - 直接转发到Service。"""
    self.serial_service.refresh_ports()

connect_serial 

connect_serial(port: str, config: Dict[str, Any]) -> None

连接串口命令 - 直接转发到Service。

参数：

名称 类型 描述 默认
port str

端口名称。

 必需
config Dict[str, Any]

配置参数。

 必需
源代码位于： src/controller/controller/presentation/view_models/serial_view_model.py 
44
45
46
47
48
49
50
51
def connect_serial(self, port: str, config: Dict[str, Any]) -> None:
    """连接串口命令 - 直接转发到Service。

    Args:
        port (str): 端口名称。
        config (Dict[str, Any]): 配置参数。
    """
    self.serial_service.connect_serial(port, config)

disconnect_serial 

disconnect_serial() -> None

断开连接命令 - 直接转发到Service。

源代码位于： src/controller/controller/presentation/view_models/serial_view_model.py 
53
54
55
def disconnect_serial(self) -> None:
    """断开连接命令 - 直接转发到Service。"""
    self.serial_service.disconnect_serial()

get_available_ports 

get_available_ports() -> List[str]

获取可用端口列表 - 委托给Service。

返回：

类型 描述
List[str]

List[str]: 端口列表。
源代码位于： src/controller/controller/presentation/view_models/serial_view_model.py 
59
60
61
62
63
64
65
def get_available_ports(self) -> List[str]:
    """获取可用端口列表 - 委托给Service。

    Returns:
        List[str]: 端口列表。
    """
    return self.serial_service.get_available_ports()

get_current_port 

get_current_port() -> Optional[str]

获取当前连接的端口 - 委托给Service。

返回：

类型 描述
Optional[str]

Optional[str]: 端口名称。
源代码位于： src/controller/controller/presentation/view_models/serial_view_model.py 
67
68
69
70
71
72
73
def get_current_port(self) -> Optional[str]:
    """获取当前连接的端口 - 委托给Service。

    Returns:
        Optional[str]: 端口名称。
    """
    return self.serial_service.get_current_port()

get_connection_status 

get_connection_status() -> bool

获取连接状态 - 委托给Service。

返回：

名称 类型 描述
bool bool

是否已连接。
源代码位于： src/controller/controller/presentation/view_models/serial_view_model.py 
75
76
77
78
79
80
81
def get_connection_status(self) -> bool:
    """获取连接状态 - 委托给Service。

    Returns:
        bool: 是否已连接。
    """
    return self.serial_service.is_connected()

get_port_info 

get_port_info(port_name: str) -> Optional[Dict[str, str]]

获取端口信息 - 委托给Service。

参数：

名称 类型 描述 默认
port_name str

端口名称。

 必需

返回：

类型 描述
Optional[Dict[str, str]]

Optional[Dict[str, str]]: 端口信息。
源代码位于： src/controller/controller/presentation/view_models/serial_view_model.py 
83
84
85
86
87
88
89
90
91
92
def get_port_info(self, port_name: str) -> Optional[Dict[str, str]]:
    """获取端口信息 - 委托给Service。

    Args:
        port_name (str): 端口名称。

    Returns:
        Optional[Dict[str, str]]: 端口信息。
    """
    return self.serial_service.get_port_info(port_name)

cleanup 

cleanup() -> None

清理视图模型 - 断开信号连接。

源代码位于： src/controller/controller/presentation/view_models/serial_view_model.py 
110
111
112
113
def cleanup(self) -> None:
    """清理视图模型 - 断开信号连接。"""
    self._disconnect_service_signals()
    super().cleanup()

DisplayViewModel 

DisplayViewModel(message_display: MessageDisplay, parent=None)

Bases: BaseViewModel

显示视图模型。

作为消息显示的中心分发器，其他服务调用它的方法来显示消息。

属性：

名称 类型 描述
message_display_signal pyqtSignal

消息显示信号，携带 (消息内容, 消息类型)。
clear_requested pyqtSignal

清除消息请求信号。
message_display MessageDisplay

消息显示服务。

初始化显示视图模型。

参数：

名称 类型 描述 默认
message_display MessageDisplay

消息显示服务。

 必需
parent QObject

父对象. Defaults to None.

 None
源代码位于： src/controller/controller/presentation/view_models/display_view_model.py 
27
28
29
30
31
32
33
34
35
36
37
def __init__(self, message_display: MessageDisplay, parent=None):
    """初始化显示视图模型。

    Args:
        message_display (MessageDisplay): 消息显示服务。
        parent (QObject, optional): 父对象. Defaults to None.
    """
    super().__init__(parent)
    self.message_display = message_display
    self.message_display.message_display_signal.connect(self.message_display_signal.emit)
    self.message_display.clear_requested.connect(self.clear_requested.emit)

clear_messages 

clear_messages() -> None

清除所有消息。

发送清除消息信号。

源代码位于： src/controller/controller/presentation/view_models/display_view_model.py 
39
40
41
42
43
44
45
def clear_messages(self) -> None:
    """清除所有消息。

    发送清除消息信号。
    """
    # 发送清除消息信号
    self.clear_requested.emit()

cleanup 

cleanup() -> None

清理资源。

源代码位于： src/controller/controller/presentation/view_models/display_view_model.py 
47
48
49
def cleanup(self) -> None:
    """清理资源。"""
    super().cleanup()

ControlViewModel 

ControlViewModel(command_hub_service: CommandHubService, parent=None)

Bases: BaseViewModel

控制视图模型。

负责处理用户输入的控制命令，并分发到命令中心服务。

属性：

名称 类型 描述
command_hub_service CommandHubService

命令中心服务。

初始化控制视图模型。

参数：

名称 类型 描述 默认
command_hub_service CommandHubService

命令中心服务。

 必需
parent QObject

父对象. Defaults to None.

 None
源代码位于： src/controller/controller/presentation/view_models/control_view_model.py 
15
16
17
18
19
20
21
22
23
def __init__(self, command_hub_service: CommandHubService, parent=None):
    """初始化控制视图模型。

    Args:
        command_hub_service (CommandHubService): 命令中心服务。
        parent (QObject, optional): 父对象. Defaults to None.
    """
    super().__init__(parent)
    self.command_hub_service = command_hub_service

send_command 

send_command(config_dict: dict)

发送控制命令。

参数：

名称 类型 描述 默认
config_dict dict

命令配置字典，包含控制参数。

 必需
源代码位于： src/controller/controller/presentation/view_models/control_view_model.py 
25
26
27
28
29
30
31
def send_command(self, config_dict: dict):
    """发送控制命令。

    Args:
        config_dict (dict): 命令配置字典，包含控制参数。
    """
    self.command_hub_service.command_distribution(config_dict)

StatusViewModel 

StatusViewModel(robot_state_service: RobotStateDomainService, parent=None)

Bases: BaseViewModel

状态视图模型。

管理机器人状态显示，订阅 RobotStateDomainService 的状态更新，不再自己维护状态。

职责： 1. 订阅 RobotStateDomainService 的状态更新 2. 转换数据格式（转换为 dict 供 UI 组件使用） 3. 发射信号给 UI 组件

属性：

名称 类型 描述
status_updated pyqtSignal

状态更新信号，携带状态数据字典。
robot_state_service RobotStateDomainService

机械臂状态服务。

初始化状态视图模型。

参数：

名称 类型 描述 默认
robot_state_service RobotStateDomainService

机械臂状态服务（依赖注入）。

 必需
parent QObject

父对象. Defaults to None.

 None
源代码位于： src/controller/controller/presentation/view_models/status_view_model.py 
28
29
30
31
32
33
34
35
36
37
38
39
def __init__(self, robot_state_service: RobotStateDomainService, parent=None):
    """初始化状态视图模型。

    Args:
        robot_state_service (RobotStateDomainService): 机械臂状态服务（依赖注入）。
        parent (QObject, optional): 父对象. Defaults to None.
    """
    super().__init__(parent)
    self.robot_state_service = robot_state_service

    # 订阅状态服务的信号
    self.robot_state_service.state_updated.connect(self._on_state_updated)

get_position_string 

get_position_string() -> str

获取位置信息的字符串表示。

返回：

名称 类型 描述
str str

位置字符串，如 "[0.50, -1.20, 0.30, 0.00, 0.00, 0.00]"。如果无数据返回 "--"。
源代码位于： src/controller/controller/presentation/view_models/status_view_model.py 
66
67
68
69
70
71
72
73
74
75
76
def get_position_string(self) -> str:
    """获取位置信息的字符串表示。

    Returns:
        str: 位置字符串，如 "[0.50, -1.20, 0.30, 0.00, 0.00, 0.00]"。如果无数据返回 "--"。
    """
    snapshot = self.robot_state_service.get_current_state()
    if snapshot:
        positions = snapshot.joint_angles
        return f"[{', '.join(f'{pos:.3f}' for pos in positions[:6])}]"
    return "--"

get_status_summary 

get_status_summary() -> str

获取状态摘要。

返回：

名称 类型 描述
str str

状态摘要字符串，如 "CMD:0x06 | MODE:0x08 | STA:0x01"。如果无数据返回 "无状态数据"。
源代码位于： src/controller/controller/presentation/view_models/status_view_model.py 
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
def get_status_summary(self) -> str:
    """获取状态摘要。

    Returns:
        str: 状态摘要字符串，如 "CMD:0x06 | MODE:0x08 | STA:0x01"。如果无数据返回 "无状态数据"。
    """
    snapshot = self.robot_state_service.get_current_state()
    if not snapshot:
        return "无状态数据"

    summary = []
    summary.append(f"CMD:0x{snapshot.control:02X}")
    summary.append(f"MODE:0x{snapshot.mode:02X}")
    summary.append(f"STA:0x{snapshot.init_status:02X}")

    # 检查错误
    if any(err != 0 for err in snapshot.errors):
        summary.append(f"ERR:有错误")

    return " | ".join(summary)

EffectorViewModel 

EffectorViewModel(command_hub_service: CommandHubService, parent=None)

Bases: BaseViewModel

末端执行器视图模型。

负责处理末端执行器（如夹爪、吸盘）的控制命令。

属性：

名称 类型 描述
command_hub_service CommandHubService

命令中心服务。

初始化末端执行器视图模型。

参数：

名称 类型 描述 默认
command_hub_service CommandHubService

命令中心服务。

 必需
parent QObject

父对象. Defaults to None.

 None
源代码位于： src/controller/controller/presentation/view_models/effector_view_model.py 
17
18
19
20
21
22
23
24
25
def __init__(self, command_hub_service: CommandHubService, parent=None):
    """初始化末端执行器视图模型。

    Args:
        command_hub_service (CommandHubService): 命令中心服务。
        parent (QObject, optional): 父对象. Defaults to None.
    """
    super().__init__(parent)
    self.command_hub_service = command_hub_service

send_effector_command 

send_effector_command(config_dict: dict)

发送末端执行器命令。

参数：

名称 类型 描述 默认
config_dict dict

命令配置字典。

 必需
源代码位于： src/controller/controller/presentation/view_models/effector_view_model.py 
27
28
29
30
31
32
33
def send_effector_command(self, config_dict: dict):
    """发送末端执行器命令。

    Args:
        config_dict (dict): 命令配置字典。
    """
    self.command_hub_service.command_distribution(config_dict)

TrajectoryViewModel 

TrajectoryViewModel(parent=None)

Bases: BaseViewModel

轨迹规划视图模型。

负责轨迹规划相关的 UI 交互逻辑。

初始化轨迹规划视图模型。

参数：

名称 类型 描述 默认
parent QObject

父对象. Defaults to None.

 None
源代码位于： src/controller/controller/presentation/view_models/trajectory_view_model.py 
13
14
15
16
17
18
19
def __init__(self, parent=None):
    """初始化轨迹规划视图模型。

    Args:
        parent (QObject, optional): 父对象. Defaults to None.
    """
    super().__init__(parent)

DynamicsViewModel 

DynamicsViewModel(command_hub_service: CommandHubService, robot_state_service: RobotStateDomainService, dynamic_service: DynamicDomainService, teach_record_service: TeachRecordDomainService, record_repository: RecordRepository, parent: Optional[object] = None)

Bases: BaseViewModel

动力学视图模型。

订阅 RobotStateDomainService，实现力矩补偿和示教记录。

职责： 1. 处理示教模式切换 2. 订阅力矩补偿请求（数据驱动） 3. 计算并发送力矩补偿 4. 管理示教记录的录制、保存、播放

属性：

名称 类型 描述
recording_state_changed pyqtSignal

记录状态变化信号。
record_list_updated pyqtSignal

记录列表更新信号。
command_hub CommandHubService

命令中心服务。
robot_state RobotStateDomainService

机械臂状态服务。
dynamic_service DynamicDomainService

动力学服务。
teach_record_service TeachRecordDomainService

示教记录服务。
record_repository RecordRepository

记录仓储。

初始化动力学视图模型。

参数：

名称 类型 描述 默认
command_hub_service CommandHubService

命令中心服务。

 必需
robot_state_service RobotStateDomainService

机械臂状态服务。

 必需
dynamic_service DynamicDomainService

动力学服务。

 必需
teach_record_service TeachRecordDomainService

示教记录服务。

 必需
record_repository RecordRepository

记录仓储。

 必需
parent Optional[object]

父对象。

 None
源代码位于： src/controller/controller/presentation/view_models/dynamics_view_model.py 
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
def __init__(
    self,
    command_hub_service: CommandHubService,
    robot_state_service: RobotStateDomainService,
    dynamic_service: DynamicDomainService,
    teach_record_service: TeachRecordDomainService,
    record_repository: RecordRepository,
    parent: Optional[object] = None):
    """初始化动力学视图模型。

    Args:
        command_hub_service: 命令中心服务。
        robot_state_service: 机械臂状态服务。
        dynamic_service: 动力学服务。
        teach_record_service: 示教记录服务。
        record_repository: 记录仓储。
        parent: 父对象。
    """
    super().__init__(parent)
    self.command_hub = command_hub_service
    self.robot_state = robot_state_service
    self.dynamic_service = dynamic_service
    self.teach_record_service = teach_record_service
    self.record_repository = record_repository

    # 订阅力矩补偿请求信号（数据驱动，不是定时器）
    self.robot_state.torque_compensation_requested.connect(
        self._on_torque_compensation_requested
    )

    # 订阅记录服务的信号
    self.teach_record_service.recording_state_changed.connect(
        self.recording_state_changed.emit
    )
    self.teach_record_service.record_added.connect(
        self._on_record_added
    )
    self.teach_record_service.record_deleted.connect(
        self._on_record_deleted
    )

    # 连接记录定时器到状态更新
    record_timer = self.teach_record_service.get_record_timer()
    record_timer.timeout.connect(self._on_record_timer_timeout)

    # 加载已存在的记录
    self._load_records_from_file()

toggle_teaching_mode 

toggle_teaching_mode(command_dict: dict)

切换示教模式。

参数：

名称 类型 描述 默认
command_dict dict

命令字典，包含 control 和 mode。

 必需
源代码位于： src/controller/controller/presentation/view_models/dynamics_view_model.py 
86
87
88
89
90
91
92
93
94
95
96
97
def toggle_teaching_mode(self, command_dict: dict):
    """切换示教模式。

    Args:
        command_dict (dict): 命令字典，包含 control 和 mode。
    """
    # 1. 发送串口命令
    self.command_hub.command_distribution(command_dict)

    # 2. 更新状态服务的示教模式标志
    is_enable = (command_dict['control'] == 0x07)
    self.robot_state.set_teaching_mode(is_enable)

send_torque 

send_torque(torque_values: list)

发送力矩控制命令。

参数：

名称 类型 描述 默认
torque_values list

力矩值列表（6个关节）。

 必需
源代码位于： src/controller/controller/presentation/view_models/dynamics_view_model.py 
 99
100
101
102
103
104
105
106
107
108
109
110
def send_torque(self, torque_values: list):
    """发送力矩控制命令。

    Args:
        torque_values (list): 力矩值列表（6个关节）。
    """
    command_dict = {
        'control': 0x06,  # 力矩控制
        'mode': 0x0A,     # 周期力矩模式
        'torque': torque_values
    }
    self.command_hub.command_distribution(command_dict)

toggle_recording 

toggle_recording()

切换记录状态（开始/停止）。

源代码位于： src/controller/controller/presentation/view_models/dynamics_view_model.py 
142
143
144
145
146
147
148
149
150
151
152
def toggle_recording(self):
    """切换记录状态（开始/停止）。"""
    if self.teach_record_service.is_recording():
        # 停止记录
        record_name = self.teach_record_service.stop_recording()
        if record_name:
            # 保存到文件
            self._save_records_to_file()
    else:
        # 开始记录
        self.teach_record_service.start_recording()

delete_record 

delete_record(record_name: str) -> bool

删除指定记录。

参数：

名称 类型 描述 默认
record_name str

记录名称。

 必需

返回：

名称 类型 描述
bool bool

是否删除成功。
源代码位于： src/controller/controller/presentation/view_models/dynamics_view_model.py 
154
155
156
157
158
159
160
161
162
163
164
165
166
def delete_record(self, record_name: str) -> bool:
    """删除指定记录。

    Args:
        record_name (str): 记录名称。

    Returns:
        bool: 是否删除成功。
    """
    success = self.teach_record_service.delete_record(record_name)
    if success:
        self._save_records_to_file()
    return success

run_record 

run_record(record_name: str)

运行指定记录（播放示教轨迹）。

流程： 1. 获取示教记录的角度列表 2. 委托给 CommandHubService 处理

参数：

名称 类型 描述 默认
record_name str

记录名称。

 必需
源代码位于： src/controller/controller/presentation/view_models/dynamics_view_model.py 
168
169
170
171
172
173
174
175
176
177
178
179
180
def run_record(self, record_name: str):
    """运行指定记录（播放示教轨迹）。

    流程：
    1. 获取示教记录的角度列表
    2. 委托给 CommandHubService 处理

    Args:
        record_name (str): 记录名称。
    """
    angles_list = self.teach_record_service.get_record(record_name)
    if angles_list:
        self.command_hub.run_teach_record(angles_list)

reverse_record 

reverse_record(record_name: str) -> Optional[str]

反转指定记录。

参数：

名称 类型 描述 默认
record_name str

记录名称。

 必需

返回：

类型 描述
Optional[str]

Optional[str]: 新记录名称，失败返回 None。
源代码位于： src/controller/controller/presentation/view_models/dynamics_view_model.py 
182
183
184
185
186
187
188
189
190
191
192
193
194
def reverse_record(self, record_name: str) -> Optional[str]:
    """反转指定记录。

    Args:
        record_name (str): 记录名称。

    Returns:
        Optional[str]: 新记录名称，失败返回 None。
    """
    new_name = self.teach_record_service.reverse_record(record_name)
    if new_name:
        self._save_records_to_file()
    return new_name

get_record_names 

get_record_names() -> List[str]

获取所有记录名称。

返回：

类型 描述
List[str]

List[str]: 记录名称列表。
源代码位于： src/controller/controller/presentation/view_models/dynamics_view_model.py 
196
197
198
199
200
201
202
def get_record_names(self) -> List[str]:
    """获取所有记录名称。

    Returns:
        List[str]: 记录名称列表。
    """
    return self.teach_record_service.get_record_names()

is_recording 

is_recording() -> bool

查询是否正在记录。

返回：

名称 类型 描述
bool bool

是否正在记录。
源代码位于： src/controller/controller/presentation/view_models/dynamics_view_model.py 
204
205
206
207
208
209
210
def is_recording(self) -> bool:
    """查询是否正在记录。

    Returns:
        bool: 是否正在记录。
    """
    return self.teach_record_service.is_recording()

CameraViewModel 

CameraViewModel(app_service: CameraApplicationService, parent=None)

Bases: BaseViewModel

摄像头视图模型。

职责： - 连接 UI 和 Application Service - 管理图像显示状态（定时刷新） - 管理检测状态 - 通过 Application Service 获取图像和检测结果

架构原则： - 只依赖 Application 层，不直接访问 Domain 层 - 所有 Domain 层调用通过 Application Service 转发

属性：

名称 类型 描述
image_display_requested pyqtSignal

图像显示请求信号，携带 (image, image_type)。
status_updated pyqtSignal

状态文本更新信号。
image_info_updated pyqtSignal

图像信息更新信号。
connection_status_changed pyqtSignal

摄像头连接状态信号。
button_states_changed pyqtSignal

按钮状态变更信号，携带 (color_active, depth_active)。
clear_display_requested pyqtSignal

清除显示请求信号。
detection_status_changed pyqtSignal

检测状态变更信号。
detection_result_updated pyqtSignal

检测结果更新信号。
app_service CameraApplicationService

摄像头应用服务。
display_timer QTimer

图像刷新定时器。

初始化摄像头视图模型。

参数：

名称 类型 描述 默认
app_service CameraApplicationService

摄像头应用服务。

 必需
parent QObject

父对象. Defaults to None.

 None
源代码位于： src/controller/controller/presentation/view_models/camera_view_model.py 
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
def __init__(
    self,
    app_service: CameraApplicationService,
    parent=None
):
    """初始化摄像头视图模型。

    Args:
        app_service (CameraApplicationService): 摄像头应用服务。
        parent (QObject, optional): 父对象. Defaults to None.
    """
    super().__init__(parent)
    self.app_service = app_service

    # 显示状态
    self.current_display_mode = None  # None, 'color', 'depth'

    # 定时器（30FPS刷新图像）
    self.display_timer = QTimer()
    self.display_timer.timeout.connect(self._update_display)

    # 连接信号
    self._connect_signals()

connect_camera 

connect_camera()

连接摄像头。

源代码位于： src/controller/controller/presentation/view_models/camera_view_model.py 
74
75
76
def connect_camera(self):
    """连接摄像头。"""
    self.app_service.connect_camera()

disconnect_camera 

disconnect_camera()

断开摄像头。

源代码位于： src/controller/controller/presentation/view_models/camera_view_model.py 
78
79
80
81
82
83
84
85
def disconnect_camera(self):
    """断开摄像头。"""
    # 先停止显示
    if self.current_display_mode is not None:
        self.stop_display()

    # 断开摄像头
    self.app_service.disconnect_camera()

start_color_display 

start_color_display()

开始显示彩色图。

源代码位于： src/controller/controller/presentation/view_models/camera_view_model.py 
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
def start_color_display(self):
    """开始显示彩色图。"""
    # ✅ 通过Application层检查摄像头是否连接
    if not self.app_service.is_camera_connected():
        self.status_updated.emit("请先连接摄像头")
        return

    # 设置显示模式
    self.current_display_mode = 'color'

    # 启动定时器（33ms = ~30FPS）
    self.display_timer.start(33)

    # 更新状态
    self.status_updated.emit("显示彩色图像")
    self.button_states_changed.emit(True, False)

start_depth_display 

start_depth_display()

开始显示深度图。

源代码位于： src/controller/controller/presentation/view_models/camera_view_model.py 
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
def start_depth_display(self):
    """开始显示深度图。"""
    # ✅ 通过Application层检查摄像头是否连接
    if not self.app_service.is_camera_connected():
        self.status_updated.emit("请先连接摄像头")
        return

    # 设置显示模式
    self.current_display_mode = 'depth'

    # 启动定时器
    self.display_timer.start(33)

    # 更新状态
    self.status_updated.emit("显示深度图像")
    self.button_states_changed.emit(False, True)

stop_display 

stop_display()

停止显示。

源代码位于： src/controller/controller/presentation/view_models/camera_view_model.py 
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
def stop_display(self):
    """停止显示。"""
    # 停止定时器
    self.display_timer.stop()

    # 清除显示模式
    self.current_display_mode = None

    # 清除显示
    self.clear_display_requested.emit()

    # 更新状态
    self.status_updated.emit("已停止显示")
    self.button_states_changed.emit(False, False)
    self.image_info_updated.emit({})

start_detection 

start_detection()

开始检测。

源代码位于： src/controller/controller/presentation/view_models/camera_view_model.py 
137
138
139
def start_detection(self):
    """开始检测。"""
    self.app_service.start_detection()

stop_detection 

stop_detection()

停止检测。

源代码位于： src/controller/controller/presentation/view_models/camera_view_model.py 
141
142
143
def stop_detection(self):
    """停止检测。"""
    self.app_service.stop_detection()

move_to_detected_part 

move_to_detected_part()

运动到检测到的零件位置。

职责：仅转发到 Application 层。

源代码位于： src/controller/controller/presentation/view_models/camera_view_model.py 
145
146
147
148
149
150
def move_to_detected_part(self):
    """运动到检测到的零件位置。

    职责：仅转发到 Application 层。
    """
    self.app_service.move_to_detected_part()

cleanup 

cleanup()

清理资源。

源代码位于： src/controller/controller/presentation/view_models/camera_view_model.py 
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
def cleanup(self):
    """清理资源。"""
    try:
        # 停止检测（✅ 通过Application层）
        if self.app_service.is_detection_running():
            self.app_service.stop_detection()

        # 停止显示
        self.stop_display()

        # 断开摄像头（✅ 通过Application层）
        if self.app_service.is_camera_connected():
            self.app_service.disconnect_camera()

    except Exception as e:
        pass

MotionPlanningViewModel 

MotionPlanningViewModel(app_service: MotionPlanningApplicationService, dynamics_vm: DynamicsViewModel)

Bases: QObject

运动规划 ViewModel。

职责： 1. 转发 UI 命令到 Application Service 2. 转发 Application Service 信号到 UI 组件 3. 通过 DynamicsViewModel 获取示教记录（避免重复注入）

设计原则： - ViewModel 只依赖 Application Service，不直接访问 Domain 或 Infrastructure - 所有业务逻辑在 Application Service 中封装

属性：

名称 类型 描述
plan_list_changed pyqtSignal

方案列表变更信号。
current_plan_changed pyqtSignal

当前方案变更信号，携带索引。
point_list_changed pyqtSignal

节点列表变更信号。
current_position_received pyqtSignal

当前位置数据信号，携带角度列表（弧度）。
trajectory_preview_signal pyqtSignal

轨迹预览信号，携带 (轨迹数据, 上下文)。
app_service MotionPlanningApplicationService

运动规划应用服务。
dynamics_vm DynamicsViewModel

动力学 ViewModel。

初始化运动规划 ViewModel。

参数：

名称 类型 描述 默认
app_service MotionPlanningApplicationService

运动规划应用服务。

 必需
dynamics_vm DynamicsViewModel

动力学 ViewModel。

 必需
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
def __init__(
    self, 
    app_service: MotionPlanningApplicationService,
    dynamics_vm: DynamicsViewModel
):
    """初始化运动规划 ViewModel。

    Args:
        app_service (MotionPlanningApplicationService): 运动规划应用服务。
        dynamics_vm (DynamicsViewModel): 动力学 ViewModel。
    """
    super().__init__()
    self.app_service = app_service
    self.dynamics_vm = dynamics_vm

    # 连接Application Service的信号
    self.app_service.plan_list_changed.connect(self.plan_list_changed.emit)
    self.app_service.current_plan_changed.connect(self.current_plan_changed.emit)
    self.app_service.point_list_changed.connect(self.point_list_changed.emit)
    self.app_service.current_position_received.connect(self.current_position_received.emit)
    self.app_service.trajectory_preview_signal.connect(self.trajectory_preview_signal.emit)

create_plan 

create_plan(name: str)

创建方案。

参数：

名称 类型 描述 默认
name str

方案名称。

 必需
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
63
64
65
66
67
68
69
def create_plan(self, name: str):
    """创建方案。

    Args:
        name (str): 方案名称。
    """
    self.app_service.create_plan(name)

delete_plan 

delete_plan(index: int) -> bool

删除方案。

参数：

名称 类型 描述 默认
index int

方案索引。

 必需

返回：

名称 类型 描述
bool bool

True=删除成功, False=删除失败（违反业务规则）。
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
71
72
73
74
75
76
77
78
79
80
def delete_plan(self, index: int) -> bool:
    """删除方案。

    Args:
        index (int): 方案索引。

    Returns:
        bool: True=删除成功, False=删除失败（违反业务规则）。
    """
    return self.app_service.delete_plan(index)

switch_plan 

switch_plan(index: int)

切换方案。

参数：

名称 类型 描述 默认
index int

方案索引。

 必需
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
82
83
84
85
86
87
88
def switch_plan(self, index: int):
    """切换方案。

    Args:
        index (int): 方案索引。
    """
    self.app_service.switch_plan(index)

rename_plan 

rename_plan(index: int, new_name: str)

重命名方案。

参数：

名称 类型 描述 默认
index int

方案索引。

 必需
new_name str

新名称。

 必需
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
90
91
92
93
94
95
96
97
def rename_plan(self, index: int, new_name: str):
    """重命名方案。

    Args:
        index (int): 方案索引。
        new_name (str): 新名称。
    """
    self.app_service.rename_plan(index, new_name)

get_plan_names 

get_plan_names() -> List[str]

获取所有方案名称。

返回：

类型 描述
List[str]

List[str]: 方案名称列表。
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
 99
100
101
102
103
104
105
def get_plan_names(self) -> List[str]:
    """获取所有方案名称。

    Returns:
        List[str]: 方案名称列表。
    """
    return self.app_service.domain_service.get_plan_names()

get_current_plan_index 

get_current_plan_index() -> int

获取当前方案索引。

返回：

名称 类型 描述
int int

当前索引。
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
107
108
109
110
111
112
113
def get_current_plan_index(self) -> int:
    """获取当前方案索引。

    Returns:
        int: 当前索引。
    """
    return self.app_service.domain_service.get_current_index()

get_plan_count 

get_plan_count() -> int

获取方案数量。

返回：

名称 类型 描述
int int

方案数量。
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
115
116
117
118
119
120
121
def get_plan_count(self) -> int:
    """获取方案数量。

    Returns:
        int: 方案数量。
    """
    return self.app_service.domain_service.get_plan_count()

add_point 

add_point(point_data: dict)

添加节点。

参数：

名称 类型 描述 默认
point_data dict

节点数据。

 必需
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
125
126
127
128
129
130
131
def add_point(self, point_data: dict):
    """添加节点。

    Args:
        point_data (dict): 节点数据。
    """
    self.app_service.add_point(point_data)

delete_point 

delete_point(index: int)

删除节点。

参数：

名称 类型 描述 默认
index int

节点索引。

 必需
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
133
134
135
136
137
138
139
def delete_point(self, index: int):
    """删除节点。

    Args:
        index (int): 节点索引。
    """
    self.app_service.delete_point(index)

move_point_up 

move_point_up(index: int)

上移节点。

参数：

名称 类型 描述 默认
index int

节点索引。

 必需
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
141
142
143
144
145
146
147
def move_point_up(self, index: int):
    """上移节点。

    Args:
        index (int): 节点索引。
    """
    self.app_service.move_point_up(index)

move_point_down 

move_point_down(index: int)

下移节点。

参数：

名称 类型 描述 默认
index int

节点索引。

 必需
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
149
150
151
152
153
154
155
def move_point_down(self, index: int):
    """下移节点。

    Args:
        index (int): 节点索引。
    """
    self.app_service.move_point_down(index)

update_point 

update_point(index: int, point_data: dict)

更新节点。

参数：

名称 类型 描述 默认
index int

节点索引。

 必需
point_data dict

节点数据。

 必需
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
157
158
159
160
161
162
163
164
def update_point(self, index: int, point_data: dict):
    """更新节点。

    Args:
        index (int): 节点索引。
        point_data (dict): 节点数据。
    """
    self.app_service.update_point(index, point_data)

get_all_points 

get_all_points() -> List[dict]

获取当前方案的所有节点。

返回：

类型 描述
List[dict]

List[dict]: 节点列表。
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
166
167
168
169
170
171
172
def get_all_points(self) -> List[dict]:
    """获取当前方案的所有节点。

    Returns:
        List[dict]: 节点列表。
    """
    return self.app_service.domain_service.get_all_points()

get_single_point 

get_single_point(index: int) -> dict

获取单个节点数据。

参数：

名称 类型 描述 默认
index int

节点索引。

 必需

返回：

名称 类型 描述
dict dict

节点数据。
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
174
175
176
177
178
179
180
181
182
183
def get_single_point(self, index: int) -> dict:
    """获取单个节点数据。

    Args:
        index (int): 节点索引。

    Returns:
        dict: 节点数据。
    """
    return self.app_service.get_single_point(index)

get_teach_record_names 

get_teach_record_names() -> List[str]

获取所有示教记录名称（委托给 DynamicsViewModel）。

返回：

类型 描述
List[str]

List[str]: 记录名称列表。
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
187
188
189
190
191
192
193
def get_teach_record_names(self) -> List[str]:
    """获取所有示教记录名称（委托给 DynamicsViewModel）。

    Returns:
        List[str]: 记录名称列表。
    """
    return self.dynamics_vm.get_record_names()

get_teach_record 

get_teach_record(name: str) -> List[List[float]]

获取指定示教记录的数据（委托给 DynamicsViewModel）。

参数：

名称 类型 描述 默认
name str

记录名称。

 必需

返回：

类型 描述
List[List[float]]

List[List[float]]: 记录数据。
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
195
196
197
198
199
200
201
202
203
204
205
def get_teach_record(self, name: str) -> List[List[float]]:
    """获取指定示教记录的数据（委托给 DynamicsViewModel）。

    Args:
        name (str): 记录名称。

    Returns:
        List[List[float]]: 记录数据。
    """
    angles_list = self.dynamics_vm.teach_record_service.get_record(name)
    return angles_list if angles_list else []

execute_single_point 

execute_single_point(index: int)

执行单个节点。

参数：

名称 类型 描述 默认
index int

节点索引。

 必需
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
209
210
211
212
213
214
215
def execute_single_point(self, index: int):
    """执行单个节点。

    Args:
        index (int): 节点索引。
    """
    self.app_service.execute_single_point(index)

execute_motion_plan 

execute_motion_plan()

执行整个运动规划方案。

源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
217
218
219
def execute_motion_plan(self):
    """执行整个运动规划方案。"""
    self.app_service.execute_motion_plan()

request_current_position 

request_current_position()

请求获取当前位置（仅用于UI显示）。

委托给 Application Service 处理。

源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
223
224
225
226
227
228
def request_current_position(self):
    """请求获取当前位置（仅用于UI显示）。

    委托给 Application Service 处理。
    """
    self.app_service.request_current_position()

save_node_trajectory 

save_node_trajectory(node_index: int) -> bool

保存单个节点的轨迹。

参数：

名称 类型 描述 默认
node_index int

节点索引。

 必需

返回：

名称 类型 描述
bool bool

True=准备成功, False=准备失败。
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
232
233
234
235
236
237
238
239
240
241
def save_node_trajectory(self, node_index: int) -> bool:
    """保存单个节点的轨迹。

    Args:
        node_index (int): 节点索引。

    Returns:
        bool: True=准备成功, False=准备失败。
    """
    return self.app_service.save_node_trajectory(node_index)

save_plan_trajectory 

save_plan_trajectory() -> bool

保存整个方案的轨迹。

返回：

名称 类型 描述
bool bool

True=准备成功, False=准备失败。
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
243
244
245
246
247
248
249
def save_plan_trajectory(self) -> bool:
    """保存整个方案的轨迹。

    Returns:
        bool: True=准备成功, False=准备失败。
    """
    return self.app_service.save_plan_trajectory()

preview_node_trajectory 

preview_node_trajectory(node_index: int) -> bool

预览单个节点的轨迹曲线。

参数：

名称 类型 描述 默认
node_index int

节点索引。

 必需

返回：

名称 类型 描述
bool bool

True=准备成功, False=准备失败。
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
253
254
255
256
257
258
259
260
261
262
def preview_node_trajectory(self, node_index: int) -> bool:
    """预览单个节点的轨迹曲线。

    Args:
        node_index (int): 节点索引。

    Returns:
        bool: True=准备成功, False=准备失败。
    """
    return self.app_service.preview_node_trajectory(node_index)

preview_plan_trajectory 

preview_plan_trajectory() -> bool

预览整个方案的轨迹曲线。

返回：

名称 类型 描述
bool bool

True=准备成功, False=准备失败。
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
264
265
266
267
268
269
270
def preview_plan_trajectory(self) -> bool:
    """预览整个方案的轨迹曲线。

    Returns:
        bool: True=准备成功, False=准备失败。
    """
    return self.app_service.preview_plan_trajectory()

load_local_trajectory 

load_local_trajectory(file_path: str) -> bool

从 plans 目录加载轨迹文件。

参数：

名称 类型 描述 默认
file_path str

轨迹文件的完整路径或文件名。

 必需

返回：

名称 类型 描述
bool bool

True=加载成功, False=加载失败。
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
274
275
276
277
278
279
280
281
282
283
def load_local_trajectory(self, file_path: str) -> bool:
    """从 plans 目录加载轨迹文件。

    Args:
        file_path (str): 轨迹文件的完整路径或文件名。

    Returns:
        bool: True=加载成功, False=加载失败。
    """
    return self.app_service.load_local_trajectory(file_path)

get_local_trajectory_files 

get_local_trajectory_files() -> List[str]

获取所有可用的本地轨迹文件。

返回：

类型 描述
List[str]

List[str]: 文件名列表（不带扩展名）。
源代码位于： src/controller/controller/presentation/view_models/motion_planning_view_model.py 
285
286
287
288
289
290
291
def get_local_trajectory_files(self) -> List[str]:
    """获取所有可用的本地轨迹文件。

    Returns:
        List[str]: 文件名列表（不带扩展名）。
    """
    return self.app_service.get_local_trajectory_files()

ToolsViewModel 

ToolsViewModel(app_service: ToolsApplicationService)

Bases: QObject

工具 ViewModel。

职责： 1. 转发 UI 的计算请求到 Application Service 2. 转发 Application Service 的结果信号到 UI 3. 处理获取当前位置和逆运动学计算请求

属性：

名称 类型 描述
calculation_result pyqtSignal

正运动学结果信号，携带结果字典。
current_angles_received pyqtSignal

当前关节角度信号，携带角度列表。
inverse_result pyqtSignal

逆运动学结果信号，携带结果字典。
app_service ToolsApplicationService

工具应用服务。

初始化工具 ViewModel。

参数：

名称 类型 描述 默认
app_service ToolsApplicationService

工具应用服务。

 必需
源代码位于： src/controller/controller/presentation/view_models/tools_view_model.py 
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
def __init__(self, app_service: ToolsApplicationService):
    """初始化工具 ViewModel。

    Args:
        app_service (ToolsApplicationService): 工具应用服务。
    """
    super().__init__()
    self.app_service = app_service

    # 连接Application Service的信号
    self.app_service.calculation_result_signal.connect(
        self.calculation_result.emit
    )
    self.app_service.current_angles_signal.connect(
        self.current_angles_received.emit
    )
    self.app_service.inverse_result_signal.connect(
        self.inverse_result.emit
    )

calculate_kinematics 

calculate_kinematics(joint_angles: List[float])

请求计算正运动学（转发给 Application Service）。

参数：

名称 类型 描述 默认
joint_angles List[float]

6个关节角度（弧度）。

 必需
源代码位于： src/controller/controller/presentation/view_models/tools_view_model.py 
49
50
51
52
53
54
55
def calculate_kinematics(self, joint_angles: List[float]):
    """请求计算正运动学（转发给 Application Service）。

    Args:
        joint_angles (List[float]): 6个关节角度（弧度）。
    """
    self.app_service.calculate_forward_kinematics(joint_angles)

get_current_position 

get_current_position()

请求获取当前机械臂关节角度。

源代码位于： src/controller/controller/presentation/view_models/tools_view_model.py 
57
58
59
def get_current_position(self):
    """请求获取当前机械臂关节角度。"""
    self.app_service.get_current_joint_angles()

calculate_inverse_kinematics 

calculate_inverse_kinematics(rotation_matrix: List[List[float]], position: List[float], initial_theta: List[float] = None)

请求计算逆运动学。

参数：

名称 类型 描述 默认
rotation_matrix List[List[float]]

3x3 旋转矩阵。

 必需
position List[float]

[x, y, z] 位置（米）。

 必需
initial_theta List[float]

初始关节角度. Defaults to None.

 None
源代码位于： src/controller/controller/presentation/view_models/tools_view_model.py 
61
62
63
64
65
66
67
68
69
def calculate_inverse_kinematics(self, rotation_matrix: List[List[float]], position: List[float], initial_theta: List[float] = None):
    """请求计算逆运动学。

    Args:
        rotation_matrix (List[List[float]]): 3x3 旋转矩阵。
        position (List[float]): [x, y, z] 位置（米）。
        initial_theta (List[float], optional): 初始关节角度. Defaults to None.
    """
    self.app_service.calculate_inverse_kinematics(rotation_matrix, position, initial_theta)

DHParam dataclass 

DHParam()

机械臂 DH 参数配置。

存储运动学和动力学所需的 DH 参数。

属性：

名称 类型 描述
kinematic_dh array

运动学 DH 参数表。
dynamic_dh array

动力学 DH 参数表。

初始化默认 DH 参数。

源代码位于： src/controller/controller/domain/value_objects/dh_param.py 
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
def __init__(self):
    """初始化默认 DH 参数。"""
    self.kinematic_dh = np.array([
        [0.0, 0.0, 0.0,  0.0],
        [0.0, -pi/2, 0.0, -pi/2],
        [0.425, pi, 0.0, 0.0],
        [0.401, pi, 0.0856, pi/2],
        [0.0, pi/2, 0.086, 0.0],
        [0.0, -pi/2, 0.231, 0.0]  
    ], dtype=float)

    self.dynamic_dh = np.array([
        [0.0, 0.0, 0.0, 0.0],
        [0.0, -pi/2, 0.0, -pi/2],
        [0.425, pi, 0.0, 0.0],
        [0.401, pi, 0.0856, pi/2],
        [0.0, pi/2, 0.086, 0.0],
        [0.0, -pi/2, 0.0725, 0.0]
    ])

get_kinematic_dh 

get_kinematic_dh() -> np.ndarray

获取运动学 DH 参数。

返回：

类型 描述
ndarray

np.ndarray: 运动学 DH 参数矩阵。
源代码位于： src/controller/controller/domain/value_objects/dh_param.py 
39
40
41
42
43
44
45
def get_kinematic_dh(self) -> np.ndarray:
    """获取运动学 DH 参数。

    Returns:
        np.ndarray: 运动学 DH 参数矩阵。
    """
    return self.kinematic_dh

get_dynamic_dh 

get_dynamic_dh() -> np.ndarray

获取动力学 DH 参数。

返回：

类型 描述
ndarray

np.ndarray: 动力学 DH 参数矩阵。
源代码位于： src/controller/controller/domain/value_objects/dh_param.py 
47
48
49
50
51
52
53
def get_dynamic_dh(self) -> np.ndarray:
    """获取动力学 DH 参数。

    Returns:
        np.ndarray: 动力学 DH 参数矩阵。
    """
    return self.dynamic_dh

RobotStateSnapshot dataclass 

RobotStateSnapshot(init_status: int, control: int, mode: int, joint_positions: Tuple[int, ...], joint_angles: Tuple[float, ...], joint_speeds: Tuple[int, ...], joint_torques: Tuple[int, ...], joint_status: Tuple[int, ...], double_encoder_interpolations: Tuple[int, ...], errors: Tuple[int, ...], effector_data: Any, timestamp: float)

机械臂状态快照 - 不可变数据载体。

特点： - frozen=True: 不可变，线程安全 - 包含完整的机械臂状态 - 可以保存历史快照序列

属性：

名称 类型 描述
init_status int

初始化状态。
control int

当前命令 0x06/0x07...。
mode int

运行模式 0x08/0x0A...。
joint_positions Tuple[int, ...]

编码器位置（原始值，6个关节）。
joint_angles Tuple[float, ...]

关节角度（弧度，6个关节）。
joint_speeds Tuple[int, ...]

关节速度。
joint_torques Tuple[int, ...]

关节力矩。
joint_status Tuple[int, ...]

关节状态码。
double_encoder_interpolations Tuple[int, ...]

双编码器插值。
errors Tuple[int, ...]

错误码。
effector_data Any

夹爪数据。
timestamp float

接收时间戳。

from_decoded_message classmethod 

from_decoded_message(decoded_msg: Any) -> RobotStateSnapshot

从解码消息创建快照。

参数：

名称 类型 描述 默认
decoded_msg Any

解码后的消息对象，应包含 positions(弧度), speeds, torques 等字段。

 必需

返回：

名称 类型 描述
RobotStateSnapshot RobotStateSnapshot

创建的状态快照实例。
Note

decoded_msg.positions 已经通过 position2radian 转换为弧度值， 所以 joint_angles 直接使用该值。 joint_positions 会通过反向计算还原为原始编码器值。

源代码位于： src/controller/controller/domain/value_objects/robot_state_snapshot.py 
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
@classmethod
def from_decoded_message(cls, decoded_msg: Any) -> 'RobotStateSnapshot':
    """从解码消息创建快照。

    Args:
        decoded_msg (Any): 解码后的消息对象，应包含 positions(弧度), speeds, torques 等字段。

    Returns:
        RobotStateSnapshot: 创建的状态快照实例。

    Note:
        decoded_msg.positions 已经通过 position2radian 转换为弧度值，
        所以 joint_angles 直接使用该值。
        joint_positions 会通过反向计算还原为原始编码器值。
    """        
    robot_utils = RobotUtils()

    # decoded_msg.positions 已经是弧度值（由MessageDecoder的transform完成）
    # 所以直接使用，不需要再次转换
    angles = decoded_msg.positions  # 已经是弧度值

    # 反向计算原始编码器位置值（用于保存完整状态）
    positions = robot_utils.radian2position(angles)

    return cls(
        init_status=decoded_msg.init_status,
        control=decoded_msg.control,
        mode=decoded_msg.mode,
        joint_positions=tuple(positions),  # 反向计算的编码器值
        joint_angles=tuple(angles),        # 直接使用解码后的弧度值
        joint_speeds=tuple(decoded_msg.speeds),
        joint_torques=tuple(decoded_msg.torques),
        joint_status=tuple(decoded_msg.status),
        double_encoder_interpolations=tuple(decoded_msg.double_encoder_interpolations),
        errors=tuple(decoded_msg.errors),
        effector_data=decoded_msg.effector_data,
        timestamp=time.time()
    )

HandEyeCalibrationConfig dataclass 

HandEyeCalibrationConfig(hand_eye_matrix: ndarray, camera_intrinsics: CameraIntrinsics, target_offset: TargetOffset, end_effector_adjustment: EndEffectorAdjustment)

手眼标定完整配置。

这是一个值对象（Value Object），不可变且无副作用。

属性：

名称 类型 描述
hand_eye_matrix ndarray

4x4 齐次变换矩阵。
camera_intrinsics CameraIntrinsics

相机内参。
target_offset TargetOffset

目标偏移量。
end_effector_adjustment EndEffectorAdjustment

末端姿态调整。

__post_init__ 

__post_init__()

验证数据完整性。

引发：

类型 描述
ValueError

如果手眼标定矩阵形状不是 4x4。
源代码位于： src/controller/controller/domain/value_objects/hand_eye_calibration_config.py 
78
79
80
81
82
83
84
85
def __post_init__(self):
    """验证数据完整性。

    Raises:
        ValueError: 如果手眼标定矩阵形状不是 4x4。
    """
    if self.hand_eye_matrix.shape != (4, 4):
        raise ValueError("手眼标定矩阵必须是 4x4 矩阵")

CameraIntrinsics dataclass 

CameraIntrinsics(fx: float, fy: float, cx: float, cy: float)

相机内参。

属性：

名称 类型 描述
fx float

x 方向焦距。
fy float

y 方向焦距。
cx float

光心 x 坐标。
cy float

光心 y 坐标。

TargetOffset dataclass 

TargetOffset(x: float, y: float, z: float)

目标偏移量（米）。

属性：

名称 类型 描述
x float

x 方向偏移。
y float

y 方向偏移。
z float

z 方向偏移。

to_array 

to_array() -> np.ndarray

转换为 numpy 数组。

返回：

类型 描述
ndarray

np.ndarray: [x, y, z] 数组。
源代码位于： src/controller/controller/domain/value_objects/hand_eye_calibration_config.py 
37
38
39
40
41
42
43
def to_array(self) -> np.ndarray:
    """转换为 numpy 数组。

    Returns:
        np.ndarray: [x, y, z] 数组。
    """
    return np.array([self.x, self.y, self.z])

EndEffectorAdjustment dataclass 

EndEffectorAdjustment(z_rotation: float, y_rotation: float, x_rotation: float)

末端姿态调整（弧度）。

属性：

名称 类型 描述
z_rotation float

绕 Z 轴旋转角度。
y_rotation float

绕 Y 轴旋转角度。
x_rotation float

绕 X 轴旋转角度。

MotionOperationMode

Bases: Enum

运动操作模式枚举。

属性：

名称 类型 描述
NONE

无操作（仅获取位置）。
EXECUTE

执行运动。
SAVE

保存轨迹。
PREVIEW

预览轨迹（显示曲线）。

KinematicUtils

运动学工具类。

提供基本的运动学变换和角度处理函数。

dh2rm staticmethod 

dh2rm(a: float, alpha: float, d: float, theta: float) -> np.ndarray

根据 DH 参数计算变换矩阵。

参数：

名称 类型 描述 默认
a float

连杆长度。

 必需
alpha float

连杆扭转角。

 必需
d float

连杆偏移。

 必需
theta float

关节角。

 必需

返回：

类型 描述
ndarray

np.ndarray: 4x4 齐次变换矩阵。
源代码位于： src/controller/controller/domain/utils/kinematic_utils.py 
11
12
13
14
15
16
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
@staticmethod
def dh2rm(a: float, alpha: float, d: float, theta: float) -> np.ndarray:
    """根据 DH 参数计算变换矩阵。

    Args:
        a (float): 连杆长度。
        alpha (float): 连杆扭转角。
        d (float): 连杆偏移。
        theta (float): 关节角。

    Returns:
        np.ndarray: 4x4 齐次变换矩阵。
    """
    c_theta = cos(theta)
    s_theta = sin(theta)
    c_alpha = cos(alpha)
    s_alpha = sin(alpha)

    return np.array([
        [c_theta, -s_theta, 0, a],
        [s_theta * c_alpha, c_theta * c_alpha, -s_alpha, -s_alpha * d],
        [s_theta * s_alpha, c_theta * s_alpha, c_alpha, c_alpha * d],
        [0, 0, 0, 1]
    ], dtype=np.float64)

rm2quat staticmethod 

rm2quat(rm: ndarray) -> np.ndarray

旋转矩阵转四元数。

参数：

名称 类型 描述 默认
rm ndarray

3x3 旋转矩阵或 4x4 变换矩阵。

 必需

返回：

类型 描述
ndarray

np.ndarray: 四元数 [x, y, z, w]。
源代码位于： src/controller/controller/domain/utils/kinematic_utils.py 
36
37
38
39
40
41
42
43
44
45
46
47
48
49
@staticmethod
def rm2quat(rm: np.ndarray) -> np.ndarray:
    """旋转矩阵转四元数。

    Args:
        rm (np.ndarray): 3x3 旋转矩阵或 4x4 变换矩阵。

    Returns:
        np.ndarray: 四元数 [x, y, z, w]。
    """
    # 如果是 4x4 矩阵，提取 3x3 部分
    if rm.shape == (4, 4):
        rm = rm[:3, :3]
    return R.from_matrix(rm).as_quat()

quat2rm staticmethod 

quat2rm(quat: ndarray) -> np.ndarray

四元数转旋转矩阵。

参数：

名称 类型 描述 默认
quat ndarray

四元数 [x, y, z, w]。

 必需

返回：

类型 描述
ndarray

np.ndarray: 3x3 旋转矩阵。
源代码位于： src/controller/controller/domain/utils/kinematic_utils.py 
51
52
53
54
55
56
57
58
59
60
61
@staticmethod
def quat2rm(quat: np.ndarray) -> np.ndarray:
    """四元数转旋转矩阵。

    Args:
        quat (np.ndarray): 四元数 [x, y, z, w]。

    Returns:
        np.ndarray: 3x3 旋转矩阵。
    """
    return R.from_quat(quat).as_matrix()

quat2euler staticmethod 

quat2euler(quat: ndarray) -> np.ndarray

四元数转欧拉角 (XYZ 顺序)。

参数：

名称 类型 描述 默认
quat ndarray

四元数 [x, y, z, w]。

 必需

返回：

类型 描述
ndarray

np.ndarray: 欧拉角 [roll, pitch, yaw] (弧度)。
源代码位于： src/controller/controller/domain/utils/kinematic_utils.py 
63
64
65
66
67
68
69
70
71
72
73
@staticmethod
def quat2euler(quat: np.ndarray) -> np.ndarray:
    """四元数转欧拉角 (XYZ 顺序)。

    Args:
        quat (np.ndarray): 四元数 [x, y, z, w]。

    Returns:
        np.ndarray: 欧拉角 [roll, pitch, yaw] (弧度)。
    """
    return R.from_quat(quat).as_euler('xyz', degrees=False)

euler2quat staticmethod 

euler2quat(euler: ndarray) -> np.ndarray

欧拉角转四元数 (XYZ 顺序)。

参数：

名称 类型 描述 默认
euler ndarray

欧拉角 [roll, pitch, yaw] (弧度)。

 必需

返回：

类型 描述
ndarray

np.ndarray: 四元数 [x, y, z, w]。
源代码位于： src/controller/controller/domain/utils/kinematic_utils.py 
75
76
77
78
79
80
81
82
83
84
85
@staticmethod
def euler2quat(euler: np.ndarray) -> np.ndarray:
    """欧拉角转四元数 (XYZ 顺序)。

    Args:
        euler (np.ndarray): 欧拉角 [roll, pitch, yaw] (弧度)。

    Returns:
        np.ndarray: 四元数 [x, y, z, w]。
    """
    return R.from_euler('xyz', euler, degrees=False).as_quat()

euler2rm staticmethod 

euler2rm(euler: ndarray) -> np.ndarray

欧拉角转旋转矩阵 (XYZ 顺序)。

参数：

名称 类型 描述 默认
euler ndarray

欧拉角 [roll, pitch, yaw] (弧度)。

 必需

返回：

类型 描述
ndarray

np.ndarray: 3x3 旋转矩阵。
源代码位于： src/controller/controller/domain/utils/kinematic_utils.py 
87
88
89
90
91
92
93
94
95
96
97
@staticmethod
def euler2rm(euler: np.ndarray) -> np.ndarray:
    """欧拉角转旋转矩阵 (XYZ 顺序)。

    Args:
        euler (np.ndarray): 欧拉角 [roll, pitch, yaw] (弧度)。

    Returns:
        np.ndarray: 3x3 旋转矩阵。
    """
    return R.from_euler('xyz', euler, degrees=False).as_matrix()

rm2euler staticmethod 

rm2euler(rm: ndarray) -> np.ndarray

旋转矩阵转欧拉角 (XYZ 顺序)。

参数：

名称 类型 描述 默认
rm ndarray

3x3 旋转矩阵。

 必需

返回：

类型 描述
ndarray

np.ndarray: 欧拉角 [roll, pitch, yaw] (弧度)。
源代码位于： src/controller/controller/domain/utils/kinematic_utils.py 
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
@staticmethod
def rm2euler(rm: np.ndarray) -> np.ndarray:
    """旋转矩阵转欧拉角 (XYZ 顺序)。

    Args:
        rm (np.ndarray): 3x3 旋转矩阵。

    Returns:
        np.ndarray: 欧拉角 [roll, pitch, yaw] (弧度)。
    """
    # 如果是 4x4 矩阵，提取 3x3 部分
    if rm.shape == (4, 4):
        rm = rm[:3, :3]
    return R.from_matrix(rm).as_euler('xyz', degrees=False)

normalize_angle staticmethod 

normalize_angle(angle: float) -> float

将角度归一化到 [-pi, pi] 范围。

参数：

名称 类型 描述 默认
angle float

输入角度（弧度）。

 必需

返回：

名称 类型 描述
float float

归一化后的角度（弧度）。
源代码位于： src/controller/controller/domain/utils/kinematic_utils.py 
114
115
116
117
118
119
120
121
122
123
124
@staticmethod
def normalize_angle(angle: float) -> float:
    """将角度归一化到 [-pi, pi] 范围。

    Args:
        angle (float): 输入角度（弧度）。

    Returns:
        float: 归一化后的角度（弧度）。
    """
    return (angle + pi) % (2 * pi) - pi

MessageEncoder 

MessageEncoder(config_path='controller/config/message_encoder_config.yaml')

消息编码器。

基于 YAML 配置文件动态生成十六进制消息。

属性：

名称 类型 描述
config dict

加载的配置字典。
robot_utils RobotUtils

工具类实例。
formatters Dict[str, BaseFormatter]

已注册的格式化器字典。
Message type

动态创建的消息数据类。

初始化消息编码器。

参数：

名称 类型 描述 默认
config_path str

配置文件路径. Defaults to "controller/config/message_encoder_config.yaml".

 'controller/config/message_encoder_config.yaml'
源代码位于： src/controller/controller/domain/utils/message_encoder.py 
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
89
90
91
92
93
def __init__(self, config_path = "controller/config/message_encoder_config.yaml"):
    """初始化消息编码器。

    Args:
        config_path (str, optional): 配置文件路径. Defaults to "controller/config/message_encoder_config.yaml".
    """
    # 使用 ROS2 包资源管理方式查找配置文件
    if _HAS_AMENT_INDEX:
        try:
            # ROS2 方式：从 share 目录加载
            package_share_dir = get_package_share_directory('controller')
            config_path = os.path.join(package_share_dir, 'config', 'message_encoder_config.yaml')
        except Exception:
            # 如果包未安装，使用相对路径（开发模式）
            pass

    with open(config_path, 'r', encoding='utf-8') as f:
        self.config = yaml.safe_load(f)

    self.robot_utils = RobotUtils()
    self.formatters = self._register_formatters()
    self.Message = self._create_message_class()

create_message 

create_message(**kwargs: Any) -> Any

创建 MessageOut 实例。

参数：

名称 类型 描述 默认
**kwargs Any

消息字段值。

 {}

返回：

名称 类型 描述
Any Any

消息对象 (MessageOut)。
源代码位于： src/controller/controller/domain/utils/message_encoder.py 
134
135
136
137
138
139
140
141
142
143
def create_message(self, **kwargs: Any) -> Any:
    """创建 MessageOut 实例。

    Args:
        **kwargs: 消息字段值。

    Returns:
        Any: 消息对象 (MessageOut)。
    """
    return self.Message(**kwargs)

interpret_message 

interpret_message(message: Any) -> str

根据配置动态将消息对象编码为字符串。

参数：

名称 类型 描述 默认
message Any

MessageOut 对象。

 必需

返回：

名称 类型 描述
str str

编码后的十六进制字符串。
源代码位于： src/controller/controller/domain/utils/message_encoder.py 
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
def interpret_message(self, message: Any) -> str:
    """根据配置动态将消息对象编码为字符串。

    Args:
        message (Any): MessageOut 对象。

    Returns:
        str: 编码后的十六进制字符串。
    """
    protocol_config = self.config['protocol']
    fields_config = self.config['message_formats']['message_out']['fields']

    # 1. 添加协议头
    command = protocol_config['header']

    # 2. 按配置顺序处理每个字段
    for field_name, field_config in fields_config.items():
        field_value = getattr(message, field_name)
        command += self._format_field(field_value, field_config)

    # 3. 计算CRC（如果启用）
    if protocol_config.get('crc_enabled', False):
        crc = self.robot_utils.calculate_crc16(bytes.fromhex(command))
        command += f"{(crc >> 8) & 0xFF:02X}{crc & 0xFF:02X}"

    # 4. 添加协议尾
    command += protocol_config['footer']

    return command

MessageDecoder 

MessageDecoder(config_path='controller/config/message_decoder_config.yaml')

消息解码器。

基于 YAML 配置文件动态解析十六进制消息。

属性：

名称 类型 描述
config dict

加载的配置字典。
robot_utils RobotUtils

工具类实例。
parsers Dict[str, BaseParser]

已注册的解析器字典。
MessageIn type

动态创建的消息数据类。

初始化消息解码器。

参数：

名称 类型 描述 默认
config_path str

配置文件路径. Defaults to "controller/config/message_decoder_config.yaml".

 'controller/config/message_decoder_config.yaml'
源代码位于： src/controller/controller/domain/utils/message_decoder.py 
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
def __init__(self, config_path = "controller/config/message_decoder_config.yaml"):
    """初始化消息解码器。

    Args:
        config_path (str, optional): 配置文件路径. Defaults to "controller/config/message_decoder_config.yaml".
    """
    # 使用 ROS2 包资源管理方式查找配置文件
    if _HAS_AMENT_INDEX:
        try:
            # ROS2 方式：从 share 目录加载
            package_share_dir = get_package_share_directory('controller')
            config_path = os.path.join(package_share_dir, 'config', 'message_decoder_config.yaml')
        except Exception:
            # 如果包未安装，使用相对路径（开发模式）
            pass

    with open(config_path, 'r', encoding='utf-8') as f:
        self.config = yaml.safe_load(f)

    self.robot_utils = RobotUtils()
    self.parsers = self._register_parsers()
    self.MessageIn = self._create_message_class()

decode_message 

decode_message(hex_message: str) -> Any

解码十六进制消息。

参数：

名称 类型 描述 默认
hex_message str

原始十六进制字符串。

 必需

返回：

名称 类型 描述
Any Any

解码后的消息对象 (MessageIn)。

引发：

类型 描述
ValueError

如果消息格式错误或校验失败。
源代码位于： src/controller/controller/domain/utils/message_decoder.py 
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
def decode_message(self, hex_message: str) -> Any:
    """解码十六进制消息。

    Args:
        hex_message (str): 原始十六进制字符串。

    Returns:
        Any: 解码后的消息对象 (MessageIn)。

    Raises:
        ValueError: 如果消息格式错误或校验失败。
    """
    # 移除空格和换行符
    hex_message = hex_message.replace(' ', '').replace('\n', '').replace('\r', '')

    # 验证协议头和尾
    protocol_config = self.config['protocol']
    header = protocol_config['header']
    footer = protocol_config['footer']

    if not hex_message.startswith(header):
        raise ValueError(f"Invalid header. Expected {header}, got {hex_message[:4]}")

    if not hex_message.endswith(footer):
        raise ValueError(f"Invalid footer. Expected {footer}, got {hex_message[-4:]}")


    # 验证CRC（如果启用）
    if protocol_config.get('crc_enabled', False):
        crc_bytes = protocol_config.get('crc_bytes', 2) * 2  # 转换为十六进制字符数
        message_without_crc = hex_message[:-len(footer)-crc_bytes]
        received_crc = hex_message[-len(footer)-crc_bytes:-len(footer)]

        calculated_crc = self.robot_utils.calculate_crc16(message_without_crc)
        calculated_crc_hex = f"{calculated_crc:04X}"

        if calculated_crc_hex != received_crc:
            raise ValueError(f"CRC校验失败: 接收={received_crc}, 计算={calculated_crc_hex}")

    # 解析各个字段
    fields_config = self.config['message_formats']['message_in']['fields']
    parsed_data = {}
    current_pos = 0

    for field_name, field_config in fields_config.items():
        field_bytes = field_config['bytes']
        hex_chars = field_bytes * 2  # 每个字节对应2个十六进制字符

        # 特殊处理CRC字段
        if field_name == 'crc':
            if protocol_config.get('crc_enabled', False):
                field_hex = hex_message[current_pos:current_pos + hex_chars]
            else:
                field_hex = ""
        else:
            field_hex = hex_message[current_pos:current_pos + hex_chars]

        # 解析字段
        parsed_value = self._parse_field(field_hex, field_config)
        parsed_data[field_name] = parsed_value

        current_pos += hex_chars

    return self.MessageIn(**parsed_data)

decode_message_detailed 

decode_message_detailed(hex_message: str) -> Dict[str, Any]

解码消息并返回详细信息（包括原始十六进制值）。

参数：

名称 类型 描述 默认
hex_message str

原始十六进制字符串。

 必需

返回：

类型 描述
Dict[str, Any]

Dict[str, Any]: 包含详细信息的字典。
源代码位于： src/controller/controller/domain/utils/message_decoder.py 
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
def decode_message_detailed(self, hex_message: str) -> Dict[str, Any]:
    """解码消息并返回详细信息（包括原始十六进制值）。

    Args:
        hex_message (str): 原始十六进制字符串。

    Returns:
        Dict[str, Any]: 包含详细信息的字典。
    """
    message_in = self.decode_message(hex_message)

    # 创建详细信息字典
    detailed_info = {}
    fields_config = self.config['message_formats']['message_in']['fields']

    for field_name, _ in fields_config.items():
        value = getattr(message_in, field_name)
        detailed_info[field_name] = {
            'value': value,
            'description': fields_config[field_name].get('description', field_name)
        }

    return detailed_info

RobotUtils 

RobotUtils()

机器人工具类。

提供机器人相关的数值转换、校验和计算等工具方法。

初始化机器人参数。

源代码位于： src/controller/controller/domain/utils/robot_utils.py 
10
11
12
13
14
15
def __init__(self):
    """初始化机器人参数。"""
    self.JOINT_OFFSETS = [79119, 369835, 83627, 392414, 507293, 456372]
    self.RADIAN_TO_POS_SCALE_FACTOR = (2**19) / (2 * pi)  # 弧度转位置值的系数
    self.POS_TO_RADIAN_SCALE_FACTOR = (2 * pi) / (2**19)  # 位置值转弧度的系数
    self.init_radians = [0, -pi/2, 0, pi/2, 0, 0]

position2radian 

position2radian(position: list) -> list

编码器位置值转换为弧度。

参数：

名称 类型 描述 默认
position list

编码器位置值列表。

 必需

返回：

名称 类型 描述
list list

关节角度列表（弧度）。
源代码位于： src/controller/controller/domain/utils/robot_utils.py 
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
def position2radian(self, position: list) -> list:
    """编码器位置值转换为弧度。

    Args:
        position (list): 编码器位置值列表。

    Returns:
        list: 关节角度列表（弧度）。
    """
    radians = self.init_radians.copy()
    for i, pos in enumerate(position):
        if isinstance(pos, str):
            pos = int(pos)
        radians[i] += (pos - self.JOINT_OFFSETS[i]) * self.POS_TO_RADIAN_SCALE_FACTOR
    return radians

radian2position 

radian2position(radian: list) -> list

弧度转换为编码器位置值。

参数：

名称 类型 描述 默认
radian list

关节角度列表（弧度）。

 必需

返回：

名称 类型 描述
list list

编码器位置值列表（32位整数）。
源代码位于： src/controller/controller/domain/utils/robot_utils.py 
33
34
35
36
37
38
39
40
41
42
43
44
45
46
def radian2position(self, radian: list) -> list:
    """弧度转换为编码器位置值。

    Args:
        radian (list): 关节角度列表（弧度）。

    Returns:
        list: 编码器位置值列表（32位整数）。
    """
    positions = []
    for i, rad in enumerate(radian):
        position = int((rad - self.init_radians[i]) * self.RADIAN_TO_POS_SCALE_FACTOR + self.JOINT_OFFSETS[i]) & 0xFFFFFFFF
        positions.append(position)
    return positions

speed2position 

speed2position(speed: list) -> list

速度值转换为位置增量值。

参数：

名称 类型 描述 默认
speed list

速度列表。

 必需

返回：

名称 类型 描述
list list

对应的时间步长内的位置增量值。
源代码位于： src/controller/controller/domain/utils/robot_utils.py 
48
49
50
51
52
53
54
55
56
57
def speed2position(self, speed: list) -> list:
    """速度值转换为位置增量值。

    Args:
        speed (list): 速度列表。

    Returns:
        list: 对应的时间步长内的位置增量值。
    """
    return [int(position * self.RADIAN_TO_POS_SCALE_FACTOR) & 0xFFFFFF for position in speed]

torque_transfer staticmethod 

torque_transfer(torque: list) -> list

力矩值转换与归一化。

参数：

名称 类型 描述 默认
torque list

力矩值列表，长度不超过6。

 必需

返回：

名称 类型 描述
list list

转换后的力矩值列表（16位整数）。

引发：

类型 描述
ValueError

如果力矩列表长度超过6。
源代码位于： src/controller/controller/domain/utils/robot_utils.py 
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
@staticmethod
def torque_transfer(torque: list) -> list:
    """力矩值转换与归一化。

    Args:
        torque (list): 力矩值列表，长度不超过6。

    Returns:
        list: 转换后的力矩值列表（16位整数）。

    Raises:
        ValueError: 如果力矩列表长度超过6。
    """
    if len(torque) > 6:
        raise ValueError("力矩值列表长度不能超过6")
    transfer_torque = []
    for t in torque[:3]:
        t = min(max(t, -50), 50)
        transfer_torque.append(int(t / 87 * 1000))
    for t in torque[3:]:
        t = min(max(t, -10), 10)
        transfer_torque.append(int(t * 100))
    return [int(t) & 0xFFFF for t in transfer_torque]

effector2hex staticmethod 

effector2hex(effector_data: float) -> list

末端执行器数据转换为十六进制字节列表。

将浮点数拆分为整数部分和小数部分，分别转换为 2 字节的大端序十六进制。

参数：

名称 类型 描述 默认
effector_data float

末端执行器数据。

 必需

返回：

名称 类型 描述
list list

包含两个十六进制字符串的列表 [整数部分hex, 小数部分hex]。
源代码位于： src/controller/controller/domain/utils/robot_utils.py 
83
84
85
86
87
88
89
90
91
92
93
94
95
96
97
@staticmethod
def effector2hex(effector_data: float) -> list:
    """末端执行器数据转换为十六进制字节列表。

    将浮点数拆分为整数部分和小数部分，分别转换为 2 字节的大端序十六进制。

    Args:
        effector_data (float): 末端执行器数据。

    Returns:
        list: 包含两个十六进制字符串的列表 [整数部分hex, 小数部分hex]。
    """
    effector_data = str(float(effector_data))
    arr = effector_data.split('.')
    return [int(arr[0]).to_bytes(2, 'big').hex(), int(arr[1]).to_bytes(2, 'big').hex()]

calculate_crc16 staticmethod 

calculate_crc16(data) -> int

计算 CRC16 校验和 (CCITT)。

参数：

名称 类型 描述 默认
data Union[bytes, str]

字节数组或十六进制字符串。

 必需

返回：

名称 类型 描述
int int

16位校验和。如果输入格式错误返回 False。
源代码位于： src/controller/controller/domain/utils/robot_utils.py 
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
@staticmethod
def calculate_crc16(data) -> int:
    """计算 CRC16 校验和 (CCITT)。

    Args:
        data (Union[bytes, str]): 字节数组或十六进制字符串。

    Returns:
        int: 16位校验和。如果输入格式错误返回 False。
    """
    if isinstance(data, str):
        try:
            data = bytes.fromhex(data.strip())
        except Exception as e:
            return False

    # 计算CRC16-CCITT
    crc = 0xFFFF
    for byte in data:
        crc ^= (int(byte) << 8)
        for _ in range(8):
            if crc & 0x8000:
                crc = (crc << 1) ^ 0x1021
            else:
                crc = crc << 1
            crc &= 0xFFFF

    return crc

ImageDrawingUtils

图像绘制工具类。

提供静态方法用于在 OpenCV 图像上绘制检测结果和辅助图形。

draw_detection_result staticmethod 

draw_detection_result(image: ndarray, detection: Dict) -> np.ndarray

在图像上绘制检测结果。

参数：

名称 类型 描述 默认
image ndarray

原始图像（BGR）。

 必需
detection Dict

检测结果字典，包含： - head_center (Tuple[float, float]): 头部中心点 (x, y)。 - central_center (Tuple[float, float]): 中央中心点 (x, y)。 - real_center (Tuple[float, float]): 实际中心点 (x, y)。 - angle (float): 角度（弧度）。 - depth (float): central_center 的深度（米）。 - real_depth (float): real_center 的深度（米）。

 必需

返回：

类型 描述
ndarray

np.ndarray: 绘制后的图像（新图像，不修改原图）。
源代码位于： src/controller/controller/domain/utils/image_drawing_utils.py 
 17
 18
 19
 20
 21
 22
 23
 24
 25
 26
 27
 28
 29
 30
 31
 32
 33
 34
 35
 36
 37
 38
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
@staticmethod
def draw_detection_result(image: np.ndarray, detection: Dict) -> np.ndarray:
    """在图像上绘制检测结果。

    Args:
        image (np.ndarray): 原始图像（BGR）。
        detection (Dict): 检测结果字典，包含：
            - head_center (Tuple[float, float]): 头部中心点 (x, y)。
            - central_center (Tuple[float, float]): 中央中心点 (x, y)。
            - real_center (Tuple[float, float]): 实际中心点 (x, y)。
            - angle (float): 角度（弧度）。
            - depth (float): central_center 的深度（米）。
            - real_depth (float): real_center 的深度（米）。

    Returns:
        np.ndarray: 绘制后的图像（新图像，不修改原图）。
    """
    if not detection:
        return image

    # 复制图像，避免修改原图
    result_image = image.copy()

    try:
        # 提取检测数据
        head_center = detection.get('head_center')
        central_center = detection.get('central_center')
        real_center = detection.get('real_center')
        angle = detection.get('angle', 0.0)
        depth = detection.get('depth', 0.0)
        real_depth = detection.get('real_depth', 0.0)

        # 转换为整数坐标
        if head_center:
            head_center = (int(head_center[0]), int(head_center[1]))
            # 绘制head_center（红色圆点）
            cv2.circle(result_image, head_center, 5, (0, 0, 255), -1)
            # 绘制angle文本
            cv2.putText(
                result_image, 
                f"angle: {angle:.2f}", 
                (head_center[0], head_center[1] - 10), 
                cv2.FONT_HERSHEY_SIMPLEX, 
                0.5, 
                (0, 0, 255), 
                2
            )

        if central_center:
            central_center = (int(central_center[0]), int(central_center[1]))
            # 绘制central_center（绿色圆点）
            cv2.circle(result_image, central_center, 5, (0, 255, 0), -1)
            # 绘制depth文本
            cv2.putText(
                result_image, 
                f"depth: {depth:.2f}", 
                (central_center[0], central_center[1] - 10), 
                cv2.FONT_HERSHEY_SIMPLEX, 
                0.5, 
                (0, 255, 0), 
                2
            )
            # 绘制方向箭头
            ImageDrawingUtils.draw_direction_arrow(result_image, central_center, angle)

        if real_center:
            real_center = (int(real_center[0]), int(real_center[1]))
            # 绘制real_center（红色圆点）
            cv2.circle(result_image, real_center, 5, (0, 0, 255), -1)
            # 绘制real_depth文本
            cv2.putText(
                result_image, 
                f"real_depth: {real_depth:.2f}", 
                (real_center[0], real_center[1] - 10), 
                cv2.FONT_HERSHEY_SIMPLEX, 
                0.5, 
                (0, 0, 255), 
                2
            )

    except Exception as e:
        # 如果绘制失败，返回原图
        return image

    return result_image

draw_direction_arrow staticmethod 

draw_direction_arrow(image: ndarray, center: Tuple[int, int], angle: float)

绘制方向箭头。

参数：

名称 类型 描述 默认
image ndarray

图像（原地修改）。

 必需
center Tuple[int, int]

中心点坐标 (x, y)。

 必需
angle float

角度（弧度）- 直着向上为 0，顺时针为正。

 必需
源代码位于： src/controller/controller/domain/utils/image_drawing_utils.py 
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
@staticmethod
def draw_direction_arrow(image: np.ndarray, center: Tuple[int, int], angle: float):
    """绘制方向箭头。

    Args:
        image (np.ndarray): 图像（原地修改）。
        center (Tuple[int, int]): 中心点坐标 (x, y)。
        angle (float): 角度（弧度）- 直着向上为 0，顺时针为正。
    """
    try:
        # 箭头参数
        arrow_length = 50  # 箭头长度（像素）
        arrow_color = (255, 0, 0)  # 蓝色（BGR格式）
        arrow_thickness = 3

        # 计算箭头终点
        # 角度系统：直着向上为0，顺时针为正
        # 向上方向为 (0, -1)，因为图像坐标系Y轴向下
        end_x = center[0] + arrow_length * math.sin(angle)
        end_y = center[1] - arrow_length * math.cos(angle)
        end_point = (int(end_x), int(end_y))

        # 绘制箭头
        cv2.arrowedLine(
            image, 
            center, 
            end_point, 
            arrow_color, 
            arrow_thickness, 
            tipLength=0.3
        )

    except Exception as e:
        pass

KinematicDomainService 

KinematicDomainService()

运动学领域服务。

提供机械臂正逆运动学解算功能，包括DH参数管理、坐标变换矩阵计算等。

属性：

名称 类型 描述
dh_param DHParam

机械臂 DH 参数对象。
kinematic_dh ndarray

运动学 DH 参数矩阵。
kinematic_utils KinematicUtils

运动学工具类实例。
gripper2base ndarray

当前末端到基座的变换矩阵。

初始化运动学服务。

源代码位于： src/controller/controller/domain/services/algorithm/kinematic_domain_service.py 
21
22
23
24
25
26
def __init__(self):
    """初始化运动学服务。"""
    self.dh_param = DHParam()
    self.kinematic_dh = self.dh_param.get_kinematic_dh()
    self.kinematic_utils = KinematicUtils()
    self.gripper2base = self.get_gripper2base(self.kinematic_dh[:, 3])

get_kinematic_dh 

get_kinematic_dh() -> np.ndarray

获取运动学 DH 参数矩阵。

返回：

类型 描述
ndarray

np.ndarray: DH 参数矩阵。
源代码位于： src/controller/controller/domain/services/algorithm/kinematic_domain_service.py 
28
29
30
31
32
33
34
def get_kinematic_dh(self) -> np.ndarray:
    """获取运动学 DH 参数矩阵。

    Returns:
        np.ndarray: DH 参数矩阵。
    """
    return self.kinematic_dh

get_gripper2base 

get_gripper2base(theta_list: list[float] | ndarray = None) -> tuple[np.ndarray, np.ndarray]

计算末端执行器相对于基座的位姿（四元数 + 位置）。

参数：

名称 类型 描述 默认
theta_list list[float] | ndarray

关节角度列表（弧度）。如果不传则使用当前内部状态。

 None

返回：

名称 类型 描述
tuple tuple[ndarray, ndarray]

(quat, pos) - quat (np.ndarray): 姿态四元数 [x, y, z, w]。 - pos (np.ndarray): 位置坐标 [x, y, z]。
源代码位于： src/controller/controller/domain/services/algorithm/kinematic_domain_service.py 
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
def get_gripper2base(self, theta_list: list[float] | np.ndarray = None) -> tuple[np.ndarray, np.ndarray]:
    """计算末端执行器相对于基座的位姿（四元数 + 位置）。

    Args:
        theta_list (list[float] | np.ndarray, optional): 关节角度列表（弧度）。如果不传则使用当前内部状态。

    Returns:
        tuple: (quat, pos)
            - quat (np.ndarray): 姿态四元数 [x, y, z, w]。
            - pos (np.ndarray): 位置坐标 [x, y, z]。
    """
    if theta_list is not None:
        self.kinematic_dh[:, 3] = theta_list
        rm_list = []
        for dh in self.kinematic_dh:
            rm_list.append(self.kinematic_utils.dh2rm(*dh))
        self.gripper2base = reduce(lambda x, y: x @ y, rm_list, np.eye(4))
    quat = R.from_matrix(self.gripper2base[:3, :3]).as_quat()
    return quat, self.gripper2base[:3, 3]

get_gripper2base_rm 

get_gripper2base_rm(theta_list: list[float]) -> np.ndarray

获取末端执行器相对于基座的变换矩阵。

参数：

名称 类型 描述 默认
theta_list list[float]

关节角度列表（弧度）。

 必需

返回：

类型 描述
ndarray

np.ndarray: 4x4 齐次变换矩阵。
源代码位于： src/controller/controller/domain/services/algorithm/kinematic_domain_service.py 
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
def get_gripper2base_rm(self, theta_list: list[float]) -> np.ndarray:
    """获取末端执行器相对于基座的变换矩阵。

    Args:
        theta_list (list[float]): 关节角度列表（弧度）。

    Returns:
        np.ndarray: 4x4 齐次变换矩阵。
    """
    self.kinematic_dh[:, 3] = theta_list
    rm_list = []
    for dh in self.kinematic_dh:
        rm_list.append(self.kinematic_utils.dh2rm(*dh))
    self.gripper2base = reduce(lambda x, y: x @ y, rm_list, np.eye(4))
    return self.gripper2base

inverse_kinematic 

inverse_kinematic(rm: ndarray, pos: ndarray, initial_theta: list[float] | None = None) -> list[float]

机械臂逆运动学求解。

参数：

名称 类型 描述 默认
rm ndarray

目标姿态旋转矩阵 (3x3)。

 必需
pos ndarray

目标位置坐标 [x, y, z]。

 必需
initial_theta list[float] | None

初始关节角度猜测值，用于选择最优解。

 None

返回：

类型 描述
list[float]

list[float]: 最优关节角度解（弧度）。

引发：

类型 描述
ValueError

如果未找到有效的逆运动学解。
源代码位于： src/controller/controller/domain/services/algorithm/kinematic_domain_service.py 
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
def inverse_kinematic(self, rm: np.ndarray, pos: np.ndarray, initial_theta: list[float] | None = None) -> list[float]:
    """机械臂逆运动学求解。

    Args:
        rm (np.ndarray): 目标姿态旋转矩阵 (3x3)。
        pos (np.ndarray): 目标位置坐标 [x, y, z]。
        initial_theta (list[float] | None, optional): 初始关节角度猜测值，用于选择最优解。

    Returns:
        list[float]: 最优关节角度解（弧度）。

    Raises:
        ValueError: 如果未找到有效的逆运动学解。
    """
    if not initial_theta:
        initial_theta = self.kinematic_dh[:, 3]
    valid_solutions = []
    # rotation matrix
    nx, ny, nz = rm[:, 0]
    ox, oy, oz = rm[:, 1]
    ax, ay, az = rm[:, 2]
    px, py, pz = pos

    # DH参数
    a3 = 0.425 
    a4 = 0.401 
    d4 = 0.0856 
    d5 = 0.086 
    d6 = 0.231

    # 添加奇异性判断
    if abs(ax**2 + ay**2 - d4**2) < 1e-6:
        return valid_solutions

    # theta1计算
    m = ay * d6 - py
    n = ax * d6 - px
    try:
        delta = m**2 + n**2 - d4**2

        if delta < 0:
            return valid_solutions

        theta1_0 = atan2(m, n) - atan2(-d4, sqrt(m**2 + n**2 - d4**2))
        theta1_1 = atan2(m, n) - atan2(-d4, -sqrt(m**2 + n**2 - d4**2))
        theta1_candidate = [theta1_0, theta1_1]

    except Exception as e:
        return valid_solutions

    # theta5
    for i, theta1 in enumerate(theta1_candidate):
        s1 = sin(theta1)
        c1 = cos(theta1)
        # 修改theta5的计算方式
        cos_theta5 = -s1 * ax + c1 * ay
        if abs(cos_theta5) > 1:
            cos_theta5 = 1 if cos_theta5 > 0 else -1

        theta5_val = acos(cos_theta5)
        theta5_candidate = [theta5_val, -theta5_val]

        for j, theta5 in enumerate(theta5_candidate):
            s5 = sin(theta5)
            c5 = cos(theta5)

            # theta6
            if abs(s5) < 1e-6:
                # 腕部奇异性
                theta6 = initial_theta[5]
            else:
                m1 = -s1 * nx + c1 * ny
                n1 = -s1 * ox + c1 * oy
                theta6 = atan2(-n1 / s5, m1 / s5)

            # theta3
            s6 = sin(theta6)
            c6 = cos(theta6)
            m2 = d5 * (s6 * (nx * c1 + ny * s1) + c6 * (ox * c1 + oy * s1)) - d6 * (ax * c1 + ay * s1) + px * c1 + py * s1
            n2 = d5 * (oz * c6 + nz * s6) + pz - az * d6

            # 计算到关节3的距离
            r = sqrt(m2**2 + n2**2)

            if r > (a3 + a4) * 1.2 or r < abs(a3 - a4) * 0.8:  # 进一步放宽工作空间限制
                continue
            temp = (m2**2 + n2**2 - a3**2 - a4**2) / (2 * a3 * a4)
            if abs(temp) > 1:
                temp = 1 if temp > 0 else -1
            theta3_candidate = [acos(temp), -acos(temp)]

            for k, theta3 in enumerate(theta3_candidate):
                # theta2
                s3 = sin(theta3)
                c3 = cos(theta3)
                # 修改theta2的计算方式
                k1 = a3 + a4 * c3
                k2 = a4 * s3
                s2 = (k1 * n2 - k2 * m2) / (k1**2 + k2**2)
                c2 = (k1 * m2 + k2 * n2) / (k1**2 + k2**2)
                theta2 = -atan2(s2, c2)

                # theta4
                if abs(s5) < 1e-6:
                    # 在腕部奇异性时，使用更稳定的计算方式
                    if initial_theta is not None:
                        theta4_candidate = [initial_theta[3]]
                    else:
                        theta4_candidate = [initial_theta[3]]
                else:
                    T01 = self.kinematic_utils.dh2rm(self.kinematic_dh[0, 0], self.kinematic_dh[0, 1], self.kinematic_dh[0, 2], theta1)
                    T12 = self.kinematic_utils.dh2rm(self.kinematic_dh[1, 0], self.kinematic_dh[1, 1], self.kinematic_dh[1, 2], theta2)
                    T23 = self.kinematic_utils.dh2rm(self.kinematic_dh[2, 0], self.kinematic_dh[2, 1], self.kinematic_dh[2, 2], theta3)
                    R03 = (T01 @ T12 @ T23)[:3, :3]
                    R36 = R03.T @ rm
                    theta4 = atan2(R36[1, 2], R36[0, 2])
                    theta4_candidate = [-theta4, pi - theta4]
                for theta4 in theta4_candidate:
                    candidate = [theta1, theta2, theta3, theta4, theta5, theta6]
                    if self.verify_solution(candidate, (px, py, pz)):
                        valid_solutions.append(candidate)

    if not valid_solutions:
        raise ValueError("No valid solutions found")
    valid_solutions = [
        [self.kinematic_utils.normalize_angle(a) for a in sol] for sol in valid_solutions
    ]

    norm_initial_theta = [self.kinematic_utils.normalize_angle(a) for a in initial_theta]
    final_solution = min(valid_solutions, key=lambda sol: np.linalg.norm(np.array(sol) - np.array(norm_initial_theta)))

    gap = [n - i for n, i in zip(norm_initial_theta, initial_theta)]
    final_solution = [f - g for f, g in zip(final_solution, gap)]

    return final_solution

verify_solution 

verify_solution(theta_list: list[float], target_pos: ndarray) -> bool

验证逆运动学解的准确性。

参数：

名称 类型 描述 默认
theta_list list[float]

待验证的关节角度解。

 必需
target_pos ndarray

目标位置坐标。

 必需

返回：

名称 类型 描述
bool bool

如果正解位置与目标位置误差小于 1e-5 则返回 True。
源代码位于： src/controller/controller/domain/services/algorithm/kinematic_domain_service.py 
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
def verify_solution(self, theta_list: list[float], target_pos: np.ndarray) -> bool:
    """验证逆运动学解的准确性。

    Args:
        theta_list (list[float]): 待验证的关节角度解。
        target_pos (np.ndarray): 目标位置坐标。

    Returns:
        bool: 如果正解位置与目标位置误差小于 1e-5 则返回 True。
    """
    self.get_gripper2base(theta_list)
    current_pos = self.gripper2base[:3, 3]
    error = np.linalg.norm(current_pos - target_pos)

    return error < 1e-5

DynamicDomainService 

DynamicDomainService()

动力学领域服务 - 计算重力补偿力矩。

属性：

名称 类型 描述
link_masses list[float]

连杆质量列表 (kg)。
dh_matrix list

动力学 DH 参数矩阵。
link_com_positions list[ndarray]

连杆质心在本体坐标系下的位置 (m)。
g_vector ndarray

重力向量 (m/s²)。
n int

关节数量。
compensation_factor_front float

前3个关节的补偿系数。
compensation_factor_rear float

后3个关节的补偿系数。
源代码位于： src/controller/controller/domain/services/algorithm/dynamic_domain_service.py 
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
def __init__(self):
    # 连杆质量 (kg)
    self.link_masses = [4.64, 10.755, 3.925, 1.24, 1.24, 1.546]

    # 使用动力学DH参数
    dh_param = DHParam()
    self.dh_matrix = dh_param.get_dynamic_dh()

    # 连杆质心在本体坐标系下的位置 (m)
    self.link_com_positions = [
        np.array([0, 0, 0]),
        np.array([0.2125, 0, 0.134]),
        np.array([0.1818, 0, 0]),
        np.array([0, 0, 0]),
        np.array([0, 0, 0]),
        np.array([0, 0, 0.05])
    ]

    # 重力向量 (m/s²)
    self.g_vector = np.array([0, 0, -9.81])

    # 关节数量
    self.n = 6

    # 补偿系数（根据实际机械臂调整）
    self.compensation_factor_front = 0.35  # 前3个关节
    self.compensation_factor_rear = 0.25   # 后3个关节

compute_gravity_compensation 

compute_gravity_compensation(joint_angles: list[float]) -> list[float]

计算重力补偿力矩。

参数：

名称 类型 描述 默认
joint_angles list[float]

当前关节角度（弧度），6维数组。

 必需

返回：

类型 描述
list[float]

List[float]: 每个关节的重力补偿力矩（Nm），6维数组。

引发：

类型 描述
ValueError

如果 joint_angles 的长度不等于 6。
源代码位于： src/controller/controller/domain/services/algorithm/dynamic_domain_service.py 
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
def compute_gravity_compensation(self, joint_angles: list[float]) -> list[float]:
    """计算重力补偿力矩。

    Args:
        joint_angles (list[float]): 当前关节角度（弧度），6维数组。

    Returns:
        List[float]: 每个关节的重力补偿力矩（Nm），6维数组。

    Raises:
        ValueError: 如果 joint_angles 的长度不等于 6。
    """
    if len(joint_angles) != 6:
        raise ValueError("关节角度必须是6维数组")

    q = np.array(joint_angles, dtype=float)
    tau_g = np.zeros(self.n)  # 初始化补偿力矩

    # 对每个关节计算重力补偿力矩
    for i in range(self.n):
        # 对每个后续连杆（含自身）累加力矩贡献
        for j in range(i, self.n):
            # 1. 计算第j连杆质心在基坐标系下的位置
            com_base = self._get_link_com_in_base(q, j)

            # 2. 计算该质心对第i关节的雅可比列
            jacobian_col = self._get_jacobian_column(q, j, com_base, i)

            # 3. 计算重力对该关节的力矩贡献
            tau_g[i] += self.link_masses[j] * self.g_vector.dot(jacobian_col)

    # 应用补偿系数（根据实际机械臂调整）
    tau_g[:3] *= self.compensation_factor_front
    tau_g[3:] *= self.compensation_factor_rear

    # 返回负值（因为是补偿力矩）
    return (-tau_g).tolist()

MessageDomainService 

MessageDomainService(message_encoder: MessageEncoder, message_decoder: MessageDecoder)

消息领域服务。

负责消息的编码和解码，作为 MessageEncoder 和 MessageDecoder 的门面。

属性：

名称 类型 描述
message_encoder MessageEncoder

消息编码器。
message_decoder MessageDecoder

消息解码器。

初始化消息领域服务。

参数：

名称 类型 描述 默认
message_encoder MessageEncoder

注入的消息编码器。

 必需
message_decoder MessageDecoder

注入的消息解码器。

 必需
源代码位于： src/controller/controller/domain/services/communication/message_domain_service.py 
14
15
16
17
18
19
20
21
22
def __init__(self, message_encoder: MessageEncoder, message_decoder: MessageDecoder):
    """初始化消息领域服务。

    Args:
        message_encoder (MessageEncoder): 注入的消息编码器。
        message_decoder (MessageDecoder): 注入的消息解码器。
    """
    self.message_encoder = message_encoder
    self.message_decoder = message_decoder

encode_message 

encode_message(**message: Any) -> str

编码消息。

参数：

名称 类型 描述 默认
**message Any

消息字段键值对。

 {}

返回：

名称 类型 描述
str str

编码后的十六进制消息字符串。
源代码位于： src/controller/controller/domain/services/communication/message_domain_service.py 
24
25
26
27
28
29
30
31
32
33
34
def encode_message(self, **message: Any) -> str:
    """编码消息。

    Args:
        **message: 消息字段键值对。

    Returns:
        str: 编码后的十六进制消息字符串。
    """
    m = self.message_encoder.create_message(**message)
    return self.message_encoder.interpret_message(m)

decode_message 

decode_message(message: str) -> Any

解码消息。

参数：

名称 类型 描述 默认
message str

十六进制消息字符串。

 必需

返回：

名称 类型 描述
Any Any

解码后的消息对象 (MessageIn)。
源代码位于： src/controller/controller/domain/services/communication/message_domain_service.py 
36
37
38
39
40
41
42
43
44
45
def decode_message(self, message: str) -> Any:
    """解码消息。

    Args:
        message (str): 十六进制消息字符串。

    Returns:
        Any: 解码后的消息对象 (MessageIn)。
    """
    return self.message_decoder.decode_message(message)

SCurve 

SCurve(v_max=[pi / 4] * 6, acc_max=[pi / 8] * 6, t_j=0.5)

S曲线速度规划算法。

实现基于加加速度(Jerk)限制的S型速度曲线规划。

属性：

名称 类型 描述
v_max ndarray

最大速度限制。
acc_max ndarray

最大加速度限制。
t_j float

加加减速段时间。

初始化 S 曲线规划器。

参数：

名称 类型 描述 默认
v_max list[float]

最大速度. Defaults to [pi/4]*6.

 [pi / 4] * 6
acc_max list[float]

最大加速度. Defaults to [pi/8]*6.

 [pi / 8] * 6
t_j float

Jerk 时间参数. Defaults to 0.5.

 0.5
源代码位于： src/controller/controller/domain/services/algorithm/trajectory_domain_service.py 
16
17
18
19
20
21
22
23
24
25
26
def __init__(self, v_max=[pi/4] * 6, acc_max=[pi/8] * 6, t_j=0.5):
    """初始化 S 曲线规划器。

    Args:
        v_max (list[float], optional): 最大速度. Defaults to [pi/4]*6.
        acc_max (list[float], optional): 最大加速度. Defaults to [pi/8]*6.
        t_j (float, optional): Jerk 时间参数. Defaults to 0.5.
    """
    self.v_max = np.array(v_max)
    self.acc_max = np.array(acc_max)
    self.t_j = t_j

solve_cubic_numeric staticmethod 

solve_cubic_numeric(a, b, c, d, tol=1e-08)

求解三次方程实根。

源代码位于： src/controller/controller/domain/services/algorithm/trajectory_domain_service.py 
28
29
30
31
32
33
34
35
@staticmethod
def solve_cubic_numeric(a, b, c, d, tol = 1e-8):
    """求解三次方程实根。"""
    roots = np.roots([a, b, c, d])
    real_mask = np.isclose(roots.imag, 0, atol=tol)
    real_roots = roots[real_mask].real

    return real_roots

solve_quadratic staticmethod 

solve_quadratic(a, b, c, tol=1e-08)

求解二次方程实根。

源代码位于： src/controller/controller/domain/services/algorithm/trajectory_domain_service.py 
37
38
39
40
41
42
43
44
45
@staticmethod
def solve_quadratic(a, b, c, tol=1e-8):
    """求解二次方程实根。"""
    coeffs = [a, b, c]
    roots = np.roots(coeffs)
    real_mask = np.isclose(roots.imag, 0, atol=tol)
    real_roots = roots[real_mask].real

    return real_roots

base_method staticmethod 

base_method(t: float, jerk: float, a_start: float, v_start: float, s_start: float) -> tuple[float, float, float]

基础运动学方程计算。

返回：

名称 类型 描述
tuple tuple[float, float, float]

(a, v, s) 当前时刻的加速度、速度、位移。
源代码位于： src/controller/controller/domain/services/algorithm/trajectory_domain_service.py 
47
48
49
50
51
52
53
54
55
56
57
@staticmethod
def base_method(t: float, jerk: float, a_start: float, v_start: float, s_start: float) -> tuple[float, float, float]:
    """基础运动学方程计算。

    Returns:
        tuple: (a, v, s) 当前时刻的加速度、速度、位移。
    """
    a = jerk * t + a_start
    v = jerk * t**2 / 2 + a_start * t + v_start
    s = jerk * t**3 / 6 + a_start * t**2 / 2 + v_start * t + s_start
    return a, v, s

planning 

planning(start_angles: list[float], target_angles: list[float], v_start: list[float] = [0] * 6, dt: float = 0.01) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]

规划从起点到终点的S曲线轨迹。

参数：

名称 类型 描述 默认
start_angles list[float]

起点角度。

 必需
target_angles list[float]

终点角度。

 必需
v_start list[float]

起始速度. Defaults to [0]*6.

 [0] * 6
dt float

时间步长. Defaults to 0.01.

 0.01

返回：

名称 类型 描述
tuple tuple[ndarray, ndarray, ndarray, ndarray]

(times, accelerations, velocities, positions) 规划结果。
源代码位于： src/controller/controller/domain/services/algorithm/trajectory_domain_service.py 
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
def planning(self, start_angles: list[float], target_angles: list[float], v_start: list[float] = [0] * 6, dt: float = 0.01) -> tuple[np.ndarray, np.ndarray, np.ndarray, np.ndarray]:
    """规划从起点到终点的S曲线轨迹。

    Args:
        start_angles (list[float]): 起点角度。
        target_angles (list[float]): 终点角度。
        v_start (list[float], optional): 起始速度. Defaults to [0]*6.
        dt (float, optional): 时间步长. Defaults to 0.01.

    Returns:
        tuple: (times, accelerations, velocities, positions) 规划结果。
    """
    start_angles = np.array(start_angles)
    target_angles = np.array(target_angles)
    displacements = target_angles - start_angles
    abs_displacements = np.abs(displacements)
    if np.all(abs_displacements < 1e-6):
        num_samples = 1
        times = np.array([0.0])
        velocities = np.zeros((num_samples, 6))
        accelerations = np.zeros((num_samples, 6))
        positions = np.copy(start_angles).reshape((num_samples, 6))
        # 保持与其他返回路径一致的顺序：times, accelerations, velocities, positions
        return times, accelerations, velocities, positions
    max_displacement_idx = np.argmax(abs_displacements)
    max_displacement = abs_displacements[max_displacement_idx]
    a_max = self.acc_max[max_displacement_idx]
    v_max = self.v_max[max_displacement_idx]
    max_v_start = v_start[max_displacement_idx]
    jerk = a_max / self.t_j
    s_1, list_t_1, list_a_1, list_v_1, list_s_1 = self.get_boundary_1(jerk, 0, max_v_start, 0, v_max, a_max)
    s_2, t_2 = self.get_boundary_2(jerk, 0, max_v_start, 0)
    if s_1 < max_displacement:
        t_acc_1, t_acc_2, t_acc_3, t_dec_1, t_dec_2, t_dec_3 = list_t_1
        a_acc_1, a_acc_2, a_acc_3, a_dec_1, a_dec_2, a_dec_3 = list_a_1
        v_acc_1, v_acc_2, v_acc_3, v_dec_1, v_dec_2, v_dec_3 = list_v_1
        s_acc_1, s_acc_2, s_acc_3, s_dec_1, s_dec_2, s_dec_3 = list_s_1
        s_const = max_displacement - s_1
        t_const = s_const / v_max
        target_time = sum(list_t_1) + t_const
        param_arr = np.array([
            [0, 0, 0, 0, 0],
            [t_acc_1, jerk, 0, max_v_start, 0],
            [t_acc_1 + t_acc_2, 0, a_acc_1, v_acc_1, s_acc_1],
            [t_acc_1 + t_acc_2 + t_acc_3, -jerk, a_acc_2, v_acc_2, s_acc_2],
            [t_acc_1 + t_acc_2 + t_acc_3 + t_const, 0, a_acc_3, v_acc_3, s_acc_3],
            [t_acc_1 + t_acc_2 + t_acc_3 + t_const + t_dec_1, -jerk, a_acc_3, v_acc_3, s_acc_3 + s_const],
            [t_acc_1 + t_acc_2 + t_acc_3 + t_const + t_dec_1 + t_dec_2, 0, a_dec_1, v_dec_1, s_dec_1 + s_const],
            [t_acc_1 + t_acc_2 + t_acc_3 + t_const + t_dec_1 + t_dec_2 + t_dec_3, jerk, a_dec_2, v_dec_2, s_dec_2 + s_const],
        ])
        times, accelerations, velocities, positions = self.get_result(param_arr, target_time, dt, max_displacement_idx)
        accelerations, velocities, positions = self.scale_result(accelerations, 
                                                                 velocities,
                                                                 positions,
                                                                 max_displacement_idx, 
                                                                 displacements, 
                                                                 abs_displacements,
                                                                 start_angles, 
                                                                 target_angles)
        return times, accelerations, velocities, positions

    elif s_2 > max_displacement:
        t0 = max_v_start / jerk
        a = 2 * jerk
        b = jerk * t0 / 2
        c = 4 * max_v_start - jerk * t0**2 / 2
        d = -max_displacement
        t_j = self.solve_cubic_numeric(a, b, c, d)
        t_j = t_j[(t_j > 0) & (t_j < self.t_j)][0]
        s_acc_5, list_t_3, list_a_3, list_v_3, list_s_3 = self.get_stage_3(jerk, 0, max_v_start, 0, t_j, t0)
        t_acc_1, t_acc_2, t_dec_1, t_dec_2, t_dec_3 = list_t_3
        a_acc_1, a_acc_2, a_dec_1, a_dec_2, a_dec_3 = list_a_3
        v_acc_1, v_acc_2, v_dec_1, v_dec_2, v_dec_3 = list_v_3
        s_acc_1, s_acc_2, s_dec_1, s_dec_2, s_dec_3 = list_s_3
        target_time = sum(list_t_3)
        param_arr = np.array([
            [0, 0, 0, 0, 0],
            [t_acc_1, jerk, 0, max_v_start, 0],
            [t_acc_1 + t_acc_2, -jerk, a_acc_1, v_acc_1, s_acc_1],
            [t_acc_1 + t_acc_2 + t_dec_1, -jerk, a_acc_2, v_acc_2, s_acc_2],
            [t_acc_1 + t_acc_2 + t_dec_1 + t_dec_2, 0, a_dec_1, v_dec_1, s_dec_1],
            [t_acc_1 + t_acc_2 + t_dec_1 + t_dec_2 + t_dec_3, jerk, a_dec_2, v_dec_2, s_dec_2],
        ])
        times, accelerations, velocities, positions = self.get_result(param_arr, target_time, dt, max_displacement_idx)
        accelerations, velocities, positions = self.scale_result(accelerations, 
                                                                 velocities,
                                                                 positions,
                                                                 max_displacement_idx, 
                                                                 displacements, 
                                                                 abs_displacements,
                                                                 start_angles, 
                                                                 target_angles)
        return times, accelerations, velocities, positions
    else:
        a_acc_1, v_acc_1, s_acc_1 = self.base_method(self.t_j, jerk, 0, max_v_start, 0)
        t0 = max_v_start / a_acc_1
        s_t0 = a_max * t0**2 / 2 + jerk * self.t_j ** 2 * t0 / 2
        s_dec_3 = jerk * self.t_j ** 3 / 6
        delta_s = max_displacement - s_t0 - s_dec_3 - s_acc_1
        a = a_max / 2
        b = 3 / 2 * a_max * self.t_j + max_v_start
        c = 5 / 6 * a_max * self.t_j ** 2 + max_v_start * self.t_j - delta_s / 2 
        t_k = self.solve_quadratic(a, b, c)
        t_k = t_k[(4 * self.t_j + 2 * t_k + t0 > t_2) & (4 * self.t_j + 2 * t_k + t0 < sum(list_t_1))][0]
        s_dec_4, list_t_2, list_a_2, list_v_2, list_s_2 = self.get_stage_2(jerk, 0, max_v_start, 0, t_k, t0)
        t_acc_1, t_acc_2, t_acc_3, t_dec_1, t_dec_2, t_dec_3, t_dec_4 = list_t_2
        a_acc_1, a_acc_2, a_acc_3, a_dec_1, a_dec_2, a_dec_3, a_dec_4 = list_a_2
        v_acc_1, v_acc_2, v_acc_3, v_dec_1, v_dec_2, v_dec_3, v_dec_4 = list_v_2
        s_acc_1, s_acc_2, s_acc_3, s_dec_1, s_dec_2, s_dec_3, s_dec_4 = list_s_2

        target_time = sum(list_t_2)
        param_arr = np.array([
            [0, 0, 0, 0, 0],
            [t_acc_1, jerk, 0, max_v_start, 0],
            [t_acc_1 + t_acc_2, 0, a_acc_1, v_acc_1, s_acc_1],
            [t_acc_1 + t_acc_2 + t_acc_3, -jerk, a_acc_2, v_acc_2, s_acc_2],
            [t_acc_1 + t_acc_2 + t_acc_3 + t_dec_1, -jerk, a_acc_3, v_acc_3, s_acc_3],
            [t_acc_1 + t_acc_2 + t_acc_3 + t_dec_1 + t_dec_2, 0, a_dec_1, v_dec_1, s_dec_1],
            [t_acc_1 + t_acc_2 + t_acc_3 + t_dec_1 + t_dec_2 + t_dec_3, 0, a_dec_2, v_dec_2, s_dec_2],
            [t_acc_1 + t_acc_2 + t_acc_3 + t_dec_1 + t_dec_2 + t_dec_3 + t_dec_4, jerk, a_dec_3, v_dec_3, s_dec_3],
        ])
        times, accelerations, velocities, positions = self.get_result(param_arr, target_time, dt, max_displacement_idx)
        accelerations, velocities, positions = self.scale_result(accelerations, 
                                                        velocities,
                                                        positions,
                                                        max_displacement_idx, 
                                                        displacements, 
                                                        abs_displacements,
                                                        start_angles, 
                                                        target_angles)
        return times, accelerations, velocities, positions

SmoothDomainService 

SmoothDomainService()

轨迹平滑服务。

提供多种平滑算法： 1. Cubic Spline + Savitzky-Golay 滤波（默认，旧版） 2. TOPPRA 时间参数化平滑（可选，旧版测试方法）

适用场景： - 示教轨迹播放 - 多点轨迹规划 - 任何需要轨迹平滑的场景

初始化平滑服务。

源代码位于： src/controller/controller/domain/services/algorithm/smooth_domain_service.py 
29
30
31
def __init__(self):
    """初始化平滑服务。"""
    pass

smooth_trajectory 

smooth_trajectory(angles_list: List[List[float]], method: str = 'spline_savgol', **kwargs: Any) -> List[List[float]]

平滑轨迹（统一接口）。

参数：

名称 类型 描述 默认
angles_list List[List[float]]

原始轨迹 [[θ1,...,θ6], ...]。

 必需
method str

平滑方法 ("spline_savgol" 或 "toppra"). Defaults to "spline_savgol".

 'spline_savgol'
**kwargs Any

各算法的特定参数。

 {}

返回：

类型 描述
List[List[float]]

List[List[float]]: 平滑后的轨迹点列表。

引发：

类型 描述
ValueError

如果指定了未知的平滑方法。
源代码位于： src/controller/controller/domain/services/algorithm/smooth_domain_service.py 
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
def smooth_trajectory(
    self, 
    angles_list: List[List[float]], 
    method: str = "spline_savgol",
    **kwargs: Any
) -> List[List[float]]:
    """平滑轨迹（统一接口）。

    Args:
        angles_list (List[List[float]]): 原始轨迹 [[θ1,...,θ6], ...]。
        method (str, optional): 平滑方法 ("spline_savgol" 或 "toppra"). Defaults to "spline_savgol".
        **kwargs: 各算法的特定参数。

    Returns:
        List[List[float]]: 平滑后的轨迹点列表。

    Raises:
        ValueError: 如果指定了未知的平滑方法。
    """
    if method == "spline_savgol":
        return self.spline_then_savgol(angles_list, **kwargs)
    elif method == "toppra":
        return self.toppra_smooth(angles_list, **kwargs)
    else:
        raise ValueError(f"未知的平滑方法: {method}")

spline_then_savgol 

spline_then_savgol(angles_list: List[List[float]], upsample: int = 5, sg_window: int = 211, sg_poly: int = 3) -> List[List[float]]

Cubic Spline + Savitzky-Golay 滤波（参考旧版）。

流程： 1. Savitzky-Golay 平滑原始数据 2. Cubic Spline 上采样插值 3. 重采样回原始点数 4. 再次 Savitzky-Golay 平滑

参数：

名称 类型 描述 默认
angles_list List[List[float]]

原始轨迹 N×6。

 必需
upsample int

上采样倍数. Defaults to 5.

 5
sg_window int

Savgol 滤波窗口（必须是奇数）. Defaults to 211.

 211
sg_poly int

Savgol 多项式阶数. Defaults to 3.

 3

返回：

类型 描述
List[List[float]]

List[List[float]]: 平滑后的轨迹列表。
源代码位于： src/controller/controller/domain/services/algorithm/smooth_domain_service.py 
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
def spline_then_savgol(
    self,
    angles_list: List[List[float]],
    upsample: int = 5,
    sg_window: int = 211,
    sg_poly: int = 3
) -> List[List[float]]:
    """Cubic Spline + Savitzky-Golay 滤波（参考旧版）。

    流程：
    1. Savitzky-Golay 平滑原始数据
    2. Cubic Spline 上采样插值
    3. 重采样回原始点数
    4. 再次 Savitzky-Golay 平滑

    Args:
        angles_list (List[List[float]]): 原始轨迹 N×6。
        upsample (int, optional): 上采样倍数. Defaults to 5.
        sg_window (int, optional): Savgol 滤波窗口（必须是奇数）. Defaults to 211.
        sg_poly (int, optional): Savgol 多项式阶数. Defaults to 3.

    Returns:
        List[List[float]]: 平滑后的轨迹列表。
    """
    arr = np.asarray(angles_list, dtype=float)
    N, D = arr.shape

    # 确保窗口大小不超过数据点数
    sg_window = min(sg_window, N if N % 2 == 1 else N - 1)
    if sg_window < sg_poly + 2:
        sg_window = sg_poly + 2
        if sg_window % 2 == 0:
            sg_window += 1

    # 1. Savitzky–Golay平滑（先平滑）
    smoothed = savgol_filter(
        arr,
        window_length=sg_window,
        polyorder=sg_poly,
        axis=0
    )

    # 2. 三次样条插值（再插值）
    t = np.arange(N)
    t_new = np.linspace(0, N - 1, N * upsample)
    interp = np.empty((len(t_new), D), dtype=float)

    for j in range(D):
        cs = CubicSpline(t, smoothed[:, j])
        interp[:, j] = cs(t_new)

    # 3. 重采样回原始点数
    indices = np.linspace(0, len(t_new) - 1, N).astype(int)
    resampled = interp[indices, :]

    # 4. 再次平滑
    resampled = savgol_filter(
        resampled,
        window_length=sg_window,
        polyorder=sg_poly,
        axis=0
    )

    return resampled.tolist()

toppra_smooth 

toppra_smooth(waypoints: ndarray, v_max: ndarray | float, a_max: ndarray | float, dt: float = 0.01, grid_n: int = 800) -> List[List[float]]

使用 TOPPRA 进行平滑处理。

参数：

名称 类型 描述 默认
waypoints ndarray

路径点 (N, dof)。

 必需
v_max ndarray | float

最大速度。

 必需
a_max ndarray | float

最大加速度。

 必需
dt float

时间步长. Defaults to 0.01.

 0.01
grid_n int

网格数. Defaults to 800.

 800

返回：

名称 类型 描述
list List[List[float]]

平滑后的关节位置列表。

引发：

类型 描述
ValueError

参数错误。
RuntimeError

求解失败。
源代码位于： src/controller/controller/domain/services/algorithm/smooth_domain_service.py 
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
def toppra_smooth(self,
    waypoints: np.ndarray,
    v_max: np.ndarray | float,
    a_max: np.ndarray | float,
    dt: float = 0.01,
    grid_n: int = 800
) -> List[List[float]]:
    """使用 TOPPRA 进行平滑处理。

    Args:
        waypoints (np.ndarray): 路径点 (N, dof)。
        v_max (np.ndarray | float): 最大速度。
        a_max (np.ndarray | float): 最大加速度。
        dt (float, optional): 时间步长. Defaults to 0.01.
        grid_n (int, optional): 网格数. Defaults to 800.

    Returns:
        list: 平滑后的关节位置列表。

    Raises:
        ValueError: 参数错误。
        RuntimeError: 求解失败。
    """
    waypoints = np.asarray(waypoints, dtype=float)
    if waypoints.ndim != 2 or waypoints.shape[1] != 6 or waypoints.shape[0] < 2:
        raise ValueError("waypoints 必须是 (N,6) 且 N>=2")

    dof = waypoints.shape[1]

    # 1) 几何路径：用样条在路径参数 s∈[0,1] 上插值
    breaks = np.linspace(0.0, 1.0, waypoints.shape[0])
    path = ta.SplineInterpolator(breaks, waypoints)  # 官方示例同款 API

    # 2) 速度/加速度约束（上下界格式）
    v_max = np.full(dof, float(v_max)) if np.isscalar(v_max) else np.asarray(v_max, dtype=float)
    a_max = np.full(dof, float(a_max)) if np.isscalar(a_max) else np.asarray(a_max, dtype=float)
    if v_max.shape != (dof,) or a_max.shape != (dof,):
        raise ValueError("v_max/a_max 需为标量或 shape=(6,)")

    v_bounds = np.column_stack((-np.abs(v_max), np.abs(v_max)))  # (2,6) -> [v_min; v_max]
    a_bounds = np.column_stack((-np.abs(a_max), np.abs(a_max)))  # (2,6) -> [a_min; a_max]

    pc_vel = constraint.JointVelocityConstraint(v_bounds)
    pc_acc = constraint.JointAccelerationConstraint(a_bounds)

    # 3) TOPPRA 主算法 + 常用参数化器（常用且满足边界/约束）
    gridpoints = np.linspace(0, path.duration, int(grid_n))
    instance = algo.TOPPRA([pc_vel, pc_acc], path, gridpoints=gridpoints, parametrizer="ParametrizeConstAccel")

    # 4) 求解时间参数化，得到可按时间采样的 jnt_traj
    jnt_traj = instance.compute_trajectory(sd_start=0.0, sd_end=0.0)
    if jnt_traj is None:
        raise RuntimeError("TOPPRA 求解失败：给定约束下不可行，或路径异常。")

    # 5) 按 dt 采样
    T = float(jnt_traj.duration)
    M = max(2, int(np.ceil(T / dt)) + 1)
    t = np.linspace(0.0, T, M)

    q   = jnt_traj.eval(t)
    qd  = jnt_traj.evald(t)
    qdd = jnt_traj.evaldd(t)
    return q.tolist()

remove_redundant_points 

remove_redundant_points(q_teach: List[List[float]], eps: float = 0.0001) -> List[List[float]]

改进版去除重复点：保留“任意一个关节角变化超过阈值”的点。

参数：

名称 类型 描述 默认
q_teach List[List[float]]

示教点列表。

 必需
eps float

变化阈值. Defaults to 1e-4.

 0.0001

返回：

类型 描述
List[List[float]]

List[List[float]]: 去重后的点列表。
源代码位于： src/controller/controller/domain/services/algorithm/smooth_domain_service.py 
188
189
190
191
192
193
194
195
196
197
198
199
200
201
202
def remove_redundant_points(self, q_teach: List[List[float]], eps: float = 1e-4) -> List[List[float]]:
    """改进版去除重复点：保留“任意一个关节角变化超过阈值”的点。

    Args:
        q_teach (List[List[float]]): 示教点列表。
        eps (float, optional): 变化阈值. Defaults to 1e-4.

    Returns:
        List[List[float]]: 去重后的点列表。
    """
    q_teach = np.asarray(q_teach, dtype=float)
    dq = np.abs(np.diff(q_teach, axis=0))  # 相邻关节角绝对差
    moving_mask = np.any(dq > eps, axis=1)  # 只要任意一轴动了就认为在动
    mask = np.hstack([[True], moving_mask])  # 保留第一个点
    return q_teach[mask].tolist()

resample_equal_arclen 

resample_equal_arclen(q_points: List[List[float]], step: float = 0.002) -> List[List[float]]

按弧长重采样，使路径在关节空间中分布均匀。

原理： - 计算相邻点的欧氏距离（弧长） - 将弧长积分得到累计s - 对s均匀取样后，用插值函数生成新点

参数：

名称 类型 描述 默认
q_points List[List[float]]

原始点列表。

 必需
step float

重采样步长. Defaults to 0.002.

 0.002

返回：

类型 描述
List[List[float]]

List[List[float]]: 重采样后的点列表。
源代码位于： src/controller/controller/domain/services/algorithm/smooth_domain_service.py 
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
def resample_equal_arclen(self, q_points: List[List[float]], step: float = 0.002) -> List[List[float]]:
    """按弧长重采样，使路径在关节空间中分布均匀。

    原理：
    - 计算相邻点的欧氏距离（弧长）
    - 将弧长积分得到累计s
    - 对s均匀取样后，用插值函数生成新点

    Args:
        q_points (List[List[float]]): 原始点列表。
        step (float, optional): 重采样步长. Defaults to 0.002.

    Returns:
        List[List[float]]: 重采样后的点列表。
    """
    q_points = np.asarray(q_points, dtype=float)

    # 计算相邻点差值与长度（即弧长增量）
    diffs = np.diff(q_points, axis=0)
    ds = np.linalg.norm(diffs, axis=1)

    # 累计弧长 s[0]=0, s[-1]=总弧长
    s = np.hstack([[0], np.cumsum(ds)])
    s_total = s[-1]


    # 如果路径几乎没有长度（所有点都相同）
    if s_total < 1e-8:
        # 直接返回重复的同一个点
        return np.repeat(q_points[0:1], 2, axis=0)

    num_points = int(np.ceil(s_total / max(step, 1e-12))) + 1
    num_points = max(2, num_points)
    # 均匀取样 num_points 个弧长点
    s_uniform = np.linspace(0, s_total, num_points)

    # 构造弧长到关节角的插值函数（每个维度自动插值）
    kind = 'cubic' if q_points.shape[0] >= 4 else 'linear'
    interp_fun = interp1d(s, q_points, axis=0, kind=kind)

    # 根据均匀弧长取样，生成重采样轨迹
    q_eq = interp_fun(s_uniform)

    return q_eq.tolist()

affine_align_to_fixed_ends 

affine_align_to_fixed_ends(q_eq: List[List[float]], q_start_req: ndarray, q_end_req: ndarray) -> List[List[float]]

对弧长重采样后的轨迹做仿射变换（线性缩放+平移），使轨迹的首尾点精确对齐到指定的起点和终点。

数学形式： q_aligned = q_start_req + (q_eq - q0) * scale 其中 scale = (q_end_req - q_start_req) / (q1 - q0)

参数：

名称 类型 描述 默认
q_eq List[List[float]]

重采样后的轨迹。

 必需
q_start_req ndarray

要求的起点。

 必需
q_end_req ndarray

要求的终点。

 必需

返回：

类型 描述
List[List[float]]

List[List[float]]: 对齐后的轨迹。
源代码位于： src/controller/controller/domain/services/algorithm/smooth_domain_service.py 
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
def affine_align_to_fixed_ends(
    self,
    q_eq: List[List[float]],
    q_start_req: np.ndarray,
    q_end_req: np.ndarray,
) -> List[List[float]]:
    """对弧长重采样后的轨迹做仿射变换（线性缩放+平移），使轨迹的首尾点精确对齐到指定的起点和终点。

    数学形式：
    q_aligned = q_start_req + (q_eq - q0) * scale
    其中 scale = (q_end_req - q_start_req) / (q1 - q0)

    Args:
        q_eq (List[List[float]]): 重采样后的轨迹。
        q_start_req (np.ndarray): 要求的起点。
        q_end_req (np.ndarray): 要求的终点。

    Returns:
        List[List[float]]: 对齐后的轨迹。
    """
    q_eq = np.asarray(q_eq, dtype=float)
    q_start_req = np.asarray(q_start_req, dtype=float)
    q_end_req = np.asarray(q_end_req, dtype=float)

    # 获取原轨迹的首尾点
    q0, q1 = q_eq[0], q_eq[-1]

    # 防止除0：若某个关节起终角度相同，则强制除数不为0
    denom = (q1 - q0)
    denom[np.abs(denom) < 1e-9] = 1e-9

    # 每个关节单独计算缩放比例
    scale = (q_end_req - q_start_req) / denom

    # 仿射变换（线性缩放 + 平移）
    q_aligned = q_start_req + (q_eq - q0) * scale

    # 确保边界精确匹配
    q_aligned[0] = q_start_req
    q_aligned[-1] = q_end_req

    return q_aligned.tolist()

teach_smooth 

teach_smooth(q_teach: List[List[float]], q_start_req: ndarray, q_end_req: ndarray, step: float = 0.002, eps: float = 1e-06) -> List[List[float]]

示教轨迹整体平滑处理。

流程： 1. 去除停顿/重复点 2. 按弧长重采样（空间均匀） 3. 仿射缩放和平移对齐指定的起点/终点

参数：

名称 类型 描述 默认
q_teach List[List[float]]

原始示教轨迹。

 必需
q_start_req ndarray

要求的起点。

 必需
q_end_req ndarray

要求的终点。

 必需
step float

重采样步长. Defaults to 0.002.

 0.002
eps float

去重阈值. Defaults to 1e-6.

 1e-06

返回：

类型 描述
List[List[float]]

List[List[float]]: 最终平滑轨迹。
源代码位于： src/controller/controller/domain/services/algorithm/smooth_domain_service.py 
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
def teach_smooth(
    self,
    q_teach: List[List[float]],
    q_start_req: np.ndarray,
    q_end_req: np.ndarray,
    step: float = 0.002,
    eps: float = 1e-6,
) -> List[List[float]]:
    """示教轨迹整体平滑处理。

    流程：
    1. 去除停顿/重复点
    2. 按弧长重采样（空间均匀）
    3. 仿射缩放和平移对齐指定的起点/终点

    Args:
        q_teach (List[List[float]]): 原始示教轨迹。
        q_start_req (np.ndarray): 要求的起点。
        q_end_req (np.ndarray): 要求的终点。
        step (float, optional): 重采样步长. Defaults to 0.002.
        eps (float, optional): 去重阈值. Defaults to 1e-6.

    Returns:
        List[List[float]]: 最终平滑轨迹。
    """

LinearMotionDomainService 

LinearMotionDomainService(v_max=[pi / 4] * 6, a_max=[pi / 8] * 6, j_max=[pi / 16] * 6, dt=0.01)

直线运动规划服务。

提供空间直线插值运动规划功能，支持指定起点终点或指定方向距离的规划。

属性：

名称 类型 描述
v_max ndarray

各关节最大速度。
a_max ndarray

各关节最大加速度。
j_max ndarray

各关节最大加加速度。
dt float

时间步长。
kinematic_solver KinematicDomainService

运动学求解器。

初始化直线运动服务。

参数：

名称 类型 描述 默认
v_max list[float]

最大速度列表. Defaults to [pi/4]*6.

 [pi / 4] * 6
a_max list[float]

最大加速度列表. Defaults to [pi/8]*6.

 [pi / 8] * 6
j_max list[float]

最大加加速度列表. Defaults to [pi/16]*6.

 [pi / 16] * 6
dt float

时间步长. Defaults to 0.01.

 0.01
源代码位于： src/controller/controller/domain/services/algorithm/linear_motion_domain_service.py 
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
def __init__(self, v_max=[pi/4] * 6, a_max=[pi/8] * 6, j_max=[pi/16] * 6, dt=0.01):
    """初始化直线运动服务。

    Args:
        v_max (list[float], optional): 最大速度列表. Defaults to [pi/4]*6.
        a_max (list[float], optional): 最大加速度列表. Defaults to [pi/8]*6.
        j_max (list[float], optional): 最大加加速度列表. Defaults to [pi/16]*6.
        dt (float, optional): 时间步长. Defaults to 0.01.
    """
    self.v_max = np.asarray(v_max,dtype=float)
    self.a_max = np.asarray(a_max,dtype=float)
    self.j_max = np.asarray(j_max,dtype=float)
    self.dt = float(dt)
    self.kinematic_solver = KinematicDomainService()
    self.nearest_position = []

clamp 

clamp(x, low, high)

限制数值范围。

源代码位于： src/controller/controller/domain/services/algorithm/linear_motion_domain_service.py 
39
40
41
def clamp(self, x, low, high):
    """限制数值范围。"""
    return max(low, min(x, high))

linear_motion 

linear_motion(start_position: list[float], end_position: list[float]) -> tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]

规划两点间的直线运动。

参数：

名称 类型 描述 默认
start_position list[float]

起点关节角度。

 必需
end_position list[float]

终点关节角度。

 必需

返回：

名称 类型 描述
tuple tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]

(t_list, positions, qd, qdd) 轨迹数据。
源代码位于： src/controller/controller/domain/services/algorithm/linear_motion_domain_service.py 
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
def linear_motion(self, start_position: list[float], end_position: list[float]) -> tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]:
    """规划两点间的直线运动。

    Args:
        start_position (list[float]): 起点关节角度。
        end_position (list[float]): 终点关节角度。

    Returns:
        tuple: (t_list, positions, qd, qdd) 轨迹数据。
    """
    self.nearest_position = start_position
    start_quat, start_pos = self.kinematic_solver.get_gripper2base(start_position)
    end_quat, end_pos = self.kinematic_solver.get_gripper2base(end_position)
    quat_list, pos_list, n_seg = self.sampling(start_quat, start_pos, end_quat, end_pos)
    t_list, positions, qd, qdd = self.smooth(quat_list, pos_list, n_seg)
    return t_list, positions, qd, qdd

linear_motion_z_axis 

linear_motion_z_axis(start_position: list[float], distance: float, direction: list[float], ds: float = 0.002, include_end: bool = True) -> tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]

沿指定方向矢量进行直线运动规划。

注意：此模式下末端姿态保持不变，仅位置发生位移。

参数：

名称 类型 描述 默认
start_position list[float]

起点关节角度。

 必需
distance float

移动距离 (m)。

 必需
direction list[float]

方向矢量 [x, y, z]。

 必需
ds float

空间采样步长. Defaults to 0.002.

 0.002
include_end bool

是否包含终点. Defaults to True.

 True

返回：

名称 类型 描述
tuple tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]

(t_list, positions, qd, qdd) 轨迹数据。

引发：

类型 描述
ValueError

如果方向向量模长过小。
源代码位于： src/controller/controller/domain/services/algorithm/linear_motion_domain_service.py 
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
def linear_motion_z_axis(self, start_position: list[float], distance: float, direction: list[float], ds: float = 0.002, include_end: bool = True) -> tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]:
    """沿指定方向矢量进行直线运动规划。

    注意：此模式下末端姿态保持不变，仅位置发生位移。

    Args:
        start_position (list[float]): 起点关节角度。
        distance (float): 移动距离 (m)。
        direction (list[float]): 方向矢量 [x, y, z]。
        ds (float, optional): 空间采样步长. Defaults to 0.002.
        include_end (bool, optional): 是否包含终点. Defaults to True.

    Returns:
        tuple: (t_list, positions, qd, qdd) 轨迹数据。

    Raises:
        ValueError: 如果方向向量模长过小。
    """
    self.nearest_position = start_position
    start_quat, start_pos = self.kinematic_solver.get_gripper2base(start_position)
    # todo: 按照distance计算终点位置
    p0 = np.asarray(start_pos, dtype=float).reshape(3)
    q0 = self._q_normalize(start_quat)

    direction = np.asarray(direction, dtype=float)
    norm = np.linalg.norm(direction)
    if norm < 1e-9:
        raise ValueError("Direction vector is zero or too small")
    direction = direction / norm

    # 需要的段数（保证每步不超过 ds）
    L = abs(float(distance))
    if L < 1e-12:
        t = np.array([0.0, 1.0]) if include_end else np.array([0.0])
    else:
        n_seg = max(1, int(np.ceil(L / ds)))
        t = np.linspace(0.0, 1.0, n_seg + 1 if include_end else n_seg)

    # 直线方向：xiangliang
    d = direction * float(distance)

    # 位置线性采样；姿态保持不变
    pos_list = p0[None, :] + t[:, None] * d[None, :]
    quat_list = np.repeat(q0[None, :], len(t), axis=0)

    t_list, positions, qd, qdd = self.smooth(quat_list, pos_list, n_seg)

    return t_list, positions, qd, qdd

sampling 

sampling(start_quat: ndarray, start_pos: ndarray, end_quat: ndarray, end_pos: ndarray, sampling_dis: float = 0.002, include_end: bool = True) -> tuple[np.ndarray, np.ndarray, int]

对起点和终点进行空间线性插值采样。

参数：

名称 类型 描述 默认
start_quat ndarray

起点四元数。

 必需
start_pos ndarray

起点位置。

 必需
end_quat ndarray

终点四元数。

 必需
end_pos ndarray

终点位置。

 必需
sampling_dis float

采样间距. Defaults to 0.002.

 0.002
include_end bool

是否包含终点. Defaults to True.

 True

返回：

名称 类型 描述
tuple tuple[ndarray, ndarray, int]

(quat_list, pos_list, n_seg)
源代码位于： src/controller/controller/domain/services/algorithm/linear_motion_domain_service.py 
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
def sampling(self, start_quat: np.ndarray, start_pos: np.ndarray, end_quat: np.ndarray, end_pos: np.ndarray, sampling_dis: float = 0.002, include_end: bool = True) -> tuple[np.ndarray, np.ndarray, int]:
    """对起点和终点进行空间线性插值采样。

    Args:
        start_quat (np.ndarray): 起点四元数。
        start_pos (np.ndarray): 起点位置。
        end_quat (np.ndarray): 终点四元数。
        end_pos (np.ndarray): 终点位置。
        sampling_dis (float, optional): 采样间距. Defaults to 0.002.
        include_end (bool, optional): 是否包含终点. Defaults to True.

    Returns:
        tuple: (quat_list, pos_list, n_seg)
    """
    quat_list = []
    pos_list = []
    # todo: 按照起始四元数+位置和终点四元数+位置以及采样间隔0.02m，进行采样
    p0=start_pos
    p1=end_pos
    p0 = np.asarray(p0, dtype=float).reshape(3)
    p1 = np.asarray(p1, dtype=float).reshape(3)

    # 兼容两种四元数顺序：若最后一项绝对值最大，视为 [x,y,z,w]，转成 [w,x,y,z]
    q0=start_quat
    q1=end_quat
    q0 = np.asarray(q0, dtype=float).reshape(4)
    q1 = np.asarray(q1, dtype=float).reshape(4)
    # if np.argmax(np.abs(q0)) == 3:  # 可能是 [x,y,z,w]
    #     q0 = np.roll(q0, 1)
    # if np.argmax(np.abs(q1)) == 3:
    #     q1 = np.roll(q1, 1)

    # 计算需要的采样点数
    L = np.linalg.norm(p1 - p0)
    if L < 1e-9:
        t = np.array([0.0, 1.0]) if include_end else np.array([0.0])
    else:
        n_seg = max(1, int(np.ceil(L / float(sampling_dis))))
        t = np.linspace(0.0, 1.0, n_seg + 1 if include_end else n_seg)

    # 位置线性插值
    pos_list = (p0[None, :] + (p1 - p0)[None, :] * t[:, None])

    # 姿态 SLERP
    quat_list = np.vstack([self._q_slerp(q0, q1, ti) for ti in t])


    return quat_list, pos_list, n_seg

smooth 

smooth(quat_list: ndarray, pos_list: ndarray, n_seg: int) -> tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]

对采样点序列进行逆运动学求解和时间参数化平滑。

参数：

名称 类型 描述 默认
quat_list ndarray

四元数序列。

 必需
pos_list ndarray

位置序列。

 必需
n_seg int

分段数。

 必需

返回：

名称 类型 描述
tuple tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]

(t_list, positions, qd, qdd)
源代码位于： src/controller/controller/domain/services/algorithm/linear_motion_domain_service.py 
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
def smooth(self, quat_list: np.ndarray, pos_list: np.ndarray, n_seg: int) -> tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]:
    """对采样点序列进行逆运动学求解和时间参数化平滑。

    Args:
        quat_list (np.ndarray): 四元数序列。
        pos_list (np.ndarray): 位置序列。
        n_seg (int): 分段数。

    Returns:
        tuple: (t_list, positions, qd, qdd) 
    """
    positions = []
    for quat, pos in zip(quat_list, pos_list):
        position = self.inverse_kinematic(quat, pos)
        positions.append(position)
    positions = np.array(positions)
    # todo: 基于这个positions列表，规划rucking smooth

    q_wp = self.ensure_2d_array(positions)
    grid_n = self.clamp(6*n_seg,300,3000)    
    t_list, positions, qd, qdd= self.toppra_time_parameterize(q_wp, self.v_max, self.a_max, self.dt, grid_n)
    return t_list, positions, qd, qdd

inverse_kinematic 

inverse_kinematic(quat, pos)

单点逆运动学求解（利用上一位置作为初值）。

源代码位于： src/controller/controller/domain/services/algorithm/linear_motion_domain_service.py 
203
204
205
206
207
208
def inverse_kinematic(self, quat, pos):
    """单点逆运动学求解（利用上一位置作为初值）。"""
    rm = R.from_quat(quat).as_matrix()
    inverse_position = self.kinematic_solver.inverse_kinematic(rm, pos, initial_theta=self.nearest_position)
    self.nearest_position = inverse_position
    return inverse_position

wrap_to_pi 

wrap_to_pi(q: ndarray) -> np.ndarray

将角度包裹到 (-pi, pi]，避免 2π 跳变影响计算。

源代码位于： src/controller/controller/domain/services/algorithm/linear_motion_domain_service.py 
218
219
220
def wrap_to_pi(self,q: np.ndarray) -> np.ndarray:
    """将角度包裹到 (-pi, pi]，避免 2π 跳变影响计算。"""
    return (q + np.pi) % (2 * np.pi) - np.pi

ensure_2d_array 

ensure_2d_array(arr: ndarray) -> np.ndarray

将输入转换为二维数组 (N, dof) 并做基本校验。

引发：

类型 描述
ValueError

当路标数少于 2（缺少起点或终点）时抛出。
源代码位于： src/controller/controller/domain/services/algorithm/linear_motion_domain_service.py 
222
223
224
225
226
227
228
229
230
231
232
233
234
def ensure_2d_array(self,arr: np.ndarray) -> np.ndarray:
    """
    将输入转换为二维数组 (N, dof) 并做基本校验。

    Raises:
        ValueError: 当路标数少于 2（缺少起点或终点）时抛出。
    """
    q = np.asarray(arr, dtype=float)
    if q.ndim == 1:
        q = q[None, :]
    if not (q.ndim == 2 and q.shape[0] >= 2):
        raise ValueError("Q_waypoints 至少需要两个点（起点与终点）且维度为 (N, dof)。")
    return q

toppra_time_parameterize 

toppra_time_parameterize(waypoints: ndarray, v_max: ndarray | float, a_max: ndarray | float, dt: float = 0.01, grid_n: int = 800) -> tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]

使用 TOPPRA 进行时间参数化。

参数：

名称 类型 描述 默认
waypoints ndarray

路径点 (N, dof)。

 必需
v_max ndarray | float

最大速度。

 必需
a_max ndarray | float

最大加速度。

 必需
dt float

时间步长. Defaults to 0.01.

 0.01
grid_n int

网格点数. Defaults to 800.

 800

返回：

名称 类型 描述
tuple tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]

(t, q, qd, qdd).

引发：

类型 描述
ValueError

如果参数形状不正确。
RuntimeError

如果求解失败。
源代码位于： src/controller/controller/domain/services/algorithm/linear_motion_domain_service.py 
236
237
238
239
240
241
242
243
244
245
246
247
248
249
250
251
252
253
254
255
256
257
258
259
260
261
262
263
264
265
266
267
268
269
270
271
272
273
274
275
276
277
278
279
280
281
282
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
def toppra_time_parameterize(self,
waypoints: np.ndarray,
v_max: np.ndarray | float,
a_max: np.ndarray | float,
dt: float = 0.01,
grid_n: int = 800
) -> tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]:
    """使用 TOPPRA 进行时间参数化。

    Args:
        waypoints (np.ndarray): 路径点 (N, dof)。
        v_max (np.ndarray | float): 最大速度。
        a_max (np.ndarray | float): 最大加速度。
        dt (float, optional): 时间步长. Defaults to 0.01.
        grid_n (int, optional): 网格点数. Defaults to 800.

    Returns:
        tuple: (t, q, qd, qdd).

    Raises:
        ValueError: 如果参数形状不正确。
        RuntimeError: 如果求解失败。
    """
    waypoints = np.asarray(waypoints, dtype=float)
    if waypoints.ndim != 2 or waypoints.shape[1] != 6 or waypoints.shape[0] < 2:
        raise ValueError("waypoints 必须是 (N,6) 且 N>=2")

    dof = waypoints.shape[1]

    # 1) 几何路径：用样条在路径参数 s∈[0,1] 上插值
    breaks = np.linspace(0.0, 1.0, waypoints.shape[0])
    path = ta.SplineInterpolator(breaks, waypoints)  # 官方示例同款 API

    # 2) 速度/加速度约束（上下界格式）
    v_max = np.full(dof, float(v_max)) if np.isscalar(v_max) else np.asarray(v_max, dtype=float)
    a_max = np.full(dof, float(a_max)) if np.isscalar(a_max) else np.asarray(a_max, dtype=float)
    if v_max.shape != (dof,) or a_max.shape != (dof,):
        raise ValueError("v_max/a_max 需为标量或 shape=(6,)")

    v_bounds = np.column_stack((-np.abs(v_max), np.abs(v_max)))  # (2,6) -> [v_min; v_max]
    a_bounds = np.column_stack((-np.abs(a_max), np.abs(a_max)))  # (2,6) -> [a_min; a_max]

    pc_vel = constraint.JointVelocityConstraint(v_bounds)
    pc_acc = constraint.JointAccelerationConstraint(a_bounds)

    # 3) TOPPRA 主算法 + 常用参数化器（常用且满足边界/约束）
    gridpoints = np.linspace(0, path.duration, int(grid_n))
    instance = algo.TOPPRA([pc_vel, pc_acc], path, gridpoints=gridpoints, parametrizer="ParametrizeConstAccel")

    # 4) 求解时间参数化，得到可按时间采样的 jnt_traj
    jnt_traj = instance.compute_trajectory(sd_start=0.0, sd_end=0.0)
    if jnt_traj is None:
        raise RuntimeError("TOPPRA 求解失败：给定约束下不可行，或路径异常。")

    # 5) 按 dt 采样
    T = float(jnt_traj.duration)
    M = max(2, int(np.ceil(T / dt)) + 1)
    t = np.linspace(0.0, T, M)

    q   = jnt_traj.eval(t)
    qd  = jnt_traj.evald(t)
    qdd = jnt_traj.evaldd(t)
    return t.tolist(), q.tolist(), qd.tolist(), qdd.tolist()

CurveMotionDomainService 

CurveMotionDomainService(kinematic_solver: KinematicDomainService, v_max=[pi / 4] * 6, a_max=[pi / 8] * 6, j_max=[pi / 16] * 6, dt=0.01)

曲线运动规划服务。

提供基于弧长参数化的空间曲线规划、姿态平滑插值以及基于 TOPPRA 的时间参数化功能。

属性：

名称 类型 描述
v_max ndarray

各关节最大速度 (rad/s)。
a_max ndarray

各关节最大加速度 (rad/s²)。
j_max ndarray

各关节最大加加速度 (rad/s³)。
dt float

时间步长 (s)。
kinematic_solver KinematicDomainService

运动学求解器实例。

初始化曲线运动服务。

参数：

名称 类型 描述 默认
kinematic_solver KinematicDomainService

运动学求解器服务实例。

 必需
v_max list[float]

最大速度列表. Defaults to [pi/4]*6.

 [pi / 4] * 6
a_max list[float]

最大加速度列表. Defaults to [pi/8]*6.

 [pi / 8] * 6
j_max list[float]

最大加加速度列表. Defaults to [pi/16]*6.

 [pi / 16] * 6
dt float

采样时间步长. Defaults to 0.01.

 0.01
源代码位于： src/controller/controller/domain/services/algorithm/curve_motion.py 
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
def __init__(
    self, 
    kinematic_solver: KinematicDomainService,
    v_max=[pi / 4] * 6, 
    a_max=[pi / 8] * 6, 
    j_max=[pi / 16] * 6, 
    dt=0.01,
):
    """初始化曲线运动服务。

    Args:
        kinematic_solver: 运动学求解器服务实例。
        v_max (list[float], optional): 最大速度列表. Defaults to [pi/4]*6.
        a_max (list[float], optional): 最大加速度列表. Defaults to [pi/8]*6.
        j_max (list[float], optional): 最大加加速度列表. Defaults to [pi/16]*6.
        dt (float, optional): 采样时间步长. Defaults to 0.01.
    """
    self.v_max = np.asarray(v_max, dtype=float)
    self.a_max = np.asarray(a_max, dtype=float)
    self.j_max = np.asarray(j_max, dtype=float)
    self.dt = float(dt)
    self.kinematic_solver = kinematic_solver
    self.nearest_position = None

clamp 

clamp(x, low, high)

将值 x 限制在 [low, high] 区间内。

源代码位于： src/controller/controller/domain/services/algorithm/curve_motion.py 
50
51
52
def clamp(self, x, low, high):
    """将值 x 限制在 [low, high] 区间内。"""
    return max(low, min(x, high))

curve_motion 

curve_motion(pos_fun, u0: float, u1: float, start_position: list[float], end_position: list[float] | None, ds: float = 0.002, include_end: bool = True, orientation_mode: str = 'slerp', tool_axis: str = 'z', up_hint: ndarray = np.array([0, 0, 1.0])) -> tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]

执行曲线运动规划。

参数：

名称 类型 描述 默认
pos_fun callable

位置函数，接受参数 u 返回 (3,) 坐标。

 必需
u0 float

参数起始值。

 必需
u1 float

参数终止值。

 必需
start_position list[float]

起点关节角度。

 必需
end_position list[float] | None

终点关节角度。

 必需
ds float

采样弧长步长. Defaults to 0.002.

 0.002
include_end bool

是否包含终点. Defaults to True.

 True
orientation_mode str

姿态插值模式 ('fixed'|'slerp'|'tangent'). Defaults to "slerp".

 'slerp'
tool_axis str

工具轴方向 ('x'|'y'|'z'). Defaults to "z".

 'z'
up_hint ndarray

向上向量提示，用于切向跟随模式. Defaults to [0, 0, 1.0].

 array([0, 0, 1.0])

返回：

名称 类型 描述
tuple tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]

(t_list, positions, qd, qdd) - t_list: 时间戳列表 - positions: 关节角度轨迹列表 - qd: 关节速度轨迹列表 - qdd: 关节加速度轨迹列表

引发：

类型 描述
ValueError

如果 orientation_mode 无效或 slerp 模式下未提供 end_position。
源代码位于： src/controller/controller/domain/services/algorithm/curve_motion.py 
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
def curve_motion(
        self,
        pos_fun,  # callable u -> (3,)
        u0: float,
        u1: float,
        start_position: list[float],
        end_position: list[float] | None,
        ds: float = 0.002,
        include_end: bool = True,
        orientation_mode: str = "slerp",
        tool_axis: str = "z",
        up_hint: np.ndarray = np.array([0, 0, 1.0])
) -> tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]:
    """执行曲线运动规划。

    Args:
        pos_fun (callable): 位置函数，接受参数 u 返回 (3,) 坐标。
        u0 (float): 参数起始值。
        u1 (float): 参数终止值。
        start_position (list[float]): 起点关节角度。
        end_position (list[float] | None): 终点关节角度。
        ds (float, optional): 采样弧长步长. Defaults to 0.002.
        include_end (bool, optional): 是否包含终点. Defaults to True.
        orientation_mode (str, optional): 姿态插值模式 ('fixed'|'slerp'|'tangent'). Defaults to "slerp".
        tool_axis (str, optional): 工具轴方向 ('x'|'y'|'z'). Defaults to "z".
        up_hint (np.ndarray, optional): 向上向量提示，用于切向跟随模式. Defaults to [0, 0, 1.0].

    Returns:
        tuple: (t_list, positions, qd, qdd)
            - t_list: 时间戳列表
            - positions: 关节角度轨迹列表
            - qd: 关节速度轨迹列表
            - qdd: 关节加速度轨迹列表

    Raises:
        ValueError: 如果 orientation_mode 无效或 slerp 模式下未提供 end_position。
    """
    # 起点姿态来自起点关节角的正解
    start_quat, start_pos_fk = self.kinematic_solver.get_gripper2base(start_position)
    # 1) 等弧长采样
    pos_list, u_eq, n_seg = self.sample_parametric_equal_arclen(pos_fun, u0, u1, ds, include_end)

    # 2) 姿态生成
    if orientation_mode == "fixed":
        quat_list = np.repeat(self._q_normalize(start_quat)[None, :], len(pos_list), axis=0)

    elif orientation_mode == "slerp":
        if end_position is None:
            raise ValueError("slerp 模式需要提供 end_position 以确定终止姿态。")
        end_quat, _ = self.kinematic_solver.get_gripper2base(end_position)
        q0 = self._q_normalize(start_quat)
        q1 = self._q_normalize(end_quat)
        tau = np.linspace(0, 1, len(pos_list))
        quat_list = np.vstack([self._q_slerp(q0, q1, ti) for ti in tau])

    elif orientation_mode == "tangent":
        quat_list = self._quat_follow_tangent(pos_list, tool_axis=tool_axis, up_hint=up_hint)
    else:
        raise ValueError("orientation_mode 必须是 {'fixed','slerp','tangent'}")

    # 3) 原有平滑/IK/TOPPRA 管线复用
    t_list, positions, qd, qdd = self.smooth(quat_list, pos_list, n_seg)
    return t_list, positions, qd, qdd

make_pos_fun_spline 

make_pos_fun_spline(start_position: list[float], end_position: list[float], mid_points: list, bc_type: str = 'natural') -> tuple[callable, np.ndarray]

构造基于三次样条的位置函数。

参数：

名称 类型 描述 默认
start_position list[float]

起点关节角度。

 必需
end_position list[float]

终点关节角度。

 必需
mid_points list

中间点坐标列表 (N, 3)。

 必需
bc_type str

边界条件类型. Defaults to "natural".

 'natural'

返回：

名称 类型 描述
tuple tuple[callable, ndarray]

(pos_fun, s) - pos_fun: 位置函数 callable - s: 累积弦长数组

引发：

类型 描述
ValueError

如果点集形状不正确。
源代码位于： src/controller/controller/domain/services/algorithm/curve_motion.py 
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
def make_pos_fun_spline(
    self, 
    start_position: list[float],
    end_position: list[float],
    mid_points: list, 
    bc_type: str = "natural") -> tuple[callable, np.ndarray]:
    """构造基于三次样条的位置函数。

    Args:
        start_position (list[float]): 起点关节角度。
        end_position (list[float]): 终点关节角度。
        mid_points (list): 中间点坐标列表 (N, 3)。
        bc_type (str, optional): 边界条件类型. Defaults to "natural".

    Returns:
        tuple: (pos_fun, s)
            - pos_fun: 位置函数 callable
            - s: 累积弦长数组

    Raises:
        ValueError: 如果点集形状不正确。
    """
    _, start_pos = self.kinematic_solver.get_gripper2base(start_position)
    _, end_pos = self.kinematic_solver.get_gripper2base(end_position)
    points = np.vstack([start_pos, mid_points, end_pos])
    P = np.asarray(points, dtype=float)
    if P.ndim != 2 or P.shape[0] < 2 or P.shape[1] not in (2, 3):
        raise ValueError("points 形状必须是 (N,3) 或 (N,2)，且 N>=2")
    if P.shape[1] == 2:
        P = np.hstack([P, np.zeros((P.shape[0], 1))])  # 2D 自动补 z=0

    # --- 弦长参数化，避免参数拥挤 ---
    d = np.linalg.norm(np.diff(P, axis=0), axis=1)
    s = np.concatenate([[0.0], np.cumsum(d)])
    if s[-1] == 0:
        raise ValueError("所有点重合，无法构造曲线")
    t = s / s[-1]  # 归一化到 [0,1]

    # 分别构造 x,y,z 的样条
    cs_x = CubicSpline(t, P[:, 0], bc_type=bc_type)
    cs_y = CubicSpline(t, P[:, 1], bc_type=bc_type)
    cs_z = CubicSpline(t, P[:, 2], bc_type=bc_type)

    def _eval(u):
        u = np.asarray(u, dtype=float)
        # 可选：夹紧到 [0,1]，避免数值越界
        u = np.clip(u, 0.0, 1.0)
        X = np.stack([cs_x(u), cs_y(u), cs_z(u)], axis=-1)  # (..., 3)
        if X.ndim == 1:  # 标量 u -> (3,)
            return X
        return X  # 数组 u -> (M,3)

    return _eval, s

sample_parametric_equal_arclen 

sample_parametric_equal_arclen(pos_fun, u0: float, u1: float, ds: float = 0.002, include_end: bool = True, dense: int = 4000) -> tuple[np.ndarray, np.ndarray, int]

对参数曲线 r(u) 做等弧长采样。

参数：

名称 类型 描述 默认
pos_fun callable

参数曲线函数 r(u)。

 必需
u0 float

参数起始值。

 必需
u1 float

参数终止值。

 必需
ds float

目标弧长采样间隔. Defaults to 0.002.

 0.002
include_end bool

结果是否必须包含终点. Defaults to True.

 True
dense int

初始密采样的点数. Defaults to 4000.

 4000

返回：

名称 类型 描述
tuple tuple[ndarray, ndarray, int]

(pos_list, u_eq, n_seg) - pos_list: 等弧长采样后的坐标列表 (M, 3) - u_eq: 对应的参数值列表 (M,) - n_seg: 分段数
源代码位于： src/controller/controller/domain/services/algorithm/curve_motion.py 
195
196
197
198
199
200
201
202
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
def sample_parametric_equal_arclen(
        self,
        pos_fun,  # callable: u -> (3,) numpy array
        u0: float,
        u1: float,
        ds: float = 0.002,
        include_end: bool = True,
        dense: int = 4000
) -> tuple[np.ndarray, np.ndarray, int]:
    """对参数曲线 r(u) 做等弧长采样。

    Args:
        pos_fun (callable): 参数曲线函数 r(u)。
        u0 (float): 参数起始值。
        u1 (float): 参数终止值。
        ds (float, optional): 目标弧长采样间隔. Defaults to 0.002.
        include_end (bool, optional): 结果是否必须包含终点. Defaults to True.
        dense (int, optional): 初始密采样的点数. Defaults to 4000.

    Returns:
        tuple: (pos_list, u_eq, n_seg)
            - pos_list: 等弧长采样后的坐标列表 (M, 3)
            - u_eq: 对应的参数值列表 (M,)
            - n_seg: 分段数
    """
    # 1) 先在参数上做均匀密采样，估算弧长
    u_dense = np.linspace(u0, u1, max(1000, int(abs(u1 - u0) * dense)))
    XYZ = np.vstack([np.asarray(pos_fun(ui), dtype=float).reshape(3) for ui in u_dense])
    seg = np.linalg.norm(np.diff(XYZ, axis=0), axis=1)
    s_cum = np.concatenate([[0.0], np.cumsum(seg)])
    L = float(s_cum[-1])
    if L < 1e-12:
        # 退化：几乎零长
        pos_list = XYZ[[0, -1], :] if include_end else XYZ[[0], :]
        u_eq = np.array([u_dense[0], u_dense[-1]]) if include_end else np.array([u_dense[0]])
        return pos_list, u_eq, 0.0

    # 2) 弧长 -> u 的反查，用插值近似
    s2u = interp1d(s_cum, u_dense, kind="linear", bounds_error=False, fill_value=(u_dense[0], u_dense[-1]))

    # 3) 目标等弧长序列
    n_seg = max(1, int(np.ceil(L / ds)))
    s_target = np.linspace(0.0, L, n_seg + 1 if include_end else n_seg)
    u_eq = s2u(s_target)

    # 4) 得到等弧长位置
    pos_list = np.vstack([np.asarray(pos_fun(ui), dtype=float).reshape(3) for ui in u_eq])
    return pos_list, u_eq, n_seg

wrap_to_pi 

wrap_to_pi(q: ndarray) -> np.ndarray

将角度包裹到 (-pi, pi]，避免 2π 跳变影响计算。

源代码位于： src/controller/controller/domain/services/algorithm/curve_motion.py 
303
304
305
def wrap_to_pi(self, q: np.ndarray) -> np.ndarray:
    """将角度包裹到 (-pi, pi]，避免 2π 跳变影响计算。"""
    return (q + np.pi) % (2 * np.pi) - np.pi

ensure_2d_array 

ensure_2d_array(arr: ndarray) -> np.ndarray

将输入转换为二维数组 (N, dof) 并做基本校验。

Raises

ValueError 当路标数少于 2（缺少起点或终点）时抛出。

源代码位于： src/controller/controller/domain/services/algorithm/curve_motion.py 
307
308
309
310
311
312
313
314
315
316
317
318
319
320
321
def ensure_2d_array(self, arr: np.ndarray) -> np.ndarray:
    """
    将输入转换为二维数组 (N, dof) 并做基本校验。

    Raises
    ------
    ValueError
        当路标数少于 2（缺少起点或终点）时抛出。
    """
    q = np.asarray(arr, dtype=float)
    if q.ndim == 1:
        q = q[None, :]
    if not (q.ndim == 2 and q.shape[0] >= 2):
        raise ValueError("Q_waypoints 至少需要两个点（起点与终点）且维度为 (N, dof)。")
    return q

toppra_time_parameterize 

toppra_time_parameterize(waypoints: ndarray, v_max: ndarray | float, a_max: ndarray | float, dt: float = 0.01, grid_n: int = 400) -> tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]

使用 TOPPRA 对路径进行时间参数化。

参数：

名称 类型 描述 默认
waypoints ndarray

路径点数组 (N, dof)。

 必需
v_max ndarray | float

最大速度约束。

 必需
a_max ndarray | float

最大加速度约束。

 必需
dt float

输出轨迹的时间步长. Defaults to 0.01.

 0.01
grid_n int

TOPPRA 离散化网格点数. Defaults to 400.

 400

返回：

名称 类型 描述
tuple tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]

(t, q, qd, qdd) - t: 时间戳列表 - q: 关节位置列表 - qd: 关节速度列表 - qdd: 关节加速度列表

引发：

类型 描述
ValueError

如果 waypoints 维度不正确或约束格式错误。
RuntimeError

如果 TOPPRA 求解失败。
源代码位于： src/controller/controller/domain/services/algorithm/curve_motion.py 
323
324
325
326
327
328
329
330
331
332
333
334
335
336
337
338
339
340
341
342
343
344
345
346
347
348
349
350
351
352
353
354
355
356
357
358
359
360
361
362
363
364
365
366
367
368
369
370
371
372
373
374
375
376
377
378
379
380
381
382
383
384
385
386
387
388
389
def toppra_time_parameterize(self,
                             waypoints: np.ndarray,
                             v_max: np.ndarray | float,
                             a_max: np.ndarray | float,
                             dt: float = 0.01,
                             grid_n: int = 400,
                             ) -> tuple[list[float], list[list[float]], list[list[float]], list[list[float]]]:
    """使用 TOPPRA 对路径进行时间参数化。

    Args:
        waypoints (np.ndarray): 路径点数组 (N, dof)。
        v_max (np.ndarray | float): 最大速度约束。
        a_max (np.ndarray | float): 最大加速度约束。
        dt (float, optional): 输出轨迹的时间步长. Defaults to 0.01.
        grid_n (int, optional): TOPPRA 离散化网格点数. Defaults to 400.

    Returns:
        tuple: (t, q, qd, qdd)
            - t: 时间戳列表
            - q: 关节位置列表
            - qd: 关节速度列表
            - qdd: 关节加速度列表

    Raises:
        ValueError: 如果 waypoints 维度不正确或约束格式错误。
        RuntimeError: 如果 TOPPRA 求解失败。
    """
    waypoints = np.asarray(waypoints, dtype=float)
    if waypoints.ndim != 2 or waypoints.shape[1] != 6 or waypoints.shape[0] < 2:
        raise ValueError("waypoints 必须是 (N,6) 且 N>=2")

    dof = waypoints.shape[1]

    # 1) 几何路径：用样条在路径参数 s∈[0,1] 上插值
    breaks = np.linspace(0.0, 1.0, waypoints.shape[0])
    path = ta.SplineInterpolator(breaks, waypoints)  # 官方示例同款 API

    # 2) 速度/加速度约束（上下界格式）
    v_max = np.full(dof, float(v_max)) if np.isscalar(v_max) else np.asarray(v_max, dtype=float)
    a_max = np.full(dof, float(a_max)) if np.isscalar(a_max) else np.asarray(a_max, dtype=float)
    if v_max.shape != (dof,) or a_max.shape != (dof,):
        raise ValueError("v_max/a_max 需为标量或 shape=(6,)")

    v_bounds = np.column_stack((-np.abs(v_max), np.abs(v_max)))  # (2,6) -> [v_min; v_max]
    a_bounds = np.column_stack((-np.abs(a_max), np.abs(a_max)))  # (2,6) -> [a_min; a_max]

    pc_vel = constraint.JointVelocityConstraint(v_bounds)
    pc_acc = constraint.JointAccelerationConstraint(a_bounds)

    # 3) TOPPRA 主算法 + 常用参数化器（常用且满足边界/约束）
    gridpoints = np.linspace(0, path.duration, int(grid_n))
    instance = algo.TOPPRA([pc_vel, pc_acc], path, gridpoints=gridpoints, parametrizer="ParametrizeConstAccel")

    # 4) 求解时间参数化，得到可按时间采样的 jnt_traj
    jnt_traj = instance.compute_trajectory(sd_start=0.0, sd_end=0.0)
    if jnt_traj is None:
        raise RuntimeError("TOPPRA 求解失败：给定约束下不可行，或路径异常。")

    # 5) 按 dt 采样
    T = float(jnt_traj.duration)
    M = max(2, int(np.ceil(T / dt)) + 1)
    t = np.linspace(0.0, T, M)

    q = jnt_traj.eval(t)
    qd = jnt_traj.evald(t)
    qdd = jnt_traj.evaldd(t)
    return t.tolist(), q.tolist(), qd.tolist(), qdd.tolist()

SerialDomainService 

SerialDomainService()

Bases: QObject

串口领域服务 - 管理串口连接的核心业务逻辑

初始化串口领域服务

源代码位于： src/controller/controller/domain/services/communication/serial_domain_service.py 
44
45
46
47
48
49
50
51
52
53
54
55
def __init__(self):
    """初始化串口领域服务"""
    super().__init__()

    # 串口相关对象
    self.serial_adapter = SerialAdapter()
    self.serial_reader: Optional[SerialReader] = None
    self.serial_writer: Optional[SerialWriter] = None

    # 线程管理
    self.reader_thread: Optional[SerialReaderThread] = None
    self.writer_thread: Optional[SerialWriterThread] = None

scan_available_ports 

scan_available_ports() -> List[str]

扫描可用端口列表

返回

List[str]: 可用端口名称列表

源代码位于： src/controller/controller/domain/services/communication/serial_domain_service.py 
57
58
59
60
61
62
63
64
65
66
67
def scan_available_ports(self) -> List[str]:
    """
    扫描可用端口列表

    返回:
        List[str]: 可用端口名称列表
    """
    try:
        return PortScanner.scan_ports()
    except Exception as e:
        raise Exception(f"扫描端口失败: {str(e)}")

get_port_info 

get_port_info(port_name: str) -> Optional[Dict[str, str]]

获取指定端口信息

参数

port_name: 端口名称

返回

Optional[Dict[str, str]]: 端口信息字典，失败时返回None

源代码位于： src/controller/controller/domain/services/communication/serial_domain_service.py 
69
70
71
72
73
74
75
76
77
78
79
80
81
82
def get_port_info(self, port_name: str) -> Optional[Dict[str, str]]:
    """
    获取指定端口信息

    参数:
        port_name: 端口名称

    返回:
        Optional[Dict[str, str]]: 端口信息字典，失败时返回None
    """
    try:
        return PortScanner.get_port_info(port_name)
    except Exception as e:
        raise Exception(f"获取端口信息失败: {str(e)}")

get_current_port 

get_current_port() -> Optional[str]

获取当前连接的端口名称

返回

Optional[str]: 端口名称，未连接时返回None

源代码位于： src/controller/controller/domain/services/communication/serial_domain_service.py 
84
85
86
87
88
89
90
91
def get_current_port(self) -> Optional[str]:
    """
    获取当前连接的端口名称

    返回:
        Optional[str]: 端口名称，未连接时返回None
    """
    return self.serial_adapter.get_port_name()

is_connected 

is_connected() -> bool

检查串口是否已连接

返回

bool: 是否已连接

源代码位于： src/controller/controller/domain/services/communication/serial_domain_service.py 
 93
 94
 95
 96
 97
 98
 99
100
def is_connected(self) -> bool:
    """
    检查串口是否已连接

    返回:
        bool: 是否已连接
    """
    return self.serial_adapter.is_connected()

connect_port 

connect_port(port: str, config: Dict[str, Any]) -> bool

连接串口

参数

port: 串口名称 config: 串口配置参数

返回

bool: 连接是否成功

源代码位于： src/controller/controller/domain/services/communication/serial_domain_service.py 
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
def connect_port(self, port: str, config: Dict[str, Any]) -> bool:
    """
    连接串口

    参数:
        port: 串口名称
        config: 串口配置参数

    返回:
        bool: 连接是否成功
    """
    try:
        # 如果已连接，先断开
        if self.is_connected():
            self.disconnect_port()

        # 尝试连接串口
        if not self.serial_adapter.connect(port, config):
            raise Exception(f"连接端口 {port} 失败")

        # 获取串口对象
        serial_port = self.serial_adapter.get_serial_port()
        if not serial_port:
            raise Exception("获取串口对象失败")

        # 创建读写器
        self.serial_reader = SerialReader(serial_port)
        self.serial_writer = SerialWriter(serial_port)

        # 连接Infrastructure层信号到Domain层
        self._connect_infrastructure_signals()

        # 启动线程
        self._start_threads()

        # 发送连接成功信号
        self.connection_status_changed.emit(True)
        return True

    except Exception as e:
        self.disconnect_port()
        raise Exception(f"连接失败: {str(e)}")

disconnect_port 

disconnect_port() -> bool

断开串口连接

返回

bool: 断开是否成功

源代码位于： src/controller/controller/domain/services/communication/serial_domain_service.py 
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
def disconnect_port(self) -> bool:
    """
    断开串口连接

    返回:
        bool: 断开是否成功
    """
    try:
        # 停止线程
        self._stop_threads()

        # 断开信号连接
        self._disconnect_infrastructure_signals()

        # 停止读写器
        if self.serial_reader:
            self.serial_reader.stop()
            self.serial_reader = None

        if self.serial_writer:
            self.serial_writer.stop()
            self.serial_writer = None

        # 断开适配器连接
        success = self.serial_adapter.disconnect()

        # 发送断开连接信号
        self.connection_status_changed.emit(False)
        return success

    except Exception as e:
        raise Exception(f"断开连接失败: {str(e)}")

send_data 

send_data(data: str) -> bool

发送数据

参数

data: 要发送的数据

返回

bool: 是否成功添加到发送队列

源代码位于： src/controller/controller/domain/services/communication/serial_domain_service.py 
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
def send_data(self, data: str) -> bool:
    """
    发送数据

    参数:
        data: 要发送的数据

    返回:
        bool: 是否成功添加到发送队列
    """
    if self.serial_writer and self.is_connected():
        self.serial_writer.add_to_queue(data)
        return True
    else:
        raise Exception("串口未连接，无法发送数据")

cleanup 

cleanup() -> None

清理资源

源代码位于： src/controller/controller/domain/services/communication/serial_domain_service.py 
242
243
244
245
246
247
def cleanup(self) -> None:
    """清理资源"""
    try:
        self.disconnect_port()
    except Exception:
        pass  # 忽略清理时的异常

MotionRunner 

MotionRunner(serial_domain_service: SerialDomainService, message_domain_service: MessageDomainService)

Bases: QObject

运动执行器。

负责按时间步长定时发送运动和夹爪指令。

属性：

名称 类型 描述
motion_msg_signal pyqtSignal

运动消息发送信号，携带 (消息内容, 类型)。
serial_domain_service

串口领域服务。
message_domain_service

消息领域服务。
data_list list

待发送的数据列表。
data_index int

当前发送索引。
timer QTimer

定时器。

初始化运动执行器。

源代码位于： src/controller/controller/domain/services/motion/motion_runner.py 
29
30
31
32
33
34
35
36
37
38
39
40
41
def __init__(
    self, 
    serial_domain_service: SerialDomainService, 
    message_domain_service: MessageDomainService):
    """初始化运动执行器。"""
    super().__init__()
    self.serial_domain_service = serial_domain_service
    self.message_domain_service = message_domain_service
    self.data_list = []
    self.data_index = 0
    self.timer = QTimer()
    self.timer.setInterval(10)
    self.timer.timeout.connect(self.motion_timeout)

add_motion_data 

add_motion_data(positions)

添加运动数据点序列。

参数：

名称 类型 描述 默认
positions list | ndarray

关节角度列表序列。

 必需
源代码位于： src/controller/controller/domain/services/motion/motion_runner.py 
43
44
45
46
47
48
49
50
51
52
53
54
55
56
def add_motion_data(self, positions):
    """添加运动数据点序列。

    Args:
        positions (list | np.ndarray): 关节角度列表序列。
    """
    for position in positions:
        msg_dict = {
            'control': 0x06,
            'mode': 0x08,
            'joint_angles': list(position)
        }
        message = self.message_domain_service.encode_message(**msg_dict)
        self.data_list.append(message)

add_gripper_data 

add_gripper_data(effector_mode, effector_data, pre_delay=0.0, post_delay=1.0)

添加夹爪命令。

参数：

名称 类型 描述 默认
effector_mode int

夹爪模式。

 必需
effector_data float

夹爪参数。

 必需
pre_delay float

前置等待时间（秒）. Defaults to 0.0.

 0.0
post_delay float

后置等待时间（秒）. Defaults to 1.0.

 1.0
源代码位于： src/controller/controller/domain/services/motion/motion_runner.py 
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
def add_gripper_data(self, effector_mode, effector_data, pre_delay=0.0, post_delay=1.0):
    """添加夹爪命令。

    Args:
        effector_mode (int): 夹爪模式。
        effector_data (float): 夹爪参数。
        pre_delay (float, optional): 前置等待时间（秒）. Defaults to 0.0.
        post_delay (float, optional): 后置等待时间（秒）. Defaults to 1.0.
    """
    # 添加前置延迟（如果>0）
    if pre_delay > 0:
        self.data_list.append(DelayCommand(delay_s=pre_delay))

    # 添加夹爪命令
    msg_dict = {
        'control': 0x00,
        'mode': 0x08,
        'effector_mode': effector_mode,
        'effector_data': effector_data
    }
    message = self.message_domain_service.encode_message(**msg_dict)
    self.data_list.append(message)

    # 添加后置延迟（如果>0）
    if post_delay > 0:
        self.data_list.append(DelayCommand(delay_s=post_delay))

start_motion 

start_motion()

开始执行运动序列。

源代码位于： src/controller/controller/domain/services/motion/motion_runner.py 
85
86
87
88
def start_motion(self):
    """开始执行运动序列。"""
    self.data_index = 0
    self.timer.start()

stop_motion 

stop_motion()

停止执行运动。

源代码位于： src/controller/controller/domain/services/motion/motion_runner.py 
90
91
92
93
def stop_motion(self):
    """停止执行运动。"""
    self.data_index = 0
    self.timer.stop()

clear_data 

clear_data()

清空所有待发送数据。

源代码位于： src/controller/controller/domain/services/motion/motion_runner.py 
95
96
97
98
def clear_data(self):
    """清空所有待发送数据。"""
    self.data_index = 0
    self.data_list = []

motion_timeout 

motion_timeout()

定时器回调函数，负责单步发送或延迟等待。

源代码位于： src/controller/controller/domain/services/motion/motion_runner.py 
100
101
102
103
104
105
106
107
108
109
110
111
def motion_timeout(self):
    """定时器回调函数，负责单步发送或延迟等待。"""
    if self.data_index >= len(self.data_list):
        self.timer.stop()
        return
    current_item = self.data_list[self.data_index]
    if hasattr(current_item, 'is_delay'):
        time.sleep(current_item.delay_s)
    else:
        self.serial_domain_service.send_data(current_item)
        self.motion_msg_signal.emit(current_item, "发送")
    self.data_index += 1

MotionConstructor 

MotionConstructor(motion_runner: MotionRunner, trajectory_planner: TrajectoryPlanningService)

运动构造器 - 领域服务。

职责： 1. 管理操作状态（执行/保存/预览） 2. 协调 TrajectoryPlanningService（规划） 3. 协调 MotionRunner（执行）

属性：

名称 类型 描述
motion_runner MotionRunner

运动执行器。
trajectory_planner TrajectoryPlanningService

轨迹规划服务。

初始化运动构造器。

参数：

名称 类型 描述 默认
motion_runner MotionRunner

运动执行器。

 必需
trajectory_planner TrajectoryPlanningService

轨迹规划服务。

 必需
源代码位于： src/controller/controller/domain/services/motion/motion_constructor.py 
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
def __init__(
    self, 
    motion_runner: MotionRunner,
    trajectory_planner: TrajectoryPlanningService
):
    """初始化运动构造器。

    Args:
        motion_runner (MotionRunner): 运动执行器。
        trajectory_planner (TrajectoryPlanningService): 轨迹规划服务。
    """
    self.motion_runner = motion_runner
    self.trajectory_planner = trajectory_planner

    # 统一的操作状态
    self._operation_mode = MotionOperationMode.NONE
    self._pending_tasks = []
    self._operation_context = {}

prepare_operation 

prepare_operation(mode: MotionOperationMode, tasks: List[Dict], context: Dict = None)

准备操作（统一入口）。

参数：

名称 类型 描述 默认
mode MotionOperationMode

操作模式。 - EXECUTE: 执行运动 - SAVE: 保存轨迹 - PREVIEW: 预览轨迹

 必需
tasks List[Dict]

任务列表。

 必需
context Dict

操作上下文。 - EXECUTE: {} - SAVE: {"filename": "xxx", "type": "node|plan"} - PREVIEW: {"type": "node|plan", "node_index": 0}

 None
源代码位于： src/controller/controller/domain/services/motion/motion_constructor.py 
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
def prepare_operation(
    self,
    mode: MotionOperationMode,
    tasks: List[Dict],
    context: Dict = None
):
    """准备操作（统一入口）。

    Args:
        mode (MotionOperationMode): 操作模式。
            - EXECUTE: 执行运动
            - SAVE: 保存轨迹
            - PREVIEW: 预览轨迹
        tasks (List[Dict]): 任务列表。
        context (Dict, optional): 操作上下文。
            - EXECUTE: {}
            - SAVE: {"filename": "xxx", "type": "node|plan"}
            - PREVIEW: {"type": "node|plan", "node_index": 0}
    """
    self._operation_mode = mode
    self._pending_tasks = tasks.copy()
    self._operation_context = context or {}

has_pending_operation 

has_pending_operation() -> bool

检查是否有待处理的操作。

返回：

名称 类型 描述
bool bool

True=有待处理的操作, False=无操作。
源代码位于： src/controller/controller/domain/services/motion/motion_constructor.py 
66
67
68
69
70
71
72
def has_pending_operation(self) -> bool:
    """检查是否有待处理的操作。

    Returns:
        bool: True=有待处理的操作, False=无操作。
    """
    return self._operation_mode != MotionOperationMode.NONE

get_operation_mode 

get_operation_mode() -> MotionOperationMode

获取当前操作模式。

返回：

名称 类型 描述
MotionOperationMode MotionOperationMode

当前操作模式枚举。
源代码位于： src/controller/controller/domain/services/motion/motion_constructor.py 
74
75
76
77
78
79
80
def get_operation_mode(self) -> MotionOperationMode:
    """获取当前操作模式。

    Returns:
        MotionOperationMode: 当前操作模式枚举。
    """
    return self._operation_mode

get_pending_tasks 

get_pending_tasks() -> List[Dict]

获取待处理的任务列表。

返回：

类型 描述
List[Dict]

List[Dict]: 任务列表的副本。
源代码位于： src/controller/controller/domain/services/motion/motion_constructor.py 
82
83
84
85
86
87
88
def get_pending_tasks(self) -> List[Dict]:
    """获取待处理的任务列表。

    Returns:
        List[Dict]: 任务列表的副本。
    """
    return self._pending_tasks.copy()

get_operation_context 

get_operation_context() -> Dict

获取操作上下文。

返回：

名称 类型 描述
Dict Dict

上下文字典的副本。
源代码位于： src/controller/controller/domain/services/motion/motion_constructor.py 
90
91
92
93
94
95
96
def get_operation_context(self) -> Dict:
    """获取操作上下文。

    Returns:
        Dict: 上下文字典的副本。
    """
    return self._operation_context.copy()

clear_operation 

clear_operation()

清除操作状态。

源代码位于： src/controller/controller/domain/services/motion/motion_constructor.py 
 98
 99
100
101
102
def clear_operation(self):
    """清除操作状态。"""
    self._operation_mode = MotionOperationMode.NONE
    self._pending_tasks.clear()
    self._operation_context.clear()

execute_motion 

execute_motion(start_position: List[float])

执行运动（仅用于EXECUTE模式）。

流程： 1. 按任务顺序规划每个任务 2. 按顺序添加运动数据和夹爪命令 3. 清除状态

参数：

名称 类型 描述 默认
start_position List[float]

起始位置（当前关节角度）。

 必需

引发：

类型 描述
RuntimeError

如果当前模式不是EXECUTE。
源代码位于： src/controller/controller/domain/services/motion/motion_constructor.py 
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
def execute_motion(self, start_position: List[float]):
    """执行运动（仅用于EXECUTE模式）。

    流程：
    1. 按任务顺序规划每个任务
    2. 按顺序添加运动数据和夹爪命令
    3. 清除状态

    Args:
        start_position (List[float]): 起始位置（当前关节角度）。

    Raises:
        RuntimeError: 如果当前模式不是EXECUTE。
    """
    if self._operation_mode != MotionOperationMode.EXECUTE:
        raise RuntimeError(f"当前模式不是EXECUTE: {self._operation_mode}")

    if not self._pending_tasks:
        return

    # 执行
    self.motion_runner.clear_data()

    # 按顺序处理每个任务，保持原有顺序
    current_position = start_position

    for task in self._pending_tasks:
        if task["type"] == "gripper":
            # 夹爪任务：直接添加夹爪命令（带延迟）
            effector_mode = task["effector_mode"]
            effector_data = task["effector_data"]
            pre_delay = task.get("pre_delay", 0.0)
            post_delay = task.get("post_delay", 1.0)
            self.motion_runner.add_gripper_data(
                effector_mode, 
                effector_data,
                pre_delay=pre_delay,
                post_delay=post_delay
            )
        else:
            # 运动任务：规划轨迹并添加
            positions, end_position = self.trajectory_planner._plan_single_task(
                task, 
                current_position
            )
            # 注意：positions 可能是 numpy 数组，不能直接用 if positions
            if len(positions) > 0:
                self.motion_runner.add_motion_data(positions)
                current_position = end_position

    self.motion_runner.start_motion()

    # 清理
    self.clear_operation()

TrajectoryPlanningService 

TrajectoryPlanningService(s_curve: SCurve, smooth_service: SmoothDomainService, linear_motion_service: LinearMotionDomainService, curve_motion_service: CurveMotionDomainService)

轨迹规划服务 - 领域服务。

职责： 1. 根据任务类型规划轨迹点序列 2. 维护位置状态（起点->终点） 3. 返回纯数据，不执行任何副作用

核心原则：纯函数，可测试，可复用

属性：

名称 类型 描述
s_curve SCurve

S曲线算法服务。
smooth_service SmoothDomainService

平滑算法服务。
linear_motion_service LinearMotionDomainService

直线运动服务。
curve_motion_service CurveMotionDomainService

曲线运动服务。

初始化轨迹规划服务。

参数：

名称 类型 描述 默认
s_curve SCurve

S曲线算法服务。

 必需
smooth_service SmoothDomainService

平滑算法服务。

 必需
linear_motion_service LinearMotionDomainService

直线运动服务。

 必需
curve_motion_service CurveMotionDomainService

曲线运动服务。

 必需
源代码位于： src/controller/controller/domain/services/motion/trajectory_planning_service.py 
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
def __init__(
    self,
    s_curve: SCurve,
    smooth_service: SmoothDomainService,
    linear_motion_service: LinearMotionDomainService,
    curve_motion_service: CurveMotionDomainService
):
    """初始化轨迹规划服务。

    Args:
        s_curve (SCurve): S曲线算法服务。
        smooth_service (SmoothDomainService): 平滑算法服务。
        linear_motion_service (LinearMotionDomainService): 直线运动服务。
        curve_motion_service (CurveMotionDomainService): 曲线运动服务。
    """
    self.s_curve = s_curve
    self.smooth_service = smooth_service
    self.linear_motion_service = linear_motion_service
    self.curve_motion_service = curve_motion_service

plan_task_sequence 

plan_task_sequence(tasks: List[Dict], start_position: List[float]) -> List[List[float]]

规划任务序列（仅返回位置）。

用于：执行运动、保存轨迹。

参数：

名称 类型 描述 默认
tasks List[Dict]

任务列表。

 必需
start_position List[float]

起始位置。

 必需

返回：

类型 描述
List[List[float]]

List[List[float]]: 轨迹点序列 [[q1,...,q6], ...]。
源代码位于： src/controller/controller/domain/services/motion/trajectory_planning_service.py 
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
def plan_task_sequence(
    self,
    tasks: List[Dict],
    start_position: List[float]
) -> List[List[float]]:
    """规划任务序列（仅返回位置）。

    用于：执行运动、保存轨迹。

    Args:
        tasks (List[Dict]): 任务列表。
        start_position (List[float]): 起始位置。

    Returns:
        List[List[float]]: 轨迹点序列 [[q1,...,q6], ...]。
    """
    all_positions = []
    current_position = start_position

    for task in tasks:
        positions, end_position = self._plan_single_task(task, current_position)
        all_positions.extend(positions)
        current_position = end_position

    return all_positions

plan_task_sequence_with_derivatives 

plan_task_sequence_with_derivatives(tasks: List[Dict], start_position: List[float]) -> Dict

规划任务序列（返回位置、速度、加速度、时间）。

用于：预览轨迹曲线。

参数：

名称 类型 描述 默认
tasks List[Dict]

任务列表。

 必需
start_position List[float]

起始位置。

 必需

返回：

名称 类型 描述
Dict Dict

包含以下键的字典: - time: [t0, t1, ...] - positions: [[q1,...,q6], ...] - velocities: [[qd1,...,qd6], ...] - accelerations: [[qdd1,...,qdd6], ...]
源代码位于： src/controller/controller/domain/services/motion/trajectory_planning_service.py 
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
def plan_task_sequence_with_derivatives(
    self,
    tasks: List[Dict],
    start_position: List[float]
) -> Dict:
    """规划任务序列（返回位置、速度、加速度、时间）。

    用于：预览轨迹曲线。

    Args:
        tasks (List[Dict]): 任务列表。
        start_position (List[float]): 起始位置。

    Returns:
        Dict: 包含以下键的字典:
            - time: [t0, t1, ...]
            - positions: [[q1,...,q6], ...]
            - velocities: [[qd1,...,qd6], ...]
            - accelerations: [[qdd1,...,qdd6], ...]
    """
    all_time = []
    all_positions = []
    all_velocities = []
    all_accelerations = []

    current_position = start_position
    current_time = 0.0

    for task in tasks:
        result = self._plan_single_task_full(task, current_position)

        # 合并数据（时间累加）
        all_time.extend([t + current_time for t in result["time"]])
        all_positions.extend(result["positions"])
        all_velocities.extend(result["velocities"])
        all_accelerations.extend(result["accelerations"])

        # 更新状态
        if len(result["positions"]) > 0:
            current_position = result["positions"][-1]
            if len(result["time"]) > 0:
                current_time = all_time[-1]

    return {
        "time": all_time,
        "positions": all_positions,
        "velocities": all_velocities,
        "accelerations": all_accelerations
    }

RobotStateDomainService 

RobotStateDomainService()

Bases: QObject

机械臂状态领域服务 - 单一数据源。

职责： 1. 维护当前机械臂状态（最新快照） 2. 维护历史状态缓冲（滑动窗口） 3. 计算静摩擦补偿（基于历史数据） 4. 发射信号通知订阅者

设计原则： - Single Source of Truth: 所有状态只在这里维护 - Pub-Sub Pattern: 支持多个订阅者 - Thread Safe: 使用锁保护可变数据

属性：

名称 类型 描述
state_updated pyqtSignal

状态更新信号，携带 RobotStateSnapshot。
angles_changed pyqtSignal

角度显著变化信号，携带 [θ1, θ2, ..., θ6] 列表。
torque_compensation_requested pyqtSignal

力矩补偿请求信号，携带当前角度列表。
源代码位于： src/controller/controller/domain/services/state/robot_state_domain_service.py 
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
79
80
81
82
83
def __init__(self):
    super().__init__()

    # ════════════════════════════════════════════════════════
    # 状态存储
    # ════════════════════════════════════════════════════════

    # 当前状态（最新快照）
    self._current_state: Optional[RobotStateSnapshot] = None

    # 历史缓冲（滑动窗口，用于静摩擦计算）
    self._position_history = deque(maxlen=20)

    # ════════════════════════════════════════════════════════
    # 静摩擦计算相关（用于示教模式）
    # ════════════════════════════════════════════════════════

    # 静摩擦状态（每个关节：0=静止, 1=运动）
    self._friction_state = [0] * 6

    # 静摩擦补偿值（Nm）
    self._friction_compensation = [0.0] * 6

    # 静摩擦配置（阈值）
    self._friction_config = [4, 0, 3, 0.5, 0.5, 0.5]

    # ════════════════════════════════════════════════════════
    # 其他状态
    # ════════════════════════════════════════════════════════

    # 示教模式标志
    self._teaching_mode = False

    # 线程锁（保护可变数据）
    self._lock = threading.RLock()

    # 最后更新时间
    self._last_update_time = 0.0

update_state 

update_state(decoded_message)

更新机械臂状态（核心方法）。

由 MessageResponseService 调用，每次接收到串口数据并解码后调用。

参数：

名称 类型 描述 默认
decoded_message Any

解码后的消息对象。

 必需
源代码位于： src/controller/controller/domain/services/state/robot_state_domain_service.py 
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
def update_state(self, decoded_message):
    """更新机械臂状态（核心方法）。

    由 MessageResponseService 调用，每次接收到串口数据并解码后调用。

    Args:
        decoded_message (Any): 解码后的消息对象。
    """
    new_snapshot = RobotStateSnapshot.from_decoded_message(decoded_message)
    old_snapshot = self._current_state
    self._current_state = new_snapshot
    self._last_update_time = new_snapshot.timestamp

    with self._lock:
        self._position_history.append({
            'timestamp': new_snapshot.timestamp,
            'angles': list(new_snapshot.joint_angles),
            'positions': list(new_snapshot.joint_positions)
        })

        if self._teaching_mode and len(self._position_history) >= 20:
            self._calculate_friction_compensation()

    self.state_updated.emit(new_snapshot)

    if old_snapshot and self._angles_changed_significantly(old_snapshot, new_snapshot):
        self.angles_changed.emit(list(new_snapshot.joint_angles))

    if new_snapshot.mode == 0x0A:
        if self._teaching_mode and new_snapshot.control in [0x06, 0x07]:
            from math import pi
            init_offset = [0.0, -pi/2, 0.0, pi/2, 0.0, 0.0]
            adjusted_angles = [
                a - offset 
                for a, offset in zip(new_snapshot.joint_angles, init_offset)
            ]
            self.torque_compensation_requested.emit(adjusted_angles)

get_current_state 

get_current_state() -> Optional[RobotStateSnapshot]

获取当前状态快照（不可变，线程安全）。

返回：

类型 描述
Optional[RobotStateSnapshot]

Optional[RobotStateSnapshot]: 当前状态快照。
源代码位于： src/controller/controller/domain/services/state/robot_state_domain_service.py 
132
133
134
135
136
137
138
def get_current_state(self) -> Optional[RobotStateSnapshot]:
    """获取当前状态快照（不可变，线程安全）。

    Returns:
        Optional[RobotStateSnapshot]: 当前状态快照。
    """
    return self._current_state

get_current_angles 

get_current_angles() -> List[float]

获取当前关节角度（弧度）。

返回：

类型 描述
List[float]

List[float]: 6个关节的角度列表。
源代码位于： src/controller/controller/domain/services/state/robot_state_domain_service.py 
140
141
142
143
144
145
146
147
148
def get_current_angles(self) -> List[float]:
    """获取当前关节角度（弧度）。

    Returns:
        List[float]: 6个关节的角度列表。
    """
    if self._current_state:
        return list(self._current_state.joint_angles)
    return [0.0] * 6

get_current_positions 

get_current_positions() -> List[int]

获取当前关节位置（编码器值）。

返回：

类型 描述
List[int]

List[int]: 6个关节的编码器位置列表。
源代码位于： src/controller/controller/domain/services/state/robot_state_domain_service.py 
150
151
152
153
154
155
156
157
158
def get_current_positions(self) -> List[int]:
    """获取当前关节位置（编码器值）。

    Returns:
        List[int]: 6个关节的编码器位置列表。
    """
    if self._current_state:
        return list(self._current_state.joint_positions)
    return [0] * 6

get_position_history 

get_position_history(count: int = 20) -> List[Dict]

获取历史位置数据。

参数：

名称 类型 描述 默认
count int

获取的记录数量. Defaults to 20.

 20

返回：

类型 描述
List[Dict]

List[Dict]: 历史位置数据列表，每个元素包含 timestamp, angles, positions。
源代码位于： src/controller/controller/domain/services/state/robot_state_domain_service.py 
160
161
162
163
164
165
166
167
168
169
170
def get_position_history(self, count: int = 20) -> List[Dict]:
    """获取历史位置数据。

    Args:
        count (int, optional): 获取的记录数量. Defaults to 20.

    Returns:
        List[Dict]: 历史位置数据列表，每个元素包含 timestamp, angles, positions。
    """
    with self._lock:
        return list(self._position_history)[-count:]

get_friction_compensation 

get_friction_compensation() -> List[float]

获取当前静摩擦补偿值。

返回：

类型 描述
List[float]

List[float]: 6个关节的静摩擦补偿值列表。
源代码位于： src/controller/controller/domain/services/state/robot_state_domain_service.py 
172
173
174
175
176
177
178
179
def get_friction_compensation(self) -> List[float]:
    """获取当前静摩擦补偿值。

    Returns:
        List[float]: 6个关节的静摩擦补偿值列表。
    """
    with self._lock:
        return self._friction_compensation.copy()

set_teaching_mode 

set_teaching_mode(enabled: bool)

设置示教模式。

参数：

名称 类型 描述 默认
enabled bool

True=开启, False=关闭。

 必需
源代码位于： src/controller/controller/domain/services/state/robot_state_domain_service.py 
185
186
187
188
189
190
191
192
193
194
195
196
197
def set_teaching_mode(self, enabled: bool):
    """设置示教模式。

    Args:
        enabled (bool): True=开启, False=关闭。
    """
    self._teaching_mode = enabled
    if enabled:
        # 开启示教时，清空历史
        with self._lock:
            self._position_history.clear()
            self._friction_state = [0] * 6
            self._friction_compensation = [0.0] * 6

set_friction_config 

set_friction_config(config: List[float])

设置静摩擦配置。

参数：

名称 类型 描述 默认
config List[float]

6个关节的静摩擦阈值列表。

 必需
源代码位于： src/controller/controller/domain/services/state/robot_state_domain_service.py 
199
200
201
202
203
204
205
206
def set_friction_config(self, config: List[float]):
    """设置静摩擦配置。

    Args:
        config (List[float]): 6个关节的静摩擦阈值列表。
    """
    if len(config) == 6:
        self._friction_config = config

clear_history 

clear_history()

清空历史数据。

源代码位于： src/controller/controller/domain/services/state/robot_state_domain_service.py 
208
209
210
211
212
213
def clear_history(self):
    """清空历史数据。"""
    with self._lock:
        self._position_history.clear()
        self._friction_state = [0] * 6
        self._friction_compensation = [0.0] * 6

TeachRecordDomainService 

TeachRecordDomainService()

Bases: QObject

示教记录领域服务 - Domain层。

职责： 1. 管理示教记录数据（内存中） 2. 控制记录的开始/停止 3. 提供记录的增删改查操作 4. 生成记录名称

属性：

名称 类型 描述
recording_state_changed pyqtSignal

记录状态变化信号，携带是否正在记录。
record_added pyqtSignal

记录添加信号，携带新增的记录名称。
record_deleted pyqtSignal

记录删除信号，携带删除的记录名称。

初始化示教记录服务。

源代码位于： src/controller/controller/domain/services/state/teach_record_domain_service.py 
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
def __init__(self):
    """初始化示教记录服务。"""
    super().__init__()

    # 记录数据 {记录名: 角度列表}
    self._records: Dict[str, List[List[float]]] = {}

    # 当前记录状态
    self._is_recording = False
    self._current_record_angles: List[List[float]] = []
    self._record_counter = 0

    # 记录定时器（10ms间隔，100Hz采样率）
    self._record_timer = QTimer()
    self._record_timer.setInterval(10)
    self._record_timer.timeout.connect(self._on_record_timer_timeout)

start_recording 

start_recording()

开始记录。

源代码位于： src/controller/controller/domain/services/state/teach_record_domain_service.py 
49
50
51
52
53
54
55
56
def start_recording(self):
    """开始记录。"""
    if not self._is_recording:
        self._is_recording = True
        self._current_record_angles = []
        self._record_counter = 0
        self._record_timer.start()
        self.recording_state_changed.emit(True)

stop_recording 

stop_recording() -> Optional[str]

停止记录并保存。

返回：

类型 描述
Optional[str]

Optional[str]: 新生成的记录名称，如果没有数据则返回 None。
源代码位于： src/controller/controller/domain/services/state/teach_record_domain_service.py 
58
59
60
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
76
77
def stop_recording(self) -> Optional[str]:
    """停止记录并保存。

    Returns:
        Optional[str]: 新生成的记录名称，如果没有数据则返回 None。
    """
    if self._is_recording:
        self._is_recording = False
        self._record_timer.stop()
        self.recording_state_changed.emit(False)

        # 保存当前记录
        if self._current_record_angles:
            record_name = self._generate_record_name()
            self._records[record_name] = self._current_record_angles.copy()
            self.record_added.emit(record_name)
            return record_name
        else:
            return None
    return None

is_recording 

is_recording() -> bool

查询是否正在记录。

返回：

名称 类型 描述
bool bool

是否正在记录。
源代码位于： src/controller/controller/domain/services/state/teach_record_domain_service.py 
79
80
81
82
83
84
85
def is_recording(self) -> bool:
    """查询是否正在记录。

    Returns:
        bool: 是否正在记录。
    """
    return self._is_recording

add_angle_to_current_record 

add_angle_to_current_record(angles: List[float])

添加角度到当前记录。

参数：

名称 类型 描述 默认
angles List[float]

6维关节角度。

 必需
源代码位于： src/controller/controller/domain/services/state/teach_record_domain_service.py 
87
88
89
90
91
92
93
94
95
def add_angle_to_current_record(self, angles: List[float]):
    """添加角度到当前记录。

    Args:
        angles (List[float]): 6维关节角度。
    """
    if self._is_recording and len(angles) == 6:
        self._current_record_angles.append(angles.copy())
        self._record_counter += 1

get_record_timer 

get_record_timer() -> QTimer

获取记录定时器（用于外部连接）。

返回：

名称 类型 描述
QTimer QTimer

记录定时器。
源代码位于： src/controller/controller/domain/services/state/teach_record_domain_service.py 
104
105
106
107
108
109
110
def get_record_timer(self) -> QTimer:
    """获取记录定时器（用于外部连接）。

    Returns:
        QTimer: 记录定时器。
    """
    return self._record_timer

get_all_records 

get_all_records() -> Dict[str, List[List[float]]]

获取所有记录。

返回：

类型 描述
Dict[str, List[List[float]]]

Dict[str, List[List[float]]]: 所有记录字典。
源代码位于： src/controller/controller/domain/services/state/teach_record_domain_service.py 
116
117
118
119
120
121
122
def get_all_records(self) -> Dict[str, List[List[float]]]:
    """获取所有记录。

    Returns:
        Dict[str, List[List[float]]]: 所有记录字典。
    """
    return self._records.copy()

get_record_names 

get_record_names() -> List[str]

获取所有记录名称。

返回：

类型 描述
List[str]

List[str]: 记录名称列表。
源代码位于： src/controller/controller/domain/services/state/teach_record_domain_service.py 
124
125
126
127
128
129
130
def get_record_names(self) -> List[str]:
    """获取所有记录名称。

    Returns:
        List[str]: 记录名称列表。
    """
    return list(self._records.keys())

get_record 

get_record(name: str) -> Optional[List[List[float]]]

获取指定记录。

参数：

名称 类型 描述 默认
name str

记录名称。

 必需

返回：

类型 描述
Optional[List[List[float]]]

Optional[List[List[float]]]: 角度列表，如果不存在返回 None。
源代码位于： src/controller/controller/domain/services/state/teach_record_domain_service.py 
132
133
134
135
136
137
138
139
140
141
def get_record(self, name: str) -> Optional[List[List[float]]]:
    """获取指定记录。

    Args:
        name (str): 记录名称。

    Returns:
        Optional[List[List[float]]]: 角度列表，如果不存在返回 None。
    """
    return self._records.get(name)

delete_record 

delete_record(name: str) -> bool

删除指定记录。

参数：

名称 类型 描述 默认
name str

记录名称。

 必需

返回：

名称 类型 描述
bool bool

是否删除成功。
源代码位于： src/controller/controller/domain/services/state/teach_record_domain_service.py 
143
144
145
146
147
148
149
150
151
152
153
154
155
156
def delete_record(self, name: str) -> bool:
    """删除指定记录。

    Args:
        name (str): 记录名称。

    Returns:
        bool: 是否删除成功。
    """
    if name in self._records:
        del self._records[name]
        self.record_deleted.emit(name)
        return True
    return False

reverse_record 

reverse_record(name: str) -> Optional[str]

反转指定记录并另存为新记录。

参数：

名称 类型 描述 默认
name str

原记录名称。

 必需

返回：

类型 描述
Optional[str]

Optional[str]: 新记录名称，如果失败返回 None。
源代码位于： src/controller/controller/domain/services/state/teach_record_domain_service.py 
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
def reverse_record(self, name: str) -> Optional[str]:
    """反转指定记录并另存为新记录。

    Args:
        name (str): 原记录名称。

    Returns:
        Optional[str]: 新记录名称，如果失败返回 None。
    """
    if name not in self._records:
        return None

    try:
        # 反转角度序列
        original_list = self._records[name]
        reversed_list = list(reversed(original_list))

        # 生成新名称：原名-反转（如已存在则追加序号）
        base_name = f"{name}-反转"
        new_name = base_name
        suffix = 2
        while new_name in self._records:
            new_name = f"{base_name}-{suffix}"
            suffix += 1

        # 保存新记录
        self._records[new_name] = reversed_list
        self.record_added.emit(new_name)
        return new_name
    except Exception as e:
        return None

load_records 

load_records(records: Dict[str, List[List[float]]])

加载记录数据（从Repository）。

参数：

名称 类型 描述 默认
records Dict[str, List[List[float]]]

记录数据字典。

 必需
源代码位于： src/controller/controller/domain/services/state/teach_record_domain_service.py 
190
191
192
193
194
195
196
def load_records(self, records: Dict[str, List[List[float]]]):
    """加载记录数据（从Repository）。

    Args:
        records (Dict[str, List[List[float]]]): 记录数据字典。
    """
    self._records = records.copy()

clear_all_records 

clear_all_records()

清空所有记录。

源代码位于： src/controller/controller/domain/services/state/teach_record_domain_service.py 
198
199
200
def clear_all_records(self):
    """清空所有记录。"""
    self._records.clear()

MotionPlanningDomainService 

MotionPlanningDomainService()

运动规划Domain服务。

职责： 1. 管理多个方案（内存） 2. 维护当前激活的方案 3. 提供方案和节点的增删改查 4. 实施业务规则

属性：

名称 类型 描述
_plans List[MotionPlan]

方案列表。
_current_index int

当前激活方案的索引。

初始化服务。

源代码位于： src/controller/controller/domain/services/planning/motion_planning_domain_service.py 
22
23
24
25
def __init__(self):
    """初始化服务。"""
    self._plans: List[MotionPlan] = []
    self._current_index: int = -1

initialize 

initialize(plans: List[MotionPlan], current_index: int)

初始化数据（从Repository加载后调用）。

参数：

名称 类型 描述 默认
plans List[MotionPlan]

方案列表。

 必需
current_index int

当前方案索引。

 必需
源代码位于： src/controller/controller/domain/services/planning/motion_planning_domain_service.py 
29
30
31
32
33
34
35
36
37
def initialize(self, plans: List[MotionPlan], current_index: int):
    """初始化数据（从Repository加载后调用）。

    Args:
        plans (List[MotionPlan]): 方案列表。
        current_index (int): 当前方案索引。
    """
    self._plans = plans
    self._current_index = current_index if plans else -1

get_all_plans 

get_all_plans() -> List[MotionPlan]

获取所有方案（用于持久化）。

返回：

类型 描述
List[MotionPlan]

List[MotionPlan]: 方案列表的副本。
源代码位于： src/controller/controller/domain/services/planning/motion_planning_domain_service.py 
39
40
41
42
43
44
45
def get_all_plans(self) -> List[MotionPlan]:
    """获取所有方案（用于持久化）。

    Returns:
        List[MotionPlan]: 方案列表的副本。
    """
    return self._plans.copy()

create_plan 

create_plan(name: str) -> int

创建方案，返回索引。

参数：

名称 类型 描述 默认
name str

方案名称。

 必需

返回：

名称 类型 描述
int int

新创建方案的索引。
源代码位于： src/controller/controller/domain/services/planning/motion_planning_domain_service.py 
49
50
51
52
53
54
55
56
57
58
59
60
61
def create_plan(self, name: str) -> int:
    """创建方案，返回索引。

    Args:
        name (str): 方案名称。

    Returns:
        int: 新创建方案的索引。
    """
    plan = MotionPlan(name=name)
    self._plans.append(plan)
    self._current_index = len(self._plans) - 1
    return self._current_index

get_plan_names 

get_plan_names() -> List[str]

获取所有方案名称（用于下拉框）。

返回：

类型 描述
List[str]

List[str]: 方案名称列表。
源代码位于： src/controller/controller/domain/services/planning/motion_planning_domain_service.py 
63
64
65
66
67
68
69
def get_plan_names(self) -> List[str]:
    """获取所有方案名称（用于下拉框）。

    Returns:
        List[str]: 方案名称列表。
    """
    return [plan.name for plan in self._plans]

get_current_index 

get_current_index() -> int

获取当前方案索引。

返回：

名称 类型 描述
int int

当前索引。
源代码位于： src/controller/controller/domain/services/planning/motion_planning_domain_service.py 
71
72
73
74
75
76
77
def get_current_index(self) -> int:
    """获取当前方案索引。

    Returns:
        int: 当前索引。
    """
    return self._current_index

set_current_index 

set_current_index(index: int) -> bool

切换方案。

参数：

名称 类型 描述 默认
index int

目标索引。

 必需

返回：

名称 类型 描述
bool bool

是否切换成功。
源代码位于： src/controller/controller/domain/services/planning/motion_planning_domain_service.py 
79
80
81
82
83
84
85
86
87
88
89
90
91
def set_current_index(self, index: int) -> bool:
    """切换方案。

    Args:
        index (int): 目标索引。

    Returns:
        bool: 是否切换成功。
    """
    if 0 <= index < len(self._plans):
        self._current_index = index
        return True
    return False

delete_plan 

delete_plan(index: int) -> bool

删除方案。

业务规则：至少保留一个方案。

参数：

名称 类型 描述 默认
index int

方案索引。

 必需

返回：

名称 类型 描述
bool bool

True=删除成功, False=删除失败（违反业务规则或索引无效）。
源代码位于： src/controller/controller/domain/services/planning/motion_planning_domain_service.py 
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
def delete_plan(self, index: int) -> bool:
    """删除方案。

    业务规则：至少保留一个方案。

    Args:
        index (int): 方案索引。

    Returns:
        bool: True=删除成功, False=删除失败（违反业务规则或索引无效）。
    """
    # 业务规则：至少保留一个方案
    if len(self._plans) <= 1:
        return False

    if 0 <= index < len(self._plans):
        self._plans.pop(index)
        # 调整当前索引
        if self._current_index >= len(self._plans):
            self._current_index = max(0, len(self._plans) - 1)
        if len(self._plans) == 0:
            self._current_index = -1
        return True
    return False

can_delete_plan 

can_delete_plan() -> bool

检查是否可以删除方案。

返回：

名称 类型 描述
bool bool

True=可以删除, False=不能删除（只有一个方案）。
源代码位于： src/controller/controller/domain/services/planning/motion_planning_domain_service.py 
118
119
120
121
122
123
124
def can_delete_plan(self) -> bool:
    """检查是否可以删除方案。

    Returns:
        bool: True=可以删除, False=不能删除（只有一个方案）。
    """
    return len(self._plans) > 1

rename_plan 

rename_plan(index: int, new_name: str) -> bool

重命名方案。

参数：

名称 类型 描述 默认
index int

方案索引。

 必需
new_name str

新名称。

 必需

返回：

名称 类型 描述
bool bool

是否成功。
源代码位于： src/controller/controller/domain/services/planning/motion_planning_domain_service.py 
126
127
128
129
130
131
132
133
134
135
136
137
138
139
def rename_plan(self, index: int, new_name: str) -> bool:
    """重命名方案。

    Args:
        index (int): 方案索引。
        new_name (str): 新名称。

    Returns:
        bool: 是否成功。
    """
    if 0 <= index < len(self._plans):
        self._plans[index].name = new_name
        return True
    return False

get_current_plan 

get_current_plan() -> Optional[MotionPlan]

获取当前方案。

返回：

类型 描述
Optional[MotionPlan]

Optional[MotionPlan]: 当前方案对象，无方案时返回 None。
源代码位于： src/controller/controller/domain/services/planning/motion_planning_domain_service.py 
141
142
143
144
145
146
147
148
149
def get_current_plan(self) -> Optional[MotionPlan]:
    """获取当前方案。

    Returns:
        Optional[MotionPlan]: 当前方案对象，无方案时返回 None。
    """
    if 0 <= self._current_index < len(self._plans):
        return self._plans[self._current_index]
    return None

get_plan_count 

get_plan_count() -> int

获取方案数量。

返回：

名称 类型 描述
int int

方案数量。
源代码位于： src/controller/controller/domain/services/planning/motion_planning_domain_service.py 
151
152
153
154
155
156
157
def get_plan_count(self) -> int:
    """获取方案数量。

    Returns:
        int: 方案数量。
    """
    return len(self._plans)

add_point 

add_point(point_data: dict)

添加节点到当前方案。

参数：

名称 类型 描述 默认
point_data dict

节点数据。

 必需
源代码位于： src/controller/controller/domain/services/planning/motion_planning_domain_service.py 
161
162
163
164
165
166
167
168
169
def add_point(self, point_data: dict):
    """添加节点到当前方案。

    Args:
        point_data (dict): 节点数据。
    """
    plan = self.get_current_plan()
    if plan:
        plan.add_point(point_data)

remove_point 

remove_point(index: int)

删除节点。

参数：

名称 类型 描述 默认
index int

节点索引。

 必需
源代码位于： src/controller/controller/domain/services/planning/motion_planning_domain_service.py 
171
172
173
174
175
176
177
178
179
def remove_point(self, index: int):
    """删除节点。

    Args:
        index (int): 节点索引。
    """
    plan = self.get_current_plan()
    if plan:
        plan.remove_point(index)

move_point_up 

move_point_up(index: int) -> bool

上移节点。

参数：

名称 类型 描述 默认
index int

节点索引。

 必需

返回：

名称 类型 描述
bool bool

是否成功。
源代码位于： src/controller/controller/domain/services/planning/motion_planning_domain_service.py 
181
182
183
184
185
186
187
188
189
190
191
def move_point_up(self, index: int) -> bool:
    """上移节点。

    Args:
        index (int): 节点索引。

    Returns:
        bool: 是否成功。
    """
    plan = self.get_current_plan()
    return plan.move_point_up(index) if plan else False

move_point_down 

move_point_down(index: int) -> bool

下移节点。

参数：

名称 类型 描述 默认
index int

节点索引。

 必需

返回：

名称 类型 描述
bool bool

是否成功。
源代码位于： src/controller/controller/domain/services/planning/motion_planning_domain_service.py 
193
194
195
196
197
198
199
200
201
202
203
def move_point_down(self, index: int) -> bool:
    """下移节点。

    Args:
        index (int): 节点索引。

    Returns:
        bool: 是否成功。
    """
    plan = self.get_current_plan()
    return plan.move_point_down(index) if plan else False

update_point 

update_point(index: int, point_data: dict)

更新节点。

参数：

名称 类型 描述 默认
index int

节点索引。

 必需
point_data dict

新数据。

 必需
源代码位于： src/controller/controller/domain/services/planning/motion_planning_domain_service.py 
205
206
207
208
209
210
211
212
213
214
def update_point(self, index: int, point_data: dict):
    """更新节点。

    Args:
        index (int): 节点索引。
        point_data (dict): 新数据。
    """
    plan = self.get_current_plan()
    if plan:
        plan.update_point(index, point_data)

get_all_points 

get_all_points() -> List[dict]

获取当前方案的所有节点。

返回：

类型 描述
List[dict]

List[dict]: 节点列表的副本。
源代码位于： src/controller/controller/domain/services/planning/motion_planning_domain_service.py 
216
217
218
219
220
221
222
223
def get_all_points(self) -> List[dict]:
    """获取当前方案的所有节点。

    Returns:
        List[dict]: 节点列表的副本。
    """
    plan = self.get_current_plan()
    return plan.points.copy() if plan else []

CameraDomainService 

CameraDomainService()

Bases: QObject

摄像头领域服务。

职责： - 管理 ROS2 摄像头订阅节点的生命周期 - 维护最新的彩色/深度图像数据（线程安全） - 发射图像接收信号 - 深度图可视化处理

属性：

名称 类型 描述
color_image_received pyqtSignal

彩色图像接收信号，携带图像数据 (np.ndarray)。
depth_image_received pyqtSignal

深度图像接收信号，携带图像数据 (np.ndarray)。
connection_status_changed pyqtSignal

连接状态变更信号，携带 (connected, message)。
error_occurred pyqtSignal

错误信号，携带错误信息。

初始化摄像头领域服务。

源代码位于： src/controller/controller/domain/services/vision/camera_domain_service.py 
137
138
139
140
141
142
143
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
def __init__(self):
    """初始化摄像头领域服务。"""
    super().__init__()

    # ROS节点和执行器
    self.color_subscriber = None
    self.depth_subscriber = None
    self.ros_executor = None
    self.ros_thread = None

    # cv_bridge
    self.bridge = CvBridge()

    # 图像数据存储（线程安全）
    self.latest_color_image = None
    self.latest_depth_image = None
    self.color_image_lock = threading.Lock()
    self.depth_image_lock = threading.Lock()

    # 连接状态
    self.is_connected = False
    self.color_connected = False  # 彩色流是否有数据
    self.depth_connected = False  # 深度流是否有数据

connect 

connect() -> bool

连接摄像头（启动 ROS 订阅）。

返回：

名称 类型 描述
bool bool

连接成功返回 True，失败返回 False。
源代码位于： src/controller/controller/domain/services/vision/camera_domain_service.py 
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
182
183
184
185
186
187
188
189
190
191
192
193
194
195
196
197
198
199
200
201
def connect(self) -> bool:
    """连接摄像头（启动 ROS 订阅）。

    Returns:
        bool: 连接成功返回 True，失败返回 False。
    """
    try:
        if self.is_connected:
            return True

        # 检查并初始化ROS（灵活策略，参考旧版）
        if not rclpy.ok():
            try:
                rclpy.init()
            except RuntimeError as e:
                # ROS已经初始化但状态异常
                self.error_occurred.emit(f"ROS状态异常: {str(e)}")
                return False

        # 创建订阅节点
        self.color_subscriber = ColorImageSubscriber(self._on_color_image_received)
        self.depth_subscriber = DepthImageSubscriber(self._on_depth_image_received)

        # 创建executor
        self.ros_executor = MultiThreadedExecutor()
        self.ros_executor.add_node(self.color_subscriber)
        self.ros_executor.add_node(self.depth_subscriber)

        # 在独立线程运行executor
        self.ros_thread = threading.Thread(target=self._ros_spin_worker, daemon=True)
        self.ros_thread.start()

        self.is_connected = True
        self.connection_status_changed.emit(True, "摄像头连接成功")
        return True

    except Exception as e:
        self.error_occurred.emit(f"连接摄像头失败: {str(e)}")
        return False

disconnect 

disconnect() -> bool

断开摄像头（关闭 ROS 订阅）。

返回：

名称 类型 描述
bool bool

断开成功返回 True，失败返回 False。
源代码位于： src/controller/controller/domain/services/vision/camera_domain_service.py 
203
204
205
206
207
208
209
210
211
212
213
214
215
216
217
218
219
220
221
222
223
224
225
226
227
228
229
230
231
232
233
234
235
236
237
238
239
240
241
242
243
244
245
246
247
def disconnect(self) -> bool:
    """断开摄像头（关闭 ROS 订阅）。

    Returns:
        bool: 断开成功返回 True，失败返回 False。
    """
    try:
        if not self.is_connected:
            return True

        self.is_connected = False
        self.color_connected = False
        self.depth_connected = False

        # 停止executor（只影响摄像头节点）
        if self.ros_executor:
            self.ros_executor.shutdown()

        # 销毁节点
        if self.color_subscriber:
            self.color_subscriber.destroy_node()
            self.color_subscriber = None
        if self.depth_subscriber:
            self.depth_subscriber.destroy_node()
            self.depth_subscriber = None

        # 等待线程结束
        if self.ros_thread and self.ros_thread.is_alive():
            self.ros_thread.join(timeout=1.0)

        self.ros_executor = None
        self.ros_thread = None

        # 清空图像数据
        with self.color_image_lock:
            self.latest_color_image = None
        with self.depth_image_lock:
            self.latest_depth_image = None

        self.connection_status_changed.emit(False, "摄像头已断开")
        return True

    except Exception as e:
        self.error_occurred.emit(f"断开摄像头失败: {str(e)}")
        return False

get_latest_color_image 

get_latest_color_image() -> Optional[np.ndarray]

获取最新彩色图像（线程安全）。

返回：

类型 描述
Optional[ndarray]

Optional[np.ndarray]: 图像数据，无数据返回 None。
源代码位于： src/controller/controller/domain/services/vision/camera_domain_service.py 
249
250
251
252
253
254
255
256
def get_latest_color_image(self) -> Optional[np.ndarray]:
    """获取最新彩色图像（线程安全）。

    Returns:
        Optional[np.ndarray]: 图像数据，无数据返回 None。
    """
    with self.color_image_lock:
        return self.latest_color_image.copy() if self.latest_color_image is not None else None

get_latest_depth_image 

get_latest_depth_image() -> Optional[np.ndarray]

获取最新深度图像（线程安全）。

返回：

类型 描述
Optional[ndarray]

Optional[np.ndarray]: 图像数据，无数据返回 None。
源代码位于： src/controller/controller/domain/services/vision/camera_domain_service.py 
258
259
260
261
262
263
264
265
def get_latest_depth_image(self) -> Optional[np.ndarray]:
    """获取最新深度图像（线程安全）。

    Returns:
        Optional[np.ndarray]: 图像数据，无数据返回 None。
    """
    with self.depth_image_lock:
        return self.latest_depth_image.copy() if self.latest_depth_image is not None else None

is_color_available 

is_color_available() -> bool

检查彩色图像是否可用。

返回：

名称 类型 描述
bool bool

是否可用。
源代码位于： src/controller/controller/domain/services/vision/camera_domain_service.py 
267
268
269
270
271
272
273
def is_color_available(self) -> bool:
    """检查彩色图像是否可用。

    Returns:
        bool: 是否可用。
    """
    return self.color_connected and self.latest_color_image is not None

is_depth_available 

is_depth_available() -> bool

检查深度图像是否可用。

返回：

名称 类型 描述
bool bool

是否可用。
源代码位于： src/controller/controller/domain/services/vision/camera_domain_service.py 
275
276
277
278
279
280
281
def is_depth_available(self) -> bool:
    """检查深度图像是否可用。

    Returns:
        bool: 是否可用。
    """
    return self.depth_connected and self.latest_depth_image is not None

visualize_depth_image 

visualize_depth_image(depth_image: ndarray) -> np.ndarray

深度图可视化为伪彩色图。

参数：

名称 类型 描述 默认
depth_image ndarray

原始深度图（16位或32位浮点）。

 必需

返回：

类型 描述
ndarray

np.ndarray: 伪彩色深度图（BGR, uint8）。
源代码位于： src/controller/controller/domain/services/vision/camera_domain_service.py 
283
284
285
286
287
288
289
290
291
292
293
294
295
296
297
298
299
300
301
302
303
304
305
306
307
308
309
310
311
312
313
314
315
316
317
318
def visualize_depth_image(self, depth_image: np.ndarray) -> np.ndarray:
    """深度图可视化为伪彩色图。

    Args:
        depth_image (np.ndarray): 原始深度图（16位或32位浮点）。

    Returns:
        np.ndarray: 伪彩色深度图（BGR, uint8）。
    """
    try:
        # 过滤无效值（depth <= 0 或 nan）
        valid_mask = (depth_image > 0) & np.isfinite(depth_image)

        if not np.any(valid_mask):
            # 如果没有有效数据，返回黑色图像
            depth_display = np.zeros_like(depth_image, dtype=np.uint8)
        else:
            # 归一化到 0-255
            min_val = np.min(depth_image[valid_mask])
            max_val = np.max(depth_image[valid_mask])

            if max_val - min_val < 1e-6:
                depth_display = np.zeros_like(depth_image, dtype=np.uint8)
            else:
                norm = (depth_image - min_val) / (max_val - min_val)
                norm[~valid_mask] = 0
                depth_display = (norm * 255).astype(np.uint8)

        # 应用伪彩色映射
        depth_colormap = cv2.applyColorMap(depth_display, cv2.COLORMAP_JET)
        return depth_colormap

    except Exception as e:
        # 如果可视化失败，返回黑色图像
        height, width = depth_image.shape[:2]
        return np.zeros((height, width, 3), dtype=np.uint8)

RecognitionDomainService 

RecognitionDomainService()

Bases: QObject

零件识别领域服务。

职责： - 订阅 recognition_result ROS 话题 - 维护最新检测结果（线程安全） - 独立于摄像头服务运行 - 发射检测结果信号

属性：

名称 类型 描述
detection_result_received pyqtSignal

检测结果接收信号，携带结果字典。
detection_status_changed pyqtSignal

检测状态变更信号，携带 (is_running, message)。
error_occurred pyqtSignal

错误信号，携带错误信息。

初始化零件识别领域服务。

源代码位于： src/controller/controller/domain/services/vision/recognition_domain_service.py 
85
86
87
88
89
90
91
92
93
94
95
96
97
98
99
def __init__(self):
    """初始化零件识别领域服务。"""
    super().__init__()

    # ROS节点和执行器
    self.detection_subscriber = None
    self.ros_executor = None
    self.ros_thread = None

    # 检测结果存储（线程安全）
    self.latest_detection_result = {}
    self.detection_lock = threading.Lock()

    # 运行状态
    self.is_running = False

start_detection 

start_detection() -> bool

开始检测（启动 ROS 订阅）。

返回：

名称 类型 描述
bool bool

启动成功返回 True，失败返回 False。
源代码位于： src/controller/controller/domain/services/vision/recognition_domain_service.py 
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
def start_detection(self) -> bool:
    """开始检测（启动 ROS 订阅）。

    Returns:
        bool: 启动成功返回 True，失败返回 False。
    """
    try:
        if self.is_running:
            return True

        # 检查并初始化ROS（灵活策略，参考旧版）
        if not rclpy.ok():
            try:
                rclpy.init()
            except RuntimeError as e:
                # ROS已经初始化但状态异常
                self.error_occurred.emit(f"ROS状态异常: {str(e)}")
                return False
            except Exception as e:
                self.error_occurred.emit(f"ROS初始化失败: {str(e)}")
                return False

        # 创建订阅节点
        self.detection_subscriber = DetectionSubscriberNode(self._on_detection_received)

        # 创建executor
        self.ros_executor = SingleThreadedExecutor()
        self.ros_executor.add_node(self.detection_subscriber)

        # 在独立线程运行executor
        self.ros_thread = threading.Thread(target=self._ros_spin_worker, daemon=True)
        self.ros_thread.start()

        self.is_running = True
        self.detection_status_changed.emit(True, "开始零件识别")
        return True

    except Exception as e:
        self.error_occurred.emit(f"启动检测失败: {str(e)}")
        return False

stop_detection 

stop_detection() -> bool

停止检测（关闭 ROS 订阅）。

返回：

名称 类型 描述
bool bool

停止成功返回 True，失败返回 False。
源代码位于： src/controller/controller/domain/services/vision/recognition_domain_service.py 
144
145
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
163
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
181
def stop_detection(self) -> bool:
    """停止检测（关闭 ROS 订阅）。

    Returns:
        bool: 停止成功返回 True，失败返回 False。
    """
    try:
        if not self.is_running:
            return True

        self.is_running = False

        # 停止executor（只影响检测节点）
        if self.ros_executor:
            self.ros_executor.shutdown()

        # 销毁节点
        if self.detection_subscriber:
            self.detection_subscriber.destroy_node()
            self.detection_subscriber = None

        # 等待线程结束
        if self.ros_thread and self.ros_thread.is_alive():
            self.ros_thread.join(timeout=1.0)

        self.ros_executor = None
        self.ros_thread = None

        # 清空检测结果
        with self.detection_lock:
            self.latest_detection_result = {}

        self.detection_status_changed.emit(False, "停止零件识别")
        return True

    except Exception as e:
        self.error_occurred.emit(f"停止检测失败: {str(e)}")
        return False

get_latest_result 

get_latest_result() -> Optional[Dict]

获取最新检测结果（线程安全）。

返回：

类型 描述
Optional[Dict]

Optional[Dict]: 检测结果字典，无结果返回 None。
源代码位于： src/controller/controller/domain/services/vision/recognition_domain_service.py 
183
184
185
186
187
188
189
190
191
192
def get_latest_result(self) -> Optional[Dict]:
    """获取最新检测结果（线程安全）。

    Returns:
        Optional[Dict]: 检测结果字典，无结果返回 None。
    """
    with self.detection_lock:
        if self.latest_detection_result:
            return copy.deepcopy(self.latest_detection_result)
        return None

is_detection_running 

is_detection_running() -> bool

检查检测是否正在运行。

返回：

名称 类型 描述
bool bool

是否正在运行。
源代码位于： src/controller/controller/domain/services/vision/recognition_domain_service.py 
194
195
196
197
198
199
200
def is_detection_running(self) -> bool:
    """检查检测是否正在运行。

    Returns:
        bool: 是否正在运行。
    """
    return self.is_running

clear_result 

clear_result()

清除检测结果。

源代码位于： src/controller/controller/domain/services/vision/recognition_domain_service.py 
202
203
204
205
def clear_result(self):
    """清除检测结果。"""
    with self.detection_lock:
        self.latest_detection_result = {}

HandEyeTransformDomainService 

HandEyeTransformDomainService(config: HandEyeCalibrationConfig, kinematic_service: KinematicDomainService)

手眼标定变换服务。

职责： 1. 像素坐标 → 相机坐标系 2. 相机坐标系 → 基坐标系 3. 计算目标位姿（包含偏移和姿态调整） 4. 提供逆运动学求解

属性：

名称 类型 描述
config HandEyeCalibrationConfig

手眼标定配置。
kinematic_service KinematicDomainService

运动学服务实例。

初始化手眼标定变换服务。

参数：

名称 类型 描述 默认
config HandEyeCalibrationConfig

手眼标定配置（通过DI注入）。

 必需
kinematic_service KinematicDomainService

运动学服务。

 必需
源代码位于： src/controller/controller/domain/services/algorithm/hand_eye_transform_domain_service.py 
25
26
27
28
29
30
31
32
33
34
35
36
37
def __init__(
    self,
    config: HandEyeCalibrationConfig,
    kinematic_service: KinematicDomainService
):
    """初始化手眼标定变换服务。

    Args:
        config: 手眼标定配置（通过DI注入）。
        kinematic_service: 运动学服务。
    """
    self.config = config
    self.kinematic_service = kinematic_service

calculate_target_joint_angles 

calculate_target_joint_angles(central_center: List[float], depth: float, real_center: List[float], real_depth: float, angle: float, current_joint_angles: List[float]) -> Optional[List[float]]

计算运动到零件位置所需的目标关节角度。

完整流程： 1. 像素坐标 → 相机坐标系 2. 正运动学：当前关节角度 → 末端位姿 3. 相机坐标系 → 基坐标系 4. 计算零件方向 + 偏移量 5. 逆运动学：目标位姿 → 目标关节角度

参数：

名称 类型 描述 默认
central_center List[float]

中心点像素坐标 [u, v]。

 必需
depth float

中心点深度 (mm)。

 必需
real_center List[float]

实际中心点像素坐标 [u, v]。

 必需
real_depth float

实际中心点深度 (mm)。

 必需
angle float

零件角度（弧度）。

 必需
current_joint_angles List[float]

当前关节角度列表（弧度）。

 必需

返回：

类型 描述
Optional[List[float]]

Optional[List[float]]: 目标关节角度列表（弧度），如果无解则返回 None。
源代码位于： src/controller/controller/domain/services/algorithm/hand_eye_transform_domain_service.py 
 39
 40
 41
 42
 43
 44
 45
 46
 47
 48
 49
 50
 51
 52
 53
 54
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
def calculate_target_joint_angles(
    self,
    central_center: List[float],      # [u, v] 像素坐标
    depth: float,                     # 深度 (mm)
    real_center: List[float],         # 实际中心点 [u, v]
    real_depth: float,                # 实际深度 (mm)
    angle: float,                     # 零件角度（弧度）
    current_joint_angles: List[float] # 当前关节角度（弧度）
) -> Optional[List[float]]:
    """计算运动到零件位置所需的目标关节角度。

    完整流程：
    1. 像素坐标 → 相机坐标系
    2. 正运动学：当前关节角度 → 末端位姿
    3. 相机坐标系 → 基坐标系
    4. 计算零件方向 + 偏移量
    5. 逆运动学：目标位姿 → 目标关节角度

    Args:
        central_center (List[float]): 中心点像素坐标 [u, v]。
        depth (float): 中心点深度 (mm)。
        real_center (List[float]): 实际中心点像素坐标 [u, v]。
        real_depth (float): 实际中心点深度 (mm)。
        angle (float): 零件角度（弧度）。
        current_joint_angles (List[float]): 当前关节角度列表（弧度）。

    Returns:
        Optional[List[float]]: 目标关节角度列表（弧度），如果无解则返回 None。
    """
    # 获取配置参数
    intrinsics = self.config.camera_intrinsics

    # 1. 像素坐标 → 相机坐标系（齐次坐标）
    px1, py1, pz1 = central_center[0], central_center[1], depth
    px2, py2, pz2 = real_center[0], real_center[1], real_depth

    # 深度单位转换：mm → m
    pz1 *= 0.001
    pz2 *= 0.001

    # 使用针孔相机模型
    px1 = (px1 - intrinsics.cx) * pz1 / intrinsics.fx
    py1 = (py1 - intrinsics.cy) * pz1 / intrinsics.fy
    px2 = (px2 - intrinsics.cx) * pz2 / intrinsics.fx
    py2 = (py2 - intrinsics.cy) * pz2 / intrinsics.fy

    p1_cam = np.array([px1, py1, pz1, 1])
    p2_cam = np.array([px2, py2, pz2, 1])

    # 2. 正运动学：获取当前末端位姿（4x4变换矩阵）
    forward_matrix = self.kinematic_service.get_gripper2base_rm(current_joint_angles)

    # 3. 相机坐标系 → 基坐标系
    T_cam2base = forward_matrix @ self.config.hand_eye_matrix

    p1_base = T_cam2base @ p1_cam
    p2_base = T_cam2base @ p2_cam
    v_base = p2_base - p1_base

    # 4. 计算目标位姿
    # 4.1 计算零件方向向量
    theta = np.arctan2(v_base[1], v_base[0])

    # 4.2 构建绕 Z 轴旋转的变换矩阵
    T_target2base = self.get_z_rotation_matrix(theta)
    T_target2base[:, 3] = p2_base

    # 4.3 添加偏移量
    T_offset2target = np.eye(4)
    T_offset2target[:3, 3] = self.config.target_offset.to_array()
    T_target2base = T_target2base @ T_offset2target

    # 4.4 调整末端姿态
    adjustment = self.config.end_effector_adjustment
    T_target2base = (
        T_target2base 
        @ self.get_z_rotation_matrix(adjustment.z_rotation) 
        @ self.get_y_rotation_matrix(adjustment.y_rotation)
    )

    # 5. 逆运动学求解
    theta_list = self.kinematic_service.inverse_kinematic(
        T_target2base[:3, :3], 
        T_target2base[:3, 3],
        initial_theta=current_joint_angles
    )

    return theta_list

get_z_rotation_matrix 

get_z_rotation_matrix(angle: float) -> np.ndarray

获取绕 Z 轴旋转的齐次变换矩阵。

参数：

名称 类型 描述 默认
angle float

旋转角度（弧度）。

 必需

返回：

类型 描述
ndarray

np.ndarray: 4x4 齐次变换矩阵。
源代码位于： src/controller/controller/domain/services/algorithm/hand_eye_transform_domain_service.py 
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
def get_z_rotation_matrix(self, angle: float) -> np.ndarray:
    """获取绕 Z 轴旋转的齐次变换矩阵。

    Args:
        angle (float): 旋转角度（弧度）。

    Returns:
        np.ndarray: 4x4 齐次变换矩阵。
    """
    c = cos(angle)
    s = sin(angle)
    return np.array([
        [c, -s, 0, 0],
        [s,  c, 0, 0],
        [0,  0, 1, 0],
        [0,  0, 0, 1]
    ])

get_y_rotation_matrix 

get_y_rotation_matrix(angle: float) -> np.ndarray

获取绕 Y 轴旋转的齐次变换矩阵。

参数：

名称 类型 描述 默认
angle float

旋转角度（弧度）。

 必需

返回：

类型 描述
ndarray

np.ndarray: 4x4 齐次变换矩阵。
源代码位于： src/controller/controller/domain/services/algorithm/hand_eye_transform_domain_service.py 
146
147
148
149
150
151
152
153
154
155
156
157
158
159
160
161
162
def get_y_rotation_matrix(self, angle: float) -> np.ndarray:
    """获取绕 Y 轴旋转的齐次变换矩阵。

    Args:
        angle (float): 旋转角度（弧度）。

    Returns:
        np.ndarray: 4x4 齐次变换矩阵。
    """
    c = cos(angle)
    s = sin(angle)
    return np.array([
        [ c, 0, s, 0],
        [ 0, 1, 0, 0],
        [-s, 0, c, 0],
        [ 0, 0, 0, 1]
    ])

get_x_rotation_matrix 

get_x_rotation_matrix(angle: float) -> np.ndarray

获取绕 X 轴旋转的齐次变换矩阵。

参数：

名称 类型 描述 默认
angle float

旋转角度（弧度）。

 必需

返回：

类型 描述
ndarray

np.ndarray: 4x4 齐次变换矩阵。
源代码位于： src/controller/controller/domain/services/algorithm/hand_eye_transform_domain_service.py 
164
165
166
167
168
169
170
171
172
173
174
175
176
177
178
179
180
def get_x_rotation_matrix(self, angle: float) -> np.ndarray:
    """获取绕 X 轴旋转的齐次变换矩阵。

    Args:
        angle (float): 旋转角度（弧度）。

    Returns:
        np.ndarray: 4x4 齐次变换矩阵。
    """
    c = cos(angle)
    s = sin(angle)
    return np.array([
        [1,  0,  0, 0],
        [0,  c, -s, 0],
        [0,  s,  c, 0],
        [0,  0,  0, 1]
    ])

MotionPlan dataclass 

MotionPlan(name: str, points: List[dict] = list())

运动规划方案实体。

属性：

名称 类型 描述
name str

方案名称。
points List[dict]

节点列表。

add_point 

add_point(point_data: dict)

添加节点。

参数：

名称 类型 描述 默认
point_data dict

节点数据。

 必需
源代码位于： src/controller/controller/domain/entities/motion_plan.py 
20
21
22
23
24
25
26
def add_point(self, point_data: dict):
    """添加节点。

    Args:
        point_data (dict): 节点数据。
    """
    self.points.append(point_data)

remove_point 

remove_point(index: int)

删除节点。

参数：

名称 类型 描述 默认
index int

节点索引。

 必需
源代码位于： src/controller/controller/domain/entities/motion_plan.py 
28
29
30
31
32
33
34
35
def remove_point(self, index: int):
    """删除节点。

    Args:
        index (int): 节点索引。
    """
    if 0 <= index < len(self.points):
        self.points.pop(index)

move_point_up 

move_point_up(index: int) -> bool

上移节点。

参数：

名称 类型 描述 默认
index int

节点索引。

 必需

返回：

名称 类型 描述
bool bool

是否成功移动。
源代码位于： src/controller/controller/domain/entities/motion_plan.py 
37
38
39
40
41
42
43
44
45
46
47
48
49
50
def move_point_up(self, index: int) -> bool:
    """上移节点。

    Args:
        index (int): 节点索引。

    Returns:
        bool: 是否成功移动。
    """
    if index > 0:
        self.points[index], self.points[index-1] = \
            self.points[index-1], self.points[index]
        return True
    return False

move_point_down 

move_point_down(index: int) -> bool

下移节点。

参数：

名称 类型 描述 默认
index int

节点索引。

 必需

返回：

名称 类型 描述
bool bool

是否成功移动。
源代码位于： src/controller/controller/domain/entities/motion_plan.py 
52
53
54
55
56
57
58
59
60
61
62
63
64
65
def move_point_down(self, index: int) -> bool:
    """下移节点。

    Args:
        index (int): 节点索引。

    Returns:
        bool: 是否成功移动。
    """
    if index < len(self.points) - 1:
        self.points[index], self.points[index+1] = \
            self.points[index+1], self.points[index]
        return True
    return False

update_point 

update_point(index: int, point_data: dict)

更新节点。

参数：

名称 类型 描述 默认
index int

节点索引。

 必需
point_data dict

新的节点数据。

 必需
源代码位于： src/controller/controller/domain/entities/motion_plan.py 
67
68
69
70
71
72
73
74
75
def update_point(self, index: int, point_data: dict):
    """更新节点。

    Args:
        index (int): 节点索引。
        point_data (dict): 新的节点数据。
    """
    if 0 <= index < len(self.points):
        self.points[index] = point_data

PortScanner

串口端口扫描器。

负责扫描可用的串口端口，包含真实串口和虚拟串口。

scan_ports staticmethod 

scan_ports() -> List[str]

扫描可用的串口端口（包含真实串口和虚拟串口）。

返回：

类型 描述
List[str]

List[str]: 可用端口名称列表。
源代码位于： src/controller/controller/infrastructure/communication/port_scanner.py 
17
18
19
20
21
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
@staticmethod
def scan_ports() -> List[str]:
    """扫描可用的串口端口（包含真实串口和虚拟串口）。

    Returns:
        List[str]: 可用端口名称列表。
    """
    all_ports = []

    try:
        # 1. 扫描真实硬件串口
        ports = serial.tools.list_ports.comports()
        all_ports.extend([port.device for port in ports])

        # 2. 扫描虚拟串口（伪终端）
        virtual_port_patterns = [
            '/dev/pts/*',      # 伪终端
            '/tmp/tty*',       # 临时虚拟串口（socat创建的）
            '/dev/ttyV*',      # 虚拟串口设备
        ]

        for pattern in virtual_port_patterns:
            for device in glob.glob(pattern):
                # 检查是否是字符设备且可访问
                if os.path.exists(device) and PortScanner._is_accessible(device):
                    if device not in all_ports:
                        all_ports.append(device)

        # 排序：真实串口在前，虚拟串口在后
        all_ports.sort(key=lambda x: (not x.startswith('/dev/ttyUSB'), 
                                     not x.startswith('/dev/ttyACM'), 
                                     x))

        return all_ports

    except Exception:
        return all_ports

get_port_info staticmethod 

get_port_info(port_name: str) -> Optional[Dict[str, str]]

获取指定端口的详细信息（支持真实串口和虚拟串口）。

参数：

名称 类型 描述 默认
port_name str

端口名称。

 必需

返回：

类型 描述
Optional[Dict[str, str]]

Optional[Dict[str, str]]: 端口信息字典，包含 description 和 hwid。如果端口不可用则返回 None。
源代码位于： src/controller/controller/infrastructure/communication/port_scanner.py 
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
112
113
114
115
116
117
118
119
@staticmethod
def get_port_info(port_name: str) -> Optional[Dict[str, str]]:
    """获取指定端口的详细信息（支持真实串口和虚拟串口）。

    Args:
        port_name (str): 端口名称。

    Returns:
        Optional[Dict[str, str]]: 端口信息字典，包含 description 和 hwid。如果端口不可用则返回 None。
    """
    try:
        # 1. 先尝试从真实硬件串口获取信息
        ports = serial.tools.list_ports.comports()
        for port in ports:
            if port.device == port_name:
                return {
                    'device': port.device,
                    'description': port.description or '',
                    'hwid': port.hwid or ''
                }

        # 2. 如果是虚拟串口，返回自定义信息
        if os.path.exists(port_name):
            description = "虚拟串口"

            # 根据路径判断类型
            if port_name.startswith('/tmp/'):
                description = "临时虚拟串口 (socat)"
            elif port_name.startswith('/dev/pts/'):
                description = "伪终端 (pty)"
            elif port_name.startswith('/dev/ttyV'):
                description = "虚拟串口设备"

            return {
                'device': port_name,
                'description': description,
                'hwid': 'VIRTUAL'
            }

        return None
    except Exception:
        return None

get_all_port_info staticmethod 

get_all_port_info() -> List[Dict[str, str]]

获取所有可用端口的详细信息（包含真实串口和虚拟串口）。

返回：

类型 描述
List[Dict[str, str]]

List[Dict[str, str]]: 所有端口信息列表。
源代码位于： src/controller/controller/infrastructure/communication/port_scanner.py 
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
@staticmethod
def get_all_port_info() -> List[Dict[str, str]]:
    """获取所有可用端口的详细信息（包含真实串口和虚拟串口）。

    Returns:
        List[Dict[str, str]]: 所有端口信息列表。
    """
    try:
        all_port_info = []

        # 获取所有端口
        all_ports = PortScanner.scan_ports()

        # 为每个端口获取信息
        for port_name in all_ports:
            port_info = PortScanner.get_port_info(port_name)
            if port_info:
                all_port_info.append(port_info)

        return all_port_info
    except Exception:
        return []

SerialAdapter 

SerialAdapter()

串口连接适配器。

属性：

名称 类型 描述
serial_port Optional[Serial]

PySerial 实例。
current_config Optional[Dict[str, Any]]

当前配置字典。

初始化串口适配器。

源代码位于： src/controller/controller/infrastructure/communication/serial_adapter.py 
17
18
19
20
def __init__(self):
    """初始化串口适配器。"""
    self.serial_port: Optional[serial.Serial] = None
    self.current_config: Optional[Dict[str, Any]] = None

connect 

connect(port: str, config: Dict[str, Any]) -> bool

连接串口。

参数：

名称 类型 描述 默认
port str

串口名称。

 必需
config Dict[str, Any]

串口配置参数。

 必需

返回：

名称 类型 描述
bool bool

连接是否成功。
源代码位于： src/controller/controller/infrastructure/communication/serial_adapter.py 
22
23
24
25
26
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
def connect(self, port: str, config: Dict[str, Any]) -> bool:
    """连接串口。

    Args:
        port (str): 串口名称。
        config (Dict[str, Any]): 串口配置参数。

    Returns:
        bool: 连接是否成功。
    """
    try:
        # 如果已经连接，先断开
        if self.is_connected():
            self.disconnect()

        # 创建串口连接
        self.serial_port = serial.Serial(
            port=port,
            baudrate=config.get('baudrate', 115200),
            bytesize=config.get('bytesize', 8),
            parity=config.get('parity', 'N'),
            stopbits=config.get('stopbits', 1),
            timeout=config.get('timeout', 1),
            xonxoff=config.get('xonxoff', False),
            rtscts=config.get('rtscts', False),
            dsrdtr=config.get('dsrdtr', False)
        )

        # 保存配置
        self.current_config = config.copy()
        self.current_config['port'] = port

        return True

    except Exception:
        self.serial_port = None
        self.current_config = None
        return False

disconnect 

disconnect() -> bool

断开串口连接。

返回：

名称 类型 描述
bool bool

断开是否成功。
源代码位于： src/controller/controller/infrastructure/communication/serial_adapter.py 
61
62
63
64
65
66
67
68
69
70
71
72
73
74
75
def disconnect(self) -> bool:
    """断开串口连接。

    Returns:
        bool: 断开是否成功。
    """
    try:
        if self.serial_port and self.serial_port.is_open:
            self.serial_port.close()
        return True
    except Exception:
        return False
    finally:
        self.serial_port = None
        self.current_config = None

is_connected 

is_connected() -> bool

检查串口是否已连接。

返回：

名称 类型 描述
bool bool

是否已连接。
源代码位于： src/controller/controller/infrastructure/communication/serial_adapter.py 
77
78
79
80
81
82
83
84
def is_connected(self) -> bool:
    """检查串口是否已连接。

    Returns:
        bool: 是否已连接。
    """
    return (self.serial_port is not None and 
            self.serial_port.is_open)

get_serial_port 

get_serial_port() -> Optional[serial.Serial]

获取串口对象。

返回：

类型 描述
Optional[Serial]

Optional[serial.Serial]: 串口对象，未连接时返回None。
源代码位于： src/controller/controller/infrastructure/communication/serial_adapter.py 
86
87
88
89
90
91
92
93
94
def get_serial_port(self) -> Optional[serial.Serial]:
    """获取串口对象。

    Returns:
        Optional[serial.Serial]: 串口对象，未连接时返回None。
    """
    if self.is_connected():
        return self.serial_port
    return None

get_current_config 

get_current_config() -> Optional[Dict[str, Any]]

获取当前连接配置。

返回：

类型 描述
Optional[Dict[str, Any]]

Optional[Dict[str, Any]]: 当前配置，未连接时返回 None。
源代码位于： src/controller/controller/infrastructure/communication/serial_adapter.py 
 96
 97
 98
 99
100
101
102
def get_current_config(self) -> Optional[Dict[str, Any]]:
    """获取当前连接配置。

    Returns:
        Optional[Dict[str, Any]]: 当前配置，未连接时返回 None。
    """
    return self.current_config.copy() if self.current_config else None

get_port_name 

get_port_name() -> Optional[str]

获取当前连接的端口名称。

返回：

类型 描述
Optional[str]

Optional[str]: 端口名称，未连接时返回 None。
源代码位于： src/controller/controller/infrastructure/communication/serial_adapter.py 
104
105
106
107
108
109
110
111
112
def get_port_name(self) -> Optional[str]:
    """获取当前连接的端口名称。

    Returns:
        Optional[str]: 端口名称，未连接时返回 None。
    """
    if self.current_config:
        return self.current_config.get('port')
    return None

SerialReader 

SerialReader(serial_port: Optional[Serial] = None)

Bases: QObject

串口读取处理类。

负责在独立线程中读取串口数据，支持多种编码格式。

属性：

名称 类型 描述
data_received pyqtSignal

接收到数据时发送信号，携带十六进制字符串。

初始化串口读取器。

参数：

名称 类型 描述 默认
serial_port Serial

串口对象。

 None
源代码位于： src/controller/controller/infrastructure/communication/serial_reader.py 
23
24
25
26
27
28
29
30
31
def __init__(self, serial_port: Optional[serial.Serial] = None):
    """初始化串口读取器。

    Args:
        serial_port (serial.Serial, optional): 串口对象。
    """
    super().__init__()
    self.serial_port = serial_port
    self.stop_flag = False

stop 

stop() -> None

停止读取。

源代码位于： src/controller/controller/infrastructure/communication/serial_reader.py 
35
36
37
def stop(self) -> None:
    """停止读取。"""
    self.stop_flag = True

read_data 

read_data() -> None

读取串口数据 - 在独立线程中运行。

源代码位于： src/controller/controller/infrastructure/communication/serial_reader.py 
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
def read_data(self) -> None:
    """读取串口数据 - 在独立线程中运行。"""
    self.stop_flag = False

    while not self.stop_flag:
        if not self.serial_port or not self.serial_port.is_open:
            time.sleep(0.1)
            continue

        try:
            waiting_bytes = self.serial_port.in_waiting
        except OSError:
            time.sleep(0.1)
            continue

        if waiting_bytes > 0:
            try:
                data = self.serial_port.read(waiting_bytes)

                # 固定使用十六进制格式
                hex_data = data.hex().upper()
                self.data_received.emit(hex_data)

            except serial.SerialException:
                time.sleep(0.1)
                continue
        else:
            time.sleep(0.01)

        time.sleep(0.01)

SerialWriter 

SerialWriter(serial_port: Optional[Serial] = None)

Bases: QObject

串口发送处理类。

负责在独立线程中发送串口数据，使用队列管理发送任务。

属性：

名称 类型 描述
serial_port Optional[Serial]

串口对象。
send_queue Queue

发送任务队列。

初始化串口写入器。

参数：

名称 类型 描述 默认
serial_port Serial

串口对象。

 None
源代码位于： src/controller/controller/infrastructure/communication/serial_writer.py 
22
23
24
25
26
27
28
29
30
31
def __init__(self, serial_port: Optional[serial.Serial] = None):
    """初始化串口写入器。

    Args:
        serial_port (serial.Serial, optional): 串口对象。
    """
    super().__init__()
    self.serial_port = serial_port
    self.send_queue = Queue()
    self.stop_flag = False

stop 

stop() -> None

停止发送。

源代码位于： src/controller/controller/infrastructure/communication/serial_writer.py 
33
34
35
def stop(self) -> None:
    """停止发送。"""
    self.stop_flag = True

add_to_queue 

add_to_queue(cmd: Union[str, bytes]) -> None

添加命令到发送队列。

参数：

名称 类型 描述 默认
cmd Union[str, bytes]

待发送的命令（十六进制字符串或字节）。

 必需
源代码位于： src/controller/controller/infrastructure/communication/serial_writer.py 
37
38
39
40
41
42
43
def add_to_queue(self, cmd: Union[str, bytes]) -> None:
    """添加命令到发送队列。

    Args:
        cmd (Union[str, bytes]): 待发送的命令（十六进制字符串或字节）。
    """
    self.send_queue.put(cmd)

send_data 

send_data() -> None

发送串口数据 - 在独立线程中运行。

源代码位于： src/controller/controller/infrastructure/communication/serial_writer.py 
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
def send_data(self) -> None:
    """发送串口数据 - 在独立线程中运行。"""
    self.stop_flag = False

    while not self.stop_flag:
        try:
            if not self.serial_port or not self.serial_port.is_open:
                time.sleep(0.1)
                continue

            if not self.send_queue.empty():
                try:
                    cmd = self.send_queue.get()
                    if isinstance(cmd, str):
                        cmd = bytes.fromhex(cmd.replace(' ', ''))
                    self.serial_port.write(cmd)
                    self.serial_port.flush()
                    self.send_queue.task_done()
                except Exception:
                    # 静默处理错误，不发送信号
                    pass
            else:
                # 队列为空时短暂休眠，避免CPU空转
                time.sleep(0.01)
        except Exception:
            # 静默处理错误，不发送信号
            time.sleep(0.1)

RecordRepository 

RecordRepository(file_path: str = 'teach_record.json')

示教记录持久化仓库 - Infrastructure层。

职责： - 从JSON文件加载记录数据 - 保存记录数据到JSON文件 - 管理记录文件的读写

属性：

名称 类型 描述
file_path str

记录文件路径。

初始化记录仓库。

参数：

名称 类型 描述 默认
file_path str

记录文件路径. Defaults to "teach_record.json".

 'teach_record.json'
源代码位于： src/controller/controller/infrastructure/persistence/record_repository.py 
21
22
23
24
25
26
27
def __init__(self, file_path: str = "teach_record.json"):
    """初始化记录仓库。

    Args:
        file_path (str, optional): 记录文件路径. Defaults to "teach_record.json".
    """
    self.file_path = file_path

load_records 

load_records() -> Dict[str, List[List[float]]]

从文件加载记录数据。

返回：

类型 描述
Dict[str, List[List[float]]]

Dict[str, List[List[float]]]: 记录数据字典 {记录名: 角度列表}。
源代码位于： src/controller/controller/infrastructure/persistence/record_repository.py 
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
def load_records(self) -> Dict[str, List[List[float]]]:
    """从文件加载记录数据。

    Returns:
        Dict[str, List[List[float]]]: 记录数据字典 {记录名: 角度列表}。
    """
    try:
        if os.path.exists(self.file_path):
            with open(self.file_path, 'r', encoding='utf-8') as f:
                data = json.load(f)
            return data
        else:
            return {}
    except Exception as e:
        return {}

save_records 

save_records(records: Dict[str, List[List[float]]]) -> bool

保存记录数据到文件。

参数：

名称 类型 描述 默认
records Dict[str, List[List[float]]]

记录数据字典。

 必需

返回：

名称 类型 描述
bool bool

是否保存成功。
源代码位于： src/controller/controller/infrastructure/persistence/record_repository.py 
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
def save_records(self, records: Dict[str, List[List[float]]]) -> bool:
    """保存记录数据到文件。

    Args:
        records (Dict[str, List[List[float]]]): 记录数据字典。

    Returns:
        bool: 是否保存成功。
    """
    try:
        with open(self.file_path, 'w', encoding='utf-8') as f:
            json.dump(records, f, ensure_ascii=False, indent=2)
        return True
    except Exception as e:
        return False

delete_file 

delete_file() -> bool

删除记录文件。

返回：

名称 类型 描述
bool bool

是否删除成功。
源代码位于： src/controller/controller/infrastructure/persistence/record_repository.py 
61
62
63
64
65
66
67
68
69
70
71
72
73
def delete_file(self) -> bool:
    """删除记录文件。

    Returns:
        bool: 是否删除成功。
    """
    try:
        if os.path.exists(self.file_path):
            os.remove(self.file_path)
            return True
        return False
    except Exception as e:
        return False

MotionPlanRepository 

MotionPlanRepository(file_path: str = './motion_planning_plans.json')

运动方案持久化仓库。

职责： 1. 保存/加载方案到JSON文件 2. 管理文件系统操作

属性：

名称 类型 描述
file_path Path

方案文件路径。

初始化仓库。

参数：

名称 类型 描述 默认
file_path str

文件路径. Defaults to "./motion_planning_plans.json".

 './motion_planning_plans.json'
源代码位于： src/controller/controller/infrastructure/persistence/motion_plan_repository.py 
20
21
22
23
24
25
26
def __init__(self, file_path: str = "./motion_planning_plans.json"):
    """初始化仓库。

    Args:
        file_path (str, optional): 文件路径. Defaults to "./motion_planning_plans.json".
    """
    self.file_path = Path(file_path)

save 

save(plans_data: List[Dict], current_index: int)

保存方案数据到文件。

参数：

名称 类型 描述 默认
plans_data List[Dict]

方案数据列表。

 必需
current_index int

当前选中方案索引。

 必需
源代码位于： src/controller/controller/infrastructure/persistence/motion_plan_repository.py 
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
def save(self, plans_data: List[Dict], current_index: int):
    """保存方案数据到文件。

    Args:
        plans_data (List[Dict]): 方案数据列表。
        current_index (int): 当前选中方案索引。
    """
    data = {
        "current_index": current_index,
        "plans": plans_data
    }
    try:
        with open(self.file_path, 'w', encoding='utf-8') as f:
            json.dump(data, f, indent=2, ensure_ascii=False)
    except Exception as e:
        pass

load 

load() -> tuple[List[Dict], int]

从文件加载方案数据。

返回：

类型 描述
tuple[List[Dict], int]

tuple[List[Dict], int]: (plans_data, current_index)。 如果文件不存在或加载失败，返回默认方案。
源代码位于： src/controller/controller/infrastructure/persistence/motion_plan_repository.py 
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
71
def load(self) -> tuple[List[Dict], int]:
    """从文件加载方案数据。

    Returns:
        tuple[List[Dict], int]: (plans_data, current_index)。
            如果文件不存在或加载失败，返回默认方案。
    """
    if not self.file_path.exists():
        # 返回默认方案
        return [{"name": "默认方案", "points": []}], 0

    try:
        with open(self.file_path, 'r', encoding='utf-8') as f:
            data = json.load(f)

        plans = data.get("plans", [])
        current_index = data.get("current_index", 0)

        # 如果没有方案，创建默认方案
        if not plans:
            plans = [{"name": "默认方案", "points": []}]
            current_index = 0

        return plans, current_index

    except Exception as e:
        return [{"name": "默认方案", "points": []}], 0

HandEyeCalibrationRepository 

HandEyeCalibrationRepository(config_path: str = None)

手眼标定配置仓储 - Infrastructure层。

负责从文件系统读取配置。

职责： - 从 JSON 文件加载配置 - 转换为 Domain 层的值对象 - 处理文件不存在等异常

属性：

名称 类型 描述
config_path Path

配置文件路径。

初始化配置仓储。

参数：

名称 类型 描述 默认
config_path str

配置文件路径. Defaults to None. 如果为 None，则按以下优先级查找： 1. ROS2 share 目录 (controller/config/hand_eye_calibration.json) 2. 开发环境相对路径 (../../config/hand_eye_calibration.json)

 None
源代码位于： src/controller/controller/infrastructure/persistence/hand_eye_calibration_repository.py 
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
def __init__(self, config_path: str = None):
    """初始化配置仓储。

    Args:
        config_path (str, optional): 配置文件路径. Defaults to None.
            如果为 None，则按以下优先级查找：
            1. ROS2 share 目录 (controller/config/hand_eye_calibration.json)
            2. 开发环境相对路径 (../../config/hand_eye_calibration.json)
    """
    if config_path is None:
        # 使用 ROS2 包资源管理方式查找配置文件
        try:
            # ROS2 方式：从 share 目录加载
            package_share_dir = get_package_share_directory('controller')
            config_path = Path(package_share_dir) / 'config' / 'hand_eye_calibration.json'
        except Exception:
            # 如果包未安装，使用相对路径（开发模式）
            current_file = Path(__file__)
            controller_root = current_file.parent.parent.parent
            config_path = controller_root / "config" / "hand_eye_calibration.json"

    self.config_path = Path(config_path)

load 

load() -> HandEyeCalibrationConfig

加载手眼标定配置。

返回：

名称 类型 描述
HandEyeCalibrationConfig HandEyeCalibrationConfig

配置值对象。

引发：

类型 描述
FileNotFoundError

配置文件不存在。
ValueError

配置格式错误。
源代码位于： src/controller/controller/infrastructure/persistence/hand_eye_calibration_repository.py 
 55
 56
 57
 58
 59
 60
 61
 62
 63
 64
 65
 66
 67
 68
 69
 70
 71
 72
 73
 74
 75
 76
 77
 78
 79
 80
 81
 82
 83
 84
 85
 86
 87
 88
 89
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
def load(self) -> HandEyeCalibrationConfig:
    """加载手眼标定配置。

    Returns:
        HandEyeCalibrationConfig: 配置值对象。

    Raises:
        FileNotFoundError: 配置文件不存在。
        ValueError: 配置格式错误。
    """
    if not self.config_path.exists():
        raise FileNotFoundError(
            f"配置文件不存在: {self.config_path}\n"
            f"请在 controller/config/ 目录下创建 hand_eye_calibration.json"
        )

    try:
        with open(self.config_path, 'r', encoding='utf-8') as f:
            data = json.load(f)
    except json.JSONDecodeError as e:
        raise ValueError(f"配置文件格式错误: {e}")

    # 解析手眼标定矩阵
    hand_eye_matrix = np.array(data['hand_eye_matrix'], dtype=float)

    # 解析相机内参
    intrinsics_data = data['camera_intrinsics']
    camera_intrinsics = CameraIntrinsics(
        fx=intrinsics_data['fx'],
        fy=intrinsics_data['fy'],
        cx=intrinsics_data['cx'],
        cy=intrinsics_data['cy']
    )

    # 解析目标偏移量
    offset_data = data['target_offset']
    target_offset = TargetOffset(
        x=offset_data['x'],
        y=offset_data['y'],
        z=offset_data['z']
    )

    # 解析末端姿态调整（角度转弧度）
    adjustment_data = data['end_effector_adjustment']
    end_effector_adjustment = EndEffectorAdjustment(
        z_rotation=radians(adjustment_data['z_rotation_deg']),
        y_rotation=radians(adjustment_data['y_rotation_deg']),
        x_rotation=radians(adjustment_data['x_rotation_deg'])
    )

    return HandEyeCalibrationConfig(
        hand_eye_matrix=hand_eye_matrix,
        camera_intrinsics=camera_intrinsics,
        target_offset=target_offset,
        end_effector_adjustment=end_effector_adjustment
    )

save 

save(config: HandEyeCalibrationConfig)

保存配置到文件（可选功能，用于标定工具）。

参数：

名称 类型 描述 默认
config HandEyeCalibrationConfig

手眼标定配置对象。

 必需
源代码位于： src/controller/controller/infrastructure/persistence/hand_eye_calibration_repository.py 
112
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
130
131
132
133
134
135
136
137
138
139
140
141
142
143
144
def save(self, config: HandEyeCalibrationConfig):
    """保存配置到文件（可选功能，用于标定工具）。

    Args:
        config (HandEyeCalibrationConfig): 手眼标定配置对象。
    """
    # 确保目录存在
    self.config_path.parent.mkdir(parents=True, exist_ok=True)

    data = {
        "hand_eye_matrix": config.hand_eye_matrix.tolist(),
        "camera_intrinsics": {
            "fx": config.camera_intrinsics.fx,
            "fy": config.camera_intrinsics.fy,
            "cx": config.camera_intrinsics.cx,
            "cy": config.camera_intrinsics.cy
        },
        "target_offset": {
            "x": config.target_offset.x,
            "y": config.target_offset.y,
            "z": config.target_offset.z,
            "comment": "相对于零件位置的偏移量（米）"
        },
        "end_effector_adjustment": {
            "z_rotation_deg": np.degrees(config.end_effector_adjustment.z_rotation),
            "y_rotation_deg": np.degrees(config.end_effector_adjustment.y_rotation),
            "x_rotation_deg": np.degrees(config.end_effector_adjustment.x_rotation),
            "comment": "末端姿态调整（角度）"
        }
    }

    with open(self.config_path, 'w', encoding='utf-8') as f:
        json.dump(data, f, indent=2, ensure_ascii=False)

TrajectoryRepository 

TrajectoryRepository(plans_dir: str = './plans')

轨迹数据仓库 - Infrastructure层。

职责： 1. 保存轨迹数据到文件系统 2. 加载轨迹数据 3. 管理存储目录

属性：

名称 类型 描述
plans_dir Path

轨迹存储目录。

初始化轨迹仓库。

参数：

名称 类型 描述 默认
plans_dir str

存储目录. Defaults to "./plans".

 './plans'
源代码位于： src/controller/controller/infrastructure/persistence/trajectory_repository.py 
22
23
24
25
26
27
28
29
def __init__(self, plans_dir: str = "./plans"):
    """初始化轨迹仓库。

    Args:
        plans_dir (str, optional): 存储目录. Defaults to "./plans".
    """
    self.plans_dir = Path(plans_dir)
    self.plans_dir.mkdir(exist_ok=True)

save_trajectory 

save_trajectory(filename: str, positions: List[List[float]]) -> None

保存轨迹数据。

参数：

名称 类型 描述 默认
filename str

文件名（不含扩展名）。

 必需
positions List[List[float]]

轨迹点序列 [[q1,...,q6], ...]。

 必需
源代码位于： src/controller/controller/infrastructure/persistence/trajectory_repository.py 
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
def save_trajectory(
    self, 
    filename: str, 
    positions: List[List[float]]
) -> None:
    """保存轨迹数据。

    Args:
        filename (str): 文件名（不含扩展名）。
        positions (List[List[float]]): 轨迹点序列 [[q1,...,q6], ...]。
    """
    clean_name = self._sanitize_filename(filename)
    filepath = self.plans_dir / f"{clean_name}.json"

    # 将numpy数组转换为Python列表
    positions_serializable = self._convert_to_serializable(positions)

    with open(filepath, 'w', encoding='utf-8') as f:
        json.dump(positions_serializable, f, indent=2, ensure_ascii=False)

load_trajectory 

load_trajectory(filename: str) -> List[List[float]]

加载轨迹数据。

参数：

名称 类型 描述 默认
filename str

文件名（不含扩展名）。

 必需

返回：

类型 描述
List[List[float]]

List[List[float]]: 轨迹点序列。
源代码位于： src/controller/controller/infrastructure/persistence/trajectory_repository.py 
51
52
53
54
55
56
57
58
59
60
61
62
def load_trajectory(self, filename: str) -> List[List[float]]:
    """加载轨迹数据。

    Args:
        filename (str): 文件名（不含扩展名）。

    Returns:
        List[List[float]]: 轨迹点序列。
    """
    filepath = self.plans_dir / f"{filename}.json"
    with open(filepath, 'r', encoding='utf-8') as f:
        return json.load(f)

list_trajectory_files 

list_trajectory_files() -> List[str]

列出所有可用的轨迹文件。

返回：

名称 类型 描述
List[str]

List[str]: 文件名列表（不带扩展名）。
例如 List[str]

["默认方案-0", "默认方案-1", "test_trajectory"]
源代码位于： src/controller/controller/infrastructure/persistence/trajectory_repository.py 
64
65
66
67
68
69
70
71
72
73
74
75
76
77
78
def list_trajectory_files(self) -> List[str]:
    """列出所有可用的轨迹文件。

    Returns:
        List[str]: 文件名列表（不带扩展名）。
        例如: ["默认方案-0", "默认方案-1", "test_trajectory"]
    """
    if not self.plans_dir.exists():
        return []

    files = []
    for file in self.plans_dir.glob("*.json"):
        files.append(file.stem)  # 文件名不带扩展名

    return sorted(files)

trajectory_exists 

trajectory_exists(filename: str) -> bool

检查轨迹文件是否存在。

参数：

名称 类型 描述 默认
filename str

文件名（不含扩展名）。

 必需

返回：

名称 类型 描述
bool bool

True=文件存在, False=文件不存在。
源代码位于： src/controller/controller/infrastructure/persistence/trajectory_repository.py 
80
81
82
83
84
85
86
87
88
89
90
def trajectory_exists(self, filename: str) -> bool:
    """检查轨迹文件是否存在。

    Args:
        filename (str): 文件名（不含扩展名）。

    Returns:
        bool: True=文件存在, False=文件不存在。
    """
    filepath = self.plans_dir / f"{filename}.json"
    return filepath.exists()

register_infrastructure_services 

register_infrastructure_services(container: DIContainer) -> None

注册 Infrastructure 层服务。

参数：

名称 类型 描述 默认
container DIContainer

DI 容器实例。

 必需
源代码位于： src/controller/controller/shared/config/service_registry.py 
12
13
14
15
16
17
18
19
20
21
22
23
24
25
def register_infrastructure_services(container: DIContainer) -> None:
    """注册 Infrastructure 层服务。

    Args:
        container (DIContainer): DI 容器实例。
    """
    # 记录数据持久化仓库
    container.register_singleton(RecordRepository)
    # 运动规划方案持久化仓库
    container.register_singleton(MotionPlanRepository)
    # 手眼标定配置仓库
    container.register_singleton(HandEyeCalibrationRepository)
    # 轨迹数据仓库
    container.register_singleton(TrajectoryRepository)

register_domain_services 

register_domain_services(container: DIContainer) -> None

注册 Domain 层服务。

参数：

名称 类型 描述 默认
container DIContainer

DI 容器实例。

 必需
源代码位于： src/controller/controller/shared/config/service_registry.py 
27
28
29
30
31
32
33
34
35
36
37
38
39
40
41
42
43
44
45
46
47
48
49
50
51
52
53
54
55
56
57
58
59
60
61
62
63
64
65
66
67
68
69
70
def register_domain_services(container: DIContainer) -> None:
    """注册 Domain 层服务。

    Args:
        container (DIContainer): DI 容器实例。
    """
    container.register_singleton(SerialDomainService)
    container.register_singleton(MessageEncoder)
    container.register_singleton(MessageDecoder)
    container.register_singleton(MessageDomainService)
    container.register_singleton(MotionRunner)
    # 轨迹平滑服务
    container.register_singleton(SmoothDomainService)
    # 运动学和轨迹规划服务
    container.register_singleton(KinematicDomainService)
    container.register_singleton(LinearMotionDomainService)  # 依赖 KinematicDomainService
    container.register_singleton(CurveMotionDomainService)  # 依赖 KinematicDomainService
    # S曲线算法
    container.register_singleton(SCurve)
    # 轨迹规划服务（依赖 SCurve, SmoothDomainService, LinearMotionDomainService, CurveMotionDomainService）
    container.register_singleton(TrajectoryPlanningService)
    # 运动构造器（依赖 MotionRunner, TrajectoryPlanningService）
    container.register_singleton(MotionConstructor)
    # 机械臂状态服务
    container.register_singleton(RobotStateDomainService)
    # 动力学服务
    container.register_singleton(DynamicDomainService)
    # 示教记录服务
    container.register_singleton(TeachRecordDomainService)
    # 运动规划服务
    container.register_singleton(MotionPlanningDomainService)
    # 视觉服务
    container.register_singleton(CameraDomainService)
    container.register_singleton(RecognitionDomainService)

    # 手眼标定配置（作为值对象注入）
    def create_hand_eye_config():
        repo = resolve(HandEyeCalibrationRepository)
        return repo.load()

    container.register_singleton(HandEyeCalibrationConfig, create_hand_eye_config)

    # 手眼标定服务（会自动注入 HandEyeCalibrationConfig 和 KinematicDomainService）
    container.register_singleton(HandEyeTransformDomainService)

register_application_services 

register_application_services(container: DIContainer) -> None

注册 Application 层服务。

参数：

名称 类型 描述 默认
container DIContainer

DI 容器实例。

 必需
源代码位于： src/controller/controller/shared/config/service_registry.py 
72
73
74
75
76
77
78
79
80
81
82
83
84
85
86
87
88
def register_application_services(container: DIContainer) -> None:
    """注册 Application 层服务。

    Args:
        container (DIContainer): DI 容器实例。
    """
    container.register_singleton(MessageDisplay)
    container.register_singleton(SerialApplicationService)
    container.register_singleton(CommandHubService)
    container.register_singleton(MessageResponseService)
    container.register_singleton(MotionListener)
    # 运动规划应用服务
    container.register_singleton(MotionPlanningApplicationService)
    # 摄像头应用服务
    container.register_singleton(CameraApplicationService)
    # 工具应用服务
    container.register_singleton(ToolsApplicationService)

register_presentation_services 

register_presentation_services(container: DIContainer) -> None

注册 Presentation 层服务。

参数：

名称 类型 描述 默认
container DIContainer

DI 容器实例。

 必需
源代码位于： src/controller/controller/shared/config/service_registry.py 
 90
 91
 92
 93
 94
 95
 96
 97
 98
 99
100
101
102
103
104
105
106
107
108
109
110
111
def register_presentation_services(container: DIContainer) -> None:
    """注册 Presentation 层服务。

    Args:
        container (DIContainer): DI 容器实例。
    """

    container.register_singleton(DisplayViewModel)
    container.register_singleton(SerialViewModel)
    container.register_singleton(StatusViewModel)  
    container.register_singleton(ControlViewModel)
    container.register_singleton(EffectorViewModel)
    container.register_singleton(TrajectoryViewModel)
    container.register_singleton(DynamicsViewModel)  
    # CameraViewModel 只依赖 CameraApplicationService（Application层）
    # DI容器会自动根据构造函数注入依赖
    container.register_singleton(CameraViewModel)
    # 运动规划ViewModel
    container.register_singleton(MotionPlanningViewModel)
    # 工具ViewModel
    container.register_singleton(ToolsViewModel)
    container.register_singleton(MainViewModel)

configure_services 

configure_services() -> DIContainer

配置所有服务。

按层次注册所有依赖服务。

返回：

名称 类型 描述
DIContainer DIContainer

配置好的容器实例。
源代码位于： src/controller/controller/shared/config/service_registry.py 
113
114
115
116
117
118
119
120
121
122
123
124
125
126
127
128
129
def configure_services() -> DIContainer:
    """配置所有服务。

    按层次注册所有依赖服务。

    Returns:
        DIContainer: 配置好的容器实例。
    """
    container = get_container()

    # 按层次注册服务
    register_infrastructure_services(container)
    register_domain_services(container)
    register_application_services(container)
    register_presentation_services(container)

    return container

get_main_view_model 

get_main_view_model() -> MainViewModel

便捷方法：获取主视图模型。

返回：

名称 类型 描述
MainViewModel MainViewModel

主视图模型实例。
源代码位于： src/controller/controller/shared/config/service_registry.py 
132
133
134
135
136
137
138
def get_main_view_model() -> MainViewModel:
    """便捷方法：获取主视图模型。

    Returns:
        MainViewModel: 主视图模型实例。
    """
    return resolve(MainViewModel)

get_serial_service 

get_serial_service() -> SerialApplicationService

便捷方法：获取串口服务。

返回：

名称 类型 描述
SerialApplicationService SerialApplicationService

串口应用服务实例。
源代码位于： src/controller/controller/shared/config/service_registry.py 
141
142
143
144
145
146
147
def get_serial_service() -> SerialApplicationService:
    """便捷方法：获取串口服务。

    Returns:
        SerialApplicationService: 串口应用服务实例。
    """
    return resolve(SerialApplicationService)

get_serial_view_model 

get_serial_view_model() -> SerialViewModel

便捷方法：获取串口视图模型。

返回：

名称 类型 描述
SerialViewModel SerialViewModel

串口视图模型实例。
源代码位于： src/controller/controller/shared/config/service_registry.py 
150
151
152
153
154
155
156
def get_serial_view_model() -> SerialViewModel:
    """便捷方法：获取串口视图模型。

    Returns:
        SerialViewModel: 串口视图模型实例。
    """
    return resolve(SerialViewModel)

get_display_view_model 

get_display_view_model() -> DisplayViewModel

便捷方法：获取显示视图模型。

返回：

名称 类型 描述
DisplayViewModel DisplayViewModel

显示视图模型实例。
源代码位于： src/controller/controller/shared/config/service_registry.py 
159
160
161
162
163
164
165
def get_display_view_model() -> DisplayViewModel:
    """便捷方法：获取显示视图模型。

    Returns:
        DisplayViewModel: 显示视图模型实例。
    """
    return resolve(DisplayViewModel)

get_listener_service 

get_listener_service() -> MotionListener

便捷方法：获取监听服务。

返回：

名称 类型 描述
MotionListener MotionListener

监听服务实例。
源代码位于： src/controller/controller/shared/config/service_registry.py 
167
168
169
170
171
172
173
def get_listener_service() -> MotionListener:
    """便捷方法：获取监听服务。

    Returns:
        MotionListener: 监听服务实例。
    """
    return resolve(MotionListener)