예제 #1
0
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)
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))

        """ 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
        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.btnDefineTemplate.clicked.connect(self.def_template)
        self.ui.btnLocateTargets.clicked.connect(self.template_match)
        self.ui.btnExecutePath.clicked.connect(self.exec_path)

        self.ui.path_start_time = None
        self.executed_path = []

        self.get_template = 0
        self.templat_point = None
        self.donuts = []


    def play(self):
        """
        Play Funtion
        Continuously called by GUI
        """

        """ Renders the Video Frame """
        try:
            self.video.captureNextFrame()
            self.ui.videoFrame.setPixmap(
                self.video.convertFrame())
            self.ui.videoFrame.setScaledContents(True)
        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))

        t = [self.rex.joint_angles_fb[0],
             self.rex.joint_angles_fb[1] - 90.0 * D2R,
             self.rex.joint_angles_fb[2],
             self.rex.joint_angles_fb[3]]

        a = [0.0, 100.0, 100.0, 108.0]

        d = [116.0, 0.0, 0.0, 0.0]
        al = [-90.0 * D2R, 0.0 * D2R, 0.0 * D2R, 0.0 * D2R]

        dh_table = [t,a,d,al]
        (x, y, z, phi) = self.rex.rexarm_FK(dh_table, 3)
        self.ui.rdoutX.setText(str(x))
        self.ui.rdoutY.setText(str(y))
        self.ui.rdoutZ.setText(str(z))
        self.ui.rdoutT.setText(str(phi*R2D))

        # res = self.rex.rexarm_IK((0.0,-275.0,80.0,0.0), 1)
        # if res != None:
        #     self.rex.joint_angles[0] = res[0]
        #     self.rex.joint_angles[1] = res[1]
        #     self.rex.joint_angles[2] = res[2]
        #     self.rex.joint_angles[3] = res[3]
        #     self.rex.cmd_publish()

        """
        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 """
                aff = self.video.aff_matrix
                wx = x*aff[0][0] + y*aff[0][1] + aff[0][2]
                wy = x*aff[1][0] + y*aff[1][1] + aff[1][2]
                self.ui.rdoutMouseWorld.setText("(%.2f,%.2f)" %(wx,wy))
            else:
                self.ui.rdoutMouseWorld.setText("(-,-)")

                

        """
        Updates status label when rexarm playback is been executed.
        This will be extended to includ eother appropriate messages
        THIS SHOULD BE LAST BECAUSE IT RETURNS EARLY!!!!!!!!
        (Pedro don't be sad please)
        """
        if(self.rex.plan_status == 1):
            if self.rex.wpt_total == 0:
                self.ui.rdoutStatus.setText("No waypoints!")
                self.rex.plan_status = 0
            else:
                self.ui.rdoutStatus.setText("Playing Back - Waypoint %d"
                                            %(self.rex.wpt_number + 1))
                [start_time, prev_waypoint, prev_vel] = self.rex.plan[self.rex.wpt_number-1]
                [end_time, current_waypoint, current_vel] = self.rex.plan[self.rex.wpt_number]
                t = float(micro_time() - self.path_start_time)
               
                #Linear Interp
                for joint in range(4):
                    #Set the goal
                    self.rex.joint_angles[joint] = current_waypoint[joint]
                    
                    #Figure the speed out
                    dist = abs(current_waypoint[joint] - prev_waypoint[joint])
                    time_left = (end_time - t)/1000000.0
                    v = dist/time_left
                    # rad/sec * sec/min * rot/rad / max RPM
                    speed_val = v * (60.0/(2*math.pi)) / 59.0
                    speed_val = max(speed_val, 1.0/1023.0)

                    print speed_val, time_left
                    if time_left < 0:
                        #were behind haul ass
                        self.rex.speed[joint] = 1.0
                    else:
                        self.rex.speed[joint] = speed_val

                self.rex.cmd_publish()

                #Check if we have arrived
                def within_error(joint, wpt):
                    #print joint, wpt
                    error = math.fabs(joint * R2D - wpt * R2D)
                    #print error
                    return error < 5.0

                if within_error(self.rex.joint_angles_fb[0], current_waypoint[0]) and \
                   within_error(self.rex.joint_angles_fb[1], current_waypoint[1]) and \
                   within_error(self.rex.joint_angles_fb[2], current_waypoint[2]) and \
                   within_error(self.rex.joint_angles_fb[3], current_waypoint[3]):
                    print "Within Error Bounds: %d" %(self.rex.wpt_number)
                    self.rex.wpt_number += 1
                    self.executed_path.append([micro_time() - self.path_start_time , self.rex.joint_angles_fb[:]])
                    if self.rex.wpt_number >= len(self.rex.plan):
                        self.rex.plan_status = 0
                        self.ui.rdoutStatus.setText("Plan finished!")
                        self.rex.speed = [0.5, 0.5, 0.5, 0.5]
                        self.rex.plan = self.rex.plan[1:]
                        with open('executed_path.config', 'w') as f:
                            pickle.dump(self.executed_path, f)


    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()))

        if self.rex.plan_status == 0:
            self.rex.max_torque = self.ui.sldrMaxTorque.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 solveAffineMatrix(self):
        A = []
        b = []
        for i in range(self.video.aff_npoints):
            px = self.video.mouse_coord[i][0]
            py = self.video.mouse_coord[i][1]
            wx = self.video.real_coord[i][0]
            wy = self.video.real_coord[i][1]

            A.append([px, py, 1.0, 0.0, 0.0, 0.0])
            A.append([0.0, 0.0, 0.0, px, py, 1.0])

            b.append(wx)
            b.append(wy)
        print A, '\n', b

        x, res, rank, s = np.linalg.lstsq(A, b)
        print x
        self.video.aff_matrix = [[x[0], x[1], x[2]],
                                 [x[3], x[4], x[5]],
                                 [0.0, 0.0, 1.0]]

    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

                """ Perform affine calibration with OpenCV """
                #self.video.aff_matrix = cv2.getAffineTransform(
                #                        self.video.mouse_coord,
                #                        self.video.real_coord)
                self.solveAffineMatrix()
                print self.video.aff_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.get_template == 1:
            self.template_point = [(x-MIN_X)*(960.0/480.0), (y-MIN_Y)*(1280.0/640.0)]
            self.get_template += 1
        elif self.get_template == 2:
            self.get_template = 0
            bottom_point = [(x-MIN_X)*(960.0/480.0), (y-MIN_Y)*(1280.0/640.0)]
            dx = bottom_point[0] - self.template_point[0]
            dy = bottom_point[1] - self.template_point[1]
            grey_frame = cv2.cvtColor(self.video.currentFrame, cv2.COLOR_BGR2GRAY)
            self.template = grey_frame[self.template_point[1]-dy:bottom_point[1], self.template_point[0]-dx:bottom_point[0]]
            #self.template = cv2.cvtColor(self.template, cv2.COLOR_BGR2GRAY)
            cv2.imwrite('./template.png', self.template)
            print "Got 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):
        print "Teach and Repeat"
        self.path_start_time = micro_time()
        self.rex.plan = []
        self.rex.plan_status = 0
        self.rex.wpt_number = 0
        self.rex.max_torque = 0.0
        self.ui.sldrMaxTorque.setValue(0.0)

    def tr_add_waypoint(self):
        print "Add Waypoint"
        self.rex.plan.append([micro_time() - self.path_start_time, self.rex.joint_angles_fb[:], [0,0,0,0]])
        self.rex.wpt_total += 1

    def tr_smooth_path(self):
        print "Smooth Path"
        #Basic smoothing
        new_plan = []
        #for i, (t, p) in enumerate(self.rex.plan):
        #    np = p[:]
        #    nt = t + 1000000 * i + 500000
        #    if p[1] < 0:
        #        np[1] += 10*D2R
        #    else:
        #        np[1] -= 10*D2R
        #
        #    new_plan.append([nt-500000, np])
        #    new_plan.append([nt, p[:]])
        #    new_plan.append([nt+500000, np])

        self.rex.plan = [[0, [0, 0, 0, 0], [0,0,0,0]]] + self.rex.plan

        #Handle all other points
        for i in range(1,len(self.rex.plan)):
            [tprev, qprev, vprev] = self.rex.plan[i-1]
            [ti, qi, vi] = self.rex.plan[i]
            dt = (ti - tprev) * 0.3

            nprev = qprev[:]
            if nprev[1] < 0:
                nprev[1] += 10*D2R
            else:
                nprev[1] -= 10*D2R

            ni = qi[:]
            if ni[1] < 0:
                ni[1] += 10*D2R
            else:
                ni[1] -= 10*D2R

            new_plan.append([tprev + dt, nprev, vprev])
            new_plan.append([ti - dt, ni, vi])
            new_plan.append([ti, qi[:], vi])

        #Handle last point
        [ti, pi, vi] = self.rex.plan[-1]
        ni = qi[:]
        if ni[1] < 0:
            ni[1] += 10*D2R
        else:
            ni[1] -= 10*D2R

        new_plan.append([ti+500000, ni, vi])

        self.rex.plan = new_plan[1:]
        
        #Cubic spline smoothing
        
        # new_plan = [self.rex.plan[0][:]]
        # for i in range(1,len(self.rex.plan)):
        #     [start_time, prev_waypoint, prev_vel] = self.rex.plan[i-1]
        #     [end_time, current_waypoint, current_vel] = self.rex.plan[i]
        
        #     a0 = [0,0,0,0]
        #     a1 = [0,0,0,0]
        #     a2 = [0,0,0,0]
        #     a3 = [0,0,0,0]
        #     for joint in range(4):
        #         def calc_params(t0, q0, v0, tf, qf, vf):
        #             a0 = q0
        #             a1 = v0
        #             a2 = (3.0*(qf-q0)-(2.0*v0+vf)*(tf-t0))/math.pow(tf-t0, 2)
        #             a3 = (2.0*(q0-qf) + (v0+vf)*(tf-t0))/math.pow(tf-t0, 3)
        #             return a0, a1, a2, a3
    
        #         a0[joint], a1[joint], a2[joint], a3[joint] = calc_params(0.0,\
        #                                         float(prev_waypoint[joint]),\
        #                                         float(prev_vel[joint]),\
        #                                         1.0,\
        #                                         float(current_waypoint[joint]),\
        #                                         float(current_vel[joint]))
            
        #     duration = end_time - start_time
        #     for frac in range(1,100,10):
        #         frac = float(frac) / 100.0
        #         time = start_time + int(duration * frac)
        #         q = [0,0,0,0]
        #         for joint in range(4):
        #             q[joint] = a0[joint] + a1[joint] * frac + a2[joint] * frac * frac + a3[joint] * frac * frac * frac
                
        #         new_plan.append([time, q[:], [0.0, 0.0, 0.0, 0.0]])
            
        #     #Add the destination point
        #     new_plan.append(self.rex.plan[i][:])
           
        # with open('smooth_path.config', 'w') as f:
        #     print len(new_plan)
        #     pickle.dump(new_plan, f)

        # with open('path.config', 'w') as f:
        #     pickle.dump(self.rex.plan, f)

        # self.rex.plan = new_plan


    def tr_playback(self):
        print "Playback"
        self.path_start_time = micro_time()
        self.rex.wpt_number = 1
        self.rex.plan_status = 1
        self.rex.max_torque = 50.0
        self.ui.sldrMaxTorque.setValue(50.0)
        self.executed_path = []

        with open('path.config', 'w') as f:
            pickle.dump(self.rex.plan, f)
        self.rex.plan = [[micro_time() - self.path_start_time, self.rex.joint_angles_fb[:], [0,0,0,0]]] + self.rex.plan



    def def_template(self):
        print "Define Template"
        self.get_template = 1

    def template_match(self):
        print "Template Match"
        def make_bounding_box():
            min_x = 10000
            min_y = 10000
            max_x = 0
            max_y = 0
            for p in self.video.mouse_coord:
                x,y = mouse_to_raw(p[0], p[1])
                if x < min_x:
                    min_x = x
                if x > max_x:
                    max_x = x
                if y < min_y:
                    min_y = y
                if y > max_y:
                    max_y = y
            return [min_x, min_y], [max_x, max_y]

        #TODO get this working
        search_image = self.video.currentFrame
        #Make it greyscale
        search_image = cv2.cvtColor(search_image, cv2.COLOR_BGR2GRAY)
        [tx,ty],[bx,by] = make_bounding_box()
        print [tx,ty], [bx,by]
        search_image = search_image[tx:bx:,ty:by]
        search_size = search_image.shape
        print search_size

        #Get the size of the template
        template_size = self.template.shape
        #We just want a greyscale 2d image

        def SAD(x, y):
            sad = 0
            sub_region = search_image[x:x+template_size[0], y:y+template_size[1]]
            diff = sub_region - self.template
            sad = np.sum(diff**2)
            return sad / (template_size[0]*template_size[1])
            # for i in range(template_size[0]):
            #     for j in range(template_size[1]):
            #         sad += float(abs(int(search_image[x+i,y+j]) - int(self.template[i,j])))
            # return sad / (template_size[0]*template_size[1]*255.0)

        results = np.zeros((search_size[0]-template_size[0],
                    search_size[1]-template_size[1]))
        print "Doing SAD"
        for x in range(search_size[0] - template_size[0]):
            for y in range(search_size[1] - template_size[1]):
                results[x,y] = SAD(x,y)

        print results.max(), results.min()
        print "Done SAD"

        threshold = 70.0

        positions = []

        max_val = results.max()

        def arg_min(a):
            min_val = float('inf')
            min_x = 0
            min_y = 0
            for x in range(a.shape[0]):
                for y in range(a.shape[1]):
                    if a[x,y] < min_val:
                        min_val = a[x,y]
                        min_x = x
                        min_y = y
            return min_x, min_y

        count = 0
        while True:
            #If this is below the confidence level then quit
            min_index = arg_min(results)
            pixel_value = results[min_index]
            print pixel_value
            if pixel_value > threshold:
                break

            #Add it to potential positions
            positions.append(min_index)

            #Local max suppression
            def suppress_area(x,y):
                for i in range(-template_size[0]/2, template_size[0]/2):
                    for j in range(-template_size[1]/2, template_size[1]/2):
                        if x+i < 0 or y+j < 0:
                            continue
                        if x+i >= results.shape[0] or y+j >= results.shape[1]:
                            continue
                        results[x+i, y+j] = max_val

            suppress_area(min_index[0], min_index[1])
            out = results * (255.0/results.max())
            cv2.imwrite('./results_%d.png'%(count), out)
            count += 1

        #DONE-ish the points need to be shifted by half a template image size
        print positions

        #CONVERT POINTS TO WORLD COORDS
        cx = self.template.shape[0]/2
        cy = self.template.shape[1]/2
        positions = [raw_to_mouse(p[0]+tx+cx, p[1]+ty+cx) for p in positions]

        def aff_trans(x, y):
            aff = self.video.aff_matrix
            wx = x*aff[0][0] + y*aff[0][1] + aff[0][2]
            wy = x*aff[1][0] + y*aff[1][1] + aff[1][2]
            return wx,wy

        self.donuts = [aff_trans(p[0], p[1]) for p in positions]
        

    def exec_path(self):
        print "Execute Path"
        def get_phi(x,y):
            from math import sqrt, pow
            r = sqrt(pow(x,2)+pow(y,2))
            if r <= 95.0:
                return -135.0*D2R
            elif r > 95.0 and r < 190.0:
                return -90.0*D2R
            else:
                return -45.0*D2R

        self.donuts.sort(key=lambda x: math.atan2(x[1], x[0]))
        from itertools import chain, izip
        world_points = [(x,y, 50.0, get_phi(x,y)) for (x,y) in self.donuts]
        in_points = [(x,y, 10.0, get_phi(x,y)) for (x,y) in self.donuts]

        final_points = []
        for i,p in enumerate(world_points):
            final_points.append(p)
            final_points.append(in_points[i])
            final_points.append(p)

        config_space = [self.rex.rexarm_IK(p, 1) for p in final_points if self.rex.rexarm_IK(p, 1) != None]

        print "Playback"
        self.path_start_time = micro_time()
        self.rex.wpt_number = 1
        self.rex.plan_status = 1
        self.rex.max_torque = 50.0
        self.ui.sldrMaxTorque.setValue(50.0)
        self.executed_path = []

        self.rex.plan = [[0, self.rex.joint_angles_fb[:], [0,0,0,0]]] + \
                        [[3000000*(i+1), q, [0,0,0,0]] for i,q in enumerate(config_space)]

        print self.rex.plan
        self.rex.wpt_total = len(self.rex.plan)