#!/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()