コード例 #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)
コード例 #2
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))

        """ 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)
コード例 #3
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.ui.rdoutX, self.ui.rdoutY, self.ui.rdoutZ,
                          self.ui.rdoutT)
        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
        LAB TASK: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI 
        """
        self.ui.sldrBase.valueChanged.connect(
            functools.partial(self.sliderChange, 0))
        self.ui.sldrShoulder.valueChanged.connect(
            functools.partial(self.sliderChange, 1))
        self.ui.sldrElbow.valueChanged.connect(
            functools.partial(self.sliderChange, 2))
        self.ui.sldrWrist.valueChanged.connect(
            functools.partial(self.sliderChange, 3))
        self.ui.sldrGrip1.valueChanged.connect(
            functools.partial(self.sliderChange, 4))
        self.ui.sldrGrip2.valueChanged.connect(
            functools.partial(self.sliderChange, 5))
        self.ui.sldrMaxTorque.valueChanged.connect(
            functools.partial(self.sliderChange, 6))
        self.ui.sldrSpeed.valueChanged.connect(
            functools.partial(self.sliderChange, 7))

        #Setting the poses
        #Pick up position
        self.ui.btnUser2.setText("Pick Up Position")
        self.ui.btnUser2.clicked.connect(
            functools.partial(self.setPose,
                              [-0.063, 0.203, -.605, -1.493, -0.107, 1.702]))
        #Home position (Outside kinect view)
        self.ui.btnUser4.clicked.connect(
            functools.partial(self.setPose,
                              [-2.015, -1.89, 0.318, -1.135, -0.47, 1.723]))
        #self.ui.btnUser4.clicked.connect(functools.partial(self.setPose,[0.622,1.119,-0.069,1.125]))
        #self.ui.btnUser5.clicked.connect(functools.partial(self.setPose,[0,0,0,0]))

        #Robot frame points. Index 3 is phi, the grasping angle with respect to the world frame
        point = [0, 0.44, 0, 90 * D2R]
        #Regular rexarm position
        point = [0.002, 0.189, -0.056, (90 + 72) * D2R]
        point = [0.004, 0.388, -0.008, (90) * D2R]
        point = [-0.002, 0.361, 0, 90 * D2R]
        point = [0, 0.24, -0, 02, 90 * D2R]
        point = [0.14, 0.04, 0.14, 90 * D2R]
        #Test Case
        point = [.22, 0, .22, 18 * D2R]
        #Different test cases
        point = [.12, 0.22, 0.1, 90 * D2R]
        point = [-0.03, -0.17, 0.074, 90 * D2R]
        #Testing a pick up position (Waterbottle)
        point = [0.039, -0.002, 0.35, 37 * D2R]
        point = [0.18, 0, 0.294, (352 * D2R)]
        point = [0.131, 0.139, -0.015, 87 * D2R]
        point = [0.184, 0.202, -0.02, 38 * D2R]
        self.ui.btnUser6.setText("IK on " + str(point))
        self.ui.btnUser6.clicked.connect(functools.partial(self.runIK, point))

        #point = [0,0.05,0.082, 90 * D2R]
        #self.ui.btnUser7.setText("IK on " + str(point))
        #self.ui.btnUser7.clicked.connect(functools.partial(self.runIK,np.transpose(point)))

        self.ui.btnUser3.setText("Straight Position")
        self.ui.btnUser3.clicked.connect(
            functools.partial(self.setPose, [0, 0, 0, 0, 0, 0]))

        self.ui.btnUser11.setText("Recall Position")
        self.ui.btnUser11.clicked.connect(self.recall_pose)

        self.ui.btnUser12.setText("Save Position")
        self.ui.btnUser12.clicked.connect(self.save_pose)
        """ Commands the arm as the arm initialize to 0,0,0,0 angles """
        self.sliderChange(0)
        """ Connect Buttons to Functions 
        LAB TASK: NAME AND CONNECT BUTTONS AS NEEDED
        """
        self.ui.btnUser1.setText("Affine Calibration")
        self.ui.btnUser1.clicked.connect(self.affine_cal)

    #Holds the current rexarm position when torque has been set to zero
    def save_pose(self):
        self.saved_angles = self.rex.joint_angles_fb[:]

    def recall_pose(self):
        self.setPose(self.saved_angles)

    #Takes a list of [x,y,z] in kinect coordinates
    def kinect_world_to_rexarm_world(self, kinect_coords):
        x = kinect_coords[0]
        y = kinect_coords[1]
        z = -kinect_coords[2]

        #produce
        rot_angle = -30 * D2R
        rot_x = np.array([[1, 0, 0, 0],
                          [0, np.cos(rot_angle), -np.sin(rot_angle), 0],
                          [0, np.sin(rot_angle),
                           np.cos(rot_angle), 0], [0, 0, 0, 1]])

        rot_angle = 90 * D2R
        rot_z = np.array([[np.cos(rot_angle), -np.sin(rot_angle), 0, 0],
                          [np.sin(rot_angle),
                           np.cos(rot_angle), 0, 0], [0, 0, 1, 0],
                          [0, 0, 0, 1]])

        translation = np.array([[1, 0, 0, .072], [0, 1, 0, 0],
                                [0, 0, 1, -.625], [0, 0, 0, 1]])

        inv_xform = np.dot(np.dot(rot_x, rot_z), translation)

        #Invert the matrix produced so we can go from kinect coordinates to rexarm coordinates
        xform = numpy.linalg.inv(inv_xform)
        coords = np.array([[x], [y], [z], [1]])

        rex_coords = np.dot(xform, coords)

        #Clamp z if below this limit
        z_limit = -0.01
        if rex_coords[2][0] <= z_limit:
            print "z_limit was", rex_coords[2][0], "; clamped it to", z_limit
            rex_coords[2][0] = z_limit

        return [rex_coords[0][0], rex_coords[1][0], rex_coords[2][0]]

    #Runs inverse kinematics and only returns hardware thetas. Up to the
    #user to set the pose
    def runIK_noCommand(self, xyz_phi_world):
        #In the robot frame
        xyz_world = xyz_phi_world[0:3]
        phi_world = xyz_phi_world[3]

        #In the rexarm frame
        xyz_rexarm = None  #To be computed

        #print "World Coords:", xyz_world

        #Homogeneous coordinates
        xyz_world = np.append(xyz_world, 1)
        xyz_world = xyz_world.reshape(4, 1)

        #print "Shape:", xyz.shape
        #Convert xyz to rexarm coordinates (with respect to frame of point right below joint 1) by using inverse matrix

        #shift_horizontal_only = self.rex.trans_base.copy()
        #Don't shift along z axis. Only shift along x
        #shift_horizontal_only[2][3] = 0
        #import pdb
        #pdb.set_trace()

        #transformation = np.dot(np.dot(np.dot(self.rex.trans_magicbase,self.rex.rot_72),self.rex.rot_90),shift_horizontal_only)
        #inv_transformation = np.linalg.inv(transformation)

        #Convert desired IK coordinates from world frame to robot frame
        #xyz_rexarm = np.dot(inv_transformation,xyz_world)
        xyz_rexarm = xyz_world.copy()
        xyz_rexarm = xyz_rexarm[0:3]

        print "Rexarm Frame Coords:", xyz_rexarm

        phi_rexarm = phi_world  # - 72 * D2R #Converts grasping angle in world frame to angle in rexarm frame.
        xyz_rexarm = np.append(xyz_rexarm, phi_rexarm)

        xyz_rexarm = xyz_rexarm.reshape(4, 1)

        #import pdb
        #pdb.set_trace()

        #Run Inverse kinematics
        DH_thetas = self.rex.rexarm_IK(xyz_rexarm, 1)

        #Send arm_thetas to the rexarm
        #First convert from DH parameter angle to hardware servo angle
        hardware_thetas = list(DH_thetas.copy())
        for i in range(len(hardware_thetas)):
            hardware_thetas[i] -= self.rex.joint_offsets[i]

        # Append 0,0 for now for the remaining two joints
        hardware_thetas.append(0)
        hardware_thetas.append(0)

        return hardware_thetas

    #Runs inverse kinematics on xyz_phi_world, which has form
    #[x,y,z,phi_in_radians]
    def runIK(self, xyz_phi_world):
        hardware_thetas = self.runIK_noCommand(xyz_phi_world)
        print "Send thetas:", hardware_thetas

        #Send pose to Rexarm
        self.setPose(hardware_thetas)

    def play(self):
        """ 
        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"
            pass
        """ 
        Update GUI Joint Coordinates Labels
        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):
                """ TO DO Here is where affine calibration must be used """
                self.ui.rdoutMouseWorld.setText("(-,-)")
            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 sliderChange(self, selected_slider):
        """ 
        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
        """
        #print "Selected joint:", selected_slider
        if 0 <= selected_slider and selected_slider <= 5:
            angle = 0
            if selected_slider == 0:
                self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
                angle = self.ui.sldrBase.value() * D2R
            elif selected_slider == 1:
                self.ui.rdoutShoulder.setText(str(
                    self.ui.sldrShoulder.value()))
                angle = self.ui.sldrShoulder.value() * D2R
            elif selected_slider == 2:
                self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
                angle = self.ui.sldrElbow.value() * D2R
            elif selected_slider == 3:
                self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))
                angle = self.ui.sldrWrist.value() * D2R
            elif selected_slider == 4:
                self.ui.rdoutGrip1.setText(str(self.ui.sldrGrip1.value()))
                angle = self.ui.sldrGrip1.value() * D2R
            elif selected_slider == 5:
                self.ui.rdoutGrip2.setText(str(self.ui.sldrGrip2.value()))
                angle = self.ui.sldrGrip2.value() * D2R

            self.rex.joint_angles[selected_slider] = angle
        elif 6 <= selected_slider and selected_slider <= 7:
            if selected_slider == 6:
                self.ui.rdoutTorq.setText(
                    str(self.ui.sldrMaxTorque.value()) + "%")
                self.rex.max_torque = self.ui.sldrMaxTorque.value() / 100.0
            elif selected_slider == 7:
                self.ui.rdoutSpeed.setText(
                    str(self.ui.sldrSpeed.value()) + "%")
                self.rex.speed = self.ui.sldrSpeed.value() / 100.0
        else:
            print "Error: Unrecognized slider index", selected_slider

        self.rex.cmd_publish()

    # angles is a list of floats, an angle for each joint
    #Use this with state machine since lcm feedback handler isn't working.
    #current_angles is provided by Rexarm state machine since rex.joint_angles_fb
    #isn't being updated with the rexarm lcm handler
    #Returns the new pose as a variable
    def setPose(self, desired_angles, current_angles=[]):
        #Use the linear motion plan that the professor explained
        step_size = 0.04
        t_range = list(np.arange(0, 1 + step_size, step_size))

        desired_angles = np.array(desired_angles)

        initial_angles = None
        if len(current_angles) == 0:
            initial_angles = np.array(self.rex.joint_angles_fb)
        else:
            initial_angles = np.array(current_angles)

        for t in t_range:
            new_pose = desired_angles * t + initial_angles * (1 - t)
            #Sends motor command to rexarm
            for i in range(len(new_pose)):
                #Obey joint_limits to prevent breaking motors
                if new_pose[i] < self.rex.joint_limits[i][0]:
                    new_pose[i] = self.rex.joint_limits[i][0]
                if new_pose[i] > self.rex.joint_limits[i][1]:
                    new_pose[i] = self.rex.joint_limits[i][1]

                self.rex.joint_angles[i] = new_pose[i]
            self.rex.cmd_publish()
            time.sleep(0.1)

        return desired_angles

    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.
            
            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):
                """ 
                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)
                """ Updates Status Label to inform calibration is done """
                self.ui.rdoutStatus.setText("Waiting for input")
                """ 
                print affine calibration matrix numbers to terminal
                """
                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.ui.rdoutStatus.setText("Affine Calibration: Click Point %d" %
                                    (self.video.mouse_click_id + 1))

    #Initialize socket to send acknowledgement over
    def send_socket_ack(self):
        print "Sending socket acknowledgement..."
        self.kinect_endpoint = "/tmp/kinect_endpoint"

        #if not os.path.isfile(self.kinect_endpoint):
        #    open(self.kinect_endpoint, 'w').close()

        try:
            self.sock.sendto("1", self.kinect_endpoint)
            print "Acknowledgement successfully sent."
        except Exception as e:
            print str(e)
            print "Failed to send acknowledgement."
        return

    def init_socket(self):
        self.sock = socket.socket(
            socket.AF_UNIX,  # Local computer
            socket.SOCK_DGRAM)  # UDP
        self.kinect_path_recv = "/tmp/rexarm_endpoint"

        try:
            os.remove(self.kinect_path_recv)
        except OSError:
            pass
        self.sock.bind(self.kinect_path_recv)
        print "Socket binded. Ready to receive data."
        # Don't need this?
        #self.sock.listen(1)

        #Blocks until Kinect code connects
        #self.conn, self.addr = self.sock.accept()

    def get_socket_data(self):
        point = None

        #Grasping Point struct is 28 bytes
        while True:
            print "Waiting for data..."
            #Num bytes in grasping point struct
            struct_size = 40
            data, addr = self.sock.recvfrom(struct_size)
            print "Received", struct_size, " bytes"
            if not data:
                #Error
                pass
            #p is point. n is normal
            #TODO: Check endianness
            time, p1, p2, p3, n1, n2, n3, a1, a2, a3 = struct.unpack(
                "ifffffffff", data)
            print "Time:", time
            print "Point (mm):", p1, p2, p3
            print "Normal (mm):", n1, n2, n3
            print "Principal Axis (mm):", a1, a2, a3
            point = [p1 / 1000.0, p2 / 1000.0, p3 / 1000.0]
            axis = [a1 / 1000.0, a2 / 1000.0, a3 / 1000.0]
            break
        return point, axis

    #Busy waits code until rexarm has reached desired pose
    def wait_until_reached(self, pose, ignore_grasp=False):
        self.rex.lcm_mutex.acquire()
        while not self.reached_pose(pose, ignore_grasp):
            #time.sleep(1)
            print "Desired Pose:", pose
            print "Current Rexarm Pose:", self.rex.joint_angles_fb
            print "not reached desired pose yet. Thread going to sleep..."
            self.rex.lcm_cv.wait()
            #print "Woke up"
        self.rex.lcm_mutex.release()

    #Returns true if the rexarm's angles match the pose input approximately.
    #Use this to repeatedly check if rexarm has reached a configuration
    # pose is a list of angles for each rexarm_joint ex. [0,0,0,0,0,0]
    def reached_pose(self, pose, ignore_grasp):
        reached = True
        allowed_error = 0.3  #radians

        limit = 0
        if ignore_grasp:
            limit = len(self.rex.joint_angles_fb) - 1
        else:
            limit = len(self.rex.joint_angles_fb)

        for i in range(limit):
            #If error for any of the joints is > 0.01, then arm is not
            #at the desired location.
            if abs(self.rex.joint_angles_fb[i] - pose[i]) > allowed_error:
                reached = False
                break
        return reached

    #Instantly publishes pose to rexarm without any motion smoothing
    #Used to save time
    def instant_publish(self, pose):
        self.rex.joint_angles = pose[:]
        self.rex.cmd_publish()

    #Checks if pose, a list of length 6, has angles within the range
    #of the rexarm's joint limits
    def check_pose_valid(self, pose):
        valid = True
        for i in range(len(pose)):
            if pose[i] < self.rex.joint_limits[i][0] or self.rex.joint_limits[
                    i][1] < pose[i]:
                valid = False
                break
        return valid

    #Returns angle between x-axis and princiapl axis in range 0 to 360
    #x-axis and principal axis are 3D lists
    #Takes in x-axis and principal axis as 1D arrays of size 2
    def compute_2D_angle(self, x_axis, princ_axis):
        #Use the dot product
        x_np = np.array(x_axis)
        p_np = np.array(princ_axis)

        #Find angle
        temp = np.dot(
            x_np, p_np) / (numpy.linalg.norm(x_np) * numpy.linalg.norm(p_np))
        #Take arccos
        angle = math.acos(temp)
        #Angle is in range 0 to pi

        return_angle = None
        #Quadrant 1
        if p_np[0] > 0 and p_np[1] >= 0:
            return_angle = angle
        #Quadrant 2
        elif p_np[0] <= 0 and p_np[1] >= 0:
            return_angle = angle
        #Quadrant 3
        elif p_np[0] < 0 and p_np[1] < 0:
            return_angle = 2 * math.pi - angle
        #Quadrant 4
        elif p_np[0] >= 0 and p_np[1] < 0:
            return_angle = 2 * math.pi - angle

        return return_angle

    #Converts vector defined in kinect coordinates to vector defined in rexarm coordinates.
    #Must be lists
    def kinect_vec_to_rexarm_vec(self, kinect_vec):
        #Convert [0,0,0] and principal_axis point to rexarm coordinates
        #Get the vector in rexarm world
        start_point_rex = self.kinect_world_to_rexarm_world([0, 0, 0])
        end_point_rex = self.kinect_world_to_rexarm_world(kinect_vec)
        rex_vec = list(np.array(end_point_rex) - np.array(start_point_rex))
        return rex_vec

    #Takes in the principal axis in terms of rexarm coordinates as
    # a [x,y,z] list and returns hardware angle for wrist to rotate to
    def compute_wrist_angle(self, rex_axis_input):
        #Project the axis vector onto the rexarm's x-y plane by setting z to 0
        rex_axis = rex_axis_input[:]
        rex_axis[2] = 0

        #2D vector
        axis = rex_axis[:2]

        #Find angle between x-axis of rexarm and the rex_principal_axis in range 0
        #to 360
        x_axis = [1, 0]
        angle_2d = self.compute_2D_angle(x_axis, axis)
        print "Angle between Rexarm x axis and principal axis:", angle_2d

        assert (0 <= angle_2d and angle_2d <= 2 * math.pi)

        phi = 0

        #Based on 4 cases, compute wrist_angle
        if 0 < angle_2d and angle_2d <= math.pi / 2:
            phi = -(math.pi / 2 - angle_2d)
        if math.pi / 2 < angle_2d and angle_2d <= math.pi:
            phi = angle_2d - math.pi / 2
        if math.pi < angle_2d and angle_2d <= (3 * math.pi / 2):
            phi = -(3 * math.pi / 2 - angle_2d)
        if (3 * math.pi / 2) < angle_2d and angle_2d <= 2 * math.pi:
            phi = angle_2d - 3 * math.pi / 2

        #Add pi/2 to phi for unknown reason
        phi += math.pi / 2

        return phi

    #base vector is [x,y,z]
    def get_principal_angle(self, principal_axis_rexarm, base_vector):
        #Get angle between rexarm principal axis and vector from origin
        #to object.
        #First project both to z = 0
        p = np.array(principal_axis_rexarm[0:2])
        v = np.array(base_vector[0:2])

        #Flip principal axis if in quadrants 2 or 3
        if p[0] < 0:
            p = -p

        temp = np.dot(p, v) / (numpy.linalg.norm(p) * numpy.linalg.norm(v))
        angle = math.acos(temp)

        return angle

    def trash_state_machine(self):
        #Setting the torque and speed. Ranges from 0 to 1
        self.rex.max_torque = 0.55
        self.rex.speed = 0.4
        self.rex.cmd_publish()

        #Thread to call rex.get_feedback, which enables the callback
        #function to work
        try:
            thread.start_new_thread(self.rex.get_feedback, (False, ))
            #t = Thread(target = self.rex.get_feedback, args = (False,))
            #t.start()
            print "Started get_feedback thread"
        except:
            print "Unable to start get_feedback thread"

        self.init_socket()

        tighten_gripper = 115 * D2R
        net_base_angle = -1.71
        poses = {
            "HOME": [0, 0, 0, 0, 0, tighten_gripper],  #Tightens gripper
            "HIDE": [1.557, -2.03, -0.629, 1.079, -0.061, 1.994],
            "PRE_DUNK":
            [1.544, 0.233, -0.781, -0.736, -1.565, tighten_gripper],
            "ABOUT_TO_DUNK":
            [1.545, 0.040, -0.019, 0.046, -1.564, tighten_gripper],
            "DUNK": [1.237, -0.124, 0.26, 0.854, -1.559, tighten_gripper]
            #"NET_ARCH": [net_base_angle,-0.135,-1.223,-1.447,-0.061,tighten_gripper]
        }

        #TODO: populate this variable with the pose we're currently trying to reach
        #Use this since LCM feedback handler isn't being called :(
        current_pose = None
        next_pose = None

        #x,y,z coordinates to conduct IK for
        desired_IK = []
        #Angle at which to rotate wrist and grab
        wrist_rot_angle = 0
        #Angles to command rexarm for inverse kinematics
        IK_cmd_thetas = None

        principal_axis_kinect = None

        #State1: Turn 90 degrees at base to prevent collision
        states = [
            "START", "RUN_IK_TURN_BASE", "RUN_IK_DESCEND", "GRASP", "LIFT_UP",
            "TURN_TO_NET", "ARCH_TO_NET", "DROP", "UNARCH",
            "TURN_TO_HOME_FROM_NET", "HIDE_POSITION_1", "HIDE_POSITION_2",
            "UNHIDE", "TURN_TO_HOME_FROM_UNHIDE"
        ]
        curr_state = "START"
        synchro_timer = 1.5
        start = True
        linear = True
        #Set to True if we're in a state where we're holding object
        grasp = False
        #Set to True if we're in the grasping state
        clamp = False

        while True:
            print "----------------------------------------"
            print "Current State:", curr_state
            if curr_state == "START":
                next_pose = poses["HOME"][:]
                next_state = "HIDE_POSITION_1"
            elif curr_state == "RUN_IK_TURN_BASE":
                #Run_IK
                IK_cmd_thetas = []
                IK_successful = True
                IK_message = ""
                try:
                    IK_cmd_thetas = self.runIK_noCommand(desired_IK)
                except Exception as e:
                    IK_successful = False
                    IK_message = "ERROR: IK encountered runtime error.\n" + str(
                        e)
                if not self.check_pose_valid(IK_cmd_thetas):
                    IK_successful = False
                    IK_message = "ERROR: IK gave angles outside of feasible range.\n"

                print "IK_result:", IK_cmd_thetas

                #Check that the joint angles returned by IK are possible
                if not IK_successful:
                    print IK_message
                    print "Returning to Hide position"
                    linear = False
                    next_state = "HIDE_POSITION_2"
                else:
                    #Turn base towards object
                    next_pose[0] = IK_cmd_thetas[0]
                    #Twist wrist
                    #next_pose[4] = IK_cmd_thetas[4]
                    linear = False
                    self.instant_publish(next_pose)
                    print "Turned Base to:", next_pose[0]
                    #TODO: Get lcm joint angles returned from runIK so that
                    #we can wait before grasping
                    next_state = "TURN_WRIST"

            elif curr_state == "TURN_WRIST":
                #Current base angle
                print "Current base angle after turning:", self.rex.joint_angles_fb[
                    0]
                print "Principal Axis Vector in Kinect World:", principal_axis_kinect
                print "Principal Axis Vector in Rexarm World:", principal_axis_rexarm

                #Wrist rotation angle with respect to the x-axis of the rexarm base frame
                wrist_rot_angle = self.compute_wrist_angle(
                    principal_axis_rexarm)

                print "wrist rotation angle determined with respect to rexarm x-axis:", wrist_rot_angle

                #Subtract the rotation of the base before adding the wrist_rot_angle
                IK_cmd_thetas[
                    4] = wrist_rot_angle - self.rex.joint_angles_fb[0]
                print "wrist rotation angle after subtracting base joint rotation:", IK_cmd_thetas[
                    4]

                #Now subtract the angle between principal axis and vector from
                #base to object
                axis_baserot_angle = self.get_principal_angle(
                    principal_axis_rexarm, desired_IK[:3])

                IK_cmd_thetas[4] -= axis_baserot_angle

                #---------------------------TODO: Put in function
                #Map it to an untangled state from -wrist_limit to wrist_limit
                IK_cmd_thetas[4] = IK_cmd_thetas[4] % (2 * math.pi)
                print "wrist rotation angle after modulus with 2PI:", IK_cmd_thetas[
                    4]

                #Map it from -PI to PI
                if IK_cmd_thetas[4] > math.pi:
                    IK_cmd_thetas[4] -= 2 * math.pi
                print "wrist rotation angle after restricting to -PI to PI:", IK_cmd_thetas[
                    4]

                if IK_cmd_thetas[4] <= -self.rex.wrist_limit:
                    IK_cmd_thetas[4] += math.pi
                if IK_cmd_thetas[4] >= self.rex.wrist_limit:
                    IK_cmd_thetas[4] -= math.pi
                print "wrist rotation angle after restricting to wrist range:", IK_cmd_thetas[
                    4]

                #---------------------------TODO: Put in function

                IK_cmd_thetas[4] = 0

                next_pose[4] = IK_cmd_thetas[4]
                linear = False
                self.instant_publish(next_pose)
                next_state = "RUN_IK_DESCEND"

            elif curr_state == "RUN_IK_DESCEND":
                print "About to descend:"
                print "Current pose:", self.rex.joint_angles_fb
                print "Desired Pose:", IK_cmd_thetas
                #Descend the rest of the IK outside of base
                next_pose = IK_cmd_thetas[:]
                next_state = "GRASP"
            elif curr_state == "GRASP":
                #Set joint 5 to grasp
                next_pose[5] = tighten_gripper
                clamp = True
                next_state = "PRE_DUNK"

            #------------------------------------------------------------------------------------
            #Lift to pre-dunk state after grasping. Ignore last joint
            elif curr_state == "PRE_DUNK":
                next_pose = poses["PRE_DUNK"][:]
                grasp = True
                next_state = "ABOUT_TO_DUNK"

            elif curr_state == "ABOUT_TO_DUNK":
                next_pose = poses["ABOUT_TO_DUNK"][:]
                grasp = True
                linear = False
                self.instant_publish(next_pose)
                next_state = "DUNK"

            elif curr_state == "DUNK":
                next_pose = poses["DUNK"][:]
                grasp = True
                #linear = False
                #self.instant_publish(next_pose)
                next_state = "DROP"

            elif curr_state == "DROP":
                #Set joint 5 to 0 angle
                next_pose[5] = 0
                linear = False
                self.instant_publish(next_pose)
                next_state = "RETURN_HOME"

            elif curr_state == "RETURN_HOME":
                next_pose = poses["ABOUT_TO_DUNK"][:]
                next_state = "HOME_TURN"

            elif curr_state == "HOME_TURN":
                next_pose = poses["HOME"][:]
                linear = False
                self.instant_publish(next_pose)
                next_state = "HIDE_POSITION_2"

            #------------------------------------------------------------------------------------

            elif curr_state == "UNHIDE":
                #Go to home position. Then run IK
                next_pose = poses["HOME"][:]
                next_state = "RUN_IK_TURN_BASE"
            elif curr_state == "HIDE_POSITION_1":
                next_pose = poses["HIDE"][:]
                next_state = "SOCKET_READ"
            elif curr_state == "HIDE_POSITION_2":
                next_pose = poses["HIDE"][:]
                next_state = "SEND_ACK"
            elif curr_state == "SEND_ACK":
                #Acknowledge that we are done picking up and disposing of object
                self.send_socket_ack()
                linear = False
                next_state = "SOCKET_READ"
            elif curr_state == "SOCKET_READ":
                #Block and wait for next point of new object
                kin_point, principal_axis_kinect = self.get_socket_data()
                #Convert to rexarm coordinates from kinect coordinates
                rex_point = self.kinect_world_to_rexarm_world(kin_point)

                #Converts principal axis from kinect coordinates to rexarm coordinates
                principal_axis_rexarm = self.kinect_vec_to_rexarm_vec(
                    principal_axis_kinect)
                #principal_axis_rexarm = [2,-1,5]
                #principal_axis_rexarm = [1,0,5]
                #print "Principal Axis Vector in Kinect World:", principal_axis_kinect
                print "Principal Axis Vector in Rexarm World:", principal_axis_rexarm

                desired_IK = [
                    rex_point[0], rex_point[1], rex_point[2], 87 * D2R
                ]

                #desired_IK = [0.131,0.139,-0.015, 87 * D2R]
                #desired_IK = [0.15,0.1,0.05, 87 * D2R]
                #desired_IK = [0.15,0.15,0, 87 * D2R]
                print "Inverse Kinematics Target:"
                print "Goal x in Rexarm:", desired_IK[0]
                print "Goal y in Rexarm:", desired_IK[1]
                print "Goal z:", desired_IK[2]
                linear = False
                #Not setting next_pose
                next_state = "UNHIDE"
            if linear:
                if start:
                    self.setPose(next_pose)
                    start = False
                else:
                    self.setPose(next_pose, current_pose)
            else:
                linear = True

            #Setting current_pose to whatever next_pose was
            #determined to be
            current_pose = next_pose[:]
            if not clamp:
                if not grasp:
                    self.wait_until_reached(next_pose)
                else:
                    self.wait_until_reached(next_pose, ignore_grasp=True)
                    grasp = False
            else:
                time.sleep(synchro_timer)
                clamp = False

            print "Next State:", next_state
            print "----------------------------------------"
            curr_state = next_state
コード例 #4
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.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