import socket import json import numpy as np # import scipy.io ## # # # # # # 总配置参数 dt = 0.005 # 总体时间间隔 ## # # # # # # 第一甩枪段参数,向外摆动40°，用时0.4s匀加速运动 angle_Add_0 = -40 total_time_0 = 0.4 # 运动总时长（单位：秒） ## # # # # # # 直线段参数,梯形运动线，匀加速，匀速，匀减速 # 中间匀速阶段的速度 v_constant_1 = 1000.0 # 设定加速时间 t_acceleration_1 = 0.2 ## # # # # # # 第二段甩枪，匀速，匀减速（与第一段甩枪的角度相同） angle_Add_1 = -angle_Add_0 total_time_1 = 0.4 # 运动总时长（单位：秒） t_constant_1 = 0.2 # 匀速阶段时间 t_deceleration_1 = 0.2 # 匀减速阶段时间 #甩枪搭接 Overlap_time_0=0.2 steps_Overlap_time_0 = int(Overlap_time_0 / dt) Overlap_time_1=0.2 steps_Overlap_time_1 = int(Overlap_time_1 / dt) # 二、将起始位姿读取 try: # 打开 data.txt 文件以读取数据 with open('data.txt', 'r') as file: # 解析文件中的 JSON 数据为 Python 字典 data = json.load(file) # 从字典中提取 angle1 - angle5 数据 Angle1 = data["angle1"] Angle2 = data["angle2"] Angle3 = data["angle3"] Angle4 = data["angle4"] Angle5 = data["angle5"] # 从字典中提取 PS1 - PS5 数据，并转换为 NumPy 数组 PS1 = np.array(data["PS1"], dtype=np.float64) PS2 = np.array(data["PS2"], dtype=np.float64) PS3 = np.array(data["PS3"], dtype=np.float64) PS4 = np.array(data["PS4"], dtype=np.float64) PS5 = np.array(data["PS5"], dtype=np.float64) # 打印读取到的数据（可根据需要选择是否保留） print("Angle1:", Angle1) print("Angle2:", Angle2) print("Angle3:", Angle3) print("Angle4:", Angle4) print("Angle5:", Angle5) print("PS1:", PS1) print("PS2:", PS2) print("PS3:", PS3) print("PS4:", PS4) print("PS5:", PS5) except FileNotFoundError: print("未找到 data.txt 文件，请检查文件路径和文件名是否正确。") except json.JSONDecodeError: print("data.txt 文件不是有效的 JSON 格式，请检查文件内容。") except KeyError as e: print(f"数据中缺少所需的键 {e}，请检查 data.txt 文件内容。") except Exception as e: print(f"读取文件时出现其他错误: {e}") def check_joint_continuity(NNNNN): # 将输入转换为 NumPy 数组 joint_array = np.array(NNNNN) rows, Lie = joint_array.shape # 若行数小于 2，无法进行连续性检查 if rows < 2: print("由于数据行数少于 2，无法进行连续性检查。") return # 计算相邻两行的差值的绝对值 differences = np.abs(np.diff(joint_array, axis=0)) # 找出这些差值中的最大值 max_difference = np.max(differences) # 根据最大值判断轨迹是否连续 if max_difference > 1: print("轨迹可能不连续") else: print("关节连续") # 连接机器人控制器 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) if __name__ == "__main__": # 机器人 IP 地址 robot_ip = "192.168.1.100" result = connectETController(robot_ip) if len(result) == 2: conSuc, sock = result if conSuc: print("成功连接到机器人") try: # 第一部分：根据 PS4 和 PS5 的位置信息插值一条直线 print("开始第一部分：直线插值运动规划") # 提取 PS4 和 PS5 的位置信息 first_point = np.array(PS4[:3], dtype=np.float64) last_point = np.array(PS5[:3], dtype=np.float64) # 计算两点之间的距离 distance = np.linalg.norm(last_point - first_point) # 计算加速阶段的加速度，使用公式 v = u + at（u = 0） v_final_acceleration = v_constant_1 v_initial_acceleration = 0.0 a = (v_final_acceleration - v_initial_acceleration) / t_acceleration_1 # 计算加速阶段的距离 d_acceleration = 0.5 * a * t_acceleration_1 ** 2 # 计算匀速阶段的距离和时间 d_constant = distance - 2 * d_acceleration if d_constant > 0: t_constant = d_constant / v_constant_1 else: t_constant = 0 # 计算总时间 total_time_interp = 2 * t_acceleration_1 + t_constant time = np.arange(0, total_time_interp + dt, dt, dtype=np.float64) # 初始化位置矩阵 position = np.zeros((len(time), 3), dtype=np.float64) # 计算每个时间步的位置 for i in range(len(time)): t = time[i] if t <= t_acceleration_1: # 匀加速阶段 s = 0.5 * a * t ** 2 position[i, :] = first_point + s * (last_point - first_point) / distance elif t <= (total_time_interp - t_acceleration_1): # 匀速阶段 s = d_acceleration + v_constant_1 * (t - t_acceleration_1) position[i, :] = first_point + s * (last_point - first_point) / distance else: # 匀减速阶段 decel_time = t - (total_time_interp - t_acceleration_1) s = d_acceleration + d_constant + v_constant_1 * decel_time - 0.5 * a * decel_time ** 2 position[i, :] = first_point + s * (last_point - first_point) / distance # 生成 End_Pose_1 矩阵 End_Pose_1 = np.zeros((len(position), 6), dtype=np.float64) end_effector_rpy = np.array(PS1[3:]).reshape(1, -1) for i in range(len(position)): End_Pose_1[i, :3] = position[i, :] End_Pose_1[i, 3:] = end_effector_rpy[0, :] rows, columns = End_Pose_1.shape # 初始化存储关节位置的列表 joint_positions_1 = [] # 第一个位姿对应的关节位置 reference_pos = Angle4 for i in range(len(End_Pose_1)): # 获取当前位姿 target_pose = End_Pose_1[i].tolist() # 调用逆运动学函数计算关节位置 suc, angle, id = sendCMD(sock, "inverseKinematic", {"targetPose": target_pose, "referencePos": reference_pos}) if suc: # 若计算成功，将结果添加到列表中 joint_positions_1.append(angle) # 更新参考位置为本次计算出的关节位置 reference_pos = angle else: print(f"第 {i} 个位姿的逆运动学计算失败: {angle}") # 可以选择将结果保存为数组形式 joint_positions_array = np.array(joint_positions_1) print("第一部分：直线插值运动规划完成") # 第二部分：第六关节转动 40°，匀加速运动，总时长为 0.4s print("开始第二部分：第六关节转动 40°运动规划") Start_Pos_1 = Angle4 sixth_joint_pos_0 = Angle4[5] # 要增加的角度（单位：度） sixth_joint_pos_1 = sixth_joint_pos_0 + angle_Add_0 # 计算第六关节的总位移 total_displacement = sixth_joint_pos_0 - sixth_joint_pos_1 # 根据匀加速直线运动公式 s = 0.5 * a * t^2（初速度 v0 = 0）计算加速度 acceleration = 2 * total_displacement / (total_time_0 ** 2) current_time = 0 # 初始化当前时间为 0 Angle_position_list_0 = [] # 用于存储每个时间点的关节位置数据 End_Pose_0 = [] # 用于存储每个关节位置对应的末端位姿 # 循环计算并存储每个时间点的第六关节位置 while current_time <= total_time_0: # 根据匀加速直线运动公式 s = v0 * t + 0.5 * a * t^2（初速度 v0 = 0）计算当前位移 current_displacement = 0.5 * acceleration * (current_time ** 2) # 计算当前时间点第六关节的位置 current_position = sixth_joint_pos_1 + current_displacement new_position = Start_Pos_1.copy() # 复制 Start_Pos_1 并更新第六列的值 new_position[5] = current_position Angle_position_list_0.append(new_position) suc, end_pose, id = sendCMD(sock, "positiveKinematic", {"targetPos": new_position, "unit_type": 1}) if suc: End_Pose_0.append(end_pose) else: print(f"正运动学求解失败，时间: {current_time * 1000:.2f}ms") # 更新当前时间，进入下一个时间点 current_time += dt first_joint_position = Angle_position_list_0[0] print("第二部分：第六关节转动 40°运动规划完成") # 第三部分：第六关节新的运动规划，包含匀速和匀减速阶段 print("开始第三部分：第六关节新运动规划") Start_Pos_3 = Angle5 # 提取第六关节的当前位置 sixth_joint_pos_0 = Start_Pos_3[5] sixth_joint_pos_1 = sixth_joint_pos_0 + angle_Add_1 # 计算第六关节的总位移 total_displacement = sixth_joint_pos_1 - sixth_joint_pos_0 # 计算匀速阶段的速度 # 这里根据位移和时间计算出合适的匀速速度 v_constant = total_displacement / (t_constant_1 + 0.5 * t_deceleration_1) # 计算匀减速阶段的加速度 a = -v_constant / t_deceleration_1 current_time = 0 # 初始化当前时间为 0 Angle_position_list_2 = [] # 用于存储每个时间点的关节位置数据 End_Pose_2 = [] # 用于存储每个关节位置对应的末端位姿 # 循环计算并存储每个时间点的第六关节位置 while current_time <= total_time_1: if current_time <= t_constant_1: # 匀速阶段 current_position = sixth_joint_pos_0 + v_constant * current_time else: # 匀减速阶段 decel_time = current_time - t_constant_1 current_position = sixth_joint_pos_0 + v_constant * t_constant_1 + v_constant * decel_time + 0.5 * a * decel_time ** 2 new_position = Start_Pos_3.copy() # 复制 Start_Pos_3 并更新第六列的值 new_position[5] = current_position Angle_position_list_2.append(new_position) suc, end_pose, id = sendCMD(sock, "positiveKinematic", {"targetPos": new_position, "unit_type": 1}) if suc: End_Pose_2.append(end_pose) else: print(f"正运动学求解失败，时间: {current_time * 1000:.2f}ms") # 更新当前时间，进入下一个时间点 current_time += dt print("第三部分：第六关节新运动规划完成") except Exception as e: print(f"执行过程中出现错误: {e}") else: print("无法连接到机器人") # 假设重叠时长Overlapping_duration_1=0.20S，Overlapping_duration_2=0.20S，那么总Time_total_Actual=Time_total-Overlapping_duration_1-Overlapping_duration_2; ## # # # # 数据组装，末端位置组装和关节位置组装（请对以下的变量名进行修改，感觉MMMM数据名太差劲） ## # # # # 1.首先根据End_Pose_0，End_Pose_1，End_Pose_2的数据长度计算总时间长度Time_total_all,dt=0.005 ## # # # # 2.下面将对数据MMMM进行组装汇总，数据MMMM前0.2s完全使用End_Pose_0的数据; 数据MMMM0.2-0.4秒使用End_Pose_1的位置（0s-0.2s）使用End_Pose_0的姿态（0.2-0.4s），在0.4s后完全使用End_Pose_1的位姿； ## # # # # 3.在数据MMMM完成第一次组装后，下面将进行第二次组装组装后的数据放到新矩阵中；1.最初的新矩阵=MMMM；2.新矩阵最后0.2s使用MMMM数据的位置，使用End_Pose_2的姿态（0s-0.2s）；3.新矩阵扩张0.2s,将End_Pose_2(0.2s-0.4s)添加到新矩阵中 ## # # # # 4.将数据保存在txt中 # ... 原有代码保持不变 ... # 确保 End_Pose_0、End_Pose_1 和 End_Pose_2 是 numpy 数组 End_Pose_0 = np.array(End_Pose_0) End_Pose_1 = np.array(End_Pose_1) End_Pose_2 = np.array(End_Pose_2) # 1. 计算总时间长度 Time_total_all # 2. 第一次数据组装 # 计算 0.2s 对应的时间步数 steps_0_2s = int(Overlap_time_0 / dt) # 初始化组装后的数据矩阵 # assembled_data_1 = np.zeros((len(End_Pose_0) + len(End_Pose_1), 6)) assembled_data_1 = np.zeros((steps_0_2s + len(End_Pose_1), 6)) # 前 0.2s 完全使用 End_Pose_0 的数据 assembled_data_1[:steps_0_2s] = End_Pose_0[:steps_0_2s] # 0.2 - 0.4 秒：使用 End_Pose_1 的位置（0s - 0.2s）和 End_Pose_0 的姿态（0.2 - 0.4s） assembled_data_1[steps_0_2s:2 * steps_0_2s, :3] = End_Pose_1[:steps_0_2s, :3] assembled_data_1[steps_0_2s:2 * steps_0_2s, 3:] = End_Pose_0[steps_0_2s:2 * steps_0_2s, 3:] # 修正部分：确保赋值时形状匹配 start_index = 2 * steps_0_2s end_index = start_index + len(End_Pose_1[steps_0_2s:]) assembled_data_1[start_index:end_index] = End_Pose_1[steps_0_2s:] # 3. 第二次数据组装 # 初始化新矩阵 assembled_data_2 = assembled_data_1.copy() # 计算 0.2s 对应的时间步数 第二次搭接 steps_0_2s = int(Overlap_time_1 / dt) # 矩阵 assembled_data_2 最后 0.2s 使用 assembled_data_1 最后 0.2s 的位置信息，使用 End_Pose_2 的姿态（0s - 0.2s） last_steps_0_2s = len(assembled_data_2) - steps_0_2s assembled_data_2[last_steps_0_2s:, :3] = assembled_data_1[last_steps_0_2s:, :3] assembled_data_2[last_steps_0_2s:, 3:] = End_Pose_2[:steps_0_2s, 3:] # 矩阵 assembled_data_2 增加 0.2s，使用 End_Pose_2 的位置和姿态（0.2s - 0.4s） additional_data = End_Pose_2[steps_0_2s:] assembled_data_2 = np.vstack((assembled_data_2, additional_data)) # 4. 将数据保存在 txt 中 # np.savetxt('assembled_data.txt', assembled_data_2, delimiter=',', fmt='%.6f') # print("组装后的数据已保存到 assembled_data.txt 文件中。") # 输出 assembled_data_2 的行数和列数 assembled_data_2_array = np.array(assembled_data_2) rows, columns = assembled_data_2_array.shape print(f"assembled_data_2 的行数为: {rows}，列数为: {columns}") with open('Pose_XYZ_RPY_New_2.txt', 'w') as f: for row in assembled_data_2: formatted_row = [f"{val:.8f}" for val in row] row_str = '[' + ','.join(formatted_row) + ']' f.write(row_str + '\n') # 初始化存储关节位置的列表 joint_positions_assembled = [] # 第一个位姿对应的关节位置 reference_pos = first_joint_position for i in range(len(assembled_data_2)): # 获取当前位姿 target_pose = assembled_data_2[i].tolist() # 调用逆运动学函数计算关节位置 suc, angle, id = sendCMD(sock, "inverseKinematic", {"targetPose": target_pose, "referencePos": reference_pos}) if suc: # 若计算成功，将结果添加到列表中 joint_positions_assembled.append(angle) # 更新参考位置为本次计算出的关节位置 reference_pos = angle else: print(f"第 {i} 个位姿的逆运动学计算失败: {angle}") # 可以选择将结果保存为数组形式 joint_positions_assembled_array = np.array(joint_positions_assembled) check_joint_continuity(joint_positions_assembled_array) with open('Robot_Joint_Angle_2.txt', 'w') as f: for row in joint_positions_assembled_array: formatted_row = [f"{val:.8f}" for val in row] row_str = '[' + ','.join(formatted_row) + ']' f.write(row_str + '\n') # # 将计算得到的关节位置保存到 txt 文件中 np.savetxt('joint_positions_assembled_2.txt', joint_positions_assembled_array, delimiter=',', fmt='%.6f') print("计算得到的关节位置已保存到 joint_positions_assembled_2.txt 文件中。") # 计算 0.4 秒和 0.2 秒对应的时间步数 steps_0_4s = int(0.4 / dt) steps_0_2s = int(0.2 / dt) # 检查数据长度是否足够 if len(joint_positions_assembled_array) >= steps_0_4s: last_0_4s_joint_pos = joint_positions_assembled_array[-steps_0_4s] print(f"倒数第 0.4 秒的关节位置: {last_0_4s_joint_pos}") else: print("数据长度不足，无法获取倒数第 0.4 秒的关节位置。") if len(joint_positions_assembled_array) >= steps_0_2s: last_0_2s_joint_pos = joint_positions_assembled_array[-steps_0_2s] print(f"倒数第 0.2 秒的关节位置: {last_0_2s_joint_pos}") else: print("数据长度不足，无法获取倒数第 0.2 秒的关节位置。") # 保存到 TXT 文件 with open('last_0_4s_0_2s_joint_positions.txt', 'w') as txt_file: if len(joint_positions_assembled_array) >= steps_0_4s: formatted_0_4s = [f"{val:.8f}" for val in last_0_4s_joint_pos] line_0_4s = f"倒数第 0.4 秒的关节位置: [{' '.join(formatted_0_4s)}]\n" txt_file.write(line_0_4s) if len(joint_positions_assembled_array) >= steps_0_2s: formatted_0_2s = [f"{val:.8f}" for val in last_0_2s_joint_pos] line_0_2s = f"倒数第 0.2 秒的关节位置: [{' '.join(formatted_0_2s)}]\n" txt_file.write(line_0_2s) print("倒数第 0.4 秒和倒数第 0.2 秒的关节位置已保存到 last_0_4s_0_2s_joint_positions.txt 文件中。")