fixed docker port, middle pose, fixed noninputs not stopping, better modbus fail safeguards

This commit is contained in:
Tomás L 2024-07-29 13:42:59 -04:00
parent 993385f809
commit 887d1c9e35
3 changed files with 107 additions and 61 deletions

View File

@ -59,7 +59,7 @@ class URModbusServer:
:return: Readable angle values of each joint in radials
"""
if tries>50:
raise Exception("[Angles] Modbus Error: Failed")
raise ModbusError("[Angles] Modbus Error: Failed")
#return 0, 0, 0, 0, 0, 0
packet = self.modbusTCP.read_holding_registers(270, quantity=6)
@ -90,8 +90,8 @@ class URModbusServer:
:return: Readable angle speeds of each joint in radians per second
"""
if tries>50:
print("[Speeds] Modbus Error: Failed")
#raise Exception("[Speeds] Modbus Error: Failed")
#print("[Speeds] Modbus Error: Failed")
raise ModbusError("[Speeds] Modbus Error: Failed")
packet = self.modbusTCP.read_holding_registers(280, quantity=6)
@ -160,3 +160,6 @@ class URModbusServer:
else:
d_f = round(d_f - math.radians(360), 3)
return d_f
class ModbusError(Exception):
pass

View File

@ -2,15 +2,16 @@ from Robot.UR.URRobot import URRobot
from time import sleep
import math
import threading
from Robot.UR.URModbusServer import ModbusError
class URSentry:
def __init__(self, host):
self.robot = URRobot(host)
self.sentry_pose = [-0.0, -2.094, 0.96, -0.436, -1.571, 1.326]
self.forward_pose = [1.571, -1.949, 1.974, -2.548, -1.571, 1.326]
self.sentry_pose = [0.785, -2.094, 0.96, -0.436, -1.571, 1.326]
#self.forward_pose = [1.571, -1.949, 1.974, -2.548, -1.571, 1.326]
self.imposing_pose = [1.571, -1.41, 1.411, -2.859, -1.604, 1.326]
self.looking_down_pose = [1.571, -2.3, 2.425, -2.452, -1.604, 1.326]
self.looking_down_pose = [1.571, -2.3, 2.323, -2.452, -1.604, 1.326]
self.middle_point_pose = [(self.imposing_pose[i] + self.looking_down_pose[i]) / 2 for i in range(6)]
self.robot_speed = [0, 0, 0, 0, 0, 0]
self.detections = []
@ -31,6 +32,24 @@ class URSentry:
# Smooth stopping
self.smooth_stop_delayed_call = None
self.modbus_healthy = self.Modbus_check()
def Modbus_check(self):
try:
self.robot.get_joint_angles()
self.robot.get_joint_speeds()
except KeyboardInterrupt as ki:
raise ki
except:
print("########################################################")
print("#### MODBUS HEALTHCHECK FAILED ####")
print("########################################################")
return False
print("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~")
print("~~~~ MODBUS HEALTHCHECK PASSED ~~~~")
print("~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~")
return True
def initialize_pose(self):
self.await_stop = True
self.sentry_position()
@ -56,7 +75,7 @@ class URSentry:
self.detections = detections
def forward_position_to_base_angle_degrees(self, base_angle, a=0.5, v=1.5):
pose = self.imposing_pose
pose = self.middle_point_pose.copy()
pose[0] = math.radians(base_angle)
self.robot.movej(pose, a, v)
@ -67,8 +86,9 @@ class URSentry:
# self.robot_speed[4] = speed
def smooth_stop(self):
# print("Smooth stopping --------------------")
self.robot_speed = [0, 0, 0, 0, 0, 0]
self.robot.speedj([0, 0, 0, 0, 0, 0], 1.5)
print("xxxxxxxxx Smooth stopping xxxxxxxxx")
def lerp(self, a, b, t):
return a + t * (b - a)
@ -82,6 +102,12 @@ class URSentry:
# Check for flags that would block control due to things happening
# Initialize robot if it hasn't already
if not self.modbus_healthy:
self.modbus_healthy = self.Modbus_check()
return
try:
if not self.has_initialized:
self.initialize_pose()
return
@ -100,7 +126,7 @@ class URSentry:
# If we are awaiting a stop to send to zero, we do so right after it passes the await_stop check
if self.send_to_zero_on_stop:
print("Sending to zero --------------------")
print("Sending to zero ~~~~~~~~~~~~~~~~~~~~~~~~")
self.await_stop = True
self.send_to_zero_on_stop = False
self.has_detected_once = False
@ -109,9 +135,9 @@ class URSentry:
# If the joystick is None, we return to sentry mode after a certain amount of ticks
# We only do this if we have detected something at least once, as the camera gets buggy after fast movements
ticks_for_return_to_sentry = 600
if joystick_pos is None:
if self.has_detected_once and not self.is_on_sentry_mode:
if not self.is_on_sentry_mode:
ticks_for_return_to_sentry = 600 if self.has_detected_once else 3000
self.none_input_ticks += 1
print("None input ticks: ", self.none_input_ticks, " / ", ticks_for_return_to_sentry)
if self.none_input_ticks > ticks_for_return_to_sentry:
@ -119,6 +145,8 @@ class URSentry:
self.is_on_sentry_mode = True
self.await_stop = True
self.sentry_position()
else:
self.move_robot_with_joystick(0, 0)
return
# If we have detected something, we reset the none input ticks
@ -132,6 +160,14 @@ class URSentry:
self.move_robot_with_joystick(joystick_pos[0], joystick_pos[1])
self.has_detected_once = True
self.none_input_ticks = 0
except ModbusError as me:
print("[Modbus error]: ", me)
self.modbus_healthy = False
try:
self.smooth_stop()
except:
pass
return
def awake_from_sentry_mode(self, joystick_pos_x: float, joystick_pos_y: float):
@ -139,7 +175,7 @@ class URSentry:
return
theta_rad = math.atan2(joystick_pos_x, -joystick_pos_y)
theta_deg = math.degrees(theta_rad)
theta_deg = math.degrees(theta_rad) - 45
print("Theta: ", theta_deg)
self.await_stop = True
@ -168,8 +204,12 @@ class URSentry:
If flag await_stop is set True, we check if the current speed is 0 wait until all joints are stopped before moving again.
"""
# Reverse the x axis
joystick_pos_x = -joystick_pos_x
# Limit the y axis between +-0.5 to prevent speeding down too fast
joystick_pos_y = max(-0.5, min(0.5, joystick_pos_y))
# We omit all processing until the robot has stopped moving for a certain amount of calls ('ticks')
# print("Base Speed: ", self.robot_speed[0])
@ -223,8 +263,9 @@ class URSentry:
# We are past the maximum angle, so we undo a rotation by sending to 0
self.send_to_zero_on_stop = True
self.await_stop = True
self.robot_speed[0] = 0
self.robot.speedj(self.robot_speed, base_acceleration, 1)
self.smooth_stop()
# self.robot_speed[0] = 0
# self.robot.speedj(self.robot_speed, base_acceleration, 1)
return
# We are not in the deadzone, so we need to move the base
@ -273,7 +314,7 @@ class URSentry:
if __name__ == "__main__":
host = "172.22.114.160"
sentry = URSentry(host)
sentry.sentry_position()
#sentry.sentry_position()
# sleep(3)
#sentry.robot.movej(sentry.looking_down_pose, a=0.5, v=1.5)
#sleep(3)

View File

@ -19,4 +19,6 @@ COPY dockermain.py .
ENV UNIFI_PASSWORD=''
EXPOSE 502
CMD [ "python", "./dockermain.py" ]