From 280a7516f612dfa309b62ab329786794e4b9de50 Mon Sep 17 00:00:00 2001
From: "HJB\\13752" <13752551070@163.com>
Date: Mon, 23 Mar 2026 10:50:11 +0800
Subject: [PATCH] =?UTF-8?q?=E5=85=8B=E8=8E=B1=E5=88=86=E6=9C=BA=E6=A2=B0?=
=?UTF-8?q?=E8=87=82=E4=BB=A3=E7=A0=81?=
MIME-Version: 1.0
Content-Type: text/plain; charset=UTF-8
Content-Transfer-Encoding: 8bit
---
kelaifen/Kelaifen_V6.0/.idea/.gitignore | 3 +
kelaifen/Kelaifen_V6.0/.idea/.name | 1 +
.../inspectionProfiles/profiles_settings.xml | 6 +
kelaifen/Kelaifen_V6.0/.idea/misc.xml | 4 +
kelaifen/Kelaifen_V6.0/.idea/modules.xml | 8 +
.../树莓派上完整程序_新增功能.iml | 8 +
kelaifen/Kelaifen_V6.0/.vscode/settings.json | 4 +
kelaifen/Kelaifen_V6.0/ETController.py | 57 +
kelaifen/Kelaifen_V6.0/Example_0_5.py | 402 ++++++++
kelaifen/Kelaifen_V6.0/Pose_5.txt | 1 +
kelaifen/Kelaifen_V6.0/Pose_6.txt | 1 +
kelaifen/Kelaifen_V6.0/Pose_7.txt | 1 +
kelaifen/Kelaifen_V6.0/Pose_8.txt | 1 +
kelaifen/Kelaifen_V6.0/config.json | 280 +++++
kelaifen/Kelaifen_V6.0/gpio.py | 37 +
.../joint_positions_assembled_5.txt | 1 +
.../joint_positions_assembled_6.txt | 1 +
.../joint_positions_assembled_7.txt | 1 +
.../joint_positions_assembled_8.txt | 1 +
kelaifen/Kelaifen_V6.0/program_config.py | 78 ++
kelaifen/Kelaifen_V6.0/readme.txt | 19 +
kelaifen/Kelaifen_V6.0/serial_handler.py | 151 +++
kelaifen/Kelaifen_V6.0/serial_init.py | 27 +
kelaifen/Kelaifen_V6.0/trajectory_data.py | 634 ++++++++++++
kelaifen/Kelaifen_V6.0/trajectory_program.py | 976 ++++++++++++++++++
kelaifen/Kelaifen_V6.0/transform.py | 25 +
26 files changed, 2728 insertions(+)
create mode 100644 kelaifen/Kelaifen_V6.0/.idea/.gitignore
create mode 100644 kelaifen/Kelaifen_V6.0/.idea/.name
create mode 100644 kelaifen/Kelaifen_V6.0/.idea/inspectionProfiles/profiles_settings.xml
create mode 100644 kelaifen/Kelaifen_V6.0/.idea/misc.xml
create mode 100644 kelaifen/Kelaifen_V6.0/.idea/modules.xml
create mode 100644 kelaifen/Kelaifen_V6.0/.idea/树莓派上完整程序_新增功能.iml
create mode 100644 kelaifen/Kelaifen_V6.0/.vscode/settings.json
create mode 100644 kelaifen/Kelaifen_V6.0/ETController.py
create mode 100644 kelaifen/Kelaifen_V6.0/Example_0_5.py
create mode 100644 kelaifen/Kelaifen_V6.0/Pose_5.txt
create mode 100644 kelaifen/Kelaifen_V6.0/Pose_6.txt
create mode 100644 kelaifen/Kelaifen_V6.0/Pose_7.txt
create mode 100644 kelaifen/Kelaifen_V6.0/Pose_8.txt
create mode 100644 kelaifen/Kelaifen_V6.0/config.json
create mode 100644 kelaifen/Kelaifen_V6.0/gpio.py
create mode 100644 kelaifen/Kelaifen_V6.0/joint_positions_assembled_5.txt
create mode 100644 kelaifen/Kelaifen_V6.0/joint_positions_assembled_6.txt
create mode 100644 kelaifen/Kelaifen_V6.0/joint_positions_assembled_7.txt
create mode 100644 kelaifen/Kelaifen_V6.0/joint_positions_assembled_8.txt
create mode 100644 kelaifen/Kelaifen_V6.0/program_config.py
create mode 100644 kelaifen/Kelaifen_V6.0/readme.txt
create mode 100644 kelaifen/Kelaifen_V6.0/serial_handler.py
create mode 100644 kelaifen/Kelaifen_V6.0/serial_init.py
create mode 100644 kelaifen/Kelaifen_V6.0/trajectory_data.py
create mode 100644 kelaifen/Kelaifen_V6.0/trajectory_program.py
create mode 100644 kelaifen/Kelaifen_V6.0/transform.py
diff --git a/kelaifen/Kelaifen_V6.0/.idea/.gitignore b/kelaifen/Kelaifen_V6.0/.idea/.gitignore
new file mode 100644
index 0000000..359bb53
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/.idea/.gitignore
@@ -0,0 +1,3 @@
+# 默认忽略的文件
+/shelf/
+/workspace.xml
diff --git a/kelaifen/Kelaifen_V6.0/.idea/.name b/kelaifen/Kelaifen_V6.0/.idea/.name
new file mode 100644
index 0000000..d66c122
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/.idea/.name
@@ -0,0 +1 @@
+Example_0_5.py
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/.idea/inspectionProfiles/profiles_settings.xml b/kelaifen/Kelaifen_V6.0/.idea/inspectionProfiles/profiles_settings.xml
new file mode 100644
index 0000000..105ce2d
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/.idea/inspectionProfiles/profiles_settings.xml
@@ -0,0 +1,6 @@
+
+
+
+
+
+
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/.idea/misc.xml b/kelaifen/Kelaifen_V6.0/.idea/misc.xml
new file mode 100644
index 0000000..060d2c5
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/.idea/misc.xml
@@ -0,0 +1,4 @@
+
+
+
+
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/.idea/modules.xml b/kelaifen/Kelaifen_V6.0/.idea/modules.xml
new file mode 100644
index 0000000..b21e480
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/.idea/modules.xml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/.idea/树莓派上完整程序_新增功能.iml b/kelaifen/Kelaifen_V6.0/.idea/树莓派上完整程序_新增功能.iml
new file mode 100644
index 0000000..f571432
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/.idea/树莓派上完整程序_新增功能.iml
@@ -0,0 +1,8 @@
+
+
+
+
+
+
+
+
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/.vscode/settings.json b/kelaifen/Kelaifen_V6.0/.vscode/settings.json
new file mode 100644
index 0000000..62f621e
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/.vscode/settings.json
@@ -0,0 +1,4 @@
+{
+ "python.analysis.autoImportCompletions": true,
+ "python.analysis.typeCheckingMode": "basic"
+}
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/ETController.py b/kelaifen/Kelaifen_V6.0/ETController.py
new file mode 100644
index 0000000..04e20e9
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/ETController.py
@@ -0,0 +1,57 @@
+import json
+import socket
+import time
+
+# 连接机器人控制器
+def connectETController(ip, port=8055):
+ sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
+ try:
+ sock.connect((ip, port))
+ return (True, sock)
+ except Exception as e:
+ sock.close()
+ return (False,)
+
+# 发送命令并接收响应
+def sendCMD(sock, cmd, params=None, id=1):
+ if not params:
+ params = []
+ else:
+ params = json.dumps(params)
+ sendStr = '{"method":"%s","params":%s,"jsonrpc":"2.0","id":%d}' % (cmd, params, id) + "\n"
+ try:
+ sock.sendall(bytes(sendStr, "utf-8"))
+ ret = sock.recv(1024)
+ jdata = json.loads(str(ret, "utf-8"))
+ if "result" in jdata.keys():
+ return (True, json.loads(jdata["result"]), jdata["id"])
+ elif "error" in jdata.keys():
+ return (False, jdata["error"], jdata["id"])
+ else:
+ return (False, None, None)
+ except Exception as e:
+ return (False, None, None)
+
+
+def send_Point(sock, cmd, params=None, id=1):
+ if (not params):
+ params = []
+ else:
+ params = json.dumps(params)
+ sendStr = "{{\"method\":\"{0}\",\"params\":{1},\"jsonrpc\":\"2.0\",\"id\":{2}}}".format(cmd, params, id) + "\n"
+ sock.sendall(bytes(sendStr, "utf-8"))
+
+
+def wait_stop(sock): # 修正：添加sock参数
+ while True:
+ time.sleep(0.1)
+ ret1, result1, id1 = sendCMD(sock, "getRobotState") # getRobotstate
+ if (ret1):
+ if result1 == 0 or result1 == 4:
+ break
+ else:
+ #print("getRobotState failed")
+ continue
+
+
+
diff --git a/kelaifen/Kelaifen_V6.0/Example_0_5.py b/kelaifen/Kelaifen_V6.0/Example_0_5.py
new file mode 100644
index 0000000..e81fa07
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/Example_0_5.py
@@ -0,0 +1,402 @@
+import time
+import serial
+from serial import Serial
+import threading
+from ETController import connectETController,sendCMD,send_Point,wait_stop
+from gpio import init_gpio, turn_off_relay, turn_on_relay
+from program_config import load_process_config, program_switch,sequence_1, sequence_2, sequence_3, sequence_4
+from serial_init import serial_init
+from serial_handler import handle_special_commands, read_cmd_from_shared, serial_receive_data
+
+if __name__ == "__main__":
+
+ # 加载配置文件并提取参数
+ process_config = load_process_config()
+ if process_config:
+ # 提取预设参数列表（现在PRESET_PARAMS本身包含所有子配置）
+ PRESET_PARAMS = process_config["PRESET_PARAMS"]
+
+ init_gpio()
+ ser = serial_init()
+ # 检查串口初始化结果
+ if not ser or not isinstance(ser, serial.Serial) or not ser.is_open:
+ print("串口初始化失败，无法继续运行")
+ # 确保在函数内部返回
+ exit()
+
+ time.sleep(20)
+ # 默认关闭喷枪
+ turn_off_relay()
+
+ # 机器人IP地址
+ robot_ip = "192.168.1.100"
+ result = connectETController(robot_ip)
+ sock = None
+ if len(result) == 2:
+ conSuc, sock = result
+ if conSuc:
+ print("成功连接到机器人")
+ # 1.上电设置
+ ret, result, id = sendCMD(sock, "set_robot_power_status", {"status": 1})
+ print("开始上电")
+ time.sleep(20)
+ print("开始检测上电状态")
+ ret, result, id = sendCMD(sock, "get_robot_power_status")
+
+ if result == 0:
+ ret, result, id = sendCMD(sock, "set_robot_power_status", {"status": 1})
+ time.sleep(20)
+ else :
+ print("已上电")
+ # 2. 清除报警
+ print("清除报警")
+ ret, result, id = sendCMD(sock,"clearAlarm")
+ print("清除报警返回值")
+ print(result)
+ time.sleep(5)
+ print("报警清除完成")
+
+ # 获取同步状态
+ suc, result , id = sendCMD(sock, "getMotorStatus")
+ if result == 0:
+ # 同步伺服编码器数据
+ suc,result,id = sendCMD(sock, "syncMotorStatus")
+ if result:
+ print("同步伺服编码器数据成功")
+ else:
+ print("同步伺服编码器数据失败")
+ time. sleep (2)
+
+ # 3. 编码器零位校准
+ suc, result, id = sendCMD(sock, "getServoStatus")
+ print("获取伺服状态返回值")
+ print(result)
+ time.sleep(2)
+ if result == 0:
+ # 设置机械臂伺服状态为“开”（status=1）
+ suc, result, id = sendCMD(sock, "set_servo_status", {"status": 1})
+ print("设置机械臂伺服状态返回值")
+ print(result)
+ time.sleep(5)
+ suc, result, id = sendCMD(sock, "calibrate_encoder_zero_position")
+ print("编码器校准返回值")
+ print(result)
+ time.sleep(5)
+ if result == 1:
+ print("编码器校准成功")
+ else:
+ print("编码器校准失败")
+
+ # 初始化透传服务
+ suc, result, id = sendCMD(sock, "get_transparent_transmission_state")
+ if id == 1:
+ print(suc, result, id)
+ if result == 1:
+ suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
+ print("已清空透传缓存")
+ time.sleep(0.5)
+ else:
+ print("无缓存")
+
+ # 在此处做检测等相关操作
+ suc, result, id = sendCMD(sock, "getRobotState")
+ if (result == 4):
+ # 清除报警
+ suc, result, id = sendCMD(sock, "clearAlarm")
+ time.sleep(0.5)
+
+ # 获取同步状态
+ suc, result, id = sendCMD(sock, "getMotorStatus")
+ if (result == 0):
+ # 同步伺服编码器数据
+ suc, result, id = sendCMD(sock, "syncMotorStatus")
+ time.sleep(0.5)
+ else:
+ print("同步状态获取失败")
+
+ # 获取机械臂伺服状态
+ suc, result, id = sendCMD(sock, "getServoStatus")
+ if (result == 0):
+ # 设置机械臂伺服状态ON
+ suc, result, id = sendCMD(sock, "set_servo_status", {"status": 1})
+ time.sleep(1)
+ else:
+ print("机械臂伺服取失败")
+
+ # 获取当前机器人是否处于透传状态
+ suc, result, id = sendCMD(sock, "get_transparent_transmission_state")
+ print(result)
+ if (result == 1):
+ # 清空透传缓存
+ suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
+ time.sleep(0.5)
+ else:
+ print("非缓存状态")
+
+ # 启动串口接收线程（daemon=True：主程序退出时线程自动终止）
+ receive_thread = threading.Thread(target=serial_receive_data, args=(ser,), daemon=True)
+ receive_thread.start()
+ print("串口接收线程已启动")
+
+ delay_time_10_13579_30 = 2
+
+ # 外层主循环
+ while True:
+ try:
+ # 等待主指令（从共享缓冲区读取）
+ print("\n等待串口指令...")
+ main_cmd_bytes = None
+ while main_cmd_bytes is None:
+ # 喷漆工艺参数配置选择（速度、角度、搭接时间）
+ selected_delay_map, selected_relay_config = handle_special_commands(PRESET_PARAMS)
+ if selected_delay_map is not None and selected_relay_config is not None:
+ # 更新delay配置
+ program_delay_map = selected_delay_map
+ # 更新继电器参数
+ turn_on_relay_start = selected_relay_config["turn_on_relay_start"]
+ turn_on_relay_end = selected_relay_config["turn_on_relay_end"]
+ turn_off_relay_start = selected_relay_config["turn_off_relay_start"]
+ turn_off_relay_end = selected_relay_config["turn_off_relay_end"]
+
+ print(f"\n✅ 参数已更新为选中配置：")
+ print(f"program_delay_map: {program_delay_map}")
+ print(f"继电器开启区间：{turn_on_relay_start} ~ {turn_on_relay_end}")
+ print(f"继电器关闭区间：{turn_off_relay_start} ~ {turn_off_relay_end}")
+ # 喷漆开始命令接收函数
+ main_cmd_bytes = read_cmd_from_shared(timeout=1.0)
+ # 解析主指令（帧头后第一字节为指令值）
+ cmd = main_cmd_bytes[1]
+ print(f"收到串口指令: 0x{cmd:02X}")
+
+ # region 情况1
+ # 情况1: 接收到1
+ if cmd == 1:
+ print("触发情况1: 执行program1")
+ program_switch[1](sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, turn_off_relay_end)
+ time.sleep(delay_time_10_13579_30)
+
+ for i in range(3):
+ ser.write(b'\x80\x80')
+ ser.flush()
+ time.sleep(0.01)
+ print("执行program1结束,已发送响应0x80 0x80")
+
+ # 等待子指令接收
+ sub_cmd_bytes = None
+ while sub_cmd_bytes is None:
+ sub_cmd_bytes = read_cmd_from_shared(timeout=1.0)
+ time.sleep(0.01)
+ sub_cmd = sub_cmd_bytes[0] # 应为0xAA
+ sub_cmd1 = sub_cmd_bytes[1] # 子指令值（如0x20、0x0a）
+ print(f"收到子指令: 0x{sub_cmd:02X} 0x{sub_cmd1:02X}")
+
+ if sub_cmd == 0xAA:
+ if sub_cmd1 == 0x20:
+ print("开始情况1循环序列: program2->300->4->100...")
+ current_index = 0
+ running = True
+ while running:
+ # 执行当前序列中的程序
+ current_program = sequence_1[current_index]
+ #print(f"执行序列程序: {current_program}")
+
+ # 读取可能的停止指令（超时0.1秒，不阻塞循环）
+ 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)
+ running = False
+ time.sleep(0.5)
+ else:
+ program_switch[current_program](sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, turn_off_relay_end)
+ # print(current_program)
+
+ # 按程序编号从字典获取延时，精准执行
+ delay = program_delay_map.get(current_program, 0.0)
+ if delay > 0:
+ print(f"program{current_program}执行完成，延时{delay}秒...")
+ time.sleep(delay)
+ else:
+ print(f"当前执行程序是: {current_program}（无指定延时）")
+
+ # 移动到下一个程序索引
+ current_index = (current_index + 1) % len(sequence_1) # 注意这里要换成当前使用的 sequence
+ # time.sleep(0.05)
+ # 回到洗枪位置指令
+ elif sub_cmd1 == 0x0a:
+ print(f"接收到指令0xAA 0x{sub_cmd1:02X}，切换执行对应程序")
+ program_switch[10](sock)
+ time.sleep(delay_time_10_13579_30)
+ for i in range(3):
+ ser.write(b'\x80\x80')
+ ser.flush()
+ time.sleep(0.01)
+ print("执行program10结束,已发送响应0x80 0x80")
+ print("返回最外层等待串口数据")
+
+ else:
+ print(f"收到未知子指令: 0x{sub_cmd:02X}，继续等待")
+ #endregion
+ # region 情况3
+ # 情况3: 接收到5
+ elif cmd == 5:
+ print("触发情况3: 执行program5")
+ program_switch[5](sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, turn_off_relay_end)
+
+ time.sleep(delay_time_10_13579_30)
+ for i in range(3):
+ ser.write(b'\x80\x80')
+ ser.flush()
+ time.sleep(0.01)
+ print("执行program5结束,已发送响应0x80 0x80")
+
+ # 子指令接收（调用read_cmd_from_shared）
+ print("等待0x20开始循环或其他指令...")
+ sub_cmd_bytes = None
+ while sub_cmd_bytes is None:
+ sub_cmd_bytes = read_cmd_from_shared(timeout=1.0)
+ time.sleep(0.01)
+ sub_cmd = sub_cmd_bytes[0]
+ sub_cmd1 = sub_cmd_bytes[1]
+ print(f"收到子指令: 0x{sub_cmd:02X}")
+
+ if sub_cmd == 0xAA:
+ if sub_cmd1 == 0x20:
+ print("开始情况3循环序列: program6->700->8->500...")
+ current_index = 0
+ running = True
+ while running:
+ current_program = sequence_3[current_index]
+ print(f"执行序列程序: {current_program}")
+
+ # 读取可能的停止指令（超时0.1秒，不阻塞循环）
+ 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)
+ running = False
+ time.sleep(0.5)
+ else:
+ program_switch[current_program](sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, turn_off_relay_end)
+
+ # 统一字典映射延时
+ delay = program_delay_map.get(current_program, 0.0)
+ if delay > 0:
+ print(f"program{current_program}执行完成，延时{delay}秒...")
+ time.sleep(delay)
+ else:
+ print(f"当前执行程序是: {current_program}（无指定延时）")
+
+ # 移动到下一个程序索引
+ current_index = (current_index + 1) % len(sequence_3) # 注意这里要换成当前使用的 sequence
+ # time.sleep(0.05)
+ #回到洗枪位置指令
+ elif sub_cmd1 == 0x0a:
+ print(f"接收到指令0xAA 0x{sub_cmd1:02X}，切换执行对应程序")
+ program_switch[10](sock)
+ time.sleep(delay_time_10_13579_30)
+ for i in range(3):
+ ser.write(b'\x80\x80')
+ ser.flush()
+ time.sleep(0.01)
+ print("执行program10结束,已发送响应0x80 0x80")
+ print("返回最外层等待串口数据")
+ else:
+ print(f"收到未知子指令: 0x{sub_cmd:02X}，继续等待")
+ #endregion
+
+ # region 程序9，返回停机位置
+ elif cmd == 9:
+ print("执行program9,返回停机位置")
+ program_switch[9](sock)
+ time.sleep(delay_time_10_13579_30)
+ for i in range(3):
+ ser.write(b'\x80\x80')
+ ser.flush()
+ time.sleep(0.1)
+ print("执行program9结束,已发送响应0x80 0x80")
+ print("返回最外层等待串口数据")
+ #endregion
+
+ # region 程序10，前往初始位置
+ elif cmd == 10:
+ print("触发情况6: 执行program10")
+ program_switch[10](sock)
+ time.sleep(delay_time_10_13579_30)
+ for i in range(3):
+ ser.write(b'\x80\x80')
+ ser.flush()
+ time.sleep(0.1)
+ print("执行program10结束,已发送响应0x80 0x80")
+ print("返回最外层等待串口数据")
+ # endregion
+
+ # region 程序11，前往洗枪位置
+ elif cmd == 11:
+ print("执行program11,前往洗枪位置")
+ program_switch[11](sock)
+ time.sleep(delay_time_10_13579_30)
+ for i in range(3):
+ ser.write(b'\x80\x80')
+ ser.flush()
+ time.sleep(0.1)
+ print("执行program11结束,已发送响应0x80 0x80")
+ print("返回最外层等待串口数据")
+ #endregion
+
+ # region 程序12，前往试枪位置
+ elif cmd == 12:
+ print("执行program12，前往试枪位置")
+ program_switch[12](sock)
+ time.sleep(delay_time_10_13579_30)
+ for i in range(3):
+ ser.write(b'\x80\x80')
+ ser.flush()
+ time.sleep(0.1)
+ print("执行program12结束,已发送响应0x80 0x80")
+ print("返回最外层等待串口数据")
+ #endregion
+
+ # 退出指令
+ elif cmd == 0x00:
+ print("收到退出指令，程序终止")
+ break
+ # 开喷枪
+ elif cmd == 16:
+ turn_on_relay()
+ print("打开喷枪")
+ # 关喷枪
+ elif cmd == 17:
+ turn_off_relay()
+ print("关闭喷枪")
+ # 未知指令
+ else:
+ print(f"未知指令: 0x{cmd:02X}，等待下一条指令...")
+ time.sleep(0.01)
+
+ except KeyboardInterrupt:
+ print("程序被手动中断")
+ break
+ except Exception as e:
+ print(f"主循环错误: {e}")
+ time.sleep(0.5)
+ else:
+ print("连接机器人失败")
+ else:
+ print("无法连接到机器人")
+
+
+
+ # 资源清理
+ if sock:
+ sock.close()
+ if ser and isinstance(ser, serial.Serial) and ser.is_open:
+ ser.close()
+ print("串口已关闭")
+
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/Pose_5.txt b/kelaifen/Kelaifen_V6.0/Pose_5.txt
new file mode 100644
index 0000000..a3987f9
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/Pose_5.txt
@@ -0,0 +1 @@
+318.173,-94.776,446.581,4.743102,-0.066481,-4.024368
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/Pose_6.txt b/kelaifen/Kelaifen_V6.0/Pose_6.txt
new file mode 100644
index 0000000..bba19d9
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/Pose_6.txt
@@ -0,0 +1 @@
+318.901,437.099,583.004,4.685241,-0.089038,-2.083531
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/Pose_7.txt b/kelaifen/Kelaifen_V6.0/Pose_7.txt
new file mode 100644
index 0000000..9a98d6d
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/Pose_7.txt
@@ -0,0 +1 @@
+132.209,693.315,699.278,4.182,0.075,-0.740
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/Pose_8.txt b/kelaifen/Kelaifen_V6.0/Pose_8.txt
new file mode 100644
index 0000000..d4cd6e0
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/Pose_8.txt
@@ -0,0 +1 @@
+663.317,241.195,699.278,4.182,0.075,2.306
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/config.json b/kelaifen/Kelaifen_V6.0/config.json
new file mode 100644
index 0000000..ee145ba
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/config.json
@@ -0,0 +1,280 @@
+{
+ "PRESET_PARAMS": [
+ {
+ "params": [50, -100],
+ "program_delay_map": {
+ "1": 1.0,
+ "100": 0.1,
+ "2": 0.1,
+ "3": 1.0,
+ "300": 0.1,
+ "4": 0.1,
+ "5": 1.0,
+ "500": 0.1,
+ "6": 0.5,
+ "7": 1.0,
+ "700": 0.1,
+ "8": 0.5
+ },
+ "relay_config": {
+ "turn_on_relay_start": 20,
+ "turn_on_relay_end": 25,
+ "turn_off_relay_start": 135,
+ "turn_off_relay_end": 140
+ }
+ },
+ {
+ "params": [50, -50],
+ "program_delay_map": {
+ "1": 1.0,
+ "100": 0.1,
+ "2": 0.1,
+ "3": 1.0,
+ "300": 0.1,
+ "4": 0.1,
+ "5": 1.0,
+ "500": 0.1,
+ "6": 0.5,
+ "7": 1.0,
+ "700": 0.1,
+ "8": 0.5
+ },
+ "relay_config": {
+ "turn_on_relay_start": 20,
+ "turn_on_relay_end": 25,
+ "turn_off_relay_start": 135,
+ "turn_off_relay_end": 140
+ }
+ },
+ {
+ "params": [50, 0],
+ "program_delay_map": {
+ "1": 1.0,
+ "100": 0.1,
+ "2": 0.1,
+ "3": 1.0,
+ "300": 0.1,
+ "4": 0.1,
+ "5": 1.0,
+ "500": 0.1,
+ "6": 0.5,
+ "7": 1.0,
+ "700": 0.1,
+ "8": 0.5
+ },
+ "relay_config": {
+ "turn_on_relay_start": 20,
+ "turn_on_relay_end": 25,
+ "turn_off_relay_start": 135,
+ "turn_off_relay_end": 140
+ }
+ },
+ {
+ "params": [50, 50],
+ "program_delay_map": {
+ "1": 1.0,
+ "100": 0.1,
+ "2": 0.1,
+ "3": 1.0,
+ "300": 0.1,
+ "4": 0.1,
+ "5": 1.0,
+ "500": 0.1,
+ "6": 0.5,
+ "7": 1.0,
+ "700": 0.1,
+ "8": 0.5
+ },
+ "relay_config": {
+ "turn_on_relay_start": 20,
+ "turn_on_relay_end": 25,
+ "turn_off_relay_start": 135,
+ "turn_off_relay_end": 140
+ }
+ },
+ {
+ "params": [50, 100],
+ "program_delay_map": {
+ "1": 1.0,
+ "100": 0.1,
+ "2": 0.1,
+ "3": 1.0,
+ "300": 0.1,
+ "4": 0.1,
+ "5": 1.0,
+ "500": 0.1,
+ "6": 0.5,
+ "7": 1.0,
+ "700": 0.1,
+ "8": 0.5
+ },
+ "relay_config": {
+ "turn_on_relay_start": 20,
+ "turn_on_relay_end": 25,
+ "turn_off_relay_start": 135,
+ "turn_off_relay_end": 140
+ }
+ },
+ {
+ "params": [100, -100],
+ "program_delay_map": {
+ "1": 1.0,
+ "100": 0.1,
+ "2": 0.1,
+ "3": 1.0,
+ "300": 0.1,
+ "4": 0.1,
+ "5": 1.0,
+ "500": 0.1,
+ "6": 0.5,
+ "7": 1.0,
+ "700": 0.1,
+ "8": 0.5
+ },
+ "relay_config": {
+ "turn_on_relay_start": 20,
+ "turn_on_relay_end": 25,
+ "turn_off_relay_start": 135,
+ "turn_off_relay_end": 140
+ }
+ },
+ {
+ "params": [100, -50],
+ "program_delay_map": {
+ "1": 1.0,
+ "100": 0.1,
+ "2": 0.1,
+ "3": 1.0,
+ "300": 0.1,
+ "4": 0.1,
+ "5": 1.0,
+ "500": 0.1,
+ "6": 0.5,
+ "7": 1.0,
+ "700": 0.1,
+ "8": 0.5
+ },
+ "relay_config": {
+ "turn_on_relay_start": 20,
+ "turn_on_relay_end": 25,
+ "turn_off_relay_start": 135,
+ "turn_off_relay_end": 140
+ }
+ },
+ {
+ "params": [100, 0],
+ "program_delay_map": {
+ "1": 1.0,
+ "100": 0.1,
+ "2": 0.1,
+ "3": 1.0,
+ "300": 0.1,
+ "4": 0.1,
+ "5": 1.0,
+ "500": 0.1,
+ "6": 0.5,
+ "7": 1.0,
+ "700": 0.1,
+ "8": 0.5
+ },
+ "relay_config": {
+ "turn_on_relay_start": 20,
+ "turn_on_relay_end": 25,
+ "turn_off_relay_start": 135,
+ "turn_off_relay_end": 140
+ }
+ },
+ {
+ "params": [100, 50],
+ "program_delay_map": {
+ "1": 1.0,
+ "100": 0.1,
+ "2": 0.1,
+ "3": 1.0,
+ "300": 0.1,
+ "4": 0.1,
+ "5": 1.0,
+ "500": 0.1,
+ "6": 0.5,
+ "7": 1.0,
+ "700": 0.1,
+ "8": 0.5
+ },
+ "relay_config": {
+ "turn_on_relay_start": 20,
+ "turn_on_relay_end": 25,
+ "turn_off_relay_start": 135,
+ "turn_off_relay_end": 140
+ }
+ },
+ {
+ "params": [100, 100],
+ "program_delay_map": {
+ "1": 1.0,
+ "100": 0.1,
+ "2": 0.1,
+ "3": 1.0,
+ "300": 0.1,
+ "4": 0.1,
+ "5": 1.0,
+ "500": 0.1,
+ "6": 0.5,
+ "7": 1.0,
+ "700": 0.1,
+ "8": 0.5
+ },
+ "relay_config": {
+ "turn_on_relay_start": 20,
+ "turn_on_relay_end": 25,
+ "turn_off_relay_start": 135,
+ "turn_off_relay_end": 140
+ }
+ },
+ {
+ "params": [90, 50],
+ "program_delay_map": {
+ "1": 1.0,
+ "100": 0.1,
+ "2": 0.1,
+ "3": 1.0,
+ "300": 0.1,
+ "4": 0.1,
+ "5": 1.0,
+ "500": 0.1,
+ "6": 0.5,
+ "7": 1.0,
+ "700": 0.1,
+ "8": 0.5
+ },
+ "relay_config": {
+ "turn_on_relay_start": 20,
+ "turn_on_relay_end": 25,
+ "turn_off_relay_start": 135,
+ "turn_off_relay_end": 140
+ }
+ },
+ {
+ "params": [90, 150],
+ "program_delay_map": {
+ "1": 1.0,
+ "100": 0.1,
+ "2": 0.1,
+ "3": 1.0,
+ "300": 0.1,
+ "4": 0.1,
+ "5": 1.0,
+ "500": 0.1,
+ "6": 0.5,
+ "7": 1.0,
+ "700": 0.1,
+ "8": 0.5
+ },
+ "relay_config": {
+ "turn_on_relay_start": 20,
+ "turn_on_relay_end": 25,
+ "turn_off_relay_start": 135,
+ "turn_off_relay_end": 140
+ }
+ }
+ ]
+}
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/gpio.py b/kelaifen/Kelaifen_V6.0/gpio.py
new file mode 100644
index 0000000..8183be7
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/gpio.py
@@ -0,0 +1,37 @@
+# region 继电器
+
+
+import RPi.GPIO as GPIO
+
+
+import time
+# GPIO 代码
+# 定义继电器引脚
+EN_Relay = 17
+
+
+def init_gpio():
+ """初始化GPIO设置"""
+ GPIO.setmode(GPIO.BCM)
+ GPIO.setup(EN_Relay, GPIO.OUT)
+ # 初始化为高电平（关闭状态）
+ GPIO.output(EN_Relay, GPIO.LOW)
+ time.sleep(1) # 等待稳定
+
+
+def turn_on_relay():
+ """打开继电器（设置为LOW）"""
+ try:
+ GPIO.output(EN_Relay, GPIO.HIGH)
+ #print('Relay turned on (LOW)')
+ except Exception as e:
+ print(f"Error turning on relay: {e}")
+
+
+def turn_off_relay():
+ """关闭继电器（设置为HIGH）"""
+ try:
+ GPIO.output(EN_Relay, GPIO.LOW)
+ #print('Relay turned off (HIGH)')
+ except Exception as e:
+ print(f"Error turning off relay: {e}")
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/joint_positions_assembled_5.txt b/kelaifen/Kelaifen_V6.0/joint_positions_assembled_5.txt
new file mode 100644
index 0000000..bd75977
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/joint_positions_assembled_5.txt
@@ -0,0 +1 @@
+-44.397,-150.247,154.254,-92.630,87.801,17.825
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/joint_positions_assembled_6.txt b/kelaifen/Kelaifen_V6.0/joint_positions_assembled_6.txt
new file mode 100644
index 0000000..e8cc3ef
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/joint_positions_assembled_6.txt
@@ -0,0 +1 @@
+37.181,-125.450,137.593,-98.126,88.215,26.116
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/joint_positions_assembled_7.txt b/kelaifen/Kelaifen_V6.0/joint_positions_assembled_7.txt
new file mode 100644
index 0000000..75800e6
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/joint_positions_assembled_7.txt
@@ -0,0 +1 @@
+65.718,-110.716,120.667,-78.282,94.033,2.384
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/joint_positions_assembled_8.txt b/kelaifen/Kelaifen_V6.0/joint_positions_assembled_8.txt
new file mode 100644
index 0000000..4cf6e75
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/joint_positions_assembled_8.txt
@@ -0,0 +1 @@
+6.496,-110.716,120.667,-78.282,94.033,2.384
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/program_config.py b/kelaifen/Kelaifen_V6.0/program_config.py
new file mode 100644
index 0000000..20dfb7e
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/program_config.py
@@ -0,0 +1,78 @@
+import json
+from trajectory_program import (
+ program1, program1_1, program2, program3, program3_1, program4,
+ program5, program5_1, program6, program7, program7_1, program8,
+ program9, program10, program11, program12
+)
+
+program_switch = {
+ 1: program1,
+ 100: program1_1,
+ 2: program2,
+ 3: program3,
+ 300: program3_1,
+ 4: program4,
+ 5: program5,
+ 500: program5_1,
+ 6: program6,
+ 7: program7,
+ 700: program7_1,
+ 8: program8,
+ 9: program9, # 停机位置
+ 10: program10, # 初始位置
+ 11: program11, # 洗枪位置
+ 12: program12, # 试枪位置
+}
+
+# 定义四种循环序列
+sequence_1 = [2, 300, 4, 100] # 情况1的循环序列，更改为循环序列使用透传
+sequence_2 = [4, 1, 2, 3] # 情况2的循环序列，待修改,暂从程序中删除逻辑，如有需要，从V2.0中复制
+sequence_3 = [6, 700, 8, 500] # 情况3的循环序列，更改为循环序列使用透传
+sequence_4 = [8, 5, 6, 7] # 情况4的循环序列，待修改,暂从程序中删除逻辑，如有需要，从V2.0中复制
+
+def load_process_config(config_path="/home/raspberrypi/robot1/config.json"):
+ try:
+ # 读取 JSON 文件
+ with open(config_path, "r", encoding="utf-8") as f:
+ config = json.load(f)
+
+ # ========== 关键：适配新的 JSON 结构 ==========
+ processed_presets = []
+ for preset in config["PRESET_PARAMS"]:
+ # 1. 处理单个预设项内的 program_delay_map（键转整数，值转浮点数）
+ processed_delay_map = {
+ int(key): float(value)
+ for key, value in preset["program_delay_map"].items()
+ }
+
+ # 2. 处理 params（数组转元组，保持原代码格式）
+ processed_params = tuple(preset["params"])
+
+ # 3. 保留 relay_config（无需类型转换，直接使用）
+ processed_relay = preset["relay_config"]
+
+ # 4. 组装处理后的预设项
+ processed_presets.append({
+ "params": processed_params,
+ "program_delay_map": processed_delay_map,
+ "relay_config": processed_relay
+ })
+
+ # 替换原 PRESET_PARAMS 为处理后的版本
+ config["PRESET_PARAMS"] = processed_presets
+
+ print(f"✅ 已加载配置文件，共加载 {len(processed_presets)} 个预设参数组")
+ return config
+
+ except FileNotFoundError:
+ print(f"❌ 配置文件 {config_path} 未找到！")
+ return None
+ except json.JSONDecodeError:
+ print(f"❌ 配置文件 {config_path} 格式错误！")
+ return None
+ except KeyError as e:
+ print(f"❌ 配置文件结构错误：缺少关键字段 {str(e)}，请检查 JSON 格式")
+ return None
+ except Exception as e:
+ print(f"❌ 加载配置失败：{str(e)}")
+ return None
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/readme.txt b/kelaifen/Kelaifen_V6.0/readme.txt
new file mode 100644
index 0000000..5940947
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/readme.txt
@@ -0,0 +1,19 @@
+1、config.json:
+（1）、config.json中的program_delay_map代表每段程序执行结束后的延时时间，单位秒
+
+ sequence_1 = [2, 300, 4, 100] # 情况1的循环序列
+ sequence_3 = [6, 700, 8, 500] # 情况3的循环序列
+
+（2）、config.json中的PRESET_PARAMS代表预设的速度和角度配置列表（速度百分比，角度偏移），
+可根据实际需求修改，配置顺序为1-12
+（3）、config.json中的relay_config喷枪开启和关闭位置
+
+2、可根据实际情况修改机械臂位置
+joint_positions_assembled_5、Pose_5.txt分别代表停机位置的关节位置和位姿
+joint_positions_assembled_6.txt、Pose_6.txt分别代表初始位置的关节位置和位姿
+joint_positions_assembled_7.txt、Pose_7.txt分别代表洗枪位置的关节位置和位姿
+joint_positions_assembled_8.txt、Pose_8.txt分别代表试枪位置的关节位置和位姿
+
+3、计算方法
+直接将示教器屏幕显示关节位置数据直接填到joint_positions_assembled文件
+位姿数据前三个数据不变，后三个数据，使用屏幕数据，根据公式4.76* (后三数据/180)，将计算结果放到Pose文件
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/serial_handler.py b/kelaifen/Kelaifen_V6.0/serial_handler.py
new file mode 100644
index 0000000..a3114bc
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/serial_handler.py
@@ -0,0 +1,151 @@
+# serial_handler.py
+import serial
+import time
+from serial_init import SerialSharedData
+from transform import transform_data
+
+# 声明需要用到的全局变量（与主程序保持一致）
+speed_adjustment = 100 # 初始值设为100，在50-100范围内
+Trajectory_angle = 0
+compensate_queue = None # 后续由主程序赋值
+
+# 初始化全局共享数据实例
+serial_shared = SerialSharedData()
+# 串口接收线程函数，持续监听串口数据，解析0xFF坐标帧并更新补偿变量
+def serial_receive_data(ser):
+ """线程内持续接收串口数据，同步解析0xFF坐标帧并更新全局补偿变量"""
+ frame_header = 0xFF
+ frame_length = 7 # 0xFF + x(2) + y(2) + z(2)
+
+ while True:
+ try:
+ if not ser or not isinstance(ser, serial.Serial) or not ser.is_open:
+ time.sleep(0.1)
+ continue
+
+ if ser.in_waiting > 0:
+ data = ser.read(ser.in_waiting)
+ with serial_shared.lock:
+ serial_shared.buffer += data
+ serial_shared.cmd_buffer += data
+
+ # 在接收线程中解析0xFF坐标帧
+ with serial_shared.lock:
+ current_buf = serial_shared.buffer[:]
+
+ if len(current_buf) >= frame_length:
+ header_index = current_buf.find(bytes([frame_header]))
+ if header_index != -1 and header_index + frame_length <= len(current_buf):
+ # 提取完整帧并更新缓冲区
+ frame = current_buf[header_index:header_index+frame_length]
+ with serial_shared.lock:
+ serial_shared.buffer = serial_shared.buffer[header_index+frame_length:]
+
+ # 坐标解码
+ x_coord = (frame[1] << 8) | frame[2]
+ x_coord = x_coord - 0x10000 if x_coord & 0x8000 else x_coord
+ if x_coord>90:
+ x_coord = 90
+ elif x_coord<-90:
+ x_coord = -90
+ y_coord = (frame[3] << 8) | frame[4]
+ y_coord = y_coord - 0x10000 if y_coord & 0x8000 else y_coord
+ if y_coord>50:
+ y_coord = 50
+ elif y_coord<-50:
+ y_coord = -50
+ z_coord = (frame[5] << 8) | frame[6]
+ z_coord = z_coord - 0x10000 if z_coord & 0x8000 else z_coord
+ if z_coord>90:
+ z_coord = 90
+ elif z_coord<-90:
+ z_coord = -90
+
+ inc_x, inc_y, inc_z = transform_data(x_coord, y_coord, z_coord)
+ # 存入队列（覆盖旧值，确保取到最新）
+ if compensate_queue is not None and not compensate_queue.empty():
+ compensate_queue.get_nowait()
+ if compensate_queue is not None:
+ compensate_queue.put((inc_x, inc_y, inc_z))
+
+ print(f"接收线程解析更新 - x:{x_coord:.1f}, y:{y_coord:.1f}, z:{z_coord:.1f}")
+
+ except Exception as e:
+ print(f"串口接收线程错误: {str(e)}")
+ time.sleep(0.005)
+# 从共享缓冲区读取指令帧，解析0xAA指令并返回指令内容（加锁确保线程安全）
+def read_cmd_from_shared(timeout=1.0):
+ """从共享缓冲区读取指令帧，解析0xAA指令并返回指令内容（加锁确保线程安全）"""
+ frame_header = 0xAA
+ cmd_length = 2 # 指令帧结构：0xAA（帧头） + 1字节指令值
+ start_time = time.time()
+
+ while (time.time() - start_time) < timeout:
+ # 加锁读取指令缓冲区
+ with serial_shared.lock:
+ current_cmd_buf = serial_shared.cmd_buffer[:]
+
+ if len(current_cmd_buf) >= cmd_length:
+ header_index = current_cmd_buf.find(bytes([frame_header]))
+ if header_index != -1:
+ # 提取完整指令帧
+ cmd_bytes = current_cmd_buf[header_index:header_index+cmd_length]
+ # 更新共享缓冲区（移除已处理指令）
+ with serial_shared.lock:
+ serial_shared.cmd_buffer = serial_shared.cmd_buffer[header_index+cmd_length:]
+ return cmd_bytes
+ time.sleep(0.01)
+ return None
+# 处理特殊指令0xBB，更新全局参数并打印清晰信息
+def handle_special_commands(PRESET_PARAMS):
+
+ global speed_adjustment, Trajectory_angle
+
+ bb_header = 0xBB
+ bb_frame_length = 2 # 0xBB + 1字节配置选择值(0-11)
+ # 初始化返回值（默认无选中配置）
+ selected_delay_map = None
+ selected_relay_config = None
+
+ # 加锁读取缓冲区，避免线程冲突
+ with serial_shared.lock:
+ current_cmd_buf = serial_shared.cmd_buffer[:]
+
+ # 处理0xBB指令（参数配置选择）
+ bb_index = current_cmd_buf.find(bytes([bb_header]))
+ if bb_index != -1 and bb_index + bb_frame_length <= len(current_cmd_buf):
+ # 提取完整的0xBB帧
+ bb_frame = current_cmd_buf[bb_index:bb_index+bb_frame_length]
+ # 解析配置选择值（1-12有效，对应PRESET_PARAMS的0-11索引）
+ config_select = bb_frame[1]
+
+ # 校验配置值范围（仅处理1-12，共12组）
+ if 1 <= config_select <= len(PRESET_PARAMS):
+ # 匹配预设参数项（索引 = 配置值 - 1）
+ selected_preset = PRESET_PARAMS[config_select - 1]
+ # 提取params并更新全局变量
+ speed_val, angle_val = selected_preset["params"]
+ speed_adjustment = speed_val
+ Trajectory_angle = angle_val
+ # 提取当前配置项对应的delay和relay配置（赋值给返回变量）
+ selected_delay_map = selected_preset["program_delay_map"]
+ selected_relay_config = selected_preset["relay_config"]
+
+ # 打印清晰的配置信息
+ print(f"=== 0xBB指令处理成功 ===")
+ print(f"配置选择值：{config_select}")
+ print(f"speed_adjustment: {speed_adjustment}%")
+ print(f"Trajectory_angle: {Trajectory_angle}")
+ print(f"已加载对应program_delay_map和relay_config配置")
+
+ # 移除已处理的帧（加锁更新缓冲区）
+ with serial_shared.lock:
+ serial_shared.cmd_buffer = serial_shared.cmd_buffer[bb_index+bb_frame_length:]
+ else:
+ # 无效配置值处理：清理帧但不更新参数
+ with serial_shared.lock:
+ serial_shared.cmd_buffer = serial_shared.cmd_buffer[bb_index+bb_frame_length:]
+ print(f"⚠️ 收到0xBB指令，配置选择值{config_select}无效（仅支持1-{len(PRESET_PARAMS)}）")
+
+ # 返回选中的delay配置和relay配置（无选中则返回None）
+ return selected_delay_map, selected_relay_config
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/serial_init.py b/kelaifen/Kelaifen_V6.0/serial_init.py
new file mode 100644
index 0000000..6414e73
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/serial_init.py
@@ -0,0 +1,27 @@
+# region 串口通信
+# 初始化串口
+import threading
+import serial
+
+
+def serial_init():
+ try:
+ ser = serial.Serial(
+ port="/dev/ttyS0", # 笔记本Windows端口，Linux/Mac替换为"/dev/ttyUSB0"
+ baudrate=115200,
+ timeout=0.1
+ )
+ print("串口初始化成功")
+ return ser
+ except Exception as e:
+ print(f"笔记本串口初始化失败: {e}")
+ return None
+
+
+# 矩阵转换函数：将P_L1_O2通过R_T转换为P_L1_O1
+class SerialSharedData:
+ """线程安全的串口数据共享存储类"""
+ def __init__(self):
+ self.lock = threading.Lock() # 防止多线程数据竞争
+ self.buffer = b'' # 用于解析0xFF坐标帧的通用缓冲区
+ self.cmd_buffer = b'' # 用于解析0xAA指令帧（主指令/子指令/停止指令）的缓冲区
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/trajectory_data.py b/kelaifen/Kelaifen_V6.0/trajectory_data.py
new file mode 100644
index 0000000..1f841ac
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/trajectory_data.py
@@ -0,0 +1,634 @@
+
+
+Pose_1 = [
+ [1549.9, -66.747, 360.2, -3.1056, -0.0095749, -1.6164],
+ [1549.9, -66.747, 360.2, -3.1056, -0.0095788, -1.6163],
+ [1549.9, -66.747, 360.2, -3.1056, -0.0096731, -1.6137],
+ [1549.9, -66.747, 360.2, -3.1057, -0.0098926, -1.6076],
+ [1549.9, -66.747, 360.2, -3.1058, -0.010237, -1.598],
+ [1549.9, -66.747, 360.2, -3.1059, -0.010705, -1.5849],
+ [1549.9, -66.747, 360.2, -3.1061, -0.011295, -1.5683],
+ [1549.9, -66.747, 360.2, -3.1063, -0.012005, -1.5483],
+ [1549.9, -66.747, 360.2, -3.1066, -0.012833, -1.5247],
+ [1549.9, -66.747, 360.2, -3.107, -0.013774, -1.4977],
+ [1549.9, -66.747, 360.2, -3.1074, -0.014824, -1.4672],
+ [1549.8, -66.704, 360.2, -3.1079, -0.015979, -1.4331],
+ [1548.7, -65.671, 360.33, -3.1086, -0.01723, -1.3956],
+ [1546.2, -63.262, 360.62, -3.1093, -0.01857, -1.3546],
+ [1542.3, -59.476, 361.08, -3.1101, -0.019988, -1.3101],
+ [1536.8, -54.313, 361.7, -3.1111, -0.021474, -1.2622],
+ [1530, -47.773, 362.49, -3.1123, -0.023012, -1.2107],
+ [1521.7, -39.857, 363.44, -3.1136, -0.024587, -1.1557],
+ [1512, -30.564, 364.56, -3.1151, -0.026181, -1.0972],
+ [1500.8, -19.894, 365.85, -3.1168, -0.027772, -1.0353],
+ [1488.2, -7.8477, 367.3, -3.1186, -0.029337, -0.96982],
+ [1474.2, 5.5325, 368.91, -3.1202, -0.030478, -0.91844],
+ [1459.8, 19.3, 370.57, -3.1202, -0.030478, -0.91844],
+ [1445.4, 33.068, 372.23, -3.1202, -0.030478, -0.91844],
+ [1431, 46.835, 373.89, -3.1202, -0.030478, -0.91844],
+ [1416.6, 60.602, 375.55, -3.1202, -0.030478, -0.91844],
+ [1402.2, 74.37, 377.21, -3.1202, -0.030478, -0.91844],
+ [1387.7, 88.137, 378.87, -3.1202, -0.030478, -0.91844],
+ [1373.3, 101.91, 380.53, -3.1202, -0.030478, -0.91844],
+ [1358.9, 115.67, 382.19, -3.1202, -0.030478, -0.91844],
+ [1344.5, 129.44, 383.85, -3.1202, -0.030478, -0.91844],
+ [1330.1, 143.21, 385.51, -3.1202, -0.030478, -0.91844],
+ [1315.7, 156.97, 387.17, -3.1202, -0.030478, -0.91844],
+ [1301.3, 170.74, 388.83, -3.1202, -0.030478, -0.91844],
+ [1286.8, 184.51, 390.49, -3.1202, -0.030478, -0.91844],
+ [1272.4, 198.28, 392.15, -3.1202, -0.030478, -0.91844],
+ [1258, 212.04, 393.81, -3.1202, -0.030478, -0.91844],
+ [1243.6, 225.81, 395.47, -3.1202, -0.030478, -0.91844],
+ [1229.2, 239.58, 397.13, -3.1202, -0.030478, -0.91844],
+ [1214.8, 253.35, 398.79, -3.1202, -0.030478, -0.91844],
+ [1196.8, 270.56, 400.86, -3.1202, -0.030478, -0.91844],
+ [1182.4, 284.32, 402.52, -3.1202, -0.030478, -0.91844],
+ [1168, 298.09, 404.18, -3.1202, -0.030478, -0.91844],
+ [1153.5, 311.86, 405.84, -3.1202, -0.030478, -0.91844],
+ [1139.1, 325.63, 407.5, -3.1202, -0.030478, -0.91844],
+ [1124.7, 339.39, 409.16, -3.1202, -0.030478, -0.91844],
+ [1110.3, 353.16, 410.82, -3.1202, -0.030478, -0.91844],
+ [1095.9, 366.93, 412.48, -3.1202, -0.030478, -0.91844],
+ [1081.5, 380.7, 414.14, -3.1202, -0.030478, -0.91844],
+ [1067.1, 394.46, 415.8, -3.1202, -0.030478, -0.91844],
+ [1052.7, 408.23, 417.46, -3.1202, -0.030478, -0.91844],
+ [1038.2, 422, 419.12, -3.1202, -0.030478, -0.91844],
+ [1023.8, 435.77, 420.78, -3.1202, -0.030478, -0.91844],
+ [1009.4, 449.53, 422.44, -3.1202, -0.030478, -0.91844],
+ [995.01, 463.3, 424.1, -3.1202, -0.030478, -0.91844],
+ [980.6, 477.07, 425.75, -3.1202, -0.030478, -0.91844],
+ [966.19, 490.84, 427.42, -3.1202, -0.030478, -0.91844],
+ [951.77, 504.6, 429.07, -3.1202, -0.030478, -0.91844],
+ [937.36, 518.37, 430.73, -3.1202, -0.030478, -0.91844],
+ [922.95, 532.14, 432.39, -3.1202, -0.030478, -0.91844],
+ [908.54, 545.9, 434.05, -3.1202, -0.030478, -0.91844],
+ [894.13, 559.67, 435.71, -3.1202, -0.030478, -0.91844],
+ [879.72, 573.44, 437.37, -3.1202, -0.030478, -0.91844],
+ [865.3, 587.21, 439.03, -3.1202, -0.030478, -0.91844],
+ [850.89, 600.98, 440.69, -3.1202, -0.030478, -0.91844],
+ [836.48, 614.74, 442.35, -3.1202, -0.030478, -0.91844],
+ [822.07, 628.51, 444.01, -3.1202, -0.030478, -0.91844],
+ [807.66, 642.28, 445.67, -3.1202, -0.030478, -0.91844],
+ [793.24, 656.04, 447.33, -3.1202, -0.030478, -0.91844],
+ [778.83, 669.81, 448.99, -3.1202, -0.030478, -0.91844],
+ [764.42, 683.58, 450.65, -3.1202, -0.030478, -0.91844],
+ [750.01, 697.35, 452.31, -3.1202, -0.030478, -0.91844],
+ [735.6, 711.12, 453.97, -3.1202, -0.030478, -0.91844],
+ [721.18, 724.88, 455.63, -3.1202, -0.030478, -0.91844],
+ [706.77, 738.65, 457.29, -3.1202, -0.030478, -0.91844],
+ [692.36, 752.42, 458.95, -3.1202, -0.030478, -0.91844],
+ [677.95, 766.18, 460.61, -3.1202, -0.030478, -0.91844],
+ [663.54, 779.95, 462.27, -3.1202, -0.030478, -0.91844],
+ [649.12, 793.72, 463.93, -3.1202, -0.030478, -0.91844],
+ [634.71, 807.49, 465.59, -3.1202, -0.030478, -0.91844],
+ [620.3, 821.25, 467.25, -3.1202, -0.030478, -0.91844],
+ [605.89, 835.02, 468.91, -3.1202, -0.030478, -0.91844],
+ [591.48, 848.79, 470.57, -3.1202, -0.030478, -0.91844],
+ [577.07, 862.56, 472.23, -3.1202, -0.030478, -0.91844],
+ [562.65, 876.32, 473.88, -3.1202, -0.030478, -0.91844],
+ [548.24, 890.09, 475.54, -3.1202, -0.030478, -0.91844],
+ [533.83, 903.86, 477.2, -3.1202, -0.030478, -0.91844],
+ [519.42, 917.63, 478.86, -3.1202, -0.030478, -0.91844],
+ [505.01, 931.39, 480.52, -3.1202, -0.030478, -0.91844],
+ [490.6, 945.16, 482.18, -3.1202, -0.030478, -0.91844],
+ [476.18, 958.93, 483.84, -3.1202, -0.030478, -0.91844],
+ [461.77, 972.7, 485.5, -3.1202, -0.030478, -0.91844],
+ [447.36, 986.46, 487.16, -3.1202, -0.030478, -0.91844],
+ [432.95, 1000.2, 488.82, -3.1202, -0.030478, -0.91844],
+ [418.54, 1014, 490.48, -3.1202, -0.030478, -0.91844],
+ [404.12, 1027.8, 492.14, -3.1202, -0.030478, -0.91844],
+ [389.71, 1041.5, 493.8, -3.1202, -0.030478, -0.91844],
+ [375.3, 1055.3, 495.46, -3.1202, -0.030478, -0.91844],
+ [360.89, 1069.1, 497.12, -3.1202, -0.030478, -0.91844],
+ [346.48, 1082.8, 498.78, -3.1202, -0.030478, -0.91844],
+ [332.06, 1096.6, 500.44, -3.1202, -0.030478, -0.91844],
+ [317.65, 1110.4, 502.1, -3.1202, -0.030478, -0.91844],
+ [303.24, 1124.1, 503.76, -3.1202, -0.030478, -0.91844],
+ [288.83, 1137.9, 505.42, -3.1202, -0.030478, -0.91844],
+ [274.42, 1151.7, 507.08, -3.1202, -0.030478, -0.91844],
+ [260, 1165.4, 508.74, -3.1202, -0.030478, -0.91844],
+ [245.59, 1179.2, 510.4, -3.1202, -0.030478, -0.91844],
+ [231.18, 1193, 512.06, -3.1202, -0.030478, -0.91844],
+ [216.77, 1206.7, 513.72, -3.1202, -0.030478, -0.91844],
+ [202.36, 1220.5, 515.38, -3.1202, -0.030478, -0.91844],
+ [187.94, 1234.3, 517.04, -3.1202, -0.030478, -0.91844],
+ [173.53, 1248, 518.69, -3.1202, -0.030478, -0.91844],
+ [159.12, 1261.8, 520.36, -3.1202, -0.030478, -0.91844],
+ [144.71, 1275.6, 522.01, -3.1202, -0.030478, -0.91844],
+ [130.3, 1289.3, 523.67, -3.1202, -0.030478, -0.91844],
+ [115.89, 1303.1, 525.33, -3.1202, -0.030478, -0.91844],
+ [97.871, 1320.3, 527.41, -3.1202, -0.030478, -0.91844],
+ [83.459, 1334.1, 529.07, -3.1202, -0.030478, -0.91844],
+ [69.047, 1347.9, 530.73, -3.1202, -0.030478, -0.91844],
+ [54.635, 1361.6, 532.39, -3.1202, -0.030478, -0.91844],
+ [40.223, 1375.4, 534.05, -3.1202, -0.030478, -0.91844],
+ [25.811, 1389.2, 535.71, -3.1202, -0.030478, -0.91844],
+ [11.399, 1402.9, 537.37, -3.1202, -0.030478, -0.91844],
+ [-3.0127, 1416.7, 539.03, -3.1202, -0.030478, -0.91844],
+ [-17.425, 1430.5, 540.69, -3.1202, -0.030478, -0.91844],
+ [-31.836, 1444.2, 542.35, -3.1202, -0.030478, -0.91844],
+ [-46.248, 1458, 544, -3.1202, -0.030478, -0.91844],
+ [-60.66, 1471.8, 545.66, -3.1202, -0.030478, -0.91844],
+ [-75.072, 1485.5, 547.32, -3.1202, -0.030478, -0.91844],
+ [-89.484, 1499.3, 548.98, -3.1202, -0.030478, -0.91844],
+ [-103.9, 1513.1, 550.64, -3.1202, -0.030478, -0.91844],
+ [-118.31, 1526.8, 552.3, -3.1202, -0.030478, -0.91844],
+ [-132.72, 1540.6, 553.96, -3.1202, -0.030478, -0.91844],
+ [-147.13, 1554.4, 555.62, -3.1202, -0.030478, -0.91844],
+ [-161.44, 1568, 557.27, -3.1202, -0.030478, -0.91844],
+ [-174.58, 1580.6, 558.78, -3.1216, -0.031442, -0.87188],
+ [-186.28, 1591.8, 560.13, -3.1231, -0.032338, -0.82532],
+ [-196.54, 1601.6, 561.31, -3.1246, -0.033164, -0.77876],
+ [-205.36, 1610, 562.33, -3.1262, -0.033918, -0.7322],
+ [-212.73, 1617, 563.18, -3.1278, -0.034598, -0.68564],
+ [-218.67, 1622.7, 563.86, -3.1294, -0.035204, -0.63907],
+ [-223.16, 1627, 564.38, -3.1311, -0.035734, -0.5925],
+ [-226.22, 1629.9, 564.73, -3.1327, -0.036186, -0.54593],
+ [-227.83, 1631.5, 564.91, -3.1344, -0.036559, -0.49936],
+ [-228.1, 1631.7, 564.95, -3.1361, -0.036854, -0.45279],
+ [-228.1, 1631.7, 564.95, -3.1378, -0.037059, -0.40854],
+ [-228.1, 1631.7, 564.95, -3.1392, -0.037182, -0.36895],
+ [-228.1, 1631.7, 564.95, -3.1405, -0.037242, -0.33402],
+ [-228.1, 1631.7, 564.95, 3.1415, -0.037257, -0.30375],
+ [-228.1, 1631.7, 564.95, 3.1406, -0.037243, -0.27814],
+ [-228.1, 1631.7, 564.95, 3.1398, -0.037214, -0.25718],
+ [-228.1, 1631.7, 564.95, 3.1392, -0.03718, -0.24088],
+ [-228.1, 1631.7, 564.95, 3.1388, -0.037149, -0.22923],
+ [-228.1, 1631.7, 564.95, 3.1385, -0.037128, -0.22225],
+ [-228.1, 1631.7, 564.95, 3.1384, -0.037122, -0.22006]
+]
+joint_positions_assembled_1 = [[-8.303461, -48.278357, 71.508258, -115.343, 89.659, -5.706]]
+Pose_2 = [
+ [-228.1, 1631.7, 372.19, 3.1384, -0.037122, -0.22006],
+ [-228.1, 1631.7, 372.19, 3.1385, -0.037128, -0.22225],
+ [-228.1, 1631.7, 372.19, 3.1388, -0.037149, -0.22923],
+ [-228.1, 1631.7, 372.19, 3.1392, -0.03718, -0.24088],
+ [-228.1, 1631.7, 372.19, 3.1398, -0.037214, -0.25718],
+ [-228.1, 1631.7, 372.19, 3.1406, -0.037243, -0.27814],
+ [-228.1, 1631.7, 372.19, 3.1415, -0.037257, -0.30375],
+ [-228.1, 1631.7, 372.19, -3.1405, -0.037242, -0.33402],
+ [-228.1, 1631.7, 372.19, -3.1392, -0.037182, -0.36895],
+ [-228.1, 1631.7, 372.19, -3.1378, -0.037059, -0.40854],
+ [-228.1, 1631.7, 372.19, -3.1361, -0.036854, -0.45279],
+ [-228.06, 1631.7, 372.2, -3.1344, -0.036559, -0.49936],
+ [-226.98, 1630.7, 372.32, -3.1327, -0.036186, -0.54593],
+ [-224.46, 1628.2, 372.61, -3.1311, -0.035734, -0.5925],
+ [-220.49, 1624.5, 373.07, -3.1294, -0.035204, -0.63907],
+ [-215.09, 1619.3, 373.69, -3.1278, -0.034598, -0.68564],
+ [-208.24, 1612.8, 374.48, -3.1262, -0.033918, -0.7322],
+ [-199.96, 1604.8, 375.43, -3.1246, -0.033164, -0.77876],
+ [-190.23, 1595.5, 376.55, -3.1231, -0.032338, -0.82532],
+ [-179.06, 1584.9, 377.84, -3.1216, -0.031442, -0.87188],
+ [-166.45, 1572.8, 379.29, -3.1202, -0.030478, -0.91844],
+ [-152.44, 1559.5, 380.91, -3.1202, -0.030478, -0.91844],
+ [-138.03, 1545.7, 382.56, -3.1202, -0.030478, -0.91844],
+ [-123.62, 1531.9, 384.22, -3.1202, -0.030478, -0.91844],
+ [-109.21, 1518.2, 385.88, -3.1202, -0.030478, -0.91844],
+ [-94.794, 1504.4, 387.54, -3.1202, -0.030478, -0.91844],
+ [-80.382, 1490.6, 389.2, -3.1202, -0.030478, -0.91844],
+ [-65.971, 1476.8, 390.86, -3.1202, -0.030478, -0.91844],
+ [-51.559, 1463.1, 392.52, -3.1202, -0.030478, -0.91844],
+ [-37.147, 1449.3, 394.18, -3.1202, -0.030478, -0.91844],
+ [-22.735, 1435.5, 395.84, -3.1202, -0.030478, -0.91844],
+ [-8.3231, 1421.8, 397.5, -3.1202, -0.030478, -0.91844],
+ [6.0887, 1408, 399.16, -3.1202, -0.030478, -0.91844],
+ [20.501, 1394.2, 400.82, -3.1202, -0.030478, -0.91844],
+ [34.912, 1380.5, 402.48, -3.1202, -0.030478, -0.91844],
+ [49.324, 1366.7, 404.14, -3.1202, -0.030478, -0.91844],
+ [63.736, 1352.9, 405.8, -3.1202, -0.030478, -0.91844],
+ [78.148, 1339.2, 407.46, -3.1202, -0.030478, -0.91844],
+ [92.56, 1325.4, 409.12, -3.1202, -0.030478, -0.91844],
+ [106.97, 1311.6, 410.78, -3.1202, -0.030478, -0.91844],
+ [124.99, 1294.4, 412.85, -3.1202, -0.030478, -0.91844],
+ [139.4, 1280.7, 414.51, -3.1202, -0.030478, -0.91844],
+ [153.81, 1266.9, 416.17, -3.1202, -0.030478, -0.91844],
+ [168.22, 1253.1, 417.83, -3.1202, -0.030478, -0.91844],
+ [182.63, 1239.4, 419.49, -3.1202, -0.030478, -0.91844],
+ [197.05, 1225.6, 421.15, -3.1202, -0.030478, -0.91844],
+ [211.46, 1211.8, 422.81, -3.1202, -0.030478, -0.91844],
+ [225.87, 1198.1, 424.47, -3.1202, -0.030478, -0.91844],
+ [240.28, 1184.3, 426.13, -3.1202, -0.030478, -0.91844],
+ [254.69, 1170.5, 427.79, -3.1202, -0.030478, -0.91844],
+ [269.11, 1156.8, 429.45, -3.1202, -0.030478, -0.91844],
+ [283.52, 1143, 431.11, -3.1202, -0.030478, -0.91844],
+ [297.93, 1129.2, 432.77, -3.1202, -0.030478, -0.91844],
+ [312.34, 1115.5, 434.43, -3.1202, -0.030478, -0.91844],
+ [326.75, 1101.7, 436.09, -3.1202, -0.030478, -0.91844],
+ [341.16, 1087.9, 437.75, -3.1202, -0.030478, -0.91844],
+ [355.58, 1074.2, 439.41, -3.1202, -0.030478, -0.91844],
+ [369.99, 1060.4, 441.07, -3.1202, -0.030478, -0.91844],
+ [384.4, 1046.6, 442.73, -3.1202, -0.030478, -0.91844],
+ [398.81, 1032.8, 444.39, -3.1202, -0.030478, -0.91844],
+ [413.22, 1019.1, 446.05, -3.1202, -0.030478, -0.91844],
+ [427.63, 1005.3, 447.71, -3.1202, -0.030478, -0.91844],
+ [442.05, 991.54, 449.37, -3.1202, -0.030478, -0.91844],
+ [456.46, 977.78, 451.02, -3.1202, -0.030478, -0.91844],
+ [470.87, 964.01, 452.69, -3.1202, -0.030478, -0.91844],
+ [485.28, 950.24, 454.35, -3.1202, -0.030478, -0.91844],
+ [499.69, 936.47, 456, -3.1202, -0.030478, -0.91844],
+ [514.11, 922.71, 457.66, -3.1202, -0.030478, -0.91844],
+ [528.52, 908.94, 459.32, -3.1202, -0.030478, -0.91844],
+ [542.93, 895.17, 460.98, -3.1202, -0.030478, -0.91844],
+ [557.34, 881.4, 462.64, -3.1202, -0.030478, -0.91844],
+ [571.75, 867.64, 464.3, -3.1202, -0.030478, -0.91844],
+ [586.17, 853.87, 465.96, -3.1202, -0.030478, -0.91844],
+ [600.58, 840.1, 467.62, -3.1202, -0.030478, -0.91844],
+ [614.99, 826.33, 469.28, -3.1202, -0.030478, -0.91844],
+ [629.4, 812.57, 470.94, -3.1202, -0.030478, -0.91844],
+ [643.81, 798.8, 472.6, -3.1202, -0.030478, -0.91844],
+ [658.23, 785.03, 474.26, -3.1202, -0.030478, -0.91844],
+ [672.64, 771.26, 475.92, -3.1202, -0.030478, -0.91844],
+ [687.05, 757.5, 477.58, -3.1202, -0.030478, -0.91844],
+ [701.46, 743.73, 479.24, -3.1202, -0.030478, -0.91844],
+ [715.87, 729.96, 480.9, -3.1202, -0.030478, -0.91844],
+ [730.28, 716.19, 482.56, -3.1202, -0.030478, -0.91844],
+ [744.7, 702.43, 484.22, -3.1202, -0.030478, -0.91844],
+ [759.11, 688.66, 485.88, -3.1202, -0.030478, -0.91844],
+ [773.52, 674.89, 487.54, -3.1202, -0.030478, -0.91844],
+ [787.93, 661.12, 489.2, -3.1202, -0.030478, -0.91844],
+ [802.34, 647.36, 490.86, -3.1202, -0.030478, -0.91844],
+ [816.75, 633.59, 492.52, -3.1202, -0.030478, -0.91844],
+ [831.17, 619.82, 494.18, -3.1202, -0.030478, -0.91844],
+ [845.58, 606.05, 495.84, -3.1202, -0.030478, -0.91844],
+ [859.99, 592.29, 497.5, -3.1202, -0.030478, -0.91844],
+ [874.4, 578.52, 499.15, -3.1202, -0.030478, -0.91844],
+ [888.81, 564.75, 500.81, -3.1202, -0.030478, -0.91844],
+ [903.23, 550.98, 502.47, -3.1202, -0.030478, -0.91844],
+ [917.64, 537.22, 504.13, -3.1202, -0.030478, -0.91844],
+ [932.05, 523.45, 505.79, -3.1202, -0.030478, -0.91844],
+ [946.46, 509.68, 507.45, -3.1202, -0.030478, -0.91844],
+ [960.87, 495.91, 509.11, -3.1202, -0.030478, -0.91844],
+ [975.28, 482.15, 510.77, -3.1202, -0.030478, -0.91844],
+ [989.7, 468.38, 512.43, -3.1202, -0.030478, -0.91844],
+ [1004.1, 454.61, 514.09, -3.1202, -0.030478, -0.91844],
+ [1018.5, 440.84, 515.75, -3.1202, -0.030478, -0.91844],
+ [1032.9, 427.08, 517.41, -3.1202, -0.030478, -0.91844],
+ [1047.3, 413.31, 519.07, -3.1202, -0.030478, -0.91844],
+ [1061.8, 399.54, 520.73, -3.1202, -0.030478, -0.91844],
+ [1076.2, 385.77, 522.39, -3.1202, -0.030478, -0.91844],
+ [1090.6, 372.01, 524.05, -3.1202, -0.030478, -0.91844],
+ [1105, 358.24, 525.71, -3.1202, -0.030478, -0.91844],
+ [1119.4, 344.47, 527.37, -3.1202, -0.030478, -0.91844],
+ [1133.8, 330.7, 529.03, -3.1202, -0.030478, -0.91844],
+ [1148.2, 316.94, 530.69, -3.1202, -0.030478, -0.91844],
+ [1162.6, 303.17, 532.35, -3.1202, -0.030478, -0.91844],
+ [1177, 289.4, 534.01, -3.1202, -0.030478, -0.91844],
+ [1191.5, 275.63, 535.67, -3.1202, -0.030478, -0.91844],
+ [1205.9, 261.87, 537.33, -3.1202, -0.030478, -0.91844],
+ [1223.9, 244.66, 539.4, -3.1202, -0.030478, -0.91844],
+ [1238.3, 230.89, 541.06, -3.1202, -0.030478, -0.91844],
+ [1252.7, 217.12, 542.72, -3.1202, -0.030478, -0.91844],
+ [1267.1, 203.35, 544.38, -3.1202, -0.030478, -0.91844],
+ [1281.5, 189.59, 546.04, -3.1202, -0.030478, -0.91844],
+ [1296, 175.82, 547.7, -3.1202, -0.030478, -0.91844],
+ [1310.4, 162.05, 549.36, -3.1202, -0.030478, -0.91844],
+ [1324.8, 148.28, 551.02, -3.1202, -0.030478, -0.91844],
+ [1339.2, 134.52, 552.68, -3.1202, -0.030478, -0.91844],
+ [1353.6, 120.75, 554.34, -3.1202, -0.030478, -0.91844],
+ [1368, 106.98, 556, -3.1202, -0.030478, -0.91844],
+ [1382.4, 93.214, 557.66, -3.1202, -0.030478, -0.91844],
+ [1396.8, 79.447, 559.32, -3.1202, -0.030478, -0.91844],
+ [1411.2, 65.679, 560.98, -3.1202, -0.030478, -0.91844],
+ [1425.7, 51.912, 562.64, -3.1202, -0.030478, -0.91844],
+ [1440.1, 38.144, 564.3, -3.1202, -0.030478, -0.91844],
+ [1454.5, 24.377, 565.96, -3.1202, -0.030478, -0.91844],
+ [1468.9, 10.609, 567.62, -3.1202, -0.030478, -0.91844],
+ [1483.2, -3.0582, 569.26, -3.1186, -0.029337, -0.96982],
+ [1496.3, -15.613, 570.78, -3.1168, -0.027772, -1.0353],
+ [1508, -26.79, 572.12, -3.1151, -0.026181, -1.0972],
+ [1518.3, -36.591, 573.31, -3.1136, -0.024587, -1.1557],
+ [1527.1, -45.015, 574.32, -3.1123, -0.023012, -1.2107],
+ [1534.5, -52.062, 575.17, -3.1111, -0.021474, -1.2622],
+ [1540.4, -57.733, 575.85, -3.1101, -0.019988, -1.3101],
+ [1544.9, -62.026, 576.37, -3.1093, -0.01857, -1.3546],
+ [1548, -64.943, 576.72, -3.1086, -0.01723, -1.3956],
+ [1549.6, -66.484, 576.91, -3.1079, -0.015979, -1.4331],
+ [1549.9, -66.747, 576.94, -3.1074, -0.014824, -1.4672],
+ [1549.9, -66.747, 576.94, -3.107, -0.013774, -1.4977],
+ [1549.9, -66.747, 576.94, -3.1066, -0.012833, -1.5247],
+ [1549.9, -66.747, 576.94, -3.1063, -0.012005, -1.5483],
+ [1549.9, -66.747, 576.94, -3.1061, -0.011295, -1.5683],
+ [1549.9, -66.747, 576.94, -3.1059, -0.010705, -1.5849],
+ [1549.9, -66.747, 576.94, -3.1058, -0.010237, -1.598],
+ [1549.9, -66.747, 576.94, -3.1057, -0.0098926, -1.6076],
+ [1549.9, -66.747, 576.94, -3.1056, -0.0096731, -1.6137],
+ [1549.9, -66.747, 576.94, -3.1056, -0.0095788, -1.6163],
+ [1549.9, -66.747, 576.94, -3.1056, -0.0095749, -1.6164]
+]
+joint_positions_assembled_2 = [[92.442,-41.967,60.096,-110.133,90.041,11.741]]
+Pose_3 = [
+ [1549.9, -66.747, 576.94, 3.1398, 0.034928, -1.5584],
+ [1549.9, -66.747, 576.94, 3.1398, 0.034929, -1.558],
+ [1549.9, -66.747, 576.94, 3.14, 0.034937, -1.5534],
+ [1549.9, -66.747, 576.94, 3.1403, 0.03495, -1.544],
+ [1549.9, -66.747, 576.94, 3.1408, 0.034964, -1.5301],
+ [1549.9, -66.747, 576.94, 3.1415, 0.034973, -1.5114],
+ [1549.9, -66.747, 576.94, -3.1409, 0.034966, -1.4882],
+ [1549.9, -66.747, 576.94, -3.1399, 0.034933, -1.4602],
+ [1549.9, -66.747, 576.94, -3.1388, 0.034861, -1.4276],
+ [1549.9, -66.747, 576.94, -3.1375, 0.034732, -1.3904],
+ [1549.9, -66.747, 576.94, -3.1361, 0.03453, -1.3484],
+ [1549.9, -66.737, 576.94, -3.1345, 0.034239, -1.3025],
+ [1549, -65.885, 576.84, -3.1329, 0.03387, -1.2559],
+ [1546.6, -63.657, 576.57, -3.1313, 0.033428, -1.2093],
+ [1542.9, -60.051, 576.13, -3.1298, 0.032914, -1.1628],
+ [1537.6, -55.069, 575.53, -3.1282, 0.032328, -1.1162],
+ [1531, -48.711, 574.77, -3.1268, 0.031672, -1.0696],
+ [1522.9, -40.975, 573.83, -3.1253, 0.030947, -1.0231],
+ [1513.3, -31.862, 572.74, -3.1239, 0.030156, -0.97652],
+ [1502.4, -21.373, 571.47, -3.1225, 0.029299, -0.92996],
+ [1490, -9.5074, 570.04, -3.1212, 0.028379, -0.88341],
+ [1476.1, 3.7255, 568.44, -3.1205, 0.027896, -0.86013],
+ [1461.7, 17.493, 566.79, -3.1205, 0.027896, -0.86013],
+ [1447.3, 31.261, 565.13, -3.1205, 0.027896, -0.86013],
+ [1432.9, 45.028, 563.47, -3.1205, 0.027896, -0.86013],
+ [1418.5, 58.796, 561.81, -3.1205, 0.027896, -0.86013],
+ [1404, 72.563, 560.15, -3.1205, 0.027896, -0.86013],
+ [1389.6, 86.331, 558.49, -3.1205, 0.027896, -0.86013],
+ [1375.2, 100.1, 556.83, -3.1205, 0.027896, -0.86013],
+ [1360.8, 113.87, 555.17, -3.1205, 0.027896, -0.86013],
+ [1346.4, 127.63, 553.51, -3.1205, 0.027896, -0.86013],
+ [1332, 141.4, 551.85, -3.1205, 0.027896, -0.86013],
+ [1317.6, 155.17, 550.19, -3.1205, 0.027896, -0.86013],
+ [1303.2, 168.94, 548.53, -3.1205, 0.027896, -0.86013],
+ [1288.7, 182.7, 546.87, -3.1205, 0.027896, -0.86013],
+ [1274.3, 196.47, 545.21, -3.1205, 0.027896, -0.86013],
+ [1259.9, 210.24, 543.55, -3.1205, 0.027896, -0.86013],
+ [1245.5, 224.01, 541.89, -3.1205, 0.027896, -0.86013],
+ [1231.1, 237.77, 540.23, -3.1205, 0.027896, -0.86013],
+ [1216.7, 251.54, 538.57, -3.1205, 0.027896, -0.86013],
+ [1198.7, 268.75, 536.5, -3.1205, 0.027896, -0.86013],
+ [1184.3, 282.52, 534.84, -3.1205, 0.027896, -0.86013],
+ [1169.8, 296.29, 533.18, -3.1205, 0.027896, -0.86013],
+ [1155.4, 310.05, 531.52, -3.1205, 0.027896, -0.86013],
+ [1141, 323.82, 529.86, -3.1205, 0.027896, -0.86013],
+ [1126.6, 337.59, 528.2, -3.1205, 0.027896, -0.86013],
+ [1112.2, 351.36, 526.54, -3.1205, 0.027896, -0.86013],
+ [1097.8, 365.12, 524.88, -3.1205, 0.027896, -0.86013],
+ [1083.4, 378.89, 523.22, -3.1205, 0.027896, -0.86013],
+ [1069, 392.66, 521.56, -3.1205, 0.027896, -0.86013],
+ [1054.5, 406.43, 519.9, -3.1205, 0.027896, -0.86013],
+ [1040.1, 420.19, 518.24, -3.1205, 0.027896, -0.86013],
+ [1025.7, 433.96, 516.58, -3.1205, 0.027896, -0.86013],
+ [1011.3, 447.73, 514.92, -3.1205, 0.027896, -0.86013],
+ [996.9, 461.5, 513.26, -3.1205, 0.027896, -0.86013],
+ [982.49, 475.26, 511.6, -3.1205, 0.027896, -0.86013],
+ [968.08, 489.03, 509.94, -3.1205, 0.027896, -0.86013],
+ [953.67, 502.8, 508.28, -3.1205, 0.027896, -0.86013],
+ [939.26, 516.57, 506.62, -3.1205, 0.027896, -0.86013],
+ [924.84, 530.33, 504.96, -3.1205, 0.027896, -0.86013],
+ [910.43, 544.1, 503.3, -3.1205, 0.027896, -0.86013],
+ [896.02, 557.87, 501.64, -3.1205, 0.027896, -0.86013],
+ [881.61, 571.63, 499.99, -3.1205, 0.027896, -0.86013],
+ [867.2, 585.4, 498.32, -3.1205, 0.027896, -0.86013],
+ [852.78, 599.17, 496.66, -3.1205, 0.027896, -0.86013],
+ [838.37, 612.94, 495.01, -3.1205, 0.027896, -0.86013],
+ [823.96, 626.71, 493.35, -3.1205, 0.027896, -0.86013],
+ [809.55, 640.47, 491.69, -3.1205, 0.027896, -0.86013],
+ [795.14, 654.24, 490.03, -3.1205, 0.027896, -0.86013],
+ [780.73, 668.01, 488.37, -3.1205, 0.027896, -0.86013],
+ [766.31, 681.77, 486.71, -3.1205, 0.027896, -0.86013],
+ [751.9, 695.54, 485.05, -3.1205, 0.027896, -0.86013],
+ [737.49, 709.31, 483.39, -3.1205, 0.027896, -0.86013],
+ [723.08, 723.08, 481.73, -3.1205, 0.027896, -0.86013],
+ [708.67, 736.85, 480.07, -3.1205, 0.027896, -0.86013],
+ [694.25, 750.61, 478.41, -3.1205, 0.027896, -0.86013],
+ [679.84, 764.38, 476.75, -3.1205, 0.027896, -0.86013],
+ [665.43, 778.15, 475.09, -3.1205, 0.027896, -0.86013],
+ [651.02, 791.91, 473.43, -3.1205, 0.027896, -0.86013],
+ [636.61, 805.68, 471.77, -3.1205, 0.027896, -0.86013],
+ [622.2, 819.45, 470.11, -3.1205, 0.027896, -0.86013],
+ [607.78, 833.22, 468.45, -3.1205, 0.027896, -0.86013],
+ [593.37, 846.99, 466.79, -3.1205, 0.027896, -0.86013],
+ [578.96, 860.75, 465.13, -3.1205, 0.027896, -0.86013],
+ [564.55, 874.52, 463.47, -3.1205, 0.027896, -0.86013],
+ [550.14, 888.29, 461.81, -3.1205, 0.027896, -0.86013],
+ [535.72, 902.05, 460.15, -3.1205, 0.027896, -0.86013],
+ [521.31, 915.82, 458.49, -3.1205, 0.027896, -0.86013],
+ [506.9, 929.59, 456.83, -3.1205, 0.027896, -0.86013],
+ [492.49, 943.36, 455.17, -3.1205, 0.027896, -0.86013],
+ [478.08, 957.12, 453.51, -3.1205, 0.027896, -0.86013],
+ [463.67, 970.89, 451.86, -3.1205, 0.027896, -0.86013],
+ [449.25, 984.66, 450.19, -3.1205, 0.027896, -0.86013],
+ [434.84, 998.43, 448.54, -3.1205, 0.027896, -0.86013],
+ [420.43, 1012.2, 446.88, -3.1205, 0.027896, -0.86013],
+ [406.02, 1026, 445.22, -3.1205, 0.027896, -0.86013],
+ [391.61, 1039.7, 443.56, -3.1205, 0.027896, -0.86013],
+ [377.19, 1053.5, 441.9, -3.1205, 0.027896, -0.86013],
+ [362.78, 1067.3, 440.24, -3.1205, 0.027896, -0.86013],
+ [348.37, 1081, 438.58, -3.1205, 0.027896, -0.86013],
+ [333.96, 1094.8, 436.92, -3.1205, 0.027896, -0.86013],
+ [319.55, 1108.6, 435.26, -3.1205, 0.027896, -0.86013],
+ [305.13, 1122.3, 433.6, -3.1205, 0.027896, -0.86013],
+ [290.72, 1136.1, 431.94, -3.1205, 0.027896, -0.86013],
+ [276.31, 1149.9, 430.28, -3.1205, 0.027896, -0.86013],
+ [261.9, 1163.6, 428.62, -3.1205, 0.027896, -0.86013],
+ [247.49, 1177.4, 426.96, -3.1205, 0.027896, -0.86013],
+ [233.07, 1191.2, 425.3, -3.1205, 0.027896, -0.86013],
+ [218.66, 1204.9, 423.64, -3.1205, 0.027896, -0.86013],
+ [204.25, 1218.7, 421.98, -3.1205, 0.027896, -0.86013],
+ [189.84, 1232.5, 420.32, -3.1205, 0.027896, -0.86013],
+ [175.43, 1246.2, 418.66, -3.1205, 0.027896, -0.86013],
+ [161.02, 1260, 417, -3.1205, 0.027896, -0.86013],
+ [146.6, 1273.8, 415.34, -3.1205, 0.027896, -0.86013],
+ [132.19, 1287.5, 413.68, -3.1205, 0.027896, -0.86013],
+ [117.78, 1301.3, 412.02, -3.1205, 0.027896, -0.86013],
+ [99.766, 1318.5, 409.95, -3.1205, 0.027896, -0.86013],
+ [85.354, 1332.3, 408.29, -3.1205, 0.027896, -0.86013],
+ [70.942, 1346.1, 406.63, -3.1205, 0.027896, -0.86013],
+ [56.53, 1359.8, 404.97, -3.1205, 0.027896, -0.86013],
+ [42.118, 1373.6, 403.31, -3.1205, 0.027896, -0.86013],
+ [27.706, 1387.4, 401.65, -3.1205, 0.027896, -0.86013],
+ [13.295, 1401.1, 399.99, -3.1205, 0.027896, -0.86013],
+ [-1.1172, 1414.9, 398.33, -3.1205, 0.027896, -0.86013],
+ [-15.529, 1428.7, 396.67, -3.1205, 0.027896, -0.86013],
+ [-29.941, 1442.4, 395.01, -3.1205, 0.027896, -0.86013],
+ [-44.353, 1456.2, 393.35, -3.1205, 0.027896, -0.86013],
+ [-58.765, 1470, 391.69, -3.1205, 0.027896, -0.86013],
+ [-73.176, 1483.7, 390.03, -3.1205, 0.027896, -0.86013],
+ [-87.588, 1497.5, 388.37, -3.1205, 0.027896, -0.86013],
+ [-102, 1511.3, 386.71, -3.1205, 0.027896, -0.86013],
+ [-116.41, 1525, 385.05, -3.1205, 0.027896, -0.86013],
+ [-130.82, 1538.8, 383.39, -3.1205, 0.027896, -0.86013],
+ [-145.24, 1552.6, 381.74, -3.1205, 0.027896, -0.86013],
+ [-159.6, 1566.3, 380.08, -3.12, 0.027526, -0.84279],
+ [-172.93, 1579, 378.55, -3.1182, 0.026015, -0.77558],
+ [-184.82, 1590.4, 377.18, -3.1166, 0.024474, -0.71188],
+ [-195.27, 1600.4, 375.97, -3.1152, 0.022926, -0.65167],
+ [-204.28, 1609, 374.94, -3.1139, 0.021392, -0.59495],
+ [-211.84, 1616.2, 374.06, -3.1128, 0.01989, -0.54173],
+ [-217.97, 1622, 373.36, -3.1119, 0.018435, -0.492],
+ [-222.65, 1626.5, 372.82, -3.1111, 0.017042, -0.44576],
+ [-225.9, 1629.6, 372.45, -3.1103, 0.01572, -0.40302],
+ [-227.7, 1631.3, 372.24, -3.1098, 0.014482, -0.36376],
+ [-228.1, 1631.7, 372.19, -3.1093, 0.013334, -0.328],
+ [-228.1, 1631.7, 372.19, -3.1088, 0.012284, -0.29572],
+ [-228.1, 1631.7, 372.19, -3.1085, 0.011336, -0.26694],
+ [-228.1, 1631.7, 372.19, -3.1082, 0.010495, -0.24164],
+ [-228.1, 1631.7, 372.19, -3.108, 0.0097651, -0.21984],
+ [-228.1, 1631.7, 372.19, -3.1078, 0.0091482, -0.20152],
+ [-228.1, 1631.7, 372.19, -3.1077, 0.0086465, -0.18669],
+ [-228.1, 1631.7, 372.19, -3.1076, 0.0082616, -0.17536],
+ [-228.1, 1631.7, 372.19, -3.1075, 0.0079945, -0.16751],
+ [-228.1, 1631.7, 372.19, -3.1075, 0.0078459, -0.16315],
+ [-228.1, 1631.7, 372.19, -3.1075, 0.0078124, -0.16216]
+]
+joint_positions_assembled_3 = [[-8.437,-52.830,64.769,-101.519,91.959,-9.155]]
+Pose_4 = [
+ [-228.1, 1631.7, 564.95, -3.1075, 0.0078124, -0.16216],
+ [-228.1, 1631.7, 564.95, -3.1075, 0.0078459, -0.16315],
+ [-228.1, 1631.7, 564.95, -3.1075, 0.0079945, -0.16751],
+ [-228.1, 1631.7, 564.95, -3.1076, 0.0082616, -0.17536],
+ [-228.1, 1631.7, 564.95, -3.1077, 0.0086465, -0.18669],
+ [-228.1, 1631.7, 564.95, -3.1078, 0.0091482, -0.20152],
+ [-228.1, 1631.7, 564.95, -3.108, 0.0097651, -0.21984],
+ [-228.1, 1631.7, 564.95, -3.1082, 0.010495, -0.24164],
+ [-228.1, 1631.7, 564.95, -3.1085, 0.011336, -0.26694],
+ [-228.1, 1631.7, 564.95, -3.1088, 0.012284, -0.29572],
+ [-228.1, 1631.7, 564.95, -3.1093, 0.013334, -0.328],
+ [-228.09, 1631.7, 564.95, -3.1098, 0.014482, -0.36376],
+ [-227.2, 1630.9, 564.84, -3.1103, 0.01572, -0.40302],
+ [-224.87, 1628.6, 564.58, -3.1111, 0.017042, -0.44576],
+ [-221.1, 1625, 564.14, -3.1119, 0.018435, -0.492],
+ [-215.88, 1620, 563.54, -3.1128, 0.01989, -0.54173],
+ [-209.23, 1613.7, 562.77, -3.1139, 0.021392, -0.59495],
+ [-201.13, 1606, 561.84, -3.1152, 0.022926, -0.65167],
+ [-191.59, 1596.8, 560.74, -3.1166, 0.024474, -0.71188],
+ [-180.61, 1586.3, 559.48, -3.1182, 0.026015, -0.77558],
+ [-168.19, 1574.5, 558.05, -3.12, 0.027526, -0.84279],
+ [-154.34, 1561.3, 556.45, -3.1205, 0.027896, -0.86013],
+ [-139.93, 1547.5, 554.79, -3.1205, 0.027896, -0.86013],
+ [-125.51, 1533.7, 553.13, -3.1205, 0.027896, -0.86013],
+ [-111.1, 1520, 551.47, -3.1205, 0.027896, -0.86013],
+ [-96.69, 1506.2, 549.81, -3.1205, 0.027896, -0.86013],
+ [-82.278, 1492.4, 548.15, -3.1205, 0.027896, -0.86013],
+ [-67.866, 1478.7, 546.49, -3.1205, 0.027896, -0.86013],
+ [-53.454, 1464.9, 544.84, -3.1205, 0.027896, -0.86013],
+ [-39.042, 1451.1, 543.17, -3.1205, 0.027896, -0.86013],
+ [-24.631, 1437.3, 541.51, -3.1205, 0.027896, -0.86013],
+ [-10.219, 1423.6, 539.86, -3.1205, 0.027896, -0.86013],
+ [4.1933, 1409.8, 538.2, -3.1205, 0.027896, -0.86013],
+ [18.605, 1396, 536.54, -3.1205, 0.027896, -0.86013],
+ [33.017, 1382.3, 534.88, -3.1205, 0.027896, -0.86013],
+ [47.429, 1368.5, 533.22, -3.1205, 0.027896, -0.86013],
+ [61.841, 1354.7, 531.56, -3.1205, 0.027896, -0.86013],
+ [76.253, 1341, 529.9, -3.1205, 0.027896, -0.86013],
+ [90.665, 1327.2, 528.24, -3.1205, 0.027896, -0.86013],
+ [105.08, 1313.4, 526.58, -3.1205, 0.027896, -0.86013],
+ [123.09, 1296.2, 524.5, -3.1205, 0.027896, -0.86013],
+ [137.5, 1282.5, 522.84, -3.1205, 0.027896, -0.86013],
+ [151.91, 1268.7, 521.18, -3.1205, 0.027896, -0.86013],
+ [166.33, 1254.9, 519.52, -3.1205, 0.027896, -0.86013],
+ [180.74, 1241.2, 517.87, -3.1205, 0.027896, -0.86013],
+ [195.15, 1227.4, 516.21, -3.1205, 0.027896, -0.86013],
+ [209.56, 1213.6, 514.55, -3.1205, 0.027896, -0.86013],
+ [223.97, 1199.9, 512.89, -3.1205, 0.027896, -0.86013],
+ [238.39, 1186.1, 511.23, -3.1205, 0.027896, -0.86013],
+ [252.8, 1172.3, 509.57, -3.1205, 0.027896, -0.86013],
+ [267.21, 1158.6, 507.91, -3.1205, 0.027896, -0.86013],
+ [281.62, 1144.8, 506.25, -3.1205, 0.027896, -0.86013],
+ [296.03, 1131, 504.59, -3.1205, 0.027896, -0.86013],
+ [310.45, 1117.2, 502.93, -3.1205, 0.027896, -0.86013],
+ [324.86, 1103.5, 501.27, -3.1205, 0.027896, -0.86013],
+ [339.27, 1089.7, 499.61, -3.1205, 0.027896, -0.86013],
+ [353.68, 1076, 497.95, -3.1205, 0.027896, -0.86013],
+ [368.09, 1062.2, 496.29, -3.1205, 0.027896, -0.86013],
+ [382.5, 1048.4, 494.63, -3.1205, 0.027896, -0.86013],
+ [396.92, 1034.7, 492.97, -3.1205, 0.027896, -0.86013],
+ [411.33, 1020.9, 491.31, -3.1205, 0.027896, -0.86013],
+ [425.74, 1007.1, 489.65, -3.1205, 0.027896, -0.86013],
+ [440.15, 993.35, 487.99, -3.1205, 0.027896, -0.86013],
+ [454.56, 979.58, 486.33, -3.1205, 0.027896, -0.86013],
+ [468.98, 965.81, 484.67, -3.1205, 0.027896, -0.86013],
+ [483.39, 952.04, 483.01, -3.1205, 0.027896, -0.86013],
+ [497.8, 938.28, 481.35, -3.1205, 0.027896, -0.86013],
+ [512.21, 924.51, 479.69, -3.1205, 0.027896, -0.86013],
+ [526.62, 910.74, 478.03, -3.1205, 0.027896, -0.86013],
+ [541.04, 896.98, 476.37, -3.1205, 0.027896, -0.86013],
+ [555.45, 883.21, 474.72, -3.1205, 0.027896, -0.86013],
+ [569.86, 869.44, 473.06, -3.1205, 0.027896, -0.86013],
+ [584.27, 855.67, 471.39, -3.1205, 0.027896, -0.86013],
+ [598.68, 841.91, 469.74, -3.1205, 0.027896, -0.86013],
+ [613.1, 828.14, 468.08, -3.1205, 0.027896, -0.86013],
+ [627.51, 814.37, 466.42, -3.1205, 0.027896, -0.86013],
+ [641.92, 800.6, 464.76, -3.1205, 0.027896, -0.86013],
+ [656.33, 786.84, 463.1, -3.1205, 0.027896, -0.86013],
+ [670.74, 773.07, 461.44, -3.1205, 0.027896, -0.86013],
+ [685.15, 759.3, 459.78, -3.1205, 0.027896, -0.86013],
+ [699.57, 745.53, 458.12, -3.1205, 0.027896, -0.86013],
+ [713.98, 731.77, 456.46, -3.1205, 0.027896, -0.86013],
+ [728.39, 718, 454.8, -3.1205, 0.027896, -0.86013],
+ [742.8, 704.23, 453.14, -3.1205, 0.027896, -0.86013],
+ [757.22, 690.46, 451.48, -3.1205, 0.027896, -0.86013],
+ [771.63, 676.7, 449.82, -3.1205, 0.027896, -0.86013],
+ [786.04, 662.93, 448.16, -3.1205, 0.027896, -0.86013],
+ [800.45, 649.16, 446.5, -3.1205, 0.027896, -0.86013],
+ [814.86, 635.39, 444.84, -3.1205, 0.027896, -0.86013],
+ [829.27, 621.63, 443.18, -3.1205, 0.027896, -0.86013],
+ [843.69, 607.86, 441.52, -3.1205, 0.027896, -0.86013],
+ [858.1, 594.09, 439.86, -3.1205, 0.027896, -0.86013],
+ [872.51, 580.32, 438.2, -3.1205, 0.027896, -0.86013],
+ [886.92, 566.56, 436.54, -3.1205, 0.027896, -0.86013],
+ [901.33, 552.79, 434.88, -3.1205, 0.027896, -0.86013],
+ [915.75, 539.02, 433.22, -3.1205, 0.027896, -0.86013],
+ [930.16, 525.25, 431.56, -3.1205, 0.027896, -0.86013],
+ [944.57, 511.49, 429.9, -3.1205, 0.027896, -0.86013],
+ [958.98, 497.72, 428.25, -3.1205, 0.027896, -0.86013],
+ [973.39, 483.95, 426.58, -3.1205, 0.027896, -0.86013],
+ [987.8, 470.18, 424.93, -3.1205, 0.027896, -0.86013],
+ [1002.2, 456.42, 423.27, -3.1205, 0.027896, -0.86013],
+ [1016.6, 442.65, 421.61, -3.1205, 0.027896, -0.86013],
+ [1031, 428.88, 419.95, -3.1205, 0.027896, -0.86013],
+ [1045.5, 415.11, 418.29, -3.1205, 0.027896, -0.86013],
+ [1059.9, 401.35, 416.63, -3.1205, 0.027896, -0.86013],
+ [1074.3, 387.58, 414.97, -3.1205, 0.027896, -0.86013],
+ [1088.7, 373.81, 413.31, -3.1205, 0.027896, -0.86013],
+ [1103.1, 360.05, 411.65, -3.1205, 0.027896, -0.86013],
+ [1117.5, 346.28, 409.99, -3.1205, 0.027896, -0.86013],
+ [1131.9, 332.51, 408.33, -3.1205, 0.027896, -0.86013],
+ [1146.3, 318.74, 406.67, -3.1205, 0.027896, -0.86013],
+ [1160.8, 304.98, 405.01, -3.1205, 0.027896, -0.86013],
+ [1175.2, 291.21, 403.35, -3.1205, 0.027896, -0.86013],
+ [1189.6, 277.44, 401.69, -3.1205, 0.027896, -0.86013],
+ [1204, 263.67, 400.03, -3.1205, 0.027896, -0.86013],
+ [1222, 246.46, 397.96, -3.1205, 0.027896, -0.86013],
+ [1236.4, 232.7, 396.3, -3.1205, 0.027896, -0.86013],
+ [1250.8, 218.93, 394.64, -3.1205, 0.027896, -0.86013],
+ [1265.2, 205.16, 392.98, -3.1205, 0.027896, -0.86013],
+ [1279.7, 191.39, 391.32, -3.1205, 0.027896, -0.86013],
+ [1294.1, 177.63, 389.66, -3.1205, 0.027896, -0.86013],
+ [1308.5, 163.86, 388, -3.1205, 0.027896, -0.86013],
+ [1322.9, 150.09, 386.34, -3.1205, 0.027896, -0.86013],
+ [1337.3, 136.32, 384.68, -3.1205, 0.027896, -0.86013],
+ [1351.7, 122.56, 383.02, -3.1205, 0.027896, -0.86013],
+ [1366.1, 108.79, 381.36, -3.1205, 0.027896, -0.86013],
+ [1380.5, 95.021, 379.7, -3.1205, 0.027896, -0.86013],
+ [1394.9, 81.254, 378.04, -3.1205, 0.027896, -0.86013],
+ [1409.3, 67.486, 376.38, -3.1205, 0.027896, -0.86013],
+ [1423.8, 53.719, 374.72, -3.1205, 0.027896, -0.86013],
+ [1438.2, 39.951, 373.06, -3.1205, 0.027896, -0.86013],
+ [1452.6, 26.184, 371.4, -3.1205, 0.027896, -0.86013],
+ [1467, 12.416, 369.74, -3.1205, 0.027896, -0.86013],
+ [1481.4, -1.3082, 368.09, -3.1212, 0.028379, -0.88341],
+ [1494.7, -14.043, 366.55, -3.1225, 0.029299, -0.92996],
+ [1506.6, -25.401, 365.18, -3.1239, 0.030156, -0.97652],
+ [1517, -35.383, 363.98, -3.1253, 0.030947, -1.0231],
+ [1526, -43.987, 362.94, -3.1268, 0.031672, -1.0696],
+ [1533.6, -51.215, 362.07, -3.1282, 0.032328, -1.1162],
+ [1539.7, -57.066, 361.37, -3.1298, 0.032914, -1.1628],
+ [1544.4, -61.541, 360.83, -3.1313, 0.033428, -1.2093],
+ [1547.7, -64.638, 360.45, -3.1329, 0.03387, -1.2559],
+ [1549.5, -66.359, 360.25, -3.1345, 0.034239, -1.3025],
+ [1549.9, -66.747, 360.2, -3.1361, 0.03453, -1.3484],
+ [1549.9, -66.747, 360.2, -3.1375, 0.034732, -1.3904],
+ [1549.9, -66.747, 360.2, -3.1388, 0.034861, -1.4276],
+ [1549.9, -66.747, 360.2, -3.1399, 0.034933, -1.4602],
+ [1549.9, -66.747, 360.2, -3.1409, 0.034966, -1.4882],
+ [1549.9, -66.747, 360.2, 3.1415, 0.034973, -1.5114],
+ [1549.9, -66.747, 360.2, 3.1408, 0.034964, -1.5301],
+ [1549.9, -66.747, 360.2, 3.1403, 0.03495, -1.544],
+ [1549.9, -66.747, 360.2, 3.14, 0.034937, -1.5534],
+ [1549.9, -66.747, 360.2, 3.1398, 0.034929, -1.558],
+ [1549.9, -66.747, 360.2, 3.1398, 0.034928, -1.5584]
+]
+joint_positions_assembled_4 = [[92.553,-45.178,53.196,-97.284,97.995,15.177]]
\ No newline at end of file
diff --git a/kelaifen/Kelaifen_V6.0/trajectory_program.py b/kelaifen/Kelaifen_V6.0/trajectory_program.py
new file mode 100644
index 0000000..f7b9b11
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/trajectory_program.py
@@ -0,0 +1,976 @@
+from queue import Queue
+from ETController import send_Point, sendCMD, wait_stop
+from gpio import turn_off_relay, turn_on_relay
+# from program_config import (
+# PRESET_PARAMS, turn_off_relay_end, turn_off_relay_start,
+# turn_on_relay_end, turn_on_relay_start
+# )
+from serial_init import SerialSharedData
+from trajectory_data import (
+ Pose_1, Pose_2, Pose_3, Pose_4,
+ joint_positions_assembled_1, joint_positions_assembled_2,
+ joint_positions_assembled_3, joint_positions_assembled_4
+ )
+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_on_relay_end, turn_off_relay_start, turn_off_relay_end):
+ 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_on_relay_end, turn_off_relay_start, turn_off_relay_end):
+ 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_on_relay_end, turn_off_relay_start, turn_off_relay_end):
+ 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 and i <= turn_on_relay_end):
+ turn_on_relay()
+ print("打开喷枪")
+ if(i >= turn_off_relay_start and i <= turn_off_relay_end):
+ turn_off_relay()
+ print("关闭喷枪")
+ i = i + 1
+
+ suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
+ print("第二段程序执行完")
+
+
+def program3(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, turn_off_relay_end):
+ 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_on_relay_end, turn_off_relay_start, turn_off_relay_end):
+ 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_on_relay_end, turn_off_relay_start, turn_off_relay_end):
+ 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 >= turn_on_relay_start and i <= turn_on_relay_end):
+ turn_on_relay()
+ print("打开喷枪")
+ if(i >= turn_off_relay_start and i <= turn_off_relay_end):
+ turn_off_relay()
+ print("关闭喷枪")
+ i = i + 1
+
+ suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
+ print("第四段程序执行完")
+
+
+def program5(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, turn_off_relay_end):
+ 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_on_relay_end, turn_off_relay_start, turn_off_relay_end):
+ 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_on_relay_end, turn_off_relay_start, turn_off_relay_end):
+ 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 and i <= turn_on_relay_end):
+ turn_on_relay()
+ print("打开喷枪")
+ if(i >= turn_off_relay_start and i <= turn_off_relay_end):
+ turn_off_relay()
+ print("关闭喷枪")
+ i = i + 1
+ suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
+ print("第六段程序执行完")
+
+
+def program7(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, turn_off_relay_end):
+ 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_on_relay_end, turn_off_relay_start, turn_off_relay_end):
+ 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_on_relay_end, turn_off_relay_start, turn_off_relay_end):
+ 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 >= turn_on_relay_start and i <= turn_on_relay_end):
+ turn_on_relay()
+ print("打开喷枪")
+ if(i >= turn_off_relay_start and i <= turn_off_relay_end):
+ turn_off_relay()
+ print("关闭喷枪")
+ 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("第十二段程序执行完")
diff --git a/kelaifen/Kelaifen_V6.0/transform.py b/kelaifen/Kelaifen_V6.0/transform.py
new file mode 100644
index 0000000..63b740a
--- /dev/null
+++ b/kelaifen/Kelaifen_V6.0/transform.py
@@ -0,0 +1,25 @@
+
+
+import numpy as np
+# 定义转换矩阵R_T
+R_T = np.array([
+ [-0.7206, -0.6908, 0],
+ [0.6884, -0.7231, 0],
+ [0.0830, 0, 1]
+])
+
+
+def transform_data(x_coord, y_coord, z_coord):
+ # 将接收到的3个坐标组成列矩阵P_L1_O2
+ P_L1_O2 = np.array([[x_coord], [y_coord], [z_coord]], dtype=np.float64)
+
+ # 执行矩阵乘法：P_L1_O1 = R_T * P_L1_O2
+ P_L1_O1 = np.dot(R_T, P_L1_O2)
+
+ # 提取转换后的三个数据
+ increments_x = P_L1_O1[0][0]
+ increments_y = P_L1_O1[1][0]
+ increments_z = P_L1_O1[2][0]
+
+ #print(f"矩阵转换完成 - increments_x: {increments_x:.4f}, increments_y: {increments_y:.4f}, increments_z: {increments_z:.4f}")
+ return increments_x, increments_y, increments_z
\ No newline at end of file