def __init__(self, pipeOR, processStarter): super(trajApp,self).__init__(None) self.pipeOR = pipeOR self.setupUi(self) self.syncList = ListInteractor(self.trajList) self.processStarter = processStarter self.addSlots() self.startVal.setText('start: ') self.endVal.setText('end: ')
class trajApp(QtGui.QMainWindow,Ui_MainWindow): @QtCore.pyqtSignature("") def on_pbClose_clicked(self): self.closeAll() def __init__(self, pipeOR, processStarter): super(trajApp,self).__init__(None) self.pipeOR = pipeOR self.setupUi(self) self.syncList = ListInteractor(self.trajList) self.processStarter = processStarter self.addSlots() self.startVal.setText('start: ') self.endVal.setText('end: ') def closeEvent(self, event): """ Redefine the close event. """ QtGui.QApplication.quit() self.processStarter.terminate() def addSlots(self): QtCore.QObject.connect(self.playSelectedButton, QtCore.SIGNAL("clicked()"), self.clicked_playSelectedButton) QtCore.QObject.connect(self.removeButton, QtCore.SIGNAL("clicked()"), self.clicked_removeButton) QtCore.QObject.connect(self.addButton, QtCore.SIGNAL("clicked()"), self.clicked_addButton) QtCore.QObject.connect(self.exportButton, QtCore.SIGNAL("clicked()"), self.clicked_exportButton) QtCore.QObject.connect(self.downButton, QtCore.SIGNAL("clicked()"), self.clicked_downButton) QtCore.QObject.connect(self.copyButton, QtCore.SIGNAL("clicked()"), self.clicked_copyButton) QtCore.QObject.connect(self.upButton, QtCore.SIGNAL("clicked()"), self.clicked_upButton) QtCore.QObject.connect(self.startSlider, QtCore.SIGNAL("sliderMoved(int)"), self.moved_startSlider) QtCore.QObject.connect(self.endSlider, QtCore.SIGNAL("sliderMoved(int)"), self.moved_endSlider) QtCore.QObject.connect(self.playSlider, QtCore.SIGNAL("valueChanged(int)"), self.moved_playSlider) QtCore.QObject.connect(self.trajList, QtCore.SIGNAL("currentRowChanged(int)"), self.changed_trajList) def changed_trajList(self, row): ## update the sliders and various labels if 0 <= row < self.syncList.length(): print "changed list row : ", row segitem = self.syncList.segmentList[row] self.startSlider.setMinimum(0) self.startSlider.setMaximum(segitem.slen-1) self.endSlider.setMinimum(0) self.endSlider.setMaximum(segitem.slen-1) self.playSlider.setMinimum(segitem.sstart) self.playSlider.setMaximum(segitem.send) self.startSlider.setValue(segitem.sstart) self.endSlider.setValue(segitem.slen-1-segitem.send) self.playSlider.setValue(segitem.sstart) self.startVal.setText('start: %d'% segitem.sstart) self.endVal.setText('end: %d'% segitem.send) def setVisibilityModifiers(self, visible): self.addButton.setDisabled(visible) self.removeButton.setDisabled(visible) self.exportButton.setDisabled(visible) self.downButton.setDisabled(visible) self.upButton.setDisabled(visible) self.copyButton.setDisabled(visible) self.startSlider.setDisabled(visible) self.endSlider.setDisabled(visible) self.trajList.setDisabled(visible) def clicked_playSelectedButton(self): caption = self.playSelectedButton.text() if caption=="Play Selected": self.playSelectedButton.setText("Pause") selection = self.trajList.currentRow() if selection >=0 and selection < self.syncList.length(): self.setVisibilityModifiers(True) segitem = self.syncList.segmentList[selection] jTask = JointsPusher(self.playSlider.value(), segitem.send, self.playSlider, self) jTask.start() else: self.playSelectedButton.setText("Pause") jTask = JointsPusher(0, -1, self.playSlider, self) jTask.start() def clicked_removeButton(self): if self.syncList.length() > 0: self.syncList.removeItem(self.trajList.currentRow()) if self.syncList.length() > 0: selection = self.trajList.currentRow() if selection >=0: trajItem = self.syncList.segmentList[selection] self.startVal.setText('start: %d'%trajItem.sstart) self.endVal.setText('end: %d'%trajItem.send) else: self.startVal.setText('start: ') self.endVal.setText('end: ') def clicked_addButton(self): fname = self.dialog.getOpenFileName(caption='Add Trajectory') self.add_segments(fname) def add_segments(self, fname): """ Open a scene file and add all segments in that into the list obj. """ segments = parsescene(fname) for seg in segments: self.syncList.addItem(seg) def write_point_data(self, ptypes, psecs, points, tfile): n = 0 for i,ptype in enumerate(ptypes): tfile.write('\t%s\n'%ptype) ptnum = 0 while ptnum < psecs[i]: x,y,z = points[n, :] tfile.write('\t\t%f\t%f\t%f\n'%(x,y,z)) n += 1 ptnum += 1 tfile.write('end-points\n') def clicked_exportButton(self): starttime = 0 segs_copy = copy.deepcopy(self.syncList.segmentList) for i in xrange(len(segs_copy)): segitem = segs_copy[i] print "i : ", i print "segment end : ", segitem.send print "len jtimes :", segitem.seg['jtimes'].shape seg_starttime = segitem.seg['jtimes'][segitem.sstart] seg_endtime = segitem.seg['jtimes'][segitem.send] # filter the joints/ joint-times: segitem.seg['joints'] = segitem.seg['joints'][segitem.sstart:segitem.send+1,:] segitem.seg['jtimes'] = segitem.seg['jtimes'][segitem.sstart:segitem.send+1] - seg_starttime + starttime # filter the grips based on start and end points of the segment: g_start = np.searchsorted(segitem.seg['gtimes'], seg_starttime, 'left') g_end = np.searchsorted(segitem.seg['gtimes'], seg_endtime, 'right') segitem.seg['gtimes'] = segitem.seg['gtimes'][g_start:g_end+1] - seg_starttime + starttime segitem.seg['grips'] = segitem.seg['grips'][g_start:g_end+1] # filter the point clouds based on the start and end points of the segment p_start = np.searchsorted(segitem.seg['ptimes'], seg_starttime, 'left') p_end = np.searchsorted(segitem.seg['ptimes'], seg_endtime, 'right') segitem.seg['ptimes'] = segitem.seg['ptimes'][p_start:p_end+1] - seg_starttime + starttime segitem.seg['points'] = segitem.seg['points'][p_start:p_end+1,:,:] # undo the bulletsim-openrave offset. segitem.seg['points'] += np.array((0,0, 0.05)) # update the start-time for the next segment: starttime = seg_starttime fname = self.dialog.getSaveFileName(caption='Save Trajectory') tfile = open(fname, 'w') grip_dict = {0: 'release r', 1:'grab r', 2:'release l', 3:'grab l'} for segitem in segs_copy: if (segitem.seg['jtimes'].shape[0] > 0): ## add the look (denoting start of a new segment), only for the original segments. #if 'copy' not in segitem.seg['name']: looktime = segitem.seg['jtimes'][0] tfile.write("%f : look\n"%looktime) alltimes = np.unique(np.concatenate([segitem.seg['jtimes'], segitem.seg['ptimes'], segitem.seg['gtimes']])) ji = pi = gi = 0 for t in alltimes: # write points if pi < segitem.seg['ptimes'].shape[0] and segitem.seg['ptimes'][pi]==t: tfile.write('%f : points\n'%segitem.seg['ptimes'][pi]) self.write_point_data(segitem.seg['ptypes'], segitem.seg['point_secs'], segitem.seg['points'][pi,:,:], tfile) pi += 1 # write grips: if gi < segitem.seg['gtimes'].shape[0] and segitem.seg['gtimes'][gi]==t: tfile.write('%f : %s\n'%(segitem.seg['gtimes'][gi], grip_dict[segitem.seg['grips'][gi]])) gi += 1 # write joints: if ji < segitem.seg['jtimes'].shape[0] and segitem.seg['jtimes'][ji]==t: joint_msg = '%f : joints :\t'%segitem.seg['jtimes'][ji] for j in segitem.seg['joints'][ji,:]: joint_msg += '%f '%j joint_msg += '\n' tfile.write(joint_msg) ji += 1 tfile.close() print colorize('DONE EXPORTING THE FILE', 'red', True) def clicked_downButton(self): selection = self.trajList.currentRow() if selection >=0 and selection < self.syncList.length()-1: item = self.trajList.takeItem(selection) self.trajList.insertItem(selection+1, item) self.syncList.segmentList[selection], self.syncList.segmentList[selection+1] = self.syncList.segmentList[selection+1], self.syncList.segmentList[selection] self.trajList.setCurrentRow(selection+1) def clicked_upButton(self): selection = self.trajList.currentRow() if selection > 0 and selection < self.syncList.length(): item = self.trajList.takeItem(selection) self.trajList.insertItem(selection-1, item) self.syncList.segmentList[selection], self.syncList.segmentList[selection-1] = self.syncList.segmentList[selection-1], self.syncList.segmentList[selection] self.trajList.setCurrentRow(selection-1) def clicked_copyButton(self): selection = self.trajList.currentRow() if 0 <= selection < self.syncList.length(): segitem = self.syncList.segmentList[selection] copy_seg = copy.deepcopy(segitem.seg) copy_seg['name'] = segitem.seg['name'] + '-copy' self.syncList.addItem(copy_seg, segitem.sstart, segitem.send) def moved_startSlider(self, pos): selection = self.trajList.currentRow() if selection >= 0: segitem = self.syncList.segmentList[selection] endPos = segitem.slen - self.endSlider.value() if pos >= endPos: pos = endPos self.startSlider.setValue(pos) self.syncList.segmentList[selection].sstart = pos self.playSlider.setMinimum(segitem.sstart) self.startVal.setText("start: %d"%segitem.sstart) def moved_endSlider(self, pos): selection = self.trajList.currentRow() print "end-slider >>>>>> : ", pos, " | selection : ", selection if selection >= 0: segitem = self.syncList.segmentList[selection] print '\t\t >> : len : ', segitem.slen startPos = self.startSlider.value() p = segitem.slen-1-pos if p < startPos: pos = segitem.slen-1-startPos self.endSlider.setValue(pos) self.syncList.segmentList[selection].send = segitem.slen- 1 - pos self.playSlider.setMaximum(segitem.send) self.endVal.setText("end: %d"%segitem.send) def moved_playSlider(self, pos): self.updateJoints(pos) def updateJoints(self, tickPos): selection = self.trajList.currentRow() if selection >=0: segitem = self.syncList.segmentList[selection] if 0<= tickPos < segitem.seg['joints'].shape[0]: joints = segitem.seg['joints'][tickPos,:] self.pipeOR.send(['SetJoints', cPickle.dumps(joints)]) points_idx = segitem.seg['j2ptimes'][tickPos] if points_idx >=0: self.pipeOR.send(['PlotPoints', cPickle.dumps(segitem.seg['points'][points_idx,:,:])])