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)
""" Groups of ui commonents """
self.error_lcds = [
self.ui.rdoutBaseErrors, self.ui.rdoutShoulderErrors,
self.ui.rdoutElbowErrors, self.ui.rdoutWristErrors,
self.ui.rdoutWrist2Errors, self.ui.rdoutWrist3Errors
]
self.joint_readouts = [
self.ui.rdoutBaseJC, self.ui.rdoutShoulderJC, self.ui.rdoutElbowJC,
self.ui.rdoutWristJC, self.ui.rdoutWrist2JC, self.ui.rdoutWrist3JC
]
self.joint_slider_rdouts = [
self.ui.rdoutBase, self.ui.rdoutShoulder, self.ui.rdoutElbow,
self.ui.rdoutWrist, self.ui.rdoutWrist2, self.ui.rdoutWrist3
]
self.joint_sliders = [
self.ui.sldrBase, self.ui.sldrShoulder, self.ui.sldrElbow,
self.ui.sldrWrist, self.ui.sldrWrist2, self.ui.sldrWrist3
]
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm()
self.tp = TrajectoryPlanner(self.rexarm)
self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
self.sm.is_logging = False
"""
Attach Functions to Buttons & Sliders
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
# Video
self.ui.videoDisplay.setMouseTracking(True)
self.ui.videoDisplay.mouseMoveEvent = self.trackMouse
self.ui.videoDisplay.mousePressEvent = self.calibrateMousePress
# Buttons
# Handy lambda function that can be used with Partial to only set the new state if the rexarm is initialized
nxt_if_arm_init = lambda next_state: self.sm.set_next_state(
next_state if self.rexarm.initialized else None)
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btn_init_arm.clicked.connect(self.initRexarm)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(partial(nxt_if_arm_init, 'calibrate'))
##OUR_CODE
self.ui.btn_exec.clicked.connect(self.execute)
self.ui.btnUser2.clicked.connect(self.record)
self.ui.btnUser3.clicked.connect(self.playback)
self.ui.btnUser4.clicked.connect(self.execute_tp)
self.ui.btnUser5.clicked.connect(self.toggle_logging)
self.ui.btnUser1.clicked.connect(self.calibrate)
self.ui.btnUser6.clicked.connect(self.blockDetect)
self.ui.btnUser7.clicked.connect(self.openGripper)
self.ui.btnUser8.clicked.connect(self.closeGripper)
self.ui.btnUser9.clicked.connect(self.clickGrab)
# Sliders
for sldr in self.joint_sliders:
sldr.valueChanged.connect(self.sliderChange)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
# Direct Control
self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
# Status
self.ui.rdoutStatus.setText("Waiting for input")
# Auto exposure
self.ui.chkAutoExposure.stateChanged.connect(self.autoExposureChk)
"""initalize manual control off"""
self.ui.SliderFrame.setEnabled(False)
"""Setup Threads"""
# Rexarm runs its own thread
# Video
self.videoThread = VideoThread(self.kinect)
self.videoThread.updateFrame.connect(self.setImage)
self.videoThread.start()
# State machine
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
# Display
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.updateJointErrors.connect(self.updateJointErrors)
self.displayThread.start()
""" Slots attach callback functions to signals emitted from threads"""
@pyqtSlot(QImage, QImage)
def setImage(self, rgb_image, depth_image):
"""!
@brief Display the images from the kinect.
@param rgb_image The rgb image
@param depth_image The 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):
for rdout, joint in zip(self.joint_readouts, joints):
rdout.setText(str('%+.2f' % (joint * R2D)))
@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(list)
def updateJointErrors(self, errors):
for lcd, error in zip(self.error_lcds, errors):
lcd.display(error)
@pyqtSlot(str)
def updateStatusMessage(self, msg):
self.ui.rdoutStatus.setText(msg)
""" Other callback functions attached to GUI elements"""
def estop(self):
self.sm.set_next_state("estop")
def execute(self):
self.sm.set_next_state("execute")
def record(self):
self.sm.set_next_state("record")
def playback(self):
self.sm.set_next_state("playback")
def execute_tp(self):
self.sm.set_next_state("execute_tp")
def calibrate(self):
self.sm.set_next_state("calibrate")
def blockDetect(self):
self.kinect.blockDetector()
def openGripper(self):
self.rexarm.open_gripper()
def closeGripper(self):
self.rexarm.close_gripper()
def clickGrab(self):
self.sm.set_next_state("clickGrab")
def toggle_logging(self):
if not self.sm.is_logging:
# with open('log_data.csv', 'a') as log_file:
self.rexarm.log_file = open('log_data.csv', 'a')
self.rexarm.csv_writer = csv.writer(self.rexarm.log_file,
delimiter=',')
self.sm.is_logging = True
else:
self.rexarm.log_file.close()
self.sm.is_logging = False
def sliderChange(self):
"""!
@brief Slider changed
Function to change the slider labels when sliders are moved and to command the arm to the given position
"""
for rdout, sldr in zip(self.joint_slider_rdouts, self.joint_sliders):
rdout.setText(str(sldr.value()))
self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
# Do nothing if the rexarm is not initialized
if self.rexarm.initialized:
self.rexarm.set_torque_limits(
[self.ui.sldrMaxTorque.value() / 100.0] *
self.rexarm.num_joints)
self.rexarm.set_speeds_normalized_all(self.ui.sldrSpeed.value() /
100.0)
joint_positions = np.array(
[sldr.value() * D2R for sldr in self.joint_sliders])
# Only send the joints that the rexarm has
self.rexarm.set_positions(
joint_positions[0:self.rexarm.num_joints])
def directControlChk(self, state):
"""!
@brief Changes to direct control mode
Will only work if the rexarm is initialized.
@param state State of the checkbox
"""
if state == Qt.Checked and self.rexarm.initialized:
# Go to manual and enable sliders
self.sm.set_next_state("manual")
self.ui.SliderFrame.setEnabled(True)
else:
# Lock sliders and go to idle
self.sm.set_next_state("idle")
self.ui.SliderFrame.setEnabled(False)
self.ui.chk_directcontrol.setChecked(False)
def autoExposureChk(self, state):
"""!
@brief Sets the Kinect auto exposer
@param state State of the checkbox
"""
if state == Qt.Checked and self.kinect.kinectConnected == True:
self.kinect.toggleExposure(True)
else:
self.kinect.toggleExposure(False)
def trackMouse(self, mouse_event):
"""!
@brief Show the mouse position in GUI
TODO: after implementing workspace calibration display the world coordinates the mouse points to in the RGB
video image.
@param mouse_event QtMouseEvent containing the pose of the mouse at the time of the event not current time
"""
# if self.kinect.DepthFrameRaw.any() != 0:
# self.ui.rdoutMousePixels.setText("(-,-,-)")
# self.ui.rdoutMouseWorld.setText("(-,-,-)")
if self.kinect.cameraCalibrated:
pixel = np.array([mouse_event.y(), mouse_event.x()])
# cameraCoord = self.kinect.pixel2Camera(pixel)
worldCoord = self.kinect.getWorldCoord(pixel)
self.kinect.worldCoords = worldCoord
# print(worldCoord)
self.ui.rdoutMousePixels.setText(np.array2string(pixel))
# self.ui.rdoutMouseWorld.setText(np.array2string((worldCoord * 100).astype(int)))
self.ui.rdoutMouseWorld.setText(np.array2string((worldCoord)))
def calibrateMousePress(self, mouse_event):
"""!
@brief Record mouse click positions for calibration
@param mouse_event QtMouseEvent containing the pose of the mouse at the time of the event not current time
"""
""" Get mouse posiiton """
pt = mouse_event.pos()
self.kinect.last_click[0] = pt.x()
self.kinect.last_click[1] = pt.y()
self.kinect.new_click = True
# print(self.kinect.last_click)
def initRexarm(self):
"""!
@brief Initializes the rexarm.
"""
self.sm.set_next_state('initialize_rexarm')
self.ui.SliderFrame.setEnabled(False)
self.ui.chk_directcontrol.setChecked(False)
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)
port_num = self.dxlbus.port()
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)
wrst3 = DXL_XL(port_num, 6)
gripper = DXL_XL(port_num, 7)
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm((base, shld, elbw, wrst, wrst2, wrst3), gripper)
self.tp = TrajectoryPlanner(self.rexarm)
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.teach)
self.ui.btn_task2.clicked.connect(self.record)
self.ui.btn_task3.clicked.connect(self.repeat)
self.ui.btn_task4.clicked.connect(self.traj)
self.ui.btn_exec_6.clicked.connect(self.pickplace)
self.ui.btnUser3.clicked.connect(self.grip)
self.ui.btnUser2.clicked.connect(self.release)
self.ui.btnUser4.clicked.connect(self.task1)
self.ui.btnUser5.clicked.connect(self.task2)
self.ui.btnUser6.clicked.connect(self.task3)
self.ui.btnUser7.clicked.connect(self.task4)
self.ui.btnUser8.clicked.connect(self.task5)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(
partial(self.sm.set_next_state, "calibrate"))
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.sldrGrip1.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist3.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, QImage, QImage)
def setImage(self, rgb_image, depth_image, blob_rgb, blog_depth):
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))
if (self.ui.radioUsr1.isChecked()):
self.ui.videoDisplay.setPixmap(QPixmap.fromImage(blob_rgb))
if (self.ui.radioUsr2.isChecked()):
self.ui.videoDisplay.setPixmap(QPixmap.fromImage(blog_depth))
@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")
def teach(self):
self.sm.set_next_state("teach")
def record(self):
self.sm.waypoints.append(self.rexarm.get_positions()[:])
print(self.rexarm.get_positions())
def repeat(self):
self.sm.set_next_state("repeat")
def traj(self):
self.sm.set_next_state("traj")
def pickplace(self):
self.sm.set_next_state("pickplace")
def grip(self):
self.rexarm.close_gripper()
def release(self):
self.rexarm.open_gripper()
def task1(self):
self.sm.set_next_state("task1")
def task2(self):
self.sm.set_next_state("task2")
def task3(self):
self.sm.set_next_state("task3")
def task4(self):
self.sm.set_next_state("task4")
def task5(self):
self.sm.set_next_state("task5")
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)
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()
if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutMouseWorld.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
if (self.kinect.currentDepthFrame.any() != 0):
z = self.kinect.currentDepthFrame[y][x]
xw, yw, zw = self.kinect.getWorldCoord((x, y, z))
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" %
(x, y, z))
self.ui.rdoutMouseWorld.setText("(%.0f,%.0f,%.0f)" %
(xw, yw, zw))
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
