class main(QtGui.QMainWindow):
def __init__(self,parent=None):
QtGui.QWidget.__init__(self,parent)
self.file_dialog=QtGui.QFileDialog()
self.ui=Ui_MainWindow()
self.ui.setupUi(self)
self.fn=None
QtCore.QObject.connect(self.ui.action_Save,QtCore.SIGNAL("triggered()"),self.save_file)
QtCore.QObject.connect(self.ui.actionNew,QtCore.SIGNAL("triggered()"),self.new_file)
QtCore.QObject.connect(self.ui.actionOpen,QtCore.SIGNAL("triggered()"),self.open_file)
QtCore.QObject.connect(self.ui.action_Saveas,QtCore.SIGNAL("triggered()"),self.saveas_file)
QtCore.QObject.connect(self.ui.actionPrint,QtCore.SIGNAL("triggered()"),self.save_file)
QtCore.QObject.connect(self.ui.actionPrint_Preview,QtCore.SIGNAL("triggered()"),self.save_file)
QtCore.QObject.connect(self.ui.action_quit,QtCore.SIGNAL("triggered()"),self.quit)
def save_file(self):
if self.fn!=None:
fileobj=open(self.fn,'w')
fileobj.write(self.ui.textEdit.toPlainText())
fileobj.close()
else:
filename=self.file_dialog.getSaveFileName(self,u"保存文件")
try:
fileobj=open(filename,'w')
fileobj.write(self.ui.textEdit.toPlainText())
self.fn=filename
fileobj.close()
except Exception ,e:
pass
class StartQT4(QtGui.QMainWindow):
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self.clidict = Param_To_Cli()
self.ui.interface_choice.addItems(netifaces.interfaces())
QtCore.QObject.connect(
self.ui.start, QtCore.SIGNAL("clicked()"), self.start_sniffer)
# QtCore.QObject.connect(
# self.ui.interface_choice, QtCore.SIGNAL("activated(const QString&)"), self.get_interface)
#
# QtCore.QObject.connect(
# self.ui.mode_choice, QtCore.SIGNAL("activated(const QString&)"), self.get_mode)
# QtCore.QObject.connect(
# self.ui.lineEdit, QtCore.SIGNAL("editingFinished()"), self.get_filter)
def start_sniffer(self):
self.clidict.interface = str(self.ui.interface_choice.currentText())
self.clidict.mode = str(self.ui.mode_choice.currentText())
self.clidict.filter = str(self.ui.lineEdit.text())
log.debug(self.clidict.interface + '' +
self.clidict.mode + '' + self.clidict.filter)
sniffer_prepare(self.clidict)
self.sniff_queue = Queue()
sniffer = SnifferProcess(
self.sniff_queue, iface=self.clidict.interface)
sniffer.start()
while True:
item = self.sniff_queue.get()
self.ui.packet_list.addItem(item.summary())
QtGui.QApplication.processEvents()
class Gui(QtGui.QMainWindow):
def __init__(self,capture, getFrame, parent = None):
QtGui.QWidget.__init__(self,parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self.video = Video(capture, getFrame)
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.play)
self._timer.start(27)
self.update()
def play(self):
try:
self.video.captureNextFrame()
self.ui.videoFrame.setPixmap(self.video.convertFrame())
self.ui.videoFrame.setScaledContents(True)
gethistogram(self.ui.axes,tuple(self.video.histogram.values()),tuple(self.video.histogram.keys()))
# l = [random.randint(0, 10) for i in range(4)]
# self.ui.axes.plot(self.video.histogram.values(), 'r')
self.ui.canvas.draw()
if(len(self.video.labels)!=0):
self.ui.label1.setText(self.video.name)
# self.ui.label2.setText(self.video.labels[1])
# self.ui.label3.setText(self.video.labels[2])
# self.ui.label4.setText(self.video.labels[3])
# self.ui.label5.setText(self.video.labels[4])
except TypeError as e:
print("No Frame", e)
class MyForm(QtGui.QMainWindow):
def showMessage(self,string):
self.ui.messageTextbox.setText(string)
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
#here start
downloader.fileLabel = self.ui.fileLabel
downloader.nowSizeLabel = self.ui.nowSizeLabel
downloader.totalSizeLabel = self.ui.totalSizeLabel
downloader.messageTextbox = self.ui.messageTextbox
#pass para
self.connect(self.ui.aButton,QtCore.SIGNAL('clicked()'),self.aButton_clicked)
self.connect(self.ui.bButton,QtCore.SIGNAL('clicked()'),self.bButton_clicked)
self.connect(self.ui.aAfternoonButton,QtCore.SIGNAL('clicked()'),self.aAfternoonButton_clicked)
self.connect(self.ui.bAfternoonButton,QtCore.SIGNAL('clicked()'),self.bAfternoonButton_clicked)
self.connect(self.ui.downloadPercentButton,QtCore.SIGNAL('clicked()'),self.downloadPercentButton_clicked)
self.connect(self.ui.nextButton,QtCore.SIGNAL('clicked()'),self.nextButton_clicked)
#event
def aButton_clicked(self):
thread.start_new_thread(downloader.main,(1,True))
def bButton_clicked(self):
thread.start_new_thread(downloader.main,(0,True))
def aAfternoonButton_clicked(self):
thread.start_new_thread(downloader.main,(1,False))
def bAfternoonButton_clicked(self):
thread.start_new_thread(downloader.main,(0,False))
def nextButton_clicked(self):
downloader.goNext = True
def downloadPercentButton_clicked(self):
downloader.refreshPercent()
class mainForm(QtGui.QMainWindow):
def __init__(self, parent = None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self.pannel = []
# 기본 패널
self.pannel.append(tab_widget(addTab, self))
self.func_insert_tab(u'새로운 탭')
self.pannel.append(tab_widget(communicationPannel, self))
self.func_insert_tab(u'DataPort')
QtCore.QObject.connect(self.pannel[0].pannel.ui.clb, QtCore.SIGNAL("clicked()"), self.AddTab)
def AddTab(self):
current_index = len(self.ui.tabWidget)
self.pannel.append(tab_widget(communicationPannel, self))
self.func_insert_tab(u'DataPort')
# 프로그램을 닫으려 할때
def closeEvent(self, event):
reply = QtGui.QMessageBox.warning(self,
u'프로그램 종료', u"진행중인 모든 데이터 송·수신을 중지합니다.\n프로그램을 종료하시겠습니까?",
QtGui.QMessageBox.Yes | QtGui.QMessageBox.No,
QtGui.QMessageBox.No)
if reply == QtGui.QMessageBox.Yes:
event.accept()
elif reply == QtGui.QMessageBox.No:
event.ignore()
def func_insert_tab(self, tab_name):
"""
* 탭이 추가되면 항상 '새탭추가탭' 앞 쪽에 위치
* 추가된 탭이 활성화
"""
current_index = len(self.ui.tabWidget)
self.ui.tabWidget.insertTab(current_index - 1, self.pannel[current_index], unicode(tab_name))
self.ui.tabWidget.setCurrentIndex(current_index - 1)
def slot_close_tab(self, tab_index):
print tab_index
if 1 == self.ui.tabWidget.count():
# '새탭 추가탭'을 닫으려 할 경우
return
reply = QtGui.QMessageBox.warning(
self,u'탭 닫기', u"저장하지 않은 작업 내용은 잃게됩니다.\n탭을 닫으시겠습니까?",
QtGui.QMessageBox.Yes | QtGui.QMessageBox.No,
QtGui.QMessageBox.No)
if reply == QtGui.QMessageBox.Yes:
self.pannel.pop(tab_index + 1)
self.ui.tabWidget.removeTab(tab_index)
elif reply == QtGui.QMessageBox.No: pass
class Gui(QtGui.QMainWindow):
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self.video = Video(cv2.VideoCapture(0))
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.play)
self._timer.start(27)
self.update()
def play(self):
'''
capture frame and display
'''
try:
self.video.capture_next_frame()
self.ui.videoFrame.setPixmap(
self.video.convert_frame())
self.ui.videoFrame.setScaledContents(True)
except TypeError:
print "No frame"
def mousePressEvent(self, event):
'''
click mouse and put point on opencv window
'''
self.video.add_point(QtCore.QPoint(event.pos()),
self.ui.videoFrame.geometry())
class MyForm(QtGui.QMainWindow):
def __init__(self, logAnalyser, parent=None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self, len(logAnalyser.files))
self.logAnalyser = logAnalyser
self.threadLogAnalyser = ThreadedLogAnalyser(self, self.logAnalyser)
self.connect(self.threadLogAnalyser, QtCore.SIGNAL("appendLogs"), self.ui.appendLogs)
def main(self):
self.threadLogAnalyser.start()
def __init__(self):
"""
Initializes the DecrypterWindow
"""
super(DecrypterWindow, self).__init__()
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
# connect event handlers
self.ui.calibrateButton.clicked.connect(self.calibrateButtonHandler)
self.ui.importButton.clicked.connect(self.importButtonHandler)
self.ui.exportButton.clicked.connect(self.exportButtonHandler)
for c in string.lowercase:
self.ui.edit[c].textEdited.connect(self.editModifiedHandler)
# disable unavailable functionality
self.ui.importButton.setDisabled(True)
self.ui.exportButton.setDisabled(True)
for c in string.lowercase:
self.ui.edit[c].setDisabled(True)
self.decrypter = None
self.show()
self.setFixedSize(self.size())
def __init__(self, settings, application=None):
'''
UI Object that draws the main window.
Listens to the following settings:
- delay:
'''
QtGui.QMainWindow.__init__(self)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self._renderwidgets = [
RenderWidget(self),
RenderWidget(self),
RenderWidget(self),
RenderWidget(self),
]
self.update()
self.application = application
self.settings = settings
self.settings.settingChanged.connect(self.settingChanged)
self.ui.delay.setText(str(settings.getSetting("delay")))
self._loadScreens(self.settings.getSetting("layouts")[self.settings.getSetting("selectedlayout")]["screen"])
self._bindings = {}
self._windowstate = None
self._loadBindings(settings.getSetting('keybinding'))
def __init__(self, pod):
super().__init__()
MainWindow.console = ConsoleWidget(
namespace = {
'pod': pod,
'win': self,
},
text='You can use this window to enter Python commands.')
MainWindow.console.setWindowTitle('Python interaction')
self._ui = Ui_MainWindow()
self._ui.setupUi(self)
self._ui.startButton.clicked.connect(pod.begin)
self._ui.submitButton.clicked.connect(self.submitCommand)
pod.connected.connect(self.enable)
self._ui.actionSettings.triggered.connect(self.networkDialog)
self._ui.actionReconnect.triggered.connect(pod.begin)
self._ui.actionConsoleOpen.triggered.connect(self.openConsole)
pod.add_listener('*', self.appendNetworkLog)
pod.add_listener('v', self.updateVelocityLCD)
pod.add_listener('v', self._ui.velocityPlot.datum)
pod.add_listener('h', self.updateHeightLCD)
pod.add_listener('h', self._ui.heightPlot.datum)
pod.add_listener('d', self.updateDistanceLCD)
pod.add_listener('d', self._ui.distancePlot.datum)
self.command.connect(pod)
self.networkUpdate.connect(pod.try_connect)
self.timer = QTimer()
self.timer.timeout.connect(self._ui.velocityPlot.update)
self.timer.timeout.connect(self._ui.heightPlot.update)
self.timer.timeout.connect(self._ui.distancePlot.update)
self.timer.start(_1s)
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self.clidict = Param_To_Cli()
self.ui.interface_choice.addItems(netifaces.interfaces())
QtCore.QObject.connect(
self.ui.start, QtCore.SIGNAL("clicked()"), self.start_sniffer)
def __init__(self):
super(Main, self).__init__()
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self.load_data()
self.connect_signals()
self.show()
def __init__(self,parent=None): QtGui.QWidget.__init__(self,parent) self.ui = Ui_MainWindow() self.ui.setupUi(self) self.ui.start_button.clicked.connect(self.start_master) self.ui.connect_button.clicked.connect(self.start_client) self.ui.close_button.clicked.connect(self.close) self.CONNECTION_PORT = 1111 self.update()
def __init__(self,capture, getFrame, parent = None):
QtGui.QWidget.__init__(self,parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self.video = Video(capture, getFrame)
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.play)
self._timer.start(27)
self.update()
class StartQT4(QtGui.QMainWindow):
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self.clidict = Param_To_Cli()
self.ui.interface_choice.addItems(netifaces.interfaces())
QtCore.QObject.connect(
self.ui.start, QtCore.SIGNAL("clicked()"), self.start_sniffer)
QtCore.QObject.connect(
self.ui.action_stop, QtCore.SIGNAL("triggered()"), self.stop_sniffer)
#self.ui.packet_list.setHorizontalHeaderLabels(['时间','源地址','目的地址','内容'])
# QtCore.QObject.connect(
# self.ui.interface_choice, QtCore.SIGNAL("activated(const QString&)"), self.get_interface)
#
# QtCore.QObject.connect(
# self.ui.mode_choice, QtCore.SIGNAL("activated(const QString&)"), self.get_mode)
# QtCore.QObject.connect(
# self.ui.lineEdit, QtCore.SIGNAL("editingFinished()"), self.get_filter)
def start_sniffer(self):
self.clidict.interface = str(self.ui.interface_choice.currentText())
self.clidict.mode = str(self.ui.mode_choice.currentText())
self.clidict.filter = str(self.ui.lineEdit.text())
log.debug(self.clidict.interface + '' + self.clidict.mode + '' + self.clidict.filter)
sniffer_prepare(self.clidict)
self.sniff_queue = multiprocessing.JoinableQueue()
self.sniffer = SnifferProcess(self.sniff_queue, iface=str(self.clidict.interface))
self.sniffer.start()
self.displaythreading = DisplayPacket(self.sniff_queue, self.ui.packet_list)
self.displaythreading.start()
self.ui.action_stop.setEnabled(True)
def stop_sniffer(self):
self.sniffer.terminate()
self.sniff_queue.close()
while True:
if self.sniff_queue.empty():
break
self.displaythreading.terminate()
class Gui(QtGui.QMainWindow): def __init__(self,parent=None): QtGui.QWidget.__init__(self,parent) self.ui = Ui_MainWindow() self.ui.setupUi(self) self.ui.start_button.clicked.connect(self.start_master) self.ui.connect_button.clicked.connect(self.start_client) self.ui.close_button.clicked.connect(self.close) self.CONNECTION_PORT = 1111 self.update() def start_master(self): hablar_master(self.CONNECTION_PORT, self.ui.label) def start_client(self): ip = self.ui.ip.getText() hablar_client(ip, self.CONNECTION_PORT, self.ui.label) def close(self): sys.exit()
def __init__(self,parent=None):
QtGui.QWidget.__init__(self,parent)
self.file_dialog=QtGui.QFileDialog()
self.ui=Ui_MainWindow()
self.ui.setupUi(self)
self.fn=None
QtCore.QObject.connect(self.ui.action_Save,QtCore.SIGNAL("triggered()"),self.save_file)
QtCore.QObject.connect(self.ui.actionNew,QtCore.SIGNAL("triggered()"),self.new_file)
QtCore.QObject.connect(self.ui.actionOpen,QtCore.SIGNAL("triggered()"),self.open_file)
QtCore.QObject.connect(self.ui.action_Saveas,QtCore.SIGNAL("triggered()"),self.saveas_file)
QtCore.QObject.connect(self.ui.actionPrint,QtCore.SIGNAL("triggered()"),self.save_file)
QtCore.QObject.connect(self.ui.actionPrint_Preview,QtCore.SIGNAL("triggered()"),self.save_file)
QtCore.QObject.connect(self.ui.action_quit,QtCore.SIGNAL("triggered()"),self.quit)
class FreereaderUi(QtGui.QMainWindow):
def __init__(self, parent=None):
QtGui.QWidget.__init__(self,parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
QtCore.QObject.connect(self.ui.readButton, QtCore.SIGNAL("clicked()"), self.readButton)
QtCore.QMetaObject.connectSlotsByName(self)
def readButton(self):
self.notify('please wait')
try:
obj = feedparser.parse(str(self.ui.urlText.text()))
self.notify('feed readed')
if obj.entries != []:
for entry in obj.entries:
self.ui.listWidget.addItem(entry.title)
except:
self.notify('error in parsing url')
def notify(self, string = ''):
self.ui.notification.setText(string)
class MainWindow(QMainWindow):
'''First application window the user sees'''
def __init__(self, pod):
super().__init__()
self._pod = pod
self._ui = Ui_MainWindow()
self._ui.setupUi(self)
self._ui.actionNetwork.triggered.connect(self.networkDialog)
def networkDialog(self):
'''Open a window for changing network settings'''
la = self._pod.get_local_addr()
ra = self._pod.get_remote_addr()
info = NetworkInfo(local=la, remote=ra)
dialog = NetworkDialog(info)
if dialog.exec_() == QDialog.Accepted:
la, old = info.local, la
if la != old:
self._pod.set_local_addr(info.local)
ra, old = info.remote, ra
if ra != old:
self._pod.set_remote_addr(info.remote)
def __init__(self, parent = None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self.pannel = []
# 기본 패널
self.pannel.append(tab_widget(addTab, self))
self.func_insert_tab(u'새로운 탭')
self.pannel.append(tab_widget(communicationPannel, self))
self.func_insert_tab(u'DataPort')
QtCore.QObject.connect(self.pannel[0].pannel.ui.clb, QtCore.SIGNAL("clicked()"), self.AddTab)
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
#here start
downloader.fileLabel = self.ui.fileLabel
downloader.nowSizeLabel = self.ui.nowSizeLabel
downloader.totalSizeLabel = self.ui.totalSizeLabel
downloader.messageTextbox = self.ui.messageTextbox
#pass para
self.connect(self.ui.aButton,QtCore.SIGNAL('clicked()'),self.aButton_clicked)
self.connect(self.ui.bButton,QtCore.SIGNAL('clicked()'),self.bButton_clicked)
self.connect(self.ui.aAfternoonButton,QtCore.SIGNAL('clicked()'),self.aAfternoonButton_clicked)
self.connect(self.ui.bAfternoonButton,QtCore.SIGNAL('clicked()'),self.bAfternoonButton_clicked)
self.connect(self.ui.downloadPercentButton,QtCore.SIGNAL('clicked()'),self.downloadPercentButton_clicked)
self.connect(self.ui.nextButton,QtCore.SIGNAL('clicked()'),self.nextButton_clicked)
def __init__(self, **args):
super(QApplication, self).__init__([])
self.setApplicationName(NAME)
self.setApplicationVersion(VERSION)
self.setOrganizationName(COMPANY)
self.setOrganizationDomain(DOMAIN)
self.settings = QSettings()
self._mw = QMainWindow()
self.ui = Ui_MainWindow()
self.ui.setupUi(self._mw)
self.ui.actionOpen_MM.triggered.connect(self.openMM)
self.ui.actionOpen_M2T.triggered.connect(self.openM2T)
self.ui.actionMerge.triggered.connect(self.merge)
self.ui.actionSave.triggered.connect(self.save)
self.ui.actionSave_As.triggered.connect(self.saveAs)
self.ui.actionAbout_M2Todo.triggered.connect(self.about)
self.ui.actionHelp.triggered.connect(self.help)
self.ui.actionFreeMind.triggered.connect(self.launchFreemind)
self.ui.treeWidget.itemChanged.connect(self.itemStriker)
self._openPath = None
self._savePath = None
self.ui.actionMerge.setDisabled(True)
self._mw.show()
lastFile = str(self.settings.value("session/lastOpenFile", None).toString())
if lastFile:
fType = self.settings.value("session/lastOpenFileType")
self._openPath = {"type": fType, "path": lastFile}
if fType == "m2t":
self._parseM2T(lastFile)
elif fType == "mm":
self._parseMM(lastFile)
self._mw.statusBar().showMessage(lastFile)
self._mw.setWindowTitle(basename(str(lastFile)) + " - M2Todo")
self.ui.actionMerge.setDisabled(False)
# not debug!! here because it's always the last thing to do
# even if/when we have a "last open file" option
self.ui.actionSave.setDisabled(True)
def __init__(self):
super(MainWindow, self).__init__()
# UI Setup
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
# Set the default icons for UI actions
self.ui.actionNew.setIcon(QtGui.QIcon.fromTheme("document-new"))
self.ui.actionOpen.setIcon(QtGui.QIcon.fromTheme("document-open"))
self.ui.actionSave.setIcon(QtGui.QIcon.fromTheme("document-save"))
self.ui.actionSave_As.setIcon(QtGui.QIcon.fromTheme("document-save-as"))
self.ui.actionPrint.setIcon(QtGui.QIcon.fromTheme("document-print"))
self.ui.actionQuit.setIcon(QtGui.QIcon.fromTheme("system-log-out"))
self.ui.actionUndo.setIcon(QtGui.QIcon.fromTheme("edit-undo"))
self.ui.actionRedo.setIcon(QtGui.QIcon.fromTheme("edit-redo"))
self.ui.actionCut.setIcon(QtGui.QIcon.fromTheme("edit-cut"))
self.ui.actionCopy.setIcon(QtGui.QIcon.fromTheme("edit-copy"))
self.ui.actionPaste.setIcon(QtGui.QIcon.fromTheme("edit-paste"))
self.ui.actionDelete.setIcon(QtGui.QIcon.fromTheme("edit-delete"))
self.ui.actionPreferences.setIcon(QtGui.QIcon.fromTheme("preferences"))
self.ui.actionAbout.setIcon(QtGui.QIcon.fromTheme("help-browser"))
self.ui.actionRun.setIcon(QtGui.QIcon.fromTheme("go-next"))
# UI ToolBox
self.buttonGroup = QtGui.QButtonGroup()
self.buttonGroup.setExclusive(False)
self.buttonGroup.buttonClicked[int].connect(self.toolBoxButtonClicked)
self.toolBox = self.ui.componentBrowser
# Populate the toolbox
self.createToolBox()
# Create a scene for the GraphicsView
self.scene=DiagramScene()
self.ui.graphicsView.setScene(self.scene)
self.scene.setSceneRect(0,0,600,400)
# Make it bigger
self.setWindowState(QtCore.Qt.WindowMaximized)
# Create an UNDO stack and view
self.undoStack = QtGui.QUndoStack(self)
self.undoView = QtGui.QUndoView(self.undoStack)
self.undoView.setWindowTitle("Undo View")
self.undoView.show()
self.undoView.setAttribute(QtCore.Qt.WA_QuitOnClose, False)
self.createActions()
self.createMenus()
# Set the window title
self.setWindowTitle("Schematix")
def __init__(self, N):
super(MainWindow, self).__init__()
# This is always the same
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
# no resizing
self.setFixedSize(self.width(), self.height())
from sph_demo import SPHDemo
enable_advanced_rendering = not '--disable-advanced-rendering' in sys.argv
self.sph_demo = SPHDemo(N, size=(self.ui.fluid.width(), self.ui.fluid.height()), enable_advanced_rendering=enable_advanced_rendering)
# ui.fluid is the FluidWidget QGLWidget-widget.
self.ui.fluid.init(self.sph_demo)
self.ui.fluid.gl_initialized.connect(self.update_gui_from_params)
self.setup_signals()
statusBarInfo = QtGui.QLabel()
statusBarInfo.setText("%s particles, initial density: %s, mass: %.02f, gas constant k: %s, timestep: %.05f" % (
self.sph_demo.fluid_simulator.N,
self.sph_demo.fluid_simulator.density0,
self.sph_demo.fluid_simulator.mass,
self.sph_demo.fluid_simulator.k,
self.sph_demo.fluid_simulator.dt
))
self.statusBarFps = QtGui.QLabel()
self.statusBar().addPermanentWidget(self.statusBarFps)
self.statusBar().addWidget(statusBarInfo)
if not self.sph_demo.enable_advanced_rendering:
# disable ui elements dealing with advanced rendering
self.ui.rm_points.setChecked(True)
for el in (self.ui.rm_balls,
self.ui.rm_advanced,
self.ui.advanced,):
el.setEnabled(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)
""" 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)
"""
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'))
self.ui.btn_exec.clicked.connect(self.execute)
self.ui.btnUser2.setText("Reset Waypoints")
self.ui.btnUser2.clicked.connect(self.reset)
self.ui.btnUser3.setText("Add Waypoint")
self.ui.btnUser3.clicked.connect(self.teach)
self.ui.btnUser4.setText("Gripper Toggle")
self.ui.btnUser4.clicked.connect(self.toggle_gripper)
self.ui.btnUser5.setText("Add Color Points")
self.ui.btnUser5.clicked.connect(self.colPoints)
self.ui.btnUser6.setText("Click and Grab")
self.ui.btnUser6.clicked.connect(self.click_grab)
# 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, QImage)
def setImage(self, rgb_image, depth_image, depth_filter_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))
if (self.ui.radioUsr1.isChecked()):
self.ui.videoDisplay.setPixmap(
QPixmap.fromImage(depth_filter_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")
self.sm.set_next_state("execute_tp")
def reset(self):
self.sm.set_next_state("reset")
def teach(self):
self.sm.set_next_state("teach")
def colPoints(self):
if self.sm.current_state == "color": #if we're already doing colors, now save it
self.sm.set_next_state("save_color")
else:
self.sm.set_next_state("color")
def toggle_gripper(self):
self.rexarm.toggle_gripper()
def click_grab(self):
# TODO: WRITE FUNCTION HERE!!!
pass
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:
u = mouse_event.pos().x()
v = mouse_event.pos().y()
d = self.kinect.DepthFrameRaw[v, u]
self.ui.rdoutMousePixels.setText("(" + str(u) + "," + str(v) +
"," + str(d) + ")")
worldCoords = self.kinect.pix2Glob(np.array(
[u, v, 1])) * 1000 #1000 for mm
self.ui.rdoutMouseWorld.setText(
f"({np.round(worldCoords[0])},{np.round(worldCoords[1])},{np.round(worldCoords[2])})"
)
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()
if mouse_event.button() == Qt.LeftButton:
self.kinect.last_click[0] = pt.x()
self.kinect.last_click[1] = pt.y()
self.kinect.new_click = True
elif mouse_event.button() == Qt.RightButton:
self.kinect.last_rclick[0] = pt.x()
self.kinect.last_rclick[1] = pt.y()
self.kinect.new_rclick = True
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 jaabaGUI(QMainWindow):
""" controller for the blob labeling GUI"""
def __init__(self,parent=None):
QMainWindow.__init__(self,parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
#add new slider
# self.positionSlider=QSlider(Qt.Horizontal)
# self.positionSlider.setGeometry (800,800,100,30)
# self.positionSlider.setRange(0, 0)
# self.positionSlider.sliderMoved.connect(self.setPosition)
#setup Video
#video player
self.mediaPlayer1 = QMediaPlayer(None, QMediaPlayer.VideoSurface)
self.mediaPlayer2 = QMediaPlayer(None, QMediaPlayer.VideoSurface)
#self.mediaPlayer.metaDataChanged.connect(self.metaDataChanged)
self.mediaPlayer1.durationChanged.connect(self.durationChanged)
self.mediaPlayer1.positionChanged.connect(self.positionChanged)
self.mediaPlayer2.positionChanged.connect(self.positionChanged)
#visualizetion
self.scene = QGraphicsScene()
self.ui.graphicsView.setScene(self.scene)
#self.scene.setBackgroundBrush(Qt.black)
self.videoItem1 = QGraphicsVideoItem()
self.videoItem2 = QGraphicsVideoItem()
self.scene.addItem(self.videoItem1)
self.scene.addItem(self.videoItem2)
self.mediaPlayer1.setVideoOutput(self.videoItem1)
self.mediaPlayer2.setVideoOutput(self.videoItem2)
#slide bar
print self.ui.horizontalSlider
self.ui.horizontalSlider.setRange(0, 0)
self.ui.horizontalSlider.sliderMoved.connect(self.setPosition)
# self.ui.horizontalSlider.sliderPressed.connect(self.sliderPressed)
#print self.ui.graphicsView.width()/2,self.ui.graphicsView.height()
#self.videoItem1.setSize(QSizeF(self.ui.graphicsView.width()/2,self.ui.graphicsView.height()))
#self.videoItem2.setSize(QSizeF(self.ui.graphicsView.width()*10,self.ui.graphicsView.height()*10))
# self.videoItem2.setSize(graphicsView.size())
#self.videoItem2.setOffset(QPointF(500,500))
#self.videoItem2.setOffset(QPointF(self.ui.graphicsView.width()/2,0))
#self.videoItem2.setPos(QPointF(0,0))
# print self.ui.graphicsView.width(), self.ui.graphicsView.height()
# print self.ui.graphicsView.size()
# print self.videoItem2.boundingRect().width(), self.videoItem2.boundingRect().height()
# print self.ui.graphicsView.sceneRect()
#self.mediaPlayer.stateChanged.connect(self.mediaStateChanged)
#callbacks
self.ui.actionQuit.triggered.connect(self.quit)
self.ui.actionLoad_Project.triggered.connect(self.loadVideo)
#self.ui.buttonPlay.clicked[bool].connect(self.setToggleText)
self.ui.buttonPlay.clicked.connect(self.play)
#print self.ui.graphicsView.sizeHint()
#initialization
self.loaded = False
self.videoFilename = None
self.frame_count=None
self.width=None
self.height=None
self.frame_trans=None
# ###actions starts from here###
def quit(self):
QApplication.quit()
def loadVideo(self):
self.writeLog("Loading video...")
self.videoFilename = QFileDialog.getOpenFileName(self, 'Open File', '.')[0]
if not self.videoFilename:
self.writeLog("User cancelled - no video loaded")
return
else:
cap=cv2.VideoCapture(self.videoFilename)
self.frame_count=cap.get(cv2.CAP_PROP_FRAME_COUNT)
self.width=cap.get(3)
self.height=cap.get(4)
self.mediaPlayer2.setMedia(QMediaContent(QUrl.fromLocalFile(self.videoFilename )))
self.mediaPlayer1.setMedia(QMediaContent(QUrl.fromLocalFile(self.videoFilename )))
self.ui.buttonPlay.setEnabled(True)
# self.mediaPlayer2.setVideoOutput(self.videoItem2)
# self.mediaPlayer1.setVideoOutput(self.videoItem1)
# size= self.videoItem2.nativeSize()
# print size
#print self.mediaPlayer.duration()
#print self.mediaPlayer.metaData()
self.writeLog("Video loaded!")
def play(self):
self.videoItem1.setAspectRatioMode(0)
self.videoItem2.setAspectRatioMode(0)
self.scene.setSceneRect(0,0,self.ui.graphicsView.width(),self.ui.graphicsView.height())
self.videoItem1.setSize(QSizeF(self.ui.graphicsView.width()/2,self.ui.graphicsView.height()))
self.videoItem2.setSize(QSizeF(self.ui.graphicsView.width()/2,self.ui.graphicsView.height()))
self.videoItem1.setPos(QPointF(0,0))
self.videoItem2.setPos(QPointF(self.ui.graphicsView.width()/2,0))
#self.ui.graphicsView.setGeometry(0,0, 600,800)
#print 'graphicsView size', self.ui.graphicsView.size()
#print 'graphicsScene size', self.scene.sceneRect()
#self.videoItem2.setSize(QSizeF(1000,300))
#print 'graphicsVideoItem size',self.videoItem2.size()
# print 'item x',self.videoItem2.scenePos().x()
# print 'item y', self.videoItem2.scenePos().y()
# print 'item x',self.videoItem1.scenePos().x()
# print 'item y', self.videoItem1.scenePos().y()
if self.mediaPlayer1.state() == QMediaPlayer.PlayingState:
self.ui.buttonPlay.setIcon(self.ui.style().standardIcon(PyQt5.QtWidgets.QStyle.SP_MediaPlay))
self.ui.buttonPlay.setText("Play")
self.mediaPlayer1.pause()
self.writeLog("Video paused")
else:
self.ui.buttonPlay.setIcon(self.ui.style().standardIcon(PyQt5.QtWidgets.QStyle.SP_MediaPause))
self.ui.buttonPlay.setText("Stop")
self.mediaPlayer1.play()
self.writeLog("Playing video")
if self.mediaPlayer2.state() == QMediaPlayer.PlayingState:
self.mediaPlayer2.pause()
else:
self.mediaPlayer2.play()
#size= self.videoItem2.nativeSize()
# print self.mediaPlayer.duration()
#print self.mediaPlayer.metaData()
# print self.ui.graphicsView.width(), self.ui.graphicsView.height()
# print self.ui.graphicsView.size()
# print self.videoItem2.boundingRect().width(), self.videoItem2.boundingRect().height()
# print self.ui.graphicsView.sceneRect()
# print self.scene.sceneRect()
# print self.ui.graphicsView.sizeHint()
def setPosition(self, position):
self.mediaPlayer1.setPosition(position)
self.mediaPlayer2.setPosition(position)
# when position of media changed, set slider and text box accordingly.
def positionChanged(self, position):
self.ui.horizontalSlider.setValue(position)
if isinstance(self.frame_trans,float):
# print type(position),position
# print type(self.frame_trans),self.frame_trans
# print position/self.frame_trans
self.ui.lineEdit.setText(str(int(round(position/self.frame_trans,0))))
self.writeLog(str(position))
def durationChanged(self, duration):
self.ui.horizontalSlider.setRange(0, duration)
self.frame_trans=self.mediaPlayer1.duration()/self.frame_count
print self.frame_trans
def writeLog(self,text):
self.ui.log.setText(text)
class MainWindow(QtGui.QMainWindow):
def __init__(self, N):
super(MainWindow, self).__init__()
# This is always the same
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
# no resizing
self.setFixedSize(self.width(), self.height())
from sph_demo import SPHDemo
enable_advanced_rendering = not '--disable-advanced-rendering' in sys.argv
self.sph_demo = SPHDemo(N, size=(self.ui.fluid.width(), self.ui.fluid.height()), enable_advanced_rendering=enable_advanced_rendering)
# ui.fluid is the FluidWidget QGLWidget-widget.
self.ui.fluid.init(self.sph_demo)
self.ui.fluid.gl_initialized.connect(self.update_gui_from_params)
self.setup_signals()
statusBarInfo = QtGui.QLabel()
statusBarInfo.setText("%s particles, initial density: %s, mass: %.02f, gas constant k: %s, timestep: %.05f" % (
self.sph_demo.fluid_simulator.N,
self.sph_demo.fluid_simulator.density0,
self.sph_demo.fluid_simulator.mass,
self.sph_demo.fluid_simulator.k,
self.sph_demo.fluid_simulator.dt
))
self.statusBarFps = QtGui.QLabel()
self.statusBar().addPermanentWidget(self.statusBarFps)
self.statusBar().addWidget(statusBarInfo)
if not self.sph_demo.enable_advanced_rendering:
# disable ui elements dealing with advanced rendering
self.ui.rm_points.setChecked(True)
for el in (self.ui.rm_balls,
self.ui.rm_advanced,
self.ui.advanced,):
el.setEnabled(False)
def setup_signals(self):
"""
Set up callbacks for the widgets.
"""
ui = self.ui
self.fps_timer = QtCore.QTimer()
def callback():
self.statusBarFps.setText("%.2f fps" % self.sph_demo.fps)
self.fps_timer.timeout.connect(callback)
self.fps_timer.start(1000)
def callback(p):
self.sph_demo.params.paused = p
ui.paused.toggled.connect(callback)
if self.sph_demo.enable_advanced_rendering:
#from fluid_rendering.fluid_renderer import FluidRenderer
def callback(p):
self.sph_demo.params.blur_thickness_map = p
ui.blur_thickness_map.toggled.connect(callback)
def callback(p):
self.sph_demo.params.render_mean_curvature = p
ui.render_mean_curvature.toggled.connect(callback)
def callback():
self.sph_demo.params.render_mode = 0
ui.rm_points.pressed.connect(callback)
def callback():
self.sph_demo.params.render_mode = 1
ui.rm_balls.pressed.connect(callback)
def callback():
self.sph_demo.params.render_mode = 2
ui.rm_advanced.pressed.connect(callback)
ui.rm_advanced.toggled.connect(ui.advanced.setEnabled)
def callback(n):
self.sph_demo.params.smoothing_iterations = n
ui.smoothing_iterations.valueChanged.connect(callback)
def callback(n):
self.sph_demo.params.smoothing_z_contrib = n
ui.smoothing_z_contrib.valueChanged.connect(callback)
def update_gui_from_params(self):
params = self.sph_demo.params
if not params.dirty:
return
ui = self.ui
ui.paused.setOn(params.paused)
if self.sph_demo.enable_advanced_rendering:
from fluid_rendering.fluid_renderer import FluidRenderer
ui.blur_thickness_map.setOn(params.blur_thickness_map)
ui.render_mean_curvature.setOn(params.render_mean_curvature)
{ FluidRenderer.RENDERMODE_POINTS: ui.rm_points,
FluidRenderer.RENDERMODE_BALLS: ui.rm_balls,
FluidRenderer.RENDERMODE_ADVANCED: ui.rm_advanced }[params.render_mode].setChecked(True)
ui.smoothing_iterations.setValue(params.smoothing_iterations)
ui.smoothing_z_contrib.setValue(params.smoothing_z_contrib)
ui.advanced.setEnabled(ui.rm_advanced.isChecked())
params.dirty = False
def keyPressEvent(self, event):
"""
Handle keyboard shortcuts
"""
key = event.key()
params = self.sph_demo.params
if key == QtCore.Qt.Key_R:
self.sph_demo.fluid_simulator.set_positions()
elif key == QtCore.Qt.Key_P:
params.paused = not params.paused
elif key == QtCore.Qt.Key_B:
params.blur_thickness_map = not params.blur_thickness_map
elif key == QtCore.Qt.Key_N:
params.render_mode = (params.render_mode+1) % self.sph_demo.fluid_renderer.number_of_render_modes
elif key == QtCore.Qt.Key_C:
params.render_mean_curvature = not params.render_mean_curvature
else:
super(MainWindow, self).keyPressEvent(event)
self.update_gui_from_params()
class Gui(QtGui.QMainWindow):
"""
Main GUI Class
It contains the main function and interfaces between
the GUI and functions
"""
def __init__(self,parent=None):
QtGui.QWidget.__init__(self,parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Main Variables Using Other Classes"""
self.rex = Rexarm()
self.video = Video(cv2.VideoCapture(0))
self.world_coord = np.float32()
""" Play and Repeat Variable """
self.wayPoints = []
self.wayPointsPos = []
self.wayPointsSpeed = []
self.wayPointsTime = []
""" Other Variables """
self.last_click = np.float32([-1,-1])
self.define_template_flag = -1
self.click_point1 = np.float32([-1,-1])
self.click_point2 = np.float32([-1,-1])
self.template = None
self.targets = []
self.waypointsfp = csv.writer(open("waypoint.csv","wb"))
self.currtime = 0
""" Set GUI to track mouse """
QtGui.QWidget.setMouseTracking(self,True)
"""
Video Function
Creates a timer and calls play() function
according to the given time delay (27mm)
"""
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.play)
self._timer.start(27)
"""
LCM Arm Feedback
Creates a timer to call LCM handler continuously
No delay implemented. Reads all time
"""
self._timer2 = QtCore.QTimer(self)
self._timer2.timeout.connect(self.rex.get_feedback)
self._timer2.start()
"""
ARM Plan and Command Thread
Creates a timer to call REXARM.plan_command function continuously
"""
self._timer3 = QtCore.QTimer(self)
self._timer3.timeout.connect(self.rex.plan_command)
self._timer3.start()
"""
Connect Sliders to Function
TO DO: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI
"""
self.ui.sldrBase.valueChanged.connect(self.slider_change)
self.ui.sldrShoulder.valueChanged.connect(self.slider_change)
self.ui.sldrElbow.valueChanged.connect(self.slider_change)
self.ui.sldrWrist.valueChanged.connect(self.slider_change)
self.ui.sldrMaxTorque.valueChanged.connect(self.slider_change)
self.ui.sldrSpeed.valueChanged.connect(self.slider_change)
""" Commands the arm as the arm initialize to 0,0,0,0 angles """
self.slider_change()
""" Connect Buttons to Functions """
self.ui.btnLoadCameraCal.clicked.connect(self.load_camera_cal)
self.ui.btnPerfAffineCal.clicked.connect(self.affine_cal)
self.ui.btnTeachRepeat.clicked.connect(self.tr_initialize)
self.ui.btnAddWaypoint.clicked.connect(self.tr_add_waypoint)
self.ui.btnSmoothPath.clicked.connect(self.tr_smooth_path)
self.ui.btnPlayback.clicked.connect(self.tr_playback)
self.ui.btnLoadPlan.clicked.connect(self.tr_load)
self.ui.btnDefineTemplate.clicked.connect(self.def_template)
self.ui.btnLocateTargets.clicked.connect(self.template_match)
self.ui.btnExecutePath.clicked.connect(self.exec_path)
def play(self):
"""
Play Funtion
Continuously called by GUI
"""
""" Renders the Video Frame """
try:
self.video.captureNextFrame()
for t in self.targets:
self.video.addTarget(t)
self.ui.videoFrame.setPixmap(self.video.convertFrame())
self.ui.videoFrame.setScaledContents(True)
cv2.imwrite("curretFrame.png", self.video.currentFrame)
except TypeError:
print "No frame"
"""
Update GUI Joint Coordinates Labels
TO DO: include the other slider labels
"""
self.ui.rdoutBaseJC.setText(str(self.rex.joint_angles_fb[0]*R2D))
self.ui.rdoutShoulderJC.setText(str(self.rex.joint_angles_fb[1]*R2D))
self.ui.rdoutElbowJC.setText(str(self.rex.joint_angles_fb[2]*R2D))
self.ui.rdoutWristJC.setText(str(self.rex.joint_angles_fb[3]*R2D))
fk_result = self.rex.rexarm_FK(3)
#print fk_result
self.ui.rdoutX.setText(repr(fk_result[0]))
self.ui.rdoutY.setText(repr(fk_result[1]))
self.ui.rdoutZ.setText(repr(fk_result[2]))
self.ui.rdoutT.setText(repr(fk_result[3]))
"""
Mouse position presentation in GUI
TO DO: after getting affine calibration make the apprriate label
to present the value of mouse position in world coordinates
"""
x = QtGui.QWidget.mapFromGlobal(self,QtGui.QCursor.pos()).x()
y = QtGui.QWidget.mapFromGlobal(self,QtGui.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
self.ui.rdoutMousePixels.setText("(%.0f,%.0f)" % (x,y))
if (self.video.aff_flag == 2):
""" TO DO Here is where affine calibration must be used """
self.ui.rdoutMouseWorld.setText("(%0.f,%0.f)" % (self.world_coord[0][0], self.world_coord[1][0]))
else:
self.ui.rdoutMouseWorld.setText("(-,-)")
"""
Updates status label when rexarm playback is been executed.
This will be extended to includ eother appropriate messages
"""
if(self.rex.plan_status == 1):
self.ui.rdoutStatus.setText("Playing Back - Waypoint %d"
%(self.rex.wpt_number + 1))
def slider_change(self):
"""
Function to change the slider labels when sliders are moved
and to command the arm to the given position
TO DO: Implement for the other sliders
"""
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.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
self.rex.max_torque = self.ui.sldrMaxTorque.value()/100.0
for i in xrange(4):
self.rex.speed[i] = self.ui.sldrSpeed.value()/100.0
self.rex.joint_angles[0] = self.ui.sldrBase.value()*D2R
self.rex.joint_angles[1] = self.ui.sldrShoulder.value()*D2R
self.rex.joint_angles[2] = self.ui.sldrElbow.value()*D2R
self.rex.joint_angles[3] = self.ui.sldrWrist.value()*D2R
self.rex.cmd_publish()
def mousePressEvent(self, QMouseEvent):
"""
Function used to record mouse click positions for
affine 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.last_click[0] = x - MIN_X
self.last_click[1] = y - MIN_Y
""" If affine calibration is been performed """
if (self.video.aff_flag == 1):
""" Save last mouse coordinate """
self.video.mouse_coord[self.video.mouse_click_id] = [(x-MIN_X),(y-MIN_Y)]
""" Update the number of used poitns for calibration """
self.video.mouse_click_id += 1
""" Update status label text """
self.ui.rdoutStatus.setText("Affine Calibration: Click point %d"
%(self.video.mouse_click_id + 1))
"""
If the number of click is equal to the expected number of points
computes the affine calibration.
TO DO: Change this code to use you programmed affine calibration
and NOT openCV pre-programmed function as it is done now.
"""
if(self.video.mouse_click_id == self.video.aff_npoints):
"""
Update status of calibration flag and number of mouse
clicks
"""
self.video.aff_flag = 2
self.video.mouse_click_id = 0
print self.video.mouse_coord
print self.video.real_coord
""" Perform affine calibration with OpenCV """
#self.video.aff_matrix = cv2.getAffineTransform(
# self.video.mouse_coord,
# self.video.real_coord)
self.video.compute_affine_matrix()
""" Updates Status Label to inform calibration is done """
self.ui.rdoutStatus.setText("Waiting for input")
"""
Uncomment to gether affine calibration matrix numbers
on terminal
"""
print self.video.aff_matrix
if self.video.aff_flag == 2:
mouse_coord = np.array([[(x-MIN_X)], [(y-MIN_Y)],[1]])
self.world_coord = np.dot(self.video.aff_matrix, mouse_coord)
if self.define_template_flag == 0:
self.click_point1 = copy.deepcopy(self.last_click)
self.define_template_flag = 1
elif self.define_template_flag == 1:
self.click_point2 = copy.deepcopy(self.last_click)
self.template = copy.deepcopy(self.video.bwFrame[2*self.click_point1[1]:2*self.click_point2[1],2*self.click_point1[0]:2*self.click_point2[0]])
print self.click_point1
print self.click_point2
self.define_template_flag = -1
cv2.imwrite('./template.png', self.template)
def affine_cal(self):
"""
Function called when affine calibration button is called.
Note it only chnage the flag to record the next mouse clicks
and updates the status text label
"""
self.video.aff_flag = 1
self.ui.rdoutStatus.setText("Affine Calibration: Click point %d"
%(self.video.mouse_click_id + 1))
def load_camera_cal(self):
print "Load Camera Cal"
self.video.loadCalibration()
def tr_initialize(self):
self.wayPointsPos = []
self.wayPointsSpeed = []
self.wayPointsTime = []
print "Teach and Repeat"
def tr_add_waypoint(self):
#waypoints1 = copy.deepcopy(self.rex.joint_angles_fb)
#waypoints2 = copy.deepcopy(self.rex.joint_angles_fb)
#self.wayPointsPos.append(waypoints1)
#self.wayPointsTime.append([self.currtime, self.currtime, self.currtime, self.currtime])
#self.wayPointsSpeed.append([0.1, 0.1, 0.1, 0.1])
#self.currtime += 70000
#waypoints2[1] -= 0.7
#self.wayPointsPos.append(waypoints2)
#self.wayPointsTime.append([self.currtime, self.currtime, self.currtime, self.currtime])
#self.wayPointsSpeed.append([0.1, 0.1, 0.1, 0.1])
#self.currtime += 70000
#self.waypointsfp.writerow(waypoints1)
#self.waypointsfp.writerow(waypoints2)
#np.save("waypointsPos",self.wayPointsPos)
#np.save("waypointsSpeed", self.wayPointsSpeed)
#np.save("waypointsTime", self.wayPointsTime)
self.wayPointsPos.append(copy.deepcopy(self.rex.joint_angles_fb))
self.wayPointsSpeed.append(copy.deepcopy(self.rex.speed_fb))
self.wayPointsTime.append(copy.deepcopy(self.rex.time_fb))
np.save("waypointsPos",self.wayPointsPos)
np.save("waypointsSpeed", self.wayPointsSpeed)
np.save("waypointsTime", self.wayPointsTime)
#print self.wayPoints
print "Add Waypoint"
def cubic_spline(self, q0, q0dot, t0, qf, qfdot, tf, stepnum):
a0 = q0
a1 = q0dot
a2 = (3*(qf-q0) - (2*q0dot+qfdot)*float(tf-t0)) / float((tf-t0)*(tf-t0))
a3 = (2*(q0-qf) + (q0dot+qfdot)*float(tf-t0)) / float((tf-t0)*(tf-t0)*(tf-t0))
stepsize = float(tf-t0)/float(stepnum)
currtime = t0 + stepsize
pos_interpolated = []
speed_interpolated = []
for i in xrange(stepnum-1):
pos_interpolated.append(a0 + a1*currtime + a2*currtime*currtime + \
a3*currtime*currtime*currtime)
speed_interpolated.append(np.abs(a1 + 2*a2*currtime + 3*a3*currtime*currtime))
currtime += stepsize
#print q0, qf
#print pos_interpolated
return (pos_interpolated, speed_interpolated)
def tr_smooth_path(self):
if len(self.wayPointsPos) != len(self.wayPointsSpeed) and len(self.wayPointsPos) != len(self.wayPointsTime):
print "Error on waypoints number, cannot smooth path"
return
for i in xrange(len(self.wayPointsTime)):
for j in xrange(4):
self.wayPointsTime[i][j] *= US2S
for i in xrange(len(self.wayPointsSpeed)):
for j in xrange(4):
self.wayPointsSpeed[i][j] *= percent2rads
interpolated_waypoints_pos = []
interpolated_waypoints_speed = []
for i in xrange(len(self.wayPointsPos)-1):
time_offset = self.wayPointsTime[i]
startPos = self.wayPointsPos[i]
endPos = self.wayPointsPos[i+1]
startSpeed = self.wayPointsSpeed[i]
endSpeed = self.wayPointsSpeed[i+1]
startTime = self.wayPointsTime[i]
endTime = self.wayPointsTime[i+1]
stepnum = 3
four_joint_interpolated_pos = []
four_joint_interpolated_speed = []
for j in xrange(4):
q0 = startPos[j]
q0dot = startSpeed[j]
t0 = startTime[j] - time_offset[j]
qf = endPos[j]
qfdot = endSpeed[j]
tf = endTime[j] - time_offset[j]
res = self.cubic_spline(q0, q0dot, t0, qf, qfdot, tf, stepnum)
four_joint_interpolated_pos.append(res[0])
four_joint_interpolated_speed.append(res[1])
interpolated_waypoints_pos.append(startPos)
for i in xrange(len(four_joint_interpolated_pos[0])):
pos = []
for j in xrange(len(four_joint_interpolated_pos)):
pos.append(four_joint_interpolated_pos[j][i])
interpolated_waypoints_pos.append(pos)
interpolated_waypoints_pos.append(endPos)
interpolated_waypoints_speed.append(startSpeed)
for i in xrange(len(four_joint_interpolated_speed[0])):
speed = []
for j in xrange(len(four_joint_interpolated_speed)):
speed.append(four_joint_interpolated_speed[j][i])
interpolated_waypoints_speed.append(speed)
interpolated_waypoints_speed.append(endSpeed)
self.wayPointsPos = interpolated_waypoints_pos
self.wayPointsSpeed = interpolated_waypoints_speed
for i in xrange(len(self.wayPointsSpeed)):
for j in xrange(len(self.wayPointsSpeed[0])):
self.wayPointsSpeed[i][j] /= percent2rads
#pprint.pprint(self.wayPointsPos)
#pprint.pprint(self.wayPointsSpeed)
np.save("interpolated_waypoints_pos", self.wayPointsPos)
np.save("interpolated_waypoints_speed", self.wayPointsSpeed)
print "Smooth Path"
def tr_playback(self):
#print self.wayPoints
self.rex.planPos = self.wayPointsPos
self.rex.planSpeed = self.wayPointsSpeed
#print self.rex.plan
self.rex.save_data = True
self.rex.plan_status = 1
self.rex.wpt_number = 0
self.rex.wpt_total = len(self.rex.planPos)
print "Playback"
def tr_load(self):
self.wayPointsPos = np.load("waypointsPos.npy")
self.wayPointsSpeed = np.load("waypointsSpeed.npy")
self.wayPointsTime = np.load("waypointsTime.npy")
print "Load waypoints"
def def_template(self):
print "Define Template"
self.define_template_flag = 0
@do_cprofile
def template_match(self):
print "Template Match"
self.targets = []
result_pq = Queue.PriorityQueue()
template = cv2.resize(self.template, None, fx=0.6, fy=0.6, interpolation=cv2.INTER_AREA)
frame = cv2.resize(self.video.bwFrame, None, fx=0.6, fy=0.6, interpolation=cv2.INTER_AREA)
height, width = template.shape
for i in xrange(0, frame.shape[0] - height):
for j in xrange(0, frame.shape[1] - width):
center_x = (i + height/2.0)/0.6
center_y = (j + width/2.0)/0.6
to_compare = frame[i:i+height,j:j+width]
num = la.norm(to_compare - template)
result_pq.put((num, center_y, center_x))
result = []
#for i in xrange(40):
# t = result_pq.get()
# print t[0]
# result.append([int(t[1]), int(t[2])])
for i in xrange((frame.shape[0]-height)*(frame.shape[1]-width)):
t = result_pq.get()
if t[0] > 350:
break
else:
result.append([int(t[1]), int(t[2])])
distort = sys.maxint
cluster_size = 1
#print result
while distort > 20:
clustered_result, distort = scv.kmeans(np.array(result), cluster_size)
cluster_size += 1
clustered_result, distort = scv.kmeans(np.array(result), 4)
print "cluster_size: ", cluster_size-1
print "distort", distort
for r in clustered_result:
print r
self.targets.append((r[0], r[1]))
#circles = cv2.HoughCircles(self.video.bwFrame, cv2.cv.CV_HOUGH_GRADIENT, 1, minDist=5, param1=50, param2=50, minRadius=1, maxRadius=30)
#for c in circlesi[0,:]:
# print c
# self.targets.append((c[0],c[1]))
#img = np.zeros((1000,1000,3), np.uint8)
#for t in self.targets:
# img = cv2.circle(img, t, 10, (255,255,255), -1)
#cv2.imwrite("./result.png", img)
def exec_path(self):
#waypoints = [(-80.0, 90.0, 350.0), (70.0, 80.0, 400.0), (-180.0, -250.0, 125.0),(240.0, -190.0, 95.0), (-80.0, 90.0, 350.0)]
#waypoints = [(-100.0, 100.0, 5.0), (-100.0, 100.0, 200.0), (100.0, 100.0, 200.0),(100.0, 100.0, 5.0), (100.0, -100.0, 5.0), (100.0, -100.0, 200.0), (100.0, 100.0, 200.0)]
waypoints = []
for t in xrange(0,12,1):
x = 150 * np.cos(t)
y = 150 * np.sin(t)
z = 5.0 + 30.0 * t
waypoints.append((x, y, z))
#waypoints = []
#if not self.targets or len(self.targets) == 0:
# return
#for t in self.targets:
# img_coord = np.array([[(t[0]/2.0)], [(t[1]/2.0)], [1]])
# world_coord = np.dot(self.video.aff_matrix, img_coord)
# print "img coord: ", img_coord
# print "world coord: ", world_coord
# waypoints.append((world_coord[0], world_coord[1], 5))
print waypoints
self.wayPointsPos = []
self.wayPointsSpeed = []
self.wayPointsTime = []
for i in xrange(0, 3*len(waypoints), 3):
x, y, z = waypoints[i/3]
self.wayPointsPos.append(self.rex.rexarm_IK_Search((x,y,z)))
self.wayPointsSpeed.append(copy.deepcopy([0.15, 0.15, 0.15, 0.15]))
#jointsUp = self.rex.rexarm_IK_Search((x, y, 50.0))
#jointsDown = self.rex.rexarm_IK_Search((x, y, 0.0))
#if jointsUp:
# self.wayPointsPos.append(copy.deepcopy(jointsUp))
# self.wayPointsSpeed.append(copy.deepcopy([0.15, 0.15, 0.15, 0.15]))
# self.wayPointsTime.append([2000000*i,2000000*i,2000000*i,2000000*i])
#
#if jointsDown:
# self.wayPointsPos.append(copy.deepcopy(jointsDown))
# self.wayPointsSpeed.append(copy.deepcopy([0.05, 0.05, 0.05, 0.05]))
# self.wayPointsTime.append([2000000*(i+1),2000000*(i+1),2000000*(i+1),2000000*(i+1)])
#if jointsUp:
# self.wayPointsPos.append(copy.deepcopy(jointsUp))
# self.wayPointsSpeed.append(copy.deepcopy([0.05, 0.05, 0.05, 0.05]))
# self.wayPointsTime.append([2000000*(i+2),2000000*(i+2),2000000*(i+2),2000000*(i+2)])
np.save("funPathPos",self.wayPointsPos)
np.save("funPathSpeed", self.wayPointsSpeed)
np.save("funPathTime", self.wayPointsTime)
self.rex.planPos = self.wayPointsPos
self.rex.planSpeed = self.wayPointsSpeed
print self.rex.planPos
self.rex.save_data = True
self.rex.plan_status = 1
self.rex.wpt_number = 0
self.rex.wpt_total = len(self.rex.planPos)
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_XL(port_num, 1)
shld = DXL_XL(port_num, 2)
elbw = DXL_XL(port_num, 3)
wrst = DXL_XL(port_num, 4)
grip = DXL_XL(port_num, 5)
"""Objects Using Other Classes"""
self.camera = PiCamera()
self.rexarm = Rexarm((base, shld, elbw, wrst, grip))
self.tp = TrajectoryPlanner(self.rexarm)
self.sm = StateMachine(self.rexarm, self.tp)
self.taskThread = TaskThread(self.sm)
self.rgb_image = None
"""
Attach Functions to Buttons & Sliders
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(
partial(self.sm.set_current_state, "calibrate"))
self.ui.btn_task1.clicked.connect(
partial(self.taskThread.set_task_num, 1))
self.ui.btn_task2.clicked.connect(
partial(self.taskThread.set_task_num, 2))
self.ui.btn_task3.clicked.connect(
partial(self.taskThread.set_task_num, 3))
self.ui.btn_task4.clicked.connect(
partial(self.taskThread.set_task_num, 4))
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.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.camera)
self.videoThread.updateFrame.connect(self.setImage)
self.videoThread.updateAprilTags.connect(self.updateAprilTags)
self.videoThread.updateImage.connect(self.updateImage)
self.videoThread.start()
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
self.taskThread.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)
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 -- add other motors here"""
self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
port_num = self.dxlbus.port()
base = DXL_MX(port_num, 0)
shld = DXL_MX(port_num, 1)
elbw = DXL_MX(port_num, 2)
wrst = DXL_AX(port_num, 3)
wrst2 = DXL_XL(port_num, 4)
grip = DXL_XL(port_num, 5)
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2,grip),0)
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(partial(self.sm.set_next_state, "execute"))
self.ui.btn_cali.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
# self.ui.btnUser1.setText("Calibrate")
# self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
self.ui.btnUser1.setText("Teach and Repeat")
self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "teachNRepeat"))
self.ui.btnUser2.setText("Record Waypoints")
self.ui.btnUser2.clicked.connect(partial(self.sm.set_next_state, "recordWaypoint"))
self.ui.btnUser3.setText("Play")
self.ui.btnUser3.clicked.connect(partial(self.sm.set_next_state, "play"))
self.ui.btnUser4.setText("Block Detection Start")
self.ui.btnUser4.clicked.connect(partial(self.sm.set_next_state, "blockDetectionStart"))
self.ui.btnUser5.setText("Block Message")
self.ui.btnUser5.clicked.connect(partial(self.sm.set_next_state, "blockMessage"))
self.ui.btnUser6.setText("Block Detection End")
self.ui.btnUser6.clicked.connect(partial(self.sm.set_next_state, "blockDetectionEnd"))
self.ui.btnUser7.setText("Click and Grab")
self.ui.btnUser7.clicked.connect(partial(self.sm.set_next_state, "clickNGrab"))
self.ui.btnUser8.setText("Pick n' Place")
self.ui.btnUser8.clicked.connect(partial(self.sm.set_next_state, "pickNPlace"))
self.ui.btnUser9.setText("Pick n' Stack")
self.ui.btnUser9.clicked.connect(partial(self.sm.set_next_state, "pickNStack"))
self.ui.btnUser10.setText("Line 'em Up!")
self.ui.btnUser10.clicked.connect(partial(self.sm.set_next_state, "lineUp"))
self.ui.btnUser11.setText("Stack 'em High!")
self.ui.btnUser11.clicked.connect(partial(self.sm.set_next_state, "stackHigh"))
self.ui.btnUser12.setText("Pyramid Builder!")
self.ui.btnUser12.clicked.connect(partial(self.sm.set_next_state, "buildPyramid"))
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.sldrGrip2.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)))
@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])))
@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 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.sldrGrip1.setText(str(self.ui.sldrGrip1.value()))
# self.ui.sldrGrip2.setText(str(self.ui.sldrGrip2.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.sldrGrip1.value()*D2R,
self.ui.sldrGrip2.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
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]
mouse_coord = [x,y,1]
# affine = [[1,1,1],[1,1,1],[0,0,1]]
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" % (x,y,z))
if (self.kinect.kinectCalibrated):
# Z = self.kinect.worldHeight - 0.1236 * 1000 * np.tan(z/2842.5 + 1.1863)
# world_coord = np.matmul(self.kinect.convert_to_world, mouse_coord)
world_coord = self.kinect.world_coord(x,y)
self.ui.rdoutMouseWorld.setText("(%.0f,%.0f,%.0f)" % (world_coord[0],world_coord[1],world_coord[2]))
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
class Gui(QtGui.QMainWindow):
"""
Main GUI Class
It contains the main function and interfaces between
the GUI and functions
"""
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Main Variables Using Other Classes"""
self.rex = Rexarm(self.ui)
self.video = Video(cv2.VideoCapture(0))
"""
Zhentao added Here.
Initialize the statemachine.
"""
self.statemanager = StateManager(self.rex, self.video)
""" Other Variables """
self.last_click = np.float32([0, 0])
""" Set GUI to track mouse """
QtGui.QWidget.setMouseTracking(self, True)
"""
Video Function
Creates a timer and calls play() function
according to the given time delay (27mm)
"""
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.play)
self._timer.start(27)
"""
LCM Arm Feedback
Creates a timer to call LCM handler continuously
No delay implemented. Reads all time
"""
self._timer2 = QtCore.QTimer(self)
self._timer2.timeout.connect(self.rex.get_feedback)
self._timer2.start()
"""
Connect Sliders to Function
LAB TASK: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI
"""
## TODO: IMPLEMENT GRIP VALUE CONTROLS ##
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.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
""" Commands the arm as the arm initialize to 0,0,0,0 angles """
self.sliderChange()
""" Connect Buttons to Functions
LAB TASK: NAME AND CONNECT BUTTONS AS NEEDED
"""
#This is needed.
self.ui.btnUser1.setText("Affine Calibration")
self.ui.btnUser1.clicked.connect(self.affine_cal)
self.ui.btnUser2.setText("SM Test")
self.ui.btnUser2.clicked.connect(self.iTestSM)
self.ui.btnUser3.setText("Reset Position")
self.ui.btnUser3.clicked.connect(self.rex.iResetPosition)
self.ui.btnUser4.setText("Replay")
self.ui.btnUser4.clicked.connect(self.iReplay)
"""
self.ui.btnUser4.setText("OpenGripper")
self.ui.btnUser4.clicked.connect(self.iTestGripperOpen)
self.ui.btnUser5.setText("CloseGripper")
self.ui.btnUser5.clicked.connect(self.iTestGripperClose)
"""
"""
self.ui.btnUser2.setText("STEP1: Reset Position")
self.ui.btnUser2.clicked.connect(self.iResetPosition)
self.ui.btnUser3.clicked.connect(self.iResetTorqueAndSpeed)
self.ui.btnUser3.setText("STEP2: Reset Torque and Speed")
self.ui.btnUser4.setText("STEP3: Train Begin")
self.ui.btnUser4.clicked.connect(self.iTrainBegin)
self.ui.btnUser5.setText("STEP4(r): GetWayPoint")
self.ui.btnUser5.clicked.connect(self.iGetWayPoint)
self.ui.btnUser5.setEnabled(False)
self.ui.btnUser6.setText("STEP5: Stop Recording")
self.ui.btnUser6.clicked.connect(self.iTrainStop)
self.ui.btnUser6.setEnabled(False)
self.ui.btnUser7.clicked.connect(self.iReplayBegin)
self.ui.btnUser7.setText("Replay WholeWay")
self.ui.btnUser8.clicked.connect(self.iReplayWPTBegin_FAST)
self.ui.btnUser8.setText("Replay WayPoint(FAST)")
self.ui.btnUser9.clicked.connect(self.iReplayWPTBegin_SLOW)
self.ui.btnUser9.setText("Replay WayPoint(SLOW)")
self.ui.btnUser10.setText("PlayStop")
self.ui.btnUser10.clicked.connect(self.iReplayStop)
self.ui.btnUser11.setText("Save WPT Data")
self.ui.btnUser11.clicked.connect(self.iSaveData)
self.ui.btnUser12.setText("Load WPT Data")
self.ui.btnUser12.clicked.connect(self.iLoadData)
"""
def play(self):
self.statemanager.Statemanager_MoveToNextState()
"""
Play Funtion
Continuously called by GUI
"""
""" Renders the Video Frame """
try:
self.video.captureNextFrame()
#self.video.blobDetector()
self.ui.videoFrame.setPixmap(self.video.convertFrame())
self.ui.videoFrame.setScaledContents(True)
except TypeError:
print "No frame"
"""
Update GUI Joint Coordinates Labels on Sliders
LAB TASK: include the other slider labels
"""
self.ui.rdoutBaseJC.setText(
str("%.2f" % (self.rex.joint_angles_fb[0] * R2D)))
self.ui.rdoutShoulderJC.setText(
str("%.2f" % (self.rex.joint_angles_fb[1] * R2D)))
self.ui.rdoutElbowJC.setText(
str("%.2f" % (self.rex.joint_angles_fb[2] * R2D)))
self.ui.rdoutWristJC.setText(
str("%.2f" % (self.rex.joint_angles_fb[3] * R2D)))
"""
Mouse position presentation in GUI
TO DO: after getting affine calibration make the apprriate label
to present the value of mouse position in world coordinates
"""
x = QtGui.QWidget.mapFromGlobal(self, QtGui.QCursor.pos()).x()
y = QtGui.QWidget.mapFromGlobal(self, QtGui.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
self.ui.rdoutMousePixels.setText("(%.0f,%.0f)" % (x, y))
if (self.video.aff_flag == 2):
"""
ZHENTAO: STATE CONTROL: after finishing the camera calibration, move to the next states.
"""
"""
########################################
TED added world_coords label to frame
########################################
"""
world_coords = np.dot(self.video.aff_matrix,
np.array([[x], [y], [1]]))
self.ui.rdoutMouseWorld.setText(
"(%.0f,%.0f)" % (world_coords[0], world_coords[1]))
else:
self.ui.rdoutMouseWorld.setText("(-,-)")
"""
Set button avalibity.
"""
#self.iPrintStatusTerminal()########################################Here should be activated.
self.iSetButtonAbility()
self.iUpdateStatusBar()
"""
Updates status label when rexarm playback is been executed.
This will be extended to include other appropriate messages
"""
"""
if(self.rex.plan_status == 1):
self.ui.rdoutStatus.setText("Playing Back - Waypoint %d"
%(self.rex.wpt_number + 1))
if (self.rex.plan_status == 0 and self.rex.way_total == 0):
self.ui.rdoutStatus.setText("Click [Train Begin] button to start train.")
if (self.rex.plan_status == 2):
self.ui.rdoutStatus.setText("Click [Get Way Point] button to record way point. Click [Stop Recording] to stop recording.")
if (self.rex.plan_status == 0 and self.rex.way_total != 0 and self.rex.way_number == 0):
self.ui.rdoutStatus.setText("Click [Replay Wholeway] to play the whole way. Click [Save Data] to Save the data.")
if (self.rex.plan_status == 5):
self.ui.rdoutStatus.setText("Click [Play Stop] to stop")
"""
"""###############################################
Frank Added Here
###############################################"""
if (self.rex.plan_status == 2):
self.ui.sldrBase.setProperty("value",
self.rex.joint_angles_fb[0] * R2D)
self.ui.sldrShoulder.setProperty("value",
self.rex.joint_angles_fb[1] * R2D)
self.ui.sldrElbow.setProperty("value",
self.rex.joint_angles_fb[2] * R2D)
self.ui.sldrWrist.setProperty("value",
self.rex.joint_angles_fb[3] * R2D)
self.iTrain_AddOneWay()
# replay continuously recorded waypoints
if (self.rex.plan_status == 5):
self.iReplay_PlayOneWay()
# replay manually recorded waypoints
if (self.rex.plan_status == 3 or self.rex.plan_status == 4):
self.iReplayWPT_PlayOneWay()
self.iShowFK()
def sliderChange(self):
"""
Function to change the slider labels when sliders are moved
and to command the arm to the given position
TO DO: Implement for the other sliders
"""
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.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.rex.max_torque = self.ui.sldrMaxTorque.value() / 100.0
self.rex.speed = self.ui.sldrSpeed.value() / 100.0
#self.rex.joint_angles[0] = self.ui.sldrBase.value()*D2R
#self.rex.joint_angles[1] = self.ui.sldrShoulder.value()*D2R
#self.rex.joint_angles[2] = self.ui.sldrElbow.value()*D2R
#elf.rex.joint_angles[3] = self.ui.sldrWrist.value()*D2R
self.rex.joint_angles[4] = self.ui.sldrGrip1.value() * D2R
self.rex.cmd_publish()
def mousePressEvent(self, QMouseEvent):
"""
Function used to record mouse click positions for
affine 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.last_click[0] = x - MIN_X
self.last_click[1] = y - MIN_Y
### TESTING BLOB DETECTION TESTING ###
#self.video.blobDetector()
### TESTING BLOB DETECTION TESTING ###
# if aff_flag is 2, affine transform has been performed
if (self.video.aff_flag == 2):
ik_wcoords = np.dot(self.video.aff_matrix,
np.array([[x - MIN_X], [y - MIN_Y], [1]]))
#self.iTestIK(ik_wcoords[0], ik_wcoords[1], 40,4*PI/4)
"""
#TODO: Temperary here to take the place of Camera that Ted will write.
"""
self.iMimicCamera(ik_wcoords[0], ik_wcoords[1])
""" If affine calibration is being performed """
if (self.video.aff_flag == 1):
""" Save last mouse coordinate """
self.video.mouse_coord[self.video.mouse_click_id] = [(x - MIN_X),
(y - MIN_Y)]
""" Update the number of used poitns for calibration """
self.video.mouse_click_id += 1
""" Update status label text """
self.ui.rdoutStatus.setText("Affine Calibration: Click Point %d" %
(self.video.mouse_click_id + 1))
"""
If the number of click is equal to the expected number of points
computes the affine calibration.
LAB TASK: Change this code to use your affine calibration routine
and NOT openCV pre-programmed function as it is done now.
"""
if (self.video.mouse_click_id == self.video.aff_npoints):
""" Perform affine calibration with OpenCV
self.video.aff_matrix = cv2.getAffineTransform(
self.video.mouse_coord,
self.video.real_coord)
"""
"""
########################################
TED added affineTransform function here
########################################
"""
self.video.aff_matrix = self.video.affineTransform()
self.video.aff_flag = 2
self.video.mouse_click_id = 0
self.video.calculateBoundaryMask()
self.video.calculateArmMask()
self.video.calculateBaseMask()
self.video.calculatePokeballMask()
self.video.generateBoundaryMask()
"""
Update status of calibration flag and number of mouse
clicks
"""
""" Updates Status Label to inform calibration is done """
self.ui.rdoutStatus.setText("Affine Transform Completed")
"""
print affine calibration matrix numbers to terminal
"""
print("[Msg]: Affine Calibration Finished.")
print("[Msg]: Affine Calibration Matrix: "),
print self.video.aff_matrix
def affine_cal(self):
"""
Function called when affine calibration button is called.
Note it only chnage the flag to record the next mouse clicks
and updates the status text label
"""
self.video.aff_flag = 1
self.video.mouse_click_id = 0
self.ui.rdoutStatus.setText("Affine Calibration: Click Point %d" %
(self.video.mouse_click_id + 1))
"""
Button 1: Reset Position
"""
def iResetTorqueAndSpeed(self):
self.rex.iSetTorque(0.0)
self.rex.iSetSpeed(0.1)
self.rex.cmd_publish()
"""
Button 3: Start Train.
"""
def iTrain_ClearRecord(self):
self.rex.wpt = []
self.rex.wpt_number = 0
self.rex.wpt_total = 0
self.rex.way = []
self.rex.way_number = 0
self.rex.way_total = 0
def iTrainBegin(self):
self.rex.iSetTorque(0.0)
self.iTrain_ClearRecord()
self.rex.plan_status = 2
"""
In Play, keep recording.
"""
def iTrain_FetchSensorData(self):
return [
self.rex.joint_angles_fb[0], self.rex.joint_angles_fb[1],
self.rex.joint_angles_fb[2], self.rex.joint_angles_fb[3]
]
# fetch list of sensor data and append to the list of waypoints continuously
def iTrain_AddOneWay(self):
SensorData = self.iTrain_FetchSensorData()
self.rex.way.append(SensorData)
self.rex.way_total = self.rex.way_total + 1 # Have such data point up to now.
"""
Get Way Point:
"""
def iGetWayPoint(self):
SensorData = self.iTrain_FetchSensorData()
currentTime = self.iGetTime_now()
SensorData.append(currentTime)
self.rex.wpt.append(SensorData)
self.rex.wpt_total = self.rex.wpt_total + 1 # Have such data point up to now.
print self.rex.wpt_number
"""
Train stop:
"""
def iTrainStop(self):
self.rex.plan_status = 0
print("Stop Recording!")
"""
Replay
"""
# Begin replaying waypoints that were continuously set
def iReplayBegin(self):
self.rex.iSetTorque(0.5)
self.rex.plan_status = 5
self.rex.way_number = 0
print("Replay Start")
"""
"""
# Stop replaying waypoints and reset waypoint number to 0
def iReplayStop(self):
self.rex.plan_status = 0
self.rex.way_number = 0
self.rex.wpt_number = 0
### TESTING TED ADDED ###
def iReplay_SetOneSensorData(self, valueIndex):
sensorData = self.rex.way[valueIndex]
self.rex.iSetJointAngle(0, sensorData[0])
self.rex.iSetJointAngle(1, sensorData[1])
self.rex.iSetJointAngle(2, sensorData[2])
self.rex.iSetJointAngle(3, sensorData[3])
self.rex.cmd_publish()
# TODO: add comments
def iReplay_PlayOneWay(self):
if (self.rex.way_number == self.rex.way_total):
self.iReplayStop()
else:
self.iReplay_SetOneSensorData(self.rex.way_number)
self.rex.way_number = self.rex.way_number + 1
"Replay WPT"
# begin replaying manually set waypoints at a slow speed
#Slow: plan_status = 3.
def iReplayWPTBegin_SLOW(self):
self.rex.iSetTorque(0.7)
self.rex.iSetSpeed(GLOBALSLOWSPEED)
self.rex.plan_status = 3
self.rex.wpt_number = 0
self.rex.recslow_total = 0
self.rex.recslow = []
#self.calcCubicCoeffs()
# begin replaying manually set waypoints at a fast speed
# Fast mode: plan_status = 4.
def iReplayWPTBegin_FAST(self):
self.rex.iSetTorque(0.7)
self.rex.iSetSpeed(GLOBALFASTSPEED)
self.rex.plan_status = 4
self.rex.wpt_number = 0
self.rex.recfast_total = 0
self.rex.recfast = []
#self.calcCubicCoeffs()
def iReplayWPT_GetSensorData(self):
return [
self.rex.joint_angles_fb[0], self.rex.joint_angles_fb[1],
self.rex.joint_angles_fb[2], self.rex.joint_angles_fb[3]
]
def iCheckIfArrived(self, target, errorTorrance):
sensorData = self.iReplayWPT_GetSensorData()
error0 = abs(target[0] - sensorData[0])
error1 = abs(target[1] - sensorData[1])
error2 = abs(target[2] - sensorData[2])
error3 = abs(target[3] - sensorData[3])
if (error0 < errorTorrance and error1 < errorTorrance
and error2 < errorTorrance and error3 < errorTorrance):
return True
else:
return False
"""
def iReplayWPT_PlayOneWay(self):
if (self.rex.wpt_number == self.rex.wpt_total):
self.iReplayStop()
else:
target = self.rex.wpt[self.rex.wpt_number]
arrived = self.iCheckIfArrived(target,GLOBALERRORTORRANCE)
if (arrived):
self.rex.wpt_number = self.rex.wpt_number + 1
print "CURRENT WAYPOINT SENT TO CALC CUBIC"
print(self.rex.wpt_number),
self.calcCubicCoeffs()
else:
self.cubicPoly()
self.rex.cmd_publish()
"""
def iReplayWPT_PlayOneWay(self):
if (self.rex.wpt_number == self.rex.wpt_total):
self.iReplayStop()
else:
target = self.rex.wpt[self.rex.wpt_number]
arrived = self.iCheckIfArrived(target, GLOBALERRORTORRANCE)
#Record one real time data into self.rex.rec list and self.rex.rec_total += 1.
#SLOW
if (self.rex.plan_status == 3):
mode = 0
#Fast:
elif (self.rex.plan_status == 4):
mode = 1
else:
print("Error: iReplayWPT_PlayOneWay: Undefined status.")
self.iRecord_JointAngleFB(mode)
if (arrived):
self.rex.wpt_number = self.rex.wpt_number + 1
else:
self.rex.iSetJointAngle(0, target[0])
self.rex.iSetJointAngle(1, target[1])
self.rex.iSetJointAngle(2, target[2])
self.rex.iSetJointAngle(3, target[3])
self.rex.cmd_publish()
#self.ui.sldrBase.setProperty("value",self.rex.joint_angles[0]*R2D)
#self.ui.sldrShoulder.setProperty("value",self.rex.joint_angles[1]*R2D)
#self.ui.sldrElbow.setProperty("value",self.rex.joint_angles[2]*R2D)
#self.ui.sldrWrist.setProperty("value",self.rex.joint_angles[3]*R2D)
"""
Button Availibity
"""
def iSetButtonAbility(self):
if (self.statemanager.currentState == STATE_CODE_INIT):
self.ui.btnUser1.setEnabled(1)
else:
self.ui.btnUser1.setEnabled(0)
if (self.statemanager.currentState == STATE_CODE_END):
self.ui.btnUser4.setEnabled(1)
else:
self.ui.btnUser4.setEnabled(0)
"""
if (self.rex.plan_status == 0):
self.ui.btnUser4.setEnabled(True)
self.ui.btnUser5.setEnabled(False)
self.ui.btnUser6.setEnabled(False)
if (self.rex.plan_status == 2):
self.ui.btnUser4.setEnabled(False)
self.ui.btnUser5.setEnabled(True)
self.ui.btnUser6.setEnabled(True)
'''Replay way'''
if (self.rex.plan_status == 0 and self.rex.way_total != 0):
self.ui.btnUser7.setEnabled(True)
else:
self.ui.btnUser7.setEnabled(False)
'''
replay way point.
'''
if (self.rex.plan_status == 0 and self.rex.wpt_total != 0):
self.ui.btnUser8.setEnabled(True)
self.ui.btnUser9.setEnabled(True)
else:
self.ui.btnUser8.setEnabled(False)
self.ui.btnUser9.setEnabled(False)
'''
save data.
'''
if (self.rex.plan_status == 0 and self.rex.way_total != 0):
self.ui.btnUser11.setEnabled(True)
else:
self.ui.btnUser11.setEnabled(False)
'''
Load Data
'''
if (self.rex.plan_status == 0):
self.ui.btnUser12.setEnabled(True)
else:
self.ui.btnUser12.setEnabled(False)
"""
def iPrintStatusTerminal(self):
print("[Status = "),
print(self.rex.plan_status),
print(",#way="),
print(self.rex.way_total),
print(",#way_now="),
print(self.rex.way_number),
print(",#wpt="),
print(self.rex.wpt_total),
print(",#wpt_now="),
print(self.rex.wpt_number),
print(",#recslow="),
print(self.rex.recslow_total),
print(",#recfast="),
print(self.rex.recfast_total),
print("]")
def iShowFK(self):
self.rex.rexarm_FK(self.rex.joint_angles_fb)
"""
print("[x]:"),
print(P0[0][0])
print("[y]:"),
print(P0[1][0])
print("[z]:"),
print(P0[2][0])
print("\n")
"""
self.ui.rdoutX.setText(str(float("{0:.2f}".format(self.rex.P0[0]))))
self.ui.rdoutY.setText(str(float("{0:.2f}".format(self.rex.P0[1]))))
self.ui.rdoutZ.setText(str(float("{0:.2f}".format(self.rex.P0[2]))))
self.ui.rdoutT.setText(str(float("{0:.2f}".format(self.rex.T * R2D))))
# function which sets the self.rex.joint_angles for each joint to the values
# calculated by the cubic polynomials as a function of time
def cubicPoly(self):
if self.rex.wpt_number == self.rex.wpt_total:
self.iReplayStop()
# TODO: break from function here
# TODO: set the start time when at the first waypoint!
for i in range(0, 4):
t = self.iGetTime_now() - self.rex.st
self.rex.joint_angles[i] = (
self.rex.cubic_coeffs[i]
)[0] #+((self.rex.cubic_coeffs[i])[1]*t)+((self.rex.cubic_coeffs[i])[2]*(t**2))+((self.rex.cubic_coeffs[i])[3]*(t**3))
# function which calculates the coefficients for the cubic polynomial function
def calcCubicCoeffs(self):
#NOTE: each array index corresponds to the joint
#TODO: Forward Kinematics + Calculate Time + Set time for moving + set tf.
v0 = [0, 0, 0, 0]
vf = [0, 0, 0, 0]
t0 = [0, 0, 0, 0]
tf = [1, 1, 1, 1]
current_wpt = self.rex.wpt_number
next_wpt = self.rex.wpt_number + 1
q0 = [
self.rex.wpt[current_wpt][0], self.rex.wpt[current_wpt][1],
self.rex.wpt[current_wpt][2], self.rex.wpt[current_wpt][3]
]
qf = [
self.rex.wpt[next_wpt][0], self.rex.wpt[next_wpt][1],
self.rex.wpt[next_wpt][2], self.rex.wpt[next_wpt][3]
]
A = []
b = []
A = list()
b = list()
# NOTE: format is Ax=b where A is the constant waypoint relation matrix,
# x is the unknown coefficients and bi is the [q0,v0,qf,vf] column vector
# for each joint
# For each joint create arrays A and b
# A: list of 4 numpy arrays that are 4x4
# b: list of 4 numpy arrays that are 4x1
# self.rex.cubic_coeffs: list of 4 numpy arrays that are 4x1
for i in range(0, 4):
A.append(
np.array([[1, t0[i], t0[i]**2, t0[i]**3],
[0, 1, 2 * t0[i], 3 * (t0[i]**2)],
[1, tf[i], tf[i]**2, tf[i]**3],
[0, 1, 2 * tf[i], 3 * (tf[i]**2)]]))
b.append(np.array([q0[i], v0[i], qf[i], vf[i]]))
self.rex.cubic_coeffs[i] = np.dot(np.linalg.inv(A[i]), b[i])
#set the start time used by the cubic
self.rex.st = self.iGetTime_now()
def iSaveData(self):
"""
The data variable to save is:
self.wpt = []
self.wpt_total = 0
self.way = []
self.way_total = 0
"""
"""
f = open(GLOBALDFILENAME_WAYNUM,'wt')
writer = csv.writer(f)
writer.writerow([self.rex.way_total])
f.close()
"""
f = open(GLOBALDFILENAME_WAY, 'wt')
writer = csv.writer(f)
for ii in range(self.rex.way_total):
writer.writerow(self.rex.way[ii])
f.close()
"""
f = open(GLOBALDFILENAME_WPTNUM,'wt')
writer = csv.writer(f)
writer.writerow([self.rex.wpt_total])
f.close()
"""
f = open(GLOBALDFILENAME_WPT, 'wt')
writer = csv.writer(f)
for ii in range(self.rex.wpt_total):
writer.writerow(self.rex.wpt[ii])
f.close()
f = open(GLOBALDFILENAME_RECSLOW, 'wt')
writer = csv.writer(f)
for ii in range(self.rex.recslow_total):
writer.writerow(self.rex.recslow[ii])
f.close()
f = open(GLOBALDFILENAME_RECFAST, 'wt')
writer = csv.writer(f)
for ii in range(self.rex.recfast_total):
writer.writerow(self.rex.recfast[ii])
f.close()
print("Saving")
def iLoadData(self):
"""
the data variable to load is:
self.wpt = []
self.wpt_total = 0
self.way = []
self.way_total = 0
"""
self.rex.wpt = []
self.rex.way = []
self.rex.wpt_total = 0
self.rex.way_total = 0
self.rex.wpt_number = 0
self.rex.way_number = 0
datafile = open(GLOBALDFILENAME_WAY, 'r')
datafileReader = csv.reader(datafile)
for ii in datafileReader:
ii = map(float, ii)
self.rex.way.append(ii)
self.rex.way_total = self.rex.way_total + 1
datafile = open(GLOBALDFILENAME_WPT, 'r')
datafileReader = csv.reader(datafile)
for ii in datafileReader:
ii = map(float, ii)
self.rex.wpt.append(ii)
self.rex.wpt_total = self.rex.wpt_total + 1
print("Loading Finished.")
def iGetTime_now(self):
return int(time.time() * 1E6)
# Record the real time joint angle feedback when replaying the way points.
# mdoe == 0: slow mode, 1: fast mode
def iRecord_JointAngleFB(self, mode):
if (mode == 0):
temp = []
#0: Time
temp.append(self.iGetTime_now())
#1-4: Joint angles.
temp.append(self.rex.joint_angles_fb[0])
temp.append(self.rex.joint_angles_fb[1])
temp.append(self.rex.joint_angles_fb[2])
temp.append(self.rex.joint_angles_fb[3])
#5-7: Forward kinematics.
temp.append(self.rex.P0[0])
temp.append(self.rex.P0[1])
temp.append(self.rex.P0[2])
temp.append(self.rex.T)
print(len(self.rex.recslow))
print(temp)
self.rex.recslow.append(temp)
self.rex.recslow_total = self.rex.recslow_total + 1
elif (mode == 1):
temp = []
temp.append(self.iGetTime_now())
temp.append(self.rex.joint_angles_fb[0])
temp.append(self.rex.joint_angles_fb[1])
temp.append(self.rex.joint_angles_fb[2])
temp.append(self.rex.joint_angles_fb[3])
temp.append(self.rex.P0[0])
temp.append(self.rex.P0[1])
temp.append(self.rex.P0[2])
temp.append(self.rex.T)
self.rex.recfast.append(temp)
print(temp)
self.rex.recfast_total = self.rex.recfast_total + 1
"""
A function to test the IK, not need for final competition.
"""
def iMimicCamera(self, x, y):
print("[Msg]: MimicCam is called.")
self.video.numPokRemain = 1
self.video.whetherFinishedCam = True
self.video.nextLocationofPokmon = [x, y]
"""
self.numPokRemain = 0
self.whetherFinishedCam = False;
self.nextLocationofPokmon = [0,0];
"""
def iTestIK(self, x, y, z, phi):
phi = self.rex.rexarm_IK_CatchAnglePlaner([x, y, z])
print("[Msg]: IK is called.")
[
validity_1, IK_conf_1, validity_2, IK_conf_2, validity_3,
IK_conf_3, validity_4, IK_conf_4
] = self.rex.rexarm_IK([x, y, z, phi], 1)
if (validity_1):
self.rex.iSetJointAngle(0, IK_conf_1[0])
self.rex.iSetJointAngle(1, IK_conf_1[1])
self.rex.iSetJointAngle(2, IK_conf_1[2])
self.rex.iSetJointAngle(3, IK_conf_1[3])
self.rex.cmd_publish()
else:
print("[Msg]: IK is not reachable.")
"""
State manager Testing.
"""
def iTestSM(self):
self.statemanager.StateManager_Test()
"""
Update the name of the state to the state bar.
"""
def iUpdateStatusBar(self):
if (self.video.aff_flag == 1):
pass
if (self.statemanager.currentState == STATE_CODE_INIT):
self.ui.rdoutStatus.setText("STATE_CODE_INIT")
if (self.statemanager.currentState == STATE_CODE_RP):
self.ui.rdoutStatus.setText("STATE_CODE_RP")
if (self.statemanager.currentState == STATE_CODE_MTB):
self.ui.rdoutStatus.setText("STATE_CODE_MTB")
if (self.statemanager.currentState == STATE_CODE_MTFT):
self.ui.rdoutStatus.setText("STATE_CODE_MTFT")
if (self.statemanager.currentState == STATE_CODE_CP):
self.ui.rdoutStatus.setText("STATE_CODE_CP")
if (self.statemanager.currentState == STATE_CODE_OG):
self.ui.rdoutStatus.setText("STATE_CODE_OG")
if (self.statemanager.currentState == STATE_CODE_CCACFP):
self.ui.rdoutStatus.setText("STATE_CODE_CCACFP")
if (self.statemanager.currentState == STATE_CODE_END):
self.ui.rdoutStatus.setText("STATE_CODE_END")
if (self.statemanager.currentState == STATE_CODE_RAP):
self.ui.rdoutStatus.setText("STATE_CODE_RAP")
"""
STATE_CODE_INIT = 1
STATE_CODE_CCACFP = 2
STATE_CODE_OG = 3
STATE_CODE_MTFT = 4
STATE_CODE_CP = 5
STATE_CODE_MTB = 6
STATE_CODE_RP = 7
STATE_END = 8
"""
def iTestGripperOpen(self):
self.rex.rexarm_gripper_grab(1)
def iTestGripperClose(self):
self.rex.rexarm_gripper_grab(0)
def iReplay(self):
if (self.statemanager.currentState == STATE_CODE_END):
self.statemanager.currentState = STATE_CODE_INIT
class App(QtWidgets.QMainWindow):
path1 = ""
path2 = ""
path3 = ""
def __init__(self):
super(App, self).__init__()
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self.ui.b1.clicked.connect(self.loaddata1)
self.ui.b3.clicked.connect(self.loaddata2)
self.ui.b4.clicked.connect(self.loaddata3)
self.ui.b2.clicked.connect(self.apply_harris)
self.ui.b5.clicked.connect(self.matching)
def loaddata1(self):
filename = QFileDialog.getOpenFileName(self)
if filename[0]:
self.path1 = filename[0]
image = cv2.imread(self.path1)
self.ui.w1.show()
self.ui.w1.setImage(np.rot90(image,1))
def loaddata2(self):
filename = QFileDialog.getOpenFileName(self)
if filename[0]:
self.path2 = filename[0]
image = cv2.imread(self.path2)
self.ui.w3.show()
self.ui.w3.setImage(np.rot90(image,1))
self.ui.w5.show()
self.ui.w5.setImage(np.rot90(image,1))
def loaddata3(self):
filename = QFileDialog.getOpenFileName(self)
if filename[0]:
self.path3 = filename[0]
image = cv2.imread(self.path3)
self.ui.w4.show()
self.ui.w4.setImage(np.rot90(image,1))
self.ui.w6.show()
self.ui.w6.setImage(np.rot90(image,1))
def apply_harris(self):
image = cv2.imread(self.path1)
img_plot = np.copy(image)
gray = cv2.cvtColor(image,cv2.COLOR_BGR2GRAY)
gray_img = np.float32(gray)
points = self.harris(gray_img)
for point in points:
cv2.circle(img_plot, (point[1],point[0]), 5 , [255,0,255], -1)
cv2.imwrite("out/harris_points.png", img_plot)
out_harris = cv2.imread("out/harris_points.png")
self.ui.w2.show()
self.ui.w2.setImage(np.rot90(out_harris,1))
def harris(self,gray_img):
smothed_image = signal.convolve2d(gray_img, self.gaussian_kernel2d(7,1) ,'same')
Ix = cv2.Sobel(smothed_image,cv2.CV_64F,1,0,ksize=3)
Iy = cv2.Sobel(smothed_image,cv2.CV_64F,0,1,ksize=3)
Ixx = np.multiply( Ix, Ix)
Iyy = np.multiply( Iy, Iy)
Ixy = np.multiply( Ix, Iy)
Ixx_hat = signal.convolve2d( Ixx , self.box_filter(3) ,'same')
Iyy_hat = signal.convolve2d( Iyy , self.box_filter(3) ,'same')
Ixy_hat = signal.convolve2d( Ixy , self.box_filter(3) ,'same')
k = 0.04
detM = np.multiply(Ixx_hat,Iyy_hat) - np.multiply(Ixy_hat,Ixy_hat)
trM = Ixx_hat + Iyy_hat
R = detM - (k * (trM**2))
corners = R > 0.01*R.max()
points_x = np.where(corners==True)[0].reshape(len(np.where(corners==True)[0]),-1)
points_y = np.where(corners==True)[1].reshape(len(np.where(corners==True)[1]),-1)
points = np.concatenate((points_x,points_y),axis = 1)
return points
def apply_sift(self):
image1 = cv2.imread(self.path2)
image1 = cv2.resize(image1, (1000,1000), interpolation = cv2.INTER_AREA)
gray1 = cv2.cvtColor(image1,cv2.COLOR_BGR2GRAY)
gray_img1 = np.float32(gray1)
points1 = self.harris(gray_img1)
discreptors1_points ,discreptors1 = self.sift(points1 , gray_img1,[8,980])
image2 = cv2.imread(self.path3)
image2 = cv2.resize(image2, (300,300), interpolation = cv2.INTER_AREA)
gray2 = cv2.cvtColor(image2,cv2.COLOR_BGR2GRAY)
gray_img2 = np.float32(gray2)
points2 = self.harris(gray_img2)
discreptors2_points ,discreptors2 = self.sift(points2 , gray_img2,[8,280])
return image1 , image2 , discreptors1_points , discreptors1 , discreptors2_points , discreptors2
def sift(self, points,gray_img,limit):
direction_list = [0, 45, 90, 135,180,225,270,315,360]
gaussian_img = convolve2d( gray_img , self.gaussian_kernel2d(7,1.5) , 'same', 'symm')
gauss_kernal = self.gaussian_kernel2d(std = 1.5, kernlen = 16)
gx,gy,magnitude,direction = self.sift_gradient(gaussian_img)
## orinatation & rotate
magnitude = self.patch_orientation(direction,magnitude,limit,points)
## Gaussian with patch sigma=1.5
for x , y in points:
if((x > limit[0] and x < limit[1] ) and ( y > limit[0] and y < limit[1])):
lT_point_x = x - 7
lT_point_y = y - 7
magnitude[lT_point_x:lT_point_x+16,lT_point_y:lT_point_y+16] = magnitude[lT_point_x:lT_point_x+16,lT_point_y:lT_point_y+16] * gauss_kernal
## Get the 4*4 parts & compute descreptors
discreptors_points = []
flag2 = 0
for x , y in points:
if((x > 15 and x < 980 ) and ( y > 15 and y < 980)):
flag1 = 0
for part in self.get_4parts(magnitude , x , y):
if (flag1 == 0):
hist_arr = np.histogram(part, bins = direction_list)[0]
flag1 = 1
else:
hist_arr = np.concatenate((hist_arr,np.array(np.histogram(part, bins = direction_list)[0])))
if(flag2 == 0):
discreptors = hist_arr.reshape(128,-1)
flag2 = 1
else:
discreptors = np.concatenate((discreptors ,hist_arr.reshape(128,-1)),axis = 1)
discreptors_points.append((x,y))
return discreptors_points,discreptors.T
def matching(self):
image1 , image2 , discreptors1_points , discreptors1 , discreptors2_points , discreptors2 = self.apply_sift()
match_arr = np.zeros((discreptors1.shape[0],discreptors2.shape[0]))
for i in range(discreptors1.shape[0]):
for j in range(discreptors2.shape[0]):
match_arr[i,j] = np.sum(np.square(discreptors1[i] - discreptors2[j]))
## SSD match
plot_img1 = np.copy(image1)
plot_img2 = np.copy(image2)
point_des_1 = np.where(match_arr < np.min(match_arr)+80)[0]
point_des_2 = np.where(match_arr < np.min(match_arr)+80)[1]
for index in point_des_1:
cv2.circle(plot_img1, (discreptors1_points[index][1],discreptors1_points[index][0]), 2, [255,0,255], -1)
cv2.imwrite("out/SSD_match_main_image.png", plot_img1)
for index in point_des_2:
cv2.circle(plot_img2, (discreptors2_points[index][1],discreptors2_points[index][0]), 1, [255,0,255], -1)
cv2.imwrite("out/SSD_match_temp_image.png", plot_img2)
out_main_ssd = cv2.imread("out/SSD_match_main_image.png")
self.ui.w3.show()
self.ui.w3.setImage(np.rot90(out_main_ssd,1))
out_temp_ssd = cv2.imread("out/SSD_match_temp_image.png")
self.ui.w4.show()
self.ui.w4.setImage(np.rot90(out_temp_ssd,1))
# Normalize cross correlation match
match_arr_norm = np.zeros((discreptors1.shape[0],discreptors2.shape[0]))
for i in range(discreptors1.shape[0]):
for j in range(discreptors2.shape[0]):
num =((np.sum(np.square(self.norm_data(discreptors1[i]))))*(np.sum(np.square(self.norm_data(discreptors2[j])))))
dom = (np.sum(self.norm_data(discreptors1[i]) * self.norm_data(discreptors2[j])))
match_arr_norm[i,j] = num / dom if dom else 0
plot_n1 = np.copy(image1)
plot_n2 = np.copy(image2)
point_norm_1 = np.where(match_arr_norm > np.max(match_arr_norm)-(4*pow(10,19)))[0]
point_norm_2 = np.where(match_arr_norm > np.max(match_arr_norm)-(4*pow(10,19)))[1]
print(point_norm_1.shape)
for index in point_norm_1:
cv2.circle(plot_n1, (discreptors1_points[index][1],discreptors1_points[index][0]), 2, [255,0,255], -1)
cv2.imwrite("out/norm_match_main_image.png", plot_n1)
for index in point_norm_2:
cv2.circle(plot_n2, (discreptors2_points[index][1],discreptors2_points[index][0]), 1, [255,0,255], -1)
cv2.imwrite("out/norm_match_temp_image.png", plot_n2)
out_main_norm = cv2.imread("out/norm_match_main_image.png")
self.ui.w5.show()
self.ui.w5.setImage(np.rot90(out_main_norm,1))
out_temp_norm = cv2.imread("out/norm_match_temp_image.png")
self.ui.w6.show()
self.ui.w6.setImage(np.rot90(out_temp_norm,1))
def gaussian_kernel1d(self,kernlen=7, std=1.5):
kernel1d = signal.gaussian(kernlen, std=std)
kernel1d = kernel1d.reshape(kernlen, 1)
return kernel1d / kernel1d.sum()
def gaussian_kernel2d(self,kernlen=7, std=1.5):
gkern1d = self.gaussian_kernel1d(kernlen,std)
gkern2d = np.outer(gkern1d, gkern1d)
return gkern2d
def box_filter(self,f):
kernel = np.ones((f,f)) / (f**2)
return kernel
def sift_gradient(self,img):
dx = np.array([[-1,0,1],
[-2,0,2],
[-1,0,1]])
dy = dx.T
gx = signal.convolve2d( img , dx , boundary='symm', mode='same' )
gy = signal.convolve2d( img , dy , boundary='symm', mode='same' )
magnitude = np.sqrt( gx * gx + gy * gy )
direction = np.rad2deg( np.arctan2( gy , gx )) % 360
return gx,gy,magnitude,direction
def get_4parts(self,img,x,y):
parts = []
counter = 0
lT_point_x = x - 7
lT_point_y = y - 7
for i in range(16):
parts.append(img[lT_point_x+counter:lT_point_x+counter+4,lT_point_y+counter:lT_point_y+counter+4])
counter = counter + 4
return parts
def patch_orientation(self,direction,magnitude,limit,points):
direction_list = [0, 45, 90, 135,180,225,270,315,360]
points_orientation = []
for x , y in points:
if((x > limit[0] and x < limit[1] ) and ( y > limit[0] and y < limit[1])):
lT_point_x = x - 7
lT_point_y = y - 7
window = direction[lT_point_x:lT_point_x+16,lT_point_y:lT_point_y+16]
orientation = direction_list[np.argmax(np.histogram(window, bins = direction_list)[0])]
magnitude[lT_point_x:lT_point_x+16,lT_point_y:lT_point_y+16] = ndimage.rotate(magnitude [lT_point_x:lT_point_x+16,lT_point_y:lT_point_y+16], orientation, reshape=False)
return magnitude
def norm_data(self,data):
mean_data=np.mean(data)
std_data=np.std(data, ddof=1)
return (data-mean_data)/(std_data)
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
class MainWindow(QtGui.QMainWindow):
def __init__(self, settings, application=None):
'''
UI Object that draws the main window.
Listens to the following settings:
- delay:
'''
QtGui.QMainWindow.__init__(self)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self._renderwidgets = [
RenderWidget(self),
RenderWidget(self),
RenderWidget(self),
RenderWidget(self),
]
self.update()
self.application = application
self.settings = settings
self.settings.settingChanged.connect(self.settingChanged)
self.ui.delay.setText(str(settings.getSetting("delay")))
self._loadScreens(
self.settings.getSetting("layouts")[self.settings.getSetting(
"selectedlayout")]["screen"])
self._bindings = {}
self._windowstate = None
self._loadBindings(settings.getSetting('keybinding'))
# end __init__()
# signals
incDelay = Signal() #+>=. (while running)
decDelay = Signal() #-<, (while running)
incFrame = Signal() #+>=. (while paused)
decFrame = Signal() #-<, (while paused)
help = Signal() #F1
edit = Signal() #F2
togglePlay = Signal() #F7 <space>
recordBuffer = Signal() #F12
# signals (new)
processFrame = Signal(str, object)
processGroup = Signal(str, object)
resized = Signal()
def resizeEvent(self, *args, **kwargs):
returncode = QtGui.QMainWindow.resizeEvent(self, *args, **kwargs)
self.resized.emit()
return returncode
@Slot(str, object)
def settingChanged(self, name, value):
if name == "delay":
self.ui.delay.setText(str(value))
elif name == "keybinding":
self._loadBindings(value)
elif name == "selectedlayout":
self._loadScreens(
self.settings.getSetting("layouts")[value]["screen"])
## reimplemented ##
def showEvent(self, *args, **kwargs):
if self._windowstate is None:
self._windowstate = self.settings.getSetting("delayanalysisui")
self.setGeometry(*self._windowstate['geometry'])
if self._windowstate['mode'] == 'fullscreen':
self.showFullScreen()
elif self._windowstate['mode'] == 'maximised':
self.showMaximized()
return QtGui.QMainWindow.showEvent(self, *args, **kwargs)
def closeEvent(self, *args, **kwargs):
if self._windowstate is not None:
self._windowstate = self.settings.getSetting("delayanalysisui")
if self._windowstate['savemode'] == True:
if self.windowState() == QtCore.Qt.WindowFullScreen:
self._windowstate['mode'] = "fullscreen"
elif self.windowState() == QtCore.Qt.WindowMaximized:
self._windowstate['mode'] = "maximised"
else:
self._windowstate['mode'] = "normal"
if self._windowstate['savegeometry'] == True:
self._windowstate['geometry'] = self.geometry().getRect()
self.settings.setSetting('delayanalysisui', self._windowstate)
return QtGui.QMainWindow.closeEvent(self, *args, **kwargs)
## support ##
def _loadScreens(self, screens):
self._screens = screens
self._layouts = min(4, len(self._screens))
# print "Layouts: %s" % self._layouts
# print "Screens: %s" % self._screens
for i in range(self._layouts):
self._renderwidgets[i].setLayout(screens[i])
def _loadBindings(self, bindings):
'''
Extracts the actual key ids from the string representations of them provided.
@param bindings: dict containing keybindings {'<keyname>': [('<group>', '<function>', <optionargs>,...), ...]}
'''
try:
groups = {
'core': None,
'processframe': lambda m, c: self.processFrame.emit(m, c),
'processgroup': lambda m, c: self.processGroup.emit(m, c),
}
corefuncs = {
'quit': lambda a, b: self.close(),
'play': lambda a, b: self.togglePlay.emit(),
'incdelay': lambda a, b: self.incDelay.emit(),
'decdelay': lambda a, b: self.decDelay.emit(),
'incframe': lambda a, b: self.incFrame.emit(),
'decframe': lambda a, b: self.decFrame.emit(),
'fullscreen': lambda a, b: self.toggleFullScreen(),
'edit': lambda a, b: self.edit.emit(),
'help': lambda a, b: self.help.emit(),
}
errors = []
self._bindings = {}
for keystr, events in bindings.items():
for event in events:
try:
# convert to integer
keyint = getattr(QtCore.Qt, "Key_%s" % str(keystr))
# get functions to perform tasks
func = groups[event[0]]
if func is None:
func = corefuncs[event[1]]
if keyint in self._bindings:
self._bindings[keyint].append(
(func, event[1], event[2:]))
else:
self._bindings[keyint] = [(func, event[1],
event[2:])]
except AttributeError:
errors.append("Key: %s" % keystr)
except KeyError:
errors.append("Group: %s" % keystr)
if len(errors) > 0:
print "Ignoring the following invalid key bindings (%s)" % (
", ".join(errors))
except:
pass
def toggleFullScreen(self):
'''Toggles between fullscreen and normal mode'''
if not self.isFullScreen():
self.showFullScreen()
else:
self.showNormal()
def keyPressEvent(self, e):
'''Perform tasks for various key events'''
if e.key() in self._bindings:
for binding in self._bindings[e.key()]:
func, action, config = binding
func(action, config)
else:
print "key: %s" % (e.key(), )
def renderFrameset(self, frameset):
'''
Renders the relevent frames from the frameset to screen
'''
for i in range(self._layouts):
self._renderwidgets[i].process(frameset)
# def updateView(self, vid, pixmap):
# '''Updates a view to display given pixmap'''
# self.ui.videoFrame.setPixmap(pixmap)
# self.ui.videoFrame.setScaledContents(True)
def setFrameId(self, frameid):
self.ui.frameNum.setText(frameid)
def setFrameRate(self, framerate):
self.ui.frameRate.setText(framerate)
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_XL(port_num, 1)
shld = DXL_XL(port_num, 2)
elbw = DXL_XL(port_num, 3)
wrst = DXL_XL(port_num, 4)
grip = DXL_XL(port_num, 5)
"""Objects Using Other Classes"""
self.camera = PiCamera()
self.rexarm = Rexarm((base, shld, elbw, wrst, grip))
self.tp = TrajectoryPlanner(self.rexarm)
self.sm = StateMachine(self.rexarm, self.tp)
self.taskThread = TaskThread(self.sm)
self.rgb_image = None
"""
Attach Functions to Buttons & Sliders
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(
partial(self.sm.set_current_state, "calibrate"))
self.ui.btn_task1.clicked.connect(
partial(self.taskThread.set_task_num, 1))
self.ui.btn_task2.clicked.connect(
partial(self.taskThread.set_task_num, 2))
self.ui.btn_task3.clicked.connect(
partial(self.taskThread.set_task_num, 3))
self.ui.btn_task4.clicked.connect(
partial(self.taskThread.set_task_num, 4))
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.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.camera)
self.videoThread.updateFrame.connect(self.setImage)
self.videoThread.updateAprilTags.connect(self.updateAprilTags)
self.videoThread.updateImage.connect(self.updateImage)
self.videoThread.start()
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
self.taskThread.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"""
# Convert image to Qt image for display.
def convertImage(self, image):
qimg = QImage(image, image.shape[1], image.shape[0],
QImage.Format_RGB888)
return QPixmap.fromImage(qimg)
# TODO: Add more QImage in the list for visualization and debugging
@pyqtSlot(list)
def setImage(self, image_list):
rgb_image = image_list[0]
apriltag_image = image_list[1]
block_image = image_list[2]
if (self.ui.radioVideo.isChecked()):
# self.ui.videoDisplay.setPixmap(self.convertImage(rgb_image))
self.rgb_image = rgb_image
self.sm.rgb_image = rgb_image
if (self.ui.radioApril.isChecked()):
self.ui.videoDisplay.setPixmap(self.convertImage(apriltag_image))
if (self.ui.radioUsr1.isChecked()):
self.ui.videoDisplay.setPixmap(self.convertImage(block_image))
@pyqtSlot(list)
def updateAprilTags(self, tags):
# print('UPDATING APRIL TAGS')
self.sm.tags = tags
@pyqtSlot(list)
def updateImage(self, image):
# self.sm.image = image
# detectColor(self.sm, self.sm.image)
pass
@pyqtSlot(list)
def updateJointReadout(self, joints):
self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0] * R2D)))
self.ui.rdoutShoulderJC.setText(str("%+.2f" % (joints[1] * R2D)))
self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2] * R2D)))
self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3] * 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])))
@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_current_state("estop")
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.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.sldrGrip1.value() * D2R
])
self.rexarm.set_positions(joint_positions, update_now=False)
def directControlChk(self, state):
if state == Qt.Checked:
self.sm.set_current_state("manual")
self.ui.SliderFrame.setEnabled(True)
else:
self.sm.set_current_state("idle")
self.ui.SliderFrame.setEnabled(False)
def trackMouse(self):
"""
Mouse position presentation in GUI
TODO: Display the rgb/hsv value
"""
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.rdoutRGB.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,-)" % (x, y))
rgb = self.rgb_image[y, x] # [r, g, b] of this pixel
hsv_image = cv2.cvtColor(self.rgb_image, cv2.COLOR_RGB2HSV)
hsv = hsv_image[y, x]
# self.ui.rdoutRGB.setText("({},{},{})".format(*rgb))
# self.ui.rdoutRGB.setText("({},{},{})".format(*hsv))
self.ui.rdoutRGB.setText(hue_to_classification(hsv[0]))
print(hsv[0])
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
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)
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm([base, shld, elbw, wrst, wrst2], 0)
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
"""
# Currently Changed into Pick & Place
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btn_exec.clicked.connect(self.execute)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(
partial(self.sm.set_next_state, "calibrate"))
self.ui.btnUser2.setText("Wait4Action")
self.ui.btnUser2.clicked.connect(self.wait4action)
self.ui.btnUser3.setText("Detection")
self.ui.btnUser3.clicked.connect(self.detection)
self.ui.btnUser4.setText("Pick")
self.ui.btnUser4.clicked.connect(self.pick)
self.ui.btnUser5.setText("Place")
self.ui.btnUser5.clicked.connect(self.place)
self.ui.btnUser6.setText("QuitTask")
self.ui.btnUser6.clicked.connect(self.quittask)
self.ui.btnUser7.setText("LoadCalibration")
self.ui.btnUser7.clicked.connect(self.loadcalibration)
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.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)
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)
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm([base, shld, elbw, wrst, wrst2], 0)
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
"""
# Currently Changed into Pick & Place
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btn_exec.clicked.connect(self.execute)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(
partial(self.sm.set_next_state, "calibrate"))
self.ui.btnUser2.setText("Wait4Action")
self.ui.btnUser2.clicked.connect(self.wait4action)
self.ui.btnUser3.setText("Detection")
self.ui.btnUser3.clicked.connect(self.detection)
self.ui.btnUser4.setText("Pick")
self.ui.btnUser4.clicked.connect(self.pick)
self.ui.btnUser5.setText("Place")
self.ui.btnUser5.clicked.connect(self.place)
self.ui.btnUser6.setText("QuitTask")
self.ui.btnUser6.clicked.connect(self.quittask)
self.ui.btnUser7.setText("LoadCalibration")
self.ui.btnUser7.clicked.connect(self.loadcalibration)
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.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))))
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 create(self):
self.sm.set_next_state("wait4record")
def record(self):
self.sm.set_next_state("record")
def exit(self):
self.sm.set_next_state("idle")
def execute(self):
print('callback func')
self.sm.set_next_state("execute")
# Pick & Place Relavant Functions
def wait4action(self):
self.sm.set_next_state("wait4action")
def pick(self):
self.sm.set_next_state("pick")
def place(self):
self.sm.set_next_state("place")
def detection(self):
self.sm.set_next_state("detection")
def quittask(self):
self.sm.set_next_state("quittask")
def loadcalibration(self):
# load the calibration result
depth2rgb_affine = np.load("./data/depth2rgb_affine.npy")
extrinsic = np.load("./data/extrinsic.npy")
self.kinect.depth2rgb_affine = depth2rgb_affine
self.kinect.extrinsic = extrinsic
self.kinect.kinectCalibrated = True
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.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]
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" %
(x, y, z))
if not self.kinect.kinectCalibrated:
self.ui.rdoutMouseWorld.setText("(-,-,-)")
else:
w_x, w_y, w_z = self.kinect.toWorldCoord(x, y, z)
self.ui.rdoutMouseWorld.setText("(%.1f,%.1f,%.1f)" %
(w_x, w_y, w_z))
#
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
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)
"""
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'))
self.ui.btn_exec.clicked.connect(self.execute)
self.ui.btnUser2.setText("Reset Waypoints")
self.ui.btnUser2.clicked.connect(self.reset)
self.ui.btnUser3.setText("Add Waypoint")
self.ui.btnUser3.clicked.connect(self.teach)
self.ui.btnUser4.setText("Gripper Toggle")
self.ui.btnUser4.clicked.connect(self.toggle_gripper)
self.ui.btnUser5.setText("Add Color Points")
self.ui.btnUser5.clicked.connect(self.colPoints)
self.ui.btnUser6.setText("Click and Grab")
self.ui.btnUser6.clicked.connect(self.click_grab)
# 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()
class ScoreboardWindow(QtWidgets.QMainWindow):
def __init__(self):
super(ScoreboardWindow,self).__init__()
self.t1=[]
self.t2=[]
self.t3=[]
self.clock=0
self.ui=Ui_MainWindow()
self.ui.setupUi(self)
# 设置表头内容
header1=["iusse","read","execution","write"]
# header2=["LD F6 34,R2",'LD F2 45,R3','MULT F0 F2,F4',\
# 'SUBD F8 F6,F2','DIVD F10 F0,F6','ADDD F6 F8,F2']
self.ui.table1.setHorizontalHeaderLabels(header1)
# self.ui.table1.setVerticalHeaderLabels(header2)
self.ui.table1.horizontalHeader().setSectionResizeMode(QtWidgets.QHeaderView.Stretch)
header3=["busy","op","Fi","Fj",'Fk','Qj','Qk','Rj','Rk']
header4=['Integer','Mult1','Mult2','Add','Divide']
self.ui.table2.setHorizontalHeaderLabels(header3)
self.ui.table2.setVerticalHeaderLabels(header4)
self.ui.table2.horizontalHeader().setSectionResizeMode(QtWidgets.QHeaderView.Stretch)
header5=['F0','F2','F4','F6','F8','F10']
header6=['item']
self.ui.table3.setHorizontalHeaderLabels(header5)
self.ui.table3.setVerticalHeaderLabels(header6)
self.ui.table3.horizontalHeader().setSectionResizeMode(QtWidgets.QHeaderView.Stretch)
self.ui.button1.clicked.connect(self.click_next_step)
self.ui.button_run.clicked.connect(self.run_all)
self.ui.button_reset_current.clicked.connect(self.reset_current_run)
self.ui.button_run_to.clicked.connect(self.run_to_step)
self.ui.button_load.clicked.connect(self.load_instructions)
self.ui.button_load_fromtxt.clicked.connect(self.load_instructions_fromtxt)
def load_instructions_fromtxt(self):
instructions = []
# 初始化这个实例，设置一些基本属性
dlg = QtWidgets.QFileDialog(directory='./',filter="*.txt")
dlg.setFileMode(QtWidgets.QFileDialog.AnyFile)
dlg.setFilter(QtCore.QDir.Files)
# 当选择器关闭的时候
if dlg.exec_():
# 拿到所选择的的文本
filenames = dlg.selectedFiles()
# 读取文本内容设置到TextEdit当中来
# print(filenames)#可以选择多个文件，返回的是一个list
f = open(filenames[0], 'r')
with f:
x=f.readlines()
# print(x)
# .strip('\n').split('\n')
for i in x:
i = i.strip()
if i:
i_list = i.split(',')
if len(i_list) == 4:
i_list.append(2)
i_list.append(0)
instructions.append(i_list)
elif len(i_list) == 5:
i_list[4] = int(i_list[4])
i_list.append(0)
instructions.append(i_list)
else:
instructions=[]
break
if instructions:
self.t1, self.t2, self.t3 = scoreboard(instructions)
self.ui.table1.setRowCount(len(instructions))
header_row=[(','.join(i[0:4]))for i in instructions]
self.ui.table1.setVerticalHeaderLabels(header_row)
QtWidgets.QMessageBox.information(self, '提示', '代码加载成功', QtWidgets.QMessageBox.Yes)
else:
QtWidgets.QMessageBox.warning(self, "警告", "指令读入错误或者文件中没有代码，请重新加载",
QtWidgets.QMessageBox.Yes)
def load_instructions(self):
instructions=self.read_instructions()
if instructions:
self.t1, self.t2, self.t3 = scoreboard(instructions)
self.ui.table1.setRowCount(len(instructions))
header_row=[(','.join(i[0:4]))for i in instructions]
self.ui.table1.setVerticalHeaderLabels(header_row)
QtWidgets.QMessageBox.information(self, '提示', '代码加载成功', QtWidgets.QMessageBox.Yes)
def run_to_step(self):
if self.t1==[]:
QtWidgets.QMessageBox.warning(self,'警告','当前面板没有代码，请点击加载代码',QtWidgets.QMessageBox.Yes)
else:
step=self.ui.step_to.text()
if step=='':
QtWidgets.QMessageBox.information(self, "提示", '请输入步数', QtWidgets.QMessageBox.Yes)
else:
step=int(step)
if step>len(self.t1):
QtWidgets.QMessageBox.information(
self, "提示", "输入的步数"+str(step)+"大于当前代码运行的最大步数，请重新输入", QtWidgets.QMessageBox.Yes
)
elif step==self.clock:
QtWidgets.QMessageBox.information(
self, "提示", "已经运行到"+str(step)+"步，请重新输入步数", QtWidgets.QMessageBox.Yes
)
else:
self.clock=0
self.ui.table1.clearContents()
self.ui.table2.clearContents()
self.ui.table3.clearContents()
for i in range(step):
self.update_table()
def reset_current_run(self):
self.clock=0
self.ui.table1.clearContents()
self.ui.table2.clearContents()
self.ui.table3.clearContents()
self.ui.step_to.setText('')
def read_instructions(self):
result=[]
x=self.ui.instructions.toPlainText().strip('\n').split('\n')
for i in x:
i=i.strip()
i_list=i.split(',')
if len(i_list)==4:
i_list.append(2)
i_list.append(0)
result.append(i_list)
elif len(i_list)==5:
i_list[4]=int(i_list[4])
i_list.append(0)
result.append(i_list)
else:
QtWidgets.QMessageBox.warning(self,"警告","指令输入错误"+str(i),
QtWidgets.QMessageBox.Yes)
return []
return result
def run_all(self):
if len(self.t1)==0:
QtWidgets.QMessageBox.warning(self, '警告', '当前面板没有代码，请点击加载代码', QtWidgets.QMessageBox.Yes)
else:
clock=self.clock
if clock>=len(self.t1):
QtWidgets.QMessageBox.information(self, "提示", "代码运行完成，如果需要请重新开始",
QtWidgets.QMessageBox.Yes)
else:
while clock<len(self.t1):
self.update_table()
clock+=1
def update_table(self):
clock = self.clock
if clock < len(self.t1):
count = 0
while count < len(self.t1[clock]):
if self.t1[clock][count][0]:
a = QtWidgets.QTableWidgetItem(str(self.t1[clock][count][0]))
a.setTextAlignment(QtCore.Qt.AlignCenter)
self.ui.table1.setItem(count, 0, a)
if self.t1[clock][count][1]:
a = QtWidgets.QTableWidgetItem(str(self.t1[clock][count][1]))
a.setTextAlignment(QtCore.Qt.AlignCenter)
self.ui.table1.setItem(count, 1, a)
if self.t1[clock][count][2]:
a = QtWidgets.QTableWidgetItem(str(self.t1[clock][count][2]))
a.setTextAlignment(QtCore.Qt.AlignCenter)
self.ui.table1.setItem(count, 2, a)
if self.t1[clock][count][3]:
a = QtWidgets.QTableWidgetItem(str(self.t1[clock][count][3]))
a.setTextAlignment(QtCore.Qt.AlignCenter)
self.ui.table1.setItem(count, 3, a)
count += 1
for i, item in enumerate(self.t2[clock][0]):
a = QtWidgets.QTableWidgetItem(str(item))
a.setTextAlignment(QtCore.Qt.AlignCenter)
self.ui.table2.setItem(0, i, a)
for i, item in enumerate(self.t2[clock][1]):
a = QtWidgets.QTableWidgetItem(str(item))
a.setTextAlignment(QtCore.Qt.AlignCenter)
self.ui.table2.setItem(1, i, a)
for i, item in enumerate(self.t2[clock][2]):
a = QtWidgets.QTableWidgetItem(str(item))
a.setTextAlignment(QtCore.Qt.AlignCenter)
self.ui.table2.setItem(2, i, a)
for i, item in enumerate(self.t2[clock][3]):
a = QtWidgets.QTableWidgetItem(str(item))
a.setTextAlignment(QtCore.Qt.AlignCenter)
self.ui.table2.setItem(3, i, a)
# if self.t2[clock]['Divide'][0]:
for i, item in enumerate(self.t2[clock][4]):
a = QtWidgets.QTableWidgetItem(str(item))
a.setTextAlignment(QtCore.Qt.AlignCenter)
self.ui.table2.setItem(4, i, a)
a = QtWidgets.QTableWidgetItem(str(self.t3[clock]['F0']))
a.setTextAlignment(QtCore.Qt.AlignCenter)
self.ui.table3.setItem(0, 0, a)
a = QtWidgets.QTableWidgetItem(str(self.t3[clock]['F2']))
a.setTextAlignment(QtCore.Qt.AlignCenter)
self.ui.table3.setItem(0, 1, a)
a = QtWidgets.QTableWidgetItem(str(self.t3[clock]['F4']))
a.setTextAlignment(QtCore.Qt.AlignCenter)
self.ui.table3.setItem(0, 2, a)
a = QtWidgets.QTableWidgetItem(str(self.t3[clock]['F6']))
a.setTextAlignment(QtCore.Qt.AlignCenter)
self.ui.table3.setItem(0, 3, a)
a = QtWidgets.QTableWidgetItem(str(self.t3[clock]['F8']))
a.setTextAlignment(QtCore.Qt.AlignCenter)
self.ui.table3.setItem(0, 4, a)
a = QtWidgets.QTableWidgetItem(str(self.t3[clock]['F10']))
a.setTextAlignment(QtCore.Qt.AlignCenter)
self.ui.table3.setItem(0, 5, a)
self.ui.text3.setText(str(clock + 1))
self.clock += 1
else:
QtWidgets.QMessageBox.information(self, "提示", "代码运行完成，如果需要请重新开始",
QtWidgets.QMessageBox.Yes)
def click_next_step(self):
if self.t1==[]:
QtWidgets.QMessageBox.warning(self,'警告','当前面板没有代码，请点击加载代码',QtWidgets.QMessageBox.Yes)
else:
self.update_table()
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)
gripper1 = DXL_XL(port_num, 6)
gripper2 = DXL_XL(port_num, 7)
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm(
(base, shld, elbw, wrst, wrst2, gripper1, gripper2), 0)
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_teach.clicked.connect(self.teach)
self.ui.btn_repeat.clicked.connect(self.repeat)
self.ui.btn_rls.clicked.connect(self.rls)
self.ui.btn_grab.clicked.connect(self.grab)
self.ui.btn_drop_off.clicked.connect(self.drop_off)
self.ui.btn_move.clicked.connect(self.move)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(
partial(self.sm.set_next_state, "calibrate"))
self.ui.btnUser2.clicked.connect(
partial(self.sm.set_next_state, "event_1"))
self.ui.btnUser3.clicked.connect(
partial(self.sm.set_next_state, "event_2"))
self.ui.btnUser4.clicked.connect(
partial(self.sm.set_next_state, "event_3"))
self.ui.btnUser5.clicked.connect(
partial(self.sm.set_next_state, "event_4"))
self.ui.btnUser6.clicked.connect(
partial(self.sm.set_next_state, "event_5"))
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.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))))
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 move(self):
self.sm.set_next_state("move")
def grab(self):
self.sm.set_next_state("grab")
def drop_off(self):
self.sm.set_next_state("drop_off")
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 repeat(self):
self.sm.set_next_state("repeat")
def rls(self):
self.sm.set_next_state("rls")
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.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.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):
d = self.kinect.currentDepthFrame[y][x] # depth value
d = np.clip(d, 0, 2**10 - 1)
Zc = 0.1236 * math.tan(d / 2842.5 + 1.1863)
# print Zc
# print('----control station intrinsic_matrix')
# print self.kinect.intrinsic_matrix
XYZ_camera = Zc * np.matmul(
np.linalg.inv(self.kinect.intrinsic_matrix),
np.array([x, y, 1]))
# print('----control station co_eff_camera_2_world')
# print self.kinect.co_eff_camera_2_world
W = np.matmul(self.kinect.co_eff_camera_2_world,
np.append(XYZ_camera, 1))
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" %
(x, y, d))
# W = np.matmul(self.sm.coeff_rgb_2_world, np.array([x, y, 1]))
#linear fitting
# d = 2047-z
# 2047 - 718 --> 0
# 2047 - 705 --> 38
# 2047 - 688 --> 38*2
# 2047 - 668 --> 38*3
# 2047 - 646 --> 38*4
# 2047 - 624 --> 38*5
# 2047 - 598 --> 38*6
# 2047 - 570 --> 38*7
# 2047 - 538 --> 38*8
# 2047 - 501 --> 38*9
# 2047 - 462 --> 38*10
# need better calibration function
# W[2] = (8.00506778e-06)*d**3-(3.79099906e-02)*d**2 + (6.08296089e+01)*d - (3.26712433e+04)
#W[2] = (1.46565657e+00)*d - (1.91508256e+03)
#W[2] = (-4.15178471e-08)*d**4 + (2.49769770e-04)*d**3 - (5.65159066e-01)*d**2 + (5.71205622e+02)*d - (2.17696573e+05)
self.ui.rdoutMouseWorld.setText("(%.0f,%.0f,%.0f)" %
(W[0], W[1], W[2]))
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
#print(self.kinect.last_click)
d = self.kinect.currentDepthFrame[self.kinect.last_click[1]][
self.kinect.last_click[0]] # depth value
d = np.clip(d, 0, 2**10 - 1)
Zc = 0.1236 * math.tan(d / 2842.5 + 1.1863)
XYZ_camera = Zc * np.matmul(
np.linalg.inv(self.kinect.intrinsic_matrix),
np.array([self.kinect.last_click[0], self.kinect.last_click[1], 1
]))
W = np.matmul(self.kinect.co_eff_camera_2_world,
np.append(XYZ_camera, 1))
self.sm.world_mouse = deepcopy(W[:3])
def __init__(self):
super(TranslateText, self).__init__()
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self.init_UI()
class QMain(QMainWindow):
def __init__(self):
super(QMain, self).__init__()
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
# 过滤掉未获取到IP地址的适配器
self.ip_address_list = sorted(
[a for a in self.get_all_ip_address() if a[:3] != '169'])
self.ui.listenaddress.addItems(self.ip_address_list)
self.ui.connectaddress.addItems(self.ip_address_list)
self.update_network_list = MySignals()
# 信号连接槽，需要更新端口转发地址时发送信号触发函数调用
self.update_network_list.signal.connect(self.get_all_network) # noqa
self.get_all_network()
self.check_service_status()
@staticmethod
def get_all_ip_address():
"""
获取本机所有的IP地址
:return: IP地址列表
"""
return [
ifaddresses(a)[AF_INET][0]['addr'] for a in interfaces()
if (AF_INET in ifaddresses(a))
]
def start(self):
"""
运行程序
:return: None
"""
network = self.ui.network.currentText()
listen_address = self.ui.listenaddress.currentText()
listen_port = self.ui.listenport.text()
connect_address = self.ui.connectaddress.currentText()
connect_port = self.ui.connectport.text()
if self.check_ip_port(listen_address, listen_port, connect_address,
connect_port):
command = 'netsh interface portproxy add %s listenaddress=%s listenport=%s connectaddress=%s connectport=%s' # noqa
command = command % (network, listen_address, listen_port,
connect_address, connect_port)
self.run_and_check_result(command)
# 执行命令结束发出信号更新network_list
self.update_network_list.signal.emit(True) # noqa
def check_ip_port(self, listen_address, listen_port, connect_address,
connect_port):
"""
检测IP地址和端口是否合法
:param listen_address: 监听地址
:param listen_port: 监听端口
:param connect_address: 连接地址
:param connect_port: 连接端口
:return: bool
"""
if not (listen_address and listen_port and connect_address
and connect_port):
text = ''
if not listen_address:
text += '监听地址，'
if not listen_port:
text += '监听端口，'
if not connect_address:
text += '连接地址，'
if not connect_port:
text += '连接端口，'
QMessageBox.warning(self, '提示', f'{text[:-1]}不能为空！',
QMessageBox.Ok, QMessageBox.Ok)
return False
rule_ipaddress = re.compile(
r'^((2(5[0-5]|[0-4]\d))|[0-1]?\d{1,2})(\.((2(5[0-5]|[0-4]\d))|[0-1]?\d{1,2})){3}$'
)
rule_port = re.compile(
r'^[1-9]$|(^[1-9][0-9]$)|(^[1-9][0-9][0-9]$)|(^[1-9][0-9][0-9][0-9]$)|(^[1-6][0-9][0-9][0-9][0-9]$)'
)
if rule_ipaddress.match(listen_address) and rule_ipaddress.match(connect_address) and \
rule_port.match(listen_port) and rule_port.match(connect_port):
if 1 <= int(listen_port) <= 65535 and 1 <= int(
connect_port) <= 65535:
return True
else:
if not (1 <= int(listen_port) <= 65535):
QMessageBox.warning(self, '提示', '监听端口不合法！', QMessageBox.Ok,
QMessageBox.Ok)
return False
if not (1 <= int(connect_port) <= 65535):
QMessageBox.warning(self, '提示', '连接端口不合法！', QMessageBox.Ok,
QMessageBox.Ok)
return False
else:
text = ''
if not rule_ipaddress.match(listen_address):
text += '监听地址，'
if not rule_ipaddress.match(connect_address):
text += '连接地址，'
if not rule_port.match(listen_port):
text += '监听端口，'
if not rule_port.match(connect_port):
text += '连接端口，'
QMessageBox.warning(self, '提示', f'{text[:-1]}不合法！', QMessageBox.Ok,
QMessageBox.Ok)
return False
def run_and_check_result(self, command):
"""
运行程序并且检测返回结果
:param command: 需要运行的命令
:return: None
"""
stdout_data, stderr_data = subprocess.Popen(
command,
stdout=subprocess.PIPE,
stderr=subprocess.PIPE,
stdin=subprocess.PIPE, # GUI程序中记得设置标准输入
shell=True).communicate()
stdout_data = stdout_data.decode('gbk')
stderr_data = stderr_data.decode('gbk')
if stdout_data == '请求的操作需要提升(作为管理员运行)。\n\r\n':
QMessageBox.warning(self, '提示', '请使用管理员权限运行程序！', QMessageBox.Ok,
QMessageBox.Ok)
elif stdout_data == '\r\n' and stderr_data == '':
QMessageBox.information(self, '提示', 'OK!', QMessageBox.Ok,
QMessageBox.Ok)
else:
QMessageBox.information(self, '提示',
f'{stdout_data}\r\n{stderr_data}',
QMessageBox.Ok, QMessageBox.Ok)
def edit_item(self, item: QListWidgetItem):
"""
编辑IP和端口
:param item: 选中的item
:return: None
"""
src, des = item.text().split(' -> ')
src_ip, src_port = src.split(':')
des_ip, des_port = des.split(':')
self.ui.listenaddress.setCurrentText(src_ip)
self.ui.listenport.setText(src_port)
self.ui.connectaddress.setCurrentText(des_ip)
self.ui.connectport.setText(des_port)
def get_all_network(self):
"""
获取所有的网络适配器中包含的地址
:return: None
"""
# 先清空之前的值，再更新
self.ui.network_list.clear()
parent = r'(\S+?)\s+?(\d+?) \s+?(\S+?)\s+?(\d+?)\s+?'
get_ip_port = re.compile(parent, re.VERBOSE)
command = 'netsh interface portproxy show all' # noqa
stdout_data, stderr_data = subprocess.Popen(command,
stdout=subprocess.PIPE,
stderr=subprocess.PIPE,
stdin=subprocess.PIPE,
shell=True).communicate()
stdout_data, stderr_data = stdout_data.decode(
'gbk'), stderr_data.decode('gbk')
result = re.findall(get_ip_port, stdout_data)
for a in result:
self.ui.network_list.addItem(f'{a[0]}:{a[1]} -> {a[2]}:{a[3]}')
def my_listwidget_context(self, position): # noqa
"""
右键菜单
:param position: 选中的条目索引
:return: None
"""
# 只有在选中item时才会出现菜单
if self.ui.network_list.itemAt(position):
menu = QMenu()
del_act = QAction("删除", self)
del_act.triggered.connect(self.del_network) # noqa
menu.addAction(del_act)
menu.exec_(self.ui.network_list.mapToGlobal(position))
def del_network(self):
"""
删除数据
:return: None
"""
ip, port = self.ui.network_list.currentItem().text().split(
' -> ')[0].split(':')
command = f'netsh interface portproxy delete {self.ui.network.currentText()} listenaddress={ip} listenport={port}' # noqa
self.run_and_check_result(command)
# 执行命令结束发出信号更新network_list
self.update_network_list.signal.emit(True) # noqa
def check_service_status(self):
"""
检查端口转发所需服务状态
:return: None
"""
command = 'sc query iphlpsvc' # noqa
stdout_data, stderr_data = subprocess.Popen(command,
stdin=subprocess.PIPE,
stdout=subprocess.PIPE,
stderr=subprocess.PIPE,
shell=True).communicate()
stdout_data, stderr_data = stdout_data.decode(
'gbk'), stderr_data.decode('gbk')
print(stdout_data, stderr_data)
if stdout_data:
service_status = re.compile(r'STATE\s+?:\s+?(\d)').search(
stdout_data).group(1)
if service_status == '4':
self.setWindowTitle(f'{self.windowTitle()} 服务状态：运行')
return
if service_status == '1':
self.setWindowTitle(f'{self.windowTitle()} 服务状态：停止')
return
else:
self.setWindowTitle(f'{self.windowTitle()} 服务状态：未知')
return
else:
self.setWindowTitle(f'{self.windowTitle()} 服务状态：未知')
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()
"""
467TODO:
Based on motors we are using, modify ports and definition of rexarm
Assume only use shld, elbw, wrst for now
"""
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, 1)
"""Objects Using Other Classes"""
self.kinect = None #Kinect()
#self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2),0)
self.rexarm = Rexarm((base,shld,elbw, wrst),0)
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.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.sldrWrist2.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)
import sys from PyQt5.QtCore import * from PyQt5.QtGui import * from PyQt5.QtWidgets import * from ui import Ui_MainWindow QApplication.setAttribute(Qt.AA_EnableHighDpiScaling) app = QApplication(sys.argv) window = QMainWindow() Ui_MainWindow().setupUi(window) window.show() sys.exit(app.exec_())
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 -- add other motors here"""
self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
port_num = self.dxlbus.port()
base = DXL_MX(port_num, 0)
shld = DXL_MX(port_num, 1)
elbw = DXL_MX(port_num, 2)
wrst = DXL_AX(port_num, 3)
wrst2 = DXL_XL(port_num, 4)
grip = DXL_XL(port_num, 5)
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2,grip),0)
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(partial(self.sm.set_next_state, "execute"))
self.ui.btn_cali.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
# self.ui.btnUser1.setText("Calibrate")
# self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
self.ui.btnUser1.setText("Teach and Repeat")
self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "teachNRepeat"))
self.ui.btnUser2.setText("Record Waypoints")
self.ui.btnUser2.clicked.connect(partial(self.sm.set_next_state, "recordWaypoint"))
self.ui.btnUser3.setText("Play")
self.ui.btnUser3.clicked.connect(partial(self.sm.set_next_state, "play"))
self.ui.btnUser4.setText("Block Detection Start")
self.ui.btnUser4.clicked.connect(partial(self.sm.set_next_state, "blockDetectionStart"))
self.ui.btnUser5.setText("Block Message")
self.ui.btnUser5.clicked.connect(partial(self.sm.set_next_state, "blockMessage"))
self.ui.btnUser6.setText("Block Detection End")
self.ui.btnUser6.clicked.connect(partial(self.sm.set_next_state, "blockDetectionEnd"))
self.ui.btnUser7.setText("Click and Grab")
self.ui.btnUser7.clicked.connect(partial(self.sm.set_next_state, "clickNGrab"))
self.ui.btnUser8.setText("Pick n' Place")
self.ui.btnUser8.clicked.connect(partial(self.sm.set_next_state, "pickNPlace"))
self.ui.btnUser9.setText("Pick n' Stack")
self.ui.btnUser9.clicked.connect(partial(self.sm.set_next_state, "pickNStack"))
self.ui.btnUser10.setText("Line 'em Up!")
self.ui.btnUser10.clicked.connect(partial(self.sm.set_next_state, "lineUp"))
self.ui.btnUser11.setText("Stack 'em High!")
self.ui.btnUser11.clicked.connect(partial(self.sm.set_next_state, "stackHigh"))
self.ui.btnUser12.setText("Pyramid Builder!")
self.ui.btnUser12.clicked.connect(partial(self.sm.set_next_state, "buildPyramid"))
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.sldrGrip2.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)
class MainWindow:
def __init__(self, studentIDS):
global prereq
global isPrereqFor
isPrereqFor = {}
prereq = {}
self.main_win = QMainWindow()
self.ui = Ui_MainWindow()
self.ui.setupUi(self.main_win)
self.ui.listWidget.addItems(studentIDS)
self.ui.pushButton.clicked.connect(self.pushButton_click)
prereq_file = pd.read_excel(PREREQ_LIST, skiprows=1)
self.formPrereq()
def show(self):
self.main_win.show()
def pushButton_click(self):
global student_id
student_id = self.ui.listWidget.currentItem().text().split('-')[1]
global student_name
student_name = self.ui.listWidget.currentItem().text().split('-')[0]
global main_win
main_win = SecondPage()
main_win.show()
def formPrereq(self):
prereq_file = pd.read_excel(PREREQ_LIST, skiprows=1)
or_and = ""
for i in range(len(prereq_file['Subject'])):
course1 = prereq_file['Subject'][i].strip(
) + prereq_file['Catalog'][i].strip()
if str(prereq_file['preq1 subject'][i]) == 'nan':
continue
l = prereq_file['preq1 subject'][i].strip(
) + prereq_file['preq1 catalog'][i].strip()
prereq[course1] = [l]
if l not in isPrereqFor:
isPrereqFor[l] = [course1]
else:
isPrereqFor[l].append(course1)
#######################################################################
if str(prereq_file['preq2 sub'][i]) == 'nan':
continue
l = prereq_file['preq2 sub'][i].strip(
) + prereq_file['preq2 cat'][i].strip()
prereq[course1].append(l)
if l not in isPrereqFor:
isPrereqFor[l] = [course1]
else:
isPrereqFor[l].append(course1)
#######################################################################
if str(prereq_file['preq3 no'][i]) == 'nan':
continue
l = prereq_file['preq3 no'][i].strip(
) + prereq_file['preq3 cat'][i].strip()
prereq[course1].append(l)
if l not in isPrereqFor:
isPrereqFor[l] = [course1]
else:
isPrereqFor[l].append(course1)
#######################################################################
if str(prereq_file['preq4 no'][i]) == 'nan':
continue
l = prereq_file['preq4 no'][i].strip(
) + prereq_file['preq4 cat'][i].strip()
prereq[course1].append(l)
if l not in isPrereqFor:
isPrereqFor[l] = [course1]
else:
isPrereqFor[l].append(course1)
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Main Variables Using Other Classes"""
self.rex = Rexarm(self.ui)
self.video = Video(cv2.VideoCapture(0))
"""
Zhentao added Here.
Initialize the statemachine.
"""
self.statemanager = StateManager(self.rex, self.video)
""" Other Variables """
self.last_click = np.float32([0, 0])
""" Set GUI to track mouse """
QtGui.QWidget.setMouseTracking(self, True)
"""
Video Function
Creates a timer and calls play() function
according to the given time delay (27mm)
"""
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.play)
self._timer.start(27)
"""
LCM Arm Feedback
Creates a timer to call LCM handler continuously
No delay implemented. Reads all time
"""
self._timer2 = QtCore.QTimer(self)
self._timer2.timeout.connect(self.rex.get_feedback)
self._timer2.start()
"""
Connect Sliders to Function
LAB TASK: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI
"""
## TODO: IMPLEMENT GRIP VALUE CONTROLS ##
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.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
""" Commands the arm as the arm initialize to 0,0,0,0 angles """
self.sliderChange()
""" Connect Buttons to Functions
LAB TASK: NAME AND CONNECT BUTTONS AS NEEDED
"""
#This is needed.
self.ui.btnUser1.setText("Affine Calibration")
self.ui.btnUser1.clicked.connect(self.affine_cal)
self.ui.btnUser2.setText("SM Test")
self.ui.btnUser2.clicked.connect(self.iTestSM)
self.ui.btnUser3.setText("Reset Position")
self.ui.btnUser3.clicked.connect(self.rex.iResetPosition)
self.ui.btnUser4.setText("Replay")
self.ui.btnUser4.clicked.connect(self.iReplay)
"""
self.ui.btnUser4.setText("OpenGripper")
self.ui.btnUser4.clicked.connect(self.iTestGripperOpen)
self.ui.btnUser5.setText("CloseGripper")
self.ui.btnUser5.clicked.connect(self.iTestGripperClose)
"""
"""
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)
grip = DXL_XL(port_num, 7)
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm((base, shld, elbw, wrst, wrst2, wrst3), grip)
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.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(
partial(self.sm.set_next_state, "calibrate"))
self.ui.btnUser2.setText("Waypoint")
self.ui.btnUser2.clicked.connect(
partial(self.sm.set_next_state, "waypoint"))
self.ui.btnUser3.setText("Teach")
self.ui.btnUser3.clicked.connect(
partial(self.sm.set_next_state, "teach"))
self.ui.btnUser4.setText("Record")
self.ui.btnUser4.clicked.connect(
partial(self.sm.set_next_state, "record"))
self.ui.btnUser5.setText("Repeat")
self.ui.btnUser5.clicked.connect(
partial(self.sm.set_next_state, "repeat"))
self.ui.btn_task1.setText("Pick n Stack")
self.ui.btn_task1.clicked.connect(partial(self.sm.set_next_state,
"IK"))
self.ui.btnUser6.setText("Click and Place")
self.ui.btnUser6.clicked.connect(
partial(self.sm.set_next_state, "clickgrab"))
self.ui.btnUser7.setText("Open Gripper")
self.ui.btnUser7.clicked.connect(
partial(self.sm.set_next_state, "open_gripper"))
self.ui.btnUser8.setText("Close Gripper")
self.ui.btnUser8.clicked.connect(
partial(self.sm.set_next_state, "close_gripper"))
self.ui.btnUser9.setText("Incline")
self.ui.btnUser9.clicked.connect(
partial(self.sm.set_next_state, "incline"))
self.ui.btn_task2.setText("Line Em Up")
self.ui.btn_task2.clicked.connect(
partial(self.sm.set_next_state, "color_sort"))
self.ui.btn_task3.setText("Stack Em High")
self.ui.btn_task3.clicked.connect(
partial(self.sm.set_next_state, "color_stack"))
self.ui.btn_task4.setText("Block Mover")
self.ui.btn_task4.clicked.connect(
partial(self.sm.set_next_state, "square"))
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, QImage)
def setImage(self, rgb_image, depth_image, gray_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))
if (self.ui.radioUsr1.isChecked()):
self.ui.videoDisplay.setPixmap(QPixmap.fromImage(gray_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 waypoint(self):
# self.sm.set_next_state("waypoint")
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.sldrGrip1.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]
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" %
(x, y, z))
if self.kinect.calibrated == True:
mouse = np.array([[x], [y], [1]], dtype=float)
depth = (0.1236 * np.tan((z) / 2842.5 + 1.1863))
camera = depth * np.matmul(inv(self.kinect.camera_matrix),
mouse)
points = np.array([[camera[0]], [camera[1]], [1]])
world = np.matmul(self.kinect.affineB.astype(float),
points.astype(float))
#world = np.matmul(self.kinect.affineB.astype(float),mouse)
#depth = -2.5333*z + 1836.7
board_depth = 0.1236 * np.tan(718 / 2842.5 + 1.1863)
#depth = (1-depth/board_depth)*1000
depth = (board_depth - depth) * 1000
"""
correction_factor = 0.000626
world[0]=world[0]*(1-correction_factor*depth)
world[1]=world[1]*(1-correction_factor*depth)
"""
#self.ui.rdoutMouseWorld.setText("(%3.2f,%3.2f,%3.2f)" % (camera[0],camera[1],camera[2]))
self.ui.rdoutMouseWorld.setText(
"(%.0f,%.0f,%.0f)" % (world[0], world[1], depth))
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
class mywindow(QtWidgets.QMainWindow):
# включение базы данных
db = Databass()
def __init__(self):
super(mywindow, self).__init__()
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self.ui.tableWidget.setColumnCount(5)
self.ui.tableWidget.setHorizontalHeaderLabels(
("ID", "Название", "Режисер", "год выпуска", "Страна"))
# добавление значений в комбо боксы
self.ui.comboBox.addItems(("название", "режисер", "год", "страна"))
self.ui.pole.addItems(("название", "режисер", "год", "страна"))
# подключение кнопочек
self.ui.save.clicked.connect(self.saveClicked)
self.ui.load.clicked.connect(self.loadClicked)
self.ui.change.clicked.connect(self.changeClicked)
self.ui.find.clicked.connect(self.findClicked)
self.ui.delete_2.clicked.connect(self.deleteClicked)
self.ui.pushButton.clicked.connect(self.addClicked)
def saveClicked(self):
self.db.file_name = self.ui.file_name.text()
self.db.save()
def loadClicked(self):
self.db.file_name = self.ui.file_name.text()
self.db.load()
self.ui.tableWidget.setRowCount(len(self.db.get_id()))
for i in self.db.get_id():
self.ui.id.addItem(str(i))
self.ui.id_2.addItem(str(i))
for i in range(len(self.db.get_id())):
info = QtWidgets.QTableWidgetItem(str(self.db.get_id()[i]))
self.ui.tableWidget.setItem(i, 0, info)
info = QtWidgets.QTableWidgetItem(self.db.get_name()[i])
self.ui.tableWidget.setItem(i, 1, info)
info = QtWidgets.QTableWidgetItem(self.db.get_rej()[i])
self.ui.tableWidget.setItem(i, 2, info)
info = QtWidgets.QTableWidgetItem(self.db.get_year()[i])
self.ui.tableWidget.setItem(i, 3, info)
info = QtWidgets.QTableWidgetItem(self.db.get_country()[i])
self.ui.tableWidget.setItem(i, 4, info)
for j in range(5):
self.ui.tableWidget.item(i, j).setBackground(
QtGui.QColor(255, 255, 255))
def changeClicked(self):
id = int(self.ui.id.currentText())
pole = self.ui.pole.currentText()
print(pole)
word = self.ui.word.text()
print(word)
self.db.change(id, pole, word)
self.ui.id.clear()
self.ui.id_2.clear()
self.loadClicked()
def findClicked(self):
for i in range(len(self.db.get_id())):
for j in range(5):
self.ui.tableWidget.item(i, j).setBackground(
QtGui.QColor(255, 255, 255))
pole = self.ui.comboBox.currentText()
word = self.ui.lineEdit.text()
id = self.db.find(pole, word)
for i in range(5):
self.ui.tableWidget.item(id,
i).setBackground(QtGui.QColor(0, 0, 255))
def deleteClicked(self):
id = int(self.ui.id_2.currentText())
self.db.delete(id)
self.ui.id.clear()
self.ui.id_2.clear()
self.loadClicked()
def addClicked(self):
name = self.ui.Name.text()
rej = self.ui.Rej.text()
year = self.ui.Year.text()
country = self.ui.Country.text()
self.db.append(name, rej, year, country)
self.ui.Name.clear()
self.ui.Rej.clear()
self.ui.Year.clear()
self.ui.Country.clear()
self.ui.id.clear()
self.ui.id_2.clear()
self.loadClicked()
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)
#grip = DXL_XL(port_num, 7)
#wrst2.set_compliance(8,64)
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm((base, shld, elbw, wrst, wrst2), 0)
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.record)
self.ui.btn_task2.clicked.connect(self.clear_waypoints)
self.ui.btn_task3.clicked.connect(self.toggle_gripper)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(
partial(self.sm.set_next_state, "calibrate"))
self.ui.btnUser2.setText("Block Detector")
self.ui.btnUser2.clicked.connect(
partial(self.sm.set_next_state, "block detection"))
self.ui.btnUser2.setText("Color Buckets")
self.ui.btnUser2.clicked.connect(
partial(self.sm.set_next_state, "color buckets"))
self.ui.btnUser3.setText("Click Grab/Drop Mode")
self.ui.btnUser3.clicked.connect(
partial(self.sm.set_next_state, "click grab drop"))
self.ui.btnUser4.setText("Pick n' Stack")
self.ui.btnUser4.clicked.connect(
partial(self.sm.set_next_state, "pick and stack"))
self.ui.btnUser5.setText("Line 'em up")
self.ui.btnUser5.clicked.connect(
partial(self.sm.set_next_state, "line them up"))
self.ui.btnUser6.setText("Stack 'em high")
self.ui.btnUser6.clicked.connect(
partial(self.sm.set_next_state, "stack them high"))
self.ui.btnUser7.setText("Block slider")
self.ui.btnUser7.clicked.connect(
partial(self.sm.set_next_state, "block slider"))
self.ui.btnUser8.setText("Hot swap")
self.ui.btnUser8.clicked.connect(
partial(self.sm.set_next_state, "hot swap"))
self.ui.btnUser9.setText("Calibration Accuracy")
self.ui.btnUser9.clicked.connect(
partial(self.sm.set_next_state, "Calibration Accuracy"))
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, QImage, QImage, QImage, QImage)
def setImage(self, rgb_image, depth_image, blocks_depth_image, h_image,
s_image, v_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))
if (self.ui.radioUsr1.isChecked()):
self.ui.videoDisplay.setPixmap(
QPixmap.fromImage(blocks_depth_image))
if (self.ui.radioUsr2.isChecked()):
self.ui.videoDisplay.setPixmap(QPixmap.fromImage(h_image))
if (self.ui.radioUsr3.isChecked()):
self.ui.videoDisplay.setPixmap(QPixmap.fromImage(s_image))
if (self.ui.radioUsr4.isChecked()):
self.ui.videoDisplay.setPixmap(QPixmap.fromImage(v_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.ui.sldrMaxTorque.setValue(50)
def toggle_gripper(self):
self.rexarm.toggle_gripper()
#self.rexarm.pause(1)
def record(self):
self.sm.set_next_state("record")
def clear_waypoints(self):
self.sm.waypoints = []
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.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)
self.ui.sldrMaxTorque.setValue(0)
else:
self.sm.set_next_state("idle")
self.ui.SliderFrame.setEnabled(False)
self.ui.sldrMaxTorque.setValue(50)
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]
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" %
(x, y, z))
if (self.kinect.kinectCalibrated):
z_w = .1236 * np.tan(z / 2842.5 + 1.1863) - 0.94
xy_world = self.sm.worldCoordinates(x, y)
self.ui.rdoutMouseWorld.setText(
"(%.3f,%.3f,%.3f)" %
(xy_world[0], xy_world[1], xy_world[2]))
def mousePressEvent(self, QMouseEvent):
"""
Function used to record mouse click positions for calibration
"""
""" Get mouse posiiton """
#print("mouse event")
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
if self.sm.current_state == "click grab drop":
if not self.sm.grab_position:
self.sm.grab_position = (x, y)
print("Have grab position")
return
if self.sm.grab_position:
self.sm.drop_position = (x, y)
print("Have drop position")
if self.sm.current_state == "calibrate":
""" 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
if self.sm.current_state == "Calibration Accuracy":
self.sm.calibrateaccuracypt = (x, y)
def __init__(self,parent=None):
QtGui.QWidget.__init__(self,parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Main Variables Using Other Classes"""
self.rex = Rexarm()
self.video = Video(cv2.VideoCapture(0))
self.world_coord = np.float32()
""" Play and Repeat Variable """
self.wayPoints = []
self.wayPointsPos = []
self.wayPointsSpeed = []
self.wayPointsTime = []
""" Other Variables """
self.last_click = np.float32([-1,-1])
self.define_template_flag = -1
self.click_point1 = np.float32([-1,-1])
self.click_point2 = np.float32([-1,-1])
self.template = None
self.targets = []
self.waypointsfp = csv.writer(open("waypoint.csv","wb"))
self.currtime = 0
""" Set GUI to track mouse """
QtGui.QWidget.setMouseTracking(self,True)
"""
Video Function
Creates a timer and calls play() function
according to the given time delay (27mm)
"""
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.play)
self._timer.start(27)
"""
LCM Arm Feedback
Creates a timer to call LCM handler continuously
No delay implemented. Reads all time
"""
self._timer2 = QtCore.QTimer(self)
self._timer2.timeout.connect(self.rex.get_feedback)
self._timer2.start()
"""
ARM Plan and Command Thread
Creates a timer to call REXARM.plan_command function continuously
"""
self._timer3 = QtCore.QTimer(self)
self._timer3.timeout.connect(self.rex.plan_command)
self._timer3.start()
"""
Connect Sliders to Function
TO DO: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI
"""
self.ui.sldrBase.valueChanged.connect(self.slider_change)
self.ui.sldrShoulder.valueChanged.connect(self.slider_change)
self.ui.sldrElbow.valueChanged.connect(self.slider_change)
self.ui.sldrWrist.valueChanged.connect(self.slider_change)
self.ui.sldrMaxTorque.valueChanged.connect(self.slider_change)
self.ui.sldrSpeed.valueChanged.connect(self.slider_change)
""" Commands the arm as the arm initialize to 0,0,0,0 angles """
self.slider_change()
""" Connect Buttons to Functions """
self.ui.btnLoadCameraCal.clicked.connect(self.load_camera_cal)
self.ui.btnPerfAffineCal.clicked.connect(self.affine_cal)
self.ui.btnTeachRepeat.clicked.connect(self.tr_initialize)
self.ui.btnAddWaypoint.clicked.connect(self.tr_add_waypoint)
self.ui.btnSmoothPath.clicked.connect(self.tr_smooth_path)
self.ui.btnPlayback.clicked.connect(self.tr_playback)
self.ui.btnLoadPlan.clicked.connect(self.tr_load)
self.ui.btnDefineTemplate.clicked.connect(self.def_template)
self.ui.btnLocateTargets.clicked.connect(self.template_match)
self.ui.btnExecutePath.clicked.connect(self.exec_path)
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)
#grip = DXL_XL(port_num, 7)
#wrst2.set_compliance(8,64)
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm((base, shld, elbw, wrst, wrst2), 0)
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.record)
self.ui.btn_task2.clicked.connect(self.clear_waypoints)
self.ui.btn_task3.clicked.connect(self.toggle_gripper)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(
partial(self.sm.set_next_state, "calibrate"))
self.ui.btnUser2.setText("Block Detector")
self.ui.btnUser2.clicked.connect(
partial(self.sm.set_next_state, "block detection"))
self.ui.btnUser2.setText("Color Buckets")
self.ui.btnUser2.clicked.connect(
partial(self.sm.set_next_state, "color buckets"))
self.ui.btnUser3.setText("Click Grab/Drop Mode")
self.ui.btnUser3.clicked.connect(
partial(self.sm.set_next_state, "click grab drop"))
self.ui.btnUser4.setText("Pick n' Stack")
self.ui.btnUser4.clicked.connect(
partial(self.sm.set_next_state, "pick and stack"))
self.ui.btnUser5.setText("Line 'em up")
self.ui.btnUser5.clicked.connect(
partial(self.sm.set_next_state, "line them up"))
self.ui.btnUser6.setText("Stack 'em high")
self.ui.btnUser6.clicked.connect(
partial(self.sm.set_next_state, "stack them high"))
self.ui.btnUser7.setText("Block slider")
self.ui.btnUser7.clicked.connect(
partial(self.sm.set_next_state, "block slider"))
self.ui.btnUser8.setText("Hot swap")
self.ui.btnUser8.clicked.connect(
partial(self.sm.set_next_state, "hot swap"))
self.ui.btnUser9.setText("Calibration Accuracy")
self.ui.btnUser9.clicked.connect(
partial(self.sm.set_next_state, "Calibration Accuracy"))
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)
def __init__(self, pod):
super().__init__()
self._pod = pod
self._ui = Ui_MainWindow()
self._ui.setupUi(self)
self._ui.actionNetwork.triggered.connect(self.networkDialog)
class ApplicationWindow(QtWidgets.QMainWindow):
def __init__(self):
super(ApplicationWindow, self).__init__()
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self.index = int(0)
self.on = 1
self.k2 = 42.1
self.D = 0.05
self.Uo = 8
self.hieght = 1.7
self.add_mask = 1
self.t = []
self.Xp = []
self.d_display = 1
self.image = [[]] * 2
self.ax = []
self.fig = []
self.cax = []
self.divider = []
self.plotWidget = []
self.is_mask = 0
self.planes = ["sagittal", "coronal"]
self.current_img = [np.zeros((251, 251)), np.zeros((251, 251))]
self.img = [np.zeros((251, 251)), np.zeros((251, 251))]
self.initialize_plt()
self.ui.start_simulation.clicked.connect(self.start)
self.ui.save_video.clicked.connect(self.save_video)
self.ui.stop_simulation.clicked.connect(self.stop_restart)
self.ui.distance_slider.valueChanged.connect(self.change_dist)
self.ui.hide_plot.clicked.connect(self.ui.frame.hide)
def change_dist(self):
temp = self.ui.distance_slider.value()
self.ui.source_distance.setText(f" {temp} m From Source")
def initialize_plt(self):
plt.ioff()
for i in range(3):
fig, ax = plt.subplots()
self.fig.append(fig)
self.ax.append(ax)
if i != 2:
divider = make_axes_locatable(self.ax[i])
self.divider.append(divider)
cax = self.divider[i].append_axes('right', size='3%', pad=0)
self.cax.append(cax)
self.ax[i].set_title(f"{self.planes[i]} plane ")
ax = self.ax[i].imshow(np.zeros((251, 251)),
cmap=plt.cm.jet,
origin='lower')
self.fig[i].colorbar(ax,
cax=self.cax[i],
orientation='vertical')
self.plotWidget.append(FigureCanvas(self.fig[i]))
self.ui.display_layout.addWidget(self.plotWidget[i])
else:
self.plotWidget.append(FigureCanvas(self.fig[i]))
self.ui.diplay_on_click_layout.addWidget(self.plotWidget[i])
cid = self.fig[0].canvas.mpl_connect(
'button_press_event', lambda event: self.on_press(event, 0))
cid2 = self.fig[1].canvas.mpl_connect(
'button_press_event', lambda event: self.on_press(event, 1))
def start(self):
self.get_parameters()
self.ui.stop_simulation.setText("Stop")
self.ui.frame.hide()
self.ui.hide_plot.hide()
self.on = 1
if self.image[0] != []:
self.timer.stop()
self.image = [[]] * 2
self.img = [np.zeros((251, 251)), np.zeros((251, 251))]
self.index = 0
for i in range(2):
self.ax[i].cla()
self.cax[i].cla()
self.fig[i].delaxes(self.cax[i])
divider = make_axes_locatable(self.ax[i])
self.divider.append(divider)
cax = self.divider[i].append_axes('right', size='3%', pad=0)
self.cax[i] = cax
ax = self.ax[i].imshow(self.img[i],
cmap=plt.cm.jet,
origin='lower')
self.ax[i].set_title(f"{self.planes[i]} plane ")
self.fig[i].colorbar(ax,
cax=self.cax[i],
orientation='vertical')
self.fig[i].canvas.draw_idle()
self.timer = QtCore.QTimer()
self.timer.setInterval(50)
self.timer.timeout.connect(self.start_simulation)
self.timer.start()
def stop_restart(self):
if self.on == 1:
self.timer.stop()
self.on = 0
self.ui.stop_simulation.setText("Restart")
else:
self.timer.start()
self.on = 1
self.ui.stop_simulation.setText("Stop")
def get_parameters(self):
self.d_display = self.ui.distance_slider.value()
person_hieght = self.ui.person_hieght.text()
discharge_velocity = self.ui.discharge_velocity.text()
if discharge_velocity != '':
self.Uo = float(discharge_velocity)
if person_hieght != '':
self.hieght = float(person_hieght)
temp = [3, 5]
if self.ui.add_mask.isChecked():
self.is_mask = 1
self.add_mask = temp[self.ui.select_mask.currentIndex()]
else:
self.is_mask = 0
self.add_mask = 1
def start_simulation(self):
if self.index < 251:
temp_d = ((self.index + 1) / 250) * self.d_display
if temp_d <= 0.272:
temp_t = temp_d / self.Uo
self.ui.time_display.setText(f"Time: {np.round(temp_t,5)} s")
central_velocity = self.Uo * ((1 - self.is_mask * 0.97) /
(self.is_mask * self.index + 1))
else:
temp_t = ((temp_d**2) + 0.074) / (0.544 * self.Uo)
self.ui.time_display.setText(f"Time: {np.round(temp_t,5)} s")
central_velocity = (2.176 / temp_d) * (1 - self.is_mask)
self.ui.distance_display.setText(
f"Distance: {np.round(temp_d,4)} m")
self.t.append(temp_t)
limit_pixel = int(
np.round(125 - self.index *
np.tan(self.add_mask * np.pi * 15 / 180) - 2))
self.img[0][125, self.index] = central_velocity
self.img[1][125, 125] = central_velocity
if limit_pixel > 250:
limit_pixel = 250
for n in range(limit_pixel, 126):
dist_vertical = ((125 - n) / 125) * self.hieght
vertial_v = central_velocity * np.exp(-self.k2 * 0.2 *
(dist_vertical)**2)
self.img[0][n, self.index] = vertial_v
self.img[0][250 - n, self.index] = vertial_v
y = np.sqrt((125 - limit_pixel)**2 - (125 - n)**2)
y = int(np.round(y))
for m in range(125, 125 + y + 1):
dist = np.sqrt((n - 125)**2 + (m - 125)**2)
dist_vertical = ((dist) / 125) * self.hieght
vertial_v2 = central_velocity * np.exp(-self.k2 * 0.2 *
(dist_vertical)**2)
self.img[1][m, 250 - n] = vertial_v2
self.img[1][250 - m, 250 - n] = vertial_v2
self.img[1][m, n] = vertial_v2
self.img[1][250 - m, n] = vertial_v2
for i in range(2):
self.ax[i].cla()
self.ax[i].set_title(
f"{self.planes[i]} plane at {np.round(temp_t,5)} s")
ax = self.ax[i].imshow(self.img[i],
cmap=plt.cm.jet,
origin='lower')
self.cax[i].cla()
self.fig[i].colorbar(ax,
cax=self.cax[i],
orientation='vertical')
self.fig[i].savefig(f"imgs/img{i}_{self.index}.jpeg")
self.fig[i].canvas.draw_idle()
temp = np.empty_like(self.img[i])
temp[:, :] = self.img[i]
self.image[i].append(temp)
self.index += 1
else:
self.timer.stop()
def on_press(self, point, index):
if point.xdata != None and point.ydata != None:
self.ui.hide_plot.show()
x_data = point.xdata
y_data = point.ydata
area = [0]
if index == 0:
temp_d = ((x_data + 1) / 251) * self.d_display
dist_center = (abs(y_data - 125) / 125) * self.hieght
if temp_d <= 0.272:
t_start = temp_d / self.Uo
max_velocit = self.Uo * ((1 - self.is_mask * 0.97) /
(self.is_mask * self.index + 1))
else:
t_start = ((temp_d**2) + 0.074) / (0.544 * self.Uo)
max_velocit = (2.176 / temp_d) * (1 - self.is_mask)
else:
temp_d = ((self.index + 1) / 251) * self.d_display
if temp_d <= 0.272:
t_start = temp_d / self.Uo
max_velocit = self.Uo * ((1 - self.is_mask * 0.97) /
(self.is_mask * self.index + 1))
else:
t_start = ((temp_d**2) + 0.074) / (0.544 * self.Uo)
max_velocit = (2.176 / temp_d) * (1 - self.is_mask)
dist_center = (np.sqrt((y_data - 125)**2 +
(x_data - 125)**2) / 125) * self.hieght
x_data = self.index
self.draw(max_velocit, dist_center, t_start, temp_d, x_data,
y_data)
def draw(self, max_velocity, dist_center, t_start, x_dist, xdata, ydata):
self.ui.frame.show()
self.ax[2].cla()
t = np.linspace(0.0001, 5.0001, 50001)
if np.abs(ydata - 125) > (np.abs(xdata) * np.tan(np.pi / 12) + 4):
f = np.zeros((np.size(t)))
else:
index_of_t_start = int(np.round(t_start / 0.0001))
f = max_velocity * np.exp(-10 * t / ((0.272 + (
(self.d_display**2) - 0.073984) / 0.544)) / self.Uo) * np.exp(
-self.k2 * (dist_center)**2)
f[0:index_of_t_start] = np.repeat(0, index_of_t_start)
self.ax[2].plot(t, f)
self.ax[2].set(title="V at poin Versus Time",
xlabel="time (s)",
ylabel="V (m/s)")
self.fig[2].canvas.draw()
def save_video(self):
self.frame_no = int(self.index) - 1
self.ui.save_video.setText("Saving Progressing")
self.directory = self.get_video_directory()
if self.directory == None:
self.directory = "kimo"
frame = cv2.imread("imgs/img0_0.jpeg")
height, width, layers = frame.shape
width = int(np.round(width * 0.7)) * 2
self.ui.videoprogress.show()
self.video = cv2.VideoWriter(f"{self.directory}.avi", 0, 5,
(width, height))
self.index_2 = 0
self.timer1 = QtCore.QTimer()
self.timer1.setInterval(50)
self.timer1.timeout.connect(
lambda: self.save_video_iterate(width, height))
self.timer1.start()
def save_video_iterate(self, width, height):
width = int(0.5 * width)
if self.index_2 <= self.frame_no:
self.video.write(
np.concatenate(
(cv2.resize(cv2.imread(f"imgs/img{0}_{self.index_2}.jpeg"),
(width, height)),
cv2.resize(cv2.imread(f"imgs/img{1}_{self.index_2}.jpeg"),
(width, height))),
axis=1))
self.index_2 += 1
self.ui.videoprogress.setValue(
int(np.round(100 * self.index_2 / self.frame_no)))
else:
self.ui.save_video.setText("Save Video")
self.timer1.stop()
cv2.destroyAllWindows()
self.video.release()
self.ui.videoprogress.setValue(100)
time.sleep(0.5)
self.ui.videoprogress.hide()
def get_video_directory(self):
options = QtWidgets.QFileDialog.Options()
options |= QtWidgets.QFileDialog.DontUseNativeDialog
fileName, _ = QtWidgets.QFileDialog.getSaveFileName(
self,
"Choose Directory and Name",
"",
"All Files (*);;Python Files (*.py)",
options=options)
if fileName:
return fileName
def __init__(self,parent=None):
QMainWindow.__init__(self,parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
#add new slider
# self.positionSlider=QSlider(Qt.Horizontal)
# self.positionSlider.setGeometry (800,800,100,30)
# self.positionSlider.setRange(0, 0)
# self.positionSlider.sliderMoved.connect(self.setPosition)
#setup Video
#video player
self.mediaPlayer1 = QMediaPlayer(None, QMediaPlayer.VideoSurface)
self.mediaPlayer2 = QMediaPlayer(None, QMediaPlayer.VideoSurface)
#self.mediaPlayer.metaDataChanged.connect(self.metaDataChanged)
self.mediaPlayer1.durationChanged.connect(self.durationChanged)
self.mediaPlayer1.positionChanged.connect(self.positionChanged)
self.mediaPlayer2.positionChanged.connect(self.positionChanged)
#visualizetion
self.scene = QGraphicsScene()
self.ui.graphicsView.setScene(self.scene)
#self.scene.setBackgroundBrush(Qt.black)
self.videoItem1 = QGraphicsVideoItem()
self.videoItem2 = QGraphicsVideoItem()
self.scene.addItem(self.videoItem1)
self.scene.addItem(self.videoItem2)
self.mediaPlayer1.setVideoOutput(self.videoItem1)
self.mediaPlayer2.setVideoOutput(self.videoItem2)
#slide bar
print self.ui.horizontalSlider
self.ui.horizontalSlider.setRange(0, 0)
self.ui.horizontalSlider.sliderMoved.connect(self.setPosition)
# self.ui.horizontalSlider.sliderPressed.connect(self.sliderPressed)
#print self.ui.graphicsView.width()/2,self.ui.graphicsView.height()
#self.videoItem1.setSize(QSizeF(self.ui.graphicsView.width()/2,self.ui.graphicsView.height()))
#self.videoItem2.setSize(QSizeF(self.ui.graphicsView.width()*10,self.ui.graphicsView.height()*10))
# self.videoItem2.setSize(graphicsView.size())
#self.videoItem2.setOffset(QPointF(500,500))
#self.videoItem2.setOffset(QPointF(self.ui.graphicsView.width()/2,0))
#self.videoItem2.setPos(QPointF(0,0))
# print self.ui.graphicsView.width(), self.ui.graphicsView.height()
# print self.ui.graphicsView.size()
# print self.videoItem2.boundingRect().width(), self.videoItem2.boundingRect().height()
# print self.ui.graphicsView.sceneRect()
#self.mediaPlayer.stateChanged.connect(self.mediaStateChanged)
#callbacks
self.ui.actionQuit.triggered.connect(self.quit)
self.ui.actionLoad_Project.triggered.connect(self.loadVideo)
#self.ui.buttonPlay.clicked[bool].connect(self.setToggleText)
self.ui.buttonPlay.clicked.connect(self.play)
#print self.ui.graphicsView.sizeHint()
#initialization
self.loaded = False
self.videoFilename = None
self.frame_count=None
self.width=None
self.height=None
self.frame_trans=None
from ui import Ui_MainWindow from PyQt5.QtWidgets import QApplication, QMainWindow app = QApplication([]) window = QMainWindow() ui = Ui_MainWindow() ui.setupUi(window) app.exec_()
def __init__(self):
super(CurrencyConv, self).__init__()
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self.init_UI()
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)
grip = DXL_XL(port_num, 7)
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm((base, shld, elbw, wrst, wrst2, wrst3), grip)
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.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(
partial(self.sm.set_next_state, "calibrate"))
self.ui.btnUser2.setText("Waypoint")
self.ui.btnUser2.clicked.connect(
partial(self.sm.set_next_state, "waypoint"))
self.ui.btnUser3.setText("Teach")
self.ui.btnUser3.clicked.connect(
partial(self.sm.set_next_state, "teach"))
self.ui.btnUser4.setText("Record")
self.ui.btnUser4.clicked.connect(
partial(self.sm.set_next_state, "record"))
self.ui.btnUser5.setText("Repeat")
self.ui.btnUser5.clicked.connect(
partial(self.sm.set_next_state, "repeat"))
self.ui.btn_task1.setText("Pick n Stack")
self.ui.btn_task1.clicked.connect(partial(self.sm.set_next_state,
"IK"))
self.ui.btnUser6.setText("Click and Place")
self.ui.btnUser6.clicked.connect(
partial(self.sm.set_next_state, "clickgrab"))
self.ui.btnUser7.setText("Open Gripper")
self.ui.btnUser7.clicked.connect(
partial(self.sm.set_next_state, "open_gripper"))
self.ui.btnUser8.setText("Close Gripper")
self.ui.btnUser8.clicked.connect(
partial(self.sm.set_next_state, "close_gripper"))
self.ui.btnUser9.setText("Incline")
self.ui.btnUser9.clicked.connect(
partial(self.sm.set_next_state, "incline"))
self.ui.btn_task2.setText("Line Em Up")
self.ui.btn_task2.clicked.connect(
partial(self.sm.set_next_state, "color_sort"))
self.ui.btn_task3.setText("Stack Em High")
self.ui.btn_task3.clicked.connect(
partial(self.sm.set_next_state, "color_stack"))
self.ui.btn_task4.setText("Block Mover")
self.ui.btn_task4.clicked.connect(
partial(self.sm.set_next_state, "square"))
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)
class App(QtWidgets.QMainWindow):
"""
main app window
"""
#logger = Logger("c:\\temp\\price_bot.log").get_logger()
# params to init bot with
params = []
username = ""
password = ""
selected_site = ""
selected_exec_type = ""
selected_frequency = ""
thresh = ""
bot_state = "off"
def __init__(self):
"""
constructor
"""
logging.debug(" init apps ")
# ui init
super(App, self).__init__()
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self.mgr = Manager(self.ui)
self.selected_google_auth_file = self.ui.get_google_auth_file_name()
# init starting events
self.ui.activateButton.setEnabled(False)
self.ui.deactivateButton.setEnabled(False)
self.ui.refreshButton.setEnabled(False)
self.ui.activateButton.clicked.connect(self.mgr.activate)
self.ui.deactivateButton.clicked.connect(self.mgr.deactivate)
self.ui.quitButton.clicked.connect(self.quit)
self.ui.refreshButton.clicked.connect(self.refresh)
# init and make selections
self.ui.cmbbox_site.activated[str].connect(self.on_site_select)
self.ui.exec_type_box.activated[str].connect(self.on_exec_type_select)
self.ui.cmbbox_freq.activated[str].connect(self.on_frequency_select)
def capture_thresh(self):
"""
capture threshold
"""
print(" *** IN CAPTURE REFRESH ")
thresh_val = self.ui.reduct_field.text()
if (isinstance(thresh_val, str)):
if (thresh_val.isdigit()):
if (isinstance(int(thresh_val), int)):
if (int(thresh_val) >= 0 and int(thresh_val) <= 100):
self.thresh = thresh_val
else:
logging.error("frequency must be greater than zero")
self.reset_form()
else:
logging.error("frequency is not numeric")
self.reset_form()
else:
logging.error("frequency is not numeric")
self.reset_form()
else:
logging.error("frequency is not numeric")
self.reset_form()
def refresh(self):
"""
tries to re-init the bot
"""
print(" *** IN REFRESH ")
self.ui.activateButton.setEnabled(False)
self.params = []
self.capture_thresh()
self.enable_buttons()
def on_site_select(self, site):
"""
selecting site
"""
self.selected_site = site
if (self.bot_state == "off"):
self.capture_thresh()
self.enable_buttons()
else:
self.ui.refreshButton.setEnabled(True)
self.ui.activateButton.setEnabled(False)
self.bot_state = "off"
def on_exec_type_select(self, exec_type):
"""
exec type select
"""
self.selected_exec_type = exec_type
if (self.bot_state == "off"):
self.capture_thresh()
self.enable_buttons()
else:
self.ui.refreshButton.setEnabled(True)
self.ui.activateButton.setEnabled(False)
self.bot_state = "off"
def on_frequency_select(self, freq):
"""
frequency select
"""
self.selected_frequency_type = freq
if (self.bot_state == "off"):
self.frequency = str(self.ui.freq_value_box.text())
if (isinstance(self.frequency, str)):
if (self.frequency.isdigit()):
if (isinstance(int(self.frequency), int)):
if (int(self.frequency) > 0):
self.capture_thresh()
self.enable_buttons()
else:
logging.error(
"frequency must be greater than zero")
self.ui.reduct_field.setText("")
else:
logging.error("frequency is not numeric")
self.ui.reduct_field.setText("")
else:
logging.error("frequency is not numeric")
self.ui.reduct_field.setText("")
else:
logging.error("frequency is not numeric")
self.ui.reduct_field.setText("")
else:
self.ui.refreshButton.setEnabled(True)
self.ui.activateButton.setEnabled(False)
self.bot_state = "off"
def reset_form(self):
"""
reset freq
"""
#self.capture_thresh()
self.ui.freq_value_box.setText("")
self.ui.cmbbox_freq.setCurrentIndex(0)
def enable_buttons(self):
"""
enable buttons
"""
try:
print(" *** ENABLING BUTTONS ")
if (self.selected_exec_type != ""
and self.selected_frequency_type != ""
and self.selected_site != ""):
self.username = self.ui.username_field.text()
self.password = self.ui.password_field.text()
self.frequency = self.ui.freq_value_box.text()
self.params.append(self.username)
self.params.append(self.password)
self.params.append(self.selected_site)
self.params.append(self.selected_frequency_type)
self.params.append(self.frequency)
self.params.append(self.selected_exec_type)
self.params.append(self.selected_google_auth_file)
self.params.append(self.thresh)
#self.ui.refreshButton.setEnabled(False)
print(self.params)
self.mgr.init_bot(self.params)
self.bot_state = "on"
else:
print(" CANNOT ENABLE ")
except Exception as e:
print(" *** EXCEPTION WHEN ENABLING ")
print(e)
self.ui.refreshButton.setEnabled(True)
self.ui.activateButton.setEnabled(False)
logging.debug(e)
def quit(self):
self.mgr.deactivate()
sys.exit()
def __init__(self):
super(MainWindow, self).__init__()
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
self.ui.label.setText('Hello World!')
