320dcfe6d7从这个提交复制而来

Debug/KeLaiFen.list

@@ -39,13 +39,13 @@ Idx Name          Size      VMA       LMA       File off  Algn
                   CONTENTS, READONLY, DEBUGGING, OCTETS
  17 .debug_line   0006887f  00000000  00000000  000ea363  2**0
                   CONTENTS, READONLY, DEBUGGING, OCTETS
- 18 .debug_str    001b0145  00000000  00000000  00152be2  2**0
+ 18 .debug_str    001b0124  00000000  00000000  00152be2  2**0
                   CONTENTS, READONLY, DEBUGGING, OCTETS
- 19 .comment      00000043  00000000  00000000  00302d27  2**0
+ 19 .comment      00000043  00000000  00000000  00302d06  2**0
                   CONTENTS, READONLY
- 20 .debug_frame  0000e258  00000000  00000000  00302d6c  2**2
+ 20 .debug_frame  0000e258  00000000  00000000  00302d4c  2**2
                   CONTENTS, READONLY, DEBUGGING, OCTETS
- 21 .debug_line_str 00000049  00000000  00000000  00310fc4  2**0
+ 21 .debug_line_str 00000047  00000000  00000000  00310fa4  2**0
                   CONTENTS, READONLY, DEBUGGING, OCTETS
 
 Disassembly of section .text:
@@ -25141,14 +25141,14 @@ void Decive_MainLoop()
  802a4ea:	2200      	movs	r2, #0
  802a4ec:	601a      	str	r2, [r3, #0]
 	}
-//	Vehicle_Heart_Beat();//机械臂心跳
+	//Vehicle_Heart_Beat();//机械臂心跳
 	Globle_Time_Count++;//全局计时
  802a4ee:	4b04      	ldr	r3, [pc, #16]	@ (802a500 <Decive_MainLoop+0x54>)
  802a4f0:	681b      	ldr	r3, [r3, #0]
  802a4f2:	3301      	adds	r3, #1
  802a4f4:	4a02      	ldr	r2, [pc, #8]	@ (802a500 <Decive_MainLoop+0x54>)
  802a4f6:	6013      	str	r3, [r2, #0]
-//  VehicleConnectDetect();//大臂车心跳检测
+    //VehicleConnectDetect();//大臂车心跳检测
 }
  802a4f8:	bf00      	nop
  802a4fa:	bd80      	pop	{r7, pc}
@@ -30091,46 +30091,103 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle)
  802c35c:	400200b8 	.word	0x400200b8
 
 0802c360 <Reset_Handler>:
+
+    .section  .text.Reset_Handler
+  .weak  Reset_Handler
+  .type  Reset_Handler, %function
+Reset_Handler:
+  ldr   sp, =_estack      /* set stack pointer */
  802c360:	f8df d034 	ldr.w	sp, [pc, #52]	@ 802c398 <LoopFillZerobss+0xe>
+
+/* Call the clock system initialization function.*/
+  bl  SystemInit
  802c364:	f7fe fe0c 	bl	802af80 <SystemInit>
+
+/* Copy the data segment initializers from flash to SRAM */
+  ldr r0, =_sdata
  802c368:	480c      	ldr	r0, [pc, #48]	@ (802c39c <LoopFillZerobss+0x12>)
+  ldr r1, =_edata
  802c36a:	490d      	ldr	r1, [pc, #52]	@ (802c3a0 <LoopFillZerobss+0x16>)
+  ldr r2, =_sidata
  802c36c:	4a0d      	ldr	r2, [pc, #52]	@ (802c3a4 <LoopFillZerobss+0x1a>)
+  movs r3, #0
  802c36e:	2300      	movs	r3, #0
+  b LoopCopyDataInit
  802c370:	e002      	b.n	802c378 <LoopCopyDataInit>
 
 0802c372 <CopyDataInit>:
+
+CopyDataInit:
+  ldr r4, [r2, r3]
  802c372:	58d4      	ldr	r4, [r2, r3]
+  str r4, [r0, r3]
  802c374:	50c4      	str	r4, [r0, r3]
+  adds r3, r3, #4
  802c376:	3304      	adds	r3, #4
 
 0802c378 <LoopCopyDataInit>:
+
+LoopCopyDataInit:
+  adds r4, r0, r3
  802c378:	18c4      	adds	r4, r0, r3
+  cmp r4, r1
  802c37a:	428c      	cmp	r4, r1
+  bcc CopyDataInit
  802c37c:	d3f9      	bcc.n	802c372 <CopyDataInit>
+/* Zero fill the bss segment. */
+  ldr r2, =_sbss
  802c37e:	4a0a      	ldr	r2, [pc, #40]	@ (802c3a8 <LoopFillZerobss+0x1e>)
+  ldr r4, =_ebss
  802c380:	4c0a      	ldr	r4, [pc, #40]	@ (802c3ac <LoopFillZerobss+0x22>)
+  movs r3, #0
  802c382:	2300      	movs	r3, #0
+  b LoopFillZerobss
  802c384:	e001      	b.n	802c38a <LoopFillZerobss>
 
 0802c386 <FillZerobss>:
+
+FillZerobss:
+  str  r3, [r2]
  802c386:	6013      	str	r3, [r2, #0]
+  adds r2, r2, #4
  802c388:	3204      	adds	r2, #4
 
 0802c38a <LoopFillZerobss>:
+
+LoopFillZerobss:
+  cmp r2, r4
  802c38a:	42a2      	cmp	r2, r4
+  bcc FillZerobss
  802c38c:	d3fb      	bcc.n	802c386 <FillZerobss>
+
+/* Call static constructors */
+    bl __libc_init_array
  802c38e:	f01a f8ff 	bl	8046590 <__libc_init_array>
+/* Call the application's entry point.*/
+  bl  main
  802c392:	f7fe f8d5 	bl	802a540 <main>
+  bx  lr
  802c396:	4770      	bx	lr
+  ldr   sp, =_estack      /* set stack pointer */
  802c398:	24080000 	.word	0x24080000
+  ldr r0, =_sdata
  802c39c:	24000000 	.word	0x24000000
+  ldr r1, =_edata
  802c3a0:	240000d0 	.word	0x240000d0
+  ldr r2, =_sidata
  802c3a4:	0804b3ec 	.word	0x0804b3ec
+  ldr r2, =_sbss
  802c3a8:	24000190 	.word	0x24000190
+  ldr r4, =_ebss
  802c3ac:	24012ef8 	.word	0x24012ef8
 
 0802c3b0 <ADC3_IRQHandler>:
+ * @retval None
+*/
+    .section  .text.Default_Handler,"ax",%progbits
+Default_Handler:
+Infinite_Loop:
+  b  Infinite_Loop
  802c3b0:	e7fe      	b.n	802c3b0 <ADC3_IRQHandler>
 
 0802c3b2 <LAN8742_RegisterBusIO>:

Kelaifen_V6.0/.idea/.gitignore

@@ -0,0 +1,3 @@
+# 默认忽略的文件
+/shelf/
+/workspace.xml

Kelaifen_V6.0/.idea/.name

@@ -0,0 +1 @@
+Example_0_5.py

Kelaifen_V6.0/.idea/inspectionProfiles/profiles_settings.xml

@@ -0,0 +1,6 @@
+<component name="InspectionProjectProfileManager">
+  <settings>
+    <option name="USE_PROJECT_PROFILE" value="false" />
+    <version value="1.0" />
+  </settings>
+</component>

Kelaifen_V6.0/.idea/misc.xml

@@ -0,0 +1,4 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectRootManager" version="2" project-jdk-name="Python 3.12" project-jdk-type="Python SDK" />
+</project>

Kelaifen_V6.0/.idea/modules.xml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<project version="4">
+  <component name="ProjectModuleManager">
+    <modules>
+      <module fileurl="file://$PROJECT_DIR$/.idea/树莓派上完整程序_新增功能.iml" filepath="$PROJECT_DIR$/.idea/树莓派上完整程序_新增功能.iml" />
+    </modules>
+  </component>
+</project>

Kelaifen_V6.0/.idea/树莓派上完整程序_新增功能.iml

@@ -0,0 +1,8 @@
+<?xml version="1.0" encoding="UTF-8"?>
+<module type="PYTHON_MODULE" version="4">
+  <component name="NewModuleRootManager">
+    <content url="file://$MODULE_DIR$" />
+    <orderEntry type="jdk" jdkName="Python 3.12" jdkType="Python SDK" />
+    <orderEntry type="sourceFolder" forTests="false" />
+  </component>
+</module>

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
+
+
+

Kelaifen_V6.0/Example_0_5.py

@@ -0,0 +1,396 @@
+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_off_relay_start = selected_relay_config["turn_off_relay_start"]
+                            ex_turn_on_relay_start = selected_relay_config["ex_turn_on_relay_start"]
+                            ex_turn_off_relay_start = selected_relay_config["ex_turn_off_relay_start"]
+
+                            if turn_off_relay_start >= 155:
+                                turn_off_relay_start = 154
+                            if ex_turn_off_relay_start >= 155:
+                                ex_turn_off_relay_start = 154
+
+                            print(f"\n✅ 参数已更新为选中配置：")
+                            print(f"program_delay_map: {program_delay_map}")
+                            print(f"轨迹1开枪时间：{turn_on_relay_start} ~ {turn_off_relay_start}")
+                            print(f"轨迹2开枪时间：{ex_turn_on_relay_start} ~ {ex_turn_off_relay_start}")
+                        # 喷漆开始命令接收函数
+                        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_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start)
+                        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秒，不阻塞循环）
+                                    delay = program_delay_map.get(current_program, 0.0)
+                                    if delay <= 0:
+                                        delay = 0  
+                                    current_program1 = sequence_1[current_index + 1]
+                                    delay1 = program_delay_map.get(current_program1, 0.0)
+                                    if delay1 <= 0:
+                                        delay1 = 0
+                                    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:
+                                        running = program_switch[current_program](sock, ser, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start, delay, delay1)
+                            # 回到洗枪位置指令
+                            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_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start)
+
+                        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秒，不阻塞循环）
+                                    delay = program_delay_map.get(current_program, 0.0)
+                                    if delay <= 0:
+                                        delay = 0   
+                                    current_program1 = sequence_3[current_index + 1]
+                                    delay1 = program_delay_map.get(current_program1, 0.0)
+                                    if delay1 <= 0:
+                                        delay1 = 0
+                                    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:                              
+                                        running = program_switch[current_program](sock,  ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay, delay1)
+                            #回到洗枪位置指令
+                            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("串口已关闭")
+            

Kelaifen_V6.0/Pose_5.txt

@@ -0,0 +1 @@
+318.173,-94.776,446.581,4.743102,-0.066481,-4.024368

Kelaifen_V6.0/Pose_6.txt

@@ -0,0 +1 @@
+318.901,437.099,583.004,4.685241,-0.089038,-2.083531

Kelaifen_V6.0/Pose_7.txt

@@ -0,0 +1 @@
+132.209,693.315,699.278,4.182,0.075,-0.740

Kelaifen_V6.0/Pose_8.txt

@@ -0,0 +1 @@
+663.317,241.195,699.278,4.182,0.075,2.306

Kelaifen_V6.0/config.json

@@ -0,0 +1,220 @@
+{
+    "PRESET_PARAMS": [
+        {
+            "params": [
+                80,
+                0
+            ],
+            "program_delay_map": {
+                "2": 0,
+                "300": 0,
+                "6": 0,
+                "700": 0
+            },
+            "relay_config": {
+                "turn_on_relay_start": 10,
+                "turn_off_relay_start": 144,
+                "ex_turn_on_relay_start": 10,
+                "ex_turn_off_relay_start": 144
+            }
+        },
+        {
+            "params": [
+                80,
+                0
+            ],
+            "program_delay_map": {
+                "2": 0,
+                "300": 0,
+                "6": 0,
+                "700": 0
+            },
+            "relay_config": {
+                "turn_on_relay_start": 10,
+                "turn_off_relay_start": 144,
+                "ex_turn_on_relay_start": 10,
+                "ex_turn_off_relay_start": 144
+            }
+        },
+        {
+            "params": [
+                80,
+                0
+            ],
+            "program_delay_map": {
+                "2": 0,
+                "300": 0,
+                "6": 0,
+                "700": 0
+            },
+            "relay_config": {
+                "turn_on_relay_start": 10,
+                "turn_off_relay_start": 144,
+                "ex_turn_on_relay_start": 10,
+                "ex_turn_off_relay_start": 144
+            }
+        },
+        {
+            "params": [
+                80,
+                0
+            ],
+            "program_delay_map": {
+                "2": 0,
+                "300": 0,
+                "6": 0,
+                "700": 0
+            },
+            "relay_config": {
+                "turn_on_relay_start": 10,
+                "turn_off_relay_start": 144,
+                "ex_turn_on_relay_start": 10,
+                "ex_turn_off_relay_start": 144
+            }
+        },
+        {
+            "params": [
+                80,
+                0
+            ],
+            "program_delay_map": {
+                "2": 0,
+                "300": 0,
+                "6": 0,
+                "700": 0
+            },
+            "relay_config": {
+                "turn_on_relay_start": 10,
+                "turn_off_relay_start": 144,
+                "ex_turn_on_relay_start": 10,
+                "ex_turn_off_relay_start": 144
+            }
+        },
+        {
+            "params": [
+                80,
+                0
+            ],
+            "program_delay_map": {
+                "2": 0,
+                "300": 0,
+                "6": 0,
+                "700": 0
+            },
+            "relay_config": {
+                "turn_on_relay_start": 10,
+                "turn_off_relay_start": 144,
+                "ex_turn_on_relay_start": 10,
+                "ex_turn_off_relay_start": 144
+            }
+        },
+        {
+            "params": [
+                80,
+                0
+            ],
+            "program_delay_map": {
+                "2": 0,
+                "300": 0,
+                "6": 0,
+                "700": 0
+            },
+            "relay_config": {
+                "turn_on_relay_start": 10,
+                "turn_off_relay_start": 144,
+                "ex_turn_on_relay_start": 10,
+                "ex_turn_off_relay_start": 144
+            }
+        },
+        {
+            "params": [
+                80,
+                0
+            ],
+            "program_delay_map": {
+                "2": 0,
+                "300": 0,
+                "6": 0,
+                "700": 0
+            },
+            "relay_config": {
+                "turn_on_relay_start": 10,
+                "turn_off_relay_start": 144,
+                "ex_turn_on_relay_start": 10,
+                "ex_turn_off_relay_start": 144
+            }
+        },
+        {
+            "params": [
+                80,
+                0
+            ],
+            "program_delay_map": {
+                "2": 0,
+                "300": 0,
+                "6": 0,
+                "700": 0
+            },
+            "relay_config": {
+                "turn_on_relay_start": 10,
+                "turn_off_relay_start": 144,
+                "ex_turn_on_relay_start": 10,
+                "ex_turn_off_relay_start": 144
+            }
+        },
+        {
+            "params": [
+                80,
+                0
+            ],
+            "program_delay_map": {
+                "2": 0,
+                "300": 0,
+                "6": 0,
+                "700": 0
+            },
+            "relay_config": {
+                "turn_on_relay_start": 10,
+                "turn_off_relay_start": 144,
+                "ex_turn_on_relay_start": 10,
+                "ex_turn_off_relay_start": 144
+            }
+        },
+        {
+            "params": [
+                80,
+                0
+            ],
+            "program_delay_map": {
+                "2": 0,
+                "300": 0,
+                "6": 0,
+                "700": 0
+            },
+            "relay_config": {
+                "turn_on_relay_start": 10,
+                "turn_off_relay_start": 144,
+                "ex_turn_on_relay_start": 10,
+                "ex_turn_off_relay_start": 144
+            }
+        },
+        {
+            "params": [
+                80,
+                0
+            ],
+            "program_delay_map": {
+                "2": 0,
+                "300": 0,
+                "6": 0,
+                "700": 0
+            },
+            "relay_config": {
+                "turn_on_relay_start": 10,
+                "turn_off_relay_start": 144,
+                "ex_turn_on_relay_start": 10,
+                "ex_turn_off_relay_start": 144
+            }
+        }
+    ]
+}

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}")

Kelaifen_V6.0/joint_positions_assembled_5.txt

@@ -0,0 +1 @@
+-44.397,-150.247,154.254,-92.630,87.801,17.825

Kelaifen_V6.0/joint_positions_assembled_6.txt

@@ -0,0 +1 @@
+37.181,-125.450,137.593,-98.126,88.215,26.116

Kelaifen_V6.0/joint_positions_assembled_7.txt

@@ -0,0 +1 @@
+65.718,-110.716,120.667,-78.282,94.033,2.384

Kelaifen_V6.0/joint_positions_assembled_8.txt

@@ -0,0 +1 @@
+6.496,-110.716,120.667,-78.282,94.033,2.384

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, program100, program101
+)
+
+program_switch = {
+    1:     program1,
+    100:   program1_1,
+    2:     program101,
+    3:     program3,
+    300:   program3_1,
+    4:     program4,
+    5:     program5,
+    500:   program5_1,
+    6:     program100,
+    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

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文件

Kelaifen_V6.0/serial_handler.py

@@ -0,0 +1,157 @@
+# 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"]
+            if speed_val < 50:
+                speed_val = 50
+            if speed_val > 150:
+                speed_val = 150
+            if angle_val > 200:
+                angle_val = 200
+            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

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指令帧（主指令/子指令/停止指令）的缓冲区

Kelaifen_V6.0/trajectory_data.py

@@ -0,0 +1,737 @@
+
+
+joint_positions_assembled_1 = [[-8.303461, -48.278357, 71.508258, -115.343, 89.659, -5.706]]
+
+joint_positions_assembled_2 = [[92.442,-41.967,60.096,-110.133,90.041,11.741]]
+
+joint_positions_assembled_3 = [[-8.437,-52.830,64.769,-101.519,91.959,-9.155]]
+
+joint_positions_assembled_4 = [[92.553,-45.178,53.196,-97.284,97.995,15.177]]
+
+Pose_1 = [
+    [1549.9000, -66.7470, 360.2000, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 360.2000, 3.1398, 0.0349, -1.5580],
+    [1549.9000, -66.7470, 360.2000, 3.1400, 0.0349, -1.5534],
+    [1549.9000, -66.7470, 360.2000, 3.1403, 0.0350, -1.5440],
+    [1549.9000, -66.7470, 360.2000, 3.1408, 0.0350, -1.5301],
+    [1549.9000, -66.7470, 360.2000, 3.1415, 0.0350, -1.5114],
+    [1549.9000, -66.7470, 360.2000, -3.1409, 0.0350, -1.4882],
+    [1549.9000, -66.7470, 360.2000, -3.1399, 0.0349, -1.4602],
+    [1549.9000, -66.7470, 360.2000, -3.1388, 0.0349, -1.4276],
+    [1549.9000, -66.7470, 360.2000, -3.1375, 0.0347, -1.3904],
+    [1549.9000, -66.7470, 360.2000, -3.1361, 0.0345, -1.3484],
+    [1549.5000, -66.3590, 360.2500, -3.1345, 0.0342, -1.3025],
+    [1547.7000, -64.6380, 360.4500, -3.1329, 0.0339, -1.2559],
+    [1544.4000, -61.5410, 360.8300, -3.1313, 0.0334, -1.2093],
+    [1539.7000, -57.0660, 361.3700, -3.1298, 0.0329, -1.1628],
+    [1533.6000, -51.2150, 362.0700, -3.1282, 0.0323, -1.1162],
+    [1526.0000, -43.9870, 362.9400, -3.1268, 0.0317, -1.0696],
+    [1517.0000, -35.3830, 363.9800, -3.1253, 0.0309, -1.0231],
+    [1506.6000, -25.4010, 365.1800, -3.1239, 0.0302, -0.9765],
+    [1494.7000, -14.0430, 366.5500, -3.1225, 0.0293, -0.9300],
+    [1481.4000, -1.3082, 368.0900, -3.1212, 0.0284, -0.8834],
+    [1467.0000, 12.4160, 369.7400, -3.1205, 0.0279, -0.8601],
+    [1452.6000, 26.1840, 371.4000, -3.1205, 0.0279, -0.8601],
+    [1438.2000, 39.9510, 373.0600, -3.1205, 0.0279, -0.8601],
+    [1423.8000, 53.7190, 374.7200, -3.1205, 0.0279, -0.8601],
+    [1409.3000, 67.4860, 376.3800, -3.1205, 0.0279, -0.8601],
+    [1394.9000, 81.2540, 378.0400, -3.1205, 0.0279, -0.8601],
+    [1380.5000, 95.0210, 379.7000, -3.1205, 0.0279, -0.8601],
+    [1366.1000, 108.7900, 381.3600, -3.1205, 0.0279, -0.8601],
+    [1351.7000, 122.5600, 383.0200, -3.1205, 0.0279, -0.8601],
+    [1337.3000, 136.3200, 384.6800, -3.1205, 0.0279, -0.8601],
+    [1322.9000, 150.0900, 386.3400, -3.1205, 0.0279, -0.8601],
+    [1308.5000, 163.8600, 388.0000, -3.1205, 0.0279, -0.8601],
+    [1294.1000, 177.6300, 389.6600, -3.1205, 0.0279, -0.8601],
+    [1279.7000, 191.3900, 391.3200, -3.1205, 0.0279, -0.8601],
+    [1265.2000, 205.1600, 392.9800, -3.1205, 0.0279, -0.8601],
+    [1250.8000, 218.9300, 394.6400, -3.1205, 0.0279, -0.8601],
+    [1236.4000, 232.7000, 396.3000, -3.1205, 0.0279, -0.8601],
+    [1222.0000, 246.4600, 397.9600, -3.1205, 0.0279, -0.8601],
+    [1204.0000, 263.6700, 400.0300, -3.1205, 0.0279, -0.8601],
+    [1189.6000, 277.4400, 401.6900, -3.1205, 0.0279, -0.8601],
+    [1175.2000, 291.2100, 403.3500, -3.1205, 0.0279, -0.8601],
+    [1160.8000, 304.9800, 405.0100, -3.1205, 0.0279, -0.8601],
+    [1146.3000, 318.7400, 406.6700, -3.1205, 0.0279, -0.8601],
+    [1131.9000, 332.5100, 408.3300, -3.1205, 0.0279, -0.8601],
+    [1117.5000, 346.2800, 409.9900, -3.1205, 0.0279, -0.8601],
+    [1103.1000, 360.0500, 411.6500, -3.1205, 0.0279, -0.8601],
+    [1088.7000, 373.8100, 413.3100, -3.1205, 0.0279, -0.8601],
+    [1074.3000, 387.5800, 414.9700, -3.1205, 0.0279, -0.8601],
+    [1059.9000, 401.3500, 416.6300, -3.1205, 0.0279, -0.8601],
+    [1045.5000, 415.1100, 418.2900, -3.1205, 0.0279, -0.8601],
+    [1031.0000, 428.8800, 419.9500, -3.1205, 0.0279, -0.8601],
+    [1016.6000, 442.6500, 421.6100, -3.1205, 0.0279, -0.8601],
+    [1002.2000, 456.4200, 423.2700, -3.1205, 0.0279, -0.8601],
+    [987.8000, 470.1800, 424.9300, -3.1205, 0.0279, -0.8601],
+    [973.3900, 483.9500, 426.5800, -3.1205, 0.0279, -0.8601],
+    [958.9800, 497.7200, 428.2500, -3.1205, 0.0279, -0.8601],
+    [944.5700, 511.4900, 429.9000, -3.1205, 0.0279, -0.8601],
+    [930.1600, 525.2500, 431.5600, -3.1205, 0.0279, -0.8601],
+    [915.7500, 539.0200, 433.2200, -3.1205, 0.0279, -0.8601],
+    [901.3300, 552.7900, 434.8800, -3.1205, 0.0279, -0.8601],
+    [886.9200, 566.5600, 436.5400, -3.1205, 0.0279, -0.8601],
+    [872.5100, 580.3200, 438.2000, -3.1205, 0.0279, -0.8601],
+    [858.1000, 594.0900, 439.8600, -3.1205, 0.0279, -0.8601],
+    [843.6900, 607.8600, 441.5200, -3.1205, 0.0279, -0.8601],
+    [829.2700, 621.6300, 443.1800, -3.1205, 0.0279, -0.8601],
+    [814.8600, 635.3900, 444.8400, -3.1205, 0.0279, -0.8601],
+    [800.4500, 649.1600, 446.5000, -3.1205, 0.0279, -0.8601],
+    [786.0400, 662.9300, 448.1600, -3.1205, 0.0279, -0.8601],
+    [771.6300, 676.7000, 449.8200, -3.1205, 0.0279, -0.8601],
+    [757.2200, 690.4600, 451.4800, -3.1205, 0.0279, -0.8601],
+    [742.8000, 704.2300, 453.1400, -3.1205, 0.0279, -0.8601],
+    [728.3900, 718.0000, 454.8000, -3.1205, 0.0279, -0.8601],
+    [713.9800, 731.7700, 456.4600, -3.1205, 0.0279, -0.8601],
+    [699.5700, 745.5300, 458.1200, -3.1205, 0.0279, -0.8601],
+    [685.1500, 759.3000, 459.7800, -3.1205, 0.0279, -0.8601],
+    [670.7400, 773.0700, 461.4400, -3.1205, 0.0279, -0.8601],
+    [656.3300, 786.8400, 463.1000, -3.1205, 0.0279, -0.8601],
+    [641.9200, 800.6000, 464.7600, -3.1205, 0.0279, -0.8601],
+    [627.5100, 814.3700, 466.4200, -3.1205, 0.0279, -0.8601],
+    [613.1000, 828.1400, 468.0800, -3.1205, 0.0279, -0.8601],
+    [598.6800, 841.9100, 469.7400, -3.1205, 0.0279, -0.8601],
+    [584.2700, 855.6700, 471.3900, -3.1205, 0.0279, -0.8601],
+    [569.8600, 869.4400, 473.0600, -3.1205, 0.0279, -0.8601],
+    [555.4500, 883.2100, 474.7200, -3.1205, 0.0279, -0.8601],
+    [541.0400, 896.9800, 476.3700, -3.1205, 0.0279, -0.8601],
+    [526.6200, 910.7400, 478.0300, -3.1205, 0.0279, -0.8601],
+    [512.2100, 924.5100, 479.6900, -3.1205, 0.0279, -0.8601],
+    [497.8000, 938.2800, 481.3500, -3.1205, 0.0279, -0.8601],
+    [483.3900, 952.0400, 483.0100, -3.1205, 0.0279, -0.8601],
+    [468.9800, 965.8100, 484.6700, -3.1205, 0.0279, -0.8601],
+    [454.5600, 979.5800, 486.3300, -3.1205, 0.0279, -0.8601],
+    [440.1500, 993.3500, 487.9900, -3.1205, 0.0279, -0.8601],
+    [425.7400, 1007.1000, 489.6500, -3.1205, 0.0279, -0.8601],
+    [411.3300, 1020.9000, 491.3100, -3.1205, 0.0279, -0.8601],
+    [396.9200, 1034.7000, 492.9700, -3.1205, 0.0279, -0.8601],
+    [382.5000, 1048.4000, 494.6300, -3.1205, 0.0279, -0.8601],
+    [368.0900, 1062.2000, 496.2900, -3.1205, 0.0279, -0.8601],
+    [353.6800, 1076.0000, 497.9500, -3.1205, 0.0279, -0.8601],
+    [339.2700, 1089.7000, 499.6100, -3.1205, 0.0279, -0.8601],
+    [324.8600, 1103.5000, 501.2700, -3.1205, 0.0279, -0.8601],
+    [310.4500, 1117.2000, 502.9300, -3.1205, 0.0279, -0.8601],
+    [296.0300, 1131.0000, 504.5900, -3.1205, 0.0279, -0.8601],
+    [281.6200, 1144.8000, 506.2500, -3.1205, 0.0279, -0.8601],
+    [267.2100, 1158.6000, 507.9100, -3.1205, 0.0279, -0.8601],
+    [252.8000, 1172.3000, 509.5700, -3.1205, 0.0279, -0.8601],
+    [238.3900, 1186.1000, 511.2300, -3.1205, 0.0279, -0.8601],
+    [223.9700, 1199.9000, 512.8900, -3.1205, 0.0279, -0.8601],
+    [209.5600, 1213.6000, 514.5500, -3.1205, 0.0279, -0.8601],
+    [195.1500, 1227.4000, 516.2100, -3.1205, 0.0279, -0.8601],
+    [180.7400, 1241.2000, 517.8700, -3.1205, 0.0279, -0.8601],
+    [166.3300, 1254.9000, 519.5200, -3.1205, 0.0279, -0.8601],
+    [151.9100, 1268.7000, 521.1800, -3.1205, 0.0279, -0.8601],
+    [137.5000, 1282.5000, 522.8400, -3.1205, 0.0279, -0.8601],
+    [123.0900, 1296.2000, 524.5000, -3.1205, 0.0279, -0.8601],
+    [105.0800, 1313.4000, 526.5800, -3.1205, 0.0279, -0.8601],
+    [90.6650, 1327.2000, 528.2400, -3.1205, 0.0279, -0.8601],
+    [76.2530, 1341.0000, 529.9000, -3.1205, 0.0279, -0.8601],
+    [61.8410, 1354.7000, 531.5600, -3.1205, 0.0279, -0.8601],
+    [47.4290, 1368.5000, 533.2200, -3.1205, 0.0279, -0.8601],
+    [33.0170, 1382.3000, 534.8800, -3.1205, 0.0279, -0.8601],
+    [18.6050, 1396.0000, 536.5400, -3.1205, 0.0279, -0.8601],
+    [4.1933, 1409.8000, 538.2000, -3.1205, 0.0279, -0.8601],
+    [-10.2190, 1423.6000, 539.8600, -3.1205, 0.0279, -0.8601],
+    [-24.6310, 1437.3000, 541.5100, -3.1205, 0.0279, -0.8601],
+    [-39.0420, 1451.1000, 543.1700, -3.1205, 0.0279, -0.8601],
+    [-53.4540, 1464.9000, 544.8400, -3.1205, 0.0279, -0.8601],
+    [-67.8660, 1478.7000, 546.4900, -3.1205, 0.0279, -0.8601],
+    [-82.2780, 1492.4000, 548.1500, -3.1205, 0.0279, -0.8601],
+    [-96.6900, 1506.2000, 549.8100, -3.1205, 0.0279, -0.8601],
+    [-111.1000, 1520.0000, 551.4700, -3.1205, 0.0279, -0.8601],
+    [-125.5100, 1533.7000, 553.1300, -3.1205, 0.0279, -0.8601],
+    [-139.9300, 1547.5000, 554.7900, -3.1205, 0.0279, -0.8601],
+    [-154.3400, 1561.3000, 556.4500, -3.1205, 0.0279, -0.8601],
+    [-168.1900, 1574.5000, 558.0500, -3.1200, 0.0275, -0.8428],
+    [-180.6100, 1586.3000, 559.4800, -3.1182, 0.0260, -0.7756],
+    [-191.5900, 1596.8000, 560.7400, -3.1166, 0.0245, -0.7119],
+    [-201.1300, 1606.0000, 561.8400, -3.1152, 0.0229, -0.6517],
+    [-209.2300, 1613.7000, 562.7700, -3.1139, 0.0214, -0.5949],
+    [-215.8800, 1620.0000, 563.5400, -3.1128, 0.0199, -0.5417],
+    [-221.1000, 1625.0000, 564.1400, -3.1119, 0.0184, -0.4920],
+    [-224.8700, 1628.6000, 564.5800, -3.1111, 0.0170, -0.4458],
+    [-227.2000, 1630.9000, 564.8400, -3.1103, 0.0157, -0.4030],
+    [-228.0900, 1631.7000, 564.9500, -3.1098, 0.0145, -0.3638],
+    [-228.1000, 1631.7000, 564.9500, -3.1093, 0.0133, -0.3280],
+    [-228.1000, 1631.7000, 564.9500, -3.1088, 0.0123, -0.2957],
+    [-228.1000, 1631.7000, 564.9500, -3.1085, 0.0113, -0.2669],
+    [-228.1000, 1631.7000, 564.9500, -3.1082, 0.0105, -0.2416],
+    [-228.1000, 1631.7000, 564.9500, -3.1080, 0.0098, -0.2198],
+    [-228.1000, 1631.7000, 564.9500, -3.1078, 0.0091, -0.2015],
+    [-228.1000, 1631.7000, 564.9500, -3.1077, 0.0086, -0.1867],
+    [-228.1000, 1631.7000, 564.9500, -3.1076, 0.0083, -0.1754],
+    [-228.1000, 1631.7000, 564.9500, -3.1075, 0.0080, -0.1675],
+    [-228.1000, 1631.7000, 564.9500, -3.1075, 0.0078, -0.1631],
+    [-228.1000, 1631.7000, 564.9500, -3.1075, 0.0078, -0.1622]
+]
+
+Pose_1_2 = [
+    [-228.1000, 1631.7000, 564.9500, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 554.1737, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 543.3974, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 532.6211, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 521.8447, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 511.0684, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 500.2921, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 489.5158, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 478.7395, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 467.9632, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 457.1868, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 446.4105, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 435.6342, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 424.8579, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 414.0816, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 403.3053, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 392.5289, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 381.7526, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 370.9763, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 360.2000, -3.1075, 0.0078, -0.1622]
+]
+
+Pose_2 = [
+    [-228.1000, 1631.7000, 360.2000, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 360.2000, -3.1075, 0.0078, -0.1631],
+    [-228.1000, 1631.7000, 360.2000, -3.1075, 0.0080, -0.1675],
+    [-228.1000, 1631.7000, 360.2000, -3.1076, 0.0083, -0.1754],
+    [-228.1000, 1631.7000, 360.2000, -3.1077, 0.0086, -0.1867],
+    [-228.1000, 1631.7000, 360.2000, -3.1078, 0.0091, -0.2015],
+    [-228.1000, 1631.7000, 360.2000, -3.1080, 0.0098, -0.2198],
+    [-228.1000, 1631.7000, 360.2000, -3.1082, 0.0105, -0.2416],
+    [-228.1000, 1631.7000, 360.2000, -3.1085, 0.0113, -0.2669],
+    [-228.1000, 1631.7000, 360.2000, -3.1088, 0.0123, -0.2957],
+    [-228.1000, 1631.7000, 360.2000, -3.1093, 0.0133, -0.3280],
+    [-228.0900, 1631.7000, 360.2500, -3.1098, 0.0145, -0.3638],
+    [-227.2000, 1630.9000, 360.4500, -3.1103, 0.0157, -0.4030],
+    [-224.8700, 1628.6000, 360.8300, -3.1111, 0.0170, -0.4458],
+    [-221.1000, 1625.0000, 361.3700, -3.1119, 0.0184, -0.4920],
+    [-215.8800, 1620.0000, 362.0700, -3.1128, 0.0199, -0.5417],
+    [-209.2300, 1613.7000, 362.9400, -3.1139, 0.0214, -0.5949],
+    [-201.1300, 1606.0000, 363.9800, -3.1152, 0.0229, -0.6517],
+    [-191.5900, 1596.8000, 365.1800, -3.1166, 0.0245, -0.7119],
+    [-180.6100, 1586.3000, 366.5500, -3.1182, 0.0260, -0.7756],
+    [-168.1900, 1574.5000, 368.0900, -3.1200, 0.0275, -0.8428],
+    [-154.3400, 1561.3000, 369.7400, -3.1205, 0.0279, -0.8601],
+    [-139.9300, 1547.5000, 371.4000, -3.1205, 0.0279, -0.8601],
+    [-125.5100, 1533.7000, 373.0600, -3.1205, 0.0279, -0.8601],
+    [-111.1000, 1520.0000, 374.7200, -3.1205, 0.0279, -0.8601],
+    [-96.6900, 1506.2000, 376.3800, -3.1205, 0.0279, -0.8601],
+    [-82.2780, 1492.4000, 378.0400, -3.1205, 0.0279, -0.8601],
+    [-67.8660, 1478.7000, 379.7000, -3.1205, 0.0279, -0.8601],
+    [-53.4540, 1464.9000, 381.3600, -3.1205, 0.0279, -0.8601],
+    [-39.0420, 1451.1000, 383.0200, -3.1205, 0.0279, -0.8601],
+    [-24.6310, 1437.3000, 384.6800, -3.1205, 0.0279, -0.8601],
+    [-10.2190, 1423.6000, 386.3400, -3.1205, 0.0279, -0.8601],
+    [4.1933, 1409.8000, 388.0000, -3.1205, 0.0279, -0.8601],
+    [18.6050, 1396.0000, 389.6600, -3.1205, 0.0279, -0.8601],
+    [33.0170, 1382.3000, 391.3200, -3.1205, 0.0279, -0.8601],
+    [47.4290, 1368.5000, 392.9800, -3.1205, 0.0279, -0.8601],
+    [61.8410, 1354.7000, 394.6400, -3.1205, 0.0279, -0.8601],
+    [76.2530, 1341.0000, 396.3000, -3.1205, 0.0279, -0.8601],
+    [90.6650, 1327.2000, 397.9600, -3.1205, 0.0279, -0.8601],
+    [105.0800, 1313.4000, 400.0300, -3.1205, 0.0279, -0.8601],
+    [123.0900, 1296.2000, 401.6900, -3.1205, 0.0279, -0.8601],
+    [137.5000, 1282.5000, 403.3500, -3.1205, 0.0279, -0.8601],
+    [151.9100, 1268.7000, 405.0100, -3.1205, 0.0279, -0.8601],
+    [166.3300, 1254.9000, 406.6700, -3.1205, 0.0279, -0.8601],
+    [180.7400, 1241.2000, 408.3300, -3.1205, 0.0279, -0.8601],
+    [195.1500, 1227.4000, 409.9900, -3.1205, 0.0279, -0.8601],
+    [209.5600, 1213.6000, 411.6500, -3.1205, 0.0279, -0.8601],
+    [223.9700, 1199.9000, 413.3100, -3.1205, 0.0279, -0.8601],
+    [238.3900, 1186.1000, 414.9700, -3.1205, 0.0279, -0.8601],
+    [252.8000, 1172.3000, 416.6300, -3.1205, 0.0279, -0.8601],
+    [267.2100, 1158.6000, 418.2900, -3.1205, 0.0279, -0.8601],
+    [281.6200, 1144.8000, 419.9500, -3.1205, 0.0279, -0.8601],
+    [296.0300, 1131.0000, 421.6100, -3.1205, 0.0279, -0.8601],
+    [310.4500, 1117.2000, 423.2700, -3.1205, 0.0279, -0.8601],
+    [324.8600, 1103.5000, 424.9300, -3.1205, 0.0279, -0.8601],
+    [339.2700, 1089.7000, 426.5800, -3.1205, 0.0279, -0.8601],
+    [353.6800, 1076.0000, 428.2500, -3.1205, 0.0279, -0.8601],
+    [368.0900, 1062.2000, 429.9000, -3.1205, 0.0279, -0.8601],
+    [382.5000, 1048.4000, 431.5600, -3.1205, 0.0279, -0.8601],
+    [396.9200, 1034.7000, 433.2200, -3.1205, 0.0279, -0.8601],
+    [411.3300, 1020.9000, 434.8800, -3.1205, 0.0279, -0.8601],
+    [425.7400, 1007.1000, 436.5400, -3.1205, 0.0279, -0.8601],
+    [440.1500, 993.3500, 438.2000, -3.1205, 0.0279, -0.8601],
+    [454.5600, 979.5800, 439.8600, -3.1205, 0.0279, -0.8601],
+    [468.9800, 965.8100, 441.5200, -3.1205, 0.0279, -0.8601],
+    [483.3900, 952.0400, 443.1800, -3.1205, 0.0279, -0.8601],
+    [497.8000, 938.2800, 444.8400, -3.1205, 0.0279, -0.8601],
+    [512.2100, 924.5100, 446.5000, -3.1205, 0.0279, -0.8601],
+    [526.6200, 910.7400, 448.1600, -3.1205, 0.0279, -0.8601],
+    [541.0400, 896.9800, 449.8200, -3.1205, 0.0279, -0.8601],
+    [555.4500, 883.2100, 451.4800, -3.1205, 0.0279, -0.8601],
+    [569.8600, 869.4400, 453.1400, -3.1205, 0.0279, -0.8601],
+    [584.2700, 855.6700, 454.8000, -3.1205, 0.0279, -0.8601],
+    [598.6800, 841.9100, 456.4600, -3.1205, 0.0279, -0.8601],
+    [613.1000, 828.1400, 458.1200, -3.1205, 0.0279, -0.8601],
+    [627.5100, 814.3700, 459.7800, -3.1205, 0.0279, -0.8601],
+    [641.9200, 800.6000, 461.4400, -3.1205, 0.0279, -0.8601],
+    [656.3300, 786.8400, 463.1000, -3.1205, 0.0279, -0.8601],
+    [670.7400, 773.0700, 464.7600, -3.1205, 0.0279, -0.8601],
+    [685.1500, 759.3000, 466.4200, -3.1205, 0.0279, -0.8601],
+    [699.5700, 745.5300, 468.0800, -3.1205, 0.0279, -0.8601],
+    [713.9800, 731.7700, 469.7400, -3.1205, 0.0279, -0.8601],
+    [728.3900, 718.0000, 471.3900, -3.1205, 0.0279, -0.8601],
+    [742.8000, 704.2300, 473.0600, -3.1205, 0.0279, -0.8601],
+    [757.2200, 690.4600, 474.7200, -3.1205, 0.0279, -0.8601],
+    [771.6300, 676.7000, 476.3700, -3.1205, 0.0279, -0.8601],
+    [786.0400, 662.9300, 478.0300, -3.1205, 0.0279, -0.8601],
+    [800.4500, 649.1600, 479.6900, -3.1205, 0.0279, -0.8601],
+    [814.8600, 635.3900, 481.3500, -3.1205, 0.0279, -0.8601],
+    [829.2700, 621.6300, 483.0100, -3.1205, 0.0279, -0.8601],
+    [843.6900, 607.8600, 484.6700, -3.1205, 0.0279, -0.8601],
+    [858.1000, 594.0900, 486.3300, -3.1205, 0.0279, -0.8601],
+    [872.5100, 580.3200, 487.9900, -3.1205, 0.0279, -0.8601],
+    [886.9200, 566.5600, 489.6500, -3.1205, 0.0279, -0.8601],
+    [901.3300, 552.7900, 491.3100, -3.1205, 0.0279, -0.8601],
+    [915.7500, 539.0200, 492.9700, -3.1205, 0.0279, -0.8601],
+    [930.1600, 525.2500, 494.6300, -3.1205, 0.0279, -0.8601],
+    [944.5700, 511.4900, 496.2900, -3.1205, 0.0279, -0.8601],
+    [958.9800, 497.7200, 497.9500, -3.1205, 0.0279, -0.8601],
+    [973.3900, 483.9500, 499.6100, -3.1205, 0.0279, -0.8601],
+    [987.8000, 470.1800, 501.2700, -3.1205, 0.0279, -0.8601],
+    [1002.2000, 456.4200, 502.9300, -3.1205, 0.0279, -0.8601],
+    [1016.6000, 442.6500, 504.5900, -3.1205, 0.0279, -0.8601],
+    [1031.0000, 428.8800, 506.2500, -3.1205, 0.0279, -0.8601],
+    [1045.5000, 415.1100, 507.9100, -3.1205, 0.0279, -0.8601],
+    [1059.9000, 401.3500, 509.5700, -3.1205, 0.0279, -0.8601],
+    [1074.3000, 387.5800, 511.2300, -3.1205, 0.0279, -0.8601],
+    [1088.7000, 373.8100, 512.8900, -3.1205, 0.0279, -0.8601],
+    [1103.1000, 360.0500, 514.5500, -3.1205, 0.0279, -0.8601],
+    [1117.5000, 346.2800, 516.2100, -3.1205, 0.0279, -0.8601],
+    [1131.9000, 332.5100, 517.8700, -3.1205, 0.0279, -0.8601],
+    [1146.3000, 318.7400, 519.5200, -3.1205, 0.0279, -0.8601],
+    [1160.8000, 304.9800, 521.1800, -3.1205, 0.0279, -0.8601],
+    [1175.2000, 291.2100, 522.8400, -3.1205, 0.0279, -0.8601],
+    [1189.6000, 277.4400, 524.5000, -3.1205, 0.0279, -0.8601],
+    [1204.0000, 263.6700, 526.5800, -3.1205, 0.0279, -0.8601],
+    [1222.0000, 246.4600, 528.2400, -3.1205, 0.0279, -0.8601],
+    [1236.4000, 232.7000, 529.9000, -3.1205, 0.0279, -0.8601],
+    [1250.8000, 218.9300, 531.5600, -3.1205, 0.0279, -0.8601],
+    [1265.2000, 205.1600, 533.2200, -3.1205, 0.0279, -0.8601],
+    [1279.7000, 191.3900, 534.8800, -3.1205, 0.0279, -0.8601],
+    [1294.1000, 177.6300, 536.5400, -3.1205, 0.0279, -0.8601],
+    [1308.5000, 163.8600, 538.2000, -3.1205, 0.0279, -0.8601],
+    [1322.9000, 150.0900, 539.8600, -3.1205, 0.0279, -0.8601],
+    [1337.3000, 136.3200, 541.5100, -3.1205, 0.0279, -0.8601],
+    [1351.7000, 122.5600, 543.1700, -3.1205, 0.0279, -0.8601],
+    [1366.1000, 108.7900, 544.8400, -3.1205, 0.0279, -0.8601],
+    [1380.5000, 95.0210, 546.4900, -3.1205, 0.0279, -0.8601],
+    [1394.9000, 81.2540, 548.1500, -3.1205, 0.0279, -0.8601],
+    [1409.3000, 67.4860, 549.8100, -3.1205, 0.0279, -0.8601],
+    [1423.8000, 53.7190, 551.4700, -3.1205, 0.0279, -0.8601],
+    [1438.2000, 39.9510, 553.1300, -3.1205, 0.0279, -0.8601],
+    [1452.6000, 26.1840, 554.7900, -3.1205, 0.0279, -0.8601],
+    [1467.0000, 12.4160, 556.4500, -3.1205, 0.0279, -0.8601],
+    [1481.4000, -1.3082, 558.0500, -3.1212, 0.0284, -0.8834],
+    [1494.7000, -14.0430, 559.4800, -3.1225, 0.0293, -0.9300],
+    [1506.6000, -25.4010, 560.7400, -3.1239, 0.0302, -0.9765],
+    [1517.0000, -35.3830, 561.8400, -3.1253, 0.0309, -1.0231],
+    [1526.0000, -43.9870, 562.7700, -3.1268, 0.0317, -1.0696],
+    [1533.6000, -51.2150, 563.5400, -3.1282, 0.0323, -1.1162],
+    [1539.7000, -57.0660, 564.1400, -3.1298, 0.0329, -1.1628],
+    [1544.4000, -61.5410, 564.5800, -3.1313, 0.0334, -1.2093],
+    [1547.7000, -64.6380, 564.8400, -3.1329, 0.0339, -1.2559],
+    [1549.5000, -66.3590, 564.9500, -3.1345, 0.0342, -1.3025],
+    [1549.9000, -66.7470, 564.9500, -3.1361, 0.0345, -1.3484],
+    [1549.9000, -66.7470, 564.9500, -3.1375, 0.0347, -1.3904],
+    [1549.9000, -66.7470, 564.9500, -3.1388, 0.0349, -1.4276],
+    [1549.9000, -66.7470, 564.9500, -3.1399, 0.0349, -1.4602],
+    [1549.9000, -66.7470, 564.9500, -3.1409, 0.0350, -1.4882],
+    [1549.9000, -66.7470, 564.9500, 3.1415, 0.0350, -1.5114],
+    [1549.9000, -66.7470, 564.9500, 3.1408, 0.0350, -1.5301],
+    [1549.9000, -66.7470, 564.9500, 3.1403, 0.0350, -1.5440],
+    [1549.9000, -66.7470, 564.9500, 3.1400, 0.0349, -1.5534],
+    [1549.9000, -66.7470, 564.9500, 3.1398, 0.0349, -1.5580],
+    [1549.9000, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584]
+]
+
+Pose_2_1 = [
+    [1549.9000, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 554.1737, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 543.3974, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 532.6211, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 521.8447, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 511.0684, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 500.2921, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 489.5158, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 478.7395, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 467.9632, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 457.1868, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 446.4105, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 435.6342, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 424.8579, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 414.0816, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 403.3053, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 392.5289, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 381.7526, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 370.9763, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 360.2000, 3.1398, 0.0349, -1.5584]
+]
+
+Pose_3 = [
+    [1549.9000, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 564.9500, 3.1398, 0.0349, -1.5580],
+    [1549.9000, -66.7470, 564.9500, 3.1400, 0.0349, -1.5534],
+    [1549.9000, -66.7470, 564.9500, 3.1403, 0.0350, -1.5440],
+    [1549.9000, -66.7470, 564.9500, 3.1408, 0.0350, -1.5301],
+    [1549.9000, -66.7470, 564.9500, 3.1415, 0.0350, -1.5114],
+    [1549.9000, -66.7470, 564.9500, -3.1409, 0.0350, -1.4882],
+    [1549.9000, -66.7470, 564.9500, -3.1399, 0.0349, -1.4602],
+    [1549.9000, -66.7470, 564.9500, -3.1388, 0.0349, -1.4276],
+    [1549.9000, -66.7470, 564.9500, -3.1375, 0.0347, -1.3904],
+    [1549.9000, -66.7470, 564.9500, -3.1361, 0.0345, -1.3484],
+    [1549.5000, -66.3590, 564.9500, -3.1345, 0.0342, -1.3025],
+    [1547.7000, -64.6380, 564.8400, -3.1329, 0.0339, -1.2559],
+    [1544.4000, -61.5410, 564.5800, -3.1313, 0.0334, -1.2093],
+    [1539.7000, -57.0660, 564.1400, -3.1298, 0.0329, -1.1628],
+    [1533.6000, -51.2150, 563.5400, -3.1282, 0.0323, -1.1162],
+    [1526.0000, -43.9870, 562.7700, -3.1268, 0.0317, -1.0696],
+    [1517.0000, -35.3830, 561.8400, -3.1253, 0.0309, -1.0231],
+    [1506.6000, -25.4010, 560.7400, -3.1239, 0.0302, -0.9765],
+    [1494.7000, -14.0430, 559.4800, -3.1225, 0.0293, -0.9300],
+    [1481.4000, -1.3082, 558.0500, -3.1212, 0.0284, -0.8834],
+    [1467.0000, 12.4160, 556.4500, -3.1205, 0.0279, -0.8601],
+    [1452.6000, 26.1840, 554.7900, -3.1205, 0.0279, -0.8601],
+    [1438.2000, 39.9510, 553.1300, -3.1205, 0.0279, -0.8601],
+    [1423.8000, 53.7190, 551.4700, -3.1205, 0.0279, -0.8601],
+    [1409.3000, 67.4860, 549.8100, -3.1205, 0.0279, -0.8601],
+    [1394.9000, 81.2540, 548.1500, -3.1205, 0.0279, -0.8601],
+    [1380.5000, 95.0210, 546.4900, -3.1205, 0.0279, -0.8601],
+    [1366.1000, 108.7900, 544.8400, -3.1205, 0.0279, -0.8601],
+    [1351.7000, 122.5600, 543.1700, -3.1205, 0.0279, -0.8601],
+    [1337.3000, 136.3200, 541.5100, -3.1205, 0.0279, -0.8601],
+    [1322.9000, 150.0900, 539.8600, -3.1205, 0.0279, -0.8601],
+    [1308.5000, 163.8600, 538.2000, -3.1205, 0.0279, -0.8601],
+    [1294.1000, 177.6300, 536.5400, -3.1205, 0.0279, -0.8601],
+    [1279.7000, 191.3900, 534.8800, -3.1205, 0.0279, -0.8601],
+    [1265.2000, 205.1600, 533.2200, -3.1205, 0.0279, -0.8601],
+    [1250.8000, 218.9300, 531.5600, -3.1205, 0.0279, -0.8601],
+    [1236.4000, 232.7000, 529.9000, -3.1205, 0.0279, -0.8601],
+    [1222.0000, 246.4600, 528.2400, -3.1205, 0.0279, -0.8601],
+    [1204.0000, 263.6700, 526.5800, -3.1205, 0.0279, -0.8601],
+    [1189.6000, 277.4400, 524.5000, -3.1205, 0.0279, -0.8601],
+    [1175.2000, 291.2100, 522.8400, -3.1205, 0.0279, -0.8601],
+    [1160.8000, 304.9800, 521.1800, -3.1205, 0.0279, -0.8601],
+    [1146.3000, 318.7400, 519.5200, -3.1205, 0.0279, -0.8601],
+    [1131.9000, 332.5100, 517.8700, -3.1205, 0.0279, -0.8601],
+    [1117.5000, 346.2800, 516.2100, -3.1205, 0.0279, -0.8601],
+    [1103.1000, 360.0500, 514.5500, -3.1205, 0.0279, -0.8601],
+    [1088.7000, 373.8100, 512.8900, -3.1205, 0.0279, -0.8601],
+    [1074.3000, 387.5800, 511.2300, -3.1205, 0.0279, -0.8601],
+    [1059.9000, 401.3500, 509.5700, -3.1205, 0.0279, -0.8601],
+    [1045.5000, 415.1100, 507.9100, -3.1205, 0.0279, -0.8601],
+    [1031.0000, 428.8800, 506.2500, -3.1205, 0.0279, -0.8601],
+    [1016.6000, 442.6500, 504.5900, -3.1205, 0.0279, -0.8601],
+    [1002.2000, 456.4200, 502.9300, -3.1205, 0.0279, -0.8601],
+    [987.8000, 470.1800, 501.2700, -3.1205, 0.0279, -0.8601],
+    [973.3900, 483.9500, 499.6100, -3.1205, 0.0279, -0.8601],
+    [958.9800, 497.7200, 497.9500, -3.1205, 0.0279, -0.8601],
+    [944.5700, 511.4900, 496.2900, -3.1205, 0.0279, -0.8601],
+    [930.1600, 525.2500, 494.6300, -3.1205, 0.0279, -0.8601],
+    [915.7500, 539.0200, 492.9700, -3.1205, 0.0279, -0.8601],
+    [901.3300, 552.7900, 491.3100, -3.1205, 0.0279, -0.8601],
+    [886.9200, 566.5600, 489.6500, -3.1205, 0.0279, -0.8601],
+    [872.5100, 580.3200, 487.9900, -3.1205, 0.0279, -0.8601],
+    [858.1000, 594.0900, 486.3300, -3.1205, 0.0279, -0.8601],
+    [843.6900, 607.8600, 484.6700, -3.1205, 0.0279, -0.8601],
+    [829.2700, 621.6300, 483.0100, -3.1205, 0.0279, -0.8601],
+    [814.8600, 635.3900, 481.3500, -3.1205, 0.0279, -0.8601],
+    [800.4500, 649.1600, 479.6900, -3.1205, 0.0279, -0.8601],
+    [786.0400, 662.9300, 478.0300, -3.1205, 0.0279, -0.8601],
+    [771.6300, 676.7000, 476.3700, -3.1205, 0.0279, -0.8601],
+    [757.2200, 690.4600, 474.7200, -3.1205, 0.0279, -0.8601],
+    [742.8000, 704.2300, 473.0600, -3.1205, 0.0279, -0.8601],
+    [728.3900, 718.0000, 471.3900, -3.1205, 0.0279, -0.8601],
+    [713.9800, 731.7700, 469.7400, -3.1205, 0.0279, -0.8601],
+    [699.5700, 745.5300, 468.0800, -3.1205, 0.0279, -0.8601],
+    [685.1500, 759.3000, 466.4200, -3.1205, 0.0279, -0.8601],
+    [670.7400, 773.0700, 464.7600, -3.1205, 0.0279, -0.8601],
+    [656.3300, 786.8400, 463.1000, -3.1205, 0.0279, -0.8601],
+    [641.9200, 800.6000, 461.4400, -3.1205, 0.0279, -0.8601],
+    [627.5100, 814.3700, 459.7800, -3.1205, 0.0279, -0.8601],
+    [613.1000, 828.1400, 458.1200, -3.1205, 0.0279, -0.8601],
+    [598.6800, 841.9100, 456.4600, -3.1205, 0.0279, -0.8601],
+    [584.2700, 855.6700, 454.8000, -3.1205, 0.0279, -0.8601],
+    [569.8600, 869.4400, 453.1400, -3.1205, 0.0279, -0.8601],
+    [555.4500, 883.2100, 451.4800, -3.1205, 0.0279, -0.8601],
+    [541.0400, 896.9800, 449.8200, -3.1205, 0.0279, -0.8601],
+    [526.6200, 910.7400, 448.1600, -3.1205, 0.0279, -0.8601],
+    [512.2100, 924.5100, 446.5000, -3.1205, 0.0279, -0.8601],
+    [497.8000, 938.2800, 444.8400, -3.1205, 0.0279, -0.8601],
+    [483.3900, 952.0400, 443.1800, -3.1205, 0.0279, -0.8601],
+    [468.9800, 965.8100, 441.5200, -3.1205, 0.0279, -0.8601],
+    [454.5600, 979.5800, 439.8600, -3.1205, 0.0279, -0.8601],
+    [440.1500, 993.3500, 438.2000, -3.1205, 0.0279, -0.8601],
+    [425.7400, 1007.1000, 436.5400, -3.1205, 0.0279, -0.8601],
+    [411.3300, 1020.9000, 434.8800, -3.1205, 0.0279, -0.8601],
+    [396.9200, 1034.7000, 433.2200, -3.1205, 0.0279, -0.8601],
+    [382.5000, 1048.4000, 431.5600, -3.1205, 0.0279, -0.8601],
+    [368.0900, 1062.2000, 429.9000, -3.1205, 0.0279, -0.8601],
+    [353.6800, 1076.0000, 428.2500, -3.1205, 0.0279, -0.8601],
+    [339.2700, 1089.7000, 426.5800, -3.1205, 0.0279, -0.8601],
+    [324.8600, 1103.5000, 424.9300, -3.1205, 0.0279, -0.8601],
+    [310.4500, 1117.2000, 423.2700, -3.1205, 0.0279, -0.8601],
+    [296.0300, 1131.0000, 421.6100, -3.1205, 0.0279, -0.8601],
+    [281.6200, 1144.8000, 419.9500, -3.1205, 0.0279, -0.8601],
+    [267.2100, 1158.6000, 418.2900, -3.1205, 0.0279, -0.8601],
+    [252.8000, 1172.3000, 416.6300, -3.1205, 0.0279, -0.8601],
+    [238.3900, 1186.1000, 414.9700, -3.1205, 0.0279, -0.8601],
+    [223.9700, 1199.9000, 413.3100, -3.1205, 0.0279, -0.8601],
+    [209.5600, 1213.6000, 411.6500, -3.1205, 0.0279, -0.8601],
+    [195.1500, 1227.4000, 409.9900, -3.1205, 0.0279, -0.8601],
+    [180.7400, 1241.2000, 408.3300, -3.1205, 0.0279, -0.8601],
+    [166.3300, 1254.9000, 406.6700, -3.1205, 0.0279, -0.8601],
+    [151.9100, 1268.7000, 405.0100, -3.1205, 0.0279, -0.8601],
+    [137.5000, 1282.5000, 403.3500, -3.1205, 0.0279, -0.8601],
+    [123.0900, 1296.2000, 401.6900, -3.1205, 0.0279, -0.8601],
+    [105.0800, 1313.4000, 400.0300, -3.1205, 0.0279, -0.8601],
+    [90.6650, 1327.2000, 397.9600, -3.1205, 0.0279, -0.8601],
+    [76.2530, 1341.0000, 396.3000, -3.1205, 0.0279, -0.8601],
+    [61.8410, 1354.7000, 394.6400, -3.1205, 0.0279, -0.8601],
+    [47.4290, 1368.5000, 392.9800, -3.1205, 0.0279, -0.8601],
+    [33.0170, 1382.3000, 391.3200, -3.1205, 0.0279, -0.8601],
+    [18.6050, 1396.0000, 389.6600, -3.1205, 0.0279, -0.8601],
+    [4.1933, 1409.8000, 388.0000, -3.1205, 0.0279, -0.8601],
+    [-10.2190, 1423.6000, 386.3400, -3.1205, 0.0279, -0.8601],
+    [-24.6310, 1437.3000, 384.6800, -3.1205, 0.0279, -0.8601],
+    [-39.0420, 1451.1000, 383.0200, -3.1205, 0.0279, -0.8601],
+    [-53.4540, 1464.9000, 381.3600, -3.1205, 0.0279, -0.8601],
+    [-67.8660, 1478.7000, 379.7000, -3.1205, 0.0279, -0.8601],
+    [-82.2780, 1492.4000, 378.0400, -3.1205, 0.0279, -0.8601],
+    [-96.6900, 1506.2000, 376.3800, -3.1205, 0.0279, -0.8601],
+    [-111.1000, 1520.0000, 374.7200, -3.1205, 0.0279, -0.8601],
+    [-125.5100, 1533.7000, 373.0600, -3.1205, 0.0279, -0.8601],
+    [-139.9300, 1547.5000, 371.4000, -3.1205, 0.0279, -0.8601],
+    [-154.3400, 1561.3000, 369.7400, -3.1205, 0.0279, -0.8601],
+    [-168.1900, 1574.5000, 368.0900, -3.1200, 0.0275, -0.8428],
+    [-180.6100, 1586.3000, 366.5500, -3.1182, 0.0260, -0.7756],
+    [-191.5900, 1596.8000, 365.1800, -3.1166, 0.0245, -0.7119],
+    [-201.1300, 1606.0000, 363.9800, -3.1152, 0.0229, -0.6517],
+    [-209.2300, 1613.7000, 362.9400, -3.1139, 0.0214, -0.5949],
+    [-215.8800, 1620.0000, 362.0700, -3.1128, 0.0199, -0.5417],
+    [-221.1000, 1625.0000, 361.3700, -3.1119, 0.0184, -0.4920],
+    [-224.8700, 1628.6000, 360.8300, -3.1111, 0.0170, -0.4458],
+    [-227.2000, 1630.9000, 360.4500, -3.1103, 0.0157, -0.4030],
+    [-228.0900, 1631.7000, 360.2500, -3.1098, 0.0145, -0.3638],
+    [-228.1000, 1631.7000, 360.2000, -3.1093, 0.0133, -0.3280],
+    [-228.1000, 1631.7000, 360.2000, -3.1088, 0.0123, -0.2957],
+    [-228.1000, 1631.7000, 360.2000, -3.1085, 0.0113, -0.2669],
+    [-228.1000, 1631.7000, 360.2000, -3.1082, 0.0105, -0.2416],
+    [-228.1000, 1631.7000, 360.2000, -3.1080, 0.0098, -0.2198],
+    [-228.1000, 1631.7000, 360.2000, -3.1078, 0.0091, -0.2015],
+    [-228.1000, 1631.7000, 360.2000, -3.1077, 0.0086, -0.1867],
+    [-228.1000, 1631.7000, 360.2000, -3.1076, 0.0083, -0.1754],
+    [-228.1000, 1631.7000, 360.2000, -3.1075, 0.0080, -0.1675],
+    [-228.1000, 1631.7000, 360.2000, -3.1075, 0.0078, -0.1631],
+    [-228.1000, 1631.7000, 360.2000, -3.1075, 0.0078, -0.1622]
+]
+
+
+Pose_3_4 = [
+    [-228.1000, 1631.7000, 360.2000, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 370.9763, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 381.7526, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 392.5289, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 403.3053, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 414.0816, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 424.8579, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 435.6342, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 446.4105, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 457.1868, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 467.9632, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 478.7395, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 489.5158, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 500.2921, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 511.0684, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 521.8447, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 532.6211, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 543.3974, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 554.1737, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 564.9500, -3.1075, 0.0078, -0.1622]
+]
+
+Pose_4 = [
+    [-228.1000, 1631.7000, 564.9500, -3.1075, 0.0078, -0.1622],
+    [-228.1000, 1631.7000, 564.9500, -3.1075, 0.0078, -0.1631],
+    [-228.1000, 1631.7000, 564.9500, -3.1075, 0.0080, -0.1675],
+    [-228.1000, 1631.7000, 564.9500, -3.1076, 0.0083, -0.1754],
+    [-228.1000, 1631.7000, 564.9500, -3.1077, 0.0086, -0.1867],
+    [-228.1000, 1631.7000, 564.9500, -3.1078, 0.0091, -0.2015],
+    [-228.1000, 1631.7000, 564.9500, -3.1080, 0.0098, -0.2198],
+    [-228.1000, 1631.7000, 564.9500, -3.1082, 0.0105, -0.2416],
+    [-228.1000, 1631.7000, 564.9500, -3.1085, 0.0113, -0.2669],
+    [-228.1000, 1631.7000, 564.9500, -3.1088, 0.0123, -0.2957],
+    [-228.1000, 1631.7000, 564.9500, -3.1093, 0.0133, -0.3280],
+    [-228.0900, 1631.7000, 564.9500, -3.1098, 0.0145, -0.3638],
+    [-227.2000, 1630.9000, 564.8400, -3.1103, 0.0157, -0.4030],
+    [-224.8700, 1628.6000, 564.5800, -3.1111, 0.0170, -0.4458],
+    [-221.1000, 1625.0000, 564.1400, -3.1119, 0.0184, -0.4920],
+    [-215.8800, 1620.0000, 563.5400, -3.1128, 0.0199, -0.5417],
+    [-209.2300, 1613.7000, 562.7700, -3.1139, 0.0214, -0.5949],
+    [-201.1300, 1606.0000, 561.8400, -3.1152, 0.0229, -0.6517],
+    [-191.5900, 1596.8000, 560.7400, -3.1166, 0.0245, -0.7119],
+    [-180.6100, 1586.3000, 559.4800, -3.1182, 0.0260, -0.7756],
+    [-168.1900, 1574.5000, 558.0500, -3.1200, 0.0275, -0.8428],
+    [-154.3400, 1561.3000, 556.4500, -3.1205, 0.0279, -0.8601],
+    [-139.9300, 1547.5000, 554.7900, -3.1205, 0.0279, -0.8601],
+    [-125.5100, 1533.7000, 553.1300, -3.1205, 0.0279, -0.8601],
+    [-111.1000, 1520.0000, 551.4700, -3.1205, 0.0279, -0.8601],
+    [-96.6900, 1506.2000, 549.8100, -3.1205, 0.0279, -0.8601],
+    [-82.2780, 1492.4000, 548.1500, -3.1205, 0.0279, -0.8601],
+    [-67.8660, 1478.7000, 546.4900, -3.1205, 0.0279, -0.8601],
+    [-53.4540, 1464.9000, 544.8400, -3.1205, 0.0279, -0.8601],
+    [-39.0420, 1451.1000, 543.1700, -3.1205, 0.0279, -0.8601],
+    [-24.6310, 1437.3000, 541.5100, -3.1205, 0.0279, -0.8601],
+    [-10.2190, 1423.6000, 539.8600, -3.1205, 0.0279, -0.8601],
+    [4.1933, 1409.8000, 538.2000, -3.1205, 0.0279, -0.8601],
+    [18.6050, 1396.0000, 536.5400, -3.1205, 0.0279, -0.8601],
+    [33.0170, 1382.3000, 534.8800, -3.1205, 0.0279, -0.8601],
+    [47.4290, 1368.5000, 533.2200, -3.1205, 0.0279, -0.8601],
+    [61.8410, 1354.7000, 531.5600, -3.1205, 0.0279, -0.8601],
+    [76.2530, 1341.0000, 529.9000, -3.1205, 0.0279, -0.8601],
+    [90.6650, 1327.2000, 528.2400, -3.1205, 0.0279, -0.8601],
+    [105.0800, 1313.4000, 526.5800, -3.1205, 0.0279, -0.8601],
+    [123.0900, 1296.2000, 524.5000, -3.1205, 0.0279, -0.8601],
+    [137.5000, 1282.5000, 522.8400, -3.1205, 0.0279, -0.8601],
+    [151.9100, 1268.7000, 521.1800, -3.1205, 0.0279, -0.8601],
+    [166.3300, 1254.9000, 519.5200, -3.1205, 0.0279, -0.8601],
+    [180.7400, 1241.2000, 517.8700, -3.1205, 0.0279, -0.8601],
+    [195.1500, 1227.4000, 516.2100, -3.1205, 0.0279, -0.8601],
+    [209.5600, 1213.6000, 514.5500, -3.1205, 0.0279, -0.8601],
+    [223.9700, 1199.9000, 512.8900, -3.1205, 0.0279, -0.8601],
+    [238.3900, 1186.1000, 511.2300, -3.1205, 0.0279, -0.8601],
+    [252.8000, 1172.3000, 509.5700, -3.1205, 0.0279, -0.8601],
+    [267.2100, 1158.6000, 507.9100, -3.1205, 0.0279, -0.8601],
+    [281.6200, 1144.8000, 506.2500, -3.1205, 0.0279, -0.8601],
+    [296.0300, 1131.0000, 504.5900, -3.1205, 0.0279, -0.8601],
+    [310.4500, 1117.2000, 502.9300, -3.1205, 0.0279, -0.8601],
+    [324.8600, 1103.5000, 501.2700, -3.1205, 0.0279, -0.8601],
+    [339.2700, 1089.7000, 499.6100, -3.1205, 0.0279, -0.8601],
+    [353.6800, 1076.0000, 497.9500, -3.1205, 0.0279, -0.8601],
+    [368.0900, 1062.2000, 496.2900, -3.1205, 0.0279, -0.8601],
+    [382.5000, 1048.4000, 494.6300, -3.1205, 0.0279, -0.8601],
+    [396.9200, 1034.7000, 492.9700, -3.1205, 0.0279, -0.8601],
+    [411.3300, 1020.9000, 491.3100, -3.1205, 0.0279, -0.8601],
+    [425.7400, 1007.1000, 489.6500, -3.1205, 0.0279, -0.8601],
+    [440.1500, 993.3500, 487.9900, -3.1205, 0.0279, -0.8601],
+    [454.5600, 979.5800, 486.3300, -3.1205, 0.0279, -0.8601],
+    [468.9800, 965.8100, 484.6700, -3.1205, 0.0279, -0.8601],
+    [483.3900, 952.0400, 483.0100, -3.1205, 0.0279, -0.8601],
+    [497.8000, 938.2800, 481.3500, -3.1205, 0.0279, -0.8601],
+    [512.2100, 924.5100, 479.6900, -3.1205, 0.0279, -0.8601],
+    [526.6200, 910.7400, 478.0300, -3.1205, 0.0279, -0.8601],
+    [541.0400, 896.9800, 476.3700, -3.1205, 0.0279, -0.8601],
+    [555.4500, 883.2100, 474.7200, -3.1205, 0.0279, -0.8601],
+    [569.8600, 869.4400, 473.0600, -3.1205, 0.0279, -0.8601],
+    [584.2700, 855.6700, 471.3900, -3.1205, 0.0279, -0.8601],
+    [598.6800, 841.9100, 469.7400, -3.1205, 0.0279, -0.8601],
+    [613.1000, 828.1400, 468.0800, -3.1205, 0.0279, -0.8601],
+    [627.5100, 814.3700, 466.4200, -3.1205, 0.0279, -0.8601],
+    [641.9200, 800.6000, 464.7600, -3.1205, 0.0279, -0.8601],
+    [656.3300, 786.8400, 463.1000, -3.1205, 0.0279, -0.8601],
+    [670.7400, 773.0700, 461.4400, -3.1205, 0.0279, -0.8601],
+    [685.1500, 759.3000, 459.7800, -3.1205, 0.0279, -0.8601],
+    [699.5700, 745.5300, 458.1200, -3.1205, 0.0279, -0.8601],
+    [713.9800, 731.7700, 456.4600, -3.1205, 0.0279, -0.8601],
+    [728.3900, 718.0000, 454.8000, -3.1205, 0.0279, -0.8601],
+    [742.8000, 704.2300, 453.1400, -3.1205, 0.0279, -0.8601],
+    [757.2200, 690.4600, 451.4800, -3.1205, 0.0279, -0.8601],
+    [771.6300, 676.7000, 449.8200, -3.1205, 0.0279, -0.8601],
+    [786.0400, 662.9300, 448.1600, -3.1205, 0.0279, -0.8601],
+    [800.4500, 649.1600, 446.5000, -3.1205, 0.0279, -0.8601],
+    [814.8600, 635.3900, 444.8400, -3.1205, 0.0279, -0.8601],
+    [829.2700, 621.6300, 443.1800, -3.1205, 0.0279, -0.8601],
+    [843.6900, 607.8600, 441.5200, -3.1205, 0.0279, -0.8601],
+    [858.1000, 594.0900, 439.8600, -3.1205, 0.0279, -0.8601],
+    [872.5100, 580.3200, 438.2000, -3.1205, 0.0279, -0.8601],
+    [886.9200, 566.5600, 436.5400, -3.1205, 0.0279, -0.8601],
+    [901.3300, 552.7900, 434.8800, -3.1205, 0.0279, -0.8601],
+    [915.7500, 539.0200, 433.2200, -3.1205, 0.0279, -0.8601],
+    [930.1600, 525.2500, 431.5600, -3.1205, 0.0279, -0.8601],
+    [944.5700, 511.4900, 429.9000, -3.1205, 0.0279, -0.8601],
+    [958.9800, 497.7200, 428.2500, -3.1205, 0.0279, -0.8601],
+    [973.3900, 483.9500, 426.5800, -3.1205, 0.0279, -0.8601],
+    [987.8000, 470.1800, 424.9300, -3.1205, 0.0279, -0.8601],
+    [1002.2000, 456.4200, 423.2700, -3.1205, 0.0279, -0.8601],
+    [1016.6000, 442.6500, 421.6100, -3.1205, 0.0279, -0.8601],
+    [1031.0000, 428.8800, 419.9500, -3.1205, 0.0279, -0.8601],
+    [1045.5000, 415.1100, 418.2900, -3.1205, 0.0279, -0.8601],
+    [1059.9000, 401.3500, 416.6300, -3.1205, 0.0279, -0.8601],
+    [1074.3000, 387.5800, 414.9700, -3.1205, 0.0279, -0.8601],
+    [1088.7000, 373.8100, 413.3100, -3.1205, 0.0279, -0.8601],
+    [1103.1000, 360.0500, 411.6500, -3.1205, 0.0279, -0.8601],
+    [1117.5000, 346.2800, 409.9900, -3.1205, 0.0279, -0.8601],
+    [1131.9000, 332.5100, 408.3300, -3.1205, 0.0279, -0.8601],
+    [1146.3000, 318.7400, 406.6700, -3.1205, 0.0279, -0.8601],
+    [1160.8000, 304.9800, 405.0100, -3.1205, 0.0279, -0.8601],
+    [1175.2000, 291.2100, 403.3500, -3.1205, 0.0279, -0.8601],
+    [1189.6000, 277.4400, 401.6900, -3.1205, 0.0279, -0.8601],
+    [1204.0000, 263.6700, 400.0300, -3.1205, 0.0279, -0.8601],
+    [1222.0000, 246.4600, 397.9600, -3.1205, 0.0279, -0.8601],
+    [1236.4000, 232.7000, 396.3000, -3.1205, 0.0279, -0.8601],
+    [1250.8000, 218.9300, 394.6400, -3.1205, 0.0279, -0.8601],
+    [1265.2000, 205.1600, 392.9800, -3.1205, 0.0279, -0.8601],
+    [1279.7000, 191.3900, 391.3200, -3.1205, 0.0279, -0.8601],
+    [1294.1000, 177.6300, 389.6600, -3.1205, 0.0279, -0.8601],
+    [1308.5000, 163.8600, 388.0000, -3.1205, 0.0279, -0.8601],
+    [1322.9000, 150.0900, 386.3400, -3.1205, 0.0279, -0.8601],
+    [1337.3000, 136.3200, 384.6800, -3.1205, 0.0279, -0.8601],
+    [1351.7000, 122.5600, 383.0200, -3.1205, 0.0279, -0.8601],
+    [1366.1000, 108.7900, 381.3600, -3.1205, 0.0279, -0.8601],
+    [1380.5000, 95.0210, 379.7000, -3.1205, 0.0279, -0.8601],
+    [1394.9000, 81.2540, 378.0400, -3.1205, 0.0279, -0.8601],
+    [1409.3000, 67.4860, 376.3800, -3.1205, 0.0279, -0.8601],
+    [1423.8000, 53.7190, 374.7200, -3.1205, 0.0279, -0.8601],
+    [1438.2000, 39.9510, 373.0600, -3.1205, 0.0279, -0.8601],
+    [1452.6000, 26.1840, 371.4000, -3.1205, 0.0279, -0.8601],
+    [1467.0000, 12.4160, 369.7400, -3.1205, 0.0279, -0.8601],
+    [1481.4000, -1.3082, 368.0900, -3.1212, 0.0284, -0.8834],
+    [1494.7000, -14.0430, 366.5500, -3.1225, 0.0293, -0.9300],
+    [1506.6000, -25.4010, 365.1800, -3.1239, 0.0302, -0.9765],
+    [1517.0000, -35.3830, 363.9800, -3.1253, 0.0309, -1.0231],
+    [1526.0000, -43.9870, 362.9400, -3.1268, 0.0317, -1.0696],
+    [1533.6000, -51.2150, 362.0700, -3.1282, 0.0323, -1.1162],
+    [1539.7000, -57.0660, 361.3700, -3.1298, 0.0329, -1.1628],
+    [1544.4000, -61.5410, 360.8300, -3.1313, 0.0334, -1.2093],
+    [1547.7000, -64.6380, 360.4500, -3.1329, 0.0339, -1.2559],
+    [1549.5000, -66.3590, 360.2500, -3.1345, 0.0342, -1.3025],
+    [1549.9000, -66.7470, 360.2000, -3.1361, 0.0345, -1.3484],
+    [1549.9000, -66.7470, 360.2000, -3.1375, 0.0347, -1.3904],
+    [1549.9000, -66.7470, 360.2000, -3.1388, 0.0349, -1.4276],
+    [1549.9000, -66.7470, 360.2000, -3.1399, 0.0349, -1.4602],
+    [1549.9000, -66.7470, 360.2000, -3.1409, 0.0350, -1.4882],
+    [1549.9000, -66.7470, 360.2000, 3.1415, 0.0350, -1.5114],
+    [1549.9000, -66.7470, 360.2000, 3.1408, 0.0350, -1.5301],
+    [1549.9000, -66.7470, 360.2000, 3.1403, 0.0350, -1.5440],
+    [1549.9000, -66.7470, 360.2000, 3.1400, 0.0349, -1.5534],
+    [1549.9000, -66.7470, 360.2000, 3.1398, 0.0349, -1.5580],
+    [1549.9000, -66.7470, 360.2000, 3.1398, 0.0349, -1.5584]
+]
+
+Pose_4_3 = [
+    [1549.9000, -66.7470, 360.2000, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 370.9763, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 381.7526, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 392.5289, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 403.3053, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 414.0816, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 424.8579, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 435.6342, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 446.4105, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 457.1868, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 467.9632, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 478.7395, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 489.5158, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 500.2921, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 511.0684, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 521.8447, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 532.6211, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 543.3974, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 554.1737, 3.1398, 0.0349, -1.5584],
+    [1549.9000, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584]
+]
+
+
+

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