diff --git a/kelaifen/Kelaifen_V6.0/Example_0_5.py b/kelaifen/Kelaifen_V6.0/Example_0_5.py
index a2c297e..868e152 100644
--- a/kelaifen/Kelaifen_V6.0/Example_0_5.py
+++ b/kelaifen/Kelaifen_V6.0/Example_0_5.py
@@ -154,20 +154,14 @@ if __name__ == "__main__":
                             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"]
                             ex_turn_on_relay_start = selected_relay_config["ex_turn_on_relay_start"]
-                            ex_turn_on_relay_end = selected_relay_config["ex_turn_on_relay_end"]
                             ex_turn_off_relay_start = selected_relay_config["ex_turn_off_relay_start"]
-                            ex_turn_off_relay_end = selected_relay_config["ex_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}")
-                            print(f"继电器开启区间2：{ex_turn_on_relay_start} ~ {ex_turn_on_relay_end}")
-                            print(f"继电器关闭区间2：{ex_turn_off_relay_start} ~ {ex_turn_off_relay_end}")
+                            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)
                     # 解析主指令（帧头后第一字节为指令值）
@@ -178,7 +172,7 @@ if __name__ == "__main__":
                     # 情况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, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end)
+                        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):
@@ -217,7 +211,7 @@ if __name__ == "__main__":
                                         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, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end)
+                                        program_switch[current_program](sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start)
                                         # print(current_program)
                                 
                                     # 按程序编号从字典获取延时，精准执行
@@ -250,7 +244,7 @@ if __name__ == "__main__":
                     # 情况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, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end)
+                        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):
@@ -282,9 +276,8 @@ if __name__ == "__main__":
                                     stop_cmd_bytes = read_cmd_from_shared(timeout=0.1)
                                     delay = program_delay_map.get(current_program, 0.0)
                                     if delay <= 0:
-                                        delay = 0
-                                    else:                                    
-                                        running = program_switch[current_program](sock,  ser, stop_cmd_bytes, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay)
+                                        delay = 0                                 
+                                    running = program_switch[current_program](sock,  ser, stop_cmd_bytes, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay)
 
                                     # 统一字典映射延时
                                     # delay = program_delay_map.get(current_program, 0.0)
diff --git a/kelaifen/Kelaifen_V6.0/config.json b/kelaifen/Kelaifen_V6.0/config.json
index b85ff5c..43d26a8 100644
--- a/kelaifen/Kelaifen_V6.0/config.json
+++ b/kelaifen/Kelaifen_V6.0/config.json
@@ -1,7 +1,7 @@
 {
     "PRESET_PARAMS": [
         {
-            "params": [50, -100],
+            "params": [80, -150],
             "program_delay_map": {
                 "1": 1.0,
                 "100": 0.1,
@@ -11,20 +11,20 @@
                 "4": 0.1,
                 "5": 1.0,
                 "500": 0.1,
-                "6": 0.1,
+                "6": 10,
                 "7": 1.0,
                 "700": 0.1,
-                "8": 0.1
+                "8": 0.2
             },
             "relay_config": {
-                "turn_on_relay_start": 20,
-                "turn_off_relay_start": 135,
-				"ex_turn_on_relay_start": 20,
-                "ex_turn_off_relay_start": 135
+                "turn_on_relay_start": 0,
+                "turn_off_relay_start": 134,
+				        "ex_turn_on_relay_start": 0,
+                "ex_turn_off_relay_start": 134
             }
         },
         {
-            "params": [50, -50],
+            "params": [80, -150],
             "program_delay_map": {
                 "1": 1.0,
                 "100": 0.1,
@@ -34,20 +34,20 @@
                 "4": 0.1,
                 "5": 1.0,
                 "500": 0.1,
-                "6": 0.5,
+                "6": 10,
                 "7": 1.0,
                 "700": 0.1,
-                "8": 0.5
+                "8": 0.2
             },
             "relay_config": {
-                "turn_on_relay_start": 20,
-                "turn_off_relay_start": 135,
-				"ex_turn_on_relay_start": 20,
-                "ex_turn_off_relay_start": 135
+                "turn_on_relay_start": 0,
+                "turn_off_relay_start": 134,
+				        "ex_turn_on_relay_start": 0,
+                "ex_turn_off_relay_start": 134
             }
         },
         {
-            "params": [50, 0],
+            "params": [80, -150],
             "program_delay_map": {
                 "1": 1.0,
                 "100": 0.1,
@@ -57,20 +57,20 @@
                 "4": 0.1,
                 "5": 1.0,
                 "500": 0.1,
-                "6": 0.5,
+                "6": 10,
                 "7": 1.0,
                 "700": 0.1,
-                "8": 0.5
+                "8": 0.2
             },
             "relay_config": {
-                "turn_on_relay_start": 20,
-                "turn_off_relay_start": 135,
-				"ex_turn_on_relay_start": 20,
-                "ex_turn_off_relay_start": 135
+                "turn_on_relay_start": 0,
+                "turn_off_relay_start": 134,
+				        "ex_turn_on_relay_start": 0,
+                "ex_turn_off_relay_start": 134
             }
         },
         {
-            "params": [50, 50],
+            "params": [80, -150],
             "program_delay_map": {
                 "1": 1.0,
                 "100": 0.1,
@@ -80,20 +80,20 @@
                 "4": 0.1,
                 "5": 1.0,
                 "500": 0.1,
-                "6": 0.5,
+                "6": 10,
                 "7": 1.0,
                 "700": 0.1,
-                "8": 0.5
+                "8": 0.2
             },
             "relay_config": {
-                "turn_on_relay_start": 20,
-                "turn_off_relay_start": 135,
-				"ex_turn_on_relay_start": 20,
-                "ex_turn_off_relay_start": 135
+                "turn_on_relay_start": 0,
+                "turn_off_relay_start": 134,
+				        "ex_turn_on_relay_start": 0,
+                "ex_turn_off_relay_start": 134
             }
         },
         {
-            "params": [50, 100],
+            "params": [80, -150],
             "program_delay_map": {
                 "1": 1.0,
                 "100": 0.1,
@@ -103,20 +103,20 @@
                 "4": 0.1,
                 "5": 1.0,
                 "500": 0.1,
-                "6": 0.5,
+                "6": 10,
                 "7": 1.0,
                 "700": 0.1,
-                "8": 0.5
+                "8": 0.2
             },
             "relay_config": {
-                "turn_on_relay_start": 20,
-                "turn_off_relay_start": 135,
-				"ex_turn_on_relay_start": 20,
-                "ex_turn_off_relay_start": 135
+                "turn_on_relay_start": 0,
+                "turn_off_relay_start": 134,
+				        "ex_turn_on_relay_start": 0,
+                "ex_turn_off_relay_start": 134
             }
         },
         {
-            "params": [100, -100],
+            "params": [80, -150],
             "program_delay_map": {
                 "1": 1.0,
                 "100": 0.1,
@@ -126,20 +126,20 @@
                 "4": 0.1,
                 "5": 1.0,
                 "500": 0.1,
-                "6": 0.5,
+                "6": 10,
                 "7": 1.0,
                 "700": 0.1,
-                "8": 0.5
+                "8": 0.2
             },
             "relay_config": {
-                "turn_on_relay_start": 20,
-                "turn_off_relay_start": 135,
-				"ex_turn_on_relay_start": 20,
-                "ex_turn_off_relay_start": 135
+                "turn_on_relay_start": 0,
+                "turn_off_relay_start": 134,
+				        "ex_turn_on_relay_start": 0,
+                "ex_turn_off_relay_start": 134
             }
         },
         {
-            "params": [100, -50],
+            "params": [80, -150],
             "program_delay_map": {
                 "1": 1.0,
                 "100": 0.1,
@@ -149,20 +149,20 @@
                 "4": 0.1,
                 "5": 1.0,
                 "500": 0.1,
-                "6": 0.5,
+                "6": 10,
                 "7": 1.0,
                 "700": 0.1,
-                "8": 0.5
+                "8": 0.2
             },
             "relay_config": {
-                "turn_on_relay_start": 20,
-                "turn_off_relay_start": 135,
-				"ex_turn_on_relay_start": 20,
-                "ex_turn_off_relay_start": 135
+                "turn_on_relay_start": 0,
+                "turn_off_relay_start": 134,
+				        "ex_turn_on_relay_start": 0,
+                "ex_turn_off_relay_start": 134
             }
         },
         {
-            "params": [100, 0],
+            "params": [80, -150],
             "program_delay_map": {
                 "1": 1.0,
                 "100": 0.1,
@@ -172,20 +172,20 @@
                 "4": 0.1,
                 "5": 1.0,
                 "500": 0.1,
-                "6": 0.5,
+                "6": 10,
                 "7": 1.0,
                 "700": 0.1,
-                "8": 0.5
+                "8": 0.2
             },
             "relay_config": {
-                "turn_on_relay_start": 20,
-                "turn_off_relay_start": 135,
-				"ex_turn_on_relay_start": 20,
-                "ex_turn_off_relay_start": 135
+                "turn_on_relay_start": 0,
+                "turn_off_relay_start": 134,
+				        "ex_turn_on_relay_start": 0,
+                "ex_turn_off_relay_start": 134
             }
         },
         {
-            "params": [100, 50],
+            "params": [80, -150],
             "program_delay_map": {
                 "1": 1.0,
                 "100": 0.1,
@@ -195,20 +195,20 @@
                 "4": 0.1,
                 "5": 1.0,
                 "500": 0.1,
-                "6": 0.5,
+                "6": 10,
                 "7": 1.0,
                 "700": 0.1,
-                "8": 0.5
+                "8": 0.2
             },
             "relay_config": {
-                "turn_on_relay_start": 20,
-                "turn_off_relay_start": 135,
-				"ex_turn_on_relay_start": 20,
-                "ex_turn_off_relay_start": 135
+                "turn_on_relay_start": 0,
+                "turn_off_relay_start": 134,
+				        "ex_turn_on_relay_start": 0,
+                "ex_turn_off_relay_start": 134
             }
         },
         {
-            "params": [100, 100],
+            "params": [80, -150],
             "program_delay_map": {
                 "1": 1.0,
                 "100": 0.1,
@@ -218,20 +218,20 @@
                 "4": 0.1,
                 "5": 1.0,
                 "500": 0.1,
-                "6": 0.5,
+                "6": 10,
                 "7": 1.0,
                 "700": 0.1,
-                "8": 0.5
+                "8": 0.2
             },
             "relay_config": {
-                "turn_on_relay_start": 20,
-                "turn_off_relay_start": 135,
-				"ex_turn_on_relay_start": 20,
-                "ex_turn_off_relay_start": 135
+                "turn_on_relay_start": 0,
+                "turn_off_relay_start": 134,
+				        "ex_turn_on_relay_start": 0,
+                "ex_turn_off_relay_start": 134
             }
         },
         {
-            "params": [90, 50],
+            "params": [80, -150],
             "program_delay_map": {
                 "1": 1.0,
                 "100": 0.1,
@@ -241,20 +241,20 @@
                 "4": 0.1,
                 "5": 1.0,
                 "500": 0.1,
-                "6": 0.5,
+                "6": 10,
                 "7": 1.0,
                 "700": 0.1,
-                "8": 0.5
+                "8": 0.2
             },
             "relay_config": {
-                "turn_on_relay_start": 20,
-                "turn_off_relay_start": 135,
-				"ex_turn_on_relay_start": 20,
-                "ex_turn_off_relay_start": 135
+                "turn_on_relay_start": 0,
+                "turn_off_relay_start": 134,
+				        "ex_turn_on_relay_start": 0,
+                "ex_turn_off_relay_start": 134
             }
         },
         {
-            "params": [90, 150],
+            "params": [80, -150],
             "program_delay_map": {
                 "1": 1.0,
                 "100": 0.1,
@@ -264,16 +264,16 @@
                 "4": 0.1,
                 "5": 1.0,
                 "500": 0.1,
-                "6": 0.5,
+                "6": 10,
                 "7": 1.0,
                 "700": 0.1,
-                "8": 0.5
+                "8": 0.2
             },
             "relay_config": {
-                "turn_on_relay_start": 20,
-                "turn_off_relay_start": 135,
-				"ex_turn_on_relay_start": 20,
-                "ex_turn_off_relay_start": 135
+                "turn_on_relay_start": 0,
+                "turn_off_relay_start": 134,
+				        "ex_turn_on_relay_start": 0,
+                "ex_turn_off_relay_start": 134
             }
         }
     ]
diff --git a/kelaifen/Kelaifen_V6.0/trajectory_data.py b/kelaifen/Kelaifen_V6.0/trajectory_data.py
index 7257751..87e874e 100644
--- a/kelaifen/Kelaifen_V6.0/trajectory_data.py
+++ b/kelaifen/Kelaifen_V6.0/trajectory_data.py
@@ -1,685 +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.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, 1.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]
+    [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]
 ]
-joint_positions_assembled_1 = [[-8.303461, -48.278357, 71.508258, -115.343, 89.659, -5.706]]
+
+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.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]
+    [-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]
 ]
-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]
+
+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]
 ]
-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]
+
+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]
 ]
-joint_positions_assembled_4 = [[92.553,-45.178,53.196,-97.284,97.995,15.177]]
+
 
 Pose_3_4 = [
-    [-228.1, 1631.7, 370.4375, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 380.6750, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 390.9125, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 401.1500, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 411.3875, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 421.6250, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 431.8625, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 442.1, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 452.3375, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 462.5750, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 472.8125, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 483.0500, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 493.2875, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 503.5250, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 513.7625, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 524.0, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 534.2375, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 544.4750, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 554.7125, -3.1075, 0.0078, -0.1622],
-    [-228.1, 1631.7, 564.9500, -3.1075, 0.0078, -0.1622],
+    [-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_3 = [
-    [1549.9, -66.7470, 370.4375, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 380.6750, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 390.9125, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 401.1500, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 411.3875, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 421.6250, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 431.8625, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 442.1, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 452.3375, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 462.5750, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 472.8125, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 483.0500, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 493.2875, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 503.5250, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 513.7625, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 524.0, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 534.2375, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 544.4750, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 554.7125, 3.1398, 0.0349, -1.5584],
-    [1549.9, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584],
+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]
+]
 
 
 
diff --git a/kelaifen/Kelaifen_V6.0/trajectory_program.py b/kelaifen/Kelaifen_V6.0/trajectory_program.py
index 5d70b78..0d30035 100644
--- a/kelaifen/Kelaifen_V6.0/trajectory_program.py
+++ b/kelaifen/Kelaifen_V6.0/trajectory_program.py
@@ -4,8 +4,8 @@ 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
+#     PRESET_PARAMS,  turn_off_relay_start, 
+#      turn_on_relay_start
 #     )
 from serial_init import SerialSharedData
 from trajectory_data import (
@@ -60,7 +60,7 @@ Interpolation_points = 20
 # 第九段程序封装为函数
 # 第十二段程序封装为函数
 
-def program1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, turn_off_relay_end, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end):
+def program1(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
     i = 0
     suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
     turn_off_relay()
@@ -115,7 +115,7 @@ def program1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start,
     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, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end):
+def program1_1(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
     print("第1_1段程序开始执行")
     i = 0
     suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
@@ -181,7 +181,7 @@ def program1_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star
     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, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end):
+def program2(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
     global increments_x, increments_y, increments_z
     global Coordinate_compensation
     global sample_time
@@ -227,19 +227,19 @@ def program2(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start,
         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):
+        if(i >= turn_on_relay_start):
             turn_on_relay()
-            print(f"打开喷枪2：{turn_on_relay_start} ~ {turn_on_relay_end}")
-        if(i >= turn_off_relay_start and i <= turn_off_relay_end):
+            print(f"打开喷枪2：{turn_on_relay_start}")
+        if(i >= turn_off_relay_start):
             turn_off_relay()
-            print(f"关闭喷枪2：{turn_off_relay_start} ~ {turn_off_relay_end}")
+            print(f"关闭喷枪2：{turn_off_relay_start}")
         i = i + 1
 
     suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
     print("第二段程序执行完")
 
 
-def program3(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, turn_off_relay_end, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end):
+def program3(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
     print("第三段程序开始执行")
     i = 0
     suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
@@ -293,7 +293,7 @@ def program3(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start,
     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, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end):
+def program3_1(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
     print("第3_1段程序开始执行")
     i = 0
     suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
@@ -359,7 +359,7 @@ def program3_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star
     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, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end):
+def program4(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
     print("第四段程序开始执行")
     global increments_x, increments_y, increments_z
     global Coordinate_compensation
@@ -402,19 +402,19 @@ def program4(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start,
                 "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0})
         send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list})
         time.sleep(sleep_time)
-        if(i >= ex_turn_on_relay_start and i <= ex_turn_on_relay_end):
+        if(i >= ex_turn_on_relay_start):
             turn_on_relay()
-            print(f"打开喷枪4：{ex_turn_on_relay_start} ~ {ex_turn_on_relay_end}")
-        if(i >= ex_turn_off_relay_start and i <= ex_turn_off_relay_end):
+            print(f"打开喷枪4：{ex_turn_on_relay_start}")
+        if(i >= ex_turn_off_relay_start):
             turn_off_relay()
-            print(f"关闭喷枪4：{ex_turn_off_relay_start} ~ {ex_turn_off_relay_end}")
+            print(f"关闭喷枪4：{ex_turn_off_relay_start}")
         i = i + 1
 
     suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
     print("第四段程序执行完")
 
 
-def program5(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, turn_off_relay_end, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end):
+def program5(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
     print("第五段程序开始执行")
     i = 0
     suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
@@ -470,7 +470,7 @@ def program5(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start,
     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, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end):
+def program5_1(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
     print("第5_1段程序开始执行")
     i = 0
     suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
@@ -536,7 +536,7 @@ def program5_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star
     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, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end):
+def program6(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
     global increments_x, increments_y, increments_z
     global Coordinate_compensation
     global sample_time
@@ -577,18 +577,18 @@ def program6(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start,
                 "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):
+        if(i >= turn_on_relay_start):
             turn_on_relay()
-            print(f"打开喷枪6：{turn_on_relay_start} ~ {turn_on_relay_end}")
-        if(i >= turn_off_relay_start and i <= turn_off_relay_end):
+            print(f"打开喷枪6：{turn_on_relay_start}")
+        if(i >= turn_off_relay_start):
             turn_off_relay()
-            print(f"关闭喷枪6：{turn_off_relay_start} ~ {turn_off_relay_end}")
+            print(f"关闭喷枪6：{turn_off_relay_start}")
         i = i + 1
     suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
     print("第六段程序执行完")
 
 
-def program7(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start, turn_off_relay_end, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end):
+def program7(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
     i = 0
     suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
     turn_off_relay()
@@ -642,7 +642,7 @@ def program7(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start,
     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, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end):
+def program7_1(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
     print("第7_1段程序开始执行")
     i = 0
     suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
@@ -706,7 +706,7 @@ def program7_1(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_star
     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, ex_turn_on_relay_start, ex_turn_on_relay_end, ex_turn_off_relay_start, ex_turn_off_relay_end):
+def program8(sock, turn_on_relay_start,  turn_off_relay_start,  ex_turn_on_relay_start,  ex_turn_off_relay_start):
     global increments_x, increments_y, increments_z
     global Coordinate_compensation
     global sample_time
@@ -749,12 +749,12 @@ def program8(sock, turn_on_relay_start, turn_on_relay_end, turn_off_relay_start,
                 "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0})
         send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list})
         time.sleep(sleep_time)
-        if(i >= ex_turn_on_relay_start and i <= ex_turn_on_relay_end):
+        if(i >= ex_turn_on_relay_start):
             turn_on_relay()
-            print(f"打开喷枪8：{ex_turn_on_relay_start} ~ {ex_turn_on_relay_end}")
-        if(i >= ex_turn_off_relay_start and i <= ex_turn_off_relay_end):
+            print(f"打开喷枪8：{ex_turn_on_relay_start}")
+        if(i >= ex_turn_off_relay_start):
             turn_off_relay()
-            print(f"关闭喷枪8：{ex_turn_off_relay_start} ~ {ex_turn_off_relay_end}")
+            print(f"关闭喷枪8：{ex_turn_off_relay_start}")
         i = i + 1
 
     suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf")
@@ -1033,10 +1033,11 @@ def program100(sock, ser, stop_cmd_bytes, turn_on_relay_start, turn_off_relay_st
     # 第2道开枪时间在第15个点位（Pose_4_M的开枪点位 lookahead_time/sample_time+15），，关枪时间在第150点位示例（lookahead_time/sample_time+135，，，，如果数据>155，那么关枪就在Pose_4_3_M中）
     #队列，延时时间计算
     off_set_Time=int(lookahead_time / sample_time)
+    off_set_Time=0
     # 1.喷枪1开关时间计算
     OPEN_1=turn_on_relay_start
     CLOSE_1=turn_off_relay_start
-    OPEN_1_M = off_set_Time+5
+    OPEN_1_M = off_set_Time+OPEN_1
     CLOSE_1_M1S=off_set_Time+CLOSE_1
     CLOSE_1_M1E = off_set_Time + CLOSE_1-155
     #2.喷枪2开关时间计算
@@ -1086,14 +1087,15 @@ def program100(sock, ser, stop_cmd_bytes, turn_on_relay_start, turn_off_relay_st
                 for point in traj:
                     send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point})
                     time.sleep(sleep_time)
-                if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30':
-                    print("收到0xAA 0x30,停止循环")
-                    for i in range(3):
-                        ser.write(b'\x80\x80')
-                        ser.flush()
-                        time.sleep(0.01)
-                    time.sleep(0.5)
-                    return False
+                    print(f"停止轨迹，NUM = {NUM}, delay = {delay}")
+                # if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30':
+                #     print("收到0xAA 0x30,停止循环")
+                #     for i in range(3):
+                #         ser.write(b'\x80\x80')
+                #         ser.flush()
+                #         time.sleep(0.01)
+                #     time.sleep(0.5)
+                #     return False
             print("轨迹执行完成")
 
     return False