尝试在RL里判断最大速度，尝试失败
This commit is contained in:
parent
2fbe500d1d
commit
9fa42fb3e1
@ -1,3 +1,4 @@
|
||||
import random
|
||||
from time import sleep, time
|
||||
from sys import argv
|
||||
from os import scandir, mkdir
|
||||
@ -149,13 +150,30 @@ def gen_result_file(path, curve_data, axis, _reach, _load, _speed, count):
|
||||
|
||||
def run_rl(path, loadsel, hr, md, config_file, prj_file, result_dirs, w2t):
|
||||
_count = 0
|
||||
speed_max = 0
|
||||
display_pdo_params = [
|
||||
{"name": "hw_joint_vel_feedback", "channel": 0},
|
||||
{"name": "hw_joint_vel_feedback", "channel": 1},
|
||||
{"name": "hw_joint_vel_feedback", "channel": 2},
|
||||
{"name": "hw_joint_vel_feedback", "channel": 3},
|
||||
{"name": "hw_joint_vel_feedback", "channel": 4},
|
||||
{"name": "hw_joint_vel_feedback", "channel": 5},
|
||||
{"name": "device_servo_trq_feedback", "channel": 0},
|
||||
{"name": "device_servo_trq_feedback", "channel": 1},
|
||||
{"name": "device_servo_trq_feedback", "channel": 2},
|
||||
{"name": "device_servo_trq_feedback", "channel": 3},
|
||||
{"name": "device_servo_trq_feedback", "channel": 4},
|
||||
{"name": "device_servo_trq_feedback", "channel": 5},
|
||||
{"name": "device_safety_estop", "channel": 0},
|
||||
]
|
||||
for condition in result_dirs:
|
||||
_reach = condition.split('_')[0].removeprefix('reach')
|
||||
_load = condition.split('_')[1].removeprefix('load')
|
||||
_speed = condition.split('_')[2].removeprefix('speed')
|
||||
|
||||
for axis in range(1, 4):
|
||||
_get_speed_max = 0
|
||||
md.write_axis(axis)
|
||||
speed_max = 0
|
||||
if axis == 3 and _reach != '100':
|
||||
continue
|
||||
for count in range(1, 4):
|
||||
@ -163,39 +181,37 @@ def run_rl(path, loadsel, hr, md, config_file, prj_file, result_dirs, w2t):
|
||||
w2t(f"[{_count}/64-{count}] 正在执行{axis}轴{condition}的制动测试......", 0, 0, 'purple', 'Automatic Test')
|
||||
|
||||
# 1. 关闭诊断曲线，触发软急停，并解除，目的是让可能正在运行着的机器停下来，切手动模式并下电
|
||||
_response = execution('diagnosis.open', hr, w2t, open=False, display_open=False)
|
||||
md.set_stop0(1)
|
||||
md.trigger_estop()
|
||||
md.reset_estop()
|
||||
_response = execution('diagnosis.open', hr, w2t, open=False, display_open=False)
|
||||
sleep(2) # 让曲线彻底关闭
|
||||
_response = execution('state.switch_manual', hr, w2t)
|
||||
_response = execution('state.switch_motor_off', hr, w2t)
|
||||
|
||||
# 2. 修改未要执行的场景
|
||||
ssh = SSHClient()
|
||||
ssh.set_missing_host_key_policy(AutoAddPolicy())
|
||||
ssh.connect('192.168.0.160', 22, username='luoshi', password='luoshi2019')
|
||||
_rl_cmd = f"brake_E(j{axis}_{_reach}_p, j{axis}_{_reach}_n, p_speed, p_tool)"
|
||||
_rl_speed = f"VelSet {_speed}"
|
||||
cmd = 'cd /home/luoshi/bin/controller/; '
|
||||
cmd += 'sudo sed -i "/brake_E/d" projects/target/_build/brake/main.mod; '
|
||||
cmd += f'sudo sed -i "/DONOTDELETE/i {_rl_cmd}" projects/target/_build/brake/main.mod; '
|
||||
cmd += f'sudo sed -i "/VelSet/d" projects/target/_build/brake/main.mod; '
|
||||
cmd += f'sudo sed -i "/MoveAbsJ/i {_rl_speed}" projects/target/_build/brake/main.mod; '
|
||||
stdin, stdout, stderr = ssh.exec_command(cmd, get_pty=True)
|
||||
stdin.write('luoshi2019' + '\n')
|
||||
stdin.flush()
|
||||
print(stdout.read().decode()) # 必须得输出一下stdout，才能正确执行sudo
|
||||
print(stderr.read().decode()) # 顺便也执行以下stderr
|
||||
if count == 1:
|
||||
ssh = SSHClient()
|
||||
ssh.set_missing_host_key_policy(AutoAddPolicy())
|
||||
ssh.connect('192.168.0.160', 22, username='luoshi', password='luoshi2019')
|
||||
_rl_cmd = f"brake_E(j{axis}_{_reach}_p, j{axis}_{_reach}_n, p_speed, p_tool)"
|
||||
_rl_speed = f"VelSet {_speed}"
|
||||
cmd = 'cd /home/luoshi/bin/controller/; '
|
||||
cmd += 'sudo sed -i "/brake_E/d" projects/target/_build/brake/main.mod; '
|
||||
cmd += f'sudo sed -i "/DONOTDELETE/i {_rl_cmd}" projects/target/_build/brake/main.mod; '
|
||||
cmd += f'sudo sed -i "/VelSet/d" projects/target/_build/brake/main.mod; '
|
||||
cmd += f'sudo sed -i "/MoveAbsJ/i {_rl_speed}" projects/target/_build/brake/main.mod; '
|
||||
stdin, stdout, stderr = ssh.exec_command(cmd, get_pty=True)
|
||||
stdin.write('luoshi2019' + '\n')
|
||||
stdin.flush()
|
||||
print(stdout.read().decode()) # 必须得输出一下stdout，才能正确执行sudo
|
||||
print(stderr.read().decode()) # 顺便也执行以下stderr
|
||||
|
||||
# 3. reload工程后，pp2main，并且自动模式和上电
|
||||
# 3. reload工程后，pp2main，并且自动模式和上电，最后运行程序
|
||||
prj_path = 'target/_build/target.prj'
|
||||
_response = execution('overview.reload', hr, w2t, prj_path=prj_path, tasks=['brake', 'stop0_related'])
|
||||
_response = execution('overview.get_cur_prj', hr, w2t)
|
||||
_response = execution('rl_task.pp_to_main', hr, w2t, tasks=['brake', 'stop0_related'])
|
||||
_response = execution('state.switch_auto', hr, w2t)
|
||||
_response = execution('state.switch_motor_on', hr, w2t)
|
||||
|
||||
# 4. 第一次开始运行程序，运行两轮
|
||||
_response = execution('rl_task.run', hr, w2t, tasks=['brake', 'stop0_related'])
|
||||
for i in range(3):
|
||||
if md.read_ready_to_go() == 1:
|
||||
@ -208,33 +224,16 @@ def run_rl(path, loadsel, hr, md, config_file, prj_file, result_dirs, w2t):
|
||||
else:
|
||||
w2t("未收到机器人的运行信号，需要确认RL程序编写正确并正常执行...", 0, 111, 'red', 'Automatic Test')
|
||||
|
||||
if _get_speed_max == 0:
|
||||
# 5. 第一次打开诊断曲线，并执行采集
|
||||
_get_speed_max = 1
|
||||
# 4. 第一次打开诊断曲线，并执行采集8s，之后触发软急停，关闭曲线采集，找出最大速度，传递给RL程序，最后清除相关记录
|
||||
if count == 1:
|
||||
sleep(4) # 排除从零点位置到目标位置的干扰
|
||||
_response = execution('diagnosis.open', hr, w2t, open=True, display_open=True)
|
||||
display_pdo_params = [
|
||||
{"name": "hw_joint_vel_feedback", "channel": 0},
|
||||
{"name": "hw_joint_vel_feedback", "channel": 1},
|
||||
{"name": "hw_joint_vel_feedback", "channel": 2},
|
||||
{"name": "hw_joint_vel_feedback", "channel": 3},
|
||||
{"name": "hw_joint_vel_feedback", "channel": 4},
|
||||
{"name": "hw_joint_vel_feedback", "channel": 5},
|
||||
{"name": "device_servo_trq_feedback", "channel": 0},
|
||||
{"name": "device_servo_trq_feedback", "channel": 1},
|
||||
{"name": "device_servo_trq_feedback", "channel": 2},
|
||||
{"name": "device_servo_trq_feedback", "channel": 3},
|
||||
{"name": "device_servo_trq_feedback", "channel": 4},
|
||||
{"name": "device_servo_trq_feedback", "channel": 5},
|
||||
{"name": "device_safety_estop", "channel": 0},
|
||||
]
|
||||
_response = execution('diagnosis.set_params', hr, w2t, display_pdo_params=display_pdo_params)
|
||||
sleep(8) # 前八秒获取实际最大速度
|
||||
|
||||
# 6. 关闭诊断曲线，并处理数据，找出最大速度
|
||||
_response = execution('rl_task.stop', hr, w2t, tasks=['brake'])
|
||||
md.trigger_estop()
|
||||
_response = execution('diagnosis.open', hr, w2t, open=False, display_open=False)
|
||||
# 找出最大速度
|
||||
speed_max = 0
|
||||
for _msg in hr.c_msg:
|
||||
if 'diagnosis.result' in _msg:
|
||||
dict_results = loads(_msg)['data']
|
||||
@ -244,71 +243,60 @@ def run_rl(path, loadsel, hr, md, config_file, prj_file, result_dirs, w2t):
|
||||
speed_max = max(_, speed_max)
|
||||
wb = load_workbook(config_file, read_only=True)
|
||||
ws = wb['Target']
|
||||
speed_target = float(ws.cell(row=3, column=axis+1).value)
|
||||
if speed_max < speed_target*0.95:
|
||||
speed_target = float(ws.cell(row=3, column=axis+1).value) * float(_speed) / 100
|
||||
print(f"speed target = {speed_target}")
|
||||
print(f"speed max = {speed_max}")
|
||||
if speed_max < speed_target*0.95 or speed_max > speed_target*1.05:
|
||||
w2t(f"Axis: {axis}-{count} | Speed: {speed_max*RADIAN} | Shouldbe: {speed_target}", 0, 0, 'indigo')
|
||||
|
||||
# 7. 清零c_msg，重新运行程序，并打开曲线发送继续运动信号，开始运动，当速度达到最大值时，通过DO触发急停
|
||||
for _msg in hr.c_msg:
|
||||
if 'diagnosis.result' in _msg:
|
||||
_index = hr.c_msg.index(_msg)
|
||||
del hr.c_msg[_index:]
|
||||
hr.c_msg_xs.clear()
|
||||
break
|
||||
md.write_speed_max(speed_max)
|
||||
sleep(1)
|
||||
|
||||
_response = execution('rl_task.pp_to_main', hr, w2t, tasks=['brake', 'stop0_related'])
|
||||
_response = execution('state.switch_auto', hr, w2t)
|
||||
_response = execution('state.switch_motor_on', hr, w2t)
|
||||
_response = execution('rl_task.run', hr, w2t, tasks=['brake', 'stop0_related'])
|
||||
for i in range(3):
|
||||
if md.read_ready_to_go() == 1:
|
||||
md.write_act(True)
|
||||
sleep(1)
|
||||
md.write_act(False)
|
||||
break
|
||||
for _msg in hr.c_msg:
|
||||
if 'diagnosis.result' in _msg:
|
||||
_index = hr.c_msg.index(_msg)
|
||||
del hr.c_msg[_index:]
|
||||
hr.c_msg_xs.clear()
|
||||
break
|
||||
|
||||
# 5. 清除软急停，重新运行程序，并打开曲线发送继续运动信号，当速度达到最大值时，通过DO触发急停
|
||||
if count == 1:
|
||||
md.reset_estop()
|
||||
_response = execution('overview.reload', hr, w2t, prj_path=prj_path, tasks=['brake', 'stop0_related'])
|
||||
_response = execution('rl_task.pp_to_main', hr, w2t, tasks=['brake', 'stop0_related'])
|
||||
_response = execution('state.switch_auto', hr, w2t)
|
||||
_response = execution('state.switch_motor_on', hr, w2t)
|
||||
_response = execution('rl_task.run', hr, w2t, tasks=['brake', 'stop0_related'])
|
||||
for i in range(3):
|
||||
if md.read_ready_to_go() == 1:
|
||||
md.write_act(True)
|
||||
sleep(1)
|
||||
md.write_act(False)
|
||||
break
|
||||
else:
|
||||
sleep(1)
|
||||
else:
|
||||
sleep(1)
|
||||
else:
|
||||
w2t("未收到机器人的运行信号，需要确认RL程序编写正确并正常执行...", 0, 111, 'red', 'Automatic Test')
|
||||
w2t("未收到机器人的运行信号，需要确认RL程序编写正确并正常执行...", 0, 111, 'red', 'Automatic Test')
|
||||
|
||||
_response = execution('diagnosis.open', hr, w2t, open=True, display_open=True)
|
||||
_response = execution('diagnosis.set_params', hr, w2t, display_pdo_params=display_pdo_params)
|
||||
sleep(4)
|
||||
_flg = 1
|
||||
sleep(random.randint(2, 8))
|
||||
md.write_probe(True)
|
||||
_t_start = time()
|
||||
while _flg:
|
||||
sleep(0.05)
|
||||
for _msg in hr.c_msg:
|
||||
if 'diagnosis.result' in _msg:
|
||||
if _flg == 0:
|
||||
break
|
||||
dict_results = loads(_msg)['data']
|
||||
for item in dict_results:
|
||||
if item.get('channel', None) == axis-1 and item.get('name', None) == 'hw_joint_vel_feedback':
|
||||
_ = abs(RADIAN*sum(item['value'])/len(item['value']))
|
||||
if abs(_-speed_max) < 5:
|
||||
print(f"speed actual = {_}")
|
||||
print(f"speed max = {speed_max}")
|
||||
print(f"speed target = {speed_target*float(_speed)/100}")
|
||||
print(f"abs minus = {abs(_-speed_max)}")
|
||||
for _i in range(5):
|
||||
# md.set_stop0(0) # 触发IO急停
|
||||
_response = execution('register.set_value', hr, w2t, name='stop0_signal', type='bool', bias=0, value=0)
|
||||
sleep(0.02)
|
||||
_flg = 0
|
||||
break
|
||||
else:
|
||||
if (time() - _t_start) // 60 > 1:
|
||||
w2t(f"规定时间内未找到合适的点触发急停，需要确认RL/Python程序编写正确并正常执行...", 0, 111, 'red', 'Automatic Test')
|
||||
else:
|
||||
break
|
||||
while True:
|
||||
if md.read_brake_done() == 1:
|
||||
sleep(1) # 保证所有数据均已返回
|
||||
md.write_probe(False)
|
||||
_response = execution('diagnosis.open', hr, w2t, open=False, display_open=False)
|
||||
sleep(1) # 保证所有数据均已返回
|
||||
break
|
||||
else:
|
||||
if (time() - _t_start) // 60 > 1:
|
||||
w2t(f"规定时间内未找到合适的点触发急停，需要确认RL/Python程序编写正确并正常执行...", 0, 111, 'red', 'Automatic Test')
|
||||
else:
|
||||
sleep(1)
|
||||
|
||||
# 8. 关闭诊断曲线，且换成手动模式
|
||||
sleep(1) # 保证所有数据均已返回
|
||||
_response = execution('diagnosis.open', hr, w2t, open=False, display_open=False)
|
||||
_response = execution('state.switch_manual', hr, w2t)
|
||||
|
||||
# 9. 保留数据并处理输出
|
||||
# 6. 保留数据并处理输出
|
||||
curve_data = []
|
||||
for _msg in hr.c_msg:
|
||||
if 'diagnosis.result' in _msg:
|
||||
|
||||
@ -5,7 +5,7 @@ import selectors
|
||||
from time import time, sleep
|
||||
from os.path import dirname
|
||||
from pymodbus.client.tcp import ModbusTcpClient
|
||||
from pymodbus.payload import BinaryPayloadDecoder
|
||||
from pymodbus.payload import BinaryPayloadDecoder, BinaryPayloadBuilder
|
||||
from pymodbus.constants import Endian
|
||||
|
||||
MAX_FRAME_SIZE = 1024
|
||||
@ -125,13 +125,48 @@ class ModbusRequest(object):
|
||||
self.w2t(f"{Err}")
|
||||
self.w2t("无法读取准备信号，连接Modbus失败，需要确认网络是否通畅，或是未正确导入寄存器文件...", 0, 100, 'red', self.tab_name)
|
||||
|
||||
def set_stop0(self, number):
|
||||
def write_stop0(self, number):
|
||||
try:
|
||||
self.c.write_register(41004, number)
|
||||
except Exception as Err:
|
||||
self.w2t(f"{Err}")
|
||||
self.w2t("无法通过IO操作stop0急停，连接Modbus失败，需要确认网络是否通畅，或是未正确导入寄存器文件...", 0, 100, 'red', self.tab_name)
|
||||
|
||||
def write_speed_max(self, speed):
|
||||
try:
|
||||
builder = BinaryPayloadBuilder(byteorder=Endian.BIG, wordorder=Endian.LITTLE)
|
||||
builder.add_32bit_float(float(speed))
|
||||
payload = builder.build()
|
||||
self.c.write_registers(41005, payload, skip_encode=True)
|
||||
except Exception as Err:
|
||||
self.w2t(f"{Err}")
|
||||
self.w2t("无法写入速度值，连接Modbus失败，需要确认网络是否通畅，或是未正确导入寄存器文件...", 0, 100, 'red', self.tab_name)
|
||||
|
||||
def read_brake_done(self):
|
||||
try:
|
||||
results = self.c.read_holding_registers(41007, 1)
|
||||
return results.registers[0]
|
||||
except Exception as Err:
|
||||
self.w2t(f"{Err}")
|
||||
self.w2t("无法读取制动已执行信号，连接Modbus失败，需要确认网络是否通畅，或是未正确导入寄存器文件...", 0, 100, 'red', self.tab_name)
|
||||
|
||||
def write_axis(self, axis):
|
||||
try:
|
||||
builder = BinaryPayloadBuilder(byteorder=Endian.BIG, wordorder=Endian.LITTLE)
|
||||
builder.add_32bit_int(int(axis))
|
||||
payload = builder.to_registers()
|
||||
self.c.write_registers(41008, payload)
|
||||
except Exception as Err:
|
||||
self.w2t(f"{Err}")
|
||||
self.w2t("无法写入速度值，连接Modbus失败，需要确认网络是否通畅，或是未正确导入寄存器文件...", 0, 100, 'red', self.tab_name)
|
||||
|
||||
def write_probe(self, probe):
|
||||
try:
|
||||
self.c.write_register(41010, probe)
|
||||
except Exception as Err:
|
||||
self.w2t(f"{Err}")
|
||||
self.w2t("无法写入速度探测信号，连接Modbus失败，需要确认网络是否通畅，或是未正确导入寄存器文件...", 0, 100, 'red', self.tab_name)
|
||||
|
||||
|
||||
class HmiRequest(object):
|
||||
def __init__(self, w2t):
|
||||
@ -188,7 +223,8 @@ class HmiRequest(object):
|
||||
md.reset_estop()
|
||||
md.clear_alarm()
|
||||
md.write_act(False)
|
||||
md.set_stop0(1)
|
||||
md.write_probe(False)
|
||||
md.write_axis(1)
|
||||
except Exception as Err:
|
||||
self.w2t("Connection failed...", 0, 0, 'red', tab_name=self.tab_name)
|
||||
with open(f"{current_path}/../../assets/templates/heartbeat", "w", encoding='utf-8') as f_hb:
|
||||
@ -233,7 +269,7 @@ class HmiRequest(object):
|
||||
f_hb.write(_flag)
|
||||
if _flag == '0':
|
||||
self.w2t(f"{_id} 心跳丢失，连接失败，重新连接中...", 0, 7, 'red', tab_name=self.tab_name)
|
||||
sleep(2)
|
||||
sleep(1.5)
|
||||
# with open(f"{current_path}/../../assets/templates/c_msg.log", "w", encoding='utf-8') as f:
|
||||
# for msg in self.c_msg:
|
||||
# f.write(str(loads(msg)) + '\n')
|
||||
|
||||
Reference in New Issue
Block a user