class Gui(QMainWindow):
"""
Main GUI Class
contains the main function and interfaces between
the GUI and functions
"""
def __init__(self, parent=None):
QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Set GUI to track mouse """
QWidget.setMouseTracking(self, True)
"""
Dynamixel bus
TODO: add other motors here as needed with their correct address"""
self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
print(self.dxlbus)
port_num = self.dxlbus.port()
print(port_num)
base = DXL_MX(port_num, 1)
shld = DXL_MX(port_num, 2)
elbw = DXL_MX(port_num, 3)
wrst = DXL_AX(port_num, 4)
wrst2 = DXL_AX(port_num, 5)
grip = DXL_XL(port_num, 6)
wrst3 = DXL_XL(port_num, 7)
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm((base, shld, elbw, wrst, wrst2, wrst3), grip)
#self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2), 0)
self.tp = TrajectoryPlanner(self.rexarm, self.kinect)
self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
"""
Attach Functions to Buttons & Sliders
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btn_exec.clicked.connect(self.execute)
self.ui.btn_task1.clicked.connect(self.operation)
self.ui.btn_task2.clicked.connect(self.record)
self.ui.btn_task3.clicked.connect(self.opex)
self.ui.btn_task4.clicked.connect(self.opplay)
self.ui.btn_task5.clicked.connect(self.FK_check)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(
partial(self.sm.set_next_state, "calibrate"))
self.ui.btnUser2.setText("Block Detection")
self.ui.btnUser2.clicked.connect(self.block_detect)
self.ui.btnUser3.setText("Task 1")
self.ui.btnUser3.clicked.connect(self.task_1)
self.ui.btnUser4.setText("Task 2")
self.ui.btnUser4.clicked.connect(self.task_2)
self.ui.btnUser5.setText("Task 3")
self.ui.btnUser5.clicked.connect(self.task_2)
self.ui.btnUser6.setText("Traj Collect")
self.ui.btnUser6.clicked.connect(self.collect_traj_data)
self.ui.sldrBase.valueChanged.connect(self.sliderChange)
self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist3.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
self.ui.rdoutStatus.setText("Waiting for input")
"""initalize manual control off"""
self.ui.SliderFrame.setEnabled(False)
"""initalize rexarm"""
self.rexarm.initialize()
"""Setup Threads"""
self.videoThread = VideoThread(self.kinect)
self.videoThread.updateFrame.connect(self.setImage)
self.videoThread.start()
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
self.displayThread = DisplayThread(self.rexarm, self.sm)
self.displayThread.updateJointReadout.connect(self.updateJointReadout)
self.displayThread.updateEndEffectorReadout.connect(
self.updateEndEffectorReadout)
self.displayThread.updateStatusMessage.connect(
self.updateStatusMessage)
self.displayThread.start()
"""
Setup Timer
this runs the trackMouse function every 50ms
"""
self._timer = QTimer(self)
self._timer.timeout.connect(self.trackMouse)
self._timer.start(50)
""" Slots attach callback functions to signals emitted from threads"""
@pyqtSlot(QImage, QImage)
def setImage(self, rgb_image, depth_image):
if (self.ui.radioVideo.isChecked()):
self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image))
if (self.ui.radioDepth.isChecked()):
self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image))
@pyqtSlot(list)
def updateJointReadout(self, joints):
self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0] * R2D)))
self.ui.rdoutShoulderJC.setText(
str("%+.2f" % ((joints[1] * R2D) + 90.0)))
self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2] * R2D)))
self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3] * R2D)))
self.ui.rdoutWrist2JC.setText(str("%+.2f" % (joints[4] * R2D)))
if (len(joints) > 5):
self.ui.rdoutWrist3JC.setText(str("%+.2f" % (joints[5] * R2D)))
else:
self.ui.rdoutWrist3JC.setText(str("N.A."))
@pyqtSlot(list)
def updateEndEffectorReadout(self, pos):
self.ui.rdoutX.setText(str("%+.2f" % (pos[0])))
self.ui.rdoutY.setText(str("%+.2f" % (pos[1])))
self.ui.rdoutZ.setText(str("%+.2f" % (pos[2])))
# self.ui.rdoutT.setText(str("%+.2f" % (pos[3])))
# self.ui.rdoutG.setText(str("%+.2f" % (pos[4])))
# self.ui.rdoutP.setText(str("%+.2f" % (pos[5])))
@pyqtSlot(str)
def updateStatusMessage(self, msg):
self.ui.rdoutStatus.setText(msg)
""" Other callback functions attached to GUI elements"""
def estop(self):
self.rexarm.estop = True
self.sm.set_next_state("estop")
def execute(self):
self.sm.set_next_state("execute")
self.rexarm.pause(5)
def operation(self):
if path.exists("op_joints.csv"):
os.remove("op_joints.csv")
self.sm.set_next_state("operation")
def opex(self):
self.sm.set_next_state("idle")
def opplay(self):
self.sm.set_next_state("opplay")
def FK_check(self):
self.sm.set_next_state("FK_check")
def block_detect(self):
self.sm.set_next_state("block_detect")
def task_1(self):
self.sm.set_next_state("Task 1")
def task_2(self):
self.sm.set_next_state("Task 2")
def task_3(self):
self.sm.set_next_state("Task 3")
def collect_traj_data(self):
self.sm.set_next_state("Collect Traj")
def record(self):
if self.sm.current_state == "operation":
rec_joints = self.rexarm.get_positions()
strec = str(rec_joints)[1:-1] + "\n"
if path.exists("op_joints.csv"):
with open('op_joints.csv', 'a') as f:
f.write(strec)
else:
with open('op_joints.csv', 'w') as f:
f.write(strec)
def sliderChange(self):
"""
Function to change the slider labels when sliders are moved
and to command the arm to the given position
"""
self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))
self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))
self.ui.rdoutWrist2.setText(str(self.ui.sldrWrist2.value()))
self.ui.rdoutWrist3.setText(str(self.ui.sldrWrist3.value()))
self.ui.rdoutGrip1.setText(str(self.ui.sldrGrip1.value()))
self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value() / 100.0] *
self.rexarm.num_joints,
update_now=False)
self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value() /
100.0,
update_now=False)
joint_positions = np.array([
self.ui.sldrBase.value() * D2R,
self.ui.sldrShoulder.value() * D2R,
self.ui.sldrElbow.value() * D2R,
self.ui.sldrWrist.value() * D2R,
self.ui.sldrWrist2.value() * D2R,
self.ui.sldrWrist3.value() * D2R
])
self.rexarm.set_positions(joint_positions, update_now=False)
self.rexarm.gripper.set_position(
np.array([self.ui.sldrGrip1.value() * D2R]))
def directControlChk(self, state):
if state == Qt.Checked:
self.sm.set_next_state("manual")
self.ui.SliderFrame.setEnabled(True)
else:
self.sm.set_next_state("idle")
self.ui.SliderFrame.setEnabled(False)
def trackMouse(self):
"""
Mouse position presentation in GUI
TODO: after implementing workspace calibration
display the world coordinates the mouse points to
in the RGB video image.
"""
x = QWidget.mapFromGlobal(self, QCursor.pos()).x()
y = QWidget.mapFromGlobal(self, QCursor.pos()).y()
count = 0
if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutMouseWorld.setText("(-,-,-)")
# posesall = self.rexarm.get_positions()
# endeffectorpos = FK_dh(posesall,0)
# if path.exists("traj_fast_not_smooth.txt"):
# with open('traj_fast_not_smooth.txt','a') as f:
# f.write(str(self.rexarm.get_wrist_pose())+'\n')
# else :
# with open('traj_fast_not_smooth.txt','w') as f:
# f.write("Traj Not Smooth\n")
else:
# Subtracting the X and Y distance corresponding to image origin frame to find cursor location with reference to imae frame
x = x - MIN_X
y = y - MIN_Y
# Checking if the Kinect depth camera is producing output
if (self.kinect.currentDepthFrame.any() != 0):
z = self.kinect.currentDepthFrame[y][x]
# Display the x,y (pixels), z (10 bit number) coordinates
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" %
(x, y, z))
# Checking if the calibration has been done
if self.sm.calibration_state() == True:
#############################################
# CAMERA FRAME TO DEPTH FRAME #
#############################################
# Taking in the pixel values in camera frame and transforming to the kinect depth frame
pixel_value = np.array([x, y])
# Converting 10 bit depth to real distance using provided analytical function
z = self.kinect.currentDepthFrame[int(
pixel_value.item(1))][int(pixel_value.item(0))]
Z = 12.36 * np.tan(float(z) / 2842.5 + 1.1863)
# 95 cm marks the z location of the base plane wrt to the camera. Subtracting 95 to measure +z from the base plane
Z_modified = 95 - Z
#############################################
# CAMERA FRAME TO WORLD FRAME #
#############################################
# Extracting the origin of the camera frame (Following 4 quadrant system)
pix_center = self.sm.pixel_center_loc()
# X and Y locations in the RGB space in pixels with (0,0) at the robot base center
x = x - pix_center.item(0)
y = pix_center.item(1) - y
# Taking in the pixel values in camera frame and transforming to the world frame
pixel_value = np.array([x, y])
pixel_value = np.transpose(pixel_value)
# Extracting the affine matrix computed during camera calibration
affine = self.sm.return_affine()
affine = affine[0:2, 0:2]
# World x,y location corresponding to iamge frame x,y location
world_value = np.matmul(affine, pixel_value)
#############################################
# SOLVE PNP #
#############################################
# rot,trans = self.sm.return_solvepnp()
# cam = self.sm.return_intrinsic()
# xyz_c = Z*rgb_pt.T
# xyz_c = np.linalg.inv(cam).dot(xyz_c)
# xyz_c = xyz_c - trans
# world_value = xyz_c*rot
# -0.197*float(z) + 142.772
# self.kinect.detectBlocksInDepthImage()
# self.kinect.processVideoFrame()
# Displaying the World X,Y and Z coordinates in GUI
self.ui.rdoutMouseWorld.setText(
"(%.0f,%.0f,%.1f)" %
(world_value.item(0), world_value.item(1), Z_modified))
# self.sm.WC = [world_value.item(0)*10,world_value.item(1)*10,ZZ*10]
else:
self.ui.rdoutMouseWorld.setText("(-,-,-)")
def mousePressEvent(self, QMouseEvent):
"""
Function used to record mouse click positions for calibration
"""
""" Get mouse posiiton """
x = QMouseEvent.x()
y = QMouseEvent.y()
""" If mouse position is not over the camera image ignore """
if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)): return
""" Change coordinates to image axis """
self.kinect.last_click[0] = x - MIN_X
self.kinect.last_click[1] = y - MIN_Y
self.kinect.new_click = True
