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