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_XL(port_num, 1)
shld = DXL_XL(port_num, 2)
elbw = DXL_XL(port_num, 3)
wrst = DXL_XL(port_num, 4)
grip = DXL_XL(port_num, 5)
"""Objects Using Other Classes"""
self.camera = PiCamera()
self.rexarm = Rexarm((base, shld, elbw, wrst, grip))
self.tp = TrajectoryPlanner(self.rexarm)
self.sm = StateMachine(self.rexarm, self.tp)
self.taskThread = TaskThread(self.sm)
self.rgb_image = 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_current_state, "calibrate"))
self.ui.btn_task1.clicked.connect(
partial(self.taskThread.set_task_num, 1))
self.ui.btn_task2.clicked.connect(
partial(self.taskThread.set_task_num, 2))
self.ui.btn_task3.clicked.connect(
partial(self.taskThread.set_task_num, 3))
self.ui.btn_task4.clicked.connect(
partial(self.taskThread.set_task_num, 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.sldrWrist2.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.camera)
self.videoThread.updateFrame.connect(self.setImage)
self.videoThread.updateAprilTags.connect(self.updateAprilTags)
self.videoThread.updateImage.connect(self.updateImage)
self.videoThread.start()
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
self.taskThread.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)
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)
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)
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()
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.kinect = Kinect()
self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2,grip),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(partial(self.sm.set_next_state, "execute"))
self.ui.btn_cali.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
# self.ui.btnUser1.setText("Calibrate")
# self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
self.ui.btnUser1.setText("Teach and Repeat")
self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "teachNRepeat"))
self.ui.btnUser2.setText("Record Waypoints")
self.ui.btnUser2.clicked.connect(partial(self.sm.set_next_state, "recordWaypoint"))
self.ui.btnUser3.setText("Play")
self.ui.btnUser3.clicked.connect(partial(self.sm.set_next_state, "play"))
self.ui.btnUser4.setText("Block Detection Start")
self.ui.btnUser4.clicked.connect(partial(self.sm.set_next_state, "blockDetectionStart"))
self.ui.btnUser5.setText("Block Message")
self.ui.btnUser5.clicked.connect(partial(self.sm.set_next_state, "blockMessage"))
self.ui.btnUser6.setText("Block Detection End")
self.ui.btnUser6.clicked.connect(partial(self.sm.set_next_state, "blockDetectionEnd"))
self.ui.btnUser7.setText("Click and Grab")
self.ui.btnUser7.clicked.connect(partial(self.sm.set_next_state, "clickNGrab"))
self.ui.btnUser8.setText("Pick n' Place")
self.ui.btnUser8.clicked.connect(partial(self.sm.set_next_state, "pickNPlace"))
self.ui.btnUser9.setText("Pick n' Stack")
self.ui.btnUser9.clicked.connect(partial(self.sm.set_next_state, "pickNStack"))
self.ui.btnUser10.setText("Line 'em Up!")
self.ui.btnUser10.clicked.connect(partial(self.sm.set_next_state, "lineUp"))
self.ui.btnUser11.setText("Stack 'em High!")
self.ui.btnUser11.clicked.connect(partial(self.sm.set_next_state, "stackHigh"))
self.ui.btnUser12.setText("Pyramid Builder!")
self.ui.btnUser12.clicked.connect(partial(self.sm.set_next_state, "buildPyramid"))
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)
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()
"""
467TODO:
Based on motors we are using, modify ports and definition of rexarm
Assume only use shld, elbw, wrst for now
"""
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, 1)
"""Objects Using Other Classes"""
self.kinect = None #Kinect()
#self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2),0)
self.rexarm = Rexarm((base,shld,elbw, wrst),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.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.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)
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Main Variables Using Other Classes"""
self.rex = Rexarm(self.ui)
self.video = Video(cv2.VideoCapture(0))
"""
Zhentao added Here.
Initialize the statemachine.
"""
self.statemanager = StateManager(self.rex, self.video)
""" Other Variables """
self.last_click = np.float32([0, 0])
""" Set GUI to track mouse """
QtGui.QWidget.setMouseTracking(self, True)
"""
Video Function
Creates a timer and calls play() function
according to the given time delay (27mm)
"""
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.play)
self._timer.start(27)
"""
LCM Arm Feedback
Creates a timer to call LCM handler continuously
No delay implemented. Reads all time
"""
self._timer2 = QtCore.QTimer(self)
self._timer2.timeout.connect(self.rex.get_feedback)
self._timer2.start()
"""
Connect Sliders to Function
LAB TASK: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI
"""
## TODO: IMPLEMENT GRIP VALUE CONTROLS ##
self.ui.sldrBase.valueChanged.connect(self.sliderChange)
self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
""" Commands the arm as the arm initialize to 0,0,0,0 angles """
self.sliderChange()
""" Connect Buttons to Functions
LAB TASK: NAME AND CONNECT BUTTONS AS NEEDED
"""
#This is needed.
self.ui.btnUser1.setText("Affine Calibration")
self.ui.btnUser1.clicked.connect(self.affine_cal)
self.ui.btnUser2.setText("SM Test")
self.ui.btnUser2.clicked.connect(self.iTestSM)
self.ui.btnUser3.setText("Reset Position")
self.ui.btnUser3.clicked.connect(self.rex.iResetPosition)
self.ui.btnUser4.setText("Replay")
self.ui.btnUser4.clicked.connect(self.iReplay)
"""
self.ui.btnUser4.setText("OpenGripper")
self.ui.btnUser4.clicked.connect(self.iTestGripperOpen)
self.ui.btnUser5.setText("CloseGripper")
self.ui.btnUser5.clicked.connect(self.iTestGripperClose)
"""
"""
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Main Variables Using Other Classes"""
self.rex = Rexarm()
self.video = Video()
self.statemachine = Statemachine()
""" Other Variables """
self.last_click = np.float32([0, 0])
""" Customized events """
self.flag_pickAndPlace = 0
self.n_pickLocation = 2
self.flag_event = 0
""" Set GUI to track mouse """
QtGui.QWidget.setMouseTracking(self, True)
"""
GUI timer
Creates a timer and calls update_gui() function
according to the given time delay (27ms)
"""
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.update_gui)
self._timer.start(27)
"""
Event timer
Creates a timer and calls updateEvent() function
according to the given time delay (???ms)
"""
# Thread(target=self.event_one).start()
# self._timer3 = QtCore.QTimer(self)
# self._timer3.timeout.connect(self.event_one)
# self._timer3.start(50)
"""
LCM Arm Feedback timer
Creates a timer to call LCM handler for Rexarm feedback
"""
self._timer2 = QtCore.QTimer(self)
self._timer2.timeout.connect(self.rex.get_feedback)
self._timer2.start()
"""
Connect Sliders to Function
TODO: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI
"""
self.ui.sldrBase.valueChanged.connect(self.sliderChange)
self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip2.valueChanged.connect(self.sliderChange)
""" Initial command of the arm to home position"""
self.sliderChange()
""" Connect Buttons to Functions
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btnUser1.setText("Reset Location")
self.ui.btnUser1.clicked.connect(self.reset)
self.ui.btnUser2.setText("Play Event 1")
self.ui.btnUser2.clicked.connect(self.event_one)
self.ui.btnUser3.setText("Play Event 2")
self.ui.btnUser3.clicked.connect(self.event_two)
self.ui.btnUser4.setText("Play Event 3")
self.ui.btnUser4.clicked.connect(self.event_three)
self.ui.btnUser5.setText("Play Event 4")
self.ui.btnUser5.clicked.connect(self.event_four)
self.ui.btnUser6.setText("Play Event 5")
self.ui.btnUser6.clicked.connect(self.event_five)
# self.ui.btnUser6.setText("Test Arm Movement")
# self.ui.btnUser6.clicked.connect(self.testArm)
self.ui.btnUser7.setText("Click to grab and place")
self.ui.btnUser7.clicked.connect(self.GrabAndPlace)
# self.ui.btnUser8.setText("Teach and Repeat")
# self.ui.btnUser8.clicked.connect(self.teach)
# self.ui.btnUser9.setText("Record Arm Location")
# self.ui.btnUser9.clicked.connect(self.record)
# self.ui.btnUser10.setText("Play Arm Locations")
# self.ui.btnUser10.clicked.connect(self.playback)
self.ui.btnUser10.setText("Calibrate Camera")
self.ui.btnUser10.clicked.connect(self.simple_cal)
self.ui.btnUser11.setText("Configure Servos")
self.ui.btnUser12.clicked.connect(self.rex.cfg_publish_default)
self.ui.btnUser12.setText("Emergency Stop")
self.ui.btnUser12.clicked.connect(self.estop)
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Main Variables Using Other Classes"""
self.rex = Rexarm()
self.video = Video()
self.statemachine = Statemachine()
self.pos_idx_teach = 0
""" Other Variables """
self.last_click = np.float32([0, 0])
self.check_move_time = 60
self.check_time = 70
self.check_move_counter = 0
self.d_t = 10. * self.check_move_time / 1000.
self.spe_coef = [0.0] * self.rex.num_joints
self.pos_pre = [0.0] * self.rex.num_joints
self.plan_point = 0
self.move_to_point_cn = 0
self.event3_color_check = [0] * 8
self.event3_finish = 0
self.event4_color_idx = 0
self.vel = []
self.event4_f_idx = 0
self.event5_block_num = 0
""" Set GUI to track mouse """
QtGui.QWidget.setMouseTracking(self, True)
"""
GUI timer
Creates a timer and calls update_gui() function
according to the given time delay (27ms)
"""
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.update_gui)
self._timer.start(27)
"""
LCM Arm Feedback timer
Creates a timer to call LCM handler for Rexarm feedback
"""
self._timer2 = QtCore.QTimer(self)
self._timer2.timeout.connect(self.rex.get_feedback)
self._timer2.start()
# self._timer_setplan = QtCore.QTimer(self)
# self._timer_setplan.timeout.connect(self.setplan)
# self._timer_teach = QtCore.QTimer(self)
# self._timer_teach.timeout.connect(self.check_move)
self._timer_waypoint = QtCore.QTimer(self)
self._timer_waypoint.timeout.connect(self.move_to_point)
self._timer_event1 = QtCore.QTimer(self)
self._timer_event2 = QtCore.QTimer(self)
self._timer_event3 = QtCore.QTimer(self)
self._timer_event4 = QtCore.QTimer(self)
self._timer_event5 = QtCore.QTimer(self)
"""
Connect Sliders to Function
TODO: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI
"""
self.ui.sldrBase.valueChanged.connect(self.sliderChange)
self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip2.valueChanged.connect(self.sliderChange)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
""" Initial command of the arm to home position"""
self.sliderChange()
""" Connect Buttons to Functions
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btnUser1.setText("Configure Servos")
self.ui.btnUser1.clicked.connect(self.rex.cfg_publish_default)
self.ui.btnUser2.setText("Simple Calibration")
self.ui.btnUser2.clicked.connect(self.simple_cal)
# self.ui.btnUser3.setText("start teach and repeat")
# self.ui.btnUser3.clicked.connect(self.teach_repeat_start)
# self.ui.btnUser4.setText("record position")
# self.ui.btnUser4.clicked.connect(self.teach_repeat_record)
# self.ui.btnUser5.setText("play teach and repeat")
# self.ui.btnUser5.clicked.connect(self.teach_repeat_play)
# self.ui.btnUser6.setText("save rgb")
# self.ui.btnUser6.clicked.connect(self.save_rgb)
self.ui.btnUser3.setText("EVENT1")
self.ui.btnUser3.clicked.connect(self.event1)
self.ui.btnUser4.setText("EVENT2")
self.ui.btnUser4.clicked.connect(self.event2)
self.ui.btnUser5.setText("EVENT3")
self.ui.btnUser5.clicked.connect(self.event3)
self.ui.btnUser6.setText("EVENT4")
self.ui.btnUser6.clicked.connect(self.event4)
self.ui.btnUser7.setText("EVENT5")
self.ui.btnUser7.clicked.connect(self.event5)
self.ui.btnUser8.setText("depth test")
self.ui.btnUser8.clicked.connect(self.video.blockDetector)
self.ui.btnUser9.setText("recover")
self.ui.btnUser9.clicked.connect(self.recover)
self.ui.btnUser10.setText("click put")
self.ui.btnUser10.clicked.connect(self.click_put)
self.ui.btnUser12.setText("Emergency Stop!")
self.ui.btnUser12.clicked.connect(self.estop)
def __init__(self, parent=None):
print 'TEST INITial'
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Main Variables Using Other Classes"""
self.rex = Rexarm()
self.video = Video(cv2.VideoCapture(0))
""" Other Variables """
self.last_click = np.float32([0, 0])
""" Set GUI to track mouse """
QtGui.QWidget.setMouseTracking(self, True)
"""
Video Function
Creates a timer and calls play() function
according to the given time delay (27mm)
"""
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.play)
self._timer.start(27)
"""
LCM Arm Feedback
Creates a timer to call LCM handler continuously
No delay implemented. Reads all time
"""
self._timer2 = QtCore.QTimer(self)
self._timer2.timeout.connect(self.rex.get_feedback)
self._timer2.start()
"""
Connect Sliders to Function
LAB TASK: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI
"""
self.ui.sldrShoulder.setValue(-112.1)
self.ui.sldrElbow.setValue(10.8)
self.ui.sldrWrist.setValue(18.4)
self.ui.sldrMaxTorque.setValue(90)
self.ui.sldrSpeed.setValue(12)
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.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
""" Commands the arm as the arm initialize to 0,0,0,0 angles """
self.sliderChange()
""" Connect Buttons to Functions
LAB TASK: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btnUser1.setText("Check Pressure Sensor")
self.ui.btnUser1.clicked.connect(self.isPressureSensorOccupied)
self.ui.btnUser2.setText("Mix Drink")
self.ui.btnUser2.clicked.connect(self.mixDrink)
self.ui.btnUser3.setText("Home")
self.ui.btnUser3.clicked.connect(self.driveToHome)
self.ui.btnUser4.setText("Bottle 0")
self.ui.btnUser4.clicked.connect(self.driveToBottle0)
self.ui.btnUser5.setText("Bottle 1")
self.ui.btnUser5.clicked.connect(self.driveToBottle1)
self.ui.btnUser6.setText("Bottle 2")
self.ui.btnUser6.clicked.connect(self.driveToBottle2)
self.ui.btnUser7.setText("Bottle 3")
self.ui.btnUser7.clicked.connect(self.driveToBottle3)
self.ui.btnUser8.setText("Bottle 4")
self.ui.btnUser8.clicked.connect(self.driveToBottle4)
self.ui.btnUser9.setText("Bottle 5")
self.ui.btnUser9.clicked.connect(self.driveToBottle5)
self.ui.btnUser10.setText("Bottle 6")
self.ui.btnUser10.clicked.connect(self.driveToBottle6)
self.ui.btnUser11.setText("Bottle 7")
self.ui.btnUser11.clicked.connect(self.driveToBottle7)
self.ui.btnUser12.setText("Bottle 8")
self.ui.btnUser12.clicked.connect(self.driveToBottle8)
#!/usr/bin/python3
"""!
Test the gripper
TODO: Use this file and modify as you see fit to test gripper
"""
import os
script_path = os.path.dirname(os.path.realpath(__file__))
os.sys.path.append(os.path.realpath(script_path + '/../'))
import time
from rexarm import Rexarm
rexarm = Rexarm()
rexarm.initialize(config_file=script_path +
'/../config/gripper_only_config.csv')
# Test blocking versions
rexarm.open_gripper_blocking()
rexarm.close_gripper_blocking()
rexarm.toggle_gripper_blocking()
# Test non-blocking versions
rexarm.open_gripper()
time.sleep(1)
rexarm.close_gripper()
time.sleep(1)
rexarm.toggle_gripper()
time.sleep(1)
rexarm.disable_torque()
