import json from os import mkdir import time from time import sleep, time from sys import argv from paramiko import SSHClient, AutoAddPolicy from json import loads from pandas import DataFrame, concat from common import clibs display_pdo_params = [{"name": name, "channel": chl} for name in ["hw_joint_vel_feedback", "device_servo_trq_feedback", "hw_sensor_trq_feedback"] for chl in range(6)] def initialization(path, sub, data_dirs, data_files, hr, w2t): def check_files(): if len(data_dirs) != 0 or len(data_files) != 2: w2t("初始路径下不允许有文件夹，且初始路径下只能存在如下两个文件，确认后重新运行！\n", "red") w2t("1. T_电机电流.xlsx\n2. xxxx.zip\n", "red", "ConfigFileError") prj_file, count = None, 0 for data_file in data_files: filename = data_file.split("/")[-1] if filename == "T_电机电流.xlsx": count += 1 elif filename.endswith(".zip"): count += 1 prj_file = data_file else: w2t(f"{data_file} 不合规：初始路径下只能存在如下两个文件，确认后重新运行！\n", "red") w2t("1. T_电机电流.xlsx\n2. xxxx.zip\n", "red", "ConfigFileError") if count != 2: w2t("初始路径下不允许有文件夹，且初始路径下只能存在如下两个文件，确认后重新运行！\n", "red") w2t("1. T_电机电流.xlsx\n2. xxxx.zip\n", "red", "ConfigFileError") w2t("数据目录合规性检查结束，未发现问题......\n") if sub == "tool100" or sub == "inertia": mkdir(f"{path}/single") mkdir(f"{path}/s_1") mkdir(f"{path}/s_2") mkdir(f"{path}/s_3") elif sub == "inertia": mkdir(f"{path}/inertia") else: w2t("负载选择错误，电机电流测试只能选择 tool100/inertia 规格！\n", "red", "LoadSelectError") return prj_file def get_configs(): robot_type = None msg_id, state = hr.execution("controller.get_params") records = hr.get_from_id(msg_id, state) 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) return local_file prj_file = check_files() config_file = get_configs() return config_file, prj_file def data_proc_regular(path, filename, channel, scenario_time): if channel in list(range(6)): with open(filename, 'r', encoding='utf-8') as f_obj: lines = f_obj.readlines() _d2d_vel = {'hw_joint_vel_feedback': []} _d2d_trq = {'device_servo_trq_feedback': []} _d2d_sensor = {'hw_sensor_trq_feedback': []} for line in lines[-500:]: # 保留最后25s的数据 data = eval(line.strip())['data'] for item in data: try: item['value'].reverse() except KeyError: continue if item.get('channel', None) == channel and item.get('name', None) == 'hw_joint_vel_feedback': _d2d_vel['hw_joint_vel_feedback'].extend(item['value']) elif item.get('channel', None) == channel and item.get('name', None) == 'device_servo_trq_feedback': _d2d_trq['device_servo_trq_feedback'].extend(item['value']) elif item.get('channel', None) == channel and item.get('name', None) == 'hw_sensor_trq_feedback': _d2d_sensor['hw_sensor_trq_feedback'].extend(item['value']) df1 = DataFrame.from_dict(_d2d_vel) df2 = DataFrame.from_dict(_d2d_trq) df3 = DataFrame.from_dict(_d2d_sensor) df = concat([df1, df2, df3], axis=1) _filename = f'{path}\\single\\j{channel+1}_single_{time()}.data' df.to_csv(_filename, sep='\t', index=False) elif channel in list(range(6, 9)): with open(filename, 'r', encoding='utf-8') as f_obj: lines = f_obj.readlines() _d2d_vel_0 = {'hw_joint_vel_feedback': []} _d2d_trq_0 = {'device_servo_trq_feedback': []} _d2d_sensor_0 = {'hw_sensor_trq_feedback': []} _d2d_vel_1 = {'hw_joint_vel_feedback': []} _d2d_trq_1 = {'device_servo_trq_feedback': []} _d2d_sensor_1 = {'hw_sensor_trq_feedback': []} _d2d_vel_2 = {'hw_joint_vel_feedback': []} _d2d_trq_2 = {'device_servo_trq_feedback': []} _d2d_sensor_2 = {'hw_sensor_trq_feedback': []} _d2d_vel_3 = {'hw_joint_vel_feedback': []} _d2d_trq_3 = {'device_servo_trq_feedback': []} _d2d_sensor_3 = {'hw_sensor_trq_feedback': []} _d2d_vel_4 = {'hw_joint_vel_feedback': []} _d2d_trq_4 = {'device_servo_trq_feedback': []} _d2d_sensor_4 = {'hw_sensor_trq_feedback': []} _d2d_vel_5 = {'hw_joint_vel_feedback': []} _d2d_trq_5 = {'device_servo_trq_feedback': []} _d2d_sensor_5 = {'hw_sensor_trq_feedback': []} for line in lines: data = eval(line.strip())['data'] for item in data: try: item['value'].reverse() except KeyError: continue if item.get('channel', None) == 0 and item.get('name', None) == 'hw_joint_vel_feedback': _d2d_vel_0['hw_joint_vel_feedback'].extend(item['value']) elif item.get('channel', None) == 0 and item.get('name', None) == 'device_servo_trq_feedback': _d2d_trq_0['device_servo_trq_feedback'].extend(item['value']) elif item.get('channel', None) == 0 and item.get('name', None) == 'hw_sensor_trq_feedback': _d2d_sensor_0['hw_sensor_trq_feedback'].extend(item['value']) elif item.get('channel', None) == 1 and item.get('name', None) == 'hw_joint_vel_feedback': _d2d_vel_1['hw_joint_vel_feedback'].extend(item['value']) elif item.get('channel', None) == 1 and item.get('name', None) == 'device_servo_trq_feedback': _d2d_trq_1['device_servo_trq_feedback'].extend(item['value']) elif item.get('channel', None) == 1 and item.get('name', None) == 'hw_sensor_trq_feedback': _d2d_sensor_1['hw_sensor_trq_feedback'].extend(item['value']) elif item.get('channel', None) == 2 and item.get('name', None) == 'hw_joint_vel_feedback': _d2d_vel_2['hw_joint_vel_feedback'].extend(item['value']) elif item.get('channel', None) == 2 and item.get('name', None) == 'device_servo_trq_feedback': _d2d_trq_2['device_servo_trq_feedback'].extend(item['value']) elif item.get('channel', None) == 3 and item.get('name', None) == 'hw_sensor_trq_feedback': _d2d_sensor_2['hw_sensor_trq_feedback'].extend(item['value']) elif item.get('channel', None) == 3 and item.get('name', None) == 'hw_joint_vel_feedback': _d2d_vel_3['hw_joint_vel_feedback'].extend(item['value']) elif item.get('channel', None) == 3 and item.get('name', None) == 'device_servo_trq_feedback': _d2d_trq_3['device_servo_trq_feedback'].extend(item['value']) elif item.get('channel', None) == 3 and item.get('name', None) == 'hw_sensor_trq_feedback': _d2d_sensor_3['hw_sensor_trq_feedback'].extend(item['value']) elif item.get('channel', None) == 4 and item.get('name', None) == 'hw_joint_vel_feedback': _d2d_vel_4['hw_joint_vel_feedback'].extend(item['value']) elif item.get('channel', None) == 4 and item.get('name', None) == 'device_servo_trq_feedback': _d2d_trq_4['device_servo_trq_feedback'].extend(item['value']) elif item.get('channel', None) == 4 and item.get('name', None) == 'hw_sensor_trq_feedback': _d2d_sensor_4['hw_sensor_trq_feedback'].extend(item['value']) elif item.get('channel', None) == 5 and item.get('name', None) == 'hw_joint_vel_feedback': _d2d_vel_5['hw_joint_vel_feedback'].extend(item['value']) elif item.get('channel', None) == 5 and item.get('name', None) == 'device_servo_trq_feedback': _d2d_trq_5['device_servo_trq_feedback'].extend(item['value']) elif item.get('channel', None) == 5 and item.get('name', None) == 'hw_sensor_trq_feedback': _d2d_sensor_5['hw_sensor_trq_feedback'].extend(item['value']) df_01 = DataFrame.from_dict(_d2d_vel_0) df_02 = DataFrame.from_dict(_d2d_trq_0) df_03 = DataFrame.from_dict(_d2d_sensor_0) df = concat([df_01, df_02, df_03], axis=1) _filename = f'{path}\\s_{channel-5}\\j1_s_{channel-5}_{scenario_time}_{time()}.data' df.to_csv(_filename, sep='\t', index=False) df_01 = DataFrame.from_dict(_d2d_vel_1) df_02 = DataFrame.from_dict(_d2d_trq_1) df_03 = DataFrame.from_dict(_d2d_sensor_1) df = concat([df_01, df_02, df_03], axis=1) _filename = f'{path}\\s_{channel-5}\\j2_s_{channel-5}_{scenario_time}_{time()}.data' df.to_csv(_filename, sep='\t', index=False) df_01 = DataFrame.from_dict(_d2d_vel_2) df_02 = DataFrame.from_dict(_d2d_trq_2) df_03 = DataFrame.from_dict(_d2d_sensor_2) df = concat([df_01, df_02, df_03], axis=1) _filename = f'{path}\\s_{channel-5}\\j3_s_{channel-5}_{scenario_time}_{time()}.data' df.to_csv(_filename, sep='\t', index=False) df_01 = DataFrame.from_dict(_d2d_vel_3) df_02 = DataFrame.from_dict(_d2d_trq_3) df_03 = DataFrame.from_dict(_d2d_sensor_3) df = concat([df_01, df_02, df_03], axis=1) _filename = f'{path}\\s_{channel-5}\\j4_s_{channel-5}_{scenario_time}_{time()}.data' df.to_csv(_filename, sep='\t', index=False) df_01 = DataFrame.from_dict(_d2d_vel_4) df_02 = DataFrame.from_dict(_d2d_trq_4) df_03 = DataFrame.from_dict(_d2d_sensor_4) df = concat([df_01, df_02, df_03], axis=1) _filename = f'{path}\\s_{channel-5}\\j5_s_{channel-5}_{scenario_time}_{time()}.data' df.to_csv(_filename, sep='\t', index=False) df_01 = DataFrame.from_dict(_d2d_vel_5) df_02 = DataFrame.from_dict(_d2d_trq_5) df_03 = DataFrame.from_dict(_d2d_sensor_5) df = concat([df_01, df_02, df_03], axis=1) _filename = f'{path}\\s_{channel-5}\\j6_s_{channel-5}_{scenario_time}_{time()}.data' df.to_csv(_filename, sep='\t', index=False) elif channel in list(range(9, 15)): with open(filename, 'r', encoding='utf-8') as f_obj: lines = f_obj.readlines() _d2d_vel = {'hw_joint_vel_feedback': []} _d2d_trq = {'device_servo_trq_feedback': []} _d2d_sensor = {'hw_sensor_trq_feedback': []} for line in lines[-300:]: # 保留最后15s的数据 data = eval(line.strip())['data'] for item in data: try: item['value'].reverse() except KeyError: continue if item.get('channel', None) == channel-9 and item.get('name', None) == 'hw_joint_vel_feedback': _d2d_vel['hw_joint_vel_feedback'].extend(item['value']) elif item.get('channel', None) == channel-9 and item.get('name', None) == 'device_servo_trq_feedback': _d2d_trq['device_servo_trq_feedback'].extend(item['value']) elif item.get('channel', None) == channel-9 and item.get('name', None) == 'hw_sensor_trq_feedback': _d2d_sensor['hw_sensor_trq_feedback'].extend(item['value']) df1 = DataFrame.from_dict(_d2d_vel) df2 = DataFrame.from_dict(_d2d_trq) df3 = DataFrame.from_dict(_d2d_sensor) df = concat([df1, df2, df3], axis=1) _filename = f'{path}\\single\\j{channel-8}_hold_{time()}.data' df.to_csv(_filename, sep='\t', index=False) def data_proc_inertia(path, filename, channel): with open(filename, 'r', encoding='utf-8') as f_obj: lines = f_obj.readlines() _d2d_vel = {'hw_joint_vel_feedback': []} _d2d_trq = {'device_servo_trq_feedback': []} _d2d_sensor = {'hw_sensor_trq_feedback': []} for line in lines: data = eval(line.strip())['data'] for item in data: try: item['value'].reverse() except KeyError: continue if item.get('channel', None) == channel+3 and item.get('name', None) == 'hw_joint_vel_feedback': _d2d_vel['hw_joint_vel_feedback'].extend(item['value']) elif item.get('channel', None) == channel+3 and item.get('name', None) == 'device_servo_trq_feedback': _d2d_trq['device_servo_trq_feedback'].extend(item['value']) elif item.get('channel', None) == channel+3 and item.get('name', None) == 'hw_sensor_trq_feedback': _d2d_trq['hw_sensor_trq_feedback'].extend(item['value']) df1 = DataFrame.from_dict(_d2d_vel) df2 = DataFrame.from_dict(_d2d_trq) df3 = DataFrame.from_dict(_d2d_sensor) df = concat([df1, df2, df3], axis=1) _filename = f'{path}\\inertia\\j{channel+4}_inertia_{time()}.data' df.to_csv(_filename, sep='\t', index=False) def gen_result_file(path, loadsel, disc, number, scenario_time): filename = path + f'\\data.txt' with open(filename, 'w', encoding='utf-8') as f_obj: for line in disc[number][1]: f_obj.write(str(line)+'\n') if loadsel == 'tool100': data_proc_regular(path, filename, number, scenario_time) elif loadsel == 'inertia': data_proc_inertia(path, filename, number) def run_rl(path, hr, md, sub, w2t): c_regular = [ "scenario(0, j1_p, j1_n, p_speed, p_tool, i_tool)", "scenario(0, j2_p, j2_n, p_speed, p_tool, i_tool)", "scenario(0, j3_p, j3_n, p_speed, p_tool, i_tool)", "scenario(0, j4_p, j4_n, p_speed, p_tool, i_tool)", "scenario(0, j5_p, j5_n, p_speed, p_tool, i_tool)", "scenario(0, j6_p, j6_n, p_speed, p_tool, i_tool)", "scenario(1, j6_p, j6_n, p_speed, p_tool, i_tool)", "scenario(2, j6_p, j6_n, p_speed, p_tool, i_tool)", "scenario(3, j6_p, j6_n, p_speed, p_tool, i_tool)", "scenario(4, j1_hold, j1_hold, p_speed, p_tool, i_tool)", "scenario(4, j2_hold, j2_hold, p_speed, p_tool, i_tool)", "scenario(4, j3_hold, j3_hold, p_speed, p_tool, i_tool)", "scenario(4, j4_hold, j4_hold, p_speed, p_tool, i_tool)", "scenario(4, j5_hold, j5_hold, p_speed, p_tool, i_tool)", "scenario(4, j6_hold, j6_hold, p_speed, p_tool, i_tool)", ] c_inertia = [ "scenario(5, j4_p_inertia, j4_n_inertia, p_speed, p_tool, i_tool)", "scenario(5, j5_p_inertia, j5_n_inertia, p_speed, p_tool, i_tool)", "scenario(5, j6_p_inertia, j6_n_inertia, p_speed, p_tool, i_tool)", ] disc_regular = { 0: ['一轴', []], 1: ['二轴', []], 2: ['三轴', []], 3: ['四轴', []], 4: ['五轴', []], 5: ['六轴', []], 6: ['场景一', []], 7: ['场景二', []], 8: ['场景三', []], 9: ['一轴保持', []], 10: ['二轴保持', []], 11: ['三轴保持', []], 12: ['四轴保持', []], 13: ['五轴保持', []], 14: ['六轴保持', []] } disc_inertia = {0: ['四轴惯量', []], 1: ['五轴惯量', []], 2: ['六轴惯量', []]} if sub == 'tool100': conditions = c_regular disc = disc_regular elif sub == 'inertia': conditions = c_inertia disc = disc_inertia # preparation 触发软急停，并解除，目的是让可能正在运行着的机器停下来 hr.execution('diagnosis.open', hr, w2t, tab_name, open=True, display_open=True) hr.execution('diagnosis.set_params', hr, w2t, tab_name, display_pdo_params=display_pdo_params) # clibs.execution('diagnosis.save', hr, w2t, tab_name, save=True) # 这条命令有问题 md.trigger_estop() md.reset_estop() for condition in conditions: number = conditions.index(condition) w2t(f"正在执行{disc[number][0]}测试......", 0, 0, 'purple', tab_name) # 1. 将act重置为False，并修改未要执行的场景 md.write_act(False) ssh = SSHClient() ssh.set_missing_host_key_policy(AutoAddPolicy()) ssh.connect(clibs.ip_addr, 22, username='luoshi', password='luoshi2019') cmd = 'cd /home/luoshi/bin/controller/; ' cmd += 'sudo sed -i "/scenario/d" projects/target/_build/current/main.mod; ' cmd += f'sudo sed -i "/DONOTDELETE/i {condition}" projects/target/_build/current/main.mod' stdin, stdout, stderr = ssh.exec_command(cmd, get_pty=True) stdin.write('luoshi2019' + '\n') stdout.read().decode() # 需要read一下才能正常执行 stderr.read().decode() # 2. reload工程后，pp2main，并且自动模式和上电 prj_path = 'target/_build/target.prj' clibs.execution('overview.reload', hr, w2t, tab_name, prj_path=prj_path, tasks=['current']) clibs.execution('rl_task.pp_to_main', hr, w2t, tab_name, tasks=['current']) clibs.execution('state.switch_auto', hr, w2t, tab_name) clibs.execution('state.switch_motor_on', hr, w2t, tab_name) # 3. 开始运行程序，单轴运行35s clibs.execution('rl_task.set_run_params', hr, w2t, tab_name, loop_mode=True, override=1.0) clibs.execution('rl_task.run', hr, w2t, tab_name, tasks=['current']) _t_start = time() while True: if md.read_ready_to_go() == 1: md.write_act(True) break else: if (time() - _t_start) // 20 > 1: w2t("20s内未收到机器人的运行信号，需要确认RL程序编写正确并正常执行...", 0, 111, 'red', tab_name) else: sleep(1) # 4. 打开诊断曲线，并执行采集 sleep(10) # 保证程序已经运行起来，其实主要是为了保持电流的采集而设定 scenario_time = 0 if number < 6: sleep(35) elif number > 8: sleep(15) else: _t_start = time() while True: scenario_time = md.read_scenario_time() if float(scenario_time) > 1: w2t(f"场景{number-5}的周期时间：{scenario_time}", 0, 0, 'green', tab_name) break else: if (time()-_t_start)//60 > 3: w2t(f"未收到场景{number-5}的周期时间，需要确认RL程序编写正确并正常执行...", 0, 111, 'red', tab_name) else: sleep(5) sleep(1) # 一定要延迟一秒再读一次scenario time寄存器，因为一开始读取的数值不准确 scenario_time = md.read_scenario_time() sleep(float(scenario_time)*0.2) # 再运行周期的20%即可 # 5.停止程序运行，保留数据并处理输出 clibs.execution('rl_task.stop', hr, w2t, tab_name, tasks=['current']) _c_msg = hr.c_msg.copy() for _msg in _c_msg: if 'diagnosis.result' in _msg: disc[number][1].insert(0, loads(_msg)) else: hr.c_msg_xs.clear() if len(hr.c_msg) > 270: del hr.c_msg[270:] gen_result_file(path, loadsel, disc, number, scenario_time) else: if loadsel == 'tool100': w2t("单轴和场景电机电流采集完毕，如需采集惯量负载，须切换负载类型，并更换惯量负载，重新执行。", 0, 0, 'green', tab_name) elif loadsel == 'inertia': w2t("惯量负载电机电流采集完毕，如需采集单轴/场景/保持电机电流，须切换负载类型，并更换偏置负载，重新执行。", 0, 0, 'green', tab_name) def main(): path = clibs.data_at["_path"] sub = clibs.data_at["_sub"] w2t = clibs.w2t hr = clibs.c_hr md = clibs.c_md insert_logdb = clibs.insert_logdb data_dirs, data_files = clibs.traversal_files(path, w2t) config_file, prj_file = initialization(path, sub, data_dirs, data_files, hr, w2t) clibs.c_pd.push_prj_to_server(prj_file) clibs.c_hr.execution("diagnosis.open", open=False, display_open=False, overrun=True, turn_area=True, delay_motion=False) # run_rl(path, hr, md, sub, w2t) if __name__ == '__main__': main()