def generate_jbi_file(file_num):
    try:
        # 读取关节角度文件
        joint_angle_file = f'Robot_Joint_Angle_{file_num}.txt'
        with open(joint_angle_file, 'r') as f1:
            first_line = f1.readline().strip()
            ref_joint_pos_str = first_line if first_line.startswith('[') and first_line.endswith(']') else f'[{first_line}]'

        # 读取位置姿态值文件
        pose_file = f'Pose_XYZ_RPY_New_{file_num}.txt'
        with open(pose_file, 'r') as f2:
            pose_values = f2.readlines()
            length = len(pose_values)
            pose_str = ''.join([line.strip() + '\n' for line in pose_values])

        # .jbi 文件模板
        template = """//trajfile
version2.15.0
interval [5]ms
refJointPos {ref_joint_pos}
length [{length}]
postype [pose]
outputNumber [3]
refFrame [0, 0, 0, 0, 0, 0]
{pose_str}"""

        # 填充模板
        output_content = template.format(ref_joint_pos=ref_joint_pos_str, length=length, pose_str=pose_str)

        # 生成 .jbi 文件
        output_filename = f'output_{file_num}.jbi'
        with open(output_filename, 'w') as f_out:
            f_out.write(output_content)
        print(f"新的 .jbi 文件已生成：{output_filename}")

    except FileNotFoundError:
        print(f"未找到 {joint_angle_file} 或 {pose_file} 文件，请检查文件路径。")
    except IndexError:
        print(f"{joint_angle_file} 文件为空，请检查文件内容。")

# 处理第一组文件
generate_jbi_file(1)
# 处理第二组文件
generate_jbi_file(2)