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
