commit dcdbe4019a5081b198ab882bf421caaf587570c6 Author: cnc Date: Fri May 22 15:11:33 2026 +0800 Initial project snapshot diff --git a/gpb_GLCanon_arcseg_sim_rtcp_rs274_kins.py b/gpb_GLCanon_arcseg_sim_rtcp_rs274_kins.py new file mode 100644 index 0000000..978c41b --- /dev/null +++ b/gpb_GLCanon_arcseg_sim_rtcp_rs274_kins.py @@ -0,0 +1,12697 @@ +#!/usr/bin/env python3 +# -*- coding: utf-8 -*- +""" +GLCanon 五轴刀具路径解析器 - 完整集成版 +包含：RTCP运动学 + 完整G代码解析 + 梯形速度规划 + 虚拟HAL + 完整状态机 + JSON配置支持 +集成 LinuxCNC 运动学算法：trtfuncs.c, 5axiskins.c, genhexkins.c, genserfuncs.c, +pumakins.c, scarakins.c, tripodkins.c, pentakins.c, rotarydeltakins.c, rotatekins.c, +corexykins.c, scorbot-kins.c, rosekins.c +集成 LinuxCNC 完整参数表系统 (linuxcnc_parameter_table) + +版本: 5.0.0 - 完整集成所有 LinuxCNC 运动学算法和参数系统 +""" + +import os +import sys +import re +import math +import time +import threading +import tempfile +import json +import traceback +from typing import List, Dict, Optional, Tuple, Any, Callable, Union, Set +from enum import IntEnum, Enum +from dataclasses import dataclass, field +from collections import deque, OrderedDict +from abc import ABC, abstractmethod +import dataclasses + +# ==================== 导入 LinuxCNC 参数表系统 ==================== +# ==================== 导入 LinuxCNC 参数表系统 ==================== +try: + import linuxcnc_parameter_table as lcnc_param + from linuxcnc_parameter_table import ( + LinuxCNCParameterTable, + InterpParameterIndex, + ParameterAttribute, + ParameterValue, + ExpressionEvaluator, + HALVariableManager, + SystemVariableQuery, + Context, + StateTag, + Block, + # G代码常量 + G_0, G_1, G_2, G_3, G_4, G_5, G_5_1, G_5_2, G_5_3, + G_6, G_6_1, G_6_2, G_6_3, G_7, G_8, G_10, + G_17, G_17_1, G_18, G_18_1, G_19, G_19_1, + G_20, G_21, G_28, G_28_1, G_30, G_30_1, + G_33, G_33_1, G_38_2, G_38_3, G_38_4, G_38_5, + G_40, G_41, G_41_1, G_42, G_42_1, + G_43, G_43_1, G_43_2, G_49, G_52, G_53, + G_54, G_55, G_56, G_57, G_58, G_59, + G_59_1, G_59_2, G_59_3, G_61, G_61_1, G_64, + G_70, G_71, G_71_1, G_71_2, G_72, G_72_1, G_72_2, + G_73, G_74, G_76, G_80, G_81, G_82, G_83, G_84, + G_85, G_86, G_87, G_88, G_89, G_90, G_90_1, + G_91, G_91_1, G_92, G_92_1, G_92_2, G_92_3, + G_93, G_94, G_95, G_96, G_97, G_98, G_99, + # 常量 + RS274NGC_MAX_PARAMETERS, READONLY_PARAMETERS, REQUIRED_PARAMETERS, + INTERP_SUB_PARAMS, INTERP_SUB_ROUTINE_LEVELS, + TOLERANCE_EQUAL, TOLERANCE_CONCAVE_CORNER, + EMCMOT_MAX_AXIS, EMCMOT_MAX_JOINTS, EMCMOT_MAX_SPINDLES, + ) +except ImportError: + print("错误: 无法导入 linuxcnc_parameter_table 模块") + print("请确保 linuxcnc_parameter_table.py 在同一目录下") + sys.exit(1) + +# ==================== 常量定义 ==================== +VERSION = "5.0.0" + +# 数学常量 +CART_FUZZ = 1e-5 +TO_RAD = math.pi / 180.0 +TO_DEG = 180.0 / math.pi +GO_PI_2 = math.pi / 2.0 +GO_PI = math.pi +PM_PI = math.pi +PM_2_PI = 2.0 * math.pi +TOLERANCE_EQUAL = 1e-6 +TOLERANCE_CONCAVE_CORNER = 1e-5 +TINY = 1e-7 + +# 单位转换 +INCH_PER_MM = 0.03937007874016 +MM_PER_INCH = 25.4 + +# 最大轴数 +EMCMOT_MAX_AXIS = 9 +EMCMOT_MAX_JOINTS = 9 +EMCMOT_MAX_SPINDLES = 4 + +# 刀具表大小 +CANON_POCKETS_MAX = 56 + +# 栈深度 +STACK_LEN = 50 +STACK_ENTRY_LEN = 80 + +# 行长度 +LINELEN = 256 + +# 从 linuxcnc_parameter_table 导入常量 +RS274NGC_MAX_PARAMETERS = lcnc_param.RS274NGC_MAX_PARAMETERS +READONLY_PARAMETERS = lcnc_param.READONLY_PARAMETERS +REQUIRED_PARAMETERS = lcnc_param.REQUIRED_PARAMETERS +INTERP_SUB_PARAMS = lcnc_param.INTERP_SUB_PARAMS +INTERP_SUB_ROUTINE_LEVELS = lcnc_param.INTERP_SUB_ROUTINE_LEVELS + +# G代码常量（从lcnc_param导入） +G_0 = lcnc_param.G_0 +G_1 = lcnc_param.G_1 +G_2 = lcnc_param.G_2 +G_3 = lcnc_param.G_3 +G_4 = lcnc_param.G_4 +G_5 = lcnc_param.G_5 +G_5_1 = lcnc_param.G_5_1 +G_5_2 = lcnc_param.G_5_2 +G_5_3 = lcnc_param.G_5_3 +G_6 = lcnc_param.G_6 +G_6_1 = lcnc_param.G_6_1 +G_6_2 = lcnc_param.G_6_2 +G_6_3 = lcnc_param.G_6_3 +G_7 = lcnc_param.G_7 +G_8 = lcnc_param.G_8 +G_10 = lcnc_param.G_10 +G_17 = lcnc_param.G_17 +G_17_1 = lcnc_param.G_17_1 +G_18 = lcnc_param.G_18 +G_18_1 = lcnc_param.G_18_1 +G_19 = lcnc_param.G_19 +G_19_1 = lcnc_param.G_19_1 +G_20 = lcnc_param.G_20 +G_21 = lcnc_param.G_21 +G_28 = lcnc_param.G_28 +G_28_1 = lcnc_param.G_28_1 +G_30 = lcnc_param.G_30 +G_30_1 = lcnc_param.G_30_1 +G_33 = lcnc_param.G_33 +G_33_1 = lcnc_param.G_33_1 +G_38_2 = lcnc_param.G_38_2 +G_38_3 = lcnc_param.G_38_3 +G_38_4 = lcnc_param.G_38_4 +G_38_5 = lcnc_param.G_38_5 +G_40 = lcnc_param.G_40 +G_41 = lcnc_param.G_41 +G_41_1 = lcnc_param.G_41_1 +G_42 = lcnc_param.G_42 +G_42_1 = lcnc_param.G_42_1 +G_43 = lcnc_param.G_43 +G_43_1 = lcnc_param.G_43_1 +G_43_2 = lcnc_param.G_43_2 +G_49 = lcnc_param.G_49 +G_52 = lcnc_param.G_52 +G_53 = lcnc_param.G_53 +G_54 = lcnc_param.G_54 +G_55 = lcnc_param.G_55 +G_56 = lcnc_param.G_56 +G_57 = lcnc_param.G_57 +G_58 = lcnc_param.G_58 +G_59 = lcnc_param.G_59 +G_59_1 = lcnc_param.G_59_1 +G_59_2 = lcnc_param.G_59_2 +G_59_3 = lcnc_param.G_59_3 +G_61 = lcnc_param.G_61 +G_61_1 = lcnc_param.G_61_1 +G_64 = lcnc_param.G_64 +G_70 = lcnc_param.G_70 +G_71 = lcnc_param.G_71 +G_71_1 = lcnc_param.G_71_1 +G_71_2 = lcnc_param.G_71_2 +G_72 = lcnc_param.G_72 +G_72_1 = lcnc_param.G_72_1 +G_72_2 = lcnc_param.G_72_2 +G_73 = lcnc_param.G_73 +G_74 = lcnc_param.G_74 +G_76 = lcnc_param.G_76 +G_80 = lcnc_param.G_80 +G_81 = lcnc_param.G_81 +G_82 = lcnc_param.G_82 +G_83 = lcnc_param.G_83 +G_84 = lcnc_param.G_84 +G_85 = lcnc_param.G_85 +G_86 = lcnc_param.G_86 +G_87 = lcnc_param.G_87 +G_88 = lcnc_param.G_88 +G_89 = lcnc_param.G_89 +G_90 = lcnc_param.G_90 +G_90_1 = lcnc_param.G_90_1 +G_91 = lcnc_param.G_91 +G_91_1 = lcnc_param.G_91_1 +G_92 = lcnc_param.G_92 +G_92_1 = lcnc_param.G_92_1 +G_92_2 = lcnc_param.G_92_2 +G_92_3 = lcnc_param.G_92_3 +G_93 = lcnc_param.G_93 +G_94 = lcnc_param.G_94 +G_95 = lcnc_param.G_95 +G_96 = lcnc_param.G_96 +G_97 = lcnc_param.G_97 +G_98 = lcnc_param.G_98 +G_99 = lcnc_param.G_99 + +# 默认颜色字典 +DEFAULT_COLORS = { + 'back': '#000000', + 'grid': '#404040', + 'axis': '#c0c0c0', + 'rapid': '#00ff00', + 'feed': '#ffff00', + 'arc_cw': '#00ffff', + 'arc_ccw': '#ff00ff', + 'dwell': '#ffffff', + 'probe': '#ff8000', + 'selected': '#ff0000', + 'traverse': '#00ff00', + 'straight_feed': '#ffff00', + 'arc_feed': '#00ffff', + 'm1xx': '#ff8000', + 'rtcp_on': '#00ff88', + 'rtcp_off': '#ff6600', + 'program_end': '#ff0000', + 'g0': '#00ff00', + 'g1': '#ffff00', + 'g2': '#00ffff', + 'g3': '#ff00ff', + 'g38': '#ff8000', + 'g80': '#808080', + 'm6': '#ff0000', + 'm3': '#00ff00', + 'm4': '#ff0000', + 'm5': '#ffff00', + 'm7': '#0088ff', + 'm8': '#0088ff', + 'm9': '#ff0000', +} + + +# ==================== 纯 Python 实现的包围盒计算 ==================== +def calc_extents_python(*lists): + """ + 计算刀具路径的包围盒（纯 Python 实现，替代 gcode.calc_extents） + """ + INF = 9e99 + + min_x = min_y = min_z = INF + max_x = max_y = max_z = -INF + + min_xt = min_yt = min_zt = INF + max_xt = max_yt = max_zt = -INF + + for seq in lists: + if not seq: + continue + + for item in seq: + if len(item) == 5: + linenum, start, end, feed, tooloffset = item + elif len(item) == 4: + linenum, start, end, tooloffset = item + else: + continue + + xs, ys, zs = start[0], start[1], start[2] + xe, ye, ze = end[0], end[1], end[2] + + if len(tooloffset) >= 3: + xt, yt, zt = tooloffset[0], tooloffset[1], tooloffset[2] + else: + xt = yt = zt = 0.0 + + for (x, y, z) in [(xs, ys, zs), (xe, ye, ze)]: + if x < min_x: min_x = x + if x > max_x: max_x = x + if y < min_y: min_y = y + if y > max_y: max_y = y + if z < min_z: min_z = z + if z > max_z: max_z = z + + for (x, y, z) in [(xs + xt, ys + yt, zs + zt), (xe + xt, ye + yt, ze + zt)]: + if x < min_xt: min_xt = x + if x > max_xt: max_xt = x + if y < min_yt: min_yt = y + if y > max_yt: max_yt = y + if z < min_zt: min_zt = z + if z > max_zt: max_zt = z + + if min_x == INF: + min_x = min_y = min_z = max_x = max_y = max_z = 0 + if min_xt == INF: + min_xt = min_yt = min_zt = max_xt = max_yt = max_zt = 0 + + return ( + [min_x, min_y, min_z], [max_x, max_y, max_z], + [min_xt, min_yt, min_zt], [max_xt, max_yt, max_zt] + ) + + +# ==================== 纯 Python 实现的圆弧转线段 ==================== +def arcs_to_segments_python( + canon_obj: Any, + x1: float, y1: float, + cx: float, cy: float, + rot: int, + z1: float, + a: float, b: float, c: float, + u: float, v: float, w: float, + max_segments: int = 128 +) -> List[Tuple[float, ...]]: + """ + 圆弧转线段 - 纯 Python 实现，替代 gcode.arc_to_segments + """ + if hasattr(canon_obj, 'lo'): + o = list(canon_obj.lo) + else: + o = [0.0] * 9 + + while len(o) < 9: + o.append(0.0) + + plane = getattr(canon_obj, 'plane', 17) + + rotation_cos = getattr(canon_obj, 'rotation_cos', 1.0) + rotation_sin = getattr(canon_obj, 'rotation_sin', 0.0) + + g5x_offset = [ + getattr(canon_obj, 'g5x_offset_x', 0.0), + getattr(canon_obj, 'g5x_offset_y', 0.0), + getattr(canon_obj, 'g5x_offset_z', 0.0), + getattr(canon_obj, 'g5x_offset_a', 0.0), + getattr(canon_obj, 'g5x_offset_b', 0.0), + getattr(canon_obj, 'g5x_offset_c', 0.0), + getattr(canon_obj, 'g5x_offset_u', 0.0), + getattr(canon_obj, 'g5x_offset_v', 0.0), + getattr(canon_obj, 'g5x_offset_w', 0.0) + ] + + g92_offset = [ + getattr(canon_obj, 'g92_offset_x', 0.0), + getattr(canon_obj, 'g92_offset_y', 0.0), + getattr(canon_obj, 'g92_offset_z', 0.0), + getattr(canon_obj, 'g92_offset_a', 0.0), + getattr(canon_obj, 'g92_offset_b', 0.0), + getattr(canon_obj, 'g92_offset_c', 0.0), + getattr(canon_obj, 'g92_offset_u', 0.0), + getattr(canon_obj, 'g92_offset_v', 0.0), + getattr(canon_obj, 'g92_offset_w', 0.0) + ] + + if plane == 17: + X, Y, Z = 0, 1, 2 + elif plane == 19: + X, Y, Z = 2, 0, 1 + else: + X, Y, Z = 1, 2, 0 + + n = [0.0] * 9 + n[X] = x1 + n[Y] = y1 + n[Z] = z1 + n[3] = a + n[4] = b + n[5] = c + n[6] = u + n[7] = v + n[8] = w + + for ax in range(9): + o[ax] -= g5x_offset[ax] + + tx = o[X] + ty = o[Y] + o[X] = tx * rotation_cos + ty * rotation_sin + o[Y] = -tx * rotation_sin + ty * rotation_cos + + for ax in range(9): + o[ax] -= g92_offset[ax] + + theta1 = math.atan2(o[Y] - cy, o[X] - cx) + theta2 = math.atan2(n[Y] - cy, n[X] - cx) + len_arc = math.hypot(o[X] - n[X], o[Y] - n[Y]) + + if rot < 0: + if theta1 < theta2: + theta2 -= 2.0 * math.pi + if len_arc < CART_FUZZ: + theta2 -= 2.0 * math.pi + else: + if theta1 > theta2: + theta2 += 2.0 * math.pi + if len_arc < CART_FUZZ: + theta2 += 2.0 * math.pi + + if rot < -1: + theta2 += 2.0 * math.pi * (rot + 1) + if rot > 1: + theta2 += 2.0 * math.pi * (rot - 1) + + delta_theta = theta2 - theta1 + steps = max(3, int(max_segments * abs(delta_theta) / math.pi)) + rsteps = 1.0 / steps + + d = [0.0] * 9 + d[Z] = n[Z] - o[Z] + d[3] = n[3] - o[3] + d[4] = n[4] - o[4] + d[5] = n[5] - o[5] + d[6] = n[6] - o[6] + d[7] = n[7] - o[7] + d[8] = n[8] - o[8] + + segs = [] + tx = o[X] - cx + ty = o[Y] - cy + dc = math.cos(delta_theta * rsteps) + ds = math.sin(delta_theta * rsteps) + + for i in range(steps - 1): + f = (i + 1) * rsteps + new_tx = tx * dc - ty * ds + new_ty = tx * ds + ty * dc + tx = new_tx + ty = new_ty + + p = [0.0] * 9 + p[X] = tx + cx + p[Y] = ty + cy + p[Z] = o[Z] + d[Z] * f + p[3] = o[3] + d[3] * f + p[4] = o[4] + d[4] * f + p[5] = o[5] + d[5] * f + p[6] = o[6] + d[6] * f + p[7] = o[7] + d[7] * f + p[8] = o[8] + d[8] * f + + for ax in range(9): + p[ax] += g92_offset[ax] + + px = p[X] + py = p[Y] + p[X] = px * rotation_cos - py * rotation_sin + p[Y] = px * rotation_sin + py * rotation_cos + + for ax in range(9): + p[ax] += g5x_offset[ax] + + segs.append(tuple(p)) + + for ax in range(9): + n[ax] += g92_offset[ax] + + nx = n[X] + ny = n[Y] + n[X] = nx * rotation_cos - ny * rotation_sin + n[Y] = nx * rotation_sin + ny * rotation_cos + + for ax in range(9): + n[ax] += g5x_offset[ax] + + segs.append(tuple(n)) + + return segs + + +# ==================== 单位转换函数 ==================== +def convert_units(value: float, from_units: int, to_units: int) -> float: + """单位转换""" + if from_units == to_units: + return value + if from_units == 20 and to_units == 21: # 英寸 -> 毫米 + return value * MM_PER_INCH + if from_units == 21 and to_units == 20: # 毫米 -> 英寸 + return value * INCH_PER_MM + return value + + +def program_to_user_len(value: float, units: int) -> float: + """程序单位 -> 用户单位（长度）""" + return value + + +def user_to_program_len(value: float, units: int) -> float: + """用户单位 -> 程序单位（长度）""" + return value + + +def program_to_user_ang(value: float) -> float: + """程序单位 -> 用户单位（角度）""" + return value + + +def user_to_program_ang(value: float) -> float: + """用户单位 -> 程序单位（角度）""" + return value + + +# ==================== JSON配置数据类 ==================== +@dataclass +class ProgramConfig: + """程序配置""" + name: str = "" + description: str = "" + content: str = "" + + +@dataclass +class SubroutineConfig: + """子程序配置""" + name: str = "" + description: str = "" + content: str = "" + parameters: List[Dict[str, Any]] = field(default_factory=list) + + +@dataclass +class MachineConfig: + """机床配置数据类""" + name: str = "" + type: str = "5axis_mill" + kinematics_type: str = "TRT_BC" + kinematics_params: Dict[str, Any] = field(default_factory=dict) + axes: Dict[str, Dict[str, float]] = field(default_factory=dict) + spindle: Dict[str, Any] = field(default_factory=dict) + tool_table: List[Dict[str, Any]] = field(default_factory=list) + work_offsets: Dict[str, Dict[str, float]] = field(default_factory=dict) + + +@dataclass +class SimulationConfig: + """仿真配置""" + acceleration: float = 500.0 + deceleration: float = 500.0 + max_rapid_rate: float = 10000.0 + max_feed_rate: float = 5000.0 + arc_division: int = 64 + enable_rtcp_debug: bool = False + max_points: int = 50000 + colors: Dict[str, str] = field(default_factory=dict) + rapid_override: float = 1.0 + feed_override: float = 1.0 + spindle_override: float = 1.0 + + +@dataclass +class HALPinConfig: + """HAL引脚配置""" + name: str = "" + pin_type: str = "float" + direction: str = "in" + default: Any = 0.0 + min_val: Optional[float] = None + max_val: Optional[float] = None + description: str = "" + + +@dataclass +class HALConfig: + """HAL配置数据类""" + version: str = "1.0" + description: str = "" + pins: Dict[str, HALPinConfig] = field(default_factory=dict) + connections: List[Dict[str, str]] = field(default_factory=list) + + +@dataclass +class VisualizationConfig: + """可视化配置""" + camera: Dict[str, Any] = field(default_factory=dict) + grid: Dict[str, Any] = field(default_factory=dict) + show_tool: bool = True + show_path: bool = True + show_coord_systems: bool = True + show_workpiece: Dict[str, Any] = field(default_factory=dict) + show_grid: bool = True + show_axes: bool = True + + +@dataclass +class ProjectConfig: + """项目完整配置""" + project: Dict[str, str] = field(default_factory=dict) + machine: MachineConfig = field(default_factory=MachineConfig) + hal_config: HALConfig = field(default_factory=HALConfig) + main_program: ProgramConfig = field(default_factory=ProgramConfig) + subroutines: List[SubroutineConfig] = field(default_factory=list) + simulation_config: SimulationConfig = field(default_factory=SimulationConfig) + visualization: VisualizationConfig = field(default_factory=VisualizationConfig) + execution_order: List[Dict[str, str]] = field(default_factory=list) + + +# ==================== 运动学抽象基类 ==================== + +@dataclass +class KinematicsParams: + """运动学参数基类""" + pass + + +class BaseKinematics(ABC): + """运动学抽象基类""" + + def __init__(self, params: KinematicsParams = None): + self.params = params + self.rtcp_enabled = True + self.debug = False + self._tool_length = 0.0 + + @abstractmethod + def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + """ + 正运动学：关节坐标 -> 世界坐标 (刀尖点) + 参数: joints = [轴1, 轴2, ...] + 返回: (X, Y, Z, A, B, C) + """ + pass + + @abstractmethod + def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]: + """ + 逆运动学：世界坐标 (刀尖点) -> 关节坐标 + 参数: world = (X, Y, Z, A, B, C) + 返回: [轴1, 轴2, ...] + """ + pass + + def set_tool_length(self, length: float): + """设置刀具长度 (用于 RTCP)""" + self._tool_length = length + + def get_tool_length(self) -> float: + """获取刀具长度""" + return self._tool_length + + def enable_rtcp(self, enable: bool = True): + """启用/禁用 RTCP""" + self.rtcp_enabled = enable + + def set_params(self, **kwargs): + """更新运动学参数""" + if self.params: + for key, value in kwargs.items(): + if hasattr(self.params, key): + setattr(self.params, key, value) + + def joints_to_world(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + """关节坐标转世界坐标 (别名)""" + if not self.rtcp_enabled: + if len(joints) >= 6: + return (joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]) + return (joints[0], joints[1], joints[2], 0.0, 0.0, 0.0) + return self.forward(joints) + + def world_to_joints(self, world: Tuple[float, float, float, float, float, float]) -> List[float]: + """世界坐标转关节坐标 (别名)""" + if not self.rtcp_enabled: + result = [world[0], world[1], world[2], world[3], world[4], world[5]] + while len(result) < EMCMOT_MAX_JOINTS: + result.append(0.0) + return result + result = self.inverse(world) + while len(result) < EMCMOT_MAX_JOINTS: + result.append(0.0) + return result + + +# ==================== 运动学类型枚举 ==================== + +class KinematicsType(Enum): + """五轴运动学类型 - 基于LinuxCNC实现""" + IDENTITY = 0 + TRT_AC = 1 + TRT_BC = 2 + MAXKINS_BC = 3 + FIVEAXIS_BC = 4 + HEXAPOD = 5 + SERIAL_DH = 6 + PUMA = 7 + SCARA = 8 + SCORBOT = 9 + LINEAR_DELTA = 10 + ROTARY_DELTA = 11 + TRIPOD = 12 + PENTAPOD = 13 + ROTATE = 14 + COREXY = 15 + ROSE = 16 + + +# 运动学关节配置 +KINEMATICS_JOINT_CONFIG = { + KinematicsType.IDENTITY: { + 'num_joints': 6, + 'joint_names': ['X', 'Y', 'Z', 'A', 'B', 'C'], + 'x_idx': 0, 'y_idx': 1, 'z_idx': 2, + 'a_idx': 3, 'b_idx': 4, 'c_idx': 5, + }, + KinematicsType.TRT_AC: { + 'num_joints': 5, + 'joint_names': ['X', 'Y', 'Z', 'A', 'C'], + 'x_idx': 0, 'y_idx': 1, 'z_idx': 2, + 'a_idx': 3, 'b_idx': -1, 'c_idx': 4, + }, + KinematicsType.TRT_BC: { + 'num_joints': 5, + 'joint_names': ['X', 'Y', 'Z', 'B', 'C'], + 'x_idx': 0, 'y_idx': 1, 'z_idx': 2, + 'a_idx': -1, 'b_idx': 3, 'c_idx': 4, + }, + KinematicsType.MAXKINS_BC: { + 'num_joints': 9, + 'joint_names': ['X', 'Y', 'Z', 'A', 'B', 'C', 'U', 'V', 'W'], + 'x_idx': 0, 'y_idx': 1, 'z_idx': 2, + 'a_idx': 3, 'b_idx': 4, 'c_idx': 5, + }, + KinematicsType.FIVEAXIS_BC: { + 'num_joints': 6, + 'joint_names': ['X', 'Y', 'Z', 'B', 'C', 'W'], + 'x_idx': 0, 'y_idx': 1, 'z_idx': 2, + 'a_idx': -1, 'b_idx': 3, 'c_idx': 4, + }, + KinematicsType.HEXAPOD: { + 'num_joints': 6, + 'joint_names': ['S1', 'S2', 'S3', 'S4', 'S5', 'S6'], + 'x_idx': -1, 'y_idx': -1, 'z_idx': -1, + 'a_idx': -1, 'b_idx': -1, 'c_idx': -1, + }, + KinematicsType.PUMA: { + 'num_joints': 6, + 'joint_names': ['J1', 'J2', 'J3', 'J4', 'J5', 'J6'], + 'x_idx': -1, 'y_idx': -1, 'z_idx': -1, + 'a_idx': -1, 'b_idx': -1, 'c_idx': -1, + }, + KinematicsType.SCARA: { + 'num_joints': 4, + 'joint_names': ['J1', 'J2', 'Z', 'J4'], + 'x_idx': -1, 'y_idx': -1, 'z_idx': 2, + 'a_idx': -1, 'b_idx': -1, 'c_idx': 3, + }, + KinematicsType.SCORBOT: { + 'num_joints': 5, + 'joint_names': ['J0', 'J1', 'J2', 'J3', 'J4'], + 'x_idx': -1, 'y_idx': -1, 'z_idx': -1, + 'a_idx': 3, 'b_idx': 4, 'c_idx': -1, + }, + KinematicsType.LINEAR_DELTA: { + 'num_joints': 3, + 'joint_names': ['Z1', 'Z2', 'Z3'], + 'x_idx': -1, 'y_idx': -1, 'z_idx': -1, + 'a_idx': -1, 'b_idx': -1, 'c_idx': -1, + }, + KinematicsType.ROTARY_DELTA: { + 'num_joints': 3, + 'joint_names': ['T1', 'T2', 'T3'], + 'x_idx': -1, 'y_idx': -1, 'z_idx': -1, + 'a_idx': -1, 'b_idx': -1, 'c_idx': -1, + }, + KinematicsType.TRIPOD: { + 'num_joints': 3, + 'joint_names': ['L1', 'L2', 'L3'], + 'x_idx': -1, 'y_idx': -1, 'z_idx': -1, + 'a_idx': -1, 'b_idx': -1, 'c_idx': -1, + }, + KinematicsType.PENTAPOD: { + 'num_joints': 5, + 'joint_names': ['L1', 'L2', 'L3', 'L4', 'L5'], + 'x_idx': -1, 'y_idx': -1, 'z_idx': -1, + 'a_idx': -1, 'b_idx': -1, 'c_idx': -1, + }, + KinematicsType.ROTATE: { + 'num_joints': 9, + 'joint_names': ['X', 'Y', 'Z', 'A', 'B', 'C', 'U', 'V', 'W'], + 'x_idx': 0, 'y_idx': 1, 'z_idx': 2, + 'a_idx': 3, 'b_idx': 4, 'c_idx': 5, + }, + KinematicsType.COREXY: { + 'num_joints': 9, + 'joint_names': ['X', 'Y', 'Z', 'A', 'B', 'C', 'U', 'V', 'W'], + 'x_idx': 0, 'y_idx': 1, 'z_idx': 2, + 'a_idx': 3, 'b_idx': 4, 'c_idx': 5, + }, + KinematicsType.ROSE: { + 'num_joints': 3, + 'joint_names': ['R', 'Z', 'Theta'], + 'x_idx': -1, 'y_idx': -1, 'z_idx': 1, + 'a_idx': -1, 'b_idx': -1, 'c_idx': 2, + }, +} + + +class MoveType(IntEnum): + """运动类型枚举""" + RAPID = 0 + FEED = 1 + ARC_CW = 2 + ARC_CCW = 3 + DWELL = 4 + PROBE = 5 + RTCP_ON = 6 + RTCP_OFF = 7 + PROGRAM_END = 8 + TOOL_CHANGE = 9 + SPINDLE_ON = 10 + SPINDLE_OFF = 11 + COOLANT_ON = 12 + COOLANT_OFF = 13 + COMMENT = 14 + MESSAGE = 15 + USER_DEFINED = 16 + WAIT = 17 + + +class CompType(IntEnum): + """刀具补偿类型""" + OFF = 0 + LEFT = 1 + RIGHT = 2 + DYNAMIC = 3 + + +class MachineState(Enum): + """机床状态枚举""" + OFF = "OFF" + RESET = "RESET" + ESTOP = "ESTOP" + IDLE = "IDLE" + RUNNING = "RUNNING" + PAUSED = "PAUSED" + HOLDING = "HOLDING" + JOGGING = "JOGGING" + HOMING = "HOMING" + ALARM = "ALARM" + MDI = "MDI" + PROBING = "PROBING" + TOOL_CHANGING = "TOOL_CHANGING" + + +class SpindleState(Enum): + """主轴状态""" + OFF = "OFF" + CW = "CW" + CCW = "CCW" + + +class CoolantState(Enum): + """冷却液状态""" + OFF = "OFF" + FLOOD = "FLOOD" + MIST = "MIST" + BOTH = "BOTH" + + +class FeedMode(Enum): + """进给模式""" + UNITS_PER_MINUTE = 94 + UNITS_PER_REVOLUTION = 95 + INVERSE_TIME = 93 + + +class DistanceMode(Enum): + """距离模式""" + ABSOLUTE = 90 + INCREMENTAL = 91 + + +class PlaneMode(Enum): + """平面模式""" + XY = 17 + XZ = 18 + YZ = 19 + UV = 171 + UW = 181 + VW = 191 + + +class UnitsMode(Enum): + """单位模式""" + INCHES = 20 + MM = 21 + + +@dataclass +class Point6D: + """六维点 (X, Y, Z, A, B, C)""" + x: float = 0.0 + y: float = 0.0 + z: float = 0.0 + a: float = 0.0 + b: float = 0.0 + c: float = 0.0 + u: float = 0.0 + v: float = 0.0 + w: float = 0.0 + + def to_list(self) -> List[float]: + return [self.x, self.y, self.z, self.a, self.b, self.c, self.u, self.v, self.w] + + def to_tuple(self) -> Tuple[float, ...]: + return (self.x, self.y, self.z, self.a, self.b, self.c, self.u, self.v, self.w) + + def to_xyz_tuple(self) -> Tuple[float, float, float]: + return (self.x, self.y, self.z) + + def to_xyzabc_tuple(self) -> Tuple[float, float, float, float, float, float]: + return (self.x, self.y, self.z, self.a, self.b, self.c) + + def distance_to(self, other: 'Point6D') -> float: + dx = self.x - other.x + dy = self.y - other.y + dz = self.z - other.z + return math.sqrt(dx*dx + dy*dy + dz*dz) + + def distance_to_6d(self, other: 'Point6D') -> float: + """计算六维距离（包含旋转轴）""" + dx = self.x - other.x + dy = self.y - other.y + dz = self.z - other.z + da = self.a - other.a + db = self.b - other.b + dc = self.c - other.c + return math.sqrt(dx*dx + dy*dy + dz*dz + da*da + db*db + dc*dc) + + def copy(self) -> 'Point6D': + return Point6D(self.x, self.y, self.z, self.a, self.b, self.c, self.u, self.v, self.w) + + def to_dict(self) -> Dict[str, float]: + return {'x': self.x, 'y': self.y, 'z': self.z, + 'a': self.a, 'b': self.b, 'c': self.c, + 'u': self.u, 'v': self.v, 'w': self.w} + + @classmethod + def from_list(cls, lst: List[float]) -> 'Point6D': + if len(lst) >= 9: + return cls(lst[0], lst[1], lst[2], lst[3], lst[4], lst[5], lst[6], lst[7], lst[8]) + elif len(lst) >= 6: + return cls(lst[0], lst[1], lst[2], lst[3], lst[4], lst[5], 0, 0, 0) + elif len(lst) >= 3: + return cls(lst[0], lst[1], lst[2], 0, 0, 0, 0, 0, 0) + return cls() + + @classmethod + def from_xyzabc(cls, x: float, y: float, z: float, a: float, b: float, c: float) -> 'Point6D': + return cls(x, y, z, a, b, c, 0, 0, 0) + + def __add__(self, other: 'Point6D') -> 'Point6D': + return Point6D( + self.x + other.x, self.y + other.y, self.z + other.z, + self.a + other.a, self.b + other.b, self.c + other.c, + self.u + other.u, self.v + other.v, self.w + other.w + ) + + def __sub__(self, other: 'Point6D') -> 'Point6D': + return Point6D( + self.x - other.x, self.y - other.y, self.z - other.z, + self.a - other.a, self.b - other.b, self.c - other.c, + self.u - other.u, self.v - other.v, self.w - other.w + ) + + def __mul__(self, scalar: float) -> 'Point6D': + return Point6D( + self.x * scalar, self.y * scalar, self.z * scalar, + self.a * scalar, self.b * scalar, self.c * scalar, + self.u * scalar, self.v * scalar, self.w * scalar + ) + + def __truediv__(self, scalar: float) -> 'Point6D': + if scalar == 0: + return self.copy() + return Point6D( + self.x / scalar, self.y / scalar, self.z / scalar, + self.a / scalar, self.b / scalar, self.c / scalar, + self.u / scalar, self.v / scalar, self.w / scalar + ) + + def __eq__(self, other: 'Point6D') -> bool: + if not isinstance(other, Point6D): + return False + return (abs(self.x - other.x) < TOLERANCE_EQUAL and + abs(self.y - other.y) < TOLERANCE_EQUAL and + abs(self.z - other.z) < TOLERANCE_EQUAL and + abs(self.a - other.a) < TOLERANCE_EQUAL and + abs(self.b - other.b) < TOLERANCE_EQUAL and + abs(self.c - other.c) < TOLERANCE_EQUAL) + + def __str__(self) -> str: + return f"Point6D(x={self.x:.3f}, y={self.y:.3f}, z={self.z:.3f}, a={self.a:.3f}, b={self.b:.3f}, c={self.c:.3f})" + + def __repr__(self) -> str: + return self.__str__() + + +@dataclass +class ToolData: + """刀具数据 - 完整版""" + tool_number: int = 0 + pocket: int = 0 + diameter: float = 0.0 + radius: float = 0.0 + length_offset: float = 0.0 + offset: Point6D = field(default_factory=Point6D) + front_angle: float = 0.0 + back_angle: float = 0.0 + orientation: int = 0 + name: str = "" + comment: str = "" + + def __post_init__(self): + if self.diameter > 0 and self.radius == 0: + self.radius = self.diameter / 2.0 + elif self.radius > 0 and self.diameter == 0: + self.diameter = self.radius * 2.0 + + +@dataclass +class MoveSegment: + """运动段数据""" + type: MoveType + start: Point6D + end: Point6D + line_number: int = 0 + feedrate: float = 0.0 + center_x: float = 0.0 + center_y: float = 0.0 + center_z: float = 0.0 + radius: float = 0.0 + turn: int = 0 + dwell_time: float = 0.0 + comp_type: CompType = CompType.OFF + comp_radius: float = 0.0 + d_word: float = 0.0 + duration: float = 0.0 + max_velocity: float = 0.0 + acceleration_time: float = 0.0 + constant_time: float = 0.0 + deceleration_time: float = 0.0 + profile_type: str = "" + is_rtcp: bool = False + world_start: Optional[Point6D] = None + world_end: Optional[Point6D] = None + gcode: str = "" + machine_start: Optional[Point6D] = None # 绝对坐标起点 + machine_end: Optional[Point6D] = None # 绝对坐标终点 + tool_number: int = 0 + spindle_speed: float = 0.0 + spindle_state: str = "" + coolant_state: str = "" + comment: str = "" + message: str = "" + probe_tripped: bool = False + + # ===== 新增字段 ===== + coord_system: str = "WORLD" # "WORLD" | "ABSOLUTE" | "MACHINE" + active_csys: int = 1 # 当前激活的坐标系 (1=G54, 2=G55, ...) + g92_active: bool = False # G92 是否激活 + + def __post_init__(self): + """初始化后处理：确保坐标完整性""" + # 如果 world_start 为空，使用 start 作为默认值 + if self.world_start is None: + self.world_start = self.start.copy() if self.start else None + + # 如果 world_end 为空，使用 end 作为默认值 + if self.world_end is None: + self.world_end = self.end.copy() if self.end else None + + # 确保圆弧段有正确的圆心信息 + if self.type in [MoveType.ARC_CW, MoveType.ARC_CCW]: + if self.radius == 0 and self.world_start and self.center_x: + self.radius = math.hypot( + self.world_start.x - self.center_x, + self.world_start.y - self.center_y + ) + + def get_color(self, colors: Dict[str, str] = None) -> str: + """获取运动段的显示颜色""" + if colors is None: + colors = DEFAULT_COLORS + + if self.type == MoveType.RAPID: + return colors.get('rapid', '#00ff00') + elif self.type == MoveType.FEED: + return colors.get('feed', '#ffff00') + elif self.type == MoveType.ARC_CW: + return colors.get('arc_cw', '#00ffff') + elif self.type == MoveType.ARC_CCW: + return colors.get('arc_ccw', '#ff00ff') + elif self.type == MoveType.DWELL: + return colors.get('dwell', '#ffffff') + elif self.type == MoveType.PROBE: + return colors.get('probe', '#ff8000') + elif self.type == MoveType.RTCP_ON: + return colors.get('rtcp_on', '#00ff88') + elif self.type == MoveType.RTCP_OFF: + return colors.get('rtcp_off', '#ff6600') + elif self.type == MoveType.PROGRAM_END: + return colors.get('program_end', '#ff0000') + else: + return '#ffffff' + + + + # ===== 新增：坐标系兼容属性 ===== + + @property + def effective_start(self) -> Point6D: + """获取有效的起点（始终返回工件坐标）""" + if self.world_start is not None: + return self.world_start + return self.start + + @property + def effective_end(self) -> Point6D: + """获取有效的终点（始终返回工件坐标）""" + if self.world_end is not None: + return self.world_end + return self.end + + def get_start_in_world(self) -> Point6D: + """获取工件坐标系下的起点（兼容方法）""" + if self.world_start is not None: + return self.world_start + # 向后兼容：如果没有 world 坐标，假定 start 就是工件坐标 + return self.start + + def get_end_in_world(self) -> Point6D: + """获取工件坐标系下的终点（兼容方法）""" + if self.world_end is not None: + return self.world_end + return self.end + + def get_start_in_machine(self) -> Point6D: + """获取绝对坐标系下的起点""" + if self.machine_start is not None: + return self.machine_start + return self.start + + def get_end_in_machine(self) -> Point6D: + """获取绝对坐标系下的终点""" + if self.machine_end is not None: + return self.machine_end + return self.end + + def has_world_coords(self) -> bool: + """是否有工件坐标""" + return self.world_start is not None and self.world_end is not None + + def has_machine_coords(self) -> bool: + """是否有绝对坐标""" + return self.machine_start is not None and self.machine_end is not None + + def get_arc_info_in_world(self) -> Dict[str, float]: + """ + 获取工件坐标系下的圆弧信息 + 用于几何验证和可视化 + """ + start_pt = self.get_start_in_world() + end_pt = self.get_end_in_world() + return { + 'start_x': start_pt.x, + 'start_y': start_pt.y, + 'start_z': start_pt.z, + 'end_x': end_pt.x, + 'end_y': end_pt.y, + 'end_z': end_pt.z, + 'center_x': self.center_x, + 'center_y': self.center_y, + 'center_z': self.center_z, + 'radius': self.radius, + 'turn': self.turn, + } + + + + + + +@dataclass +class Subroutine: + """子程序定义""" + name: str + lines: List[str] = field(default_factory=list) + line_numbers: List[int] = field(default_factory=list) + start_line: int = 0 + parameters: List[str] = field(default_factory=list) + description: str = "" + source_file: str = "" + + +@dataclass +class ToolpathData: + """刀具路径数据""" + segments: List[MoveSegment] = field(default_factory=list) + bounds: Dict[str, float] = field(default_factory=dict) + total_length: float = 0.0 + total_time: float = 0.0 + point_count: int = 0 + generation_time_ms: float = 0.0 + filename: str = "" + tool_changes: List[Dict] = field(default_factory=list) + tool_table: Dict[int, ToolData] = field(default_factory=dict) + subroutines: Dict[str, Subroutine] = field(default_factory=dict) + variables: Dict[int, float] = field(default_factory=dict) + named_variables: Dict[str, float] = field(default_factory=dict) + rtcp_enabled: bool = False + kinematics_type: str = "" + program_comments: List[str] = field(default_factory=list) + program_messages: List[str] = field(default_factory=list) + errors: List[str] = field(default_factory=list) + warnings: List[str] = field(default_factory=list) + + def calculate_bounds(self): + if not self.segments: + return + + min_x = min_y = min_z = min_a = min_b = min_c = float('inf') + max_x = max_y = max_z = max_a = max_b = max_c = float('-inf') + + for seg in self.segments: + for point in [seg.start, seg.end]: + min_x = min(min_x, point.x) + max_x = max(max_x, point.x) + min_y = min(min_y, point.y) + max_y = max(max_y, point.y) + min_z = min(min_z, point.z) + max_z = max(max_z, point.z) + min_a = min(min_a, point.a) + max_a = max(max_a, point.a) + min_b = min(min_b, point.b) + max_b = max(max_b, point.b) + min_c = min(min_c, point.c) + max_c = max(max_c, point.c) + + self.bounds = { + 'minX': min_x if min_x != float('inf') else 0, + 'maxX': max_x if max_x != float('-inf') else 0, + 'minY': min_y if min_y != float('inf') else 0, + 'maxY': max_y if max_y != float('-inf') else 0, + 'minZ': min_z if min_z != float('inf') else 0, + 'maxZ': max_z if max_z != float('-inf') else 0, + 'minA': min_a if min_a != float('inf') else 0, + 'maxA': max_a if max_a != float('-inf') else 0, + 'minB': min_b if min_b != float('inf') else 0, + 'maxB': max_b if max_b != float('-inf') else 0, + 'minC': min_c if min_c != float('inf') else 0, + 'maxC': max_c if max_c != float('-inf') else 0, + 'width': (max_x - min_x) if max_x != float('-inf') else 0, + 'height': (max_y - min_y) if max_y != float('-inf') else 0, + 'depth': (max_z - min_z) if max_z != float('-inf') else 0, + } + + def calculate_length_and_time(self) -> Tuple[float, float]: + total = 0.0 + total_time = 0.0 + for seg in self.segments: + if seg.type in [MoveType.RAPID, MoveType.FEED]: + total += seg.start.distance_to(seg.end) + elif seg.type in [MoveType.ARC_CW, MoveType.ARC_CCW]: + total += abs(seg.turn) * seg.radius * math.pi + total_time += seg.duration + self.total_length = total + self.total_time = total_time + return total, total_time + + def get_statistics(self) -> Dict[str, Any]: + return { + 'total_segments': len(self.segments), + 'rapid_moves': sum(1 for s in self.segments if s.type == MoveType.RAPID), + 'feed_moves': sum(1 for s in self.segments if s.type == MoveType.FEED), + 'arc_moves': sum(1 for s in self.segments if s.type in [MoveType.ARC_CW, MoveType.ARC_CCW]), + 'dwells': sum(1 for s in self.segments if s.type == MoveType.DWELL), + 'probes': sum(1 for s in self.segments if s.type == MoveType.PROBE), + 'rtcp_on_count': sum(1 for s in self.segments if s.type == MoveType.RTCP_ON), + 'rtcp_off_count': sum(1 for s in self.segments if s.type == MoveType.RTCP_OFF), + 'compensated_segments': sum(1 for s in self.segments if s.comp_type != CompType.OFF), + 'tool_changes': len(self.tool_changes), + 'subroutines': len(self.subroutines), + 'variables': len(self.variables), + 'named_variables': len(self.named_variables), + 'rtcp_enabled': self.rtcp_enabled, + 'kinematics_type': self.kinematics_type, + 'errors': len(self.errors), + 'warnings': len(self.warnings), + } + + def get_segments_by_type(self, move_type: MoveType) -> List[MoveSegment]: + """获取指定类型的运动段""" + return [s for s in self.segments if s.type == move_type] + + def get_segments_by_tool(self, tool_number: int) -> List[MoveSegment]: + """获取指定刀具的运动段""" + return [s for s in self.segments if s.tool_number == tool_number] + + def get_segments_by_line(self, line_number: int) -> List[MoveSegment]: + """获取指定行号的运动段""" + return [s for s in self.segments if s.line_number == line_number] + + def add_error(self, error: str): + """添加错误""" + self.errors.append(error) + + def add_warning(self, warning: str): + """添加警告""" + self.warnings.append(warning) + + def add_comment(self, comment: str): + """添加注释""" + self.program_comments.append(comment) + + def add_message(self, message: str): + """添加消息""" + self.program_messages.append(message) + + + +# ==================== 关节坐标映射工具 ==================== + +def joints_to_point6d(joints: List[float], kinematics_type: KinematicsType) -> Point6D: + """ + 根据运动学类型的关节配置，将关节数组转换为 Point6D + + 参数: + joints: 关节坐标数组 + kinematics_type: 运动学类型 + + 返回: + Point6D，其中 x/y/z/a/b/c 根据运动学配置正确映射 + """ + config = KINEMATICS_JOINT_CONFIG.get(kinematics_type, + KINEMATICS_JOINT_CONFIG[KinematicsType.IDENTITY]) + + def safe_get(idx: int, default: float = 0.0) -> float: + if idx >= 0 and idx < len(joints): + return joints[idx] + return default + + return Point6D( + safe_get(config.get('x_idx', 0), 0.0), + safe_get(config.get('y_idx', 1), 0.0), + safe_get(config.get('z_idx', 2), 0.0), + safe_get(config.get('a_idx', 3), 0.0), + safe_get(config.get('b_idx', 4), 0.0), + safe_get(config.get('c_idx', 5), 0.0), + ) + + +def point6d_to_joints(pos: Point6D, kinematics_type: KinematicsType) -> List[float]: + """ + 根据运动学类型的关节配置，将 Point6D 转换为关节数组 + + 参数: + pos: Point6D 坐标 + kinematics_type: 运动学类型 + + 返回: + 关节坐标数组 + """ + config = KINEMATICS_JOINT_CONFIG.get(kinematics_type, + KINEMATICS_JOINT_CONFIG[KinematicsType.IDENTITY]) + num_joints = config.get('num_joints', 6) + joints = [0.0] * num_joints + + # 根据配置将 Point6D 的轴值放到正确位置 + axes_map = { + 'x': ('x_idx', pos.x), + 'y': ('y_idx', pos.y), + 'z': ('z_idx', pos.z), + 'a': ('a_idx', pos.a), + 'b': ('b_idx', pos.b), + 'c': ('c_idx', pos.c), + } + + for key_name, (config_key, value) in axes_map.items(): + idx = config.get(config_key, -1) + if idx >= 0 and idx < num_joints: + joints[idx] = value + + return joints + + + +# ==================== TRT 双转台运动学参数 ==================== + +@dataclass +class TRTParams(KinematicsParams): + """双转台/摆头式五轴参数 (对应 trtfuncs.c 的 haldata)""" + rot_center_x: float = 0.0 + rot_center_y: float = 0.0 + rot_center_z: float = 0.0 + axis_offset_x: float = 0.0 + axis_offset_y: float = 0.0 + axis_offset_z: float = 0.0 + tool_length: float = 0.0 + conventional_directions: bool = False + pivot_length: float = 250.0 + + +# ==================== XYZAC_TRTKinematics 类 ==================== + +class XYZAC_TRTKinematics(BaseKinematics): + """ + XYZAC 双转台运动学 - 完全参照 LinuxCNC trtfuncs.c + A 轴绕 X 轴倾斜，C 轴绕 Z 轴旋转。 + + 参数映射 (LinuxCNC -> JSON): + rot_center_x/y/z = 旋转台回转中心在机床坐标系中的位置 + axis_offset_x/y/z = 直线轴相对于旋转台的安装偏置 + tool_length = 刀具长度补偿值 + """ + + def __init__(self, params: TRTParams = None, debug: bool = False): + super().__init__(params or TRTParams()) + self.debug = debug + self.p = self.params + + # 同步刀具长度 + if hasattr(self.p, 'tool_length') and self.p.tool_length > 0: + self._tool_length = self.p.tool_length + + def _get_con(self) -> float: + """获取旋转方向因子""" + return 1.0 if self.p.conventional_directions else -1.0 + + def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + """ + 正运动学：关节坐标 -> 世界坐标 + + 参照 LinuxCNC xyzacKinematicsForward()： + 1. 从关节坐标中减去旋转中心坐标，把坐标系原点移到旋转中心 + 2. 再减去轴安装偏置（axis_offset） + 3. 乘上旋转矩阵（A 轴绕 X，C 轴绕 Z） + 4. 把旋转中心加回来，还原为机床坐标系下的刀尖位置 + + 关节顺序: [X, Y, Z, A, C] + 返回: (X, Y, Z, A, B, C) 刀尖点世界坐标 + """ + if len(joints) < 5: + joints = list(joints) + [0.0] * (5 - len(joints)) + + x = joints[0] # 关节 X + y = joints[1] # 关节 Y + z = joints[2] # 关节 Z（主轴参考点） + a = joints[3] # A 轴角度（度） + c = joints[4] # C 轴角度（度） + + con = self._get_con() + a_rad = a * TO_RAD + c_rad = c * TO_RAD + + # 旋转中心 - 对应 LinuxCNC 的 x_rot_point, y_rot_point, z_rot_point + rx = self.p.rot_center_x + ry = self.p.rot_center_y + rz = self.p.rot_center_z + + # 轴偏置 - 对应 LinuxCNC 的 x_offset, y_offset, z_offset + dx = self.p.axis_offset_x + dy = self.p.axis_offset_y + dz = self.p.axis_offset_z + self._tool_length # z_offset + tool_offset + + # ===== 正解计算（完全参照 LinuxCNC 官方代码）===== + + # 注意：dx (axis_offset_x) 在 AC 结构中用于 X 轴方向 + # 这里的关键是：从关节坐标中减去旋转中心和轴偏置， + # 然后进行旋转变换，最后再加回旋转中心 + + pos_x = ( + + math.cos(c_rad) * (x - dx - rx) + - con * math.sin(c_rad) * math.cos(a_rad) * (y - dy - ry) + + math.sin(c_rad) * math.sin(a_rad) * (z - dz - rz) + - con * math.sin(c_rad) * dy + + rx + ) + + pos_y = ( + + con * math.sin(c_rad) * (x - dx - rx) + + math.cos(c_rad) * math.cos(a_rad) * (y - dy - ry) + - con * math.cos(c_rad) * math.sin(a_rad) * (z - dz - rz) + + math.cos(c_rad) * dy + + ry + ) + + pos_z = ( + + con * math.sin(a_rad) * (y - dy - ry) + + math.cos(a_rad) * (z - dz - rz) + + dz + + rz + ) + + if self.debug: + print(f"[XYZAC FWD] joints(x={x:.3f},y={y:.3f},z={z:.3f},a={a:.3f},c={c:.3f}) " + f"-> world({pos_x:.3f},{pos_y:.3f},{pos_z:.3f})") + print(f"[XYZAC FWD] rot_center=({rx:.1f},{ry:.1f},{rz:.1f}) " + f"axis_offset=({dx:.1f},{dy:.1f},{dz:.1f})") + + return (pos_x, pos_y, pos_z, a, 0.0, c) + + def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]: + """ + 逆运动学：世界坐标 -> 关节坐标 + + 参照 LinuxCNC xyzacKinematicsInverse()： + 1. 把世界坐标减去旋转中心，把坐标系移到旋转中心 + 2. 乘上逆旋转矩阵 + 3. 加上轴偏置和旋转中心，还原成关节坐标 + + 输入: (X, Y, Z, A, B, C) 世界坐标 + 返回: [X, Y, Z, A, C] 关节坐标 + """ + if len(world) < 6: + world = list(world) + [0.0] * (6 - len(world)) + + wx = world[0] # 世界 X + wy = world[1] # 世界 Y + wz = world[2] # 世界 Z + wa = world[3] # A 轴角度 + wb = world[4] # B 轴（不使用） + wc = world[5] # C 轴角度 + + con = self._get_con() + a_rad = wa * TO_RAD + c_rad = wc * TO_RAD + + # 旋转中心 + rx = self.p.rot_center_x + ry = self.p.rot_center_y + rz = self.p.rot_center_z + + # 轴偏置 + dx = self.p.axis_offset_x + dy = self.p.axis_offset_y + dz = self.p.axis_offset_z + self._tool_length + + # ===== 逆解计算（完全参照 LinuxCNC 官方代码）===== + + # 第一步：先把世界坐标减去旋转中心，计算 P 向量 + Px = ( + + math.cos(c_rad) * (wx - rx) + + con * math.sin(c_rad) * (wy - ry) + + rx + ) + + Py = ( + - con * math.sin(c_rad) * math.cos(a_rad) * (wx - rx) + + math.cos(c_rad) * math.cos(a_rad) * (wy - ry) + + con * math.sin(a_rad) * (wz - rz) + - math.cos(a_rad) * dy + - con * math.sin(a_rad) * dz + + dy + + ry + ) + + Pz = ( + + math.sin(c_rad) * math.sin(a_rad) * (wx - rx) + - con * math.cos(c_rad) * math.sin(a_rad) * (wy - ry) + + math.cos(a_rad) * (wz - rz) + + con * math.sin(a_rad) * dy + - math.cos(a_rad) * dz + + dz + + rz + ) + + if self.debug: + print(f"[XYZAC INV] world({wx:.3f},{wy:.3f},{wz:.3f},a={wa:.1f},c={wc:.1f}) " + f"-> joints({Px:.3f},{Py:.3f},{Pz:.3f})") + print(f"[XYZAC INV] rot_center=({rx:.1f},{ry:.1f},{rz:.1f}) " + f"axis_offset=({dx:.1f},{dy:.1f},{dz:.1f})") + + return [Px, Py, Pz, wa, wc] + + def set_tool_length(self, length: float): + """设置刀具长度 - 确保参数同步""" + self._tool_length = length + if hasattr(self, 'p') and self.p is not None: + self.p.tool_length = length + if self.debug: + print(f"[XYZAC TLO] set_tool_length({length}) -> " + f"_tool_length={self._tool_length}, " + f"p.tool_length={self.p.tool_length if hasattr(self.p, 'tool_length') else 'N/A'}") + + +# ==================== XYZBC_TRTKinematics 类 ==================== + +class XYZBC_TRTKinematics(BaseKinematics): + """ + XYZBC 双转台运动学 (基于 trtfuncs.c 的 xyzbcKinematicsForward/Inverse) + B 轴绕 Y 轴倾斜，C 轴绕 Z 轴旋转。 + """ + + def __init__(self, params: TRTParams = None, debug: bool = False): + super().__init__(params or TRTParams()) + self.debug = debug + self.p = self.params + # ★ 同样添加刀具长度同步 ★ + if hasattr(self.p, 'tool_length') and self.p.tool_length > 0: + self._tool_length = self.p.tool_length + + def _get_con(self) -> float: + """获取方向因子""" + return 1.0 if self.p.conventional_directions else -1.0 + + def set_params(self, **kwargs): + """更新运动学参数""" + supported = { + 'rot_center_x', 'rot_center_y', 'rot_center_z', + 'axis_offset_x', 'axis_offset_y', 'axis_offset_z', + 'tool_length', 'conventional_directions', 'pivot_length' + } + for key, value in kwargs.items(): + if key in supported and hasattr(self.p, key): + setattr(self.p, key, value) + + if self.debug: + print(f"[XYZBC_TRT] 参数更新: {kwargs}") + + def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + """ + 正运动学：关节坐标 -> 世界坐标 (刀尖点) + 关节顺序: [X, Y, Z, B, C] + 返回: (X, Y, Z, A, B, C) + """ + if len(joints) < 5: + joints = list(joints) + [0.0] * (5 - len(joints)) + + x, y, z, b, c = joints[0], joints[1], joints[2], joints[3], joints[4] + + con = self._get_con() + b_rad = b * TO_RAD + c_rad = c * TO_RAD + + rx = self.p.rot_center_x + ry = self.p.rot_center_y + rz = self.p.rot_center_z + + dx = self.p.axis_offset_x + dz = self.p.axis_offset_z + self._tool_length + + pos_x = ( + + math.cos(c_rad) * math.cos(b_rad) * (x - dx - rx) + - con * math.sin(c_rad) * (y - ry) + + con * math.cos(c_rad) * math.sin(b_rad) * (z - dz - rz) + + math.cos(c_rad) * dx + + rx + ) + + pos_y = ( + + con * math.sin(c_rad) * math.cos(b_rad) * (x - dx - rx) + + math.cos(c_rad) * (y - ry) + + math.sin(c_rad) * math.sin(b_rad) * (z - dz - rz) + + con * math.sin(c_rad) * dx + + ry + ) + + pos_z = ( + - con * math.sin(b_rad) * (x - dx - rx) + + math.cos(b_rad) * (z - dz - rz) + + dz + + rz + ) + + if self.debug: + print(f"[XYZBC] FWD: joints({x:.3f},{y:.3f},{z:.3f},{b:.3f},{c:.3f}) -> world({pos_x:.3f},{pos_y:.3f},{pos_z:.3f})") + + return (pos_x, pos_y, pos_z, 0.0, b, c) + + def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]: + """ + 逆运动学：世界坐标 (刀尖点) -> 关节坐标 + 输入: (X, Y, Z, A, B, C) + 返回: [X, Y, Z, B, C] + """ + if len(world) < 6: + world = list(world) + [0.0] * (6 - len(world)) + + wx, wy, wz, wa, wb, wc = world[0], world[1], world[2], world[3], world[4], world[5] + + con = self._get_con() + b_rad = wb * TO_RAD + c_rad = wc * TO_RAD + + rx = self.p.rot_center_x + ry = self.p.rot_center_y + rz = self.p.rot_center_z + + dx = self.p.axis_offset_x + dz = self.p.axis_offset_z + self._tool_length + + dpx = -math.cos(b_rad) * dx + math.sin(b_rad) * dz + dx + dpz = -math.sin(b_rad) * dx - math.cos(b_rad) * dz + dz + + Px = ( + + math.cos(c_rad) * math.cos(b_rad) * (wx - rx) + + con * math.sin(c_rad) * math.cos(b_rad) * (wy - ry) + - con * math.sin(b_rad) * (wz - rz) + + dpx + + rx + ) + + Py = ( + - con * math.sin(c_rad) * (wx - rx) + + math.cos(c_rad) * (wy - ry) + + ry + ) + + Pz = ( + + con * math.cos(c_rad) * math.sin(b_rad) * (wx - rx) + + math.sin(c_rad) * math.sin(b_rad) * (wy - ry) + + math.cos(b_rad) * (wz - rz) + + dpz + + rz + ) + + if self.debug: + print(f"[XYZBC] INV: world({wx:.3f},{wy:.3f},{wz:.3f},{wb:.3f},{wc:.3f}) -> joints({Px:.3f},{Py:.3f},{Pz:.3f})") + + return [Px, Py, Pz, wb, wc] + + def set_tool_length(self, length: float): + self._tool_length = length + self.p.tool_length = length + if self.debug: + print(f"[XYZBC] 刀具长度设置为: {length:.3f} mm") + + def set_pivot_length(self, length: float): + """设置枢轴长度（为兼容性保留）""" + self.p.pivot_length = length + if self.debug: + print(f"[XYZBC] 枢轴长度设置为: {length:.3f} mm") + + +# ==================== 五轴桥式铣床运动学 (5axiskins) ==================== + +class FiveAxisBCKinematics(BaseKinematics): + """ + 五轴桥式铣床运动学 (基于 5axiskins.c) + 采用球坐标描述刀具指向。 + """ + + def __init__(self, pivot_length: float = 250.0, tool_length: float = 0.0, debug: bool = False): + super().__init__() + self.pivot_length = pivot_length + self._tool_length = tool_length + self.debug = debug + + def _s2r(self, r: float, t: float, p: float) -> Tuple[float, float, float]: + """ + 球坐标转直角坐标 (来自 5axiskins.c 的 s2r 函数) + r = 向量长度 + p = phi = 向量与 Z 轴夹角 (度) + t = theta = 向量在 XY 平面投影与 X 轴夹角 (度) + """ + t_rad = t * TO_RAD + p_rad = p * TO_RAD + x = r * math.sin(p_rad) * math.cos(t_rad) + y = r * math.sin(p_rad) * math.sin(t_rad) + z = r * math.cos(p_rad) + return (x, y, z) + + def _r2s(self, x: float, y: float, z: float) -> Tuple[float, float, float]: + """直角坐标转球坐标""" + r = math.sqrt(x*x + y*y + z*z) + if r < CART_FUZZ: + return (0.0, 0.0, 0.0) + p = math.acos(z / r) * TO_DEG + t = math.atan2(y, x) * TO_DEG + return (r, t, p) + + def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + if len(joints) < 6: + joints = list(joints) + [0.0] * (6 - len(joints)) + + x, y, z, b, c, w = joints[0], joints[1], joints[2], joints[3], joints[4], joints[5] + + rx, ry, rz = self._s2r(self.pivot_length + w, c, 180.0 - b) + + pos_x = x + rx + pos_y = y + ry + pos_z = z + self.pivot_length + rz + + if self.debug: + print(f"[5AXISBC] FWD: joints({x:.3f},{y:.3f},{z:.3f},{b:.3f},{c:.3f},{w:.3f}) -> world({pos_x:.3f},{pos_y:.3f},{pos_z:.3f})") + + return (pos_x, pos_y, pos_z, 0.0, b, c) + + def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]: + if len(world) < 6: + world = list(world) + [0.0] * (6 - len(world)) + + wx, wy, wz, _, wb, wc = world[0], world[1], world[2], world[3], world[4], world[5] + + rx, ry, rz = self._s2r(self.pivot_length, wc, 180.0 - wb) + + Px = wx - rx + Py = wy - ry + Pz = wz - self.pivot_length - rz + + if self.debug: + print(f"[5AXISBC] INV: world({wx:.3f},{wy:.3f},{wz:.3f},{wb:.3f},{wc:.3f}) -> joints({Px:.3f},{Py:.3f},{Pz:.3f})") + + return [Px, Py, Pz, wb, wc, 0.0] + + def set_tool_length(self, length: float): + self._tool_length = length + if self.debug: + print(f"[5AXISBC] 刀具长度设置为: {length:.3f} mm") + + +# ==================== Maxkins BC 运动学 ==================== + +class MaxkinsBCKinematics(BaseKinematics): + """ + Max 五轴铣床运动学 (基于 maxkins.c) + 摆头 (B轴) + 转台 (C轴) 结构 + """ + + def __init__(self, pivot_length: float = 0.666, tool_length: float = 0.0, + conventional_directions: bool = False, debug: bool = False): + super().__init__() + self.pivot_length = pivot_length + self._tool_length = tool_length + self.conventional_directions = conventional_directions + self.debug = debug + + def _get_con(self) -> float: + return 1.0 if self.conventional_directions else -1.0 + + def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + if len(joints) < 9: + joints = list(joints) + [0.0] * (9 - len(joints)) + + x, y, z, a, b, c, u, v, w = joints[0], joints[1], joints[2], joints[3], joints[4], joints[5], joints[6], joints[7], joints[8] + + con = self._get_con() + b_rad = b * TO_RAD + c_rad = c * TO_RAD + + zb = (self.pivot_length + w) * math.cos(b_rad) + xb = (self.pivot_length + w) * math.sin(b_rad) + + xyr = math.hypot(x, y) + xytheta = math.atan2(y, x) + c_rad + + zv = u * math.sin(b_rad) + xv = u * math.cos(b_rad) + + pos_x = xyr * math.cos(xytheta) - con * xb - xv + pos_y = xyr * math.sin(xytheta) - v + pos_z = z - zb - con * zv + self.pivot_length + + if self.debug: + print(f"[MAXKINS] FWD: joints -> world({pos_x:.3f},{pos_y:.3f},{pos_z:.3f})") + + return (pos_x, pos_y, pos_z, a, b, c) + + def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]: + if len(world) < 9: + world = list(world) + [0.0] * (9 - len(world)) + + wx, wy, wz, wa, wb, wc, wu, wv, ww = world[0], world[1], world[2], world[3], world[4], world[5], world[6], world[7], world[8] + + con = self._get_con() + b_rad = wb * TO_RAD + c_rad = wc * TO_RAD + + zb = (self.pivot_length + ww) * math.cos(b_rad) + xb = (self.pivot_length + ww) * math.sin(b_rad) + + xyr = math.hypot(wx, wy) + xytheta = math.atan2(wy, wx) - c_rad + + xv = wu * math.cos(b_rad) + zv = wu * math.sin(b_rad) + + Px = xyr * math.cos(xytheta) + con * xb + xv + Py = xyr * math.sin(xytheta) + wv + Pz = wz + zb - con * zv - self.pivot_length + + if self.debug: + print(f"[MAXKINS] INV: world -> joints({Px:.3f},{Py:.3f},{Pz:.3f})") + + return [Px, Py, Pz, wa, wb, wc, wu, wv, ww] + + def set_tool_length(self, length: float): + self._tool_length = length + if self.debug: + print(f"[MAXKINS] 刀具长度设置为: {length:.3f} mm") + + +# ==================== Hexapod 运动学参数 ==================== + +@dataclass +class HexapodParams(KinematicsParams): + """Hexapod 参数 (对应 genhexkins.c 的 haldata)""" + base_joints: List[Tuple[float, float, float]] = field(default_factory=lambda: [ + (-22.95, 13.25, 0), (22.95, 13.25, 0), (22.95, 13.25, 0), + (0, -26.5, 0), (0, -26.5, 0), (-22.95, 13.25, 0) + ]) + platform_joints: List[Tuple[float, float, float]] = field(default_factory=lambda: [ + (-1.0, 11.5, 0), (1.0, 11.5, 0), (10.459, -4.884, 0), + (9.459, -6.616, 0), (-9.459, -6.616, 0), (-10.459, -4.884, 0) + ]) + base_joint_axes: List[Tuple[float, float, float]] = field(default_factory=lambda: [ + (0.707107, 0.0, 0.707107), (0.0, -0.707107, 0.707107), (-0.707107, 0.0, 0.707107), + (-0.707107, 0.0, 0.707107), (0.0, 0.707107, 0.707107), (0.707107, 0.0, 0.707107) + ]) + platform_joint_axes: List[Tuple[float, float, float]] = field(default_factory=lambda: [ + (-1.0, 0.0, 0.0), (0.866025, 0.5, 0.0), (0.866025, 0.5, 0.0), + (0.866025, -0.5, 0.0), (0.866025, -0.5, 0.0), (-1.0, 0.0, 0.0) + ]) + spindle_offset: float = 0.0 + tool_offset: float = 0.0 + screw_lead: float = 0.0 + convergence_criterion: float = 1e-9 + max_iterations: int = 120 + max_error: float = 500.0 + + +# ==================== Hexapod 运动学 ==================== + +class HexapodKinematics(BaseKinematics): + """ + 通用 Hexapod (Stewart 平台) 运动学 - 完整版 + 基于 genhexkins.c 完整实现 + """ + + NUM_STRUTS = 6 + + def __init__(self, params: HexapodParams = None, debug: bool = False): + super().__init__(params or HexapodParams()) + self.debug = debug + self.p = self.params + self._update_offsets() + self.last_iterations = 0 + self.max_iterations_used = 0 + self._forward_fail = False + self._conv_err = 0.0 + + def _update_offsets(self): + """更新带有主轴偏移和刀具偏移的坐标""" + offset_z = self.p.spindle_offset + self._tool_length + + self.b = [] + for bx, by, bz in self.p.base_joints: + self.b.append([bx, by, bz + offset_z]) + + self.a = [] + for ax, ay, az in self.p.platform_joints: + self.a.append([ax, ay, az + offset_z]) + + self.nb1 = [list(n) for n in self.p.base_joint_axes] + self.na0 = [list(n) for n in self.p.platform_joint_axes] + + def set_tool_length(self, length: float): + self._tool_length = length + self.p.tool_offset = length + self._update_offsets() + if self.debug: + print(f"[HEXAPOD] 刀具长度设置为: {length:.3f} mm") + + def _rotation_matrix(self, a: float, b: float, c: float) -> List[List[float]]: + """从 RPY 角 (A, B, C) 构建旋转矩阵""" + a_rad, b_rad, c_rad = a * TO_RAD, b * TO_RAD, c * TO_RAD + + ca, sa = math.cos(a_rad), math.sin(a_rad) + cb, sb = math.cos(b_rad), math.sin(b_rad) + cc, sc = math.cos(c_rad), math.sin(c_rad) + + return [ + [cc*cb, cc*sb*sa - sc*ca, cc*sb*ca + sc*sa], + [sc*cb, sc*sb*sa + cc*ca, sc*sb*ca - cc*sa], + [ -sb, cb*sa, cb*ca] + ] + + def _cross_product(self, a: List[float], b: List[float]) -> List[float]: + """三维向量叉积""" + return [ + a[1]*b[2] - a[2]*b[1], + a[2]*b[0] - a[0]*b[2], + a[0]*b[1] - a[1]*b[0] + ] + + def _dot_product(self, a: List[float], b: List[float]) -> float: + """三维向量点积""" + return a[0]*b[0] + a[1]*b[1] + a[2]*b[2] + + def _vector_magnitude(self, v: List[float]) -> float: + """向量长度""" + return math.sqrt(v[0]*v[0] + v[1]*v[1] + v[2]*v[2]) + + def _normalize(self, v: List[float]) -> List[float]: + """向量归一化""" + mag = self._vector_magnitude(v) + if mag < 1e-12: + return [0.0, 0.0, 1.0] + return [v[0]/mag, v[1]/mag, v[2]/mag] + + def _mat_vec_mult(self, M: List[List[float]], v: List[float]) -> List[float]: + """矩阵乘向量""" + return [ + M[0][0]*v[0] + M[0][1]*v[1] + M[0][2]*v[2], + M[1][0]*v[0] + M[1][1]*v[1] + M[1][2]*v[2], + M[2][0]*v[0] + M[2][1]*v[1] + M[2][2]*v[2] + ] + + def _strut_length_correction(self, strut_unit: List[float], R: List[List[float]], + strut_idx: int) -> float: + """支腿长度修正 (对应 StrutLengthCorrection)""" + if self.p.screw_lead == 0.0: + return 0.0 + + nb2 = self._cross_product(self.nb1[strut_idx], strut_unit) + nb3 = self._cross_product(strut_unit, nb2) + nb3 = self._normalize(nb3) + + na1 = self._mat_vec_mult(R, self.na0[strut_idx]) + na2 = self._cross_product(na1, strut_unit) + na2 = self._normalize(na2) + + dotprod = self._dot_product(nb3, na2) + dotprod = max(-1.0, min(1.0, dotprod)) + + return self.p.screw_lead * math.asin(dotprod) / (2 * math.pi) + + def _mat_invert_6x6(self, J: List[List[float]]) -> List[List[float]]: + """6x6 矩阵求逆 (高斯-约当消元法)""" + n = 6 + aug = [[0.0] * (2 * n) for _ in range(n)] + + for i in range(n): + for j in range(n): + aug[i][j] = J[i][j] + aug[i][i + n] = 1.0 + + for k in range(n): + if abs(aug[k][k]) < 0.01: + for j in range(k + 1, n): + if abs(aug[j][k]) > 0.01: + aug[k], aug[j] = aug[j], aug[k] + break + + pivot = aug[k][k] + if abs(pivot) < 1e-12: + raise ValueError(f"Matrix is singular at row {k}") + + for j in range(2 * n): + aug[k][j] /= pivot + + for i in range(n): + if i != k: + factor = aug[i][k] + for j in range(2 * n): + aug[i][j] -= factor * aug[k][j] + + inv = [[aug[i][j + n] for j in range(n)] for i in range(n)] + return inv + + def _mat_mult_6x6_vec(self, J: List[List[float]], x: List[float]) -> List[float]: + """6x6 矩阵乘 6x1 向量""" + result = [0.0] * 6 + for i in range(6): + for j in range(6): + result[i] += J[i][j] * x[j] + return result + + def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + """正运动学：牛顿-拉夫逊迭代法""" + if len(joints) < 6: + raise ValueError("Hexapod requires 6 strut lengths") + + if any(j <= 0 for j in joints): + self._forward_fail = True + return (-1, -1, -1, 0, 0, 0) + + x, y, z = 0.0, 0.0, 0.0 + a, b, c = 0.0, 0.0, 0.0 + + iteration = 0 + converge = False + self._forward_fail = False + + while iteration < self.p.max_iterations: + iteration += 1 + + if abs(self._conv_err) > self.p.max_error: + self._forward_fail = True + return (-1, -1, -1, 0, 0, 0) + + R = self._rotation_matrix(a, b, c) + + strut_diff = [] + jacobian_inv = [[0.0] * 6 for _ in range(6)] + + for i in range(self.NUM_STRUTS): + ax, ay, az = self.a[i] + bx, by, bz = self.b[i] + + aw_x = R[0][0]*ax + R[0][1]*ay + R[0][2]*az + x + aw_y = R[1][0]*ax + R[1][1]*ay + R[1][2]*az + y + aw_z = R[2][0]*ax + R[2][1]*ay + R[2][2]*az + z + + vx = aw_x - bx + vy = aw_y - by + vz = aw_z - bz + + length = math.sqrt(vx*vx + vy*vy + vz*vz) + + if length > 1e-12: + ux, uy, uz = vx/length, vy/length, vz/length + else: + ux, uy, uz = 0.0, 0.0, 1.0 + + if self.p.screw_lead != 0.0: + corr = self._strut_length_correction([ux, uy, uz], R, i) + length += corr + + strut_diff.append(length - joints[i]) + + rx = R[0][0]*ax + R[0][1]*ay + R[0][2]*az + ry = R[1][0]*ax + R[1][1]*ay + R[1][2]*az + rz = R[2][0]*ax + R[2][1]*ay + R[2][2]*az + + cross_x = ry * uz - rz * uy + cross_y = rz * ux - rx * uz + cross_z = rx * uy - ry * ux + + jacobian_inv[i] = [ux, uy, uz, cross_x, cross_y, cross_z] + + self._conv_err = sum(abs(d) for d in strut_diff) + max_diff = max(abs(d) for d in strut_diff) + + if max_diff < self.p.convergence_criterion: + converge = True + break + + try: + jacobian = self._mat_invert_6x6(jacobian_inv) + delta = self._mat_mult_6x6_vec(jacobian, strut_diff) + + x -= delta[0] + y -= delta[1] + z -= delta[2] + a -= delta[3] * TO_DEG + b -= delta[4] * TO_DEG + c -= delta[5] * TO_DEG + except ValueError: + self._forward_fail = True + return (-1, -1, -1, 0, 0, 0) + + self.last_iterations = iteration + if iteration > self.max_iterations_used: + self.max_iterations_used = iteration + + if not converge: + return (-1, -1, -1, 0, 0, 0) + + if self.debug: + print(f"[HEXAPOD] FWD: converged after {iteration} iterations") + + return (x, y, z, a, b, c) + + def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]: + """逆运动学：闭式解""" + if len(world) < 6: + raise ValueError("Hexapod requires 6 coordinates: X, Y, Z, A, B, C") + + x, y, z, a, b, c = world[0], world[1], world[2], world[3], world[4], world[5] + + R = self._rotation_matrix(a, b, c) + + struts = [] + for i in range(self.NUM_STRUTS): + ax, ay, az = self.a[i] + bx, by, bz = self.b[i] + + aw_x = R[0][0]*ax + R[0][1]*ay + R[0][2]*az + x + aw_y = R[1][0]*ax + R[1][1]*ay + R[1][2]*az + y + aw_z = R[2][0]*ax + R[2][1]*ay + R[2][2]*az + z + + vx = aw_x - bx + vy = aw_y - by + vz = aw_z - bz + + length = math.sqrt(vx*vx + vy*vy + vz*vz) + + if self.p.screw_lead != 0.0 and length > 1e-12: + u = [vx/length, vy/length, vz/length] + corr = self._strut_length_correction(u, R, i) + length += corr + + struts.append(length) + + if self.debug: + print(f"[HEXAPOD] INV: world -> {len(struts)} struts") + + return struts + + +# ==================== PUMA 运动学参数 ==================== + +@dataclass +class PUMAParams(KinematicsParams): + """PUMA 560 参数""" + a2: float = 300.0 + a3: float = 50.0 + d3: float = 70.0 + d4: float = 400.0 + d6: float = 70.0 + + +# ==================== PUMA 运动学 ==================== + +class PUMAKinematics(BaseKinematics): + """ + PUMA 560 运动学 - 完整版 + 基于 pumakins.c 完整实现 + """ + + PUMA_SHOULDER_RIGHT = 0x01 + PUMA_ELBOW_DOWN = 0x02 + PUMA_WRIST_FLIP = 0x04 + PUMA_SINGULAR = 0x08 + PUMA_REACH = 0x01 + + def __init__(self, params: PUMAParams = None, debug: bool = False): + super().__init__(params or PUMAParams()) + self.debug = debug + self.p = self.params + self.inverse_flags = 0 + self.SINGULAR_FUZZ = 0.000001 + self.FLAG_FUZZ = 0.000001 + self._last_joints = [0.0] * 6 + + def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + """正运动学 - 完整实现""" + if len(joints) < 6: + raise ValueError("PUMA requires 6 joints") + + s1, s2, s3, s4, s5, s6 = [math.sin(j * TO_RAD) for j in joints[:6]] + c1, c2, c3, c4, c5, c6 = [math.cos(j * TO_RAD) for j in joints[:6]] + + s23 = c2 * s3 + s2 * c3 + c23 = c2 * c3 - s2 * s3 + + t1 = c4 * c5 * c6 - s4 * s6 + t2 = s23 * s5 * c6 + t3 = s4 * c5 * c6 + c4 * s6 + t4 = c23 * t1 - t2 + + r11 = c1 * t4 + s1 * t3 + r21 = s1 * t4 - c1 * t3 + r31 = -s23 * t1 - c23 * s5 * c6 + + t1 = -c4 * c5 * s6 - s4 * c6 + t2 = s23 * s5 * s6 + t3 = c4 * c6 - s4 * c5 * s6 + t4 = c23 * t1 + t2 + + r12 = c1 * t4 + s1 * t3 + r22 = s1 * t4 - c1 * t3 + r32 = -s23 * t1 + c23 * s5 * s6 + + t1 = c23 * c4 * s5 + s23 * c5 + r13 = -c1 * t1 - s1 * s4 * s5 + r23 = -s1 * t1 + c1 * s4 * s5 + r33 = s23 * c4 * s5 - c23 * c5 + + t1_pos = self.p.a2 * c2 + self.p.a3 * c23 - self.p.d4 * s23 + px = c1 * t1_pos - self.p.d3 * s1 + py = s1 * t1_pos + self.p.d3 * c1 + pz = -self.p.a3 * s23 - self.p.a2 * s2 - self.p.d4 * c23 + + px += r13 * self.p.d6 + py += r23 * self.p.d6 + pz += r33 * self.p.d6 + + if abs(r31) < 0.99999: + b = math.atan2(-r31, math.sqrt(r11**2 + r21**2)) + c_angle = math.atan2(r21/math.cos(b), r11/math.cos(b)) + a_angle = math.atan2(r32/math.cos(b), r33/math.cos(b)) + else: + c_angle = 0 + if r31 <= -1: + b = math.pi / 2 + a_angle = math.atan2(r12, r13) + else: + b = -math.pi / 2 + a_angle = math.atan2(-r12, -r13) + + if self.debug: + print(f"[PUMA] FWD: joints -> world({px:.3f},{py:.3f},{pz:.3f})") + + return (px, py, pz, a_angle * TO_DEG, b * TO_DEG, c_angle * TO_DEG) + + def inverse(self, world: Tuple[float, float, float, float, float, float], + flags: int = 0) -> List[float]: + """逆运动学 - 完整实现""" + if len(world) < 6: + raise ValueError("PUMA requires 6 coordinates") + + px, py, pz, a_deg, b_deg, c_deg = world[:6] + + a_rad, b_rad, c_rad = a_deg * TO_RAD, b_deg * TO_RAD, c_deg * TO_RAD + + ca, sa = math.cos(a_rad), math.sin(a_rad) + cb, sb = math.cos(b_rad), math.sin(b_rad) + cc, sc = math.cos(c_rad), math.sin(c_rad) + + R = [ + [cc*cb, cc*sb*sa - sc*ca, cc*sb*ca + sc*sa], + [sc*cb, sc*sb*sa + cc*ca, sc*sb*ca - cc*sa], + [ -sb, cb*sa, cb*ca] + ] + + px = px - self.p.d6 * R[0][2] + py = py - self.p.d6 * R[1][2] + pz = pz - self.p.d6 * R[2][2] + + sumSq = px*px + py*py - self.p.d3*self.p.d3 + if sumSq < 0: + sumSq = 0 + + if flags & self.PUMA_SHOULDER_RIGHT: + th1 = math.atan2(py, px) - math.atan2(self.p.d3, -math.sqrt(sumSq)) + else: + th1 = math.atan2(py, px) - math.atan2(self.p.d3, math.sqrt(sumSq)) + + s1 = math.sin(th1) + c1 = math.cos(th1) + + k = (sumSq + pz*pz - self.p.a2*self.p.a2 - self.p.a3*self.p.a3 - self.p.d4*self.p.d4) / (2.0 * self.p.a2) + k_limit = math.sqrt(self.p.a3*self.p.a3 + self.p.d4*self.p.d4) + if abs(k) > k_limit: + k = k_limit if k > 0 else -k_limit + + if flags & self.PUMA_ELBOW_DOWN: + th3 = math.atan2(self.p.a3, self.p.d4) - math.atan2(k, -math.sqrt(self.p.a3*self.p.a3 + self.p.d4*self.p.d4 - k*k)) + else: + th3 = math.atan2(self.p.a3, self.p.d4) - math.atan2(k, math.sqrt(self.p.a3*self.p.a3 + self.p.d4*self.p.d4 - k*k)) + + s3 = math.sin(th3) + c3 = math.cos(th3) + + t1 = (-self.p.a3 - self.p.a2 * c3) * pz + (c1*px + s1*py) * (self.p.a2*s3 - self.p.d4) + t2 = (self.p.a2*s3 - self.p.d4) * pz + (self.p.a3 + self.p.a2*c3) * (c1*px + s1*py) + t3 = pz*pz + (c1*px + s1*py)*(c1*px + s1*py) + + th23 = math.atan2(t1, t2) + th2 = th23 - th3 + + s23 = t1 / t3 if t3 != 0 else 0 + c23 = t2 / t3 if t3 != 0 else 0 + + t1_j4 = -R[2][0] * s1 + R[2][1] * c1 + t2_j4 = -R[2][0] * c1 * c23 - R[2][1] * s1 * c23 + R[2][2] * s23 + + if abs(t1_j4) < self.SINGULAR_FUZZ and abs(t2_j4) < self.SINGULAR_FUZZ: + th4 = self._last_joints[3] * TO_RAD if len(self._last_joints) > 3 else 0 + else: + th4 = math.atan2(t1_j4, t2_j4) + + s4 = math.sin(th4) + c4 = math.cos(th4) + + s5 = R[2][2] * (s23*c4) - R[2][0] * (c1*c23*c4 + s1*s4) - R[2][1] * (s1*c23*c4 - c1*s4) + c5 = -R[2][0] * (c1*s23) - R[2][1] * (s1*s23) - R[2][2] * c23 + th5 = math.atan2(s5, c5) + + s6 = R[0][2] * (s23*s4) - R[0][0] * (c1*c23*s4 - s1*c4) - R[0][1] * (s1*c23*s4 + c1*c4) + c6 = R[0][0] * ((c1*c23*c4 + s1*s4)*c5 - c1*s23*s5) + \ + R[0][1] * ((s1*c23*c4 - c1*s4)*c5 - s1*s23*s5) - \ + R[0][2] * (s23*c4*c5 + c23*s5) + th6 = math.atan2(s6, c6) + + if flags & self.PUMA_WRIST_FLIP: + th4 = th4 + math.pi + th5 = -th5 + th6 = th6 + math.pi + + joints = [ + th1 * TO_DEG, th2 * TO_DEG, th3 * TO_DEG, + th4 * TO_DEG, th5 * TO_DEG, th6 * TO_DEG + ] + + joints = [(j + 180) % 360 - 180 for j in joints] + self._last_joints = joints.copy() + + if self.debug: + print(f"[PUMA] INV: world -> joints({joints[0]:.3f}, {joints[1]:.3f}, ...)") + + return joints + + +# ==================== SCARA 运动学参数 ==================== + +@dataclass +class SCARAParams(KinematicsParams): + """SCARA 参数""" + d1: float = 490.0 + d2: float = 340.0 + d3: float = 50.0 + d4: float = 250.0 + d5: float = 50.0 + d6: float = 50.0 + + +# ==================== SCARA 运动学 ==================== + +class SCARAKinematics(BaseKinematics): + """ + SCARA 运动学 (基于 scarakins.c) + """ + + def __init__(self, params: SCARAParams = None, debug: bool = False): + super().__init__(params or SCARAParams()) + self.debug = debug + self.p = self.params + self._inverse_flags = 0 + + def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + if len(joints) < 4: + raise ValueError("SCARA requires at least 4 joints") + + a0 = joints[0] * TO_RAD + a1 = joints[1] * TO_RAD + a3 = joints[3] * TO_RAD if len(joints) > 3 else 0 + + a1_total = a1 + a0 + a3_total = a3 + a1_total + + x = self.p.d2 * math.cos(a0) + self.p.d4 * math.cos(a1_total) + self.p.d6 * math.cos(a3_total) + y = self.p.d2 * math.sin(a0) + self.p.d4 * math.sin(a1_total) + self.p.d6 * math.sin(a3_total) + z = self.p.d1 + self.p.d3 - joints[2] - self.p.d5 + c = a3_total * TO_DEG + + if self.debug: + print(f"[SCARA] FWD: joints -> world({x:.3f},{y:.3f},{z:.3f})") + + return (x, y, z, 0.0, 0.0, c) + + def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]: + if len(world) < 6: + raise ValueError("SCARA requires 6 coordinates") + + x, y, z, _, _, c = world[:6] + a3 = c * TO_RAD + + xt = x - self.p.d6 * math.cos(a3) + yt = y - self.p.d6 * math.sin(a3) + + rsq = xt*xt + yt*yt + cc = (rsq - self.p.d2*self.p.d2 - self.p.d4*self.p.d4) / (2 * self.p.d2 * self.p.d4) + cc = max(-1.0, min(1.0, cc)) + + q1 = math.acos(cc) + if self._inverse_flags & 1: + q1 = -q1 + + q0 = math.atan2(yt, xt) - math.atan2(self.p.d4 * math.sin(q1), self.p.d2 + self.p.d4 * math.cos(q1)) + + joints = [ + q0 * TO_DEG, + q1 * TO_DEG, + self.p.d1 + self.p.d3 - self.p.d5 - z, + c - (q0 + q1) * TO_DEG, + 0.0, 0.0 + ] + + if self.debug: + print(f"[SCARA] INV: world({x:.3f},{y:.3f},{z:.3f}) -> joints") + + return joints + + def set_flags(self, flags: int): + """设置逆解标志""" + self._inverse_flags = flags + + +# ==================== 线性 Delta 运动学 ==================== + +class LinearDeltaKinematics(BaseKinematics): + """ + 线性 Delta 机器人运动学 (基于 lineardeltakins-common.h) + Rostock 风格 Delta 机器人 + """ + + def __init__(self, radius: float = 165.25, rod_length: float = 269.0, debug: bool = False): + super().__init__() + self.R = radius + self.L = rod_length + self.debug = debug + self._update_geometry() + + def _update_geometry(self): + self.L2 = self.L * self.L + SQ3 = math.sqrt(3) + SIN_60 = SQ3 / 2 + COS_60 = 0.5 + + self.Ax = 0.0 + self.Ay = self.R + + self.Bx = -SIN_60 * self.R + self.By = -COS_60 * self.R + + self.Cx = SIN_60 * self.R + self.Cy = -COS_60 * self.R + + def set_geometry(self, radius: float, rod_length: float): + self.R = radius + self.L = rod_length + self._update_geometry() + if self.debug: + print(f"[LINEAR_DELTA] 几何更新: R={radius:.3f}, L={rod_length:.3f}") + + def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + if len(joints) < 3: + raise ValueError("Linear Delta requires 3 joints") + + q1, q2, q3 = joints[0], joints[1], joints[2] + + den = (self.By - self.Ay) * self.Cx - (self.Cy - self.Ay) * self.Bx + + if abs(den) < 1e-12: + return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + w1 = self.Ay * self.Ay + q1 * q1 + w2 = self.Bx * self.Bx + self.By * self.By + q2 * q2 + w3 = self.Cx * self.Cx + self.Cy * self.Cy + q3 * q3 + + a1 = (q2 - q1) * (self.Cy - self.Ay) - (q3 - q1) * (self.By - self.Ay) + b1 = -((w2 - w1) * (self.Cy - self.Ay) - (w3 - w1) * (self.By - self.Ay)) / 2.0 + + a2 = -(q2 - q1) * self.Cx + (q3 - q1) * self.Bx + b2 = ((w2 - w1) * self.Cx - (w3 - w1) * self.Bx) / 2.0 + + a = a1 * a1 + a2 * a2 + den * den + b = 2 * (a1 * b1 + a2 * (b2 - self.Ay * den) - q1 * den * den) + c = (b2 - self.Ay * den) * (b2 - self.Ay * den) + b1 * b1 + den * den * (q1 * q1 - self.L2) + + discr = b * b - 4.0 * a * c + if discr < 0: + return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + z = -0.5 * (b + math.sqrt(discr)) / a + x = (a1 * z + b1) / den + y = (a2 * z + b2) / den + + if self.debug: + print(f"[LINEAR_DELTA] FWD: joints({q1:.3f},{q2:.3f},{q3:.3f}) -> world({x:.3f},{y:.3f},{z:.3f})") + + return (x, y, z, 0.0, 0.0, 0.0) + + def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]: + x, y, z = world[0], world[1], world[2] + + def sq(v): return v * v + + q1 = z + math.sqrt(max(0, self.L2 - sq(self.Ax - x) - sq(self.Ay - y))) + q2 = z + math.sqrt(max(0, self.L2 - sq(self.Bx - x) - sq(self.By - y))) + q3 = z + math.sqrt(max(0, self.L2 - sq(self.Cx - x) - sq(self.Cy - y))) + + if self.debug: + print(f"[LINEAR_DELTA] INV: world({x:.3f},{y:.3f},{z:.3f}) -> joints({q1:.3f},{q2:.3f},{q3:.3f})") + + return [q1, q2, q3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + +# ==================== DH 参数串联运动学 ==================== + +@dataclass +class DHLink: + """Denavit-Hartenberg 参数""" + a: float = 0.0 + alpha: float = 0.0 + d: float = 0.0 + theta: float = 0.0 + is_revolute: bool = True + min_limit: float = -180.0 + max_limit: float = 180.0 + + +@dataclass +class SerialDHParams(KinematicsParams): + """串联 DH 参数""" + links: List[DHLink] = field(default_factory=list) + max_iterations: int = 100 + convergence_epsilon: float = 1e-6 + use_kinematic_decoupling: bool = False + + +class SerialDHKinematics(BaseKinematics): + """ + 通用串联机器人运动学 - 完整版 + 基于 genserfuncs.c 完整实现 + """ + + def __init__(self, params: SerialDHParams = None, debug: bool = False): + super().__init__(params or SerialDHParams()) + self.debug = debug + self.p = self.params + self.iterations = 0 + self.GO_REAL_EPSILON = 1e-6 + + def _dh_transform(self, link: DHLink, joint_value: float) -> List[List[float]]: + """计算单个连杆的变换矩阵""" + if link.is_revolute: + theta = (link.theta + joint_value) * TO_RAD + d = link.d + else: + theta = link.theta * TO_RAD + d = link.d + joint_value + + alpha = link.alpha * TO_RAD + a = link.a + + ct = math.cos(theta) + st = math.sin(theta) + ca = math.cos(alpha) + sa = math.sin(alpha) + + return [ + [ct, -st*ca, st*sa, a*ct], + [st, ct*ca, -ct*sa, a*st], + [ 0, sa, ca, d], + [ 0, 0, 0, 1] + ] + + def _mat_mult_4x4(self, A: List[List[float]], B: List[List[float]]) -> List[List[float]]: + """4x4 矩阵乘法""" + result = [[0.0]*4 for _ in range(4)] + for i in range(4): + for j in range(4): + for k in range(4): + result[i][j] += A[i][k] * B[k][j] + return result + + def _mat_mult_3x3_vec(self, M: List[List[float]], v: List[float]) -> List[float]: + """3x3 矩阵乘 3x1 向量""" + return [ + M[0][0]*v[0] + M[0][1]*v[1] + M[0][2]*v[2], + M[1][0]*v[0] + M[1][1]*v[1] + M[1][2]*v[2], + M[2][0]*v[0] + M[2][1]*v[1] + M[2][2]*v[2] + ] + + def _cross_product_matrix(self, v: List[float]) -> List[List[float]]: + """向量的叉积矩阵 (用于雅可比计算)""" + return [ + [ 0, -v[2], v[1]], + [ v[2], 0, -v[0]], + [-v[1], v[0], 0] + ] + + def _mat_mult_3x3(self, A: List[List[float]], B: List[List[float]]) -> List[List[float]]: + """3x3 矩阵乘法""" + result = [[0.0]*3 for _ in range(3)] + for i in range(3): + for j in range(3): + for k in range(3): + result[i][j] += A[i][k] * B[k][j] + return result + + def _mat_transpose_3x3(self, M: List[List[float]]) -> List[List[float]]: + """3x3 矩阵转置""" + return [[M[j][i] for j in range(3)] for i in range(3)] + + def _rpy_to_rot_matrix(self, r: float, p: float, y: float) -> List[List[float]]: + """RPY 角转旋转矩阵""" + cr, sr = math.cos(r), math.sin(r) + cp, sp = math.cos(p), math.sin(p) + cy, sy = math.cos(y), math.sin(y) + + return [ + [cy*cp, cy*sp*sr - sy*cr, cy*sp*cr + sy*sr], + [sy*cp, sy*sp*sr + cy*cr, sy*sp*cr - cy*sr], + [ -sp, cp*sr, cp*cr] + ] + + def _rot_matrix_to_rpy(self, R: List[List[float]]) -> Tuple[float, float, float]: + """旋转矩阵转 RPY 角""" + if abs(R[2][0]) < 0.99999: + p = math.atan2(-R[2][0], math.sqrt(R[0][0]**2 + R[1][0]**2)) + y = math.atan2(R[1][0]/math.cos(p), R[0][0]/math.cos(p)) + r = math.atan2(R[2][1]/math.cos(p), R[2][2]/math.cos(p)) + else: + y = 0 + if R[2][0] <= -1: + p = math.pi / 2 + r = math.atan2(R[0][1], R[0][2]) + else: + p = -math.pi / 2 + r = math.atan2(-R[0][1], -R[0][2]) + return (r, p, y) + + def _compute_jfwd(self, joints: List[float]) -> Tuple[List[List[float]], List[List[float]]]: + """计算正向雅可比矩阵""" + n = len(joints) + + Jv = [[0.0] * n for _ in range(3)] + Jw = [[0.0] * n for _ in range(3)] + + if self.p.links[0].is_revolute: + Jw[2][0] = 1.0 + else: + Jv[2][0] = 1.0 + + T_curr = [[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]] + poses = [] + + for i, link in enumerate(self.p.links): + T_link = self._dh_transform(link, joints[i]) + T_curr = self._mat_mult_4x4(T_curr, T_link) + poses.append(T_curr) + + R_end = [[T_curr[i][j] for j in range(3)] for i in range(3)] + + for col in range(1, n): + T_prev = poses[col - 1] + R_prev = [[T_prev[i][j] for j in range(3)] for i in range(3)] + + link = self.p.links[col] + if link.is_revolute: + theta = (link.theta + joints[col]) * TO_RAD + d = link.d + else: + theta = link.theta * TO_RAD + d = link.d + joints[col] + + a = link.a + P_i_ip1 = [a * math.cos(theta), a * math.sin(theta), d] + P_ip1_0 = self._mat_mult_3x3_vec(R_prev, P_i_ip1) + + Jw_col = [0.0, 0.0, 1.0] if link.is_revolute else [0.0, 0.0, 0.0] + cross = self._cross_product_matrix(P_ip1_0) + Jv_col = [cross[i][0]*Jw_col[0] + cross[i][1]*Jw_col[1] + cross[i][2]*Jw_col[2] for i in range(3)] + + for i in range(3): + Jv[i][col] = Jv_col[i] + Jw[i][col] = Jw_col[i] + + R_end_inv = self._mat_transpose_3x3(R_end) + Jv = [self._mat_mult_3x3_vec(R_end_inv, [Jv[i][j] for i in range(3)]) for j in range(n)] + Jv = [[Jv[j][i] for j in range(n)] for i in range(3)] + Jw = [self._mat_mult_3x3_vec(R_end_inv, [Jw[i][j] for i in range(3)]) for j in range(n)] + Jw = [[Jw[j][i] for j in range(n)] for i in range(3)] + + Jfwd = [[0.0] * n for _ in range(6)] + for col in range(n): + for row in range(3): + Jfwd[row][col] = Jv[row][col] + Jfwd[row + 3][col] = Jw[row][col] + + return Jfwd, R_end + + def _mat_invert_nxn(self, M: List[List[float]], n: int) -> List[List[float]]: + """nxn 矩阵求逆""" + aug = [[0.0] * (2 * n) for _ in range(n)] + + for i in range(n): + for j in range(n): + aug[i][j] = M[i][j] + aug[i][i + n] = 1.0 + + for k in range(n): + pivot = aug[k][k] + if abs(pivot) < 1e-12: + raise ValueError(f"Matrix is singular at row {k}") + + for j in range(2 * n): + aug[k][j] /= pivot + + for i in range(n): + if i != k: + factor = aug[i][k] + for j in range(2 * n): + aug[i][j] -= factor * aug[k][j] + + return [[aug[i][j + n] for j in range(n)] for i in range(n)] + + def _compute_jinv(self, Jfwd: List[List[float]]) -> List[List[float]]: + """计算雅可比矩阵的逆或伪逆""" + m = len(Jfwd) + n = len(Jfwd[0]) + + if m == n: + return self._mat_invert_nxn(Jfwd, m) + elif m < n: + JJT = [[0.0] * m for _ in range(m)] + for i in range(m): + for j in range(m): + for k in range(n): + JJT[i][j] += Jfwd[i][k] * Jfwd[j][k] + + JJT_inv = self._mat_invert_nxn(JJT, m) + JT = [[Jfwd[j][i] for j in range(m)] for i in range(n)] + Jinv = [[0.0] * m for _ in range(n)] + for i in range(n): + for j in range(m): + for k in range(m): + Jinv[i][j] += JT[i][k] * JJT_inv[k][j] + return Jinv + else: + JTJ = [[0.0] * n for _ in range(n)] + for i in range(n): + for j in range(n): + for k in range(m): + JTJ[i][j] += Jfwd[k][i] * Jfwd[k][j] + + JTJ_inv = self._mat_invert_nxn(JTJ, n) + JT = [[Jfwd[j][i] for j in range(m)] for i in range(n)] + Jinv = [[0.0] * m for _ in range(n)] + for i in range(n): + for j in range(m): + for k in range(n): + Jinv[i][j] += JTJ_inv[i][k] * JT[k][j] + return Jinv + + def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + """正运动学""" + if len(joints) != len(self.p.links): + raise ValueError(f"Expected {len(self.p.links)} joints, got {len(joints)}") + + T = [[1,0,0,0], [0,1,0,0], [0,0,1,0], [0,0,0,1]] + + for i, link in enumerate(self.p.links): + T_link = self._dh_transform(link, joints[i]) + T = self._mat_mult_4x4(T, T_link) + + x, y, z = T[0][3], T[1][3], T[2][3] + + r, p, y_rpy = self._rot_matrix_to_rpy([[T[i][j] for j in range(3)] for i in range(3)]) + + if self.debug: + print(f"[SERIAL_DH] FWD: joints -> world({x:.3f},{y:.3f},{z:.3f})") + + return (x, y, z, r * TO_DEG, p * TO_DEG, y_rpy * TO_DEG) + + def inverse(self, world: Tuple[float, float, float, float, float, float], + initial_joints: List[float] = None) -> List[float]: + """逆运动学：牛顿-拉夫逊迭代法""" + if len(world) < 6: + raise ValueError("Serial DH requires 6 coordinates") + + n = len(self.p.links) + + if initial_joints and len(initial_joints) == n: + jest = list(initial_joints) + else: + jest = [0.0] * n + + x, y, z, a, b, c = world[:6] + R_target = self._rpy_to_rot_matrix(a * TO_RAD, b * TO_RAD, c * TO_RAD) + + self.iterations = 0 + + for iteration in range(self.p.max_iterations): + self.iterations = iteration + 1 + + Jfwd, R_current = self._compute_jfwd(jest) + current_pose = self.forward(jest) + cx, cy, cz = current_pose[0], current_pose[1], current_pose[2] + + dx = x - cx + dy = y - cy + dz = z - cz + + R_diff = self._mat_mult_3x3(R_target, self._mat_transpose_3x3(R_current)) + rx = 0.5 * (R_diff[2][1] - R_diff[1][2]) + ry = 0.5 * (R_diff[0][2] - R_diff[2][0]) + rz = 0.5 * (R_diff[1][0] - R_diff[0][1]) + + error = [dx, dy, dz, rx, ry, rz] + + max_error = max(abs(e) for e in error) + if max_error < self.p.convergence_epsilon: + if self.debug: + print(f"[SERIAL_DH] INV: converged after {iteration + 1} iterations") + return jest + + try: + Jinv = self._compute_jinv(Jfwd) + except ValueError: + break + + dq = [0.0] * n + for i in range(n): + for j in range(6): + dq[i] += Jinv[i][j] * error[j] + + for i in range(n): + jest[i] += dq[i] + + if self.debug: + print(f"[SERIAL_DH] INV: did not converge after {self.p.max_iterations} iterations") + + return jest + + +# ==================== 新增运动学类 ==================== + +@dataclass +class TripodParams(KinematicsParams): + """Tripod 参数""" + Bx: float = 1.0 + Cx: float = 1.0 + Cy: float = 1.0 + + +class TripodKinematics(BaseKinematics): + """Tripod运动学 - 基于 tripodkins.c""" + + def __init__(self, params: TripodParams = None, debug: bool = False): + super().__init__(params or TripodParams()) + self.debug = debug + self.p = self.params + + def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + if len(joints) < 3: + return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + P = joints[0] * joints[0] + Q = joints[1] * joints[1] - self.p.Bx * self.p.Bx + R = joints[2] * joints[2] - self.p.Cx * self.p.Cx - self.p.Cy * self.p.Cy + + s = -2.0 * self.p.Bx + t = -2.0 * self.p.Cx + u = -2.0 * self.p.Cy + + if abs(s) < 1e-12 or abs(u) < 1e-12: + return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + x = (Q - P) / s + y = (R - Q - (t - s) * x) / u + z = P - x*x - y*y + + if z < 0: + return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + z = math.sqrt(z) + + if self.debug: + print(f"[TRIPOD] FWD: joints -> world({x:.3f},{y:.3f},{z:.3f})") + return (x, y, z, 0.0, 0.0, 0.0) + + def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]: + x, y, z = world[0], world[1], world[2] + + L1 = math.sqrt(x*x + y*y + z*z) + L2 = math.sqrt((x - self.p.Bx)*(x - self.p.Bx) + y*y + z*z) + L3 = math.sqrt((x - self.p.Cx)*(x - self.p.Cx) + (y - self.p.Cy)*(y - self.p.Cy) + z*z) + + if self.debug: + print(f"[TRIPOD] INV: world({x:.3f},{y:.3f},{z:.3f}) -> joints({L1:.3f},{L2:.3f},{L3:.3f})") + return [L1, L2, L3, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + +@dataclass +class PentapodParams(KinematicsParams): + """Pentapod 参数""" + base: List[Tuple[float, float, float]] = field(default_factory=lambda: [ + (-418.03, 324.56, 895.56), (417.96, 324.56, 895.56), + (-418.03, -325.44, 895.56), (417.96, -325.44, 895.56), + (-0.06, -492.96, 895.56) + ]) + effector_r: List[float] = field(default_factory=lambda: [80.32] * 5) + effector_z: List[float] = field(default_factory=lambda: [-185.50, -159.50, -67.50, -41.50, -14.00]) + tool_offset: float = 0.0 + convergence_criterion: float = 1e-9 + max_iterations: int = 120 + max_error: float = 100.0 + + +class PentapodKinematics(BaseKinematics): + """Pentapod运动学 - 基于 pentakins.c""" + + NUM_STRUTS = 5 + + def __init__(self, params: PentapodParams = None, debug: bool = False): + super().__init__(params or PentapodParams()) + self.debug = debug + self.p = self.params + self.last_iterations = 0 + + def _rotation_matrix(self, r: float, p: float, y: float) -> List[List[float]]: + cr, sr = math.cos(r), math.sin(r) + cp, sp = math.cos(p), math.sin(p) + return [ + [cp, sp*sr, sp*cr], + [0, cr, -sr], + [-sp, cp*sr, cp*cr] + ] + + def _inv_kins(self, coord: List[float]) -> List[float]: + x, y, z = coord[0], coord[1], coord[2] + r, p = coord[3], coord[4] + + R = self._rotation_matrix(r, p, 0) + R_inv = [[R[j][i] for j in range(3)] for i in range(3)] + + struts = [] + for i in range(self.NUM_STRUTS): + bx, by, bz = self.p.base[i] + bz += self.p.tool_offset + ra = self.p.effector_r[i] + za = self.p.effector_z[i] + self.p.tool_offset + + dx = bx - x + dy = by - y + dz = bz - z + + tx = R_inv[0][0]*dx + R_inv[0][1]*dy + R_inv[0][2]*dz + ty = R_inv[1][0]*dx + R_inv[1][1]*dy + R_inv[1][2]*dz + tz = R_inv[2][0]*dx + R_inv[2][1]*dy + R_inv[2][2]*dz + + L = math.sqrt((tz - za)**2 + (math.sqrt(tx*tx + ty*ty) - ra)**2) + struts.append(L) + + return struts + + def _mat_invert_5x5(self, J: List[List[float]]) -> List[List[float]]: + n = 5 + aug = [[0.0] * (2 * n) for _ in range(n)] + + for i in range(n): + for j in range(n): + aug[i][j] = J[i][j] + aug[i][i + n] = 1.0 + + for k in range(n): + if abs(aug[k][k]) < 0.01: + for j in range(k + 1, n): + if abs(aug[j][k]) > 0.01: + aug[k], aug[j] = aug[j], aug[k] + break + + pivot = aug[k][k] + if abs(pivot) < 1e-12: + continue + + for j in range(2 * n): + aug[k][j] /= pivot + + for i in range(n): + if i != k: + factor = aug[i][k] + for j in range(2 * n): + aug[i][j] -= factor * aug[k][j] + + return [[aug[i][j + n] for j in range(n)] for i in range(n)] + + def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + if len(joints) < 5: + return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + coord = [0.0, 0.0, 0.0, 0.0, 0.0] + iteration = 0 + + while iteration < self.p.max_iterations: + iteration += 1 + + struts = self._inv_kins(coord) + diff = [struts[i] - joints[i] for i in range(5)] + + max_diff = max(abs(d) for d in diff) + if max_diff < self.p.convergence_criterion: + break + + J = [[0.0]*5 for _ in range(5)] + delta = 1e-4 + for i in range(5): + orig = coord[i] + coord[i] += delta + struts_pert = self._inv_kins(coord) + coord[i] = orig + for j in range(5): + J[j][i] = (struts_pert[j] - struts[j]) / delta + + try: + Jinv = self._mat_invert_5x5(J) + dq = [0.0] * 5 + for i in range(5): + for j in range(5): + dq[i] += Jinv[i][j] * diff[j] + + for i in range(5): + coord[i] -= dq[i] + except: + pass + + self.last_iterations = iteration + + if self.debug: + print(f"[PENTAPOD] FWD: converged after {iteration} iterations") + + return (coord[0], coord[1], coord[2], coord[3] * TO_DEG, coord[4] * TO_DEG, 0.0) + + def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]: + x, y, z = world[0], world[1], world[2] + a, b = world[3] * TO_RAD, world[4] * TO_RAD + + coord = [x, y, z, a, b] + struts = self._inv_kins(coord) + + if self.debug: + print(f"[PENTAPOD] INV: world -> {len(struts)} struts") + + return struts + [0.0] * (9 - len(struts)) + + def set_tool_length(self, length: float): + self.p.tool_offset = length + if self.debug: + print(f"[PENTAPOD] 刀具长度设置为: {length:.3f} mm") + + +@dataclass +class RotaryDeltaParams(KinematicsParams): + """Rotary Delta 参数""" + platform_radius: float = 10.0 + thigh_length: float = 10.0 + shin_length: float = 14.0 + foot_radius: float = 6.0 + + +class RotaryDeltaKinematics(BaseKinematics): + """Rotary Delta运动学 - 基于 rotarydeltakins.c""" + + def __init__(self, params: RotaryDeltaParams = None, debug: bool = False): + super().__init__(params or RotaryDeltaParams()) + self.debug = debug + self.p = self.params + + def _inverse_j0(self, x: float, y: float, z: float) -> Optional[float]: + pfr = self.p.platform_radius + fr = self.p.foot_radius + tl = self.p.thigh_length + sl = self.p.shin_length + + a = 0.5 * (x*x + (y-fr)*(y-fr) + z*z + tl*tl - sl*sl - pfr*pfr) / z + b = (fr - pfr - y) / z + + d = tl*tl * (b*b + 1) - (a - b*pfr)*(a - b*pfr) + if d < 0: + return None + + knee_y = (pfr + a*b + math.sqrt(d)) / (b*b + 1) + knee_z = b * knee_y - a + theta = math.atan2(knee_z, knee_y - pfr) * TO_DEG + return theta + + def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + if len(joints) < 3: + return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + j0, j1, j2 = [j * TO_RAD for j in joints[:3]] + pfr = self.p.platform_radius + fr = self.p.foot_radius + tl = self.p.thigh_length + + y1 = -(pfr - fr + tl * math.cos(j0)) + z1 = -tl * math.sin(j0) + + y2 = (pfr - fr + tl * math.cos(j1)) * 0.5 + x2 = y2 * math.sqrt(3) + z2 = -tl * math.sin(j1) + + y3 = (pfr - fr + tl * math.cos(j2)) * 0.5 + x3 = -y3 * math.sqrt(3) + z3 = -tl * math.sin(j2) + + denom = x3*(y2-y1) - x2*(y3-y1) + if abs(denom) < 1e-12: + return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + w1 = y1*y1 + z1*z1 + w2 = x2*x2 + y2*y2 + z2*z2 + w3 = x3*x3 + y3*y3 + z3*z3 + + a1 = (z2-z1)*(y3-y1) - (z3-z1)*(y2-y1) + b1 = -((w2-w1)*(y3-y1) - (w3-w1)*(y2-y1)) / 2.0 + a2 = -(z2-z1)*x3 + (z3-z1)*x2 + b2 = ((w2-w1)*x3 - (w3-w1)*x2) / 2.0 + + a = a1*a1 + a2*a2 + denom*denom + b = 2 * (a1*b1 + a2*(b2 - y1*denom) - z1*denom*denom) + c = (b2 - y1*denom)*(b2 - y1*denom) + b1*b1 + denom*denom*(z1*z1 - self.p.shin_length*self.p.shin_length) + + d = b*b - 4*a*c + if d < 0: + return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + z = (-b - math.sqrt(d)) / (2*a) + x = (a1*z + b1) / denom + y = (a2*z + b2) / denom + + if self.debug: + print(f"[ROTARY_DELTA] FWD: joints -> world({x:.3f},{y:.3f},{z:.3f})") + + return (x, y, z, 0.0, 0.0, 0.0) + + def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]: + x, y, z = world[0], world[1], world[2] + + def rotate(x: float, y: float, theta: float) -> Tuple[float, float]: + c, s = math.cos(theta), math.sin(theta) + return (x*c - y*s, x*s + y*c) + + j0 = self._inverse_j0(x, y, z) + if j0 is None: + return [0.0] * 9 + + xr, yr = rotate(x, y, -2*math.pi/3) + j1 = self._inverse_j0(xr, yr, z) + if j1 is None: + return [0.0] * 9 + + xr, yr = rotate(x, y, 2*math.pi/3) + j2 = self._inverse_j0(xr, yr, z) + if j2 is None: + return [0.0] * 9 + + if self.debug: + print(f"[ROTARY_DELTA] INV: world -> joints({j0:.3f},{j1:.3f},{j2:.3f})") + + return [j0, j1, j2, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + +class RotateKinematics(BaseKinematics): + """Rotary Table运动学 - 基于 rotatekins.c""" + + def __init__(self, params: KinematicsParams = None, debug: bool = False): + super().__init__(params) + self.debug = debug + + def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + if len(joints) < 6: + joints = list(joints) + [0.0] * (6 - len(joints)) + + x, y, z, a, b, c = joints[0], joints[1], joints[2], joints[3], joints[4], joints[5] + c_rad = -c * TO_RAD + + pos_x = x * math.cos(c_rad) - y * math.sin(c_rad) + pos_y = x * math.sin(c_rad) + y * math.cos(c_rad) + + return (pos_x, pos_y, z, a, b, c) + + def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]: + if len(world) < 6: + world = list(world) + [0.0] * (6 - len(world)) + + wx, wy, wz, wa, wb, wc = world[0], world[1], world[2], world[3], world[4], world[5] + c_rad = wc * TO_RAD + + x = wx * math.cos(c_rad) - wy * math.sin(c_rad) + y = wx * math.sin(c_rad) + wy * math.cos(c_rad) + + u = world[6] if len(world) > 6 else 0.0 + v = world[7] if len(world) > 7 else 0.0 + w = world[8] if len(world) > 8 else 0.0 + + return [x, y, wz, wa, wb, wc, u, v, w] + + +class CoreXYKinematics(BaseKinematics): + """CoreXY运动学 - 基于 corexykins.c""" + + def __init__(self, params: KinematicsParams = None, debug: bool = False): + super().__init__(params) + self.debug = debug + + def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + if len(joints) < 3: + return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + x = 0.5 * (joints[0] + joints[1]) + y = 0.5 * (joints[0] - joints[1]) + z = joints[2] if len(joints) > 2 else 0.0 + a = joints[3] if len(joints) > 3 else 0.0 + b = joints[4] if len(joints) > 4 else 0.0 + c = joints[5] if len(joints) > 5 else 0.0 + + return (x, y, z, a, b, c) + + def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]: + x, y, z, a, b, c = world[0], world[1], world[2], world[3], world[4], world[5] + + j0 = x + y + j1 = x - y + j2 = z + j3 = a + j4 = b + j5 = c + + u = world[6] if len(world) > 6 else 0.0 + v = world[7] if len(world) > 7 else 0.0 + w = world[8] if len(world) > 8 else 0.0 + + return [j0, j1, j2, j3, j4, j5, u, v, w] + + +class ScorbotKinematics(BaseKinematics): + """Scorbot ER3运动学 - 基于 scorbot-kins.c""" + + L0_HORIZONTAL = 16.0 + L0_VERTICAL = 140.0 + L1 = 221.0 + L2 = 221.0 + + def __init__(self, params: KinematicsParams = None, debug: bool = False): + super().__init__(params) + self.debug = debug + + + def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + if len(joints) < 4: + return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + j0, j1, j2 = joints[0], joints[1], joints[2] + j0_rad, j1_rad, j2_rad = j0 * TO_RAD, j1 * TO_RAD, j2 * TO_RAD + + j1_x = self.L0_HORIZONTAL * math.cos(j0_rad) + j1_y = self.L0_HORIZONTAL * math.sin(j0_rad) + j1_z = self.L0_VERTICAL + + r1 = self.L1 * math.cos(j1_rad) + j2_x = r1 * math.cos(j0_rad) + j2_y = r1 * math.sin(j0_rad) + j2_z = self.L1 * math.sin(j1_rad) + + r2 = self.L2 * math.cos(j2_rad) + j3_x = r2 * math.cos(j0_rad) + j3_y = r2 * math.sin(j0_rad) + j3_z = self.L2 * math.sin(j2_rad) + + x = j1_x + j2_x + j3_x + y = j1_y + j2_y + j3_y + z = j1_z + j2_z + j3_z + + a = joints[3] if len(joints) > 3 else 0.0 + b = joints[4] if len(joints) > 4 else 0.0 + + return (x, y, z, a, b, 0.0) + + def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]: + x, y, z = world[0], world[1], world[2] + + j0 = math.atan2(y, x) * TO_DEG + + r_j1 = self.L0_HORIZONTAL + z_j1 = self.L0_VERTICAL + + r_cp = math.sqrt(x*x + y*y) - r_j1 + z_cp = z - z_j1 + + dist_to_cp = math.sqrt(r_cp*r_cp + z_cp*z_cp) + dist_to_center = dist_to_cp / 2.0 + + angle_to_cp = math.acos(r_cp / dist_to_cp) * TO_DEG if dist_to_cp > 0 else 0 + if z_cp < 0: + angle_to_cp = -angle_to_cp + + if dist_to_center > self.L1: + dist_to_center = self.L1 + + j1_angle = math.acos(dist_to_center / self.L1) * TO_DEG + j1 = angle_to_cp + j1_angle + + z_j2 = self.L1 * math.sin(j1 * TO_RAD) + j2 = -math.asin(max(-1, min(1, (z_j2 - z_cp) / self.L2))) * TO_DEG + + j3 = world[3] if len(world) > 3 else 0.0 + j4 = world[4] if len(world) > 4 else 0.0 + + return [j0, j1, j2, j3, j4, 0.0, 0.0, 0.0, 0.0] + + +@dataclass +class RoseParams(KinematicsParams): + """Rose Engine 参数""" + pass + + +class RoseKinematics(BaseKinematics): + """Rose Engine运动学 - 基于 rosekins.c""" + + def __init__(self, params: RoseParams = None, debug: bool = False): + super().__init__(params or RoseParams()) + self.debug = debug + self._revolutions = 0 + self._old_quad = 0 + + def forward(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + if len(joints) < 3: + return (0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + radius, z, theta = joints[0], joints[1], joints[2] + theta_rad = theta * TO_RAD + + x = radius * math.cos(theta_rad) + y = radius * math.sin(theta_rad) + + return (x, y, z, 0.0, 0.0, 0.0) + + def inverse(self, world: Tuple[float, float, float, float, float, float]) -> List[float]: + x, y, z = world[0], world[1], world[2] + + radius = math.hypot(x, y) + theta = math.atan2(y, x) * TO_DEG + + if x >= 0 and y >= 0: + now_quad = 1 + elif x < 0 and y >= 0: + now_quad = 2 + elif x < 0 and y < 0: + now_quad = 3 + else: + now_quad = 4 + + if self._old_quad == 2 and now_quad == 3: + self._revolutions += 1 + elif self._old_quad == 3 and now_quad == 2: + self._revolutions -= 1 + + big_theta = theta + 360.0 * self._revolutions + self._old_quad = now_quad + + if self.debug: + print(f"[ROSE] INV: world({x:.3f},{y:.3f},{z:.3f}) -> joints(R={radius:.3f},Z={z:.3f},Theta={big_theta:.3f})") + + return [radius, z, big_theta, 0.0, 0.0, 0.0, 0.0, 0.0, 0.0] + + +# ==================== 运动学工厂类 ==================== + +class KinematicsFactory: + """运动学工厂类，根据类型创建对应的运动学实例""" + + _default_params = { + KinematicsType.TRT_AC: { + 'rot_center_x': 0.0, 'rot_center_y': 0.0, 'rot_center_z': 0.0, + 'axis_offset_x': 0.0, 'axis_offset_y': 0.0, 'axis_offset_z': 0.0, + 'tool_length': 100.0, 'conventional_directions': False + }, + KinematicsType.TRT_BC: { + 'rot_center_x': 0.0, 'rot_center_y': 0.0, 'rot_center_z': 0.0, + 'axis_offset_x': 0.0, 'axis_offset_y': 0.0, 'axis_offset_z': 0.0, + 'tool_length': 100.0, 'conventional_directions': False + }, + KinematicsType.MAXKINS_BC: { + 'pivot_length': 250.0, 'tool_length': 100.0, 'conventional_directions': False + }, + KinematicsType.FIVEAXIS_BC: { + 'pivot_length': 250.0, 'tool_length': 100.0 + }, + KinematicsType.HEXAPOD: { + 'tool_offset': 100.0, 'spindle_offset': 0.0, + 'convergence_criterion': 1e-9, 'max_iterations': 120 + }, + KinematicsType.PUMA: { + 'a2': 300.0, 'a3': 50.0, 'd3': 70.0, 'd4': 400.0, 'd6': 70.0 + }, + KinematicsType.SCARA: { + 'd1': 490.0, 'd2': 340.0, 'd3': 50.0, + 'd4': 250.0, 'd5': 50.0, 'd6': 50.0 + }, + KinematicsType.SCORBOT: {}, + KinematicsType.LINEAR_DELTA: { + 'radius': 165.25, 'rod_length': 269.0 + }, + KinematicsType.ROTARY_DELTA: { + 'platform_radius': 10.0, 'thigh_length': 10.0, + 'shin_length': 14.0, 'foot_radius': 6.0 + }, + KinematicsType.TRIPOD: { + 'Bx': 1.0, 'Cx': 1.0, 'Cy': 1.0 + }, + KinematicsType.PENTAPOD: {}, + KinematicsType.ROTATE: {}, + KinematicsType.COREXY: {}, + KinematicsType.ROSE: {}, + } + + @classmethod + def create(cls, kin_type: KinematicsType, debug: bool = False, **params) -> Optional[BaseKinematics]: + if kin_type == KinematicsType.IDENTITY: + return None + + merged_params = cls._default_params.get(kin_type, {}).copy() + merged_params.update(params) + + if kin_type == KinematicsType.TRT_AC: + p = TRTParams(**{k: v for k, v in merged_params.items() if hasattr(TRTParams, k)}) + return XYZAC_TRTKinematics(p, debug=debug) + elif kin_type == KinematicsType.TRT_BC: + p = TRTParams(**{k: v for k, v in merged_params.items() if hasattr(TRTParams, k)}) + return XYZBC_TRTKinematics(p, debug=debug) + elif kin_type == KinematicsType.MAXKINS_BC: + return MaxkinsBCKinematics(debug=debug, **merged_params) + elif kin_type == KinematicsType.FIVEAXIS_BC: + return FiveAxisBCKinematics(debug=debug, **merged_params) + elif kin_type == KinematicsType.HEXAPOD: + p = HexapodParams(**{k: v for k, v in merged_params.items() if hasattr(HexapodParams, k)}) + return HexapodKinematics(p, debug=debug) + elif kin_type == KinematicsType.PUMA: + p = PUMAParams(**{k: v for k, v in merged_params.items() if hasattr(PUMAParams, k)}) + return PUMAKinematics(p, debug=debug) + elif kin_type == KinematicsType.SCARA: + p = SCARAParams(**{k: v for k, v in merged_params.items() if hasattr(SCARAParams, k)}) + return SCARAKinematics(p, debug=debug) + elif kin_type == KinematicsType.SCORBOT: + return ScorbotKinematics(debug=debug) + elif kin_type == KinematicsType.LINEAR_DELTA: + return LinearDeltaKinematics(debug=debug, **merged_params) + elif kin_type == KinematicsType.ROTARY_DELTA: + p = RotaryDeltaParams(**{k: v for k, v in merged_params.items() if hasattr(RotaryDeltaParams, k)}) + return RotaryDeltaKinematics(p, debug=debug) + elif kin_type == KinematicsType.TRIPOD: + p = TripodParams(**{k: v for k, v in merged_params.items() if hasattr(TripodParams, k)}) + return TripodKinematics(p, debug=debug) + elif kin_type == KinematicsType.PENTAPOD: + p = PentapodParams(**{k: v for k, v in merged_params.items() if hasattr(PentapodParams, k)}) + return PentapodKinematics(p, debug=debug) + elif kin_type == KinematicsType.ROTATE: + return RotateKinematics(debug=debug) + elif kin_type == KinematicsType.COREXY: + return CoreXYKinematics(debug=debug) + elif kin_type == KinematicsType.ROSE: + p = RoseParams() + return RoseKinematics(p, debug=debug) + elif kin_type == KinematicsType.SERIAL_DH: + p = SerialDHParams(**{k: v for k, v in merged_params.items() if hasattr(SerialDHParams, k)}) + return SerialDHKinematics(p, debug=debug) + + return None + + @classmethod + def get_supported_types(cls) -> List[KinematicsType]: + return list(cls._default_params.keys()) + + @classmethod + def get_type_info(cls, kin_type: KinematicsType) -> Dict[str, Any]: + info = { + KinematicsType.IDENTITY: {'name': 'IDENTITY', 'description': '三轴恒等运动学', 'joints': 3, 'source': 'trivkins.c'}, + KinematicsType.TRT_AC: {'name': 'TRT_AC', 'description': 'XYZAC 双转台五轴', 'joints': 5, 'source': 'trtfuncs.c'}, + KinematicsType.TRT_BC: {'name': 'TRT_BC', 'description': 'XYZBC 双转台五轴', 'joints': 5, 'source': 'trtfuncs.c'}, + KinematicsType.MAXKINS_BC: {'name': 'MAXKINS_BC', 'description': 'Max 五轴铣床', 'joints': 9, 'source': 'maxkins.c'}, + KinematicsType.FIVEAXIS_BC: {'name': 'FIVEAXIS_BC', 'description': '五轴桥式铣床', 'joints': 6, 'source': '5axiskins.c'}, + KinematicsType.HEXAPOD: {'name': 'HEXAPOD', 'description': 'Stewart 六足平台', 'joints': 6, 'source': 'genhexkins.c'}, + KinematicsType.PUMA: {'name': 'PUMA', 'description': 'PUMA 560 机器人', 'joints': 6, 'source': 'pumakins.c'}, + KinematicsType.SCARA: {'name': 'SCARA', 'description': 'SCARA 机器人', 'joints': 4, 'source': 'scarakins.c'}, + KinematicsType.SCORBOT: {'name': 'SCORBOT', 'description': 'Scorbot ER3', 'joints': 5, 'source': 'scorbot-kins.c'}, + KinematicsType.LINEAR_DELTA: {'name': 'LINEAR_DELTA', 'description': '线性Delta机器人', 'joints': 3, 'source': 'lineardeltakins.c'}, + KinematicsType.ROTARY_DELTA: {'name': 'ROTARY_DELTA', 'description': '旋转Delta机器人', 'joints': 3, 'source': 'rotarydeltakins.c'}, + KinematicsType.TRIPOD: {'name': 'TRIPOD', 'description': 'Tripod机器人', 'joints': 3, 'source': 'tripodkins.c'}, + KinematicsType.PENTAPOD: {'name': 'PENTAPOD', 'description': 'Pentapod机器人', 'joints': 5, 'source': 'pentakins.c'}, + KinematicsType.ROTATE: {'name': 'ROTATE', 'description': '旋转工作台', 'joints': 9, 'source': 'rotatekins.c'}, + KinematicsType.COREXY: {'name': 'COREXY', 'description': 'CoreXY运动学', 'joints': 9, 'source': 'corexykins.c'}, + KinematicsType.ROSE: {'name': 'ROSE', 'description': 'Rose Engine', 'joints': 3, 'source': 'rosekins.c'}, + } + return info.get(kin_type, {'name': 'UNKNOWN', 'description': '未知类型'}) + + +# ==================== 五轴运动学统一接口 (向后兼容) ==================== +@dataclass +class KinematicsParamsLegacy: + rot_center_x: float = 0.0 + rot_center_y: float = 0.0 + rot_center_z: float = 0.0 + axis_offset_x: float = 0.0 + axis_offset_y: float = 0.0 + axis_offset_z: float = 0.0 + pivot_length: float = 250.0 + tool_length: float = 0.0 + conventional_directions: bool = False + + +class FiveAxisKinematics: + def __init__(self, kin_type: KinematicsType = KinematicsType.TRT_BC, debug: bool = False): + self.kin_type = kin_type + self.debug = debug + self.rtcp_enabled = False + self.params = KinematicsParamsLegacy() + self._kinematics = KinematicsFactory.create(kin_type, debug=debug) + + def set_params(self, **kwargs): + for key, value in kwargs.items(): + if hasattr(self.params, key): + setattr(self.params, key, value) + if self._kinematics: + param_dict = { + 'rot_center_x': self.params.rot_center_x, + 'rot_center_y': self.params.rot_center_y, + 'rot_center_z': self.params.rot_center_z, + 'axis_offset_x': self.params.axis_offset_x, + 'axis_offset_y': self.params.axis_offset_y, + 'axis_offset_z': self.params.axis_offset_z, + 'tool_length': self.params.tool_length, + 'conventional_directions': self.params.conventional_directions, + 'pivot_length': self.params.pivot_length, + } + self._kinematics.set_params(**param_dict) + + def set_tool_length(self, length: float): + self.params.tool_length = length + if self._kinematics: + self._kinematics.set_tool_length(length) + + def enable_rtcp(self, enable: bool = True): + self.rtcp_enabled = enable + if self._kinematics: + self._kinematics.enable_rtcp(enable) + + def forward_transform(self, joints: List[float]) -> Tuple[float, float, float, float, float, float]: + if not self.rtcp_enabled or self._kinematics is None: + if len(joints) >= 6: + return (joints[0], joints[1], joints[2], joints[3], joints[4], joints[5]) + return (joints[0], joints[1], joints[2], 0, 0, 0) + return self._kinematics.forward(joints) + + def inverse_transform(self, pos: Tuple[float, float, float, float, float, float]) -> List[float]: + if not self.rtcp_enabled or self._kinematics is None: + return [pos[0], pos[1], pos[2], pos[3], pos[4], pos[5]] + return self._kinematics.inverse(pos) + + +# ==================== LineCodeWrapper ==================== +class LineCodeWrapper: + """行代码包装器""" + def __init__(self, seq_num: int, plane: int = 17): + self.sequence_number = seq_num + self.plane = plane + self.x = 0.0 + self.y = 0.0 + self.z = 0.0 + self.a = 0.0 + self.b = 0.0 + self.c = 0.0 + self.u = 0.0 + self.v = 0.0 + self.w = 0.0 + self.i = 0.0 + self.j = 0.0 + self.k = 0.0 + self.r = 0.0 + self.f = 0.0 + self.s = 0.0 + self.t = 0 + self.h = -1 + self.d = -1 + self.p = -1.0 + self.q = -1.0 + self.l = -1 + self.comment = "" + + +@dataclass +class CNCRuntimeStatus: + """CNC运行时状态""" + position_x: float = 0.0 + position_y: float = 0.0 + position_z: float = 0.0 + position_a: float = 0.0 + position_b: float = 0.0 + position_c: float = 0.0 + position_u: float = 0.0 + position_v: float = 0.0 + position_w: float = 0.0 + + world_x: float = 0.0 + world_y: float = 0.0 + world_z: float = 0.0 + + machine_x: float = 0.0 + machine_y: float = 0.0 + machine_z: float = 0.0 + machine_a: float = 0.0 + machine_b: float = 0.0 + machine_c: float = 0.0 + + spindle_speed: float = 0.0 + spindle_state: str = "OFF" + spindle_mode: int = 0 + spindle_override_enabled: bool = True + + coolant_flood: bool = False + coolant_mist: bool = False + coolant_state: str = "OFF" + + current_tool: int = 0 + selected_tool: int = 1 + tool_length: float = 0.0 + tool_diameter: float = 0.0 + tool_offset: Point6D = field(default_factory=Point6D) + + feedrate: float = 0.0 + feed_override: int = 100 + rapid_override: int = 100 + spindle_override: int = 100 + feed_override_enabled: bool = True + adaptive_feed_enabled: bool = False + feed_hold_enabled: bool = True + + state: str = MachineState.OFF.value + current_line: int = 0 + total_lines: int = 0 + progress_percent: float = 0.0 + + coordinate_mode: str = "G90" + plane_mode: str = "G17" + unit_mode: str = "G21" + cutter_comp: str = "G40" + current_offset: str = "G54" + distance_mode: int = 90 + feed_mode: int = 94 + ijk_distance_mode: int = 91 + retract_mode: int = 98 + control_mode: int = 610 + lathe_diameter_mode: bool = False + + alarm_code: int = 0 + alarm_message: str = "" + + rtcp_enabled: bool = False + kinematics_type: str = "TRT_BC" + + program_name: str = "" + path_length: float = 0.0 + total_time: float = 0.0 + path_segments: int = 0 + + g92_applied: bool = False + g92_x: float = 0.0 + g92_y: float = 0.0 + g92_z: float = 0.0 + g92_a: float = 0.0 # ★ 添加 + g92_b: float = 0.0 # ★ 添加 + g92_c: float = 0.0 # ★ 添加 + + g5x_index: int = 1 + g5x_offset_x: float = 0.0 + g5x_offset_y: float = 0.0 + g5x_offset_z: float = 0.0 + g5x_offset_a: float = 0.0 # ★ 添加 + g5x_offset_b: float = 0.0 # ★ 添加 + g5x_offset_c: float = 0.0 # ★ 添加 + + work_offset_x: float = 0.0 + work_offset_y: float = 0.0 + work_offset_z: float = 0.0 + + rotation_xy: float = 0.0 + + motion_mode: int = -1 + current_pocket: int = 0 + selected_pocket: int = 0 + + probe_tripped: bool = False + probe_x: float = 0.0 + probe_y: float = 0.0 + probe_z: float = 0.0 + + block_delete: bool = False + optional_stop: bool = True + + cycle_active: bool = False + cycle_type: int = 0 + cycle_r: float = 0.0 + cycle_z: float = 0.0 + cycle_q: float = 0.0 + cycle_l: int = 1 + + def to_dict(self) -> Dict[str, Any]: + """转换为字典""" + return { + 'position': {'x': self.position_x, 'y': self.position_y, 'z': self.position_z, + 'a': self.position_a, 'b': self.position_b, 'c': self.position_c}, + 'world_position': {'x': self.world_x, 'y': self.world_y, 'z': self.world_z}, + 'machine_position': {'x': self.machine_x, 'y': self.machine_y, 'z': self.machine_z}, + 'spindle': {'speed': self.spindle_speed, 'state': self.spindle_state}, + 'coolant': {'flood': self.coolant_flood, 'mist': self.coolant_mist}, + 'tool': {'current': self.current_tool, 'selected': self.selected_tool, + 'length': self.tool_length, 'diameter': self.tool_diameter}, + 'feedrate': self.feedrate, + 'overrides': {'feed': self.feed_override, 'rapid': self.rapid_override, + 'spindle': self.spindle_override}, + 'state': self.state, + 'line': self.current_line, + 'progress': self.progress_percent, + 'modes': {'coordinate': self.coordinate_mode, 'plane': self.plane_mode, + 'units': self.unit_mode, 'cutter_comp': self.cutter_comp, + 'offset': self.current_offset}, + 'rtcp_enabled': self.rtcp_enabled, + 'kinematics_type': self.kinematics_type, + 'program_name': self.program_name, + } + + +# ==================== Translated 类（处理坐标系变换） ==================== +class Translated: + """处理 G5x 和 G92 坐标系偏移以及 XY 旋转""" + + def __init__(self): + self.g92_offset_x = 0.0 + self.g92_offset_y = 0.0 + self.g92_offset_z = 0.0 + self.g92_offset_a = 0.0 + self.g92_offset_b = 0.0 + self.g92_offset_c = 0.0 + self.g92_offset_u = 0.0 + self.g92_offset_v = 0.0 + self.g92_offset_w = 0.0 + + self.g5x_offset_x = 0.0 + self.g5x_offset_y = 0.0 + self.g5x_offset_z = 0.0 + self.g5x_offset_a = 0.0 + self.g5x_offset_b = 0.0 + self.g5x_offset_c = 0.0 + self.g5x_offset_u = 0.0 + self.g5x_offset_v = 0.0 + self.g5x_offset_w = 0.0 + + self.g52_offset_x = 0.0 + self.g52_offset_y = 0.0 + self.g52_offset_z = 0.0 + self.g52_offset_a = 0.0 + self.g52_offset_b = 0.0 + self.g52_offset_c = 0.0 + self.g52_offset_u = 0.0 + self.g52_offset_v = 0.0 + self.g52_offset_w = 0.0 + + self.rotation_xy = 0.0 + self.rotation_sin = 0.0 + self.rotation_cos = 1.0 + + self.g5x_index = 1 + + self._g53_active = False + + def rotate_and_translate(self, x: float, y: float, z: float, + a: float, b: float, c: float, + u: float, v: float, w: float) -> Tuple[float, ...]: + """应用 G5x、G92、G52 偏移和 XY 旋转""" + if self._g53_active: + # G53 模式下不应用任何偏移 + return (x, y, z, a, b, c, u, v, w) + + # 应用 G92 偏移 + x += self.g92_offset_x + y += self.g92_offset_y + z += self.g92_offset_z + a += self.g92_offset_a + b += self.g92_offset_b + c += self.g92_offset_c + u += self.g92_offset_u + v += self.g92_offset_v + w += self.g92_offset_w + + # 应用 G52 偏移 + x += self.g52_offset_x + y += self.g52_offset_y + z += self.g52_offset_z + a += self.g52_offset_a + b += self.g52_offset_b + c += self.g52_offset_c + + # XY 旋转 + if self.rotation_xy != 0: + rotx = x * self.rotation_cos - y * self.rotation_sin + roty = x * self.rotation_sin + y * self.rotation_cos + x, y = rotx, roty + + # 应用 G5x 偏移 + x += self.g5x_offset_x + y += self.g5x_offset_y + z += self.g5x_offset_z + a += self.g5x_offset_a + b += self.g5x_offset_b + c += self.g5x_offset_c + u += self.g5x_offset_u + v += self.g5x_offset_v + w += self.g5x_offset_w + + return (x, y, z, a, b, c, u, v, w) + + def reverse_rotate_and_translate(self, x: float, y: float, z: float, + a: float, b: float, c: float, + u: float, v: float, w: float) -> Tuple[float, ...]: + """反向应用偏移（用于从机器坐标计算工件坐标）""" + if self._g53_active: + return (x, y, z, a, b, c, u, v, w) + + # 减去 G5x 偏移 + x -= self.g5x_offset_x + y -= self.g5x_offset_y + z -= self.g5x_offset_z + a -= self.g5x_offset_a + b -= self.g5x_offset_b + c -= self.g5x_offset_c + u -= self.g5x_offset_u + v -= self.g5x_offset_v + w -= self.g5x_offset_w + + # 反向 XY 旋转 + if self.rotation_xy != 0: + rotx = x * self.rotation_cos + y * self.rotation_sin + roty = -x * self.rotation_sin + y * self.rotation_cos + x, y = rotx, roty + + # 减去 G52 偏移 + x -= self.g52_offset_x + y -= self.g52_offset_y + z -= self.g52_offset_z + a -= self.g52_offset_a + b -= self.g52_offset_b + c -= self.g52_offset_c + + # 减去 G92 偏移 + x -= self.g92_offset_x + y -= self.g92_offset_y + z -= self.g92_offset_z + a -= self.g92_offset_a + b -= self.g92_offset_b + c -= self.g92_offset_c + u -= self.g92_offset_u + v -= self.g92_offset_v + w -= self.g92_offset_w + + return (x, y, z, a, b, c, u, v, w) + + def set_g5x_offset(self, csys: int, x: float = None, y: float = None, z: float = None, + a: float = 0.0, b: float = 0.0, c: float = 0.0, + u: float = 0.0, v: float = 0.0, w: float = 0.0): + """ + G10 L2: 直接设置坐标系偏移 + + 完全参照 LinuxCNC 官方实现： + rs274ngc.cpp: Interp::convert_setup (G10 L2 分支) + + Args: + csys: 坐标系编号 (1-9) + x, y, z, a, b, c, u, v, w: 新的偏移值（程序单位） + """ + if csys < 1 or csys > 9: + if self.debug: + print(f"[G10 L2] 错误: 坐标系编号 {csys} 超出范围 (1-9)") + return + + if csys not in self.work_offsets: + self.work_offsets[csys] = {} + + # 获取旧偏移 + old_offsets = self.work_offsets[csys].copy() + old_g5x_x = old_offsets.get('X', 0.0) + old_g5x_y = old_offsets.get('Y', 0.0) + old_g5x_z = old_offsets.get('Z', 0.0) + old_g5x_a = old_offsets.get('A', 0.0) + old_g5x_b = old_offsets.get('B', 0.0) + old_g5x_c = old_offsets.get('C', 0.0) + + # 使用新值或保留旧值 + new_x = x if x is not None else old_g5x_x + new_y = y if y is not None else old_g5x_y + new_z = z if z is not None else old_g5x_z + new_a = a if a is not None else old_g5x_a + new_b = b if b is not None else old_g5x_b + new_c = c if c is not None else old_g5x_c + new_u = u if u is not None else old_offsets.get('U', 0.0) + new_v = v if v is not None else old_offsets.get('V', 0.0) + new_w = w if w is not None else old_offsets.get('W', 0.0) + new_r = old_offsets.get('R', 0.0) # G10 L2 不改变旋转角度 + + if self.debug: + print(f"[G10 L2] ========== 设置坐标系 G{53 + csys} ==========") + print(f"[G10 L2] 旧偏移: X={old_g5x_x:.3f}, Y={old_g5x_y:.3f}, Z={old_g5x_z:.3f}") + print(f"[G10 L2] 新偏移: X={new_x:.3f}, Y={new_y:.3f}, Z={new_z:.3f}") + print(f"[G10 L2] 是否为当前坐标系: {csys == self.g5x_index}") + + # ===== 如果是当前坐标系，需要调整当前位置 ===== + if csys == self.g5x_index: + # 第1步：正向旋转当前坐标（去掉旧 XY 旋转） + if self.rotation_xy != 0: + rot_rad = math.radians(self.rotation_xy) + cos_r = math.cos(rot_rad) + sin_r = math.sin(rot_rad) + rot_x = self.current_pos.x * cos_r - self.current_pos.y * sin_r + rot_y = self.current_pos.x * sin_r + self.current_pos.y * cos_r + self.current_pos.x = rot_x + self.current_pos.y = rot_y + + # 第2步：加上旧 G5x 偏移 + self.current_pos.x += old_g5x_x + self.current_pos.y += old_g5x_y + self.current_pos.z += old_g5x_z + self.current_pos.a += old_g5x_a + self.current_pos.b += old_g5x_b + self.current_pos.c += old_g5x_c + + # 第3步：设置新 G5x 偏移 + self.g5x_offset_x = new_x + self.g5x_offset_y = new_y + self.g5x_offset_z = new_z + self.g5x_offset_a = new_a + self.g5x_offset_b = new_b + self.g5x_offset_c = new_c + + # 第4步：减去新 G5x 偏移 + self.current_pos.x -= new_x + self.current_pos.y -= new_y + self.current_pos.z -= new_z + self.current_pos.a -= new_a + self.current_pos.b -= new_b + self.current_pos.c -= new_c + + if self.debug: + print(f"[G10 L2] 调整后工件坐标: ({self.current_pos.x:.3f}, {self.current_pos.y:.3f}, {self.current_pos.z:.3f})") + + # ===== 存储新偏移 ===== + self.work_offsets[csys] = { + 'X': new_x, 'Y': new_y, 'Z': new_z, + 'A': new_a, 'B': new_b, 'C': new_c, + 'U': new_u, 'V': new_v, 'W': new_w, + 'R': new_r + } + + # ===== 同步到参数表和机器坐标 ===== + if hasattr(self, 'var_manager') and self.var_manager: + self.var_manager.set_csys_origin(csys, new_x, new_y, new_z, new_a, new_b, new_c) + + self._sync_machine_from_current() + self._sync_parameters_from_state() + + if self.debug: + print(f"[G10 L2] ========== G10 L2 完成 ==========") + + def set_g92_offset(self, x: float = 0.0, y: float = 0.0, z: float = 0.0, + a: float = 0.0, b: float = 0.0, c: float = 0.0, + u: float = 0.0, v: float = 0.0, w: float = 0.0): + """G92: 设置临时坐标系偏移""" + self.g92_offset_x = x + self.g92_offset_y = y + self.g92_offset_z = z + self.g92_offset_a = a + self.g92_offset_b = b + self.g92_offset_c = c + self.g92_offset_u = u + self.g92_offset_v = v + self.g92_offset_w = w + + def apply_g92_from_current(self, target_x: float = None, target_y: float = None, + target_z: float = None, current_x: float = 0.0, + current_y: float = 0.0, current_z: float = 0.0): + """根据当前坐标计算并应用 G92 偏移""" + if target_x is not None: + self.g92_offset_x = current_x + self.g92_offset_x - target_x + if target_y is not None: + self.g92_offset_y = current_y + self.g92_offset_y - target_y + if target_z is not None: + self.g92_offset_z = current_z + self.g92_offset_z - target_z + + def clear_g92_offset(self): + """G92.1: 清除 G92 偏移""" + self.g92_offset_x = 0.0 + self.g92_offset_y = 0.0 + self.g92_offset_z = 0.0 + self.g92_offset_a = 0.0 + self.g92_offset_b = 0.0 + self.g92_offset_c = 0.0 + self.g92_offset_u = 0.0 + self.g92_offset_v = 0.0 + self.g92_offset_w = 0.0 + + def set_g52_offset(self, x: float = 0.0, y: float = 0.0, z: float = 0.0, + a: float = 0.0, b: float = 0.0, c: float = 0.0): + """G52: 设置临时工件坐标系偏移""" + self.g52_offset_x = x + self.g52_offset_y = y + self.g52_offset_z = z + self.g52_offset_a = a + self.g52_offset_b = b + self.g52_offset_c = c + + def clear_g52_offset(self): + """清除 G52 偏移""" + self.g52_offset_x = 0.0 + self.g52_offset_y = 0.0 + self.g52_offset_z = 0.0 + self.g52_offset_a = 0.0 + self.g52_offset_b = 0.0 + self.g52_offset_c = 0.0 + + def set_xy_rotation(self, theta: float): + """设置 XY 旋转角度""" + self.rotation_xy = theta + t = math.radians(theta) + self.rotation_sin = math.sin(t) + self.rotation_cos = math.cos(t) + + # ========== 新增：同步到参数表 ========== + # 如果有 var_manager 属性则同步 + if hasattr(self, 'var_manager') and self.var_manager is not None: + self.var_manager.set_xy_rotation(theta) + + def set_g53_active(self, active: bool = True): + """设置 G53 模式""" + self._g53_active = active + + +# ==================== ArcsToSegmentsMixin 类 ==================== +class ArcsToSegmentsMixin: + """圆弧转线段的混入类""" + + plane = 17 + arcdivision = 64 + + def set_plane(self, plane: int): + self.plane = plane + + def set_arc_division(self, division: int): + self.arcdivision = division + + def arc_feed_to_segments(self, x1: float, y1: float, cx: float, cy: float, + rot: int, z1: float, a: float, b: float, c: float, + u: float, v: float, w: float): + """将圆弧转换为线段""" + segs = arcs_to_segments_python( + self, x1, y1, cx, cy, rot, z1, a, b, c, u, v, w, self.arcdivision + ) + self.straight_arcsegments(segs) + + def straight_arcsegments(self, segs: List[Tuple[float, ...]]): + """处理圆弧线段（由子类实现）""" + raise NotImplementedError + + +# ==================== 纯 Python GLCanon 类 ==================== +class GLCanonPure(Translated, ArcsToSegmentsMixin): + """纯 Python 实现的 GLCanon 类""" + + def __init__(self, colors=None, geometry="XYZ", is_foam=0): + super().__init__() + + if colors is None: + colors = DEFAULT_COLORS.copy() + + self.colors = colors + self.geometry = geometry + self.is_foam = is_foam + + self.traverse: List[Tuple] = [] + self.feed: List[Tuple] = [] + self.arcfeed: List[Tuple] = [] + self.dwells: List[Tuple] = [] + self.tool_list: List[int] = [] + + self.preview_zero_rxy: List[Tuple] = [] + + self.lineno = -1 + self.first_move = True + self.feedrate = 1.0 + self.lo = [0.0] * 9 + self.xo = self.yo = self.zo = self.ao = self.bo = self.co = self.uo = self.vo = self.wo = 0.0 + + self.min_extents = [9e99, 9e99, 9e99] + self.max_extents = [-9e99, -9e99, -9e99] + self.min_extents_notool = [9e99, 9e99, 9e99] + self.max_extents_notool = [-9e99, -9e99, -9e99] + self.min_extents_zero_rxy = [9e99, 9e99, 9e99] + self.max_extents_zero_rxy = [-9e99, -9e99, -9e99] + self.min_extents_notool_zero_rxy = [9e99, 9e99, 9e99] + self.max_extents_notool_zero_rxy = [-9e99, -9e99, -9e99] + + self.foam_z = 0.0 + self.foam_w = 1.5 + + self.in_arc = 0 + self.suppress = 0 + self.dwell_time = 0.0 + self.notify = 0 + self.notify_message = "" + self.highlight_line = None + + self.state = type('State', (), {})() + self.state.plane = 17 + self.state.feedrate = 1.0 + self.state.spindle_speed = 0.0 + self.state.spindle_mode = 0 + self.state.tool = 0 + self.state.units = 21 + + self.g92_offset_u = 0.0 + self.g92_offset_v = 0.0 + self.g92_offset_w = 0.0 + + self.g5x_offset_u = 0.0 + self.g5x_offset_v = 0.0 + self.g5x_offset_w = 0.0 + + def comment(self, arg: str): + """处理注释""" + if arg.startswith("AXIS,") or arg.startswith("PREVIEW,"): + parts = arg.split(",") + command = parts[1] if len(parts) > 1 else "" + if command == "stop": + raise KeyboardInterrupt("Preview stop requested") + elif command == "hide": + self.suppress += 1 + elif command == "show": + self.suppress -= 1 + elif command == "XY_Z_POS" and len(parts) > 2: + try: + self.foam_z = float(parts[2]) + except ValueError: + pass + elif command == "UV_Z_POS" and len(parts) > 2: + try: + self.foam_w = float(parts[2]) + except ValueError: + pass + elif command == "notify": + self.notify += 1 + self.notify_message = "(AXIS,notify):" + str(self.notify) + if len(parts) > 2 and parts[2]: + self.notify_message = parts[2] + + def message(self, message: str): + """处理消息""" + pass + + def check_abort(self): + """检查是否中止""" + pass + + def next_line(self, st: LineCodeWrapper): + """处理下一行""" + self.state = st + self.lineno = getattr(st, 'sequence_number', -1) + + def calc_extents(self): + """计算包围盒""" + if not self.arcfeed and not self.feed and not self.traverse: + self.min_extents = self.max_extents = [0, 0, 0] + self.min_extents_notool = self.max_extents_notool = [0, 0, 0] + self.min_extents_zero_rxy = self.max_extents_zero_rxy = [0, 0, 0] + self.min_extents_notool_zero_rxy = self.max_extents_notool_zero_rxy = [0, 0, 0] + return + + (min_p, max_p, min_pt, max_pt) = calc_extents_python(self.arcfeed, self.feed, self.traverse) + + self.min_extents = min_p + self.max_extents = max_p + self.min_extents_notool = min_pt + self.max_extents_notool = max_pt + + self.unrotate_preview() + (min_pz, max_pz, min_ptz, max_ptz) = calc_extents_python(self.preview_zero_rxy) + + self.min_extents_zero_rxy = min_pz + self.max_extents_zero_rxy = max_pz + self.min_extents_notool_zero_rxy = min_ptz + self.max_extents_notool_zero_rxy = max_ptz + + if self.is_foam: + min_z = min(self.foam_z, self.foam_w) + max_z = max(self.foam_z, self.foam_w) + self.min_extents[2] = min_z + self.max_extents[2] = max_z + self.min_extents_notool[2] = min_z + self.max_extents_notool[2] = max_z + + def unrotate_preview(self): + """反向旋转预览数据""" + angle = math.radians(-self.rotation_xy) + cos = math.cos(angle) + sin = math.sin(angle) + g5x_x = self.g5x_offset_x + g5x_y = self.g5x_offset_y + + self.preview_zero_rxy = [] + + for lst in [self.feed, self.arcfeed]: + for item in lst: + if len(item) == 5: + linenum, start, end, feed, tooloffset = item + else: + continue + + tsx = start[0] - g5x_x + tsy = start[1] - g5x_y + tex = end[0] - g5x_x + tey = end[1] - g5x_y + + rsx = tsx * cos - tsy * sin + g5x_x + rsy = tsx * sin + tsy * cos + g5x_y + rex = tex * cos - tey * sin + g5x_x + rey = tex * sin + tey * cos + g5x_y + + self.preview_zero_rxy.append(( + linenum, (rsx, rsy) + start[2:], (rex, rey) + end[2:], feed, tooloffset + )) + + for item in self.traverse: + if len(item) == 4: + linenum, start, end, tooloffset = item + else: + continue + + tsx = start[0] - g5x_x + tsy = start[1] - g5x_y + tex = end[0] - g5x_x + tey = end[1] - g5x_y + + rsx = tsx * cos - tsy * sin + g5x_x + rsy = tsx * sin + tsy * cos + g5x_y + rex = tex * cos - tey * sin + g5x_x + rey = tex * sin + tey * cos + g5x_y + + self.preview_zero_rxy.append(( + linenum, (rsx, rsy) + start[2:], (rex, rey) + end[2:], tooloffset + )) + + def tool_offset(self, xo: float, yo: float, zo: float, + ao: float, bo: float, co: float, + uo: float, vo: float, wo: float): + """设置刀具偏移""" + self.first_move = True + x, y, z, a, b, c, u, v, w = self.lo + self.lo = ( + x - xo + self.xo, y - yo + self.yo, z - zo + self.zo, + a - ao + self.ao, b - bo + self.bo, c - co + self.co, + u - uo + self.uo, v - vo + self.vo, w - wo + self.wo + ) + self.xo, self.yo, self.zo = xo, yo, zo + self.ao, self.bo, self.co = ao, bo, co + self.uo, self.vo, self.wo = uo, vo, wo + + def set_spindle_rate(self, arg: float): + """设置主轴转速""" + pass + + def set_feed_rate(self, arg: float): + """设置进给率""" + self.feedrate = arg / 60.0 + + def select_plane(self, plane: int): + """选择平面""" + pass + + def change_tool(self, arg: int): + """换刀""" + self.first_move = True + try: + self.tool_list.append(arg) + except Exception as e: + print(f"Error in change_tool: {e}") + + def straight_traverse(self, x: float, y: float, z: float, + a: float, b: float, c: float, + u: float, v: float, w: float): + """快速移动""" + if self.suppress > 0: + return + l = self.rotate_and_translate(x, y, z, a, b, c, u, v, w) + if not self.first_move: + self.traverse.append((self.lineno, self.lo, l, (self.xo, self.yo, self.zo))) + self.lo = l + self.first_move = False + + def straight_feed(self, x: float, y: float, z: float, + a: float, b: float, c: float, + u: float, v: float, w: float): + """直线进给""" + if self.suppress > 0: + return + self.first_move = False + l = self.rotate_and_translate(x, y, z, a, b, c, u, v, w) + self.feed.append((self.lineno, self.lo, l, self.feedrate, (self.xo, self.yo, self.zo))) + self.lo = l + + straight_probe = straight_feed + + def arc_feed(self, x: float, y: float, z: float, + a: float, b: float, c: float, + i: float, j: float, k: float, turn: int, feed: float, + u: float = 0.0, v: float = 0.0, w: float = 0.0): + """圆弧进给""" + if self.suppress > 0: + return + self.first_move = False + self.in_arc = True + try: + self.arc_feed_to_segments(x, y, i, j, turn, z, a, b, c, u, v, w) + finally: + self.in_arc = False + + def straight_arcsegments(self, segs: List[Tuple[float, ...]]): + """处理圆弧线段""" + self.first_move = False + lo = self.lo + lineno = self.lineno + feedrate = self.feedrate + to = (self.xo, self.yo, self.zo) + + for l in segs: + self.arcfeed.append((lineno, lo, l, feedrate, to)) + lo = l + self.lo = lo + + def rigid_tap(self, x: float, y: float, z: float): + """刚性攻丝""" + if self.suppress > 0: + return + self.first_move = False + l = self.rotate_and_translate(x, y, z, 0, 0, 0, 0, 0, 0)[:3] + l += (self.lo[3], self.lo[4], self.lo[5], self.lo[6], self.lo[7], self.lo[8]) + self.feed.append((self.lineno, self.lo, l, self.feedrate, (self.xo, self.yo, self.zo))) + self.feed.append((self.lineno, l, self.lo, self.feedrate, (self.xo, self.yo, self.zo))) + + def user_defined_function(self, i: int, p: float, q: float): + """用户自定义函数""" + if self.suppress > 0: + return + color = self.colors.get('m1xx', '#ff8000') + plane = int(getattr(self.state, 'plane', 17) / 10 - 17) + self.dwells.append((self.lineno, color, self.lo[0], self.lo[1], self.lo[2], plane)) + + def dwell(self, arg: float): + """暂停""" + if self.suppress > 0: + return + self.dwell_time += arg + color = self.colors.get('dwell', '#ffffff') + plane = int(getattr(self.state, 'plane', 17) / 10 - 17) + self.dwells.append((self.lineno, color, self.lo[0], self.lo[1], self.lo[2], plane)) + + def get_extents(self) -> Dict: + """获取包围盒""" + self.calc_extents() + return { + 'min': self.min_extents, + 'max': self.max_extents, + 'min_notool': self.min_extents_notool, + 'max_notool': self.max_extents_notool, + 'min_zero_rxy': self.min_extents_zero_rxy, + 'max_zero_rxy': self.max_extents_zero_rxy, + } + + def get_traverse(self) -> List[Tuple]: + return self.traverse + + def get_feed(self) -> List[Tuple]: + return self.feed + + def get_arcfeed(self) -> List[Tuple]: + return self.arcfeed + + def get_dwells(self) -> List[Tuple]: + return self.dwells + + +# ==================== 梯形速度曲线运动规划器 ==================== + +# ==================== 梯形速度曲线运动规划器（完整版）==================== + +class MotionProfile: + """速度曲线规划结果""" + def __init__(self): + self.duration: float = 0.0 # 总时间 (s) + self.v_max: float = 0.0 # 最大速度 (mm/s) + self.t_acc: float = 0.0 # 加速时间 (s) + self.t_const: float = 0.0 # 匀速时间 (s) + self.t_dec: float = 0.0 # 减速时间 (s) + self.s_acc: float = 0.0 # 加速距离 (mm) + self.s_const: float = 0.0 # 匀速距离 (mm) + self.s_dec: float = 0.0 # 减速距离 (mm) + self.total_distance: float = 0.0 # 总距离 (mm) + self.profile_type: str = "zero" # zero / trapezoidal / triangular + + +class TrapezoidalMotionPlanner: + """梯形速度曲线运动规划器 - 支持实时位置查询""" + + def __init__(self, + acceleration: float = 500.0, + deceleration: float = 500.0, + max_rapid_rate: float = 10000.0, + max_feed_rate: float = 5000.0, + rapid_override: float = 1.0, + feed_override: float = 1.0): + + self.acc = acceleration # mm/s² + self.dec = deceleration # mm/s² + self.max_rapid = max_rapid_rate / 60.0 # mm/s + self.max_feed = max_feed_rate / 60.0 + self.rapid_override = rapid_override + self.feed_override = feed_override + self._jerk = 10000.0 # 加加速度 (mm/s³) - 预留 + + def plan_motion(self, distance: float, commanded_feedrate: float = None, + is_rapid: bool = False) -> MotionProfile: + """ + 规划运动速度曲线 + + 参数: + distance: 运动距离 (mm) + commanded_feedrate: 编程进给率 (mm/min) + is_rapid: 是否为G0快速移动 + + 返回: MotionProfile 包含完整速度曲线参数 + """ + profile = MotionProfile() + profile.total_distance = distance + + if distance < CART_FUZZ: + profile.profile_type = "zero" + return profile + + # 确定目标速度 (mm/s) + if is_rapid: + v_target = self.max_rapid * self.rapid_override + else: + if commanded_feedrate is not None and commanded_feedrate > 0: + v_target = (commanded_feedrate / 60.0) * self.feed_override + else: + v_target = self.max_feed * self.feed_override + + max_allowed = self.max_rapid if is_rapid else self.max_feed + v_target = min(v_target, max_allowed) + v_target = max(v_target, 0.001) + + # 计算加速距离和减速距离 + s_acc = (v_target * v_target) / (2.0 * self.acc) + s_dec = (v_target * v_target) / (2.0 * self.dec) + s_cruise = distance - s_acc - s_dec + + if s_cruise >= 0: + # ===== 梯形速度曲线 ===== + profile.profile_type = "trapezoidal" + profile.t_acc = v_target / self.acc + profile.t_dec = v_target / self.dec + profile.t_const = s_cruise / v_target + profile.v_max = v_target + profile.s_acc = s_acc + profile.s_const = s_cruise + profile.s_dec = s_dec + profile.duration = profile.t_acc + profile.t_const + profile.t_dec + else: + # ===== 三角形速度曲线 ===== + # v_peak = sqrt(2 * s * a1 * a2 / (a1 + a2)) + profile.profile_type = "triangular" + v_peak = math.sqrt( + 2.0 * distance * self.acc * self.dec / (self.acc + self.dec) + ) + profile.v_max = v_peak + profile.t_acc = v_peak / self.acc + profile.t_dec = v_peak / self.dec + profile.t_const = 0.0 + profile.s_acc = 0.5 * self.acc * profile.t_acc * profile.t_acc + profile.s_const = 0.0 + profile.s_dec = 0.5 * self.dec * profile.t_dec * profile.t_dec + profile.duration = profile.t_acc + profile.t_dec + + return profile + + def get_position_at_time(self, elapsed: float, profile: MotionProfile) -> float: + """ + 根据速度曲线获取当前位置 (mm) + + 参数: + elapsed: 已用时间 (s) + profile: 速度曲线 + + 返回: 当前位置 (mm) + """ + if elapsed <= 0: + return 0.0 + if elapsed >= profile.duration: + return profile.total_distance + + if profile.profile_type == "zero": + return 0.0 + + elif profile.profile_type == "trapezoidal": + if elapsed <= profile.t_acc: + # 加速段: s = 1/2 * a * t² + return 0.5 * self.acc * elapsed * elapsed + elif elapsed <= profile.t_acc + profile.t_const: + # 匀速段: s = s_acc + v * (t - t_acc) + return profile.s_acc + profile.v_max * (elapsed - profile.t_acc) + else: + # 减速段: s = s_acc + s_const + v*(Δt) - 1/2*dec*(Δt)² + dt = elapsed - profile.t_acc - profile.t_const + s = (profile.s_acc + profile.s_const + + profile.v_max * dt - 0.5 * self.dec * dt * dt) + return min(s, profile.total_distance) + + elif profile.profile_type == "triangular": + if elapsed <= profile.t_acc: + return 0.5 * self.acc * elapsed * elapsed + else: + dt = elapsed - profile.t_acc + s = (profile.s_acc + + profile.v_max * dt - 0.5 * self.dec * dt * dt) + return min(s, profile.total_distance) + + return 0.0 + + def get_position_ratio(self, elapsed: float, profile: MotionProfile) -> float: + """获取当前位置比例 (0.0 ~ 1.0)""" + if profile.total_distance < CART_FUZZ: + return 1.0 + pos = self.get_position_at_time(elapsed, profile) + return min(pos / profile.total_distance, 1.0) + + def get_velocity_at_time(self, elapsed: float, profile: MotionProfile) -> float: + """获取当前速度 (mm/s)""" + if elapsed <= 0 or elapsed >= profile.duration: + return 0.0 + + if profile.profile_type == "zero": + return 0.0 + + elif profile.profile_type == "trapezoidal": + if elapsed <= profile.t_acc: + return self.acc * elapsed + elif elapsed <= profile.t_acc + profile.t_const: + return profile.v_max + else: + dt = elapsed - profile.t_acc - profile.t_const + return max(0.0, profile.v_max - self.dec * dt) + + elif profile.profile_type == "triangular": + if elapsed <= profile.t_acc: + return self.acc * elapsed + else: + dt = elapsed - profile.t_acc + return max(0.0, profile.v_max - self.dec * dt) + + return 0.0 + + def calculate_time(self, distance: float, commanded_feedrate: float = None, + is_rapid: bool = False) -> Dict[str, float]: + """向后兼容的 calculate_time 方法""" + profile = self.plan_motion(distance, commanded_feedrate, is_rapid) + return { + 'duration': profile.duration, + 'max_velocity': profile.v_max, + 'acceleration_time': profile.t_acc, + 'constant_time': profile.t_const, + 'deceleration_time': profile.t_dec, + 'profile_type': profile.profile_type + } + + def get_position_at_time_legacy(self, distance: float, duration: float, + commanded_feedrate: float = None, + is_rapid: bool = False) -> Callable[[float], float]: + """向后兼容的旧接口""" + profile = self.plan_motion(distance, commanded_feedrate, is_rapid) + + def pos_func(t: float) -> float: + return self.get_position_at_time(t, profile) + + return pos_func + + def get_velocity_at_time_legacy(self, distance: float, duration: float, + commanded_feedrate: float = None, + is_rapid: bool = False) -> Callable[[float], float]: + """向后兼容的旧接口""" + profile = self.plan_motion(distance, commanded_feedrate, is_rapid) + + def vel_func(t: float) -> float: + return self.get_velocity_at_time(t, profile) + + return vel_func + + def get_profile_summary(self, profile: MotionProfile) -> str: + """获取速度曲线摘要""" + if profile.profile_type == "zero": + return "静止" + + v_max_mm_min = profile.v_max * 60.0 + parts = [] + if profile.t_acc > 0.001: + parts.append(f"加速{profile.t_acc*1000:.0f}ms({profile.s_acc:.1f}mm)") + if profile.t_const > 0.001: + parts.append(f"匀速{profile.t_const*1000:.0f}ms({profile.s_const:.1f}mm)") + if profile.t_dec > 0.001: + parts.append(f"减速{profile.t_dec*1000:.0f}ms({profile.s_dec:.1f}mm)") + + return (f"Vmax={v_max_mm_min:.0f}mm/min " + f"耗时{profile.duration*1000:.0f}ms [" + " → ".join(parts) + "]") + + def set_feed_override(self, value: float): + """设置进给倍率 (0.0 ~ 2.0)""" + self.feed_override = max(0.0, min(2.0, value)) + + def set_rapid_override(self, value: float): + """设置快速倍率 (0.0 ~ 1.0)""" + self.rapid_override = max(0.0, min(1.0, value)) + + def set_acceleration(self, value: float): + """设置加速度""" + self.acc = max(1.0, value) + + def set_deceleration(self, value: float): + """设置减速度""" + self.dec = max(1.0, value) + +# ==================== 控制结构处理器 ==================== + +class ControlStructureHandler: + """O代码控制结构处理器""" + + def __init__(self, var_manager: 'lcnc_param.LinuxCNCParameterTable', debug: bool = False): + self.var_manager = var_manager + self.debug = debug + + self.while_stack: List[Dict] = [] + self.repeat_stack: List[Dict] = [] + self.if_stack: List[Dict] = [] + self.subroutine_stack: List[Dict] = [] + + self.jump_targets: Dict[str, int] = {} + + self._skip_mode = False + self._skip_depth = 0 + self._skip_target = None + + def evaluate_condition(self, condition: str, local_level: int = 0) -> bool: + """评估条件表达式""" + try: + expanded = self.var_manager.expand_variables(condition, local_level) + expanded = re.sub(r'(?i)EQ', '==', expanded) + expanded = re.sub(r'(?i)NE', '!=', expanded) + expanded = re.sub(r'(?i)GT', '>', expanded) + expanded = re.sub(r'(?i)GE', '>=', expanded) + expanded = re.sub(r'(?i)LT', '<', expanded) + expanded = re.sub(r'(?i)LE', '<=', expanded) + expanded = re.sub(r'(?i)XOR', '!=', expanded) + expanded = re.sub(r'(?i)AND', 'and', expanded) + expanded = re.sub(r'(?i)OR', 'or', expanded) + expanded = re.sub(r'(?i)NOT', 'not', expanded) + result = eval(expanded, {"__builtins__": {}}, {"math": math}) + return bool(result) + except Exception as e: + if self.debug: + print(f" [DEBUG] 条件评估失败: {condition}, 错误: {e}") + return False + + def parse_o_code(self, line: str, line_num: int, current_line_idx: int, + lines: List[str], local_level: int = 0) -> Optional[int]: + """解析O代码控制结构""" + line_upper = line.upper().strip() + + # 如果在跳过模式 + if self._skip_mode: + if re.match(r'O<[^>]+>\s+IF\s+\[', line_upper, re.I): + self._skip_depth += 1 + elif re.match(r'O<[^>]+>\s+ENDIF', line_upper, re.I): + self._skip_depth -= 1 + if self._skip_depth == 0: + self._skip_mode = False + self._skip_target = None + elif re.match(r'O<[^>]+>\s+ELSEIF', line_upper, re.I) and self._skip_depth == 1: + # 在跳过模式中遇到 ELSEIF，检查条件 + if_match = re.match(r'O<([^>]+)>\s+ELSEIF\s+\[(.+)\]', line_upper, re.I) + if if_match: + condition = if_match.group(2) + if self.evaluate_condition(condition, local_level): + self._skip_mode = False + self._skip_depth = 0 + elif re.match(r'O<[^>]+>\s+ELSE', line_upper, re.I) and self._skip_depth == 1: + # 在跳过模式中遇到 ELSE，停止跳过 + self._skip_mode = False + self._skip_depth = 0 + return None + + # SUB + if re.match(r'O<[^>]+>\s+SUB', line_upper, re.I): + return None + + # ENDSUB + if re.match(r'O<[^>]+>\s+ENDSUB', line_upper, re.I): + if self.subroutine_stack: + call_info = self.subroutine_stack.pop() + return call_info['return_line'] + return None + + # CALL + if re.match(r'O<[^>]+>\s+CALL', line_upper, re.I): + return None + + # IF + if_match = re.match(r'O<([^>]+)>\s+IF\s+\[(.+)\]', line_upper, re.I) + if if_match: + label = if_match.group(1) + condition = if_match.group(2) + result = self.evaluate_condition(condition, local_level) + self.if_stack.append({ + 'label': label, + 'condition_result': result, + 'line_num': line_num + }) + if not result: + self._skip_mode = True + self._skip_depth = 1 + self._skip_target = label + return self._skip_to_endif(current_line_idx, lines) + return None + + # ELSEIF + elseif_match = re.match(r'O<([^>]+)>\s+ELSEIF\s+\[(.+)\]', line_upper, re.I) + if elseif_match: + label = elseif_match.group(1) + condition = elseif_match.group(2) + if self.if_stack: + top = self.if_stack[-1] + if top['condition_result']: + self._skip_mode = True + self._skip_depth = 1 + return self._skip_to_endif(current_line_idx, lines) + else: + result = self.evaluate_condition(condition, local_level) + if result: + top['condition_result'] = True + else: + self._skip_mode = True + self._skip_depth = 1 + return self._skip_to_endif(current_line_idx, lines) + return None + + # ELSE + if re.match(r'O<[^>]+>\s+ELSE', line_upper, re.I): + if self.if_stack: + top = self.if_stack[-1] + if top['condition_result']: + self._skip_mode = True + self._skip_depth = 1 + return self._skip_to_endif(current_line_idx, lines) + return None + + # ENDIF + if re.match(r'O<[^>]+>\s+ENDIF', line_upper, re.I): + if self.if_stack: + self.if_stack.pop() + return None + + # WHILE + while_match = re.match(r'O<([^>]+)>\s+WHILE\s+\[(.+)\]', line_upper, re.I) + if while_match: + label = while_match.group(1) + condition = while_match.group(2) + result = self.evaluate_condition(condition, local_level) + self.while_stack.append({ + 'label': label, + 'condition': condition, + 'start_line': current_line_idx, + 'line_num': line_num + }) + if not result: + return self._skip_to_endwhile(current_line_idx, lines) + return None + + # ENDWHILE + if re.match(r'O<[^>]+>\s+ENDWHILE', line_upper, re.I): + if self.while_stack: + loop_info = self.while_stack[-1] + result = self.evaluate_condition(loop_info['condition'], local_level) + if result: + return loop_info['start_line'] + else: + self.while_stack.pop() + return None + + # REPEAT + repeat_match = re.match(r'O<([^>]+)>\s+REPEAT\s+\[(.+)\]', line_upper, re.I) + if repeat_match: + label = repeat_match.group(1) + count_expr = repeat_match.group(2) + try: + expanded = self.var_manager.expand_variables(count_expr, local_level) + count = int(eval(expanded, {"__builtins__": {}}, {"math": math})) + except: + count = 1 + self.repeat_stack.append({ + 'label': label, + 'count': count, + 'remaining': count, + 'start_line': current_line_idx + }) + return None + + # ENDREPEAT + if re.match(r'O<[^>]+>\s+ENDREPEAT', line_upper, re.I): + if self.repeat_stack: + loop_info = self.repeat_stack[-1] + loop_info['remaining'] -= 1 + if loop_info['remaining'] > 0: + return loop_info['start_line'] + else: + self.repeat_stack.pop() + return None + + # BREAK + if re.match(r'O<[^>]+>\s+BREAK', line_upper, re.I): + if self.while_stack: + return self._skip_to_endwhile(current_line_idx, lines) + if self.repeat_stack: + self.repeat_stack.pop() + return None + + # CONTINUE + if re.match(r'O<[^>]+>\s+CONTINUE', line_upper, re.I): + if self.while_stack: + return self.while_stack[-1]['start_line'] + if self.repeat_stack: + return self.repeat_stack[-1]['start_line'] + return None + + # GOTO + goto_match = re.match(r'O<[^>]+>\s+GOTO\s+(\d+)', line_upper, re.I) + if goto_match: + target_line = int(goto_match.group(1)) + for i, l in enumerate(lines): + n_match = re.search(r'N(\d+)', l, re.I) + if n_match and int(n_match.group(1)) == target_line: + return i + if target_line <= len(lines): + return target_line - 1 + return None + + # RETURN + if re.match(r'O<[^>]+>\s+RETURN', line_upper, re.I): + if self.subroutine_stack: + call_info = self.subroutine_stack.pop() + return call_info['return_line'] + return None + + return None + + def _skip_to_endif(self, line_idx: int, lines: List[str]) -> int: + """跳过到对应的 ENDIF""" + i = line_idx + 1 + depth = 1 + while i < len(lines): + line_upper = lines[i].upper().strip() + if re.match(r'O<[^>]+>\s+IF\s+\[', line_upper, re.I): + depth += 1 + elif re.match(r'O<[^>]+>\s+ENDIF', line_upper, re.I): + depth -= 1 + if depth == 0: + return i + i += 1 + return len(lines) - 1 + + def _skip_to_endwhile(self, line_idx: int, lines: List[str]) -> int: + """跳过到对应的 ENDWHILE""" + i = line_idx + 1 + depth = 1 + while i < len(lines): + line_upper = lines[i].upper().strip() + if re.match(r'O<[^>]+>\s+WHILE\s+\[', line_upper, re.I): + depth += 1 + elif re.match(r'O<[^>]+>\s+ENDWHILE', line_upper, re.I): + depth -= 1 + if depth == 0: + return i + i += 1 + return len(lines) - 1 + + def reset(self): + """重置状态""" + self.while_stack.clear() + self.repeat_stack.clear() + self.if_stack.clear() + self.subroutine_stack.clear() + self.jump_targets.clear() + self._skip_mode = False + self._skip_depth = 0 + self._skip_target = None + + +# ==================== 虚拟HAL系统 ==================== + +class VirtualHAL: + """虚拟HAL系统 - 模拟LinuxCNC的硬件抽象层""" + + def __init__(self, debug: bool = False): + self.debug = debug + self.pins: Dict[str, Any] = {} + self.signals: Dict[str, Any] = {} + self.params: Dict[str, Any] = {} + self.watchers: Dict[str, List[Callable]] = {} + self._comp_id = 0 + self._initialized = False + self._init_pins() + + def _init_pins(self): + """初始化默认引脚""" + # 关节 + for i in range(EMCMOT_MAX_JOINTS): + self.pins[f'joint.{i}.position'] = 0.0 + self.pins[f'joint.{i}.velocity'] = 0.0 + self.pins[f'joint.{i}.acceleration'] = 0.0 + self.pins[f'joint.{i}.home'] = 0 + self.pins[f'joint.{i}.homing'] = 0 + self.pins[f'joint.{i}.error'] = 0.0 + self.pins[f'joint.{i}.f-error'] = 0.0 + self.pins[f'joint.{i}.enabled'] = 1 + self.pins[f'joint.{i}.max-limit'] = 1000.0 + self.pins[f'joint.{i}.min-limit'] = -1000.0 + + # 轴 + for axis in ['x', 'y', 'z', 'a', 'b', 'c', 'u', 'v', 'w']: + self.pins[f'axis.{axis}.position'] = 0.0 + self.pins[f'axis.{axis}.velocity'] = 0.0 + self.pins[f'axis.{axis}.home'] = 0 + self.pins[f'axis.{axis}.homing'] = 0 + self.pins[f'axis.{axis}.enabled'] = 1 + self.pins[f'axis.{axis}.max-limit'] = 1000.0 + self.pins[f'axis.{axis}.min-limit'] = -1000.0 + + # 主轴 + for i in range(EMCMOT_MAX_SPINDLES): + self.pins[f'spindle.{i}.speed'] = 0.0 + self.pins[f'spindle.{i}.direction'] = 0 + self.pins[f'spindle.{i}.at-speed'] = 0 + self.pins[f'spindle.{i}.brake'] = 0 + self.pins[f'spindle.{i}.enable'] = 0 + self.pins[f'spindle.{i}.speed-out'] = 0.0 + self.pins[f'spindle.{i}.speed-out-abs'] = 0.0 + + # 冷却液 + self.pins['coolant.flood'] = 0 + self.pins['coolant.mist'] = 0 + + # 刀具 + self.pins['tool.number'] = 0 + self.pins['tool.pocket'] = 0 + self.pins['tool.length'] = 0.0 + self.pins['tool.diameter'] = 0.0 + self.pins['tool.prep-number'] = 0 + self.pins['tool.prep-pocket'] = 0 + + # 倍率 + self.pins['feed-override'] = 1.0 + self.pins['rapid-override'] = 1.0 + self.pins['spindle-override'] = 1.0 + self.pins['max-velocity'] = 10000.0 + + # 运动 + self.pins['motion.kins-type'] = 2 + self.pins['motion.rtcp-active'] = 0 + self.pins['motion.switchkins-type'] = 0 + self.pins['motion.pivot-length'] = 250.0 + self.pins['motion.tool-length'] = 0.0 + self.pins['motion.rot-center-x'] = 0.0 + self.pins['motion.rot-center-y'] = 0.0 + self.pins['motion.rot-center-z'] = 0.0 + self.pins['motion.enabled'] = 0 + self.pins['motion.in-position'] = 1 + self.pins['motion.feed-hold'] = 0 + self.pins['motion.adaptive-feed'] = 1.0 + + # 急停 + self.pins['estop.active'] = 0 + self.pins['estop.reset-request'] = 0 + + # 机床 + self.pins['machine.power-on'] = 0 + self.pins['machine.power-request'] = 0 + self.pins['machine.reset'] = 0 + self.pins['machine.reset-done'] = 1 + self.pins['machine.state'] = 'OFF' + self.pins['machine.is-on'] = 0 + + # 回零 + self.pins['home.all'] = 0 + for axis in ['x', 'y', 'z', 'a', 'b', 'c']: + self.pins[f'home.{axis}'] = 0 + self.pins[f'home.{axis}-done'] = 0 + self.pins[f'home.{axis}-switch'] = 0 + self.pins['home.all-done'] = 0 + + # 程序 + self.pins['program.run'] = 0 + self.pins['program.pause'] = 0 + self.pins['program.resume'] = 0 + self.pins['program.stop'] = 0 + self.pins['program.status'] = 'IDLE' + self.pins['program.line'] = 0 + self.pins['program.progress'] = 0.0 + self.pins['program.block-delete'] = 0 + self.pins['program.optional-stop'] = 1 + + # 告警 + self.pins['alarm.active'] = 0 + self.pins['alarm.code'] = 0 + self.pins['alarm.message'] = '' + + # 模拟量 + for i in range(8): + self.pins[f'analog.out.{i}'] = 0.0 + + # 数字量 + for i in range(32): + self.pins[f'digital.in.{i}'] = 0 + self.pins[f'digital.out.{i}'] = 0 + + # 探测 + self.pins['probe.input'] = 0 + self.pins['probe.tripped'] = 0 + self.pins['probe.position-x'] = 0.0 + self.pins['probe.position-y'] = 0.0 + self.pins['probe.position-z'] = 0.0 + + # 固定循环 + self.pins['motion.canned-cycle'] = 0 + self.pins['motion.canned-cycle-type'] = 0 + self.pins['motion.canned-cycle-retract'] = 0.0 + self.pins['motion.canned-cycle-clearance'] = 0.0 + + def hal_init(self, comp_name: str) -> int: + """初始化HAL组件""" + self._comp_id = hash(comp_name) & 0x7FFFFFFF + self._initialized = True + return self._comp_id + + def hal_ready(self, comp_id: int) -> int: + """标记HAL组件就绪""" + return 0 + + def hal_exit(self, comp_id: int): + """退出HAL""" + self._initialized = False + + def set(self, pin: str, value: Any) -> bool: + """设置引脚值""" + old_value = self.pins.get(pin) + self.pins[pin] = value + self._notify_watchers(pin, value, old_value) + return True + + def get(self, pin: str, default: Any = 0) -> Any: + """获取引脚值""" + return self.pins.get(pin, default) + + def add_watcher(self, pin: str, callback: Callable): + """添加监听器""" + if pin not in self.watchers: + self.watchers[pin] = [] + self.watchers[pin].append(callback) + + def remove_watcher(self, pin: str, callback: Callable): + """移除监听器""" + if pin in self.watchers: + try: + self.watchers[pin].remove(callback) + except ValueError: + pass + + def _notify_watchers(self, pin: str, value: Any, old_value: Any): + """通知监听器""" + if pin in self.watchers: + for callback in self.watchers[pin]: + try: + callback(pin, value, old_value) + except Exception as e: + if self.debug: + print(f"[HAL] 监听器回调错误: {e}") + + def update_axis_position(self, axis: str, position: float): + """更新轴位置""" + self.set(f'axis.{axis}.position', position) + axis_map = {'x': 0, 'y': 1, 'z': 2, 'a': 3, 'b': 4, 'c': 5, 'u': 6, 'v': 7, 'w': 8} + if axis.lower() in axis_map: + self.set(f'joint.{axis_map[axis.lower()]}.position', position) + + def update_all_positions(self, positions: Dict[str, float]): + """更新所有轴位置""" + for axis, pos in positions.items(): + self.update_axis_position(axis, pos) + + def dump_state(self) -> Dict[str, Any]: + """导出状态""" + return self.pins.copy() + + def load_state(self, state: Dict[str, Any]): + """加载状态""" + for pin, value in state.items(): + if pin in self.pins: + self.pins[pin] = value + + def to_json(self) -> str: + """导出为JSON""" + serializable = {} + for k, v in self.pins.items(): + if isinstance(v, (str, int, float, bool, type(None))): + serializable[k] = v + else: + serializable[k] = str(v) + return json.dumps(serializable, indent=2) + + def reset(self): + """重置""" + self._init_pins() + if self.debug: + print("[HAL] 系统已重置") + + +# ==================== 机床状态机 ==================== + +class MachineStateMachine: + """机床状态机 - 模拟真实CNC控制器的状态转换""" + + STATE_OFF = "OFF" + STATE_RESET = "RESET" + STATE_ESTOP = "ESTOP" + STATE_IDLE = "IDLE" + STATE_RUNNING = "RUNNING" + STATE_PAUSED = "PAUSED" + STATE_HOMING = "HOMING" + STATE_JOGGING = "JOGGING" + STATE_ALARM = "ALARM" + STATE_PROBING = "PROBING" + STATE_MDI = "MDI" + STATE_TOOL_CHANGING = "TOOL_CHANGING" + + TRANSITIONS = { + STATE_OFF: [STATE_RESET, STATE_ESTOP], + STATE_RESET: [STATE_IDLE, STATE_ESTOP, STATE_ALARM], + STATE_ESTOP: [STATE_RESET, STATE_OFF], + STATE_IDLE: [STATE_RUNNING, STATE_HOMING, STATE_JOGGING, STATE_MDI, + STATE_ESTOP, STATE_ALARM, STATE_OFF, STATE_TOOL_CHANGING, STATE_RESET], + STATE_RUNNING: [STATE_PAUSED, STATE_IDLE, STATE_ESTOP, STATE_ALARM], + STATE_PAUSED: [STATE_RUNNING, STATE_IDLE, STATE_ESTOP, STATE_ALARM], + STATE_HOMING: [STATE_IDLE, STATE_ESTOP, STATE_ALARM], + STATE_JOGGING: [STATE_IDLE, STATE_ESTOP, STATE_ALARM], + STATE_ALARM: [STATE_RESET, STATE_OFF], + STATE_PROBING: [STATE_IDLE, STATE_ESTOP], + STATE_MDI: [STATE_IDLE, STATE_ESTOP, STATE_ALARM], + STATE_TOOL_CHANGING: [STATE_IDLE, STATE_ESTOP, STATE_ALARM], + } + + def __init__(self, hal: VirtualHAL, debug: bool = False): + self.hal = hal + self.debug = debug + self.current_state = self.STATE_OFF + self.previous_state = self.STATE_OFF + self.state_entry_time = time.time() + self.alarm_reason = "" + self.alarm_code = 0 + + self.transition_callbacks: List[Callable] = [] + + self._update_hal_pins() + + if self.debug: + print(f"[状态机] 初始化: {self.current_state}") + + def _update_hal_pins(self): + """更新 HAL 引脚""" + self.hal.set('machine.state', self.current_state) + self.hal.set('estop.active', 1 if self.current_state == self.STATE_ESTOP else 0) + + is_powered = self.current_state not in [self.STATE_OFF, self.STATE_ESTOP] + self.hal.set('machine.power-on', 1 if is_powered else 0) + self.hal.set('machine.is-on', 1 if is_powered else 0) + + self.hal.set('alarm.active', 1 if self.current_state == self.STATE_ALARM else 0) + self.hal.set('machine.reset-done', 1 if self.current_state == self.STATE_IDLE else 0) + + def add_transition_callback(self, callback: Callable): + """添加状态转换回调""" + self.transition_callbacks.append(callback) + + def _notify_transition(self, from_state: str, to_state: str): + """通知状态转换""" + for callback in self.transition_callbacks: + try: + callback(from_state, to_state) + except Exception as e: + if self.debug: + print(f"[状态机] 回调错误: {e}") + + def _can_transition(self, target_state: str) -> bool: + """检查是否可以转换""" + return target_state in self.TRANSITIONS.get(self.current_state, []) + + def _log_transition(self, target_state: str): + """记录状态转换""" + if self.debug: + print(f"[状态机] {self.current_state} -> {target_state}") + + def transition_to(self, target_state: str) -> bool: + """执行状态转换""" + if not self._can_transition(target_state): + if self.debug: + print(f"[状态机] 无效转换: {self.current_state} -> {target_state}") + return False + + old_state = self.current_state + self.previous_state = old_state + self.current_state = target_state + self.state_entry_time = time.time() + + self._log_transition(target_state) + self._update_hal_pins() + self._notify_transition(old_state, target_state) + + return True + + def request_power_on(self) -> bool: + """请求上电""" + if self.current_state == self.STATE_OFF: + return self.transition_to(self.STATE_RESET) + return False + + def request_power_off(self) -> bool: + """请求断电""" + if self.current_state == self.STATE_IDLE: + return self.transition_to(self.STATE_OFF) + return False + + def emergency_stop(self) -> bool: + """急停""" + if self.current_state != self.STATE_ESTOP: + self.hal.set('estop.active', 1) + return self.transition_to(self.STATE_ESTOP) + return False + + def reset(self) -> bool: + """复位""" + if self.current_state in [self.STATE_ESTOP, self.STATE_ALARM, self.STATE_IDLE, self.STATE_RESET]: + self.hal.set('machine.reset', 1) + result = self.transition_to(self.STATE_RESET) + self.hal.set('machine.reset', 0) + return result + return False + + def home_all(self) -> bool: + """回零""" + if self.current_state == self.STATE_IDLE: + self.hal.set('home.all', 1) + result = self.transition_to(self.STATE_HOMING) + self.hal.set('home.all', 0) + return result + return False + + def start_program(self) -> bool: + """启动程序""" + if self.current_state == self.STATE_IDLE: + return self.transition_to(self.STATE_RUNNING) + return False + + def pause_program(self) -> bool: + """暂停程序""" + if self.current_state == self.STATE_RUNNING: + return self.transition_to(self.STATE_PAUSED) + return False + + def resume_program(self) -> bool: + """恢复程序""" + if self.current_state == self.STATE_PAUSED: + return self.transition_to(self.STATE_RUNNING) + return False + + def stop_program(self) -> bool: + """停止程序""" + if self.current_state in [self.STATE_RUNNING, self.STATE_PAUSED]: + return self.transition_to(self.STATE_IDLE) + return False + + def start_jogging(self) -> bool: + """开始点动""" + if self.current_state == self.STATE_IDLE: + return self.transition_to(self.STATE_JOGGING) + return False + + def stop_jogging(self) -> bool: + """停止点动""" + if self.current_state == self.STATE_JOGGING: + return self.transition_to(self.STATE_IDLE) + return False + + def start_mdi(self) -> bool: + """开始MDI""" + if self.current_state == self.STATE_IDLE: + return self.transition_to(self.STATE_MDI) + return False + + def stop_mdi(self) -> bool: + """停止MDI""" + if self.current_state == self.STATE_MDI: + return self.transition_to(self.STATE_IDLE) + return False + + def start_tool_change(self) -> bool: + """开始换刀""" + if self.current_state == self.STATE_IDLE: + return self.transition_to(self.STATE_TOOL_CHANGING) + return False + + def stop_tool_change(self) -> bool: + """停止换刀""" + if self.current_state == self.STATE_TOOL_CHANGING: + return self.transition_to(self.STATE_IDLE) + return False + + def set_alarm(self, code: int, message: str) -> bool: + """设置告警""" + if self.current_state not in [self.STATE_ESTOP, self.STATE_OFF]: + self.alarm_reason = message + self.alarm_code = code + self.hal.set('alarm.code', code) + self.hal.set('alarm.message', message) + return self.transition_to(self.STATE_ALARM) + return False + + def clear_alarm(self) -> bool: + """清除告警""" + if self.current_state == self.STATE_ALARM: + self.alarm_reason = "" + self.alarm_code = 0 + self.hal.set('alarm.code', 0) + self.hal.set('alarm.message', '') + return self.transition_to(self.STATE_RESET) + return False + + def get_state(self) -> str: + """获取当前状态""" + return self.current_state + + def get_state_duration(self) -> float: + """获取当前状态持续时间""" + return time.time() - self.state_entry_time + + def is_operational(self) -> bool: + """是否可操作""" + return self.current_state == self.STATE_IDLE + + def is_running(self) -> bool: + """是否正在运行""" + return self.current_state == self.STATE_RUNNING + + def is_paused(self) -> bool: + """是否已暂停""" + return self.current_state == self.STATE_PAUSED + + def is_estop(self) -> bool: + """是否急停""" + return self.current_state == self.STATE_ESTOP + + def is_alarm(self) -> bool: + """是否有告警""" + return self.current_state == self.STATE_ALARM + + def update(self): + """更新状态机""" + if self.hal.get('estop.reset-request'): + self.reset() + self.hal.set('estop.reset-request', 0) + + if self.hal.get('machine.power-request'): + self.request_power_on() + self.hal.set('machine.power-request', 0) + + if self.hal.get('program.run'): + self.start_program() + self.hal.set('program.run', 0) + + if self.hal.get('program.pause'): + self.pause_program() + self.hal.set('program.pause', 0) + + if self.hal.get('program.resume'): + self.resume_program() + self.hal.set('program.resume', 0) + + if self.hal.get('program.stop'): + self.stop_program() + self.hal.set('program.stop', 0) + + if self.current_state == self.STATE_RESET: + if self.get_state_duration() >= 0.5: + self.transition_to(self.STATE_IDLE) + + if self.current_state == self.STATE_HOMING: + if self.hal.get('home.all-done'): + self.transition_to(self.STATE_IDLE) + self.hal.set('home.all-done', 0) + + self.hal.set('program.status', self.current_state) + + +# ==================== GLCanonPathCollectorWithRTCP 类 ==================== + + + +# ==================== 内置G代码解析器 ==================== + +class FullGCodeParserWithRTCP: + """内置G代码解析器 - 支持LinuxCNC宏指令和子程序""" + + def __init__(self, max_points: int = 50000, + tool_radius_map: Dict[int, float] = None, + tool_length_map: Dict[int, float] = None, + acceleration: float = 500.0, + max_rapid_rate: float = 10000.0, + max_feed_rate: float = 5000.0, + kinematics_type: KinematicsType = KinematicsType.TRT_BC, + kinematics_params: Dict[str, Any] = None, + debug: bool = False): + + self.max_points = max_points + self.tool_radius_map = tool_radius_map or {1: 2.0, 2: 3.0, 3: 4.0} + self.tool_length_map = tool_length_map or {1: 100.0, 2: 120.0, 3: 150.0} + self.acceleration = acceleration + self.max_rapid_rate = max_rapid_rate + self.max_feed_rate = max_feed_rate + self.kinematics_type = kinematics_type + self.kinematics_params = kinematics_params or {} + self.debug = debug + + self.collector: Optional[GLCanonPathCollectorWithRTCP] = None + self.current_plane = 17 + self.current_filename = "" + self.subroutine_path = "" + + self.subroutines: Dict[str, List[Tuple[int, str]]] = {} + self.call_level = 0 + + self.saved_states: List[Dict] = [] + + self._line_number_map: Dict[int, int] = {} + + def set_subroutine_path(self, path: str): + self.subroutine_path = path + + def load_subroutine_file(self, filename: str) -> bool: + if self.debug: + print(f"[DEBUG] load_subroutine_file: 尝试加载 '{filename}'") + + search_paths = [filename] + if self.subroutine_path: + search_paths.append(os.path.join(self.subroutine_path, os.path.basename(filename))) + if self.current_filename: + search_paths.append(os.path.join(os.path.dirname(self.current_filename), os.path.basename(filename))) + search_paths.append(os.path.join(os.getcwd(), os.path.basename(filename))) + + for path in search_paths: + if os.path.exists(path): + try: + with open(path, 'r', encoding='utf-8', errors='ignore') as f: + content = f.read() + lines = content.split('\n') + self._collect_subroutines_from_lines(lines, path) + return True + except Exception as e: + if self.debug: + print(f"[DEBUG] 加载失败: {path}, 错误: {e}") + + return False + + def _collect_subroutines_from_lines(self, lines: List[str], source: str = ""): + i = 0 + while i < len(lines): + raw_line = lines[i] + line = self._preprocess_line(raw_line) + + if not line: + i += 1 + continue + + sub_match = re.match(r'o<([^>]+)>\s*sub', line, re.I) + if sub_match: + sub_name = sub_match.group(1) + sub_lines = [] + sub_line_nums = [] + i += 1 + + while i < len(lines): + raw_sub_line = lines[i] + sub_line = self._preprocess_line(raw_sub_line) + + if sub_line and re.match(r'o<[^>]+>\s*endsub', sub_line, re.I): + break + + if sub_line: + sub_lines.append(sub_line) + sub_line_nums.append(i + 1) + i += 1 + + self.subroutines[sub_name] = list(zip(sub_line_nums, sub_lines)) + i += 1 + + def _preprocess_line(self, line: str) -> Optional[str]: + """移除注释和行号，返回处理后的字符串或 None""" + if not line: + return None + + # 移除行号 N... + line = re.sub(r'^N\d+\s*', '', line) + + # 移除分号注释 + if ';' in line: + line = line.split(';')[0] + + # 使用状态机正确移除圆括号注释（处理嵌套和复杂表达式） + result_line = [] + paren_depth = 0 + for char in line: + if char == '(': + paren_depth += 1 + elif char == ')': + if paren_depth > 0: + paren_depth -= 1 + else: + if paren_depth == 0: + result_line.append(char) + + line = ''.join(result_line) + + # 移除头尾空白 + line = line.strip() + + # 如果处理后为空，返回 None + if not line: + return None + + # 转换为大写进行处理 + return line.upper() + + def _parse_call_params(self, params_str: str) -> List[float]: + args = [] + if not params_str: + return args + + pattern = r'\[([^\]]+)\]' + matches = re.findall(pattern, params_str) + + for match in matches: + try: + if self.collector: + value = self.collector.var_manager.evaluate_expression(match, self.call_level) + else: + value = float(match) + args.append(value) + except Exception: + args.append(0.0) + return args + + + + def expand_named_params(self, expr): + """ + 展开命名参数 + + 将 #<_x> 替换为当前X坐标值 + 将 #<_y> 替换为当前Y坐标值 + 将 #<_z> 替换为当前Z坐标值 + 将 #<_a> 替换为当前A角度值 + 将 #<_b> 替换为当前B角度值 + 将 #<_c> 替换为当前C角度值 + 将 # 替换为子程序参数值 + 将 # 替换为子程序参数值 + + Args: + expr: 包含命名参数的表达式字符串 + + Returns: + 展开后的表达式字符串 + """ + if not isinstance(expr, str): + return expr + + result = expr + + # ===== 展开系统变量 #<_x>, #<_y>, #<_z>, #<_a>, #<_b>, #<_c>, #<_w> ===== + if self.collector: + system_vars = { + '#<_x>': str(self.collector.current_pos.x), + '#<_y>': str(self.collector.current_pos.y), + '#<_z>': str(self.collector.current_pos.z), + '#<_a>': str(self.collector.current_pos.a), + '#<_b>': str(self.collector.current_pos.b), + '#<_c>': str(self.collector.current_pos.c), + '#<_w>': '0.0', # W轴默认0 + } + + for var, value in system_vars.items(): + result = result.replace(var, value) + + # ===== 展开子程序参数 #, #, #, #, #, #, #, # 等 ===== + if self.collector and hasattr(self.collector, 'var_manager'): + # 使用参数表系统的 expand_variables 展开剩余变量 + result = self.collector.var_manager.expand_variables(result, self.call_level) + elif hasattr(self, 'param_stack') and self.param_stack: + # 备用方案：直接查找子程序参数栈 + current_frame = self.param_stack[-1] + param_pattern = re.compile(r'#<(\w+)>') + + def replace_param(match): + param_name = match.group(1) + # 跳过系统变量（已经处理过） + if param_name in ['_x', '_y', '_z', '_a', '_b', '_c', '_w']: + return match.group(0) + # 跳过HAL变量 + if param_name.startswith('_hal['): + return match.group(0) + # 跳过INI变量 + if param_name.startswith('_ini['): + return match.group(0) + # 查找子程序参数 + if param_name in current_frame: + return str(current_frame[param_name]) + return match.group(0) + + result = param_pattern.sub(replace_param, result) + + return result + + + + def _parse_line(self, line: str, line_num: int) -> Optional[int]: + """解析单行G代码 - 完整版，使用参数表系统""" + if not line or not line.strip(): + return None + + line_upper = line.upper().strip() + + # ==================== G10 通用解析函数 ==================== + # ★★★ 定义在此处，供所有 G10 分支 (L1/L10/L11/L2/L20) 共享 ★★★ + def parse_g10_value(axis, expanded_upper): + """ + 解析 G10 坐标值，支持： + - X1.234, X=1.234 + - X[#1+#2], X=[#1+#2] + """ + patterns = [ + rf'{axis}\s*=\s*([+-]?\d*\.?\d+)', + rf'{axis}\s*=\s*(\[.+?\])', + rf'{axis}\s*([+-]?\d*\.?\d+)', + rf'{axis}\s*(\[.+?\])', + rf'{axis}([+-]?\d*\.?\d+)', + ] + for pattern in patterns: + match = re.search(pattern, expanded_upper, re.I) + if match: + val_str = match.group(1) + try: + if val_str.startswith('['): + return self.collector.var_manager.evaluate_expression(val_str, self.call_level) + else: + return float(val_str) + except (ValueError, Exception) as e: + if self.debug: + print(f"[DEBUG] G10 值解析失败: {axis}={val_str}, 错误: {e}") + continue + return None + + # ==================== 变量赋值（使用参数表） ==================== + + # 数字变量赋值: #1 = 10 或 #1 = [1+2] 或 #1 = #2 + var_match = re.match(r'#(\d+)\s*=\s*(.+)', line, re.I) + if var_match: + var_num = int(var_match.group(1)) + value_expr = var_match.group(2).strip() + if self.collector: + # 检查只读参数 + if lcnc_param.InterpParameterIndex.is_readonly(var_num): + if self.debug: + print(f"[警告] 参数 #{var_num} 是只读的，不能赋值") + else: + # 使用参数表系统计算表达式 + value = self.collector.var_manager.evaluate_expression(value_expr, self.call_level) + if self.debug: + print(f"[DEBUG] 变量赋值: #{var_num} = {value_expr} -> {value}") + self.collector.var_manager.set_param(var_num, value, self.call_level > 0) + return None + + # 命名变量赋值: # = #1 或 # = 100 或 # = [1+2] + named_var_match = re.match(r'#<([^>]+)>\s*=\s*(.+)', line, re.I) + if named_var_match: + var_name = named_var_match.group(1) + value_expr = named_var_match.group(2).strip() + if self.collector: + # 使用参数表系统计算表达式 + value = self.collector.var_manager.evaluate_expression(value_expr, self.call_level) + if self.debug: + print(f"[DEBUG] 变量赋值: #<{var_name}> = {value_expr} -> {value}") + self.collector.var_manager.set_named(var_name, value) + return None + + # ==================== 子程序调用 ==================== + + o_call_match = re.match(r'o<([^>]+)>\s*call\s*(.*)', line, re.I) + if o_call_match: + sub_name = o_call_match.group(1) + params_str = o_call_match.group(2).strip() + + if self.debug: + print(f"[DEBUG] ========== 子程序调用 ==========") + print(f"[DEBUG] 子程序: {sub_name}") + print(f"[DEBUG] 原始参数字符串: '{params_str}'") + + found_sub = None + for key in self.subroutines.keys(): + if key.lower() == sub_name.lower(): + found_sub = key + break + + if not found_sub: + if self.debug: + print(f"[DEBUG] 子程序 {sub_name} 未找到，尝试加载文件...") + self.load_subroutine_file(f"{sub_name}.ngc") + for key in self.subroutines.keys(): + if key.lower() == sub_name.lower(): + found_sub = key + break + + if found_sub: + if self.debug: + print(f"[DEBUG] 找到子程序 {found_sub}，开始执行...") + + # 使用参数表系统解析调用参数 + args = self._parse_call_params(params_str) + if self.debug: + print(f"[DEBUG] 解析参数: {args}") + print(f"[DEBUG] 参数个数: {len(args)}") + + self.call_level += 1 + if self.collector: + # 推入局部变量帧 + self.collector.var_manager.push_local_frame() + # 设置子程序参数 #1..#30 + for i, arg in enumerate(args, 1): + if i <= lcnc_param.INTERP_SUB_PARAMS: + self.collector.var_manager.set_param(i, arg, self.call_level) + if self.debug: + print(f"[DEBUG] 设置参数 #{i} = {arg}") + + # 设置 n_args 命名参数 + self.collector.var_manager.add_named_param("n_args", + lcnc_param.ParameterAttribute.PA_READONLY) + self.collector.var_manager.set_named("n_args", float(len(args))) + + sub_lines = self.subroutines[found_sub] + if self.debug: + print(f"[DEBUG] 子程序 {found_sub} 共 {len(sub_lines)} 行") + + for sub_line_num, sub_line in sub_lines: + if self.debug: + display_line = sub_line[:80] + "..." if len(sub_line) > 80 else sub_line + print(f"[DEBUG] 执行子程序行 {sub_line_num}: {display_line}") + self._parse_line(sub_line, sub_line_num) + + self.call_level -= 1 + if self.collector: + # 弹出局部变量帧 + self.collector.var_manager.pop_local_frame() + + if self.debug: + print(f"[DEBUG] ========== 子程序 {found_sub} 执行完成 ==========") + else: + if self.debug: + print(f"[DEBUG] 子程序 {sub_name} 未找到！") + + return None + + # ==================== 跳转指令 ==================== + + goto_match = re.match(r'goto\s+(\d+)', line, re.I) + if goto_match: + target_line = int(goto_match.group(1)) + return target_line + + n_match = re.match(r'n(\d+)', line, re.I) + if n_match: + label = int(n_match.group(1)) + if self.collector and self.collector.control_handler: + self.collector.control_handler.jump_targets[str(label)] = line_num + return None + + # ==================== O代码行 ==================== + + if line_upper.startswith('O'): + return None + + # ==================== 创建行代码包装器 ==================== + + line_code = LineCodeWrapper(line_num, self.current_plane) + if self.collector: + if re.search(r'\bG53\b', line_upper): + self.collector._g53_active = True + self.collector.next_line(line_code) + + # ==================== 程序结束 ==================== + + if 'M30' in line_upper or 'M02' in line_upper: + if self.collector: + self.collector.program_end() + return None + + if 'M00' in line_upper: + if self.collector: + self.collector.program_stop() + return None + + if 'M01' in line_upper: + if self.collector: + self.collector.optional_stop() + return None + + # ==================== M70/M71/M72/M73 ==================== + + if 'M70' in line_upper: + if self.collector: + saved_state = self.collector.save_state(restore_on_return=False) + self.saved_states.append(saved_state) + if self.debug: + print(f"[DEBUG] M70: 保存状态，当前栈深度: {len(self.saved_states)}") + return None + + if 'M73' in line_upper: + if self.collector: + saved_state = self.collector.save_state(restore_on_return=True) + self.saved_states.append(saved_state) + if self.debug: + print(f"[DEBUG] M73: 保存状态（返回时恢复），当前栈深度: {len(self.saved_states)}") + return None + + if 'M72' in line_upper: + if self.collector and self.saved_states: + saved_state = self.saved_states.pop() + self.collector.restore_state(saved_state) + if self.debug: + print(f"[DEBUG] M72: 恢复状态，剩余栈深度: {len(self.saved_states)}") + return None + + if 'M71' in line_upper: + if self.saved_states: + self.saved_states.pop() + if self.debug: + print(f"[DEBUG] M71: 丢弃保存的状态，剩余栈深度: {len(self.saved_states)}") + return None + + # ==================== RTCP控制 ==================== + + # 在 _parse_line 方法中找到 G49 处理部分，修改为： + if 'G49' in line_upper: + if self.collector: + self.collector.set_rtcp_enable(False) + self.collector.cancel_tool_length_offset() # ★ 添加此行 + return None + + if 'G43.4' in line_upper or 'G43.5' in line_upper: + if self.collector: + h_match = re.search(r'H\s*=?\s*(\d+)', line_upper, re.I) + if not h_match: + h_match = re.search(r'H\s*(\d+)', line_upper, re.I) + if h_match: + h_code = int(h_match.group(1)) + self.collector.set_tool_length_offset(h_code) + self.collector.set_rtcp_enable(True) + return None + + # ==================== 运动学切换 ==================== + + if 'M429' in line_upper: + if self.collector: + self.collector.set_kinematics_by_type(0) + if self.debug: + print(f"[DEBUG] M429: 切换到三轴运动学 (IDENTITY)") + return None + + if 'M428' in line_upper: + if self.collector: + self.collector.set_kinematics_by_type(1) + if self.debug: + print(f"[DEBUG] M428: 恢复到五轴运动学") + return None + + if 'M430' in line_upper: + if self.collector: + self.collector.set_kinematics_by_type(2) + if self.debug: + print(f"[DEBUG] M430: 切换到 FIVEAXIS_BC") + return None + + # ==================== G10 坐标系设置（使用参数表） ==================== + + # ==================== G10 错误检查 ==================== + if 'G10' in line_upper: + # 检查 L 值 + l_match = re.search(r'L\s*=?\s*(\d+)', line_upper, re.I) or re.search(r'L\s*(\d+)', line_upper, re.I) + l_val = int(l_match.group(1)) if l_match else 0 + + if l_val not in [0, 1, 2, 10, 11, 20]: + if self.debug: + print(f"[警告] G10 L{l_val} 不支持，支持的 L 值: 0, 1, 2, 10, 11, 20") + return None + + # 检查 P 值是否为整数 + p_match = re.search(r'P\s*=?\s*([+-]?\d*\.?\d+)', line_upper, re.I) or re.search(r'P\s*([+-]?\d*\.?\d+)', line_upper, re.I) + p_val = float(p_match.group(1)) if p_match else -1 + p_int = round(p_val) + + if abs(p_val - p_int) > 0.0002: + if self.debug: + print(f"[警告] G10 P 值 {p_val} 不是整数") + return None + + # L2/L20: P 值范围 0-9 + if l_val in [2, 20] and (p_int < 0 or p_int > 9): + if self.debug: + print(f"[警告] G10 L{l_val} P 值 {p_int} 超出范围 (0-9)") + return None + + # L1/L10/L11: P 值必须 >= 1 + if l_val in [1, 10, 11] and p_int < 1: + if self.debug: + print(f"[警告] G10 L{l_val} P 值 {p_int} 必须 >= 1") + return None + + # L1: 必须有至少一个偏移值 + if l_val == 1: + has_offset = False + for axis in ['X', 'Y', 'Z', 'A', 'B', 'C', 'U', 'V', 'W', 'R', 'I', 'J', 'Q']: + if re.search(rf'{axis}\s*[=]?\s*[\d\[\.]', line_upper, re.I): + has_offset = True + break + if not has_offset: + if self.debug: + print(f"[警告] G10 L1 没有任何偏移值") + + # ==================== G10 L20 坐标系设置 ==================== + + # G10 L20 Pn X_ Y_ Z_ A_ B_ C_ - 必须最先检测，避免被 L2 分支拦截 + if 'G10' in line_upper and 'L20' in line_upper and self.collector: + expanded_line = self.collector.var_manager.expand_variables(line, self.call_level) + expanded_upper = expanded_line.upper() + + if self.debug: + print(f"[DEBUG] G10 L20 原始行: '{line}'") + print(f"[DEBUG] G10 L20 展开后: '{expanded_line}'") + + p_match_g10 = re.search(r'P\s*=?\s*(\d+)', expanded_upper) or re.search(r'P\s*(\d+)', expanded_upper) + if p_match_g10: + p_val = int(p_match_g10.group(1)) + + # 构建参数字符串，使用表达式求值 + params_parts = [] + for axis in ['X', 'Y', 'Z', 'A', 'B', 'C']: + val = parse_g10_value(axis, expanded_upper) + if val is not None: + params_parts.append(f"{axis}{val}") + if self.debug: + print(f"[DEBUG] G10 L20 解析: {axis}={val}") + + params_str = ' '.join(params_parts) + + if self.debug: + print(f"[DEBUG] G10 L20 P{p_val}: params='{params_str}'") + + self.collector.set_coordinate_system_g10_l20(p_val, params_str) + return None + + # ==================== G10 L2 坐标系设置 ==================== + + # G10 L2 Pn X_ Y_ Z_ A_ B_ C_ - 必须在 L20 之后，且排除 L20 + if 'G10' in line_upper and 'L2' in line_upper and 'L20' not in line_upper and self.collector: + expanded_line = self.collector.var_manager.expand_variables(line, self.call_level) + expanded_upper = expanded_line.upper() + + if self.debug: + print(f"[DEBUG] G10 L2 原始行: '{line}'") + print(f"[DEBUG] G10 L2 展开后: '{expanded_line}'") + + p_match_g10 = re.search(r'P\s*=?\s*(\d+)', expanded_upper) or re.search(r'P\s*(\d+)', expanded_upper) + if p_match_g10: + p_val = int(p_match_g10.group(1)) + + x = y = z = a = b = c = u = v = w = 0.0 + + x_val = parse_g10_value('X', expanded_upper) + y_val = parse_g10_value('Y', expanded_upper) + z_val = parse_g10_value('Z', expanded_upper) + a_val = parse_g10_value('A', expanded_upper) + b_val = parse_g10_value('B', expanded_upper) + c_val = parse_g10_value('C', expanded_upper) + + if x_val is not None: x = x_val + if y_val is not None: y = y_val + if z_val is not None: z = z_val + if a_val is not None: a = a_val + if b_val is not None: b = b_val + if c_val is not None: c = c_val + + if self.debug: + print(f"[DEBUG] G10 L2 P{p_val}: X={x} Y={y} Z={z} A={a} B={b} C={c}") + + self.collector.set_g5x_offset(p_val, x, y, z, a, b, c, u, v, w) + return None + + # ==================== G10 L1 刀具偏移设置 ==================== + + if 'G10' in line_upper and 'L1' in line_upper and 'L10' not in line_upper and 'L11' not in line_upper and self.collector: + expanded_line = self.collector.var_manager.expand_variables(line, self.call_level) + expanded_upper = expanded_line.upper() + + if self.debug: + print(f"[DEBUG] ========== G10 L1 刀具偏移设置 ==========") + print(f"[DEBUG] 行号: {line_num}") + print(f"[DEBUG] 原始行: '{line.strip()}'") + print(f"[DEBUG] 展开后: '{expanded_line}'") + + # 解析 P 值（刀具号） + p_match = re.search(r'P\s*=?\s*(\d+)', expanded_upper) or re.search(r'P\s*(\d+)', expanded_upper) + if not p_match: + if self.debug: + print(f"[DEBUG] G10 L1 缺少 P 值") + return None + + tool_number = int(p_match.group(1)) + + # ★ 使用公共 parse_g10_value 函数 ★ + x_val = parse_g10_value('X', expanded_upper) + y_val = parse_g10_value('Y', expanded_upper) + z_val = parse_g10_value('Z', expanded_upper) + a_val = parse_g10_value('A', expanded_upper) + b_val = parse_g10_value('B', expanded_upper) + c_val = parse_g10_value('C', expanded_upper) + u_val = parse_g10_value('U', expanded_upper) + v_val = parse_g10_value('V', expanded_upper) + w_val = parse_g10_value('W', expanded_upper) + + # 刀具参数 + r_val = parse_g10_value('R', expanded_upper) + i_val = parse_g10_value('I', expanded_upper) + j_val = parse_g10_value('J', expanded_upper) + + # Q 值 + q_match = re.search(r'Q\s*=?\s*(\d+)', expanded_upper) or re.search(r'Q\s*(\d+)', expanded_upper) + orientation = int(q_match.group(1)) if q_match else None + + # 检查是否有任何偏移值 + has_any_offset = any(v is not None for v in [x_val, y_val, z_val, a_val, b_val, c_val, u_val, v_val, w_val, r_val, i_val, j_val, q_match]) + if not has_any_offset: + if self.debug: + print(f"[DEBUG] G10 L1 没有任何偏移值，跳过") + return None + + # 查找或创建刀具 + tool_idx = self.collector.find_or_create_tool(tool_number) + if tool_idx < 0: + if self.debug: + print(f"[DEBUG] G10 L1 无法创建刀具 T{tool_number}") + return None + + tool = self.collector.tool_table[tool_idx] + changed = [] + + # 更新偏移 + if x_val is not None: + tool.offset.x = x_val + changed.append(f'X={x_val:.3f}') + if y_val is not None: + tool.offset.y = y_val + changed.append(f'Y={y_val:.3f}') + if z_val is not None: + tool.offset.z = z_val + self.collector.tool_lengths[tool_number] = z_val + changed.append(f'Z={z_val:.3f}') + if a_val is not None: + tool.offset.a = a_val + changed.append(f'A={a_val:.3f}') + if b_val is not None: + tool.offset.b = b_val + changed.append(f'B={b_val:.3f}') + if c_val is not None: + tool.offset.c = c_val + changed.append(f'C={c_val:.3f}') + if u_val is not None: + tool.offset.u = u_val + changed.append(f'U={u_val:.3f}') + if v_val is not None: + tool.offset.v = v_val + changed.append(f'V={v_val:.3f}') + if w_val is not None: + tool.offset.w = w_val + changed.append(f'W={w_val:.3f}') + + # 更新刀具参数 + if r_val is not None: + tool.diameter = r_val + tool.radius = r_val / 2.0 + self.collector.tool_radii[tool_number] = tool.radius + changed.append(f'D={r_val:.3f}') + if i_val is not None: + tool.front_angle = i_val + changed.append(f'I={i_val:.3f}') + if j_val is not None: + tool.back_angle = j_val + changed.append(f'J={j_val:.3f}') + if orientation is not None: + tool.orientation = orientation + changed.append(f'Q={orientation}') + + # 同步到参数表 + self.collector._sync_tool_parameters_to_table(tool_idx) + + # 如果是当前刀具，更新补偿 + self.collector._handle_g10_current_tool_update(tool_idx, tool_number) + + if self.debug: + print(f"[DEBUG] G10 L1 T{tool_number}: 修改 {', '.join(changed)}") + + return None + + # ==================== G10 L10 刀具偏移设置（工件坐标系基准） ==================== + + if 'G10' in line_upper and 'L10' in line_upper and self.collector: + expanded_line = self.collector.var_manager.expand_variables(line, self.call_level) + expanded_upper = expanded_line.upper() + + if self.debug: + print(f"[DEBUG] ========== G10 L10 刀具偏移设置（工件坐标系） ==========") + print(f"[DEBUG] 行号: {line_num}") + print(f"[DEBUG] 原始行: '{line.strip()}'") + print(f"[DEBUG] 展开后: '{expanded_line}'") + + p_match = re.search(r'P\s*=?\s*(\d+)', expanded_upper) or re.search(r'P\s*(\d+)', expanded_upper) + if not p_match: + if self.debug: + print(f"[DEBUG] G10 L10 缺少 P 值") + return None + + tool_number = int(p_match.group(1)) + + destination_system = self.collector.g5x_index + tx, ty, tz, ta, tb, tc, tu, tv, tw = self.collector._get_current_in_system_without_tlo(destination_system) + dest_rotation = self.collector._get_csys_rotation(destination_system) + + if self.debug: + print(f"[DEBUG] G10 L10: 目标坐标系=G{53 + destination_system}, 旋转={dest_rotation}°") + print(f"[DEBUG] G10 L10: 当前位置(no TLO)=({tx:.3f},{ty:.3f},{tz:.3f})") + + # ★ 使用公共 parse_g10_value 函数 ★ + x_val = parse_g10_value('X', expanded_upper) + y_val = parse_g10_value('Y', expanded_upper) + z_val = parse_g10_value('Z', expanded_upper) + a_val = parse_g10_value('A', expanded_upper) + b_val = parse_g10_value('B', expanded_upper) + c_val = parse_g10_value('C', expanded_upper) + u_val = parse_g10_value('U', expanded_upper) + v_val = parse_g10_value('V', expanded_upper) + w_val = parse_g10_value('W', expanded_upper) + + r_val = parse_g10_value('R', expanded_upper) + i_val = parse_g10_value('I', expanded_upper) + j_val = parse_g10_value('J', expanded_upper) + q_match = re.search(r'Q\s*=?\s*(\d+)', expanded_upper) or re.search(r'Q\s*(\d+)', expanded_upper) + orientation = int(q_match.group(1)) if q_match else None + + tool_idx = self.collector.find_or_create_tool(tool_number) + if tool_idx < 0: + return None + + tool = self.collector.tool_table[tool_idx] + changed = [] + + # X/Y 轴 + if x_val is not None and y_val is not None: + dx = tx - x_val + dy = ty - y_val + rot_rad = math.radians(dest_rotation) + cos_r = math.cos(rot_rad) + sin_r = math.sin(rot_rad) + tool.offset.x = dx * cos_r + dy * sin_r + tool.offset.y = -dx * sin_r + dy * cos_r + changed.append(f'X={tool.offset.x:.3f} Y={tool.offset.y:.3f}') + + # Z 轴 + if z_val is not None: + tool.offset.z = tz - z_val + self.collector.tool_lengths[tool_number] = tool.offset.z + changed.append(f'Z={tool.offset.z:.3f}') + + # 旋转轴 + if a_val is not None: + tool.offset.a = ta - a_val + changed.append(f'A={tool.offset.a:.3f}') + if b_val is not None: + tool.offset.b = tb - b_val + changed.append(f'B={tool.offset.b:.3f}') + if c_val is not None: + tool.offset.c = tc - c_val + changed.append(f'C={tool.offset.c:.3f}') + + # U/V/W + if u_val is not None: + tool.offset.u = tu - u_val + if v_val is not None: + tool.offset.v = tv - v_val + if w_val is not None: + tool.offset.w = tw - w_val + + # 刀具参数 + if r_val is not None: + tool.diameter = r_val + tool.radius = r_val / 2.0 + self.collector.tool_radii[tool_number] = tool.radius + if i_val is not None: + tool.front_angle = i_val + if j_val is not None: + tool.back_angle = j_val + if orientation is not None: + tool.orientation = orientation + + # 同步 + self.collector._sync_tool_parameters_to_table(tool_idx) + self.collector._handle_g10_current_tool_update(tool_idx, tool_number) + + if self.debug: + print(f"[DEBUG] G10 L10 T{tool_number}: 修改 {', '.join(changed)}") + + return None + + # ==================== G10 L11 刀具偏移设置（夹具坐标系基准） ==================== + + if 'G10' in line_upper and 'L11' in line_upper and self.collector: + expanded_line = self.collector.var_manager.expand_variables(line, self.call_level) + expanded_upper = expanded_line.upper() + + if self.debug: + print(f"[DEBUG] ========== G10 L11 刀具偏移设置（夹具坐标系） ==========") + print(f"[DEBUG] 行号: {line_num}") + print(f"[DEBUG] 原始行: '{line.strip()}'") + print(f"[DEBUG] 展开后: '{expanded_line}'") + + p_match = re.search(r'P\s*=?\s*(\d+)', expanded_upper) or re.search(r'P\s*(\d+)', expanded_upper) + if not p_match: + if self.debug: + print(f"[DEBUG] G10 L11 缺少 P 值") + return None + + tool_number = int(p_match.group(1)) + + fixture_system = 9 # G59.3 + tx, ty, tz, ta, tb, tc, tu, tv, tw = self.collector._get_current_in_system_without_tlo(fixture_system) + fixture_rotation = self.collector._get_csys_rotation(fixture_system) + + if self.debug: + print(f"[DEBUG] G10 L11: 夹具坐标系 G59.3, 旋转={fixture_rotation}°") + print(f"[DEBUG] G10 L11: 当前位置(no TLO)=({tx:.3f},{ty:.3f},{tz:.3f})") + + # ★ 使用公共 parse_g10_value 函数 ★ + x_val = parse_g10_value('X', expanded_upper) + y_val = parse_g10_value('Y', expanded_upper) + z_val = parse_g10_value('Z', expanded_upper) + a_val = parse_g10_value('A', expanded_upper) + b_val = parse_g10_value('B', expanded_upper) + c_val = parse_g10_value('C', expanded_upper) + u_val = parse_g10_value('U', expanded_upper) + v_val = parse_g10_value('V', expanded_upper) + w_val = parse_g10_value('W', expanded_upper) + + r_val = parse_g10_value('R', expanded_upper) + i_val = parse_g10_value('I', expanded_upper) + j_val = parse_g10_value('J', expanded_upper) + q_match = re.search(r'Q\s*=?\s*(\d+)', expanded_upper) or re.search(r'Q\s*(\d+)', expanded_upper) + orientation = int(q_match.group(1)) if q_match else None + + tool_idx = self.collector.find_or_create_tool(tool_number) + if tool_idx < 0: + return None + + tool = self.collector.tool_table[tool_idx] + changed = [] + + # X/Y 轴 + if x_val is not None and y_val is not None: + dx = tx - x_val + dy = ty - y_val + rot_rad = math.radians(fixture_rotation) + cos_r = math.cos(rot_rad) + sin_r = math.sin(rot_rad) + tool.offset.x = dx * cos_r + dy * sin_r + tool.offset.y = -dx * sin_r + dy * cos_r + changed.append(f'X={tool.offset.x:.3f} Y={tool.offset.y:.3f}') + + # Z 轴 + if z_val is not None: + tool.offset.z = tz - z_val + self.collector.tool_lengths[tool_number] = tool.offset.z + changed.append(f'Z={tool.offset.z:.3f}') + + # 旋转轴 + if a_val is not None: + tool.offset.a = ta - a_val + if b_val is not None: + tool.offset.b = tb - b_val + if c_val is not None: + tool.offset.c = tc - c_val + + # U/V/W + if u_val is not None: + tool.offset.u = tu - u_val + if v_val is not None: + tool.offset.v = tv - v_val + if w_val is not None: + tool.offset.w = tw - w_val + + # 刀具参数 + if r_val is not None: + tool.diameter = r_val + tool.radius = r_val / 2.0 + self.collector.tool_radii[tool_number] = tool.radius + if i_val is not None: + tool.front_angle = i_val + if j_val is not None: + tool.back_angle = j_val + if orientation is not None: + tool.orientation = orientation + + # 同步 + self.collector._sync_tool_parameters_to_table(tool_idx) + self.collector._handle_g10_current_tool_update(tool_idx, tool_number) + + if self.debug: + print(f"[DEBUG] G10 L11 T{tool_number}: 修改 {', '.join(changed)}") + + return None + + # ==================== G28.1/G30.1 参考点设置（使用参数表） ==================== + + if 'G28.1' in line_upper and self.collector: + expanded_line = self.collector.var_manager.expand_variables(line, self.call_level) + expanded_upper = expanded_line.upper() + + x = y = z = a = b = c = 0.0 + + def parse_coord_simple(ax): + patterns = [ + rf'{ax}\s*=\s*([+-]?\d*\.?\d+)', + rf'{ax}\s*([+-]?\d*\.?\d+)', + rf'{ax}([+-]?\d*\.?\d+)', + ] + for pattern in patterns: + match = re.search(pattern, expanded_upper, re.I) + if match: + try: + return float(match.group(1)) + except ValueError: + continue + return None + + x_val = parse_coord_simple('X') + y_val = parse_coord_simple('Y') + z_val = parse_coord_simple('Z') + + if x_val is not None: x = x_val + if y_val is not None: y = y_val + if z_val is not None: z = z_val + + self.collector.set_g28(x, y, z, a, b, c) + return None + + if 'G30.1' in line_upper and self.collector: + expanded_line = self.collector.var_manager.expand_variables(line, self.call_level) + expanded_upper = expanded_line.upper() + + x = y = z = a = b = c = 0.0 + + x_val = parse_coord_simple('X') + y_val = parse_coord_simple('Y') + z_val = parse_coord_simple('Z') + + if x_val is not None: x = x_val + if y_val is not None: y = y_val + if z_val is not None: z = z_val + + self.collector.set_g30(x, y, z, a, b, c) + return None + + # ==================== 解析坐标（使用参数表展开变量） ==================== + + x = y = z = a = b = c = u = v = w = None + i = j = k = r_coord = None + + if self.collector: + + current = self.collector.current_pos + + # 使用参数表展开变量 + expanded_line = self.collector.var_manager.expand_variables(line, self.call_level) + expanded_upper = expanded_line.upper() + + is_g0 = (re.search(r'\bG0\b', expanded_upper) or re.search(r'\bG00\b', expanded_upper) or + expanded_upper.startswith('G0') or expanded_upper.startswith('G00')) + is_g1 = (re.search(r'\bG1\b', expanded_upper) or re.search(r'\bG01\b', expanded_upper) or + expanded_upper.startswith('G1') or expanded_upper.startswith('G01')) + is_g2 = ('G2' in expanded_upper or 'G02' in expanded_upper) and not any(cmd in expanded_upper for cmd in ['G20', 'G21', 'G28', 'G92']) + is_g3 = ('G3' in expanded_upper or 'G03' in expanded_upper) and not any(cmd in expanded_upper for cmd in ['G30', 'G38']) + is_motion_line = is_g0 or is_g1 or is_g2 or is_g3 + + if self.debug and is_motion_line: + print(f"[DEBUG] ========== 运动指令解析 ==========") + print(f"[DEBUG] 行号: {line_num}") + print(f"[DEBUG] 原始行: '{line.strip()}'") + print(f"[DEBUG] 展开后: '{expanded_line}'") + print(f"[DEBUG] expanded_upper: '{expanded_upper}'") + print(f"[DEBUG] call_level: {self.call_level}") + print(f"[DEBUG] 当前坐标: X={current.x:.3f}, Y={current.y:.3f}, Z={current.z:.3f}") + print(f"[DEBUG] RTCP状态: {self.collector.rtcp_enabled}") + print(f"[DEBUG] is_g0={is_g0}, is_g1={is_g1}, is_g2={is_g2}, is_g3={is_g3}") + + # ===== 修复后的 parse_coord 函数 ===== + def parse_coord(axis): + """ + 解析坐标值 + 支持格式: + - 纯数字: X20, Y-10.5, Z+5 + - 等号格式: X=20, Y=-10.5 + - 方括号表达式: X[#<_x> + #], Y[20 - 10] + + 参数: + axis: 轴名称 ('X', 'Y', 'Z', 'A', 'B', 'C', 'I', 'J', 'K', 'R', 'F', 'S', 'P') + + 返回: + 解析后的浮点数值，如果未找到则返回 None + """ + nonlocal_expanded = expanded_upper + + # ===== 先尝试匹配方括号表达式 ===== + bracket_pattern = rf'{axis}\[(.*?)\]' + bracket_match = re.search(bracket_pattern, nonlocal_expanded, re.IGNORECASE) + if bracket_match: + expr = bracket_match.group(1) + try: + # 展开命名参数 + expanded_expr = self.expand_named_params(expr) + # 计算表达式 + if self.collector and hasattr(self.collector, 'var_manager'): + result = self.collector.var_manager.evaluate_expression(expanded_expr, self.call_level) + else: + # 简单eval后备 + result = eval(expanded_expr, {"__builtins__": {}}, {"math": math}) + if self.debug: + print(f"[DEBUG] parse_coord({axis}): 模式='BRACKET EXPR', " + f"原始表达式='{expr}', 展开='{expanded_expr}', 解析值={result}") + return float(result) + except Exception as e: + if self.debug: + print(f"[DEBUG] parse_coord({axis}): 方括号求值失败 '{expr}': {e}") + return None + + # ===== 再匹配带等号的数字: X=20, Y=-10.5 ===== + eq_pattern = rf'{axis}\s*=\s*([+\-]?\d+\.?\d*)' + eq_match = re.search(eq_pattern, nonlocal_expanded, re.IGNORECASE) + if eq_match: + value = float(eq_match.group(1)) + if self.debug: + print(f"[DEBUG] parse_coord({axis}): 模式='EQ VALUE', " + f"原始匹配='{eq_match.group(1)}', 解析值={value}") + return value + + # ===== 再匹配普通数字: X20, Y-10.5, Z+5 ===== + number_pattern = rf'{axis}([+\-]?\d+\.?\d*)' + number_match = re.search(number_pattern, nonlocal_expanded, re.IGNORECASE) + if number_match: + value = float(number_match.group(1)) + if self.debug: + print(f"[DEBUG] parse_coord({axis}): 模式='AXIS VALUE', " + f"原始匹配='{number_match.group(1)}', 解析值={value}") + return value + + return None + + # 解析所有坐标。保留原始轴字，后续 G92/G52 需要区分 + # “未指定该轴”和“指定为 0”两种情况。 + x_word = parse_coord('X') + y_word = parse_coord('Y') + z_word = parse_coord('Z') + a_word = parse_coord('A') + b_word = parse_coord('B') + c_word = parse_coord('C') + u_word = parse_coord('U') + v_word = parse_coord('V') + w_word = parse_coord('W') + + x = x_word + y = y_word + z = z_word + a = a_word + b = b_word + c = c_word + u = u_word + v = v_word + w = w_word + + i = parse_coord('I') + j = parse_coord('J') + k = parse_coord('K') + r_coord = parse_coord('R') + + + # 进给率 + f_match = re.search(r'F\s*=?\s*([+-]?\d*\.?\d+)', expanded_upper, re.I) + if not f_match: + f_match = re.search(r'F\s*([+-]?\d*\.?\d+)', expanded_upper, re.I) + if f_match: + feed_val = float(f_match.group(1)) + self.collector.set_feed_rate(feed_val) + if self.debug: + print(f"[DEBUG] 设置进给率: F={feed_val}") + + # 主轴速度 + s_match = re.search(r'S\s*=?\s*(\d+)', expanded_upper, re.I) + if not s_match: + s_match = re.search(r'S\s*(\d+)', expanded_upper, re.I) + if s_match: + self.collector.set_spindle_speed(float(s_match.group(1))) + if self.debug: + print(f"[DEBUG] 设置主轴速度: S={s_match.group(1)}") + + # 暂停参数 + p_dwell_match = re.search(r'P\s*=?\s*([+-]?\d*\.?\d+)', expanded_upper, re.I) + if not p_dwell_match: + p_dwell_match = re.search(r'P\s*([+-]?\d*\.?\d+)', expanded_upper, re.I) + + # 圆弧圈数参数 (P) + p_turn_match = re.search(r'P\s*=?\s*([+-]?\d*\.?\d+)', expanded_upper, re.I) + if not p_turn_match: + p_turn_match = re.search(r'P\s*([+-]?\d*\.?\d+)', expanded_upper, re.I) + p_turn = float(p_turn_match.group(1)) if p_turn_match else 1.0 + + # 计算绝对/相对坐标 + if not self.collector.is_absolute: + x = current.x + x if x is not None else current.x + y = current.y + y if y is not None else current.y + z = current.z + z if z is not None else current.z + a = current.a + (a if a is not None else 0) + b = current.b + (b if b is not None else 0) + c = current.c + (c if c is not None else 0) + u = getattr(current, 'u', 0.0) + (u if u is not None else 0) + v = getattr(current, 'v', 0.0) + (v if v is not None else 0) + w = getattr(current, 'w', 0.0) + (w if w is not None else 0) + else: + x = x if x is not None else current.x + y = y if y is not None else current.y + z = z if z is not None else current.z + a = a if a is not None else current.a + b = b if b is not None else current.b + c = c if c is not None else current.c + u = u if u is not None else getattr(current, 'u', 0.0) + v = v if v is not None else getattr(current, 'v', 0.0) + w = w if w is not None else getattr(current, 'w', 0.0) + + # ==================== G代码运动 ==================== + if self.collector: + # 刀具补偿 + h_match = re.search(r'H\s*=?\s*(\d+)', line_upper, re.I) + if not h_match: + h_match = re.search(r'H\s*(\d+)', line_upper, re.I) + if h_match: + h_code = int(h_match.group(1)) + self.collector.set_tool_length_offset(h_code) + + d_match = re.search(r'D\s*=?\s*(\d+)', line_upper, re.I) + if not d_match: + d_match = re.search(r'D\s*(\d+)', line_upper, re.I) + if d_match: + self.collector.set_tool_radius_offset(int(d_match.group(1))) + + t_match = re.search(r'T\s*=?\s*(\d+)', line_upper, re.I) + if not t_match: + t_match = re.search(r'T\s*(\d+)', line_upper, re.I) + if t_match: + self.collector.select_tool(int(t_match.group(1))) + + # 换刀 + if 'M6' in line_upper: + tool = int(t_match.group(1)) if t_match else 1 + self.collector.change_tool(tool) + + # 主轴控制 + if 'M3' in line_upper: + self.collector.spindle_control(1) + elif 'M4' in line_upper: + self.collector.spindle_control(2) + elif 'M5' in line_upper: + self.collector.spindle_control(0) + + # 冷却液控制 + if 'M8' in line_upper: + self.collector.set_flood(True) + elif 'M7' in line_upper: + self.collector.set_mist(True) + elif 'M9' in line_upper: + self.collector.set_flood(False) + self.collector.set_mist(False) + + # M66 等待输入 + if 'M66' in line_upper: + e_match = re.search(r'E\s*=?\s*(\d+)', line_upper) or re.search(r'E\s*(\d+)', line_upper) + l_match = re.search(r'L\s*=?\s*(\d+)', line_upper) or re.search(r'L\s*(\d+)', line_upper) + if e_match: + pin = int(e_match.group(1)) + mode = int(l_match.group(1)) if l_match else 0 + self.collector.wait_input(pin, mode) + + # M68 模拟量输出 + if 'M68' in line_upper: + e_match = re.search(r'E\s*=?\s*(\d+)', line_upper) or re.search(r'E\s*(\d+)', line_upper) + q_match = re.search(r'Q\s*=?\s*([+-]?\d*\.?\d+)', line_upper) or re.search(r'Q\s*([+-]?\d*\.?\d+)', line_upper) + if e_match and q_match: + pin = int(e_match.group(1)) + value = float(q_match.group(1)) + self.collector.set_analog_output(pin, value) + + current = self.collector.current_pos + + # G0 - 快速移动 + if is_g0: + if x is not None or y is not None or z is not None: + if self.debug: + print(f"[G0] 执行快速移动: X={x}, Y={y}, Z={z}, A={a}, B={b}, C={c}") + self.collector.straight_traverse( + x if x is not None else current.x, + y if y is not None else current.y, + z if z is not None else current.z, + a if a is not None else current.a, + b if b is not None else current.b, + c if c is not None else current.c, + u if u is not None else 0, + v if v is not None else 0, + w if w is not None else 0 + ) + + # G1 - 直线进给 + elif is_g1: + if x is not None or y is not None or z is not None: + if self.debug: + print(f"[G1] 执行直线进给: X={x}, Y={y}, Z={z}") + self.collector.straight_feed( + x if x is not None else current.x, + y if y is not None else current.y, + z if z is not None else current.z, + a if a is not None else current.a, + b if b is not None else current.b, + c if c is not None else current.c, + u if u is not None else 0, + v if v is not None else 0, + w if w is not None else 0 + ) + + # G2 - 顺时针圆弧 + + + # G2 - 顺时针圆弧 + elif is_g2: + turn = -int(p_turn) if p_turn >= 1 else -1 + + # 如果只指定IJK没指定X/Y，终点=起点(全圆) + if x is None and y is None: + if i is not None or j is not None: + x = current.x + y = current.y + + if x is not None or y is not None: + x = x if x is not None else current.x + y = y if y is not None else current.y + + # ===== R格式圆弧：计算圆心 ===== + if r_coord is not None and (i is None or j is None): + dx = x - current.x + dy = y - current.y + d = math.sqrt(dx*dx + dy*dy) + if d > 0 and abs(r_coord) >= d/2 - 1e-9: + h = math.sqrt(max(0, r_coord*r_coord - d*d/4)) + # G2 (turn < 0) + if r_coord > 0: + # R>0: 小弧(<180°) + cx_calc = current.x + dx/2 - h * dy/d + cy_calc = current.y + dy/2 + h * dx/d + else: + # R<0: 大弧(>180°) + cx_calc = current.x + dx/2 + h * dy/d + cy_calc = current.y + dy/2 - h * dx/d + + + + # 转换为增量IJ + i = cx_calc - current.x + j = cy_calc - current.y + else: + # 半圆或无效R + i = (x - current.x) / 2 + j = (y - current.y) / 2 + + # ===== 确保IJ是增量（相对于起点） ===== + # ===== IJK 处理逻辑（对照 LinuxCNC interp_convert.cc arc_data_ijk）===== + # 在 G91.1（增量模式）下，I, J 被视为相对于起点 + # 在 G90.1（绝对模式）下，I, J 被视为绝对坐标 + i_final = i if i is not None else 0.0 + j_final = j if j is not None else 0.0 + k_final = k if k is not None else 0.0 + + # 如果是绝对 IJK 模式 (G90.1)，转换为增量 + if self.collector and self.collector.ijk_distance_mode == 90: # 90 = G90.1 绝对IJK + i_final = i_final - current.x + j_final = j_final - current.y + k_final = k_final - current.z + # 在增量模式下（G91.1，默认），IJK 已经是相对于起点的值，无需转换 + + if self.debug: + center_x_dbg = current.x + i_final + center_y_dbg = current.y + j_final + print(f"[G2] 执行圆弧: end=({x},{y}) " + f"center=({center_x_dbg:.1f},{center_y_dbg:.1f}) " + f"I={i_final:.3f} J={j_final:.3f} turn={turn}") + + self.collector.arc_feed( + x, y, + z if z is not None else current.z, + a if a is not None else current.a, + b if b is not None else current.b, + c if c is not None else current.c, + i_final if i_final != 0 else 0, + j_final if j_final != 0 else 0, + k if k is not None else 0, + turn, self.collector.feedrate, + u if u is not None else 0, + v if v is not None else 0, + w if w is not None else 0 + ) + + # G3 - 逆时针圆弧 + + # G3 - 逆时针圆弧 + elif is_g3: + turn = int(p_turn) if p_turn >= 1 else 1 + + # 如果只指定IJK没指定X/Y，终点=起点(全圆) + if x is None and y is None: + if i is not None or j is not None: + x = current.x + y = current.y + + if x is not None or y is not None: + x = x if x is not None else current.x + y = y if y is not None else current.y + + # ===== R格式圆弧：计算圆心 ===== + if r_coord is not None and (i is None or j is None): + dx = x - current.x + dy = y - current.y + d = math.sqrt(dx*dx + dy*dy) + if d > 0 and abs(r_coord) >= d/2 - 1e-9: + h = math.sqrt(max(0, r_coord*r_coord - d*d/4)) + # G3 (turn > 0) + if r_coord > 0: + # R>0: 小弧(<180°) + cx_calc = current.x + dx/2 + h * dy/d + cy_calc = current.y + dy/2 - h * dx/d + else: + # R<0: 大弧(>180°) + cx_calc = current.x + dx/2 - h * dy/d + cy_calc = current.y + dy/2 + h * dx/d + # 转换为增量IJ + i = cx_calc - current.x + j = cy_calc - current.y + else: + # 半圆或无效R + i = (x - current.x) / 2 + j = (y - current.y) / 2 + + # ===== 确保IJ是增量（相对于起点） ===== + # ===== IJK 处理逻辑（对照 LinuxCNC interp_convert.cc arc_data_ijk）===== + # 在 G91.1（增量模式）下，I, J 被视为相对于起点 + # 在 G90.1（绝对模式）下，I, J 被视为绝对坐标 + i_final = i if i is not None else 0.0 + j_final = j if j is not None else 0.0 + k_final = k if k is not None else 0.0 + + # 如果是绝对 IJK 模式 (G90.1)，转换为增量 + if self.collector and self.collector.ijk_distance_mode == 90: # 90 = G90.1 绝对IJK + i_final = i_final - current.x + j_final = j_final - current.y + k_final = k_final - current.z + # 在增量模式下（G91.1，默认），IJK 已经是相对于起点的值，无需转换 + + if self.debug: + center_x_dbg = current.x + i_final + center_y_dbg = current.y + j_final + print(f"[G3] 执行圆弧: end=({x},{y}) " + f"center=({center_x_dbg:.1f},{center_y_dbg:.1f}) " + f"I={i_final:.3f} J={j_final:.3f} turn={turn}") + + self.collector.arc_feed( + x, y, + z if z is not None else current.z, + a if a is not None else current.a, + b if b is not None else current.b, + c if c is not None else current.c, + i_final if i_final != 0 else 0, + j_final if j_final != 0 else 0, + k if k is not None else 0, + turn, self.collector.feedrate, + u if u is not None else 0, + v if v is not None else 0, + w if w is not None else 0 + ) + + # G4 - 暂停 + elif re.search(r'\bG4\b', expanded_upper) or re.search(r'\bG04\b', expanded_upper): + dwell_time = 0.0 + if p_dwell_match: + dwell_time = float(p_dwell_match.group(1)) / 1000.0 + if dwell_time > 0: + self.collector.dwell(dwell_time) + + # G28/G30 返回参考点 + if 'G28' in line_upper and self.collector and 'G28.1' not in line_upper: + ref_x, ref_y, ref_z = self.collector.get_g28() + self.collector.straight_traverse(ref_x, ref_y, ref_z, current.a, current.b, current.c, 0, 0, 0) + + if 'G30' in line_upper and self.collector and 'G30.1' not in line_upper: + ref_x, ref_y, ref_z = self.collector.get_g30() + self.collector.straight_traverse(ref_x, ref_y, ref_z, current.a, current.b, current.c, 0, 0, 0) + + # ==================== 平面选择 ==================== + + if re.search(r'\bG17\b', line_upper): + self.current_plane = 17 + if self.collector: + self.collector.select_plane(17) + elif re.search(r'\bG18\b', line_upper): + self.current_plane = 18 + if self.collector: + self.collector.select_plane(18) + elif re.search(r'\bG19\b', line_upper): + self.current_plane = 19 + if self.collector: + self.collector.select_plane(19) + + # ==================== 坐标模式 ==================== + + if re.search(r'\bG90\b', line_upper) and self.collector: + self.collector.set_absolute_mode() + elif re.search(r'\bG91\b', line_upper) and self.collector: + self.collector.set_relative_mode() + + # ==================== 单位 ==================== + + if re.search(r'\bG20\b', line_upper) and self.collector: + self.collector.state.units = 20 + if self.debug: + print(f"[DEBUG] 设置为英制单位 G20") + elif re.search(r'\bG21\b', line_upper) and self.collector: + self.collector.state.units = 21 + if self.debug: + print(f"[DEBUG] 设置为公制单位 G21") + + # ==================== 刀具半径补偿 ==================== + + if re.search(r'\bG40\b', line_upper) and self.collector: + self.collector.set_cutter_compensation_state(CompType.OFF, 0.0, 0, 0) + + if (re.search(r'\bG41\b', line_upper) or re.search(r'\bG41\.1\b', line_upper)) and self.collector: + d_match = re.search(r'D\s*=?\s*(\d+)', line, re.I) or re.search(r'D\s*(\d+)', line, re.I) + if d_match: + d_code = int(d_match.group(1)) + else: + d_code = self.collector.current_d_code if self.collector.current_d_code > 0 else 1 + + radius = self.tool_radius_map.get(d_code, 0.0) + self.collector.set_cutter_compensation_state(CompType.LEFT, radius, 0, d_code) + self.collector.set_tool_radius_offset(d_code) + + elif (re.search(r'\bG42\b', line_upper) or re.search(r'\bG42\.1\b', line_upper)) and self.collector: + d_match = re.search(r'D\s*=?\s*(\d+)', line, re.I) or re.search(r'D\s*(\d+)', line, re.I) + if d_match: + d_code = int(d_match.group(1)) + else: + d_code = self.collector.current_d_code if self.collector.current_d_code > 0 else 1 + + radius = self.tool_radius_map.get(d_code, 0.0) + self.collector.set_cutter_compensation_state(CompType.RIGHT, radius, 0, d_code) + self.collector.set_tool_radius_offset(d_code) + + # ==================== 固定循环 ==================== + + if re.search(r'\bG80\b', line_upper) and self.collector: + self.collector.canned_cycle_active = False + + # ==================== 坐标系设置 ==================== + + if re.search(r'\bG92\b', line_upper) and self.collector and 'G92.1' not in line_upper and 'G92.2' not in line_upper and 'G92.3' not in line_upper: + self.collector.set_g92_offset( + x_word if 'x_word' in locals() else None, + y_word if 'y_word' in locals() else None, + z_word if 'z_word' in locals() else None, + a_word if 'a_word' in locals() else None, + b_word if 'b_word' in locals() else None, + c_word if 'c_word' in locals() else None + ) + + if re.search(r'\bG92\.1\b', line_upper) and self.collector: + self.collector.clear_g92() + + if re.search(r'\bG52\b', line_upper) and self.collector: + self.collector.set_g52_offset( + x if x is not None else 0, + y if y is not None else 0, + z if z is not None else 0 + ) + + return None + + + + def parse_string(self, program: str, filename: str = "program.ngc") -> ToolpathData: + import time + + start_time = time.time() + + self.current_filename = filename + + lines = program.split('\n') + processed_lines = [] + line_number_map = [] + + for line_num, line in enumerate(lines, 1): + processed = self._preprocess_line(line) + if processed and processed.strip(): + if re.match(r'o<[^>]+>\s*sub', processed.lower()): + continue + if re.match(r'o<[^>]+>\s*endsub', processed.lower()): + continue + processed_lines.append(processed) + line_number_map.append(line_num) + self._line_number_map[len(processed_lines) - 1] = line_num + + self.collector = GLCanonPathCollectorWithRTCP( + colors=DEFAULT_COLORS.copy(), max_points=self.max_points, + acceleration=self.acceleration, max_rapid_rate=self.max_rapid_rate, + max_feed_rate=self.max_feed_rate, kinematics_type=self.kinematics_type, + kinematics_params=self.kinematics_params, debug=self.debug + ) + + for h_code, length in self.tool_length_map.items(): + self.collector.tool_lengths[h_code] = length + for d_code, radius in self.tool_radius_map.items(): + self.collector.tool_radii[d_code] = radius + + control_handler = self.collector.control_handler + + i = 0 + loop_count = 0 + max_iterations = len(processed_lines) * 100 + + while i < len(processed_lines): + loop_count += 1 + if loop_count > max_iterations: + if self.debug: + print(f" [警告] 解析达到最大迭代次数") + break + + if self.collector and len(self.collector.data.segments) >= self.max_points: + if self.debug: + print(f" [警告] 达到最大点数限制 {self.max_points}") + break + + line = processed_lines[i] + original_line_num = line_number_map[i] + + if line.startswith('/') and self.collector and self.collector.block_delete: + i += 1 + continue + + if control_handler and line.strip(): + next_idx = control_handler.parse_o_code( + line, original_line_num, i, processed_lines, self.call_level + ) + if next_idx is not None: + i = next_idx + continue + + self._parse_line(line, original_line_num) + i += 1 + + if self.saved_states: + for saved_state in reversed(self.saved_states): + if saved_state.get('restore_on_return', False) and self.collector: + self.collector.restore_state(saved_state) + + if self.debug: + print(f" [解析完成] 生成 {len(self.collector.data.segments) if self.collector else 0} 个路径段") + + if self.collector: + self.collector.data.filename = filename + self.collector.data.kinematics_type = self.kinematics_type.name + self.collector.data.point_count = len(self.collector.data.segments) + self.collector.data.generation_time_ms = (time.time() - start_time) * 1000 + self.collector.data.calculate_bounds() + self.collector.data.calculate_length_and_time() + + return self.collector.data + + return ToolpathData() + + +# ==================== 完整RS274解析器 ==================== + +class FullRS274Parser: + """完整的RS274NGC解释器包装器 - 支持多实例""" + + def __init__(self, max_points: int = 50000, + tool_radius_map: Dict[int, float] = None, + tool_length_map: Dict[int, float] = None, + acceleration: float = 500.0, + max_rapid_rate: float = 10000.0, + max_feed_rate: float = 5000.0, + kinematics_type: KinematicsType = KinematicsType.TRT_BC, + kinematics_params: Dict[str, Any] = None, + debug: bool = False): + + self.max_points = max_points + self.tool_radius_map = tool_radius_map or {1: 2.0, 2: 3.0, 3: 4.0} + self.tool_length_map = tool_length_map or {1: 100.0, 2: 120.0, 3: 150.0} + self.acceleration = acceleration + self.max_rapid_rate = max_rapid_rate + self.max_feed_rate = max_feed_rate + self.kinematics_type = kinematics_type + self.kinematics_params = kinematics_params or {} + self.debug = debug + + self._builtin_parser: Optional[FullGCodeParserWithRTCP] = None + self.collector: Optional[GLCanonPathCollectorWithRTCP] = None + + self._init_parser() + + def _init_parser(self): + self._builtin_parser = FullGCodeParserWithRTCP( + max_points=self.max_points, + tool_radius_map=self.tool_radius_map, + tool_length_map=self.tool_length_map, + acceleration=self.acceleration, + max_rapid_rate=self.max_rapid_rate, + max_feed_rate=self.max_feed_rate, + kinematics_type=self.kinematics_type, + kinematics_params=self.kinematics_params, + debug=self.debug + ) + + def register_subroutine(self, name: str, content: str): + if not self._builtin_parser: + self._init_parser() + + lines = content.split('\n') + sub_lines = [] + sub_line_nums = [] + + in_sub = False + for i, line in enumerate(lines): + processed = self._builtin_parser._preprocess_line(line) + if processed and processed.strip(): + if re.match(r'o<([^>]+)>\s*sub', processed, re.I): + in_sub = True + continue + if re.match(r'o<([^>]+)>\s*endsub', processed, re.I): + in_sub = False + continue + if in_sub: + sub_lines.append(processed) + sub_line_nums.append(i + 1) + + if sub_lines: + self._builtin_parser.subroutines[name] = list(zip(sub_line_nums, sub_lines)) + + def parse_string(self, program: str, filename: str = "program.ngc") -> ToolpathData: + if not self._builtin_parser: + self._init_parser() + + return self._builtin_parser.parse_string(program, filename) + + def parse_file(self, filename: str) -> ToolpathData: + if not os.path.exists(filename): + raise FileNotFoundError(f"文件不存在: {filename}") + + with open(filename, 'r', encoding='utf-8', errors='ignore') as f: + program = f.read() + + return self.parse_string(program, os.path.basename(filename)) + + def set_subroutine_path(self, path: str): + if self._builtin_parser: + self._builtin_parser.set_subroutine_path(path) + + +# ==================== CNC内核 ==================== + +class CNCKernel: + """CNC内核 - 整合所有功能的主控制器""" + + def __init__(self, debug: bool = False, name: str = "CNC1"): + self.name = name + self.debug = debug + self.virtual_hal = VirtualHAL(debug=debug) + self.state_machine = MachineStateMachine(self.virtual_hal, debug=debug) + + self.parser: Optional[FullRS274Parser] = None + self.current_toolpath: Optional[ToolpathData] = None + self.status = CNCRuntimeStatus() + + self.is_running = False + self.is_paused = False + self.current_segment_index = 0 + self.program_ended = False + self.single_step_mode = False + + self.planner = TrapezoidalMotionPlanner() + + self.rtcp_enabled = False + self._kinematics_type = KinematicsType.TRT_BC + self._kinematics_instance: Optional[BaseKinematics] = None + self._original_kinematics_type = KinematicsType.TRT_BC + + self._update_kinematics_instance() + + self.run_thread: Optional[threading.Thread] = None + self.state_monitor_thread: Optional[threading.Thread] = None + self.state_monitor_running = False + + self.update_callback: Optional[Callable[[CNCRuntimeStatus], None]] = None + + self.tool_table = { + 1: {"length": 100.0, "diameter": 10.0, "name": "端铣刀"}, + 2: {"length": 120.0, "diameter": 8.0, "name": "球头刀"}, + 3: {"length": 80.0, "diameter": 12.0, "name": "面铣刀"}, + 4: {"length": 150.0, "diameter": 6.0, "name": "钻头"}, + 5: {"length": 90.0, "diameter": 10.0, "name": "倒角刀"}, + } + + # 初始化默认工具长度 + self.selected_tool = 1 + if 1 in self.tool_table: + self.status.tool_length = self.tool_table[1].get('length', 100.0) + self.status.tool_diameter = self.tool_table[1].get('diameter', 10.0) + self.status.current_tool = 1 + + # 设置默认坐标系 + self.status.coordinate_mode = "G54" + self.status.current_offset = "G54" + + self.work_offsets = { + 54: {'x': 0, 'y': 0, 'z': 0, 'a': 0, 'b': 0, 'c': 0}, + 55: {'x': 0, 'y': 0, 'z': 0, 'a': 0, 'b': 0, 'c': 0}, + 56: {'x': 0, 'y': 0, 'z': 0, 'a': 0, 'b': 0, 'c': 0}, + 57: {'x': 0, 'y': 0, 'z': 0, 'a': 0, 'b': 0, 'c': 0}, + 58: {'x': 0, 'y': 0, 'z': 0, 'a': 0, 'b': 0, 'c': 0}, + 59: {'x': 0, 'y': 0, 'z': 0, 'a': 0, 'b': 0, 'c': 0}, + } + self.current_work_offset = 54 + + + # 设置默认坐标系显示 + self.status.coordinate_mode = "G54" # 改为G54而非G90 + self.status.current_offset = "G54" + + # 设置默认RTCP状态 + self.status.rtcp_enabled = False # 默认关闭，等G43.4时开启 + + + + self._init_hal() + self.start_state_monitor() + + + print(f"✓ CNC内核 [{self.name}] 初始化完成") + + @property + def kinematics_type(self) -> KinematicsType: + return self._kinematics_type + + @kinematics_type.setter + def kinematics_type(self, value: KinematicsType): + if self._kinematics_type != value: + self._kinematics_type = value + self._update_kinematics_instance() + self.status.kinematics_type = value.name + + def _update_kinematics_instance(self): + if self._kinematics_type == KinematicsType.IDENTITY: + self._kinematics_instance = None + else: + self._kinematics_instance = KinematicsFactory.create( + self._kinematics_type, debug=self.debug + ) + if self._kinematics_instance: + self._kinematics_instance.set_tool_length(self.status.tool_length) + self._kinematics_instance.enable_rtcp(self.rtcp_enabled) + + def _init_hal(self): + self.virtual_hal.set('motion.kins-type', 2) + self.virtual_hal.set('motion.pivot-length', 250.0) + self.virtual_hal.set('motion.tool-length', 100.0) + + def _sync_offsets_to_status(self): + """从解析器的收集器同步偏移信息到status对象""" + try: + if hasattr(self, 'parser') and self.parser: + parser = self.parser + if hasattr(parser, '_builtin_parser') and parser._builtin_parser: + bp = parser._builtin_parser + if bp.collector: + c = bp.collector + # 同步 G5x 偏移 + self.status.g5x_offset_x = c.g5x_offset_x + self.status.g5x_offset_y = c.g5x_offset_y + self.status.g5x_offset_z = c.g5x_offset_z + self.status.g5x_offset_a = getattr(c, 'g5x_offset_a', 0.0) + self.status.g5x_offset_b = getattr(c, 'g5x_offset_b', 0.0) + self.status.g5x_offset_c = getattr(c, 'g5x_offset_c', 0.0) + + # 同步 G92 偏移 + self.status.g92_x = c.g92_offset_x + self.status.g92_y = c.g92_offset_y + self.status.g92_z = c.g92_offset_z + self.status.g92_a = getattr(c, 'g92_offset_a', 0.0) + self.status.g92_b = getattr(c, 'g92_offset_b', 0.0) + self.status.g92_c = getattr(c, 'g92_offset_c', 0.0) + + # 同步刀具长度 + self.status.tool_length = c.tool_offset.z + + # ★ 关键：同步 RTCP 状态 ★ + self.status.rtcp_enabled = c.rtcp_enabled + self.rtcp_enabled = c.rtcp_enabled + + # 同步坐标系 + if c.g5x_index: + self.status.current_offset = f'G{54 + c.g5x_index - 1}' + self.status.g5x_index = c.g5x_index + + return True + except Exception as e: + if self.debug: + print(f"[{self.name}] 同步偏移信息失败: {e}") + return False + + + + def _update_world_position(self): + if self.status.rtcp_enabled and self._kinematics_instance is not None: + # 根据运动学类型构建正确顺序的joints数组 + # TRT_AC: [X, Y, Z, A, C] (5个) + # TRT_BC: [X, Y, Z, B, C] (5个) + config = KINEMATICS_JOINT_CONFIG.get( + self._kinematics_type if hasattr(self, '_kinematics_type') else self.kinematics_type, + KINEMATICS_JOINT_CONFIG[KinematicsType.IDENTITY] + ) + num_joints = config.get('num_joints', 6) + joints = [0.0] * num_joints + + x_idx = config.get('x_idx', 0) + y_idx = config.get('y_idx', 1) + z_idx = config.get('z_idx', 2) + a_idx = config.get('a_idx', 3) + b_idx = config.get('b_idx', 4) + c_idx = config.get('c_idx', 5) + + if x_idx >= 0: joints[x_idx] = self.status.machine_x + if y_idx >= 0: joints[y_idx] = self.status.machine_y + if z_idx >= 0: joints[z_idx] = self.status.machine_z + if a_idx >= 0: joints[a_idx] = self.status.machine_a + if b_idx >= 0: joints[b_idx] = self.status.machine_b + if c_idx >= 0: joints[c_idx] = self.status.machine_c + + world = self._kinematics_instance.joints_to_world(joints) + self.status.world_x = world[0] + self.status.world_y = world[1] + self.status.world_z = world[2] + else: + self.status.world_x = self.status.position_x + self.status.world_y = self.status.position_y + self.status.world_z = self.status.position_z + + + + def _update_world_position_bak(self): + if self.status.rtcp_enabled and self._kinematics_instance is not None: + # ★修复: 使用MCS(关节坐标)调用正运动学, 不是WCS(工件坐标)★ + joints = [self.status.machine_x, self.status.machine_y, self.status.machine_z, + self.status.machine_a, self.status.machine_b, self.status.machine_c] + world = self._kinematics_instance.joints_to_world(joints) + self.status.world_x = world[0] + self.status.world_y = world[1] + self.status.world_z = world[2] + else: + self.status.world_x = self.status.position_x + self.status.world_y = self.status.position_y + self.status.world_z = self.status.position_z + + def _update_current_from_machine(self): + """ + 从机器坐标反向计算工件坐标 + + 逆变换顺序: + 机器坐标 → 正运动学 → 绝对坐标 → -偏移链 → 工件坐标 + + 与 _sync_machine_from_current 互为逆操作： + - _sync_machine_from_current: 工件→机器 + - _update_current_from_machine: 机器→工件 + """ + # ===== 第1步：正运动学 - 机器坐标→绝对坐标 ===== + if self.rtcp_enabled and self._kinematics_instance is not None: + # 构建符合运动学类型的关节数组 + config = KINEMATICS_JOINT_CONFIG.get( + self._kinematics_type if hasattr(self, '_kinematics_type') else self.kinematics_type, + KINEMATICS_JOINT_CONFIG[KinematicsType.IDENTITY] + ) + num_joints = config.get('num_joints', 6) + joints = [0.0] * num_joints + + x_idx = config.get('x_idx', 0) + y_idx = config.get('y_idx', 1) + z_idx = config.get('z_idx', 2) + a_idx = config.get('a_idx', 3) + b_idx = config.get('b_idx', 4) + c_idx = config.get('c_idx', 5) + + if x_idx >= 0 and x_idx < num_joints: + joints[x_idx] = self.status.machine_x + if y_idx >= 0 and y_idx < num_joints: + joints[y_idx] = self.status.machine_y + if z_idx >= 0 and z_idx < num_joints: + joints[z_idx] = self.status.machine_z + if a_idx >= 0 and a_idx < num_joints: + joints[a_idx] = self.status.machine_a + if b_idx >= 0 and b_idx < num_joints: + joints[b_idx] = self.status.machine_b + if c_idx >= 0 and c_idx < num_joints: + joints[c_idx] = self.status.machine_c + + try: + world = self._kinematics_instance.joints_to_world(joints) + abs_x = world[0] + abs_y = world[1] + abs_z = world[2] + abs_a = world[3] if len(world) > 3 else self.status.machine_a + abs_b = world[4] if len(world) > 4 else self.status.machine_b + abs_c = world[5] if len(world) > 5 else self.status.machine_c + except Exception as e: + if self.debug: + print(f"[{self.name}] 正运动学计算失败: {e}") + abs_x = self.status.machine_x + abs_y = self.status.machine_y + abs_z = self.status.machine_z + abs_a = self.status.machine_a + abs_b = self.status.machine_b + abs_c = self.status.machine_c + else: + # RTCP禁用：机器坐标=绝对坐标 + abs_x = self.status.machine_x + abs_y = self.status.machine_y + abs_z = self.status.machine_z + abs_a = self.status.machine_a + abs_b = self.status.machine_b + abs_c = self.status.machine_c + + # ===== 第2步：逆变换 - 绝对坐标→工件坐标 ===== + # 获取偏移值 + g5x_x = getattr(self.status, 'g5x_offset_x', 0.0) + g5x_y = getattr(self.status, 'g5x_offset_y', 0.0) + g5x_z = getattr(self.status, 'g5x_offset_z', 0.0) + g5x_a = getattr(self.status, 'g5x_offset_a', 0.0) + g5x_b = getattr(self.status, 'g5x_offset_b', 0.0) + g5x_c = getattr(self.status, 'g5x_offset_c', 0.0) + + g92_x = getattr(self.status, 'g92_x', 0.0) + g92_y = getattr(self.status, 'g92_y', 0.0) + g92_z = getattr(self.status, 'g92_z', 0.0) + g92_a = getattr(self.status, 'g92_a', 0.0) + g92_b = getattr(self.status, 'g92_b', 0.0) + g92_c = getattr(self.status, 'g92_c', 0.0) + + tlo = getattr(self.status, 'tool_length', 0.0) + + # 逆序减去偏移（正变换的逆操作） + # 正变换: 工件 + G92 + XY旋转 + G5x + TLO = 绝对 + # 逆变换: 绝对 - TLO - G5x - XY旋转 - G92 = 工件 + + x = abs_x + y = abs_y + z = abs_z + a = abs_a + b = abs_b + c = abs_c + + # 减去 TLO（刀具长度补偿主要在Z方向） + z = z - tlo + + # 减去 G5x 偏移 + x = x - g5x_x + y = y - g5x_y + z = z - g5x_z + a = a - g5x_a + b = b - g5x_b + c = c - g5x_c + + # 减去 G92 偏移 + x = x - g92_x + y = y - g92_y + z = z - g92_z + a = a - g92_a + b = b - g92_b + c = c - g92_c + + # 更新工件坐标 + self.status.position_x = x + self.status.position_y = y + self.status.position_z = z + self.status.position_a = a + self.status.position_b = b + self.status.position_c = c + + # 更新世界坐标 + self._update_world_position() + + if self.debug: + print(f"[MCS→WCS] machine({self.status.machine_x:.1f},{self.status.machine_y:.1f}," + f"{self.status.machine_z:.1f}) → workpiece({x:.1f},{y:.1f},{z:.1f})") + + + def _sync_offsets_to_status(self): + """从解析器的收集器同步偏移信息到status对象""" + try: + if hasattr(self, 'parser') and self.parser: + parser = self.parser + if hasattr(parser, '_builtin_parser') and parser._builtin_parser: + bp = parser._builtin_parser + if bp.collector: + c = bp.collector + # 同步 G5x 偏移 + self.status.g5x_offset_x = c.g5x_offset_x + self.status.g5x_offset_y = c.g5x_offset_y + self.status.g5x_offset_z = c.g5x_offset_z + self.status.g5x_offset_a = c.g5x_offset_a + self.status.g5x_offset_b = c.g5x_offset_b + self.status.g5x_offset_c = c.g5x_offset_c + + # 同步 G92 偏移 + self.status.g92_x = c.g92_offset_x + self.status.g92_y = c.g92_offset_y + self.status.g92_z = c.g92_offset_z + self.status.g92_a = c.g92_offset_a + self.status.g92_b = c.g92_offset_b + self.status.g92_c = c.g92_offset_c + + # 同步刀具长度 + self.status.tool_length = c.tool_offset.z + + # 同步 RTCP 状态 + self.status.rtcp_enabled = c.rtcp_enabled + + # 同步坐标系 + if c.g5x_index: + self.status.current_offset = f'G{54 + c.g5x_index - 1}' + self.status.g5x_index = c.g5x_index + except Exception as e: + if self.debug: + print(f"[{self.name}] 同步偏移信息失败: {e}") + + + def _notify_update(self): + """通知更新 - 增强版，包含关节坐标""" + self.virtual_hal.update_axis_position('x', self.status.position_x) + self.virtual_hal.update_axis_position('y', self.status.position_y) + self.virtual_hal.update_axis_position('z', self.status.position_z) + + # 同时更新HAL中的机器坐标 + self.virtual_hal.set('joint.0.position', self.status.machine_x) + self.virtual_hal.set('joint.1.position', self.status.machine_y) + self.virtual_hal.set('joint.2.position', self.status.machine_z) + + self.virtual_hal.set('spindle.0.speed', self.status.spindle_speed) + self.virtual_hal.set('coolant.flood', 1 if self.status.coolant_flood else 0) + self.virtual_hal.set('coolant.mist', 1 if self.status.coolant_mist else 0) + self.virtual_hal.set('tool.number', self.status.current_tool) + self.virtual_hal.set('motion.rtcp-active', 1 if self.status.rtcp_enabled else 0) + + if self.update_callback: + try: + self.update_callback(self.status) + except Exception as e: + if self.debug: + print(f"[{self.name}] 回调执行错误: {e}") + + def start_state_monitor(self): + if self.state_monitor_thread is None: + self.state_monitor_running = True + self.state_monitor_thread = threading.Thread(target=self._monitor_state, daemon=True) + self.state_monitor_thread.start() + + def _monitor_state(self): + while self.state_monitor_running: + self.state_machine.update() + self.status.state = self.state_machine.get_state() + time.sleep(0.1) + + def ensure_operational(self) -> bool: + current_state = self.state_machine.get_state() + + if current_state == MachineStateMachine.STATE_IDLE: + return True + + if current_state == MachineStateMachine.STATE_OFF: + self.power_on() + time.sleep(0.5) + return self.state_machine.get_state() == MachineStateMachine.STATE_IDLE + + if current_state == MachineStateMachine.STATE_ESTOP: + self.reset() + time.sleep(0.5) + return self.state_machine.get_state() == MachineStateMachine.STATE_IDLE + + return False + + def power_on(self) -> Dict: + if self.state_machine.request_power_on(): + time.sleep(0.5) + self.status.state = self.state_machine.get_state() + self._notify_update() + return {"success": True, "message": "上电成功"} + return {"success": False, "message": "无法上电"} + + def reset(self) -> Dict: + if self.state_machine.reset(): + time.sleep(0.5) + self.status.state = self.state_machine.get_state() + self.status.alarm_code = 0 + self.status.alarm_message = "" + self._notify_update() + return {"success": True, "message": "复位成功"} + return {"success": False, "message": "无法复位"} + + def emergency_stop(self) -> Dict: + if self.state_machine.emergency_stop(): + self.is_running = False + self.is_paused = False + self.status.state = MachineState.ESTOP.value + self._notify_update() + return {"success": True, "message": "急停已激活"} + return {"success": False, "message": "无法急停"} + + def load_program_string(self, program: str, filename: str = "program.ngc") -> Dict: + try: + self.parser = FullRS274Parser( + debug=self.debug, kinematics_type=self.kinematics_type + ) + self.current_toolpath = self.parser.parse_string(program, filename) + + self.status.total_lines = len(self.current_toolpath.segments) + self.status.current_line = 0 + self.status.progress_percent = 0 + self.status.rtcp_enabled = self.current_toolpath.rtcp_enabled + self.status.kinematics_type = self.current_toolpath.kinematics_type + self.status.program_name = filename + self.status.path_length = self.current_toolpath.total_length + self.status.total_time = self.current_toolpath.total_time + + self._notify_update() + + return {"success": True, "message": f"程序加载成功", "segments": len(self.current_toolpath.segments)} + except Exception as e: + return {"success": False, "message": f"加载失败: {str(e)}"} + + def start_program(self) -> Dict: + if not self.ensure_operational(): + return {"success": False, "message": "机床未就绪"} + + if not self.current_toolpath: + return {"success": False, "message": "没有加载程序"} + + self.state_machine.start_program() + self.is_running = True + self.is_paused = False + self.current_segment_index = 0 + + # 在这行之后添加: + self.program_ended = False + + self.status.state = MachineState.RUNNING.value + + self.run_thread = threading.Thread(target=self._run_program, daemon=True) + self.run_thread.start() + + self._notify_update() + return {"success": True, "message": "程序开始运行"} + + def pause_program(self) -> Dict: + if not self.is_running: + return {"success": False, "message": "程序未运行"} + + self.state_machine.pause_program() + self.is_paused = True + self.status.state = MachineState.PAUSED.value + self._notify_update() + return {"success": True, "message": "程序已暂停"} + + def resume_program(self) -> Dict: + if not self.is_paused: + return {"success": False, "message": "程序未暂停"} + + self.state_machine.resume_program() + self.is_paused = False + self.status.state = MachineState.RUNNING.value + self._notify_update() + return {"success": True, "message": "程序已恢复"} + + def stop_program(self) -> Dict: + self.state_machine.stop_program() + self.is_running = False + self.is_paused = False + self.status.state = MachineState.IDLE.value + self._notify_update() + return {"success": True, "message": "程序已停止"} + + + def _run_program(self): + """按照真实速度曲线执行G代码""" + if not self.current_toolpath: + return + + total_segments = len(self.current_toolpath.segments) + + if self.debug: + print(f"\n{'='*60}") + print(f"[执行] 开始执行程序: {self.status.program_name}") + print(f"[执行] 总段数: {total_segments}") + print(f"[执行] 路径总长: {self.current_toolpath.total_length:.2f} mm") + print(f"[执行] 预估时间: {self.current_toolpath.total_time:.2f} s") + print(f"{'='*60}\n") + + # 预计算所有运动段的速度曲线（用于精确时间估算） + motion_profiles = [] + for segment in self.current_toolpath.segments: + if segment.type in [MoveType.RAPID, MoveType.FEED]: + start = segment.start + end = segment.end + dx = end.x - start.x + dy = end.y - start.y + dz = end.z - start.z + # ★ 修复: 将旋转角度变化计入等效距离 ★ + da = end.a - start.a + db = end.b - start.b + dc = end.c - start.c + linear_dist = math.sqrt(dx*dx + dy*dy + dz*dz) + angular_dist = math.sqrt(da*da + db*db + dc*dc) + distance = math.sqrt(linear_dist*linear_dist + angular_dist*angular_dist) + if distance < CART_FUZZ and angular_dist > CART_FUZZ: + distance = angular_dist + + is_rapid = (segment.type == MoveType.RAPID) + feedrate = segment.feedrate if segment.type == MoveType.FEED else None + profile = self.planner.plan_motion(distance, feedrate, is_rapid) + motion_profiles.append(profile) + elif segment.type in [MoveType.ARC_CW, MoveType.ARC_CCW]: + radius = getattr(segment, 'radius', 1.0) + turn = abs(getattr(segment, 'turn', 1)) + arc_length = turn * 2.0 * math.pi * radius + profile = self.planner.plan_motion(arc_length, segment.feedrate, False) + motion_profiles.append(profile) + else: + motion_profiles.append(MotionProfile()) + + # 主执行循环 + for seg_idx in range(total_segments): + if not self.is_running or self.program_ended: + break + + # 检查急停和报警 + if self.state_machine.is_estop() or self.state_machine.is_alarm(): + self.is_running = False + if self.debug: + print(f"[执行] 紧急停止! 状态={self.state_machine.get_state()}") + break + + # 检查暂停 + while self.is_paused: + time.sleep(0.05) + if not self.is_running: + break + + segment = self.current_toolpath.segments[seg_idx] + + + # ★ 修复: 直接从segment同步RTCP状态 ★ + if segment.type == MoveType.RTCP_ON: + self.status.rtcp_enabled = True + self.rtcp_enabled = True + if self._kinematics_instance: + self._kinematics_instance.enable_rtcp(True) + elif segment.type == MoveType.RTCP_OFF: + self.status.rtcp_enabled = False + self.rtcp_enabled = False + if self._kinematics_instance: + self._kinematics_instance.enable_rtcp(False) + elif hasattr(segment, 'is_rtcp'): + self.status.rtcp_enabled = segment.is_rtcp + self.rtcp_enabled = segment.is_rtcp + if self._kinematics_instance: + self._kinematics_instance.enable_rtcp(segment.is_rtcp) + + self.current_segment_index = seg_idx + + + # ★ 在这里添加偏移同步 ★ + self._sync_offsets_to_status() + + # 更新状态 + self.status.current_line = segment.line_number + self.status.progress_percent = (seg_idx / total_segments) * 100.0 + self.status.feedrate = segment.feedrate if segment.type == MoveType.FEED else 0 + + # 处理RTCP状态 + # 处理RTCP状态 + if segment.type == MoveType.RTCP_ON: + self.status.rtcp_enabled = True + self.rtcp_enabled = True + if self._kinematics_instance: + self._kinematics_instance.enable_rtcp(True) + elif segment.type == MoveType.RTCP_OFF: + self.status.rtcp_enabled = False + self.rtcp_enabled = False + if self._kinematics_instance: + self._kinematics_instance.enable_rtcp(False) + + # ★ 每次运动段执行前同步偏移 ★ + self._sync_offsets_to_status() + + self._notify_update() + + # 执行运动段 + profile = motion_profiles[seg_idx] if seg_idx < len(motion_profiles) else None + self._execute_segment_real(segment, profile) + + # 更新世界坐标 + if hasattr(segment, 'world_end') and segment.world_end: + self.status.world_x = segment.world_end.x + self.status.world_y = segment.world_end.y + self.status.world_z = segment.world_end.z + + self.current_segment_index = seg_idx + 1 + + # 单步模式 + if self.single_step_mode: + self.is_paused = True + self.status.state = MachineState.PAUSED.value + self.single_step_mode = False + self._notify_update() + + # 程序完成 + self.program_ended = True + self.status.progress_percent = 100.0 + self.is_running = False + self.state_machine.transition_to(MachineStateMachine.STATE_IDLE) + self.status.state = MachineState.IDLE.value + self._notify_update() + + if self.debug: + print(f"[执行] 程序执行完成! 最终进度=100%") + + def _execute_segment_real(self, segment: MoveSegment, profile: MotionProfile = None): + """按照真实速度曲线执行运动段""" + + # ★ 同步偏移信息 ★ + self._sync_offsets_to_status() + + # 暂停 + if segment.type == MoveType.DWELL: + dwell_time = getattr(segment, 'dwell_time', 0.0) + if dwell_time > 0: + if self.debug: + print(f" [G4] 暂停 {dwell_time*1000:.0f}ms") + start = time.time() + while time.time() - start < dwell_time: + if not self.is_running: + return + while self.is_paused: + time.sleep(0.05) + start += 0.05 + time.sleep(0.01) + return + + # RTCP/程序结束 + if segment.type in [MoveType.RTCP_ON, MoveType.RTCP_OFF, MoveType.PROGRAM_END]: + time.sleep(0.05) + return + + # 无运动距离 + if not profile or profile.duration <= 0: + if hasattr(segment, 'end'): + self._set_position_from_point(segment.end) + return + + # ===== 直线运动 ===== + if segment.type in [MoveType.RAPID, MoveType.FEED]: + self._execute_linear_real(segment, profile) + + # ===== 圆弧运动 ===== + elif segment.type in [MoveType.ARC_CW, MoveType.ARC_CCW]: + self._execute_arc_real(segment, profile) + + def _sync_offsets_to_status(self): + """从解析器的收集器同步偏移信息到status对象""" + try: + if hasattr(self, 'parser') and self.parser: + parser = self.parser + if hasattr(parser, '_builtin_parser') and parser._builtin_parser: + bp = parser._builtin_parser + if bp.collector: + c = bp.collector + self.status.g5x_offset_x = c.g5x_offset_x + self.status.g5x_offset_y = c.g5x_offset_y + self.status.g5x_offset_z = c.g5x_offset_z + self.status.g92_x = c.g92_offset_x + self.status.g92_y = c.g92_offset_y + self.status.g92_z = c.g92_offset_z + except Exception: + pass # 如果解析器尚未初始化，使用默认值0 + + + def _execute_linear_real(self, segment: MoveSegment, profile: MotionProfile): + """执行直线运动（真实速度曲线）- 适配工件坐标""" + + # ★修复: 从segment同步RTCP状态★ + if hasattr(segment, 'is_rtcp') and self._kinematics_instance: + expected_rtcp = segment.is_rtcp + if self.rtcp_enabled != expected_rtcp: + self.rtcp_enabled = expected_rtcp + self.status.rtcp_enabled = expected_rtcp + self._kinematics_instance.enable_rtcp(expected_rtcp) + + # ★ 同步偏移信息 ★ + self._sync_offsets_to_status() + + # 使用工件坐标 + world_start = segment.get_start_in_world() + world_end = segment.get_end_in_world() + + start = world_start + end = world_end + + dx = end.x - start.x + dy = end.y - start.y + dz = end.z - start.z + da = end.a - start.a + db = end.b - start.b + dc = end.c - start.c + + # ★ 修复: 将旋转角度变化计入等效距离，使F值能控制旋转速度 ★ + # 1度角度变化 = 1mm等效直线距离 + angular_dist = math.sqrt(da*da + db*db + dc*dc) + linear_dist = math.sqrt(dx*dx + dy*dy + dz*dz) + total_6d_dist = math.sqrt(linear_dist*linear_dist + angular_dist*angular_dist) + + distance = max(profile.total_distance, total_6d_dist * 0.5) + # 确保distance不为零 + if distance < CART_FUZZ and total_6d_dist > CART_FUZZ: + distance = total_6d_dist + + if self.debug: + is_rapid = (segment.type == MoveType.RAPID) + gcode = "G0" if is_rapid else "G1" + print(f" [{gcode}] {self.planner.get_profile_summary(profile)}") + + if profile.duration <= 0: + self._set_position_from_world_point(end) + self._update_world_position() + self._notify_update() + return + + start_time = time.time() + last_update_time = start_time + update_interval = 0.03 # ~30Hz更新 + + while True: + if not self.is_running: + return + if self.state_machine.is_estop() or self.state_machine.is_alarm(): + return + + while self.is_paused: + time.sleep(0.05) + start_time += 0.05 + last_update_time += 0.05 + if not self.is_running: + return + + elapsed = time.time() - start_time + if elapsed >= profile.duration: + break + + s = self.planner.get_position_at_time(elapsed, profile) + ratio = s / distance if distance > CART_FUZZ else 1.0 + ratio = max(0.0, min(1.0, ratio)) + + # 插值工件坐标 + wx = start.x + dx * ratio + wy = start.y + dy * ratio + wz = start.z + dz * ratio + wa = start.a + da * ratio + wb = start.b + db * ratio + wc = start.c + dc * ratio + + self._set_workpiece_position(wx, wy, wz, wa, wb, wc) + + # 控制更新频率 + now = time.time() + if now - last_update_time >= update_interval: + self._notify_update() + last_update_time = now + + time.sleep(0.005) + + # 确保到达终点 + self._set_workpiece_position(end.x, end.y, end.z, end.a, end.b, end.c) + self._notify_update() + + # 在 gpb_GLCanon_arcseg_sim_rtcp_rs274_kins.py 中找到 _set_workpiece_position 方法 + # 将其替换为以下代码： + + + + def _set_workpiece_position(self, x, y, z, a, b, c): + """设置当前位置（工件坐标）并计算世界坐标和机器坐标""" + # 同步偏移信息 + self._sync_offsets_to_status() + + # ★ 调试：检查tool_offset是否正确 + if self.debug: + tlo = getattr(self.status, 'tool_length', 0.0) + print(f"[DEBUG] _set_workpiece_position: " + f"工件({x:.1f},{y:.1f},{z:.1f}) TLO={tlo:.1f} " + f"RTCP={self.rtcp_enabled}") + + + # ★修复: 直接使用self.rtcp_enabled★ + rtcp_on = self.rtcp_enabled and self._kinematics_instance is not None + + # 更新工件坐标 (WCS) + self.status.position_x = x + self.status.position_y = y + self.status.position_z = z + self.status.position_a = a + self.status.position_b = b + self.status.position_c = c + + # 获取偏移值 + g5x_x = getattr(self.status, 'g5x_offset_x', 0.0) + g5x_y = getattr(self.status, 'g5x_offset_y', 0.0) + g5x_z = getattr(self.status, 'g5x_offset_z', 0.0) + g5x_a = getattr(self.status, 'g5x_offset_a', 0.0) + g5x_b = getattr(self.status, 'g5x_offset_b', 0.0) + g5x_c = getattr(self.status, 'g5x_offset_c', 0.0) + + g92_x = getattr(self.status, 'g92_x', 0.0) + g92_y = getattr(self.status, 'g92_y', 0.0) + g92_z = getattr(self.status, 'g92_z', 0.0) + g92_a = getattr(self.status, 'g92_a', 0.0) + g92_b = getattr(self.status, 'g92_b', 0.0) + g92_c = getattr(self.status, 'g92_c', 0.0) + + tlo = getattr(self.status, 'tool_length', 0.0) + + # ★ 关键修复：计算绝对坐标 ★ + # 工件坐标 + G5x + G92 + TLO = 绝对坐标 + abs_x = x + g5x_x + g92_x + abs_y = y + g5x_y + g92_y + abs_z = z + g5x_z + g92_z + tlo # TLO加在Z上 + abs_a = a + g5x_a + g92_a + abs_b = b + g5x_b + g92_b + abs_c = c + g5x_c + g92_c + + # ★修复: RTCP逆解 - 使用self.rtcp_enabled★ + rtcp_on = self.rtcp_enabled and self._kinematics_instance is not None + + if not rtcp_on: + # 非RTCP：绝对坐标 = 机器坐标 + self.status.machine_x = abs_x + self.status.machine_y = abs_y + self.status.machine_z = abs_z + self.status.machine_a = abs_a + self.status.machine_b = abs_b + self.status.machine_c = abs_c + else: + # RTCP开启：调用逆运动学 + try: + # 构造完整的世界坐标元组 (X, Y, Z, A, B, C) + world_tuple = (abs_x, abs_y, abs_z, abs_a, abs_b, abs_c) + + # ★ 调用逆运动学 ★ + joints = self._kinematics_instance.inverse(world_tuple) + + if self.debug: + print(f"[RTCP INV] world=({abs_x:.2f},{abs_y:.2f},{abs_z:.2f}," + f"A={abs_a:.1f},B={abs_b:.1f},C={abs_c:.1f})") + print(f"[RTCP INV] joints={[f'{j:.2f}' for j in joints]}") + + # 根据运动学类型赋值关节坐标 + config = KINEMATICS_JOINT_CONFIG.get( + self._kinematics_type, + KINEMATICS_JOINT_CONFIG[KinematicsType.IDENTITY] + ) + + x_idx = config.get('x_idx', 0) + y_idx = config.get('y_idx', 1) + z_idx = config.get('z_idx', 2) + a_idx = config.get('a_idx', 3) + b_idx = config.get('b_idx', 4) + c_idx = config.get('c_idx', 5) + + # ★ 关键：确保从joints数组中正确取值 ★ + if x_idx >= 0 and x_idx < len(joints): + self.status.machine_x = joints[x_idx] + else: + self.status.machine_x = abs_x + + if y_idx >= 0 and y_idx < len(joints): + self.status.machine_y = joints[y_idx] + else: + self.status.machine_y = abs_y + + if z_idx >= 0 and z_idx < len(joints): + self.status.machine_z = joints[z_idx] + else: + self.status.machine_z = abs_z + + if a_idx >= 0 and a_idx < len(joints): + self.status.machine_a = joints[a_idx] + else: + self.status.machine_a = abs_a + + if b_idx >= 0 and b_idx < len(joints): + self.status.machine_b = joints[b_idx] + else: + self.status.machine_b = abs_b + + if c_idx >= 0 and c_idx < len(joints): + self.status.machine_c = joints[c_idx] + else: + self.status.machine_c = abs_c + + except Exception as e: + if self.debug: + print(f"[RTCP] 逆运动学计算失败: {e}") + import traceback + traceback.print_exc() + # 逆解失败时使用绝对坐标作为后备 + self.status.machine_x = abs_x + self.status.machine_y = abs_y + self.status.machine_z = abs_z + self.status.machine_a = abs_a + self.status.machine_b = abs_b + self.status.machine_c = abs_c + + # 更新世界坐标 + self._update_world_position() + + + + def _set_position_from_world_point(self, point): + """从 Point6D 设置当前位置（工件坐标）""" + if point is None: + return + self.status.position_x = point.x + self.status.position_y = point.y + self.status.position_z = point.z + self.status.position_a = point.a + self.status.position_b = point.b + self.status.position_c = point.c + + + + def _execute_arc_real(self, segment: MoveSegment, profile: MotionProfile): + """执行圆弧运动（真实速度曲线）- 适配工件坐标""" + + # ★修复: 从segment同步RTCP状态★ + if hasattr(segment, 'is_rtcp') and self._kinematics_instance: + expected_rtcp = segment.is_rtcp + if self.rtcp_enabled != expected_rtcp: + self.rtcp_enabled = expected_rtcp + self.status.rtcp_enabled = expected_rtcp + self._kinematics_instance.enable_rtcp(expected_rtcp) + + # 使用工件坐标 + world_start = segment.get_start_in_world() + world_end = segment.get_end_in_world() + + start = world_start + end = world_end + + center_x = segment.center_x + center_y = segment.center_y + center_z = segment.center_z + radius = segment.radius + turn = abs(segment.turn) + + arc_length = profile.total_distance + + dz = end.z - start.z + da = end.a - start.a + db = end.b - start.b + dc = end.c - start.c + + is_cw = (segment.type == MoveType.ARC_CW) + + if self.debug: + gcode = "G2" if is_cw else "G3" + print(f" [{gcode}] R={radius:.1f}mm 圈数={turn} " + f"{self.planner.get_profile_summary(profile)}") + + if profile.duration <= 0: + self._set_position_from_world_point(end) + self._update_world_position() + self._notify_update() + return + + # 计算起始角度（工件坐标系） + start_angle = math.atan2(start.y - center_y, start.x - center_x) + + start_time = time.time() + last_update_time = start_time + update_interval = 0.03 + + while True: + if not self.is_running: + return + if self.state_machine.is_estop() or self.state_machine.is_alarm(): + return + + while self.is_paused: + time.sleep(0.05) + start_time += 0.05 + last_update_time += 0.05 + if not self.is_running: + return + + elapsed = time.time() - start_time + if elapsed >= profile.duration: + break + + s = self.planner.get_position_at_time(elapsed, profile) + angle_ratio = s / arc_length if arc_length > CART_FUZZ else 1.0 + angle_ratio = max(0.0, min(1.0, angle_ratio)) + + # 计算当前角度 + if is_cw: + current_angle = start_angle - angle_ratio * turn * 2.0 * math.pi + else: + current_angle = start_angle + angle_ratio * turn * 2.0 * math.pi + + # 计算圆弧上的工件坐标点 + wx = center_x + radius * math.cos(current_angle) + wy = center_y + radius * math.sin(current_angle) + wz = start.z + dz * angle_ratio + wa = start.a + da * angle_ratio + wb = start.b + db * angle_ratio + wc = start.c + dc * angle_ratio + + self._set_workpiece_position(wx, wy, wz, wa, wb, wc) + + now = time.time() + if now - last_update_time >= update_interval: + self._notify_update() + last_update_time = now + + time.sleep(0.005) + + # 确保到达终点 + self._set_workpiece_position(end.x, end.y, end.z, end.a, end.b, end.c) + self._notify_update() + + + + def _set_position_from_point(self, point): + """从 Point6D 设置当前位置（保留向后兼容）""" + if point is None: + return + self._set_workpiece_position(point.x, point.y, point.z, point.a, point.b, point.c) + + + + def _execute_segment(self, segment: MoveSegment): + """兼容旧接口的执行方法 - 适配工件坐标""" + if segment.type in [MoveType.RAPID, MoveType.FEED]: + world_start = segment.get_start_in_world() + world_end = segment.get_end_in_world() + dx = world_end.x - world_start.x + dy = world_end.y - world_start.y + dz = world_end.z - world_start.z + distance = math.sqrt(dx*dx + dy*dy + dz*dz) + is_rapid = (segment.type == MoveType.RAPID) + feedrate = segment.feedrate if segment.type == MoveType.FEED else None + profile = self.planner.plan_motion(distance, feedrate, is_rapid) + elif segment.type in [MoveType.ARC_CW, MoveType.ARC_CCW]: + radius = segment.radius + turn = abs(segment.turn) + arc_length = turn * 2.0 * math.pi * radius + profile = self.planner.plan_motion(arc_length, segment.feedrate, False) + else: + profile = MotionProfile() + + self._execute_segment_real(segment, profile) + + + def enable_rtcp(self, enable: bool = True) -> Dict: + self.status.rtcp_enabled = enable + self.rtcp_enabled = enable + if self._kinematics_instance: + self._kinematics_instance.enable_rtcp(enable) + self.virtual_hal.set('motion.rtcp-active', 1 if enable else 0) + self._update_world_position() + self._notify_update() + return {"success": True, "message": f"RTCP {'启用' if enable else '禁用'}"} + + def mdi_execute(self, command: str) -> Dict: + cmd_upper = command.upper().strip() + + if 'G49' in cmd_upper: + return self.enable_rtcp(False) + + if 'G43.4' in cmd_upper: + return self.enable_rtcp(True) + + if cmd_upper == 'M429': + self._kinematics_type = KinematicsType.IDENTITY + self.status.kinematics_type = "IDENTITY" + self.rtcp_enabled = False + self.status.rtcp_enabled = False + if self._kinematics_instance: + self._kinematics_instance.enable_rtcp(False) + self.virtual_hal.set('motion.rtcp-active', 0) + self._update_world_position() + self._notify_update() + return {"success": True, "message": "切换到三轴运动学"} + + if cmd_upper == 'M428': + self._kinematics_type = self._original_kinematics_type + self.status.kinematics_type = self._original_kinematics_type.name + self.rtcp_enabled = True + self.status.rtcp_enabled = True + self._update_kinematics_instance() + if self._kinematics_instance: + self._kinematics_instance.enable_rtcp(True) + self.virtual_hal.set('motion.rtcp-active', 1) + self._update_world_position() + self._notify_update() + return {"success": True, "message": f"切换到五轴运动学 ({self._original_kinematics_type.name})"} + + if cmd_upper == 'M430' or cmd_upper.startswith('M430'): + self._original_kinematics_type = KinematicsType.FIVEAXIS_BC + self._kinematics_type = KinematicsType.FIVEAXIS_BC + self.status.kinematics_type = KinematicsType.FIVEAXIS_BC.name + self._kinematics_instance = KinematicsFactory.create( + KinematicsType.FIVEAXIS_BC, debug=self.debug + ) + self.rtcp_enabled = True + self.status.rtcp_enabled = True + if self._kinematics_instance: + self._kinematics_instance.enable_rtcp(True) + self._kinematics_instance.set_tool_length(self.status.tool_length) + self.virtual_hal.set('motion.rtcp-active', 1) + self.virtual_hal.set('motion.switchkins-type', 2) + self._update_world_position() + self._notify_update() + return {"success": True, "message": "切换到用户运动学 (FIVEAXIS_BC)"} + + return {"success": True, "message": f"MDI: {command}"} + + def get_status(self) -> CNCRuntimeStatus: + return self.status + + def shutdown(self): + self.state_monitor_running = False + self.is_running = False + print(f"✓ CNC内核 [{self.name}] 已关闭") + + + + def verify_jog_coordinate_chain(self) -> Dict[str, Any]: + """ + 验证Jog操作的坐标变换链正确性 + + 通过正反运动学验证工件坐标和机器坐标的一致性。 + 在RTCP启用时特别重要，确保刀尖跟随正确。 + + Returns: + dict: 验证结果，包含各部分坐标和一致性检查 + """ + result = { + 'timestamp': time.time(), + 'rtcp_enabled': self.rtcp_enabled, + 'kinematics_type': self.kinematics_type.name if self.kinematics_type else 'NONE', + 'workpiece_position': {}, + 'absolute_position': {}, + 'machine_position': {}, + 'world_position': {}, + 'offsets': {}, + 'consistency_checks': {}, + } + + # 1. 收集偏移信息 + g5x_x = getattr(self.status, 'g5x_offset_x', 0.0) + g5x_y = getattr(self.status, 'g5x_offset_y', 0.0) + g5x_z = getattr(self.status, 'g5x_offset_z', 0.0) + g92_x = getattr(self.status, 'g92_x', 0.0) + g92_y = getattr(self.status, 'g92_y', 0.0) + g92_z = getattr(self.status, 'g92_z', 0.0) + tlo = getattr(self.status, 'tool_length', 0.0) + + result['offsets'] = { + 'g5x': {'x': g5x_x, 'y': g5x_y, 'z': g5x_z}, + 'g92': {'x': g92_x, 'y': g92_y, 'z': g92_z}, + 'tlo': tlo, + } + + # 2. 工件坐标 + result['workpiece_position'] = { + 'x': self.status.position_x, + 'y': self.status.position_y, + 'z': self.status.position_z, + 'a': self.status.position_a, + 'c': self.status.position_c, + } + + # 3. 计算机器坐标 + result['machine_position'] = { + 'x': self.status.machine_x, + 'y': self.status.machine_y, + 'z': self.status.machine_z, + 'a': self.status.machine_a, + 'c': self.status.machine_c, + } + + # 4. 计算绝对坐标 + abs_x = self.status.position_x + g5x_x + g92_x + abs_y = self.status.position_y + g5x_y + g92_y + abs_z = self.status.position_z + g5x_z + g92_z + tlo + abs_a = self.status.position_a + abs_c = self.status.position_c + + result['absolute_position'] = { + 'x': abs_x, 'y': abs_y, 'z': abs_z, + 'a': abs_a, 'c': abs_c, + } + + # 5. RTCP一致性验证 + if self.rtcp_enabled and self._kinematics_instance: + try: + # 正向验证：机器坐标 → 正运动学 → 应等于绝对坐标 + joints = [ + self.status.machine_x, self.status.machine_y, + self.status.machine_z, + self.status.machine_a, self.status.machine_b, + self.status.machine_c + ] + world_fwd = self._kinematics_instance.joints_to_world(joints) + + result['world_position'] = { + 'x': world_fwd[0], 'y': world_fwd[1], 'z': world_fwd[2], + 'a': world_fwd[3], 'c': world_fwd[5] if len(world_fwd) > 5 else 0, + } + + # 检查正运动学结果是否接近绝对坐标 + tolerance = 0.5 # mm，允许0.5mm误差 + fwd_consistent = True + if abs(world_fwd[0] - abs_x) > tolerance: + fwd_consistent = False + if abs(world_fwd[1] - abs_y) > tolerance: + fwd_consistent = False + if abs(world_fwd[2] - abs_z) > tolerance: + fwd_consistent = False + + result['consistency_checks']['forward_kinematics'] = { + 'consistent': fwd_consistent, + 'tolerance': tolerance, + 'error_x': abs(world_fwd[0] - abs_x), + 'error_y': abs(world_fwd[1] - abs_y), + 'error_z': abs(world_fwd[2] - abs_z), + } + + # 反向验证：绝对坐标 → 逆运动学 → 应等于机器坐标 + joints_inv = self._kinematics_instance.world_to_joints( + (abs_x, abs_y, abs_z, abs_a, 0.0, abs_c) + ) + + inv_consistent = True + if len(joints_inv) > 0: + if abs(joints_inv[0] - self.status.machine_x) > tolerance: + inv_consistent = False + if len(joints_inv) > 1: + if abs(joints_inv[1] - self.status.machine_y) > tolerance: + inv_consistent = False + if len(joints_inv) > 2: + if abs(joints_inv[2] - self.status.machine_z) > tolerance: + inv_consistent = False + + result['consistency_checks']['inverse_kinematics'] = { + 'consistent': inv_consistent, + 'tolerance': tolerance, + 'joints_calculated': joints_inv[:5] if len(joints_inv) >= 5 else joints_inv, + } + + # 总体一致性 + result['consistency_checks']['overall'] = { + 'consistent': fwd_consistent and inv_consistent, + 'message': '坐标链一致 ✓' if (fwd_consistent and inv_consistent) + else '坐标链不一致 ✗ - 请检查运动学参数', + } + + except Exception as e: + result['consistency_checks']['error'] = str(e) + else: + # RTCP禁用时，机器坐标应等于绝对坐标 + machine_eq_absolute = ( + abs(self.status.machine_x - abs_x) < 0.001 and + abs(self.status.machine_y - abs_y) < 0.001 and + abs(self.status.machine_z - abs_z) < 0.001 + ) + result['consistency_checks']['machine_equals_absolute'] = machine_eq_absolute + + return result + + + +# ==================== 完善后的 GLCanonPathCollectorWithRTCP 类 ==================== + +class GLCanonPathCollectorWithRTCP(GLCanonPure): + """增强版路径收集器 - 完整五轴RTCP + 所有LinuxCNC状态管理""" + + def __init__(self, colors=None, geometry=None, is_foam=0, max_points=50000, + acceleration: float = 500.0, + max_rapid_rate: float = 10000.0, + max_feed_rate: float = 5000.0, + kinematics_type: KinematicsType = KinematicsType.TRT_BC, + kinematics_params: Dict[str, Any] = None, + debug: bool = False): + + if colors is None: + colors = DEFAULT_COLORS.copy() + if geometry is None: + geometry = "XYZ" + + super().__init__(colors, geometry, is_foam) + + self.max_points = max_points + self.debug = debug + self._point_count = 0 + + self.data = ToolpathData() + self.data.filename = "" + self.data.rtcp_enabled = False + self.data.kinematics_type = kinematics_type.name + + self.rtcp_enabled = False + self.kinematics_type = kinematics_type + + self.kinematics: Optional[BaseKinematics] = KinematicsFactory.create( + kinematics_type, debug=debug, **(kinematics_params or {}) + ) + + # ===== 添加这一行 ===== + self._saved_kinematics_type = None # 用于 M428/M429 切换时保存原始类型 + + self.planner = TrapezoidalMotionPlanner( + acceleration=acceleration, + max_rapid_rate=max_rapid_rate, + max_feed_rate=max_feed_rate + ) + + # 当前位置 - 工件坐标系 + self.current_pos = Point6D(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + + # 机床坐标 + self.machine_x = 0.0 + self.machine_y = 0.0 + self.machine_z = 0.0 + self.machine_a = 0.0 + self.machine_b = 0.0 + self.machine_c = 0.0 + + # 工件偏移 (G5x) + self.work_offsets: Dict[int, Dict[str, float]] = {} + for i in range(1, 10): + self.work_offsets[i] = {'X': 0.0, 'Y': 0.0, 'Z': 0.0, + 'A': 0.0, 'B': 0.0, 'C': 0.0, + 'U': 0.0, 'V': 0.0, 'W': 0.0, 'R': 0.0} + self.current_work_offset = 1 # G54 + self.g5x_index = 1 + + # G92偏移 + self.g92_offset_x = 0.0 + self.g92_offset_y = 0.0 + self.g92_offset_z = 0.0 + self.g92_offset_a = 0.0 + self.g92_offset_b = 0.0 + self.g92_offset_c = 0.0 + self.g92_offset_u = 0.0 + self.g92_offset_v = 0.0 + self.g92_offset_w = 0.0 + self.g92_active = False + + # G52偏移 + self.g52_offset_x = 0.0 + self.g52_offset_y = 0.0 + self.g52_offset_z = 0.0 + self.g52_offset_a = 0.0 + self.g52_offset_b = 0.0 + self.g52_offset_c = 0.0 + self.g52_offset_u = 0.0 + self.g52_offset_v = 0.0 + self.g52_offset_w = 0.0 + + # XY旋转 + self.rotation_xy = 0.0 + self.rotation_sin = 0.0 + self.rotation_cos = 1.0 + + # G53模式 + self._g53_active = False + + # 工件偏移（额外的work offset） + self.work_offset_x = 0.0 + self.work_offset_y = 0.0 + self.work_offset_z = 0.0 + self.work_offset_a = 0.0 + self.work_offset_b = 0.0 + self.work_offset_c = 0.0 + + # 刀具数据 + self.tool_lengths: Dict[int, float] = {} + self.tool_radii: Dict[int, float] = {} + self.tool_table: Dict[int, ToolData] = {} + self.current_tool: Optional[ToolData] = None + self.current_h_code = 0 + self.current_d_code = 0 + self.selected_tool = 0 + self.selected_pocket = 0 + self.current_pocket = 0 + + # 刀具补偿 + self.comp_type = CompType.OFF + self.comp_radius = 0.0 + self.comp_side = 0 + self.d_word = 0.0 + self.cutter_comp_active = False + self.cutter_comp_firstmove = True + self.cutter_comp_orientation = 0 + self.arc_not_allowed = True + self.program_x = 0.0 + self.program_y = 0.0 + self.program_z = 0.0 + + # 刀具长度补偿 + self.tool_offset = Point6D(0.0, 0.0, 0.0, 0.0, 0.0, 0.0) + self.g43_with_zero_offset = False + + # 模态状态 + self.is_absolute = True + self.current_plane = 17 + self.feedrate = 1000.0 + self.current_line = 0 + self.cumulative_time = 0.0 + self.motion_mode = -1 + self.retract_mode = 98 + self.ijk_distance_mode = 91 + self.lathe_diameter_mode = False + self.return_value = 0.0 + self.value_returned = False + self.remap_level = 0 + self.call_level = 0 + + # 状态 + self.state = type('State', (), {})() + self.state.plane = 17 + self.state.distance_mode = 90 + self.state.feedrate = 1000.0 + self.state.spindle_mode = 0 + self.state.spindle_speed = 0.0 + self.state.spindle_state = "OFF" + self.state.coolant_mode = 9 + self.state.coolant_flood = False + self.state.coolant_mist = False + self.state.tool = 0 + self.state.units = 21 + self.state.feed_mode = 94 + + self.optional_stop = False + self.block_delete = False + + # 固定循环 + self.canned_cycle_active = False + self.canned_cycle_type = 0 + self.canned_cycle_r = 0.0 + self.canned_cycle_z = 0.0 + self.canned_cycle_q = 0.0 + self.canned_cycle_l = 1 + self.canned_cycle_p = 0.0 + self.canned_cycle_il = 0.0 + self.canned_cycle_il_flag = False + + + # ===== 添加这一行 ===== + self._saved_kinematics_type = None # 用于 M428/M429 切换时保存原始类型 + + # ===== 添加旋转轴保存变量 ===== + self._saved_rotation_a = 0.0 + self._saved_rotation_b = 0.0 + self._saved_rotation_c = 0.0 + + + # 参考点 + self.g28_ref: Dict[str, float] = {axis: 0.0 for axis in 'XYZABC'} + self.g30_ref: Dict[str, float] = {axis: 0.0 for axis in 'XYZABC'} + + self.state_machine: Optional[MachineStateMachine] = None + self.call_counts: Dict[str, int] = {} + self.saved_states: List[Dict] = [] + + self._sync_machine_from_current() + + # 参数表系统 + self.var_manager = LinuxCNCParameterTable(debug=debug) + self.var_manager._current_line = self.current_line + + self.control_handler = ControlStructureHandler(self.var_manager, debug=debug) + self._sync_parameters_from_state() + + self._gcode_buffer: List[str] = [] + self._parse_errors: List[str] = [] + + # ===== 新增：坐标系管理 ===== + self._coord_mode = 'WORLD' # 当前坐标模式: 'WORLD' | 'ABSOLUTE' | 'MACHINE' + self._last_segment_was_arc = False + + # 初始化参数表中的当前位置 + self._init_params_position() + + if self.debug: + print(f"GLCanonPathCollectorWithRTCP 初始化完成") + print(f" 运动学类型: {kinematics_type.name}") + print(f" RTCP状态: {'启用' if self.rtcp_enabled else '禁用'}") + + def _init_params_position(self): + """初始化参数表中的当前位置为 (0,0,0,0,0,0)""" + self.var_manager._current_position = { + 'X': self.current_pos.x, + 'Y': self.current_pos.y, + 'Z': self.current_pos.z, + 'A': self.current_pos.a, + 'B': self.current_pos.b, + 'C': self.current_pos.c, + } + + + def _sync_position_to_params(self): + """同步当前位置到参数表（5420-5428 相对位置参数）""" + # 工件坐标 + self.var_manager._current_position['X'] = self.current_pos.x + self.var_manager._current_position['Y'] = self.current_pos.y + self.var_manager._current_position['Z'] = self.current_pos.z + self.var_manager._current_position['A'] = self.current_pos.a + self.var_manager._current_position['B'] = self.current_pos.b + self.var_manager._current_position['C'] = self.current_pos.c + + # 同步到参数 5420-5428 + self.var_manager._params[5420] = self.current_pos.x + self.var_manager._params[5421] = self.current_pos.y + self.var_manager._params[5422] = self.current_pos.z + self.var_manager._params[5423] = self.current_pos.a + self.var_manager._params[5424] = self.current_pos.b + self.var_manager._params[5425] = self.current_pos.c + + + + def _compute_machine_coords(self, wx: float, wy: float, wz: float, + wa: float, wb: float, wc: float, + wu: float = 0.0, wv: float = 0.0, ww: float = 0.0 + ) -> Tuple[float, float, float, float, float, float, float, float, float]: + """ + 计算机器坐标（关节坐标） + + 参照 LinuxCNC 源码的坐标转换链： + rs274ngc.cpp: find_current_in_system() + kinematics.cpp: kinematicsInverse() + + 转换步骤（与 _sync_machine_from_current 保持一致）： + 1. 工件坐标 + G92偏移 + XY旋转 + G5x偏移 + TLO + G52偏移 = 绝对坐标 + 2. 如果 RTCP 启用，调用逆运动学计算关节坐标 + 3. 如果 RTCP 禁用，直接返回绝对坐标 + + 对应 C++ 中： + - settings->current_x → 工件坐标 + - settings->tool_offset.tran.z → TLO + - motion controller 调用 kinematicsInverse() → 关节坐标 + """ + # ===== 第1步：加上 G92 偏移 (axis_offset) ===== + # 对应 C++: *x += s->axis_offset_x + abs_x = wx + self.g92_offset_x + abs_y = wy + self.g92_offset_y + abs_z = wz + self.g92_offset_z + abs_a = wa + self.g92_offset_a + abs_b = wb + self.g92_offset_b + abs_c = wc + self.g92_offset_c + abs_u = wu + self.g92_offset_u + abs_v = wv + self.g92_offset_v + abs_w = ww + self.g92_offset_w + + # ===== 第2步：XY 旋转 ===== + # 对应 C++: rotate(x, y, s->rotation_xy) + if abs(self.rotation_xy) > 1e-9: + rot = math.radians(self.rotation_xy) + cos_r = math.cos(rot) + sin_r = math.sin(rot) + rx = abs_x * cos_r - abs_y * sin_r + ry = abs_x * sin_r + abs_y * cos_r + abs_x, abs_y = rx, ry + + # ===== 第3步：加上 G5x 偏移 (origin_offset) ===== + # 对应 C++: *x += s->origin_offset_x + abs_x += self.g5x_offset_x + abs_y += self.g5x_offset_y + abs_z += self.g5x_offset_z + abs_a += self.g5x_offset_a + abs_b += self.g5x_offset_b + abs_c += self.g5x_offset_c + abs_u += self.g5x_offset_u + abs_v += self.g5x_offset_v + abs_w += self.g5x_offset_w + + # ===== 第4步：加上刀具长度补偿 (tool_offset) ===== + # 对应 C++: *x += s->tool_offset.tran.x + # 这是 TLO，在 convert_tool_length_offset 中设置 + abs_x += self.tool_offset.x + abs_y += self.tool_offset.y + abs_z += self.tool_offset.z + abs_a += self.tool_offset.a + abs_b += self.tool_offset.b + abs_c += self.tool_offset.c + abs_u += self.tool_offset.u + abs_v += self.tool_offset.v + abs_w += self.tool_offset.w + + # ===== 第5步：加上 G52 偏移 ===== + abs_x += self.g52_offset_x + abs_y += self.g52_offset_y + abs_z += self.g52_offset_z + abs_a += self.g52_offset_a + abs_b += self.g52_offset_b + abs_c += self.g52_offset_c + + # ===== 第6步：RTCP 逆运动学 ===== + # 对应 C++: motion controller 调用 kinematicsInverse() + # 传入的是包含 TLO 的绝对坐标 + if self.rtcp_enabled and self.kinematics is not None: + try: + # 将绝对坐标传给运动学逆解 + world_tuple = (abs_x, abs_y, abs_z, abs_a, abs_b, abs_c) + joints = self.kinematics.world_to_joints(world_tuple) + + # 使用关节映射函数转换为标准9轴格式 + config = KINEMATICS_JOINT_CONFIG.get( + self.kinematics_type, + KINEMATICS_JOINT_CONFIG[KinematicsType.IDENTITY] + ) + + result = [0.0] * 9 + for i in range(min(len(joints), 9)): + result[i] = joints[i] + + # 根据运动学类型映射关节 + x_idx = config.get('x_idx', 0) + y_idx = config.get('y_idx', 1) + z_idx = config.get('z_idx', 2) + a_idx = config.get('a_idx', 3) + b_idx = config.get('b_idx', 4) + c_idx = config.get('c_idx', 5) + + return ( + joints[x_idx] if (x_idx >= 0 and x_idx < len(joints)) else abs_x, + joints[y_idx] if (y_idx >= 0 and y_idx < len(joints)) else abs_y, + joints[z_idx] if (z_idx >= 0 and z_idx < len(joints)) else abs_z, + joints[a_idx] if (a_idx >= 0 and a_idx < len(joints)) else abs_a, + joints[b_idx] if (b_idx >= 0 and b_idx < len(joints)) else abs_b, + joints[c_idx] if (c_idx >= 0 and c_idx < len(joints)) else abs_c, + result[6], result[7], result[8] + ) + except Exception as e: + if self.debug: + print(f"[MCS] 逆运动学计算失败: {e}") + traceback.print_exc() + # 逆解失败时返回绝对坐标作为后备 + return (abs_x, abs_y, abs_z, abs_a, abs_b, abs_c, abs_u, abs_v, abs_w) + else: + # RTCP 禁用：直接返回绝对坐标 + return (abs_x, abs_y, abs_z, abs_a, abs_b, abs_c, abs_u, abs_v, abs_w) + + + def _add_world_segment(self, move_type: MoveType, end_world: Point6D, **kwargs): + """ + 统一的运动段添加方法 - 基于参数表进行坐标管理 + + 所有运动段（直线、圆弧、暂停等）都通过此方法创建。 + + 核心原则（参照 LinuxCNC 源码设计）： + - segment.start/end 使用工件坐标系 (G代码编程坐标) + - segment.world_start/world_end 是工件坐标的副本 + - segment.machine_start/machine_end 通过逆运动学计算关节坐标 + - 几何验证在工件坐标系下进行 + + 参数: + move_type: 运动类型 (RAPID, FEED, DWELL, PROGRAM_END等) + end_world: 工件坐标系下的目标位置 + **kwargs: 额外参数 + - center_x, center_y, center_z: 圆弧圆心（工件坐标） + - radius: 圆弧半径 + - turn: 圆弧圈数 + - dwell_time: 暂停时间 + - gcode: G代码字符串 + """ + if self._point_count >= self.max_points: + return + + # 1. 工件坐标系下的起点 + start_world = self.current_pos.copy() + + # 2. 计算直线距离 + if move_type in [MoveType.RAPID, MoveType.FEED]: + linear_distance = start_world.distance_to(end_world) + else: + linear_distance = 0.0 + + # 3. 规划速度曲线 + if move_type == MoveType.RAPID: + result = self.planner.calculate_time(linear_distance, None, is_rapid=True) + elif move_type == MoveType.FEED: + result = self.planner.calculate_time(linear_distance, self.feedrate, is_rapid=False) + elif move_type == MoveType.DWELL: + dwell_time = kwargs.get('dwell_time', 0) + result = { + 'duration': dwell_time, + 'max_velocity': 0.0, + 'acceleration_time': 0.0, + 'constant_time': 0.0, + 'deceleration_time': 0.0, + 'profile_type': 'dwell' + } + elif move_type == MoveType.PROGRAM_END: + dwell_time = kwargs.get('dwell_time', 0.1) + result = { + 'duration': dwell_time, + 'max_velocity': 0.0, + 'acceleration_time': 0.0, + 'constant_time': 0.0, + 'deceleration_time': 0.0, + 'profile_type': 'program_end' + } + else: + result = { + 'duration': 0.1, + 'max_velocity': 0.0, + 'acceleration_time': 0.0, + 'constant_time': 0.0, + 'deceleration_time': 0.0, + 'profile_type': 'other' + } + + # 4. 计算机器坐标（包含逆运动学） + # ★ 修改：使用 _compute_machine_coords 替代 program_to_absolute + # 参照 LinuxCNC: kinematicsInverse() 接收绝对坐标 + abs_start_tuple = self._compute_machine_coords( + start_world.x, start_world.y, start_world.z, + start_world.a, start_world.b, start_world.c, + getattr(start_world, 'u', 0.0), + getattr(start_world, 'v', 0.0), + getattr(start_world, 'w', 0.0) + ) + abs_end_tuple = self._compute_machine_coords( + end_world.x, end_world.y, end_world.z, + end_world.a, end_world.b, end_world.c, + getattr(end_world, 'u', 0.0), + getattr(end_world, 'v', 0.0), + getattr(end_world, 'w', 0.0) + ) + + abs_start_pt = Point6D( + abs_start_tuple[0], abs_start_tuple[1], abs_start_tuple[2], + abs_start_tuple[3], abs_start_tuple[4], abs_start_tuple[5] + ) + abs_end_pt = Point6D( + abs_end_tuple[0], abs_end_tuple[1], abs_end_tuple[2], + abs_end_tuple[3], abs_end_tuple[4], abs_end_tuple[5] + ) + + # 5. 创建 MoveSegment + segment = MoveSegment( + type=move_type, + start=start_world.copy(), # 工件坐标 + end=end_world.copy(), # 工件坐标 + line_number=self.current_line, + feedrate=self.feedrate if move_type == MoveType.FEED else 0.0, + comp_type=self.comp_type, + comp_radius=self.comp_radius, + d_word=self.d_word, + duration=result['duration'], + max_velocity=result['max_velocity'], + acceleration_time=result['acceleration_time'], + constant_time=result['constant_time'], + deceleration_time=result['deceleration_time'], + profile_type=result['profile_type'], + is_rtcp=self.rtcp_enabled, + world_start=start_world.copy(), + world_end=end_world.copy(), + machine_start=abs_start_pt, # ★ 机器坐标（已通过逆解） + machine_end=abs_end_pt, # ★ 机器坐标（已通过逆解） + tool_number=self.state.tool, + spindle_speed=self.state.spindle_speed, + spindle_state=self.state.spindle_state, + coord_system='WORLD', + active_csys=self.var_manager.active_csys, + g92_active=self.var_manager._g92_active, + **{k: v for k, v in kwargs.items() + if k in ['center_x', 'center_y', 'center_z', 'radius', + 'turn', 'dwell_time', 'gcode']} + ) + + # 6. 添加到路径数据 + self.data.segments.append(segment) + + if move_type in [MoveType.RAPID, MoveType.FEED]: + self.data.total_length += linear_distance + + self.cumulative_time += result['duration'] + self._point_count += 1 + + # 7. 更新工件坐标位置 + self.current_pos = end_world.copy() + + # 8. 同步到参数表 + self._sync_position_to_params() + + # 9. 同步机器坐标 + self._sync_machine_from_current() + + # 10. 同步所有参数 + self._sync_parameters_from_state() + + # 11. 标记运动段类型 + self._last_segment_was_arc = move_type in [MoveType.ARC_CW, MoveType.ARC_CCW] + + + def find_or_create_tool(self, tool_number: int) -> int: + """ + 查找或创建刀具，返回刀具索引 + 对应 C++ 中的 find_tool_index -> tooldata_find_index_for_tool + """ + # 先在现有刀具表中查找 + for idx, tool in self.tool_table.items(): + if tool.tool_number == tool_number: + return idx + + # 创建新刀具 + # 查找一个可用的 pocket + new_pocket = tool_number + if new_pocket in self.tool_table: + # 找到一个未使用的 pocket + for p in range(1, 1000): + if p not in self.tool_table: + new_pocket = p + break + + new_tool = ToolData( + tool_number=tool_number, + pocket=new_pocket, + diameter=0.0, + radius=0.0, + front_angle=0.0, + back_angle=0.0, + orientation=0, + name=f"T{tool_number}" + ) + self.tool_table[new_pocket] = new_tool + + # 初始化刀具长度和半径映射 + if tool_number not in self.tool_lengths: + self.tool_lengths[tool_number] = 0.0 + if tool_number not in self.tool_radii: + self.tool_radii[tool_number] = 0.0 + + if self.debug: + print(f"[TOOL] 创建新刀具 T{tool_number} in pocket {new_pocket}") + + return new_pocket + + def _get_csys_rotation(self, csys_num: int) -> float: + """ + 获取指定坐标系的 XY 旋转角度 + 对应 C++ 中 parameters[5210 + (origin * 20)] + """ + if csys_num in self.work_offsets: + return self.work_offsets[csys_num].get('R', 0.0) + return 0.0 + + def _get_current_in_system_without_tlo(self, system: int): + """ + 计算当前在指定坐标系中的位置（不带刀具长度补偿） + + 完全参照 LinuxCNC 官方实现： + rs274ngc.cpp: Interp::find_current_in_system_without_tlo + + 变换顺序： + 1. 从工件坐标开始 + 2. 加上 G92 偏移 (axis_offset) + 3. 加上 G52 偏移 + 4. XY 旋转（正向, rotation_xy） + 5. 加上 G5x 偏移 (origin_offset) + 6. 加上刀具长度补偿 (tool_offset) + 7. 减去目标坐标系的 G5x 偏移 + 8. XY 旋转（反向, 目标坐标系的 rotation_xy） + 9. 如果 G92 激活且目标是夹具坐标系(9=G59.3)，减去 G92 偏移 + + 用于 G10 L10/L11 计算刀具偏移 + + Args: + system: 坐标系编号 (1-9, 对应 G54-G59.3) + + Returns: + tuple: (x, y, z, a, b, c, u, v, w) 在目标坐标系中的坐标（不含TLO） + """ + # ===== 第1步：从工件坐标开始 ===== + x = self.current_pos.x + y = self.current_pos.y + z = self.current_pos.z + a = self.current_pos.a + b = self.current_pos.b + c = self.current_pos.c + u = getattr(self.current_pos, 'u', 0.0) + v = getattr(self.current_pos, 'v', 0.0) + w = getattr(self.current_pos, 'w', 0.0) + + # ===== 第2步：加上 G92 偏移 (axis_offset) ===== + x += self.g92_offset_x + y += self.g92_offset_y + z += self.g92_offset_z + a += self.g92_offset_a + b += self.g92_offset_b + c += self.g92_offset_c + u += self.g92_offset_u + v += self.g92_offset_v + w += self.g92_offset_w + + # ===== 第3步：加上 G52 偏移 ===== + x += self.g52_offset_x + y += self.g52_offset_y + z += self.g52_offset_z + a += self.g52_offset_a + b += self.g52_offset_b + c += self.g52_offset_c + u += self.g52_offset_u + v += self.g52_offset_v + w += self.g52_offset_w + + # ===== 第4步：XY 旋转（正向, rotation_xy）===== + if self.rotation_xy != 0: + rot_rad = math.radians(self.rotation_xy) + cos_r = math.cos(rot_rad) + sin_r = math.sin(rot_rad) + + rot_x = x * cos_r - y * sin_r + rot_y = x * sin_r + y * cos_r + x, y = rot_x, rot_y + + # ===== 第5步：加上 G5x 偏移 (origin_offset) ===== + x += self.g5x_offset_x + y += self.g5x_offset_y + z += self.g5x_offset_z + a += self.g5x_offset_a + b += self.g5x_offset_b + c += self.g5x_offset_c + u += self.g5x_offset_u + v += self.g5x_offset_v + w += self.g5x_offset_w + + # ===== 第6步：加上刀具长度补偿 (tool_offset) ★ 修复：新增步骤 ===== + x += self.tool_offset.x + y += self.tool_offset.y + z += self.tool_offset.z + a += self.tool_offset.a + b += self.tool_offset.b + c += self.tool_offset.c + u += self.tool_offset.u + v += self.tool_offset.v + w += self.tool_offset.w + + # ===== 第7步：减去目标坐标系的 G5x 偏移 ===== + target_offsets = self.work_offsets.get(system, {}) + + x -= target_offsets.get('X', 0.0) + y -= target_offsets.get('Y', 0.0) + z -= target_offsets.get('Z', 0.0) + a -= target_offsets.get('A', 0.0) + b -= target_offsets.get('B', 0.0) + c -= target_offsets.get('C', 0.0) + u -= target_offsets.get('U', 0.0) + v -= target_offsets.get('V', 0.0) + w -= target_offsets.get('W', 0.0) + + # ===== 第8步：XY 旋转（反向, 目标坐标系的 rotation_xy）===== + target_rotation = target_offsets.get('R', 0.0) + if target_rotation != 0: + rot_rad = math.radians(-target_rotation) + cos_r = math.cos(rot_rad) + sin_r = math.sin(rot_rad) + + rot_x = x * cos_r - y * sin_r + rot_y = x * sin_r + y * cos_r + x, y = rot_x, rot_y + + # ===== 第9步：如果 G92 激活，减去 G92 偏移 ===== + if self.g92_active: + x -= self.g92_offset_x + y -= self.g92_offset_y + z -= self.g92_offset_z + a -= self.g92_offset_a + b -= self.g92_offset_b + c -= self.g92_offset_c + u -= self.g92_offset_u + v -= self.g92_offset_v + w -= self.g92_offset_w + + if self.debug: + print(f"[CSYS] _get_current_in_system_without_tlo(system={system}):") + print(f" 工件坐标: ({self.current_pos.x:.3f}, {self.current_pos.y:.3f}, {self.current_pos.z:.3f})") + print(f" 目标坐标(不含TLO): ({x:.3f}, {y:.3f}, {z:.3f})") + + return (x, y, z, a, b, c, u, v, w) + + + def _get_current_in_system(self, system: int) -> Tuple[float, float, float, float, float, float, float, float, float]: + """ + 计算当前在指定坐标系中的位置（带刀具长度补偿） + + 完全参照 LinuxCNC 官方实现： + rs274ngc.cpp: Interp::find_current_in_system + + 变换顺序： + 1. 从工件坐标开始 + 2. 加上 G92 偏移 (axis_offset) + 3. 加上 G52 偏移 + 4. XY 旋转（正向, rotation_xy） + 5. 加上 G5x 偏移 (origin_offset) + 6. 加上刀具长度补偿 (tool_offset) + 7. 减去目标坐标系的 G5x 偏移 + 8. XY 旋转（反向, 目标坐标系的 rotation_xy） + 9. 如果 G92 激活，减去 G92 偏移 + + 与 _get_current_in_system_without_tlo 的区别： + - 此方法包含刀具长度补偿 + - 用于 G10 L20 和坐标系切换 (G54-G59) + + Args: + system: 坐标系编号 (1-9, 对应 G54-G59.3) + + Returns: + tuple: (x, y, z, a, b, c, u, v, w) 在目标坐标系中的坐标 + """ + # ===== 第1步：从工件坐标开始 ===== + x = self.current_pos.x + y = self.current_pos.y + z = self.current_pos.z + a = self.current_pos.a + b = self.current_pos.b + c = self.current_pos.c + u = getattr(self.current_pos, 'u', 0.0) + v = getattr(self.current_pos, 'v', 0.0) + w = getattr(self.current_pos, 'w', 0.0) + + # ===== 第2步：加上 G92 偏移 (axis_offset) ===== + # 对应官方: x += s->axis_offset_x; ... + x += self.g92_offset_x + y += self.g92_offset_y + z += self.g92_offset_z + a += self.g92_offset_a + b += self.g92_offset_b + c += self.g92_offset_c + u += self.g92_offset_u + v += self.g92_offset_v + w += self.g92_offset_w + + # ===== 第3步：加上 G52 偏移 ===== + x += self.g52_offset_x + y += self.g52_offset_y + z += self.g52_offset_z + a += self.g52_offset_a + b += self.g52_offset_b + c += self.g52_offset_c + u += self.g52_offset_u + v += self.g52_offset_v + w += self.g52_offset_w + + # ===== 第4步：XY 旋转（正向, rotation_xy）===== + # 对应官方: rotate(x, y, s->rotation_xy); + if self.rotation_xy != 0: + rot_rad = math.radians(self.rotation_xy) + cos_r = math.cos(rot_rad) + sin_r = math.sin(rot_rad) + + # rotate(&x, &y, theta): + # xx = *x * cos(t) - *y * sin(t) + # yy = *x * sin(t) + *y * cos(t) + rot_x = x * cos_r - y * sin_r + rot_y = x * sin_r + y * cos_r + x, y = rot_x, rot_y + + # ===== 第5步：加上 G5x 偏移 (origin_offset) ===== + # 对应官方: x += s->origin_offset_x; ... + x += self.g5x_offset_x + y += self.g5x_offset_y + z += self.g5x_offset_z + a += self.g5x_offset_a + b += self.g5x_offset_b + c += self.g5x_offset_c + u += self.g5x_offset_u + v += self.g5x_offset_v + w += self.g5x_offset_w + + # ===== 第6步：加上刀具长度补偿 (tool_offset) ===== + # 对应官方: x += s->tool_offset.tran.x; ... + x += self.tool_offset.x + y += self.tool_offset.y + z += self.tool_offset.z + a += self.tool_offset.a + b += self.tool_offset.b + c += self.tool_offset.c + u += self.tool_offset.u + v += self.tool_offset.v + w += self.tool_offset.w + + # ===== 第7步：减去目标坐标系的 G5x 偏移 ===== + # 对应官方: + # *x -= USER_TO_PROGRAM_LEN(p[5201 + system * 20]); + # *y -= USER_TO_PROGRAM_LEN(p[5202 + system * 20]); + target_offsets = self.work_offsets.get(system, {}) + + x -= target_offsets.get('X', 0.0) + y -= target_offsets.get('Y', 0.0) + z -= target_offsets.get('Z', 0.0) + a -= target_offsets.get('A', 0.0) + b -= target_offsets.get('B', 0.0) + c -= target_offsets.get('C', 0.0) + u -= target_offsets.get('U', 0.0) + v -= target_offsets.get('V', 0.0) + w -= target_offsets.get('W', 0.0) + + # ===== 第8步：XY 旋转（反向, 目标坐标系的 rotation_xy）===== + # 对应官方: rotate(x, y, -p[5210 + system * 20]); + target_rotation = target_offsets.get('R', 0.0) + if target_rotation != 0: + rot_rad = math.radians(-target_rotation) + cos_r = math.cos(rot_rad) + sin_r = math.sin(rot_rad) + + rot_x = x * cos_r - y * sin_r + rot_y = x * sin_r + y * cos_r + x, y = rot_x, rot_y + + # ===== 第9步：如果 G92 激活，减去 G92 偏移 ===== + # 对应官方: + # if (p[5210]) { + # *x -= USER_TO_PROGRAM_LEN(p[5211]); + # ... + # } + if self.g92_active: + x -= self.g92_offset_x + y -= self.g92_offset_y + z -= self.g92_offset_z + a -= self.g92_offset_a + b -= self.g92_offset_b + c -= self.g92_offset_c + u -= self.g92_offset_u + v -= self.g92_offset_v + w -= self.g92_offset_w + + if self.debug: + print(f"[CSYS] _get_current_in_system(system={system}):") + print(f" 工件坐标: ({self.current_pos.x:.3f}, {self.current_pos.y:.3f}, {self.current_pos.z:.3f})") + print(f" 目标坐标: ({x:.3f}, {y:.3f}, {z:.3f})") + + return (x, y, z, a, b, c, u, v, w) + + + def _sync_tool_parameters_to_table(self, tool_idx: int): + """ + 同步刀具参数到参数表（5400-5413）和 HAL + + 对应 C++ 中 convert_setup_tool 中的： + - set_tool_parameters() + - SET_TOOL_TABLE_ENTRY() + """ + if tool_idx < 0 or tool_idx not in self.tool_table: + return + + tool = self.tool_table[tool_idx] + + # ===== 同步到参数表 (#5400-#5413) ===== + # 对应 C++ 中 setup::set_tool_parameters() + + # 5400: 刀具号 + # 随机换刀: 5400 = tool_table[0].toolno (如果 >= 0) 否则 -1 + # 非随机换刀: 5400 = tool_table[0].toolno (如果 > 0) 否则 0 + if self.current_tool and self.current_tool.tool_number == tool.tool_number: + self.var_manager._params[5400] = float(tool.tool_number) + elif tool_idx == 0: + self.var_manager._params[5400] = float(tool.tool_number) if tool.tool_number > 0 else 0.0 + else: + # 非当前刀具，不影响 5400 + pass + + # 5401-5409: 刀具偏移 + self.var_manager._params[5401] = tool.offset.x + self.var_manager._params[5402] = tool.offset.y + self.var_manager._params[5403] = tool.offset.z + self.var_manager._params[5404] = tool.offset.a + self.var_manager._params[5405] = tool.offset.b + self.var_manager._params[5406] = tool.offset.c + self.var_manager._params[5407] = tool.offset.u + self.var_manager._params[5408] = tool.offset.v + self.var_manager._params[5409] = tool.offset.w + + # 5410: 刀具直径 + self.var_manager._params[5410] = tool.diameter + + # 5411: 前角 + self.var_manager._params[5411] = tool.front_angle + + # 5412: 后角 + self.var_manager._params[5412] = tool.back_angle + + # 5413: 刀具方向 + self.var_manager._params[5413] = float(tool.orientation) + + # ===== 同步到 HAL 组件 ===== + # 对应 C++ 中 SET_TOOL_TABLE_ENTRY() + if hasattr(self, 'virtual_hal') and self.virtual_hal: + # 更新刀具相关 HAL 引脚 + self.virtual_hal.set('tool.number', tool.tool_number) + self.virtual_hal.set('tool.diameter', tool.diameter) + self.virtual_hal.set('tool.length', tool.offset.z) + + # ===== 如果是当前刀具，更新 tool_offset ===== + if self.current_tool and self.current_tool.tool_number == tool.tool_number: + self.tool_offset.x = tool.offset.x + self.tool_offset.y = tool.offset.y + self.tool_offset.z = tool.offset.z + self.tool_offset.a = tool.offset.a + self.tool_offset.b = tool.offset.b + self.tool_offset.c = tool.offset.c + + # 更新运动学的刀具长度 + if self.kinematics and self.current_h_code > 0: + self.kinematics.set_tool_length(tool.offset.z) + + if self.debug: + tool = self.tool_table[tool_idx] + print(f"[SYNC] 刀具 T{tool.tool_number} 参数已同步:") + print(f" #5400(ToolNo)={self.var_manager._params[5400]:.0f}") + print(f" #5403(Z-Off)={self.var_manager._params[5403]:.3f}") + print(f" #5410(Dia)={self.var_manager._params[5410]:.3f}") + + + def _handle_g10_current_tool_update(self, tool_idx: int, tool_number: int): + """ + 当 G10 修改的是当前刀具时，更新所有相关状态 + + 对应 C++ 中 convert_setup_tool 的最后部分： + - 更新 tool_table[0]（如果非随机换刀） + - 更新参数 5400-5413 + - 调用 SET_TOOL_TABLE_ENTRY + """ + if not self.current_tool or self.current_tool.tool_number != tool_number: + return + + tool = self.tool_table[tool_idx] + + # 更新当前刀具偏移 + self.tool_offset.x = tool.offset.x + self.tool_offset.y = tool.offset.y + self.tool_offset.z = tool.offset.z + self.tool_offset.a = tool.offset.a + self.tool_offset.b = tool.offset.b + self.tool_offset.c = tool.offset.c + + # 更新刀具长度映射 + self.tool_lengths[tool_number] = tool.offset.z + + # 更新刀具半径映射 + self.tool_radii[tool_number] = tool.radius + + # 如果有长度补偿激活，更新运动学 + if self.current_h_code > 0 and self.kinematics: + self.kinematics.set_tool_length(tool.offset.z) + + # 如果有半径补偿激活，更新半径 + if self.cutter_comp_active: + self.comp_radius = tool.radius + + # 同步到参数表 + self.var_manager.update_tool_params( + tool.tool_number, + { + 'x': tool.offset.x, 'y': tool.offset.y, 'z': tool.offset.z, + 'a': tool.offset.a, 'b': tool.offset.b, 'c': tool.offset.c, + 'u': tool.offset.u, 'v': tool.offset.v, 'w': tool.offset.w + }, + tool.diameter, + tool.front_angle, + tool.back_angle, + tool.orientation + ) + + if self.debug: + print(f"[G10] 当前刀具 T{tool_number} 已更新") + + + + + + + + + + # ==================== 参数同步 ==================== + + + + + + + + + + + + + + def _sync_parameters_from_state(self): + """同步状态到参数表""" + self.var_manager.update_position_params( + self.current_pos.x, self.current_pos.y, self.current_pos.z, + self.current_pos.a, self.current_pos.b, self.current_pos.c + ) + + self.var_manager.active_csys = self.g5x_index + + # 同步 G92 偏移 + self.var_manager._g92_active = self.g92_active + self.var_manager._g92_offsets['X'] = self.g92_offset_x + self.var_manager._g92_offsets['Y'] = self.g92_offset_y + self.var_manager._g92_offsets['Z'] = self.g92_offset_z + self.var_manager._g92_offsets['A'] = self.g92_offset_a + self.var_manager._g92_offsets['B'] = self.g92_offset_b + self.var_manager._g92_offsets['C'] = self.g92_offset_c + + # 同步 G52 偏移 + self.var_manager._g52_offsets['X'] = self.g52_offset_x + self.var_manager._g52_offsets['Y'] = self.g52_offset_y + self.var_manager._g52_offsets['Z'] = self.g52_offset_z + + # 同步刀具偏移 + self.var_manager._tool_offset['X'] = self.tool_offset.x + self.var_manager._tool_offset['Y'] = self.tool_offset.y + self.var_manager._tool_offset['Z'] = self.tool_offset.z + self.var_manager._tool_offset['A'] = self.tool_offset.a + self.var_manager._tool_offset['B'] = self.tool_offset.b + self.var_manager._tool_offset['C'] = self.tool_offset.c + self.var_manager._tool_offset_active = (self.current_h_code > 0) + + # 同步 XY 旋转 + self.var_manager.set_xy_rotation(self.rotation_xy) + + # 同步 G5x 坐标系偏移 + if self.g5x_index in self.work_offsets: + wo = self.work_offsets[self.g5x_index] + self.var_manager.set_csys_origin( + self.g5x_index, + wo.get('X', 0.0), wo.get('Y', 0.0), wo.get('Z', 0.0), + wo.get('A', 0.0), wo.get('B', 0.0), wo.get('C', 0.0) + ) + + # 同步模态状态 + self.var_manager._distance_mode = 'ABSOLUTE' if self.is_absolute else 'INCREMENTAL' + self.var_manager._plane_mode = {17: 'XY', 18: 'XZ', 19: 'YZ'}.get(self.current_plane, 'XY') + self.var_manager._feed_rate = self.feedrate + self.var_manager._spindle_speed = self.state.spindle_speed + + self.var_manager.set_system('_x', self.current_pos.x) + self.var_manager.set_system('_y', self.current_pos.y) + self.var_manager.set_system('_z', self.current_pos.z) + self.var_manager.set_system('_feed', self.feedrate) + self.var_manager.set_system('_current_tool', float(self.state.tool)) + self.var_manager.set_system('_rpm', self.state.spindle_speed) + self.var_manager.set_system('_absolute', 1.0 if self.is_absolute else 0.0) + self.var_manager.set_system('_metric', 1.0 if self.state.units == 21 else 0.0) + self.var_manager._current_line = self.current_line + + + def _sync_machine_from_current(self): + """从工件坐标同步机器坐标 + + 正变换顺序（与 _compute_machine_coords 严格对应）： + 工件坐标 → +G92偏移 → +XY旋转 → +G5x偏移 → +TLO → +G52偏移 → 逆运动学 → 关节坐标 + + 参照 LinuxCNC 源码： + - rs274ngc.cpp: find_current_in_system() 计算绝对坐标 + - kinematics.cpp: kinematicsInverse() 计算关节坐标 + """ + # ===== 第1步：加上 G92 偏移 ===== + # 对应 C++: *x += s->axis_offset_x + abs_x = self.current_pos.x + self.g92_offset_x + abs_y = self.current_pos.y + self.g92_offset_y + abs_z = self.current_pos.z + self.g92_offset_z + abs_a = self.current_pos.a + self.g92_offset_a + abs_b = self.current_pos.b + self.g92_offset_b + abs_c = self.current_pos.c + self.g92_offset_c + + # ===== 第2步：XY 旋转 ===== + # 对应 C++: rotate(x, y, s->rotation_xy) + if abs(self.rotation_xy) > 1e-9: + rot = math.radians(self.rotation_xy) + cos_r = math.cos(rot) + sin_r = math.sin(rot) + rx = abs_x * cos_r - abs_y * sin_r + ry = abs_x * sin_r + abs_y * cos_r + abs_x, abs_y = rx, ry + + # ===== 第3步：加上 G5x 偏移 ===== + # 对应 C++: *x += s->origin_offset_x + abs_x += self.g5x_offset_x + abs_y += self.g5x_offset_y + abs_z += self.g5x_offset_z + abs_a += self.g5x_offset_a + abs_b += self.g5x_offset_b + abs_c += self.g5x_offset_c + + # ===== 第4步：加上刀具长度补偿 (TLO) ===== + # 对应 C++: *x += s->tool_offset.tran.x + abs_x += self.tool_offset.x + abs_y += self.tool_offset.y + abs_z += self.tool_offset.z + abs_a += self.tool_offset.a + abs_b += self.tool_offset.b + abs_c += self.tool_offset.c + + # ===== 第5步：加上额外工件偏移 ===== + abs_x += self.work_offset_x + abs_y += self.work_offset_y + abs_z += self.work_offset_z + abs_a += self.work_offset_a + abs_b += self.work_offset_b + abs_c += self.work_offset_c + + # ===== 第6步：加上 G52 偏移 ===== + abs_x += self.g52_offset_x + abs_y += self.g52_offset_y + abs_z += self.g52_offset_z + abs_a += self.g52_offset_a + abs_b += self.g52_offset_b + abs_c += self.g52_offset_c + + # ===== 第7步：计算关节坐标（如果需要RTCP）===== + # 对应 C++: motion controller 调用 kinematicsInverse() + if self.rtcp_enabled and self.kinematics is not None: + try: + # 传入包含 TLO 的绝对坐标 + world_tuple = (abs_x, abs_y, abs_z, abs_a, abs_b, abs_c) + joints = self.kinematics.world_to_joints(world_tuple) + + # 根据运动学类型映射关节坐标 + config = KINEMATICS_JOINT_CONFIG.get( + self.kinematics_type, + KINEMATICS_JOINT_CONFIG[KinematicsType.IDENTITY] + ) + + x_idx = config.get('x_idx', 0) + y_idx = config.get('y_idx', 1) + z_idx = config.get('z_idx', 2) + a_idx = config.get('a_idx', 3) + b_idx = config.get('b_idx', 4) + c_idx = config.get('c_idx', 5) + + self.machine_x = joints[x_idx] if (x_idx >= 0 and x_idx < len(joints)) else abs_x + self.machine_y = joints[y_idx] if (y_idx >= 0 and y_idx < len(joints)) else abs_y + self.machine_z = joints[z_idx] if (z_idx >= 0 and z_idx < len(joints)) else abs_z + self.machine_a = joints[a_idx] if (a_idx >= 0 and a_idx < len(joints)) else abs_a + self.machine_b = joints[b_idx] if (b_idx >= 0 and b_idx < len(joints)) else abs_b + self.machine_c = joints[c_idx] if (c_idx >= 0 and c_idx < len(joints)) else abs_c + except Exception as e: + if self.debug: + print(f"[MCS] 逆运动学计算失败: {e}") + self.machine_x = abs_x + self.machine_y = abs_y + self.machine_z = abs_z + self.machine_a = abs_a + self.machine_b = abs_b + self.machine_c = abs_c + else: + # RTCP 禁用：直接使用绝对坐标作为机器坐标 + self.machine_x = abs_x + self.machine_y = abs_y + self.machine_z = abs_z + self.machine_a = abs_a + self.machine_b = abs_b + self.machine_c = abs_c + + + def _update_current_from_machine(self): + """从机器坐标更新工件坐标 + + 逆变换顺序（与 _sync_machine_from_current 严格对应）： + 绝对坐标 → -G52偏移 → -TLO → -G5x偏移 → -XY旋转 → -G92偏移 → 工件坐标 + + 正变换: 工件 + G92 + XY旋转 + G5x + TLO + G52 = 绝对 + 逆变换: 绝对 - G52 - TLO - G5x - XY旋转 - G92 = 工件 + """ + # ===== 第1步：运动学正解（关节→世界）===== + if self.rtcp_enabled and self.kinematics is not None: + # 构建符合运动学类型的关节数组 + config = KINEMATICS_JOINT_CONFIG.get(self.kinematics_type, {}) + num_joints = config.get('num_joints', 6) + joints = [0.0] * num_joints + + x_idx = config.get('x_idx', 0) + y_idx = config.get('y_idx', 1) + z_idx = config.get('z_idx', 2) + a_idx = config.get('a_idx', 3) + b_idx = config.get('b_idx', 4) + c_idx = config.get('c_idx', 5) + + if x_idx >= 0: joints[x_idx] = self.machine_x + if y_idx >= 0: joints[y_idx] = self.machine_y + if z_idx >= 0: joints[z_idx] = self.machine_z + if a_idx >= 0: joints[a_idx] = self.machine_a + if b_idx >= 0: joints[b_idx] = self.machine_b + if c_idx >= 0: joints[c_idx] = self.machine_c + + world = self.kinematics.joints_to_world(joints) + + abs_x, abs_y, abs_z = world[0], world[1], world[2] + abs_a, abs_b, abs_c = world[3], world[4], world[5] + else: + # 非RTCP模式：机器坐标 = 绝对坐标 + abs_x, abs_y, abs_z = self.machine_x, self.machine_y, self.machine_z + abs_a, abs_b, abs_c = self.machine_a, self.machine_b, self.machine_c + + # ===== 第2步：逆变换（与 _sync_machine_from_current 严格对应）===== + # 正变换: 工件 + G92 + XY旋转 + G5x + TLO + G52 → 绝对 + # 逆变换: 绝对 - G52 - TLO - G5x - XY旋转 - G92 → 工件 + + # ★ 关键修复：逆变换减法顺序与正变换加法顺序相反 ★ + # Step 2a: 减去 G52 偏移 + x = abs_x - self.g52_offset_x + y = abs_y - self.g52_offset_y + z = abs_z - self.g52_offset_z + a = abs_a - self.g52_offset_a + b = abs_b - self.g52_offset_b + c = abs_c - self.g52_offset_c + + # Step 2b: 减去额外工件偏移 + x -= self.work_offset_x + y -= self.work_offset_y + z -= self.work_offset_z + a -= self.work_offset_a + b -= self.work_offset_b + c -= self.work_offset_c + + # Step 2c: 减去刀具长度补偿 (TLO) + x -= self.tool_offset.x + y -= self.tool_offset.y + z -= self.tool_offset.z + a -= self.tool_offset.a + b -= self.tool_offset.b + c -= self.tool_offset.c + + # Step 2d: 减去 G5x 坐标系偏移 + x -= self.g5x_offset_x + y -= self.g5x_offset_y + z -= self.g5x_offset_z + a -= self.g5x_offset_a + b -= self.g5x_offset_b + c -= self.g5x_offset_c + + # Step 2e: 反向 XY 旋转（如果设置了旋转角度） + # 注意：这里需要在减去G92之前反向旋转 + if abs(self.rotation_xy) > 1e-9: + rot = math.radians(-self.rotation_xy) # 反向旋转 + cos_r = math.cos(rot) + sin_r = math.sin(rot) + rx = x * cos_r - y * sin_r + ry = x * sin_r + y * cos_r + x, y = rx, ry + + # Step 2f: 减去 G92 偏移 + x -= self.g92_offset_x + y -= self.g92_offset_y + z -= self.g92_offset_z + a -= self.g92_offset_a + b -= self.g92_offset_b + c -= self.g92_offset_c + + # ===== 第3步：更新工件坐标 ===== + self.current_pos.x = x + self.current_pos.y = y + self.current_pos.z = z + self.current_pos.a = a + self.current_pos.b = b + self.current_pos.c = c + + # ===== 第4步：同步参数表 ===== + self._sync_parameters_from_state() + + # ==================== 坐标系管理 ==================== + + + def set_coordinate_system(self, csys: int): + """ + G54-G59.3: 选择坐标系 + + 完全参照 LinuxCNC 官方实现： + rs274ngc.cpp: Interp::convert_coordinate_system + + 关键：切换坐标系时，使用 _get_current_in_system 重新计算工件坐标 + + Args: + csys: 坐标系编号 (1-9) + """ + if csys < 1 or csys > 9: + if self.debug: + print(f"[CSYS] 错误: 坐标系编号 {csys} 超出范围 (1-9)") + return + + if csys == self.g5x_index: + if self.debug: + print(f"[CSYS] 已经是 G{53 + csys}，无需切换") + return + + if self.debug: + print(f"[CSYS] ========== 切换坐标系 G{53 + self.g5x_index} → G{53 + csys} ==========") + print(f"[CSYS] 切换前工件坐标: ({self.current_pos.x:.3f}, {self.current_pos.y:.3f}, {self.current_pos.z:.3f})") + + # ===== 第1步：计算当前在目标坐标系中的位置 ===== + # 对应官方: find_current_in_system(settings, origin, &settings->current_x, ...) + (new_x, new_y, new_z, new_a, new_b, new_c, new_u, new_v, new_w) = \ + self._get_current_in_system(csys) + + # ===== 第2步：更新当前位置 ===== + self.current_pos.x = new_x + self.current_pos.y = new_y + self.current_pos.z = new_z + self.current_pos.a = new_a + self.current_pos.b = new_b + self.current_pos.c = new_c + + # ===== 第3步：更新 origin_index ===== + self.g5x_index = csys + self.current_work_offset = csys + + # ===== 第4步：从参数表加载新坐标系偏移 ===== + # 对应官方: parameters[5201 + (origin * 20)] + new_offsets = self.work_offsets.get(csys, {}) + + self.g5x_offset_x = new_offsets.get('X', 0.0) + self.g5x_offset_y = new_offsets.get('Y', 0.0) + self.g5x_offset_z = new_offsets.get('Z', 0.0) + self.g5x_offset_a = new_offsets.get('A', 0.0) + self.g5x_offset_b = new_offsets.get('B', 0.0) + self.g5x_offset_c = new_offsets.get('C', 0.0) + self.g5x_offset_u = new_offsets.get('U', 0.0) + self.g5x_offset_v = new_offsets.get('V', 0.0) + self.g5x_offset_w = new_offsets.get('W', 0.0) + + # ===== 第5步：更新 XY 旋转 ===== + # 对应官方: settings->rotation_xy = parameters[5210 + (origin * 20)]; + new_rotation = new_offsets.get('R', 0.0) + self.rotation_xy = new_rotation + t = math.radians(new_rotation) + self.rotation_sin = math.sin(t) + self.rotation_cos = math.cos(t) + + # ===== 第6步：同步到参数表 ===== + self._sync_parameters_from_state() + self._sync_machine_from_current() + + if self.debug: + print(f"[CSYS] 切换后工件坐标: ({self.current_pos.x:.3f}, {self.current_pos.y:.3f}, {self.current_pos.z:.3f})") + print(f"[CSYS] 新G5x偏移: ({self.g5x_offset_x:.3f}, {self.g5x_offset_y:.3f}, {self.g5x_offset_z:.3f})") + print(f"[CSYS] 新XY旋转: {self.rotation_xy}°") + print(f"[CSYS] ========== 坐标系切换完成 ==========") + + + def set_g5x_offset(self, csys: int, x: float = None, y: float = None, z: float = None, + a: float = 0.0, b: float = 0.0, c: float = 0.0, + u: float = 0.0, v: float = 0.0, w: float = 0.0): + """G10 L2: 直接设置坐标系偏移""" + if csys < 1 or csys > 9: + return + + if csys not in self.work_offsets: + self.work_offsets[csys] = {} + + old_offsets = self.work_offsets[csys].copy() + updates = {'X': x, 'Y': y, 'Z': z, 'A': a, 'B': b, 'C': c, 'U': u, 'V': v, 'W': w} + + for axis, val in updates.items(): + if val is not None: + self.work_offsets[csys][axis] = val + + if csys == self.g5x_index: + for axis in 'XYZABC': + if axis in old_offsets: + delta = old_offsets[axis] - self.work_offsets[csys].get(axis, 0.0) + if axis == 'X': self.current_pos.x += delta + elif axis == 'Y': self.current_pos.y += delta + elif axis == 'Z': self.current_pos.z += delta + elif axis == 'A': self.current_pos.a += delta + elif axis == 'B': self.current_pos.b += delta + elif axis == 'C': self.current_pos.c += delta + + self.g5x_offset_x = self.work_offsets[csys].get('X', 0.0) + self.g5x_offset_y = self.work_offsets[csys].get('Y', 0.0) + self.g5x_offset_z = self.work_offsets[csys].get('Z', 0.0) + self.g5x_offset_a = self.work_offsets[csys].get('A', 0.0) + self.g5x_offset_b = self.work_offsets[csys].get('B', 0.0) + self.g5x_offset_c = self.work_offsets[csys].get('C', 0.0) + + self.var_manager.set_csys_origin(csys, + self.work_offsets[csys].get('X', 0.0), + self.work_offsets[csys].get('Y', 0.0), + self.work_offsets[csys].get('Z', 0.0), + self.work_offsets[csys].get('A', 0.0), + self.work_offsets[csys].get('B', 0.0), + self.work_offsets[csys].get('C', 0.0)) + + self._sync_machine_from_current() + self._sync_parameters_from_state() + + + + + def set_coordinate_system_g10_l20_bak(self, csys: int, params_str: str): + """G10 L20: 设置坐标系使当前位置变为指定值""" + if csys < 1 or csys > 9: + return + + target_csys = csys + old_offsets = self.work_offsets.get(target_csys, {}) + + cx = self.current_pos.x + self.g92_offset_x + self.g52_offset_x + cy = self.current_pos.y + self.g92_offset_y + self.g52_offset_y + cz = self.current_pos.z + self.g92_offset_z + self.g52_offset_z + ca = self.current_pos.a + self.g92_offset_a + self.g52_offset_a + cb = self.current_pos.b + self.g92_offset_b + self.g52_offset_b + cc = self.current_pos.c + self.g92_offset_c + self.g52_offset_c + + if self.rotation_xy != 0: + crot = math.cos(math.radians(self.rotation_xy)) + srot = math.sin(math.radians(self.rotation_xy)) + rotx = cx * crot - cy * srot + roty = cx * srot + cy * crot + cx, cy = rotx, roty + + cx += self.g5x_offset_x + cy += self.g5x_offset_y + cz += self.g5x_offset_z + ca += self.g5x_offset_a + cb += self.g5x_offset_b + cc += self.g5x_offset_c + + target_g5x_x = old_offsets.get('X', 0.0) + target_g5x_y = old_offsets.get('Y', 0.0) + target_g5x_z = old_offsets.get('Z', 0.0) + + cx -= target_g5x_x + cy -= target_g5x_y + cz -= target_g5x_z + + target_x = cx + target_y = cy + target_z = cz + + for match in re.finditer(r'([XYZ])\s*=\s*([+-]?\d*\.?\d+)', params_str, re.I): + axis = match.group(1).upper() + value = float(match.group(2)) + if axis == 'X': target_x = value + elif axis == 'Y': target_y = value + elif axis == 'Z': target_z = value + + new_g5x_x = target_g5x_x + (cx - target_x) + new_g5x_y = target_g5x_y + (cy - target_y) + new_g5x_z = target_g5x_z + (cz - target_z) + + self.work_offsets[target_csys]['X'] = new_g5x_x + self.work_offsets[target_csys]['Y'] = new_g5x_y + self.work_offsets[target_csys]['Z'] = new_g5x_z + + if target_csys == self.g5x_index: + self.g5x_offset_x = new_g5x_x + self.g5x_offset_y = new_g5x_y + self.g5x_offset_z = new_g5x_z + + self.current_pos.x = target_x + self.current_pos.y = target_y + self.current_pos.z = target_z + + self.var_manager.set_csys_origin(target_csys, new_g5x_x, new_g5x_y, new_g5x_z) + self._sync_machine_from_current() + self._sync_parameters_from_state() + + + def set_coordinate_system_g10_l20(self, csys: int, params_str: str): + """ + G10 L20: 设置坐标系使当前位置变为指定值 + + 完全参照 LinuxCNC 官方实现： + rs274ngc.cpp: Interp::convert_setup (G10 L20 分支) + + 核心逻辑： + 1. 获取目标坐标系的旧偏移和旧旋转 + 2. 使用 _get_current_in_system 计算当前在目标坐标系中的位置 + 3. 应用用户指定的目标值 + 4. 正向 XY 旋转（使用旧 rotation_xy） + 5. 计算新 G5x 偏移：old_position + old_origin - new_position = new_origin + 6. 存储结果 + 7. 如果是当前坐标系，更新 rotation_xy 为 0 + + Args: + csys: 坐标系编号 (0=当前坐标系, 1-9=指定坐标系) + params_str: 参数字符串，如 "X0 Y0 Z100 A0 C0" + """ + # 处理 P0（当前坐标系） + if csys == 0: + csys = self.g5x_index + + if csys < 1 or csys > 9: + if self.debug: + print(f"[G10 L20] 错误: 坐标系编号 {csys} 超出范围 (1-9)") + return + + if self.debug: + print(f"[G10 L20] ========== 设置坐标系 G{53 + csys} ==========") + print(f"[G10 L20] 参数: '{params_str}'") + print(f"[G10 L20] 当前工件坐标: ({self.current_pos.x:.3f}, {self.current_pos.y:.3f}, {self.current_pos.z:.3f})") + print(f"[G10 L20] 当前G5x索引: {self.g5x_index}, 旋转: {self.rotation_xy}°") + + # ===== 第1步：获取目标坐标系的旧偏移和旧旋转 ===== + old_offsets = self.work_offsets.get(csys, {}).copy() + + old_g5x_x = old_offsets.get('X', 0.0) + old_g5x_y = old_offsets.get('Y', 0.0) + old_g5x_z = old_offsets.get('Z', 0.0) + old_g5x_a = old_offsets.get('A', 0.0) + old_g5x_b = old_offsets.get('B', 0.0) + old_g5x_c = old_offsets.get('C', 0.0) + old_rotation = old_offsets.get('R', 0.0) + + # ===== 第2步：计算当前在目标坐标系中的位置 ===== + # 对应官方: find_current_in_system(settings, p_int, &cx, &cy, &cz, ...) + (cx, cy, cz, ca, cb, cc, cu, cv, cw) = self._get_current_in_system(csys) + + if self.debug: + print(f"[G10 L20] 当前在G{53+csys}中的位置: ({cx:.3f}, {cy:.3f}, {cz:.3f})") + print(f"[G10 L20] 旧G5x偏移: ({old_g5x_x:.3f}, {old_g5x_y:.3f}, {old_g5x_z:.3f})") + print(f"[G10 L20] 旧XY旋转: {old_rotation}°") + + # ===== 第3步：记录旧坐标并应用用户指定的值 ===== + # 对应官方: double oldx = cx, oldy = cy; x = cx; y = cy; + oldx = cx + oldy = cy + x = cx + y = cy + z = cz + a = ca + b = cb + c_val = cc + + # 解析用户指定的坐标值（支持 X=0 和 X0 两种格式） + for match in re.finditer(r'([XYZABC])\s*=\s*([+-]?\d*\.?\d+)', params_str, re.I): + axis = match.group(1).upper() + value = float(match.group(2)) + + if axis == 'X': x = value + elif axis == 'Y': y = value + elif axis == 'Z': z = value + elif axis == 'A': a = value + elif axis == 'B': b = value + elif axis == 'C': c_val = value + + if self.debug: + print(f"[G10 L20] 用户指定值: ({x:.3f}, {y:.3f}, {z:.3f}, A={a}, B={b}, C={c_val})") + + # ===== 第4步：正向 XY 旋转（使用旧 rotation_xy）===== + # 对应官方: rotate(&oldx, &oldy, r); rotate(&x, &y, r); + if old_rotation != 0: + rot_rad = math.radians(old_rotation) + cos_r = math.cos(rot_rad) + sin_r = math.sin(rot_rad) + + rot_oldx = oldx * cos_r - oldy * sin_r + rot_oldy = oldx * sin_r + oldy * cos_r + + rot_x = x * cos_r - y * sin_r + rot_y = x * sin_r + y * cos_r + + oldx, oldy = rot_oldx, rot_oldy + x, y = rot_x, rot_y + + # ===== 第5步：计算新 G5x 偏移 ===== + # 对应官方: + # x = oldx + USER_TO_PROGRAM_LEN(parameters[5201 + (p_int * 20)]) - x; + # y = oldy + USER_TO_PROGRAM_LEN(parameters[5202 + (p_int * 20)]) - y; + new_g5x_x = oldx + old_g5x_x - x + new_g5x_y = oldy + old_g5x_y - y + new_g5x_z = cz + old_g5x_z - z + new_g5x_a = ca + old_g5x_a - a + new_g5x_b = cb + old_g5x_b - b + new_g5x_c = cc + old_g5x_c - c_val + + # ===== 第6步：存储新偏移 ===== + if csys not in self.work_offsets: + self.work_offsets[csys] = {} + + self.work_offsets[csys]['X'] = new_g5x_x + self.work_offsets[csys]['Y'] = new_g5x_y + self.work_offsets[csys]['Z'] = new_g5x_z + self.work_offsets[csys]['A'] = new_g5x_a + self.work_offsets[csys]['B'] = new_g5x_b + self.work_offsets[csys]['C'] = new_g5x_c + self.work_offsets[csys]['U'] = 0.0 + self.work_offsets[csys]['V'] = 0.0 + self.work_offsets[csys]['W'] = 0.0 + self.work_offsets[csys]['R'] = 0.0 # G10 L20 将 XY 旋转清零 + + # ===== 第7步：如果是当前坐标系，更新 rotation_xy ===== + # 对应官方: + # if (p_int == settings->origin_index) { + # rotate(&settings->current_x, &settings->current_y, settings->rotation_xy); + # settings->rotation_xy = 0; + # } + if csys == self.g5x_index: + # 先正向旋转当前坐标（去掉旧旋转） + if self.rotation_xy != 0: + rot_rad = math.radians(self.rotation_xy) + cos_r = math.cos(rot_rad) + sin_r = math.sin(rot_rad) + rot_x = self.current_pos.x * cos_r - self.current_pos.y * sin_r + rot_y = self.current_pos.x * sin_r + self.current_pos.y * cos_r + self.current_pos.x = rot_x + self.current_pos.y = rot_y + + # 加上旧 G5x 偏移 + self.current_pos.x += self.g5x_offset_x + self.current_pos.y += self.g5x_offset_y + self.current_pos.z += self.g5x_offset_z + self.current_pos.a += self.g5x_offset_a + self.current_pos.b += self.g5x_offset_b + self.current_pos.c += self.g5x_offset_c + + # 减去新 G5x 偏移 + self.current_pos.x -= new_g5x_x + self.current_pos.y -= new_g5x_y + self.current_pos.z -= new_g5x_z + self.current_pos.a -= new_g5x_a + self.current_pos.b -= new_g5x_b + self.current_pos.c -= new_g5x_c + + # 更新 G5x 偏移变量 + self.g5x_offset_x = new_g5x_x + self.g5x_offset_y = new_g5x_y + self.g5x_offset_z = new_g5x_z + self.g5x_offset_a = new_g5x_a + self.g5x_offset_b = new_g5x_b + self.g5x_offset_c = new_g5x_c + + # 清零 XY 旋转 + self.rotation_xy = 0.0 + self.rotation_sin = 0.0 + self.rotation_cos = 1.0 + + # ===== 第8步：同步到参数表 ===== + if hasattr(self, 'var_manager') and self.var_manager: + self.var_manager.set_csys_origin( + csys, new_g5x_x, new_g5x_y, new_g5x_z, + new_g5x_a, new_g5x_b, new_g5x_c + ) + self.var_manager.set_xy_rotation( + 0.0 if csys == self.g5x_index else self.rotation_xy + ) + + # ===== 第9步：同步机器坐标和参数 ===== + self._sync_machine_from_current() + self._sync_parameters_from_state() + + if self.debug: + print(f"[G10 L20] 新G5x偏移: ({new_g5x_x:.3f}, {new_g5x_y:.3f}, {new_g5x_z:.3f})") + print(f"[G10 L20] 更新后工件坐标: ({self.current_pos.x:.3f}, {self.current_pos.y:.3f}, {self.current_pos.z:.3f})") + print(f"[G10 L20] 更新后XY旋转: {self.rotation_xy}°") + print(f"[G10 L20] ========== G10 L20 完成 ==========") + + + + def set_g92_offset(self, x: float = None, y: float = None, z: float = None, + a: float = None, b: float = None, c: float = None, + u: float = None, v: float = None, w: float = None): + """G92: 设置临时坐标系偏移。 + + LinuxCNC 的 G92 允许把某个轴设置为 0，因此不能用 0.0 + 表示“该轴未指定”。这里用 None 表示未指定，只对实际出现 + 在 G92 程序段中的轴计算 offset。 + """ + old_x, old_y, old_z = self.current_pos.x, self.current_pos.y, self.current_pos.z + + any_axis = False + + if x is not None: + self.g92_offset_x = self.g92_offset_x + self.current_pos.x - x + self.current_pos.x = x + any_axis = True + if y is not None: + self.g92_offset_y = self.g92_offset_y + self.current_pos.y - y + self.current_pos.y = y + any_axis = True + if z is not None: + self.g92_offset_z = self.g92_offset_z + self.current_pos.z - z + self.current_pos.z = z + any_axis = True + if a is not None: + self.g92_offset_a = self.g92_offset_a + self.current_pos.a - a + self.current_pos.a = a + any_axis = True + if b is not None: + self.g92_offset_b = self.g92_offset_b + self.current_pos.b - b + self.current_pos.b = b + any_axis = True + if c is not None: + self.g92_offset_c = self.g92_offset_c + self.current_pos.c - c + self.current_pos.c = c + any_axis = True + if u is not None: + self.g92_offset_u = self.g92_offset_u + getattr(self.current_pos, 'u', 0.0) - u + self.current_pos.u = u + any_axis = True + if v is not None: + self.g92_offset_v = self.g92_offset_v + getattr(self.current_pos, 'v', 0.0) - v + self.current_pos.v = v + any_axis = True + if w is not None: + self.g92_offset_w = self.g92_offset_w + getattr(self.current_pos, 'w', 0.0) - w + self.current_pos.w = w + any_axis = True + + if not any_axis: + return + + self.g92_active = True + + # 同步参数表。apply_g92 需要 G92 生效前的当前位置。 + self.var_manager.apply_g92(x, y, z, old_x, old_y, old_z) + self.var_manager._params[InterpParameterIndex.G92_A] = self.g92_offset_a + self.var_manager._params[InterpParameterIndex.G92_B] = self.g92_offset_b + self.var_manager._params[InterpParameterIndex.G92_C] = self.g92_offset_c + self.var_manager._params[InterpParameterIndex.G92_U] = self.g92_offset_u + self.var_manager._params[InterpParameterIndex.G92_V] = self.g92_offset_v + self.var_manager._params[InterpParameterIndex.G92_W] = self.g92_offset_w + + self._sync_machine_from_current() + self._sync_parameters_from_state() + + def clear_g92(self): + """G92.1: 清除G92偏移""" + self.current_pos.x += self.g92_offset_x + self.current_pos.y += self.g92_offset_y + self.current_pos.z += self.g92_offset_z + self.current_pos.a += self.g92_offset_a + self.current_pos.b += self.g92_offset_b + self.current_pos.c += self.g92_offset_c + + self.g92_offset_x = 0.0 + self.g92_offset_y = 0.0 + self.g92_offset_z = 0.0 + self.g92_offset_a = 0.0 + self.g92_offset_b = 0.0 + self.g92_offset_c = 0.0 + self.g92_active = False + + self.var_manager.clear_g92() + self._sync_machine_from_current() + self._sync_parameters_from_state() + + def set_g52_offset(self, x: float = 0.0, y: float = 0.0, z: float = 0.0, + a: float = 0.0, b: float = 0.0, c: float = 0.0): + """G52: 设置临时工件偏移""" + old_g52_x = self.g52_offset_x + old_g52_y = self.g52_offset_y + old_g52_z = self.g52_offset_z + + self.g52_offset_x = x + self.g52_offset_y = y + self.g52_offset_z = z + self.g52_offset_a = a + self.g52_offset_b = b + self.g52_offset_c = c + + self.current_pos.x += old_g52_x - x + self.current_pos.y += old_g52_y - y + self.current_pos.z += old_g52_z - z + self.current_pos.a += a if a != 0.0 else 0.0 + self.current_pos.b += b if b != 0.0 else 0.0 + self.current_pos.c += c if c != 0.0 else 0.0 + + self._sync_machine_from_current() + self._sync_parameters_from_state() + + def set_xy_rotation(self, theta: float): + """设置XY旋转（同步到参数表）""" + self.rotation_xy = theta + t = math.radians(theta) + self.rotation_sin = math.sin(t) + self.rotation_cos = math.cos(t) + # 同步到参数表 + self.var_manager.set_xy_rotation(theta) + + + # ==================== 运动学切换 ==================== + + def set_kinematics_by_type(self, kin_type: int): + """ + 运动学切换 + kin_type: 0 = IDENTITY (M429), 1 = 原始五轴 (M428), 2 = FIVEAXIS_BC (M430) + """ + if kin_type == 0: # M429: 切换到三轴恒等运动学 + self._switch_to_identity() + elif kin_type == 1: # M428: 恢复到原始五轴运动学 + self._switch_to_original() + elif kin_type == 2: # M430: 切换到 FIVEAXIS_BC + self._switch_to_fiveaxis_bc() + else: + # 未知的运动学类型，记录警告但不中断 + if self.debug: + print(f"[KINS] 警告: 未知的运动学切换类型: {kin_type}, 忽略") + + + def _switch_to_identity(self): + """ + M429: 切换到三轴恒等运动学 (IDENTITY) + + 关键修复: + 1. 保存当前运动学类型（用于M428恢复） + 2. 根据运动学类型保存对应的旋转轴角度 + 3. 清零所有旋转轴 (A, B, C) + 4. 禁用RTCP + """ + if self.debug: + print(f"[KINS] M429: 切换到三轴运动学 (IDENTITY)") + + # 保存切换前的运动学类型，以便 M428 恢复 + if self.kinematics_type != KinematicsType.IDENTITY: + self._saved_kinematics_type = self.kinematics_type + + # ★ 修复: 保存当前运动学参数，以便M428恢复 ★ + if self.kinematics and hasattr(self.kinematics, 'p'): + p = self.kinematics.p + self._saved_kinematics_params = { + 'rot_center_x': getattr(p, 'rot_center_x', 0.0), + 'rot_center_y': getattr(p, 'rot_center_y', 0.0), + 'rot_center_z': getattr(p, 'rot_center_z', -300.0), + 'axis_offset_x': getattr(p, 'axis_offset_x', 0.0), + 'axis_offset_y': getattr(p, 'axis_offset_y', 0.0), + 'axis_offset_z': getattr(p, 'axis_offset_z', 200.0), + 'tool_length': self.kinematics._tool_length, + 'conventional_directions': getattr(p, 'conventional_directions', False), + } + + if self.debug: + print(f"[KINS] 已保存原始运动学类型: {self._saved_kinematics_type.name}") + else: + # 已经是IDENTITY，不需要重复切换 + if self.debug: + print(f"[KINS] 已经是IDENTITY模式，跳过切换") + return + + # ===== 保存旋转轴位置（根据运动学类型） ===== + # 不同运动学类型使用的旋转轴不同 + self._saved_rotation_a = self.current_pos.a if self.current_pos.a is not None else 0.0 + self._saved_rotation_b = self.current_pos.b if self.current_pos.b is not None else 0.0 + self._saved_rotation_c = self.current_pos.c if self.current_pos.c is not None else 0.0 + + # 记录当前运动学类型，用于M428恢复时判断哪些轴需要恢复 + self._saved_rotation_kinematics = self.kinematics_type + + if self.debug: + kin_name = self.kinematics_type.name + # 根据运动学类型显示不同的轴信息 + if self.kinematics_type == KinematicsType.TRT_AC: + print(f"[KINS] 保存{kin_name}旋转轴: A={self._saved_rotation_a:.3f}, C={self._saved_rotation_c:.3f}") + elif self.kinematics_type == KinematicsType.TRT_BC: + print(f"[KINS] 保存{kin_name}旋转轴: B={self._saved_rotation_b:.3f}, C={self._saved_rotation_c:.3f}") + elif self.kinematics_type == KinematicsType.FIVEAXIS_BC: + print(f"[KINS] 保存{kin_name}旋转轴: B={self._saved_rotation_b:.3f}, C={self._saved_rotation_c:.3f}") + elif self.kinematics_type == KinematicsType.MAXKINS_BC: + print(f"[KINS] 保存{kin_name}旋转轴: A={self._saved_rotation_a:.3f}, B={self._saved_rotation_b:.3f}, C={self._saved_rotation_c:.3f}") + else: + print(f"[KINS] 保存{kin_name}旋转轴: A={self._saved_rotation_a:.3f}, B={self._saved_rotation_b:.3f}, C={self._saved_rotation_c:.3f}") + + # 切换到恒等运动学 + self.kinematics_type = KinematicsType.IDENTITY + self.data.kinematics_type = "IDENTITY" + self.rtcp_enabled = False + self.data.rtcp_enabled = False + + # 禁用当前运动学实例的 RTCP + if self.kinematics: + self.kinematics.enable_rtcp(False) + + # 设置恒等模式：直接透传坐标 + self.kinematics = None + + # ===== 清零所有旋转轴 ===== + # IDENTITY运动学不支持旋转轴，必须清零 + self.current_pos.a = 0.0 + self.current_pos.b = 0.0 + self.current_pos.c = 0.0 + + self.machine_a = 0.0 + self.machine_b = 0.0 + self.machine_c = 0.0 + + if self.debug: + print(f"[KINS] 清零旋转轴: A→0, B→0, C→0") + + # 同步参数表 + self._sync_parameters_from_state() + + if self.debug: + print(f"[KINS] 当前运动学: IDENTITY, RTCP: 禁用") + + + + def _switch_to_original(self): + """ + M428: 恢复到原始五轴运动学 + + 关键修复: + 1. 恢复保存的运动学类型 + 2. 根据运动学类型恢复对应的旋转轴角度 + 3. 重新创建运动学实例 + 4. 恢复刀具长度补偿 + 5. 启用RTCP + """ + if self.debug: + print(f"[KINS] M428: 恢复到五轴运动学") + + # 恢复保存的运动学类型 + # 优先级1: 使用M429之前通过_switch_to_identity保存的类型 + if hasattr(self, '_saved_kinematics_type') and self._saved_kinematics_type is not None: + restore_type = self._saved_kinematics_type + if self.debug: + print(f"[KINS] 使用保存的运动学类型: {restore_type.name}") + # 优先级2: 如果当前不是IDENTITY，说明从未切换过，使用当前类型 + elif self.kinematics_type != KinematicsType.IDENTITY: + restore_type = self.kinematics_type + if self.debug: + print(f"[KINS] 使用当前运动学类型: {restore_type.name}") + # 优先级3: 从data.kinematics_type记录恢复 + else: + saved_name = self.data.kinematics_type + type_map = { + 'TRT_AC': KinematicsType.TRT_AC, + 'TRT_BC': KinematicsType.TRT_BC, + 'FIVEAXIS_BC': KinematicsType.FIVEAXIS_BC, + 'MAXKINS_BC': KinematicsType.MAXKINS_BC, + 'IDENTITY': KinematicsType.IDENTITY, + } + restore_type = type_map.get(saved_name, KinematicsType.TRT_AC) + if self.debug: + print(f"[KINS] 从data记录恢复: {saved_name} -> {restore_type.name}") + + # 如果已经是目标类型，不需要重复切换 + if self.kinematics_type == restore_type and self.kinematics_type != KinematicsType.IDENTITY: + if self.debug: + print(f"[KINS] 已经是{restore_type.name}模式，跳过切换") + return + + self.kinematics_type = restore_type + self.data.kinematics_type = restore_type.name + self.rtcp_enabled = True + self.data.rtcp_enabled = True + + # ★ 修复: 重新创建运动学实例，使用保存的参数 ★ + # 获取保存的参数（如果之前有运动学实例） + saved_params = {} + if hasattr(self, '_saved_kinematics_params') and self._saved_kinematics_params: + saved_params = self._saved_kinematics_params + else: + # 从当前work_offsets或默认值重建参数 + saved_params = { + 'rot_center_x': getattr(self, '_saved_rot_center_x', 0.0), + 'rot_center_y': getattr(self, '_saved_rot_center_y', 0.0), + 'rot_center_z': getattr(self, '_saved_rot_center_z', -300.0), + 'axis_offset_x': getattr(self, '_saved_axis_offset_x', 0.0), + 'axis_offset_y': getattr(self, '_saved_axis_offset_y', 0.0), + 'axis_offset_z': getattr(self, '_saved_axis_offset_z', 200.0), + 'tool_length': 100.0, # 默认值 + } + + self.kinematics = KinematicsFactory.create( + restore_type, debug=self.debug, **saved_params + ) + + if self.kinematics: + self.kinematics.enable_rtcp(True) + # 重新设置刀具长度 + if self.current_h_code > 0: + tool_len = self.tool_lengths.get(self.current_h_code, 0.0) + self.kinematics.set_tool_length(tool_len) + if self.debug: + print(f"[KINS] 恢复刀具长度补偿: H{self.current_h_code} = {tool_len:.1f}mm") + elif self.debug: + print(f"[KINS] 注意: 无刀具长度补偿激活 (current_h_code={self.current_h_code})") + + # ===== 根据运动学类型恢复旋转轴 ===== + if hasattr(self, '_saved_rotation_a') and hasattr(self, '_saved_rotation_kinematics'): + saved_kin = self._saved_rotation_kinematics + + if self.debug: + print(f"[KINS] 保存时的运动学类型: {saved_kin.name}, 恢复为: {restore_type.name}") + + # ===== TRT_AC: 只有A轴和C轴 ===== + if restore_type == KinematicsType.TRT_AC: + self.current_pos.a = self._saved_rotation_a + self.current_pos.b = 0.0 # TRT_AC没有B轴，强制为0 + self.machine_b = 0.0 + self.current_pos.c = self._saved_rotation_c + if self.debug: + print(f"[KINS] 恢复TRT_AC旋转轴: A={self._saved_rotation_a:.3f}, " + f"C={self._saved_rotation_c:.3f}, B=0(强制)") + + # ===== TRT_BC: 只有B轴和C轴 ===== + elif restore_type == KinematicsType.TRT_BC: + self.current_pos.a = 0.0 # TRT_BC没有A轴，强制为0 + self.machine_a = 0.0 + self.current_pos.b = self._saved_rotation_b + self.current_pos.c = self._saved_rotation_c + if self.debug: + print(f"[KINS] 恢复TRT_BC旋转轴: B={self._saved_rotation_b:.3f}, " + f"C={self._saved_rotation_c:.3f}, A=0(强制)") + + # ===== FIVEAXIS_BC: B轴和C轴 ===== + elif restore_type == KinematicsType.FIVEAXIS_BC: + self.current_pos.a = 0.0 # FIVEAXIS_BC没有A轴 + self.current_pos.b = self._saved_rotation_b + self.current_pos.c = self._saved_rotation_c + if self.debug: + print(f"[KINS] 恢复FIVEAXIS_BC旋转轴: B={self._saved_rotation_b:.3f}, " + f"C={self._saved_rotation_c:.3f}, A=0(强制)") + + # ===== MAXKINS_BC: A, B, C轴都有 ===== + elif restore_type == KinematicsType.MAXKINS_BC: + self.current_pos.a = self._saved_rotation_a + self.current_pos.b = self._saved_rotation_b + self.current_pos.c = self._saved_rotation_c + if self.debug: + print(f"[KINS] 恢复MAXKINS_BC旋转轴: A={self._saved_rotation_a:.3f}, " + f"B={self._saved_rotation_b:.3f}, C={self._saved_rotation_c:.3f}") + + # ===== IDENTITY或其他: 保持为0 ===== + else: + self.current_pos.a = 0.0 + self.current_pos.b = 0.0 + self.current_pos.c = 0.0 + if self.debug: + print(f"[KINS] 恢复{restore_type.name}旋转轴: 全部清零") + else: + # 没有保存的旋转轴信息，全部清零 + self.current_pos.a = 0.0 + self.current_pos.b = 0.0 + self.current_pos.c = 0.0 + if self.debug: + print(f"[KINS] 无保存的旋转轴信息，全部清零") + + # 重新同步机器坐标 + self._sync_machine_from_current() + self._sync_parameters_from_state() + + if self.debug: + if self.kinematics: + world_pos = self.current_pos + tool_len = getattr(self.kinematics, '_tool_length', 0.0) + joints = self.kinematics.world_to_joints(( + world_pos.x, world_pos.y, world_pos.z + tool_len, + world_pos.a, world_pos.b, world_pos.c + )) + print(f"[XYZAC] INV: world({world_pos.x:.3f},{world_pos.y:.3f}," + f"{world_pos.z + tool_len:.3f},{world_pos.a:.3f},{world_pos.c:.3f}) -> " + f"joints({joints[0]:.3f},{joints[1]:.3f},{joints[2]:.3f})") + print(f"[KINS] 当前运动学: {restore_type.name}, RTCP: 启用") + + def _switch_to_fiveaxis_bc(self): + """M430: 切换到五轴桥式铣床运动学 (FIVEAXIS_BC)""" + if self.debug: + print(f"[KINS] M430: 切换到五轴桥式铣床运动学 (FIVEAXIS_BC)") + + # 保存切换前的运动学类型，以便M428恢复 + # 关键修复：只在当前不是FIVEAXIS_BC时才保存 + if self.kinematics_type != KinematicsType.FIVEAXIS_BC: + self._saved_kinematics_type = self.kinematics_type + if self.debug: + print(f"[KINS] 已保存原始运动学类型: {self._saved_kinematics_type.name}") + + # 切换到 FIVEAXIS_BC + self.kinematics_type = KinematicsType.FIVEAXIS_BC + self.data.kinematics_type = "FIVEAXIS_BC" + self.rtcp_enabled = True + self.data.rtcp_enabled = True + + # 创建新的运动学实例 + self.kinematics = KinematicsFactory.create( + KinematicsType.FIVEAXIS_BC, debug=self.debug + ) + + if self.kinematics: + self.kinematics.enable_rtcp(True) + # 恢复刀具长度补偿 + if self.current_h_code > 0: + tool_len = self.tool_lengths.get(self.current_h_code, 0.0) + self.kinematics.set_tool_length(tool_len) + if self.debug: + print(f"[KINS] 设置刀具长度: H{self.current_h_code} = {tool_len:.1f}mm") + + # 重新同步机器坐标 + self._sync_machine_from_current() + self._sync_parameters_from_state() + + if self.debug: + print(f"[KINS] 当前运动学: FIVEAXIS_BC, RTCP: 启用") + + + # ==================== RTCP 控制 ==================== + + + # ==================== RTCP 控制 ==================== + + def set_rtcp_enable(self, enable: bool = True, with_tool_length: bool = True): + """启用/禁用RTCP""" + if self.kinematics is None: + return + + old_rtcp = self.rtcp_enabled + self.rtcp_enabled = enable + self.data.rtcp_enabled = enable + self.kinematics.enable_rtcp(enable) + + # ===== 新增：生成RTCP状态切换的segment ===== + if enable and not old_rtcp: + segment = MoveSegment( + type=MoveType.RTCP_ON, + start=self.current_pos.copy(), + end=self.current_pos.copy(), + line_number=self.current_line, + is_rtcp=True + ) + self.data.segments.append(segment) + self._point_count += 1 + elif not enable and old_rtcp: + segment = MoveSegment( + type=MoveType.RTCP_OFF, + start=self.current_pos.copy(), + end=self.current_pos.copy(), + line_number=self.current_line, + is_rtcp=False + ) + self.data.segments.append(segment) + self._point_count += 1 + + self._sync_parameters_from_state() + + def set_tool_length_offset(self, h_code: int): + """ + G43 H: 设置刀具长度补偿（同步到参数表） + + 完全参照 LinuxCNC 官方实现： + rs274ngc.cpp: Interp::convert_tool_length_offset + + Args: + h_code: H 代码编号 + """ + self.current_h_code = h_code + tool_len = self.tool_lengths.get(h_code, 0.0) + + # ★ 关键修复：设置 tool_offset.z ★ + self.tool_offset.z = tool_len + + # 同步刀具偏移到参数表 + if hasattr(self, 'var_manager') and self.var_manager: + self.var_manager._tool_offset['Z'] = tool_len + self.var_manager._tool_offset_active = True + + # 更新运动学实例的刀具长度 + if self.kinematics: + self.kinematics.set_tool_length(tool_len) + if self.debug: + print(f"[TLO] 刀具长度补偿: H{h_code} = {tool_len:.1f}mm, 运动学已更新") + else: + if self.debug: + print(f"[TLO] 刀具长度补偿: H{h_code} = {tool_len:.1f}mm (运动学实例为空, IDENTITY模式)") + + def cancel_tool_length_offset(self): + """ + G49: 取消刀具长度补偿 + + 完全参照 LinuxCNC 官方实现： + rs274ngc.cpp: Interp::convert_tool_length_offset (G49 分支) + """ + self.current_h_code = 0 + self.tool_offset.z = 0.0 + + if hasattr(self, 'var_manager') and self.var_manager: + self.var_manager._tool_offset['Z'] = 0.0 + self.var_manager._tool_offset_active = False + + if self.kinematics: + self.kinematics.set_tool_length(0.0) + if self.debug: + print(f"[TLO] 刀具长度补偿已取消 (G49)") + + # ==================== 刀具补偿 ==================== + + def set_tool_radius_offset(self, d_code: int): + """设置刀具半径补偿值（从刀具表读取） + + Args: + d_code: D代码编号，对应刀具表中的刀具半径 + """ + self.current_d_code = d_code + radius = self.tool_radii.get(d_code, 0.0) + self.comp_radius = radius + + # 同步刀具半径到参数表 + self.var_manager._cutter_comp_radius = radius + + if self.debug: + print(f"[COMP] D{d_code} 刀具半径设置为: {radius:.3f} mm") + + def cancel_tool_radius_offset(self): + """取消刀具半径补偿（G40）""" + self.current_d_code = 0 + self.comp_radius = 0.0 + self.var_manager._cutter_comp_radius = 0.0 + + if self.debug: + print("[COMP] 刀具半径补偿已取消") + + def set_cutter_compensation_state(self, comp_type: CompType, radius: float = 0.0, + side: int = 0, d_word: float = 0.0): + """设置刀具半径补偿（同步到参数表）""" + old_type = self.comp_type + self.comp_type = comp_type + self.comp_radius = radius + self.d_word = d_word + self.cutter_comp_active = (comp_type != CompType.OFF) + + # ========== 同步到参数表 ========== + self.var_manager.set_cutter_compensation_active(self.cutter_comp_active) + + if comp_type == CompType.OFF and old_type != CompType.OFF: + self.cutter_comp_firstmove = True + self.arc_not_allowed = True + elif comp_type != CompType.OFF and old_type == CompType.OFF: + self.cutter_comp_firstmove = True + self.arc_not_allowed = False + self.program_x = self.current_pos.x + self.program_y = self.current_pos.y + self.program_z = self.current_pos.z + + + def cancel_cutter_compensation(self): + """G40: 取消刀具半径补偿""" + self.set_cutter_compensation_state(CompType.OFF, 0.0, 0, 0.0) + + # ==================== 运动段添加 ==================== + + def _add_segment_remove(self, move_type: MoveType, end_world: Point6D, **kwargs): + """添加运动段""" + if self._point_count >= self.max_points: + return + + start_world = self.current_pos.copy() + linear_distance = start_world.distance_to(end_world) + + if move_type in [MoveType.RAPID, MoveType.FEED]: + feed_for_calc = self.feedrate if move_type == MoveType.FEED else None + result = self.planner.calculate_time(linear_distance, feed_for_calc, + is_rapid=(move_type == MoveType.RAPID)) + elif move_type == MoveType.DWELL: + dwell_time = kwargs.get('dwell_time', 0) + result = {'duration': dwell_time, 'max_velocity': 0, 'acceleration_time': 0, + 'constant_time': 0, 'deceleration_time': 0, 'profile_type': 'dwell'} + else: + result = {'duration': 0.1, 'max_velocity': 0, 'acceleration_time': 0, + 'constant_time': 0, 'deceleration_time': 0, 'profile_type': 'other'} + + old_machine_x = self.machine_x + old_machine_y = self.machine_y + old_machine_z = self.machine_z + old_machine_a = self.machine_a + old_machine_b = self.machine_b + old_machine_c = self.machine_c + + self.current_pos = end_world.copy() + self._sync_machine_from_current() + + machine_start = Point6D(old_machine_x, old_machine_y, old_machine_z, + old_machine_a, old_machine_b, old_machine_c) + machine_end = Point6D(self.machine_x, self.machine_y, self.machine_z, + self.machine_a, self.machine_b, self.machine_c) + + segment = MoveSegment( + type=move_type, start=machine_start, end=machine_end, + line_number=self.current_line, + feedrate=self.feedrate if move_type == MoveType.FEED else 0, + comp_type=self.comp_type, comp_radius=self.comp_radius, d_word=self.d_word, + duration=result['duration'], max_velocity=result['max_velocity'], + acceleration_time=result['acceleration_time'], + constant_time=result['constant_time'], + deceleration_time=result['deceleration_time'], + profile_type=result['profile_type'], + is_rtcp=self.rtcp_enabled, + world_start=start_world.copy(), world_end=end_world.copy(), + tool_number=self.state.tool, + spindle_speed=self.state.spindle_speed, + spindle_state=self.state.spindle_state, + **{k: v for k, v in kwargs.items() + if k in ['center_x', 'center_y', 'center_z', 'radius', 'turn', 'dwell_time']} + ) + + self.data.segments.append(segment) + self.data.total_length += linear_distance + self.cumulative_time += result['duration'] + self._point_count += 1 + self._sync_parameters_from_state() + + # ==================== 运动方法 ==================== + + def straight_traverse(self, x: float, y: float, z: float, + a: float, b: float, c: float, u: float, v: float, w: float): + """G0 快速移动 - 使用工件坐标记录""" + if self._g53_active: + self._handle_g53_move(x, y, z, a, b, c, u, v, w, MoveType.RAPID) + return + + # 处理绝对/相对坐标 + if not self.is_absolute: + x = self.current_pos.x + (x if x is not None else 0) + y = self.current_pos.y + (y if y is not None else 0) + z = self.current_pos.z + (z if z is not None else 0) + a = self.current_pos.a + (a if a is not None else 0) + b = self.current_pos.b + (b if b is not None else 0) + c = self.current_pos.c + (c if c is not None else 0) + else: + x = x if x is not None else self.current_pos.x + y = y if y is not None else self.current_pos.y + z = z if z is not None else self.current_pos.z + a = a if a is not None else self.current_pos.a + b = b if b is not None else self.current_pos.b + c = c if c is not None else self.current_pos.c + + # 创建工件坐标目标点 + end = Point6D(x, y, z, a, b, c) + + # 使用统一的运动段添加方法 + self._add_world_segment(MoveType.RAPID, end) + + # 更新 GLCanonPure 需要的坐标 + self.lo = [x, y, z, a, b, c, u, v, w] + + def straight_feed(self, x: float, y: float, z: float, + a: float, b: float, c: float, u: float, v: float, w: float): + """G1 直线进给 - 使用工件坐标记录""" + if self._g53_active: + self._handle_g53_move(x, y, z, a, b, c, u, v, w, MoveType.FEED) + return + + # 处理绝对/相对坐标 + if not self.is_absolute: + x = self.current_pos.x + (x if x is not None else 0) + y = self.current_pos.y + (y if y is not None else 0) + z = self.current_pos.z + (z if z is not None else 0) + a = self.current_pos.a + (a if a is not None else 0) + b = self.current_pos.b + (b if b is not None else 0) + c = self.current_pos.c + (c if c is not None else 0) + else: + x = x if x is not None else self.current_pos.x + y = y if y is not None else self.current_pos.y + z = z if z is not None else self.current_pos.z + a = a if a is not None else self.current_pos.a + b = b if b is not None else self.current_pos.b + c = c if c is not None else self.current_pos.c + + # 创建工件坐标目标点 + end = Point6D(x, y, z, a, b, c) + + # 处理刀具补偿 + if self.cutter_comp_active and self.comp_radius > 0: + comp_x, comp_y, comp_z = self._compensate_straight(end.x, end.y, end.z) + end = Point6D(comp_x, comp_y, comp_z, end.a, end.b, end.c) + + # 使用统一的运动段添加方法 + self._add_world_segment(MoveType.FEED, end) + + # 更新 GLCanonPure 需要的坐标 + self.lo = [x, y, z, a, b, c, u, v, w] + + + def _handle_g53_move(self, x: float, y: float, z: float, + a: float, b: float, c: float, + u: float, v: float, w: float, move_type: MoveType): + """处理G53模式下的运动 + + G53使用机床坐标系，刀具长度补偿(TLO)在G53模式下已被取消 + （LinuxCNC标准行为：G53自动取消刀具长度补偿） + """ + old_mx, old_my, old_mz = self.machine_x, self.machine_y, self.machine_z + old_ma, old_mb, old_mc = self.machine_a, self.machine_b, self.machine_c + + self.machine_x = x if x is not None else self.machine_x + self.machine_y = y if y is not None else self.machine_y + self.machine_z = z if z is not None else self.machine_z + self.machine_a = a if a is not None else self.machine_a + self.machine_b = b if b is not None else self.machine_b + self.machine_c = c if c is not None else self.machine_c + + # G53模式下，临时清零刀具长度补偿 + saved_tool_offset_z = self.tool_offset.z + self.tool_offset.z = 0.0 + + self._update_current_from_machine() + + # 恢复刀具长度补偿 + self.tool_offset.z = saved_tool_offset_z + + end = self.current_pos.copy() + + machine_start = Point6D(old_mx, old_my, old_mz, old_ma, old_mb, old_mc) + machine_end = Point6D(self.machine_x, self.machine_y, self.machine_z, + self.machine_a, self.machine_b, self.machine_c) + + segment = MoveSegment( + type=move_type, start=machine_start, end=machine_end, + line_number=self.current_line, + feedrate=self.feedrate if move_type == MoveType.FEED else 0, + duration=0.1, profile_type='g53', + is_rtcp=False, world_start=self.current_pos.copy(), world_end=end.copy(), + tool_number=self.state.tool + ) + self.data.segments.append(segment) + self._g53_active = False + self._sync_parameters_from_state() + + + + def arc_feed_bak(self, x: float, y: float, z: float, + a: float, b: float, c: float, + i: float, j: float, k: float, turn: int, feed: float, + u: float = 0.0, v: float = 0.0, w: float = 0.0): + """ + G2/G3 圆弧进给 + + 参数说明: + x, y, z: 圆弧终点坐标 (工件坐标系) + a, b, c: 终点旋转轴角度 + i, j, k: 圆心相对于起点的偏移量 (INCREMENTAL IJK) + turn: 圈数 (负数=G2顺时针, 正数=G3逆时针) + feed: 进给率 (mm/min) + + 修复说明: + I, J, K 始终作为相对于起点的增量处理 + 圆心 = 起点 + 偏移量 + """ + start_x, start_y, start_z = self.current_pos.x, self.current_pos.y, self.current_pos.z + + # 处理绝对/相对坐标 + if not self.is_absolute: + x = self.current_pos.x + x + y = self.current_pos.y + y + z = self.current_pos.z + z + a = self.current_pos.a + (a if a is not None else 0) + b = self.current_pos.b + (b if b is not None else 0) + c = self.current_pos.c + (c if c is not None else 0) + else: + x = x if x is not None else self.current_pos.x + y = y if y is not None else self.current_pos.y + z = z if z is not None else self.current_pos.z + a = a if a is not None else self.current_pos.a + b = b if b is not None else self.current_pos.b + c = c if c is not None else self.current_pos.c + + # ===== 关键修复：圆心计算 ===== + # I, J, K 始终是相对于起点的增量偏移量 + # 圆心坐标 = 起点坐标 + 偏移量 + i_val = i if i is not None else 0.0 + j_val = j if j is not None else 0.0 + k_val = k if k is not None else 0.0 + + cx = start_x + i_val + cy = start_y + j_val + cz = start_z + k_val + + # 计算半径 + radius = math.hypot(i_val, j_val) + + if self.debug: + print(f"[ARC] 起点=({start_x:.3f},{start_y:.3f}) " + f"圆心=({cx:.3f},{cy:.3f}) " + f"I={i_val:.3f} J={j_val:.3f} R={radius:.3f}") + + # 验证终点在圆上 + # 验证终点在圆上（对照 LinuxCNC arc_data_ijk 中的容差检查） + end_dist = math.hypot(x - cx, y - cy) + # 使用与 LinuxCNC 一致的容差（spiral_abs_tolerance） + spiral_tolerance = 0.01 # mm，对于公制单位 + if abs(end_dist - radius) > spiral_tolerance * 100.0: + # 相对误差检查 + rel_err = abs(end_dist - radius) / max(radius, end_dist) if max(radius, end_dist) > 0 else 0 + if rel_err > 0.001 and abs(end_dist - radius) > spiral_tolerance: + if self.debug: + print(f"[ARC] ⚠ 终点({x:.3f},{y:.3f})不在圆上: " + f"距圆心={end_dist:.6f}, R={radius:.6f}, " + f"绝对误差={abs(end_dist - radius):.6f}mm, " + f"相对误差={rel_err:.6f}") + # 将终点调整到圆上（容错处理） + if end_dist > TINY: + angle = math.atan2(y - cy, x - cx) + x = cx + radius * math.cos(angle) + y = cy + radius * math.sin(angle) + if self.debug: + print(f"[ARC] 调整终点到圆上: ({x:.3f},{y:.3f})") + else: + if self.debug: + print(f"[ARC] 终点误差在容差范围内，不调整") + + # 创建终点 + end = Point6D(x, y, z, a, b, c) + + # 保存原始进给率 + original_feedrate = self.feedrate + if feed > 0: + self.feedrate = feed + + # 计算弧长 + arc_length = abs(turn) * 2.0 * math.pi * radius + result = self.planner.calculate_time(arc_length, self.feedrate, is_rapid=False) + + # 机器坐标 + machine_start = Point6D(self.machine_x, self.machine_y, self.machine_z, + self.machine_a, self.machine_b, self.machine_c) + + # 计算终点关节坐标 + if self.rtcp_enabled and self.kinematics is not None: + wx = x + self.work_offset_x + self.g5x_offset_x + self.g92_offset_x + self.g52_offset_x + wy = y + self.work_offset_y + self.g5x_offset_y + self.g92_offset_y + self.g52_offset_y + wz = z + self.work_offset_z + self.g5x_offset_z + self.g92_offset_z + self.g52_offset_z + wa = a + self.work_offset_a + self.g5x_offset_a + self.g92_offset_a + self.g52_offset_a + wb = b + self.work_offset_b + self.g5x_offset_b + self.g92_offset_b + self.g52_offset_b + wc = c + self.work_offset_c + self.g5x_offset_c + self.g92_offset_c + self.g52_offset_c + + world_tuple = (wx, wy, wz, wa, wb, wc) + joints = self.kinematics.world_to_joints(world_tuple) + # 使用统一的关节映射函数 + end_joints = joints_to_point6d(joints, self.kinematics_type) + else: + end_joints = Point6D(x, y, z, a, b, c) + + machine_end = Point6D( + self.machine_x + (end_joints.x - self.current_pos.x), + self.machine_y + (end_joints.y - self.current_pos.y), + self.machine_z + (end_joints.z - self.current_pos.z), + end_joints.a, end_joints.b, end_joints.c + ) + + # 创建圆弧运动段 + arc_segment = MoveSegment( + type=MoveType.ARC_CW if turn < 0 else MoveType.ARC_CCW, + start=machine_start, end=machine_end, + line_number=self.current_line, feedrate=self.feedrate, + center_x=cx, center_y=cy, center_z=cz, + radius=radius, turn=turn, + comp_type=self.comp_type, comp_radius=self.comp_radius, d_word=self.d_word, + duration=result['duration'], max_velocity=result['max_velocity'], + acceleration_time=result['acceleration_time'], + constant_time=result['constant_time'], + deceleration_time=result['deceleration_time'], + profile_type=result['profile_type'], + is_rtcp=self.rtcp_enabled, + world_start=self.current_pos.copy(), world_end=end.copy(), + tool_number=self.state.tool, + spindle_speed=self.state.spindle_speed, + spindle_state=self.state.spindle_state + ) + self.data.segments.append(arc_segment) + self.data.total_length += arc_length + self.cumulative_time += result['duration'] + + # 更新当前位置 + self.current_pos = end_joints.copy() + self.machine_x = machine_end.x + self.machine_y = machine_end.y + self.machine_z = machine_end.z + self.machine_a = machine_end.a + self.machine_b = machine_end.b + self.machine_c = machine_end.c + + # 恢复进给率 + self.feedrate = original_feedrate + self._sync_parameters_from_state() + + + + def arc_feed(self, x: float, y: float, z: float, + a: float, b: float, c: float, + i: float, j: float, k: float, turn: int, feed: float, + u: float = 0.0, v: float = 0.0, w: float = 0.0): + """ + G2/G3 圆弧进给 - 基于参数表进行坐标管理 + + 参照 LinuxCNC interp_convert.cc convert_arc2() 的逻辑： + 1. 在工件坐标系中计算圆心 + 2. 验证圆弧几何（参照 arc_data_ijk 的容差检查） + 3. 创建 MoveSegment（使用工件坐标） + 4. 更新当前位置（工件坐标） + + 参数: + x, y, z: 圆弧终点（工件坐标） + a, b, c: 终点旋转轴角度（工件坐标） + i, j, k: 圆心相对起点的偏移（增量，工件坐标） + turn: 圈数（负数=G2顺时针，正数=G3逆时针） + feed: 进给率 (mm/min) + """ + # ===== 第1步：获取起点（工件坐标）===== + start_x = self.current_pos.x + start_y = self.current_pos.y + start_z = self.current_pos.z + start_a = self.current_pos.a + start_b = self.current_pos.b + start_c = self.current_pos.c + + # ===== 第2步：处理绝对/相对坐标 ===== + if not self.is_absolute: + x = start_x + (x if x is not None else 0) + y = start_y + (y if y is not None else 0) + z = start_z + (z if z is not None else 0) + a = start_a + (a if a is not None else 0) + b = start_b + (b if b is not None else 0) + c = start_c + (c if c is not None else 0) + else: + x = x if x is not None else start_x + y = y if y is not None else start_y + z = z if z is not None else start_z + a = a if a is not None else start_a + b = b if b is not None else start_b + c = c if c is not None else start_c + + # ===== 第3步：计算圆心（工件坐标系）===== + # 参照 LinuxCNC arc_data_ijk：IJK 始终是相对于起点的增量 + i_val = i if i is not None else 0.0 + j_val = j if j is not None else 0.0 + k_val = k if k is not None else 0.0 + + # 圆心 = 起点 + IJK 偏移 + cx = start_x + i_val + cy = start_y + j_val + cz = start_z + k_val + + # 计算半径 + radius = math.hypot(i_val, j_val) + + if self.debug: + print(f"[ARC] 起点=({start_x:.3f},{start_y:.3f}) " + f"圆心=({cx:.3f},{cy:.3f}) " + f"I={i_val:.3f} J={j_val:.3f} R={radius:.3f}") + + # ===== 第4步：验证圆弧几何（参照 LinuxCNC 容差）===== + units = getattr(self.state, 'units', 21) + is_valid, msg, error = self.var_manager.validate_arc_geometry( + start_x, start_y, x, y, cx, cy, radius, units + ) + + if not is_valid and '零半径' in msg: + if self.debug: + print(f"[ARC] ⚠ {msg}") + return + + if not is_valid: + if self.debug: + print(f"[ARC] ⚠ {msg}") + # 容错处理：将终点调整到圆上 + end_radius = math.hypot(x - cx, y - cy) + if end_radius > TINY: + angle = math.atan2(y - cy, x - cx) + x = cx + radius * math.cos(angle) + y = cy + radius * math.sin(angle) + if self.debug: + print(f"[ARC] 调整终点到圆上: ({x:.6f}, {y:.6f})") + + # ===== 第5步：计算弧长和速度曲线 ===== + arc_length = abs(turn) * 2.0 * math.pi * radius + + # 保存当前进给率 + original_feedrate = self.feedrate + if feed > 0: + self.feedrate = feed + + result = self.planner.calculate_time(arc_length, self.feedrate, is_rapid=False) + + # ===== 第6步：创建工件坐标和绝对坐标点 ===== + world_start = Point6D(start_x, start_y, start_z, start_a, start_b, start_c) + world_end = Point6D(x, y, z, a, b, c) + + # 通过参数表计算绝对坐标 + abs_start = self.var_manager.program_to_absolute( + start_x, start_y, start_z, start_a, start_b, start_c + ) + abs_end = self.var_manager.program_to_absolute(x, y, z, a, b, c) + + abs_start_pt = Point6D( + abs_start[0], abs_start[1], abs_start[2], + abs_start[3], abs_start[4], abs_start[5] + ) + abs_end_pt = Point6D( + abs_end[0], abs_end[1], abs_end[2], + abs_end[3], abs_end[4], abs_end[5] + ) + + # ===== 第7步：创建 MoveSegment ===== + arc_segment = MoveSegment( + type=MoveType.ARC_CW if turn < 0 else MoveType.ARC_CCW, + start=world_start.copy(), + end=world_end.copy(), + line_number=self.current_line, + feedrate=self.feedrate, + center_x=cx, + center_y=cy, + center_z=cz, + radius=radius, + turn=turn, + comp_type=self.comp_type, + comp_radius=self.comp_radius, + d_word=self.d_word, + duration=result['duration'], + max_velocity=result['max_velocity'], + acceleration_time=result['acceleration_time'], + constant_time=result['constant_time'], + deceleration_time=result['deceleration_time'], + profile_type=result['profile_type'], + is_rtcp=self.rtcp_enabled, + world_start=world_start.copy(), + world_end=world_end.copy(), + machine_start=abs_start_pt, + machine_end=abs_end_pt, + tool_number=self.state.tool, + spindle_speed=self.state.spindle_speed, + spindle_state=self.state.spindle_state, + coord_system='WORLD', + active_csys=self.var_manager.active_csys, + g92_active=self.var_manager._g92_active, + gcode=f"G{2 if turn < 0 else 3}" + ) + + # ===== 第8步：添加到路径数据 ===== + self.data.segments.append(arc_segment) + self.data.total_length += arc_length + self.cumulative_time += result['duration'] + self._point_count += 1 + + # 更新 GLCanonPure 需要的坐标 + self.lo = [x, y, z, a, b, c, u, v, w] + + # ===== 第9步：更新工件坐标位置 ===== + self.current_pos = world_end.copy() + + # ===== 第10步：同步到参数表 ===== + self._sync_position_to_params() + + # ===== 第11步：同步机器坐标 ===== + self._sync_machine_from_current() + + # ===== 第12步：恢复进给率 ===== + self.feedrate = original_feedrate + + # ===== 第13步：同步所有参数 ===== + self._sync_parameters_from_state() + + # ===== 第14步：标记运动段类型 ===== + self._last_segment_was_arc = True + + def dwell(self, seconds: float): + """G4 暂停 - 使用工件坐标记录""" + self._add_world_segment( + MoveType.DWELL, + self.current_pos.copy(), + dwell_time=seconds + ) + + # ==================== 刀具补偿辅助 ==================== + + def _compensate_straight(self, end_x: float, end_y: float, end_z: float) -> Tuple[float, float, float]: + """直线刀具半径补偿""" + if not self.cutter_comp_active or self.comp_radius <= 0: + return (end_x, end_y, end_z) + + if self.cutter_comp_firstmove: + dx = end_x - self.program_x + dy = end_y - self.program_y + length = math.hypot(dx, dy) + + if length > CART_FUZZ: + angle = math.atan2(dy, dx) + factor = 1.0 if self.comp_type == CompType.LEFT else -1.0 + alpha = angle + factor * (math.pi / 2) + comp_end_x = end_x + self.comp_radius * math.cos(alpha) + comp_end_y = end_y + self.comp_radius * math.sin(alpha) + else: + comp_end_x, comp_end_y = end_x, end_y + + self.cutter_comp_firstmove = False + self.program_x = end_x + self.program_y = end_y + self.program_z = end_z + return (comp_end_x, comp_end_y, end_z) + else: + dx = end_x - self.program_x + dy = end_y - self.program_y + length = math.hypot(dx, dy) + + if length > CART_FUZZ: + ux, uy = dx / length, dy / length + factor = 1.0 if self.comp_type == CompType.LEFT else -1.0 + perp_x, perp_y = -uy * factor, ux * factor + comp_end_x = end_x + self.comp_radius * perp_x + comp_end_y = end_y + self.comp_radius * perp_y + else: + comp_end_x, comp_end_y = end_x, end_y + + self.program_x = end_x + self.program_y = end_y + self.program_z = end_z + return (comp_end_x, comp_end_y, end_z) + + def _world_to_joints(self, world_pos: Point6D) -> Point6D: + """工件坐标转关节坐标""" + if not self.rtcp_enabled or self.kinematics is None: + return world_pos + + wx = world_pos.x + self.work_offset_x + self.g5x_offset_x + self.g92_offset_x + self.g52_offset_x + wy = world_pos.y + self.work_offset_y + self.g5x_offset_y + self.g92_offset_y + self.g52_offset_y + wz = world_pos.z + self.work_offset_z + self.g5x_offset_z + self.g92_offset_z + self.g52_offset_z + wa = world_pos.a + self.work_offset_a + self.g5x_offset_a + self.g92_offset_a + self.g52_offset_a + wb = world_pos.b + self.work_offset_b + self.g5x_offset_b + self.g92_offset_b + self.g52_offset_b + wc = world_pos.c + self.work_offset_c + self.g5x_offset_c + self.g92_offset_c + self.g52_offset_c + + world_tuple = (wx, wy, wz, wa, wb, wc) + joints = self.kinematics.world_to_joints(world_tuple) + + return joints_to_point6d(joints, self.kinematics_type) + + def _joints_to_world(self, joints_pos: Point6D) -> Point6D: + """关节坐标转工件坐标""" + if not self.rtcp_enabled or self.kinematics is None: + return joints_pos + + # 构建符合运动学类型的关节数组 + config = KINEMATICS_JOINT_CONFIG.get(self.kinematics_type, {}) + num_joints = config.get('num_joints', 6) + joints = [0.0] * num_joints + + x_idx = config.get('x_idx', 0) + y_idx = config.get('y_idx', 1) + z_idx = config.get('z_idx', 2) + a_idx = config.get('a_idx', 3) + b_idx = config.get('b_idx', 4) + c_idx = config.get('c_idx', 5) + + if x_idx >= 0: joints[x_idx] = joints_pos.x + if y_idx >= 0: joints[y_idx] = joints_pos.y + if z_idx >= 0: joints[z_idx] = joints_pos.z + if a_idx >= 0: joints[a_idx] = joints_pos.a + if b_idx >= 0: joints[b_idx] = joints_pos.b + if c_idx >= 0: joints[c_idx] = joints_pos.c + + world = self.kinematics.joints_to_world(joints) + + wx = world[0] - self.work_offset_x - self.g5x_offset_x - self.g92_offset_x - self.g52_offset_x + wy = world[1] - self.work_offset_y - self.g5x_offset_y - self.g92_offset_y - self.g52_offset_y + wz = world[2] - self.work_offset_z - self.g5x_offset_z - self.g92_offset_z - self.g52_offset_z + wa = world[3] - self.work_offset_a - self.g5x_offset_a - self.g92_offset_a - self.g52_offset_a + wb = world[4] - self.work_offset_b - self.g5x_offset_b - self.g92_offset_b - self.g52_offset_b + wc = world[5] - self.work_offset_c - self.g5x_offset_c - self.g92_offset_c - self.g52_offset_c + + return Point6D(wx, wy, wz, wa, wb, wc) + + # ==================== 其他方法 ==================== + + def set_feed_rate(self, feed: float): + self.feedrate = feed + self.state.feedrate = feed + self._sync_parameters_from_state() + + def set_spindle_speed(self, speed: float): + self.state.spindle_speed = speed + self._sync_parameters_from_state() + + def change_tool(self, tool: int): + if tool not in self.tool_table: + self.tool_table[tool] = ToolData(tool_number=tool) + self.current_tool = self.tool_table[tool] + self.state.tool = tool + self.current_pocket = tool + + offsets = {'x': 0.0, 'y': 0.0, 'z': self.tool_lengths.get(tool, 0.0), + 'a': 0.0, 'b': 0.0, 'c': 0.0, + 'u': 0.0, 'v': 0.0, 'w': 0.0} + self.var_manager.update_tool_params(tool, offsets, + self.current_tool.diameter) + self._sync_parameters_from_state() + self.data.tool_changes.append({'tool': tool, 'line': self.current_line, + 'position': self.current_pos.to_list()}) + + def select_tool(self, tool: int): + self.selected_tool = tool + self.selected_pocket = tool + + def set_absolute_mode(self): + self.is_absolute = True + self.state.distance_mode = 90 + self._sync_parameters_from_state() + + def set_relative_mode(self): + self.is_absolute = False + self.state.distance_mode = 91 + self._sync_parameters_from_state() + + def set_plane(self, plane: int): + self.current_plane = plane + self.plane = plane + self.state.plane = plane + self._sync_parameters_from_state() + + def spindle_control(self, mode: int): + self.state.spindle_mode = mode + if mode == 1: + self.state.spindle_state = "CW" + elif mode == 2: + self.state.spindle_state = "CCW" + elif mode == 0: + self.state.spindle_state = "OFF" + self._sync_parameters_from_state() + + def set_flood(self, enable: bool): + self.state.coolant_flood = enable + self._sync_parameters_from_state() + + def set_mist(self, enable: bool): + self.state.coolant_mist = enable + self._sync_parameters_from_state() + + def program_stop(self): + pass + + def optional_stop(self): + pass + + def program_end(self): + """程序结束 - 使用工件坐标记录""" + self._add_world_segment( + MoveType.PROGRAM_END, + self.current_pos.copy(), + dwell_time=0.1 + ) + + def set_g28(self, x: float, y: float, z: float, a: float = 0.0, b: float = 0.0, c: float = 0.0): + self.g28_ref = {'X': x, 'Y': y, 'Z': z, 'A': a, 'B': b, 'C': c} + self.var_manager.set_g28(x, y, z, a, b, c) + + def set_g30(self, x: float, y: float, z: float, a: float = 0.0, b: float = 0.0, c: float = 0.0): + self.g30_ref = {'X': x, 'Y': y, 'Z': z, 'A': a, 'B': b, 'C': c} + self.var_manager.set_g30(x, y, z, a, b, c) + + def get_g28(self) -> Tuple[float, float, float]: + return (self.g28_ref.get('X', 0.0), self.g28_ref.get('Y', 0.0), self.g28_ref.get('Z', 0.0)) + + def get_g30(self) -> Tuple[float, float, float]: + return (self.g30_ref.get('X', 0.0), self.g30_ref.get('Y', 0.0), self.g30_ref.get('Z', 0.0)) + + def save_state(self, restore_on_return: bool = False) -> Dict: + return { + 'motion_mode': self.motion_mode, + 'plane': self.current_plane, + 'distance_mode': 90 if self.is_absolute else 91, + 'current_pos': self.current_pos.copy(), + 'feedrate': self.feedrate, + 'tool': self.state.tool, + 'restore_on_return': restore_on_return + } + + def restore_state(self, saved_state: Dict = None): + if saved_state is None: + return + + if 'g_codes' in saved_state: + g = saved_state['g_codes'] + self.current_plane = g.get('plane', self.current_plane) + self.is_absolute = (g.get('distance_mode', 90) == 90) + + if 'positions' in saved_state: + p = saved_state['positions'] + self.current_pos = Point6D( + p.get('x', self.current_pos.x), p.get('y', self.current_pos.y), + p.get('z', self.current_pos.z), p.get('a', self.current_pos.a), + p.get('b', self.current_pos.b), p.get('c', self.current_pos.c) + ) + self.machine_x = p.get('machine_x', self.machine_x) + self.machine_y = p.get('machine_y', self.machine_y) + self.machine_z = p.get('machine_z', self.machine_z) + + self.feedrate = saved_state.get('feedrate', self.feedrate) + self.state.tool = saved_state.get('tool', self.state.tool) + self._sync_parameters_from_state() + + def next_line(self, st): + if hasattr(st, 'sequence_number'): + self.current_line = st.sequence_number + self.lineno = st.sequence_number + if hasattr(st, 'plane'): + self.current_plane = st.plane + self.plane = st.plane + self.state.plane = st.plane + self._sync_parameters_from_state() + + def get_total_time(self) -> float: + return self.cumulative_time + + +# ==================== CNCKernel 扩展方法 ==================== + +class CNCKernelExtensions: + """CNCKernel 类的扩展方法集合""" + + @staticmethod + def add_power_off(kernel_instance): + def power_off(self) -> Dict: + if not self.ensure_operational(): + return {"success": False, "message": f"机床未就绪"} + if self.state_machine.request_power_off(): + self.is_running = False + self.is_paused = False + self.status.state = MachineState.OFF.value + self.status.spindle_state = "OFF" + self.status.spindle_speed = 0 + self.status.coolant_flood = False + self.status.coolant_mist = False + self._notify_update() + return {"success": True, "message": "断电成功"} + return {"success": False, "message": "无法断电"} + kernel_instance.power_off = power_off.__get__(kernel_instance, type(kernel_instance)) + + @staticmethod + def add_home_all(kernel_instance): + def home_all(self) -> Dict: + if not self.ensure_operational(): + return {"success": False, "message": "机床未就绪"} + self.state_machine.home_all() + self.status.state = MachineState.HOMING.value + self._notify_update() + + def home_motion(): + for axis, current in [('X', self.status.position_x), ('Y', self.status.position_y), + ('Z', self.status.position_z), ('A', self.status.position_a), + ('B', self.status.position_b), ('C', self.status.position_c)]: + if abs(current) < 0.001: + continue + step = current / 20 + for _ in range(20): + if axis == 'X': self.status.position_x -= step + elif axis == 'Y': self.status.position_y -= step + elif axis == 'Z': self.status.position_z -= step + elif axis == 'A': self.status.position_a -= step + elif axis == 'B': self.status.position_b -= step + elif axis == 'C': self.status.position_c -= step + self._update_world_position() + self._notify_update() + time.sleep(0.05) + if axis == 'X': self.status.position_x = 0.0 + elif axis == 'Y': self.status.position_y = 0.0 + elif axis == 'Z': self.status.position_z = 0.0 + elif axis == 'A': self.status.position_a = 0.0 + elif axis == 'B': self.status.position_b = 0.0 + elif axis == 'C': self.status.position_c = 0.0 + self.state_machine.transition_to(MachineStateMachine.STATE_IDLE) + self.status.state = MachineState.IDLE.value + self._notify_update() + + threading.Thread(target=home_motion, daemon=True).start() + return {"success": True, "message": "回零中..."} + kernel_instance.home_all = home_all.__get__(kernel_instance, type(kernel_instance)) + + @staticmethod + def add_jog(kernel_instance): + """添加点动功能 - 支持RTCP刀尖跟随 + + 坐标变换链（与_set_workpiece_position保持一致）： + 工件坐标 → +G92偏移 → +XY旋转 → +G5x偏移 → +TLO → +G52偏移 → 逆运动学 → 关节坐标 + + 参数: + axis: 轴名称 ('X','Y','Z','A','B','C') + direction: 方向 (1=正向, -1=负向) + distance: 移动距离 (mm或度) + + 返回: + dict: {'success': bool, 'message': str, 'position': dict} + """ + def jog(self, axis: str, direction: int, distance: float) -> Dict: + """执行点动操作""" + if not self.ensure_operational(): + return {"success": False, "message": "机床未就绪"} + + axis_upper = axis.upper() + + # 验证轴名称 + valid_axes = {'X', 'Y', 'Z', 'A', 'B', 'C'} + if axis_upper not in valid_axes: + return {"success": False, "message": f"无效轴: {axis}, 有效轴: {valid_axes}"} + + # 计算移动量 + step = direction * distance + + # 记录移动前坐标（用于调试） + old_pos = { + 'x': self.status.position_x, + 'y': self.status.position_y, + 'z': self.status.position_z, + 'a': self.status.position_a, + 'b': self.status.position_b, + 'c': self.status.position_c, + } + old_machine = { + 'x': self.status.machine_x, + 'y': self.status.machine_y, + 'z': self.status.machine_z, + 'a': self.status.machine_a, + 'b': self.status.machine_b, + 'c': self.status.machine_c, + } + + # 计算目标工件坐标 + target_x = self.status.position_x + target_y = self.status.position_y + target_z = self.status.position_z + target_a = self.status.position_a + target_b = self.status.position_b + target_c = self.status.position_c + + if axis_upper == 'X': + target_x += step + elif axis_upper == 'Y': + target_y += step + elif axis_upper == 'Z': + target_z += step + elif axis_upper == 'A': + target_a += step + elif axis_upper == 'B': + target_b += step + elif axis_upper == 'C': + target_c += step + + # ★★★ 核心修复：调用_set_workpiece_position处理完整坐标变换链 ★★★ + # 该方法内部会： + # 1. 更新工件坐标 + # 2. 计算绝对坐标（工件+G5x+G92+TLO） + # 3. RTCP启用时执行逆运动学得到关节坐标 + # 4. RTCP禁用时直接使用绝对坐标作为关节坐标 + # 5. 更新世界坐标 + self._set_workpiece_position( + target_x, target_y, target_z, + target_a, target_b, target_c + ) + + # 更新世界坐标（正运动学验证） + self._update_world_position() + + # 通知更新（包括MQTT回调，发送的是machine坐标） + self._notify_update() + + # 调试输出 + if self.debug: + rtcp_status = "启用" if self.rtcp_enabled else "禁用" + print(f"\n[JOG] {axis_upper}{'+' if direction > 0 else '-'}{distance} " + f"(RTCP: {rtcp_status})") + print(f" 工件坐标: ({old_pos['x']:.3f}, {old_pos['y']:.3f}, " + f"{old_pos['z']:.3f}) → ({target_x:.3f}, {target_y:.3f}, " + f"{target_z:.3f})") + print(f" 机器坐标: ({old_machine['x']:.3f}, {old_machine['y']:.3f}, " + f"{old_machine['z']:.3f}) → ({self.status.machine_x:.3f}, " + f"{self.status.machine_y:.3f}, {self.status.machine_z:.3f})") + if axis_upper in ('A', 'B', 'C'): + print(f" 旋转轴: {axis_upper}={self.status.__dict__.get(f'position_{axis_upper.lower()}', 0):.2f}°") + if self.rtcp_enabled: + # 计算RTCP补偿量 + dx = self.status.machine_x - self.status.position_x + dy = self.status.machine_y - self.status.position_y + dz = self.status.machine_z - self.status.position_z + print(f" RTCP补偿: ΔX={dx:.3f}, ΔY={dy:.3f}, ΔZ={dz:.3f}") + + return { + "success": True, + "message": f"点动 {axis_upper}{'+' if direction > 0 else '-'}{distance}", + "data": { + "axis": axis_upper, + "direction": direction, + "distance": distance, + "rtcp_enabled": self.rtcp_enabled, + "position": { + "workpiece": { + "x": self.status.position_x, + "y": self.status.position_y, + "z": self.status.position_z, + "a": self.status.position_a, + "b": self.status.position_b, + "c": self.status.position_c, + }, + "machine": { + "x": self.status.machine_x, + "y": self.status.machine_y, + "z": self.status.machine_z, + "a": self.status.machine_a, + "b": self.status.machine_b, + "c": self.status.machine_c, + }, + } + } + } + + # 绑定方法到实例 + kernel_instance.jog = jog.__get__(kernel_instance, type(kernel_instance)) + + + @staticmethod + def add_jog_machine(kernel_instance): + """添加机器坐标系点动功能（G53模式） + + 在机器坐标系下直接点动关节，适用于： + - 手动调整关节位置 + - 调试运动学参数 + - 紧急情况直接控制关节 + + 注意：机器坐标点动不走RTCP逆解，直接修改关节坐标。 + """ + def jog_machine(self, axis: str, direction: int, distance: float) -> Dict: + """在机器坐标系下执行点动""" + if not self.ensure_operational(): + return {"success": False, "message": "机床未就绪"} + + axis_upper = axis.upper() + valid_axes = {'X', 'Y', 'Z', 'A', 'B', 'C'} + if axis_upper not in valid_axes: + return {"success": False, "message": f"无效轴: {axis}"} + + step = direction * distance + + # 直接修改机器坐标 + if axis_upper == 'X': + self.status.machine_x += step + elif axis_upper == 'Y': + self.status.machine_y += step + elif axis_upper == 'Z': + self.status.machine_z += step + elif axis_upper == 'A': + self.status.machine_a += step + elif axis_upper == 'B': + self.status.machine_b += step + elif axis_upper == 'C': + self.status.machine_c += step + + # 反向计算工件坐标（通过_update_current_from_machine） # ★ 调用新增的方法反向计算工件坐标 + self._update_current_from_machine() + + # 更新世界坐标 + self._update_world_position() + + # 通知更新 + self._notify_update() + + return { + "success": True, + "message": f"机器点动 {axis_upper}{'+' if direction > 0 else '-'}{distance}", + "data": { + "axis": axis_upper, + "direction": direction, + "distance": distance, + "rtcp_enabled": self.rtcp_enabled, + "position": { + "machine": { + "x": self.status.machine_x, + "y": self.status.machine_y, + "z": self.status.machine_z, + }, + "workpiece": { + "x": self.status.position_x, + "y": self.status.position_y, + "z": self.status.position_z, + } + } + } + } + + kernel_instance.jog_machine = jog_machine.__get__(kernel_instance, type(kernel_instance)) + + + @staticmethod + def add_jog_incremental(kernel_instance): + """添加增量式点动（明确使用增量模式） + + 与jog功能相同，但明确表示增量模式（G91）。 + 在增量模式下，每次移动都是相对于当前位置。 + """ + def jog_incremental(self, axis: str, direction: int, distance: float) -> Dict: + """增量式点动""" + # 直接委托给jog方法 + return self.jog(axis, direction, distance) + + kernel_instance.jog_incremental = jog_incremental.__get__(kernel_instance, type(kernel_instance)) + + + + + + @staticmethod + def add_spindle_control(kernel_instance): + def spindle_start(self, direction: str = "CW", speed: float = 1000.0) -> Dict: + self.status.spindle_speed = speed + self.status.spindle_state = direction.upper() + self.virtual_hal.set('spindle.0.speed', speed) + self.virtual_hal.set('spindle.0.direction', 1 if direction.upper() == "CW" else -1) + self.virtual_hal.set('spindle.0.enable', 1) + self._notify_update() + return {"success": True, "message": f"主轴 {direction} {speed} RPM"} + + def spindle_stop(self) -> Dict: + self.status.spindle_speed = 0 + self.status.spindle_state = "OFF" + self.virtual_hal.set('spindle.0.speed', 0) + self.virtual_hal.set('spindle.0.enable', 0) + self._notify_update() + return {"success": True, "message": "主轴停止"} + + kernel_instance.spindle_start = spindle_start.__get__(kernel_instance, type(kernel_instance)) + kernel_instance.spindle_stop = spindle_stop.__get__(kernel_instance, type(kernel_instance)) + + @staticmethod + def add_coolant_control(kernel_instance): + def coolant_on(self, coolant_type: str = "flood") -> Dict: + if coolant_type == "flood": self.status.coolant_flood = True + elif coolant_type == "mist": self.status.coolant_mist = True + elif coolant_type == "both": self.status.coolant_flood = self.status.coolant_mist = True + self._notify_update() + return {"success": True, "message": f"冷却液开启 ({coolant_type})"} + + def coolant_off(self) -> Dict: + self.status.coolant_flood = False + self.status.coolant_mist = False + self._notify_update() + return {"success": True, "message": "冷却液关闭"} + + kernel_instance.coolant_on = coolant_on.__get__(kernel_instance, type(kernel_instance)) + kernel_instance.coolant_off = coolant_off.__get__(kernel_instance, type(kernel_instance)) + + @staticmethod + def add_override_controls(kernel_instance): + def set_feed_override(self, value: int) -> Dict: + self.status.feed_override = max(0, min(200, value)) + self.planner.set_feed_override(self.status.feed_override / 100.0) + self._notify_update() + return {"success": True, "message": f"进给倍率 {self.status.feed_override}%"} + + def set_rapid_override(self, value: int) -> Dict: + self.status.rapid_override = max(0, min(100, value)) + self.planner.set_rapid_override(self.status.rapid_override / 100.0) + self._notify_update() + return {"success": True, "message": f"快速倍率 {self.status.rapid_override}%"} + + def set_spindle_override(self, value: int) -> Dict: + self.status.spindle_override = max(50, min(120, value)) + self._notify_update() + return {"success": True, "message": f"主轴倍率 {self.status.spindle_override}%"} + + kernel_instance.set_feed_override = set_feed_override.__get__(kernel_instance, type(kernel_instance)) + kernel_instance.set_rapid_override = set_rapid_override.__get__(kernel_instance, type(kernel_instance)) + kernel_instance.set_spindle_override = set_spindle_override.__get__(kernel_instance, type(kernel_instance)) + + @staticmethod + def add_get_full_config(kernel_instance): + def get_full_config(self) -> Dict: + return { + 'machine_id': self.name, + 'machine_name': self.name, + 'state': self.state_machine.get_state(), + 'status': { + 'position': {'x': self.status.position_x, 'y': self.status.position_y, + 'z': self.status.position_z, 'a': self.status.position_a, + 'b': self.status.position_b, 'c': self.status.position_c}, + 'world_position': {'x': self.status.world_x, 'y': self.status.world_y, 'z': self.status.world_z}, + 'spindle': {'speed': self.status.spindle_speed, 'state': self.status.spindle_state}, + 'coolant': {'flood': self.status.coolant_flood, 'mist': self.status.coolant_mist}, + 'tool': {'current': self.status.current_tool, 'length': self.status.tool_length}, + 'feedrate': self.status.feedrate, + 'overrides': {'feed': self.status.feed_override, 'rapid': self.status.rapid_override}, + 'program': {'name': self.status.program_name, 'progress': self.status.progress_percent}, + 'rtcp_enabled': self.status.rtcp_enabled, + 'kinematics_type': self.status.kinematics_type + }, + 'tool_table': self.tool_table, + 'work_offsets': self.work_offsets + } + kernel_instance.get_full_config = get_full_config.__get__(kernel_instance, type(kernel_instance)) + + @staticmethod + def add_change_tool(kernel_instance): + def change_tool(self, tool_number: int) -> Dict: + if tool_number in self.tool_table: + self.status.current_tool = tool_number + self.selected_tool = tool_number + self.status.tool_length = self.tool_table[tool_number]["length"] + self.status.tool_diameter = self.tool_table[tool_number]["diameter"] + self._notify_update() + return {"success": True, "message": f"换刀 T{tool_number}"} + return {"success": False, "message": f"刀具 T{tool_number} 不存在"} + kernel_instance.change_tool = change_tool.__get__(kernel_instance, type(kernel_instance)) + + @staticmethod + def extend_all(kernel_instance): + """一次性扩展所有方法""" + CNCKernelExtensions.add_power_off(kernel_instance) + CNCKernelExtensions.add_home_all(kernel_instance) + CNCKernelExtensions.add_jog(kernel_instance) # ★ 已修复 + CNCKernelExtensions.add_jog_machine(kernel_instance) # ★ 新增 + CNCKernelExtensions.add_jog_incremental(kernel_instance) # ★ 新增 + CNCKernelExtensions.add_spindle_control(kernel_instance) + CNCKernelExtensions.add_coolant_control(kernel_instance) + CNCKernelExtensions.add_override_controls(kernel_instance) + CNCKernelExtensions.add_get_full_config(kernel_instance) + CNCKernelExtensions.add_change_tool(kernel_instance) + + +# ==================== MachineManager 类 ==================== + +class MachineManager: + """机床管理器 - 简化版""" + + def __init__(self, debug: bool = False): + self.debug = debug + self._machines: Dict[str, CNCKernel] = {} + self._machine_configs: Dict[str, Dict] = {} + self._machine_programs: Dict[str, Dict] = {} + + + + def create_machine(self, machine_id: str, config: Optional[Dict] = None) -> CNCKernel: + if machine_id in self._machines: + return self._machines[machine_id] + + machine_name = config.get('machine_name', machine_id) if config else machine_id + + # 使用 CNCKernelWithJSON 替代 CNCKernel + kernel = CNCKernelWithJSON(debug=self.debug, name=machine_name) + + if config: + self._machine_configs[machine_id] = config + + kin_config = config.get('kinematics', {}) + kin_type_str = kin_config.get('type', 'TRT_BC').upper() + type_map = { + 'TRT_AC': KinematicsType.TRT_AC, 'XYZAC_TRT': KinematicsType.TRT_AC, + 'TRT_BC': KinematicsType.TRT_BC, 'XYZBC_TRT': KinematicsType.TRT_BC, + 'IDENTITY': KinematicsType.IDENTITY, 'FIVEAXIS_BC': KinematicsType.FIVEAXIS_BC, + 'MAXKINS_BC': KinematicsType.MAXKINS_BC, 'HEXAPOD': KinematicsType.HEXAPOD, + 'PUMA': KinematicsType.PUMA, 'SCARA': KinematicsType.SCARA, + 'LINEAR_DELTA': KinematicsType.LINEAR_DELTA, + } + kernel.kinematics_type = type_map.get(kin_type_str, KinematicsType.TRT_BC) + kernel._original_kinematics_type = kernel.kinematics_type + + if kernel._kinematics_instance: + kernel._kinematics_instance.set_params(**kin_config.get('parameters', {})) + + for tool in config.get('tool_table', []): + pocket = tool.get('pocket', 1) + kernel.tool_table[pocket] = { + 'length': tool.get('length', 100.0), + 'diameter': tool.get('diameter', 10.0), + 'name': tool.get('name', f'T{pocket}') + } + + for offset_name, offset in config.get('work_offsets', {}).items(): + if offset_name.startswith('G'): + try: + offset_num = int(offset_name[1:]) + kernel.work_offsets[offset_num] = { + 'x': offset.get('X', 0.0), 'y': offset.get('Y', 0.0), + 'z': offset.get('Z', 0.0), 'a': offset.get('A', 0.0), + 'b': offset.get('B', 0.0), 'c': offset.get('C', 0.0) + } + except ValueError: + pass + + for pin_name, pin_value in config.get('hal_pins', {}).items(): + kernel.virtual_hal.set(pin_name, pin_value) + + self._machines[machine_id] = kernel + if self.debug: + print(f"[MachineManager] 创建机床: {machine_id}") + return kernel + + + def create_machine_CNCKernel(self, machine_id: str, config: Optional[Dict] = None) -> CNCKernel: + if machine_id in self._machines: + return self._machines[machine_id] + + machine_name = config.get('machine_name', machine_id) if config else machine_id + kernel = CNCKernel(debug=self.debug, name=machine_name) + CNCKernelExtensions.extend_all(kernel) + + if config: + self._machine_configs[machine_id] = config + + # 设置运动学类型 + kin_config = config.get('kinematics', {}) + kin_type_str = kin_config.get('type', 'TRT_BC').upper() + type_map = { + 'TRT_AC': KinematicsType.TRT_AC, 'XYZAC_TRT': KinematicsType.TRT_AC, + 'TRT_BC': KinematicsType.TRT_BC, 'XYZBC_TRT': KinematicsType.TRT_BC, + 'IDENTITY': KinematicsType.IDENTITY, 'FIVEAXIS_BC': KinematicsType.FIVEAXIS_BC, + 'MAXKINS_BC': KinematicsType.MAXKINS_BC, 'HEXAPOD': KinematicsType.HEXAPOD, + 'PUMA': KinematicsType.PUMA, 'SCARA': KinematicsType.SCARA, + 'LINEAR_DELTA': KinematicsType.LINEAR_DELTA, + } + kernel.kinematics_type = type_map.get(kin_type_str, KinematicsType.TRT_BC) + kernel._original_kinematics_type = kernel.kinematics_type + + # 设置运动学参数 + if kernel._kinematics_instance: + kernel._kinematics_instance.set_params(**kin_config.get('parameters', {})) + + # 设置刀具表 + for tool in config.get('tool_table', []): + pocket = tool.get('pocket', 1) + kernel.tool_table[pocket] = { + 'length': tool.get('length', 100.0), + 'diameter': tool.get('diameter', 10.0), + 'name': tool.get('name', f'T{pocket}') + } + + # 设置工件坐标系 + for offset_name, offset in config.get('work_offsets', {}).items(): + if offset_name.startswith('G'): + try: + offset_num = int(offset_name[1:]) + kernel.work_offsets[offset_num] = { + 'x': offset.get('X', 0.0), 'y': offset.get('Y', 0.0), + 'z': offset.get('Z', 0.0), 'a': offset.get('A', 0.0), + 'b': offset.get('B', 0.0), 'c': offset.get('C', 0.0) + } + except ValueError: + pass + + # 设置HAL引脚 + for pin_name, pin_value in config.get('hal_pins', {}).items(): + kernel.virtual_hal.set(pin_name, pin_value) + + self._machines[machine_id] = kernel + if self.debug: + print(f"[MachineManager] 创建机床: {machine_id}") + return kernel + + def get_machine(self, machine_id: str) -> Optional[CNCKernel]: + return self._machines.get(machine_id) + + def remove_machine(self, machine_id: str) -> bool: + if machine_id in self._machines: + self._machines[machine_id].shutdown() + del self._machines[machine_id] + self._machine_programs.pop(machine_id, None) + self._machine_configs.pop(machine_id, None) + return True + return False + + def list_machines(self) -> List[str]: + return list(self._machines.keys()) + + def get_status(self, machine_id: str) -> Optional[Dict]: + kernel = self.get_machine(machine_id) + if not kernel: + return None + s = kernel.status + + # 尝试从收集器获取偏移信息 + g5x_x = g5x_y = g5x_z = 0.0 + g92_x = g92_y = g92_z = 0.0 + + if hasattr(kernel, 'parser') and kernel.parser: + parser = kernel.parser + if hasattr(parser, '_builtin_parser') and parser._builtin_parser: + bp = parser._builtin_parser + if bp.collector: + c = bp.collector + g5x_x = c.g5x_offset_x + g5x_y = c.g5x_offset_y + g5x_z = c.g5x_offset_z + g92_x = c.g92_offset_x + g92_y = c.g92_offset_y + g92_z = c.g92_offset_z + + return { + 'machine_id': machine_id, + 'machine_name': kernel.name, + 'state': kernel.state_machine.get_state(), + # ... 现有字段 ... + 'g5x_offset_x': g5x_x, + 'g5x_offset_y': g5x_y, + 'g5x_offset_z': g5x_z, + 'g92_x': g92_x, + 'g92_y': g92_y, + 'g92_z': g92_z, + 'tool_length': s.tool_length, + 'coordinate_mode': s.current_offset if s.current_offset else 'G54', + 'rtcp_enabled': s.rtcp_enabled, + 'current_tool': s.current_tool, + 'selected_tool': s.selected_tool, + 'tool_length': s.tool_length, + 'tool_diameter': s.tool_diameter, + 'position': {'x': s.position_x, 'y': s.position_y, 'z': s.position_z, + 'a': s.position_a, 'b': s.position_b, 'c': s.position_c}, + 'world_position': {'x': s.world_x, 'y': s.world_y, 'z': s.world_z}, + 'machine_position': {'x': s.machine_x, 'y': s.machine_y, 'z': s.machine_z}, + 'spindle_speed': s.spindle_speed, + 'spindle_state': s.spindle_state, + 'coolant_flood': s.coolant_flood, + 'coolant_mist': s.coolant_mist, + 'coolant_state': s.coolant_state, + 'feedrate': s.feedrate, + 'feed_override': s.feed_override, + 'rapid_override': s.rapid_override, + 'spindle_override': s.spindle_override, + 'program_name': s.program_name, + 'program_progress': s.progress_percent, + 'current_line': s.current_line, + 'total_lines': s.total_lines, + 'path_length': s.path_length, + 'total_time': s.total_time, + 'alarm_code': s.alarm_code, + 'alarm_message': s.alarm_message, + 'coordinate_mode': s.coordinate_mode, + 'plane_mode': s.plane_mode, + 'unit_mode': s.unit_mode, + 'cutter_comp': s.cutter_comp, + 'current_offset': s.current_offset, + } + + def get_all_status(self) -> Dict[str, Dict]: + result = {} + for mid in self._machines: + status = self.get_status(mid) + if status: + result[mid] = status + return result + + def load_program(self, machine_id: str, program_config: Dict) -> bool: + kernel = self.get_machine(machine_id) + if not kernel: + return False + + try: + self._machine_programs[machine_id] = program_config + mp = program_config.get('main_program', {}) + content = mp.get('content', '') + if not content: + return False + + # 获取运动学参数 + kinematics_params = {} + if kernel._kinematics_instance and kernel._kinematics_instance.params: + p = kernel._kinematics_instance.params + kinematics_params = { + 'rot_center_x': getattr(p, 'rot_center_x', 0.0), + 'rot_center_y': getattr(p, 'rot_center_y', 0.0), + 'rot_center_z': getattr(p, 'rot_center_z', 0.0), + 'pivot_length': getattr(p, 'pivot_length', 250.0), + 'tool_length': getattr(p, 'tool_length', 0.0), + 'conventional_directions': getattr(p, 'conventional_directions', False) + } + + parser = FullRS274Parser( + debug=self.debug, + kinematics_type=kernel.kinematics_type, + kinematics_params=kinematics_params, + max_points=program_config.get('simulation_config', {}).get('max_points', 50000), + acceleration=program_config.get('simulation_config', {}).get('acceleration', 500.0), + max_rapid_rate=program_config.get('simulation_config', {}).get('max_rapid_rate', 10000.0), + max_feed_rate=program_config.get('simulation_config', {}).get('max_feed_rate', 5000.0), + tool_length_map={k: v['length'] for k, v in kernel.tool_table.items()}, + tool_radius_map={k: v['diameter'] / 2.0 for k, v in kernel.tool_table.items()} + ) + + for sub in program_config.get('subroutines', []): + if sub.get('name') and sub.get('content'): + parser.register_subroutine(sub['name'], sub['content']) + + toolpath = parser.parse_string(content, mp.get('name', 'program.ngc')) + if toolpath: + kernel.current_toolpath = toolpath + kernel.status.total_lines = len(toolpath.segments) + kernel.status.path_length = toolpath.total_length + kernel.status.total_time = toolpath.total_time + kernel.status.program_name = mp.get('name', '') + kernel.status.rtcp_enabled = toolpath.rtcp_enabled + kernel.status.kinematics_type = toolpath.kinematics_type + if self.debug: + print(f"[MachineManager] 程序加载成功: {machine_id}, 段数: {len(toolpath.segments)}") + return True + return False + except Exception as e: + if self.debug: + print(f"[MachineManager] 加载程序失败: {e}") + return False + + def get_machine_program_info(self, machine_id: str) -> Optional[Dict]: + kernel = self.get_machine(machine_id) + if not kernel or not kernel.current_toolpath: + return None + tp = kernel.current_toolpath + pc = self._machine_programs.get(machine_id, {}) + return { + 'program_id': pc.get('program_id', ''), + 'program_name': pc.get('program_name', tp.filename), + 'filename': tp.filename, + 'total_segments': len(tp.segments), + 'total_length': tp.total_length, + 'total_time': tp.total_time, + 'bounds': tp.bounds, + 'statistics': tp.get_statistics(), + 'rtcp_enabled': tp.rtcp_enabled, + 'kinematics_type': tp.kinematics_type, + 'tool_changes': tp.tool_changes, + 'subroutines': list(tp.subroutines.keys()) if tp.subroutines else [], + 'errors': tp.errors, + 'warnings': tp.warnings, + } + + +# ==================== CNCKernelWithJSON 类 ==================== + +class CNCKernelWithJSON(CNCKernel): + """支持JSON配置的CNC内核""" + + def __init__(self, debug: bool = False, name: str = "CNC1", + json_config: Optional[Union[str, Dict]] = None): + super().__init__(debug=debug, name=name) + + CNCKernelExtensions.extend_all(self) + + self.project_config: Optional[ProjectConfig] = None + self.current_program_config: Optional[Dict] = None + self.machine_id = name + + if json_config: + self.load_config(json_config) + + def load_config(self, json_source: Union[str, Dict]) -> Tuple[ProjectConfig, Optional[ToolpathData]]: + """加载JSON配置""" + if isinstance(json_source, str): + if os.path.exists(json_source): + with open(json_source, 'r', encoding='utf-8') as f: + config_data = json.load(f) + else: + config_data = json.loads(json_source) + else: + config_data = json_source + + config = self._parse_config(config_data) + self.project_config = config + self._apply_machine_config(config) + + toolpath = self._load_program_from_config(config) + self.current_toolpath = toolpath + + if toolpath: + self.status.total_lines = len(toolpath.segments) + self.status.path_length = toolpath.total_length + self.status.total_time = toolpath.total_time + self.status.path_segments = len(toolpath.segments) + self.status.rtcp_enabled = toolpath.rtcp_enabled + self.status.kinematics_type = toolpath.kinematics_type + self.status.program_name = config.main_program.name if config.main_program else '' + + self._notify_update() + return config, toolpath + + def _parse_config(self, data: Dict) -> ProjectConfig: + """解析配置数据""" + config = ProjectConfig() + + if 'project' in data: + config.project = data['project'] + + if 'machine' in data: + mc = data['machine'] + config.machine = MachineConfig( + name=mc.get('name', ''), + type=mc.get('type', '5axis_mill'), + kinematics_type=mc.get('kinematics', {}).get('type', 'TRT_BC'), + kinematics_params=mc.get('kinematics', {}).get('parameters', {}), + axes=mc.get('axes', {}), + spindle=mc.get('spindle', {}), + tool_table=mc.get('tool_table', []), + work_offsets=mc.get('work_offsets', {}) + ) + + if 'hal_config' in data: + hc = data['hal_config'] + pins = {} + for name, pin in hc.get('pins', {}).items(): + if isinstance(pin, dict): + pins[name] = HALPinConfig( + name=name, + pin_type=pin.get('type', 'float'), + direction=pin.get('direction', 'in'), + default=pin.get('default', 0.0), + description=pin.get('description', '') + ) + else: + pins[name] = HALPinConfig( + name=name, + default=float(pin) if isinstance(pin, (int, float)) else 0.0 + ) + config.hal_config = HALConfig( + version=hc.get('version', '1.0'), + description=hc.get('description', ''), + pins=pins, + connections=hc.get('connections', []) + ) + else: + config.hal_config = HALConfig() + + if 'main_program' in data: + mp = data['main_program'] + config.main_program = ProgramConfig( + name=mp.get('name', ''), + description=mp.get('description', ''), + content=mp.get('content', '') + ) + + if 'subroutines' in data: + config.subroutines = [ + SubroutineConfig( + name=sub.get('name', ''), + description=sub.get('description', ''), + content=sub.get('content', ''), + parameters=sub.get('parameters', []) + ) + for sub in data['subroutines'] + ] + + if 'simulation_config' in data: + sc = data['simulation_config'] + config.simulation_config = SimulationConfig( + acceleration=sc.get('acceleration', 500.0), + max_rapid_rate=sc.get('max_rapid_rate', 10000.0), + max_feed_rate=sc.get('max_feed_rate', 5000.0), + arc_division=sc.get('arc_division', 64), + enable_rtcp_debug=sc.get('enable_rtcp_debug', False), + max_points=sc.get('max_points', 50000), + colors=sc.get('colors', {}) + ) + else: + config.simulation_config = SimulationConfig() + + return config + + def _apply_machine_config(self, config: ProjectConfig): + """应用机床配置""" + # 设置HAL引脚 + for pin_name, pin_config in config.hal_config.pins.items(): + if isinstance(pin_config, HALPinConfig): + self.virtual_hal.set(pin_name, pin_config.default) + elif isinstance(pin_config, dict): + self.virtual_hal.set(pin_name, pin_config.get('default', 0)) + else: + self.virtual_hal.set(pin_name, pin_config) + + # 设置刀具表 + for tool in config.machine.tool_table: + pocket = tool.get('pocket', 1) + self.tool_table[pocket] = { + 'length': tool.get('length', 100.0), + 'diameter': tool.get('diameter', 10.0), + 'name': tool.get('name', f'T{pocket}') + } + + # 设置工件坐标系 + for offset_name, offset_data in config.machine.work_offsets.items(): + if offset_name.startswith('G'): + try: + offset_num = int(offset_name[1:]) + self.work_offsets[offset_num] = { + 'x': offset_data.get('X', 0.0), + 'y': offset_data.get('Y', 0.0), + 'z': offset_data.get('Z', 0.0), + 'a': offset_data.get('A', 0.0), + 'b': offset_data.get('B', 0.0), + 'c': offset_data.get('C', 0.0) + } + except ValueError: + pass + + # 设置运动学类型 + kin_type_str = config.machine.kinematics_type.upper() + type_map = { + 'IDENTITY': KinematicsType.IDENTITY, + 'TRT_AC': KinematicsType.TRT_AC, + 'XYZAC_TRT': KinematicsType.TRT_AC, + 'TRT_BC': KinematicsType.TRT_BC, + 'XYZBC_TRT': KinematicsType.TRT_BC, + 'MAXKINS_BC': KinematicsType.MAXKINS_BC, + 'FIVEAXIS_BC': KinematicsType.FIVEAXIS_BC, + 'HEXAPOD': KinematicsType.HEXAPOD, + 'PUMA': KinematicsType.PUMA, + 'SCARA': KinematicsType.SCARA, + 'LINEAR_DELTA': KinematicsType.LINEAR_DELTA, + } + self._original_kinematics_type = type_map.get(kin_type_str, KinematicsType.TRT_AC) + self.kinematics_type = self._original_kinematics_type + self.status.kinematics_type = self.kinematics_type.name + + # 设置运动学参数 + kin_params = config.machine.kinematics_params + if self._kinematics_instance: + self._kinematics_instance.set_params(**kin_params) + + self.virtual_hal.set('motion.pivot-length', kin_params.get('pivot_length', 250.0)) + + def _load_program_from_config(self, config: ProjectConfig) -> Optional[ToolpathData]: + """从配置加载程序""" + sim_config = config.simulation_config + + parser = FullRS274Parser( + max_points=sim_config.max_points, + acceleration=sim_config.acceleration, + max_rapid_rate=sim_config.max_rapid_rate, + max_feed_rate=sim_config.max_feed_rate, + kinematics_type=self.kinematics_type, + kinematics_params=config.machine.kinematics_params, + debug=sim_config.enable_rtcp_debug + ) + + # 设置刀具表 + for tool in config.machine.tool_table: + pocket = tool.get('pocket', 1) + parser.tool_length_map[pocket] = tool.get('length', 100.0) + parser.tool_radius_map[pocket] = tool.get('diameter', 10.0) / 2.0 + + # 注册子程序 + for sub in config.subroutines: + name = sub.name if hasattr(sub, 'name') else sub.get('name', '') + content = sub.content if hasattr(sub, 'content') else sub.get('content', '') + if name and content: + parser.register_subroutine(name, content) + + # 解析主程序 + if config.main_program and config.main_program.content: + return parser.parse_string( + config.main_program.content, + config.main_program.name + ) + return None + + def load_from_json(self, json_source: Union[str, Dict]) -> Tuple[ProjectConfig, Optional[ToolpathData]]: + return self.load_config(json_source) + + def load_from_json_file(self, filepath: str) -> Tuple[ProjectConfig, Optional[ToolpathData]]: + return self.load_config(filepath) + + def load_from_json_string(self, json_str: str) -> Tuple[ProjectConfig, Optional[ToolpathData]]: + return self.load_config(json_str) + + def load_program_config(self, program_config: Dict) -> bool: + try: + self.current_program_config = program_config + full_config = self._build_config_from_program(program_config) + self.load_config(full_config) + return True + except Exception as e: + if self.debug: + print(f"加载程序配置失败: {e}") + return False + + def _build_config_from_program(self, program_config: Dict) -> Dict: + machine_config = { + "name": self.machine_id, + "kinematics": { + "type": self.kinematics_type.name, + "parameters": {} + } + } + + if self.project_config: + machine_config = { + "name": self.machine_id, + "type": self.project_config.machine.type, + "kinematics": { + "type": self.project_config.machine.kinematics_type, + "parameters": self.project_config.machine.kinematics_params + }, + "tool_table": self.project_config.machine.tool_table, + "work_offsets": self.project_config.machine.work_offsets, + "axes": self.project_config.machine.axes + } + + return { + "project": {"name": program_config.get('program_id', 'Program')}, + "machine": machine_config, + "main_program": program_config.get('main_program', {}), + "subroutines": program_config.get('subroutines', []), + "simulation_config": program_config.get('simulation_config', { + "acceleration": 500.0, + "max_rapid_rate": 10000.0, + "max_feed_rate": 5000.0, + "max_points": 50000, + "enable_rtcp_debug": self.debug + }), + "hal_config": {"version": "1.0", "pins": {}} + } + + + + + + + + + + + + + + +# ==================== 测试函数 ==================== + +def test_all_kinematics(): + """测试所有运动学类型""" + print("=" * 80) + print("测试所有 LinuxCNC 运动学类型") + print("=" * 80) + + kin_types = [ + KinematicsType.TRT_AC, KinematicsType.TRT_BC, KinematicsType.MAXKINS_BC, + KinematicsType.FIVEAXIS_BC, KinematicsType.HEXAPOD, KinematicsType.PUMA, + KinematicsType.SCARA, KinematicsType.SCORBOT, KinematicsType.LINEAR_DELTA, + KinematicsType.ROTARY_DELTA, KinematicsType.TRIPOD, KinematicsType.PENTAPOD, + KinematicsType.ROTATE, KinematicsType.COREXY, KinematicsType.ROSE, + ] + + for kin_type in kin_types: + print(f"\n测试 {kin_type.name}:") + try: + kin = KinematicsFactory.create(kin_type, debug=False) + if kin: + print(f" ✓ 创建成功") + info = KinematicsFactory.get_type_info(kin_type) + print(f" 描述: {info.get('description', 'N/A')}") + print(f" 来源: {info.get('source', 'N/A')}") + else: + print(f" - IDENTITY类型返回None") + except Exception as e: + print(f" ✗ 创建失败: {e}") + + +def test_kinematics(): + """测试五轴运动学""" + print("=" * 60) + print("测试五轴运动学") + print("=" * 60) + + kin = FiveAxisKinematics(kin_type=KinematicsType.TRT_BC, debug=True) + kin.set_tool_length(100.0) + kin.enable_rtcp(True) + + joints = [0, 0, 0, 30, 0] + world = kin.forward_transform(joints) + print(f"关节坐标 {joints} -> 世界坐标 ({world[0]:.3f}, {world[1]:.3f}, {world[2]:.3f})") + + world_pos = (0, 0, 100, 30, 0, 0) + joints_back = kin.inverse_transform(world_pos) + print(f"世界坐标 {world_pos} -> 关节坐标 ({joints_back[0]:.3f}, {joints_back[1]:.3f}, {joints_back[2]:.3f})") + + +def test_parser(): + """测试解析器""" + print("=" * 60) + print("测试G代码解析器") + print("=" * 60) + + test_program = """ +G90 G17 G21 G40 G49 G80 +G54 +T1 M6 +G43.4 H1 +G0 X0 Y0 Z100 +G1 X50 Y0 Z50 F1000 +G1 X50 Y50 Z50 +G1 X0 Y50 Z50 +G1 X0 Y0 Z50 +G0 Z100 +G49 +M30 +""" + + parser = FullRS274Parser(debug=True) + toolpath = parser.parse_string(test_program, "test.ngc") + + print(f"\n解析结果:") + print(f" 路径段数: {len(toolpath.segments)}") + print(f" 总长度: {toolpath.total_length:.3f} mm") + print(f" 总时间: {toolpath.total_time:.3f} s") + print(f" 边界: {toolpath.bounds}") + + +def main(): + """主函数""" + print("=" * 80) + print("GLCanon 五轴刀具路径解析器 - 完整集成版") + print(f"版本: {VERSION}") + print("包含: LinuxCNC 运动学算法完整集成") + print("包含: LinuxCNC 完整参数表系统 (RS274NGC)") + print("支持的构型:") + print(" - TRT_AC (xyzac-trt-kins)") + print(" - TRT_BC (xyzbc-trt-kins)") + print(" - MAXKINS_BC (maxkins)") + print(" - FIVEAXIS_BC (5axiskins)") + print(" - HEXAPOD (genhexkins)") + print(" - SERIAL_DH (genserkins)") + print(" - PUMA (pumakins)") + print(" - SCARA (scarakins)") + print(" - SCORBOT (scorbot-kins)") + print(" - LINEAR_DELTA (lineardeltakins)") + print(" - ROTARY_DELTA (rotarydeltakins)") + print(" - TRIPOD (tripodkins)") + print(" - PENTAPOD (pentakins)") + print(" - ROTATE (rotatekins)") + print(" - COREXY (corexykins)") + print(" - ROSE (rosekins)") + print("=" * 80) + + test_kinematics() + print("\n") + + test_all_kinematics() + print("\n") + + test_parser() + print("\n") + + print("=" * 80) + print("所有测试完成!") + print("=" * 80) + + +if __name__ == "__main__": + main() + + \ No newline at end of file diff --git a/inputtext.txt b/inputtext.txt new file mode 100644 index 0000000..3703d42 --- /dev/null +++ b/inputtext.txt @@ -0,0 +1,12 @@ +目标：对标 LinuxCNC，分阶段完善 Web 版数控系统仿真的 WASM 程序，逐步补齐完整功能并严格测试。 + +近期重点： +- 刀补 +- 坐标系 +- 固定循环 +- 五轴 / RTCP +- M428、M429、M430 +- 子程序调用 + +实现约束：若 WASM 编译后涉及文件读写，可考虑使用 OPFS 满足文件处理需求。 +继续进一步完善程序，步步为营。G代码语法，与linuxcnc一致。程序主要来源于linuxcnc项目中的源程序。功能和G代码与linuxcnc对标。 \ No newline at end of file diff --git a/linuxcnc b/linuxcnc new file mode 160000 index 0000000..60597ee --- /dev/null +++ b/linuxcnc @@ -0,0 +1 @@ +Subproject commit 60597ee0718873d2449058c824262a275e5e4bad diff --git a/wasm-simulator b/wasm-simulator new file mode 160000 index 0000000..0ff0058 --- /dev/null +++ b/wasm-simulator @@ -0,0 +1 @@ +Subproject commit 0ff00584175154bd65a4229df55d56825f967abd