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)