class MainNode(): def __init__(self): rospy.init_node('default_offboard', anonymous=True) self.rate = rospy.Rate(20) self.mav1 = Mav("mavros") self._command_sub = rospy.Subscriber("pose_command", String, self._pose_command_callback) # wait for FCU connection self.mav1.wait_for_connection() print("mavs connected") self.sm = StateMachine() self.sm.set_params(self.mav1) self.sm.set_current_state(self.sm.States.TAKE_OFF) def loop(self): # enter the main loop while (True): # print "Entered whiled loop" self.sm.execute() self.rate.sleep() def _pose_command_callback(self, topic=String()): text = topic.data textarr = np.array(text.split(";")) arr = textarr.astype(np.float) print("new target pose: " + str(arr.tolist())) pose = create_setpoint_message_xyz_yaw(arr[0], arr[1], arr[2], arr[3]) self.mav1.set_target_pose(pose) self.mav1.max_speed = 1 self.sm.set_next_state(self.sm.States.IDLE) self.sm.set_current_state(self.sm.States.WAITING_TO_ARRIVE) pass
class MainNode(): def __init__(self): rospy.init_node('default_offboard', anonymous=True) self.rate = rospy.Rate(20) self.mav1 = Mav("uav1/mavros") self.mav2 = Mav("uav2/mavros") self.sm = StateMachine() self.sm.set_params((self.mav1, self.mav2)) self._command_sub = rospy.Subscriber("pose_command", String, self._pose_command_callback) # wait for FCU connection self.mav1.wait_for_connection() self.mav2.wait_for_connection() self.last_request = rospy.Time.now() - rospy.Duration(5.0) self.sm.set_current_state(self.sm.States.TAKE_OFF) def loop(self): # enter the main loop while (True): # print "Entered whiled loop" self.arm_and_set_mode() self.sm.execute() self.rate.sleep() def arm_and_set_mode(self): if rospy.Time.now() - self.last_request > rospy.Duration(1.0): if self.mav1.UAV_state.mode != "OFFBOARD": self.mav1.set_mode(0, 'OFFBOARD') print("enabling offboard mode") self.last_request = rospy.Time.now() if not self.mav1.UAV_state.armed: if self.mav1.set_arming(True): print("Vehicle armed") self.last_request = rospy.Time.now() if self.mav2.UAV_state.mode != "OFFBOARD": self.mav2.set_mode(0, 'OFFBOARD') print("enabling offboard mode") self.last_request = rospy.Time.now() if not self.mav2.UAV_state.armed: if self.mav2.set_arming(True): print("Vehicle armed") self.last_request = rospy.Time.now() def _pose_command_callback(self, topic=String()): text = topic.data textarr = np.array(text.split(";")) arr = textarr.astype(np.float) print("new target pose: " + str(arr.tolist())) pose = create_setpoint_message_xyz_yaw(arr[0], arr[1], arr[2], arr[3]) self.mav1.set_target_pose(pose) self.mav2.set_target_pose(pose) self.sm.set_next_state(self.sm.States.IDLE) self.sm.set_current_state(self.sm.States.WAITING_TO_ARRIVE) pass
class Gui(QMainWindow): """ Main GUI Class contains the main function and interfaces between the GUI and functions """ def __init__(self,parent=None): QWidget.__init__(self,parent) self.ui = Ui_MainWindow() self.ui.setupUi(self) """ Set GUI to track mouse """ QWidget.setMouseTracking(self,True) """ Dynamixel bus TODO: add other motors here as needed with their correct address""" self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE) port_num = self.dxlbus.port() base = DXL_MX(port_num, 1) shld = DXL_MX(port_num, 2) elbw = DXL_MX(port_num, 3) wrst = DXL_AX(port_num, 4) wrst2 = DXL_AX(port_num, 5) wrst3 = DXL_XL(port_num, 6) gripper = DXL_XL(port_num, 7) """Objects Using Other Classes""" self.kinect = Kinect() self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2,wrst3),gripper) self.tp = TrajectoryPlanner(self.rexarm) self.sm = StateMachine(self.rexarm, self.tp, self.kinect) """ Attach Functions to Buttons & Sliders TODO: NAME AND CONNECT BUTTONS AS NEEDED """ self.ui.btn_exec.clicked.connect(self.execute) self.ui.btn_estop.clicked.connect(self.estop) self.ui.btnUser1.setText("Calibrate") self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "calibrate")) self.ui.sldrBase.valueChanged.connect(self.sliderChange) self.ui.sldrShoulder.valueChanged.connect(self.sliderChange) self.ui.sldrElbow.valueChanged.connect(self.sliderChange) self.ui.sldrWrist.valueChanged.connect(self.sliderChange) self.ui.sldrWrist2.valueChanged.connect(self.sliderChange) self.ui.sldrWrist3.valueChanged.connect(self.sliderChange) #self.ui.sldrGrip1.valueChanged.connect(self.sliderChange) self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange) self.ui.sldrSpeed.valueChanged.connect(self.sliderChange) self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk) self.ui.rdoutStatus.setText("Waiting for input") """Team10 section for buttons""" self.ui.btnUser2.setText("teach") self.ui.btnUser2.clicked.connect(partial(self.sm.set_next_state, "teach")) self.ui.btnUser3.setText("repeat") self.ui.btnUser3.clicked.connect(partial(self.sm.set_next_state, "repeat")) self.ui.btnUser4.setText("Set ROI") self.ui.btnUser4.clicked.connect(partial(self.sm.set_next_state, "set_roi")) self.ui.btnUser5.setText("Set Exclusion") self.ui.btnUser5.clicked.connect(partial(self.sm.set_next_state, "set_exclusion")) self.ui.btnUser6.setText("Save frames") self.ui.btnUser6.clicked.connect(partial(self.sm.set_next_state, "save_frames")) self.ui.btn_task3.clicked.connect(partial(self.sm.set_next_state, "task3")) self.ui.btnUser7.setText("Click & Grab") self.ui.btnUser7.clicked.connect(partial(self.sm.set_next_state, "ClickandGrab")) """initalize manual control off""" self.ui.SliderFrame.setEnabled(False) """initalize rexarm""" self.rexarm.initialize() """Setup Threads""" self.videoThread = VideoThread(self.kinect) self.videoThread.updateFrame.connect(self.setImage) self.videoThread.start() self.logicThread = LogicThread(self.sm) self.logicThread.start() self.displayThread = DisplayThread(self.rexarm, self.sm) self.displayThread.updateJointReadout.connect(self.updateJointReadout) self.displayThread.updateEndEffectorReadout.connect(self.updateEndEffectorReadout) self.displayThread.updateStatusMessage.connect(self.updateStatusMessage) self.displayThread.start() """ Setup Timer this runs the trackMouse function every 50ms """ self._timer = QTimer(self) self._timer.timeout.connect(self.trackMouse) self._timer.start(50) """ Slots attach callback functions to signals emitted from threads""" @pyqtSlot(QImage, QImage, QImage, QImage) def setImage(self, rgb_image, depth_image, level_image, superpose_frame): if(self.ui.radioVideo.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image)) if(self.ui.radioDepth.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image)) if(self.ui.radioUsr2.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(level_image)) if(self.ui.radioUsr1.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(superpose_frame)) @pyqtSlot(list) def updateJointReadout(self, joints): self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0]*R2D))) self.ui.rdoutShoulderJC.setText(str("%+.2f" % ((joints[1]*R2D)+90.0))) self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2]*R2D))) self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3]*R2D))) self.ui.rdoutWrist2JC.setText(str("%+.2f" % (joints[4]*R2D))) self.ui.rdoutWrist3JC.setText(str("%+.2f" % (joints[4]*R2D))) if(len(joints)>5): self.ui.rdoutWrist3JC.setText(str("%+.2f" % (joints[5]*R2D))) else: self.ui.rdoutWrist3JC.setText(str("N.A.")) @pyqtSlot(list) def updateEndEffectorReadout(self, pos): self.ui.rdoutX.setText(str("%+.2f" % (pos[0]))) self.ui.rdoutY.setText(str("%+.2f" % (pos[1]))) self.ui.rdoutZ.setText(str("%+.2f" % (pos[2]))) self.ui.rdoutT.setText(str("%+.2f" % (pos[3]))) self.ui.rdoutG.setText(str("%+.2f" % (pos[4]))) self.ui.rdoutP.setText(str("%+.2f" % (pos[5]))) @pyqtSlot(str) def updateStatusMessage(self, msg): self.ui.rdoutStatus.setText(msg) """ Other callback functions attached to GUI elements""" def execute(self): #self.rexarm.set_positions() self.sm.set_next_state("execute") def estop(self): self.rexarm.estop = True self.sm.set_next_state("estop") 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.rdoutWrist2.setText(str(self.ui.sldrWrist2.value())) self.ui.rdoutWrist3.setText(str(self.ui.sldrWrist3.value())) #self.ui.rdoutGrip1.setText(str(self.ui.sldrGrip1.value())) self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%") self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%") self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value()/100.0]*self.rexarm.num_joints, update_now = False) self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value()/100.0, update_now = False) joint_positions = np.array([self.ui.sldrBase.value()*D2R, self.ui.sldrShoulder.value()*D2R, self.ui.sldrElbow.value()*D2R, self.ui.sldrWrist.value()*D2R, self.ui.sldrWrist2.value()*D2R, self.ui.sldrWrist3.value()*D2R]) self.rexarm.set_positions(joint_positions, update_now = False) #self.rexarm.gripper.set_position(self.ui.sldrGrip1.value()*D2R) def directControlChk(self, state): if state == Qt.Checked: self.sm.set_next_state("manual") self.ui.SliderFrame.setEnabled(True) else: self.sm.set_next_state("idle") self.ui.SliderFrame.setEnabled(False) def trackMouse(self): """ Mouse position presentation in GUI TODO: after implementing workspace calibration display the world coordinates the mouse points to in the RGB video image. """ x = QWidget.mapFromGlobal(self,QCursor.pos()).x() y = QWidget.mapFromGlobal(self,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 if(self.kinect.currentDepthFrame.any() != 0): z = self.kinect.currentDepthFrame[y][x] self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" % (x,y,z)) PointCameraFrm = self.kinect.ConvertImagePointToCameraFrame(np.array([x,y])) PointWorldFrm = self.kinect.ConvertCameraFrametoWorlFrame(PointCameraFrm) #self.ui.rdoutMouseWorld.setText("(-,-,-)") self.ui.rdoutMouseWorld.setText("(%.3f,%.3f,%.3f)" % (PointWorldFrm[0],PointWorldFrm[1],PointWorldFrm[2])) 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.kinect.last_click[0] = x - MIN_X self.kinect.last_click[1] = y - MIN_Y if (self.kinect.kinectCalibrated == True): self.kinect.PointCamera_last_click = self.kinect.ConvertImagePointToCameraFrame(np.array([x - MIN_X, y - MIN_Y])) self.kinect.PointWorld_last_click = self.kinect.ConvertCameraFrametoWorlFrame(self.kinect.PointCamera_last_click) for i in range(10): self.kinect.BlockCenter, self.kinect.BlockOrientation_last_click = self.kinect.SelectBlock(np.array([ self.kinect.last_click[0], self.kinect.last_click[1]])) if not np.array_equal(self.kinect.BlockCenter,np.array([0.0, 0.0, 0.0])): self.kinect.PointWorld_last_click = self.kinect.BlockCenter #print("Coodinates: "+str(self.kinect.PointWorld_last_click)) #print("Orientation: "+str(self.kinect.BlockOrientation_last_click)) break time.sleep(0.010) print("Wold point selected: "+str(self.kinect.PointWorld_last_click)) print("Orientation: " +str(self.kinect.BlockOrientation_last_click)) self.kinect.new_click = True
class Gui(QMainWindow): """ Main GUI Class contains the main function and interfaces between the GUI and functions """ def __init__(self,parent=None): QWidget.__init__(self,parent) self.ui = Ui_MainWindow() self.ui.setupUi(self) """ Set GUI to track mouse """ QWidget.setMouseTracking(self,True) """dynamixel bus -- add other motors here""" self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE) port_num = self.dxlbus.port() base = DXL_MX(port_num, 0) shld = DXL_MX(port_num, 1) elbw = DXL_MX(port_num, 2) wrst = DXL_AX(port_num, 3) wrst2 = DXL_XL(port_num, 4) # NIC 10/4 grip = DXL_XL(port_num, 5) # NIC 10/4 """Objects Using Other Classes""" self.kinect = Kinect() self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2),grip) # NIC 10/4 self.tp = TrajectoryPlanner(self.rexarm) self.sm = StateMachine(self.rexarm, self.tp, self.kinect) """ Attach Functions to Buttons & Sliders TODO: NAME AND CONNECT BUTTONS AS NEEDED """ self.ui.btn_estop.clicked.connect(self.estop) self.ui.btn_exec.clicked.connect(partial(self.sm.set_next_state,"execute_plan")) self.ui.btn_task1.clicked.connect(partial(self.sm.set_next_state,"task_1")) self.ui.btn_task2.clicked.connect(partial(self.sm.set_next_state,"task_2")) self.ui.btn_task4.clicked.connect(partial(self.sm.set_next_state,"task_5")) #self.ui.btn_exec.clicked.connect(partial(self.sm.set_next_state,"execute")) self.ui.btnUser1.setText("Calibrate") self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "calibrate")) self.ui.btnUser2.setText("Record Waypoints") self.ui.btnUser2.clicked.connect(partial(self.sm.set_next_state, "record")) self.ui.btnUser3.setText("Play") self.ui.btnUser3.clicked.connect(partial(self.sm.set_next_state, "play")) self.ui.btnUser4.setText("Open Gripper") # NIC 10/4 self.ui.btnUser4.clicked.connect(self.rexarm.open_gripper) # NIC 10/4 self.ui.btnUser5.setText("Close Gripper") # NIC 10/4 self.ui.btnUser5.clicked.connect(self.rexarm.close_gripper) # NIC 10/4 self.ui.sldrBase.valueChanged.connect(self.sliderChange) self.ui.sldrShoulder.valueChanged.connect(self.sliderChange) self.ui.sldrElbow.valueChanged.connect(self.sliderChange) self.ui.sldrWrist.valueChanged.connect(self.sliderChange) self.ui.sldrGrip1.valueChanged.connect(self.sliderChange) self.ui.sldrGrip2.valueChanged.connect(self.sliderChange) self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange) self.ui.sldrSpeed.valueChanged.connect(self.sliderChange) self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk) self.ui.rdoutStatus.setText("Waiting for input") """initalize manual control off""" self.ui.SliderFrame.setEnabled(False) """initalize rexarm""" self.rexarm.initialize() """Setup Threads""" self.videoThread = VideoThread(self.kinect) self.videoThread.updateFrame.connect(self.setImage) self.videoThread.start() self.logicThread = LogicThread(self.sm) self.logicThread.start() self.displayThread = DisplayThread(self.rexarm, self.sm) self.displayThread.updateJointReadout.connect(self.updateJointReadout) self.displayThread.updateEndEffectorReadout.connect(self.updateEndEffectorReadout) self.displayThread.updateStatusMessage.connect(self.updateStatusMessage) self.displayThread.start() """ Setup Timer this runs the trackMouse function every 50ms """ self._timer = QTimer(self) self._timer.timeout.connect(self.trackMouse) self._timer.start(50) """ Slots attach callback functions to signals emitted from threads""" @pyqtSlot(QImage, QImage) def setImage(self, rgb_image, depth_image): if(self.ui.radioVideo.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image)) if(self.ui.radioDepth.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image)) @pyqtSlot(list) def updateJointReadout(self, joints): self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0]*R2D))) self.ui.rdoutShoulderJC.setText(str("%+.2f" % ((joints[1]*R2D)+90.0))) self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2]*R2D))) self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3]*R2D))) @pyqtSlot(list) def updateEndEffectorReadout(self, pos): self.ui.rdoutX.setText(str("%+.2f" % (pos[0]))) self.ui.rdoutY.setText(str("%+.2f" % (pos[1]))) self.ui.rdoutZ.setText(str("%+.2f" % (pos[2]))) self.ui.rdoutT.setText(str("%+.2f" % (pos[3]))) @pyqtSlot(str) def updateStatusMessage(self, msg): self.ui.rdoutStatus.setText(msg) """ Other callback functions attached to GUI elements""" def estop(self): self.rexarm.estop = True self.sm.set_next_state("estop") 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())) #enter the vale for the slider :slider rdoutGrip2 and rdoutGrip2 self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%") self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%") self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value()/100.0]*self.rexarm.num_joints, update_now = False) self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value()/100.0, update_now = False) joint_positions = np.array([self.ui.sldrBase.value()*D2R, self.ui.sldrShoulder.value()*D2R, self.ui.sldrElbow.value()*D2R, self.ui.sldrWrist.value()*D2R, self.ui.sldrGrip1.value()*D2R, self.ui.sldrGrip2.value()*D2R]) self.rexarm.set_positions(joint_positions, update_now = False) def directControlChk(self, state): if state == Qt.Checked: self.sm.set_next_state("manual") self.ui.SliderFrame.setEnabled(True) else: self.sm.set_next_state("idle") self.ui.SliderFrame.setEnabled(False) def trackMouse(self): """ Mouse position presentation in GUI TODO: after implementing workspace calibration display the world coordinates the mouse points to in the RGB video image. """ x = QWidget.mapFromGlobal(self,QCursor.pos()).x() y = QWidget.mapFromGlobal(self,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 if(self.kinect.currentDepthFrame.any() != 0): z = self.kinect.currentDepthFrame[y][x] self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" % (x,y,z)) if (self.kinect.kinectCalibrated): zw = self.kinect.depthcalibration(z) xwyw = self.kinect.pixeltoworldcoordinates(np.array([x,y,1]), z) self.ui.rdoutMouseWorld.setText("(%.0f,%.0f,%.0f)" % (xwyw[0],xwyw[1],zw)) 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.kinect.last_click[0] = x - MIN_X self.kinect.last_click[1] = y - MIN_Y self.kinect.new_click = True
class Gui(QMainWindow): """! Main GUI Class Contains the main function and interfaces between the GUI and functions. """ def __init__(self, parent=None): QWidget.__init__(self, parent) self.ui = Ui_MainWindow() self.ui.setupUi(self) """ Groups of ui commonents """ self.error_lcds = [ self.ui.rdoutBaseErrors, self.ui.rdoutShoulderErrors, self.ui.rdoutElbowErrors, self.ui.rdoutWristErrors, self.ui.rdoutWrist2Errors, self.ui.rdoutWrist3Errors ] self.joint_readouts = [ self.ui.rdoutBaseJC, self.ui.rdoutShoulderJC, self.ui.rdoutElbowJC, self.ui.rdoutWristJC, self.ui.rdoutWrist2JC, self.ui.rdoutWrist3JC ] self.joint_slider_rdouts = [ self.ui.rdoutBase, self.ui.rdoutShoulder, self.ui.rdoutElbow, self.ui.rdoutWrist, self.ui.rdoutWrist2, self.ui.rdoutWrist3 ] self.joint_sliders = [ self.ui.sldrBase, self.ui.sldrShoulder, self.ui.sldrElbow, self.ui.sldrWrist, self.ui.sldrWrist2, self.ui.sldrWrist3 ] """Objects Using Other Classes""" self.kinect = Kinect() self.rexarm = Rexarm() self.tp = TrajectoryPlanner(self.rexarm) self.sm = StateMachine(self.rexarm, self.tp, self.kinect) self.sm.is_logging = False """ Attach Functions to Buttons & Sliders TODO: NAME AND CONNECT BUTTONS AS NEEDED """ # Video self.ui.videoDisplay.setMouseTracking(True) self.ui.videoDisplay.mouseMoveEvent = self.trackMouse self.ui.videoDisplay.mousePressEvent = self.calibrateMousePress # Buttons # Handy lambda function that can be used with Partial to only set the new state if the rexarm is initialized nxt_if_arm_init = lambda next_state: self.sm.set_next_state( next_state if self.rexarm.initialized else None) self.ui.btn_estop.clicked.connect(self.estop) self.ui.btn_init_arm.clicked.connect(self.initRexarm) self.ui.btnUser1.setText("Calibrate") self.ui.btnUser1.clicked.connect(partial(nxt_if_arm_init, 'calibrate')) ##OUR_CODE self.ui.btn_exec.clicked.connect(self.execute) self.ui.btnUser2.clicked.connect(self.record) self.ui.btnUser3.clicked.connect(self.playback) self.ui.btnUser4.clicked.connect(self.execute_tp) self.ui.btnUser5.clicked.connect(self.toggle_logging) self.ui.btnUser1.clicked.connect(self.calibrate) self.ui.btnUser6.clicked.connect(self.blockDetect) self.ui.btnUser7.clicked.connect(self.openGripper) self.ui.btnUser8.clicked.connect(self.closeGripper) self.ui.btnUser9.clicked.connect(self.clickGrab) # Sliders for sldr in self.joint_sliders: sldr.valueChanged.connect(self.sliderChange) self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange) self.ui.sldrSpeed.valueChanged.connect(self.sliderChange) # Direct Control self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk) # Status self.ui.rdoutStatus.setText("Waiting for input") # Auto exposure self.ui.chkAutoExposure.stateChanged.connect(self.autoExposureChk) """initalize manual control off""" self.ui.SliderFrame.setEnabled(False) """Setup Threads""" # Rexarm runs its own thread # Video self.videoThread = VideoThread(self.kinect) self.videoThread.updateFrame.connect(self.setImage) self.videoThread.start() # State machine self.logicThread = LogicThread(self.sm) self.logicThread.start() # Display self.displayThread = DisplayThread(self.rexarm, self.sm) self.displayThread.updateJointReadout.connect(self.updateJointReadout) self.displayThread.updateEndEffectorReadout.connect( self.updateEndEffectorReadout) self.displayThread.updateStatusMessage.connect( self.updateStatusMessage) self.displayThread.updateJointErrors.connect(self.updateJointErrors) self.displayThread.start() """ Slots attach callback functions to signals emitted from threads""" @pyqtSlot(QImage, QImage) def setImage(self, rgb_image, depth_image): """! @brief Display the images from the kinect. @param rgb_image The rgb image @param depth_image The depth image """ if (self.ui.radioVideo.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image)) if (self.ui.radioDepth.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image)) @pyqtSlot(list) def updateJointReadout(self, joints): for rdout, joint in zip(self.joint_readouts, joints): rdout.setText(str('%+.2f' % (joint * R2D))) @pyqtSlot(list) def updateEndEffectorReadout(self, pos): self.ui.rdoutX.setText(str("%+.2f" % (pos[0]))) self.ui.rdoutY.setText(str("%+.2f" % (pos[1]))) self.ui.rdoutZ.setText(str("%+.2f" % (pos[2]))) self.ui.rdoutT.setText(str("%+.2f" % (pos[3]))) self.ui.rdoutG.setText(str("%+.2f" % (pos[4]))) self.ui.rdoutP.setText(str("%+.2f" % (pos[5]))) @pyqtSlot(list) def updateJointErrors(self, errors): for lcd, error in zip(self.error_lcds, errors): lcd.display(error) @pyqtSlot(str) def updateStatusMessage(self, msg): self.ui.rdoutStatus.setText(msg) """ Other callback functions attached to GUI elements""" def estop(self): self.sm.set_next_state("estop") def execute(self): self.sm.set_next_state("execute") def record(self): self.sm.set_next_state("record") def playback(self): self.sm.set_next_state("playback") def execute_tp(self): self.sm.set_next_state("execute_tp") def calibrate(self): self.sm.set_next_state("calibrate") def blockDetect(self): self.kinect.blockDetector() def openGripper(self): self.rexarm.open_gripper() def closeGripper(self): self.rexarm.close_gripper() def clickGrab(self): self.sm.set_next_state("clickGrab") def toggle_logging(self): if not self.sm.is_logging: # with open('log_data.csv', 'a') as log_file: self.rexarm.log_file = open('log_data.csv', 'a') self.rexarm.csv_writer = csv.writer(self.rexarm.log_file, delimiter=',') self.sm.is_logging = True else: self.rexarm.log_file.close() self.sm.is_logging = False def sliderChange(self): """! @brief Slider changed Function to change the slider labels when sliders are moved and to command the arm to the given position """ for rdout, sldr in zip(self.joint_slider_rdouts, self.joint_sliders): rdout.setText(str(sldr.value())) self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%") self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%") # Do nothing if the rexarm is not initialized if self.rexarm.initialized: self.rexarm.set_torque_limits( [self.ui.sldrMaxTorque.value() / 100.0] * self.rexarm.num_joints) self.rexarm.set_speeds_normalized_all(self.ui.sldrSpeed.value() / 100.0) joint_positions = np.array( [sldr.value() * D2R for sldr in self.joint_sliders]) # Only send the joints that the rexarm has self.rexarm.set_positions( joint_positions[0:self.rexarm.num_joints]) def directControlChk(self, state): """! @brief Changes to direct control mode Will only work if the rexarm is initialized. @param state State of the checkbox """ if state == Qt.Checked and self.rexarm.initialized: # Go to manual and enable sliders self.sm.set_next_state("manual") self.ui.SliderFrame.setEnabled(True) else: # Lock sliders and go to idle self.sm.set_next_state("idle") self.ui.SliderFrame.setEnabled(False) self.ui.chk_directcontrol.setChecked(False) def autoExposureChk(self, state): """! @brief Sets the Kinect auto exposer @param state State of the checkbox """ if state == Qt.Checked and self.kinect.kinectConnected == True: self.kinect.toggleExposure(True) else: self.kinect.toggleExposure(False) def trackMouse(self, mouse_event): """! @brief Show the mouse position in GUI TODO: after implementing workspace calibration display the world coordinates the mouse points to in the RGB video image. @param mouse_event QtMouseEvent containing the pose of the mouse at the time of the event not current time """ # if self.kinect.DepthFrameRaw.any() != 0: # self.ui.rdoutMousePixels.setText("(-,-,-)") # self.ui.rdoutMouseWorld.setText("(-,-,-)") if self.kinect.cameraCalibrated: pixel = np.array([mouse_event.y(), mouse_event.x()]) # cameraCoord = self.kinect.pixel2Camera(pixel) worldCoord = self.kinect.getWorldCoord(pixel) self.kinect.worldCoords = worldCoord # print(worldCoord) self.ui.rdoutMousePixels.setText(np.array2string(pixel)) # self.ui.rdoutMouseWorld.setText(np.array2string((worldCoord * 100).astype(int))) self.ui.rdoutMouseWorld.setText(np.array2string((worldCoord))) def calibrateMousePress(self, mouse_event): """! @brief Record mouse click positions for calibration @param mouse_event QtMouseEvent containing the pose of the mouse at the time of the event not current time """ """ Get mouse posiiton """ pt = mouse_event.pos() self.kinect.last_click[0] = pt.x() self.kinect.last_click[1] = pt.y() self.kinect.new_click = True # print(self.kinect.last_click) def initRexarm(self): """! @brief Initializes the rexarm. """ self.sm.set_next_state('initialize_rexarm') self.ui.SliderFrame.setEnabled(False) self.ui.chk_directcontrol.setChecked(False)
class Gui(QMainWindow): """! Main GUI Class Contains the main function and interfaces between the GUI and functions. """ def __init__(self, parent=None, dh_config_file=None): QWidget.__init__(self, parent) self.ui = Ui_MainWindow() self.ui.setupUi(self) """ Groups of ui commonents """ self.joint_readouts = [ self.ui.rdoutBaseJC, self.ui.rdoutShoulderJC, self.ui.rdoutElbowJC, self.ui.rdoutWristAJC, self.ui.rdoutWristRJC, ] self.joint_slider_rdouts = [ self.ui.rdoutBase, self.ui.rdoutShoulder, self.ui.rdoutElbow, self.ui.rdoutWristA, self.ui.rdoutWristR, ] self.joint_sliders = [ self.ui.sldrBase, self.ui.sldrShoulder, self.ui.sldrElbow, self.ui.sldrWristA, self.ui.sldrWristR, ] """Objects Using Other Classes""" self.camera = Camera() print("Creating rx arm...") if (dh_config_file is not None): self.rxarm = RXArm(dh_config_file=dh_config_file) else: self.rxarm = RXArm() print("Done creating rx arm instance.") self.sm = StateMachine(self.rxarm, self.camera) """ Attach Functions to Buttons & Sliders TODO: NAME AND CONNECT BUTTONS AS NEEDED """ # Video self.ui.videoDisplay.setMouseTracking(True) self.ui.videoDisplay.mouseMoveEvent = self.trackMouse self.ui.videoDisplay.mousePressEvent = self.calibrateMousePress # Buttons # Handy lambda function that can be used with Partial to only set the new state if the rxarm is initialized #nxt_if_arm_init = lambda next_state: self.sm.set_next_state(next_state if self.rxarm.initialized else None) nxt_if_arm_init = lambda next_state: self.sm.set_next_state(next_state) self.ui.btn_estop.clicked.connect(self.estop) self.ui.btn_init_arm.clicked.connect(self.initRxarm) self.ui.btn_torq_off.clicked.connect( lambda: self.rxarm.disable_torque()) self.ui.btn_torq_on.clicked.connect(lambda: self.rxarm.enable_torque()) self.ui.btn_sleep_arm.clicked.connect(lambda: self.rxarm.sleep()) #User Buttons self.ui.btnUser1.setText("Calibrate") self.ui.btnUser1.clicked.connect(partial(nxt_if_arm_init, 'calibrate')) self.ui.btnUser2.setText('Open Gripper') self.ui.btnUser2.clicked.connect(lambda: self.rxarm.open_gripper()) self.ui.btnUser3.setText('Close Gripper') self.ui.btnUser3.clicked.connect(lambda: self.rxarm.close_gripper()) self.ui.btnUser4.setText('Execute') self.ui.btnUser4.clicked.connect(partial(nxt_if_arm_init, 'execute')) # Sliders for sldr in self.joint_sliders: sldr.valueChanged.connect(self.sliderChange) self.ui.sldrMoveTime.valueChanged.connect(self.sliderChange) self.ui.sldrAccelTime.valueChanged.connect(self.sliderChange) # Direct Control self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk) # Status self.ui.rdoutStatus.setText("Waiting for input") """initalize manual control off""" self.ui.SliderFrame.setEnabled(False) """Setup Threads""" # State machine self.StateMachineThread = StateMachineThread(self.sm) self.StateMachineThread.updateStatusMessage.connect( self.updateStatusMessage) self.StateMachineThread.start() self.VideoThread = VideoThread(self.camera) self.VideoThread.updateFrame.connect(self.setImage) self.VideoThread.start() self.ArmThread = RXArmThread(self.rxarm) self.ArmThread.updateJointReadout.connect(self.updateJointReadout) self.ArmThread.updateEndEffectorReadout.connect( self.updateEndEffectorReadout) self.ArmThread.start() """ Slots attach callback functions to signals emitted from threads""" @pyqtSlot(str) def updateStatusMessage(self, msg): self.ui.rdoutStatus.setText(msg) @pyqtSlot(list) def updateJointReadout(self, joints): for rdout, joint in zip(self.joint_readouts, joints): rdout.setText(str('%+.2f' % (joint * R2D))) ### TODO: output the rest of the orientation according to the convention chosen @pyqtSlot(list) def updateEndEffectorReadout(self, pos): self.ui.rdoutX.setText(str("%+.2f mm" % (1000 * pos[0]))) self.ui.rdoutY.setText(str("%+.2f mm" % (1000 * pos[1]))) self.ui.rdoutZ.setText(str("%+.2f mm" % (1000 * pos[2]))) self.ui.rdoutPhi.setText(str("%+.2f rad" % (pos[3]))) #self.ui.rdoutTheta.setText(str("%+.2f" % (pos[4]))) #self.ui.rdoutPsi.setText(str("%+.2f" % (pos[5]))) @pyqtSlot(QImage, QImage) def setImage(self, rgb_image, depth_image): """! @brief Display the images from the camera. @param rgb_image The rgb image @param depth_image The depth image """ if (self.ui.radioVideo.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image)) if (self.ui.radioDepth.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image)) """ Other callback functions attached to GUI elements""" def estop(self): self.rxarm.disable_torque() self.sm.set_next_state('estop') def sliderChange(self): """! @brief Slider changed Function to change the slider labels when sliders are moved and to command the arm to the given position """ for rdout, sldr in zip(self.joint_slider_rdouts, self.joint_sliders): rdout.setText(str(sldr.value())) self.ui.rdoutMoveTime.setText( str(self.ui.sldrMoveTime.value() / 10.0) + "s") self.ui.rdoutAccelTime.setText( str(self.ui.sldrAccelTime.value() / 20.0) + "s") self.rxarm.set_moving_time(self.ui.sldrMoveTime.value() / 10.0) self.rxarm.set_accel_time(self.ui.sldrAccelTime.value() / 20.0) # Do nothing if the rxarm is not initialized if self.rxarm.initialized: joint_positions = np.array( [sldr.value() * D2R for sldr in self.joint_sliders]) # Only send the joints that the rxarm has self.rxarm.set_positions(joint_positions[0:self.rxarm.num_joints]) def directControlChk(self, state): """! @brief Changes to direct control mode Will only work if the rxarm is initialized. @param state State of the checkbox """ if state == Qt.Checked and self.rxarm.initialized: # Go to manual and enable sliders self.sm.set_next_state("manual") self.ui.SliderFrame.setEnabled(True) else: # Lock sliders and go to idle self.sm.set_next_state("idle") self.ui.SliderFrame.setEnabled(False) self.ui.chk_directcontrol.setChecked(False) def trackMouse(self, mouse_event): """! @brief Show the mouse position in GUI TODO: after implementing workspace calibration display the world coordinates the mouse points to in the RGB video image. @param mouse_event QtMouseEvent containing the pose of the mouse at the time of the event not current time """ pt = mouse_event.pos() if self.camera.DepthFrameRaw.any() != 0: z = self.camera.DepthFrameRaw[pt.y()][pt.x()] self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" % (pt.x(), pt.y(), z)) self.ui.rdoutMouseWorld.setText("(-,-,-)") def calibrateMousePress(self, mouse_event): """! @brief Record mouse click positions for calibration @param mouse_event QtMouseEvent containing the pose of the mouse at the time of the event not current time """ """ Get mouse posiiton """ pt = mouse_event.pos() self.camera.last_click[0] = pt.x() self.camera.last_click[1] = pt.y() self.camera.new_click = True # print(self.camera.last_click) def initRxarm(self): """! @brief Initializes the rxarm. """ self.ui.SliderFrame.setEnabled(False) self.ui.chk_directcontrol.setChecked(False) self.rxarm.enable_torque() self.sm.set_next_state('initialize_rxarm')
class Gui(QMainWindow): """ Main GUI Class contains the main function and interfaces between the GUI and functions """ def __init__(self, parent=None): QWidget.__init__(self, parent) self.ui = Ui_MainWindow() self.ui.setupUi(self) """ Set GUI to track mouse """ QWidget.setMouseTracking(self, True) """dynamixel bus -- add other motors here""" self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE) port_num = self.dxlbus.port() base = DXL_MX(port_num, 0) shld = DXL_MX(port_num, 1) elbw = DXL_MX(port_num, 2) wrst = DXL_AX(port_num, 3) wrst2 = DXL_XL(port_num, 4) grip = DXL_XL(port_num, 5) """Objects Using Other Classes""" self.rexarm = Rexarm((base, shld, elbw, wrst, wrst2), grip) self.kinect = Kinect(self.rexarm) self.tp = TrajectoryPlanner(self.rexarm) self.sm = StateMachine(self.rexarm, self.tp, self.kinect) """ Attach Functions to Buttons & Sliders TODO: NAME AND CONNECT BUTTONS AS NEEDED """ self.ui.btn_estop.clicked.connect(self.estop) self.ui.btnUser1.setText("Calibrate") self.ui.btnUser1.clicked.connect( partial(self.sm.set_next_state, "calibrate")) self.ui.sldrBase.valueChanged.connect(self.sliderChange) self.ui.sldrShoulder.valueChanged.connect(self.sliderChange) self.ui.sldrElbow.valueChanged.connect(self.sliderChange) self.ui.sldrWrist.valueChanged.connect(self.sliderChange) ### sliders gripper self.ui.sldrGrip1.valueChanged.connect(self.sliderChange) self.ui.sldrGrip2.valueChanged.connect(self.sliderChange) ### button gripper open & close self.ui.btnUser5.clicked.connect(self.btn2clicked) self.ui.btnUser6.clicked.connect(self.btn1clicked) self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange) self.ui.sldrSpeed.valueChanged.connect(self.sliderChange) self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk) self.ui.rdoutStatus.setText("Waiting for input") self.ui.btn_exec.clicked.connect( partial(self.sm.set_next_state, "execute")) self.ui.btnUser2.setText("Record wp") self.ui.btnUser3.setText("Clear wp") self.ui.btnUser4.setText("Click and pick") self.ui.btnUser2.clicked.connect( partial(self.sm.set_next_state, "add_wp")) self.ui.btnUser3.clicked.connect( partial(self.sm.set_next_state, "clear_wp")) self.ui.btnUser4.clicked.connect( partial(self.sm.set_next_state, "click_and_pick")) self.ui.btnUser7.setText("Save Calibration Points") self.ui.btnUser8.setText("Load Previous Calibration") self.ui.btnUser7.clicked.connect( partial(self.sm.set_next_state, "save_calibration_points")) self.ui.btnUser8.clicked.connect( partial(self.sm.set_next_state, "load_previous_calibration")) self.ui.btnUser9.setText("Record Block Position") self.ui.btnUser9.clicked.connect( partial(self.sm.set_next_state, "record_block_position")) self.ui.btnUser5.setText("Open Gripper") self.ui.btnUser6.setText("Close Gripper") self.ui.btn_task1.clicked.connect( partial(self.sm.set_next_state, "mirror")) self.ui.btn_task2.clicked.connect( partial(self.sm.set_next_state, "stack_3")) self.ui.btn_task3.clicked.connect( partial(self.sm.set_next_state, "line_em_up")) self.ui.btn_task4.clicked.connect( partial(self.sm.set_next_state, "stack_em_high")) self.ui.btn_exec_6.clicked.connect( partial(self.sm.set_next_state, "pyramid5")) """initalize manual control off""" self.ui.SliderFrame.setEnabled(False) """initalize rexarm""" self.rexarm.initialize() """Setup Threads""" self.videoThread = VideoThread(self.kinect) self.videoThread.updateFrame.connect(self.setImage) self.videoThread.start() self.logicThread = LogicThread(self.sm) self.logicThread.start() self.displayThread = DisplayThread(self.rexarm, self.sm) self.displayThread.updateJointReadout.connect(self.updateJointReadout) self.displayThread.updateEndEffectorReadout.connect( self.updateEndEffectorReadout) self.displayThread.updateStatusMessage.connect( self.updateStatusMessage) self.displayThread.start() """ Setup Timer this runs the trackMouse function every 50ms """ self._timer = QTimer(self) self._timer.timeout.connect(self.trackMouse) self._timer.start(50) """ Slots attach callback functions to signals emitted from threads""" @pyqtSlot(QImage, QImage, QImage) def setImage(self, rgb_image, depth_image, detect_image): if (self.ui.radioVideo.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image)) if (self.ui.radioDepth.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image)) if (self.ui.radioDetect.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(detect_image)) @pyqtSlot(list) def updateJointReadout(self, joints): self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0] * R2D))) self.ui.rdoutShoulderJC.setText( str("%+.2f" % ((joints[1] * R2D) + 90.0))) self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2] * R2D))) self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3] * R2D))) ### #self.ui.rdoutGrip1.setText(str("%+.2f" % (joints[4]*R2D))) #self.ui.rdoutGrip2.setText(str("%+.2f" % (joints[5]*R2D))) ### @pyqtSlot(list) def updateEndEffectorReadout(self, pos): self.ui.rdoutX.setText(str("%+.2f" % (pos[0]))) self.ui.rdoutY.setText(str("%+.2f" % (pos[1]))) self.ui.rdoutZ.setText(str("%+.2f" % (pos[2]))) self.ui.rdoutT.setText(str("%+.2f" % (pos[3]))) @pyqtSlot(str) def updateStatusMessage(self, msg): self.ui.rdoutStatus.setText(msg) """ Other callback functions attached to GUI elements""" def estop(self): self.rexarm.estop = True self.sm.set_next_state("estop") 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.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value() / 100.0] * self.rexarm.num_joints, update_now=False) self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value() / 100.0, update_now=False) ### joint_positions = np.array([ self.ui.sldrBase.value() * D2R, self.ui.sldrShoulder.value() * D2R, self.ui.sldrElbow.value() * D2R, self.ui.sldrWrist.value() * D2R, (self.ui.sldrGrip1.value()) * D2R ]) self.rexarm.set_positions(joint_positions, update_now=False) ### def btn1clicked(self): self.rexarm.close_gripper() def btn2clicked(self): self.rexarm.open_gripper() def btn3clicked(self): initdeg = -39.5 self.rexarm.set_gripper_rotate(initdeg) def directControlChk(self, state): if state == Qt.Checked: self.sm.set_next_state("manual") self.ui.SliderFrame.setEnabled(True) else: self.sm.set_next_state("idle") self.ui.SliderFrame.setEnabled(False) def trackMouse(self): """ Mouse position presentation in GUI TODO: after implementing workspace calibration display the world coordinates the mouse points to in the RGB video image. """ x = QWidget.mapFromGlobal(self, QCursor.pos()).x() y = QWidget.mapFromGlobal(self, 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 """color calibration""" #color = self.kinect.colorDetector(x,y) #self.kinect.colorCalibration(x,y) #self.kinect.block_detection_verification(x,y) # map real world real_x = self.kinect.real_coord[x][y][0] real_y = self.kinect.real_coord[x][y][1] if (self.kinect.currentDepthFrame.any() != 0): z = self.kinect.currentDepthFrame[y][x] real_z = self.kinect.convertDepthtomm(z) self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" % (x, y, z)) if self.kinect.kinectCalibrated == True: self.ui.rdoutMouseWorld.setText("(%.0f,%.0f,%.0f)" % (real_x, real_y, real_z)) else: self.ui.rdoutMouseWorld.setText("(-,-,-)") 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.kinect.last_click[0] = x - MIN_X self.kinect.last_click[1] = y - MIN_Y self.kinect.new_click = True #print(self.kinect.last_click) def shutdown(self): self.rexarm.shutdown() def closeEvent(self, *args, **kwargs): super(QMainWindow, self).closeEvent(*args, **kwargs) print("EXITING...") self.shutdown()
class Gui(QMainWindow): """ Main GUI Class contains the main function and interfaces between the GUI and functions """ def __init__(self, parent=None): QWidget.__init__(self, parent) self.ui = Ui_MainWindow() self.ui.setupUi(self) """ Set GUI to track mouse """ QWidget.setMouseTracking(self, True) """ Dynamixel bus TODO: add other motors here as needed with their correct address""" self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE) port_num = self.dxlbus.port() base = DXL_MX(port_num, 1) shld = DXL_MX(port_num, 2) elbw = DXL_MX(port_num, 3) wrst = DXL_AX(port_num, 4) wrst2 = DXL_AX(port_num, 5) #wrst3 = DXL_XL(port_num, 6) #grip = DXL_XL(port_num, 7) #wrst2.set_compliance(8,64) """Objects Using Other Classes""" self.kinect = Kinect() self.rexarm = Rexarm((base, shld, elbw, wrst, wrst2), 0) self.tp = TrajectoryPlanner(self.rexarm) self.sm = StateMachine(self.rexarm, self.tp, self.kinect) """ Attach Functions to Buttons & Sliders TODO: NAME AND CONNECT BUTTONS AS NEEDED """ self.ui.btn_estop.clicked.connect(self.estop) self.ui.btn_exec.clicked.connect(self.execute) self.ui.btn_task1.clicked.connect(self.record) self.ui.btn_task2.clicked.connect(self.clear_waypoints) self.ui.btn_task3.clicked.connect(self.toggle_gripper) self.ui.btnUser1.setText("Calibrate") self.ui.btnUser1.clicked.connect( partial(self.sm.set_next_state, "calibrate")) self.ui.btnUser2.setText("Block Detector") self.ui.btnUser2.clicked.connect( partial(self.sm.set_next_state, "block detection")) self.ui.btnUser2.setText("Color Buckets") self.ui.btnUser2.clicked.connect( partial(self.sm.set_next_state, "color buckets")) self.ui.btnUser3.setText("Click Grab/Drop Mode") self.ui.btnUser3.clicked.connect( partial(self.sm.set_next_state, "click grab drop")) self.ui.btnUser4.setText("Pick n' Stack") self.ui.btnUser4.clicked.connect( partial(self.sm.set_next_state, "pick and stack")) self.ui.btnUser5.setText("Line 'em up") self.ui.btnUser5.clicked.connect( partial(self.sm.set_next_state, "line them up")) self.ui.btnUser6.setText("Stack 'em high") self.ui.btnUser6.clicked.connect( partial(self.sm.set_next_state, "stack them high")) self.ui.btnUser7.setText("Block slider") self.ui.btnUser7.clicked.connect( partial(self.sm.set_next_state, "block slider")) self.ui.btnUser8.setText("Hot swap") self.ui.btnUser8.clicked.connect( partial(self.sm.set_next_state, "hot swap")) self.ui.btnUser9.setText("Calibration Accuracy") self.ui.btnUser9.clicked.connect( partial(self.sm.set_next_state, "Calibration Accuracy")) self.ui.sldrBase.valueChanged.connect(self.sliderChange) self.ui.sldrShoulder.valueChanged.connect(self.sliderChange) self.ui.sldrElbow.valueChanged.connect(self.sliderChange) self.ui.sldrWrist.valueChanged.connect(self.sliderChange) self.ui.sldrWrist2.valueChanged.connect(self.sliderChange) self.ui.sldrWrist3.valueChanged.connect(self.sliderChange) self.ui.sldrGrip1.valueChanged.connect(self.sliderChange) self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange) self.ui.sldrSpeed.valueChanged.connect(self.sliderChange) self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk) self.ui.rdoutStatus.setText("Waiting for input") """initalize manual control off""" self.ui.SliderFrame.setEnabled(False) """initalize rexarm""" self.rexarm.initialize() """Setup Threads""" self.videoThread = VideoThread(self.kinect) self.videoThread.updateFrame.connect(self.setImage) self.videoThread.start() self.logicThread = LogicThread(self.sm) self.logicThread.start() self.displayThread = DisplayThread(self.rexarm, self.sm) self.displayThread.updateJointReadout.connect(self.updateJointReadout) self.displayThread.updateEndEffectorReadout.connect( self.updateEndEffectorReadout) self.displayThread.updateStatusMessage.connect( self.updateStatusMessage) self.displayThread.start() """ Setup Timer this runs the trackMouse function every 50ms """ self._timer = QTimer(self) self._timer.timeout.connect(self.trackMouse) self._timer.start(50) """ Slots attach callback functions to signals emitted from threads""" @pyqtSlot(QImage, QImage, QImage, QImage, QImage, QImage) def setImage(self, rgb_image, depth_image, blocks_depth_image, h_image, s_image, v_image): if (self.ui.radioVideo.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image)) if (self.ui.radioDepth.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image)) if (self.ui.radioUsr1.isChecked()): self.ui.videoDisplay.setPixmap( QPixmap.fromImage(blocks_depth_image)) if (self.ui.radioUsr2.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(h_image)) if (self.ui.radioUsr3.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(s_image)) if (self.ui.radioUsr4.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(v_image)) @pyqtSlot(list) def updateJointReadout(self, joints): self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0] * R2D))) self.ui.rdoutShoulderJC.setText( str("%+.2f" % ((joints[1] * R2D) + 90.0))) self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2] * R2D))) self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3] * R2D))) self.ui.rdoutWrist2JC.setText(str("%+.2f" % (joints[4] * R2D))) if (len(joints) > 5): self.ui.rdoutWrist3JC.setText(str("%+.2f" % (joints[5] * R2D))) else: self.ui.rdoutWrist3JC.setText(str("N.A.")) @pyqtSlot(list) def updateEndEffectorReadout(self, pos): self.ui.rdoutX.setText(str("%+.2f" % (pos[0]))) self.ui.rdoutY.setText(str("%+.2f" % (pos[1]))) self.ui.rdoutZ.setText(str("%+.2f" % (pos[2]))) self.ui.rdoutT.setText(str("%+.2f" % (pos[3]))) self.ui.rdoutG.setText(str("%+.2f" % (pos[4]))) self.ui.rdoutP.setText(str("%+.2f" % (pos[5]))) @pyqtSlot(str) def updateStatusMessage(self, msg): self.ui.rdoutStatus.setText(msg) """ Other callback functions attached to GUI elements""" def estop(self): self.rexarm.estop = True self.sm.set_next_state("estop") def execute(self): self.sm.set_next_state("execute") self.ui.sldrMaxTorque.setValue(50) def toggle_gripper(self): self.rexarm.toggle_gripper() #self.rexarm.pause(1) def record(self): self.sm.set_next_state("record") def clear_waypoints(self): self.sm.waypoints = [] 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.rdoutWrist2.setText(str(self.ui.sldrWrist2.value())) self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%") self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%") self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value() / 100.0] * self.rexarm.num_joints, update_now=False) self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value() / 100.0, update_now=False) joint_positions = np.array([ self.ui.sldrBase.value() * D2R, self.ui.sldrShoulder.value() * D2R, self.ui.sldrElbow.value() * D2R, self.ui.sldrWrist.value() * D2R, self.ui.sldrWrist2.value() * D2R, self.ui.sldrWrist3.value() * D2R ]) self.rexarm.set_positions(joint_positions, update_now=False) def directControlChk(self, state): if state == Qt.Checked: self.sm.set_next_state("manual") self.ui.SliderFrame.setEnabled(True) self.ui.sldrMaxTorque.setValue(0) else: self.sm.set_next_state("idle") self.ui.SliderFrame.setEnabled(False) self.ui.sldrMaxTorque.setValue(50) def trackMouse(self): """ Mouse position presentation in GUI TODO: after implementing workspace calibration display the world coordinates the mouse points to in the RGB video image. """ x = QWidget.mapFromGlobal(self, QCursor.pos()).x() y = QWidget.mapFromGlobal(self, 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 if (self.kinect.currentDepthFrame.any() != 0): z = self.kinect.currentDepthFrame[y][x] self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" % (x, y, z)) if (self.kinect.kinectCalibrated): z_w = .1236 * np.tan(z / 2842.5 + 1.1863) - 0.94 xy_world = self.sm.worldCoordinates(x, y) self.ui.rdoutMouseWorld.setText( "(%.3f,%.3f,%.3f)" % (xy_world[0], xy_world[1], xy_world[2])) def mousePressEvent(self, QMouseEvent): """ Function used to record mouse click positions for calibration """ """ Get mouse posiiton """ #print("mouse event") 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 if self.sm.current_state == "click grab drop": if not self.sm.grab_position: self.sm.grab_position = (x, y) print("Have grab position") return if self.sm.grab_position: self.sm.drop_position = (x, y) print("Have drop position") if self.sm.current_state == "calibrate": """ Change coordinates to image axis """ self.kinect.last_click[0] = x - MIN_X self.kinect.last_click[1] = y - MIN_Y self.kinect.new_click = True if self.sm.current_state == "Calibration Accuracy": self.sm.calibrateaccuracypt = (x, y)
class Gui(QMainWindow): """ Main GUI Class contains the main function and interfaces between the GUI and functions """ def __init__(self, parent=None): QWidget.__init__(self, parent) self.ui = Ui_MainWindow() self.ui.setupUi(self) """ Set GUI to track mouse """ QWidget.setMouseTracking(self, True) """ Dynamixel bus TODO: add other motors here as needed with their correct address""" self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE) port_num = self.dxlbus.port() base = DXL_MX(port_num, 1) shld = DXL_MX(port_num, 2) elbw = DXL_MX(port_num, 3) wrst = DXL_AX(port_num, 4) wrst2 = DXL_AX(port_num, 5) gripper1 = DXL_XL(port_num, 6) gripper2 = DXL_XL(port_num, 7) """Objects Using Other Classes""" self.kinect = Kinect() self.rexarm = Rexarm( (base, shld, elbw, wrst, wrst2, gripper1, gripper2), 0) self.tp = TrajectoryPlanner(self.rexarm) self.sm = StateMachine(self.rexarm, self.tp, self.kinect) """ Attach Functions to Buttons & Sliders TODO: NAME AND CONNECT BUTTONS AS NEEDED """ self.ui.btn_estop.clicked.connect(self.estop) self.ui.btn_exec.clicked.connect(self.execute) self.ui.btn_teach.clicked.connect(self.teach) self.ui.btn_repeat.clicked.connect(self.repeat) self.ui.btn_rls.clicked.connect(self.rls) self.ui.btn_grab.clicked.connect(self.grab) self.ui.btn_drop_off.clicked.connect(self.drop_off) self.ui.btn_move.clicked.connect(self.move) self.ui.btnUser1.setText("Calibrate") self.ui.btnUser1.clicked.connect( partial(self.sm.set_next_state, "calibrate")) self.ui.btnUser2.clicked.connect( partial(self.sm.set_next_state, "event_1")) self.ui.btnUser3.clicked.connect( partial(self.sm.set_next_state, "event_2")) self.ui.btnUser4.clicked.connect( partial(self.sm.set_next_state, "event_3")) self.ui.btnUser5.clicked.connect( partial(self.sm.set_next_state, "event_4")) self.ui.btnUser6.clicked.connect( partial(self.sm.set_next_state, "event_5")) self.ui.sldrBase.valueChanged.connect(self.sliderChange) self.ui.sldrShoulder.valueChanged.connect(self.sliderChange) self.ui.sldrElbow.valueChanged.connect(self.sliderChange) self.ui.sldrWrist.valueChanged.connect(self.sliderChange) self.ui.sldrWrist2.valueChanged.connect(self.sliderChange) self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange) self.ui.sldrSpeed.valueChanged.connect(self.sliderChange) self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk) self.ui.rdoutStatus.setText("Waiting for input") """initalize manual control off""" self.ui.SliderFrame.setEnabled(False) """initalize rexarm""" self.rexarm.initialize() """Setup Threads""" self.videoThread = VideoThread(self.kinect) self.videoThread.updateFrame.connect(self.setImage) self.videoThread.start() self.logicThread = LogicThread(self.sm) self.logicThread.start() self.displayThread = DisplayThread(self.rexarm, self.sm) self.displayThread.updateJointReadout.connect(self.updateJointReadout) self.displayThread.updateEndEffectorReadout.connect( self.updateEndEffectorReadout) self.displayThread.updateStatusMessage.connect( self.updateStatusMessage) self.displayThread.start() """ Setup Timer this runs the trackMouse function every 50ms """ self._timer = QTimer(self) self._timer.timeout.connect(self.trackMouse) self._timer.start(50) """ Slots attach callback functions to signals emitted from threads""" @pyqtSlot(QImage, QImage) def setImage(self, rgb_image, depth_image): if (self.ui.radioVideo.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image)) if (self.ui.radioDepth.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image)) @pyqtSlot(list) def updateJointReadout(self, joints): self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0] * R2D))) self.ui.rdoutShoulderJC.setText(str("%+.2f" % ((joints[1] * R2D)))) self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2] * R2D))) self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3] * R2D))) self.ui.rdoutWrist2JC.setText(str("%+.2f" % (joints[4] * R2D))) if (len(joints) > 5): self.ui.rdoutWrist3JC.setText(str("%+.2f" % (joints[5] * R2D))) else: self.ui.rdoutWrist3JC.setText(str("N.A.")) @pyqtSlot(list) def updateEndEffectorReadout(self, pos): self.ui.rdoutX.setText(str("%+.2f" % (pos[0]))) self.ui.rdoutY.setText(str("%+.2f" % (pos[1]))) self.ui.rdoutZ.setText(str("%+.2f" % (pos[2]))) self.ui.rdoutT.setText(str("%+.2f" % (pos[3]))) self.ui.rdoutG.setText(str("%+.2f" % (pos[4]))) self.ui.rdoutP.setText(str("%+.2f" % (pos[5]))) @pyqtSlot(str) def updateStatusMessage(self, msg): self.ui.rdoutStatus.setText(msg) """ Other callback functions attached to GUI elements""" def move(self): self.sm.set_next_state("move") def grab(self): self.sm.set_next_state("grab") def drop_off(self): self.sm.set_next_state("drop_off") def estop(self): self.rexarm.estop = True self.sm.set_next_state("estop") def execute(self): self.sm.set_next_state("execute") def teach(self): self.sm.set_next_state("teach") def repeat(self): self.sm.set_next_state("repeat") def rls(self): self.sm.set_next_state("rls") 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.rdoutWrist2.setText(str(self.ui.sldrWrist2.value())) self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%") self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%") self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value() / 100.0] * self.rexarm.num_joints, update_now=False) self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value() / 100.0, update_now=False) joint_positions = np.array([ self.ui.sldrBase.value() * D2R, self.ui.sldrShoulder.value() * D2R, self.ui.sldrElbow.value() * D2R, self.ui.sldrWrist.value() * D2R, self.ui.sldrWrist2.value() * D2R ]) self.rexarm.set_positions(joint_positions, update_now=False) def directControlChk(self, state): if state == Qt.Checked: self.sm.set_next_state("manual") self.ui.SliderFrame.setEnabled(True) else: self.sm.set_next_state("idle") self.ui.SliderFrame.setEnabled(False) def trackMouse(self): """ Mouse position presentation in GUI TODO: after implementing workspace calibration display the world coordinates the mouse points to in the RGB video image. """ x = QWidget.mapFromGlobal(self, QCursor.pos()).x() y = QWidget.mapFromGlobal(self, 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 if (self.kinect.currentDepthFrame.any() != 0): d = self.kinect.currentDepthFrame[y][x] # depth value d = np.clip(d, 0, 2**10 - 1) Zc = 0.1236 * math.tan(d / 2842.5 + 1.1863) # print Zc # print('----control station intrinsic_matrix') # print self.kinect.intrinsic_matrix XYZ_camera = Zc * np.matmul( np.linalg.inv(self.kinect.intrinsic_matrix), np.array([x, y, 1])) # print('----control station co_eff_camera_2_world') # print self.kinect.co_eff_camera_2_world W = np.matmul(self.kinect.co_eff_camera_2_world, np.append(XYZ_camera, 1)) self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" % (x, y, d)) # W = np.matmul(self.sm.coeff_rgb_2_world, np.array([x, y, 1])) #linear fitting # d = 2047-z # 2047 - 718 --> 0 # 2047 - 705 --> 38 # 2047 - 688 --> 38*2 # 2047 - 668 --> 38*3 # 2047 - 646 --> 38*4 # 2047 - 624 --> 38*5 # 2047 - 598 --> 38*6 # 2047 - 570 --> 38*7 # 2047 - 538 --> 38*8 # 2047 - 501 --> 38*9 # 2047 - 462 --> 38*10 # need better calibration function # W[2] = (8.00506778e-06)*d**3-(3.79099906e-02)*d**2 + (6.08296089e+01)*d - (3.26712433e+04) #W[2] = (1.46565657e+00)*d - (1.91508256e+03) #W[2] = (-4.15178471e-08)*d**4 + (2.49769770e-04)*d**3 - (5.65159066e-01)*d**2 + (5.71205622e+02)*d - (2.17696573e+05) self.ui.rdoutMouseWorld.setText("(%.0f,%.0f,%.0f)" % (W[0], W[1], W[2])) 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.kinect.last_click[0] = x - MIN_X self.kinect.last_click[1] = y - MIN_Y self.kinect.new_click = True #print(self.kinect.last_click) d = self.kinect.currentDepthFrame[self.kinect.last_click[1]][ self.kinect.last_click[0]] # depth value d = np.clip(d, 0, 2**10 - 1) Zc = 0.1236 * math.tan(d / 2842.5 + 1.1863) XYZ_camera = Zc * np.matmul( np.linalg.inv(self.kinect.intrinsic_matrix), np.array([self.kinect.last_click[0], self.kinect.last_click[1], 1 ])) W = np.matmul(self.kinect.co_eff_camera_2_world, np.append(XYZ_camera, 1)) self.sm.world_mouse = deepcopy(W[:3])
class Gui(QMainWindow): """ Main GUI Class contains the main function and interfaces between the GUI and functions """ def __init__(self, parent=None): QWidget.__init__(self, parent) self.ui = Ui_MainWindow() self.ui.setupUi(self) """ Set GUI to track mouse """ QWidget.setMouseTracking(self, True) """ Dynamixel bus TODO: add other motors here as needed with their correct address""" self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE) port_num = self.dxlbus.port() base = DXL_MX(port_num, 1) shld = DXL_MX(port_num, 2) elbw = DXL_MX(port_num, 3) wrst = DXL_AX(port_num, 4) wrst2 = DXL_AX(port_num, 5) # wrst3 = DXL_XL(port_num, 6) """Objects Using Other Classes""" self.kinect = Kinect() self.rexarm = Rexarm([base, shld, elbw, wrst, wrst2], 0) self.tp = TrajectoryPlanner(self.rexarm) self.sm = StateMachine(self.rexarm, self.tp, self.kinect) """ Attach Functions to Buttons & Sliders TODO: NAME AND CONNECT BUTTONS AS NEEDED """ # Currently Changed into Pick & Place self.ui.btn_estop.clicked.connect(self.estop) self.ui.btn_exec.clicked.connect(self.execute) self.ui.btnUser1.setText("Calibrate") self.ui.btnUser1.clicked.connect( partial(self.sm.set_next_state, "calibrate")) self.ui.btnUser2.setText("Wait4Action") self.ui.btnUser2.clicked.connect(self.wait4action) self.ui.btnUser3.setText("Detection") self.ui.btnUser3.clicked.connect(self.detection) self.ui.btnUser4.setText("Pick") self.ui.btnUser4.clicked.connect(self.pick) self.ui.btnUser5.setText("Place") self.ui.btnUser5.clicked.connect(self.place) self.ui.btnUser6.setText("QuitTask") self.ui.btnUser6.clicked.connect(self.quittask) self.ui.btnUser7.setText("LoadCalibration") self.ui.btnUser7.clicked.connect(self.loadcalibration) self.ui.sldrBase.valueChanged.connect(self.sliderChange) self.ui.sldrShoulder.valueChanged.connect(self.sliderChange) self.ui.sldrElbow.valueChanged.connect(self.sliderChange) self.ui.sldrWrist.valueChanged.connect(self.sliderChange) self.ui.sldrWrist2.valueChanged.connect(self.sliderChange) self.ui.sldrWrist3.valueChanged.connect(self.sliderChange) self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange) self.ui.sldrSpeed.valueChanged.connect(self.sliderChange) self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk) self.ui.rdoutStatus.setText("Waiting for input") """initalize manual control off""" self.ui.SliderFrame.setEnabled(False) """initalize rexarm""" self.rexarm.initialize() """Setup Threads""" self.videoThread = VideoThread(self.kinect) self.videoThread.updateFrame.connect(self.setImage) self.videoThread.start() self.logicThread = LogicThread(self.sm) self.logicThread.start() self.displayThread = DisplayThread(self.rexarm, self.sm) self.displayThread.updateJointReadout.connect(self.updateJointReadout) self.displayThread.updateEndEffectorReadout.connect( self.updateEndEffectorReadout) self.displayThread.updateStatusMessage.connect( self.updateStatusMessage) self.displayThread.start() """ Setup Timer this runs the trackMouse function every 50ms """ self._timer = QTimer(self) self._timer.timeout.connect(self.trackMouse) self._timer.start(50) """ Slots attach callback functions to signals emitted from threads""" @pyqtSlot(QImage, QImage) def setImage(self, rgb_image, depth_image): if (self.ui.radioVideo.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image)) if (self.ui.radioDepth.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image)) @pyqtSlot(list) def updateJointReadout(self, joints): self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0] * R2D))) self.ui.rdoutShoulderJC.setText(str("%+.2f" % ((joints[1] * R2D)))) self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2] * R2D))) self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3] * R2D))) self.ui.rdoutWrist2JC.setText(str("%+.2f" % (joints[4] * R2D))) if (len(joints) > 5): self.ui.rdoutWrist3JC.setText(str("%+.2f" % (joints[5] * R2D))) else: self.ui.rdoutWrist3JC.setText(str("N.A.")) @pyqtSlot(list) def updateEndEffectorReadout(self, pos): self.ui.rdoutX.setText(str("%+.2f" % (pos[0]))) self.ui.rdoutY.setText(str("%+.2f" % (pos[1]))) self.ui.rdoutZ.setText(str("%+.2f" % (pos[2]))) self.ui.rdoutT.setText(str("%+.2f" % (pos[3]))) self.ui.rdoutG.setText(str("%+.2f" % (pos[4]))) self.ui.rdoutP.setText(str("%+.2f" % (pos[5]))) @pyqtSlot(str) def updateStatusMessage(self, msg): self.ui.rdoutStatus.setText(msg) """ Other callback functions attached to GUI elements""" def estop(self): self.rexarm.estop = True self.sm.set_next_state("estop") def create(self): self.sm.set_next_state("wait4record") def record(self): self.sm.set_next_state("record") def exit(self): self.sm.set_next_state("idle") def execute(self): print('callback func') self.sm.set_next_state("execute") # Pick & Place Relavant Functions def wait4action(self): self.sm.set_next_state("wait4action") def pick(self): self.sm.set_next_state("pick") def place(self): self.sm.set_next_state("place") def detection(self): self.sm.set_next_state("detection") def quittask(self): self.sm.set_next_state("quittask") def loadcalibration(self): # load the calibration result depth2rgb_affine = np.load("./data/depth2rgb_affine.npy") extrinsic = np.load("./data/extrinsic.npy") self.kinect.depth2rgb_affine = depth2rgb_affine self.kinect.extrinsic = extrinsic self.kinect.kinectCalibrated = 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.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.rdoutWrist2.setText(str(self.ui.sldrWrist2.value())) self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%") self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%") self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value() / 100.0] * self.rexarm.num_joints, update_now=False) self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value() / 100.0, update_now=False) joint_positions = np.array([ self.ui.sldrBase.value() * D2R, self.ui.sldrShoulder.value() * D2R, self.ui.sldrElbow.value() * D2R, self.ui.sldrWrist.value() * D2R, self.ui.sldrWrist2.value() * D2R, self.ui.sldrWrist3.value() * D2R ]) self.rexarm.set_positions(joint_positions, update_now=False) def directControlChk(self, state): if state == Qt.Checked: self.sm.set_next_state("manual") self.ui.SliderFrame.setEnabled(True) else: self.sm.set_next_state("idle") self.ui.SliderFrame.setEnabled(False) def trackMouse(self): """ Mouse position presentation in GUI TODO: after implementing workspace calibration display the world coordinates the mouse points to in the RGB video image. """ x = QWidget.mapFromGlobal(self, QCursor.pos()).x() y = QWidget.mapFromGlobal(self, 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 if (self.kinect.currentDepthFrame.any() != 0): z = self.kinect.currentDepthFrame[y][x] self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" % (x, y, z)) if not self.kinect.kinectCalibrated: self.ui.rdoutMouseWorld.setText("(-,-,-)") else: w_x, w_y, w_z = self.kinect.toWorldCoord(x, y, z) self.ui.rdoutMouseWorld.setText("(%.1f,%.1f,%.1f)" % (w_x, w_y, w_z)) # 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.kinect.last_click[0] = x - MIN_X self.kinect.last_click[1] = y - MIN_Y self.kinect.new_click = True
class Gui(QMainWindow): """ Main GUI Class contains the main function and interfaces between the GUI and functions """ def __init__(self, parent=None): QWidget.__init__(self, parent) self.ui = Ui_MainWindow() self.ui.setupUi(self) """ Set GUI to track mouse """ QWidget.setMouseTracking(self, True) """ Dynamixel bus TODO: add other motors here as needed with their correct address""" self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE) print(self.dxlbus) port_num = self.dxlbus.port() print(port_num) base = DXL_MX(port_num, 1) shld = DXL_MX(port_num, 2) elbw = DXL_MX(port_num, 3) wrst = DXL_AX(port_num, 4) wrst2 = DXL_AX(port_num, 5) grip = DXL_XL(port_num, 6) wrst3 = DXL_XL(port_num, 7) """Objects Using Other Classes""" self.kinect = Kinect() self.rexarm = Rexarm((base, shld, elbw, wrst, wrst2, wrst3), grip) #self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2), 0) self.tp = TrajectoryPlanner(self.rexarm, self.kinect) self.sm = StateMachine(self.rexarm, self.tp, self.kinect) """ Attach Functions to Buttons & Sliders TODO: NAME AND CONNECT BUTTONS AS NEEDED """ self.ui.btn_estop.clicked.connect(self.estop) self.ui.btn_exec.clicked.connect(self.execute) self.ui.btn_task1.clicked.connect(self.operation) self.ui.btn_task2.clicked.connect(self.record) self.ui.btn_task3.clicked.connect(self.opex) self.ui.btn_task4.clicked.connect(self.opplay) self.ui.btn_task5.clicked.connect(self.FK_check) self.ui.btnUser1.setText("Calibrate") self.ui.btnUser1.clicked.connect( partial(self.sm.set_next_state, "calibrate")) self.ui.btnUser2.setText("Block Detection") self.ui.btnUser2.clicked.connect(self.block_detect) self.ui.btnUser3.setText("Task 1") self.ui.btnUser3.clicked.connect(self.task_1) self.ui.btnUser4.setText("Task 2") self.ui.btnUser4.clicked.connect(self.task_2) self.ui.btnUser5.setText("Task 3") self.ui.btnUser5.clicked.connect(self.task_2) self.ui.btnUser6.setText("Traj Collect") self.ui.btnUser6.clicked.connect(self.collect_traj_data) self.ui.sldrBase.valueChanged.connect(self.sliderChange) self.ui.sldrShoulder.valueChanged.connect(self.sliderChange) self.ui.sldrElbow.valueChanged.connect(self.sliderChange) self.ui.sldrWrist.valueChanged.connect(self.sliderChange) self.ui.sldrWrist2.valueChanged.connect(self.sliderChange) self.ui.sldrWrist3.valueChanged.connect(self.sliderChange) self.ui.sldrGrip1.valueChanged.connect(self.sliderChange) self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange) self.ui.sldrSpeed.valueChanged.connect(self.sliderChange) self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk) self.ui.rdoutStatus.setText("Waiting for input") """initalize manual control off""" self.ui.SliderFrame.setEnabled(False) """initalize rexarm""" self.rexarm.initialize() """Setup Threads""" self.videoThread = VideoThread(self.kinect) self.videoThread.updateFrame.connect(self.setImage) self.videoThread.start() self.logicThread = LogicThread(self.sm) self.logicThread.start() self.displayThread = DisplayThread(self.rexarm, self.sm) self.displayThread.updateJointReadout.connect(self.updateJointReadout) self.displayThread.updateEndEffectorReadout.connect( self.updateEndEffectorReadout) self.displayThread.updateStatusMessage.connect( self.updateStatusMessage) self.displayThread.start() """ Setup Timer this runs the trackMouse function every 50ms """ self._timer = QTimer(self) self._timer.timeout.connect(self.trackMouse) self._timer.start(50) """ Slots attach callback functions to signals emitted from threads""" @pyqtSlot(QImage, QImage) def setImage(self, rgb_image, depth_image): if (self.ui.radioVideo.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image)) if (self.ui.radioDepth.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image)) @pyqtSlot(list) def updateJointReadout(self, joints): self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0] * R2D))) self.ui.rdoutShoulderJC.setText( str("%+.2f" % ((joints[1] * R2D) + 90.0))) self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2] * R2D))) self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3] * R2D))) self.ui.rdoutWrist2JC.setText(str("%+.2f" % (joints[4] * R2D))) if (len(joints) > 5): self.ui.rdoutWrist3JC.setText(str("%+.2f" % (joints[5] * R2D))) else: self.ui.rdoutWrist3JC.setText(str("N.A.")) @pyqtSlot(list) def updateEndEffectorReadout(self, pos): self.ui.rdoutX.setText(str("%+.2f" % (pos[0]))) self.ui.rdoutY.setText(str("%+.2f" % (pos[1]))) self.ui.rdoutZ.setText(str("%+.2f" % (pos[2]))) # self.ui.rdoutT.setText(str("%+.2f" % (pos[3]))) # self.ui.rdoutG.setText(str("%+.2f" % (pos[4]))) # self.ui.rdoutP.setText(str("%+.2f" % (pos[5]))) @pyqtSlot(str) def updateStatusMessage(self, msg): self.ui.rdoutStatus.setText(msg) """ Other callback functions attached to GUI elements""" def estop(self): self.rexarm.estop = True self.sm.set_next_state("estop") def execute(self): self.sm.set_next_state("execute") self.rexarm.pause(5) def operation(self): if path.exists("op_joints.csv"): os.remove("op_joints.csv") self.sm.set_next_state("operation") def opex(self): self.sm.set_next_state("idle") def opplay(self): self.sm.set_next_state("opplay") def FK_check(self): self.sm.set_next_state("FK_check") def block_detect(self): self.sm.set_next_state("block_detect") def task_1(self): self.sm.set_next_state("Task 1") def task_2(self): self.sm.set_next_state("Task 2") def task_3(self): self.sm.set_next_state("Task 3") def collect_traj_data(self): self.sm.set_next_state("Collect Traj") def record(self): if self.sm.current_state == "operation": rec_joints = self.rexarm.get_positions() strec = str(rec_joints)[1:-1] + "\n" if path.exists("op_joints.csv"): with open('op_joints.csv', 'a') as f: f.write(strec) else: with open('op_joints.csv', 'w') as f: f.write(strec) 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.rdoutWrist2.setText(str(self.ui.sldrWrist2.value())) self.ui.rdoutWrist3.setText(str(self.ui.sldrWrist3.value())) self.ui.rdoutGrip1.setText(str(self.ui.sldrGrip1.value())) self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%") self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%") self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value() / 100.0] * self.rexarm.num_joints, update_now=False) self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value() / 100.0, update_now=False) joint_positions = np.array([ self.ui.sldrBase.value() * D2R, self.ui.sldrShoulder.value() * D2R, self.ui.sldrElbow.value() * D2R, self.ui.sldrWrist.value() * D2R, self.ui.sldrWrist2.value() * D2R, self.ui.sldrWrist3.value() * D2R ]) self.rexarm.set_positions(joint_positions, update_now=False) self.rexarm.gripper.set_position( np.array([self.ui.sldrGrip1.value() * D2R])) def directControlChk(self, state): if state == Qt.Checked: self.sm.set_next_state("manual") self.ui.SliderFrame.setEnabled(True) else: self.sm.set_next_state("idle") self.ui.SliderFrame.setEnabled(False) def trackMouse(self): """ Mouse position presentation in GUI TODO: after implementing workspace calibration display the world coordinates the mouse points to in the RGB video image. """ x = QWidget.mapFromGlobal(self, QCursor.pos()).x() y = QWidget.mapFromGlobal(self, QCursor.pos()).y() count = 0 if ((x < MIN_X) or (x >= MAX_X) or (y < MIN_Y) or (y >= MAX_Y)): self.ui.rdoutMousePixels.setText("(-,-,-)") self.ui.rdoutMouseWorld.setText("(-,-,-)") # posesall = self.rexarm.get_positions() # endeffectorpos = FK_dh(posesall,0) # if path.exists("traj_fast_not_smooth.txt"): # with open('traj_fast_not_smooth.txt','a') as f: # f.write(str(self.rexarm.get_wrist_pose())+'\n') # else : # with open('traj_fast_not_smooth.txt','w') as f: # f.write("Traj Not Smooth\n") else: # Subtracting the X and Y distance corresponding to image origin frame to find cursor location with reference to imae frame x = x - MIN_X y = y - MIN_Y # Checking if the Kinect depth camera is producing output if (self.kinect.currentDepthFrame.any() != 0): z = self.kinect.currentDepthFrame[y][x] # Display the x,y (pixels), z (10 bit number) coordinates self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" % (x, y, z)) # Checking if the calibration has been done if self.sm.calibration_state() == True: ############################################# # CAMERA FRAME TO DEPTH FRAME # ############################################# # Taking in the pixel values in camera frame and transforming to the kinect depth frame pixel_value = np.array([x, y]) # Converting 10 bit depth to real distance using provided analytical function z = self.kinect.currentDepthFrame[int( pixel_value.item(1))][int(pixel_value.item(0))] Z = 12.36 * np.tan(float(z) / 2842.5 + 1.1863) # 95 cm marks the z location of the base plane wrt to the camera. Subtracting 95 to measure +z from the base plane Z_modified = 95 - Z ############################################# # CAMERA FRAME TO WORLD FRAME # ############################################# # Extracting the origin of the camera frame (Following 4 quadrant system) pix_center = self.sm.pixel_center_loc() # X and Y locations in the RGB space in pixels with (0,0) at the robot base center x = x - pix_center.item(0) y = pix_center.item(1) - y # Taking in the pixel values in camera frame and transforming to the world frame pixel_value = np.array([x, y]) pixel_value = np.transpose(pixel_value) # Extracting the affine matrix computed during camera calibration affine = self.sm.return_affine() affine = affine[0:2, 0:2] # World x,y location corresponding to iamge frame x,y location world_value = np.matmul(affine, pixel_value) ############################################# # SOLVE PNP # ############################################# # rot,trans = self.sm.return_solvepnp() # cam = self.sm.return_intrinsic() # xyz_c = Z*rgb_pt.T # xyz_c = np.linalg.inv(cam).dot(xyz_c) # xyz_c = xyz_c - trans # world_value = xyz_c*rot # -0.197*float(z) + 142.772 # self.kinect.detectBlocksInDepthImage() # self.kinect.processVideoFrame() # Displaying the World X,Y and Z coordinates in GUI self.ui.rdoutMouseWorld.setText( "(%.0f,%.0f,%.1f)" % (world_value.item(0), world_value.item(1), Z_modified)) # self.sm.WC = [world_value.item(0)*10,world_value.item(1)*10,ZZ*10] else: self.ui.rdoutMouseWorld.setText("(-,-,-)") 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.kinect.last_click[0] = x - MIN_X self.kinect.last_click[1] = y - MIN_Y self.kinect.new_click = True
class Gui(QMainWindow): """! Main GUI Class Contains the main function and interfaces between the GUI and functions. """ def __init__(self, parent=None): QWidget.__init__(self, parent) self.ui = Ui_MainWindow() self.ui.setupUi(self) """ Groups of ui commonents """ self.error_lcds = [ self.ui.rdoutBaseErrors, self.ui.rdoutShoulderErrors, self.ui.rdoutElbowErrors, self.ui.rdoutWristErrors, self.ui.rdoutWrist2Errors, self.ui.rdoutWrist3Errors ] self.joint_readouts = [ self.ui.rdoutBaseJC, self.ui.rdoutShoulderJC, self.ui.rdoutElbowJC, self.ui.rdoutWristJC, self.ui.rdoutWrist2JC, self.ui.rdoutWrist3JC ] self.joint_slider_rdouts = [ self.ui.rdoutBase, self.ui.rdoutShoulder, self.ui.rdoutElbow, self.ui.rdoutWrist, self.ui.rdoutWrist2, self.ui.rdoutWrist3 ] self.joint_sliders = [ self.ui.sldrBase, self.ui.sldrShoulder, self.ui.sldrElbow, self.ui.sldrWrist, self.ui.sldrWrist2, self.ui.sldrWrist3 ] """Objects Using Other Classes""" self.kinect = Kinect() self.rexarm = Rexarm() self.tp = TrajectoryPlanner(self.rexarm) self.sm = StateMachine(self.rexarm, self.tp, self.kinect) """ Attach Functions to Buttons & Sliders TODO: NAME AND CONNECT BUTTONS AS NEEDED """ # Video self.ui.videoDisplay.setMouseTracking(True) self.ui.videoDisplay.mouseMoveEvent = self.trackMouse self.ui.videoDisplay.mousePressEvent = self.calibrateMousePress # Buttons # Handy lambda function that can be used with Partial to only set the new state if the rexarm is initialized nxt_if_arm_init = lambda next_state: self.sm.set_next_state( next_state if self.rexarm.initialized else None) self.ui.btn_estop.clicked.connect(self.estop) self.ui.btn_init_arm.clicked.connect(self.initRexarm) self.ui.btnUser1.setText("Calibrate") self.ui.btnUser1.clicked.connect(partial(nxt_if_arm_init, 'calibrate')) self.ui.btn_exec.clicked.connect(self.execute) self.ui.btnUser2.setText("Reset Waypoints") self.ui.btnUser2.clicked.connect(self.reset) self.ui.btnUser3.setText("Add Waypoint") self.ui.btnUser3.clicked.connect(self.teach) self.ui.btnUser4.setText("Gripper Toggle") self.ui.btnUser4.clicked.connect(self.toggle_gripper) self.ui.btnUser5.setText("Add Color Points") self.ui.btnUser5.clicked.connect(self.colPoints) self.ui.btnUser6.setText("Click and Grab") self.ui.btnUser6.clicked.connect(self.click_grab) # Sliders for sldr in self.joint_sliders: sldr.valueChanged.connect(self.sliderChange) self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange) self.ui.sldrSpeed.valueChanged.connect(self.sliderChange) # Direct Control self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk) # Status self.ui.rdoutStatus.setText("Waiting for input") # Auto exposure self.ui.chkAutoExposure.stateChanged.connect(self.autoExposureChk) """initalize manual control off""" self.ui.SliderFrame.setEnabled(False) """Setup Threads""" # Rexarm runs its own thread # Video self.videoThread = VideoThread(self.kinect) self.videoThread.updateFrame.connect(self.setImage) self.videoThread.start() # State machine self.logicThread = LogicThread(self.sm) self.logicThread.start() # Display self.displayThread = DisplayThread(self.rexarm, self.sm) self.displayThread.updateJointReadout.connect(self.updateJointReadout) self.displayThread.updateEndEffectorReadout.connect( self.updateEndEffectorReadout) self.displayThread.updateStatusMessage.connect( self.updateStatusMessage) self.displayThread.updateJointErrors.connect(self.updateJointErrors) self.displayThread.start() """ Slots attach callback functions to signals emitted from threads""" @pyqtSlot(QImage, QImage, QImage) def setImage(self, rgb_image, depth_image, depth_filter_image): """! @brief Display the images from the kinect. @param rgb_image The rgb image @param depth_image The depth image """ if (self.ui.radioVideo.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image)) if (self.ui.radioDepth.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image)) if (self.ui.radioUsr1.isChecked()): self.ui.videoDisplay.setPixmap( QPixmap.fromImage(depth_filter_image)) @pyqtSlot(list) def updateJointReadout(self, joints): for rdout, joint in zip(self.joint_readouts, joints): rdout.setText(str('%+.2f' % (joint * R2D))) @pyqtSlot(list) def updateEndEffectorReadout(self, pos): self.ui.rdoutX.setText(str("%+.2f" % (pos[0]))) self.ui.rdoutY.setText(str("%+.2f" % (pos[1]))) self.ui.rdoutZ.setText(str("%+.2f" % (pos[2]))) self.ui.rdoutT.setText(str("%+.2f" % (pos[3]))) # self.ui.rdoutG.setText(str("%+.2f" % (pos[4]))) # self.ui.rdoutP.setText(str("%+.2f" % (pos[5]))) @pyqtSlot(list) def updateJointErrors(self, errors): for lcd, error in zip(self.error_lcds, errors): lcd.display(error) @pyqtSlot(str) def updateStatusMessage(self, msg): self.ui.rdoutStatus.setText(msg) """ Other callback functions attached to GUI elements""" def estop(self): self.sm.set_next_state("estop") def execute(self): # self.sm.set_next_state("execute") self.sm.set_next_state("execute_tp") def reset(self): self.sm.set_next_state("reset") def teach(self): self.sm.set_next_state("teach") def colPoints(self): if self.sm.current_state == "color": #if we're already doing colors, now save it self.sm.set_next_state("save_color") else: self.sm.set_next_state("color") def toggle_gripper(self): self.rexarm.toggle_gripper() def click_grab(self): # TODO: WRITE FUNCTION HERE!!! pass def sliderChange(self): """! @brief Slider changed Function to change the slider labels when sliders are moved and to command the arm to the given position """ for rdout, sldr in zip(self.joint_slider_rdouts, self.joint_sliders): rdout.setText(str(sldr.value())) self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%") self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%") # Do nothing if the rexarm is not initialized if self.rexarm.initialized: self.rexarm.set_torque_limits( [self.ui.sldrMaxTorque.value() / 100.0] * self.rexarm.num_joints) self.rexarm.set_speeds_normalized_all(self.ui.sldrSpeed.value() / 100.0) joint_positions = np.array( [sldr.value() * D2R for sldr in self.joint_sliders]) # Only send the joints that the rexarm has self.rexarm.set_positions( joint_positions[0:self.rexarm.num_joints]) def directControlChk(self, state): """! @brief Changes to direct control mode Will only work if the rexarm is initialized. @param state State of the checkbox """ if state == Qt.Checked and self.rexarm.initialized: # Go to manual and enable sliders self.sm.set_next_state("manual") self.ui.SliderFrame.setEnabled(True) else: # Lock sliders and go to idle self.sm.set_next_state("idle") self.ui.SliderFrame.setEnabled(False) self.ui.chk_directcontrol.setChecked(False) def autoExposureChk(self, state): """! @brief Sets the Kinect auto exposer @param state State of the checkbox """ if state == Qt.Checked and self.kinect.kinectConnected == True: self.kinect.toggleExposure(True) else: self.kinect.toggleExposure(False) def trackMouse(self, mouse_event): """! @brief Show the mouse position in GUI TODO: after implementing workspace calibration display the world coordinates the mouse points to in the RGB video image. @param mouse_event QtMouseEvent containing the pose of the mouse at the time of the event not current time """ if self.kinect.DepthFrameRaw.any() != 0: u = mouse_event.pos().x() v = mouse_event.pos().y() d = self.kinect.DepthFrameRaw[v, u] self.ui.rdoutMousePixels.setText("(" + str(u) + "," + str(v) + "," + str(d) + ")") worldCoords = self.kinect.pix2Glob(np.array( [u, v, 1])) * 1000 #1000 for mm self.ui.rdoutMouseWorld.setText( f"({np.round(worldCoords[0])},{np.round(worldCoords[1])},{np.round(worldCoords[2])})" ) def calibrateMousePress(self, mouse_event): """! @brief Record mouse click positions for calibration @param mouse_event QtMouseEvent containing the pose of the mouse at the time of the event not current time """ """ Get mouse posiiton """ pt = mouse_event.pos() if mouse_event.button() == Qt.LeftButton: self.kinect.last_click[0] = pt.x() self.kinect.last_click[1] = pt.y() self.kinect.new_click = True elif mouse_event.button() == Qt.RightButton: self.kinect.last_rclick[0] = pt.x() self.kinect.last_rclick[1] = pt.y() self.kinect.new_rclick = True def initRexarm(self): """! @brief Initializes the rexarm. """ self.sm.set_next_state('initialize_rexarm') self.ui.SliderFrame.setEnabled(False) self.ui.chk_directcontrol.setChecked(False)
class Gui(QMainWindow): """ Main GUI Class contains the main function and interfaces between the GUI and functions """ def __init__(self, parent=None): QWidget.__init__(self, parent) self.ui = Ui_MainWindow() self.ui.setupUi(self) """ Set GUI to track mouse """ QWidget.setMouseTracking(self, True) """ Dynamixel bus TODO: add other motors here as needed with their correct address""" self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE) port_num = self.dxlbus.port() base = DXL_MX(port_num, 1) shld = DXL_MX(port_num, 2) elbw = DXL_MX(port_num, 3) wrst = DXL_AX(port_num, 4) wrst2 = DXL_AX(port_num, 5) wrst3 = DXL_XL(port_num, 6) grip = DXL_XL(port_num, 7) """Objects Using Other Classes""" self.kinect = Kinect() self.rexarm = Rexarm((base, shld, elbw, wrst, wrst2, wrst3), grip) self.tp = TrajectoryPlanner(self.rexarm) self.sm = StateMachine(self.rexarm, self.tp, self.kinect) """ Attach Functions to Buttons & Sliders TODO: NAME AND CONNECT BUTTONS AS NEEDED """ self.ui.btn_estop.clicked.connect(self.estop) self.ui.btnUser1.setText("Calibrate") self.ui.btnUser1.clicked.connect( partial(self.sm.set_next_state, "calibrate")) self.ui.btnUser2.setText("Waypoint") self.ui.btnUser2.clicked.connect( partial(self.sm.set_next_state, "waypoint")) self.ui.btnUser3.setText("Teach") self.ui.btnUser3.clicked.connect( partial(self.sm.set_next_state, "teach")) self.ui.btnUser4.setText("Record") self.ui.btnUser4.clicked.connect( partial(self.sm.set_next_state, "record")) self.ui.btnUser5.setText("Repeat") self.ui.btnUser5.clicked.connect( partial(self.sm.set_next_state, "repeat")) self.ui.btn_task1.setText("Pick n Stack") self.ui.btn_task1.clicked.connect(partial(self.sm.set_next_state, "IK")) self.ui.btnUser6.setText("Click and Place") self.ui.btnUser6.clicked.connect( partial(self.sm.set_next_state, "clickgrab")) self.ui.btnUser7.setText("Open Gripper") self.ui.btnUser7.clicked.connect( partial(self.sm.set_next_state, "open_gripper")) self.ui.btnUser8.setText("Close Gripper") self.ui.btnUser8.clicked.connect( partial(self.sm.set_next_state, "close_gripper")) self.ui.btnUser9.setText("Incline") self.ui.btnUser9.clicked.connect( partial(self.sm.set_next_state, "incline")) self.ui.btn_task2.setText("Line Em Up") self.ui.btn_task2.clicked.connect( partial(self.sm.set_next_state, "color_sort")) self.ui.btn_task3.setText("Stack Em High") self.ui.btn_task3.clicked.connect( partial(self.sm.set_next_state, "color_stack")) self.ui.btn_task4.setText("Block Mover") self.ui.btn_task4.clicked.connect( partial(self.sm.set_next_state, "square")) self.ui.sldrBase.valueChanged.connect(self.sliderChange) self.ui.sldrShoulder.valueChanged.connect(self.sliderChange) self.ui.sldrElbow.valueChanged.connect(self.sliderChange) self.ui.sldrWrist.valueChanged.connect(self.sliderChange) self.ui.sldrWrist2.valueChanged.connect(self.sliderChange) self.ui.sldrWrist3.valueChanged.connect(self.sliderChange) #self.ui.sldrGrip1.valueChanged.connect(self.sliderChange) self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange) self.ui.sldrSpeed.valueChanged.connect(self.sliderChange) self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk) self.ui.rdoutStatus.setText("Waiting for input") """initalize manual control off""" self.ui.SliderFrame.setEnabled(False) """initalize rexarm""" self.rexarm.initialize() """Setup Threads""" self.videoThread = VideoThread(self.kinect) self.videoThread.updateFrame.connect(self.setImage) self.videoThread.start() self.logicThread = LogicThread(self.sm) self.logicThread.start() self.displayThread = DisplayThread(self.rexarm, self.sm) self.displayThread.updateJointReadout.connect(self.updateJointReadout) self.displayThread.updateEndEffectorReadout.connect( self.updateEndEffectorReadout) self.displayThread.updateStatusMessage.connect( self.updateStatusMessage) self.displayThread.start() """ Setup Timer this runs the trackMouse function every 50ms """ self._timer = QTimer(self) self._timer.timeout.connect(self.trackMouse) self._timer.start(50) """ Slots attach callback functions to signals emitted from threads""" @pyqtSlot(QImage, QImage, QImage) def setImage(self, rgb_image, depth_image, gray_depth_image): if (self.ui.radioVideo.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image)) if (self.ui.radioDepth.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image)) if (self.ui.radioUsr1.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(gray_depth_image)) @pyqtSlot(list) def updateJointReadout(self, joints): self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0] * R2D))) self.ui.rdoutShoulderJC.setText( str("%+.2f" % ((joints[1] * R2D) + 90.0))) self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2] * R2D))) self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3] * R2D))) self.ui.rdoutWrist2JC.setText(str("%+.2f" % (joints[4] * R2D))) if (len(joints) > 5): self.ui.rdoutWrist3JC.setText(str("%+.2f" % (joints[5] * R2D))) else: self.ui.rdoutWrist3JC.setText(str("N.A.")) @pyqtSlot(list) def updateEndEffectorReadout(self, pos): self.ui.rdoutX.setText(str("%+.2f" % (pos[0]))) self.ui.rdoutY.setText(str("%+.2f" % (pos[1]))) self.ui.rdoutZ.setText(str("%+.2f" % (pos[2]))) self.ui.rdoutT.setText(str("%+.2f" % (pos[3]))) self.ui.rdoutG.setText(str("%+.2f" % (pos[4]))) self.ui.rdoutP.setText(str("%+.2f" % (pos[5]))) @pyqtSlot(str) def updateStatusMessage(self, msg): self.ui.rdoutStatus.setText(msg) """ Other callback functions attached to GUI elements""" def estop(self): self.rexarm.estop = True self.sm.set_next_state("estop") #def waypoint(self): # self.sm.set_next_state("waypoint") 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.rdoutWrist2.setText(str(self.ui.sldrWrist2.value())) self.ui.rdoutWrist3.setText(str(self.ui.sldrWrist3.value())) #self.ui.sldrGrip1.setText(str(self.ui.sldrGrip1.value())) self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%") self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%") self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value() / 100.0] * self.rexarm.num_joints, update_now=False) self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value() / 100.0, update_now=False) joint_positions = np.array([ self.ui.sldrBase.value() * D2R, self.ui.sldrShoulder.value() * D2R, self.ui.sldrElbow.value() * D2R, self.ui.sldrWrist.value() * D2R, self.ui.sldrWrist2.value() * D2R, self.ui.sldrWrist3.value() * D2R ]) self.rexarm.set_positions(joint_positions, update_now=False) def directControlChk(self, state): if state == Qt.Checked: self.sm.set_next_state("manual") self.ui.SliderFrame.setEnabled(True) else: self.sm.set_next_state("idle") self.ui.SliderFrame.setEnabled(False) def trackMouse(self): """ Mouse position presentation in GUI TODO: after implementing workspace calibration display the world coordinates the mouse points to in the RGB video image. """ x = QWidget.mapFromGlobal(self, QCursor.pos()).x() y = QWidget.mapFromGlobal(self, 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 if (self.kinect.currentDepthFrame.any() != 0): z = self.kinect.currentDepthFrame[y][x] self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" % (x, y, z)) if self.kinect.calibrated == True: mouse = np.array([[x], [y], [1]], dtype=float) depth = (0.1236 * np.tan((z) / 2842.5 + 1.1863)) camera = depth * np.matmul(inv(self.kinect.camera_matrix), mouse) points = np.array([[camera[0]], [camera[1]], [1]]) world = np.matmul(self.kinect.affineB.astype(float), points.astype(float)) #world = np.matmul(self.kinect.affineB.astype(float),mouse) #depth = -2.5333*z + 1836.7 board_depth = 0.1236 * np.tan(718 / 2842.5 + 1.1863) #depth = (1-depth/board_depth)*1000 depth = (board_depth - depth) * 1000 """ correction_factor = 0.000626 world[0]=world[0]*(1-correction_factor*depth) world[1]=world[1]*(1-correction_factor*depth) """ #self.ui.rdoutMouseWorld.setText("(%3.2f,%3.2f,%3.2f)" % (camera[0],camera[1],camera[2])) self.ui.rdoutMouseWorld.setText( "(%.0f,%.0f,%.0f)" % (world[0], world[1], depth)) 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.kinect.last_click[0] = x - MIN_X self.kinect.last_click[1] = y - MIN_Y self.kinect.new_click = True
class Gui(QMainWindow): """ Main GUI Class contains the main function and interfaces between the GUI and functions """ def __init__(self, parent=None): QWidget.__init__(self, parent) self.ui = Ui_MainWindow() self.ui.setupUi(self) """Set the running mode to simulation""" self.mode = 'SIM' """ Set GUI to track mouse """ # QWidget.setMouseTracking(self, True) """ Dynamixel bus TODO: add other motors here as needed with their correct address""" self.dxlbus = None # port_num = self.dxlbus.port() base = DXL_MX(None, 1) shld = DXL_MX(None, 2) elbw = DXL_MX(None, 3) wrst = DXL_AX(None, 4) # wrst2 = DXL_AX(5) self.dxl = [base, shld, elbw, wrst] """Objects Using Other Classes""" self.gzclient = GzSimClient(8081) self.gzrexarm = gzrexarm.GzRexarm(self.gzclient) self.real_rexarm = rexarm.Rexarm((base, shld, elbw, wrst), 0) self.rexarm = rexarm_com.RexarmCom(self.gzrexarm, self.real_rexarm, self.mode) # self.kinect = Kinect() self.tp = TrajectoryPlanner(self.rexarm) # self.sm = StateMachine(self.rexarm, self.tp, self.kinect) self.sm = StateMachine(self.rexarm, self.tp, None) """ Attach Functions to Buttons & Sliders TODO: NAME AND CONNECT BUTTONS AS NEEDED """ self.ui.btn_estop.clicked.connect(self.estop) self.ui.btnUser1.setText("Calibrate") self.ui.btnUser1.clicked.connect( partial(self.sm.set_next_state, "calibrate")) self.ui.btnClearTrj.clicked.connect(self.clear_trj) self.ui.btnAddCube.clicked.connect(self.add_cube) self.ui.sldrBase.valueChanged.connect(self.sliderChange) self.ui.sldrShoulder.valueChanged.connect(self.sliderChange) self.ui.sldrElbow.valueChanged.connect(self.sliderChange) self.ui.sldrWrist.valueChanged.connect(self.sliderChange) self.ui.sldrGrip1.valueChanged.connect(self.sliderChange) self.ui.sldrGrip2.valueChanged.connect(self.sliderChange) self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange) self.ui.sldrSpeed.valueChanged.connect(self.sliderChange) # self.ui.sldrMaxTorque.valueChanged.connect(self.sliderTqSpChange) # self.ui.sldrSpeed.valueChanged.connect(self.sliderTqSpChange) self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk) self.ui.chk_simulation.stateChanged.connect(self.simulationChk) self.ui.chk_attached.stateChanged.connect(self.attachChk) self.ui.chk_showtrj.stateChanged.connect(self.trjChk) self.ui.rdoutStatus.setText("Waiting for input") """Initalize manual control off""" self.ui.SliderFrame.setEnabled(False) """Lock torque and speed control soider in simulation""" # self.lock_tqspslider() """Initalize the simulation""" self.gzclient.start() self.gzrexarm.initialize() """Setup Threads""" # self.videoThread = VideoThread(self.kinect) # self.videoThread.updateFrame.connect(self.setImage) # self.videoThread.start() self.logicThread = LogicThread(self.sm) self.logicThread.start() self.displayThread = DisplayThread(self.sm, self.real_rexarm, self.gzrexarm, self.mode) self.displayThread.updateJointReadout.connect(self.updateJointReadout) self.displayThread.updateEndEffectorReadout.connect( self.updateEndEffectorReadout) self.displayThread.updateStatusMessage.connect( self.updateStatusMessage) self.displayThread.start() """ Setup Timer this runs the trackMouse function every 50ms """ # self._timer = QTimer(self) # self._timer.timeout.connect(self.trackMouse) # self._timer.start(50) """ Slots attach callback functions to signals emitted from threads""" @pyqtSlot(QImage, QImage) def setImage(self, rgb_image, depth_image): if (self.ui.radioVideo.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image)) if (self.ui.radioDepth.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image)) @pyqtSlot(list, list) def updateJointReadout(self, gzjoints, joints): if self.ui.chk_simulation.checkState() == Qt.Checked and len( gzjoints) == 4: self.ui.GzrdoutBaseJC.setText(str("%+.2f" % (gzjoints[0] * R2D))) self.ui.GzrdoutShoulderJC.setText( str("%+.2f" % (gzjoints[1] * R2D))) self.ui.GzrdoutElbowJC.setText(str("%+.2f" % (gzjoints[2] * R2D))) self.ui.GzrdoutWristJC.setText(str("%+.2f" % (gzjoints[3] * R2D))) if self.ui.chk_attached.checkState( ) == Qt.Checked and len(joints) >= 4: self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0] * R2D))) self.ui.rdoutShoulderJC.setText( str("%+.2f" % ((joints[1] * R2D) + 90.0))) self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2] * R2D))) self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3] * R2D))) self.ui.rdoutWrist2JC.setText(str("%+.2f" % (joints[4] * R2D))) if len(joints) > 5: self.ui.rdoutWrist3JC.setText(str("%+.2f" % (joints[5] * R2D))) else: self.ui.rdoutWrist3JC.setText(str("N.A.")) @pyqtSlot(list, list) def updateEndEffectorReadout(self, gzpose, pose): if self.ui.chk_simulation.checkState() == Qt.Checked: self.ui.GzEEX.setText(str("%+.2f" % (gzpose[0]))) self.ui.GzEEY.setText(str("%+.2f" % (gzpose[1]))) self.ui.GzEEZ.setText(str("%+.2f" % (gzpose[2]))) if self.ui.chk_attached.checkState() == Qt.Checked: self.ui.rdoutX.setText(str("%+.2f" % (pos[0]))) self.ui.rdoutY.setText(str("%+.2f" % (pos[1]))) self.ui.rdoutZ.setText(str("%+.2f" % (pos[2]))) self.ui.rdoutT.setText(str("%+.2f" % (pos[3]))) self.ui.rdoutG.setText(str("%+.2f" % (pos[4]))) self.ui.rdoutP.setText(str("%+.2f" % (pos[5]))) @pyqtSlot(str) def updateStatusMessage(self, msg): self.ui.rdoutStatus.setText(msg) """ Other callback functions attached to GUI elements""" def estop(self): self.rexarm.set_estop(True) self.sm.set_next_state("estop") def clear_trj(self): """Clear the end-effector trajectory marker""" msg = '3,1\n' self.gzclient.send_msg(msg) def add_cube(self): """Add a cube to the workspace in Gazebo""" msg = '4,1\n' self.gzclient.send_msg(msg) def reset_sliders(self): self.ui.sldrBase.setValue(0) self.ui.sldrShoulder.setValue(0) self.ui.sldrElbow.setValue(0) self.ui.sldrWrist.setValue(0) self.ui.sldrGrip1.setValue(0) self.ui.sldrGrip2.setValue(0) self.ui.sldrMaxTorque.setValue(50) self.ui.sldrSpeed.setValue(25) # def lock_tqspslider(self): # self.ui.sldrMaxTorque.setEnabled(False) # self.ui.sldrSpeed.setEnabled(False) # def unlock_tqspslider(self): # self.ui.sldrMaxTorque.setEnabled(True) # self.ui.sldrSpeed.setEnabled(True) # def sliderTqSpChange(self): # self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value()/100.0]* # self.rexarm.num_joints(), update_now=False) # self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value()/100.0, # update_now=False) # self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%") # self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%") 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())) joint_positions = np.array([ self.ui.sldrBase.value() * D2R, self.ui.sldrShoulder.value() * D2R, self.ui.sldrElbow.value() * D2R, self.ui.sldrWrist.value() * D2R, self.ui.sldrGrip1.value() * D2R, self.ui.sldrGrip2.value() * D2R ]) self.rexarm.set_positions(joint_positions, update_now=False) self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value() / 100.0] * self.rexarm.num_joints(), update_now=False) self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value() / 100.0, update_now=False) self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%") self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%") def directControlChk(self, state): if state == Qt.Checked: self.sm.set_next_state("manual") self.ui.SliderFrame.setEnabled(True) else: self.sm.set_next_state("idle") self.ui.SliderFrame.setEnabled(False) def simulationChk(self, state): if state == Qt.Checked: # Initialized the gzrexarm if self.gzclient.is_running() is False: self.gzclient.start() # Update the running mode if self.ui.chk_attached.checkState() == Qt.Checked: self.mode = 'SIM_ATTACHED' else: self.mode = 'SIM' # self.lock_tqspslider() else: if self.gzclient.is_running() is True: self.gzclient.stop() if self.ui.chk_attached.checkState() == Qt.Checked: self.mode = 'ATTACHED' else: self.mode = 'DETACHED' # self.unlock_tqspslider() self.rexarm.set_mode(self.mode) self.rexarm.initialize() self.reset_sliders() def attachChk(self, state): if state == Qt.Checked: if self.dxlbus is None: self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE) port_num = self.dxl_bus.port() for joint in self.dxl: joint.set_port(port_num) joint.set_mode(2) if self.ui.chk_simulation.checkState() == Qt.Checked: self.mode = 'SIM_ATTACHED' else: self.mode = 'ATTACHED' # self.unlock_tqspslider() else: if self.dxlbus is not None: self.dxlbus.close() self.dxlbus = None if self.ui.chk_simulation.checkState() == Qt.Checked: self.mode = 'SIM' else: self.mode = 'DETACHED' # self.lock_tqspslider() self.rexarm.set_mode(self.mode) self.rexarm.initalize() self.reset_sliders() def trjChk(self, state): if state == Qt.Checked: msg = '3,2\n' else: msg = '3,3\n' self.gzclient.send_msg(msg) def trackMouse(self): """ Mouse position presentation in GUI TODO: after implementing workspace calibration display the world coordinates the mouse points to in the RGB video image. """ x = QWidget.mapFromGlobal(self, QCursor.pos()).x() y = QWidget.mapFromGlobal(self, 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 if (self.kinect.currentDepthFrame.any() != 0): z = self.kinect.currentDepthFrame[y][x] self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" % (x, y, z)) self.ui.rdoutMouseWorld.setText("(-,-,-)") 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.kinect.last_click[0] = x - MIN_X self.kinect.last_click[1] = y - MIN_Y self.kinect.new_click = True print(self.kinect.last_click)
class Gui(QMainWindow): """ Main GUI Class contains the main function and interfaces between the GUI and functions """ def __init__(self, parent=None): QWidget.__init__(self, parent) self.ui = Ui_MainWindow() self.ui.setupUi(self) """ Set GUI to track mouse """ QWidget.setMouseTracking(self, True) """ Dynamixel bus TODO: add other motors here as needed with their correct address""" self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE) port_num = self.dxlbus.port() base = DXL_MX(port_num, 1) shld = DXL_MX(port_num, 2) elbw = DXL_MX(port_num, 3) wrst = DXL_AX(port_num, 4) wrst2 = DXL_AX(port_num, 5) wrst3 = DXL_XL(port_num, 6) gripper = DXL_XL(port_num, 7) """Objects Using Other Classes""" self.kinect = Kinect() self.rexarm = Rexarm((base, shld, elbw, wrst, wrst2, wrst3), gripper) self.tp = TrajectoryPlanner(self.rexarm) self.sm = StateMachine(self.rexarm, self.tp, self.kinect) """ Attach Functions to Buttons & Sliders TODO: NAME AND CONNECT BUTTONS AS NEEDED """ self.ui.btn_estop.clicked.connect(self.estop) self.ui.btnUser1.setText("Calibrate") self.ui.btnUser1.clicked.connect( partial(self.sm.set_next_state, "calibrate")) self.ui.btnUser2.setText("Record") self.ui.btnUser2.clicked.connect( partial(self.sm.set_next_state, "record")) self.ui.btnUser3.setText("Playback") self.ui.btnUser3.clicked.connect( partial(self.sm.set_next_state, "playback")) self.ui.sldrBase.valueChanged.connect(self.sliderChange) self.ui.sldrShoulder.valueChanged.connect(self.sliderChange) self.ui.sldrElbow.valueChanged.connect(self.sliderChange) self.ui.sldrWrist.valueChanged.connect(self.sliderChange) self.ui.sldrWrist2.valueChanged.connect(self.sliderChange) self.ui.sldrWrist3.valueChanged.connect(self.sliderChange) self.ui.sldrHand.valueChanged.connect(self.sliderChange) self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange) self.ui.sldrSpeed.valueChanged.connect(self.sliderChange) self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk) self.ui.rdoutStatus.setText("Waiting for input") self.ui.btn_exec.clicked.connect( partial(self.sm.set_next_state, "execute")) self.ui.btnUser4.setText("TP test") self.ui.btnUser4.clicked.connect( partial(self.sm.set_next_state, "TP test")) self.ui.btnUser5.setText("Detect blocks") self.ui.btnUser5.clicked.connect( partial(self.sm.set_next_state, "Detect Blocks")) self.ui.btnUser11.setText("IK_set_pose") self.ui.btnUser11.clicked.connect( partial(self.sm.set_next_state, "IK_set_pose")) self.ui.btnUser12.setText("IK_Test") self.ui.btnUser12.clicked.connect( partial(self.sm.set_next_state, "IK_test")) self.ui.btnUser10.setText("Grab_Place") self.ui.btnUser10.clicked.connect( partial(self.sm.set_next_state, "Grab_Place")) self.ui.btnUser9.setText("BlockSlider") self.ui.btnUser9.clicked.connect( partial(self.sm.set_next_state, "BlockSlider")) self.ui.btnUser8.setText("Pick_N_Stack") self.ui.btnUser8.clicked.connect( partial(self.sm.set_next_state, "Pick_N_Stack")) """initalize manual control off""" self.ui.SliderFrame.setEnabled(False) """initalize rexarm""" self.rexarm.initialize() """Setup Threads""" self.videoThread = VideoThread(self.kinect) self.videoThread.updateFrame.connect(self.setImage) self.videoThread.start() self.logicThread = LogicThread(self.sm) self.logicThread.start() self.displayThread = DisplayThread(self.rexarm, self.sm) self.displayThread.updateJointReadout.connect(self.updateJointReadout) self.displayThread.updateEndEffectorReadout.connect( self.updateEndEffectorReadout) self.displayThread.updateStatusMessage.connect( self.updateStatusMessage) self.displayThread.start() """ Setup Timer this runs the trackMouse function every 50ms """ self._timer = QTimer(self) self._timer.timeout.connect(self.trackMouse) self._timer.start(50) """ Slots attach callback functions to signals emitted from threads""" @pyqtSlot(QImage, QImage, QImage) def setImage(self, rgb_image, depth_image, block_image): if (self.ui.radioVideo.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(rgb_image)) if (self.ui.radioDepth.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(depth_image)) if (self.ui.radioBlockDetect.isChecked()): self.ui.videoDisplay.setPixmap(QPixmap.fromImage(block_image)) @pyqtSlot(list) def updateJointReadout(self, joints): self.ui.rdoutBaseJC.setText(str("%+.2f" % (joints[0] * R2D))) self.ui.rdoutShoulderJC.setText( str("%+.2f" % ((joints[1] * R2D) + 90.0))) self.ui.rdoutElbowJC.setText(str("%+.2f" % (joints[2] * R2D))) self.ui.rdoutWristJC.setText(str("%+.2f" % (joints[3] * R2D))) self.ui.rdoutWrist2JC.setText(str("%+.2f" % (joints[4] * R2D))) if (len(joints) > 5): self.ui.rdoutWrist3JC.setText(str("%+.2f" % (joints[5] * R2D))) else: self.ui.rdoutWrist3JC.setText(str("N.A.")) @pyqtSlot(list) def updateEndEffectorReadout(self, pos): self.ui.rdoutX.setText(str("%+.2f" % (pos[0]))) self.ui.rdoutY.setText(str("%+.2f" % (pos[1]))) self.ui.rdoutZ.setText(str("%+.2f" % (pos[2]))) self.ui.rdoutT.setText(str("%+.2f" % (pos[3]))) self.ui.rdoutG.setText(str("%+.2f" % (pos[4]))) self.ui.rdoutP.setText(str("%+.2f" % (pos[5]))) @pyqtSlot(str) def updateStatusMessage(self, msg): self.ui.rdoutStatus.setText(msg) """ Other callback functions attached to GUI elements""" def estop(self): self.rexarm.estop = True self.sm.set_next_state("estop") 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.rdoutWrist2.setText(str(self.ui.sldrWrist2.value())) self.ui.rdoutWrist2.setText(str(self.ui.sldrWrist3.value())) self.ui.rdoutWrist2.setText(str(self.ui.sldrHand.value())) self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%") self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%") self.rexarm.set_torque_limits([self.ui.sldrMaxTorque.value() / 100.0] * self.rexarm.num_joints, update_now=False) self.rexarm.set_speeds_normalized_global(self.ui.sldrSpeed.value() / 100.0, update_now=False) joint_positions = np.array([ self.ui.sldrBase.value() * D2R, self.ui.sldrShoulder.value() * D2R, self.ui.sldrElbow.value() * D2R, self.ui.sldrWrist.value() * D2R, self.ui.sldrWrist2.value() * D2R, self.ui.sldrWrist3.value() * D2R ]) self.rexarm.set_positions(joint_positions, update_now=False) # self.rexarm.pause(0.1) self.rexarm.set_gripper_position(self.ui.sldrHand.value() * D2R, update_now=False) # self.rexarm.gripper.set_position(self.ui.sldrHand.value()*D2R) def directControlChk(self, state): if state == Qt.Checked: self.sm.set_next_state("manual") self.ui.SliderFrame.setEnabled(True) else: self.sm.set_next_state("idle") self.ui.SliderFrame.setEnabled(False) def trackMouse(self): """ Mouse position presentation in GUI TODO: after implementing workspace calibration display the world coordinates the mouse points to in the RGB video image. """ x = QWidget.mapFromGlobal(self, QCursor.pos()).x() y = QWidget.mapFromGlobal(self, 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 if (self.kinect.currentDepthFrame.any() != 0): z = self.kinect.currentDepthFrame[y][x] self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" % (x, y, z)) #convert camera data to depth in mm depth = 1000 * 0.1236 * np.tan(z / 2842.5 + 1.1863) # if self.kinect.kinectCalibrated == True : world_frame = depth * np.dot(self.kinect.projection, [x, y, 1]) #To convert depth to IK convention world_frame[2] = -world_frame[2] + 939 self.ui.rdoutMouseWorld.setText( "(%.0f,%.0f,%.0f)" % (world_frame[0], world_frame[1], world_frame[2])) self.kinect.world_frame = world_frame # use this variable in click and grab #else : # self.ui.rdoutMouseWorld.setText("(-,-,-)") 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.kinect.last_click[0] = x - MIN_X self.kinect.last_click[1] = y - MIN_Y self.kinect.new_click = True