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"""
