Huangjiabao_backup/kelaifen/Kelaifen_V6.0/trajectory_program.py

import sys
import copy
from queue import Queue
from ETController import send_Point, sendCMD, wait_stop
from gpio import turn_off_relay, turn_on_relay
from serial_handler import read_cmd_from_shared
# from program_config import ( 
#     PRESET_PARAMS,  turn_off_relay_start, 
#      turn_on_relay_start
#     )
from serial_init import SerialSharedData
from trajectory_data import (
    Pose_1, Pose_2, Pose_3, Pose_4,  Pose_3_4,  Pose_4_3,
    joint_positions_assembled_1, joint_positions_assembled_2, 
    joint_positions_assembled_3, joint_positions_assembled_4,
    Pose_1_2, Pose_2_1, 
    )
import time
# 导入串口处理模块
import serial_handler

# 采样时间、延时时间和衔接机器人速度
SAMP_TIME = 20
SLP_TIME = 0.02
# 补偿值更新队列（解耦线程通信，避免锁竞争）
compensate_queue = Queue(maxsize=1)  # 最多缓存1个最新值，确保数据时效性
# 给串口模块赋值队列（新增，关键！）
serial_handler.compensate_queue = compensate_queue
# 全局坐标补偿变量
increments_x = 0
increments_y = 0
increments_z = 0
Coordinate_compensation = [0, 0, 0, 0, 0, 0]
# 预期的帧长度，可根据实际情况修改
EXPECTED_LENGTH = 7
global sample_time
global sleep_time
move_speed = 100
# 前瞻时间
lookahead_time = 400
# 调整之后的位姿
Pose_after_adjustment = [0, 0, 0, 0, 0, 0]
# 运动速度调整、z方向初始位置调整
# speed_adjustment = serial_handler.speed_adjustment    
# Trajectory_angle = serial_handler.Trajectory_angle                
velocity_coefficient = 100    
z_positionadjustment = -200      # 轨迹整体调整

# 初始化全局共享数据实例
serial_shared = SerialSharedData()
# 轨迹插值点数
Interpolation_points = 20

# 第一段程序封装为函数
# 第二段程序封装为函数(上升喷涂)
# 第三段程序封装为函数
# 第四段程序封装为函数（上升喷涂）
# 第五段程序封装为函数
# 第六段程序封装为函数（下降喷涂）
# 第七段程序封装为函数
# 第八段程序封装为函数（下降喷涂）
# 第九段程序封装为函数
# 第十二段程序封装为函数

def program1(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
    i = 0
    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    turn_off_relay()
    global increments_x, increments_y, increments_z
    global Coordinate_compensation
    global sample_time
    global sleep_time

    sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment)
    sleep_time = sample_time*0.001

    # 从队列获取最新补偿值（非阻塞，无新值则使用当前值）
    if not compensate_queue.empty():
        increments_x, increments_y, increments_z = compensate_queue.get_nowait()
    increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0]
    Coordinate_compensation = increments
    reference_pos = None
    new_joint_pos = None

    if len(Pose_1) > 0 and len(Pose_1[0]) == 6:
        Path_First_Position = Pose_1[0]  # 获取第一行数据
        Modified_First_Position = [val + inc for val, inc in zip(Path_First_Position, Coordinate_compensation)]
        target_pose = Modified_First_Position
    else:
        return

    for line_list in joint_positions_assembled_1:
        # 确保每行是6个数值（与原文件的逗号分割后逻辑一致）
        if len(line_list) != 6:
            continue
        if reference_pos is None:
            reference_pos = line_list
            suc, angle, id = sendCMD(sock, "inverseKinematic", {"targetPose": target_pose, "referencePos": reference_pos})
            if suc and angle is not None:
                new_joint_pos = angle
            else:
                print("逆解计算失败，使用原有关节位置")
                new_joint_pos = line_list
        modified_list = new_joint_pos if new_joint_pos is not None else line_list

        if (i == 0):
            while True:
                suc, result, id = sendCMD(sock, "moveByJoint", {"targetPos": modified_list, "speed": 100})
                if suc:
                    break
                print("moveByJoint命令发送失败,重试...")
                time.sleep(0.1)
            wait_stop(sock)
        time.sleep(sleep_time)
        i = i + 1

    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    print("第一段程序执行完")

def program1_1(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
    print("第1_1段程序开始执行")
    i = 0
    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    turn_off_relay()
    global increments_x, increments_y, increments_z
    global Coordinate_compensation
    global sample_time
    global sleep_time
    global lookahead_time

    sample_time = SAMP_TIME
    sleep_time = sample_time*0.001
    lookahead_time = sample_time * Interpolation_points  

    # 从队列获取最新补偿值（非阻塞，无新值则使用当前值）
    if not compensate_queue.empty():
        increments_x, increments_y, increments_z = compensate_queue.get_nowait()
    increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0]
    Coordinate_compensation = increments

    # 获取插值起点和插值终点
    start_pose = Pose_after_adjustment
    end_pose = Pose_1[0].copy()
    # 计算带补偿的终点位姿（最终插值要到达的目标）
    end_pose_with_comp = [end_pose[i] + Coordinate_compensation[i] for i in range(6)]

    interpolate_points = Interpolation_points     # 插值点数（包含起止点）
    interpolated_poses = []
 
    for idx in range(interpolate_points):
        # 计算插值权重（0→1，对应从起点第三列到终点第三列）
        t = idx / (interpolate_points - 1) if interpolate_points > 1 else 1.0
        
        # 组装当前插值位姿：仅更新第三列，其余列沿用起点值
        current_pose = start_pose.copy()
        # 第三列：从起点值 线性插值到 终点补偿值的第三列
        current_pose[2] = start_pose[2] * (1 - t) + end_pose_with_comp[2] * t
        
        # 前两列：同步从起点值 线性调整到 终点补偿值的前两列（与第三列同权重）
        current_pose[0] = start_pose[0] * (1 - t) + end_pose_with_comp[0] * t
        current_pose[1] = start_pose[1] * (1 - t) + end_pose_with_comp[1] * t
        
        # 添加到插值结果列表
        interpolated_poses.append(current_pose)
        # print("\n=== 插值后的位姿列表（逐行）===")
        # for i, pose in enumerate(interpolated_poses):
        #     print(f"第 {i+1} 个插值点: {pose}")
    # 遍历插值结果，逐个发送位姿（保留你原有发送逻辑）
    for modified_list in interpolated_poses:
        # 初始化透传
        if i == 0:
            suc, result, id = sendCMD(sock, "transparent_transmission_init", {
                "lookahead": lookahead_time, 
                "t": sample_time, 
                "smoothness": 1, 
                "response_enable": 0
            })
        # 发送当前插值得到的位姿
        send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list})
        time.sleep(sleep_time)
        i = i + 1

    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    print("第1_1段程序执行完")

def program2(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
    global increments_x, increments_y, increments_z
    global Coordinate_compensation
    global sample_time
    global sleep_time
    global lookahead_time
    global Pose_after_adjustment

    lookahead_time = 400
    i = 0
    sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment)
    sleep_time = sample_time*0.001

    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")

    print("第二段程序开始执行")

    # 遍历Pose_1，同时获取行号（enumerate的index从0开始）
    for line_idx, line_list in enumerate(Pose_1):
        if len(line_list) != 6:
            continue
        actual_line_num = line_idx + 1  # 关键：将枚举的索引转为从1开始的行号
        # 核心逻辑：判断行号是否在13到143之间，修改第三列（索引为2，因为列表从0开始）
        if 13 <= actual_line_num <= 143:
            # 计算新的第三列值：360.2 + ((204.75 + Trajectory_angle)/143) * 行号
            new_third_value = 360.2 + ((204.75 + serial_handler.Trajectory_angle) / 131) * (actual_line_num-12)
            # 复制原列表并修改第三列（避免修改原列表的引用）
            line_list = line_list.copy()
            line_list[2] = new_third_value  # 第三列对应索引2
            if actual_line_num == 143:
                row_143_third_value = new_third_value
        elif actual_line_num > 143 and row_143_third_value is not None:
            # 行号大于143时，用143行的第三列值替换
            line_list = line_list.copy()
            line_list[2] = row_143_third_value

        increments = Coordinate_compensation
        modified_list = [val + inc for val, inc in zip(line_list, increments)]
        Pose_after_adjustment = modified_list
        # print(Pose_after_adjustment)
        if (i == 0):
            suc, result, id = sendCMD(sock, "transparent_transmission_init", {
                "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0})
        send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list})
        time.sleep(sleep_time)

        if(i >= turn_on_relay_start):
            turn_on_relay()
            print(f"打开喷枪2：{turn_on_relay_start}")
        if(i >= turn_off_relay_start):
            turn_off_relay()
            print(f"关闭喷枪2：{turn_off_relay_start}")
        i = i + 1

    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    print("第二段程序执行完")


def program3(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
    print("第三段程序开始执行")
    i = 0
    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    turn_off_relay()
    global increments_x, increments_y, increments_z
    global Coordinate_compensation
    global sample_time
    global sleep_time

    sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment)
    sleep_time = sample_time*0.001

    # 从队列获取最新补偿值（非阻塞，无新值则使用当前值）
    if not compensate_queue.empty():
        increments_x, increments_y, increments_z = compensate_queue.get_nowait()
    increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0]
    Coordinate_compensation = increments
    reference_pos = None
    new_joint_pos = None

    if len(Pose_2) > 0 and len(Pose_2[0]) == 6:
        Path_First_Position = Pose_2[0]  # 获取第一行数据
        Modified_First_Position = [val + inc for val, inc in zip(Path_First_Position, Coordinate_compensation)]
        target_pose = Modified_First_Position
    else:
        return

    for line_list in joint_positions_assembled_2:
        if len(line_list) != 6:
            continue
        if reference_pos is None:
            reference_pos = line_list
            suc, angle, id = sendCMD(sock, "inverseKinematic", {"targetPose": target_pose, "referencePos": reference_pos})
            if suc and angle is not None:
                new_joint_pos = angle
            else:
                print("逆解计算失败，使用原有关节位置")
                new_joint_pos = line_list
        modified_list = new_joint_pos if new_joint_pos is not None else line_list

        if (i == 0):
            while True:
                suc, result, id = sendCMD(sock, "moveByJoint", {"targetPos": modified_list, "speed": 100})
                if suc:
                    break
                print("moveByJoint命令发送失败,重试...")
                time.sleep(0.1)
            wait_stop(sock)
        time.sleep(sleep_time)
        i = i + 1
    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    print("第三段程序执行完")

def program3_1(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
    print("第3_1段程序开始执行")
    i = 0
    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    turn_off_relay()
    global increments_x, increments_y, increments_z
    global Coordinate_compensation
    global sample_time
    global sleep_time
    global lookahead_time

    sample_time = SAMP_TIME
    sleep_time = sample_time*0.001
    lookahead_time = sample_time * Interpolation_points  

    # 从队列获取最新补偿值（非阻塞，无新值则使用当前值）
    if not compensate_queue.empty():
        increments_x, increments_y, increments_z = compensate_queue.get_nowait()
    increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0]
    Coordinate_compensation = increments

    # 获取插值起点和插值终点
    start_pose = Pose_after_adjustment
    end_pose = Pose_2[0].copy()
    # 计算带补偿的终点位姿（最终插值要到达的目标）
    end_pose_with_comp = [end_pose[i] + Coordinate_compensation[i] for i in range(6)]

    interpolate_points = Interpolation_points     
    interpolated_poses = []
    
    for idx in range(interpolate_points):
        # 计算插值权重（0→1，对应从起点第三列到终点第三列）
        t = idx / (interpolate_points - 1) if interpolate_points > 1 else 1.0
        
        # 组装当前插值位姿：仅更新第三列，其余列沿用起点值
        current_pose = start_pose.copy()

        # 第三列：从起点值 线性插值到 终点补偿值的第三列
        current_pose[2] = start_pose[2] * (1 - t) + end_pose_with_comp[2] * t        
        # 前两列：同步从起点值 线性调整到 终点补偿值的前两列（与第三列同权重）
        current_pose[0] = start_pose[0] * (1 - t) + end_pose_with_comp[0] * t
        current_pose[1] = start_pose[1] * (1 - t) + end_pose_with_comp[1] * t
        
        # 添加到插值结果列表
        interpolated_poses.append(current_pose)
        # print("\n=== 插值后的位姿列表（逐行）===")
        # for i, pose in enumerate(interpolated_poses):
        #     print(f"第 {i+1} 个插值点: {pose}")

    # 遍历插值结果，逐个发送位姿（保留你原有发送逻辑）
    for modified_list in interpolated_poses:
        # 初始化透传
        if i == 0:
            suc, result, id = sendCMD(sock, "transparent_transmission_init", {
                "lookahead": lookahead_time, 
                "t": sample_time, 
                "smoothness": 1, 
                "response_enable": 0
            })
        # 发送当前插值得到的位姿
        send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list})
        time.sleep(sleep_time)
        i = i + 1
    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    print("第3_1段程序执行完")

def program4(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
    print("第四段程序开始执行")
    global increments_x, increments_y, increments_z
    global Coordinate_compensation
    global sample_time
    global sleep_time
    global lookahead_time
    global Pose_after_adjustment

    lookahead_time = 400
    i = 0
    sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment)
    sleep_time = sample_time*0.001
    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")

    for line_idx, line_list in enumerate(Pose_2):
        if len(line_list) != 6:
            continue

        actual_line_num = line_idx + 1
        # 核心逻辑：判断行号是否在13-143之间，修改第三列（索引2，列表从0开始）
        if 13 <= actual_line_num <= 143:
            # 计算新的第三列值
            new_third_value = 372.19 + ((204.75 + serial_handler.Trajectory_angle) / 131) * (actual_line_num-12)
            # 复制原列表并修改（避免修改原列表的引用）
            line_list = line_list.copy()
            line_list[2] = new_third_value  # 第三列对应索引2
            if actual_line_num == 143:
                row_143_third_value = new_third_value
        elif actual_line_num > 143 and row_143_third_value is not None:
            # 行号大于143时，用143行的第三列值替换
            line_list = line_list.copy()
            line_list[2] = row_143_third_value

        increments = Coordinate_compensation
        modified_list = [val + inc for val, inc in zip(line_list, increments)]
        Pose_after_adjustment = modified_list
        # print(Pose_after_adjustment)
        if (i == 0):
            suc, result, id = sendCMD(sock, "transparent_transmission_init", {
                "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0})
        send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list})
        time.sleep(sleep_time)
        if(i >= ex_turn_on_relay_start):
            turn_on_relay()
            print(f"打开喷枪4：{ex_turn_on_relay_start}")
        if(i >= ex_turn_off_relay_start):
            turn_off_relay()
            print(f"关闭喷枪4：{ex_turn_off_relay_start}")
        i = i + 1

    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    print("第四段程序执行完")


def program5(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
    print("第五段程序开始执行")
    i = 0
    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    turn_off_relay()
    global increments_x, increments_y, increments_z
    global Coordinate_compensation
    global sample_time
    global sleep_time

    sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment)
    sleep_time = sample_time*0.001

    if not compensate_queue.empty():
        increments_x, increments_y, increments_z = compensate_queue.get_nowait()
    increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0]
    Coordinate_compensation = increments
    reference_pos = None
    new_joint_pos = None

    if len(Pose_3) > 0 and len(Pose_3[0]) == 6:
        Path_First_Position = Pose_3[0]  # 获取第一行数据
        Modified_First_Position = [val + inc for val, inc in zip(Path_First_Position, Coordinate_compensation)]
        target_pose = Modified_First_Position
    else:
        return

    for line_list in joint_positions_assembled_3:
        if len(line_list) != 6:
            continue
        if reference_pos is None:
            reference_pos = line_list
            suc, angle, id = sendCMD(sock, "inverseKinematic",
                                     {"targetPose": target_pose, "referencePos": reference_pos})
            if suc and angle is not None:
                new_joint_pos = angle
            else:
                print("逆解计算失败，使用原有关节位置")
                new_joint_pos = line_list
        modified_list = new_joint_pos if new_joint_pos is not None else line_list

        if (i == 0):
            while True:
                suc, result, id = sendCMD(sock, "moveByJoint", {"targetPos": modified_list, "speed": 100})
                if suc:
                    break
                print("moveByJoint命令发送失败,重试...")
                time.sleep(0.1)
            wait_stop(sock)

        time.sleep(sleep_time)
        i = i + 1

    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    print("第五段程序执行完")

def program5_1(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
    print("第5_1段程序开始执行")
    i = 0
    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    turn_off_relay()
    global increments_x, increments_y, increments_z
    global Coordinate_compensation
    global sample_time
    global sleep_time
    global lookahead_time

    sample_time = SAMP_TIME
    sleep_time = sample_time*0.001
    lookahead_time = sample_time * Interpolation_points  

    if not compensate_queue.empty():
        increments_x, increments_y, increments_z = compensate_queue.get_nowait()
    increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0]
    Coordinate_compensation = increments

    # 获取插值起点和插值终点
    start_pose = Pose_after_adjustment
    end_pose = Pose_3[0].copy()
    # 计算带补偿的终点位姿（最终插值要到达的目标）
    end_pose_with_comp = [end_pose[i] + Coordinate_compensation[i] for i in range(6)]

    interpolate_points = Interpolation_points     
    interpolated_poses = []
    
    for idx in range(interpolate_points):
        # 计算插值权重（0→1，对应从起点第三列到终点第三列）
        t = idx / (interpolate_points - 1) if interpolate_points > 1 else 1.0
        
        # 组装当前插值位姿：仅更新第三列，其余列沿用起点值
        current_pose = start_pose.copy()
        # 第三列：从起点值 线性插值到 终点补偿值的第三列
        current_pose[2] = start_pose[2] * (1 - t) + end_pose_with_comp[2] * t
        
        # 前两列：同步从起点值 线性调整到 终点补偿值的前两列（与第三列同权重）
        current_pose[0] = start_pose[0] * (1 - t) + end_pose_with_comp[0] * t
        current_pose[1] = start_pose[1] * (1 - t) + end_pose_with_comp[1] * t
        
        
        # 添加到插值结果列表
        interpolated_poses.append(current_pose)
        # print("\n=== 插值后的位姿列表（逐行）===")
        # for i, pose in enumerate(interpolated_poses):
        #     print(f"第 {i+1} 个插值点: {pose}")

    # 遍历插值结果，逐个发送位姿（保留你原有发送逻辑）
    for modified_list in interpolated_poses:
        # 初始化透传
        if i == 0:
            suc, result, id = sendCMD(sock, "transparent_transmission_init", {
                "lookahead": lookahead_time, 
                "t": sample_time, 
                "smoothness": 1, 
                "response_enable": 0
            })
        # 发送当前插值得到的位姿
        send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list})
        time.sleep(sleep_time)
        i = i + 1
    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    print("第5_1段程序执行完")

def program6(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
    global increments_x, increments_y, increments_z
    global Coordinate_compensation
    global sample_time
    global sleep_time
    global lookahead_time
    global Pose_after_adjustment

    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    lookahead_time = 400
    i = 0
    sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment)
    sleep_time = sample_time*0.001
    
    for line_idx, line_list in enumerate(Pose_3):
        if len(line_list) != 6:
            continue
        actual_line_num = line_idx + 1
        # 核心逻辑：判断行号是否在13-143之间，修改第三列（索引2，列表从0开始）
        if 13 <= actual_line_num <= 143:
            # 计算新的第三列值（注意是减法公式）
            new_third_value = 576.94 - ((204.75 - serial_handler.Trajectory_angle) / 131) * (actual_line_num-12)
            # 复制原列表并修改（避免修改原列表的引用）
            line_list = line_list.copy()
            line_list[2] = new_third_value  # 第三列对应索引2
            if actual_line_num == 143:
                row_143_third_value = new_third_value
        elif actual_line_num > 143 and row_143_third_value is not None:
            # 行号大于143时，用143行的第三列值替换
            line_list = line_list.copy()
            line_list[2] = row_143_third_value

        increments = Coordinate_compensation
        modified_list = [val + inc for val, inc in zip(line_list, increments)]
        Pose_after_adjustment = modified_list
        # print(Pose_after_adjustment)
        if (i == 0):
            suc, result, id = sendCMD(sock, "transparent_transmission_init", {
                "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0})
        send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list})
        time.sleep(sleep_time)
        if(i >= turn_on_relay_start):
            turn_on_relay()
            print(f"打开喷枪6：{turn_on_relay_start}")
        if(i >= turn_off_relay_start):
            turn_off_relay()
            print(f"关闭喷枪6：{turn_off_relay_start}")
        i = i + 1
    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    print("第六段程序执行完")


def program7(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
    i = 0
    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    turn_off_relay()
    global increments_x, increments_y, increments_z
    global Coordinate_compensation
    global sample_time
    global sleep_time

    sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment)
    sleep_time = sample_time*0.001

    if not compensate_queue.empty():
        increments_x, increments_y, increments_z = compensate_queue.get_nowait()
    increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0]
    Coordinate_compensation = increments
    reference_pos = None
    new_joint_pos = None

    if len(Pose_4) > 0 and len(Pose_4[0]) == 6:
        Path_First_Position = Pose_4[0]  # 获取第一行数据
        Modified_First_Position = [val + inc for val, inc in zip(Path_First_Position, Coordinate_compensation)]
        target_pose = Modified_First_Position
    else:
        return

    for line_list in joint_positions_assembled_4:
        if len(line_list) != 6:
            continue
        if reference_pos is None:
            reference_pos = line_list
            suc, angle, id = sendCMD(sock, "inverseKinematic",
                                     {"targetPose": target_pose, "referencePos": reference_pos})
            if suc and angle is not None:
                new_joint_pos = angle
            else:
                print("逆解计算失败，使用原有关节位置")
                new_joint_pos = line_list
        modified_list = new_joint_pos if new_joint_pos is not None else line_list

        if (i == 0):
            while True:
                suc, result, id = sendCMD(sock, "moveByJoint", {"targetPos": modified_list, "speed": 100})
                if suc:
                    break
                print("moveByJoint命令发送失败,重试...")
                time.sleep(0.1)
            wait_stop(sock)
        time.sleep(sleep_time)
        i = i + 1

    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    print("第七段程序执行完")

def program7_1(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
    print("第7_1段程序开始执行")
    i = 0
    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    turn_off_relay()
    global increments_x, increments_y, increments_z
    global Coordinate_compensation
    global sample_time
    global sleep_time
    global lookahead_time

    sample_time = SAMP_TIME
    sleep_time = sample_time*0.001
    lookahead_time = sample_time * Interpolation_points  

    if not compensate_queue.empty():
        increments_x, increments_y, increments_z = compensate_queue.get_nowait()
    increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0]
    Coordinate_compensation = increments

    # 获取插值起点和插值终点
    start_pose = Pose_after_adjustment
    end_pose = Pose_4[0]
    # 计算带补偿的终点位姿（最终插值要到达的目标）
    end_pose_with_comp = [end_pose[i] + Coordinate_compensation[i] for i in range(6)]   

    interpolate_points = Interpolation_points     
    interpolated_poses = []
    # 仅对第三列进行线性插值，其余列保持起点值不变
    for idx in range(interpolate_points):
        # 计算插值权重（0→1，对应从起点第三列到终点第三列）
        t = idx / (interpolate_points - 1) if interpolate_points > 1 else 1.0
        # 组装当前插值位姿：仅更新第三列，其余列沿用起点值
        current_pose = start_pose.copy()
        # 第三列：从起点值 线性插值到 终点补偿值的第三列
        current_pose[2] = start_pose[2] * (1 - t) + end_pose_with_comp[2] * t
        
        # 前两列：同步从起点值 线性调整到 终点补偿值的前两列（与第三列同权重）
        current_pose[0] = start_pose[0] * (1 - t) + end_pose_with_comp[0] * t
        current_pose[1] = start_pose[1] * (1 - t) + end_pose_with_comp[1] * t

        # 添加到插值结果列表
        interpolated_poses.append(current_pose)
        # print("\n=== 插值后的位姿列表（逐行）===")
        # for i, pose in enumerate(interpolated_poses):
        #     print(f"第 {i+1} 个插值点: {pose}")

    # 遍历插值结果，逐个发送位姿（保留你原有发送逻辑）
    for modified_list in interpolated_poses:
        # 初始化透传
        if i == 0:
            suc, result, id = sendCMD(sock, "transparent_transmission_init", {
                "lookahead": lookahead_time, 
                "t": sample_time, 
                "smoothness": 1, 
                "response_enable": 0
            })
        # 发送当前插值得到的位姿
        send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list})
        time.sleep(sleep_time)
        i = i + 1
    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    print("第7_1段程序执行完")

def program8(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
    global increments_x, increments_y, increments_z
    global Coordinate_compensation
    global sample_time
    global sleep_time
    global lookahead_time
    global Pose_after_adjustment

    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    lookahead_time = 400
    i = 0
    sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment)
    sleep_time = sample_time*0.001

    # 遍历Pose_4，用enumerate获取行索引（从0开始）
    for line_idx, line_list in enumerate(Pose_4):
        if len(line_list) != 6:
            continue
        actual_line_num = line_idx + 1

        # 核心逻辑：判断行号是否在13-143之间，修改第三列（索引2，列表从0开始）
        if 13 <= actual_line_num <= 143:
            # 计算新的第三列值（注意是减法公式）
            new_third_value = 564.95 - ((204.75 - serial_handler.Trajectory_angle) / 131) * (actual_line_num-12)
            # 复制原列表并修改（避免修改原列表的引用）
            line_list = line_list.copy()
            line_list[2] = new_third_value  # 第三列对应索引2
            if actual_line_num == 143:
                row_143_third_value = new_third_value
        elif actual_line_num > 143 and row_143_third_value is not None:
            # 行号大于143时，用143行的第三列值替换
            line_list = line_list.copy()
            line_list[2] = row_143_third_value

        increments = Coordinate_compensation
        modified_list = [val + inc for val, inc in zip(line_list, increments)]
        Pose_after_adjustment = modified_list
        # print(Pose_after_adjustment)
        if (i == 0):
            suc, result, id = sendCMD(sock, "transparent_transmission_init", {
                "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0})
        send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list})
        time.sleep(sleep_time)
        if(i >= ex_turn_on_relay_start):
            turn_on_relay()
            print(f"打开喷枪8：{ex_turn_on_relay_start}")
        if(i >= ex_turn_off_relay_start):
            turn_off_relay()
            print(f"关闭喷枪8：{ex_turn_off_relay_start}")
        i = i + 1

    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    print("第八段程序执行完")


def program9(sock):
    start_time = time.time()
    i = 0
    increments = [0, 0, 0, 0, 0, 0]
    reference_pos = None  # 初始化参考位置
    new_joint_pos = None  # 初始化逆解得到的新关节位置
    pose_file = open('/home/raspberrypi/robot1/Pose_5.txt', "r")
    first_pose_line = pose_file.readline().strip()
    if first_pose_line:
        Path_First_Position = list(map(float, first_pose_line.split(',')))
        Modified_First_Position = [val + inc for val, inc in zip(Path_First_Position, increments)]
        target_pose = Modified_First_Position
    else:
        #print("Pose_5.txt为空，无法获取初始位姿")
        pose_file.close()
        return
    pose_file.close()

    file_name = '/home/raspberrypi/robot1/joint_positions_assembled_5.txt'
    fo = open(file_name, "r")
    while 1:
        # 读取一行内容
        line = fo.readline()
        if not line:
            break
        line_list = line.strip()
        line_list = list(map(float, line_list.split(',')))

        if reference_pos is None:
            reference_pos = line_list
            suc, angle, id = sendCMD(sock, "inverseKinematic",
                                     {"targetPose": target_pose, "referencePos": reference_pos})
            if suc and angle is not None:
                new_joint_pos = angle
            else:
                #print("逆解计算失败，使用原有关节位置")
                new_joint_pos = line_list  # 失败时使用原位置
        modified_list = new_joint_pos if new_joint_pos is not None else line_list
        if (i == 0):
            while True:
                suc, result, id = sendCMD(sock, "moveByJoint", {"targetPos": modified_list, "speed": 100})
                if suc:
                    break
                print("moveByJoint命令发送失败，重试...")
                time.sleep(0.1)  # 重试间隔，可根据需求调整
            wait_stop(sock)
        time.sleep(0.01)
        i = i + 1
    fo.close()
    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    end_time = time.time()
    print("第九段程序执行完")


def program10(sock):
    start_time = time.time()
    i = 0
    increments = [0, 0, 0, 0, 0, 0]
    reference_pos = None
    new_joint_pos = None

    pose_file = open('/home/raspberrypi/robot1/Pose_6.txt', "r")
    first_pose_line = pose_file.readline().strip()
    if first_pose_line:
        Path_First_Position = list(map(float, first_pose_line.split(',')))
        Modified_First_Position = [val + inc for val, inc in zip(Path_First_Position, increments)]
        target_pose = Modified_First_Position
    else:
        #print("Pose_6.txt为空，无法获取初始位姿")
        pose_file.close()
        return
    pose_file.close()

    file_name = '/home/raspberrypi/robot1/joint_positions_assembled_6.txt'
    fo = open(file_name, "r")
    while 1:
        line = fo.readline()
        if not line:
            break
        line_list = line.strip()
        line_list = list(map(float, line_list.split(',')))

        if reference_pos is None:
            reference_pos = line_list
            suc, angle, id = sendCMD(sock, "inverseKinematic",
                                     {"targetPose": target_pose, "referencePos": reference_pos})
            if suc and angle is not None:
                new_joint_pos = angle
            else:
                #print("逆解计算失败，使用原有关节位置")
                new_joint_pos = line_list  # 失败时使用原位置
        modified_list = new_joint_pos if new_joint_pos is not None else line_list

        if (i == 0):
            while True:
                suc, result, id = sendCMD(sock, "moveByJoint", {"targetPos": modified_list, "speed": 100})
                if suc:
                    break
                print("moveByJoint命令发送失败,重试...")
                time.sleep(0.1)  # 重试间隔，可根据需求调整
            wait_stop(sock)
        time.sleep(0.01)
        i = i + 1
    fo.close()
    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    end_time = time.time()
    print("第十段程序执行完")


def program11(sock):

    start_time = time.time()
    i = 0
    increments = [0, 0, 0, 0, 0, 0]
    reference_pos = None
    new_joint_pos = None
    pose_file = open('/home/raspberrypi/robot1/Pose_7.txt', "r")

    first_pose_line = pose_file.readline().strip()
    if first_pose_line:
        Path_First_Position = list(map(float, first_pose_line.split(',')))
        Modified_First_Position = [val + inc for val, inc in zip(Path_First_Position, increments)]
        target_pose = Modified_First_Position
    else:
        #print("Pose_7.txt为空，无法获取初始位姿")
        pose_file.close()
        return
    pose_file.close()
    file_name = '/home/raspberrypi/robot1/joint_positions_assembled_7.txt'
    fo = open(file_name, "r")

    while 1:
        line = fo.readline()
        if not line:
            break
        line_list = line.strip()
        line_list = list(map(float, line_list.split(',')))

        if reference_pos is None:
            reference_pos = line_list
            suc, angle, id = sendCMD(sock, "inverseKinematic",
                                     {"targetPose": target_pose, "referencePos": reference_pos})
            if suc and angle is not None:
                new_joint_pos = angle
            else:
                #print("逆解计算失败，使用原有关节位置")
                new_joint_pos = line_list  # 失败时使用原位置

        modified_list = new_joint_pos if new_joint_pos is not None else line_list

        if (i == 0):
            while True:
                suc, result, id = sendCMD(sock, "moveByJoint", {"targetPos": modified_list, "speed": 100})
                if suc:
                    break
                print("moveByJoint命令发送失败，重试...")
                time.sleep(0.1)  # 重试间隔，可根据需求调整
            wait_stop(sock)  # 等待机器人停止
        time.sleep(0.01)
        i = i + 1
    fo.close()
    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    end_time = time.time()
    print("第十一段程序执行完")

def program12(sock):
    start_time = time.time()
    i = 0
    increments = [0, 0, 0, 0, 0, 0]
    reference_pos = None
    new_joint_pos = None
    pose_file = open('/home/raspberrypi/robot1/Pose_8.txt', "r")
    first_pose_line = pose_file.readline().strip()
    if first_pose_line:
        Path_First_Position = list(map(float, first_pose_line.split(',')))
        Modified_First_Position = [val + inc for val, inc in zip(Path_First_Position, increments)]
        target_pose = Modified_First_Position
    else:
        #print("Pose_1.txt为空，无法获取初始位姿")
        pose_file.close()
        return
    pose_file.close()

    file_name = '/home/raspberrypi/robot1/joint_positions_assembled_8.txt'
    fo = open(file_name, "r")
    while 1:
        line = fo.readline()
        if not line:
            break
        line_list = line.strip()
        line_list = list(map(float, line_list.split(',')))

        if reference_pos is None:
            reference_pos = line_list
            suc, angle, id = sendCMD(sock, "inverseKinematic", {"targetPose": target_pose, "referencePos": reference_pos})
            if suc and angle is not None:
                new_joint_pos = angle
            else:
                #print("逆解计算失败，使用原有关节位置")
                new_joint_pos = line_list
        modified_list = new_joint_pos if new_joint_pos is not None else line_list
        if (i == 0):
            while True:
                suc, result, id = sendCMD(sock, "moveByJoint", {"targetPos": modified_list, "speed": 100})
                if suc:
                    break
                print("moveByJoint命令发送失败，重试...")
                time.sleep(0.1)  # 重试间隔，可根据需求调整
            wait_stop(sock)
        time.sleep(0.02)
        i = i + 1
    fo.close()
    suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
    end_time = time.time()
    print("第十二段程序执行完")


def program100(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay, delay1):
    global sample_time, lookahead_time
    lookahead_time = 400      # 前瞻时间 (ms)

    sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment)
    sleep_time = sample_time*0.001

    #0.点位平移操作
    # 1.假设 Off_Set_Value 已由外部传入（示例值）
    Actual_value_mm=133*sleep_time*80
    Comp_value_mm = 204.75 - Actual_value_mm
    Off_Set_Value = serial_handler.Trajectory_angle + Comp_value_mm# 示例值，实际由外部传入
    # 2.深拷贝原始轨迹（避免修改原数据）
    Pose_3_M = copy.deepcopy(Pose_3)
    Pose_4_M = copy.deepcopy(Pose_4)
    # 3.修改 Pose_3_M 和 Pose_4_M 的第12到144行（MATLAB索引12~144 => Python索引11~143）
    # 注意：共 133 行（144-12+1=133），但范围是 12:144 共133个点
    for i in range(11, 144):  # i = 11 ~ 143 对应 MATLAB 第12~144行
        factor = (i - 11) / 133.0  # MATLAB中 (i-12)/133，i从12开始，对应Python i=11时因子0
        Pose_3_M[i][2] = Pose_3[i][2] + Off_Set_Value * factor
        Pose_4_M[i][2] = Pose_4[i][2] + Off_Set_Value * factor
    # 修改第145行到末尾（MATLAB索引145:end => Python索引144:）
    for i in range(144, len(Pose_3_M)):
        Pose_3_M[i][2] = Pose_3[i][2] + Off_Set_Value
        Pose_4_M[i][2] = Pose_4[i][2] + Off_Set_Value
    # 4.处理 Pose_3_4 和 Pose_4_3
    num = len(Pose_3_4)  # 假设两者长度相同
    Pose_3_4_M = copy.deepcopy(Pose_3_4)
    Pose_4_3_M = copy.deepcopy(Pose_4_3)
    for i in range(num):
        # MATLAB 中 i 从 1 开始，公式： -Off_Set_Value/num*i + Off_Set_Value
        # Python 中 i 从 0 开始，所以因子为 (i+1)
        factor = (i + 1) / num
        Pose_3_4_M[i][2] = Pose_3_4[i][2] - Off_Set_Value * factor + Off_Set_Value
        Pose_4_3_M[i][2] = Pose_4_3[i][2] - Off_Set_Value * factor + Off_Set_Value

    Pose_3_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_3_M]
    Pose_4_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_4_M]
    Pose_3_4_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_3_4_M]
    Pose_4_3_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_4_3_M]

    #0.延时操作，点位延时
    #1.添加点位延时
    NUM = int(delay * 1000 / sample_time)
    NUM1 = int(delay1 * 1000 / sample_time)
    # 2.第一道喷涂延时点位
    last_point_3_4 = Pose_3_4_M[-1].copy()  # 先复制最后一个点位
    Pose_3_4_Stop = [last_point_3_4.copy() for _ in range(NUM)]  # 每个点位独立副本
    # 3.第2道喷涂延时点位
    last_point_4_3 = Pose_4_3_M[-1].copy()
    Pose_4_3_Stop = [last_point_4_3.copy() for _ in range(NUM1)]

    trajectories = [Pose_3_M, Pose_3_4_M,Pose_3_4_Stop, Pose_4_M, Pose_4_3_M,Pose_4_3_Stop]
    # ----- 一次性清理和初始化透传服务 -----
    sendCMD(sock, "tt_clear_servo_joint_buf")
    time.sleep(0.05)

    #0.喷枪开关控制,开喷枪时间最小为0，最大为155；Pose_4_3_M及Pose_3_4_M中各有20组数据
    #每道喷涂指令有155个点，
    # 第1道开枪时间在第10个点位（Pose_3_M的开枪点位 lookahead_time/sample_time+10），关枪时间在第130点位示例（lookahead_time/sample_time+135）
    # 第2道开枪时间在第15个点位（Pose_4_M的开枪点位 lookahead_time/sample_time+15），，关枪时间在第150点位示例（lookahead_time/sample_time+135，，，，如果数据>155，那么关枪就在Pose_4_3_M中）
    #队列，延时时间计算
    off_set_Time=int(lookahead_time / sample_time)
    # 1.喷枪1开关时间计算
    OPEN_1=turn_on_relay_start
    CLOSE_1=turn_off_relay_start
    OPEN_1_M = off_set_Time+OPEN_1
    CLOSE_1_M1S=off_set_Time+CLOSE_1
    CLOSE_1_M1E = off_set_Time + CLOSE_1-155
    #2.喷枪2开关时间计算
    OPEN_2=ex_turn_on_relay_start
    CLOSE_2 = ex_turn_off_relay_start
    OPEN_2_M = off_set_Time+OPEN_2
    CLOSE_2_M2S = off_set_Time+CLOSE_2
    CLOSE_2_M2E = off_set_Time + CLOSE_2- 155

    sendCMD(sock, "transparent_transmission_init", {"lookahead": lookahead_time,"t": sample_time,"smoothness": 1,"response_enable": 0 })
    cycles = 100  # 循环5次

    for _ in range(cycles):
        for traj in trajectories:
            print("开始执行轨迹...")
            # 针对不同轨迹进行计数
            if traj is Pose_3_M:
                for idx, point in enumerate(traj, start=1):
                    if idx == OPEN_1_M:
                        turn_on_relay()
                    if idx == CLOSE_1_M1S:
                        turn_off_relay()
                    send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point})
                    time.sleep(sleep_time)
            elif traj is Pose_3_4_M:
                for idx, point in enumerate(traj, start=1):
                    if idx == CLOSE_1_M1E:
                        turn_off_relay()
                    send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point})
                    time.sleep(sleep_time)
            elif traj is Pose_4_M:
                for idx, point in enumerate(traj, start=1):
                    if idx == OPEN_2_M:
                        turn_on_relay()
                    if idx == CLOSE_2_M2S:
                        turn_off_relay()
                    send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point})
                    time.sleep(sleep_time)
            elif traj is Pose_4_3_M:
                for idx, point in enumerate(traj, start=1):
                    if idx == CLOSE_2_M2E:
                        turn_off_relay()
                    send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point})
                    time.sleep(sleep_time)
            else:
                # 其他轨迹（如 Stop 轨迹）无需喷枪控制
                for point in traj:
                    send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point})
                    time.sleep(sleep_time)
                    print(f"停止轨迹，NUM = {NUM}, delay = {delay}")
                stop_cmd_bytes = read_cmd_from_shared(timeout=0.1)
                if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30':
                    print("收到0xAA 0x30,停止循环")
                    for i in range(3):
                        ser.write(b'\x80\x80')
                        ser.flush()
                        time.sleep(0.01)
                    time.sleep(0.5)
                    return False
            print("轨迹执行完成")

    return False

def program101(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay, delay1):
    global sample_time, lookahead_time
    lookahead_time = 400      # 前瞻时间 (ms)

    sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment)
    sleep_time = sample_time*0.001

    #0.点位平移操作
    # 1.假设 Off_Set_Value 已由外部传入（示例值）
    Actual_value_mm=133*sleep_time*80
    Comp_value_mm = 204.75 - Actual_value_mm
    Off_Set_Value = serial_handler.Trajectory_angle + Comp_value_mm # 示例值，实际由外部传入
    # 这里的喷涂方向与program100相反
    Off_Set_Value = -Off_Set_Value
    # 2.深拷贝原始轨迹（避免修改原数据）
    Pose_3_M = copy.deepcopy(Pose_1)
    Pose_4_M = copy.deepcopy(Pose_2)
    # 3.修改 Pose_3_M 和 Pose_4_M 的第12到144行（MATLAB索引12~144 => Python索引11~143）
    # 注意：共 133 行（144-12+1=133），但范围是 12:144 共133个点
    for i in range(11, 144):  # i = 11 ~ 143 对应 MATLAB 第12~144行
        factor = (i - 11) / 133.0  # MATLAB中 (i-12)/133，i从12开始，对应Python i=11时因子0
        Pose_3_M[i][2] = Pose_1[i][2] + Off_Set_Value * factor
        Pose_4_M[i][2] = Pose_2[i][2] + Off_Set_Value * factor
    # 修改第145行到末尾（MATLAB索引145:end => Python索引144:）
    for i in range(144, len(Pose_3_M)):
        Pose_3_M[i][2] = Pose_1[i][2] + Off_Set_Value
        Pose_4_M[i][2] = Pose_2[i][2] + Off_Set_Value
    # 4.处理 Pose_1_2 和 Pose_2_1
    num = len(Pose_1_2)  # 假设两者长度相同
    Pose_3_4_M = copy.deepcopy(Pose_1_2)
    Pose_4_3_M = copy.deepcopy(Pose_2_1)
    for i in range(num):
        # MATLAB 中 i 从 1 开始，公式： -Off_Set_Value/num*i + Off_Set_Value
        # Python 中 i 从 0 开始，所以因子为 (i+1)
        factor = (i + 1) / num
        Pose_3_4_M[i][2] = Pose_1_2[i][2] - Off_Set_Value * factor + Off_Set_Value
        Pose_4_3_M[i][2] = Pose_2_1[i][2] - Off_Set_Value * factor + Off_Set_Value

    Pose_3_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_3_M]
    Pose_4_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_4_M]
    Pose_3_4_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_3_4_M]
    Pose_4_3_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_4_3_M]

    #0.延时操作，点位延时
    #1.添加点位延时
    NUM = int(delay * 1000 / sample_time)
    NUM1 = int(delay1 * 1000 / sample_time)
    # 2.第一道喷涂延时点位
    last_point_3_4 = Pose_3_4_M[-1].copy()  # 先复制最后一个点位
    Pose_3_4_Stop = [last_point_3_4.copy() for _ in range(NUM)]  # 每个点位独立副本
    # 3.第2道喷涂延时点位
    last_point_4_3 = Pose_4_3_M[-1].copy()
    Pose_4_3_Stop = [last_point_4_3.copy() for _ in range(NUM1)]

    trajectories = [Pose_3_M, Pose_3_4_M,Pose_3_4_Stop, Pose_4_M, Pose_4_3_M,Pose_4_3_Stop]
    # ----- 一次性清理和初始化透传服务 -----
    sendCMD(sock, "tt_clear_servo_joint_buf")
    time.sleep(0.05)

    #0.喷枪开关控制,开喷枪时间最小为0，最大为155；Pose_4_3_M及Pose_3_4_M中各有20组数据
    #每道喷涂指令有155个点，
    # 第1道开枪时间在第10个点位（Pose_3_M的开枪点位 lookahead_time/sample_time+10），关枪时间在第130点位示例（lookahead_time/sample_time+135）
    # 第2道开枪时间在第15个点位（Pose_4_M的开枪点位 lookahead_time/sample_time+15），，关枪时间在第150点位示例（lookahead_time/sample_time+135，，，，如果数据>155，那么关枪就在Pose_4_3_M中）
    #队列，延时时间计算
    off_set_Time=int(lookahead_time / sample_time)
    # 1.喷枪1开关时间计算
    OPEN_1=turn_on_relay_start
    CLOSE_1=turn_off_relay_start
    OPEN_1_M = off_set_Time+OPEN_1
    CLOSE_1_M1S=off_set_Time+CLOSE_1
    CLOSE_1_M1E = off_set_Time + CLOSE_1-155
    #2.喷枪2开关时间计算
    OPEN_2=ex_turn_on_relay_start
    CLOSE_2 = ex_turn_off_relay_start
    OPEN_2_M = off_set_Time+OPEN_2
    CLOSE_2_M2S = off_set_Time+CLOSE_2
    CLOSE_2_M2E = off_set_Time + CLOSE_2- 155

    sendCMD(sock, "transparent_transmission_init", {"lookahead": lookahead_time,"t": sample_time,"smoothness": 1,"response_enable": 0 })
    cycles = 100  # 循环5次s

    for _ in range(cycles):
        for traj in trajectories:
            print("开始执行轨迹...")
            # 针对不同轨迹进行计数
            if traj is Pose_3_M:
                for idx, point in enumerate(traj, start=1):
                    if idx == OPEN_1_M:
                        turn_on_relay()
                    if idx == CLOSE_1_M1S:
                        turn_off_relay()
                    send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point})
                    time.sleep(sleep_time)
            elif traj is Pose_3_4_M:
                for idx, point in enumerate(traj, start=1):
                    if idx == CLOSE_1_M1E:
                        turn_off_relay()
                    send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point})
                    time.sleep(sleep_time)
            elif traj is Pose_4_M:
                for idx, point in enumerate(traj, start=1):
                    if idx == OPEN_2_M:
                        turn_on_relay()
                    if idx == CLOSE_2_M2S:
                        turn_off_relay()
                    send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point})
                    time.sleep(sleep_time)
            elif traj is Pose_4_3_M:
                for idx, point in enumerate(traj, start=1):
                    if idx == CLOSE_2_M2E:
                        turn_off_relay()
                    send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point})
                    time.sleep(sleep_time)
            else:
                # 其他轨迹（如 Stop 轨迹）无需喷枪控制
                for point in traj:
                    send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point})
                    time.sleep(sleep_time)
                    print(f"停止轨迹，NUM = {NUM}, delay = {delay}")
                stop_cmd_bytes = read_cmd_from_shared(timeout=0.1)
                if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30':
                    print("收到0xAA 0x30,停止循环")
                    for i in range(3):
                        ser.write(b'\x80\x80')
                        ser.flush()
                        time.sleep(0.01)
                    time.sleep(0.5)
                    return False
            print("轨迹执行完成")

    return False