From 1cd630f85afc1c4e41e0e7a96667d02a8f3dfffc Mon Sep 17 00:00:00 2001 From: Lizongdi <1210855344@qq.com> Date: Tue, 7 Jul 2026 13:35:30 +0800 Subject: [PATCH] =?UTF-8?q?http://123.207.52.103:3000/LiZongdi/Huangjiabao?= =?UTF-8?q?=5Fbackup/commit/320dcfe6d75557d0fbae81866a41384dd179a200?= =?UTF-8?q?=E4=BB=8E=E8=BF=99=E4=B8=AA=E6=8F=90=E4=BA=A4=E5=A4=8D=E5=88=B6?= =?UTF-8?q?=E8=80=8C=E6=9D=A5?= MIME-Version: 1.0 Content-Type: text/plain; charset=UTF-8 Content-Transfer-Encoding: 8bit --- Debug/KeLaiFen.list | 69 +- Kelaifen_V6.0/.idea/.gitignore | 3 + Kelaifen_V6.0/.idea/.name | 1 + .../inspectionProfiles/profiles_settings.xml | 6 + Kelaifen_V6.0/.idea/misc.xml | 4 + Kelaifen_V6.0/.idea/modules.xml | 8 + .../树莓派上完整程序_新增功能.iml | 8 + Kelaifen_V6.0/ETController.py | 57 + Kelaifen_V6.0/Example_0_5.py | 396 +++++ Kelaifen_V6.0/Pose_5.txt | 1 + Kelaifen_V6.0/Pose_6.txt | 1 + Kelaifen_V6.0/Pose_7.txt | 1 + Kelaifen_V6.0/Pose_8.txt | 1 + Kelaifen_V6.0/config.json | 220 +++ Kelaifen_V6.0/gpio.py | 37 + Kelaifen_V6.0/joint_positions_assembled_5.txt | 1 + Kelaifen_V6.0/joint_positions_assembled_6.txt | 1 + Kelaifen_V6.0/joint_positions_assembled_7.txt | 1 + Kelaifen_V6.0/joint_positions_assembled_8.txt | 1 + Kelaifen_V6.0/program_config.py | 78 + Kelaifen_V6.0/readme.txt | 19 + Kelaifen_V6.0/serial_handler.py | 157 ++ Kelaifen_V6.0/serial_init.py | 27 + Kelaifen_V6.0/trajectory_data.py | 737 +++++++++ Kelaifen_V6.0/trajectory_program.py | 1362 +++++++++++++++++ Kelaifen_V6.0/transform.py | 25 + 26 files changed, 3216 insertions(+), 6 deletions(-) create mode 100644 Kelaifen_V6.0/.idea/.gitignore create mode 100644 Kelaifen_V6.0/.idea/.name create mode 100644 Kelaifen_V6.0/.idea/inspectionProfiles/profiles_settings.xml create mode 100644 Kelaifen_V6.0/.idea/misc.xml create mode 100644 Kelaifen_V6.0/.idea/modules.xml create mode 100644 Kelaifen_V6.0/.idea/树莓派上完整程序_新增功能.iml create mode 100644 Kelaifen_V6.0/ETController.py create mode 100644 Kelaifen_V6.0/Example_0_5.py create mode 100644 Kelaifen_V6.0/Pose_5.txt create mode 100644 Kelaifen_V6.0/Pose_6.txt create mode 100644 Kelaifen_V6.0/Pose_7.txt create mode 100644 Kelaifen_V6.0/Pose_8.txt create mode 100644 Kelaifen_V6.0/config.json create mode 100644 Kelaifen_V6.0/gpio.py create mode 100644 Kelaifen_V6.0/joint_positions_assembled_5.txt create mode 100644 Kelaifen_V6.0/joint_positions_assembled_6.txt create mode 100644 Kelaifen_V6.0/joint_positions_assembled_7.txt create mode 100644 Kelaifen_V6.0/joint_positions_assembled_8.txt create mode 100644 Kelaifen_V6.0/program_config.py create mode 100644 Kelaifen_V6.0/readme.txt create mode 100644 Kelaifen_V6.0/serial_handler.py create mode 100644 Kelaifen_V6.0/serial_init.py create mode 100644 Kelaifen_V6.0/trajectory_data.py create mode 100644 Kelaifen_V6.0/trajectory_program.py create mode 100644 Kelaifen_V6.0/transform.py diff --git a/Debug/KeLaiFen.list b/Debug/KeLaiFen.list index 80ab565..48e820c 100644 --- a/Debug/KeLaiFen.list +++ b/Debug/KeLaiFen.list @@ -39,13 +39,13 @@ Idx Name Size VMA LMA File off Algn CONTENTS, READONLY, DEBUGGING, OCTETS 17 .debug_line 0006887f 00000000 00000000 000ea363 2**0 CONTENTS, READONLY, DEBUGGING, OCTETS - 18 .debug_str 001b0145 00000000 00000000 00152be2 2**0 + 18 .debug_str 001b0124 00000000 00000000 00152be2 2**0 CONTENTS, READONLY, DEBUGGING, OCTETS - 19 .comment 00000043 00000000 00000000 00302d27 2**0 + 19 .comment 00000043 00000000 00000000 00302d06 2**0 CONTENTS, READONLY - 20 .debug_frame 0000e258 00000000 00000000 00302d6c 2**2 + 20 .debug_frame 0000e258 00000000 00000000 00302d4c 2**2 CONTENTS, READONLY, DEBUGGING, OCTETS - 21 .debug_line_str 00000049 00000000 00000000 00310fc4 2**0 + 21 .debug_line_str 00000047 00000000 00000000 00310fa4 2**0 CONTENTS, READONLY, DEBUGGING, OCTETS Disassembly of section .text: @@ -25141,14 +25141,14 @@ void Decive_MainLoop() 802a4ea: 2200 movs r2, #0 802a4ec: 601a str r2, [r3, #0] } -// Vehicle_Heart_Beat();//机械臂心跳 + //Vehicle_Heart_Beat();//机械臂心跳 Globle_Time_Count++;//全局计时 802a4ee: 4b04 ldr r3, [pc, #16] @ (802a500 ) 802a4f0: 681b ldr r3, [r3, #0] 802a4f2: 3301 adds r3, #1 802a4f4: 4a02 ldr r2, [pc, #8] @ (802a500 ) 802a4f6: 6013 str r3, [r2, #0] -// VehicleConnectDetect();//大臂车心跳检测 + //VehicleConnectDetect();//大臂车心跳检测 } 802a4f8: bf00 nop 802a4fa: bd80 pop {r7, pc} @@ -30091,46 +30091,103 @@ void HAL_UART_MspInit(UART_HandleTypeDef* uartHandle) 802c35c: 400200b8 .word 0x400200b8 0802c360 : + + .section .text.Reset_Handler + .weak Reset_Handler + .type Reset_Handler, %function +Reset_Handler: + ldr sp, =_estack /* set stack pointer */ 802c360: f8df d034 ldr.w sp, [pc, #52] @ 802c398 + +/* Call the clock system initialization function.*/ + bl SystemInit 802c364: f7fe fe0c bl 802af80 + +/* Copy the data segment initializers from flash to SRAM */ + ldr r0, =_sdata 802c368: 480c ldr r0, [pc, #48] @ (802c39c ) + ldr r1, =_edata 802c36a: 490d ldr r1, [pc, #52] @ (802c3a0 ) + ldr r2, =_sidata 802c36c: 4a0d ldr r2, [pc, #52] @ (802c3a4 ) + movs r3, #0 802c36e: 2300 movs r3, #0 + b LoopCopyDataInit 802c370: e002 b.n 802c378 0802c372 : + +CopyDataInit: + ldr r4, [r2, r3] 802c372: 58d4 ldr r4, [r2, r3] + str r4, [r0, r3] 802c374: 50c4 str r4, [r0, r3] + adds r3, r3, #4 802c376: 3304 adds r3, #4 0802c378 : + +LoopCopyDataInit: + adds r4, r0, r3 802c378: 18c4 adds r4, r0, r3 + cmp r4, r1 802c37a: 428c cmp r4, r1 + bcc CopyDataInit 802c37c: d3f9 bcc.n 802c372 +/* Zero fill the bss segment. */ + ldr r2, =_sbss 802c37e: 4a0a ldr r2, [pc, #40] @ (802c3a8 ) + ldr r4, =_ebss 802c380: 4c0a ldr r4, [pc, #40] @ (802c3ac ) + movs r3, #0 802c382: 2300 movs r3, #0 + b LoopFillZerobss 802c384: e001 b.n 802c38a 0802c386 : + +FillZerobss: + str r3, [r2] 802c386: 6013 str r3, [r2, #0] + adds r2, r2, #4 802c388: 3204 adds r2, #4 0802c38a : + +LoopFillZerobss: + cmp r2, r4 802c38a: 42a2 cmp r2, r4 + bcc FillZerobss 802c38c: d3fb bcc.n 802c386 + +/* Call static constructors */ + bl __libc_init_array 802c38e: f01a f8ff bl 8046590 <__libc_init_array> +/* Call the application's entry point.*/ + bl main 802c392: f7fe f8d5 bl 802a540
+ bx lr 802c396: 4770 bx lr + ldr sp, =_estack /* set stack pointer */ 802c398: 24080000 .word 0x24080000 + ldr r0, =_sdata 802c39c: 24000000 .word 0x24000000 + ldr r1, =_edata 802c3a0: 240000d0 .word 0x240000d0 + ldr r2, =_sidata 802c3a4: 0804b3ec .word 0x0804b3ec + ldr r2, =_sbss 802c3a8: 24000190 .word 0x24000190 + ldr r4, =_ebss 802c3ac: 24012ef8 .word 0x24012ef8 0802c3b0 : + * @retval None +*/ + .section .text.Default_Handler,"ax",%progbits +Default_Handler: +Infinite_Loop: + b Infinite_Loop 802c3b0: e7fe b.n 802c3b0 0802c3b2 : diff --git a/Kelaifen_V6.0/.idea/.gitignore b/Kelaifen_V6.0/.idea/.gitignore new file mode 100644 index 0000000..359bb53 --- /dev/null +++ b/Kelaifen_V6.0/.idea/.gitignore @@ -0,0 +1,3 @@ +# 默认忽略的文件 +/shelf/ +/workspace.xml diff --git a/Kelaifen_V6.0/.idea/.name b/Kelaifen_V6.0/.idea/.name new file mode 100644 index 0000000..d66c122 --- /dev/null +++ b/Kelaifen_V6.0/.idea/.name @@ -0,0 +1 @@ +Example_0_5.py \ No newline at end of file diff --git a/Kelaifen_V6.0/.idea/inspectionProfiles/profiles_settings.xml b/Kelaifen_V6.0/.idea/inspectionProfiles/profiles_settings.xml new file mode 100644 index 0000000..105ce2d --- /dev/null +++ b/Kelaifen_V6.0/.idea/inspectionProfiles/profiles_settings.xml @@ -0,0 +1,6 @@ + + + + \ No newline at end of file diff --git a/Kelaifen_V6.0/.idea/misc.xml b/Kelaifen_V6.0/.idea/misc.xml new file mode 100644 index 0000000..060d2c5 --- /dev/null +++ b/Kelaifen_V6.0/.idea/misc.xml @@ -0,0 +1,4 @@ + + + + \ No newline at end of file diff --git a/Kelaifen_V6.0/.idea/modules.xml b/Kelaifen_V6.0/.idea/modules.xml new file mode 100644 index 0000000..b21e480 --- /dev/null +++ b/Kelaifen_V6.0/.idea/modules.xml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Kelaifen_V6.0/.idea/树莓派上完整程序_新增功能.iml b/Kelaifen_V6.0/.idea/树莓派上完整程序_新增功能.iml new file mode 100644 index 0000000..f571432 --- /dev/null +++ b/Kelaifen_V6.0/.idea/树莓派上完整程序_新增功能.iml @@ -0,0 +1,8 @@ + + + + + + + + \ No newline at end of file diff --git a/Kelaifen_V6.0/ETController.py b/Kelaifen_V6.0/ETController.py new file mode 100644 index 0000000..04e20e9 --- /dev/null +++ b/Kelaifen_V6.0/ETController.py @@ -0,0 +1,57 @@ +import json +import socket +import time + +# 连接机器人控制器 +def connectETController(ip, port=8055): + sock = socket.socket(socket.AF_INET, socket.SOCK_STREAM) + try: + sock.connect((ip, port)) + return (True, sock) + except Exception as e: + sock.close() + return (False,) + +# 发送命令并接收响应 +def sendCMD(sock, cmd, params=None, id=1): + if not params: + params = [] + else: + params = json.dumps(params) + sendStr = '{"method":"%s","params":%s,"jsonrpc":"2.0","id":%d}' % (cmd, params, id) + "\n" + try: + sock.sendall(bytes(sendStr, "utf-8")) + ret = sock.recv(1024) + jdata = json.loads(str(ret, "utf-8")) + if "result" in jdata.keys(): + return (True, json.loads(jdata["result"]), jdata["id"]) + elif "error" in jdata.keys(): + return (False, jdata["error"], jdata["id"]) + else: + return (False, None, None) + except Exception as e: + return (False, None, None) + + +def send_Point(sock, cmd, params=None, id=1): + if (not params): + params = [] + else: + params = json.dumps(params) + sendStr = "{{\"method\":\"{0}\",\"params\":{1},\"jsonrpc\":\"2.0\",\"id\":{2}}}".format(cmd, params, id) + "\n" + sock.sendall(bytes(sendStr, "utf-8")) + + +def wait_stop(sock): # 修正：添加sock参数 + while True: + time.sleep(0.1) + ret1, result1, id1 = sendCMD(sock, "getRobotState") # getRobotstate + if (ret1): + if result1 == 0 or result1 == 4: + break + else: + #print("getRobotState failed") + continue + + + diff --git a/Kelaifen_V6.0/Example_0_5.py b/Kelaifen_V6.0/Example_0_5.py new file mode 100644 index 0000000..fdc5b03 --- /dev/null +++ b/Kelaifen_V6.0/Example_0_5.py @@ -0,0 +1,396 @@ +import time +import serial +from serial import Serial +import threading +from ETController import connectETController,sendCMD,send_Point,wait_stop +from gpio import init_gpio, turn_off_relay, turn_on_relay +from program_config import load_process_config, program_switch,sequence_1, sequence_2, sequence_3, sequence_4 +from serial_init import serial_init +from serial_handler import handle_special_commands, read_cmd_from_shared, serial_receive_data + +if __name__ == "__main__": + + # 加载配置文件并提取参数 + process_config = load_process_config() + if process_config: + # 提取预设参数列表（现在PRESET_PARAMS本身包含所有子配置） + PRESET_PARAMS = process_config["PRESET_PARAMS"] + + init_gpio() + ser = serial_init() + # 检查串口初始化结果 + if not ser or not isinstance(ser, serial.Serial) or not ser.is_open: + print("串口初始化失败，无法继续运行") + # 确保在函数内部返回 + exit() + + time.sleep(20) + # 默认关闭喷枪 + turn_off_relay() + + # 机器人IP地址 + robot_ip = "192.168.1.100" + result = connectETController(robot_ip) + sock = None + if len(result) == 2: + conSuc, sock = result + if conSuc: + print("成功连接到机器人") + # 1.上电设置 + ret, result, id = sendCMD(sock, "set_robot_power_status", {"status": 1}) + print("开始上电") + time.sleep(20) + print("开始检测上电状态") + ret, result, id = sendCMD(sock, "get_robot_power_status") + + if result == 0: + ret, result, id = sendCMD(sock, "set_robot_power_status", {"status": 1}) + time.sleep(20) + else : + print("已上电") + # 2. 清除报警 + print("清除报警") + ret, result, id = sendCMD(sock,"clearAlarm") + print("清除报警返回值") + print(result) + time.sleep(5) + print("报警清除完成") + + # 获取同步状态 + suc, result , id = sendCMD(sock, "getMotorStatus") + if result == 0: + # 同步伺服编码器数据 + suc,result,id = sendCMD(sock, "syncMotorStatus") + if result: + print("同步伺服编码器数据成功") + else: + print("同步伺服编码器数据失败") + time. sleep (2) + + # 3. 编码器零位校准 + suc, result, id = sendCMD(sock, "getServoStatus") + print("获取伺服状态返回值") + print(result) + time.sleep(2) + if result == 0: + # 设置机械臂伺服状态为“开”（status=1） + suc, result, id = sendCMD(sock, "set_servo_status", {"status": 1}) + print("设置机械臂伺服状态返回值") + print(result) + time.sleep(5) + suc, result, id = sendCMD(sock, "calibrate_encoder_zero_position") + print("编码器校准返回值") + print(result) + time.sleep(5) + if result == 1: + print("编码器校准成功") + else: + print("编码器校准失败") + + # 初始化透传服务 + suc, result, id = sendCMD(sock, "get_transparent_transmission_state") + if id == 1: + print(suc, result, id) + if result == 1: + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + print("已清空透传缓存") + time.sleep(0.5) + else: + print("无缓存") + + # 在此处做检测等相关操作 + suc, result, id = sendCMD(sock, "getRobotState") + if (result == 4): + # 清除报警 + suc, result, id = sendCMD(sock, "clearAlarm") + time.sleep(0.5) + + # 获取同步状态 + suc, result, id = sendCMD(sock, "getMotorStatus") + if (result == 0): + # 同步伺服编码器数据 + suc, result, id = sendCMD(sock, "syncMotorStatus") + time.sleep(0.5) + else: + print("同步状态获取失败") + + # 获取机械臂伺服状态 + suc, result, id = sendCMD(sock, "getServoStatus") + if (result == 0): + # 设置机械臂伺服状态ON + suc, result, id = sendCMD(sock, "set_servo_status", {"status": 1}) + time.sleep(1) + else: + print("机械臂伺服取失败") + + # 获取当前机器人是否处于透传状态 + suc, result, id = sendCMD(sock, "get_transparent_transmission_state") + print(result) + if (result == 1): + # 清空透传缓存 + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + time.sleep(0.5) + else: + print("非缓存状态") + + # 启动串口接收线程（daemon=True：主程序退出时线程自动终止） + receive_thread = threading.Thread(target=serial_receive_data, args=(ser,), daemon=True) + receive_thread.start() + print("串口接收线程已启动") + + delay_time_10_13579_30 = 2 + + # 外层主循环 + while True: + try: + # 等待主指令（从共享缓冲区读取） + print("\n等待串口指令...") + main_cmd_bytes = None + while main_cmd_bytes is None: + # 喷漆工艺参数配置选择（速度、角度、搭接时间） + selected_delay_map, selected_relay_config = handle_special_commands(PRESET_PARAMS) + if selected_delay_map is not None and selected_relay_config is not None: + # 更新delay配置 + program_delay_map = selected_delay_map + # 更新继电器参数 + turn_on_relay_start = selected_relay_config["turn_on_relay_start"] + turn_off_relay_start = selected_relay_config["turn_off_relay_start"] + ex_turn_on_relay_start = selected_relay_config["ex_turn_on_relay_start"] + ex_turn_off_relay_start = selected_relay_config["ex_turn_off_relay_start"] + + if turn_off_relay_start >= 155: + turn_off_relay_start = 154 + if ex_turn_off_relay_start >= 155: + ex_turn_off_relay_start = 154 + + print(f"\n✅ 参数已更新为选中配置：") + print(f"program_delay_map: {program_delay_map}") + print(f"轨迹1开枪时间：{turn_on_relay_start} ~ {turn_off_relay_start}") + print(f"轨迹2开枪时间：{ex_turn_on_relay_start} ~ {ex_turn_off_relay_start}") + # 喷漆开始命令接收函数 + main_cmd_bytes = read_cmd_from_shared(timeout=1.0) + # 解析主指令（帧头后第一字节为指令值） + cmd = main_cmd_bytes[1] + print(f"收到串口指令: 0x{cmd:02X}") + + # region 情况1 + # 情况1: 接收到1 + if cmd == 1: + print("触发情况1: 执行program1") + program_switch[1](sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start) + time.sleep(delay_time_10_13579_30) + + for i in range(3): + ser.write(b'\x80\x80') + ser.flush() + time.sleep(0.01) + print("执行program1结束,已发送响应0x80 0x80") + + # 等待子指令接收 + sub_cmd_bytes = None + while sub_cmd_bytes is None: + sub_cmd_bytes = read_cmd_from_shared(timeout=1.0) + time.sleep(0.01) + sub_cmd = sub_cmd_bytes[0] # 应为0xAA + sub_cmd1 = sub_cmd_bytes[1] # 子指令值（如0x20、0x0a） + print(f"收到子指令: 0x{sub_cmd:02X} 0x{sub_cmd1:02X}") + + if sub_cmd == 0xAA: + if sub_cmd1 == 0x20: + print("开始情况1循环序列: program2->300->4->100...") + current_index = 0 + running = True + while running: + # 执行当前序列中的程序 + current_program = sequence_1[current_index] + #print(f"执行序列程序: {current_program}") + + # 读取可能的停止指令（超时0.1秒，不阻塞循环） + delay = program_delay_map.get(current_program, 0.0) + if delay <= 0: + delay = 0 + current_program1 = sequence_1[current_index + 1] + delay1 = program_delay_map.get(current_program1, 0.0) + if delay1 <= 0: + delay1 = 0 + stop_cmd_bytes = read_cmd_from_shared(timeout=0.1) + if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30': + print("收到0xAA 0x30,停止循环") + for i in range(3): + ser.write(b'\x80\x80') + ser.flush() + time.sleep(0.01) + running = False + time.sleep(0.5) + else: + running = program_switch[current_program](sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay, delay1) + # 回到洗枪位置指令 + elif sub_cmd1 == 0x0a: + print(f"接收到指令0xAA 0x{sub_cmd1:02X}，切换执行对应程序") + program_switch[10](sock) + time.sleep(delay_time_10_13579_30) + for i in range(3): + ser.write(b'\x80\x80') + ser.flush() + time.sleep(0.01) + print("执行program10结束,已发送响应0x80 0x80") + print("返回最外层等待串口数据") + + else: + print(f"收到未知子指令: 0x{sub_cmd:02X}，继续等待") + #endregion + # region 情况3 + # 情况3: 接收到5 + elif cmd == 5: + print("触发情况3: 执行program5") + program_switch[5](sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start) + + time.sleep(delay_time_10_13579_30) + for i in range(3): + ser.write(b'\x80\x80') + ser.flush() + time.sleep(0.01) + print("执行program5结束,已发送响应0x80 0x80") + + # 子指令接收（调用read_cmd_from_shared） + print("等待0x20开始循环或其他指令...") + sub_cmd_bytes = None + while sub_cmd_bytes is None: + sub_cmd_bytes = read_cmd_from_shared(timeout=1.0) + time.sleep(0.01) + sub_cmd = sub_cmd_bytes[0] + sub_cmd1 = sub_cmd_bytes[1] + print(f"收到子指令: 0x{sub_cmd:02X}") + + if sub_cmd == 0xAA: + if sub_cmd1 == 0x20: + print("开始情况3循环序列: program6->700->8->500...") + current_index = 0 + running = True + while running: + current_program = sequence_3[current_index] + print(f"执行序列程序: {current_program}") + + # 读取可能的停止指令（超时0.1秒，不阻塞循环） + delay = program_delay_map.get(current_program, 0.0) + if delay <= 0: + delay = 0 + current_program1 = sequence_3[current_index + 1] + delay1 = program_delay_map.get(current_program1, 0.0) + if delay1 <= 0: + delay1 = 0 + stop_cmd_bytes = read_cmd_from_shared(timeout=0.1) + if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30': + print("收到0xAA 0x30,停止循环") + for i in range(3): + ser.write(b'\x80\x80') + ser.flush() + time.sleep(0.01) + running = False + time.sleep(0.5) + else: + running = program_switch[current_program](sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay, delay1) + #回到洗枪位置指令 + elif sub_cmd1 == 0x0a: + print(f"接收到指令0xAA 0x{sub_cmd1:02X}，切换执行对应程序") + program_switch[10](sock) + time.sleep(delay_time_10_13579_30) + for i in range(3): + ser.write(b'\x80\x80') + ser.flush() + time.sleep(0.01) + print("执行program10结束,已发送响应0x80 0x80") + print("返回最外层等待串口数据") + else: + print(f"收到未知子指令: 0x{sub_cmd:02X}，继续等待") + #endregion + + # region 程序9，返回停机位置 + elif cmd == 9: + print("执行program9,返回停机位置") + program_switch[9](sock) + time.sleep(delay_time_10_13579_30) + for i in range(3): + ser.write(b'\x80\x80') + ser.flush() + time.sleep(0.1) + print("执行program9结束,已发送响应0x80 0x80") + print("返回最外层等待串口数据") + #endregion + + # region 程序10，前往初始位置 + elif cmd == 10: + print("触发情况6: 执行program10") + program_switch[10](sock) + time.sleep(delay_time_10_13579_30) + for i in range(3): + ser.write(b'\x80\x80') + ser.flush() + time.sleep(0.1) + print("执行program10结束,已发送响应0x80 0x80") + print("返回最外层等待串口数据") + # endregion + + # region 程序11，前往洗枪位置 + elif cmd == 11: + print("执行program11,前往洗枪位置") + program_switch[11](sock) + time.sleep(delay_time_10_13579_30) + for i in range(3): + ser.write(b'\x80\x80') + ser.flush() + time.sleep(0.1) + print("执行program11结束,已发送响应0x80 0x80") + print("返回最外层等待串口数据") + #endregion + + # region 程序12，前往试枪位置 + elif cmd == 12: + print("执行program12，前往试枪位置") + program_switch[12](sock) + time.sleep(delay_time_10_13579_30) + for i in range(3): + ser.write(b'\x80\x80') + ser.flush() + time.sleep(0.1) + print("执行program12结束,已发送响应0x80 0x80") + print("返回最外层等待串口数据") + #endregion + + # 退出指令 + elif cmd == 0x00: + print("收到退出指令，程序终止") + break + # 开喷枪 + elif cmd == 16: + turn_on_relay() + print("打开喷枪") + # 关喷枪 + elif cmd == 17: + turn_off_relay() + print("关闭喷枪") + # 未知指令 + else: + print(f"未知指令: 0x{cmd:02X}，等待下一条指令...") + time.sleep(0.01) + + except KeyboardInterrupt: + print("程序被手动中断") + break + except Exception as e: + print(f"主循环错误: {e}") + time.sleep(0.5) + else: + print("连接机器人失败") + else: + print("无法连接到机器人") + + + + # 资源清理 + if sock: + sock.close() + if ser and isinstance(ser, serial.Serial) and ser.is_open: + ser.close() + print("串口已关闭") + \ No newline at end of file diff --git a/Kelaifen_V6.0/Pose_5.txt b/Kelaifen_V6.0/Pose_5.txt new file mode 100644 index 0000000..a3987f9 --- /dev/null +++ b/Kelaifen_V6.0/Pose_5.txt @@ -0,0 +1 @@ +318.173,-94.776,446.581,4.743102,-0.066481,-4.024368 \ No newline at end of file diff --git a/Kelaifen_V6.0/Pose_6.txt b/Kelaifen_V6.0/Pose_6.txt new file mode 100644 index 0000000..bba19d9 --- /dev/null +++ b/Kelaifen_V6.0/Pose_6.txt @@ -0,0 +1 @@ +318.901,437.099,583.004,4.685241,-0.089038,-2.083531 \ No newline at end of file diff --git a/Kelaifen_V6.0/Pose_7.txt b/Kelaifen_V6.0/Pose_7.txt new file mode 100644 index 0000000..9a98d6d --- /dev/null +++ b/Kelaifen_V6.0/Pose_7.txt @@ -0,0 +1 @@ +132.209,693.315,699.278,4.182,0.075,-0.740 \ No newline at end of file diff --git a/Kelaifen_V6.0/Pose_8.txt b/Kelaifen_V6.0/Pose_8.txt new file mode 100644 index 0000000..d4cd6e0 --- /dev/null +++ b/Kelaifen_V6.0/Pose_8.txt @@ -0,0 +1 @@ +663.317,241.195,699.278,4.182,0.075,2.306 \ No newline at end of file diff --git a/Kelaifen_V6.0/config.json b/Kelaifen_V6.0/config.json new file mode 100644 index 0000000..3f153a2 --- /dev/null +++ b/Kelaifen_V6.0/config.json @@ -0,0 +1,220 @@ +{ + "PRESET_PARAMS": [ + { + "params": [ + 80, + 0 + ], + "program_delay_map": { + "2": 0, + "300": 0, + "6": 0, + "700": 0 + }, + "relay_config": { + "turn_on_relay_start": 10, + "turn_off_relay_start": 144, + "ex_turn_on_relay_start": 10, + "ex_turn_off_relay_start": 144 + } + }, + { + "params": [ + 80, + 0 + ], + "program_delay_map": { + "2": 0, + "300": 0, + "6": 0, + "700": 0 + }, + "relay_config": { + "turn_on_relay_start": 10, + "turn_off_relay_start": 144, + "ex_turn_on_relay_start": 10, + "ex_turn_off_relay_start": 144 + } + }, + { + "params": [ + 80, + 0 + ], + "program_delay_map": { + "2": 0, + "300": 0, + "6": 0, + "700": 0 + }, + "relay_config": { + "turn_on_relay_start": 10, + "turn_off_relay_start": 144, + "ex_turn_on_relay_start": 10, + "ex_turn_off_relay_start": 144 + } + }, + { + "params": [ + 80, + 0 + ], + "program_delay_map": { + "2": 0, + "300": 0, + "6": 0, + "700": 0 + }, + "relay_config": { + "turn_on_relay_start": 10, + "turn_off_relay_start": 144, + "ex_turn_on_relay_start": 10, + "ex_turn_off_relay_start": 144 + } + }, + { + "params": [ + 80, + 0 + ], + "program_delay_map": { + "2": 0, + "300": 0, + "6": 0, + "700": 0 + }, + "relay_config": { + "turn_on_relay_start": 10, + "turn_off_relay_start": 144, + "ex_turn_on_relay_start": 10, + "ex_turn_off_relay_start": 144 + } + }, + { + "params": [ + 80, + 0 + ], + "program_delay_map": { + "2": 0, + "300": 0, + "6": 0, + "700": 0 + }, + "relay_config": { + "turn_on_relay_start": 10, + "turn_off_relay_start": 144, + "ex_turn_on_relay_start": 10, + "ex_turn_off_relay_start": 144 + } + }, + { + "params": [ + 80, + 0 + ], + "program_delay_map": { + "2": 0, + "300": 0, + "6": 0, + "700": 0 + }, + "relay_config": { + "turn_on_relay_start": 10, + "turn_off_relay_start": 144, + "ex_turn_on_relay_start": 10, + "ex_turn_off_relay_start": 144 + } + }, + { + "params": [ + 80, + 0 + ], + "program_delay_map": { + "2": 0, + "300": 0, + "6": 0, + "700": 0 + }, + "relay_config": { + "turn_on_relay_start": 10, + "turn_off_relay_start": 144, + "ex_turn_on_relay_start": 10, + "ex_turn_off_relay_start": 144 + } + }, + { + "params": [ + 80, + 0 + ], + "program_delay_map": { + "2": 0, + "300": 0, + "6": 0, + "700": 0 + }, + "relay_config": { + "turn_on_relay_start": 10, + "turn_off_relay_start": 144, + "ex_turn_on_relay_start": 10, + "ex_turn_off_relay_start": 144 + } + }, + { + "params": [ + 80, + 0 + ], + "program_delay_map": { + "2": 0, + "300": 0, + "6": 0, + "700": 0 + }, + "relay_config": { + "turn_on_relay_start": 10, + "turn_off_relay_start": 144, + "ex_turn_on_relay_start": 10, + "ex_turn_off_relay_start": 144 + } + }, + { + "params": [ + 80, + 0 + ], + "program_delay_map": { + "2": 0, + "300": 0, + "6": 0, + "700": 0 + }, + "relay_config": { + "turn_on_relay_start": 10, + "turn_off_relay_start": 144, + "ex_turn_on_relay_start": 10, + "ex_turn_off_relay_start": 144 + } + }, + { + "params": [ + 80, + 0 + ], + "program_delay_map": { + "2": 0, + "300": 0, + "6": 0, + "700": 0 + }, + "relay_config": { + "turn_on_relay_start": 10, + "turn_off_relay_start": 144, + "ex_turn_on_relay_start": 10, + "ex_turn_off_relay_start": 144 + } + } + ] +} \ No newline at end of file diff --git a/Kelaifen_V6.0/gpio.py b/Kelaifen_V6.0/gpio.py new file mode 100644 index 0000000..8183be7 --- /dev/null +++ b/Kelaifen_V6.0/gpio.py @@ -0,0 +1,37 @@ +# region 继电器 + + +import RPi.GPIO as GPIO + + +import time +# GPIO 代码 +# 定义继电器引脚 +EN_Relay = 17 + + +def init_gpio(): + """初始化GPIO设置""" + GPIO.setmode(GPIO.BCM) + GPIO.setup(EN_Relay, GPIO.OUT) + # 初始化为高电平（关闭状态） + GPIO.output(EN_Relay, GPIO.LOW) + time.sleep(1) # 等待稳定 + + +def turn_on_relay(): + """打开继电器（设置为LOW）""" + try: + GPIO.output(EN_Relay, GPIO.HIGH) + #print('Relay turned on (LOW)') + except Exception as e: + print(f"Error turning on relay: {e}") + + +def turn_off_relay(): + """关闭继电器（设置为HIGH）""" + try: + GPIO.output(EN_Relay, GPIO.LOW) + #print('Relay turned off (HIGH)') + except Exception as e: + print(f"Error turning off relay: {e}") \ No newline at end of file diff --git a/Kelaifen_V6.0/joint_positions_assembled_5.txt b/Kelaifen_V6.0/joint_positions_assembled_5.txt new file mode 100644 index 0000000..bd75977 --- /dev/null +++ b/Kelaifen_V6.0/joint_positions_assembled_5.txt @@ -0,0 +1 @@ +-44.397,-150.247,154.254,-92.630,87.801,17.825 \ No newline at end of file diff --git a/Kelaifen_V6.0/joint_positions_assembled_6.txt b/Kelaifen_V6.0/joint_positions_assembled_6.txt new file mode 100644 index 0000000..e8cc3ef --- /dev/null +++ b/Kelaifen_V6.0/joint_positions_assembled_6.txt @@ -0,0 +1 @@ +37.181,-125.450,137.593,-98.126,88.215,26.116 \ No newline at end of file diff --git a/Kelaifen_V6.0/joint_positions_assembled_7.txt b/Kelaifen_V6.0/joint_positions_assembled_7.txt new file mode 100644 index 0000000..75800e6 --- /dev/null +++ b/Kelaifen_V6.0/joint_positions_assembled_7.txt @@ -0,0 +1 @@ +65.718,-110.716,120.667,-78.282,94.033,2.384 \ No newline at end of file diff --git a/Kelaifen_V6.0/joint_positions_assembled_8.txt b/Kelaifen_V6.0/joint_positions_assembled_8.txt new file mode 100644 index 0000000..4cf6e75 --- /dev/null +++ b/Kelaifen_V6.0/joint_positions_assembled_8.txt @@ -0,0 +1 @@ +6.496,-110.716,120.667,-78.282,94.033,2.384 \ No newline at end of file diff --git a/Kelaifen_V6.0/program_config.py b/Kelaifen_V6.0/program_config.py new file mode 100644 index 0000000..02d664a --- /dev/null +++ b/Kelaifen_V6.0/program_config.py @@ -0,0 +1,78 @@ +import json +from trajectory_program import ( + program1, program1_1, program2, program3, program3_1, program4, + program5, program5_1, program6, program7, program7_1, program8, + program9, program10, program11, program12, program100, program101 +) + +program_switch = { + 1: program1, + 100: program1_1, + 2: program101, + 3: program3, + 300: program3_1, + 4: program4, + 5: program5, + 500: program5_1, + 6: program100, + 7: program7, + 700: program7_1, + 8: program8, + 9: program9, # 停机位置 + 10: program10, # 初始位置 + 11: program11, # 洗枪位置 + 12: program12, # 试枪位置 +} + +# 定义四种循环序列 +sequence_1 = [2, 300, 4, 100] # 情况1的循环序列，更改为循环序列使用透传 +sequence_2 = [4, 1, 2, 3] # 情况2的循环序列，待修改,暂从程序中删除逻辑，如有需要，从V2.0中复制 +sequence_3 = [6, 700, 8, 500] # 情况3的循环序列，更改为循环序列使用透传 +sequence_4 = [8, 5, 6, 7] # 情况4的循环序列，待修改,暂从程序中删除逻辑，如有需要，从V2.0中复制 + +def load_process_config(config_path="/home/raspberrypi/robot1/config.json"): + try: + # 读取 JSON 文件 + with open(config_path, "r", encoding="utf-8") as f: + config = json.load(f) + + # ========== 关键：适配新的 JSON 结构 ========== + processed_presets = [] + for preset in config["PRESET_PARAMS"]: + # 1. 处理单个预设项内的 program_delay_map（键转整数，值转浮点数） + processed_delay_map = { + int(key): float(value) + for key, value in preset["program_delay_map"].items() + } + + # 2. 处理 params（数组转元组，保持原代码格式） + processed_params = tuple(preset["params"]) + + # 3. 保留 relay_config（无需类型转换，直接使用） + processed_relay = preset["relay_config"] + + # 4. 组装处理后的预设项 + processed_presets.append({ + "params": processed_params, + "program_delay_map": processed_delay_map, + "relay_config": processed_relay + }) + + # 替换原 PRESET_PARAMS 为处理后的版本 + config["PRESET_PARAMS"] = processed_presets + + print(f"✅ 已加载配置文件，共加载 {len(processed_presets)} 个预设参数组") + return config + + except FileNotFoundError: + print(f"❌ 配置文件 {config_path} 未找到！") + return None + except json.JSONDecodeError: + print(f"❌ 配置文件 {config_path} 格式错误！") + return None + except KeyError as e: + print(f"❌ 配置文件结构错误：缺少关键字段 {str(e)}，请检查 JSON 格式") + return None + except Exception as e: + print(f"❌ 加载配置失败：{str(e)}") + return None \ No newline at end of file diff --git a/Kelaifen_V6.0/readme.txt b/Kelaifen_V6.0/readme.txt new file mode 100644 index 0000000..5940947 --- /dev/null +++ b/Kelaifen_V6.0/readme.txt @@ -0,0 +1,19 @@ +1、config.json: +（1）、config.json中的program_delay_map代表每段程序执行结束后的延时时间，单位秒 + + sequence_1 = [2, 300, 4, 100] # 情况1的循环序列 + sequence_3 = [6, 700, 8, 500] # 情况3的循环序列 + +（2）、config.json中的PRESET_PARAMS代表预设的速度和角度配置列表（速度百分比，角度偏移）， +可根据实际需求修改，配置顺序为1-12 +（3）、config.json中的relay_config喷枪开启和关闭位置 + +2、可根据实际情况修改机械臂位置 +joint_positions_assembled_5、Pose_5.txt分别代表停机位置的关节位置和位姿 +joint_positions_assembled_6.txt、Pose_6.txt分别代表初始位置的关节位置和位姿 +joint_positions_assembled_7.txt、Pose_7.txt分别代表洗枪位置的关节位置和位姿 +joint_positions_assembled_8.txt、Pose_8.txt分别代表试枪位置的关节位置和位姿 + +3、计算方法 +直接将示教器屏幕显示关节位置数据直接填到joint_positions_assembled文件 +位姿数据前三个数据不变，后三个数据，使用屏幕数据，根据公式4.76* (后三数据/180)，将计算结果放到Pose文件 \ No newline at end of file diff --git a/Kelaifen_V6.0/serial_handler.py b/Kelaifen_V6.0/serial_handler.py new file mode 100644 index 0000000..1929532 --- /dev/null +++ b/Kelaifen_V6.0/serial_handler.py @@ -0,0 +1,157 @@ +# serial_handler.py +import serial +import time +from serial_init import SerialSharedData +from transform import transform_data + +# 声明需要用到的全局变量（与主程序保持一致） +speed_adjustment = 100 # 初始值设为100，在50-100范围内 +Trajectory_angle = 0 +compensate_queue = None # 后续由主程序赋值 + +# 初始化全局共享数据实例 +serial_shared = SerialSharedData() +# 串口接收线程函数，持续监听串口数据，解析0xFF坐标帧并更新补偿变量 +def serial_receive_data(ser): + """线程内持续接收串口数据，同步解析0xFF坐标帧并更新全局补偿变量""" + frame_header = 0xFF + frame_length = 7 # 0xFF + x(2) + y(2) + z(2) + + while True: + try: + if not ser or not isinstance(ser, serial.Serial) or not ser.is_open: + time.sleep(0.1) + continue + + if ser.in_waiting > 0: + data = ser.read(ser.in_waiting) + with serial_shared.lock: + serial_shared.buffer += data + serial_shared.cmd_buffer += data + + # 在接收线程中解析0xFF坐标帧 + with serial_shared.lock: + current_buf = serial_shared.buffer[:] + + if len(current_buf) >= frame_length: + header_index = current_buf.find(bytes([frame_header])) + if header_index != -1 and header_index + frame_length <= len(current_buf): + # 提取完整帧并更新缓冲区 + frame = current_buf[header_index:header_index+frame_length] + with serial_shared.lock: + serial_shared.buffer = serial_shared.buffer[header_index+frame_length:] + + # 坐标解码 + x_coord = (frame[1] << 8) | frame[2] + x_coord = x_coord - 0x10000 if x_coord & 0x8000 else x_coord + if x_coord>90: + x_coord = 90 + elif x_coord<-90: + x_coord = -90 + y_coord = (frame[3] << 8) | frame[4] + y_coord = y_coord - 0x10000 if y_coord & 0x8000 else y_coord + if y_coord>50: + y_coord = 50 + elif y_coord<-50: + y_coord = -50 + z_coord = (frame[5] << 8) | frame[6] + z_coord = z_coord - 0x10000 if z_coord & 0x8000 else z_coord + if z_coord>90: + z_coord = 90 + elif z_coord<-90: + z_coord = -90 + + inc_x, inc_y, inc_z = transform_data(x_coord, y_coord, z_coord) + # 存入队列（覆盖旧值，确保取到最新） + if compensate_queue is not None and not compensate_queue.empty(): + compensate_queue.get_nowait() + if compensate_queue is not None: + compensate_queue.put((inc_x, inc_y, inc_z)) + + print(f"接收线程解析更新 - x:{x_coord:.1f}, y:{y_coord:.1f}, z:{z_coord:.1f}") + + except Exception as e: + print(f"串口接收线程错误: {str(e)}") + time.sleep(0.005) +# 从共享缓冲区读取指令帧，解析0xAA指令并返回指令内容（加锁确保线程安全） +def read_cmd_from_shared(timeout=1.0): + """从共享缓冲区读取指令帧，解析0xAA指令并返回指令内容（加锁确保线程安全）""" + frame_header = 0xAA + cmd_length = 2 # 指令帧结构：0xAA（帧头） + 1字节指令值 + start_time = time.time() + + while (time.time() - start_time) < timeout: + # 加锁读取指令缓冲区 + with serial_shared.lock: + current_cmd_buf = serial_shared.cmd_buffer[:] + + if len(current_cmd_buf) >= cmd_length: + header_index = current_cmd_buf.find(bytes([frame_header])) + if header_index != -1: + # 提取完整指令帧 + cmd_bytes = current_cmd_buf[header_index:header_index+cmd_length] + # 更新共享缓冲区（移除已处理指令） + with serial_shared.lock: + serial_shared.cmd_buffer = serial_shared.cmd_buffer[header_index+cmd_length:] + return cmd_bytes + time.sleep(0.01) + return None +# 处理特殊指令0xBB，更新全局参数并打印清晰信息 +def handle_special_commands(PRESET_PARAMS): + + global speed_adjustment, Trajectory_angle + + bb_header = 0xBB + bb_frame_length = 2 # 0xBB + 1字节配置选择值(0-11) + # 初始化返回值（默认无选中配置） + selected_delay_map = None + selected_relay_config = None + + # 加锁读取缓冲区，避免线程冲突 + with serial_shared.lock: + current_cmd_buf = serial_shared.cmd_buffer[:] + + # 处理0xBB指令（参数配置选择） + bb_index = current_cmd_buf.find(bytes([bb_header])) + if bb_index != -1 and bb_index + bb_frame_length <= len(current_cmd_buf): + # 提取完整的0xBB帧 + bb_frame = current_cmd_buf[bb_index:bb_index+bb_frame_length] + # 解析配置选择值（1-12有效，对应PRESET_PARAMS的0-11索引） + config_select = bb_frame[1] + + # 校验配置值范围（仅处理1-12，共12组） + if 1 <= config_select <= len(PRESET_PARAMS): + # 匹配预设参数项（索引 = 配置值 - 1） + selected_preset = PRESET_PARAMS[config_select - 1] + # 提取params并更新全局变量 + speed_val, angle_val = selected_preset["params"] + if speed_val < 50: + speed_val = 50 + if speed_val > 150: + speed_val = 150 + if angle_val > 200: + angle_val = 200 + speed_adjustment = speed_val + Trajectory_angle = angle_val + # 提取当前配置项对应的delay和relay配置（赋值给返回变量） + selected_delay_map = selected_preset["program_delay_map"] + selected_relay_config = selected_preset["relay_config"] + + # 打印清晰的配置信息 + print(f"=== 0xBB指令处理成功 ===") + print(f"配置选择值：{config_select}") + print(f"speed_adjustment: {speed_adjustment}%") + print(f"Trajectory_angle: {Trajectory_angle}") + print(f"已加载对应program_delay_map和relay_config配置") + + # 移除已处理的帧（加锁更新缓冲区） + with serial_shared.lock: + serial_shared.cmd_buffer = serial_shared.cmd_buffer[bb_index+bb_frame_length:] + else: + # 无效配置值处理：清理帧但不更新参数 + with serial_shared.lock: + serial_shared.cmd_buffer = serial_shared.cmd_buffer[bb_index+bb_frame_length:] + print(f"⚠️ 收到0xBB指令，配置选择值{config_select}无效（仅支持1-{len(PRESET_PARAMS)}）") + + # 返回选中的delay配置和relay配置（无选中则返回None） + return selected_delay_map, selected_relay_config \ No newline at end of file diff --git a/Kelaifen_V6.0/serial_init.py b/Kelaifen_V6.0/serial_init.py new file mode 100644 index 0000000..6414e73 --- /dev/null +++ b/Kelaifen_V6.0/serial_init.py @@ -0,0 +1,27 @@ +# region 串口通信 +# 初始化串口 +import threading +import serial + + +def serial_init(): + try: + ser = serial.Serial( + port="/dev/ttyS0", # 笔记本Windows端口，Linux/Mac替换为"/dev/ttyUSB0" + baudrate=115200, + timeout=0.1 + ) + print("串口初始化成功") + return ser + except Exception as e: + print(f"笔记本串口初始化失败: {e}") + return None + + +# 矩阵转换函数：将P_L1_O2通过R_T转换为P_L1_O1 +class SerialSharedData: + """线程安全的串口数据共享存储类""" + def __init__(self): + self.lock = threading.Lock() # 防止多线程数据竞争 + self.buffer = b'' # 用于解析0xFF坐标帧的通用缓冲区 + self.cmd_buffer = b'' # 用于解析0xAA指令帧（主指令/子指令/停止指令）的缓冲区 \ No newline at end of file diff --git a/Kelaifen_V6.0/trajectory_data.py b/Kelaifen_V6.0/trajectory_data.py new file mode 100644 index 0000000..87e874e --- /dev/null +++ b/Kelaifen_V6.0/trajectory_data.py @@ -0,0 +1,737 @@ + + +joint_positions_assembled_1 = [[-8.303461, -48.278357, 71.508258, -115.343, 89.659, -5.706]] + +joint_positions_assembled_2 = [[92.442,-41.967,60.096,-110.133,90.041,11.741]] + +joint_positions_assembled_3 = [[-8.437,-52.830,64.769,-101.519,91.959,-9.155]] + +joint_positions_assembled_4 = [[92.553,-45.178,53.196,-97.284,97.995,15.177]] + +Pose_1 = [ + [1549.9000, -66.7470, 360.2000, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 360.2000, 3.1398, 0.0349, -1.5580], + [1549.9000, -66.7470, 360.2000, 3.1400, 0.0349, -1.5534], + [1549.9000, -66.7470, 360.2000, 3.1403, 0.0350, -1.5440], + [1549.9000, -66.7470, 360.2000, 3.1408, 0.0350, -1.5301], + [1549.9000, -66.7470, 360.2000, 3.1415, 0.0350, -1.5114], + [1549.9000, -66.7470, 360.2000, -3.1409, 0.0350, -1.4882], + [1549.9000, -66.7470, 360.2000, -3.1399, 0.0349, -1.4602], + [1549.9000, -66.7470, 360.2000, -3.1388, 0.0349, -1.4276], + [1549.9000, -66.7470, 360.2000, -3.1375, 0.0347, -1.3904], + [1549.9000, -66.7470, 360.2000, -3.1361, 0.0345, -1.3484], + [1549.5000, -66.3590, 360.2500, -3.1345, 0.0342, -1.3025], + [1547.7000, -64.6380, 360.4500, -3.1329, 0.0339, -1.2559], + [1544.4000, -61.5410, 360.8300, -3.1313, 0.0334, -1.2093], + [1539.7000, -57.0660, 361.3700, -3.1298, 0.0329, -1.1628], + [1533.6000, -51.2150, 362.0700, -3.1282, 0.0323, -1.1162], + [1526.0000, -43.9870, 362.9400, -3.1268, 0.0317, -1.0696], + [1517.0000, -35.3830, 363.9800, -3.1253, 0.0309, -1.0231], + [1506.6000, -25.4010, 365.1800, -3.1239, 0.0302, -0.9765], + [1494.7000, -14.0430, 366.5500, -3.1225, 0.0293, -0.9300], + [1481.4000, -1.3082, 368.0900, -3.1212, 0.0284, -0.8834], + [1467.0000, 12.4160, 369.7400, -3.1205, 0.0279, -0.8601], + [1452.6000, 26.1840, 371.4000, -3.1205, 0.0279, -0.8601], + [1438.2000, 39.9510, 373.0600, -3.1205, 0.0279, -0.8601], + [1423.8000, 53.7190, 374.7200, -3.1205, 0.0279, -0.8601], + [1409.3000, 67.4860, 376.3800, -3.1205, 0.0279, -0.8601], + [1394.9000, 81.2540, 378.0400, -3.1205, 0.0279, -0.8601], + [1380.5000, 95.0210, 379.7000, -3.1205, 0.0279, -0.8601], + [1366.1000, 108.7900, 381.3600, -3.1205, 0.0279, -0.8601], + [1351.7000, 122.5600, 383.0200, -3.1205, 0.0279, -0.8601], + [1337.3000, 136.3200, 384.6800, -3.1205, 0.0279, -0.8601], + [1322.9000, 150.0900, 386.3400, -3.1205, 0.0279, -0.8601], + [1308.5000, 163.8600, 388.0000, -3.1205, 0.0279, -0.8601], + [1294.1000, 177.6300, 389.6600, -3.1205, 0.0279, -0.8601], + [1279.7000, 191.3900, 391.3200, -3.1205, 0.0279, -0.8601], + [1265.2000, 205.1600, 392.9800, -3.1205, 0.0279, -0.8601], + [1250.8000, 218.9300, 394.6400, -3.1205, 0.0279, -0.8601], + [1236.4000, 232.7000, 396.3000, -3.1205, 0.0279, -0.8601], + [1222.0000, 246.4600, 397.9600, -3.1205, 0.0279, -0.8601], + [1204.0000, 263.6700, 400.0300, -3.1205, 0.0279, -0.8601], + [1189.6000, 277.4400, 401.6900, -3.1205, 0.0279, -0.8601], + [1175.2000, 291.2100, 403.3500, -3.1205, 0.0279, -0.8601], + [1160.8000, 304.9800, 405.0100, -3.1205, 0.0279, -0.8601], + [1146.3000, 318.7400, 406.6700, -3.1205, 0.0279, -0.8601], + [1131.9000, 332.5100, 408.3300, -3.1205, 0.0279, -0.8601], + [1117.5000, 346.2800, 409.9900, -3.1205, 0.0279, -0.8601], + [1103.1000, 360.0500, 411.6500, -3.1205, 0.0279, -0.8601], + [1088.7000, 373.8100, 413.3100, -3.1205, 0.0279, -0.8601], + [1074.3000, 387.5800, 414.9700, -3.1205, 0.0279, -0.8601], + [1059.9000, 401.3500, 416.6300, -3.1205, 0.0279, -0.8601], + [1045.5000, 415.1100, 418.2900, -3.1205, 0.0279, -0.8601], + [1031.0000, 428.8800, 419.9500, -3.1205, 0.0279, -0.8601], + [1016.6000, 442.6500, 421.6100, -3.1205, 0.0279, -0.8601], + [1002.2000, 456.4200, 423.2700, -3.1205, 0.0279, -0.8601], + [987.8000, 470.1800, 424.9300, -3.1205, 0.0279, -0.8601], + [973.3900, 483.9500, 426.5800, -3.1205, 0.0279, -0.8601], + [958.9800, 497.7200, 428.2500, -3.1205, 0.0279, -0.8601], + [944.5700, 511.4900, 429.9000, -3.1205, 0.0279, -0.8601], + [930.1600, 525.2500, 431.5600, -3.1205, 0.0279, -0.8601], + [915.7500, 539.0200, 433.2200, -3.1205, 0.0279, -0.8601], + [901.3300, 552.7900, 434.8800, -3.1205, 0.0279, -0.8601], + [886.9200, 566.5600, 436.5400, -3.1205, 0.0279, -0.8601], + [872.5100, 580.3200, 438.2000, -3.1205, 0.0279, -0.8601], + [858.1000, 594.0900, 439.8600, -3.1205, 0.0279, -0.8601], + [843.6900, 607.8600, 441.5200, -3.1205, 0.0279, -0.8601], + [829.2700, 621.6300, 443.1800, -3.1205, 0.0279, -0.8601], + [814.8600, 635.3900, 444.8400, -3.1205, 0.0279, -0.8601], + [800.4500, 649.1600, 446.5000, -3.1205, 0.0279, -0.8601], + [786.0400, 662.9300, 448.1600, -3.1205, 0.0279, -0.8601], + [771.6300, 676.7000, 449.8200, -3.1205, 0.0279, -0.8601], + [757.2200, 690.4600, 451.4800, -3.1205, 0.0279, -0.8601], + [742.8000, 704.2300, 453.1400, -3.1205, 0.0279, -0.8601], + [728.3900, 718.0000, 454.8000, -3.1205, 0.0279, -0.8601], + [713.9800, 731.7700, 456.4600, -3.1205, 0.0279, -0.8601], + [699.5700, 745.5300, 458.1200, -3.1205, 0.0279, -0.8601], + [685.1500, 759.3000, 459.7800, -3.1205, 0.0279, -0.8601], + [670.7400, 773.0700, 461.4400, -3.1205, 0.0279, -0.8601], + [656.3300, 786.8400, 463.1000, -3.1205, 0.0279, -0.8601], + [641.9200, 800.6000, 464.7600, -3.1205, 0.0279, -0.8601], + [627.5100, 814.3700, 466.4200, -3.1205, 0.0279, -0.8601], + [613.1000, 828.1400, 468.0800, -3.1205, 0.0279, -0.8601], + [598.6800, 841.9100, 469.7400, -3.1205, 0.0279, -0.8601], + [584.2700, 855.6700, 471.3900, -3.1205, 0.0279, -0.8601], + [569.8600, 869.4400, 473.0600, -3.1205, 0.0279, -0.8601], + [555.4500, 883.2100, 474.7200, -3.1205, 0.0279, -0.8601], + [541.0400, 896.9800, 476.3700, -3.1205, 0.0279, -0.8601], + [526.6200, 910.7400, 478.0300, -3.1205, 0.0279, -0.8601], + [512.2100, 924.5100, 479.6900, -3.1205, 0.0279, -0.8601], + [497.8000, 938.2800, 481.3500, -3.1205, 0.0279, -0.8601], + [483.3900, 952.0400, 483.0100, -3.1205, 0.0279, -0.8601], + [468.9800, 965.8100, 484.6700, -3.1205, 0.0279, -0.8601], + [454.5600, 979.5800, 486.3300, -3.1205, 0.0279, -0.8601], + [440.1500, 993.3500, 487.9900, -3.1205, 0.0279, -0.8601], + [425.7400, 1007.1000, 489.6500, -3.1205, 0.0279, -0.8601], + [411.3300, 1020.9000, 491.3100, -3.1205, 0.0279, -0.8601], + [396.9200, 1034.7000, 492.9700, -3.1205, 0.0279, -0.8601], + [382.5000, 1048.4000, 494.6300, -3.1205, 0.0279, -0.8601], + [368.0900, 1062.2000, 496.2900, -3.1205, 0.0279, -0.8601], + [353.6800, 1076.0000, 497.9500, -3.1205, 0.0279, -0.8601], + [339.2700, 1089.7000, 499.6100, -3.1205, 0.0279, -0.8601], + [324.8600, 1103.5000, 501.2700, -3.1205, 0.0279, -0.8601], + [310.4500, 1117.2000, 502.9300, -3.1205, 0.0279, -0.8601], + [296.0300, 1131.0000, 504.5900, -3.1205, 0.0279, -0.8601], + [281.6200, 1144.8000, 506.2500, -3.1205, 0.0279, -0.8601], + [267.2100, 1158.6000, 507.9100, -3.1205, 0.0279, -0.8601], + [252.8000, 1172.3000, 509.5700, -3.1205, 0.0279, -0.8601], + [238.3900, 1186.1000, 511.2300, -3.1205, 0.0279, -0.8601], + [223.9700, 1199.9000, 512.8900, -3.1205, 0.0279, -0.8601], + [209.5600, 1213.6000, 514.5500, -3.1205, 0.0279, -0.8601], + [195.1500, 1227.4000, 516.2100, -3.1205, 0.0279, -0.8601], + [180.7400, 1241.2000, 517.8700, -3.1205, 0.0279, -0.8601], + [166.3300, 1254.9000, 519.5200, -3.1205, 0.0279, -0.8601], + [151.9100, 1268.7000, 521.1800, -3.1205, 0.0279, -0.8601], + [137.5000, 1282.5000, 522.8400, -3.1205, 0.0279, -0.8601], + [123.0900, 1296.2000, 524.5000, -3.1205, 0.0279, -0.8601], + [105.0800, 1313.4000, 526.5800, -3.1205, 0.0279, -0.8601], + [90.6650, 1327.2000, 528.2400, -3.1205, 0.0279, -0.8601], + [76.2530, 1341.0000, 529.9000, -3.1205, 0.0279, -0.8601], + [61.8410, 1354.7000, 531.5600, -3.1205, 0.0279, -0.8601], + [47.4290, 1368.5000, 533.2200, -3.1205, 0.0279, -0.8601], + [33.0170, 1382.3000, 534.8800, -3.1205, 0.0279, -0.8601], + [18.6050, 1396.0000, 536.5400, -3.1205, 0.0279, -0.8601], + [4.1933, 1409.8000, 538.2000, -3.1205, 0.0279, -0.8601], + [-10.2190, 1423.6000, 539.8600, -3.1205, 0.0279, -0.8601], + [-24.6310, 1437.3000, 541.5100, -3.1205, 0.0279, -0.8601], + [-39.0420, 1451.1000, 543.1700, -3.1205, 0.0279, -0.8601], + [-53.4540, 1464.9000, 544.8400, -3.1205, 0.0279, -0.8601], + [-67.8660, 1478.7000, 546.4900, -3.1205, 0.0279, -0.8601], + [-82.2780, 1492.4000, 548.1500, -3.1205, 0.0279, -0.8601], + [-96.6900, 1506.2000, 549.8100, -3.1205, 0.0279, -0.8601], + [-111.1000, 1520.0000, 551.4700, -3.1205, 0.0279, -0.8601], + [-125.5100, 1533.7000, 553.1300, -3.1205, 0.0279, -0.8601], + [-139.9300, 1547.5000, 554.7900, -3.1205, 0.0279, -0.8601], + [-154.3400, 1561.3000, 556.4500, -3.1205, 0.0279, -0.8601], + [-168.1900, 1574.5000, 558.0500, -3.1200, 0.0275, -0.8428], + [-180.6100, 1586.3000, 559.4800, -3.1182, 0.0260, -0.7756], + [-191.5900, 1596.8000, 560.7400, -3.1166, 0.0245, -0.7119], + [-201.1300, 1606.0000, 561.8400, -3.1152, 0.0229, -0.6517], + [-209.2300, 1613.7000, 562.7700, -3.1139, 0.0214, -0.5949], + [-215.8800, 1620.0000, 563.5400, -3.1128, 0.0199, -0.5417], + [-221.1000, 1625.0000, 564.1400, -3.1119, 0.0184, -0.4920], + [-224.8700, 1628.6000, 564.5800, -3.1111, 0.0170, -0.4458], + [-227.2000, 1630.9000, 564.8400, -3.1103, 0.0157, -0.4030], + [-228.0900, 1631.7000, 564.9500, -3.1098, 0.0145, -0.3638], + [-228.1000, 1631.7000, 564.9500, -3.1093, 0.0133, -0.3280], + [-228.1000, 1631.7000, 564.9500, -3.1088, 0.0123, -0.2957], + [-228.1000, 1631.7000, 564.9500, -3.1085, 0.0113, -0.2669], + [-228.1000, 1631.7000, 564.9500, -3.1082, 0.0105, -0.2416], + [-228.1000, 1631.7000, 564.9500, -3.1080, 0.0098, -0.2198], + [-228.1000, 1631.7000, 564.9500, -3.1078, 0.0091, -0.2015], + [-228.1000, 1631.7000, 564.9500, -3.1077, 0.0086, -0.1867], + [-228.1000, 1631.7000, 564.9500, -3.1076, 0.0083, -0.1754], + [-228.1000, 1631.7000, 564.9500, -3.1075, 0.0080, -0.1675], + [-228.1000, 1631.7000, 564.9500, -3.1075, 0.0078, -0.1631], + [-228.1000, 1631.7000, 564.9500, -3.1075, 0.0078, -0.1622] +] + +Pose_1_2 = [ + [-228.1000, 1631.7000, 564.9500, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 554.1737, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 543.3974, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 532.6211, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 521.8447, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 511.0684, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 500.2921, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 489.5158, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 478.7395, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 467.9632, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 457.1868, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 446.4105, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 435.6342, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 424.8579, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 414.0816, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 403.3053, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 392.5289, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 381.7526, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 370.9763, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 360.2000, -3.1075, 0.0078, -0.1622] +] + +Pose_2 = [ + [-228.1000, 1631.7000, 360.2000, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 360.2000, -3.1075, 0.0078, -0.1631], + [-228.1000, 1631.7000, 360.2000, -3.1075, 0.0080, -0.1675], + [-228.1000, 1631.7000, 360.2000, -3.1076, 0.0083, -0.1754], + [-228.1000, 1631.7000, 360.2000, -3.1077, 0.0086, -0.1867], + [-228.1000, 1631.7000, 360.2000, -3.1078, 0.0091, -0.2015], + [-228.1000, 1631.7000, 360.2000, -3.1080, 0.0098, -0.2198], + [-228.1000, 1631.7000, 360.2000, -3.1082, 0.0105, -0.2416], + [-228.1000, 1631.7000, 360.2000, -3.1085, 0.0113, -0.2669], + [-228.1000, 1631.7000, 360.2000, -3.1088, 0.0123, -0.2957], + [-228.1000, 1631.7000, 360.2000, -3.1093, 0.0133, -0.3280], + [-228.0900, 1631.7000, 360.2500, -3.1098, 0.0145, -0.3638], + [-227.2000, 1630.9000, 360.4500, -3.1103, 0.0157, -0.4030], + [-224.8700, 1628.6000, 360.8300, -3.1111, 0.0170, -0.4458], + [-221.1000, 1625.0000, 361.3700, -3.1119, 0.0184, -0.4920], + [-215.8800, 1620.0000, 362.0700, -3.1128, 0.0199, -0.5417], + [-209.2300, 1613.7000, 362.9400, -3.1139, 0.0214, -0.5949], + [-201.1300, 1606.0000, 363.9800, -3.1152, 0.0229, -0.6517], + [-191.5900, 1596.8000, 365.1800, -3.1166, 0.0245, -0.7119], + [-180.6100, 1586.3000, 366.5500, -3.1182, 0.0260, -0.7756], + [-168.1900, 1574.5000, 368.0900, -3.1200, 0.0275, -0.8428], + [-154.3400, 1561.3000, 369.7400, -3.1205, 0.0279, -0.8601], + [-139.9300, 1547.5000, 371.4000, -3.1205, 0.0279, -0.8601], + [-125.5100, 1533.7000, 373.0600, -3.1205, 0.0279, -0.8601], + [-111.1000, 1520.0000, 374.7200, -3.1205, 0.0279, -0.8601], + [-96.6900, 1506.2000, 376.3800, -3.1205, 0.0279, -0.8601], + [-82.2780, 1492.4000, 378.0400, -3.1205, 0.0279, -0.8601], + [-67.8660, 1478.7000, 379.7000, -3.1205, 0.0279, -0.8601], + [-53.4540, 1464.9000, 381.3600, -3.1205, 0.0279, -0.8601], + [-39.0420, 1451.1000, 383.0200, -3.1205, 0.0279, -0.8601], + [-24.6310, 1437.3000, 384.6800, -3.1205, 0.0279, -0.8601], + [-10.2190, 1423.6000, 386.3400, -3.1205, 0.0279, -0.8601], + [4.1933, 1409.8000, 388.0000, -3.1205, 0.0279, -0.8601], + [18.6050, 1396.0000, 389.6600, -3.1205, 0.0279, -0.8601], + [33.0170, 1382.3000, 391.3200, -3.1205, 0.0279, -0.8601], + [47.4290, 1368.5000, 392.9800, -3.1205, 0.0279, -0.8601], + [61.8410, 1354.7000, 394.6400, -3.1205, 0.0279, -0.8601], + [76.2530, 1341.0000, 396.3000, -3.1205, 0.0279, -0.8601], + [90.6650, 1327.2000, 397.9600, -3.1205, 0.0279, -0.8601], + [105.0800, 1313.4000, 400.0300, -3.1205, 0.0279, -0.8601], + [123.0900, 1296.2000, 401.6900, -3.1205, 0.0279, -0.8601], + [137.5000, 1282.5000, 403.3500, -3.1205, 0.0279, -0.8601], + [151.9100, 1268.7000, 405.0100, -3.1205, 0.0279, -0.8601], + [166.3300, 1254.9000, 406.6700, -3.1205, 0.0279, -0.8601], + [180.7400, 1241.2000, 408.3300, -3.1205, 0.0279, -0.8601], + [195.1500, 1227.4000, 409.9900, -3.1205, 0.0279, -0.8601], + [209.5600, 1213.6000, 411.6500, -3.1205, 0.0279, -0.8601], + [223.9700, 1199.9000, 413.3100, -3.1205, 0.0279, -0.8601], + [238.3900, 1186.1000, 414.9700, -3.1205, 0.0279, -0.8601], + [252.8000, 1172.3000, 416.6300, -3.1205, 0.0279, -0.8601], + [267.2100, 1158.6000, 418.2900, -3.1205, 0.0279, -0.8601], + [281.6200, 1144.8000, 419.9500, -3.1205, 0.0279, -0.8601], + [296.0300, 1131.0000, 421.6100, -3.1205, 0.0279, -0.8601], + [310.4500, 1117.2000, 423.2700, -3.1205, 0.0279, -0.8601], + [324.8600, 1103.5000, 424.9300, -3.1205, 0.0279, -0.8601], + [339.2700, 1089.7000, 426.5800, -3.1205, 0.0279, -0.8601], + [353.6800, 1076.0000, 428.2500, -3.1205, 0.0279, -0.8601], + [368.0900, 1062.2000, 429.9000, -3.1205, 0.0279, -0.8601], + [382.5000, 1048.4000, 431.5600, -3.1205, 0.0279, -0.8601], + [396.9200, 1034.7000, 433.2200, -3.1205, 0.0279, -0.8601], + [411.3300, 1020.9000, 434.8800, -3.1205, 0.0279, -0.8601], + [425.7400, 1007.1000, 436.5400, -3.1205, 0.0279, -0.8601], + [440.1500, 993.3500, 438.2000, -3.1205, 0.0279, -0.8601], + [454.5600, 979.5800, 439.8600, -3.1205, 0.0279, -0.8601], + [468.9800, 965.8100, 441.5200, -3.1205, 0.0279, -0.8601], + [483.3900, 952.0400, 443.1800, -3.1205, 0.0279, -0.8601], + [497.8000, 938.2800, 444.8400, -3.1205, 0.0279, -0.8601], + [512.2100, 924.5100, 446.5000, -3.1205, 0.0279, -0.8601], + [526.6200, 910.7400, 448.1600, -3.1205, 0.0279, -0.8601], + [541.0400, 896.9800, 449.8200, -3.1205, 0.0279, -0.8601], + [555.4500, 883.2100, 451.4800, -3.1205, 0.0279, -0.8601], + [569.8600, 869.4400, 453.1400, -3.1205, 0.0279, -0.8601], + [584.2700, 855.6700, 454.8000, -3.1205, 0.0279, -0.8601], + [598.6800, 841.9100, 456.4600, -3.1205, 0.0279, -0.8601], + [613.1000, 828.1400, 458.1200, -3.1205, 0.0279, -0.8601], + [627.5100, 814.3700, 459.7800, -3.1205, 0.0279, -0.8601], + [641.9200, 800.6000, 461.4400, -3.1205, 0.0279, -0.8601], + [656.3300, 786.8400, 463.1000, -3.1205, 0.0279, -0.8601], + [670.7400, 773.0700, 464.7600, -3.1205, 0.0279, -0.8601], + [685.1500, 759.3000, 466.4200, -3.1205, 0.0279, -0.8601], + [699.5700, 745.5300, 468.0800, -3.1205, 0.0279, -0.8601], + [713.9800, 731.7700, 469.7400, -3.1205, 0.0279, -0.8601], + [728.3900, 718.0000, 471.3900, -3.1205, 0.0279, -0.8601], + [742.8000, 704.2300, 473.0600, -3.1205, 0.0279, -0.8601], + [757.2200, 690.4600, 474.7200, -3.1205, 0.0279, -0.8601], + [771.6300, 676.7000, 476.3700, -3.1205, 0.0279, -0.8601], + [786.0400, 662.9300, 478.0300, -3.1205, 0.0279, -0.8601], + [800.4500, 649.1600, 479.6900, -3.1205, 0.0279, -0.8601], + [814.8600, 635.3900, 481.3500, -3.1205, 0.0279, -0.8601], + [829.2700, 621.6300, 483.0100, -3.1205, 0.0279, -0.8601], + [843.6900, 607.8600, 484.6700, -3.1205, 0.0279, -0.8601], + [858.1000, 594.0900, 486.3300, -3.1205, 0.0279, -0.8601], + [872.5100, 580.3200, 487.9900, -3.1205, 0.0279, -0.8601], + [886.9200, 566.5600, 489.6500, -3.1205, 0.0279, -0.8601], + [901.3300, 552.7900, 491.3100, -3.1205, 0.0279, -0.8601], + [915.7500, 539.0200, 492.9700, -3.1205, 0.0279, -0.8601], + [930.1600, 525.2500, 494.6300, -3.1205, 0.0279, -0.8601], + [944.5700, 511.4900, 496.2900, -3.1205, 0.0279, -0.8601], + [958.9800, 497.7200, 497.9500, -3.1205, 0.0279, -0.8601], + [973.3900, 483.9500, 499.6100, -3.1205, 0.0279, -0.8601], + [987.8000, 470.1800, 501.2700, -3.1205, 0.0279, -0.8601], + [1002.2000, 456.4200, 502.9300, -3.1205, 0.0279, -0.8601], + [1016.6000, 442.6500, 504.5900, -3.1205, 0.0279, -0.8601], + [1031.0000, 428.8800, 506.2500, -3.1205, 0.0279, -0.8601], + [1045.5000, 415.1100, 507.9100, -3.1205, 0.0279, -0.8601], + [1059.9000, 401.3500, 509.5700, -3.1205, 0.0279, -0.8601], + [1074.3000, 387.5800, 511.2300, -3.1205, 0.0279, -0.8601], + [1088.7000, 373.8100, 512.8900, -3.1205, 0.0279, -0.8601], + [1103.1000, 360.0500, 514.5500, -3.1205, 0.0279, -0.8601], + [1117.5000, 346.2800, 516.2100, -3.1205, 0.0279, -0.8601], + [1131.9000, 332.5100, 517.8700, -3.1205, 0.0279, -0.8601], + [1146.3000, 318.7400, 519.5200, -3.1205, 0.0279, -0.8601], + [1160.8000, 304.9800, 521.1800, -3.1205, 0.0279, -0.8601], + [1175.2000, 291.2100, 522.8400, -3.1205, 0.0279, -0.8601], + [1189.6000, 277.4400, 524.5000, -3.1205, 0.0279, -0.8601], + [1204.0000, 263.6700, 526.5800, -3.1205, 0.0279, -0.8601], + [1222.0000, 246.4600, 528.2400, -3.1205, 0.0279, -0.8601], + [1236.4000, 232.7000, 529.9000, -3.1205, 0.0279, -0.8601], + [1250.8000, 218.9300, 531.5600, -3.1205, 0.0279, -0.8601], + [1265.2000, 205.1600, 533.2200, -3.1205, 0.0279, -0.8601], + [1279.7000, 191.3900, 534.8800, -3.1205, 0.0279, -0.8601], + [1294.1000, 177.6300, 536.5400, -3.1205, 0.0279, -0.8601], + [1308.5000, 163.8600, 538.2000, -3.1205, 0.0279, -0.8601], + [1322.9000, 150.0900, 539.8600, -3.1205, 0.0279, -0.8601], + [1337.3000, 136.3200, 541.5100, -3.1205, 0.0279, -0.8601], + [1351.7000, 122.5600, 543.1700, -3.1205, 0.0279, -0.8601], + [1366.1000, 108.7900, 544.8400, -3.1205, 0.0279, -0.8601], + [1380.5000, 95.0210, 546.4900, -3.1205, 0.0279, -0.8601], + [1394.9000, 81.2540, 548.1500, -3.1205, 0.0279, -0.8601], + [1409.3000, 67.4860, 549.8100, -3.1205, 0.0279, -0.8601], + [1423.8000, 53.7190, 551.4700, -3.1205, 0.0279, -0.8601], + [1438.2000, 39.9510, 553.1300, -3.1205, 0.0279, -0.8601], + [1452.6000, 26.1840, 554.7900, -3.1205, 0.0279, -0.8601], + [1467.0000, 12.4160, 556.4500, -3.1205, 0.0279, -0.8601], + [1481.4000, -1.3082, 558.0500, -3.1212, 0.0284, -0.8834], + [1494.7000, -14.0430, 559.4800, -3.1225, 0.0293, -0.9300], + [1506.6000, -25.4010, 560.7400, -3.1239, 0.0302, -0.9765], + [1517.0000, -35.3830, 561.8400, -3.1253, 0.0309, -1.0231], + [1526.0000, -43.9870, 562.7700, -3.1268, 0.0317, -1.0696], + [1533.6000, -51.2150, 563.5400, -3.1282, 0.0323, -1.1162], + [1539.7000, -57.0660, 564.1400, -3.1298, 0.0329, -1.1628], + [1544.4000, -61.5410, 564.5800, -3.1313, 0.0334, -1.2093], + [1547.7000, -64.6380, 564.8400, -3.1329, 0.0339, -1.2559], + [1549.5000, -66.3590, 564.9500, -3.1345, 0.0342, -1.3025], + [1549.9000, -66.7470, 564.9500, -3.1361, 0.0345, -1.3484], + [1549.9000, -66.7470, 564.9500, -3.1375, 0.0347, -1.3904], + [1549.9000, -66.7470, 564.9500, -3.1388, 0.0349, -1.4276], + [1549.9000, -66.7470, 564.9500, -3.1399, 0.0349, -1.4602], + [1549.9000, -66.7470, 564.9500, -3.1409, 0.0350, -1.4882], + [1549.9000, -66.7470, 564.9500, 3.1415, 0.0350, -1.5114], + [1549.9000, -66.7470, 564.9500, 3.1408, 0.0350, -1.5301], + [1549.9000, -66.7470, 564.9500, 3.1403, 0.0350, -1.5440], + [1549.9000, -66.7470, 564.9500, 3.1400, 0.0349, -1.5534], + [1549.9000, -66.7470, 564.9500, 3.1398, 0.0349, -1.5580], + [1549.9000, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584] +] + +Pose_2_1 = [ + [1549.9000, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 554.1737, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 543.3974, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 532.6211, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 521.8447, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 511.0684, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 500.2921, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 489.5158, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 478.7395, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 467.9632, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 457.1868, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 446.4105, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 435.6342, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 424.8579, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 414.0816, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 403.3053, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 392.5289, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 381.7526, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 370.9763, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 360.2000, 3.1398, 0.0349, -1.5584] +] + +Pose_3 = [ + [1549.9000, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 564.9500, 3.1398, 0.0349, -1.5580], + [1549.9000, -66.7470, 564.9500, 3.1400, 0.0349, -1.5534], + [1549.9000, -66.7470, 564.9500, 3.1403, 0.0350, -1.5440], + [1549.9000, -66.7470, 564.9500, 3.1408, 0.0350, -1.5301], + [1549.9000, -66.7470, 564.9500, 3.1415, 0.0350, -1.5114], + [1549.9000, -66.7470, 564.9500, -3.1409, 0.0350, -1.4882], + [1549.9000, -66.7470, 564.9500, -3.1399, 0.0349, -1.4602], + [1549.9000, -66.7470, 564.9500, -3.1388, 0.0349, -1.4276], + [1549.9000, -66.7470, 564.9500, -3.1375, 0.0347, -1.3904], + [1549.9000, -66.7470, 564.9500, -3.1361, 0.0345, -1.3484], + [1549.5000, -66.3590, 564.9500, -3.1345, 0.0342, -1.3025], + [1547.7000, -64.6380, 564.8400, -3.1329, 0.0339, -1.2559], + [1544.4000, -61.5410, 564.5800, -3.1313, 0.0334, -1.2093], + [1539.7000, -57.0660, 564.1400, -3.1298, 0.0329, -1.1628], + [1533.6000, -51.2150, 563.5400, -3.1282, 0.0323, -1.1162], + [1526.0000, -43.9870, 562.7700, -3.1268, 0.0317, -1.0696], + [1517.0000, -35.3830, 561.8400, -3.1253, 0.0309, -1.0231], + [1506.6000, -25.4010, 560.7400, -3.1239, 0.0302, -0.9765], + [1494.7000, -14.0430, 559.4800, -3.1225, 0.0293, -0.9300], + [1481.4000, -1.3082, 558.0500, -3.1212, 0.0284, -0.8834], + [1467.0000, 12.4160, 556.4500, -3.1205, 0.0279, -0.8601], + [1452.6000, 26.1840, 554.7900, -3.1205, 0.0279, -0.8601], + [1438.2000, 39.9510, 553.1300, -3.1205, 0.0279, -0.8601], + [1423.8000, 53.7190, 551.4700, -3.1205, 0.0279, -0.8601], + [1409.3000, 67.4860, 549.8100, -3.1205, 0.0279, -0.8601], + [1394.9000, 81.2540, 548.1500, -3.1205, 0.0279, -0.8601], + [1380.5000, 95.0210, 546.4900, -3.1205, 0.0279, -0.8601], + [1366.1000, 108.7900, 544.8400, -3.1205, 0.0279, -0.8601], + [1351.7000, 122.5600, 543.1700, -3.1205, 0.0279, -0.8601], + [1337.3000, 136.3200, 541.5100, -3.1205, 0.0279, -0.8601], + [1322.9000, 150.0900, 539.8600, -3.1205, 0.0279, -0.8601], + [1308.5000, 163.8600, 538.2000, -3.1205, 0.0279, -0.8601], + [1294.1000, 177.6300, 536.5400, -3.1205, 0.0279, -0.8601], + [1279.7000, 191.3900, 534.8800, -3.1205, 0.0279, -0.8601], + [1265.2000, 205.1600, 533.2200, -3.1205, 0.0279, -0.8601], + [1250.8000, 218.9300, 531.5600, -3.1205, 0.0279, -0.8601], + [1236.4000, 232.7000, 529.9000, -3.1205, 0.0279, -0.8601], + [1222.0000, 246.4600, 528.2400, -3.1205, 0.0279, -0.8601], + [1204.0000, 263.6700, 526.5800, -3.1205, 0.0279, -0.8601], + [1189.6000, 277.4400, 524.5000, -3.1205, 0.0279, -0.8601], + [1175.2000, 291.2100, 522.8400, -3.1205, 0.0279, -0.8601], + [1160.8000, 304.9800, 521.1800, -3.1205, 0.0279, -0.8601], + [1146.3000, 318.7400, 519.5200, -3.1205, 0.0279, -0.8601], + [1131.9000, 332.5100, 517.8700, -3.1205, 0.0279, -0.8601], + [1117.5000, 346.2800, 516.2100, -3.1205, 0.0279, -0.8601], + [1103.1000, 360.0500, 514.5500, -3.1205, 0.0279, -0.8601], + [1088.7000, 373.8100, 512.8900, -3.1205, 0.0279, -0.8601], + [1074.3000, 387.5800, 511.2300, -3.1205, 0.0279, -0.8601], + [1059.9000, 401.3500, 509.5700, -3.1205, 0.0279, -0.8601], + [1045.5000, 415.1100, 507.9100, -3.1205, 0.0279, -0.8601], + [1031.0000, 428.8800, 506.2500, -3.1205, 0.0279, -0.8601], + [1016.6000, 442.6500, 504.5900, -3.1205, 0.0279, -0.8601], + [1002.2000, 456.4200, 502.9300, -3.1205, 0.0279, -0.8601], + [987.8000, 470.1800, 501.2700, -3.1205, 0.0279, -0.8601], + [973.3900, 483.9500, 499.6100, -3.1205, 0.0279, -0.8601], + [958.9800, 497.7200, 497.9500, -3.1205, 0.0279, -0.8601], + [944.5700, 511.4900, 496.2900, -3.1205, 0.0279, -0.8601], + [930.1600, 525.2500, 494.6300, -3.1205, 0.0279, -0.8601], + [915.7500, 539.0200, 492.9700, -3.1205, 0.0279, -0.8601], + [901.3300, 552.7900, 491.3100, -3.1205, 0.0279, -0.8601], + [886.9200, 566.5600, 489.6500, -3.1205, 0.0279, -0.8601], + [872.5100, 580.3200, 487.9900, -3.1205, 0.0279, -0.8601], + [858.1000, 594.0900, 486.3300, -3.1205, 0.0279, -0.8601], + [843.6900, 607.8600, 484.6700, -3.1205, 0.0279, -0.8601], + [829.2700, 621.6300, 483.0100, -3.1205, 0.0279, -0.8601], + [814.8600, 635.3900, 481.3500, -3.1205, 0.0279, -0.8601], + [800.4500, 649.1600, 479.6900, -3.1205, 0.0279, -0.8601], + [786.0400, 662.9300, 478.0300, -3.1205, 0.0279, -0.8601], + [771.6300, 676.7000, 476.3700, -3.1205, 0.0279, -0.8601], + [757.2200, 690.4600, 474.7200, -3.1205, 0.0279, -0.8601], + [742.8000, 704.2300, 473.0600, -3.1205, 0.0279, -0.8601], + [728.3900, 718.0000, 471.3900, -3.1205, 0.0279, -0.8601], + [713.9800, 731.7700, 469.7400, -3.1205, 0.0279, -0.8601], + [699.5700, 745.5300, 468.0800, -3.1205, 0.0279, -0.8601], + [685.1500, 759.3000, 466.4200, -3.1205, 0.0279, -0.8601], + [670.7400, 773.0700, 464.7600, -3.1205, 0.0279, -0.8601], + [656.3300, 786.8400, 463.1000, -3.1205, 0.0279, -0.8601], + [641.9200, 800.6000, 461.4400, -3.1205, 0.0279, -0.8601], + [627.5100, 814.3700, 459.7800, -3.1205, 0.0279, -0.8601], + [613.1000, 828.1400, 458.1200, -3.1205, 0.0279, -0.8601], + [598.6800, 841.9100, 456.4600, -3.1205, 0.0279, -0.8601], + [584.2700, 855.6700, 454.8000, -3.1205, 0.0279, -0.8601], + [569.8600, 869.4400, 453.1400, -3.1205, 0.0279, -0.8601], + [555.4500, 883.2100, 451.4800, -3.1205, 0.0279, -0.8601], + [541.0400, 896.9800, 449.8200, -3.1205, 0.0279, -0.8601], + [526.6200, 910.7400, 448.1600, -3.1205, 0.0279, -0.8601], + [512.2100, 924.5100, 446.5000, -3.1205, 0.0279, -0.8601], + [497.8000, 938.2800, 444.8400, -3.1205, 0.0279, -0.8601], + [483.3900, 952.0400, 443.1800, -3.1205, 0.0279, -0.8601], + [468.9800, 965.8100, 441.5200, -3.1205, 0.0279, -0.8601], + [454.5600, 979.5800, 439.8600, -3.1205, 0.0279, -0.8601], + [440.1500, 993.3500, 438.2000, -3.1205, 0.0279, -0.8601], + [425.7400, 1007.1000, 436.5400, -3.1205, 0.0279, -0.8601], + [411.3300, 1020.9000, 434.8800, -3.1205, 0.0279, -0.8601], + [396.9200, 1034.7000, 433.2200, -3.1205, 0.0279, -0.8601], + [382.5000, 1048.4000, 431.5600, -3.1205, 0.0279, -0.8601], + [368.0900, 1062.2000, 429.9000, -3.1205, 0.0279, -0.8601], + [353.6800, 1076.0000, 428.2500, -3.1205, 0.0279, -0.8601], + [339.2700, 1089.7000, 426.5800, -3.1205, 0.0279, -0.8601], + [324.8600, 1103.5000, 424.9300, -3.1205, 0.0279, -0.8601], + [310.4500, 1117.2000, 423.2700, -3.1205, 0.0279, -0.8601], + [296.0300, 1131.0000, 421.6100, -3.1205, 0.0279, -0.8601], + [281.6200, 1144.8000, 419.9500, -3.1205, 0.0279, -0.8601], + [267.2100, 1158.6000, 418.2900, -3.1205, 0.0279, -0.8601], + [252.8000, 1172.3000, 416.6300, -3.1205, 0.0279, -0.8601], + [238.3900, 1186.1000, 414.9700, -3.1205, 0.0279, -0.8601], + [223.9700, 1199.9000, 413.3100, -3.1205, 0.0279, -0.8601], + [209.5600, 1213.6000, 411.6500, -3.1205, 0.0279, -0.8601], + [195.1500, 1227.4000, 409.9900, -3.1205, 0.0279, -0.8601], + [180.7400, 1241.2000, 408.3300, -3.1205, 0.0279, -0.8601], + [166.3300, 1254.9000, 406.6700, -3.1205, 0.0279, -0.8601], + [151.9100, 1268.7000, 405.0100, -3.1205, 0.0279, -0.8601], + [137.5000, 1282.5000, 403.3500, -3.1205, 0.0279, -0.8601], + [123.0900, 1296.2000, 401.6900, -3.1205, 0.0279, -0.8601], + [105.0800, 1313.4000, 400.0300, -3.1205, 0.0279, -0.8601], + [90.6650, 1327.2000, 397.9600, -3.1205, 0.0279, -0.8601], + [76.2530, 1341.0000, 396.3000, -3.1205, 0.0279, -0.8601], + [61.8410, 1354.7000, 394.6400, -3.1205, 0.0279, -0.8601], + [47.4290, 1368.5000, 392.9800, -3.1205, 0.0279, -0.8601], + [33.0170, 1382.3000, 391.3200, -3.1205, 0.0279, -0.8601], + [18.6050, 1396.0000, 389.6600, -3.1205, 0.0279, -0.8601], + [4.1933, 1409.8000, 388.0000, -3.1205, 0.0279, -0.8601], + [-10.2190, 1423.6000, 386.3400, -3.1205, 0.0279, -0.8601], + [-24.6310, 1437.3000, 384.6800, -3.1205, 0.0279, -0.8601], + [-39.0420, 1451.1000, 383.0200, -3.1205, 0.0279, -0.8601], + [-53.4540, 1464.9000, 381.3600, -3.1205, 0.0279, -0.8601], + [-67.8660, 1478.7000, 379.7000, -3.1205, 0.0279, -0.8601], + [-82.2780, 1492.4000, 378.0400, -3.1205, 0.0279, -0.8601], + [-96.6900, 1506.2000, 376.3800, -3.1205, 0.0279, -0.8601], + [-111.1000, 1520.0000, 374.7200, -3.1205, 0.0279, -0.8601], + [-125.5100, 1533.7000, 373.0600, -3.1205, 0.0279, -0.8601], + [-139.9300, 1547.5000, 371.4000, -3.1205, 0.0279, -0.8601], + [-154.3400, 1561.3000, 369.7400, -3.1205, 0.0279, -0.8601], + [-168.1900, 1574.5000, 368.0900, -3.1200, 0.0275, -0.8428], + [-180.6100, 1586.3000, 366.5500, -3.1182, 0.0260, -0.7756], + [-191.5900, 1596.8000, 365.1800, -3.1166, 0.0245, -0.7119], + [-201.1300, 1606.0000, 363.9800, -3.1152, 0.0229, -0.6517], + [-209.2300, 1613.7000, 362.9400, -3.1139, 0.0214, -0.5949], + [-215.8800, 1620.0000, 362.0700, -3.1128, 0.0199, -0.5417], + [-221.1000, 1625.0000, 361.3700, -3.1119, 0.0184, -0.4920], + [-224.8700, 1628.6000, 360.8300, -3.1111, 0.0170, -0.4458], + [-227.2000, 1630.9000, 360.4500, -3.1103, 0.0157, -0.4030], + [-228.0900, 1631.7000, 360.2500, -3.1098, 0.0145, -0.3638], + [-228.1000, 1631.7000, 360.2000, -3.1093, 0.0133, -0.3280], + [-228.1000, 1631.7000, 360.2000, -3.1088, 0.0123, -0.2957], + [-228.1000, 1631.7000, 360.2000, -3.1085, 0.0113, -0.2669], + [-228.1000, 1631.7000, 360.2000, -3.1082, 0.0105, -0.2416], + [-228.1000, 1631.7000, 360.2000, -3.1080, 0.0098, -0.2198], + [-228.1000, 1631.7000, 360.2000, -3.1078, 0.0091, -0.2015], + [-228.1000, 1631.7000, 360.2000, -3.1077, 0.0086, -0.1867], + [-228.1000, 1631.7000, 360.2000, -3.1076, 0.0083, -0.1754], + [-228.1000, 1631.7000, 360.2000, -3.1075, 0.0080, -0.1675], + [-228.1000, 1631.7000, 360.2000, -3.1075, 0.0078, -0.1631], + [-228.1000, 1631.7000, 360.2000, -3.1075, 0.0078, -0.1622] +] + + +Pose_3_4 = [ + [-228.1000, 1631.7000, 360.2000, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 370.9763, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 381.7526, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 392.5289, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 403.3053, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 414.0816, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 424.8579, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 435.6342, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 446.4105, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 457.1868, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 467.9632, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 478.7395, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 489.5158, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 500.2921, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 511.0684, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 521.8447, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 532.6211, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 543.3974, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 554.1737, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 564.9500, -3.1075, 0.0078, -0.1622] +] + +Pose_4 = [ + [-228.1000, 1631.7000, 564.9500, -3.1075, 0.0078, -0.1622], + [-228.1000, 1631.7000, 564.9500, -3.1075, 0.0078, -0.1631], + [-228.1000, 1631.7000, 564.9500, -3.1075, 0.0080, -0.1675], + [-228.1000, 1631.7000, 564.9500, -3.1076, 0.0083, -0.1754], + [-228.1000, 1631.7000, 564.9500, -3.1077, 0.0086, -0.1867], + [-228.1000, 1631.7000, 564.9500, -3.1078, 0.0091, -0.2015], + [-228.1000, 1631.7000, 564.9500, -3.1080, 0.0098, -0.2198], + [-228.1000, 1631.7000, 564.9500, -3.1082, 0.0105, -0.2416], + [-228.1000, 1631.7000, 564.9500, -3.1085, 0.0113, -0.2669], + [-228.1000, 1631.7000, 564.9500, -3.1088, 0.0123, -0.2957], + [-228.1000, 1631.7000, 564.9500, -3.1093, 0.0133, -0.3280], + [-228.0900, 1631.7000, 564.9500, -3.1098, 0.0145, -0.3638], + [-227.2000, 1630.9000, 564.8400, -3.1103, 0.0157, -0.4030], + [-224.8700, 1628.6000, 564.5800, -3.1111, 0.0170, -0.4458], + [-221.1000, 1625.0000, 564.1400, -3.1119, 0.0184, -0.4920], + [-215.8800, 1620.0000, 563.5400, -3.1128, 0.0199, -0.5417], + [-209.2300, 1613.7000, 562.7700, -3.1139, 0.0214, -0.5949], + [-201.1300, 1606.0000, 561.8400, -3.1152, 0.0229, -0.6517], + [-191.5900, 1596.8000, 560.7400, -3.1166, 0.0245, -0.7119], + [-180.6100, 1586.3000, 559.4800, -3.1182, 0.0260, -0.7756], + [-168.1900, 1574.5000, 558.0500, -3.1200, 0.0275, -0.8428], + [-154.3400, 1561.3000, 556.4500, -3.1205, 0.0279, -0.8601], + [-139.9300, 1547.5000, 554.7900, -3.1205, 0.0279, -0.8601], + [-125.5100, 1533.7000, 553.1300, -3.1205, 0.0279, -0.8601], + [-111.1000, 1520.0000, 551.4700, -3.1205, 0.0279, -0.8601], + [-96.6900, 1506.2000, 549.8100, -3.1205, 0.0279, -0.8601], + [-82.2780, 1492.4000, 548.1500, -3.1205, 0.0279, -0.8601], + [-67.8660, 1478.7000, 546.4900, -3.1205, 0.0279, -0.8601], + [-53.4540, 1464.9000, 544.8400, -3.1205, 0.0279, -0.8601], + [-39.0420, 1451.1000, 543.1700, -3.1205, 0.0279, -0.8601], + [-24.6310, 1437.3000, 541.5100, -3.1205, 0.0279, -0.8601], + [-10.2190, 1423.6000, 539.8600, -3.1205, 0.0279, -0.8601], + [4.1933, 1409.8000, 538.2000, -3.1205, 0.0279, -0.8601], + [18.6050, 1396.0000, 536.5400, -3.1205, 0.0279, -0.8601], + [33.0170, 1382.3000, 534.8800, -3.1205, 0.0279, -0.8601], + [47.4290, 1368.5000, 533.2200, -3.1205, 0.0279, -0.8601], + [61.8410, 1354.7000, 531.5600, -3.1205, 0.0279, -0.8601], + [76.2530, 1341.0000, 529.9000, -3.1205, 0.0279, -0.8601], + [90.6650, 1327.2000, 528.2400, -3.1205, 0.0279, -0.8601], + [105.0800, 1313.4000, 526.5800, -3.1205, 0.0279, -0.8601], + [123.0900, 1296.2000, 524.5000, -3.1205, 0.0279, -0.8601], + [137.5000, 1282.5000, 522.8400, -3.1205, 0.0279, -0.8601], + [151.9100, 1268.7000, 521.1800, -3.1205, 0.0279, -0.8601], + [166.3300, 1254.9000, 519.5200, -3.1205, 0.0279, -0.8601], + [180.7400, 1241.2000, 517.8700, -3.1205, 0.0279, -0.8601], + [195.1500, 1227.4000, 516.2100, -3.1205, 0.0279, -0.8601], + [209.5600, 1213.6000, 514.5500, -3.1205, 0.0279, -0.8601], + [223.9700, 1199.9000, 512.8900, -3.1205, 0.0279, -0.8601], + [238.3900, 1186.1000, 511.2300, -3.1205, 0.0279, -0.8601], + [252.8000, 1172.3000, 509.5700, -3.1205, 0.0279, -0.8601], + [267.2100, 1158.6000, 507.9100, -3.1205, 0.0279, -0.8601], + [281.6200, 1144.8000, 506.2500, -3.1205, 0.0279, -0.8601], + [296.0300, 1131.0000, 504.5900, -3.1205, 0.0279, -0.8601], + [310.4500, 1117.2000, 502.9300, -3.1205, 0.0279, -0.8601], + [324.8600, 1103.5000, 501.2700, -3.1205, 0.0279, -0.8601], + [339.2700, 1089.7000, 499.6100, -3.1205, 0.0279, -0.8601], + [353.6800, 1076.0000, 497.9500, -3.1205, 0.0279, -0.8601], + [368.0900, 1062.2000, 496.2900, -3.1205, 0.0279, -0.8601], + [382.5000, 1048.4000, 494.6300, -3.1205, 0.0279, -0.8601], + [396.9200, 1034.7000, 492.9700, -3.1205, 0.0279, -0.8601], + [411.3300, 1020.9000, 491.3100, -3.1205, 0.0279, -0.8601], + [425.7400, 1007.1000, 489.6500, -3.1205, 0.0279, -0.8601], + [440.1500, 993.3500, 487.9900, -3.1205, 0.0279, -0.8601], + [454.5600, 979.5800, 486.3300, -3.1205, 0.0279, -0.8601], + [468.9800, 965.8100, 484.6700, -3.1205, 0.0279, -0.8601], + [483.3900, 952.0400, 483.0100, -3.1205, 0.0279, -0.8601], + [497.8000, 938.2800, 481.3500, -3.1205, 0.0279, -0.8601], + [512.2100, 924.5100, 479.6900, -3.1205, 0.0279, -0.8601], + [526.6200, 910.7400, 478.0300, -3.1205, 0.0279, -0.8601], + [541.0400, 896.9800, 476.3700, -3.1205, 0.0279, -0.8601], + [555.4500, 883.2100, 474.7200, -3.1205, 0.0279, -0.8601], + [569.8600, 869.4400, 473.0600, -3.1205, 0.0279, -0.8601], + [584.2700, 855.6700, 471.3900, -3.1205, 0.0279, -0.8601], + [598.6800, 841.9100, 469.7400, -3.1205, 0.0279, -0.8601], + [613.1000, 828.1400, 468.0800, -3.1205, 0.0279, -0.8601], + [627.5100, 814.3700, 466.4200, -3.1205, 0.0279, -0.8601], + [641.9200, 800.6000, 464.7600, -3.1205, 0.0279, -0.8601], + [656.3300, 786.8400, 463.1000, -3.1205, 0.0279, -0.8601], + [670.7400, 773.0700, 461.4400, -3.1205, 0.0279, -0.8601], + [685.1500, 759.3000, 459.7800, -3.1205, 0.0279, -0.8601], + [699.5700, 745.5300, 458.1200, -3.1205, 0.0279, -0.8601], + [713.9800, 731.7700, 456.4600, -3.1205, 0.0279, -0.8601], + [728.3900, 718.0000, 454.8000, -3.1205, 0.0279, -0.8601], + [742.8000, 704.2300, 453.1400, -3.1205, 0.0279, -0.8601], + [757.2200, 690.4600, 451.4800, -3.1205, 0.0279, -0.8601], + [771.6300, 676.7000, 449.8200, -3.1205, 0.0279, -0.8601], + [786.0400, 662.9300, 448.1600, -3.1205, 0.0279, -0.8601], + [800.4500, 649.1600, 446.5000, -3.1205, 0.0279, -0.8601], + [814.8600, 635.3900, 444.8400, -3.1205, 0.0279, -0.8601], + [829.2700, 621.6300, 443.1800, -3.1205, 0.0279, -0.8601], + [843.6900, 607.8600, 441.5200, -3.1205, 0.0279, -0.8601], + [858.1000, 594.0900, 439.8600, -3.1205, 0.0279, -0.8601], + [872.5100, 580.3200, 438.2000, -3.1205, 0.0279, -0.8601], + [886.9200, 566.5600, 436.5400, -3.1205, 0.0279, -0.8601], + [901.3300, 552.7900, 434.8800, -3.1205, 0.0279, -0.8601], + [915.7500, 539.0200, 433.2200, -3.1205, 0.0279, -0.8601], + [930.1600, 525.2500, 431.5600, -3.1205, 0.0279, -0.8601], + [944.5700, 511.4900, 429.9000, -3.1205, 0.0279, -0.8601], + [958.9800, 497.7200, 428.2500, -3.1205, 0.0279, -0.8601], + [973.3900, 483.9500, 426.5800, -3.1205, 0.0279, -0.8601], + [987.8000, 470.1800, 424.9300, -3.1205, 0.0279, -0.8601], + [1002.2000, 456.4200, 423.2700, -3.1205, 0.0279, -0.8601], + [1016.6000, 442.6500, 421.6100, -3.1205, 0.0279, -0.8601], + [1031.0000, 428.8800, 419.9500, -3.1205, 0.0279, -0.8601], + [1045.5000, 415.1100, 418.2900, -3.1205, 0.0279, -0.8601], + [1059.9000, 401.3500, 416.6300, -3.1205, 0.0279, -0.8601], + [1074.3000, 387.5800, 414.9700, -3.1205, 0.0279, -0.8601], + [1088.7000, 373.8100, 413.3100, -3.1205, 0.0279, -0.8601], + [1103.1000, 360.0500, 411.6500, -3.1205, 0.0279, -0.8601], + [1117.5000, 346.2800, 409.9900, -3.1205, 0.0279, -0.8601], + [1131.9000, 332.5100, 408.3300, -3.1205, 0.0279, -0.8601], + [1146.3000, 318.7400, 406.6700, -3.1205, 0.0279, -0.8601], + [1160.8000, 304.9800, 405.0100, -3.1205, 0.0279, -0.8601], + [1175.2000, 291.2100, 403.3500, -3.1205, 0.0279, -0.8601], + [1189.6000, 277.4400, 401.6900, -3.1205, 0.0279, -0.8601], + [1204.0000, 263.6700, 400.0300, -3.1205, 0.0279, -0.8601], + [1222.0000, 246.4600, 397.9600, -3.1205, 0.0279, -0.8601], + [1236.4000, 232.7000, 396.3000, -3.1205, 0.0279, -0.8601], + [1250.8000, 218.9300, 394.6400, -3.1205, 0.0279, -0.8601], + [1265.2000, 205.1600, 392.9800, -3.1205, 0.0279, -0.8601], + [1279.7000, 191.3900, 391.3200, -3.1205, 0.0279, -0.8601], + [1294.1000, 177.6300, 389.6600, -3.1205, 0.0279, -0.8601], + [1308.5000, 163.8600, 388.0000, -3.1205, 0.0279, -0.8601], + [1322.9000, 150.0900, 386.3400, -3.1205, 0.0279, -0.8601], + [1337.3000, 136.3200, 384.6800, -3.1205, 0.0279, -0.8601], + [1351.7000, 122.5600, 383.0200, -3.1205, 0.0279, -0.8601], + [1366.1000, 108.7900, 381.3600, -3.1205, 0.0279, -0.8601], + [1380.5000, 95.0210, 379.7000, -3.1205, 0.0279, -0.8601], + [1394.9000, 81.2540, 378.0400, -3.1205, 0.0279, -0.8601], + [1409.3000, 67.4860, 376.3800, -3.1205, 0.0279, -0.8601], + [1423.8000, 53.7190, 374.7200, -3.1205, 0.0279, -0.8601], + [1438.2000, 39.9510, 373.0600, -3.1205, 0.0279, -0.8601], + [1452.6000, 26.1840, 371.4000, -3.1205, 0.0279, -0.8601], + [1467.0000, 12.4160, 369.7400, -3.1205, 0.0279, -0.8601], + [1481.4000, -1.3082, 368.0900, -3.1212, 0.0284, -0.8834], + [1494.7000, -14.0430, 366.5500, -3.1225, 0.0293, -0.9300], + [1506.6000, -25.4010, 365.1800, -3.1239, 0.0302, -0.9765], + [1517.0000, -35.3830, 363.9800, -3.1253, 0.0309, -1.0231], + [1526.0000, -43.9870, 362.9400, -3.1268, 0.0317, -1.0696], + [1533.6000, -51.2150, 362.0700, -3.1282, 0.0323, -1.1162], + [1539.7000, -57.0660, 361.3700, -3.1298, 0.0329, -1.1628], + [1544.4000, -61.5410, 360.8300, -3.1313, 0.0334, -1.2093], + [1547.7000, -64.6380, 360.4500, -3.1329, 0.0339, -1.2559], + [1549.5000, -66.3590, 360.2500, -3.1345, 0.0342, -1.3025], + [1549.9000, -66.7470, 360.2000, -3.1361, 0.0345, -1.3484], + [1549.9000, -66.7470, 360.2000, -3.1375, 0.0347, -1.3904], + [1549.9000, -66.7470, 360.2000, -3.1388, 0.0349, -1.4276], + [1549.9000, -66.7470, 360.2000, -3.1399, 0.0349, -1.4602], + [1549.9000, -66.7470, 360.2000, -3.1409, 0.0350, -1.4882], + [1549.9000, -66.7470, 360.2000, 3.1415, 0.0350, -1.5114], + [1549.9000, -66.7470, 360.2000, 3.1408, 0.0350, -1.5301], + [1549.9000, -66.7470, 360.2000, 3.1403, 0.0350, -1.5440], + [1549.9000, -66.7470, 360.2000, 3.1400, 0.0349, -1.5534], + [1549.9000, -66.7470, 360.2000, 3.1398, 0.0349, -1.5580], + [1549.9000, -66.7470, 360.2000, 3.1398, 0.0349, -1.5584] +] + +Pose_4_3 = [ + [1549.9000, -66.7470, 360.2000, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 370.9763, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 381.7526, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 392.5289, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 403.3053, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 414.0816, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 424.8579, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 435.6342, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 446.4105, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 457.1868, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 467.9632, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 478.7395, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 489.5158, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 500.2921, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 511.0684, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 521.8447, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 532.6211, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 543.3974, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 554.1737, 3.1398, 0.0349, -1.5584], + [1549.9000, -66.7470, 564.9500, 3.1398, 0.0349, -1.5584] +] + + + diff --git a/Kelaifen_V6.0/trajectory_program.py b/Kelaifen_V6.0/trajectory_program.py new file mode 100644 index 0000000..9787044 --- /dev/null +++ b/Kelaifen_V6.0/trajectory_program.py @@ -0,0 +1,1362 @@ +import sys +import copy +from queue import Queue +from ETController import send_Point, sendCMD, wait_stop +from gpio import turn_off_relay, turn_on_relay +from serial_handler import read_cmd_from_shared +# from program_config import ( +# PRESET_PARAMS, turn_off_relay_start, +# turn_on_relay_start +# ) +from serial_init import SerialSharedData +from trajectory_data import ( + Pose_1, Pose_2, Pose_3, Pose_4, Pose_3_4, Pose_4_3, + joint_positions_assembled_1, joint_positions_assembled_2, + joint_positions_assembled_3, joint_positions_assembled_4, + Pose_1_2, Pose_2_1, + ) +import time +import copy +from collections import deque +import queue +# 导入串口处理模块 +import serial_handler + +# 采样时间、延时时间和衔接机器人速度 +ENABLE_INCREMENTS = 1 # 表示是否支持偏移 +SAMP_TIME = 20 +SLP_TIME = 0.02 +# 补偿值更新队列（解耦线程通信，避免锁竞争） +compensate_queue = Queue(maxsize=1) # 最多缓存1个最新值，确保数据时效性 +# 给串口模块赋值队列（新增，关键！） +serial_handler.compensate_queue = compensate_queue +# 全局坐标补偿变量 +increments_x = 0 +increments_y = 0 +increments_z = 0 +Coordinate_compensation = [0, 0, 0, 0, 0, 0] + +FILTER_N = 10 + +buf_x = deque([0.0] * FILTER_N, maxlen=FILTER_N) +buf_y = deque([0.0] * FILTER_N, maxlen=FILTER_N) +buf_z = deque([0.0] * FILTER_N, maxlen=FILTER_N) + +X_M = 0.0 +Y_M = 0.0 +Z_M = 0.0 + +# 预期的帧长度，可根据实际情况修改 +EXPECTED_LENGTH = 7 +global sample_time +global sleep_time +move_speed = 100 +# 前瞻时间 +lookahead_time = 400 +# 调整之后的位姿 +Pose_after_adjustment = [0, 0, 0, 0, 0, 0] +# 运动速度调整、z方向初始位置调整 +# speed_adjustment = serial_handler.speed_adjustment +# Trajectory_angle = serial_handler.Trajectory_angle +velocity_coefficient = 100 +z_positionadjustment = -200 # 轨迹整体调整 + +# 初始化全局共享数据实例 +serial_shared = SerialSharedData() +# 轨迹插值点数 +Interpolation_points = 20 + +def reset_filter(): + + global buf_x + global buf_y + global buf_z + + global X_M + global Y_M + global Z_M + + buf_x = deque([0.0] * FILTER_N, maxlen=FILTER_N) + buf_y = deque([0.0] * FILTER_N, maxlen=FILTER_N) + buf_z = deque([0.0] * FILTER_N, maxlen=FILTER_N) + + X_M = 0.0 + Y_M = 0.0 + Z_M = 0.0 + while True: + try: + compensate_queue.get_nowait() + except: + break + + print("Filter Reset") + + +def update_filter(x, y, z): + + buf_x.append(x) + buf_y.append(y) + buf_z.append(z) + + x_m = sum(buf_x) / len(buf_x) + y_m = sum(buf_y) / len(buf_y) + z_m = sum(buf_z) / len(buf_z) + + MAX_COMP = 200 + + x_m = max(min(x_m, MAX_COMP), -MAX_COMP) + y_m = max(min(y_m, MAX_COMP), -MAX_COMP) + z_m = max(min(z_m, MAX_COMP), -MAX_COMP) + + + return x_m, y_m, z_m + +def get_compensated_point(point): + + global X_M + global Y_M + global Z_M + + try: + + while True: + + increments_x, increments_y, increments_z = \ + compensate_queue.get_nowait() + + increments_x = max(min(increments_x, 200), -200) + increments_y = max(min(increments_y, 200), -200) + increments_z = max(min(increments_z, 200), -200) + + X_M, Y_M, Z_M = update_filter( + increments_x, + increments_y, + increments_z + ) + + except queue.Empty: + + pass + + point_M = point.copy() + + if 1 == ENABLE_INCREMENTS: + point_M[0] += X_M + point_M[1] += Y_M + point_M[2] += Z_M + + return point_M + +# 第一段程序封装为函数 +# 第二段程序封装为函数(上升喷涂) +# 第三段程序封装为函数 +# 第四段程序封装为函数（上升喷涂） +# 第五段程序封装为函数 +# 第六段程序封装为函数（下降喷涂） +# 第七段程序封装为函数 +# 第八段程序封装为函数（下降喷涂） +# 第九段程序封装为函数 +# 第十二段程序封装为函数 + +def program1(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start): + i = 0 + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + turn_off_relay() + global increments_x, increments_y, increments_z + global Coordinate_compensation + global sample_time + global sleep_time + + sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) + sleep_time = sample_time*0.001 + + # 从队列获取最新补偿值（非阻塞，无新值则使用当前值） + if not compensate_queue.empty(): + increments_x, increments_y, increments_z = compensate_queue.get_nowait() + increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0] + Coordinate_compensation = increments + reference_pos = None + new_joint_pos = None + + if len(Pose_1) > 0 and len(Pose_1[0]) == 6: + Path_First_Position = Pose_1[0] # 获取第一行数据 + Modified_First_Position = [val + inc for val, inc in zip(Path_First_Position, Coordinate_compensation)] + target_pose = Modified_First_Position + else: + return + + for line_list in joint_positions_assembled_1: + # 确保每行是6个数值（与原文件的逗号分割后逻辑一致） + if len(line_list) != 6: + continue + if reference_pos is None: + reference_pos = line_list + suc, angle, id = sendCMD(sock, "inverseKinematic", {"targetPose": target_pose, "referencePos": reference_pos}) + if suc and angle is not None: + new_joint_pos = angle + else: + print("逆解计算失败，使用原有关节位置") + new_joint_pos = line_list + modified_list = new_joint_pos if new_joint_pos is not None else line_list + + if (i == 0): + while True: + suc, result, id = sendCMD(sock, "moveByJoint", {"targetPos": modified_list, "speed": 100}) + if suc: + break + print("moveByJoint命令发送失败,重试...") + time.sleep(0.1) + wait_stop(sock) + time.sleep(sleep_time) + i = i + 1 + + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + print("第一段程序执行完") + +def program1_1(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start): + print("第1_1段程序开始执行") + i = 0 + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + turn_off_relay() + global increments_x, increments_y, increments_z + global Coordinate_compensation + global sample_time + global sleep_time + global lookahead_time + + sample_time = SAMP_TIME + sleep_time = sample_time*0.001 + lookahead_time = sample_time * Interpolation_points + + # 从队列获取最新补偿值（非阻塞，无新值则使用当前值） + if not compensate_queue.empty(): + increments_x, increments_y, increments_z = compensate_queue.get_nowait() + increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0] + Coordinate_compensation = increments + + # 获取插值起点和插值终点 + start_pose = Pose_after_adjustment + end_pose = Pose_1[0].copy() + # 计算带补偿的终点位姿（最终插值要到达的目标） + end_pose_with_comp = [end_pose[i] + Coordinate_compensation[i] for i in range(6)] + + interpolate_points = Interpolation_points # 插值点数（包含起止点） + interpolated_poses = [] + + for idx in range(interpolate_points): + # 计算插值权重（0→1，对应从起点第三列到终点第三列） + t = idx / (interpolate_points - 1) if interpolate_points > 1 else 1.0 + + # 组装当前插值位姿：仅更新第三列，其余列沿用起点值 + current_pose = start_pose.copy() + # 第三列：从起点值 线性插值到 终点补偿值的第三列 + current_pose[2] = start_pose[2] * (1 - t) + end_pose_with_comp[2] * t + + # 前两列：同步从起点值 线性调整到 终点补偿值的前两列（与第三列同权重） + current_pose[0] = start_pose[0] * (1 - t) + end_pose_with_comp[0] * t + current_pose[1] = start_pose[1] * (1 - t) + end_pose_with_comp[1] * t + + # 添加到插值结果列表 + interpolated_poses.append(current_pose) + # print("\n=== 插值后的位姿列表（逐行）===") + # for i, pose in enumerate(interpolated_poses): + # print(f"第 {i+1} 个插值点: {pose}") + # 遍历插值结果，逐个发送位姿（保留你原有发送逻辑） + for modified_list in interpolated_poses: + # 初始化透传 + if i == 0: + suc, result, id = sendCMD(sock, "transparent_transmission_init", { + "lookahead": lookahead_time, + "t": sample_time, + "smoothness": 1, + "response_enable": 0 + }) + # 发送当前插值得到的位姿 + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) + time.sleep(sleep_time) + i = i + 1 + + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + print("第1_1段程序执行完") + +def program2(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start): + global increments_x, increments_y, increments_z + global Coordinate_compensation + global sample_time + global sleep_time + global lookahead_time + global Pose_after_adjustment + + lookahead_time = 400 + i = 0 + sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) + sleep_time = sample_time*0.001 + + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + + print("第二段程序开始执行") + + # 遍历Pose_1，同时获取行号（enumerate的index从0开始） + for line_idx, line_list in enumerate(Pose_1): + if len(line_list) != 6: + continue + actual_line_num = line_idx + 1 # 关键：将枚举的索引转为从1开始的行号 + # 核心逻辑：判断行号是否在13到143之间，修改第三列（索引为2，因为列表从0开始） + if 13 <= actual_line_num <= 143: + # 计算新的第三列值：360.2 + ((204.75 + Trajectory_angle)/143) * 行号 + new_third_value = 360.2 + ((204.75 + serial_handler.Trajectory_angle) / 131) * (actual_line_num-12) + # 复制原列表并修改第三列（避免修改原列表的引用） + line_list = line_list.copy() + line_list[2] = new_third_value # 第三列对应索引2 + if actual_line_num == 143: + row_143_third_value = new_third_value + elif actual_line_num > 143 and row_143_third_value is not None: + # 行号大于143时，用143行的第三列值替换 + line_list = line_list.copy() + line_list[2] = row_143_third_value + + increments = Coordinate_compensation + modified_list = [val + inc for val, inc in zip(line_list, increments)] + Pose_after_adjustment = modified_list + # print(Pose_after_adjustment) + if (i == 0): + suc, result, id = sendCMD(sock, "transparent_transmission_init", { + "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0}) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) + time.sleep(sleep_time) + + if(i >= turn_on_relay_start): + turn_on_relay() + print(f"打开喷枪2：{turn_on_relay_start}") + if(i >= turn_off_relay_start): + turn_off_relay() + print(f"关闭喷枪2：{turn_off_relay_start}") + i = i + 1 + + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + print("第二段程序执行完") + + +def program3(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start): + print("第三段程序开始执行") + i = 0 + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + turn_off_relay() + global increments_x, increments_y, increments_z + global Coordinate_compensation + global sample_time + global sleep_time + + sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) + sleep_time = sample_time*0.001 + + # 从队列获取最新补偿值（非阻塞，无新值则使用当前值） + if not compensate_queue.empty(): + increments_x, increments_y, increments_z = compensate_queue.get_nowait() + increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0] + Coordinate_compensation = increments + reference_pos = None + new_joint_pos = None + + if len(Pose_2) > 0 and len(Pose_2[0]) == 6: + Path_First_Position = Pose_2[0] # 获取第一行数据 + Modified_First_Position = [val + inc for val, inc in zip(Path_First_Position, Coordinate_compensation)] + target_pose = Modified_First_Position + else: + return + + for line_list in joint_positions_assembled_2: + if len(line_list) != 6: + continue + if reference_pos is None: + reference_pos = line_list + suc, angle, id = sendCMD(sock, "inverseKinematic", {"targetPose": target_pose, "referencePos": reference_pos}) + if suc and angle is not None: + new_joint_pos = angle + else: + print("逆解计算失败，使用原有关节位置") + new_joint_pos = line_list + modified_list = new_joint_pos if new_joint_pos is not None else line_list + + if (i == 0): + while True: + suc, result, id = sendCMD(sock, "moveByJoint", {"targetPos": modified_list, "speed": 100}) + if suc: + break + print("moveByJoint命令发送失败,重试...") + time.sleep(0.1) + wait_stop(sock) + time.sleep(sleep_time) + i = i + 1 + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + print("第三段程序执行完") + +def program3_1(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start): + print("第3_1段程序开始执行") + i = 0 + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + turn_off_relay() + global increments_x, increments_y, increments_z + global Coordinate_compensation + global sample_time + global sleep_time + global lookahead_time + + sample_time = SAMP_TIME + sleep_time = sample_time*0.001 + lookahead_time = sample_time * Interpolation_points + + # 从队列获取最新补偿值（非阻塞，无新值则使用当前值） + if not compensate_queue.empty(): + increments_x, increments_y, increments_z = compensate_queue.get_nowait() + increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0] + Coordinate_compensation = increments + + # 获取插值起点和插值终点 + start_pose = Pose_after_adjustment + end_pose = Pose_2[0].copy() + # 计算带补偿的终点位姿（最终插值要到达的目标） + end_pose_with_comp = [end_pose[i] + Coordinate_compensation[i] for i in range(6)] + + interpolate_points = Interpolation_points + interpolated_poses = [] + + for idx in range(interpolate_points): + # 计算插值权重（0→1，对应从起点第三列到终点第三列） + t = idx / (interpolate_points - 1) if interpolate_points > 1 else 1.0 + + # 组装当前插值位姿：仅更新第三列，其余列沿用起点值 + current_pose = start_pose.copy() + + # 第三列：从起点值 线性插值到 终点补偿值的第三列 + current_pose[2] = start_pose[2] * (1 - t) + end_pose_with_comp[2] * t + # 前两列：同步从起点值 线性调整到 终点补偿值的前两列（与第三列同权重） + current_pose[0] = start_pose[0] * (1 - t) + end_pose_with_comp[0] * t + current_pose[1] = start_pose[1] * (1 - t) + end_pose_with_comp[1] * t + + # 添加到插值结果列表 + interpolated_poses.append(current_pose) + # print("\n=== 插值后的位姿列表（逐行）===") + # for i, pose in enumerate(interpolated_poses): + # print(f"第 {i+1} 个插值点: {pose}") + + # 遍历插值结果，逐个发送位姿（保留你原有发送逻辑） + for modified_list in interpolated_poses: + # 初始化透传 + if i == 0: + suc, result, id = sendCMD(sock, "transparent_transmission_init", { + "lookahead": lookahead_time, + "t": sample_time, + "smoothness": 1, + "response_enable": 0 + }) + # 发送当前插值得到的位姿 + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) + time.sleep(sleep_time) + i = i + 1 + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + print("第3_1段程序执行完") + +def program4(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start): + print("第四段程序开始执行") + global increments_x, increments_y, increments_z + global Coordinate_compensation + global sample_time + global sleep_time + global lookahead_time + global Pose_after_adjustment + + lookahead_time = 400 + i = 0 + sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) + sleep_time = sample_time*0.001 + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + + for line_idx, line_list in enumerate(Pose_2): + if len(line_list) != 6: + continue + + actual_line_num = line_idx + 1 + # 核心逻辑：判断行号是否在13-143之间，修改第三列（索引2，列表从0开始） + if 13 <= actual_line_num <= 143: + # 计算新的第三列值 + new_third_value = 372.19 + ((204.75 + serial_handler.Trajectory_angle) / 131) * (actual_line_num-12) + # 复制原列表并修改（避免修改原列表的引用） + line_list = line_list.copy() + line_list[2] = new_third_value # 第三列对应索引2 + if actual_line_num == 143: + row_143_third_value = new_third_value + elif actual_line_num > 143 and row_143_third_value is not None: + # 行号大于143时，用143行的第三列值替换 + line_list = line_list.copy() + line_list[2] = row_143_third_value + + increments = Coordinate_compensation + modified_list = [val + inc for val, inc in zip(line_list, increments)] + Pose_after_adjustment = modified_list + # print(Pose_after_adjustment) + if (i == 0): + suc, result, id = sendCMD(sock, "transparent_transmission_init", { + "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0}) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) + time.sleep(sleep_time) + if(i >= ex_turn_on_relay_start): + turn_on_relay() + print(f"打开喷枪4：{ex_turn_on_relay_start}") + if(i >= ex_turn_off_relay_start): + turn_off_relay() + print(f"关闭喷枪4：{ex_turn_off_relay_start}") + i = i + 1 + + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + print("第四段程序执行完") + + +def program5(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start): + print("第五段程序开始执行") + i = 0 + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + turn_off_relay() + global increments_x, increments_y, increments_z + global Coordinate_compensation + global sample_time + global sleep_time + + sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) + sleep_time = sample_time*0.001 + + if not compensate_queue.empty(): + increments_x, increments_y, increments_z = compensate_queue.get_nowait() + increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0] + Coordinate_compensation = increments + reference_pos = None + new_joint_pos = None + + if len(Pose_3) > 0 and len(Pose_3[0]) == 6: + Path_First_Position = Pose_3[0] # 获取第一行数据 + Modified_First_Position = [val + inc for val, inc in zip(Path_First_Position, Coordinate_compensation)] + target_pose = Modified_First_Position + else: + return + + for line_list in joint_positions_assembled_3: + if len(line_list) != 6: + continue + if reference_pos is None: + reference_pos = line_list + suc, angle, id = sendCMD(sock, "inverseKinematic", + {"targetPose": target_pose, "referencePos": reference_pos}) + if suc and angle is not None: + new_joint_pos = angle + else: + print("逆解计算失败，使用原有关节位置") + new_joint_pos = line_list + modified_list = new_joint_pos if new_joint_pos is not None else line_list + + if (i == 0): + while True: + suc, result, id = sendCMD(sock, "moveByJoint", {"targetPos": modified_list, "speed": 100}) + if suc: + break + print("moveByJoint命令发送失败,重试...") + time.sleep(0.1) + wait_stop(sock) + + time.sleep(sleep_time) + i = i + 1 + + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + print("第五段程序执行完") + +def program5_1(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start): + print("第5_1段程序开始执行") + i = 0 + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + turn_off_relay() + global increments_x, increments_y, increments_z + global Coordinate_compensation + global sample_time + global sleep_time + global lookahead_time + + sample_time = SAMP_TIME + sleep_time = sample_time*0.001 + lookahead_time = sample_time * Interpolation_points + + if not compensate_queue.empty(): + increments_x, increments_y, increments_z = compensate_queue.get_nowait() + increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0] + Coordinate_compensation = increments + + # 获取插值起点和插值终点 + start_pose = Pose_after_adjustment + end_pose = Pose_3[0].copy() + # 计算带补偿的终点位姿（最终插值要到达的目标） + end_pose_with_comp = [end_pose[i] + Coordinate_compensation[i] for i in range(6)] + + interpolate_points = Interpolation_points + interpolated_poses = [] + + for idx in range(interpolate_points): + # 计算插值权重（0→1，对应从起点第三列到终点第三列） + t = idx / (interpolate_points - 1) if interpolate_points > 1 else 1.0 + + # 组装当前插值位姿：仅更新第三列，其余列沿用起点值 + current_pose = start_pose.copy() + # 第三列：从起点值 线性插值到 终点补偿值的第三列 + current_pose[2] = start_pose[2] * (1 - t) + end_pose_with_comp[2] * t + + # 前两列：同步从起点值 线性调整到 终点补偿值的前两列（与第三列同权重） + current_pose[0] = start_pose[0] * (1 - t) + end_pose_with_comp[0] * t + current_pose[1] = start_pose[1] * (1 - t) + end_pose_with_comp[1] * t + + + # 添加到插值结果列表 + interpolated_poses.append(current_pose) + # print("\n=== 插值后的位姿列表（逐行）===") + # for i, pose in enumerate(interpolated_poses): + # print(f"第 {i+1} 个插值点: {pose}") + + # 遍历插值结果，逐个发送位姿（保留你原有发送逻辑） + for modified_list in interpolated_poses: + # 初始化透传 + if i == 0: + suc, result, id = sendCMD(sock, "transparent_transmission_init", { + "lookahead": lookahead_time, + "t": sample_time, + "smoothness": 1, + "response_enable": 0 + }) + # 发送当前插值得到的位姿 + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) + time.sleep(sleep_time) + i = i + 1 + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + print("第5_1段程序执行完") + +def program6(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start): + global increments_x, increments_y, increments_z + global Coordinate_compensation + global sample_time + global sleep_time + global lookahead_time + global Pose_after_adjustment + + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + lookahead_time = 400 + i = 0 + sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) + sleep_time = sample_time*0.001 + + for line_idx, line_list in enumerate(Pose_3): + if len(line_list) != 6: + continue + actual_line_num = line_idx + 1 + # 核心逻辑：判断行号是否在13-143之间，修改第三列（索引2，列表从0开始） + if 13 <= actual_line_num <= 143: + # 计算新的第三列值（注意是减法公式） + new_third_value = 576.94 - ((204.75 - serial_handler.Trajectory_angle) / 131) * (actual_line_num-12) + # 复制原列表并修改（避免修改原列表的引用） + line_list = line_list.copy() + line_list[2] = new_third_value # 第三列对应索引2 + if actual_line_num == 143: + row_143_third_value = new_third_value + elif actual_line_num > 143 and row_143_third_value is not None: + # 行号大于143时，用143行的第三列值替换 + line_list = line_list.copy() + line_list[2] = row_143_third_value + + increments = Coordinate_compensation + modified_list = [val + inc for val, inc in zip(line_list, increments)] + Pose_after_adjustment = modified_list + # print(Pose_after_adjustment) + if (i == 0): + suc, result, id = sendCMD(sock, "transparent_transmission_init", { + "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0}) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) + time.sleep(sleep_time) + if(i >= turn_on_relay_start): + turn_on_relay() + print(f"打开喷枪6：{turn_on_relay_start}") + if(i >= turn_off_relay_start): + turn_off_relay() + print(f"关闭喷枪6：{turn_off_relay_start}") + i = i + 1 + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + print("第六段程序执行完") + + +def program7(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start): + i = 0 + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + turn_off_relay() + global increments_x, increments_y, increments_z + global Coordinate_compensation + global sample_time + global sleep_time + + sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) + sleep_time = sample_time*0.001 + + if not compensate_queue.empty(): + increments_x, increments_y, increments_z = compensate_queue.get_nowait() + increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0] + Coordinate_compensation = increments + reference_pos = None + new_joint_pos = None + + if len(Pose_4) > 0 and len(Pose_4[0]) == 6: + Path_First_Position = Pose_4[0] # 获取第一行数据 + Modified_First_Position = [val + inc for val, inc in zip(Path_First_Position, Coordinate_compensation)] + target_pose = Modified_First_Position + else: + return + + for line_list in joint_positions_assembled_4: + if len(line_list) != 6: + continue + if reference_pos is None: + reference_pos = line_list + suc, angle, id = sendCMD(sock, "inverseKinematic", + {"targetPose": target_pose, "referencePos": reference_pos}) + if suc and angle is not None: + new_joint_pos = angle + else: + print("逆解计算失败，使用原有关节位置") + new_joint_pos = line_list + modified_list = new_joint_pos if new_joint_pos is not None else line_list + + if (i == 0): + while True: + suc, result, id = sendCMD(sock, "moveByJoint", {"targetPos": modified_list, "speed": 100}) + if suc: + break + print("moveByJoint命令发送失败,重试...") + time.sleep(0.1) + wait_stop(sock) + time.sleep(sleep_time) + i = i + 1 + + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + print("第七段程序执行完") + +def program7_1(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start): + print("第7_1段程序开始执行") + i = 0 + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + turn_off_relay() + global increments_x, increments_y, increments_z + global Coordinate_compensation + global sample_time + global sleep_time + global lookahead_time + + sample_time = SAMP_TIME + sleep_time = sample_time*0.001 + lookahead_time = sample_time * Interpolation_points + + if not compensate_queue.empty(): + increments_x, increments_y, increments_z = compensate_queue.get_nowait() + increments = [increments_x, increments_y, increments_z+z_positionadjustment, 0, 0, 0] + Coordinate_compensation = increments + + # 获取插值起点和插值终点 + start_pose = Pose_after_adjustment + end_pose = Pose_4[0] + # 计算带补偿的终点位姿（最终插值要到达的目标） + end_pose_with_comp = [end_pose[i] + Coordinate_compensation[i] for i in range(6)] + + interpolate_points = Interpolation_points + interpolated_poses = [] + # 仅对第三列进行线性插值，其余列保持起点值不变 + for idx in range(interpolate_points): + # 计算插值权重（0→1，对应从起点第三列到终点第三列） + t = idx / (interpolate_points - 1) if interpolate_points > 1 else 1.0 + # 组装当前插值位姿：仅更新第三列，其余列沿用起点值 + current_pose = start_pose.copy() + # 第三列：从起点值 线性插值到 终点补偿值的第三列 + current_pose[2] = start_pose[2] * (1 - t) + end_pose_with_comp[2] * t + + # 前两列：同步从起点值 线性调整到 终点补偿值的前两列（与第三列同权重） + current_pose[0] = start_pose[0] * (1 - t) + end_pose_with_comp[0] * t + current_pose[1] = start_pose[1] * (1 - t) + end_pose_with_comp[1] * t + + # 添加到插值结果列表 + interpolated_poses.append(current_pose) + # print("\n=== 插值后的位姿列表（逐行）===") + # for i, pose in enumerate(interpolated_poses): + # print(f"第 {i+1} 个插值点: {pose}") + + # 遍历插值结果，逐个发送位姿（保留你原有发送逻辑） + for modified_list in interpolated_poses: + # 初始化透传 + if i == 0: + suc, result, id = sendCMD(sock, "transparent_transmission_init", { + "lookahead": lookahead_time, + "t": sample_time, + "smoothness": 1, + "response_enable": 0 + }) + # 发送当前插值得到的位姿 + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) + time.sleep(sleep_time) + i = i + 1 + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + print("第7_1段程序执行完") + +def program8(sock, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start): + global increments_x, increments_y, increments_z + global Coordinate_compensation + global sample_time + global sleep_time + global lookahead_time + global Pose_after_adjustment + + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + lookahead_time = 400 + i = 0 + sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) + sleep_time = sample_time*0.001 + + # 遍历Pose_4，用enumerate获取行索引（从0开始） + for line_idx, line_list in enumerate(Pose_4): + if len(line_list) != 6: + continue + actual_line_num = line_idx + 1 + + # 核心逻辑：判断行号是否在13-143之间，修改第三列（索引2，列表从0开始） + if 13 <= actual_line_num <= 143: + # 计算新的第三列值（注意是减法公式） + new_third_value = 564.95 - ((204.75 - serial_handler.Trajectory_angle) / 131) * (actual_line_num-12) + # 复制原列表并修改（避免修改原列表的引用） + line_list = line_list.copy() + line_list[2] = new_third_value # 第三列对应索引2 + if actual_line_num == 143: + row_143_third_value = new_third_value + elif actual_line_num > 143 and row_143_third_value is not None: + # 行号大于143时，用143行的第三列值替换 + line_list = line_list.copy() + line_list[2] = row_143_third_value + + increments = Coordinate_compensation + modified_list = [val + inc for val, inc in zip(line_list, increments)] + Pose_after_adjustment = modified_list + # print(Pose_after_adjustment) + if (i == 0): + suc, result, id = sendCMD(sock, "transparent_transmission_init", { + "lookahead": lookahead_time, "t": sample_time, "smoothness": 1, "response_enable": 0}) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": modified_list}) + time.sleep(sleep_time) + if(i >= ex_turn_on_relay_start): + turn_on_relay() + print(f"打开喷枪8：{ex_turn_on_relay_start}") + if(i >= ex_turn_off_relay_start): + turn_off_relay() + print(f"关闭喷枪8：{ex_turn_off_relay_start}") + i = i + 1 + + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + print("第八段程序执行完") + + +def program9(sock): + start_time = time.time() + i = 0 + increments = [0, 0, 0, 0, 0, 0] + reference_pos = None # 初始化参考位置 + new_joint_pos = None # 初始化逆解得到的新关节位置 + pose_file = open('/home/raspberrypi/robot1/Pose_5.txt', "r") + first_pose_line = pose_file.readline().strip() + if first_pose_line: + Path_First_Position = list(map(float, first_pose_line.split(','))) + Modified_First_Position = [val + inc for val, inc in zip(Path_First_Position, increments)] + target_pose = Modified_First_Position + else: + #print("Pose_5.txt为空，无法获取初始位姿") + pose_file.close() + return + pose_file.close() + + file_name = '/home/raspberrypi/robot1/joint_positions_assembled_5.txt' + fo = open(file_name, "r") + while 1: + # 读取一行内容 + line = fo.readline() + if not line: + break + line_list = line.strip() + line_list = list(map(float, line_list.split(','))) + + if reference_pos is None: + reference_pos = line_list + suc, angle, id = sendCMD(sock, "inverseKinematic", + {"targetPose": target_pose, "referencePos": reference_pos}) + if suc and angle is not None: + new_joint_pos = angle + else: + #print("逆解计算失败，使用原有关节位置") + new_joint_pos = line_list # 失败时使用原位置 + modified_list = new_joint_pos if new_joint_pos is not None else line_list + if (i == 0): + while True: + suc, result, id = sendCMD(sock, "moveByJoint", {"targetPos": modified_list, "speed": 100}) + if suc: + break + print("moveByJoint命令发送失败，重试...") + time.sleep(0.1) # 重试间隔，可根据需求调整 + wait_stop(sock) + time.sleep(0.01) + i = i + 1 + fo.close() + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + end_time = time.time() + print("第九段程序执行完") + + +def program10(sock): + start_time = time.time() + i = 0 + increments = [0, 0, 0, 0, 0, 0] + reference_pos = None + new_joint_pos = None + + pose_file = open('/home/raspberrypi/robot1/Pose_6.txt', "r") + first_pose_line = pose_file.readline().strip() + if first_pose_line: + Path_First_Position = list(map(float, first_pose_line.split(','))) + Modified_First_Position = [val + inc for val, inc in zip(Path_First_Position, increments)] + target_pose = Modified_First_Position + else: + #print("Pose_6.txt为空，无法获取初始位姿") + pose_file.close() + return + pose_file.close() + + file_name = '/home/raspberrypi/robot1/joint_positions_assembled_6.txt' + fo = open(file_name, "r") + while 1: + line = fo.readline() + if not line: + break + line_list = line.strip() + line_list = list(map(float, line_list.split(','))) + + if reference_pos is None: + reference_pos = line_list + suc, angle, id = sendCMD(sock, "inverseKinematic", + {"targetPose": target_pose, "referencePos": reference_pos}) + if suc and angle is not None: + new_joint_pos = angle + else: + #print("逆解计算失败，使用原有关节位置") + new_joint_pos = line_list # 失败时使用原位置 + modified_list = new_joint_pos if new_joint_pos is not None else line_list + + if (i == 0): + while True: + suc, result, id = sendCMD(sock, "moveByJoint", {"targetPos": modified_list, "speed": 100}) + if suc: + break + print("moveByJoint命令发送失败,重试...") + time.sleep(0.1) # 重试间隔，可根据需求调整 + wait_stop(sock) + time.sleep(0.01) + i = i + 1 + fo.close() + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + end_time = time.time() + print("第十段程序执行完") + + +def program11(sock): + + start_time = time.time() + i = 0 + increments = [0, 0, 0, 0, 0, 0] + reference_pos = None + new_joint_pos = None + pose_file = open('/home/raspberrypi/robot1/Pose_7.txt', "r") + + first_pose_line = pose_file.readline().strip() + if first_pose_line: + Path_First_Position = list(map(float, first_pose_line.split(','))) + Modified_First_Position = [val + inc for val, inc in zip(Path_First_Position, increments)] + target_pose = Modified_First_Position + else: + #print("Pose_7.txt为空，无法获取初始位姿") + pose_file.close() + return + pose_file.close() + file_name = '/home/raspberrypi/robot1/joint_positions_assembled_7.txt' + fo = open(file_name, "r") + + while 1: + line = fo.readline() + if not line: + break + line_list = line.strip() + line_list = list(map(float, line_list.split(','))) + + if reference_pos is None: + reference_pos = line_list + suc, angle, id = sendCMD(sock, "inverseKinematic", + {"targetPose": target_pose, "referencePos": reference_pos}) + if suc and angle is not None: + new_joint_pos = angle + else: + #print("逆解计算失败，使用原有关节位置") + new_joint_pos = line_list # 失败时使用原位置 + + modified_list = new_joint_pos if new_joint_pos is not None else line_list + + if (i == 0): + while True: + suc, result, id = sendCMD(sock, "moveByJoint", {"targetPos": modified_list, "speed": 100}) + if suc: + break + print("moveByJoint命令发送失败，重试...") + time.sleep(0.1) # 重试间隔，可根据需求调整 + wait_stop(sock) # 等待机器人停止 + time.sleep(0.01) + i = i + 1 + fo.close() + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + end_time = time.time() + print("第十一段程序执行完") + +def program12(sock): + start_time = time.time() + i = 0 + increments = [0, 0, 0, 0, 0, 0] + reference_pos = None + new_joint_pos = None + pose_file = open('/home/raspberrypi/robot1/Pose_8.txt', "r") + first_pose_line = pose_file.readline().strip() + if first_pose_line: + Path_First_Position = list(map(float, first_pose_line.split(','))) + Modified_First_Position = [val + inc for val, inc in zip(Path_First_Position, increments)] + target_pose = Modified_First_Position + else: + #print("Pose_1.txt为空，无法获取初始位姿") + pose_file.close() + return + pose_file.close() + + file_name = '/home/raspberrypi/robot1/joint_positions_assembled_8.txt' + fo = open(file_name, "r") + while 1: + line = fo.readline() + if not line: + break + line_list = line.strip() + line_list = list(map(float, line_list.split(','))) + + if reference_pos is None: + reference_pos = line_list + suc, angle, id = sendCMD(sock, "inverseKinematic", {"targetPose": target_pose, "referencePos": reference_pos}) + if suc and angle is not None: + new_joint_pos = angle + else: + #print("逆解计算失败，使用原有关节位置") + new_joint_pos = line_list + modified_list = new_joint_pos if new_joint_pos is not None else line_list + if (i == 0): + while True: + suc, result, id = sendCMD(sock, "moveByJoint", {"targetPos": modified_list, "speed": 100}) + if suc: + break + print("moveByJoint命令发送失败，重试...") + time.sleep(0.1) # 重试间隔，可根据需求调整 + wait_stop(sock) + time.sleep(0.02) + i = i + 1 + fo.close() + suc, result, id = sendCMD(sock, "tt_clear_servo_joint_buf") + end_time = time.time() + print("第十二段程序执行完") + + +def program100(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay, delay1): + global sample_time, lookahead_time + + if 1 == ENABLE_INCREMENTS: + reset_filter() + + lookahead_time = 400 # 前瞻时间 (ms) + + sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) + sleep_time = sample_time*0.001 + + #0.点位平移操作 + # 1.假设 Off_Set_Value 已由外部传入（示例值） + Actual_value_mm=133*sleep_time*80 + Comp_value_mm = 204.75 - Actual_value_mm + Off_Set_Value = serial_handler.Trajectory_angle + Comp_value_mm# 示例值，实际由外部传入 + # 2.深拷贝原始轨迹（避免修改原数据） + Pose_3_M = copy.deepcopy(Pose_3) + Pose_4_M = copy.deepcopy(Pose_4) + # 3.修改 Pose_3_M 和 Pose_4_M 的第12到144行（MATLAB索引12~144 => Python索引11~143） + # 注意：共 133 行（144-12+1=133），但范围是 12:144 共133个点 + for i in range(11, 144): # i = 11 ~ 143 对应 MATLAB 第12~144行 + factor = (i - 11) / 133.0 # MATLAB中 (i-12)/133，i从12开始，对应Python i=11时因子0 + Pose_3_M[i][2] = Pose_3[i][2] + Off_Set_Value * factor + Pose_4_M[i][2] = Pose_4[i][2] + Off_Set_Value * factor + # 修改第145行到末尾（MATLAB索引145:end => Python索引144:） + for i in range(144, len(Pose_3_M)): + Pose_3_M[i][2] = Pose_3[i][2] + Off_Set_Value + Pose_4_M[i][2] = Pose_4[i][2] + Off_Set_Value + # 4.处理 Pose_3_4 和 Pose_4_3 + num = len(Pose_3_4) # 假设两者长度相同 + Pose_3_4_M = copy.deepcopy(Pose_3_4) + Pose_4_3_M = copy.deepcopy(Pose_4_3) + for i in range(num): + # MATLAB 中 i 从 1 开始，公式： -Off_Set_Value/num*i + Off_Set_Value + # Python 中 i 从 0 开始，所以因子为 (i+1) + factor = (i + 1) / num + Pose_3_4_M[i][2] = Pose_3_4[i][2] - Off_Set_Value * factor + Off_Set_Value + Pose_4_3_M[i][2] = Pose_4_3[i][2] - Off_Set_Value * factor + Off_Set_Value + + Pose_3_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_3_M] + Pose_4_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_4_M] + Pose_3_4_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_3_4_M] + Pose_4_3_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_4_3_M] + + #0.延时操作，点位延时 + #1.添加点位延时 + NUM = int(delay * 1000 / sample_time) + NUM1 = int(delay1 * 1000 / sample_time) + # 2.第一道喷涂延时点位 + last_point_3_4 = Pose_3_4_M[-1].copy() # 先复制最后一个点位 + Pose_3_4_Stop = [last_point_3_4.copy() for _ in range(NUM)] # 每个点位独立副本 + # 3.第2道喷涂延时点位 + last_point_4_3 = Pose_4_3_M[-1].copy() + Pose_4_3_Stop = [last_point_4_3.copy() for _ in range(NUM1)] + + trajectories = [Pose_3_M, Pose_3_4_M,Pose_3_4_Stop, Pose_4_M, Pose_4_3_M,Pose_4_3_Stop] + # ----- 一次性清理和初始化透传服务 ----- + sendCMD(sock, "tt_clear_servo_joint_buf") + time.sleep(0.05) + + #0.喷枪开关控制,开喷枪时间最小为0，最大为155；Pose_4_3_M及Pose_3_4_M中各有20组数据 + #每道喷涂指令有155个点， + # 第1道开枪时间在第10个点位（Pose_3_M的开枪点位 lookahead_time/sample_time+10），关枪时间在第130点位示例（lookahead_time/sample_time+135） + # 第2道开枪时间在第15个点位（Pose_4_M的开枪点位 lookahead_time/sample_time+15），，关枪时间在第150点位示例（lookahead_time/sample_time+135，，，，如果数据>155，那么关枪就在Pose_4_3_M中） + #队列，延时时间计算 + off_set_Time=int(lookahead_time / sample_time) + # 1.喷枪1开关时间计算 + OPEN_1=turn_on_relay_start + CLOSE_1=turn_off_relay_start + OPEN_1_M = off_set_Time+OPEN_1 + CLOSE_1_M1S=off_set_Time+CLOSE_1 + CLOSE_1_M1E = off_set_Time + CLOSE_1-155 + #2.喷枪2开关时间计算 + OPEN_2=ex_turn_on_relay_start + CLOSE_2 = ex_turn_off_relay_start + OPEN_2_M = off_set_Time+OPEN_2 + CLOSE_2_M2S = off_set_Time+CLOSE_2 + CLOSE_2_M2E = off_set_Time + CLOSE_2- 155 + + sendCMD(sock, "transparent_transmission_init", {"lookahead": lookahead_time,"t": sample_time,"smoothness": 1,"response_enable": 0 }) + cycles = 100 # 循环5次 + + for _ in range(cycles): + for traj in trajectories: + print("开始执行轨迹...") + # 针对不同轨迹进行计数 + if traj is Pose_3_M: + for idx, point in enumerate(traj, start=1): + if idx == OPEN_1_M: + turn_on_relay() + if idx == CLOSE_1_M1S: + turn_off_relay() + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) + time.sleep(sleep_time) + elif traj is Pose_3_4_M: + for idx, point in enumerate(traj, start=1): + if idx == CLOSE_1_M1E: + turn_off_relay() + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) + time.sleep(sleep_time) + elif traj is Pose_4_M: + for idx, point in enumerate(traj, start=1): + if idx == OPEN_2_M: + turn_on_relay() + if idx == CLOSE_2_M2S: + turn_off_relay() + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) + time.sleep(sleep_time) + elif traj is Pose_4_3_M: + for idx, point in enumerate(traj, start=1): + if idx == CLOSE_2_M2E: + turn_off_relay() + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) + time.sleep(sleep_time) + else: + # 其他轨迹（如 Stop 轨迹）无需喷枪控制 + for point in traj: + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) + time.sleep(sleep_time) + print(f"停止轨迹，NUM = {NUM}, delay = {delay}") + stop_cmd_bytes = read_cmd_from_shared(timeout=0.1) + if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30': + print("收到0xAA 0x30,停止循环") + for i in range(3): + ser.write(b'\x80\x80') + ser.flush() + time.sleep(0.01) + time.sleep(0.5) + return False + print("轨迹执行完成") + + return False + +def program101(sock, ser, turn_on_relay_start, turn_off_relay_start, ex_turn_on_relay_start, ex_turn_off_relay_start, delay, delay1): + global sample_time, lookahead_time + + if 1 == ENABLE_INCREMENTS: + reset_filter() + + lookahead_time = 400 # 前瞻时间 (ms) + + sample_time = (int)(SAMP_TIME*velocity_coefficient/serial_handler.speed_adjustment) + sleep_time = sample_time*0.001 + + #0.点位平移操作 + # 1.假设 Off_Set_Value 已由外部传入（示例值） + Actual_value_mm=133*sleep_time*80 + Comp_value_mm = 204.75 - Actual_value_mm + Off_Set_Value = serial_handler.Trajectory_angle + Comp_value_mm # 示例值，实际由外部传入 + # 这里的喷涂方向与program100相反 + Off_Set_Value = -Off_Set_Value + # 2.深拷贝原始轨迹（避免修改原数据） + Pose_3_M = copy.deepcopy(Pose_1) + Pose_4_M = copy.deepcopy(Pose_2) + # 3.修改 Pose_3_M 和 Pose_4_M 的第12到144行（MATLAB索引12~144 => Python索引11~143） + # 注意：共 133 行（144-12+1=133），但范围是 12:144 共133个点 + for i in range(11, 144): # i = 11 ~ 143 对应 MATLAB 第12~144行 + factor = (i - 11) / 133.0 # MATLAB中 (i-12)/133，i从12开始，对应Python i=11时因子0 + Pose_3_M[i][2] = Pose_1[i][2] + Off_Set_Value * factor + Pose_4_M[i][2] = Pose_2[i][2] + Off_Set_Value * factor + # 修改第145行到末尾（MATLAB索引145:end => Python索引144:） + for i in range(144, len(Pose_3_M)): + Pose_3_M[i][2] = Pose_1[i][2] + Off_Set_Value + Pose_4_M[i][2] = Pose_2[i][2] + Off_Set_Value + # 4.处理 Pose_1_2 和 Pose_2_1 + num = len(Pose_1_2) # 假设两者长度相同 + Pose_3_4_M = copy.deepcopy(Pose_1_2) + Pose_4_3_M = copy.deepcopy(Pose_2_1) + for i in range(num): + # MATLAB 中 i 从 1 开始，公式： -Off_Set_Value/num*i + Off_Set_Value + # Python 中 i 从 0 开始，所以因子为 (i+1) + factor = (i + 1) / num + Pose_3_4_M[i][2] = Pose_1_2[i][2] - Off_Set_Value * factor + Off_Set_Value + Pose_4_3_M[i][2] = Pose_2_1[i][2] - Off_Set_Value * factor + Off_Set_Value + + Pose_3_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_3_M] + Pose_4_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_4_M] + Pose_3_4_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_3_4_M] + Pose_4_3_M = [[p[0], p[1], p[2] - 200] + p[3:] for p in Pose_4_3_M] + + #0.延时操作，点位延时 + #1.添加点位延时 + NUM = int(delay * 1000 / sample_time) + NUM1 = int(delay1 * 1000 / sample_time) + # 2.第一道喷涂延时点位 + last_point_3_4 = Pose_3_4_M[-1].copy() # 先复制最后一个点位 + Pose_3_4_Stop = [last_point_3_4.copy() for _ in range(NUM)] # 每个点位独立副本 + # 3.第2道喷涂延时点位 + last_point_4_3 = Pose_4_3_M[-1].copy() + Pose_4_3_Stop = [last_point_4_3.copy() for _ in range(NUM1)] + + trajectories = [Pose_3_M, Pose_3_4_M,Pose_3_4_Stop, Pose_4_M, Pose_4_3_M,Pose_4_3_Stop] + # ----- 一次性清理和初始化透传服务 ----- + sendCMD(sock, "tt_clear_servo_joint_buf") + time.sleep(0.05) + + #0.喷枪开关控制,开喷枪时间最小为0，最大为155；Pose_4_3_M及Pose_3_4_M中各有20组数据 + #每道喷涂指令有155个点， + # 第1道开枪时间在第10个点位（Pose_3_M的开枪点位 lookahead_time/sample_time+10），关枪时间在第130点位示例（lookahead_time/sample_time+135） + # 第2道开枪时间在第15个点位（Pose_4_M的开枪点位 lookahead_time/sample_time+15），，关枪时间在第150点位示例（lookahead_time/sample_time+135，，，，如果数据>155，那么关枪就在Pose_4_3_M中） + #队列，延时时间计算 + off_set_Time=int(lookahead_time / sample_time) + # 1.喷枪1开关时间计算 + OPEN_1=turn_on_relay_start + CLOSE_1=turn_off_relay_start + OPEN_1_M = off_set_Time+OPEN_1 + CLOSE_1_M1S=off_set_Time+CLOSE_1 + CLOSE_1_M1E = off_set_Time + CLOSE_1-155 + #2.喷枪2开关时间计算 + OPEN_2=ex_turn_on_relay_start + CLOSE_2 = ex_turn_off_relay_start + OPEN_2_M = off_set_Time+OPEN_2 + CLOSE_2_M2S = off_set_Time+CLOSE_2 + CLOSE_2_M2E = off_set_Time + CLOSE_2- 155 + + sendCMD(sock, "transparent_transmission_init", {"lookahead": lookahead_time,"t": sample_time,"smoothness": 1,"response_enable": 0 }) + cycles = 100 # 循环5次s + + for _ in range(cycles): + for traj in trajectories: + print("开始执行轨迹...") + # 针对不同轨迹进行计数 + if traj is Pose_3_M: + for idx, point in enumerate(traj, start=1): + if idx == OPEN_1_M: + turn_on_relay() + if idx == CLOSE_1_M1S: + turn_off_relay() + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) + time.sleep(sleep_time) + elif traj is Pose_3_4_M: + for idx, point in enumerate(traj, start=1): + if idx == CLOSE_1_M1E: + turn_off_relay() + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) + time.sleep(sleep_time) + elif traj is Pose_4_M: + for idx, point in enumerate(traj, start=1): + if idx == OPEN_2_M: + turn_on_relay() + if idx == CLOSE_2_M2S: + turn_off_relay() + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) + time.sleep(sleep_time) + elif traj is Pose_4_3_M: + for idx, point in enumerate(traj, start=1): + if idx == CLOSE_2_M2E: + turn_off_relay() + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) + time.sleep(sleep_time) + else: + # 其他轨迹（如 Stop 轨迹）无需喷枪控制 + for point in traj: + point_M = get_compensated_point(point) + send_Point(sock, "tt_put_servo_joint_to_buf", {"targetPose": point_M}) + time.sleep(sleep_time) + print(f"停止轨迹，NUM = {NUM}, delay = {delay}") + stop_cmd_bytes = read_cmd_from_shared(timeout=0.1) + if stop_cmd_bytes is not None and stop_cmd_bytes == b'\xAA\x30': + print("收到0xAA 0x30,停止循环") + for i in range(3): + ser.write(b'\x80\x80') + ser.flush() + time.sleep(0.01) + time.sleep(0.5) + return False + print("轨迹执行完成") + + return False + + + + + diff --git a/Kelaifen_V6.0/transform.py b/Kelaifen_V6.0/transform.py new file mode 100644 index 0000000..63b740a --- /dev/null +++ b/Kelaifen_V6.0/transform.py @@ -0,0 +1,25 @@ + + +import numpy as np +# 定义转换矩阵R_T +R_T = np.array([ + [-0.7206, -0.6908, 0], + [0.6884, -0.7231, 0], + [0.0830, 0, 1] +]) + + +def transform_data(x_coord, y_coord, z_coord): + # 将接收到的3个坐标组成列矩阵P_L1_O2 + P_L1_O2 = np.array([[x_coord], [y_coord], [z_coord]], dtype=np.float64) + + # 执行矩阵乘法：P_L1_O1 = R_T * P_L1_O2 + P_L1_O1 = np.dot(R_T, P_L1_O2) + + # 提取转换后的三个数据 + increments_x = P_L1_O1[0][0] + increments_y = P_L1_O1[1][0] + increments_z = P_L1_O1[2][0] + + #print(f"矩阵转换完成 - increments_x: {increments_x:.4f}, increments_y: {increments_y:.4f}, increments_z: {increments_z:.4f}") + return increments_x, increments_y, increments_z \ No newline at end of file