完成了电流->转矩的转变，基本完成

code/durable_docs/create_plot.py

@@ -0,0 +1,82 @@
+import os.path
+import matplotlib.pyplot as plt
+import pandas
+from matplotlib.widgets import Slider
+from common import clibs
+
+
+def initialization():
+    path, curves = None, None
+    try:
+        path = clibs.data_dd["path"]
+        curves = clibs.data_dd["curves"]
+    except Exception:
+        clibs.w2t("程序未开始运行，暂无数据可以展示......\n", "red")
+        return None, None
+
+    for curve in curves:
+        if not os.path.exists(f"{path}/{curve}.csv"):
+            clibs.w2t(f"{curve}曲线数据暂未生成，稍后再试......\n", "orange")
+            return None, None
+
+    return path, curves
+
+
+def data_plot(path, curve):
+    titles = {"hw_joint_vel_feedback": "各关节最大速度曲线", "device_servo_trq_feedback": "各关节平均有效转矩曲线"}
+    ylabels = {"hw_joint_vel_feedback": "速度(rad/s)", "device_servo_trq_feedback": "转矩(Nm)"}
+
+    fig, axes = plt.subplots(figsize=(10, 4.5), dpi=100)
+    cols = [f"{curve}_{i}" for i in range(6)]
+    cols.insert(0, "time")
+    df = pandas.read_csv(f"{path}/{curve}.csv")
+    plt.plot(df[cols[1]], label="一轴")
+    plt.plot(df[cols[2]], label="二轴")
+    plt.plot(df[cols[3]], label="三轴")
+    plt.plot(df[cols[4]], label="四轴")
+    plt.plot(df[cols[5]], label="五轴")
+    plt.plot(df[cols[6]], label="六轴")
+    axes.set_title(titles[curve])
+    axes.set_ylabel(ylabels[curve])
+    axes.legend(loc="upper right")
+
+    slider_position = plt.axes((0.1, 0.01, 0.8, 0.05), facecolor="blue")  # (left, bottom, width, height)
+    scrollbar = Slider(slider_position, 'Time', 1, int(len(df)), valstep=1)
+
+    def update(val):
+        pos = scrollbar.val
+        axes.set_xlim([pos, pos + 10])
+        fig.canvas.draw_idle()
+
+    scrollbar.on_changed(update)
+    fig.tight_layout(rect=(0, 0.02, 0.96, 1))  # tuple (left, bottom, right, top)
+
+
+def main():
+    path, curves = initialization()
+    if not path or not curves:
+        return
+
+    for curve in curves:
+        data_plot(path, curve)
+
+    plt.rcParams['font.sans-serif'] = ['SimHei']
+    plt.rcParams['axes.unicode_minus'] = False
+    plt.rcParams['figure.dpi'] = 100
+    plt.rcParams['font.size'] = 14
+    plt.rcParams['lines.marker'] = 'o'
+    plt.rcParams["figure.autolayout"] = True
+    plt.show()
+
+    # threads = [threading.Thread(target=data_plot, args=(path, curve)) for curve in curves]
+    # for t in threads:
+    #     t.daemon = True
+    #     t.start()
+    # for curve in curves:
+    #     t = threading.Thread(target=data_plot, args=(path, curve))
+    #     t.daemon = True
+    #     t.start()
+
+
+if __name__ == '__main__':
+    main()

code/durable_docs/factory_test.py

@@ -0,0 +1,271 @@
+import json
+import threading
+import time
+import pandas
+import math
+import csv
+import numpy
+from common import clibs
+
+
+def initialization(path, hr, data_dirs, data_files, interval, curves, w2t):
+    def check_files():
+        nonlocal interval
+        if interval == "":
+            interval = 300
+        elif interval.isdigit():
+            if int(interval) < 300:
+                w2t(f"输入时间间隔 {interval} < 300，使用默认时间间隔 300s ......\n", "orange")
+                interval = 300
+            else:
+                interval = int(interval)
+        else:
+            w2t(f"{interval} 不是有效的输入，时间间隔必须是一个大于 300 的正整数！\n", "red", "NotIntegerError")
+
+        if len(curves) == 0:
+            w2t("未查询到需要记录数据的曲线，至少选择一个！\n", "red", "CurveNameError")
+
+        if len(data_dirs) != 0 or len(data_files) != 1:
+            w2t("初始路径下不允许有文件夹，且初始路径下只能存在一个工程文件 —— *.zip，确认后重新运行！\n", "red", "InitFileError")
+
+        if not data_files[0].endswith(".zip"):
+            w2t(f"{data_files[0]} 不是一个有效的工程文件，需确认！\n", "red", "ProjectFileError")
+
+        return data_files[0], interval
+
+    def get_configs():
+        robot_type, records = None, None
+        try:
+            msg_id, state = hr.execution("controller.get_params")
+            records = hr.get_from_id(msg_id, state)
+        except Exception:
+            w2t("网络不可达，需要先连接xCore！\n", "red", "NetworkError")
+        for record in records:
+            if "请求发送成功" not in record[0]:
+                robot_type = eval(record[0])["data"]["robot_type"]
+        server_file = f"/home/luoshi/bin/controller/robot_cfg/{robot_type}/{robot_type}.cfg"
+        local_file = path + f"/{robot_type}.cfg"
+        clibs.c_pd.pull_file_from_server(server_file, local_file)
+
+        try:
+            with open(local_file, mode="r", encoding="utf-8") as f_config:
+                configs = json.load(f_config)
+        except Exception as Err:
+            clibs.insert_logdb("ERROR", "current", f"get_config: 无法打开 {local_file}，获取配置文件参数错误 {Err}")
+            w2t(f"无法打开 {local_file}\n", color="red", desc="OpenFileError")
+
+        # 最大角速度，额定电流，减速比，额定转速
+        version = configs["VERSION"]
+        m_avs = configs["MOTION"]["JOINT_MAX_SPEED"]
+        m_rts = configs["MOTOR"]["RATED_TORQUE"]  # 电机额定转矩rt for rated torque
+        m_tcs = [1, 1, 1, 1, 1, 1]  # 电机转矩常数，tc for torque constant
+        m_rcs = []
+        for i in range(len(m_tcs)):
+            m_rcs.append(m_rts[i] / m_tcs[i])  # 电机额定电流，rc for rated current
+        clibs.insert_logdb("INFO", "do_brake", f"get_configs: 机型文件版本 {robot_type}_{version}")
+        clibs.insert_logdb("INFO", "do_brake", f"get_configs: 各关节角速度 {m_avs}")
+        clibs.insert_logdb("INFO", "do_brake", f"get_configs: 各关节额定电流 {m_rcs}")
+        return m_avs, m_rcs
+
+    prj_file, interval = check_files()
+    avs, rcs = get_configs()
+    params = {
+        "prj_file": prj_file,
+        "interval": interval,
+        "avs": avs,
+        "rcs": rcs,
+    }
+    return params
+
+
+def change_curve_state(hr, curves, stat_1, stat_2):
+    display_pdo_params = [{"name": name, "channel": chl} for name in curves for chl in range(6)]
+    hr.execution("diagnosis.open", open=stat_1, display_open=stat_2, overrun=True, turn_area=True, delay_motion=False)
+    hr.execution("diagnosis.set_params", display_pdo_params=display_pdo_params, frequency=50, version="1.4.1")
+
+
+def run_rl(path, params, curves, hr, md, w2t):
+    prj_file, interval = params["prj_file"], params["interval"]
+    # 1. 关闭诊断曲线，触发软急停，并解除，目的是让可能正在运行着的机器停下来，切手动模式并下电
+    change_curve_state(hr, curves, False, False)
+    md.r_soft_estop(0)
+    md.r_soft_estop(1)
+    md.r_clear_alarm()
+    md.write_act(False)
+    time.sleep(1)  # 让曲线彻底关闭
+
+    # 2. reload工程后，pp2main，并且自动模式和上电
+    prj_name = ".".join(prj_file.split("/")[-1].split(".")[:-1])
+    prj_path = f"{prj_name}/_build/{prj_name}.prj"
+    hr.execution("overview.reload", prj_path=prj_path, tasks=["factory"])
+    hr.execution("rl_task.pp_to_main", tasks=["factory"])
+    hr.execution("state.switch_auto")
+    hr.execution("state.switch_motor_on")
+
+    # 3. 开始运行程序
+    hr.execution("rl_task.set_run_params", loop_mode=True, override=1.0)
+    hr.execution("rl_task.run", tasks=["factory"])
+    t_start = time.time()
+    while True:
+        if md.read_ready_to_go() == 1:
+            md.write_act(True)
+            break
+        else:
+            if (time.time() - t_start) > 3:
+                w2t("3s 内未收到机器人的运行信号，需要确认RL程序编写正确并正常执行...\n", "red", "ReadySignalTimeoutError")
+            else:
+                time.sleep(1)
+
+    # 4. 获取初始数据，周期时间，首次的各轴平均电流值，打开诊断曲线，并执行采集
+    time.sleep(10)  # 等待 RL 程序中 scenario_time 初始化
+    t_start = time.time()
+    while True:
+        scenario_time = float(f"{float(md.read_scenario_time()):.2f}")
+        if scenario_time != 0:
+            w2t(f"耐久工程的周期时间：{scenario_time}s | 单轮次执行时间：{scenario_time+interval}\n")
+            break
+        else:
+            time.sleep(1)
+            if (time.time() - t_start) > 300:
+                w2t(f"300s 内未收到耐久工程的周期时间，需要确认RL程序和工具通信交互是否正常执行...\n", "red", "GetScenarioTimeError")
+
+    # 6. 准备数据保存文件
+    for curve in curves:
+        with open(f"{path}/{curve}.csv", mode="a+", newline="") as f_csv:
+            titles = [f"{curve}_{i}" for i in range(6)]
+            titles.insert(0, "time")
+            csv_writer = csv.writer(f_csv)
+            csv_writer.writerow(titles)
+
+    # 7. 开始采集
+    count = 0
+    while clibs.running:
+        if not clibs.running:
+            w2t("后台数据清零完成，现在可以重新运行之前停止的程序。", "red")
+            exit()
+
+        this_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(time.time()))
+        next_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(time.time()+scenario_time+interval+1))
+        w2t(f"[{this_time}] 当前次数：{count:09d}  |  预计下次数据更新时间：{next_time}\n", "#008B8B")
+        count += 1
+        # 固定间隔，更新一次数据，打开曲线，获取周期内电流，关闭曲线
+        time.sleep(interval)
+        change_curve_state(hr, curves, True, True)
+        time.sleep(scenario_time)
+        end_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(time.time()))
+        start_time = time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(time.time()-scenario_time))
+        change_curve_state(hr, curves, False, False)
+        # 保留数据并处理输出
+        gen_results(params, curves, start_time, end_time, w2t)
+
+
+def gen_results(params, curves, start_time, end_time, w2t):
+    try:
+        clibs.lock.acquire(True)
+        clibs.cursor.execute(f"select content from logs where time between '{start_time}' and '{end_time}' and content like '%diagnosis.result%' order by id asc")
+        records = clibs.cursor.fetchall()
+    finally:
+        clibs.lock.release()
+    data_proc(records, params, curves, w2t)
+
+
+def data_proc(records, params, curves, w2t):
+    for curve in curves:
+        if curve == "device_servo_trq_feedback":
+            # proc_device_servo_trq_feedback(records, params, w2t)
+            t = threading.Thread(target=proc_device_servo_trq_feedback, args=(records, params, w2t))
+            t.daemon = True
+            t.start()
+        elif curve == "hw_joint_vel_feedback":
+            # proc_hw_joint_vel_feedback(records, params, w2t)
+            t = threading.Thread(target=proc_hw_joint_vel_feedback, args=(records, params, w2t))
+            t.daemon = True
+            t.start()
+
+
+def proc_device_servo_trq_feedback(records, params, w2t):
+    d_trq, rcs, results = [[], [], [], [], [], []], params["rcs"], [time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(time.time()))]
+    for record in records:
+        data = eval(record[0])["data"]
+        for item in data:
+            d_item = reversed(item["value"])
+            for axis in range(6):
+                if item.get("channel", None) == axis and item.get("name", None) == "device_servo_trq_feedback":
+                    d_trq[axis].extend(d_item)
+
+    for axis in range(6):
+        df = pandas.DataFrame.from_dict({"device_servo_trq_feedback": d_trq[axis]})
+        _ = math.sqrt(df.apply(lambda x: numpy.power((rcs[axis] * float(x.iloc[0]) / 1000), 2)).sum() / len(df))
+        results.append(_)
+
+    path = "/".join(params["prj_file"].split("/")[:-1])
+    with open(f"{path}/device_servo_trq_feedback.csv", mode="a+", newline="") as f_csv:
+        csv_writer = csv.writer(f_csv)
+        csv_writer.writerow(results)
+
+
+def proc_hw_joint_vel_feedback(records, params, w2t):
+    d_trq, rcs, results = [[], [], [], [], [], []], params["rcs"], [time.strftime("%Y-%m-%d %H:%M:%S", time.localtime(time.time()))]
+    for record in records:
+        data = eval(record[0])["data"]
+        for item in data:
+            d_item = reversed(item["value"])
+            for axis in range(6):
+                if item.get("channel", None) == axis and item.get("name", None) == "hw_joint_vel_feedback":
+                    d_trq[axis].extend(d_item)
+
+    for axis in range(6):
+        df = pandas.DataFrame.from_dict({"hw_joint_vel_feedback": d_trq[axis]})
+        _ = df.max().iloc[0]
+        results.append(_)
+
+    path = "/".join(params["prj_file"].split("/")[:-1])
+    with open(f"{path}/hw_joint_vel_feedback.csv", mode="a+", newline="") as f_csv:
+        csv_writer = csv.writer(f_csv)
+        csv_writer.writerow(results)
+
+
+def detect_db_size():
+    @clibs.db_lock
+    def release_memory():
+        line_number = 20000
+        leftover = 4000  # 200s
+        clibs.cursor.execute("select count(id) from logs")
+        len_records = clibs.cursor.fetchone()[0]
+        if len_records > line_number:
+            del_num = len_records - leftover + 1
+            clibs.cursor.execute(f"delete from logs where id < {del_num}")
+            clibs.cursor.execute(f"update logs set id=(id-{del_num-1}) where id > {del_num-1}")
+            clibs.cursor.execute(f"update sqlite_sequence set seq = {leftover+1} WHERE name = 'logs' ")
+            clibs.cursor.execute("vacuum")
+
+    while True:
+        release_memory()
+        time.sleep(1)
+
+
+def main():
+    t = threading.Thread(target=detect_db_size)
+    t.daemon = True
+    t.start()
+
+    path = clibs.data_dd["path"]
+    interval = clibs.data_dd["interval"].strip()
+    curves = clibs.data_dd["curves"]
+    hr = clibs.c_hr
+    md = clibs.c_md
+    w2t = clibs.w2t
+
+    data_dirs, data_files = clibs.traversal_files(path, w2t)
+    params = initialization(path, hr, data_dirs, data_files, interval, curves, w2t)
+    prj_file = params["prj_file"]
+    clibs.c_pd.push_prj_to_server(prj_file)
+    try:
+        run_rl(path, params, curves, hr, md, w2t)
+    except Exception as Err:
+        w2t(f"工厂耐久程序执行过程中出现异常，{Err}\n", "red")
+        change_curve_state(hr, curves, False, False)
+
+
+if __name__ == "__main__":
+    main()