예제 #1
0
class Gui(QtGui.QMainWindow):
    """ 
    Main GUI Class
    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()
        self.statemachine = Statemachine()

        #self.video.extrinsic = np.array(self.video.getcaldata())
        #self.video.inv_extrinsic = np.array(np.linalg.inv(self.video.extrinsic))
        """ Other Variables """
        self.last_click = np.float32([0, 0])
        """ Set GUI to track mouse """
        QtGui.QWidget.setMouseTracking(self, True)
        self.statemachine.setme(self.ui, self.rex)
        """ 
        GUI timer 
        Creates a timer and calls update_gui() function 
        according to the given time delay (27ms) 
        """
        self._timer = QtCore.QTimer(self)
        self._timer.timeout.connect(self.update_gui)
        self._timer.start(27)
        """ 
        LCM Arm Feedback timer
        Creates a timer to call LCM handler for Rexarm feedback
        """
        self._timer2 = QtCore.QTimer(self)
        self._timer2.timeout.connect(self.rex.get_feedback)
        self._timer2.start()
        """
	Giri - Statemachine timer
	"""
        #self._timer3 = QtCore.QTimer(self)
        #self._timer3.timeout.connect(self.statemachine.timerCallback)
        #self._timer3.start(100)  # frequency of the timer is set by this. it is 100 ms now. However, the time.now can give microseconds precision not just millisecond
        """ 
        Connect Sliders to Function
        TODO: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI 
        """
        self.ui.sldrBase.valueChanged.connect(self.sliderBaseChange)
        self.ui.sldrShoulder.valueChanged.connect(
            self.sliderShoulderChange)  #Giri
        self.ui.sldrElbow.valueChanged.connect(self.sliderElbowChange)  #Giri
        self.ui.sldrWrist.valueChanged.connect(self.sliderWristChange)  #Giri
        self.ui.sldrGrip1.valueChanged.connect(self.sliderGrip1Change)  #Giri
        self.ui.sldrGrip2.valueChanged.connect(self.sliderGrip2Change)  #Giri
        self.ui.sldrMaxTorque.valueChanged.connect(
            self.sliderOtherChange)  #Giri
        self.ui.sldrSpeed.valueChanged.connect(self.sliderOtherChange)  #Giri
        if (self.rex.num_joints == 6):
            self.ui.sldrGrip1.setEnabled(True)  #Giri
            self.ui.sldrGrip2.setEnabled(True)  #Giri
        else:
            self.ui.sldrGrip1.setEnabled(False)  #Giri
            self.ui.sldrGrip2.setEnabled(False)  #Giri
        """ Initial command of the arm to home position"""
        self.sliderChange()
        self.reset()
        """ Connect Buttons to Functions 
        TODO: NAME AND CONNECT BUTTONS AS NEEDED
        """
        self.ui.btnUser1.setText("Configure Servos")
        self.ui.btnUser1.clicked.connect(self.btn1)  #Giri

        self.ui.btnUser2.setText("Depth and RGB Calibration")
        self.ui.btnUser2.clicked.connect(self.btn2)  #Giri
        self.ui.btnUser2.setEnabled(False)

        self.ui.btnUser3.setText("Extrinsic Calibration")  #Giri
        self.ui.btnUser3.clicked.connect(self.btn3)

        self.ui.btnUser4.setText("Block Detector")  #Giri
        self.ui.btnUser4.clicked.connect(self.btn4)

        self.ui.btnUser5.setText("Repeat")
        self.ui.btnUser5.clicked.connect(self.btn5)  #Giri

        self.ui.btnUser6.setText("Teach")
        self.ui.btnUser6.clicked.connect(self.teach)  #Giri

        self.ui.btnUser7.setText("Smooth Repeat")
        self.ui.btnUser7.clicked.connect(self.srepeat)  #Giri
        self.ui.btnUser7.setEnabled(False)  #Giri

        self.ui.btnUser8.setText("Pick Block")
        self.ui.btnUser8.clicked.connect(self.pick)

        self.ui.btnUser9.setText("Place Block")
        self.ui.btnUser9.clicked.connect(self.place)

        self.ui.btnUser10.setText("Reset")
        self.ui.btnUser10.clicked.connect(self.reset)  #Giri

        self.ui.btnUser11.setText("Enter Competition Mode")
        self.ui.btnUser11.clicked.connect(self.competition)  #Giri

        self.ui.btnUser12.setText("Emergency Stop!")
        self.ui.btnUser12.clicked.connect(self.estop)

    def estop(self):
        self.statemachine.estop(self.rex)

    def trim_angle(self, q, motorno=1):  #for 1,2,3
        if (motorno > 2):
            max_angle = 300
        else:
            max_angle = 360

        if (q > max_angle or q < -1 * max_angle):
            new_q = 0
            print "trimmed: " + str(q) + " motor no: " + str(motorno)
        elif (q > 180):  #181 --> -179
            new_q = q - 360
            print "trimmed: " + str(q) + " motor no: " + str(motorno)
        elif (q < -180):
            new_q = 360 - q
            print "trimmed: " + str(q) + " motor no: " + str(motorno)
        else:
            new_q = q
        return new_q

    def shortorientation(self, w_angle):
        if (w_angle > 90):
            sw_angle = w_angle - 90
        elif (w_angle < -90):
            sw_angle = w_angle + 90
        else:
            sw_angle = w_angle
        return sw_angle

    def update_gui(self):
        """ 
        update_gui function
        Continuously called by timer1 
        """
        """ Renders the video frames
            Displays a dummy image if no video available
            HINT: you can use the radio buttons to select different
            video sources like is done below
        """
        color = self.statemachine.timerCallback()
        if (color == "Decluttered"):
            self.declutter()
        elif (color != "none"):
            self.getangles(color)
        if (self.video.kinectConnected == 1):
            try:
                self.video.captureVideoFrame()
                self.video.captureDepthFrame()
            except TypeError:
                self.video.kinectConnected = 0
                self.video.loadVideoFrame()
                self.video.loadDepthFrame()

        if (self.ui.radioVideo.isChecked()):
            self.ui.videoFrame.setPixmap(self.video.convertFrame())

        if (self.ui.radioDepth.isChecked()):
            self.ui.videoFrame.setPixmap(self.video.convertDepthFrame())
        """ 
        Update GUI Joint Coordinates 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)))
        self.ui.rdoutGrip1.setText(str(self.ui.sldrGrip1.value()))  #Giri
        self.ui.rdoutGrip2.setText(str(self.ui.sldrGrip2.value()))  #Giri
        """
        Update End Effector Coordinates Kevin
	"""
        endCoord = forwardKinematics(self.rex.joint_angles_fb[0] * R2D,
                                     self.rex.joint_angles_fb[1] * R2D,
                                     self.rex.joint_angles_fb[2] * R2D,
                                     self.rex.joint_angles_fb[3] * R2D)
        self.ui.rdoutX.setText(str("%2f" % (round(endCoord[0], 2))))
        self.ui.rdoutY.setText(str("%2f" % (round(endCoord[1], 2))))
        self.ui.rdoutZ.setText(str("%2f" % (round(endCoord[2], 2))))
        self.ui.rdoutT.setText(str("%2f" % (round(endCoord[3], 2))))
        """ 
        Mouse position presentation in GUI
        TODO: after implementing workspace calibration 
        display the world coordinates the mouse points to 
        in the RGB video image.
        """
        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
            z = self.video.currentDepthFrame[y][x]
            self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" % (x, y, z))
            if (self.video.cal_flag == 2):
                M = self.video.aff_matrix
                v = np.float32([x, y, 1])
                rw = np.dot(M, v)
                self.ui.rdoutMouseWorld.setText("(%.0f,%.0f,0)" %
                                                (rw[0], rw[1]))
            elif (self.video.extrinsic_cal_flag == 2):  #Josh
                xy_in_rgb = np.float32([[[x, y]]])  #Josh
                xy_in_rgb_homogenous = np.array([x, y, 1.0])
                self.video.rgb2dep_aff_matrix = np.array(
                    [[1.09403672e0, 3.44369111e-3, -1.62867595e0],
                     [-2.41966785e-3, 1.07534829e0, -2.69041712e1],
                     [0., 0., 1.]])  #Josh
                xy_in_dep = np.dot(self.video.rgb2dep_aff_matrix,
                                   xy_in_rgb_homogenous)  #Josh
                x_dep = int(xy_in_dep[0])  #Josh
                y_dep = int(xy_in_dep[1])  #Josh
                d = self.video.currentDepthFrame[y_dep][x_dep]
                Z = -1
                #Z = 0.1236*np.tan(d/2842.5 + 1.1863)*1000
                for idx in range(len(self.video.levels_mm)):
                    if d <= self.video.levels_upper_d[
                            idx] and d >= self.video.levels_lower_d[idx]:
                        Z = 941 - self.video.levels_mm[idx]
                        break
                camera_coord = Z * cv2.undistortPoints(
                    xy_in_rgb, self.video.intrinsic,
                    self.video.distortion_array)
                camera_coord_homogenous = np.transpose(
                    np.append(camera_coord, [Z, 1.0]))
                world_coord_homogenous = np.dot(self.video.extrinsic,
                                                camera_coord_homogenous)

                #camera_coord = Z*np.dot(self.video.inv_intrinsic,xy_in_rgb)
                #camera_coord_homo = np.concatenate((camera_coord,[1.]),axis = 0)
                #world_coord_homo = np.dot(self.video.extrinsic,camera_coord_homo)
                self.ui.rdoutMouseWorld.setText(
                    "(%.0f,%.0f,%.0f)" %
                    (world_coord_homogenous[0], world_coord_homogenous[1],
                     world_coord_homogenous[2]))
#self.ui.rdoutMouseWorld.setText("(%.0f,%.0f,%.0f)" % (x_dep,y_dep,d))
            else:
                self.ui.rdoutMouseWorld.setText("(-,-,-)")
        """ 
        Updates status label when rexarm playback is been executed.
        This can 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))
        self.ui.rdoutStatus.setText(
            self.statemachine.getmestatus(combined=True))

    def sliderChange(self):
        """ 
        Function to change the slider labels when sliders are moved
        and to command the arm to the given position
        """
        self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
        self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
        self.rex.torque_multiplier = self.ui.sldrMaxTorque.value() / 100.0
        self.rex.speed_multiplier = [self.ui.sldrSpeed.value() / 100.0
                                     ] * self.rex.num_joints
        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
        if (self.rex.num_joints == 6):
            self.rex.joint_angles[4] = self.ui.sldrWrist.value() * D2R
            self.rex.joint_angles[5] = self.ui.sldrWrist.value() * D2R

        self.rex.cmd_publish()

    def sliderinitChange(self):  #Giri
        """ 
        Function to change the slider labels when sliders are moved
        and to command the arm to the given position
        """
        self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
        self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))  #Giri
        self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))  #Giri
        self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))  #Giri
        self.ui.rdoutGrip1.setText(str(self.ui.sldrGrip1.value()))  #Giri
        self.ui.rdoutGrip2.setText(str(self.ui.sldrGrip2.value()))  #Giri

        self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
        self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")

        self.rex.torque_multiplier = self.ui.sldrMaxTorque.value() / 100.0
        self.rex.speed_multiplier = [self.ui.sldrSpeed.value() / 100.0
                                     ] * self.rex.num_joints
        #Giri{
        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
        if (len(self.rex.joint_angles) == 6):
            self.rex.joint_angles[4] = self.ui.sldrGrip1.value() * D2R
            self.rex.joint_angles[5] = self.ui.sldrGrip2.value() * D2R
        #Giri}
        self.rex.cmd_publish()

    #From Giri: I implemented all these function in a one function above, incase if the response time is bad, we can go back to the below five separate functions

    def sliderOtherChange(self):  #Giri
        self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
        self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
        self.rex.torque_multiplier = self.ui.sldrMaxTorque.value() / 100.0
        self.rex.speed_multiplier = [self.ui.sldrSpeed.value() / 100.0
                                     ] * self.rex.num_joints
        self.rex.cmd_publish()

    def sliderBaseChange(self):  #Kevin
        self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
        self.rex.joint_angles[0] = self.ui.sldrBase.value() * D2R
        self.rex.cmd_publish()

    def sliderShoulderChange(self):  #Kevin
        self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))
        self.rex.joint_angles[1] = self.ui.sldrShoulder.value() * D2R
        self.rex.cmd_publish()

    def sliderElbowChange(self):  #Kevin
        self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
        self.rex.joint_angles[2] = self.ui.sldrElbow.value() * D2R
        self.rex.cmd_publish()

    def sliderWristChange(self):  #Kevin
        self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))
        self.rex.joint_angles[3] = self.ui.sldrWrist.value() * D2R
        self.rex.cmd_publish()

    def sliderGrip1Change(self):  #Giri
        self.ui.rdoutGrip1.setText(str(self.ui.sldrGrip1.value()))
        self.rex.joint_angles[4] = self.ui.sldrGrip1.value() * D2R
        self.rex.cmd_publish()

    def sliderGrip2Change(self):  #Giri
        self.ui.rdoutGrip2.setText(str(self.ui.sldrGrip2.value()))
        self.rex.joint_angles[5] = self.ui.sldrGrip2.value() * D2R
        self.rex.cmd_publish()

    def mousePressEvent(self, QMouseEvent):
        """ 
        Function used to record mouse click positions for calibration 
        """
        """ Get mouse posiiton """
        x = QMouseEvent.x()
        y = QMouseEvent.y()
        """ If mouse position is not over the camera image ignore """
        if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)): return
        """ Change coordinates to image axis """
        self.last_click[0] = x - MIN_X
        self.last_click[1] = y - MIN_Y
        """ If calibration has been performed """
        if (self.video.cal_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 is recorded, please select Click Point %d"
                % (self.video.mouse_click_id, 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 workspace calibration routine
            and NOT the simple calibration function as is done now.
            """
            if (self.video.mouse_click_id == self.video.cal_points):
                """ 
                Update status of calibration flag and number of mouse
                clicks
                """
                self.video.cal_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)
                #print self.video.mouse_coord #Josh
                #print self.video.real_coord #Josh
                """ Updates Status Label to inform calibration is done """
                self.ui.rdoutStatus.setText("Waiting for input")
        if (self.video.dep2rgb_aff_flag == 1):  #Josh
            if (self.video.mouse_click_id < self.video.rgb_pts_num):
                self.video.rgb_coord[self.video.mouse_click_id * 2] = x - MIN_X
                self.video.rgb_coord[self.video.mouse_click_id * 2 +
                                     1] = y - MIN_Y
            else:
                self.video.dep_coord[2 * (self.video.mouse_click_id -
                                          self.video.rgb_pts_num)] = [
                                              (x - MIN_X), (y - MIN_Y), 1, 0,
                                              0, 0
                                          ]
                self.video.dep_coord[
                    2 * (self.video.mouse_click_id - self.video.rgb_pts_num) +
                    1] = [0, 0, 0, (x - MIN_X), (y - MIN_Y), 1]
            self.video.mouse_click_id += 1
            print self.video.mouse_click_id
            if (self.video.mouse_click_id == self.video.dep_pts_num):
                self.video.dep2rgb_aff_flag = 2
                self.video.mouse_click_id = 0
                A = self.video.dep_coord
                A_trans = np.transpose(A)
                A_AT = np.dot(A_trans, A)
                A_AT_inv = np.linalg.inv(A_AT)
                A_all = np.dot(A_AT_inv, A_trans)
                b = np.transpose(self.video.rgb_coord)
                tmp_dep2rgb_aff_array = np.dot(A_all, b)
                upper_dep2rgb_aff_matrix = np.reshape(tmp_dep2rgb_aff_array,
                                                      (2, 3))
                lower_dep2rgb_aff_matrix = np.array([[0, 0, 1]])
                self.video.dep2rgb_aff_matrix = np.concatenate(
                    (upper_dep2rgb_aff_matrix, lower_dep2rgb_aff_matrix),
                    axis=0)
                self.video.rgb2dep_aff_matrix = np.linalg.inv(
                    self.video.dep2rgb_aff_matrix)
                print self.video.rgb2dep_aff_matrix
        if (self.video.extrinsic_cal_flag == 1):  #Josh
            self.video.extrinsic_cal_pixel_coord[self.video.mouse_click_id] = [
                (x - MIN_X), (y - MIN_Y)
            ]
            self.video.mouse_click_id += 1
            if (self.video.mouse_click_id == 3):
                """
		retval, rvec, tvec = cv2.solvePnP(self.video.extrinsic_cal_world_coord, self.video.extrinsic_cal_pixel_coord, self.video.intrinsic, self.video.distortion_array)
		#print self.video.extrinsic_cal_pixel_coord, self.video.extrinsic_cal_world_coord		
		self.video.mouse_click_id = 0
		R,tmp = cv2.Rodrigues(rvec)
		tmp = np.concatenate((np.transpose(R),-1.0*tvec),axis=1)
		self.video.extrinsic = np.concatenate((tmp, np.float32([[0., 0., 0., 1.]])),axis = 0)
		print self.video.extrinsic
		self.video.inv_extrinsic = np.linalg.inv(self.video.extrinsic)
		self.video.extrinsic_cal_flag = 2
		"""
                self.video.mouse_click_id = 0
                tmp_coord = np.float32(
                    [[self.video.extrinsic_cal_pixel_coord[0]],
                     [self.video.extrinsic_cal_pixel_coord[1]],
                     [self.video.extrinsic_cal_pixel_coord[2]]])
                camera_coord = 941.0 * cv2.undistortPoints(
                    tmp_coord, self.video.intrinsic,
                    self.video.distortion_array)
                rot_rad = -1.0 * np.arctan2(
                    (camera_coord[1][0][1] - camera_coord[0][0][1]),
                    (camera_coord[1][0][0] - camera_coord[0][0][0]))
                if rot_rad < 0:
                    z_rot = rot_rad + 0.5 * np.pi
                else:
                    z_rot = 0.5 * np.pi - rot_rad
                R_T = np.array([[np.cos(z_rot),
                                 np.sin(z_rot), 0.0],
                                [np.sin(z_rot), -1.0 * np.cos(z_rot), 0.0],
                                [0.0, 0.0, -1.0]])
                R = np.transpose(R_T)
                T = np.float32([
                    (camera_coord[0][0][0] + camera_coord[2][0][0]) / 2,
                    (camera_coord[0][0][1] + camera_coord[2][0][1]) / 2, 941
                ])
                T = np.transpose(np.array([-1.0 * np.dot(R, T)]))
                tmp = np.concatenate((R, T), axis=1)
                self.video.extrinsic = np.concatenate(
                    (tmp, np.float32([[0., 0., 0., 1.]])), axis=0)
                #self.video.writecaldata(self.video.extrinsic)
                self.video.inv_extrinsic = np.linalg.inv(self.video.extrinsic)
                print "Extrinsic Matrix = ", self.video.extrinsic
                self.video.extrinsic_cal_flag = 2

    def deprgb_cali(self):  #Josh
        self.video.dep2rgb_aff_flag = 1
        self.ui.rdoutStatus.setText(
            "Start camera calibration by clicking mouse to select Click Point 1"
        )

    def extrinsic_cali(self):  #Josh

        alpha = 525.91386088
        u0 = 312.41480582
        beta = 526.52064559
        v0 = 247.78962122
        matrix1 = np.float32([[1 / alpha, 0., 0.], [0., 1 / beta, 0.],
                              [0., 0., 1.0]])
        matrix2 = np.float32([[1.0, 0., -u0], [0., 1., -v0], [0., 0., 1.0]])
        self.video.inv_intrinsic = np.dot(matrix1, matrix2)
        self.video.intrinsic = np.float32([[alpha, 0., u0], [0., beta, v0],
                                           [0., 0., 1.]])
        self.video.distortion_array = np.float32([
            2.45479098e-01, -8.27672136e-01, 1.05870966e-03, -3.01947277e-03,
            1.08683931e+00
        ])
        self.video.extrinsic_cal_flag = 1

    def teach(self):  #Giri
        [x, y, z, orientation] = [0, 14, 1, 180.0]
        q = [0.0, 0.0, 0.0, 0.0]
        [q[0], q[1], q[2], q[3]] = inverseKinematics(x, y, z, orientation)
        print q
        self.rex.joint_angles[0] = q[0] * D2R
        self.rex.joint_angles[1] = q[1] * D2R
        self.rex.joint_angles[2] = q[2] * D2R
        self.rex.joint_angles[3] = q[3] * D2R
        self.rex.cmd_publish()

    def repeat(self):  #Giri
        if (self.ui.btnUser7.text() == "Repeat"):
            self.statemachine.init(self.ui, self.rex, "Initialising")
        else:
            self.statemachine.repeat(self.ui, self.rex, False)  #Giri

    def srepeat(self):  #Giri
        self.statemachine.init(self.ui, self.rex, "initialising")

    def reset(self, hold=False):
        self.rex.torque_multiplier = self.ui.sldrMaxTorque.value() / 100.0
        self.rex.speed_multiplier = [self.ui.sldrSpeed.value() / 100.0
                                     ] * self.rex.num_joints

        self.rex.joint_angles[0] = 0.0
        self.rex.joint_angles[1] = 0.0
        self.rex.joint_angles[2] = 0.0
        self.rex.joint_angles[3] = 0.0
        if (len(self.rex.joint_angles) == 6 and hold == False):
            self.rex.joint_angles[4] = 0.0
            self.rex.joint_angles[5] = 95 * D2R
        elif (len(self.rex.joint_angles) == 6 and hold == True):
            self.rex.joint_angles[4] = 0.0
            self.rex.joint_angles[5] = -95 * D2R

        self.rex.cmd_publish()

    def getheight(self, angle, action="picking"):
        placement_offset = 1  # in cm
        if (angle - 90 < 0.1):
            gripping_height = 3
        elif (angle - 120 < 0.1):
            gripping_height = 2

        else:
            if (action == "picking"):
                gripping_height = 0.05
            elif (action == "placing"):
                gripping_height = 0.05 + placement_offset
        return gripping_height

    def getIK(self, endCoord, angle=0, action="picking"):
        intermediate_height = 4
        intermediate_angles = [0.0] * 6
        angles = [0.0] * 6
        [angles[0], angles[1], angles[2], angles[3]] = inverseKinematics(
            endCoord[0], endCoord[1],
            endCoord[2] + self.getheight(endCoord[3], action), endCoord[3])

        if (self.checkfound(angles)
            ):  # or self.checkfound(intermediate_angles,True)):
            if (abs(endCoord[3] - 90) < 0.1):
                endCoord[3] = 180
                [angles[0], angles[1], angles[2],
                 angles[3]] = inverseKinematics(
                     endCoord[0], endCoord[1],
                     endCoord[2] + self.getheight(endCoord[3], action),
                     endCoord[3])

            else:
                endCoord[3] = 90
                [angles[0], angles[1], angles[2],
                 angles[3]] = inverseKinematics(
                     endCoord[0], endCoord[1],
                     endCoord[2] + self.getheight(endCoord[3], action),
                     endCoord[3])
        if (self.checkfound(angles)
            ):  # or self.checkfound(intermediate_angles,True)):
            endCoord[3] = 120

            [angles[0], angles[1], angles[2], angles[3]] = inverseKinematics(
                endCoord[0] - 1, endCoord[1] + 1,
                endCoord[2] + self.getheight(endCoord[3], action), endCoord[3])

        if (endCoord[3] == 120):
            [
                intermediate_angles[0], intermediate_angles[1],
                intermediate_angles[2], intermediate_angles[3]
            ] = inverseKinematics(endCoord[0] - 1, endCoord[1] + 1,
                                  endCoord[2] + intermediate_height,
                                  endCoord[3])  # assuming it is reachable
            if (self.checkfound(intermediate_angles)):
                [
                    intermediate_angles[0], intermediate_angles[1],
                    intermediate_angles[2], intermediate_angles[3]
                ] = inverseKinematics(endCoord[0] - 1, endCoord[1] + 1,
                                      endCoord[2] + intermediate_height,
                                      90)  # assuming it is reachable

        if (endCoord[3] == 180):
            [
                intermediate_angles[0], intermediate_angles[1],
                intermediate_angles[2], intermediate_angles[3]
            ] = inverseKinematics(endCoord[0], endCoord[1],
                                  endCoord[2] + intermediate_height +
                                  self.getheight(endCoord[3], action),
                                  endCoord[3])  # assuming it is reachable

            print self.trim_angle(orientation(angles[0], angle * R2D))
            if (action == "placing"):
                angles[4] = 90 + angles[0]
            else:
                angles[4] = -1 * self.trim_angle(
                    orientation(angles[0], angle * R2D))

        else:
            angles[4] = 0
        self.checkfound(intermediate_angles, True)

        intermediate_angles[4] = angles[4]
        angles[5] = endCoord[3]
        intermediate_angles[5] = endCoord[3]
        if (self.checkfound(angles)
                and self.checkfound(intermediate_angles, False)):
            print "Cannot be picked"
        return [angles, intermediate_angles]

    def checkfound(self, angles, i=False):
        if (i):
            if (angles[0] == 0 and angles[1] == 0 and angles[2] == 0
                    and angles[3] == 0):
                print "intermediate angles not found"
                return True
        else:
            if (angles[0] == 0 and angles[1] == 0 and angles[2] == 0
                    and angles[3] == 0):
                print "final angles not found"
                return True
        return False

    def printfk(self, angles):
        temp = forwardKinematics(angles[0], angles[1], angles[2], angles[3])
        print "Forward Kinematics output =  ", str(temp[0]), str(temp[1]), str(
            temp[2]), str(temp[3])

    def roundoff(self, angles):
        angles[0] = round(self.trim_angle(angles[0]), 2)
        angles[1] = round(self.trim_angle(angles[1]), 2)
        angles[2] = round(self.trim_angle(angles[2]), 2)
        angles[3] = round(self.trim_angle(angles[3]), 2)
        angles[4] = round(self.trim_angle(angles[4]),
                          2)  #Dont alter these two angles
        angles[5] = round(self.trim_angle(angles[5]), 2)
        return angles

    def pick(self):  #Josh
        global putx, puty, putz, putangle
        blockx, blocky, blockz, angle = self.get_color_block_world_coord(
            'blue')
        #self.statemachine.setorientation(angle*R2D)
        [putx, puty, putz, putangle] = [-1 * blockx, blocky, blockz, angle]
        #endCoord = [20.2, -16.1, 4.4,90]
        #endCoord = [14.3, 19.4, 4.4,90]
        endCoord = [
            blockx / 10, blocky / 10, (blockz) / 10, gripper_orientation
        ]
        [angles, intermediate_angles] = self.getIK(endCoord, angle)
        if (debug):
            print "Block detector output = ", [
                blockx, blocky, blockz, angle * R2D
            ]
            print "Input to Inverse Kinematics:", endCoord
            print "Output of Inverse Kinematics: ", angles
            self.printfk(angles)

        angles = self.roundoff(angles)
        intermediate_angles = self.roundoff(intermediate_angles)
        self.statemachine.setq(angles, intermediate_angles)
        self.statemachine.setmystatus(
            "testing", "picking", "picking")  #mode="testing",action="picking")
        #self.statemachine.hold(self.ui,self.rex, angles,"picking")
    def place(self):

        endCoord = [putx / 10, puty / 10, (putz) / 10, gripper_orientation]
        [angles, intermediate_angles] = self.getIK(endCoord, putangle)

        #wangle = (self.shortorientation((putangle)*R2D-45)-self.trim_angle(angles[0]))#self.shortorientation((angle)*R2D+45)
        #self.statemachine.set_orientations(wangle,2)
        if (debug):
            print "Placing according to Block detector output = ", [
                putx, puty, putz, putangle * R2D
            ]
            print "Input to Inverse Kinematics:", endCoord
            self.printfk(angles)

        angles = self.roundoff(angles)
        intermediate_angles = self.roundoff(intermediate_angles)

        self.statemachine.setq(angles, intermediate_angles)
        self.statemachine.setmystatus(
            "testing", "placing", "placing")  #mode="testing",action="placing")


#	self.statemachine.hold(self.ui,self.rex, angles,"placing")

    def get_color_block_world_coord(self, color):  #Josh
        self.video.blockDetector()
        [blockx_rgb, blocky_rgb, angle] = self.video.color_detection(color)
        if (blockx_rgb == 0 and blocky_rgb == 0 and angle == 0):
            return 0, 0, 0, 0
        xy_in_rgb = np.float32([[[blocky_rgb, blockx_rgb]]])  #Josh
        xy_in_rgb_homogenous = np.array([blocky_rgb, blockx_rgb, 1.0])
        xy_in_dep = np.dot(self.video.rgb2dep_aff_matrix,
                           xy_in_rgb_homogenous)  #Josh
        x_dep = int(xy_in_dep[0])  #Josh
        y_dep = int(xy_in_dep[1])  #Josh
        d = self.video.currentDepthFrame[y_dep][x_dep]
        Z = -1
        for idx in range(len(self.video.levels_mm)):
            if d <= self.video.levels_upper_d[
                    idx] and d >= self.video.levels_lower_d[idx]:
                Z = 941 - self.video.levels_mm[idx]
                break
        camera_coord = Z * cv2.undistortPoints(xy_in_rgb, self.video.intrinsic,
                                               self.video.distortion_array)
        camera_coord_homogenous = np.transpose(
            np.append(camera_coord, [Z, 1.0]))
        world_coord_homogenous = np.dot(self.video.extrinsic,
                                        camera_coord_homogenous)
        theta = np.arccos(self.video.extrinsic[0, 0])
        angle = angle - theta
        return world_coord_homogenous[0], world_coord_homogenous[
            1], world_coord_homogenous[2], angle

    def getangles(self, color):
        blockx, blocky, blockz, angle = self.get_color_block_world_coord(color)
        if (blockx == 0 and blocky == 0 and blockx == 0 and angle == 0):
            angles = [0] * 6
            intermediate_angles = [0] * 6
            print "not found"
            self.statemachine.set_comp5_status("not found")

        else:
            [
                current_mode, current_action, current_movement,
                current_motorstate
            ] = self.statemachine.getmestatus()
            endCoord = [(blockx) / 10, (blocky) / 10, (blockz) / 10,
                        gripper_orientation]

            [angles, intermediate_angles] = self.getIK(endCoord, angle,
                                                       "picking")

            angles = self.roundoff(angles)
            intermediate_angles = self.roundoff(intermediate_angles)
            print "found"
            self.statemachine.set_comp5_status("found")
        self.statemachine.setq(angles, intermediate_angles)

    def generatecomp2(self):
        print "Entered generatecomp2"
        new_q = np.zeros([3, 6])
        new_qh = np.zeros([3, 6])  #intermediate heights
        #	blockx, blocky, blockz, angle = self.get_color_block_world_coord('red')
        final_x = 0
        final_y = -220
        blockz = 19.25
        #	final_x = blockx
        #	final_y = blocky
        height = 40
        endCoord = [
            -(final_x) / 10, (final_y) / 10, (blockz + height * 0) / 10,
            gripper_orientation
        ]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")

        new_q[0] = self.roundoff(angles)
        new_qh[0] = self.roundoff(intermediate_angles)

        endCoord = [
            -(final_x) / 10, (final_y) / 10, (blockz + height * 1) / 10,
            gripper_orientation
        ]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
        new_q[1] = self.roundoff(angles)
        new_qh[1] = self.roundoff(intermediate_angles)

        endCoord = [
            -(final_x) / 10, (final_y) / 10, (blockz + height * 2) / 10,
            gripper_orientation
        ]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
        new_q[2] = self.roundoff(angles)
        new_qh[2] = self.roundoff(intermediate_angles)

        self.statemachine.setq_comp(new_q, new_qh)

    def generatecomp3(self):
        print "Entered generatecomp3"
        new_q = np.zeros([8, 6])
        new_qh = np.zeros([8, 6])  #intermediate heights
        #blockx, blocky, blockz, angle = self.get_color_block_world_coord('red')
        blockz = 19.5
        final_x = -150
        final_y = -89.5
        b = 43
        endCoord = [(final_x) / 10, (final_y) / 10, (blockz) / 10,
                    gripper_orientation]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")

        new_q[0] = self.roundoff(angles)
        new_qh[0] = self.roundoff(intermediate_angles)

        endCoord = [(final_x + b * 1) / 10, (final_y) / 10, (blockz) / 10,
                    gripper_orientation]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
        new_q[1] = self.roundoff(angles)
        new_qh[1] = self.roundoff(intermediate_angles)
        print "Genereated FK: ", forwardKinematics(new_q[1][0], new_q[1][1],
                                                   new_q[1][2], new_q[1][3])

        endCoord = [(final_x + b * 2) / 10, (final_y) / 10, (blockz) / 10,
                    gripper_orientation]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
        new_q[2] = self.roundoff(angles)
        new_qh[2] = self.roundoff(intermediate_angles)

        endCoord = [(final_x + b * 3) / 10, (final_y) / 10, (blockz) / 10,
                    gripper_orientation]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")

        new_q[3] = self.roundoff(angles)
        new_qh[3] = self.roundoff(intermediate_angles)

        endCoord = [(final_x + b * 4) / 10, (final_y) / 10, (blockz) / 10,
                    gripper_orientation]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
        new_q[4] = self.roundoff(angles)
        new_qh[4] = self.roundoff(intermediate_angles)

        endCoord = [(final_x + b * 5) / 10, (final_y) / 10, (blockz) / 10,
                    gripper_orientation]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
        new_q[5] = self.roundoff(angles)
        new_qh[5] = self.roundoff(intermediate_angles)

        endCoord = [(final_x + b * 6) / 10, (final_y) / 10, (blockz) / 10,
                    gripper_orientation]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")

        new_q[6] = self.roundoff(angles)
        new_qh[6] = self.roundoff(intermediate_angles)

        endCoord = [(final_x + b * 7) / 10, (final_y) / 10, (blockz) / 10,
                    gripper_orientation]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
        new_q[7] = self.roundoff(angles)
        new_qh[7] = self.roundoff(intermediate_angles)

        self.statemachine.setq_comp(new_q, new_qh)

    def generatecomp4(self):
        print "Entered generatecomp4"
        new_q = np.zeros([8, 6])
        new_qh = np.zeros([8, 6])  #intermediate heights
        blockx, blocky, blockz, angle = self.get_color_block_world_coord(
            'blue')
        final_x = 0
        final_y = -220
        height = 40
        endCoord = [
            -(final_x) / 10, (final_y) / 10, (blockz + height * 0) / 10,
            gripper_orientation
        ]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")

        new_q[0] = self.roundoff(angles)
        new_qh[0] = self.roundoff(intermediate_angles)

        endCoord = [
            -(final_x) / 10, (final_y) / 10, (blockz + height * 1) / 10,
            gripper_orientation
        ]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
        new_q[1] = self.roundoff(angles)
        new_qh[1] = self.roundoff(intermediate_angles)
        print "Genereated FK: ", forwardKinematics(new_q[1][0], new_q[1][1],
                                                   new_q[1][2], new_q[1][3])

        endCoord = [
            -(final_x) / 10, (final_y) / 10, (blockz + height * 2) / 10,
            gripper_orientation
        ]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
        new_q[2] = self.roundoff(angles)
        new_qh[2] = self.roundoff(intermediate_angles)

        endCoord = [
            -(final_x) / 10, (final_y) / 10, (blockz + height * 3) / 10,
            gripper_orientation
        ]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")

        new_q[3] = self.roundoff(angles)
        new_qh[3] = self.roundoff(intermediate_angles)

        endCoord = [
            -(final_x) / 10, (final_y) / 10, (blockz + height * 4) / 10,
            gripper_orientation
        ]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
        new_q[4] = self.roundoff(angles)
        new_qh[4] = self.roundoff(intermediate_angles)

        endCoord = [
            -(final_x) / 10, (final_y) / 10, (blockz + height * 5) / 10,
            gripper_orientation
        ]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
        new_q[5] = self.roundoff(angles)
        new_qh[5] = self.roundoff(intermediate_angles)

        endCoord = [
            -(final_x) / 10, (final_y) / 10, (blockz + height * 6) / 10,
            gripper_orientation
        ]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
        # hard code joints angles down below here
        #new_q[6] = self.roundoff(angles)
        #new_qh[6] = self.roundoff(intermediate_angles)
        new_q[6] = [1., 2., 66.5, 15.2, 0, 90.]
        new_qh[6] = [1., 15.8, 29., 34.5, 0, 90.]

        endCoord = [
            -(final_x) / 10, (final_y) / 10, (blockz + height * 7) / 10,
            gripper_orientation
        ]
        [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
        #new_q[7] = self.roundoff(angles)
        #new_qh[7] = self.roundoff(intermediate_angles)
        new_q[7] = [1., 15.8, 29., 34.5, 0, 90.]
        new_qh[7] = [1., 23., 9., 34.5, 0, 90.]
        self.statemachine.setq_comp(new_q, new_qh)

    def generate_comp5(self):
        print "Entered generate_comp5"
        n = 3
        b = 6
        N = (n + 1) * n / 2
        coords = self.pyramid(n, b)
        new_q = np.zeros([N, 6])
        new_qh = np.zeros([N, 6])  #intermediate heights
        endCoord = np.zeros([N, 4])
        for i in range(0, N):
            endCoord[i] = [
                coords[0, i], coords[1, i], coords[2, i], gripper_orientation
            ]
            [new_q[i], new_qh[i]] = self.getIK(endCoord[i], 0, "placing")
        print "pyramid: ", coords
        self.statemachine.setq_comp(new_q, new_qh)

    def pyramid(self, nlayers, spacing):  #returns numpy array.
        coords = zeros((3, nlayers * int((nlayers + 1) / 2)))
        count = 0
        blockheight = 3.85
        for j in range(0, nlayers):
            for i in range(0, nlayers - j):
                print(nlayers - j - 1)
                coords[0][count] = 0 + spacing * (i - ((nlayers - j) / 2.0))
                coords[1][count] = -11
                coords[2][count] = (blockheight * j
                                    )  #define blockheight and offset
                count = count + 1
        return coords

    def competition(self):
        if (self.ui.btnUser11.text() == "Enter Competition Mode"):
            self.ui.btnUser11.setText("Enter Testing Mode")
            self.ui.btnUser7.setText("Draw")
            self.ui.btnUser1.setText("Competition 1")
            self.ui.btnUser2.setText("Competition 2")
            self.ui.btnUser3.setText("Competition 3")
            self.ui.btnUser4.setText("Competition 4")
            self.ui.btnUser5.setText("Competition 5")
            self.ui.btnUser2.setEnabled(True)
            self.ui.btnUser7.setEnabled(True)

        elif (self.ui.btnUser11.text() == "Enter Testing Mode"):
            self.ui.btnUser11.setText("Enter Competition Mode")
            self.ui.btnUser7.setText("Smooth Repeat")
            self.ui.btnUser1.setText("Configure Servos")
            self.ui.btnUser2.setText("Depth and RGB Calibration")
            self.ui.btnUser3.setText("Extrinsic Calibration")
            self.ui.btnUser4.setText("Block Detection")
            self.ui.btnUser5.setText("Repeat")
            self.ui.btnUser2.setEnabled(False)
            self.ui.btnUser7.setEnabled(False)

    def btn1(self):
        if (self.ui.btnUser1.text() == "Configure Servos"):
            self.rex.cfg_publish_default()
        elif (self.ui.btnUser1.text() == "Competition 1"):
            if (self.sanity_check(1)):
                self.getangles('red')
                self.statemachine.setmystatus(
                    "Competition 1", "picking",
                    "picking")  #mode="testing",action="picking")

    def btn2(self):
        if (self.ui.btnUser2.text() == "Depth and RGB Calibration"):
            self.deprgb_cali()
        elif (self.ui.btnUser2.text() == "Competition 2"):
            if (self.sanity_check(2)):
                self.generatecomp2()
                self.getangles('red')
                self.statemachine.setmystatus(
                    "Competition 2", "picking",
                    "picking")  #mode="testing",action="picking")

    def btn3(self):
        global declutter_index, declutter_competition
        if (self.ui.btnUser3.text() == "Extrinsic Calibration"):
            self.extrinsic_cali()
        elif (self.ui.btnUser3.text() == "Competition 3"):
            declutter_index = 0
            declutter_competition = "Competition 3"
            self.declutter()
            """
		if(self.sanity_check(3)):
			self.generatecomp3()
			self.getangles('black')		
			self.statemachine.setmystatus("Competition 3", "picking","picking")#mode="testing",action="picking")	
		"""

    def btn4(self):
        global declutter_index, declutter_competition
        if (self.ui.btnUser4.text() == "Block Detector"):
            self.video.blockDetector()
        elif (self.ui.btnUser4.text() == "Competition 4"):
            declutter_index = 0
            declutter_competition = "Competition 4"
            self.declutter()
            """
		if(self.sanity_check(4)):
			self.generatecomp4()
			self.getangles('black')		
			self.statemachine.setmystatus("Competition 4", "picking","picking")#mode="testing",action="picking")	
		"""

    def btn5(self):
        if (self.ui.btnUser5.text() == "Repeat"):
            self.repeat()
        elif (self.ui.btnUser5.text() == "Competition 5"):
            self.generate_comp5()
            self.getangles('all')
            self.statemachine.setmystatus(
                "Competition 5", "picking",
                "picking")  #mode="testing",action="picking")

    def declutter(self):
        blockx, blocky, blockz, angle = self.video.search_highest()
        levels_mm = np.float32(
            [0., 19.25, 57.75, 96.25, 134.75, 173.25, 211.75, 250.25])
        level = (blockz - 19.25) / 38.5
        print "level =  ", level, "and levels_mm = ", levels_mm

        if (declutter_index == 2):
            if (declutter_competition == "Competition 3"):
                if (self.sanity_check(3)):
                    self.generatecomp3()
                    self.getangles('black')
                    self.statemachine.setmystatus(
                        "Competition 3", "picking",
                        "picking")  #mode="testing",action="picking")
            elif (declutter_competition == "Competition 4"):
                if (self.sanity_check(4)):
                    self.generatecomp4()
                    self.getangles('black')
                    self.statemachine.setmystatus(
                        "Competition 4", "picking",
                        "picking")  #mode="testing",action="picking")

        if (level >= 1 and declutter_index < 4):
            endCoord = [(blockx) / 10, (blocky) / 10, (blockz) / 10,
                        gripper_orientation]
            print endCoord
            [angles, intermediate_angles] = self.getIK(endCoord, 0, "picking")
            print "from declutter IK output: ", [angles, intermediate_angles]
            angles = self.roundoff(angles)
            intermediate_angles = self.roundoff(intermediate_angles)
            if (not self.checkfound(angles)
                ):  # and not self.checkfound(intermediate_angles,True)):
                self.generatedeclutter()
                self.statemachine.setq(angles, intermediate_angles)
            self.statemachine.setmystatus(
                "Declutter", "picking",
                "picking")  #mode="testing",action="picking")
        else:
            if (declutter_competition == "Competition 3"):
                if (self.sanity_check(3)):
                    self.generatecomp3()
                    self.getangles('black')
                    self.statemachine.setmystatus(
                        "Competition 3", "picking",
                        "picking")  #mode="testing",action="picking")
            elif (declutter_competition == "Competition 4"):
                if (self.sanity_check(4)):
                    self.generatecomp4()
                    self.getangles('black')
                    self.statemachine.setmystatus(
                        "Competition 4", "picking",
                        "picking")  #mode="testing",action="picking")

    def generatedeclutter(self):
        print "Entered generatedeclutter"
        global declutter_index

        if (declutter_index < 4):
            x = [0, 250, 0, 180]
            y = [-240, -50, 240, -150]
            final_x = x[declutter_index]
            final_y = y[declutter_index]
            final_z = 19.25
            height = 20
            endCoord = [(final_x) / 10, (final_y) / 10,
                        (final_z + height) / 10, gripper_orientation]
            [angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")

            new_q = self.roundoff(angles)
            new_qh = self.roundoff(intermediate_angles)
            declutter_index = declutter_index + 1
            self.statemachine.setq_comp(new_q, new_qh)

    def sanity_check(self, comp):
        success = False
        if (comp == 1 or comp == 2):  #search for red,blue,green
            colors = ["red", "blue", "green"]
            [pos_colors, success] = self.video.blockDetector(colors)
        elif (comp == 4 or comp == 3):  #search for red,blue,green
            colors = [
                "red", "blue", "green", "orange", "violet", "black", "yellow",
                "pink"
            ]
            [pos_colors, success] = self.video.blockDetector(colors)

        return success

    """main function"""
예제 #2
0
class Gui(QtGui.QMainWindow):
    """ 
    Main GUI Class
    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()
        self.statemachine = Statemachine()

        self.pos_idx_teach = 0
        """ Other Variables """
        self.last_click = np.float32([0, 0])
        self.check_move_time = 60
        self.check_time = 70
        self.check_move_counter = 0
        self.d_t = 10. * self.check_move_time / 1000.
        self.spe_coef = [0.0] * self.rex.num_joints
        self.pos_pre = [0.0] * self.rex.num_joints
        self.plan_point = 0
        self.move_to_point_cn = 0
        self.event3_color_check = [0] * 8
        self.event3_finish = 0
        self.event4_color_idx = 0
        self.vel = []
        self.event4_f_idx = 0
        self.event5_block_num = 0
        """ Set GUI to track mouse """
        QtGui.QWidget.setMouseTracking(self, True)
        """ 
        GUI timer 

        Creates a timer and calls update_gui() function 
        according to the given time delay (27ms) 
        """
        self._timer = QtCore.QTimer(self)
        self._timer.timeout.connect(self.update_gui)
        self._timer.start(27)
        """ 
        LCM Arm Feedback timer
        Creates a timer to call LCM handler for Rexarm feedback
        """
        self._timer2 = QtCore.QTimer(self)
        self._timer2.timeout.connect(self.rex.get_feedback)
        self._timer2.start()
        # self._timer_setplan = QtCore.QTimer(self)
        # self._timer_setplan.timeout.connect(self.setplan)
        # self._timer_teach = QtCore.QTimer(self)
        # self._timer_teach.timeout.connect(self.check_move)
        self._timer_waypoint = QtCore.QTimer(self)
        self._timer_waypoint.timeout.connect(self.move_to_point)
        self._timer_event1 = QtCore.QTimer(self)
        self._timer_event2 = QtCore.QTimer(self)
        self._timer_event3 = QtCore.QTimer(self)
        self._timer_event4 = QtCore.QTimer(self)
        self._timer_event5 = QtCore.QTimer(self)
        """ 
        Connect Sliders to Function
        TODO: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI 
        """
        self.ui.sldrBase.valueChanged.connect(self.sliderChange)
        self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
        self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
        self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
        self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
        self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
        self.ui.sldrGrip2.valueChanged.connect(self.sliderChange)
        self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
        """ Initial command of the arm to home position"""
        self.sliderChange()
        """ Connect Buttons to Functions 
        TODO: NAME AND CONNECT BUTTONS AS NEEDED
        """
        self.ui.btnUser1.setText("Configure Servos")
        self.ui.btnUser1.clicked.connect(self.rex.cfg_publish_default)
        self.ui.btnUser2.setText("Simple Calibration")
        self.ui.btnUser2.clicked.connect(self.simple_cal)
        # self.ui.btnUser3.setText("start teach and repeat")
        # self.ui.btnUser3.clicked.connect(self.teach_repeat_start)
        # self.ui.btnUser4.setText("record position")
        # self.ui.btnUser4.clicked.connect(self.teach_repeat_record)
        # self.ui.btnUser5.setText("play teach and repeat")
        # self.ui.btnUser5.clicked.connect(self.teach_repeat_play)
        # self.ui.btnUser6.setText("save rgb")
        # self.ui.btnUser6.clicked.connect(self.save_rgb)
        self.ui.btnUser3.setText("EVENT1")
        self.ui.btnUser3.clicked.connect(self.event1)
        self.ui.btnUser4.setText("EVENT2")
        self.ui.btnUser4.clicked.connect(self.event2)
        self.ui.btnUser5.setText("EVENT3")
        self.ui.btnUser5.clicked.connect(self.event3)
        self.ui.btnUser6.setText("EVENT4")
        self.ui.btnUser6.clicked.connect(self.event4)
        self.ui.btnUser7.setText("EVENT5")
        self.ui.btnUser7.clicked.connect(self.event5)
        self.ui.btnUser8.setText("depth test")
        self.ui.btnUser8.clicked.connect(self.video.blockDetector)
        self.ui.btnUser9.setText("recover")
        self.ui.btnUser9.clicked.connect(self.recover)
        self.ui.btnUser10.setText("click put")
        self.ui.btnUser10.clicked.connect(self.click_put)
        self.ui.btnUser12.setText("Emergency Stop!")
        self.ui.btnUser12.clicked.connect(self.estop)

    # def save_rgb(self):
    #     img = cv2.cvtColor(self.video.currentVideoFrame, cv2.COLOR_RGB2BGR)
    #     cv2.imwrite('real_example.png',img)
    # def teach_repeat_start(self):
    #     print "i enter"
    #     global Record_Value
    #     Record_Value = [[]]
    #     self.statemachine.idle(self.rex)
    #     # self.statemachine.moving(self.rex, self.rex.rexarm_IK([207,14, 30, np.pi/2],1) + [0.0, -10], [0.1,0.1,0.1,0.1,0.1,0.1])
    #     # print self.rex.rexarm_IK([-100,-100, 40, np.pi/2],1)

    # def teach_repeat_record(self):
    #     temp_record = [self.rex.joint_angles_fb[0],
    #     self.rex.joint_angles_fb[1],
    #     self.rex.joint_angles_fb[2],
    #     self.rex.joint_angles_fb[3],
    #     self.rex.joint_angles_fb[4]]
    #     if len(Record_Value[0]) == 0:
    #         Record_Value[0] = temp_record
    #     else :
    #         Record_Value.append(temp_record)

    # def teach_repeat_play(self):
    #     print Record_Value
    #     self._timer_teach.start(self.check_time)
    # # def check_move(self):
    #     print "111111"
    #     if len(Record_Value[0]) == 0:
    #         self.check_move_counter = 0
    #         self._timer_teach.stop()
    #         return
    #     if self.check_move_counter == 0:
    #         T = np.array([[1,0,0,0,0,0],[0,1,0,0,0,0],[0,0,2,0,0,0],[1,self.d_t,self.d_t**2,self.d_t**3,self.d_t**4,self.d_t**5],
    #             [0,1,2*self.d_t,3*self.d_t**2,4*self.d_t**3,5*self.d_t**4],[0,0,2,6*self.d_t,12*self.d_t**2,20*self.d_t**3]])
    #         for i in range(self.rex.num_joints):
    #             self.spe_coef[i] = np.dot(np.linalg.inv(T), np.array([[self.rex.joint_angles_fb[i]],[0.01],[0],
    #                 [Record_Value[self.pos_idx_teach][i]],[0.1],[0.]]))
    #         print "haha"
    #         print Record_Value[self.pos_idx_teach]
    #         print "fb"
    #         print self.rex.joint_angles_fb
    #         self.check_move_counter += 1
    #     pos = [0.] * self.rex.num_joints
    #     speed = [0.] * self.rex.num_joints
    #     for i in range(self.rex.num_joints):
    #         pos[i] = np.polynomial.polynomial.polyval((self.check_move_counter * self.check_move_time / 1000), self.spe_coef[i])
    #         if i == 3:
    #             speed[i] = abs(np.polynomial.polynomial.polyval((self.check_move_counter * self.check_move_time / 1000),
    #             np.polynomial.polynomial.polyder(self.spe_coef[i]))) / SPEED_AX12
    #         else:
    #             speed[i] = abs(np.polynomial.polynomial.polyval((self.check_move_counter * self.check_move_time / 1000),
    #             np.polynomial.polynomial.polyder(self.spe_coef[i]))) / SPEED_MX28 * 20

    #     if self.check_move_counter * self.check_move_time / 1000. <= self.d_t and np.linalg.norm(np.transpose(np.asarray(Record_Value[self.pos_idx_teach])) - np.transpose(np.asarray(self.rex.joint_angles_fb))) > 0.1:
    #         # and np.linalg.norm(Record_Value[self.pos_idx_teach] - np.asarray(self.rex.joint_angles_fb)) > 0.1
    #         if self.pos_pre != pos:
    #             self.statemachine.moving(self.rex, pos, speed)
    #         self.pos_pre = pos
    #         print "pos"
    #         print pos
    #         print "feedback"
    #         print self.rex.joint_angles_fb
    #         print "speed"
    #         print speed
    #         print self.check_move_counter
    #         # self.check_move_counter += 1
    #         if np.linalg.norm(np.transpose(np.asarray(pos)) - np.transpose(np.asarray(self.rex.joint_angles_fb)),axis = 1) < 0.01:
    #             self.check_move_counter += 1
    #     else:
    #         self.check_move_counter = 0
    #         if self.pos_idx_teach < len(Record_Value) - 1:
    #             self.pos_idx_teach += 1;
    #         else:
    #             print "i stop"
    #             self.pos_idx_teach = 0
    #             self._timer_teach.stop()
    #     print "222222"
    # def check_move(self):
    #     pos = 0
    #     if len(Record_Value[0]) == 0:
    #         self._timer_teach.stop()
    #         return
    #     print len(Record_Value[self.pos_idx_teach])
    #     print np.linalg.norm(np.asarray(Record_Value[self.pos_idx_teach]) - np.asarray(self.rex.joint_angles_fb[0:5]))
    #     if np.linalg.norm(np.transpose(np.asarray(Record_Value[self.pos_idx_teach])) - np.transpose(np.asarray(self.rex.joint_angles_fb[0:5]))) > 0.04:
    #         pos = Record_Value[self.pos_idx_teach] + [0]
    #         self.statemachine.moving(self.rex, pos,[0.1,0.1,0.1,0.1,0.1,0.1])
    #         print pos
    #     else:
    #         if self.pos_idx_teach < len(Record_Value) - 1:
    #             self.pos_idx_teach += 1;
    #         else:
    #             print "i stop"
    #             self.pos_idx_teach = 0
    #             self._timer_teach.stop()
    def recover(self):
        self.rex.moving_status = 0

    def move_to_point(self):
        pos = 0
        if len(self.rex.plan) == 0:
            self._timer_waypoint.stop()
            return
        # print len(self.rex.plan[self.pos_idx_teach])
        # print self.check_arrival(self.rex.plan[self.pos_idx_teach],self.rex.joint_angles_fb)
        if self.check_arrival(self.rex.plan[self.pos_idx_teach],
                              self.rex.joint_angles_fb
                              ) > 0.07 and self.move_to_point_cn < 40:
            pos = self.rex.plan[self.pos_idx_teach]
            vel = self.vel[self.pos_idx_teach]
            self.statemachine.moving(self.rex, pos, vel)
            self.move_to_point_cn += 1
            # print "load load"
            # print self.rex.load_fb
            # print "going to pos:"
            # print pos
        else:
            if self.move_to_point_cn >= 70:
                print "cant move to that point within %d s" % (
                    self.check_time * self.move_to_point_cn)
            self.move_to_point_cn = 0
            if self.pos_idx_teach < len(self.rex.plan) - 1:
                self.pos_idx_teach += 1
            else:
                print "i stop"
                self.pos_idx_teach = 0
                self.rex.plan = []
                self.rex.plan_status = 0
                self._timer_waypoint.stop()

    def click_grab(self):
        x = self.last_click[0]
        y = self.last_click[1]
        z = 0.
        world_z = 0.
        rw = np.array([[0], [0], [0]])
        M = self.video.aff_matrix
        M_RGBtoDEP = self.video.aff_matrix_RGBtoDEP
        v = np.array([[x], [y], [1]])
        DEP_xy = np.dot(M_RGBtoDEP, v)
        if ((DEP_xy[0] >= 0) and (DEP_xy[0] <= 640) and (DEP_xy[1] >= 0)
                and (DEP_xy[1] <= 480)):
            world_z = self.video.depth - 123.6 * np.tan(
                self.video.currentDepthFrame[int(DEP_xy[1])][int(DEP_xy[0])] /
                2842.5 + 1.1863)
            z = self.video.currentDepthFrame[int(DEP_xy[1])][int(DEP_xy[0])]
            rw = np.dot(
                np.linalg.inv(self.video.WORLDtoRGB[:, [0, 1, 3]]),
                np.subtract(v, self.video.WORLDtoRGB[:, 2:3] * (-world_z)))
        else:
            z = 0.0
        if rw[2] != 0:
            # self.statemachine.moving(self.rex, self.rex.rexarm_IK([rw[0]/rw[2],rw[1]/rw[2], world_z + 30, np.pi/2],1) + [0.0, -10], [0.1,0.1,0.1,0.1,0.1,0.1])
            t = self.rex.rexarm_IK(
                [rw[0] / rw[2], rw[1] / rw[2], world_z - 9, np.pi], 1)
            self.rex.plan = [[t[0], -200, -200, -200, 0, -50 * D2R],
                             [-200, -200, t[2], t[3], 0, -200],
                             [-200, t[1], -200, -200, 0, -200],
                             [-200, -200, -200, -200, 0, 7 * D2R],
                             [-200, 0, -200, -200, -200, -200],
                             [0, 0, 0, 0 * D2R, 0, -200]]
            self._timer_waypoint.start(self.check_time)
            print "plan"
            print self.rex.plan
        else:
            print "try again"

    def click_put(self):
        x = self.last_click[0]
        y = self.last_click[1]
        z = 0.
        world_z = 0.
        rw = np.array([[0], [0], [0]])
        M = self.video.aff_matrix
        M_RGBtoDEP = self.video.aff_matrix_RGBtoDEP
        v = np.array([[x], [y], [1]])
        DEP_xy = np.dot(M_RGBtoDEP, v)
        if ((DEP_xy[0] >= 0) and (DEP_xy[0] <= 640) and (DEP_xy[1] >= 0)
                and (DEP_xy[1] <= 480)):
            world_z = self.video.depth - 123.6 * np.tan(
                self.video.currentDepthFrame[int(DEP_xy[1])][int(DEP_xy[0])] /
                2842.5 + 1.1863)
            z = self.video.currentDepthFrame[int(DEP_xy[1])][int(DEP_xy[0])]
            rw = np.dot(
                np.linalg.inv(self.video.WORLDtoRGB[:, [0, 1, 3]]),
                np.subtract(v, self.video.WORLDtoRGB[:, 2:3] * (-world_z)))
        else:
            z = 0.0
        if rw[2] != 0:
            # self.statemachine.moving(self.rex, self.rex.rexarm_IK([rw[0]/rw[2],rw[1]/rw[2], world_z + 30, np.pi/2],1) + [0.0, -10], [0.1,0.1,0.1,0.1,0.1,0.1])
            t = self.rex.rexarm_IK(
                [rw[0] / rw[2], rw[1] / rw[2], world_z + 40, np.pi / 2], 1)
            self.rex.plan = [[t[0], -200, -200, -200, 0, -200],
                             [-200, -200, t[2], t[3], 0, -200],
                             [-200, t[1], -200, -200, 0, -200],
                             [-200, -200, -200, -200, 0, -20 * D2R],
                             [-200, 0, -200, -200, -200, -200],
                             [0, 0, 0, 0 * D2R, 0, -200]]
            self._timer_waypoint.start(self.check_time)
            print "plan"
            print self.rex.plan
        else:
            print "try again"

    def check_arrival(self, pos0, pos1):
        t = 0
        for i in range(len(pos0)):
            if pos0[i] != -200:
                if i != 5:
                    t = t + (pos0[i] - pos1[i])**2
                else:
                    t = t + 0.25 * (pos0[i] - pos1[i])**2
        return np.sqrt(t)

    def event1(self):
        print "entering event1"
        self.video.det_pts_ = []
        self.video.blockDetector()
        self.plan_point = 0
        self._timer_event1.timeout.connect(self.event1_timer)
        self._timer_event1.start(self.check_time)

    def event2(self):
        print "entering event2"
        self._timer_event2.stop()
        self.video.det_pts_ = []
        self.video.blockDetector()
        self.plan_point = 0
        self._timer_event2.timeout.connect(self.event2_timer)
        self._timer_event2.start(self.check_time)

    def event3(self):
        print "entering event3"
        # self.video.blockDetector()
        self.video.det_pts_ = []
        self.event3_finish = 0
        self.plan_point = 0
        self.event3_color_check = [0] * 8
        self._timer_event3.timeout.connect(self.event3_timer)
        self._timer_event3.start(self.check_time)

    def event4(self):
        print "entering event4"
        # self.video.blockDetector()
        self.video.det_pts_ = []
        self.plan_point = 0
        self.event4_color_idx = 0
        self.event4_f_idx = 0
        self._timer_event4.timeout.connect(self.event4_timer)
        self._timer_event4.start(self.check_time)

    def event5(self):
        print "entering event5"
        # self.video.blockDetector()
        self.video.det_pts_ = []
        self.plan_point = 0
        self.event5_block_num = 0
        self._timer_event5.timeout.connect(self.event5_timer)
        self._timer_event5.start(self.check_time)

    def event1_timer(self):
        if self.rex.moving_status == 1:
            self._timer_event1.stop()
            self.rex.plan = []
            self.vel = []
            self._timer_waypoint.stop()

        if len(self.video.det_pts_) == 0:
            print "no block detected"
            self.plan_point = 0
            self._timer_event1.stop()
        else:
            if self.rex.plan_status == 0 and self.plan_point < len(
                    self.video.det_pts_):
                x = self.video.det_pts_[self.plan_point].rgb_real_pos[0]
                y = self.video.det_pts_[self.plan_point].rgb_real_pos[1]
                print "grapping %d %d %d" % (self.plan_point, x, y)
                world_z = self.video.det_pts_[self.plan_point].depth
                rw = np.array([[0], [0], [0]])
                M = self.video.aff_matrix
                M_RGBtoDEP = self.video.aff_matrix_RGBtoDEP
                v = np.array([[x], [y], [1]])
                t = self.rex.rexarm_IK([x, y, world_z, np.pi / 2], 1)
                t1 = self.rex.rexarm_IK([x, -y, world_z, np.pi / 2], 1)
                # if t[0] < 0:
                #     t[0] = np.pi + t[0]
                #     t[1] =  0 - t[1]
                #     t[2] = 0 - t[2]
                #     t[3] = 0 - t[3]
                # if t1[0] < 0:
                #     t1[0] = np.pi + t1[0]
                #     t1[1] = 0 - t1[1]
                #     t1[2] = 0 - t1[2]
                #     t1[3] = 0 - t1[3]
                # self.rex.plan = [[t[0],-200,-200,-200,0,-30*D2R],[-200,-200,t[2],t[3],0,-200],[-200,t[1],-200,-200,0,-200],[-200,-200,-200,-200,0,10*D2R],[-200,0,-200,-200,-200,-200],[-200,0,0,0*D2R,0,-200]]
                self.rex.plan = [[t[0], -200, t[2], t[3], 0, -30 * D2R],
                                 [-200, t[1], -200, -200, 0, -200],
                                 [-200, -200, -200, -200, 0, 8 * D2R],
                                 [
                                     -200, t1[1] -
                                     55 * D2R if t1[1] - 55 * D2R > 0 else 0,
                                     -200, -200, -200, -200
                                 ]]
                # t = self.rex.rexarm_IK([x,-y, world_z, np.pi/2],1)
                self.rex.plan.extend(
                    [[t1[0], -200, -200, -200, 0, -200],
                     [-200, -200, t1[2], t1[3], 0, -200],
                     [-200, t1[1], -200, -200, 0, -200],
                     [-200, -200, -200, -200, 0, -15 * D2R],
                     [
                         -200, t1[1] - 55 * D2R if t1[1] - 55 * D2R > 0 else 0,
                         t1[2] + 5 * D2R, -200, -200, -200
                     ]])
                self.vel = [[0.3, 0.25, 0.15, 0.15, 0.7, 0.8]] * len(
                    self.rex.plan)
                self.plan_point += 1
                self._timer_waypoint.start(self.check_time)
                self.rex.plan_status = 1
            elif self.plan_point == len(
                    self.video.det_pts_) and self.rex.plan_status == 0:
                print "event1 stops"
                self._timer_event1.stop()
                self.rex.plan = []
                self.vel = []
            else:
                pass

    def event2_timer(self):
        if self.rex.moving_status == 1:
            self._timer_event2.stop()
            self.rex.plan = []
            self._timer_waypoint.stop()
        if len(self.video.det_pts_) == 0:
            print "no block detected"
            self.plan_point = 0
            self._timer_event2.stop()
        else:
            if self.rex.plan_status == 0 and self.plan_point < len(
                    self.video.det_pts_):
                x = self.video.det_pts_[self.plan_point].rgb_real_pos[0]
                y = self.video.det_pts_[self.plan_point].rgb_real_pos[1]
                print "grapping %d %d %d" % (self.plan_point, x, y)
                world_z = self.video.det_pts_[self.plan_point].depth
                rw = np.array([[0], [0], [0]])
                M = self.video.aff_matrix
                M_RGBtoDEP = self.video.aff_matrix_RGBtoDEP
                v = np.array([[x], [y], [1]])
                t = self.rex.rexarm_IK([x, y, world_z, np.pi / 2], 1)
                t1 = self.rex.rexarm_IK(
                    [EVEN2_X, EVEN2_Y, 25 + 38 * self.plan_point, np.pi / 2],
                    1)
                self.rex.plan = [[t[0], -200, t[2], t[3], 0, -20 * D2R],
                                 [-200, t[1], -200, -200, 0, -200],
                                 [-200, -200, -200, -200, 0, 8 * D2R],
                                 [
                                     -200, t1[1] -
                                     55 * D2R if t1[1] - 55 * D2R > 0 else 0,
                                     t1[2], t1[3], -200, -200
                                 ]]
                self.rex.plan.extend(
                    [[t1[0], -200, t1[2], t1[3], 0, -200],
                     [-200, t1[1], -200, -200, 0, -200],
                     [-200, -200, -200, -200, 0, -20 * D2R],
                     [
                         -200, t1[1] - 55 * D2R if t1[1] - 55 * D2R > 0 else 0,
                         t1[2] + 5 * D2R, -200, -200, -200
                     ]])
                self.vel = [[0.3, 0.25, 0.15, 0.15, 0.7, 0.8]] * len(
                    self.rex.plan)
                self.plan_point += 1
                self._timer_waypoint.start(self.check_time)
                self.rex.plan_status = 1
            elif self.plan_point == len(
                    self.video.det_pts_) and self.rex.plan_status == 0:
                print "event2 stops"
                self._timer_event2.stop()
                self.rex.plan = []
                self.vel = []
            else:
                pass

    def event3_timer(self):
        if self.rex.moving_status == 1:
            self._timer_event3.stop()
            self.rex.plan = []
            self._timer_waypoint.stop()
        if len(self.video.det_pts_) == 0:
            print "begin to detect"
            self.video.blockDetector()
            points_temp = []
            for points_i in self.video.det_pts_:
                if self.event3_color_check[
                        points_i.color] == 0 and points_i.rgb_real_pos[0] < 0:
                    self.event3_color_check[points_i.color] = 1
                    points_temp.append(points_i)
            self.video.det_pts_ = points_temp
            self.plan_point = 0
            if len(self.video.det_pts_) == 0:
                if self.event3_finish == 0 and self.rex.plan_status == 0:
                    print "pushing blocks"
                    self.rex.plan = [[
                        -115 * D2R, 68 * D2R, 19 * D2R, 76 * D2R, -115 * D2R, 0
                    ], [
                        -122 * D2R, 57 * D2R, 47 * D2R, 76 * D2R, -122 * D2R, 0
                    ]]
                    self.vel = [[0.2, 0.1, 0.05, 0.05, 0.3, 0.3]] * len(
                        self.rex.plan)
                    self._timer_waypoint.start(self.check_time)
                    self.rex.plan_status = 1
                    self.event3_finish = 1
                elif self.event3_finish == 1 and self.rex.plan_status == 0:
                    print "event finishes"
                    self._timer_event3.stop()
                    self.rex.plan = []
                    self.vel = []
                return
            # self._timer_event.stop()

        if self.rex.plan_status == 0 and self.plan_point < len(
                self.video.det_pts_):
            x = self.video.det_pts_[self.plan_point].rgb_real_pos[0]
            y = self.video.det_pts_[self.plan_point].rgb_real_pos[1]
            print "grapping %d %d %d" % (self.plan_point, x, y)
            world_z = self.video.det_pts_[self.plan_point].depth
            rw = np.array([[0], [0], [0]])
            M = self.video.aff_matrix
            M_RGBtoDEP = self.video.aff_matrix_RGBtoDEP
            v = np.array([[x], [y], [1]])
            if np.arctan2(y, x) < -np.pi / 2 and np.arctan2(
                    y, x) > -3 * np.pi / 4:
                t = self.rex.rexarm_IK([x, y, world_z, np.pi / 2], 1)
            else:
                t = self.rex.rexarm_IK([x, y, world_z - 5, np.pi / 2], 1)
            self.rex.plan = [[t[0] - 2 * D2R, -200, -200, -200, 0, -30 * D2R],
                             [-200, -200, t[2], t[3], 0, -200],
                             [-200, t[1], -200, -200, 0, -200],
                             [-200, -200, -200, -200, 0, 8 * D2R],
                             [-200, 0, -200, -200, -200, -200],
                             [-200, 0, 0, 0 * D2R, 0, -200]]
            self.vel = [[0.2, 0.15, 0.13, 0.13, 0.7, 0.7]] * len(self.rex.plan)
            color_idx = self.video.det_pts_[self.plan_point].color
            t = self.rex.rexarm_IK(
                [EVENT3_X, EVENT3_Y[color_idx], 24 + 38, np.pi / 2], 1)
            t1 = self.rex.rexarm_IK(
                [EVENT3_X, EVENT3_Y[color_idx], 24, np.pi / 2], 1)
            # self.rex.plan.extend([[t[0],-200,-200,-200,t[0],-200],[-200,-200,t[2],t[3],-200,-200],[-200,t[1],-200,-200,-200,-200],[-200,-200,-200,-200,-200,-50*D2R],[-200,0,-200,-200,-200,-200],[-200,0,0,0*D2R,0,-200]])
            t4 = t[0] + np.pi / 2 if t[0] < 0 else t[0] - 3 * np.pi / 2
            self.rex.plan.extend([[t[0], -200, -200, -200, t4, -200],
                                  [-200, -200, t[2], t[3], -200, -200],
                                  [-200, t[1], -200, -200, -200, -200],
                                  [t1[0], t1[1], t1[2], t1[3], t4, -200],
                                  [-200, -200, -200, -200, -200, -50 * D2R],
                                  [-200, 0, -200, -200, -200, -200],
                                  [-200, 0, 0, 0 * D2R, 0, -200]])
            self.vel.extend([[0.2, 0.15, 0.13, 0.13, 0.7, 0.7],
                             [0.2, 0.15, 0.13, 0.13, 0.7, 0.7],
                             [0.2, 0.15, 0.13, 0.13, 0.7, 0.7],
                             [0.07, 0.05, 0.05, 0.05, 0.7, 0.5],
                             [0.2, 0.15, 0.13, 0.13, 0.7, 0.7],
                             [0.2, 0.15, 0.13, 0.13, 0.7, 0.7],
                             [0.2, 0.15, 0.13, 0.13, 0.7, 0.7]])
            self.event3_color_check[self.video.det_pts_[
                self.plan_point].color] = 1
            self.plan_point += 1
            self._timer_waypoint.start(self.check_time)
            self.rex.plan_status = 1
        elif self.plan_point == len(
                self.video.det_pts_) and self.rex.plan_status == 0:
            print "event3 finish once"
            # self._timer_event.stop()
            self.rex.plan = []
            self.vel = []
            self.plan_point = 0
            self.video.det_pts_ = []
        else:
            pass

    def event4_timer(self):
        color_status = 0
        if self.rex.moving_status == 1:
            self._timer_event4.stop()
            self.rex.plan = []
            self._timer_waypoint.stop()
        if len(self.video.det_pts_) == 0:
            print "begin to detect"
            self.video.blockDetector()
            points_temp = []
            if self.event4_color_idx == 8:
                print "event finishes"
                self._timer_event4.stop()
                self.rex.plan = []
                self.vel = []
                return
            for points_i in self.video.det_pts_:
                if points_i.color == RE_COLOR[self.event4_color_idx]:
                    points_temp.append(points_i)
                    self.event4_color_idx += 1
                    color_status = 1
                    break
            if color_status == 0:
                for points_i in self.video.det_pts_:
                    if points_i.depth > 50 and points_i.rgb_real_pos[0] < 0:
                        points_temp.append(points_i)
                        break
            self.video.det_pts_ = points_temp
            self.plan_point = 0
            # self._timer_event.stop()

        if self.rex.plan_status == 0 and self.plan_point < len(
                self.video.det_pts_):
            x = self.video.det_pts_[self.plan_point].rgb_real_pos[0]
            y = self.video.det_pts_[self.plan_point].rgb_real_pos[1]
            print "grapping %d %d %d" % (self.plan_point, x, y)
            world_z = self.video.det_pts_[self.plan_point].depth
            rw = np.array([[0], [0], [0]])
            M = self.video.aff_matrix
            M_RGBtoDEP = self.video.aff_matrix_RGBtoDEP
            v = np.array([[x], [y], [1]])
            t = self.rex.rexarm_IK([x, y, world_z, np.pi / 2], 1)

            self.rex.plan = [[t[0], -200, -200, -200, 0, -30 * D2R],
                             [-200, -200, t[2], t[3], 0, -200],
                             [-200, t[1], -200, -200, 0, -200],
                             [-200, -200, -200, -200, 0, 8 * D2R],
                             [-200, 0, -200, -200, -200, -200],
                             [-200, 0, 0, 0 * D2R, 0, -200]]
            self.vel = [[0.25, 0.2, 0.18, 0.18, 0.7, 0.7]] * len(self.rex.plan)
            color_idx = self.video.det_pts_[self.plan_point].color
            if color_status == 1:
                t = self.rex.rexarm_IK([
                    EVENT4_X, EVENT4_Y,
                    (self.event4_color_idx - 1) * 38 + 24, np.pi / 2
                ], 1)
            else:
                t = self.rex.rexarm_IK([
                    EVENT4_XF[self.event4_f_idx], EVENT4_YF[self.event4_f_idx],
                    24, np.pi / 2
                ], 1)
                self.event4_f_idx += 1
            self.rex.plan.extend([[t[0], -200, -200, -200, 0, -200],
                                  [-200, -200, t[2], t[3], 0, -200],
                                  [-200, t[1], -200, -200, 0, -200],
                                  [-200, -200, -200, -200, 0, -20 * D2R],
                                  [-200, 0, t[2], -200, -200, -200],
                                  [-200, 0, 0, 0 * D2R, 0, -200]])
            self.vel.extend([[0.25, 0.2, 0.18, 0.18, 0.7, 0.7]] * 6)
            self.plan_point += 1
            self._timer_waypoint.start(self.check_time)
            self.rex.plan_status = 1
        elif self.plan_point == len(
                self.video.det_pts_) and self.rex.plan_status == 0:
            print "event4 finish once"
            # self._timer_event.stop()
            self.rex.plan = []
            self.vel = []
            self.plan_point = 0
            self.video.det_pts_ = []
        else:
            pass

    def event5_timer(self):
        if self.rex.moving_status == 1:
            self._timer_event5.stop()
            self.rex.plan = []
            self._timer_waypoint.stop()

        if len(self.video.det_pts_) == 0:
            if self.event5_block_num == 29:
                print "event finishes"
                self._timer_event5.stop()
                self.rex.plan = []
                self.vel = []
                return
            print "begin to detect"
            self.video.blockDetector()
            points_temp = []
            points_max = 0
            for points_i in self.video.det_pts_:
                if points_i.rgb_real_pos[0] > 0 and abs(
                        np.arctan2(points_i.rgb_real_pos[1],
                                   points_i.rgb_real_pos[0])) > points_max:
                    points_temp.append(points_i)
                    points_max = abs(
                        np.arctan2(points_i.rgb_real_pos[1],
                                   points_i.rgb_real_pos[0]))
            # print points_i.depth
            self.video.det_pts_ = [points_temp[-1]]
            self.plan_point = 0

        if self.rex.plan_status == 0 and self.plan_point < len(
                self.video.det_pts_):
            x = self.video.det_pts_[self.plan_point].rgb_real_pos[0]
            y = self.video.det_pts_[self.plan_point].rgb_real_pos[1]
            print "grapping %d %d %d" % (self.plan_point, x, y)
            world_z = self.video.det_pts_[self.plan_point].depth
            rw = np.array([[0], [0], [0]])
            M = self.video.aff_matrix
            M_RGBtoDEP = self.video.aff_matrix_RGBtoDEP
            v = np.array([[x], [y], [1]])
            t = self.rex.rexarm_IK([x, y, world_z, np.pi / 2], 1)
            if x > 0 and y > 0:
                t[0] -= 2 * D2R
            self.rex.plan = [[t[0], -200, -200, -200, 0, -30 * D2R],
                             [-200, -200, t[2], t[3], 0, -200],
                             [-200, t[1], -200, -200, 0, -200],
                             [-200, -200, -200, -200, 0, 8 * D2R],
                             [-200, 0, -200, -200, -200, -200],
                             [-200, 0, 0, 0 * D2R, 0, -200]]
            self.vel = [[0.3, 0.2, 0.18, 0.18, 0.7, 0.7]] * len(self.rex.plan)
            color_idx = self.video.det_pts_[self.plan_point].color
            if self.event5_block_num < 28:
                t = self.rex.rexarm_IK([
                    EVENT5_X[self.event5_block_num],
                    EVENT5_Y[self.event5_block_num],
                    29 + EVENT5_Z[self.event5_block_num] * 38, np.pi / 2
                ], 1)
            else:
                t = self.rex.rexarm_IK([
                    EVENT5_X[27], EVENT5_Y[27], 29 + EVENT5_Z[27] * 38 + 38,
                    np.pi / 2
                ], 1)
            self.rex.plan.extend([[t[0], -200, -200, -200, 0, -200],
                                  [-200, -200, t[2], t[3], 0, -200],
                                  [-200, t[1], -200, -200, 0, -200],
                                  [-200, -200, -200, -200, 0, -20 * D2R],
                                  [-200, 0, t[2] + 5 * D2R, -200, -200, -200],
                                  [-200, 0, 0, 0 * D2R, 0, -200]])
            self.vel.extend([[0.285, 0.2, 0.18, 0.18, 0.7, 0.7]] * 6)
            self.plan_point += 1
            self.event5_block_num += 1
            self._timer_waypoint.start(self.check_time)
            self.rex.plan_status = 1
        elif self.plan_point == len(
                self.video.det_pts_) and self.rex.plan_status == 0:
            print "event5 finish once"
            # self._timer_event.stop()
            self.rex.plan = []
            self.vel = []
            self.plan_point = 0
            self.video.det_pts_ = []
        else:
            pass

    def estop(self):
        self.statemachine.estop(self.rex)

    def update_gui(self):
        """ 
        update_gui function
        Continuously called by timer1 
        """
        """ Renders the video frames
            Displays a dummy image if no video available
            HINT: you can use the radio buttons to select different
            video sources like is done below
        """

        if (self.video.kinectConnected == 1):
            try:
                self.video.captureVideoFrame()
                self.video.captureDepthFrame()
            except TypeError:
                self.video.kinectConnected = 0
                self.video.loadVideoFrame()
                self.video.loadDepthFrame()

        if (self.ui.radioVideo.isChecked()):
            self.ui.videoFrame.setPixmap(self.video.convertFrame())
            # x = QtGui.QWidget.mapFromGlobal(self,QtGui.QCursor.pos()).x()
            # y = QtGui.QWidget.mapFromGlobal(self,QtGui.QCursor.pos()).y()
            # if ((x > MIN_X) and (x < MAX_X) and (y > MIN_Y) and (y < MAX_Y)):
            #     img = self.video.currentVideoFrame
            #     hsv = cv2.cvtColor(img, cv2.COLOR_RGB2HSV)
            #     y -= 30
            #     x -= 310
            #     h = hsv[y][x][0]
            #     s = hsv[y][x][1]
            #     v = hsv[y][x][2]
            #     global hp, xp, yp
            #     if ((xp - x)*(xp - x) > 4 and (yp - y)*(yp - y) > 4) or (hp - h)*(hp - h) >= 1:
            #         print "x: %d y: %d hsv : (%d, %d, %d)" % (x,y,h,s,v)
            #     xp = x
            #     yp = y
            #     hp = h
        if (self.ui.radioDepth.isChecked()):
            self.ui.videoFrame.setPixmap(self.video.convertDepthFrame())
        """ 
        Update GUI Joint Coordinates Labels
        """
        self.rex.rexarm_FK()
        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)))
        self.ui.rdoutX.setText(str("%.2f" % (self.rex.endeffector_global[0])))
        self.ui.rdoutY.setText(str("%.2f" % (self.rex.endeffector_global[1])))
        self.ui.rdoutZ.setText(str("%.2f" % (self.rex.endeffector_global[2])))
        self.ui.rdoutT.setText(str("%.2f" % (self.rex.endeffector_global[3])))
        """ 
        Mouse position presentation in GUI
        TODO: after implementing workspace calibration 
        display the world coordinates the mouse points to 
        in the RGB video image.
        """
        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
            z = 0.
            world_z = 0.
            rw = np.array([[0], [0], [0]])
            self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%f)" % (x, y, z))
            if (self.video.cal_flag == 2):
                M = self.video.aff_matrix
                M_RGBtoDEP = self.video.aff_matrix_RGBtoDEP
                v = np.array([[x], [y], [1]])
                DEP_xy = np.dot(M_RGBtoDEP, v)
                if ((DEP_xy[0] >= 0) and (DEP_xy[0] <= 640)
                        and (DEP_xy[1] >= 0) and (DEP_xy[1] <= 480)):
                    world_z = self.video.depth - 123.6 * np.tan(
                        self.video.currentDepthFrame[int(DEP_xy[1])][int(
                            DEP_xy[0])] / 2842.5 + 1.1863)
                    z = self.video.currentDepthFrame[int(DEP_xy[1])][int(
                        DEP_xy[0])]
                    rw = np.dot(
                        np.linalg.inv(self.video.WORLDtoRGB[:, [0, 1, 3]]),
                        np.subtract(v, self.video.WORLDtoRGB[:, 2:3] *
                                    (-world_z)))
                else:
                    z = 0.0
                if rw[2] != 0:
                    self.ui.rdoutMouseWorld.setText(
                        "(%.0f,%.0f, %.0f)" %
                        (rw[0] / rw[2], rw[1] / rw[2], world_z))
                else:
                    self.ui.rdoutMouseWorld.setText("(-,-,-)")
            else:
                self.ui.rdoutMouseWorld.setText("(-,-,-)")
        """ 
        Updates status label when rexarm playback is been executed.
        This can 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))

    def sliderChange(self):
        """ 
        Function to change the slider labels when sliders are moved
        and to command the arm to the given position
        """
        self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
        self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))
        self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
        self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))
        # self.ui.rdoutGrip1.setText(str(self.ui.sldrGrip1.value()))
        # self.ui.rdoutGrip2.setText(str(self.ui.sldrGrip2.value()))

        self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
        self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
        self.rex.torque_multiplier = self.ui.sldrMaxTorque.value() / 100.0
        self.rex.speed_multiplier = 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.joint_angles[4] = self.ui.sldrGrip1.value() * D2R
        self.rex.joint_angles[5] = self.ui.sldrGrip2.value() * D2R
        # self.rex.joint_angles[6] = self.ui.sldrGrip2.value()*D2R
        self.rex.cmd_publish()

    def mousePressEvent(self, QMouseEvent):
        """ 
        Function used to record mouse click positions for calibration 
        """
        """ Get mouse posiiton """
        x = QMouseEvent.x()
        y = QMouseEvent.y()
        """ If mouse position is not over the camera image ignore """
        if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)): return
        """ Change coordinates to image axis """
        self.last_click[0] = x - MIN_X
        self.last_click[1] = y - MIN_Y
        """ If calibration has been performed """
        if (self.video.cal_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 workspace calibration routine
            and NOT the simple calibration function as is done now.
            """
            if (self.video.mouse_click_id == 2 * self.video.cal_points):
                """ 
                Update status of calibration flag and number of mouse
                clicks
                """
                self.video.cal_flag = 2
                self.video.mouse_click_id = 0
                """ Perform affine calibration with OpenCV """
                self.video.aff_matrix = cv2.getAffineTransform(
                    self.video.mouse_coord[0:3], self.video.real_coord[0:3])
                self.video.aff_matrix_RGBtoDEP = cv2.getAffineTransform(
                    self.video.mouse_coord[0:3], self.video.mouse_coord[5:8])
                self.video.aff_matrix_DEPtoRGB = cv2.getAffineTransform(
                    self.video.mouse_coord[5:8], self.video.mouse_coord[0:3])
                object_points = np.insert(self.video.real_coord,
                                          2, [0, -38, -76, -76, -38],
                                          axis=1)
                image_points = np.array(
                    self.video.mouse_coord[0:self.video.cal_points])
                reval, rvec, tvec = cv2.solvePnP(object_points, image_points,
                                                 self.video.intrinsic,
                                                 self.video.distortion,
                                                 cv2.SOLVEPNP_EPNP)
                self.video.WORLDtoRGB = np.dot(
                    self.video.intrinsic,
                    np.insert(cv2.Rodrigues(rvec)[0], [3], tvec, axis=1))
                t = np.dot(self.video.WORLDtoRGB,
                           np.array([[-100.0], [100.0], [-114.], [1.]]))
                self.video.WORLDtoRGB /= t[2]
                # print t[2]
                z1_temp = np.tan(self.video.currentDepthFrame[int(
                    self.video.mouse_coord[5][1])][int(
                        self.video.mouse_coord[5][0])] / 2842.5 + 1.1863)
                z2_temp = np.tan(self.video.currentDepthFrame[int(
                    self.video.mouse_coord[6][1])][int(
                        self.video.mouse_coord[6][0])] / 2842.5 + 1.1863)
                z3_temp = np.tan(self.video.currentDepthFrame[int(
                    self.video.mouse_coord[7][1])][int(
                        self.video.mouse_coord[7][0])] / 2842.5 + 1.1863)
                z4_temp = np.tan(self.video.currentDepthFrame[int(
                    self.video.mouse_coord[8][1])][int(
                        self.video.mouse_coord[8][0])] / 2842.5 + 1.1863)
                z5_temp = np.tan(self.video.currentDepthFrame[int(
                    self.video.mouse_coord[9][1])][int(
                        self.video.mouse_coord[9][0])] / 2842.5 + 1.1863)
                # print z1_temp
                # print z2_temp
                # print z3_temp
                # print z4_temp
                # self.video.depth = 123.6 * (z1_temp + z2_temp) / 2
                self.video.depth = 123.6 * (z1_temp + z2_temp + z3_temp +
                                            z4_temp + z5_temp) / 5 + 45.5
                """ Updates Status Label to inform calibration is done """
                self.ui.rdoutStatus.setText("Waiting for input")

    def simple_cal(self):
        """ 
        Function called when affine calibration button is called.
        Note it only chnages the flag to record the next mouse clicks
        and updates the status text label 
        """
        self.video.cal_flag = 1
        self.ui.rdoutStatus.setText("Affine Calibration: Click Point %d" %
                                    (self.video.mouse_click_id + 1))