def __init__(self, worker_num, worker_port, master_port,
master_heartbeat_port, url):
pid = os.getpid()
self.worker_num = worker_num
self.worker_port = worker_port
self.master_port = master_port
self.master_heartbeat_port = master_heartbeat_port
self.url = url
self.rex = Rexarm()
## create TCP socket on the given worker_port_number; TCP socket to receive msg; create an INET, STREAMing socket
worker_tcp_listen = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
worker_tcp_listen.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
worker_tcp_listen.bind(
(self.url, self.worker_port)) # bind the socket to the server
worker_tcp_listen.listen(20) # become server socket
## parameter
self.IK_succeed_flag = 0
## setup thread
setup_thread = threading.Thread(target=self.setup_thread)
setup_thread.start()
time_out_LCM_thread = threading.Thread(target=self.time_out_LCM)
time_out_LCM_thread.start()
time.sleep(1)
while True:
clientsocket, address = worker_tcp_listen.accept()
#print "Get order from master."
worker_job = clientsocket.recv(1024)
worker_msg = json.loads(worker_job.decode("utf-8"))
clientsocket.close()
self.do_job(worker_msg)
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)
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()
def __init__(self,parent=None):
QtGui.QWidget.__init__(self,parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Main Variables Using Other Classes"""
self.rex = Rexarm()
self.video = Video(cv2.VideoCapture(0))
self.world_coord = np.float32()
""" Play and Repeat Variable """
self.wayPoints = []
self.wayPointsPos = []
self.wayPointsSpeed = []
self.wayPointsTime = []
""" Other Variables """
self.last_click = np.float32([-1,-1])
self.define_template_flag = -1
self.click_point1 = np.float32([-1,-1])
self.click_point2 = np.float32([-1,-1])
self.template = None
self.targets = []
self.waypointsfp = csv.writer(open("waypoint.csv","wb"))
self.currtime = 0
""" Set GUI to track mouse """
QtGui.QWidget.setMouseTracking(self,True)
"""
Video Function
Creates a timer and calls play() function
according to the given time delay (27mm)
"""
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.play)
self._timer.start(27)
"""
LCM Arm Feedback
Creates a timer to call LCM handler continuously
No delay implemented. Reads all time
"""
self._timer2 = QtCore.QTimer(self)
self._timer2.timeout.connect(self.rex.get_feedback)
self._timer2.start()
"""
ARM Plan and Command Thread
Creates a timer to call REXARM.plan_command function continuously
"""
self._timer3 = QtCore.QTimer(self)
self._timer3.timeout.connect(self.rex.plan_command)
self._timer3.start()
"""
Connect Sliders to Function
TO DO: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI
"""
self.ui.sldrBase.valueChanged.connect(self.slider_change)
self.ui.sldrShoulder.valueChanged.connect(self.slider_change)
self.ui.sldrElbow.valueChanged.connect(self.slider_change)
self.ui.sldrWrist.valueChanged.connect(self.slider_change)
self.ui.sldrMaxTorque.valueChanged.connect(self.slider_change)
self.ui.sldrSpeed.valueChanged.connect(self.slider_change)
""" Commands the arm as the arm initialize to 0,0,0,0 angles """
self.slider_change()
""" Connect Buttons to Functions """
self.ui.btnLoadCameraCal.clicked.connect(self.load_camera_cal)
self.ui.btnPerfAffineCal.clicked.connect(self.affine_cal)
self.ui.btnTeachRepeat.clicked.connect(self.tr_initialize)
self.ui.btnAddWaypoint.clicked.connect(self.tr_add_waypoint)
self.ui.btnSmoothPath.clicked.connect(self.tr_smooth_path)
self.ui.btnPlayback.clicked.connect(self.tr_playback)
self.ui.btnLoadPlan.clicked.connect(self.tr_load)
self.ui.btnDefineTemplate.clicked.connect(self.def_template)
self.ui.btnLocateTargets.clicked.connect(self.template_match)
self.ui.btnExecutePath.clicked.connect(self.exec_path)
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Main Variables Using Other Classes"""
self.rex = Rexarm(self.ui.rdoutX, self.ui.rdoutY, self.ui.rdoutZ,
self.ui.rdoutT)
self.video = Video(cv2.VideoCapture(0))
""" Other Variables """
self.last_click = np.float32([0, 0])
""" Set GUI to track mouse """
QtGui.QWidget.setMouseTracking(self, True)
"""
Video Function
Creates a timer and calls play() function
according to the given time delay (27mm)
"""
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.play)
self._timer.start(27)
"""
LCM Arm Feedback
Creates a timer to call LCM handler continuously
No delay implemented. Reads all time
"""
self._timer2 = QtCore.QTimer(self)
self._timer2.timeout.connect(self.rex.get_feedback)
self._timer2.start()
"""
Connect Sliders to Function
LAB TASK: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI
"""
self.ui.sldrBase.valueChanged.connect(
functools.partial(self.sliderChange, 0))
self.ui.sldrShoulder.valueChanged.connect(
functools.partial(self.sliderChange, 1))
self.ui.sldrElbow.valueChanged.connect(
functools.partial(self.sliderChange, 2))
self.ui.sldrWrist.valueChanged.connect(
functools.partial(self.sliderChange, 3))
self.ui.sldrGrip1.valueChanged.connect(
functools.partial(self.sliderChange, 4))
self.ui.sldrGrip2.valueChanged.connect(
functools.partial(self.sliderChange, 5))
self.ui.sldrMaxTorque.valueChanged.connect(
functools.partial(self.sliderChange, 6))
self.ui.sldrSpeed.valueChanged.connect(
functools.partial(self.sliderChange, 7))
#Setting the poses
#Pick up position
self.ui.btnUser2.setText("Pick Up Position")
self.ui.btnUser2.clicked.connect(
functools.partial(self.setPose,
[-0.063, 0.203, -.605, -1.493, -0.107, 1.702]))
#Home position (Outside kinect view)
self.ui.btnUser4.clicked.connect(
functools.partial(self.setPose,
[-2.015, -1.89, 0.318, -1.135, -0.47, 1.723]))
#self.ui.btnUser4.clicked.connect(functools.partial(self.setPose,[0.622,1.119,-0.069,1.125]))
#self.ui.btnUser5.clicked.connect(functools.partial(self.setPose,[0,0,0,0]))
#Robot frame points. Index 3 is phi, the grasping angle with respect to the world frame
point = [0, 0.44, 0, 90 * D2R]
#Regular rexarm position
point = [0.002, 0.189, -0.056, (90 + 72) * D2R]
point = [0.004, 0.388, -0.008, (90) * D2R]
point = [-0.002, 0.361, 0, 90 * D2R]
point = [0, 0.24, -0, 02, 90 * D2R]
point = [0.14, 0.04, 0.14, 90 * D2R]
#Test Case
point = [.22, 0, .22, 18 * D2R]
#Different test cases
point = [.12, 0.22, 0.1, 90 * D2R]
point = [-0.03, -0.17, 0.074, 90 * D2R]
#Testing a pick up position (Waterbottle)
point = [0.039, -0.002, 0.35, 37 * D2R]
point = [0.18, 0, 0.294, (352 * D2R)]
point = [0.131, 0.139, -0.015, 87 * D2R]
point = [0.184, 0.202, -0.02, 38 * D2R]
self.ui.btnUser6.setText("IK on " + str(point))
self.ui.btnUser6.clicked.connect(functools.partial(self.runIK, point))
#point = [0,0.05,0.082, 90 * D2R]
#self.ui.btnUser7.setText("IK on " + str(point))
#self.ui.btnUser7.clicked.connect(functools.partial(self.runIK,np.transpose(point)))
self.ui.btnUser3.setText("Straight Position")
self.ui.btnUser3.clicked.connect(
functools.partial(self.setPose, [0, 0, 0, 0, 0, 0]))
self.ui.btnUser11.setText("Recall Position")
self.ui.btnUser11.clicked.connect(self.recall_pose)
self.ui.btnUser12.setText("Save Position")
self.ui.btnUser12.clicked.connect(self.save_pose)
""" Commands the arm as the arm initialize to 0,0,0,0 angles """
self.sliderChange(0)
""" Connect Buttons to Functions
LAB TASK: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btnUser1.setText("Affine Calibration")
self.ui.btnUser1.clicked.connect(self.affine_cal)
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Main Variables Using Other Classes"""
self.rex = Rexarm()
self.video = Video()
self.statemachine = Statemachine()
#self.video.extrinsic = np.array(self.video.getcaldata())
#self.video.inv_extrinsic = np.array(np.linalg.inv(self.video.extrinsic))
""" Other Variables """
self.last_click = np.float32([0, 0])
""" Set GUI to track mouse """
QtGui.QWidget.setMouseTracking(self, True)
self.statemachine.setme(self.ui, self.rex)
"""
GUI timer
Creates a timer and calls update_gui() function
according to the given time delay (27ms)
"""
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.update_gui)
self._timer.start(27)
"""
LCM Arm Feedback timer
Creates a timer to call LCM handler for Rexarm feedback
"""
self._timer2 = QtCore.QTimer(self)
self._timer2.timeout.connect(self.rex.get_feedback)
self._timer2.start()
"""
Giri - Statemachine timer
"""
#self._timer3 = QtCore.QTimer(self)
#self._timer3.timeout.connect(self.statemachine.timerCallback)
#self._timer3.start(100) # frequency of the timer is set by this. it is 100 ms now. However, the time.now can give microseconds precision not just millisecond
"""
Connect Sliders to Function
TODO: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI
"""
self.ui.sldrBase.valueChanged.connect(self.sliderBaseChange)
self.ui.sldrShoulder.valueChanged.connect(
self.sliderShoulderChange) #Giri
self.ui.sldrElbow.valueChanged.connect(self.sliderElbowChange) #Giri
self.ui.sldrWrist.valueChanged.connect(self.sliderWristChange) #Giri
self.ui.sldrGrip1.valueChanged.connect(self.sliderGrip1Change) #Giri
self.ui.sldrGrip2.valueChanged.connect(self.sliderGrip2Change) #Giri
self.ui.sldrMaxTorque.valueChanged.connect(
self.sliderOtherChange) #Giri
self.ui.sldrSpeed.valueChanged.connect(self.sliderOtherChange) #Giri
if (self.rex.num_joints == 6):
self.ui.sldrGrip1.setEnabled(True) #Giri
self.ui.sldrGrip2.setEnabled(True) #Giri
else:
self.ui.sldrGrip1.setEnabled(False) #Giri
self.ui.sldrGrip2.setEnabled(False) #Giri
""" Initial command of the arm to home position"""
self.sliderChange()
self.reset()
""" Connect Buttons to Functions
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btnUser1.setText("Configure Servos")
self.ui.btnUser1.clicked.connect(self.btn1) #Giri
self.ui.btnUser2.setText("Depth and RGB Calibration")
self.ui.btnUser2.clicked.connect(self.btn2) #Giri
self.ui.btnUser2.setEnabled(False)
self.ui.btnUser3.setText("Extrinsic Calibration") #Giri
self.ui.btnUser3.clicked.connect(self.btn3)
self.ui.btnUser4.setText("Block Detector") #Giri
self.ui.btnUser4.clicked.connect(self.btn4)
self.ui.btnUser5.setText("Repeat")
self.ui.btnUser5.clicked.connect(self.btn5) #Giri
self.ui.btnUser6.setText("Teach")
self.ui.btnUser6.clicked.connect(self.teach) #Giri
self.ui.btnUser7.setText("Smooth Repeat")
self.ui.btnUser7.clicked.connect(self.srepeat) #Giri
self.ui.btnUser7.setEnabled(False) #Giri
self.ui.btnUser8.setText("Pick Block")
self.ui.btnUser8.clicked.connect(self.pick)
self.ui.btnUser9.setText("Place Block")
self.ui.btnUser9.clicked.connect(self.place)
self.ui.btnUser10.setText("Reset")
self.ui.btnUser10.clicked.connect(self.reset) #Giri
self.ui.btnUser11.setText("Enter Competition Mode")
self.ui.btnUser11.clicked.connect(self.competition) #Giri
self.ui.btnUser12.setText("Emergency Stop!")
self.ui.btnUser12.clicked.connect(self.estop)
def __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)
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_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)
""" Slots attach callback functions to signals emitted from threads"""
# Convert image to Qt image for display.
def convertImage(self, image):
qimg = QImage(image, image.shape[1], image.shape[0],
QImage.Format_RGB888)
return QPixmap.fromImage(qimg)
# TODO: Add more QImage in the list for visualization and debugging
@pyqtSlot(list)
def setImage(self, image_list):
rgb_image = image_list[0]
apriltag_image = image_list[1]
block_image = image_list[2]
if (self.ui.radioVideo.isChecked()):
# self.ui.videoDisplay.setPixmap(self.convertImage(rgb_image))
self.rgb_image = rgb_image
self.sm.rgb_image = rgb_image
if (self.ui.radioApril.isChecked()):
self.ui.videoDisplay.setPixmap(self.convertImage(apriltag_image))
if (self.ui.radioUsr1.isChecked()):
self.ui.videoDisplay.setPixmap(self.convertImage(block_image))
@pyqtSlot(list)
def updateAprilTags(self, tags):
# print('UPDATING APRIL TAGS')
self.sm.tags = tags
@pyqtSlot(list)
def updateImage(self, image):
# self.sm.image = image
# detectColor(self.sm, self.sm.image)
pass
@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)))
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])))
@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_current_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.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.sldrGrip1.value() * D2R
])
self.rexarm.set_positions(joint_positions, update_now=False)
def directControlChk(self, state):
if state == Qt.Checked:
self.sm.set_current_state("manual")
self.ui.SliderFrame.setEnabled(True)
else:
self.sm.set_current_state("idle")
self.ui.SliderFrame.setEnabled(False)
def trackMouse(self):
"""
Mouse position presentation in GUI
TODO: Display the rgb/hsv value
"""
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.rdoutRGB.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,-)" % (x, y))
rgb = self.rgb_image[y, x] # [r, g, b] of this pixel
hsv_image = cv2.cvtColor(self.rgb_image, cv2.COLOR_RGB2HSV)
hsv = hsv_image[y, x]
# self.ui.rdoutRGB.setText("({},{},{})".format(*rgb))
# self.ui.rdoutRGB.setText("({},{},{})".format(*hsv))
self.ui.rdoutRGB.setText(hue_to_classification(hsv[0]))
print(hsv[0])
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
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)
""" 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)
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):
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):
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)
"""
"""
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
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)
class Gui(QtGui.QMainWindow):
"""
Main GUI Class
It contains the main function and interfaces between
the GUI and functions
"""
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Main Variables Using Other Classes"""
self.rex = Rexarm(self.ui)
self.video = Video(cv2.VideoCapture(0))
"""
Zhentao added Here.
Initialize the statemachine.
"""
self.statemanager = StateManager(self.rex, self.video)
""" Other Variables """
self.last_click = np.float32([0, 0])
""" Set GUI to track mouse """
QtGui.QWidget.setMouseTracking(self, True)
"""
Video Function
Creates a timer and calls play() function
according to the given time delay (27mm)
"""
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.play)
self._timer.start(27)
"""
LCM Arm Feedback
Creates a timer to call LCM handler continuously
No delay implemented. Reads all time
"""
self._timer2 = QtCore.QTimer(self)
self._timer2.timeout.connect(self.rex.get_feedback)
self._timer2.start()
"""
Connect Sliders to Function
LAB TASK: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI
"""
## TODO: IMPLEMENT GRIP VALUE CONTROLS ##
self.ui.sldrBase.valueChanged.connect(self.sliderChange)
self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
""" Commands the arm as the arm initialize to 0,0,0,0 angles """
self.sliderChange()
""" Connect Buttons to Functions
LAB TASK: NAME AND CONNECT BUTTONS AS NEEDED
"""
#This is needed.
self.ui.btnUser1.setText("Affine Calibration")
self.ui.btnUser1.clicked.connect(self.affine_cal)
self.ui.btnUser2.setText("SM Test")
self.ui.btnUser2.clicked.connect(self.iTestSM)
self.ui.btnUser3.setText("Reset Position")
self.ui.btnUser3.clicked.connect(self.rex.iResetPosition)
self.ui.btnUser4.setText("Replay")
self.ui.btnUser4.clicked.connect(self.iReplay)
"""
self.ui.btnUser4.setText("OpenGripper")
self.ui.btnUser4.clicked.connect(self.iTestGripperOpen)
self.ui.btnUser5.setText("CloseGripper")
self.ui.btnUser5.clicked.connect(self.iTestGripperClose)
"""
"""
self.ui.btnUser2.setText("STEP1: Reset Position")
self.ui.btnUser2.clicked.connect(self.iResetPosition)
self.ui.btnUser3.clicked.connect(self.iResetTorqueAndSpeed)
self.ui.btnUser3.setText("STEP2: Reset Torque and Speed")
self.ui.btnUser4.setText("STEP3: Train Begin")
self.ui.btnUser4.clicked.connect(self.iTrainBegin)
self.ui.btnUser5.setText("STEP4(r): GetWayPoint")
self.ui.btnUser5.clicked.connect(self.iGetWayPoint)
self.ui.btnUser5.setEnabled(False)
self.ui.btnUser6.setText("STEP5: Stop Recording")
self.ui.btnUser6.clicked.connect(self.iTrainStop)
self.ui.btnUser6.setEnabled(False)
self.ui.btnUser7.clicked.connect(self.iReplayBegin)
self.ui.btnUser7.setText("Replay WholeWay")
self.ui.btnUser8.clicked.connect(self.iReplayWPTBegin_FAST)
self.ui.btnUser8.setText("Replay WayPoint(FAST)")
self.ui.btnUser9.clicked.connect(self.iReplayWPTBegin_SLOW)
self.ui.btnUser9.setText("Replay WayPoint(SLOW)")
self.ui.btnUser10.setText("PlayStop")
self.ui.btnUser10.clicked.connect(self.iReplayStop)
self.ui.btnUser11.setText("Save WPT Data")
self.ui.btnUser11.clicked.connect(self.iSaveData)
self.ui.btnUser12.setText("Load WPT Data")
self.ui.btnUser12.clicked.connect(self.iLoadData)
"""
def play(self):
self.statemanager.Statemanager_MoveToNextState()
"""
Play Funtion
Continuously called by GUI
"""
""" Renders the Video Frame """
try:
self.video.captureNextFrame()
#self.video.blobDetector()
self.ui.videoFrame.setPixmap(self.video.convertFrame())
self.ui.videoFrame.setScaledContents(True)
except TypeError:
print "No frame"
"""
Update GUI Joint Coordinates Labels on Sliders
LAB TASK: include the other slider labels
"""
self.ui.rdoutBaseJC.setText(
str("%.2f" % (self.rex.joint_angles_fb[0] * R2D)))
self.ui.rdoutShoulderJC.setText(
str("%.2f" % (self.rex.joint_angles_fb[1] * R2D)))
self.ui.rdoutElbowJC.setText(
str("%.2f" % (self.rex.joint_angles_fb[2] * R2D)))
self.ui.rdoutWristJC.setText(
str("%.2f" % (self.rex.joint_angles_fb[3] * R2D)))
"""
Mouse position presentation in GUI
TO DO: after getting affine calibration make the apprriate label
to present the value of mouse position in world coordinates
"""
x = QtGui.QWidget.mapFromGlobal(self, QtGui.QCursor.pos()).x()
y = QtGui.QWidget.mapFromGlobal(self, QtGui.QCursor.pos()).y()
if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-)")
self.ui.rdoutMouseWorld.setText("(-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
self.ui.rdoutMousePixels.setText("(%.0f,%.0f)" % (x, y))
if (self.video.aff_flag == 2):
"""
ZHENTAO: STATE CONTROL: after finishing the camera calibration, move to the next states.
"""
"""
########################################
TED added world_coords label to frame
########################################
"""
world_coords = np.dot(self.video.aff_matrix,
np.array([[x], [y], [1]]))
self.ui.rdoutMouseWorld.setText(
"(%.0f,%.0f)" % (world_coords[0], world_coords[1]))
else:
self.ui.rdoutMouseWorld.setText("(-,-)")
"""
Set button avalibity.
"""
#self.iPrintStatusTerminal()########################################Here should be activated.
self.iSetButtonAbility()
self.iUpdateStatusBar()
"""
Updates status label when rexarm playback is been executed.
This will be extended to include other appropriate messages
"""
"""
if(self.rex.plan_status == 1):
self.ui.rdoutStatus.setText("Playing Back - Waypoint %d"
%(self.rex.wpt_number + 1))
if (self.rex.plan_status == 0 and self.rex.way_total == 0):
self.ui.rdoutStatus.setText("Click [Train Begin] button to start train.")
if (self.rex.plan_status == 2):
self.ui.rdoutStatus.setText("Click [Get Way Point] button to record way point. Click [Stop Recording] to stop recording.")
if (self.rex.plan_status == 0 and self.rex.way_total != 0 and self.rex.way_number == 0):
self.ui.rdoutStatus.setText("Click [Replay Wholeway] to play the whole way. Click [Save Data] to Save the data.")
if (self.rex.plan_status == 5):
self.ui.rdoutStatus.setText("Click [Play Stop] to stop")
"""
"""###############################################
Frank Added Here
###############################################"""
if (self.rex.plan_status == 2):
self.ui.sldrBase.setProperty("value",
self.rex.joint_angles_fb[0] * R2D)
self.ui.sldrShoulder.setProperty("value",
self.rex.joint_angles_fb[1] * R2D)
self.ui.sldrElbow.setProperty("value",
self.rex.joint_angles_fb[2] * R2D)
self.ui.sldrWrist.setProperty("value",
self.rex.joint_angles_fb[3] * R2D)
self.iTrain_AddOneWay()
# replay continuously recorded waypoints
if (self.rex.plan_status == 5):
self.iReplay_PlayOneWay()
# replay manually recorded waypoints
if (self.rex.plan_status == 3 or self.rex.plan_status == 4):
self.iReplayWPT_PlayOneWay()
self.iShowFK()
def sliderChange(self):
"""
Function to change the slider labels when sliders are moved
and to command the arm to the given position
TO DO: Implement for the other sliders
"""
self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))
self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))
self.ui.rdoutGrip1.setText(str(self.ui.sldrGrip1.value()))
self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
self.rex.max_torque = self.ui.sldrMaxTorque.value() / 100.0
self.rex.speed = self.ui.sldrSpeed.value() / 100.0
#self.rex.joint_angles[0] = self.ui.sldrBase.value()*D2R
#self.rex.joint_angles[1] = self.ui.sldrShoulder.value()*D2R
#self.rex.joint_angles[2] = self.ui.sldrElbow.value()*D2R
#elf.rex.joint_angles[3] = self.ui.sldrWrist.value()*D2R
self.rex.joint_angles[4] = self.ui.sldrGrip1.value() * D2R
self.rex.cmd_publish()
def mousePressEvent(self, QMouseEvent):
"""
Function used to record mouse click positions for
affine calibration
"""
""" Get mouse posiiton """
x = QMouseEvent.x()
y = QMouseEvent.y()
""" If mouse position is not over the camera image ignore """
if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)): return
""" Change coordinates to image axis """
self.last_click[0] = x - MIN_X
self.last_click[1] = y - MIN_Y
### TESTING BLOB DETECTION TESTING ###
#self.video.blobDetector()
### TESTING BLOB DETECTION TESTING ###
# if aff_flag is 2, affine transform has been performed
if (self.video.aff_flag == 2):
ik_wcoords = np.dot(self.video.aff_matrix,
np.array([[x - MIN_X], [y - MIN_Y], [1]]))
#self.iTestIK(ik_wcoords[0], ik_wcoords[1], 40,4*PI/4)
"""
#TODO: Temperary here to take the place of Camera that Ted will write.
"""
self.iMimicCamera(ik_wcoords[0], ik_wcoords[1])
""" If affine calibration is being performed """
if (self.video.aff_flag == 1):
""" Save last mouse coordinate """
self.video.mouse_coord[self.video.mouse_click_id] = [(x - MIN_X),
(y - MIN_Y)]
""" Update the number of used poitns for calibration """
self.video.mouse_click_id += 1
""" Update status label text """
self.ui.rdoutStatus.setText("Affine Calibration: Click Point %d" %
(self.video.mouse_click_id + 1))
"""
If the number of click is equal to the expected number of points
computes the affine calibration.
LAB TASK: Change this code to use your affine calibration routine
and NOT openCV pre-programmed function as it is done now.
"""
if (self.video.mouse_click_id == self.video.aff_npoints):
""" Perform affine calibration with OpenCV
self.video.aff_matrix = cv2.getAffineTransform(
self.video.mouse_coord,
self.video.real_coord)
"""
"""
########################################
TED added affineTransform function here
########################################
"""
self.video.aff_matrix = self.video.affineTransform()
self.video.aff_flag = 2
self.video.mouse_click_id = 0
self.video.calculateBoundaryMask()
self.video.calculateArmMask()
self.video.calculateBaseMask()
self.video.calculatePokeballMask()
self.video.generateBoundaryMask()
"""
Update status of calibration flag and number of mouse
clicks
"""
""" Updates Status Label to inform calibration is done """
self.ui.rdoutStatus.setText("Affine Transform Completed")
"""
print affine calibration matrix numbers to terminal
"""
print("[Msg]: Affine Calibration Finished.")
print("[Msg]: Affine Calibration Matrix: "),
print self.video.aff_matrix
def affine_cal(self):
"""
Function called when affine calibration button is called.
Note it only chnage the flag to record the next mouse clicks
and updates the status text label
"""
self.video.aff_flag = 1
self.video.mouse_click_id = 0
self.ui.rdoutStatus.setText("Affine Calibration: Click Point %d" %
(self.video.mouse_click_id + 1))
"""
Button 1: Reset Position
"""
def iResetTorqueAndSpeed(self):
self.rex.iSetTorque(0.0)
self.rex.iSetSpeed(0.1)
self.rex.cmd_publish()
"""
Button 3: Start Train.
"""
def iTrain_ClearRecord(self):
self.rex.wpt = []
self.rex.wpt_number = 0
self.rex.wpt_total = 0
self.rex.way = []
self.rex.way_number = 0
self.rex.way_total = 0
def iTrainBegin(self):
self.rex.iSetTorque(0.0)
self.iTrain_ClearRecord()
self.rex.plan_status = 2
"""
In Play, keep recording.
"""
def iTrain_FetchSensorData(self):
return [
self.rex.joint_angles_fb[0], self.rex.joint_angles_fb[1],
self.rex.joint_angles_fb[2], self.rex.joint_angles_fb[3]
]
# fetch list of sensor data and append to the list of waypoints continuously
def iTrain_AddOneWay(self):
SensorData = self.iTrain_FetchSensorData()
self.rex.way.append(SensorData)
self.rex.way_total = self.rex.way_total + 1 # Have such data point up to now.
"""
Get Way Point:
"""
def iGetWayPoint(self):
SensorData = self.iTrain_FetchSensorData()
currentTime = self.iGetTime_now()
SensorData.append(currentTime)
self.rex.wpt.append(SensorData)
self.rex.wpt_total = self.rex.wpt_total + 1 # Have such data point up to now.
print self.rex.wpt_number
"""
Train stop:
"""
def iTrainStop(self):
self.rex.plan_status = 0
print("Stop Recording!")
"""
Replay
"""
# Begin replaying waypoints that were continuously set
def iReplayBegin(self):
self.rex.iSetTorque(0.5)
self.rex.plan_status = 5
self.rex.way_number = 0
print("Replay Start")
"""
"""
# Stop replaying waypoints and reset waypoint number to 0
def iReplayStop(self):
self.rex.plan_status = 0
self.rex.way_number = 0
self.rex.wpt_number = 0
### TESTING TED ADDED ###
def iReplay_SetOneSensorData(self, valueIndex):
sensorData = self.rex.way[valueIndex]
self.rex.iSetJointAngle(0, sensorData[0])
self.rex.iSetJointAngle(1, sensorData[1])
self.rex.iSetJointAngle(2, sensorData[2])
self.rex.iSetJointAngle(3, sensorData[3])
self.rex.cmd_publish()
# TODO: add comments
def iReplay_PlayOneWay(self):
if (self.rex.way_number == self.rex.way_total):
self.iReplayStop()
else:
self.iReplay_SetOneSensorData(self.rex.way_number)
self.rex.way_number = self.rex.way_number + 1
"Replay WPT"
# begin replaying manually set waypoints at a slow speed
#Slow: plan_status = 3.
def iReplayWPTBegin_SLOW(self):
self.rex.iSetTorque(0.7)
self.rex.iSetSpeed(GLOBALSLOWSPEED)
self.rex.plan_status = 3
self.rex.wpt_number = 0
self.rex.recslow_total = 0
self.rex.recslow = []
#self.calcCubicCoeffs()
# begin replaying manually set waypoints at a fast speed
# Fast mode: plan_status = 4.
def iReplayWPTBegin_FAST(self):
self.rex.iSetTorque(0.7)
self.rex.iSetSpeed(GLOBALFASTSPEED)
self.rex.plan_status = 4
self.rex.wpt_number = 0
self.rex.recfast_total = 0
self.rex.recfast = []
#self.calcCubicCoeffs()
def iReplayWPT_GetSensorData(self):
return [
self.rex.joint_angles_fb[0], self.rex.joint_angles_fb[1],
self.rex.joint_angles_fb[2], self.rex.joint_angles_fb[3]
]
def iCheckIfArrived(self, target, errorTorrance):
sensorData = self.iReplayWPT_GetSensorData()
error0 = abs(target[0] - sensorData[0])
error1 = abs(target[1] - sensorData[1])
error2 = abs(target[2] - sensorData[2])
error3 = abs(target[3] - sensorData[3])
if (error0 < errorTorrance and error1 < errorTorrance
and error2 < errorTorrance and error3 < errorTorrance):
return True
else:
return False
"""
def iReplayWPT_PlayOneWay(self):
if (self.rex.wpt_number == self.rex.wpt_total):
self.iReplayStop()
else:
target = self.rex.wpt[self.rex.wpt_number]
arrived = self.iCheckIfArrived(target,GLOBALERRORTORRANCE)
if (arrived):
self.rex.wpt_number = self.rex.wpt_number + 1
print "CURRENT WAYPOINT SENT TO CALC CUBIC"
print(self.rex.wpt_number),
self.calcCubicCoeffs()
else:
self.cubicPoly()
self.rex.cmd_publish()
"""
def iReplayWPT_PlayOneWay(self):
if (self.rex.wpt_number == self.rex.wpt_total):
self.iReplayStop()
else:
target = self.rex.wpt[self.rex.wpt_number]
arrived = self.iCheckIfArrived(target, GLOBALERRORTORRANCE)
#Record one real time data into self.rex.rec list and self.rex.rec_total += 1.
#SLOW
if (self.rex.plan_status == 3):
mode = 0
#Fast:
elif (self.rex.plan_status == 4):
mode = 1
else:
print("Error: iReplayWPT_PlayOneWay: Undefined status.")
self.iRecord_JointAngleFB(mode)
if (arrived):
self.rex.wpt_number = self.rex.wpt_number + 1
else:
self.rex.iSetJointAngle(0, target[0])
self.rex.iSetJointAngle(1, target[1])
self.rex.iSetJointAngle(2, target[2])
self.rex.iSetJointAngle(3, target[3])
self.rex.cmd_publish()
#self.ui.sldrBase.setProperty("value",self.rex.joint_angles[0]*R2D)
#self.ui.sldrShoulder.setProperty("value",self.rex.joint_angles[1]*R2D)
#self.ui.sldrElbow.setProperty("value",self.rex.joint_angles[2]*R2D)
#self.ui.sldrWrist.setProperty("value",self.rex.joint_angles[3]*R2D)
"""
Button Availibity
"""
def iSetButtonAbility(self):
if (self.statemanager.currentState == STATE_CODE_INIT):
self.ui.btnUser1.setEnabled(1)
else:
self.ui.btnUser1.setEnabled(0)
if (self.statemanager.currentState == STATE_CODE_END):
self.ui.btnUser4.setEnabled(1)
else:
self.ui.btnUser4.setEnabled(0)
"""
if (self.rex.plan_status == 0):
self.ui.btnUser4.setEnabled(True)
self.ui.btnUser5.setEnabled(False)
self.ui.btnUser6.setEnabled(False)
if (self.rex.plan_status == 2):
self.ui.btnUser4.setEnabled(False)
self.ui.btnUser5.setEnabled(True)
self.ui.btnUser6.setEnabled(True)
'''Replay way'''
if (self.rex.plan_status == 0 and self.rex.way_total != 0):
self.ui.btnUser7.setEnabled(True)
else:
self.ui.btnUser7.setEnabled(False)
'''
replay way point.
'''
if (self.rex.plan_status == 0 and self.rex.wpt_total != 0):
self.ui.btnUser8.setEnabled(True)
self.ui.btnUser9.setEnabled(True)
else:
self.ui.btnUser8.setEnabled(False)
self.ui.btnUser9.setEnabled(False)
'''
save data.
'''
if (self.rex.plan_status == 0 and self.rex.way_total != 0):
self.ui.btnUser11.setEnabled(True)
else:
self.ui.btnUser11.setEnabled(False)
'''
Load Data
'''
if (self.rex.plan_status == 0):
self.ui.btnUser12.setEnabled(True)
else:
self.ui.btnUser12.setEnabled(False)
"""
def iPrintStatusTerminal(self):
print("[Status = "),
print(self.rex.plan_status),
print(",#way="),
print(self.rex.way_total),
print(",#way_now="),
print(self.rex.way_number),
print(",#wpt="),
print(self.rex.wpt_total),
print(",#wpt_now="),
print(self.rex.wpt_number),
print(",#recslow="),
print(self.rex.recslow_total),
print(",#recfast="),
print(self.rex.recfast_total),
print("]")
def iShowFK(self):
self.rex.rexarm_FK(self.rex.joint_angles_fb)
"""
print("[x]:"),
print(P0[0][0])
print("[y]:"),
print(P0[1][0])
print("[z]:"),
print(P0[2][0])
print("\n")
"""
self.ui.rdoutX.setText(str(float("{0:.2f}".format(self.rex.P0[0]))))
self.ui.rdoutY.setText(str(float("{0:.2f}".format(self.rex.P0[1]))))
self.ui.rdoutZ.setText(str(float("{0:.2f}".format(self.rex.P0[2]))))
self.ui.rdoutT.setText(str(float("{0:.2f}".format(self.rex.T * R2D))))
# function which sets the self.rex.joint_angles for each joint to the values
# calculated by the cubic polynomials as a function of time
def cubicPoly(self):
if self.rex.wpt_number == self.rex.wpt_total:
self.iReplayStop()
# TODO: break from function here
# TODO: set the start time when at the first waypoint!
for i in range(0, 4):
t = self.iGetTime_now() - self.rex.st
self.rex.joint_angles[i] = (
self.rex.cubic_coeffs[i]
)[0] #+((self.rex.cubic_coeffs[i])[1]*t)+((self.rex.cubic_coeffs[i])[2]*(t**2))+((self.rex.cubic_coeffs[i])[3]*(t**3))
# function which calculates the coefficients for the cubic polynomial function
def calcCubicCoeffs(self):
#NOTE: each array index corresponds to the joint
#TODO: Forward Kinematics + Calculate Time + Set time for moving + set tf.
v0 = [0, 0, 0, 0]
vf = [0, 0, 0, 0]
t0 = [0, 0, 0, 0]
tf = [1, 1, 1, 1]
current_wpt = self.rex.wpt_number
next_wpt = self.rex.wpt_number + 1
q0 = [
self.rex.wpt[current_wpt][0], self.rex.wpt[current_wpt][1],
self.rex.wpt[current_wpt][2], self.rex.wpt[current_wpt][3]
]
qf = [
self.rex.wpt[next_wpt][0], self.rex.wpt[next_wpt][1],
self.rex.wpt[next_wpt][2], self.rex.wpt[next_wpt][3]
]
A = []
b = []
A = list()
b = list()
# NOTE: format is Ax=b where A is the constant waypoint relation matrix,
# x is the unknown coefficients and bi is the [q0,v0,qf,vf] column vector
# for each joint
# For each joint create arrays A and b
# A: list of 4 numpy arrays that are 4x4
# b: list of 4 numpy arrays that are 4x1
# self.rex.cubic_coeffs: list of 4 numpy arrays that are 4x1
for i in range(0, 4):
A.append(
np.array([[1, t0[i], t0[i]**2, t0[i]**3],
[0, 1, 2 * t0[i], 3 * (t0[i]**2)],
[1, tf[i], tf[i]**2, tf[i]**3],
[0, 1, 2 * tf[i], 3 * (tf[i]**2)]]))
b.append(np.array([q0[i], v0[i], qf[i], vf[i]]))
self.rex.cubic_coeffs[i] = np.dot(np.linalg.inv(A[i]), b[i])
#set the start time used by the cubic
self.rex.st = self.iGetTime_now()
def iSaveData(self):
"""
The data variable to save is:
self.wpt = []
self.wpt_total = 0
self.way = []
self.way_total = 0
"""
"""
f = open(GLOBALDFILENAME_WAYNUM,'wt')
writer = csv.writer(f)
writer.writerow([self.rex.way_total])
f.close()
"""
f = open(GLOBALDFILENAME_WAY, 'wt')
writer = csv.writer(f)
for ii in range(self.rex.way_total):
writer.writerow(self.rex.way[ii])
f.close()
"""
f = open(GLOBALDFILENAME_WPTNUM,'wt')
writer = csv.writer(f)
writer.writerow([self.rex.wpt_total])
f.close()
"""
f = open(GLOBALDFILENAME_WPT, 'wt')
writer = csv.writer(f)
for ii in range(self.rex.wpt_total):
writer.writerow(self.rex.wpt[ii])
f.close()
f = open(GLOBALDFILENAME_RECSLOW, 'wt')
writer = csv.writer(f)
for ii in range(self.rex.recslow_total):
writer.writerow(self.rex.recslow[ii])
f.close()
f = open(GLOBALDFILENAME_RECFAST, 'wt')
writer = csv.writer(f)
for ii in range(self.rex.recfast_total):
writer.writerow(self.rex.recfast[ii])
f.close()
print("Saving")
def iLoadData(self):
"""
the data variable to load is:
self.wpt = []
self.wpt_total = 0
self.way = []
self.way_total = 0
"""
self.rex.wpt = []
self.rex.way = []
self.rex.wpt_total = 0
self.rex.way_total = 0
self.rex.wpt_number = 0
self.rex.way_number = 0
datafile = open(GLOBALDFILENAME_WAY, 'r')
datafileReader = csv.reader(datafile)
for ii in datafileReader:
ii = map(float, ii)
self.rex.way.append(ii)
self.rex.way_total = self.rex.way_total + 1
datafile = open(GLOBALDFILENAME_WPT, 'r')
datafileReader = csv.reader(datafile)
for ii in datafileReader:
ii = map(float, ii)
self.rex.wpt.append(ii)
self.rex.wpt_total = self.rex.wpt_total + 1
print("Loading Finished.")
def iGetTime_now(self):
return int(time.time() * 1E6)
# Record the real time joint angle feedback when replaying the way points.
# mdoe == 0: slow mode, 1: fast mode
def iRecord_JointAngleFB(self, mode):
if (mode == 0):
temp = []
#0: Time
temp.append(self.iGetTime_now())
#1-4: Joint angles.
temp.append(self.rex.joint_angles_fb[0])
temp.append(self.rex.joint_angles_fb[1])
temp.append(self.rex.joint_angles_fb[2])
temp.append(self.rex.joint_angles_fb[3])
#5-7: Forward kinematics.
temp.append(self.rex.P0[0])
temp.append(self.rex.P0[1])
temp.append(self.rex.P0[2])
temp.append(self.rex.T)
print(len(self.rex.recslow))
print(temp)
self.rex.recslow.append(temp)
self.rex.recslow_total = self.rex.recslow_total + 1
elif (mode == 1):
temp = []
temp.append(self.iGetTime_now())
temp.append(self.rex.joint_angles_fb[0])
temp.append(self.rex.joint_angles_fb[1])
temp.append(self.rex.joint_angles_fb[2])
temp.append(self.rex.joint_angles_fb[3])
temp.append(self.rex.P0[0])
temp.append(self.rex.P0[1])
temp.append(self.rex.P0[2])
temp.append(self.rex.T)
self.rex.recfast.append(temp)
print(temp)
self.rex.recfast_total = self.rex.recfast_total + 1
"""
A function to test the IK, not need for final competition.
"""
def iMimicCamera(self, x, y):
print("[Msg]: MimicCam is called.")
self.video.numPokRemain = 1
self.video.whetherFinishedCam = True
self.video.nextLocationofPokmon = [x, y]
"""
self.numPokRemain = 0
self.whetherFinishedCam = False;
self.nextLocationofPokmon = [0,0];
"""
def iTestIK(self, x, y, z, phi):
phi = self.rex.rexarm_IK_CatchAnglePlaner([x, y, z])
print("[Msg]: IK is called.")
[
validity_1, IK_conf_1, validity_2, IK_conf_2, validity_3,
IK_conf_3, validity_4, IK_conf_4
] = self.rex.rexarm_IK([x, y, z, phi], 1)
if (validity_1):
self.rex.iSetJointAngle(0, IK_conf_1[0])
self.rex.iSetJointAngle(1, IK_conf_1[1])
self.rex.iSetJointAngle(2, IK_conf_1[2])
self.rex.iSetJointAngle(3, IK_conf_1[3])
self.rex.cmd_publish()
else:
print("[Msg]: IK is not reachable.")
"""
State manager Testing.
"""
def iTestSM(self):
self.statemanager.StateManager_Test()
"""
Update the name of the state to the state bar.
"""
def iUpdateStatusBar(self):
if (self.video.aff_flag == 1):
pass
if (self.statemanager.currentState == STATE_CODE_INIT):
self.ui.rdoutStatus.setText("STATE_CODE_INIT")
if (self.statemanager.currentState == STATE_CODE_RP):
self.ui.rdoutStatus.setText("STATE_CODE_RP")
if (self.statemanager.currentState == STATE_CODE_MTB):
self.ui.rdoutStatus.setText("STATE_CODE_MTB")
if (self.statemanager.currentState == STATE_CODE_MTFT):
self.ui.rdoutStatus.setText("STATE_CODE_MTFT")
if (self.statemanager.currentState == STATE_CODE_CP):
self.ui.rdoutStatus.setText("STATE_CODE_CP")
if (self.statemanager.currentState == STATE_CODE_OG):
self.ui.rdoutStatus.setText("STATE_CODE_OG")
if (self.statemanager.currentState == STATE_CODE_CCACFP):
self.ui.rdoutStatus.setText("STATE_CODE_CCACFP")
if (self.statemanager.currentState == STATE_CODE_END):
self.ui.rdoutStatus.setText("STATE_CODE_END")
if (self.statemanager.currentState == STATE_CODE_RAP):
self.ui.rdoutStatus.setText("STATE_CODE_RAP")
"""
STATE_CODE_INIT = 1
STATE_CODE_CCACFP = 2
STATE_CODE_OG = 3
STATE_CODE_MTFT = 4
STATE_CODE_CP = 5
STATE_CODE_MTB = 6
STATE_CODE_RP = 7
STATE_END = 8
"""
def iTestGripperOpen(self):
self.rex.rexarm_gripper_grab(1)
def iTestGripperClose(self):
self.rex.rexarm_gripper_grab(0)
def iReplay(self):
if (self.statemanager.currentState == STATE_CODE_END):
self.statemanager.currentState = STATE_CODE_INIT
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)
""" 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)
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(QtGui.QMainWindow):
"""
Main GUI Class
contains the main function and interfaces between
the GUI and functions
"""
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Main Variables Using Other Classes"""
self.rex = Rexarm()
self.video = Video()
self.statemachine = Statemachine()
#self.video.extrinsic = np.array(self.video.getcaldata())
#self.video.inv_extrinsic = np.array(np.linalg.inv(self.video.extrinsic))
""" Other Variables """
self.last_click = np.float32([0, 0])
""" Set GUI to track mouse """
QtGui.QWidget.setMouseTracking(self, True)
self.statemachine.setme(self.ui, self.rex)
"""
GUI timer
Creates a timer and calls update_gui() function
according to the given time delay (27ms)
"""
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.update_gui)
self._timer.start(27)
"""
LCM Arm Feedback timer
Creates a timer to call LCM handler for Rexarm feedback
"""
self._timer2 = QtCore.QTimer(self)
self._timer2.timeout.connect(self.rex.get_feedback)
self._timer2.start()
"""
Giri - Statemachine timer
"""
#self._timer3 = QtCore.QTimer(self)
#self._timer3.timeout.connect(self.statemachine.timerCallback)
#self._timer3.start(100) # frequency of the timer is set by this. it is 100 ms now. However, the time.now can give microseconds precision not just millisecond
"""
Connect Sliders to Function
TODO: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI
"""
self.ui.sldrBase.valueChanged.connect(self.sliderBaseChange)
self.ui.sldrShoulder.valueChanged.connect(
self.sliderShoulderChange) #Giri
self.ui.sldrElbow.valueChanged.connect(self.sliderElbowChange) #Giri
self.ui.sldrWrist.valueChanged.connect(self.sliderWristChange) #Giri
self.ui.sldrGrip1.valueChanged.connect(self.sliderGrip1Change) #Giri
self.ui.sldrGrip2.valueChanged.connect(self.sliderGrip2Change) #Giri
self.ui.sldrMaxTorque.valueChanged.connect(
self.sliderOtherChange) #Giri
self.ui.sldrSpeed.valueChanged.connect(self.sliderOtherChange) #Giri
if (self.rex.num_joints == 6):
self.ui.sldrGrip1.setEnabled(True) #Giri
self.ui.sldrGrip2.setEnabled(True) #Giri
else:
self.ui.sldrGrip1.setEnabled(False) #Giri
self.ui.sldrGrip2.setEnabled(False) #Giri
""" Initial command of the arm to home position"""
self.sliderChange()
self.reset()
""" Connect Buttons to Functions
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btnUser1.setText("Configure Servos")
self.ui.btnUser1.clicked.connect(self.btn1) #Giri
self.ui.btnUser2.setText("Depth and RGB Calibration")
self.ui.btnUser2.clicked.connect(self.btn2) #Giri
self.ui.btnUser2.setEnabled(False)
self.ui.btnUser3.setText("Extrinsic Calibration") #Giri
self.ui.btnUser3.clicked.connect(self.btn3)
self.ui.btnUser4.setText("Block Detector") #Giri
self.ui.btnUser4.clicked.connect(self.btn4)
self.ui.btnUser5.setText("Repeat")
self.ui.btnUser5.clicked.connect(self.btn5) #Giri
self.ui.btnUser6.setText("Teach")
self.ui.btnUser6.clicked.connect(self.teach) #Giri
self.ui.btnUser7.setText("Smooth Repeat")
self.ui.btnUser7.clicked.connect(self.srepeat) #Giri
self.ui.btnUser7.setEnabled(False) #Giri
self.ui.btnUser8.setText("Pick Block")
self.ui.btnUser8.clicked.connect(self.pick)
self.ui.btnUser9.setText("Place Block")
self.ui.btnUser9.clicked.connect(self.place)
self.ui.btnUser10.setText("Reset")
self.ui.btnUser10.clicked.connect(self.reset) #Giri
self.ui.btnUser11.setText("Enter Competition Mode")
self.ui.btnUser11.clicked.connect(self.competition) #Giri
self.ui.btnUser12.setText("Emergency Stop!")
self.ui.btnUser12.clicked.connect(self.estop)
def estop(self):
self.statemachine.estop(self.rex)
def trim_angle(self, q, motorno=1): #for 1,2,3
if (motorno > 2):
max_angle = 300
else:
max_angle = 360
if (q > max_angle or q < -1 * max_angle):
new_q = 0
print "trimmed: " + str(q) + " motor no: " + str(motorno)
elif (q > 180): #181 --> -179
new_q = q - 360
print "trimmed: " + str(q) + " motor no: " + str(motorno)
elif (q < -180):
new_q = 360 - q
print "trimmed: " + str(q) + " motor no: " + str(motorno)
else:
new_q = q
return new_q
def shortorientation(self, w_angle):
if (w_angle > 90):
sw_angle = w_angle - 90
elif (w_angle < -90):
sw_angle = w_angle + 90
else:
sw_angle = w_angle
return sw_angle
def update_gui(self):
"""
update_gui function
Continuously called by timer1
"""
""" Renders the video frames
Displays a dummy image if no video available
HINT: you can use the radio buttons to select different
video sources like is done below
"""
color = self.statemachine.timerCallback()
if (color == "Decluttered"):
self.declutter()
elif (color != "none"):
self.getangles(color)
if (self.video.kinectConnected == 1):
try:
self.video.captureVideoFrame()
self.video.captureDepthFrame()
except TypeError:
self.video.kinectConnected = 0
self.video.loadVideoFrame()
self.video.loadDepthFrame()
if (self.ui.radioVideo.isChecked()):
self.ui.videoFrame.setPixmap(self.video.convertFrame())
if (self.ui.radioDepth.isChecked()):
self.ui.videoFrame.setPixmap(self.video.convertDepthFrame())
"""
Update GUI Joint Coordinates Labels
"""
self.ui.rdoutBaseJC.setText(
str("%.2f" % (self.rex.joint_angles_fb[0] * R2D)))
self.ui.rdoutShoulderJC.setText(
str("%.2f" % (self.rex.joint_angles_fb[1] * R2D)))
self.ui.rdoutElbowJC.setText(
str("%.2f" % (self.rex.joint_angles_fb[2] * R2D)))
self.ui.rdoutWristJC.setText(
str("%.2f" % (self.rex.joint_angles_fb[3] * R2D)))
self.ui.rdoutGrip1.setText(str(self.ui.sldrGrip1.value())) #Giri
self.ui.rdoutGrip2.setText(str(self.ui.sldrGrip2.value())) #Giri
"""
Update End Effector Coordinates Kevin
"""
endCoord = forwardKinematics(self.rex.joint_angles_fb[0] * R2D,
self.rex.joint_angles_fb[1] * R2D,
self.rex.joint_angles_fb[2] * R2D,
self.rex.joint_angles_fb[3] * R2D)
self.ui.rdoutX.setText(str("%2f" % (round(endCoord[0], 2))))
self.ui.rdoutY.setText(str("%2f" % (round(endCoord[1], 2))))
self.ui.rdoutZ.setText(str("%2f" % (round(endCoord[2], 2))))
self.ui.rdoutT.setText(str("%2f" % (round(endCoord[3], 2))))
"""
Mouse position presentation in GUI
TODO: after implementing workspace calibration
display the world coordinates the mouse points to
in the RGB video image.
"""
x = QtGui.QWidget.mapFromGlobal(self, QtGui.QCursor.pos()).x()
y = QtGui.QWidget.mapFromGlobal(self, QtGui.QCursor.pos()).y()
if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-,-)")
self.ui.rdoutMouseWorld.setText("(-,-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
z = self.video.currentDepthFrame[y][x]
self.ui.rdoutMousePixels.setText("(%.0f,%.0f,%.0f)" % (x, y, z))
if (self.video.cal_flag == 2):
M = self.video.aff_matrix
v = np.float32([x, y, 1])
rw = np.dot(M, v)
self.ui.rdoutMouseWorld.setText("(%.0f,%.0f,0)" %
(rw[0], rw[1]))
elif (self.video.extrinsic_cal_flag == 2): #Josh
xy_in_rgb = np.float32([[[x, y]]]) #Josh
xy_in_rgb_homogenous = np.array([x, y, 1.0])
self.video.rgb2dep_aff_matrix = np.array(
[[1.09403672e0, 3.44369111e-3, -1.62867595e0],
[-2.41966785e-3, 1.07534829e0, -2.69041712e1],
[0., 0., 1.]]) #Josh
xy_in_dep = np.dot(self.video.rgb2dep_aff_matrix,
xy_in_rgb_homogenous) #Josh
x_dep = int(xy_in_dep[0]) #Josh
y_dep = int(xy_in_dep[1]) #Josh
d = self.video.currentDepthFrame[y_dep][x_dep]
Z = -1
#Z = 0.1236*np.tan(d/2842.5 + 1.1863)*1000
for idx in range(len(self.video.levels_mm)):
if d <= self.video.levels_upper_d[
idx] and d >= self.video.levels_lower_d[idx]:
Z = 941 - self.video.levels_mm[idx]
break
camera_coord = Z * cv2.undistortPoints(
xy_in_rgb, self.video.intrinsic,
self.video.distortion_array)
camera_coord_homogenous = np.transpose(
np.append(camera_coord, [Z, 1.0]))
world_coord_homogenous = np.dot(self.video.extrinsic,
camera_coord_homogenous)
#camera_coord = Z*np.dot(self.video.inv_intrinsic,xy_in_rgb)
#camera_coord_homo = np.concatenate((camera_coord,[1.]),axis = 0)
#world_coord_homo = np.dot(self.video.extrinsic,camera_coord_homo)
self.ui.rdoutMouseWorld.setText(
"(%.0f,%.0f,%.0f)" %
(world_coord_homogenous[0], world_coord_homogenous[1],
world_coord_homogenous[2]))
#self.ui.rdoutMouseWorld.setText("(%.0f,%.0f,%.0f)" % (x_dep,y_dep,d))
else:
self.ui.rdoutMouseWorld.setText("(-,-,-)")
"""
Updates status label when rexarm playback is been executed.
This can be extended to include other appropriate messages
"""
if (self.rex.plan_status == 1):
self.ui.rdoutStatus.setText("Playing Back - Waypoint %d" %
(self.rex.wpt_number + 1))
self.ui.rdoutStatus.setText(
self.statemachine.getmestatus(combined=True))
def sliderChange(self):
"""
Function to change the slider labels when sliders are moved
and to command the arm to the given position
"""
self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
self.rex.torque_multiplier = self.ui.sldrMaxTorque.value() / 100.0
self.rex.speed_multiplier = [self.ui.sldrSpeed.value() / 100.0
] * self.rex.num_joints
self.rex.joint_angles[0] = self.ui.sldrBase.value() * D2R
self.rex.joint_angles[1] = self.ui.sldrShoulder.value() * D2R
self.rex.joint_angles[2] = self.ui.sldrElbow.value() * D2R
self.rex.joint_angles[3] = self.ui.sldrWrist.value() * D2R
if (self.rex.num_joints == 6):
self.rex.joint_angles[4] = self.ui.sldrWrist.value() * D2R
self.rex.joint_angles[5] = self.ui.sldrWrist.value() * D2R
self.rex.cmd_publish()
def sliderinitChange(self): #Giri
"""
Function to change the slider labels when sliders are moved
and to command the arm to the given position
"""
self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value())) #Giri
self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value())) #Giri
self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value())) #Giri
self.ui.rdoutGrip1.setText(str(self.ui.sldrGrip1.value())) #Giri
self.ui.rdoutGrip2.setText(str(self.ui.sldrGrip2.value())) #Giri
self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
self.rex.torque_multiplier = self.ui.sldrMaxTorque.value() / 100.0
self.rex.speed_multiplier = [self.ui.sldrSpeed.value() / 100.0
] * self.rex.num_joints
#Giri{
self.rex.joint_angles[0] = self.ui.sldrBase.value() * D2R
self.rex.joint_angles[1] = self.ui.sldrShoulder.value() * D2R
self.rex.joint_angles[2] = self.ui.sldrElbow.value() * D2R
self.rex.joint_angles[3] = self.ui.sldrWrist.value() * D2R
if (len(self.rex.joint_angles) == 6):
self.rex.joint_angles[4] = self.ui.sldrGrip1.value() * D2R
self.rex.joint_angles[5] = self.ui.sldrGrip2.value() * D2R
#Giri}
self.rex.cmd_publish()
#From Giri: I implemented all these function in a one function above, incase if the response time is bad, we can go back to the below five separate functions
def sliderOtherChange(self): #Giri
self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
self.rex.torque_multiplier = self.ui.sldrMaxTorque.value() / 100.0
self.rex.speed_multiplier = [self.ui.sldrSpeed.value() / 100.0
] * self.rex.num_joints
self.rex.cmd_publish()
def sliderBaseChange(self): #Kevin
self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
self.rex.joint_angles[0] = self.ui.sldrBase.value() * D2R
self.rex.cmd_publish()
def sliderShoulderChange(self): #Kevin
self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))
self.rex.joint_angles[1] = self.ui.sldrShoulder.value() * D2R
self.rex.cmd_publish()
def sliderElbowChange(self): #Kevin
self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
self.rex.joint_angles[2] = self.ui.sldrElbow.value() * D2R
self.rex.cmd_publish()
def sliderWristChange(self): #Kevin
self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))
self.rex.joint_angles[3] = self.ui.sldrWrist.value() * D2R
self.rex.cmd_publish()
def sliderGrip1Change(self): #Giri
self.ui.rdoutGrip1.setText(str(self.ui.sldrGrip1.value()))
self.rex.joint_angles[4] = self.ui.sldrGrip1.value() * D2R
self.rex.cmd_publish()
def sliderGrip2Change(self): #Giri
self.ui.rdoutGrip2.setText(str(self.ui.sldrGrip2.value()))
self.rex.joint_angles[5] = self.ui.sldrGrip2.value() * D2R
self.rex.cmd_publish()
def mousePressEvent(self, QMouseEvent):
"""
Function used to record mouse click positions for calibration
"""
""" Get mouse posiiton """
x = QMouseEvent.x()
y = QMouseEvent.y()
""" If mouse position is not over the camera image ignore """
if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)): return
""" Change coordinates to image axis """
self.last_click[0] = x - MIN_X
self.last_click[1] = y - MIN_Y
""" If calibration has been performed """
if (self.video.cal_flag == 1):
""" Save last mouse coordinate """
self.video.mouse_coord[self.video.mouse_click_id] = [(x - MIN_X),
(y - MIN_Y)]
""" Update the number of used poitns for calibration """
self.video.mouse_click_id += 1
""" Update status label text """
self.ui.rdoutStatus.setText(
"Affine Calibration: Click Point %d is recorded, please select Click Point %d"
% (self.video.mouse_click_id, self.video.mouse_click_id + 1))
"""
If the number of click is equal to the expected number of points
computes the affine calibration.
LAB TASK: Change this code to use your workspace calibration routine
and NOT the simple calibration function as is done now.
"""
if (self.video.mouse_click_id == self.video.cal_points):
"""
Update status of calibration flag and number of mouse
clicks
"""
self.video.cal_flag = 2
self.video.mouse_click_id = 0
""" Perform affine calibration with OpenCV """
self.video.aff_matrix = cv2.getAffineTransform(
self.video.mouse_coord, self.video.real_coord)
#print self.video.mouse_coord #Josh
#print self.video.real_coord #Josh
""" Updates Status Label to inform calibration is done """
self.ui.rdoutStatus.setText("Waiting for input")
if (self.video.dep2rgb_aff_flag == 1): #Josh
if (self.video.mouse_click_id < self.video.rgb_pts_num):
self.video.rgb_coord[self.video.mouse_click_id * 2] = x - MIN_X
self.video.rgb_coord[self.video.mouse_click_id * 2 +
1] = y - MIN_Y
else:
self.video.dep_coord[2 * (self.video.mouse_click_id -
self.video.rgb_pts_num)] = [
(x - MIN_X), (y - MIN_Y), 1, 0,
0, 0
]
self.video.dep_coord[
2 * (self.video.mouse_click_id - self.video.rgb_pts_num) +
1] = [0, 0, 0, (x - MIN_X), (y - MIN_Y), 1]
self.video.mouse_click_id += 1
print self.video.mouse_click_id
if (self.video.mouse_click_id == self.video.dep_pts_num):
self.video.dep2rgb_aff_flag = 2
self.video.mouse_click_id = 0
A = self.video.dep_coord
A_trans = np.transpose(A)
A_AT = np.dot(A_trans, A)
A_AT_inv = np.linalg.inv(A_AT)
A_all = np.dot(A_AT_inv, A_trans)
b = np.transpose(self.video.rgb_coord)
tmp_dep2rgb_aff_array = np.dot(A_all, b)
upper_dep2rgb_aff_matrix = np.reshape(tmp_dep2rgb_aff_array,
(2, 3))
lower_dep2rgb_aff_matrix = np.array([[0, 0, 1]])
self.video.dep2rgb_aff_matrix = np.concatenate(
(upper_dep2rgb_aff_matrix, lower_dep2rgb_aff_matrix),
axis=0)
self.video.rgb2dep_aff_matrix = np.linalg.inv(
self.video.dep2rgb_aff_matrix)
print self.video.rgb2dep_aff_matrix
if (self.video.extrinsic_cal_flag == 1): #Josh
self.video.extrinsic_cal_pixel_coord[self.video.mouse_click_id] = [
(x - MIN_X), (y - MIN_Y)
]
self.video.mouse_click_id += 1
if (self.video.mouse_click_id == 3):
"""
retval, rvec, tvec = cv2.solvePnP(self.video.extrinsic_cal_world_coord, self.video.extrinsic_cal_pixel_coord, self.video.intrinsic, self.video.distortion_array)
#print self.video.extrinsic_cal_pixel_coord, self.video.extrinsic_cal_world_coord
self.video.mouse_click_id = 0
R,tmp = cv2.Rodrigues(rvec)
tmp = np.concatenate((np.transpose(R),-1.0*tvec),axis=1)
self.video.extrinsic = np.concatenate((tmp, np.float32([[0., 0., 0., 1.]])),axis = 0)
print self.video.extrinsic
self.video.inv_extrinsic = np.linalg.inv(self.video.extrinsic)
self.video.extrinsic_cal_flag = 2
"""
self.video.mouse_click_id = 0
tmp_coord = np.float32(
[[self.video.extrinsic_cal_pixel_coord[0]],
[self.video.extrinsic_cal_pixel_coord[1]],
[self.video.extrinsic_cal_pixel_coord[2]]])
camera_coord = 941.0 * cv2.undistortPoints(
tmp_coord, self.video.intrinsic,
self.video.distortion_array)
rot_rad = -1.0 * np.arctan2(
(camera_coord[1][0][1] - camera_coord[0][0][1]),
(camera_coord[1][0][0] - camera_coord[0][0][0]))
if rot_rad < 0:
z_rot = rot_rad + 0.5 * np.pi
else:
z_rot = 0.5 * np.pi - rot_rad
R_T = np.array([[np.cos(z_rot),
np.sin(z_rot), 0.0],
[np.sin(z_rot), -1.0 * np.cos(z_rot), 0.0],
[0.0, 0.0, -1.0]])
R = np.transpose(R_T)
T = np.float32([
(camera_coord[0][0][0] + camera_coord[2][0][0]) / 2,
(camera_coord[0][0][1] + camera_coord[2][0][1]) / 2, 941
])
T = np.transpose(np.array([-1.0 * np.dot(R, T)]))
tmp = np.concatenate((R, T), axis=1)
self.video.extrinsic = np.concatenate(
(tmp, np.float32([[0., 0., 0., 1.]])), axis=0)
#self.video.writecaldata(self.video.extrinsic)
self.video.inv_extrinsic = np.linalg.inv(self.video.extrinsic)
print "Extrinsic Matrix = ", self.video.extrinsic
self.video.extrinsic_cal_flag = 2
def deprgb_cali(self): #Josh
self.video.dep2rgb_aff_flag = 1
self.ui.rdoutStatus.setText(
"Start camera calibration by clicking mouse to select Click Point 1"
)
def extrinsic_cali(self): #Josh
alpha = 525.91386088
u0 = 312.41480582
beta = 526.52064559
v0 = 247.78962122
matrix1 = np.float32([[1 / alpha, 0., 0.], [0., 1 / beta, 0.],
[0., 0., 1.0]])
matrix2 = np.float32([[1.0, 0., -u0], [0., 1., -v0], [0., 0., 1.0]])
self.video.inv_intrinsic = np.dot(matrix1, matrix2)
self.video.intrinsic = np.float32([[alpha, 0., u0], [0., beta, v0],
[0., 0., 1.]])
self.video.distortion_array = np.float32([
2.45479098e-01, -8.27672136e-01, 1.05870966e-03, -3.01947277e-03,
1.08683931e+00
])
self.video.extrinsic_cal_flag = 1
def teach(self): #Giri
[x, y, z, orientation] = [0, 14, 1, 180.0]
q = [0.0, 0.0, 0.0, 0.0]
[q[0], q[1], q[2], q[3]] = inverseKinematics(x, y, z, orientation)
print q
self.rex.joint_angles[0] = q[0] * D2R
self.rex.joint_angles[1] = q[1] * D2R
self.rex.joint_angles[2] = q[2] * D2R
self.rex.joint_angles[3] = q[3] * D2R
self.rex.cmd_publish()
def repeat(self): #Giri
if (self.ui.btnUser7.text() == "Repeat"):
self.statemachine.init(self.ui, self.rex, "Initialising")
else:
self.statemachine.repeat(self.ui, self.rex, False) #Giri
def srepeat(self): #Giri
self.statemachine.init(self.ui, self.rex, "initialising")
def reset(self, hold=False):
self.rex.torque_multiplier = self.ui.sldrMaxTorque.value() / 100.0
self.rex.speed_multiplier = [self.ui.sldrSpeed.value() / 100.0
] * self.rex.num_joints
self.rex.joint_angles[0] = 0.0
self.rex.joint_angles[1] = 0.0
self.rex.joint_angles[2] = 0.0
self.rex.joint_angles[3] = 0.0
if (len(self.rex.joint_angles) == 6 and hold == False):
self.rex.joint_angles[4] = 0.0
self.rex.joint_angles[5] = 95 * D2R
elif (len(self.rex.joint_angles) == 6 and hold == True):
self.rex.joint_angles[4] = 0.0
self.rex.joint_angles[5] = -95 * D2R
self.rex.cmd_publish()
def getheight(self, angle, action="picking"):
placement_offset = 1 # in cm
if (angle - 90 < 0.1):
gripping_height = 3
elif (angle - 120 < 0.1):
gripping_height = 2
else:
if (action == "picking"):
gripping_height = 0.05
elif (action == "placing"):
gripping_height = 0.05 + placement_offset
return gripping_height
def getIK(self, endCoord, angle=0, action="picking"):
intermediate_height = 4
intermediate_angles = [0.0] * 6
angles = [0.0] * 6
[angles[0], angles[1], angles[2], angles[3]] = inverseKinematics(
endCoord[0], endCoord[1],
endCoord[2] + self.getheight(endCoord[3], action), endCoord[3])
if (self.checkfound(angles)
): # or self.checkfound(intermediate_angles,True)):
if (abs(endCoord[3] - 90) < 0.1):
endCoord[3] = 180
[angles[0], angles[1], angles[2],
angles[3]] = inverseKinematics(
endCoord[0], endCoord[1],
endCoord[2] + self.getheight(endCoord[3], action),
endCoord[3])
else:
endCoord[3] = 90
[angles[0], angles[1], angles[2],
angles[3]] = inverseKinematics(
endCoord[0], endCoord[1],
endCoord[2] + self.getheight(endCoord[3], action),
endCoord[3])
if (self.checkfound(angles)
): # or self.checkfound(intermediate_angles,True)):
endCoord[3] = 120
[angles[0], angles[1], angles[2], angles[3]] = inverseKinematics(
endCoord[0] - 1, endCoord[1] + 1,
endCoord[2] + self.getheight(endCoord[3], action), endCoord[3])
if (endCoord[3] == 120):
[
intermediate_angles[0], intermediate_angles[1],
intermediate_angles[2], intermediate_angles[3]
] = inverseKinematics(endCoord[0] - 1, endCoord[1] + 1,
endCoord[2] + intermediate_height,
endCoord[3]) # assuming it is reachable
if (self.checkfound(intermediate_angles)):
[
intermediate_angles[0], intermediate_angles[1],
intermediate_angles[2], intermediate_angles[3]
] = inverseKinematics(endCoord[0] - 1, endCoord[1] + 1,
endCoord[2] + intermediate_height,
90) # assuming it is reachable
if (endCoord[3] == 180):
[
intermediate_angles[0], intermediate_angles[1],
intermediate_angles[2], intermediate_angles[3]
] = inverseKinematics(endCoord[0], endCoord[1],
endCoord[2] + intermediate_height +
self.getheight(endCoord[3], action),
endCoord[3]) # assuming it is reachable
print self.trim_angle(orientation(angles[0], angle * R2D))
if (action == "placing"):
angles[4] = 90 + angles[0]
else:
angles[4] = -1 * self.trim_angle(
orientation(angles[0], angle * R2D))
else:
angles[4] = 0
self.checkfound(intermediate_angles, True)
intermediate_angles[4] = angles[4]
angles[5] = endCoord[3]
intermediate_angles[5] = endCoord[3]
if (self.checkfound(angles)
and self.checkfound(intermediate_angles, False)):
print "Cannot be picked"
return [angles, intermediate_angles]
def checkfound(self, angles, i=False):
if (i):
if (angles[0] == 0 and angles[1] == 0 and angles[2] == 0
and angles[3] == 0):
print "intermediate angles not found"
return True
else:
if (angles[0] == 0 and angles[1] == 0 and angles[2] == 0
and angles[3] == 0):
print "final angles not found"
return True
return False
def printfk(self, angles):
temp = forwardKinematics(angles[0], angles[1], angles[2], angles[3])
print "Forward Kinematics output = ", str(temp[0]), str(temp[1]), str(
temp[2]), str(temp[3])
def roundoff(self, angles):
angles[0] = round(self.trim_angle(angles[0]), 2)
angles[1] = round(self.trim_angle(angles[1]), 2)
angles[2] = round(self.trim_angle(angles[2]), 2)
angles[3] = round(self.trim_angle(angles[3]), 2)
angles[4] = round(self.trim_angle(angles[4]),
2) #Dont alter these two angles
angles[5] = round(self.trim_angle(angles[5]), 2)
return angles
def pick(self): #Josh
global putx, puty, putz, putangle
blockx, blocky, blockz, angle = self.get_color_block_world_coord(
'blue')
#self.statemachine.setorientation(angle*R2D)
[putx, puty, putz, putangle] = [-1 * blockx, blocky, blockz, angle]
#endCoord = [20.2, -16.1, 4.4,90]
#endCoord = [14.3, 19.4, 4.4,90]
endCoord = [
blockx / 10, blocky / 10, (blockz) / 10, gripper_orientation
]
[angles, intermediate_angles] = self.getIK(endCoord, angle)
if (debug):
print "Block detector output = ", [
blockx, blocky, blockz, angle * R2D
]
print "Input to Inverse Kinematics:", endCoord
print "Output of Inverse Kinematics: ", angles
self.printfk(angles)
angles = self.roundoff(angles)
intermediate_angles = self.roundoff(intermediate_angles)
self.statemachine.setq(angles, intermediate_angles)
self.statemachine.setmystatus(
"testing", "picking", "picking") #mode="testing",action="picking")
#self.statemachine.hold(self.ui,self.rex, angles,"picking")
def place(self):
endCoord = [putx / 10, puty / 10, (putz) / 10, gripper_orientation]
[angles, intermediate_angles] = self.getIK(endCoord, putangle)
#wangle = (self.shortorientation((putangle)*R2D-45)-self.trim_angle(angles[0]))#self.shortorientation((angle)*R2D+45)
#self.statemachine.set_orientations(wangle,2)
if (debug):
print "Placing according to Block detector output = ", [
putx, puty, putz, putangle * R2D
]
print "Input to Inverse Kinematics:", endCoord
self.printfk(angles)
angles = self.roundoff(angles)
intermediate_angles = self.roundoff(intermediate_angles)
self.statemachine.setq(angles, intermediate_angles)
self.statemachine.setmystatus(
"testing", "placing", "placing") #mode="testing",action="placing")
# self.statemachine.hold(self.ui,self.rex, angles,"placing")
def get_color_block_world_coord(self, color): #Josh
self.video.blockDetector()
[blockx_rgb, blocky_rgb, angle] = self.video.color_detection(color)
if (blockx_rgb == 0 and blocky_rgb == 0 and angle == 0):
return 0, 0, 0, 0
xy_in_rgb = np.float32([[[blocky_rgb, blockx_rgb]]]) #Josh
xy_in_rgb_homogenous = np.array([blocky_rgb, blockx_rgb, 1.0])
xy_in_dep = np.dot(self.video.rgb2dep_aff_matrix,
xy_in_rgb_homogenous) #Josh
x_dep = int(xy_in_dep[0]) #Josh
y_dep = int(xy_in_dep[1]) #Josh
d = self.video.currentDepthFrame[y_dep][x_dep]
Z = -1
for idx in range(len(self.video.levels_mm)):
if d <= self.video.levels_upper_d[
idx] and d >= self.video.levels_lower_d[idx]:
Z = 941 - self.video.levels_mm[idx]
break
camera_coord = Z * cv2.undistortPoints(xy_in_rgb, self.video.intrinsic,
self.video.distortion_array)
camera_coord_homogenous = np.transpose(
np.append(camera_coord, [Z, 1.0]))
world_coord_homogenous = np.dot(self.video.extrinsic,
camera_coord_homogenous)
theta = np.arccos(self.video.extrinsic[0, 0])
angle = angle - theta
return world_coord_homogenous[0], world_coord_homogenous[
1], world_coord_homogenous[2], angle
def getangles(self, color):
blockx, blocky, blockz, angle = self.get_color_block_world_coord(color)
if (blockx == 0 and blocky == 0 and blockx == 0 and angle == 0):
angles = [0] * 6
intermediate_angles = [0] * 6
print "not found"
self.statemachine.set_comp5_status("not found")
else:
[
current_mode, current_action, current_movement,
current_motorstate
] = self.statemachine.getmestatus()
endCoord = [(blockx) / 10, (blocky) / 10, (blockz) / 10,
gripper_orientation]
[angles, intermediate_angles] = self.getIK(endCoord, angle,
"picking")
angles = self.roundoff(angles)
intermediate_angles = self.roundoff(intermediate_angles)
print "found"
self.statemachine.set_comp5_status("found")
self.statemachine.setq(angles, intermediate_angles)
def generatecomp2(self):
print "Entered generatecomp2"
new_q = np.zeros([3, 6])
new_qh = np.zeros([3, 6]) #intermediate heights
# blockx, blocky, blockz, angle = self.get_color_block_world_coord('red')
final_x = 0
final_y = -220
blockz = 19.25
# final_x = blockx
# final_y = blocky
height = 40
endCoord = [
-(final_x) / 10, (final_y) / 10, (blockz + height * 0) / 10,
gripper_orientation
]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
new_q[0] = self.roundoff(angles)
new_qh[0] = self.roundoff(intermediate_angles)
endCoord = [
-(final_x) / 10, (final_y) / 10, (blockz + height * 1) / 10,
gripper_orientation
]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
new_q[1] = self.roundoff(angles)
new_qh[1] = self.roundoff(intermediate_angles)
endCoord = [
-(final_x) / 10, (final_y) / 10, (blockz + height * 2) / 10,
gripper_orientation
]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
new_q[2] = self.roundoff(angles)
new_qh[2] = self.roundoff(intermediate_angles)
self.statemachine.setq_comp(new_q, new_qh)
def generatecomp3(self):
print "Entered generatecomp3"
new_q = np.zeros([8, 6])
new_qh = np.zeros([8, 6]) #intermediate heights
#blockx, blocky, blockz, angle = self.get_color_block_world_coord('red')
blockz = 19.5
final_x = -150
final_y = -89.5
b = 43
endCoord = [(final_x) / 10, (final_y) / 10, (blockz) / 10,
gripper_orientation]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
new_q[0] = self.roundoff(angles)
new_qh[0] = self.roundoff(intermediate_angles)
endCoord = [(final_x + b * 1) / 10, (final_y) / 10, (blockz) / 10,
gripper_orientation]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
new_q[1] = self.roundoff(angles)
new_qh[1] = self.roundoff(intermediate_angles)
print "Genereated FK: ", forwardKinematics(new_q[1][0], new_q[1][1],
new_q[1][2], new_q[1][3])
endCoord = [(final_x + b * 2) / 10, (final_y) / 10, (blockz) / 10,
gripper_orientation]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
new_q[2] = self.roundoff(angles)
new_qh[2] = self.roundoff(intermediate_angles)
endCoord = [(final_x + b * 3) / 10, (final_y) / 10, (blockz) / 10,
gripper_orientation]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
new_q[3] = self.roundoff(angles)
new_qh[3] = self.roundoff(intermediate_angles)
endCoord = [(final_x + b * 4) / 10, (final_y) / 10, (blockz) / 10,
gripper_orientation]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
new_q[4] = self.roundoff(angles)
new_qh[4] = self.roundoff(intermediate_angles)
endCoord = [(final_x + b * 5) / 10, (final_y) / 10, (blockz) / 10,
gripper_orientation]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
new_q[5] = self.roundoff(angles)
new_qh[5] = self.roundoff(intermediate_angles)
endCoord = [(final_x + b * 6) / 10, (final_y) / 10, (blockz) / 10,
gripper_orientation]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
new_q[6] = self.roundoff(angles)
new_qh[6] = self.roundoff(intermediate_angles)
endCoord = [(final_x + b * 7) / 10, (final_y) / 10, (blockz) / 10,
gripper_orientation]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
new_q[7] = self.roundoff(angles)
new_qh[7] = self.roundoff(intermediate_angles)
self.statemachine.setq_comp(new_q, new_qh)
def generatecomp4(self):
print "Entered generatecomp4"
new_q = np.zeros([8, 6])
new_qh = np.zeros([8, 6]) #intermediate heights
blockx, blocky, blockz, angle = self.get_color_block_world_coord(
'blue')
final_x = 0
final_y = -220
height = 40
endCoord = [
-(final_x) / 10, (final_y) / 10, (blockz + height * 0) / 10,
gripper_orientation
]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
new_q[0] = self.roundoff(angles)
new_qh[0] = self.roundoff(intermediate_angles)
endCoord = [
-(final_x) / 10, (final_y) / 10, (blockz + height * 1) / 10,
gripper_orientation
]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
new_q[1] = self.roundoff(angles)
new_qh[1] = self.roundoff(intermediate_angles)
print "Genereated FK: ", forwardKinematics(new_q[1][0], new_q[1][1],
new_q[1][2], new_q[1][3])
endCoord = [
-(final_x) / 10, (final_y) / 10, (blockz + height * 2) / 10,
gripper_orientation
]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
new_q[2] = self.roundoff(angles)
new_qh[2] = self.roundoff(intermediate_angles)
endCoord = [
-(final_x) / 10, (final_y) / 10, (blockz + height * 3) / 10,
gripper_orientation
]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
new_q[3] = self.roundoff(angles)
new_qh[3] = self.roundoff(intermediate_angles)
endCoord = [
-(final_x) / 10, (final_y) / 10, (blockz + height * 4) / 10,
gripper_orientation
]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
new_q[4] = self.roundoff(angles)
new_qh[4] = self.roundoff(intermediate_angles)
endCoord = [
-(final_x) / 10, (final_y) / 10, (blockz + height * 5) / 10,
gripper_orientation
]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
new_q[5] = self.roundoff(angles)
new_qh[5] = self.roundoff(intermediate_angles)
endCoord = [
-(final_x) / 10, (final_y) / 10, (blockz + height * 6) / 10,
gripper_orientation
]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
# hard code joints angles down below here
#new_q[6] = self.roundoff(angles)
#new_qh[6] = self.roundoff(intermediate_angles)
new_q[6] = [1., 2., 66.5, 15.2, 0, 90.]
new_qh[6] = [1., 15.8, 29., 34.5, 0, 90.]
endCoord = [
-(final_x) / 10, (final_y) / 10, (blockz + height * 7) / 10,
gripper_orientation
]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
#new_q[7] = self.roundoff(angles)
#new_qh[7] = self.roundoff(intermediate_angles)
new_q[7] = [1., 15.8, 29., 34.5, 0, 90.]
new_qh[7] = [1., 23., 9., 34.5, 0, 90.]
self.statemachine.setq_comp(new_q, new_qh)
def generate_comp5(self):
print "Entered generate_comp5"
n = 3
b = 6
N = (n + 1) * n / 2
coords = self.pyramid(n, b)
new_q = np.zeros([N, 6])
new_qh = np.zeros([N, 6]) #intermediate heights
endCoord = np.zeros([N, 4])
for i in range(0, N):
endCoord[i] = [
coords[0, i], coords[1, i], coords[2, i], gripper_orientation
]
[new_q[i], new_qh[i]] = self.getIK(endCoord[i], 0, "placing")
print "pyramid: ", coords
self.statemachine.setq_comp(new_q, new_qh)
def pyramid(self, nlayers, spacing): #returns numpy array.
coords = zeros((3, nlayers * int((nlayers + 1) / 2)))
count = 0
blockheight = 3.85
for j in range(0, nlayers):
for i in range(0, nlayers - j):
print(nlayers - j - 1)
coords[0][count] = 0 + spacing * (i - ((nlayers - j) / 2.0))
coords[1][count] = -11
coords[2][count] = (blockheight * j
) #define blockheight and offset
count = count + 1
return coords
def competition(self):
if (self.ui.btnUser11.text() == "Enter Competition Mode"):
self.ui.btnUser11.setText("Enter Testing Mode")
self.ui.btnUser7.setText("Draw")
self.ui.btnUser1.setText("Competition 1")
self.ui.btnUser2.setText("Competition 2")
self.ui.btnUser3.setText("Competition 3")
self.ui.btnUser4.setText("Competition 4")
self.ui.btnUser5.setText("Competition 5")
self.ui.btnUser2.setEnabled(True)
self.ui.btnUser7.setEnabled(True)
elif (self.ui.btnUser11.text() == "Enter Testing Mode"):
self.ui.btnUser11.setText("Enter Competition Mode")
self.ui.btnUser7.setText("Smooth Repeat")
self.ui.btnUser1.setText("Configure Servos")
self.ui.btnUser2.setText("Depth and RGB Calibration")
self.ui.btnUser3.setText("Extrinsic Calibration")
self.ui.btnUser4.setText("Block Detection")
self.ui.btnUser5.setText("Repeat")
self.ui.btnUser2.setEnabled(False)
self.ui.btnUser7.setEnabled(False)
def btn1(self):
if (self.ui.btnUser1.text() == "Configure Servos"):
self.rex.cfg_publish_default()
elif (self.ui.btnUser1.text() == "Competition 1"):
if (self.sanity_check(1)):
self.getangles('red')
self.statemachine.setmystatus(
"Competition 1", "picking",
"picking") #mode="testing",action="picking")
def btn2(self):
if (self.ui.btnUser2.text() == "Depth and RGB Calibration"):
self.deprgb_cali()
elif (self.ui.btnUser2.text() == "Competition 2"):
if (self.sanity_check(2)):
self.generatecomp2()
self.getangles('red')
self.statemachine.setmystatus(
"Competition 2", "picking",
"picking") #mode="testing",action="picking")
def btn3(self):
global declutter_index, declutter_competition
if (self.ui.btnUser3.text() == "Extrinsic Calibration"):
self.extrinsic_cali()
elif (self.ui.btnUser3.text() == "Competition 3"):
declutter_index = 0
declutter_competition = "Competition 3"
self.declutter()
"""
if(self.sanity_check(3)):
self.generatecomp3()
self.getangles('black')
self.statemachine.setmystatus("Competition 3", "picking","picking")#mode="testing",action="picking")
"""
def btn4(self):
global declutter_index, declutter_competition
if (self.ui.btnUser4.text() == "Block Detector"):
self.video.blockDetector()
elif (self.ui.btnUser4.text() == "Competition 4"):
declutter_index = 0
declutter_competition = "Competition 4"
self.declutter()
"""
if(self.sanity_check(4)):
self.generatecomp4()
self.getangles('black')
self.statemachine.setmystatus("Competition 4", "picking","picking")#mode="testing",action="picking")
"""
def btn5(self):
if (self.ui.btnUser5.text() == "Repeat"):
self.repeat()
elif (self.ui.btnUser5.text() == "Competition 5"):
self.generate_comp5()
self.getangles('all')
self.statemachine.setmystatus(
"Competition 5", "picking",
"picking") #mode="testing",action="picking")
def declutter(self):
blockx, blocky, blockz, angle = self.video.search_highest()
levels_mm = np.float32(
[0., 19.25, 57.75, 96.25, 134.75, 173.25, 211.75, 250.25])
level = (blockz - 19.25) / 38.5
print "level = ", level, "and levels_mm = ", levels_mm
if (declutter_index == 2):
if (declutter_competition == "Competition 3"):
if (self.sanity_check(3)):
self.generatecomp3()
self.getangles('black')
self.statemachine.setmystatus(
"Competition 3", "picking",
"picking") #mode="testing",action="picking")
elif (declutter_competition == "Competition 4"):
if (self.sanity_check(4)):
self.generatecomp4()
self.getangles('black')
self.statemachine.setmystatus(
"Competition 4", "picking",
"picking") #mode="testing",action="picking")
if (level >= 1 and declutter_index < 4):
endCoord = [(blockx) / 10, (blocky) / 10, (blockz) / 10,
gripper_orientation]
print endCoord
[angles, intermediate_angles] = self.getIK(endCoord, 0, "picking")
print "from declutter IK output: ", [angles, intermediate_angles]
angles = self.roundoff(angles)
intermediate_angles = self.roundoff(intermediate_angles)
if (not self.checkfound(angles)
): # and not self.checkfound(intermediate_angles,True)):
self.generatedeclutter()
self.statemachine.setq(angles, intermediate_angles)
self.statemachine.setmystatus(
"Declutter", "picking",
"picking") #mode="testing",action="picking")
else:
if (declutter_competition == "Competition 3"):
if (self.sanity_check(3)):
self.generatecomp3()
self.getangles('black')
self.statemachine.setmystatus(
"Competition 3", "picking",
"picking") #mode="testing",action="picking")
elif (declutter_competition == "Competition 4"):
if (self.sanity_check(4)):
self.generatecomp4()
self.getangles('black')
self.statemachine.setmystatus(
"Competition 4", "picking",
"picking") #mode="testing",action="picking")
def generatedeclutter(self):
print "Entered generatedeclutter"
global declutter_index
if (declutter_index < 4):
x = [0, 250, 0, 180]
y = [-240, -50, 240, -150]
final_x = x[declutter_index]
final_y = y[declutter_index]
final_z = 19.25
height = 20
endCoord = [(final_x) / 10, (final_y) / 10,
(final_z + height) / 10, gripper_orientation]
[angles, intermediate_angles] = self.getIK(endCoord, 0, "placing")
new_q = self.roundoff(angles)
new_qh = self.roundoff(intermediate_angles)
declutter_index = declutter_index + 1
self.statemachine.setq_comp(new_q, new_qh)
def sanity_check(self, comp):
success = False
if (comp == 1 or comp == 2): #search for red,blue,green
colors = ["red", "blue", "green"]
[pos_colors, success] = self.video.blockDetector(colors)
elif (comp == 4 or comp == 3): #search for red,blue,green
colors = [
"red", "blue", "green", "orange", "violet", "black", "yellow",
"pink"
]
[pos_colors, success] = self.video.blockDetector(colors)
return success
"""main function"""
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(QtGui.QMainWindow):
"""
Main GUI Class
It contains the main function and interfaces between
the GUI and functions
"""
def __init__(self, parent=None):
QtGui.QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Main Variables Using Other Classes"""
self.rex = Rexarm(self.ui.rdoutX, self.ui.rdoutY, self.ui.rdoutZ,
self.ui.rdoutT)
self.video = Video(cv2.VideoCapture(0))
""" Other Variables """
self.last_click = np.float32([0, 0])
""" Set GUI to track mouse """
QtGui.QWidget.setMouseTracking(self, True)
"""
Video Function
Creates a timer and calls play() function
according to the given time delay (27mm)
"""
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.play)
self._timer.start(27)
"""
LCM Arm Feedback
Creates a timer to call LCM handler continuously
No delay implemented. Reads all time
"""
self._timer2 = QtCore.QTimer(self)
self._timer2.timeout.connect(self.rex.get_feedback)
self._timer2.start()
"""
Connect Sliders to Function
LAB TASK: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI
"""
self.ui.sldrBase.valueChanged.connect(
functools.partial(self.sliderChange, 0))
self.ui.sldrShoulder.valueChanged.connect(
functools.partial(self.sliderChange, 1))
self.ui.sldrElbow.valueChanged.connect(
functools.partial(self.sliderChange, 2))
self.ui.sldrWrist.valueChanged.connect(
functools.partial(self.sliderChange, 3))
self.ui.sldrGrip1.valueChanged.connect(
functools.partial(self.sliderChange, 4))
self.ui.sldrGrip2.valueChanged.connect(
functools.partial(self.sliderChange, 5))
self.ui.sldrMaxTorque.valueChanged.connect(
functools.partial(self.sliderChange, 6))
self.ui.sldrSpeed.valueChanged.connect(
functools.partial(self.sliderChange, 7))
#Setting the poses
#Pick up position
self.ui.btnUser2.setText("Pick Up Position")
self.ui.btnUser2.clicked.connect(
functools.partial(self.setPose,
[-0.063, 0.203, -.605, -1.493, -0.107, 1.702]))
#Home position (Outside kinect view)
self.ui.btnUser4.clicked.connect(
functools.partial(self.setPose,
[-2.015, -1.89, 0.318, -1.135, -0.47, 1.723]))
#self.ui.btnUser4.clicked.connect(functools.partial(self.setPose,[0.622,1.119,-0.069,1.125]))
#self.ui.btnUser5.clicked.connect(functools.partial(self.setPose,[0,0,0,0]))
#Robot frame points. Index 3 is phi, the grasping angle with respect to the world frame
point = [0, 0.44, 0, 90 * D2R]
#Regular rexarm position
point = [0.002, 0.189, -0.056, (90 + 72) * D2R]
point = [0.004, 0.388, -0.008, (90) * D2R]
point = [-0.002, 0.361, 0, 90 * D2R]
point = [0, 0.24, -0, 02, 90 * D2R]
point = [0.14, 0.04, 0.14, 90 * D2R]
#Test Case
point = [.22, 0, .22, 18 * D2R]
#Different test cases
point = [.12, 0.22, 0.1, 90 * D2R]
point = [-0.03, -0.17, 0.074, 90 * D2R]
#Testing a pick up position (Waterbottle)
point = [0.039, -0.002, 0.35, 37 * D2R]
point = [0.18, 0, 0.294, (352 * D2R)]
point = [0.131, 0.139, -0.015, 87 * D2R]
point = [0.184, 0.202, -0.02, 38 * D2R]
self.ui.btnUser6.setText("IK on " + str(point))
self.ui.btnUser6.clicked.connect(functools.partial(self.runIK, point))
#point = [0,0.05,0.082, 90 * D2R]
#self.ui.btnUser7.setText("IK on " + str(point))
#self.ui.btnUser7.clicked.connect(functools.partial(self.runIK,np.transpose(point)))
self.ui.btnUser3.setText("Straight Position")
self.ui.btnUser3.clicked.connect(
functools.partial(self.setPose, [0, 0, 0, 0, 0, 0]))
self.ui.btnUser11.setText("Recall Position")
self.ui.btnUser11.clicked.connect(self.recall_pose)
self.ui.btnUser12.setText("Save Position")
self.ui.btnUser12.clicked.connect(self.save_pose)
""" Commands the arm as the arm initialize to 0,0,0,0 angles """
self.sliderChange(0)
""" Connect Buttons to Functions
LAB TASK: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btnUser1.setText("Affine Calibration")
self.ui.btnUser1.clicked.connect(self.affine_cal)
#Holds the current rexarm position when torque has been set to zero
def save_pose(self):
self.saved_angles = self.rex.joint_angles_fb[:]
def recall_pose(self):
self.setPose(self.saved_angles)
#Takes a list of [x,y,z] in kinect coordinates
def kinect_world_to_rexarm_world(self, kinect_coords):
x = kinect_coords[0]
y = kinect_coords[1]
z = -kinect_coords[2]
#produce
rot_angle = -30 * D2R
rot_x = np.array([[1, 0, 0, 0],
[0, np.cos(rot_angle), -np.sin(rot_angle), 0],
[0, np.sin(rot_angle),
np.cos(rot_angle), 0], [0, 0, 0, 1]])
rot_angle = 90 * D2R
rot_z = np.array([[np.cos(rot_angle), -np.sin(rot_angle), 0, 0],
[np.sin(rot_angle),
np.cos(rot_angle), 0, 0], [0, 0, 1, 0],
[0, 0, 0, 1]])
translation = np.array([[1, 0, 0, .072], [0, 1, 0, 0],
[0, 0, 1, -.625], [0, 0, 0, 1]])
inv_xform = np.dot(np.dot(rot_x, rot_z), translation)
#Invert the matrix produced so we can go from kinect coordinates to rexarm coordinates
xform = numpy.linalg.inv(inv_xform)
coords = np.array([[x], [y], [z], [1]])
rex_coords = np.dot(xform, coords)
#Clamp z if below this limit
z_limit = -0.01
if rex_coords[2][0] <= z_limit:
print "z_limit was", rex_coords[2][0], "; clamped it to", z_limit
rex_coords[2][0] = z_limit
return [rex_coords[0][0], rex_coords[1][0], rex_coords[2][0]]
#Runs inverse kinematics and only returns hardware thetas. Up to the
#user to set the pose
def runIK_noCommand(self, xyz_phi_world):
#In the robot frame
xyz_world = xyz_phi_world[0:3]
phi_world = xyz_phi_world[3]
#In the rexarm frame
xyz_rexarm = None #To be computed
#print "World Coords:", xyz_world
#Homogeneous coordinates
xyz_world = np.append(xyz_world, 1)
xyz_world = xyz_world.reshape(4, 1)
#print "Shape:", xyz.shape
#Convert xyz to rexarm coordinates (with respect to frame of point right below joint 1) by using inverse matrix
#shift_horizontal_only = self.rex.trans_base.copy()
#Don't shift along z axis. Only shift along x
#shift_horizontal_only[2][3] = 0
#import pdb
#pdb.set_trace()
#transformation = np.dot(np.dot(np.dot(self.rex.trans_magicbase,self.rex.rot_72),self.rex.rot_90),shift_horizontal_only)
#inv_transformation = np.linalg.inv(transformation)
#Convert desired IK coordinates from world frame to robot frame
#xyz_rexarm = np.dot(inv_transformation,xyz_world)
xyz_rexarm = xyz_world.copy()
xyz_rexarm = xyz_rexarm[0:3]
print "Rexarm Frame Coords:", xyz_rexarm
phi_rexarm = phi_world # - 72 * D2R #Converts grasping angle in world frame to angle in rexarm frame.
xyz_rexarm = np.append(xyz_rexarm, phi_rexarm)
xyz_rexarm = xyz_rexarm.reshape(4, 1)
#import pdb
#pdb.set_trace()
#Run Inverse kinematics
DH_thetas = self.rex.rexarm_IK(xyz_rexarm, 1)
#Send arm_thetas to the rexarm
#First convert from DH parameter angle to hardware servo angle
hardware_thetas = list(DH_thetas.copy())
for i in range(len(hardware_thetas)):
hardware_thetas[i] -= self.rex.joint_offsets[i]
# Append 0,0 for now for the remaining two joints
hardware_thetas.append(0)
hardware_thetas.append(0)
return hardware_thetas
#Runs inverse kinematics on xyz_phi_world, which has form
#[x,y,z,phi_in_radians]
def runIK(self, xyz_phi_world):
hardware_thetas = self.runIK_noCommand(xyz_phi_world)
print "Send thetas:", hardware_thetas
#Send pose to Rexarm
self.setPose(hardware_thetas)
def play(self):
"""
Play Funtion
Continuously called by GUI
"""
""" Renders the Video Frame """
try:
self.video.captureNextFrame()
self.video.blobDetector()
self.ui.videoFrame.setPixmap(self.video.convertFrame())
self.ui.videoFrame.setScaledContents(True)
except TypeError:
#print "No frame"
pass
"""
Update GUI Joint Coordinates Labels
LAB TASK: include the other slider labels
"""
self.ui.rdoutBaseJC.setText(
str("%.2f" % (self.rex.joint_angles_fb[0] * R2D)))
self.ui.rdoutShoulderJC.setText(
str("%.2f" % (self.rex.joint_angles_fb[1] * R2D)))
self.ui.rdoutElbowJC.setText(
str("%.2f" % (self.rex.joint_angles_fb[2] * R2D)))
self.ui.rdoutWristJC.setText(
str("%.2f" % (self.rex.joint_angles_fb[3] * R2D)))
"""
Mouse position presentation in GUI
TO DO: after getting affine calibration make the apprriate label
to present the value of mouse position in world coordinates
"""
x = QtGui.QWidget.mapFromGlobal(self, QtGui.QCursor.pos()).x()
y = QtGui.QWidget.mapFromGlobal(self, QtGui.QCursor.pos()).y()
if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-)")
self.ui.rdoutMouseWorld.setText("(-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
self.ui.rdoutMousePixels.setText("(%.0f,%.0f)" % (x, y))
if (self.video.aff_flag == 2):
""" TO DO Here is where affine calibration must be used """
self.ui.rdoutMouseWorld.setText("(-,-)")
else:
self.ui.rdoutMouseWorld.setText("(-,-)")
"""
Updates status label when rexarm playback is been executed.
This will be extended to includ eother appropriate messages
"""
if (self.rex.plan_status == 1):
self.ui.rdoutStatus.setText("Playing Back - Waypoint %d" %
(self.rex.wpt_number + 1))
def sliderChange(self, selected_slider):
"""
Function to change the slider labels when sliders are moved
and to command the arm to the given position
TO DO: Implement for the other sliders
"""
#print "Selected joint:", selected_slider
if 0 <= selected_slider and selected_slider <= 5:
angle = 0
if selected_slider == 0:
self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
angle = self.ui.sldrBase.value() * D2R
elif selected_slider == 1:
self.ui.rdoutShoulder.setText(str(
self.ui.sldrShoulder.value()))
angle = self.ui.sldrShoulder.value() * D2R
elif selected_slider == 2:
self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
angle = self.ui.sldrElbow.value() * D2R
elif selected_slider == 3:
self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))
angle = self.ui.sldrWrist.value() * D2R
elif selected_slider == 4:
self.ui.rdoutGrip1.setText(str(self.ui.sldrGrip1.value()))
angle = self.ui.sldrGrip1.value() * D2R
elif selected_slider == 5:
self.ui.rdoutGrip2.setText(str(self.ui.sldrGrip2.value()))
angle = self.ui.sldrGrip2.value() * D2R
self.rex.joint_angles[selected_slider] = angle
elif 6 <= selected_slider and selected_slider <= 7:
if selected_slider == 6:
self.ui.rdoutTorq.setText(
str(self.ui.sldrMaxTorque.value()) + "%")
self.rex.max_torque = self.ui.sldrMaxTorque.value() / 100.0
elif selected_slider == 7:
self.ui.rdoutSpeed.setText(
str(self.ui.sldrSpeed.value()) + "%")
self.rex.speed = self.ui.sldrSpeed.value() / 100.0
else:
print "Error: Unrecognized slider index", selected_slider
self.rex.cmd_publish()
# angles is a list of floats, an angle for each joint
#Use this with state machine since lcm feedback handler isn't working.
#current_angles is provided by Rexarm state machine since rex.joint_angles_fb
#isn't being updated with the rexarm lcm handler
#Returns the new pose as a variable
def setPose(self, desired_angles, current_angles=[]):
#Use the linear motion plan that the professor explained
step_size = 0.04
t_range = list(np.arange(0, 1 + step_size, step_size))
desired_angles = np.array(desired_angles)
initial_angles = None
if len(current_angles) == 0:
initial_angles = np.array(self.rex.joint_angles_fb)
else:
initial_angles = np.array(current_angles)
for t in t_range:
new_pose = desired_angles * t + initial_angles * (1 - t)
#Sends motor command to rexarm
for i in range(len(new_pose)):
#Obey joint_limits to prevent breaking motors
if new_pose[i] < self.rex.joint_limits[i][0]:
new_pose[i] = self.rex.joint_limits[i][0]
if new_pose[i] > self.rex.joint_limits[i][1]:
new_pose[i] = self.rex.joint_limits[i][1]
self.rex.joint_angles[i] = new_pose[i]
self.rex.cmd_publish()
time.sleep(0.1)
return desired_angles
def mousePressEvent(self, QMouseEvent):
"""
Function used to record mouse click positions for
affine calibration
"""
""" Get mouse posiiton """
x = QMouseEvent.x()
y = QMouseEvent.y()
""" If mouse position is not over the camera image ignore """
if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)): return
""" Change coordinates to image axis """
self.last_click[0] = x - MIN_X
self.last_click[1] = y - MIN_Y
""" If affine calibration is been performed """
if (self.video.aff_flag == 1):
""" Save last mouse coordinate """
self.video.mouse_coord[self.video.mouse_click_id] = [(x - MIN_X),
(y - MIN_Y)]
""" Update the number of used poitns for calibration """
self.video.mouse_click_id += 1
""" Update status label text """
self.ui.rdoutStatus.setText("Affine Calibration: Click Point %d" %
(self.video.mouse_click_id + 1))
"""
If the number of click is equal to the expected number of points
computes the affine calibration.
LAB TASK: Change this code to use your affine calibration routine
and NOT openCV pre-programmed function as it is done now.
"""
if (self.video.mouse_click_id == self.video.aff_npoints):
"""
Update status of calibration flag and number of mouse
clicks
"""
self.video.aff_flag = 2
self.video.mouse_click_id = 0
""" Perform affine calibration with OpenCV """
self.video.aff_matrix = cv2.getAffineTransform(
self.video.mouse_coord, self.video.real_coord)
""" Updates Status Label to inform calibration is done """
self.ui.rdoutStatus.setText("Waiting for input")
"""
print affine calibration matrix numbers to terminal
"""
print self.video.aff_matrix
def affine_cal(self):
"""
Function called when affine calibration button is called.
Note it only chnage the flag to record the next mouse clicks
and updates the status text label
"""
self.video.aff_flag = 1
self.ui.rdoutStatus.setText("Affine Calibration: Click Point %d" %
(self.video.mouse_click_id + 1))
#Initialize socket to send acknowledgement over
def send_socket_ack(self):
print "Sending socket acknowledgement..."
self.kinect_endpoint = "/tmp/kinect_endpoint"
#if not os.path.isfile(self.kinect_endpoint):
# open(self.kinect_endpoint, 'w').close()
try:
self.sock.sendto("1", self.kinect_endpoint)
print "Acknowledgement successfully sent."
except Exception as e:
print str(e)
print "Failed to send acknowledgement."
return
def init_socket(self):
self.sock = socket.socket(
socket.AF_UNIX, # Local computer
socket.SOCK_DGRAM) # UDP
self.kinect_path_recv = "/tmp/rexarm_endpoint"
try:
os.remove(self.kinect_path_recv)
except OSError:
pass
self.sock.bind(self.kinect_path_recv)
print "Socket binded. Ready to receive data."
# Don't need this?
#self.sock.listen(1)
#Blocks until Kinect code connects
#self.conn, self.addr = self.sock.accept()
def get_socket_data(self):
point = None
#Grasping Point struct is 28 bytes
while True:
print "Waiting for data..."
#Num bytes in grasping point struct
struct_size = 40
data, addr = self.sock.recvfrom(struct_size)
print "Received", struct_size, " bytes"
if not data:
#Error
pass
#p is point. n is normal
#TODO: Check endianness
time, p1, p2, p3, n1, n2, n3, a1, a2, a3 = struct.unpack(
"ifffffffff", data)
print "Time:", time
print "Point (mm):", p1, p2, p3
print "Normal (mm):", n1, n2, n3
print "Principal Axis (mm):", a1, a2, a3
point = [p1 / 1000.0, p2 / 1000.0, p3 / 1000.0]
axis = [a1 / 1000.0, a2 / 1000.0, a3 / 1000.0]
break
return point, axis
#Busy waits code until rexarm has reached desired pose
def wait_until_reached(self, pose, ignore_grasp=False):
self.rex.lcm_mutex.acquire()
while not self.reached_pose(pose, ignore_grasp):
#time.sleep(1)
print "Desired Pose:", pose
print "Current Rexarm Pose:", self.rex.joint_angles_fb
print "not reached desired pose yet. Thread going to sleep..."
self.rex.lcm_cv.wait()
#print "Woke up"
self.rex.lcm_mutex.release()
#Returns true if the rexarm's angles match the pose input approximately.
#Use this to repeatedly check if rexarm has reached a configuration
# pose is a list of angles for each rexarm_joint ex. [0,0,0,0,0,0]
def reached_pose(self, pose, ignore_grasp):
reached = True
allowed_error = 0.3 #radians
limit = 0
if ignore_grasp:
limit = len(self.rex.joint_angles_fb) - 1
else:
limit = len(self.rex.joint_angles_fb)
for i in range(limit):
#If error for any of the joints is > 0.01, then arm is not
#at the desired location.
if abs(self.rex.joint_angles_fb[i] - pose[i]) > allowed_error:
reached = False
break
return reached
#Instantly publishes pose to rexarm without any motion smoothing
#Used to save time
def instant_publish(self, pose):
self.rex.joint_angles = pose[:]
self.rex.cmd_publish()
#Checks if pose, a list of length 6, has angles within the range
#of the rexarm's joint limits
def check_pose_valid(self, pose):
valid = True
for i in range(len(pose)):
if pose[i] < self.rex.joint_limits[i][0] or self.rex.joint_limits[
i][1] < pose[i]:
valid = False
break
return valid
#Returns angle between x-axis and princiapl axis in range 0 to 360
#x-axis and principal axis are 3D lists
#Takes in x-axis and principal axis as 1D arrays of size 2
def compute_2D_angle(self, x_axis, princ_axis):
#Use the dot product
x_np = np.array(x_axis)
p_np = np.array(princ_axis)
#Find angle
temp = np.dot(
x_np, p_np) / (numpy.linalg.norm(x_np) * numpy.linalg.norm(p_np))
#Take arccos
angle = math.acos(temp)
#Angle is in range 0 to pi
return_angle = None
#Quadrant 1
if p_np[0] > 0 and p_np[1] >= 0:
return_angle = angle
#Quadrant 2
elif p_np[0] <= 0 and p_np[1] >= 0:
return_angle = angle
#Quadrant 3
elif p_np[0] < 0 and p_np[1] < 0:
return_angle = 2 * math.pi - angle
#Quadrant 4
elif p_np[0] >= 0 and p_np[1] < 0:
return_angle = 2 * math.pi - angle
return return_angle
#Converts vector defined in kinect coordinates to vector defined in rexarm coordinates.
#Must be lists
def kinect_vec_to_rexarm_vec(self, kinect_vec):
#Convert [0,0,0] and principal_axis point to rexarm coordinates
#Get the vector in rexarm world
start_point_rex = self.kinect_world_to_rexarm_world([0, 0, 0])
end_point_rex = self.kinect_world_to_rexarm_world(kinect_vec)
rex_vec = list(np.array(end_point_rex) - np.array(start_point_rex))
return rex_vec
#Takes in the principal axis in terms of rexarm coordinates as
# a [x,y,z] list and returns hardware angle for wrist to rotate to
def compute_wrist_angle(self, rex_axis_input):
#Project the axis vector onto the rexarm's x-y plane by setting z to 0
rex_axis = rex_axis_input[:]
rex_axis[2] = 0
#2D vector
axis = rex_axis[:2]
#Find angle between x-axis of rexarm and the rex_principal_axis in range 0
#to 360
x_axis = [1, 0]
angle_2d = self.compute_2D_angle(x_axis, axis)
print "Angle between Rexarm x axis and principal axis:", angle_2d
assert (0 <= angle_2d and angle_2d <= 2 * math.pi)
phi = 0
#Based on 4 cases, compute wrist_angle
if 0 < angle_2d and angle_2d <= math.pi / 2:
phi = -(math.pi / 2 - angle_2d)
if math.pi / 2 < angle_2d and angle_2d <= math.pi:
phi = angle_2d - math.pi / 2
if math.pi < angle_2d and angle_2d <= (3 * math.pi / 2):
phi = -(3 * math.pi / 2 - angle_2d)
if (3 * math.pi / 2) < angle_2d and angle_2d <= 2 * math.pi:
phi = angle_2d - 3 * math.pi / 2
#Add pi/2 to phi for unknown reason
phi += math.pi / 2
return phi
#base vector is [x,y,z]
def get_principal_angle(self, principal_axis_rexarm, base_vector):
#Get angle between rexarm principal axis and vector from origin
#to object.
#First project both to z = 0
p = np.array(principal_axis_rexarm[0:2])
v = np.array(base_vector[0:2])
#Flip principal axis if in quadrants 2 or 3
if p[0] < 0:
p = -p
temp = np.dot(p, v) / (numpy.linalg.norm(p) * numpy.linalg.norm(v))
angle = math.acos(temp)
return angle
def trash_state_machine(self):
#Setting the torque and speed. Ranges from 0 to 1
self.rex.max_torque = 0.55
self.rex.speed = 0.4
self.rex.cmd_publish()
#Thread to call rex.get_feedback, which enables the callback
#function to work
try:
thread.start_new_thread(self.rex.get_feedback, (False, ))
#t = Thread(target = self.rex.get_feedback, args = (False,))
#t.start()
print "Started get_feedback thread"
except:
print "Unable to start get_feedback thread"
self.init_socket()
tighten_gripper = 115 * D2R
net_base_angle = -1.71
poses = {
"HOME": [0, 0, 0, 0, 0, tighten_gripper], #Tightens gripper
"HIDE": [1.557, -2.03, -0.629, 1.079, -0.061, 1.994],
"PRE_DUNK":
[1.544, 0.233, -0.781, -0.736, -1.565, tighten_gripper],
"ABOUT_TO_DUNK":
[1.545, 0.040, -0.019, 0.046, -1.564, tighten_gripper],
"DUNK": [1.237, -0.124, 0.26, 0.854, -1.559, tighten_gripper]
#"NET_ARCH": [net_base_angle,-0.135,-1.223,-1.447,-0.061,tighten_gripper]
}
#TODO: populate this variable with the pose we're currently trying to reach
#Use this since LCM feedback handler isn't being called :(
current_pose = None
next_pose = None
#x,y,z coordinates to conduct IK for
desired_IK = []
#Angle at which to rotate wrist and grab
wrist_rot_angle = 0
#Angles to command rexarm for inverse kinematics
IK_cmd_thetas = None
principal_axis_kinect = None
#State1: Turn 90 degrees at base to prevent collision
states = [
"START", "RUN_IK_TURN_BASE", "RUN_IK_DESCEND", "GRASP", "LIFT_UP",
"TURN_TO_NET", "ARCH_TO_NET", "DROP", "UNARCH",
"TURN_TO_HOME_FROM_NET", "HIDE_POSITION_1", "HIDE_POSITION_2",
"UNHIDE", "TURN_TO_HOME_FROM_UNHIDE"
]
curr_state = "START"
synchro_timer = 1.5
start = True
linear = True
#Set to True if we're in a state where we're holding object
grasp = False
#Set to True if we're in the grasping state
clamp = False
while True:
print "----------------------------------------"
print "Current State:", curr_state
if curr_state == "START":
next_pose = poses["HOME"][:]
next_state = "HIDE_POSITION_1"
elif curr_state == "RUN_IK_TURN_BASE":
#Run_IK
IK_cmd_thetas = []
IK_successful = True
IK_message = ""
try:
IK_cmd_thetas = self.runIK_noCommand(desired_IK)
except Exception as e:
IK_successful = False
IK_message = "ERROR: IK encountered runtime error.\n" + str(
e)
if not self.check_pose_valid(IK_cmd_thetas):
IK_successful = False
IK_message = "ERROR: IK gave angles outside of feasible range.\n"
print "IK_result:", IK_cmd_thetas
#Check that the joint angles returned by IK are possible
if not IK_successful:
print IK_message
print "Returning to Hide position"
linear = False
next_state = "HIDE_POSITION_2"
else:
#Turn base towards object
next_pose[0] = IK_cmd_thetas[0]
#Twist wrist
#next_pose[4] = IK_cmd_thetas[4]
linear = False
self.instant_publish(next_pose)
print "Turned Base to:", next_pose[0]
#TODO: Get lcm joint angles returned from runIK so that
#we can wait before grasping
next_state = "TURN_WRIST"
elif curr_state == "TURN_WRIST":
#Current base angle
print "Current base angle after turning:", self.rex.joint_angles_fb[
0]
print "Principal Axis Vector in Kinect World:", principal_axis_kinect
print "Principal Axis Vector in Rexarm World:", principal_axis_rexarm
#Wrist rotation angle with respect to the x-axis of the rexarm base frame
wrist_rot_angle = self.compute_wrist_angle(
principal_axis_rexarm)
print "wrist rotation angle determined with respect to rexarm x-axis:", wrist_rot_angle
#Subtract the rotation of the base before adding the wrist_rot_angle
IK_cmd_thetas[
4] = wrist_rot_angle - self.rex.joint_angles_fb[0]
print "wrist rotation angle after subtracting base joint rotation:", IK_cmd_thetas[
4]
#Now subtract the angle between principal axis and vector from
#base to object
axis_baserot_angle = self.get_principal_angle(
principal_axis_rexarm, desired_IK[:3])
IK_cmd_thetas[4] -= axis_baserot_angle
#---------------------------TODO: Put in function
#Map it to an untangled state from -wrist_limit to wrist_limit
IK_cmd_thetas[4] = IK_cmd_thetas[4] % (2 * math.pi)
print "wrist rotation angle after modulus with 2PI:", IK_cmd_thetas[
4]
#Map it from -PI to PI
if IK_cmd_thetas[4] > math.pi:
IK_cmd_thetas[4] -= 2 * math.pi
print "wrist rotation angle after restricting to -PI to PI:", IK_cmd_thetas[
4]
if IK_cmd_thetas[4] <= -self.rex.wrist_limit:
IK_cmd_thetas[4] += math.pi
if IK_cmd_thetas[4] >= self.rex.wrist_limit:
IK_cmd_thetas[4] -= math.pi
print "wrist rotation angle after restricting to wrist range:", IK_cmd_thetas[
4]
#---------------------------TODO: Put in function
IK_cmd_thetas[4] = 0
next_pose[4] = IK_cmd_thetas[4]
linear = False
self.instant_publish(next_pose)
next_state = "RUN_IK_DESCEND"
elif curr_state == "RUN_IK_DESCEND":
print "About to descend:"
print "Current pose:", self.rex.joint_angles_fb
print "Desired Pose:", IK_cmd_thetas
#Descend the rest of the IK outside of base
next_pose = IK_cmd_thetas[:]
next_state = "GRASP"
elif curr_state == "GRASP":
#Set joint 5 to grasp
next_pose[5] = tighten_gripper
clamp = True
next_state = "PRE_DUNK"
#------------------------------------------------------------------------------------
#Lift to pre-dunk state after grasping. Ignore last joint
elif curr_state == "PRE_DUNK":
next_pose = poses["PRE_DUNK"][:]
grasp = True
next_state = "ABOUT_TO_DUNK"
elif curr_state == "ABOUT_TO_DUNK":
next_pose = poses["ABOUT_TO_DUNK"][:]
grasp = True
linear = False
self.instant_publish(next_pose)
next_state = "DUNK"
elif curr_state == "DUNK":
next_pose = poses["DUNK"][:]
grasp = True
#linear = False
#self.instant_publish(next_pose)
next_state = "DROP"
elif curr_state == "DROP":
#Set joint 5 to 0 angle
next_pose[5] = 0
linear = False
self.instant_publish(next_pose)
next_state = "RETURN_HOME"
elif curr_state == "RETURN_HOME":
next_pose = poses["ABOUT_TO_DUNK"][:]
next_state = "HOME_TURN"
elif curr_state == "HOME_TURN":
next_pose = poses["HOME"][:]
linear = False
self.instant_publish(next_pose)
next_state = "HIDE_POSITION_2"
#------------------------------------------------------------------------------------
elif curr_state == "UNHIDE":
#Go to home position. Then run IK
next_pose = poses["HOME"][:]
next_state = "RUN_IK_TURN_BASE"
elif curr_state == "HIDE_POSITION_1":
next_pose = poses["HIDE"][:]
next_state = "SOCKET_READ"
elif curr_state == "HIDE_POSITION_2":
next_pose = poses["HIDE"][:]
next_state = "SEND_ACK"
elif curr_state == "SEND_ACK":
#Acknowledge that we are done picking up and disposing of object
self.send_socket_ack()
linear = False
next_state = "SOCKET_READ"
elif curr_state == "SOCKET_READ":
#Block and wait for next point of new object
kin_point, principal_axis_kinect = self.get_socket_data()
#Convert to rexarm coordinates from kinect coordinates
rex_point = self.kinect_world_to_rexarm_world(kin_point)
#Converts principal axis from kinect coordinates to rexarm coordinates
principal_axis_rexarm = self.kinect_vec_to_rexarm_vec(
principal_axis_kinect)
#principal_axis_rexarm = [2,-1,5]
#principal_axis_rexarm = [1,0,5]
#print "Principal Axis Vector in Kinect World:", principal_axis_kinect
print "Principal Axis Vector in Rexarm World:", principal_axis_rexarm
desired_IK = [
rex_point[0], rex_point[1], rex_point[2], 87 * D2R
]
#desired_IK = [0.131,0.139,-0.015, 87 * D2R]
#desired_IK = [0.15,0.1,0.05, 87 * D2R]
#desired_IK = [0.15,0.15,0, 87 * D2R]
print "Inverse Kinematics Target:"
print "Goal x in Rexarm:", desired_IK[0]
print "Goal y in Rexarm:", desired_IK[1]
print "Goal z:", desired_IK[2]
linear = False
#Not setting next_pose
next_state = "UNHIDE"
if linear:
if start:
self.setPose(next_pose)
start = False
else:
self.setPose(next_pose, current_pose)
else:
linear = True
#Setting current_pose to whatever next_pose was
#determined to be
current_pose = next_pose[:]
if not clamp:
if not grasp:
self.wait_until_reached(next_pose)
else:
self.wait_until_reached(next_pose, ignore_grasp=True)
grasp = False
else:
time.sleep(synchro_timer)
clamp = False
print "Next State:", next_state
print "----------------------------------------"
curr_state = next_state
class Gui(QtGui.QMainWindow):
"""
Main GUI Class
It contains the main function and interfaces between
the GUI and functions
"""
def __init__(self,parent=None):
QtGui.QWidget.__init__(self,parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Main Variables Using Other Classes"""
self.rex = Rexarm()
self.video = Video(cv2.VideoCapture(0))
""" Other Variables """
self.last_click = np.float32([0,0])
""" Set GUI to track mouse """
QtGui.QWidget.setMouseTracking(self,True)
"""
Video Function
Creates a timer and calls play() function
according to the given time delay (27mm)
"""
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.play)
self._timer.start(27)
"""
LCM Arm Feedback
Creates a timer to call LCM handler continuously
No delay implemented. Reads all time
"""
self._timer2 = QtCore.QTimer(self)
self._timer2.timeout.connect(self.rex.get_feedback)
self._timer2.start()
"""
Connect Sliders to Function
TO DO: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI
"""
self.ui.sldrBase.valueChanged.connect(self.slider_change)
self.ui.sldrShoulder.valueChanged.connect(self.slider_change)
self.ui.sldrElbow.valueChanged.connect(self.slider_change)
self.ui.sldrWrist.valueChanged.connect(self.slider_change)
self.ui.sldrMaxTorque.valueChanged.connect(self.slider_change)
self.ui.sldrSpeed.valueChanged.connect(self.slider_change)
""" Commands the arm as the arm initialize to 0,0,0,0 angles """
self.slider_change()
""" Connect Buttons to Functions """
self.ui.btnLoadCameraCal.clicked.connect(self.load_camera_cal)
self.ui.btnPerfAffineCal.clicked.connect(self.affine_cal)
self.ui.btnTeachRepeat.clicked.connect(self.tr_initialize)
self.ui.btnAddWaypoint.clicked.connect(self.tr_add_waypoint)
self.ui.btnSmoothPath.clicked.connect(self.tr_smooth_path)
self.ui.btnPlayback.clicked.connect(self.tr_playback)
self.ui.btnDefineTemplate.clicked.connect(self.def_template)
self.ui.btnLocateTargets.clicked.connect(self.template_match)
self.ui.btnExecutePath.clicked.connect(self.exec_path)
self.ui.path_start_time = None
self.executed_path = []
self.get_template = 0
self.templat_point = None
self.donuts = []
def play(self):
"""
Play Funtion
Continuously called by GUI
"""
""" Renders the Video Frame """
try:
self.video.captureNextFrame()
self.ui.videoFrame.setPixmap(
self.video.convertFrame())
self.ui.videoFrame.setScaledContents(True)
except TypeError:
print "No frame"
"""
Update GUI Joint Coordinates Labels
TO DO: include the other slider labels
"""
self.ui.rdoutBaseJC.setText(str(self.rex.joint_angles_fb[0]*R2D))
self.ui.rdoutShoulderJC.setText(str(self.rex.joint_angles_fb[1]*R2D))
self.ui.rdoutElbowJC.setText(str(self.rex.joint_angles_fb[2]*R2D))
self.ui.rdoutWristJC.setText(str(self.rex.joint_angles_fb[3]*R2D))
t = [self.rex.joint_angles_fb[0],
self.rex.joint_angles_fb[1] - 90.0 * D2R,
self.rex.joint_angles_fb[2],
self.rex.joint_angles_fb[3]]
a = [0.0, 100.0, 100.0, 108.0]
d = [116.0, 0.0, 0.0, 0.0]
al = [-90.0 * D2R, 0.0 * D2R, 0.0 * D2R, 0.0 * D2R]
dh_table = [t,a,d,al]
(x, y, z, phi) = self.rex.rexarm_FK(dh_table, 3)
self.ui.rdoutX.setText(str(x))
self.ui.rdoutY.setText(str(y))
self.ui.rdoutZ.setText(str(z))
self.ui.rdoutT.setText(str(phi*R2D))
# res = self.rex.rexarm_IK((0.0,-275.0,80.0,0.0), 1)
# if res != None:
# self.rex.joint_angles[0] = res[0]
# self.rex.joint_angles[1] = res[1]
# self.rex.joint_angles[2] = res[2]
# self.rex.joint_angles[3] = res[3]
# self.rex.cmd_publish()
"""
Mouse position presentation in GUI
TO DO: after getting affine calibration make the apprriate label
to present the value of mouse position in world coordinates
"""
x = QtGui.QWidget.mapFromGlobal(self,QtGui.QCursor.pos()).x()
y = QtGui.QWidget.mapFromGlobal(self,QtGui.QCursor.pos()).y()
if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-)")
self.ui.rdoutMouseWorld.setText("(-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
self.ui.rdoutMousePixels.setText("(%.0f,%.0f)" % (x,y))
if (self.video.aff_flag == 2):
""" TO DO Here is where affine calibration must be used """
aff = self.video.aff_matrix
wx = x*aff[0][0] + y*aff[0][1] + aff[0][2]
wy = x*aff[1][0] + y*aff[1][1] + aff[1][2]
self.ui.rdoutMouseWorld.setText("(%.2f,%.2f)" %(wx,wy))
else:
self.ui.rdoutMouseWorld.setText("(-,-)")
"""
Updates status label when rexarm playback is been executed.
This will be extended to includ eother appropriate messages
THIS SHOULD BE LAST BECAUSE IT RETURNS EARLY!!!!!!!!
(Pedro don't be sad please)
"""
if(self.rex.plan_status == 1):
if self.rex.wpt_total == 0:
self.ui.rdoutStatus.setText("No waypoints!")
self.rex.plan_status = 0
else:
self.ui.rdoutStatus.setText("Playing Back - Waypoint %d"
%(self.rex.wpt_number + 1))
[start_time, prev_waypoint, prev_vel] = self.rex.plan[self.rex.wpt_number-1]
[end_time, current_waypoint, current_vel] = self.rex.plan[self.rex.wpt_number]
t = float(micro_time() - self.path_start_time)
#Linear Interp
for joint in range(4):
#Set the goal
self.rex.joint_angles[joint] = current_waypoint[joint]
#Figure the speed out
dist = abs(current_waypoint[joint] - prev_waypoint[joint])
time_left = (end_time - t)/1000000.0
v = dist/time_left
# rad/sec * sec/min * rot/rad / max RPM
speed_val = v * (60.0/(2*math.pi)) / 59.0
speed_val = max(speed_val, 1.0/1023.0)
print speed_val, time_left
if time_left < 0:
#were behind haul ass
self.rex.speed[joint] = 1.0
else:
self.rex.speed[joint] = speed_val
self.rex.cmd_publish()
#Check if we have arrived
def within_error(joint, wpt):
#print joint, wpt
error = math.fabs(joint * R2D - wpt * R2D)
#print error
return error < 5.0
if within_error(self.rex.joint_angles_fb[0], current_waypoint[0]) and \
within_error(self.rex.joint_angles_fb[1], current_waypoint[1]) and \
within_error(self.rex.joint_angles_fb[2], current_waypoint[2]) and \
within_error(self.rex.joint_angles_fb[3], current_waypoint[3]):
print "Within Error Bounds: %d" %(self.rex.wpt_number)
self.rex.wpt_number += 1
self.executed_path.append([micro_time() - self.path_start_time , self.rex.joint_angles_fb[:]])
if self.rex.wpt_number >= len(self.rex.plan):
self.rex.plan_status = 0
self.ui.rdoutStatus.setText("Plan finished!")
self.rex.speed = [0.5, 0.5, 0.5, 0.5]
self.rex.plan = self.rex.plan[1:]
with open('executed_path.config', 'w') as f:
pickle.dump(self.executed_path, f)
def slider_change(self):
"""
Function to change the slider labels when sliders are moved
and to command the arm to the given position
TO DO: Implement for the other sliders
"""
self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))
self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))
self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()))
if self.rex.plan_status == 0:
self.rex.max_torque = self.ui.sldrMaxTorque.value()/100.0
self.rex.joint_angles[0] = self.ui.sldrBase.value()*D2R
self.rex.joint_angles[1] = self.ui.sldrShoulder.value()*D2R
self.rex.joint_angles[2] = self.ui.sldrElbow.value()*D2R
self.rex.joint_angles[3] = self.ui.sldrWrist.value()*D2R
self.rex.cmd_publish()
def solveAffineMatrix(self):
A = []
b = []
for i in range(self.video.aff_npoints):
px = self.video.mouse_coord[i][0]
py = self.video.mouse_coord[i][1]
wx = self.video.real_coord[i][0]
wy = self.video.real_coord[i][1]
A.append([px, py, 1.0, 0.0, 0.0, 0.0])
A.append([0.0, 0.0, 0.0, px, py, 1.0])
b.append(wx)
b.append(wy)
print A, '\n', b
x, res, rank, s = np.linalg.lstsq(A, b)
print x
self.video.aff_matrix = [[x[0], x[1], x[2]],
[x[3], x[4], x[5]],
[0.0, 0.0, 1.0]]
def mousePressEvent(self, QMouseEvent):
"""
Function used to record mouse click positions for
affine calibration
"""
""" Get mouse posiiton """
x = QMouseEvent.x()
y = QMouseEvent.y()
""" If mouse position is not over the camera image ignore """
if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)): return
""" Change coordinates to image axis """
self.last_click[0] = x - MIN_X
self.last_click[1] = y - MIN_Y
""" If affine calibration is been performed """
if (self.video.aff_flag == 1):
""" Save last mouse coordinate """
self.video.mouse_coord[self.video.mouse_click_id] = [(x-MIN_X),
(y-MIN_Y)]
""" Update the number of used poitns for calibration """
self.video.mouse_click_id += 1
""" Update status label text """
self.ui.rdoutStatus.setText("Affine Calibration: Click point %d"
%(self.video.mouse_click_id + 1))
"""
If the number of click is equal to the expected number of points
computes the affine calibration.
TO DO: Change this code to use you programmed affine calibration
and NOT openCV pre-programmed function as it is done now.
"""
if(self.video.mouse_click_id == self.video.aff_npoints):
"""
Update status of calibration flag and number of mouse
clicks
"""
self.video.aff_flag = 2
self.video.mouse_click_id = 0
""" Perform affine calibration with OpenCV """
#self.video.aff_matrix = cv2.getAffineTransform(
# self.video.mouse_coord,
# self.video.real_coord)
self.solveAffineMatrix()
print self.video.aff_matrix
""" Updates Status Label to inform calibration is done """
self.ui.rdoutStatus.setText("Waiting for input")
"""
Uncomment to gether affine calibration matrix numbers
on terminal
"""
#print self.video.aff_matrix
if self.get_template == 1:
self.template_point = [(x-MIN_X)*(960.0/480.0), (y-MIN_Y)*(1280.0/640.0)]
self.get_template += 1
elif self.get_template == 2:
self.get_template = 0
bottom_point = [(x-MIN_X)*(960.0/480.0), (y-MIN_Y)*(1280.0/640.0)]
dx = bottom_point[0] - self.template_point[0]
dy = bottom_point[1] - self.template_point[1]
grey_frame = cv2.cvtColor(self.video.currentFrame, cv2.COLOR_BGR2GRAY)
self.template = grey_frame[self.template_point[1]-dy:bottom_point[1], self.template_point[0]-dx:bottom_point[0]]
#self.template = cv2.cvtColor(self.template, cv2.COLOR_BGR2GRAY)
cv2.imwrite('./template.png', self.template)
print "Got Template"
def affine_cal(self):
"""
Function called when affine calibration button is called.
Note it only chnage the flag to record the next mouse clicks
and updates the status text label
"""
self.video.aff_flag = 1
self.ui.rdoutStatus.setText("Affine Calibration: Click point %d"
%(self.video.mouse_click_id + 1))
def load_camera_cal(self):
print "Load Camera Cal"
self.video.loadCalibration()
def tr_initialize(self):
print "Teach and Repeat"
self.path_start_time = micro_time()
self.rex.plan = []
self.rex.plan_status = 0
self.rex.wpt_number = 0
self.rex.max_torque = 0.0
self.ui.sldrMaxTorque.setValue(0.0)
def tr_add_waypoint(self):
print "Add Waypoint"
self.rex.plan.append([micro_time() - self.path_start_time, self.rex.joint_angles_fb[:], [0,0,0,0]])
self.rex.wpt_total += 1
def tr_smooth_path(self):
print "Smooth Path"
#Basic smoothing
new_plan = []
#for i, (t, p) in enumerate(self.rex.plan):
# np = p[:]
# nt = t + 1000000 * i + 500000
# if p[1] < 0:
# np[1] += 10*D2R
# else:
# np[1] -= 10*D2R
#
# new_plan.append([nt-500000, np])
# new_plan.append([nt, p[:]])
# new_plan.append([nt+500000, np])
self.rex.plan = [[0, [0, 0, 0, 0], [0,0,0,0]]] + self.rex.plan
#Handle all other points
for i in range(1,len(self.rex.plan)):
[tprev, qprev, vprev] = self.rex.plan[i-1]
[ti, qi, vi] = self.rex.plan[i]
dt = (ti - tprev) * 0.3
nprev = qprev[:]
if nprev[1] < 0:
nprev[1] += 10*D2R
else:
nprev[1] -= 10*D2R
ni = qi[:]
if ni[1] < 0:
ni[1] += 10*D2R
else:
ni[1] -= 10*D2R
new_plan.append([tprev + dt, nprev, vprev])
new_plan.append([ti - dt, ni, vi])
new_plan.append([ti, qi[:], vi])
#Handle last point
[ti, pi, vi] = self.rex.plan[-1]
ni = qi[:]
if ni[1] < 0:
ni[1] += 10*D2R
else:
ni[1] -= 10*D2R
new_plan.append([ti+500000, ni, vi])
self.rex.plan = new_plan[1:]
#Cubic spline smoothing
# new_plan = [self.rex.plan[0][:]]
# for i in range(1,len(self.rex.plan)):
# [start_time, prev_waypoint, prev_vel] = self.rex.plan[i-1]
# [end_time, current_waypoint, current_vel] = self.rex.plan[i]
# a0 = [0,0,0,0]
# a1 = [0,0,0,0]
# a2 = [0,0,0,0]
# a3 = [0,0,0,0]
# for joint in range(4):
# def calc_params(t0, q0, v0, tf, qf, vf):
# a0 = q0
# a1 = v0
# a2 = (3.0*(qf-q0)-(2.0*v0+vf)*(tf-t0))/math.pow(tf-t0, 2)
# a3 = (2.0*(q0-qf) + (v0+vf)*(tf-t0))/math.pow(tf-t0, 3)
# return a0, a1, a2, a3
# a0[joint], a1[joint], a2[joint], a3[joint] = calc_params(0.0,\
# float(prev_waypoint[joint]),\
# float(prev_vel[joint]),\
# 1.0,\
# float(current_waypoint[joint]),\
# float(current_vel[joint]))
# duration = end_time - start_time
# for frac in range(1,100,10):
# frac = float(frac) / 100.0
# time = start_time + int(duration * frac)
# q = [0,0,0,0]
# for joint in range(4):
# q[joint] = a0[joint] + a1[joint] * frac + a2[joint] * frac * frac + a3[joint] * frac * frac * frac
# new_plan.append([time, q[:], [0.0, 0.0, 0.0, 0.0]])
# #Add the destination point
# new_plan.append(self.rex.plan[i][:])
# with open('smooth_path.config', 'w') as f:
# print len(new_plan)
# pickle.dump(new_plan, f)
# with open('path.config', 'w') as f:
# pickle.dump(self.rex.plan, f)
# self.rex.plan = new_plan
def tr_playback(self):
print "Playback"
self.path_start_time = micro_time()
self.rex.wpt_number = 1
self.rex.plan_status = 1
self.rex.max_torque = 50.0
self.ui.sldrMaxTorque.setValue(50.0)
self.executed_path = []
with open('path.config', 'w') as f:
pickle.dump(self.rex.plan, f)
self.rex.plan = [[micro_time() - self.path_start_time, self.rex.joint_angles_fb[:], [0,0,0,0]]] + self.rex.plan
def def_template(self):
print "Define Template"
self.get_template = 1
def template_match(self):
print "Template Match"
def make_bounding_box():
min_x = 10000
min_y = 10000
max_x = 0
max_y = 0
for p in self.video.mouse_coord:
x,y = mouse_to_raw(p[0], p[1])
if x < min_x:
min_x = x
if x > max_x:
max_x = x
if y < min_y:
min_y = y
if y > max_y:
max_y = y
return [min_x, min_y], [max_x, max_y]
#TODO get this working
search_image = self.video.currentFrame
#Make it greyscale
search_image = cv2.cvtColor(search_image, cv2.COLOR_BGR2GRAY)
[tx,ty],[bx,by] = make_bounding_box()
print [tx,ty], [bx,by]
search_image = search_image[tx:bx:,ty:by]
search_size = search_image.shape
print search_size
#Get the size of the template
template_size = self.template.shape
#We just want a greyscale 2d image
def SAD(x, y):
sad = 0
sub_region = search_image[x:x+template_size[0], y:y+template_size[1]]
diff = sub_region - self.template
sad = np.sum(diff**2)
return sad / (template_size[0]*template_size[1])
# for i in range(template_size[0]):
# for j in range(template_size[1]):
# sad += float(abs(int(search_image[x+i,y+j]) - int(self.template[i,j])))
# return sad / (template_size[0]*template_size[1]*255.0)
results = np.zeros((search_size[0]-template_size[0],
search_size[1]-template_size[1]))
print "Doing SAD"
for x in range(search_size[0] - template_size[0]):
for y in range(search_size[1] - template_size[1]):
results[x,y] = SAD(x,y)
print results.max(), results.min()
print "Done SAD"
threshold = 70.0
positions = []
max_val = results.max()
def arg_min(a):
min_val = float('inf')
min_x = 0
min_y = 0
for x in range(a.shape[0]):
for y in range(a.shape[1]):
if a[x,y] < min_val:
min_val = a[x,y]
min_x = x
min_y = y
return min_x, min_y
count = 0
while True:
#If this is below the confidence level then quit
min_index = arg_min(results)
pixel_value = results[min_index]
print pixel_value
if pixel_value > threshold:
break
#Add it to potential positions
positions.append(min_index)
#Local max suppression
def suppress_area(x,y):
for i in range(-template_size[0]/2, template_size[0]/2):
for j in range(-template_size[1]/2, template_size[1]/2):
if x+i < 0 or y+j < 0:
continue
if x+i >= results.shape[0] or y+j >= results.shape[1]:
continue
results[x+i, y+j] = max_val
suppress_area(min_index[0], min_index[1])
out = results * (255.0/results.max())
cv2.imwrite('./results_%d.png'%(count), out)
count += 1
#DONE-ish the points need to be shifted by half a template image size
print positions
#CONVERT POINTS TO WORLD COORDS
cx = self.template.shape[0]/2
cy = self.template.shape[1]/2
positions = [raw_to_mouse(p[0]+tx+cx, p[1]+ty+cx) for p in positions]
def aff_trans(x, y):
aff = self.video.aff_matrix
wx = x*aff[0][0] + y*aff[0][1] + aff[0][2]
wy = x*aff[1][0] + y*aff[1][1] + aff[1][2]
return wx,wy
self.donuts = [aff_trans(p[0], p[1]) for p in positions]
def exec_path(self):
print "Execute Path"
def get_phi(x,y):
from math import sqrt, pow
r = sqrt(pow(x,2)+pow(y,2))
if r <= 95.0:
return -135.0*D2R
elif r > 95.0 and r < 190.0:
return -90.0*D2R
else:
return -45.0*D2R
self.donuts.sort(key=lambda x: math.atan2(x[1], x[0]))
from itertools import chain, izip
world_points = [(x,y, 50.0, get_phi(x,y)) for (x,y) in self.donuts]
in_points = [(x,y, 10.0, get_phi(x,y)) for (x,y) in self.donuts]
final_points = []
for i,p in enumerate(world_points):
final_points.append(p)
final_points.append(in_points[i])
final_points.append(p)
config_space = [self.rex.rexarm_IK(p, 1) for p in final_points if self.rex.rexarm_IK(p, 1) != None]
print "Playback"
self.path_start_time = micro_time()
self.rex.wpt_number = 1
self.rex.plan_status = 1
self.rex.max_torque = 50.0
self.ui.sldrMaxTorque.setValue(50.0)
self.executed_path = []
self.rex.plan = [[0, self.rex.joint_angles_fb[:], [0,0,0,0]]] + \
[[3000000*(i+1), q, [0,0,0,0]] for i,q in enumerate(config_space)]
print self.rex.plan
self.rex.wpt_total = len(self.rex.plan)
class Gui(QtGui.QMainWindow):
"""
Main GUI Class
It contains the main function and interfaces between
the GUI and functions
"""
def __init__(self,parent=None):
QtGui.QWidget.__init__(self,parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Main Variables Using Other Classes"""
self.rex = Rexarm()
self.video = Video(cv2.VideoCapture(0))
self.world_coord = np.float32()
""" Play and Repeat Variable """
self.wayPoints = []
self.wayPointsPos = []
self.wayPointsSpeed = []
self.wayPointsTime = []
""" Other Variables """
self.last_click = np.float32([-1,-1])
self.define_template_flag = -1
self.click_point1 = np.float32([-1,-1])
self.click_point2 = np.float32([-1,-1])
self.template = None
self.targets = []
self.waypointsfp = csv.writer(open("waypoint.csv","wb"))
self.currtime = 0
""" Set GUI to track mouse """
QtGui.QWidget.setMouseTracking(self,True)
"""
Video Function
Creates a timer and calls play() function
according to the given time delay (27mm)
"""
self._timer = QtCore.QTimer(self)
self._timer.timeout.connect(self.play)
self._timer.start(27)
"""
LCM Arm Feedback
Creates a timer to call LCM handler continuously
No delay implemented. Reads all time
"""
self._timer2 = QtCore.QTimer(self)
self._timer2.timeout.connect(self.rex.get_feedback)
self._timer2.start()
"""
ARM Plan and Command Thread
Creates a timer to call REXARM.plan_command function continuously
"""
self._timer3 = QtCore.QTimer(self)
self._timer3.timeout.connect(self.rex.plan_command)
self._timer3.start()
"""
Connect Sliders to Function
TO DO: CONNECT THE OTHER 5 SLIDERS IMPLEMENTED IN THE GUI
"""
self.ui.sldrBase.valueChanged.connect(self.slider_change)
self.ui.sldrShoulder.valueChanged.connect(self.slider_change)
self.ui.sldrElbow.valueChanged.connect(self.slider_change)
self.ui.sldrWrist.valueChanged.connect(self.slider_change)
self.ui.sldrMaxTorque.valueChanged.connect(self.slider_change)
self.ui.sldrSpeed.valueChanged.connect(self.slider_change)
""" Commands the arm as the arm initialize to 0,0,0,0 angles """
self.slider_change()
""" Connect Buttons to Functions """
self.ui.btnLoadCameraCal.clicked.connect(self.load_camera_cal)
self.ui.btnPerfAffineCal.clicked.connect(self.affine_cal)
self.ui.btnTeachRepeat.clicked.connect(self.tr_initialize)
self.ui.btnAddWaypoint.clicked.connect(self.tr_add_waypoint)
self.ui.btnSmoothPath.clicked.connect(self.tr_smooth_path)
self.ui.btnPlayback.clicked.connect(self.tr_playback)
self.ui.btnLoadPlan.clicked.connect(self.tr_load)
self.ui.btnDefineTemplate.clicked.connect(self.def_template)
self.ui.btnLocateTargets.clicked.connect(self.template_match)
self.ui.btnExecutePath.clicked.connect(self.exec_path)
def play(self):
"""
Play Funtion
Continuously called by GUI
"""
""" Renders the Video Frame """
try:
self.video.captureNextFrame()
for t in self.targets:
self.video.addTarget(t)
self.ui.videoFrame.setPixmap(self.video.convertFrame())
self.ui.videoFrame.setScaledContents(True)
cv2.imwrite("curretFrame.png", self.video.currentFrame)
except TypeError:
print "No frame"
"""
Update GUI Joint Coordinates Labels
TO DO: include the other slider labels
"""
self.ui.rdoutBaseJC.setText(str(self.rex.joint_angles_fb[0]*R2D))
self.ui.rdoutShoulderJC.setText(str(self.rex.joint_angles_fb[1]*R2D))
self.ui.rdoutElbowJC.setText(str(self.rex.joint_angles_fb[2]*R2D))
self.ui.rdoutWristJC.setText(str(self.rex.joint_angles_fb[3]*R2D))
fk_result = self.rex.rexarm_FK(3)
#print fk_result
self.ui.rdoutX.setText(repr(fk_result[0]))
self.ui.rdoutY.setText(repr(fk_result[1]))
self.ui.rdoutZ.setText(repr(fk_result[2]))
self.ui.rdoutT.setText(repr(fk_result[3]))
"""
Mouse position presentation in GUI
TO DO: after getting affine calibration make the apprriate label
to present the value of mouse position in world coordinates
"""
x = QtGui.QWidget.mapFromGlobal(self,QtGui.QCursor.pos()).x()
y = QtGui.QWidget.mapFromGlobal(self,QtGui.QCursor.pos()).y()
if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)):
self.ui.rdoutMousePixels.setText("(-,-)")
self.ui.rdoutMouseWorld.setText("(-,-)")
else:
x = x - MIN_X
y = y - MIN_Y
self.ui.rdoutMousePixels.setText("(%.0f,%.0f)" % (x,y))
if (self.video.aff_flag == 2):
""" TO DO Here is where affine calibration must be used """
self.ui.rdoutMouseWorld.setText("(%0.f,%0.f)" % (self.world_coord[0][0], self.world_coord[1][0]))
else:
self.ui.rdoutMouseWorld.setText("(-,-)")
"""
Updates status label when rexarm playback is been executed.
This will be extended to includ eother appropriate messages
"""
if(self.rex.plan_status == 1):
self.ui.rdoutStatus.setText("Playing Back - Waypoint %d"
%(self.rex.wpt_number + 1))
def slider_change(self):
"""
Function to change the slider labels when sliders are moved
and to command the arm to the given position
TO DO: Implement for the other sliders
"""
self.ui.rdoutBase.setText(str(self.ui.sldrBase.value()))
self.ui.rdoutShoulder.setText(str(self.ui.sldrShoulder.value()))
self.ui.rdoutElbow.setText(str(self.ui.sldrElbow.value()))
self.ui.rdoutWrist.setText(str(self.ui.sldrWrist.value()))
self.ui.rdoutTorq.setText(str(self.ui.sldrMaxTorque.value()) + "%")
self.ui.rdoutSpeed.setText(str(self.ui.sldrSpeed.value()) + "%")
self.rex.max_torque = self.ui.sldrMaxTorque.value()/100.0
for i in xrange(4):
self.rex.speed[i] = self.ui.sldrSpeed.value()/100.0
self.rex.joint_angles[0] = self.ui.sldrBase.value()*D2R
self.rex.joint_angles[1] = self.ui.sldrShoulder.value()*D2R
self.rex.joint_angles[2] = self.ui.sldrElbow.value()*D2R
self.rex.joint_angles[3] = self.ui.sldrWrist.value()*D2R
self.rex.cmd_publish()
def mousePressEvent(self, QMouseEvent):
"""
Function used to record mouse click positions for
affine calibration
"""
""" Get mouse posiiton """
x = QMouseEvent.x()
y = QMouseEvent.y()
""" If mouse position is not over the camera image ignore """
if ((x < MIN_X) or (x > MAX_X) or (y < MIN_Y) or (y > MAX_Y)): return
""" Change coordinates to image axis """
self.last_click[0] = x - MIN_X
self.last_click[1] = y - MIN_Y
""" If affine calibration is been performed """
if (self.video.aff_flag == 1):
""" Save last mouse coordinate """
self.video.mouse_coord[self.video.mouse_click_id] = [(x-MIN_X),(y-MIN_Y)]
""" Update the number of used poitns for calibration """
self.video.mouse_click_id += 1
""" Update status label text """
self.ui.rdoutStatus.setText("Affine Calibration: Click point %d"
%(self.video.mouse_click_id + 1))
"""
If the number of click is equal to the expected number of points
computes the affine calibration.
TO DO: Change this code to use you programmed affine calibration
and NOT openCV pre-programmed function as it is done now.
"""
if(self.video.mouse_click_id == self.video.aff_npoints):
"""
Update status of calibration flag and number of mouse
clicks
"""
self.video.aff_flag = 2
self.video.mouse_click_id = 0
print self.video.mouse_coord
print self.video.real_coord
""" Perform affine calibration with OpenCV """
#self.video.aff_matrix = cv2.getAffineTransform(
# self.video.mouse_coord,
# self.video.real_coord)
self.video.compute_affine_matrix()
""" Updates Status Label to inform calibration is done """
self.ui.rdoutStatus.setText("Waiting for input")
"""
Uncomment to gether affine calibration matrix numbers
on terminal
"""
print self.video.aff_matrix
if self.video.aff_flag == 2:
mouse_coord = np.array([[(x-MIN_X)], [(y-MIN_Y)],[1]])
self.world_coord = np.dot(self.video.aff_matrix, mouse_coord)
if self.define_template_flag == 0:
self.click_point1 = copy.deepcopy(self.last_click)
self.define_template_flag = 1
elif self.define_template_flag == 1:
self.click_point2 = copy.deepcopy(self.last_click)
self.template = copy.deepcopy(self.video.bwFrame[2*self.click_point1[1]:2*self.click_point2[1],2*self.click_point1[0]:2*self.click_point2[0]])
print self.click_point1
print self.click_point2
self.define_template_flag = -1
cv2.imwrite('./template.png', self.template)
def affine_cal(self):
"""
Function called when affine calibration button is called.
Note it only chnage the flag to record the next mouse clicks
and updates the status text label
"""
self.video.aff_flag = 1
self.ui.rdoutStatus.setText("Affine Calibration: Click point %d"
%(self.video.mouse_click_id + 1))
def load_camera_cal(self):
print "Load Camera Cal"
self.video.loadCalibration()
def tr_initialize(self):
self.wayPointsPos = []
self.wayPointsSpeed = []
self.wayPointsTime = []
print "Teach and Repeat"
def tr_add_waypoint(self):
#waypoints1 = copy.deepcopy(self.rex.joint_angles_fb)
#waypoints2 = copy.deepcopy(self.rex.joint_angles_fb)
#self.wayPointsPos.append(waypoints1)
#self.wayPointsTime.append([self.currtime, self.currtime, self.currtime, self.currtime])
#self.wayPointsSpeed.append([0.1, 0.1, 0.1, 0.1])
#self.currtime += 70000
#waypoints2[1] -= 0.7
#self.wayPointsPos.append(waypoints2)
#self.wayPointsTime.append([self.currtime, self.currtime, self.currtime, self.currtime])
#self.wayPointsSpeed.append([0.1, 0.1, 0.1, 0.1])
#self.currtime += 70000
#self.waypointsfp.writerow(waypoints1)
#self.waypointsfp.writerow(waypoints2)
#np.save("waypointsPos",self.wayPointsPos)
#np.save("waypointsSpeed", self.wayPointsSpeed)
#np.save("waypointsTime", self.wayPointsTime)
self.wayPointsPos.append(copy.deepcopy(self.rex.joint_angles_fb))
self.wayPointsSpeed.append(copy.deepcopy(self.rex.speed_fb))
self.wayPointsTime.append(copy.deepcopy(self.rex.time_fb))
np.save("waypointsPos",self.wayPointsPos)
np.save("waypointsSpeed", self.wayPointsSpeed)
np.save("waypointsTime", self.wayPointsTime)
#print self.wayPoints
print "Add Waypoint"
def cubic_spline(self, q0, q0dot, t0, qf, qfdot, tf, stepnum):
a0 = q0
a1 = q0dot
a2 = (3*(qf-q0) - (2*q0dot+qfdot)*float(tf-t0)) / float((tf-t0)*(tf-t0))
a3 = (2*(q0-qf) + (q0dot+qfdot)*float(tf-t0)) / float((tf-t0)*(tf-t0)*(tf-t0))
stepsize = float(tf-t0)/float(stepnum)
currtime = t0 + stepsize
pos_interpolated = []
speed_interpolated = []
for i in xrange(stepnum-1):
pos_interpolated.append(a0 + a1*currtime + a2*currtime*currtime + \
a3*currtime*currtime*currtime)
speed_interpolated.append(np.abs(a1 + 2*a2*currtime + 3*a3*currtime*currtime))
currtime += stepsize
#print q0, qf
#print pos_interpolated
return (pos_interpolated, speed_interpolated)
def tr_smooth_path(self):
if len(self.wayPointsPos) != len(self.wayPointsSpeed) and len(self.wayPointsPos) != len(self.wayPointsTime):
print "Error on waypoints number, cannot smooth path"
return
for i in xrange(len(self.wayPointsTime)):
for j in xrange(4):
self.wayPointsTime[i][j] *= US2S
for i in xrange(len(self.wayPointsSpeed)):
for j in xrange(4):
self.wayPointsSpeed[i][j] *= percent2rads
interpolated_waypoints_pos = []
interpolated_waypoints_speed = []
for i in xrange(len(self.wayPointsPos)-1):
time_offset = self.wayPointsTime[i]
startPos = self.wayPointsPos[i]
endPos = self.wayPointsPos[i+1]
startSpeed = self.wayPointsSpeed[i]
endSpeed = self.wayPointsSpeed[i+1]
startTime = self.wayPointsTime[i]
endTime = self.wayPointsTime[i+1]
stepnum = 3
four_joint_interpolated_pos = []
four_joint_interpolated_speed = []
for j in xrange(4):
q0 = startPos[j]
q0dot = startSpeed[j]
t0 = startTime[j] - time_offset[j]
qf = endPos[j]
qfdot = endSpeed[j]
tf = endTime[j] - time_offset[j]
res = self.cubic_spline(q0, q0dot, t0, qf, qfdot, tf, stepnum)
four_joint_interpolated_pos.append(res[0])
four_joint_interpolated_speed.append(res[1])
interpolated_waypoints_pos.append(startPos)
for i in xrange(len(four_joint_interpolated_pos[0])):
pos = []
for j in xrange(len(four_joint_interpolated_pos)):
pos.append(four_joint_interpolated_pos[j][i])
interpolated_waypoints_pos.append(pos)
interpolated_waypoints_pos.append(endPos)
interpolated_waypoints_speed.append(startSpeed)
for i in xrange(len(four_joint_interpolated_speed[0])):
speed = []
for j in xrange(len(four_joint_interpolated_speed)):
speed.append(four_joint_interpolated_speed[j][i])
interpolated_waypoints_speed.append(speed)
interpolated_waypoints_speed.append(endSpeed)
self.wayPointsPos = interpolated_waypoints_pos
self.wayPointsSpeed = interpolated_waypoints_speed
for i in xrange(len(self.wayPointsSpeed)):
for j in xrange(len(self.wayPointsSpeed[0])):
self.wayPointsSpeed[i][j] /= percent2rads
#pprint.pprint(self.wayPointsPos)
#pprint.pprint(self.wayPointsSpeed)
np.save("interpolated_waypoints_pos", self.wayPointsPos)
np.save("interpolated_waypoints_speed", self.wayPointsSpeed)
print "Smooth Path"
def tr_playback(self):
#print self.wayPoints
self.rex.planPos = self.wayPointsPos
self.rex.planSpeed = self.wayPointsSpeed
#print self.rex.plan
self.rex.save_data = True
self.rex.plan_status = 1
self.rex.wpt_number = 0
self.rex.wpt_total = len(self.rex.planPos)
print "Playback"
def tr_load(self):
self.wayPointsPos = np.load("waypointsPos.npy")
self.wayPointsSpeed = np.load("waypointsSpeed.npy")
self.wayPointsTime = np.load("waypointsTime.npy")
print "Load waypoints"
def def_template(self):
print "Define Template"
self.define_template_flag = 0
@do_cprofile
def template_match(self):
print "Template Match"
self.targets = []
result_pq = Queue.PriorityQueue()
template = cv2.resize(self.template, None, fx=0.6, fy=0.6, interpolation=cv2.INTER_AREA)
frame = cv2.resize(self.video.bwFrame, None, fx=0.6, fy=0.6, interpolation=cv2.INTER_AREA)
height, width = template.shape
for i in xrange(0, frame.shape[0] - height):
for j in xrange(0, frame.shape[1] - width):
center_x = (i + height/2.0)/0.6
center_y = (j + width/2.0)/0.6
to_compare = frame[i:i+height,j:j+width]
num = la.norm(to_compare - template)
result_pq.put((num, center_y, center_x))
result = []
#for i in xrange(40):
# t = result_pq.get()
# print t[0]
# result.append([int(t[1]), int(t[2])])
for i in xrange((frame.shape[0]-height)*(frame.shape[1]-width)):
t = result_pq.get()
if t[0] > 350:
break
else:
result.append([int(t[1]), int(t[2])])
distort = sys.maxint
cluster_size = 1
#print result
while distort > 20:
clustered_result, distort = scv.kmeans(np.array(result), cluster_size)
cluster_size += 1
clustered_result, distort = scv.kmeans(np.array(result), 4)
print "cluster_size: ", cluster_size-1
print "distort", distort
for r in clustered_result:
print r
self.targets.append((r[0], r[1]))
#circles = cv2.HoughCircles(self.video.bwFrame, cv2.cv.CV_HOUGH_GRADIENT, 1, minDist=5, param1=50, param2=50, minRadius=1, maxRadius=30)
#for c in circlesi[0,:]:
# print c
# self.targets.append((c[0],c[1]))
#img = np.zeros((1000,1000,3), np.uint8)
#for t in self.targets:
# img = cv2.circle(img, t, 10, (255,255,255), -1)
#cv2.imwrite("./result.png", img)
def exec_path(self):
#waypoints = [(-80.0, 90.0, 350.0), (70.0, 80.0, 400.0), (-180.0, -250.0, 125.0),(240.0, -190.0, 95.0), (-80.0, 90.0, 350.0)]
#waypoints = [(-100.0, 100.0, 5.0), (-100.0, 100.0, 200.0), (100.0, 100.0, 200.0),(100.0, 100.0, 5.0), (100.0, -100.0, 5.0), (100.0, -100.0, 200.0), (100.0, 100.0, 200.0)]
waypoints = []
for t in xrange(0,12,1):
x = 150 * np.cos(t)
y = 150 * np.sin(t)
z = 5.0 + 30.0 * t
waypoints.append((x, y, z))
#waypoints = []
#if not self.targets or len(self.targets) == 0:
# return
#for t in self.targets:
# img_coord = np.array([[(t[0]/2.0)], [(t[1]/2.0)], [1]])
# world_coord = np.dot(self.video.aff_matrix, img_coord)
# print "img coord: ", img_coord
# print "world coord: ", world_coord
# waypoints.append((world_coord[0], world_coord[1], 5))
print waypoints
self.wayPointsPos = []
self.wayPointsSpeed = []
self.wayPointsTime = []
for i in xrange(0, 3*len(waypoints), 3):
x, y, z = waypoints[i/3]
self.wayPointsPos.append(self.rex.rexarm_IK_Search((x,y,z)))
self.wayPointsSpeed.append(copy.deepcopy([0.15, 0.15, 0.15, 0.15]))
#jointsUp = self.rex.rexarm_IK_Search((x, y, 50.0))
#jointsDown = self.rex.rexarm_IK_Search((x, y, 0.0))
#if jointsUp:
# self.wayPointsPos.append(copy.deepcopy(jointsUp))
# self.wayPointsSpeed.append(copy.deepcopy([0.15, 0.15, 0.15, 0.15]))
# self.wayPointsTime.append([2000000*i,2000000*i,2000000*i,2000000*i])
#
#if jointsDown:
# self.wayPointsPos.append(copy.deepcopy(jointsDown))
# self.wayPointsSpeed.append(copy.deepcopy([0.05, 0.05, 0.05, 0.05]))
# self.wayPointsTime.append([2000000*(i+1),2000000*(i+1),2000000*(i+1),2000000*(i+1)])
#if jointsUp:
# self.wayPointsPos.append(copy.deepcopy(jointsUp))
# self.wayPointsSpeed.append(copy.deepcopy([0.05, 0.05, 0.05, 0.05]))
# self.wayPointsTime.append([2000000*(i+2),2000000*(i+2),2000000*(i+2),2000000*(i+2)])
np.save("funPathPos",self.wayPointsPos)
np.save("funPathSpeed", self.wayPointsSpeed)
np.save("funPathTime", self.wayPointsTime)
self.rex.planPos = self.wayPointsPos
self.rex.planSpeed = self.wayPointsSpeed
print self.rex.planPos
self.rex.save_data = True
self.rex.plan_status = 1
self.rex.wpt_number = 0
self.rex.wpt_total = len(self.rex.planPos)
import time
from rexarm import Rexarm
import numpy as np
import argparse
from trajectory_planner import TrajectoryPlanner
# Parse cmd line
parser = argparse.ArgumentParser()
parser.add_argument('-t', '--trajectory_planner', action='store_true')
parser.add_argument('-c',
'--config_file',
type=str,
default=script_path + '/../config/rexarm_config.csv')
args = parser.parse_args()
rexarm = Rexarm()
rexarm.initialize(config_file=args.config_file)
if not rexarm.initialized:
print('Failed to initialized the rexarm')
sys.exit(-1)
rexarm.set_speeds_normalized_all(0.5)
tmp_waypoints = [[0.0, 0.0, 0.0, 0.0], [np.pi * 0.1, 0.0, np.pi / 2, 0.0],
[np.pi * 0.25, np.pi / 2, -np.pi / 2, np.pi / 2],
[np.pi * 0.4, np.pi / 2, -np.pi / 2, 0.0],
[np.pi * 0.55, 0, 0, 0], [np.pi * 0.7, 0.0, np.pi / 2, 0.0],
[np.pi * 0.85, np.pi / 2, -np.pi / 2, np.pi / 2],
[np.pi, np.pi / 2, -np.pi / 2, 0.0],
[0.0, np.pi / 2, np.pi / 2, 0.0],
[np.pi / 2, -np.pi / 2, np.pi / 2, 0.0]]
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):
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 -- 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
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)
from trajectory_planner import TrajectoryPlanner
BAUDRATE = 1000000
DEVICENAME = "/dev/ttyACM0".encode('utf-8')
dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
port_num = 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)
rexarm = Rexarm((base,shld,elbw,wrst,wrst2), grip)
rexarm.set_speeds_normalized_global(0.15)
rexarm.initialize()
rexarm.open_gripper()
tp = TrajectoryPlanner(rexarm)
time.sleep(1)
waypoints = [[1.0,0.8,1.0,1.0,1.0],
[-1.0,-0.8,-1.0,-1.0,-1.0],
[-1.0,0.8,1.0,1.0,1.0],
[1.0,-0.8,-1.0,-1.0,-1.0],
[0.0,0.0,0.0,0.0,0.0]]
for wp in waypoints:
goal_wp = wp
class Worker:
def __init__(self, worker_num, worker_port, master_port,
master_heartbeat_port, url):
pid = os.getpid()
self.worker_num = worker_num
self.worker_port = worker_port
self.master_port = master_port
self.master_heartbeat_port = master_heartbeat_port
self.url = url
self.rex = Rexarm()
## create TCP socket on the given worker_port_number; TCP socket to receive msg; create an INET, STREAMing socket
worker_tcp_listen = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
worker_tcp_listen.setsockopt(socket.SOL_SOCKET, socket.SO_REUSEADDR, 1)
worker_tcp_listen.bind(
(self.url, self.worker_port)) # bind the socket to the server
worker_tcp_listen.listen(20) # become server socket
## parameter
self.IK_succeed_flag = 0
## setup thread
setup_thread = threading.Thread(target=self.setup_thread)
setup_thread.start()
time_out_LCM_thread = threading.Thread(target=self.time_out_LCM)
time_out_LCM_thread.start()
time.sleep(1)
while True:
clientsocket, address = worker_tcp_listen.accept()
#print "Get order from master."
worker_job = clientsocket.recv(1024)
worker_msg = json.loads(worker_job.decode("utf-8"))
clientsocket.close()
self.do_job(worker_msg)
def time_out_LCM(self):
while True:
self.rex.lc.handle_timeout(50)
def setup_thread(self):
# setup heartbeat
thread_send_heartbeat = threading.Thread(target=self.send_heartbeat)
thread_send_heartbeat.start()
## send ready mag to master's TCP socket
msg_ready = {
"msg type": "status",
"worker num": self.worker_num,
"status": "ready"
}
msg_ready_json = json.dumps(msg_ready)
tcp_send_msg = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
tcp_send_msg.connect((self.url, self.master_port))
tcp_send_msg.sendall(str.encode(msg_ready_json))
tcp_send_msg.close()
def do_job(self, worker_msg):
# do job and get data
task = worker_msg["task"]
if task == "arrive":
thread_temp = threading.Thread(target=self.arrive,
args=(worker_msg["pose"],
worker_msg["method"]))
thread_temp.start()
elif task == "waypoint":
thread_temp = threading.Thread(target=self.waypoint,
args=(worker_msg["speed"],
worker_msg["torque"]))
thread_temp.start()
elif task == "grab":
thread_temp = threading.Thread(target=self.grab)
thread_temp.start()
elif task == "drop":
thread_temp = threading.Thread(target=self.drop)
thread_temp.start()
elif task == "cmd":
thread_temp = threading.Thread(target=self.cmd,
args=([worker_msg["joint angles"]]))
thread_temp.start()
def send_fb2master(self, finished_task):
# print "[Worker %s] finished job and prepare to transfer data to [Master]."%(self.worker_num)
# send back data to master
msg_finish = {
"msg type": "status",
"worker num": self.worker_num,
"status": "finished",
"task": finished_task
}
msg_finish_json = json.dumps(msg_finish)
tcp_send_msg = socket.socket(socket.AF_INET, socket.SOCK_STREAM)
tcp_send_msg.connect((self.url, self.master_port))
tcp_send_msg.sendall(str.encode(msg_finish_json))
tcp_send_msg.close()
#print "worker_img:",self.worker_num,"ready.\n"
def send_heartbeat(self):
msg_hb = {"msg type": "heartbeat", "worker num": self.worker_num}
msg_hb_json = json.dumps(msg_hb)
while True:
## UDP protocol for heartbeat
send_hb_msg = socket.socket(socket.AF_INET, socket.SOCK_DGRAM)
send_hb_msg.connect((self.url, self.master_heartbeat_port))
send_hb_msg.sendall(str.encode(msg_hb_json))
send_hb_msg.close()
time.sleep(3)
def cmd(self, cmd): ## only care anles 0~5
self.rex.joint_angles[0] = cmd[0]
self.rex.joint_angles[5] = cmd[5]
self.rex.cmd_publish()
while True:
if abs(cmd[0] -
self.rex.joint_angles_fb[0]) < self.rex.base_rotate_tol:
break
self.rex.joint_angles[1] = cmd[1]
self.rex.joint_angles[2] = cmd[2]
self.rex.joint_angles[3] = cmd[3]
self.rex.joint_angles[4] = cmd[4]
self.rex.cmd_publish()
while True:
if abs(cmd[0]-self.rex.joint_angles_fb[0])<self.rex.tol_check_cmd and\
abs(cmd[1]-self.rex.joint_angles_fb[1])<self.rex.tol_check_cmd and\
abs(cmd[2]-self.rex.joint_angles_fb[2])<self.rex.tol_check_cmd and\
abs(cmd[3]-self.rex.joint_angles_fb[3])<self.rex.tol_check_cmd and\
abs(cmd[4]-self.rex.joint_angles_fb[4])<self.rex.tol_check_cmd and\
abs(cmd[5]-self.rex.joint_angles_fb[5])<self.rex.tol_check_cmd and\
abs(cmd[6]-self.rex.joint_angles_fb[6])<self.rex.tol_check_cmd:
break
self.send_fb2master("arrive")
def arrive(self, block_pose, method):
#input: pose(x,y,z,angle) output:cmd to pose_LCM
x = block_pose[0]
y = block_pose[1]
z = block_pose[2]
angle = block_pose[3]
pose = []
self.IK_succeed_flag = 0
##calculate end-effector point
## swap method
if method == 1:
add_angle = [
0, np.pi / 2, -np.pi / 2, np.pi, -np.pi, np.pi / 4, -np.pi / 4,
np.pi / 6, -np.pi / 6, 3 * np.pi / 4, -3 * np.pi / 4,
5 * np.pi / 6, -5 * np.pi / 6, np.pi / 8, -np.pi / 8,
7 * np.pi / 8, -7 * np.pi / 8
]
for i in range(
len(add_angle)): #try four directions to find a good way
print "Method 1 iteration time:", i
angle_temp = angle + add_angle[i]
pose = []
pose.append(x - (self.rex.l5) * np.sin(angle_temp))
pose.append(y - (self.rex.l5) * np.cos(angle_temp))
pose.append(z + self.rex.l4_swap)
pose.append(angle_temp)
motor_angle = self.rex.rexarm_IK(pose, method)
if motor_angle != 0:
## first calculate base angle
self.rex.joint_angles[0] = motor_angle[0]
self.rex.cmd_publish()
while True:
if abs(motor_angle[0] - self.rex.joint_angles_fb[0]
) < self.rex.base_rotate_tol:
break
self.rex.joint_angles[1] = motor_angle[1]
self.rex.joint_angles[2] = motor_angle[2]
self.rex.joint_angles[3] = motor_angle[3]
self.rex.joint_angles[4] = motor_angle[4]
self.rex.joint_angles[5] = motor_angle[5]
self.rex.cmd_publish()
self.IK_succeed_flag = 1
break
elif method == 2:
pose.append(x - (self.rex.l5) * np.sin(angle))
pose.append(y - (self.rex.l5) * np.cos(angle))
pose.append(z - self.rex.l4_swap)
pose.append(angle)
motor_angle = self.rex.rexarm_IK(pose, method)
if motor_angle != 0:
## first calculate base angle
self.rex.joint_angles[0] = motor_angle[0]
self.rex.cmd_publish()
while True:
if abs(motor_angle[0] - self.rex.joint_angles_fb[0]
) < self.rex.base_rotate_tol:
break
self.rex.joint_angles[5] = motor_angle[5]
self.rex.cmd_publish()
while True:
#print "Turn base.",motor_angle[0],self.rex.joint_angles_fb[0]
if abs(motor_angle[5] - self.rex.joint_angles_fb[5]
) < self.rex.base_rotate_tol:
break
self.rex.joint_angles[1] = motor_angle[1]
self.rex.joint_angles[2] = motor_angle[2]
self.rex.joint_angles[3] = motor_angle[3]
self.rex.joint_angles[4] = motor_angle[4]
self.rex.joint_angles[5] = motor_angle[5]
self.rex.cmd_publish()
self.IK_succeed_flag = 1
elif method == 3:
# this is different from method 1 and 2. the input pose to IK is block position coordinate (x,y,z,theta),theta is loop angle
theta = 0
pose.append(x)
pose.append(y)
pose.append(z + 12)
pose.append(theta)
speed = 0.6
try:
speed = block_pose[5]
self.rex.max_torque = block_pose[6]
print "speed:", speed
except:
pass
self.rex.speed_list[1] = speed
self.rex.speed_list[2] = speed
self.rex.speed_list[3] = speed
#self.rex.speed_list[6] = 0.4
self.rex.cmd_publish()
try:
wrist_angle_cond = block_pose[4]
except:
print "wrist_angle is by default."
wrist_angle_cond = 0
pose.append(
wrist_angle_cond) #usually it is 0, event 3 4 final point pi/2
motor_angle = 0
sign = 1
count = 0
delta = 0.02
delta_sum = 0
while not motor_angle:
count += 1
motor_angle = self.rex.rexarm_IK(pose, method)
if count % 2 == 1:
delta_sum += delta
pose[3] = delta_sum * sign
sign = -sign
if count > 50000:
break
if motor_angle != 0:
## first calculate base angle
self.rex.joint_angles[0] = motor_angle[0]
self.rex.joint_angles[5] = motor_angle[5]
self.rex.cmd_publish()
while True:
if abs(motor_angle[0]-self.rex.joint_angles_fb[0])<self.rex.base_rotate_tol\
and abs(motor_angle[5]-self.rex.joint_angles_fb[5])<self.rex.wrist_rotate_tol:
break
self.rex.joint_angles[1] = motor_angle[1]
self.rex.joint_angles[2] = motor_angle[2]
self.rex.joint_angles[3] = motor_angle[3]
self.rex.joint_angles[4] = motor_angle[4]
self.rex.cmd_publish()
self.IK_succeed_flag = 1
if self.IK_succeed_flag == 1:
while True:
if abs(motor_angle[0]-self.rex.joint_angles_fb[0])<self.rex.tol_check and\
abs(motor_angle[1]-self.rex.joint_angles_fb[1])<self.rex.tol_check and\
abs(motor_angle[2]-self.rex.joint_angles_fb[2])<self.rex.tol_check and\
abs(motor_angle[3]-self.rex.joint_angles_fb[3])<self.rex.tol_check and\
abs(motor_angle[4]-self.rex.joint_angles_fb[4])<self.rex.tol_check and\
abs(motor_angle[5]-self.rex.joint_angles_fb[5])<self.rex.tol_check:
#print "self.rex.joint_angles_fb",self.rex.joint_angles_fb,"\nmotor_angle",motor_angle
print "Break arrive."
break
self.send_fb2master("arrive")
else:
print "-------------------IK failed.----------------------"
self.send_fb2master("failed")
def waypoint(self, speed, torque):
try:
self.rex.speed_list[1] = speed
self.rex.speed_list[2] = speed
self.rex.speed_list[3] = speed
self.rex.max_torque = torque
except:
pass
self.rex.joint_angles[1] = 0
self.rex.joint_angles[2] = 0
self.rex.joint_angles[3] = 0
self.rex.cmd_publish()
while True:
if abs(self.rex.joint_angles_fb[1])<self.rex.tol_check and\
abs(self.rex.joint_angles_fb[2])<self.rex.tol_check and\
abs(self.rex.joint_angles_fb[3])<self.rex.tol_check:
print "Break waypoint."
break
self.send_fb2master("arrive")
def grab(self):
# Gripper
#self.rex.speed_list[6] = 0.4
self.rex.max_torque = 0.6 ##
self.rex.joint_angles[6] = self.rex.grab_angle
self.rex.cmd_publish()
while True:
if self.rex.load_fb[
6] > self.rex.grab_load: # and abs(self.rex.grab_angle-self.rex.joint_angles_fb[6])<self.rex.angle_grab_tol:#self.rex.load_fb[6] > self.rex.grab_load and
print "Break grab."
break
time.sleep(0.3)
self.send_fb2master("grab")
def drop(self):
#self.rex.speed_list[6] = 0.3
self.rex.max_torque = 0.4 ##
self.rex.joint_angles[6] = self.rex.drop_angle
self.rex.cmd_publish()
while True:
if abs(
self.rex.drop_angle - self.rex.joint_angles_fb[6]
) < self.rex.angle_drop_tol: #self.rex.load_fb[6] < self.rex.drop_load and
print "Break drop."
break
time.sleep(0.4)
self.send_fb2master("drop")
def aaaa(self):
while True:
print "self.rex.joint_angles_fb", self.rex.joint_angles_fb
time.sleep(0.5)
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)
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)
