def __init__(self, src):
self._src = src
self._kinect = Kinect()
self._body = BodyDetector()
self._hand = HandDetector()
self._contour = HandContourDetector()
self._pose = PoseClassifier(MultiPoseClassifier(src))
def __init__(self, id, nsamples, dst):
self._id = id
self._nsamples = nsamples
self._dst = dst
self._kinect = Kinect()
self._body = BodyDetector()
self._hand = HandDetector()
self._contour = HandContourDetector()
self._feature = GaborDescriptors(4, 4)
def __init__(self, id, nsamples, dst):
self._id = id
self._nsamples = nsamples
self._dst = dst
self._kinect = Kinect()
self._body = BodyDetector()
self._hand = HandDetector()
self._contour = HandContourDetector()
self._fdesc = FourierDescriptors()
self._train = []
def __init__(self):
self.addr = tools.get_random_uuid()
self.engine = Engine()
self.dev = Device("cli.experimental", self.addr)
self.dev.vendor_id = 'IHSEV'
self.dev.info = 'chute'
self.engine.add_device(self.dev)
self.threadlist = []
self.fallEvent = threading.Event()
self.pepper = Pepper(self)
self.kinect = Kinect()
self.sensfloor = Sensfloor(self.engine)
def __init__(self):
self.kinect = Kinect(SEGMENT_SIZE, ENGAGE_MIN, ENGAGE_MAX)
socket_started = False
self.socket_api = None
while (not socket_started):
try:
self.socket_api = SocketAPI(TCP_IP, TCP_PORT, BUFFER_SIZE,
ACK_TIMEOUT, TERMINATOR)
socket_started = True
except:
print("Socket didn't start, waiting 3 seconds...")
time.sleep(3)
print("Connected!")
self.HandModel = RealTimeHandRecognition("RH", 32, 2)
class TestHand():
def __init__(self):
self._kinect = Kinect()
self._body = BodyDetector()
self._hand = HandDetector(HandOtsu())
self._contour = HandContourDetector()
self._palm = PalmDetector()
def run(self):
for (depth, depth8, rgb) in self._kinect.get_data():
cv2.imshow('rgb', cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB))
cv2.imshow('depth', depth8)
hand, mask = self._hand.run(depth, depth8)
(_, _, crop) = self._contour.run(mask)
if crop == None:
continue
cv2.imshow('hand', crop)
hand = self._palm.run(hand, crop)
if hand == None:
continue
cv2.imshow('hand final', hand)
cv2.waitKey(10)
def _crop(self, img, box):
crop = img[box[1]:box[1] + box[3], box[0]:box[0] + box[2]]
return crop
class TestPose():
def __init__(self, src):
self._src = src
self._kinect = Kinect()
self._body = BodyDetector()
self._hand = HandDetector()
self._contour = HandContourDetector()
self._pose = PoseClassifier(MultiPoseClassifier(src))
def run(self):
for (depth, depth8, rgb) in self._kinect.get_data():
contour = self._get_hand_contour(depth8, depth, rgb)
if contour.any():
self._contour.draw()
print self._pose.run(contour)
cv2.waitKey(5)
def _get_hand_contour(self, depth8, depth, rgb):
body = self._body.run(depth8)
(hand, _) = self._hand.run(body)
(cont, box, hc) = self._contour.run(hand)
if self._contour.not_valid():
return np.array([])
(cont, _, _) = self._contour.run(rgb, True, box, hc, depth)
return cont
class TestPose():
def __init__(self, src):
self._src = src
self._kinect = Kinect()
self._body = BodyDetector()
self._hand = HandDetector()
self._contour = HandContourDetector()
self._pose = PoseClassifier(MultiPoseClassifier(src))
def run(self):
for (depth, depth8, rgb) in self._kinect.get_data():
contour = self._get_hand_contour(depth8, depth, rgb)
if contour.any():
self._contour.draw()
print self._pose.run(contour)
cv2.waitKey(5)
def _get_hand_contour(self, depth8, depth, rgb):
body = self._body.run(depth8)
(hand, _) = self._hand.run(body)
(cont, box, hc) = self._contour.run(hand)
if self._contour.not_valid():
return np.array([])
(cont, _, _) = self._contour.run(rgb, True, box, hc, depth)
return cont
def __init__(self, src):
self._src = src
self._kinect = Kinect()
self._body = BodyDetector()
self._hand = HandDetector()
self._contour = HandContourDetector()
self._pose = PoseClassifier(MultiPoseClassifier(src))
class TestHand():
def __init__(self):
self._kinect = Kinect()
self._body = BodyDetector()
self._hand = HandDetector(HandOtsu())
self._contour = HandContourDetector()
self._palm = PalmDetector()
def run(self):
for (depth, depth8, rgb) in self._kinect.get_data():
cv2.imshow('rgb', cv2.cvtColor(rgb, cv2.COLOR_BGR2RGB))
cv2.imshow('depth', depth8)
hand, mask = self._hand.run(depth, depth8)
(_, _, crop) = self._contour.run(mask)
if crop == None:
continue
cv2.imshow('hand', crop)
hand = self._palm.run(hand, crop)
if hand == None:
continue
cv2.imshow('hand final', hand)
cv2.waitKey(10)
def _crop(self, img, box):
crop = img[box[1]:box[1]+box[3], box[0]:box[0]+box[2]]
return crop
def __init__(self):
""" Initializes the environment. """
# the state space (=observation space) are all possible depth images of the kinect camera
if (self.flattenImage):
self.observation_space = spaces.Box(
low=0,
high=255,
shape=[self.imageHeight * self.imageWidth],
dtype=np.uint8)
else:
self.observation_space = spaces.Box(
low=0,
high=255,
shape=[self.imageHeight, self.imageWidth],
dtype=np.uint8)
boundaries_r = [self.gripperRadiusMin, self.gripperRadiusMax]
boundaries_phi = [0, np.pi]
low = np.array([boundaries_r[0], boundaries_phi[0]])
high = np.array([boundaries_r[1], boundaries_phi[1]])
self.action_space = spaces.Box(low=low, high=high, dtype=np.float32)
self.reward_range = (-np.inf, np.inf)
self.seed()
# create object for box (object to pick up)
self.box = ObjectToPickUp(length=self.boxLength,
width=self.boxWidth,
height=self.boxHeight,
gripperRadius=self.gripperRadiusMax)
# create object for kinect
self.kinect = Kinect(self.imageWidth,
self.imageHeight,
x=0.0,
y=0.0,
z=1.0)
# create object for gripper
self.gripper = Gripper(self.gripperDistance,
self.gripperWidth,
self.gripperHeight,
r=0.0,
phi=0.0)
def __init__(self):
"""
Initialize the window
"""
QtGui.QMainWindow.__init__(self)
# Internal state
self.__kinect = Kinect() # This contains all the logic
self.__objectData = None # Keep this to avoid refresh issues on the image
self.__exportDirectory = os.path.expanduser('~') # Save directory
# Initialize the UI
self.__mainWidget = None
self.__previewImage = None
self.__objectImage = None
self.__captureButton = None
self.__exportButton = None
self.__initUI()
# Events
self.__captureButton.clicked.connect(self.__onCapture)
self.__exportButton.clicked.connect(self.__onExport)
def __init__(self, id, nsamples, dst):
self._id = id
self._nsamples = nsamples
self._dst = dst
self._kinect = Kinect()
self._body = BodyDetector()
self._hand = HandDetector()
self._contour = HandContourDetector()
self._fdesc = FourierDescriptors()
self._train = []
class TrainPose():
def __init__(self, id, nsamples, dst):
self._id = id
self._nsamples = nsamples
self._dst = dst
self._kinect = Kinect()
self._body = BodyDetector()
self._hand = HandDetector()
self._contour = HandContourDetector()
self._fdesc = FourierDescriptors()
self._train = []
def run(self):
warmup = True
for (depth8, depth, rgb) in self._kinect.get_data():
contour = self._get_hand_contour(depth8, depth, rgb)
if not contour:
continue
self._contour.draw()
if warmup:
key = cv2.waitKey(5)
if key == GO:
warmup = False
continue
fd = self._fdesc.run(contour)
self._train.append(fd)
if len(self._train) == self._nsamples:
self._save()
break
cv2.waitKey(5)
def _get_hand_contour(self, depth8, depth, rgb):
body = self._body.run(depth8)
(hand, _) = self._hand.run(body)
(cont, box, hc) = self._contour.run(hand)
if self._contour.not_valid():
return []
(cont, _, _) = self._contour.run(rgb, True, box, hc, depth)
return cont
def _save(self):
data = np.array(self._train)
model = EmpiricalCovariance().fit(np.array(self._train))
output = {'id': self._id, 'data': data, 'model': model}
pickle.dump(output, open(self._dst, 'wb'))
class TrainPose():
def __init__(self, id, nsamples, dst):
self._id = id
self._nsamples = nsamples
self._dst = dst
self._kinect = Kinect()
self._body = BodyDetector()
self._hand = HandDetector()
self._contour = HandContourDetector()
self._fdesc = FourierDescriptors()
self._train = []
def run(self):
warmup = True
for (depth8, depth, rgb) in self._kinect.get_data():
contour = self._get_hand_contour(depth8, depth, rgb)
if not contour:
continue
self._contour.draw()
if warmup:
key = cv2.waitKey(5)
if key == GO:
warmup = False
continue
fd = self._fdesc.run(contour)
self._train.append(fd)
if len(self._train) == self._nsamples:
self._save()
break
cv2.waitKey(5)
def _get_hand_contour(self, depth8, depth, rgb):
body = self._body.run(depth8)
(hand, _) = self._hand.run(body)
(cont, box, hc) = self._contour.run(hand)
if self._contour.not_valid():
return []
(cont, _, _) = self._contour.run(rgb, True, box, hc, depth)
return cont
def _save(self):
data = np.array(self._train)
model = EmpiricalCovariance().fit(np.array(self._train))
output = {'id': self._id, 'data': data, 'model': model}
pickle.dump(output, open(self._dst, 'wb'))
class TrainPose():
def __init__(self, id, nsamples, dst):
self._id = id
self._nsamples = nsamples
self._dst = dst
self._kinect = Kinect()
self._body = BodyDetector()
self._hand = HandDetector()
self._contour = HandContourDetector()
self._feature = GaborDescriptors(4, 4)
def run(self):
warmup = True
train = []
model = pickle.load(open('svm.pck', 'rb'))
for (depth, depth8, rgb) in self._kinect.get_data():
body = self._body.run(depth8)
(hand, _) = self._hand.run(body)
(cont, box, crop) = self._contour.run(hand)
hand = hand[box[1]:box[1]+box[3], box[0]:box[0]+box[2]]
cv2.imshow('hand', hand)
key = cv2.waitKey(2)
#if warmup:
# if key == GO:
# warmup = False
# continue
#if key != 97:
# continue
feature = self._feature.run(hand)
print model.predict(feature)
#train.append(feature)
#if len(train) == self._nsamples:
# self._save(train)
# break
#cv2.waitKey(2)
def _save(self, train):
data = np.array(train)
labels = self._id * np.ones(len(train), np.int)
output = {'labels': labels, 'data': data}
pickle.dump(output, open(self._dst, 'wb'))
def __init__(self):
self._paused = True
self._kinect = Kinect()
gtk.Window.__init__(self)
self.set_default_size(1280, 960)
vbox = gtk.VBox()
self.add(vbox)
# Kinect info visualisation.
self._display = KinectDisplay(self._kinect)
vbox.pack_start(self._display, True, True, 0)
hbox = gtk.HBox()
vbox.pack_start(hbox)
# Game scheme representation.
game_scene = GameSceneArea(self._kinect)
self._display.add_observer(game_scene)
hbox.pack_start(game_scene)
button_vbox = gtk.VBox()
hbox.pack_start(button_vbox)
# Choose static data.
self.choose = gtk.Button('Open', gtk.STOCK_OPEN)
button_vbox.pack_start(self.choose)
self.choose.connect("clicked", self._choose_cb)
# Save button.
self.save = gtk.Button('Save', gtk.STOCK_SAVE)
self.save.set_sensitive(False)
button_vbox.pack_start(self.save)
self.save.connect("clicked", self._save_cb)
# Pause/Autorefresh button.
self.pause = gtk.Button('Pause', gtk.STOCK_MEDIA_PAUSE)
button_vbox.pack_start(self.pause)
self.pause.connect("clicked", self._pause_cb)
self.connect("destroy", gtk.main_quit)
self.show_all()
# Auto-refresh at 10 frames per seconds.
self.timer_id = gobject.timeout_add(self.REFRESH_DELAY,
self._timedout)
class DepthClient:
def __init__(self):
self.kinect = Kinect(SEGMENT_SIZE, ENGAGE_MIN, ENGAGE_MAX)
socket_started = False
self.socket_api = None
while (not socket_started):
try:
self.socket_api = SocketAPI(TCP_IP, TCP_PORT, BUFFER_SIZE,
ACK_TIMEOUT, TERMINATOR)
socket_started = True
except:
print("Socket didn't start, waiting 3 seconds...")
time.sleep(3)
print("Connected!")
self.HandModel = RealTimeHandRecognition("RH", 32, 2)
def run(self):
while True:
frames = self.kinect.get()
if frames is None:
print("waiting for frames...")
time.sleep(1 / 30)
continue
(LH_probs, LH_out), (RH_probs, RH_out) = self.HandModel.classifyLR(
frames[0], frames[1])
LH_idx = np.argmax(LH_out)
LH_label = hand_postures[LH_idx]
RH_idx = np.argmax(RH_out)
RH_label = hand_postures[RH_idx]
#self.socket_api.send_to_server("user:hands:left:probs", LH_out)
#self.socket_api.set("user:hands:left:argmax", int(LH_idx))
self.socket_api.set("user:hands:left", LH_label)
#self.socket_api.send_to_server("user:hands:right:probs", RH_out)
#self.socket_api.set("user:hands:right:argmax", int(RH_idx))
self.socket_api.set("user:hands:right", RH_label)
print(LH_label, RH_label)
class Controller:
def __init__(self):
self.nao = NAO(self)
self.kinect = Kinect()
self.xaalproxy = xAALProxy()
def run(self):
while raw_input("continu ? (Y/N)") == "Y":
if self.kinect.fallDetection():
self.nao.say("detecter la chute")
self.nao.say("detecter la chute")
self.nao.moveToPerson()
self.nao.verifyPersonState(5)
if not self.nao.getVerifyState() % 2:
self.nao.say("Ne t'inquiete pas, je vais demander a quelqu'un pour t'aider")
print "verifystate ", self.nao.getVerifyState()
self.scenario()
self.nao.response(0)
self.nao.say("merci d'etre venu")
else:
self.nao.say("tres bien")
def waitPersonResponse(self, timeout):
count = 0
self.nao.resetVerifyState()
while not self.nao.getVerifyState():
time.sleep(3)
count += 1
if (timeout != 0) and (count > timeout):
print "waitpersonresponse timeout!"
return False
print "getpersonresponse"
return self.nao.getVerifyState() % 2
def sendMail(self):
link = self.webRTCaddress()
topic = "nao_robot/camera/top/camera/image_raw"
text = "please see the video from this address " + link
text = text + " with topic name " + topic
mail = Mail(text)
mail.sendMail()
def webRTCaddress(self):
ni.ifaddresses("wlan0")
ip = ni.ifaddresses("wlan0")[2][0]["addr"] + ":8080"
print "HostIP : ", ip
return ip
def scenario(self):
self.smartDeviceAction("shutterleft", "up")
self.smartDeviceAction("shutterright", "up")
self.smartDeviceAction("lamp1", "on")
self.smartDeviceAction("lamp2", "on")
self.smartDeviceAction("lamp3", "on")
self.nao.say("porte ouverte, volet remonte")
self.smartDeviceAction("switch", "off")
self.nao.say("j'ai eteint l'electricite")
try:
self.sendMail()
except:
print "mail send failed"
self.nao.say("j'ai envoye le mail a vos proches")
self.smartDeviceAction(
"mobilephone", "inform", "msg", "J'ai detecte un probleme. Votre ami a fait un malaise. Venez l'aider."
)
self.nao.say("message vocal envoye")
def smartDeviceAction(self, device, action, key=None, value=None):
self.xaalproxy.sendmsg(device, action, key, value)
def __init__(self):
self._kinect = Kinect()
self._body = BodyDetector()
self._hand = HandDetector(HandOtsu())
self._contour = HandContourDetector()
self._palm = PalmDetector()
#!/usr/bin/env python
from kinect import Kinect
import rospy
#cont=0
kin = Kinect()
def listener(kin):
#rospy.init_node('listener', anonymous=True)
# Get values
#for i in xrange(10):
#print cont, kin.get_posture()
print kin.get_posture()
#cont+=1
# spin() simply keeps python from exiting until this node is stopped
rospy.spin()
if __name__=='__main__':
# Init the Kinect object
listener(kin)
return (self._interval[0], self._interval[0] + i)
def _normalize(self, x):
xn = x / x.sum()
return xn
def show(self):
if self._body.any() and self._depth.any():
img = np.hstack((self._depth, self._body))
str = 'th1=%d th2=%d' %(self._interval[0], self._interval[1])
cv2.imshow(str, img)
if __name__ == '__main__':
from kinect import Kinect
kinect = Kinect()
body = BodyDetector()
for (d, _) in kinect.get_data():
b = body.run(d)
#body.show()
if kinect.stop(kinect.wait()):
break
#import sys; sys.stdin.read(1)
-1)
print(cam_locs)
cam_locs = np.concatenate([
cam_locs[..., ::-1], depth[cam_locs[:, 0], cam_locs[:, 1]].reshape(
-1, 1)
], -1)
valid = np.logical_and(cam_locs[:, -1] > 200, cam_locs[:, -1] < 4000)
return delta_image, cam_locs[valid], proj_locs[valid]
w = Fullscreen_Window()
w.clear()
k = Kinect()
k.start()
a = 0
all_cam_locs, all_proj_locs = [], []
for i in range(20):
im, cam_locs, proj_locs = get_me_some_correspondances(k, w)
im += 0.5
all_cam_locs.append(cam_locs)
all_proj_locs.append(proj_locs)
print(cam_locs)
plt.imshow(im)
import time from kinect import Kinect, X, Y, Z from midi_interface import MidiInterface from utilities import sprout, scale_constrain from events import Events key = 60 beat = 0.125 kinect = Kinect.retrieve() midi = MidiInterface.retrieve() min_piano_dur = 0.15 min_piano_up = 0.05 events = Events.retrieve()
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()
from kinect import Kinect
new_kinect=Kinect()
while True:
coords=new_kinect.update_xy()
print str(coords)
if coords==None:
new_kinect.select_skeleton()
import time from kinect import Kinect, X, Y, Z from midi_interface import MidiInterface from utilities import sprout, scale_constrain from events import Events key = 60 beat = 0.125 kinect = Kinect.retrieve() midi = MidiInterface.retrieve() min_piano_dur = 0.15 min_piano_up = 0.05 events = Events.retrieve()
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
from kinect import Kinect import time from settings import RUN_TIME k = Kinect() #k.run(6) # # for m in means: # print m # k.execute() k.run(RUN_TIME)
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
from kinect import Kinect
import cv2
k = Kinect()
cv2.namedWindow("Raw")
cv2.namedWindow("Thresh")
cv2.namedWindow("Masked")
cv2.namedWindow("Mask")
cv2.namedWindow("Delta")
cv2.createTrackbar("tmin", "Thresh", k.tmin, 255, lambda: None)
cv2.createTrackbar("tmax", "Thresh", k.tmax, 255, lambda: None)
print("Kinect test")
print("C: Calibrate mask")
print("O: Set origin")
while 1:
k.tmin = cv2.getTrackbarPos("tmin", "Thresh")
k.tmax = cv2.getTrackbarPos("tmax", "Thresh")
k.update()
cv2.imshow("Raw", k.raw)
cv2.imshow("Thresh", k.thresh)
cv2.imshow("Masked", k.masked)
cv2.imshow("Mask", k.mask)
delta = k.masked
cv2.circle(delta, k.origin, 8, 127, -1)
def __init__(self):
self._kinect = Kinect()
self._body = BodyDetector()
self._hand = HandDetector(HandOtsu())
self._contour = HandContourDetector()
self._palm = PalmDetector()
def __init__(self):
self.nao = NAO(self)
self.kinect = Kinect()
self.xaalproxy = xAALProxy()
import time
import pickle
from project_gl import Projector
width, height = 1920, 1080
M = pickle.load(open('direct_model/correspondances/one_matrix.pickle', 'rb'))
M = np.concatenate([M, [[0, 0, 0, 0]]], 0)
p = Projector(M,
x_res=width,
y_res=height,
proj_x_res=1366,
proj_y_res=768,
monitor=1)
from kinect import Kinect
k = Kinect()
k.start()
# depth = 1000 * np.ones([height, width], dtype=np.float32)
while True:
# rgb = 255*np.random.rand(height, width, 3)
# rgb = np.random.randint(0, 255, size=[height, width, 3])
rgb, depth = k.get_current_rgbd_frame()
if p.draw_frame(255 - rgb[..., :3], depth):
p.stop()
k.stop()
from maskrcnn_benchmark.config import cfg
from mask_rcnn_demo.predictor import COCODemo
import numpy as np
from kinect import Kinect
k = Kinect()
k.start()
config_file = "mask_rcnn_demo/maskrcnn-benchmark/configs/caffe2/e2e_mask_rcnn_R_50_FPN_1x_caffe2.yaml"
# update the config options with the config file
cfg.merge_from_file(config_file)
# manual override some options
cfg.merge_from_list(["MODEL.DEVICE", "cuda"])
coco_demo = COCODemo(
cfg,
min_image_size=800,
confidence_threshold=0.7,
)
# load image and then run prediction
while True:
print('a')
image, d = k.get_current_rgbd_frame(copy=True)
image = image[..., :3]
print('d')
predictions = coco_demo.run_on_opencv_image(image)
print('c')
print(predictions.shape, predictions.dtype)
class Main:
def __init__(self, simulate, conn, envFile='environment.xml'):
self.conn = conn #Conn is the link to our QT GUI which tells it when a traj is ready
self.simulate = simulate #boolean which dictates whether PackBot will execute a traj
self.env = Environment()
self.env.SetViewer('qtcoin')
self.env.Load(envFile) #contains the robot and the floor
self.robot = self.env.GetRobots()[0]
self.robot.SetActiveDOFs(range(5))
self.taskmanip = TaskManipulation(self.robot) #used for controlling the gripper
self.basemanip = BaseManipulation(self.robot) #used for controlling EOD arm
self.simController = RaveCreateController(self.env,'IdealController')
if self.simulate:
self.robotWrapper = RobotWrapper(self.robot, self.simController, self.simController, True)
self.robotWrapper.swapToSimController()
self.grasp = Graspability(self.env, self.robotWrapper, lambda:self.robot.GetDOFValues()[:5])
self.graspSelector = GraspInterface(self.env, self.simController, self.robotWrapper, self.grasp.getTraj, self.simulate)
else:
# real robot so we have a video stream to start
self.videoThread = Thread(target=showVideo)
self.videoThread.daemon = False
self.videoThread.start()
#Establishing a network connection to PackBot...
self.controller = PackBotController(self.env)
self.controller.InitializeCommunications()
print 'giving time to make establish full connection to packbot'
time.sleep(0.5)
#initializes the arm callback
self.robotWrapper = RobotWrapper(self.robot, self.simController, self.controller)
self.robotWrapper.swapToRealController()
time.sleep(.5)
#graspability optimizes grasps to approach cans straight-on
self.grasp = Graspability(self.env, self.robotWrapper, self.robot.GetDOFValues)
#click-to-grasp callback thread
self.graspSelector = GraspInterface(self.env, self.controller, self.robotWrapper, self.grasp.getTraj, self.simulate)
#Custom UI elements which color graspable objects green and enable camera+kinect field of view shading
self.highlight = HighlightGraspable(self.env, self.grasp.getTraj)
self.cam = HeadCamera(self.env, self.robot)
self.kinect = Kinect(self.env, self.robot)
#Using the kinect+PCL to detect cylindrical objects and add them to the env
self.detect = DetectCylinder(self.env, self.robot)
self.cylinders = self.detect.getCylinders()
self.rawKinect = RawKinect(self.env, self.robot)
#Setup the run variables
self.runLock = Lock()
self.run = True
# create a lock and a 20Hz thread to handle various FoV elements
self.DrawPackBotFoV = False
self.ViewPackBotFoV = False
self.DrawKinectFoV = False
self.UpdateLock = Lock()
self.foVUpdateThread = Thread(target=self._FoVUpdate)
self.foVUpdateThread.daemon = False
self.foVUpdateThread.start()
# We want to show the voxel grid by default
self.ShowVoxelGrid = True
# We don't want to see the point cloud by default
self.ShowPointCloud = False
# Initialize the number of voxels we want to draw
self.numVoxelY = 100
self.numVoxelX = 200
self.numVoxelZ = 300
if self.graspSelector.handle is None:
print 'Callback was not registered -- grasp selection will not work'
def Execute(self, traj=None):
'''
Execute the last valid trajectory saved in self.graspSelector
Args:
Traj: The trajectory to be executed. By default, this is none and
will be parsed from the graspselector function (click2grip.py)
Returns:
Nothing. Runs a trajectory on the robot or in simulation
'''
# save the currect active manip
previousManip = self.robot.GetActiveManipulator()
# set the active maninipulator to the gripper while we run
manip = self.robot.SetActiveManipulator("gripper")
if traj:
result = traj
else:
result = self.graspSelector.getTraj()
if result:
if self.simulate:
self.robotWrapper.swapToSimController()
self.taskmanip.ReleaseFingers()
self.robot.WaitForController(0)
self.robot.GetController().SetPath(result)
self.robot.WaitForController(0)
self.taskmanip.CloseFingers()
self.robot.WaitForController(0)
self.robotWrapper.swapToRealController()
else:
print 'Running grasp in:'
for t in ['3','2','1']:
print t
time.sleep(1)
print 'GO'
self.controller.Play()
self.controller.GripOpen()
print 'Wait for grip to open'
time.sleep(3)
self.controller.TestTrajectorySending(result)
self.controller.GripClose()
else:
print 'No valid trajectory found'
# reset the active manipulator
self.robot.SetActiveManipulator(previousManip)
def parseCommand(self, command):
'''
parseCommands receives commands sent from the QTViewer
Commmands are transmitted as a list.
'''
# TODO: make a dictionary of the valid commands with appropriate functions
print 'Received command for:', command[0]
if command[0] == 'UpdateObstacleData':
self.UpdateObstacleData()
elif command[0] == 'UpdateCylinderData':
self.UpdateCylinderData()
elif command[0] == 'Plan':
self.PlanTrajectories()
elif command[0] == 'Execute':
self.Execute()
elif command[0] == 'Quit':
self.quit()
elif command[0] == 'Home':
self.home()
elif command[0] == 'DrawPackBotFoV':
with self.UpdateLock:
self.DrawPackBotFoV = command[1]
elif command[0] == 'ViewPackBotFoV':
with self.UpdateLock:
self.ViewPackBotFoV = command[1]
elif command[0] == 'DrawKinectFoV':
with self.UpdateLock:
self.DrawKinectFoV = command[1]
elif command[0] == 'ShowVoxel':
self.ShowVoxelGrid = command[1]
elif command[0] == 'ShowPointCloud':
self.ShowPointCloud = command[1]
elif command[0] == 'UpdateNumVoxel':
if command[2] == 'X':
self.numVoxelX = command[1]
elif command[2] == 'Y':
self.numVoxelY = command[1]
elif command[2] == 'Z':
self.numVoxelZ = command[1]
def home(self):
'''
Home the PackBot (move arm to a "Packed" pose) suitable for storage
'''
solution = [-0.0018, -2.9191, 2.381, -2.592, -1.5702]
self.robot.SetActiveDOFValues(solution)
traj = self.basemanip.MoveActiveJoints(goal=solution, outputtrajobj=True,
maxiter=4000, steplength=0.01, maxtries=2, execute=False)
self.controller.Play()
self.controller.TestTrajectorySending(traj)
def UpdateObstacleData(self):
'''
Update the obstacle data from the kinect
'''
self.rawKinect.clearData()
self.rawKinect.setCylinders(self.cylinders)
self.rawKinect.displayData(self.ShowVoxelGrid, self.ShowPointCloud,
self.numVoxelY, self.numVoxelX, self.numVoxelZ)
print 'Done obstacle update'
def UpdateCylinderData(self):
'''
UpdateCylinderData clears the environment of cylinders and searches
kinect data for cylinders again
'''
self.detect.deleteCylinders(self.cylinders)
self.grasp.reset()
self.graspSelector.saveTraj(None)
self.detect.findCylinders()
self.cylinders = self.detect.getCylinders()
print 'Done cylinder update'
def PlanTrajectories(self):
'''
Plan trajectories to the detected cylinders and color them appropriately.
Graspable cylinders are green, all others are red.
'''
self.highlight.colorKinBodies(self.cylinders)
print 'Done planning'
def _FoVUpdate(self):
'''
Function to update various FoV elements at ~20Hz
'''
run = False
with self.runLock:
run = self.run
if run:
print 'Ready'
count = 0
while run:
with self.UpdateLock:
if self.ViewPackBotFoV:
self.cam.enterFirstPerson()
else:
self.cam.returnToMainCam()
if self.DrawPackBotFoV:
self.cam.drawFoV()
else:
self.cam.clearFoV()
if self.DrawKinectFoV:
self.kinect.drawFoV()
else:
self.kinect.clearFoV()
if count > 19:
count = 0
if self.graspSelector.getTraj():
self.conn.send(['SetExecuteColor', 'green'])
else:
self.conn.send(['SetExecuteColor', 'red'])
count += 1
time.sleep(0.05)
with self.runLock:
run = self.run
def start(self):
'''
Starts the main loop to check for updates from the gui and
initializes the cylinders and obstacle data
'''
run = False
with self.runLock:
run = self.run
while run:
command = self.conn.recv()
self.parseCommand(command)
with self.runLock:
run = self.run
def test(self):
'''
Function to test trajectory planning and target coloration
'''
self.cylinders = [self.env.GetKinBody('target'+str(i+1)) for i in range(1)]
self.highlight.colorKinBodies(self.cylinders)
def quit(self):
'''
Method to turn run to off and to cleanup the various threads
'''
with self.runLock:
self.run = False
self.foVUpdateThread.join()
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)
def __init__(self, simulate, conn, envFile='environment.xml'):
self.conn = conn #Conn is the link to our QT GUI which tells it when a traj is ready
self.simulate = simulate #boolean which dictates whether PackBot will execute a traj
self.env = Environment()
self.env.SetViewer('qtcoin')
self.env.Load(envFile) #contains the robot and the floor
self.robot = self.env.GetRobots()[0]
self.robot.SetActiveDOFs(range(5))
self.taskmanip = TaskManipulation(self.robot) #used for controlling the gripper
self.basemanip = BaseManipulation(self.robot) #used for controlling EOD arm
self.simController = RaveCreateController(self.env,'IdealController')
if self.simulate:
self.robotWrapper = RobotWrapper(self.robot, self.simController, self.simController, True)
self.robotWrapper.swapToSimController()
self.grasp = Graspability(self.env, self.robotWrapper, lambda:self.robot.GetDOFValues()[:5])
self.graspSelector = GraspInterface(self.env, self.simController, self.robotWrapper, self.grasp.getTraj, self.simulate)
else:
# real robot so we have a video stream to start
self.videoThread = Thread(target=showVideo)
self.videoThread.daemon = False
self.videoThread.start()
#Establishing a network connection to PackBot...
self.controller = PackBotController(self.env)
self.controller.InitializeCommunications()
print 'giving time to make establish full connection to packbot'
time.sleep(0.5)
#initializes the arm callback
self.robotWrapper = RobotWrapper(self.robot, self.simController, self.controller)
self.robotWrapper.swapToRealController()
time.sleep(.5)
#graspability optimizes grasps to approach cans straight-on
self.grasp = Graspability(self.env, self.robotWrapper, self.robot.GetDOFValues)
#click-to-grasp callback thread
self.graspSelector = GraspInterface(self.env, self.controller, self.robotWrapper, self.grasp.getTraj, self.simulate)
#Custom UI elements which color graspable objects green and enable camera+kinect field of view shading
self.highlight = HighlightGraspable(self.env, self.grasp.getTraj)
self.cam = HeadCamera(self.env, self.robot)
self.kinect = Kinect(self.env, self.robot)
#Using the kinect+PCL to detect cylindrical objects and add them to the env
self.detect = DetectCylinder(self.env, self.robot)
self.cylinders = self.detect.getCylinders()
self.rawKinect = RawKinect(self.env, self.robot)
#Setup the run variables
self.runLock = Lock()
self.run = True
# create a lock and a 20Hz thread to handle various FoV elements
self.DrawPackBotFoV = False
self.ViewPackBotFoV = False
self.DrawKinectFoV = False
self.UpdateLock = Lock()
self.foVUpdateThread = Thread(target=self._FoVUpdate)
self.foVUpdateThread.daemon = False
self.foVUpdateThread.start()
# We want to show the voxel grid by default
self.ShowVoxelGrid = True
# We don't want to see the point cloud by default
self.ShowPointCloud = False
# Initialize the number of voxels we want to draw
self.numVoxelY = 100
self.numVoxelX = 200
self.numVoxelZ = 300
if self.graspSelector.handle is None:
print 'Callback was not registered -- grasp selection will not work'
class SmartBotEnv(gym.Env):
rewardSuccess = 500
rewardFailure = 0
rewardUnreachablePosition = -5 # TODO: do we need this anymore?
# when set to True, the reward will be rewardSuccess if gripper could grasp the object, rewardFailure otherwise
# when set to False, the reward will be calculated from the distance between the gripper & the position of success
binaryReward = False
# some algorithms (like the ddpg from /home/joel/Documents/gym-gazebo/examples/pincher_arm/smartbot_pincher_kinect_ddpg.py)
# currently assume that the observation_space has shape (x,) instead of (220,300), so for those algorithms set this to True
flattenImage = False
state = np.array([])
imageWidth = 320
imageHeight = 160
boxLength = 0.02
boxWidth = 0.02
boxHeight = 0.03
gripperDistance = 0.032 # between the fingers
gripperHeight = 0.035 # from base to finger tip
gripperWidth = 0.03
gripperRadiusMax = 0.2 # maximal distance between robot base and gripper on the floor
gripperRadiusMin = 0.04 # minimal distance between robot base and gripper on the floor
firstRender = True
def __init__(self):
""" Initializes the environment. """
# the state space (=observation space) are all possible depth images of the kinect camera
if (self.flattenImage):
self.observation_space = spaces.Box(
low=0,
high=255,
shape=[self.imageHeight * self.imageWidth],
dtype=np.uint8)
else:
self.observation_space = spaces.Box(
low=0,
high=255,
shape=[self.imageHeight, self.imageWidth],
dtype=np.uint8)
boundaries_r = [self.gripperRadiusMin, self.gripperRadiusMax]
boundaries_phi = [0, np.pi]
low = np.array([boundaries_r[0], boundaries_phi[0]])
high = np.array([boundaries_r[1], boundaries_phi[1]])
self.action_space = spaces.Box(low=low, high=high, dtype=np.float32)
self.reward_range = (-np.inf, np.inf)
self.seed()
# create object for box (object to pick up)
self.box = ObjectToPickUp(length=self.boxLength,
width=self.boxWidth,
height=self.boxHeight,
gripperRadius=self.gripperRadiusMax)
# create object for kinect
self.kinect = Kinect(self.imageWidth,
self.imageHeight,
x=0.0,
y=0.0,
z=1.0)
# create object for gripper
self.gripper = Gripper(self.gripperDistance,
self.gripperWidth,
self.gripperHeight,
r=0.0,
phi=0.0)
# __init__
def seed(self, seed=None):
""" Seeds the environment (for replicating the pseudo-random processes). """
self.np_random, seed = seeding.np_random(seed)
return [seed]
# seed
def reset(self):
""" Resets the state of the environment and returns an initial observation."""
# place box
self.box.place(randomPlacement=True)
# for testing purposes
# self.box.place(randomPlacement=False, x=0.0, y=0.1, phi=np.pi/2)
# get depth image
image = self.kinect.getImage(self.box,
filter=False,
flattenImage=self.flattenImage,
saveImage=True)
self.state = image
return self.state
# reset
def close(self):
""" Closes the environment and shuts down the simulation. """
logging.info("closing SmartBotEnv")
super(gym.Env, self).close()
# close
stepcount = 0
winkel = np.linspace(0, np.pi)
def step(self, action):
""" Executes the action (i.e. moves the arm to the pose) and returns the reward and a new state (depth image). """
# determine if gripper could grasp the ObjectToPickUp
gripperR = action[0].astype(np.float64)
gripperPhi = action[1].astype(np.float64)
# # for testing purposes
# gripperR = 0.1
# gripperPhi = self.winkel[self.stepcount]
# self.stepcount += 1
self.gripper.place(gripperR, gripperPhi)
logging.debug("moving arm to position: [{0} {1}]".format(
gripperR, gripperPhi))
logging.debug("box position: [{0} {1} {2}]".format(
self.box.pos[0], self.box.pos[1], self.box.phi))
reward, graspSuccess = self.calculateReward()
logging.debug("received reward: " + str(reward))
# re-place object to pick up if grasp was successful
# if(graspSuccess):
self.box.place(randomPlacement=True)
# get depth image
image = self.kinect.getImage(self.box,
filter=False,
flattenImage=self.flattenImage,
saveImage=True)
self.state = image
done = True
info = {}
return self.state, reward, done, info
# step
def calculateReward(self):
""" Calculates the reward for the current timestep, according to the gripper position and the pickup position.
A high reward is given if the gripper could grasp the box (pickup) if it would close the gripper. """
# check if center of gravity of box is between the gripper fingers (i.e. inside the ag-bg-cg-dg polygon)
# see e.g.: https://stackoverflow.com/a/23453678
bbPath_gripper = mplPath.Path(
np.array([
self.gripper.a, self.gripper.b, self.gripper.c, self.gripper.d
]))
# one finger is between a & b, the other finger is between c & d
cogBetweenFingers = bbPath_gripper.contains_point(
(self.box.pos[0], self.box.pos[1]))
logging.debug("center of gravity is between the fingers: {}".format(
cogBetweenFingers))
# check if both gripper fingers don't intersect with the box
bbPath_box = mplPath.Path(
np.array([self.box.a, self.box.b, self.box.c, self.box.d]))
bbPath_gripper_left = mplPath.Path(
np.array([self.gripper.a, self.gripper.b]))
bbPath_gripper_right = mplPath.Path(
np.array([self.gripper.c, self.gripper.d]))
leftGripperCrashes = bbPath_box.intersects_path(bbPath_gripper_left,
filled=True)
rightGripperCrashes = bbPath_box.intersects_path(bbPath_gripper_right,
filled=True)
logging.debug("left gripper crashes: {}".format(leftGripperCrashes))
logging.debug("right gripper crashes: {}".format(rightGripperCrashes))
# if the center of gravity of the box is between the gripper fingers and none of the fingers collide with the box, we are able to grasp the box
if (cogBetweenFingers and not leftGripperCrashes
and not rightGripperCrashes):
logging.info(
"********************************************* grasping would be successful! *********************************************"
)
graspSuccess = True
else:
graspSuccess = False
if (self.binaryReward):
if (graspSuccess):
return self.rewardSuccess, graspSuccess
else:
return self.rewardFailure, graspSuccess
else:
# calculate reward according to the distance from the gripper to the center of gravity of the box
distance = np.linalg.norm(self.gripper.pos -
self.box.pos) # e.g. 0.025
# invert the distance, because smaller distance == closer to the goal == more reward
reward = 1.0 / (2 * distance)
# scale the reward if grasping would be successful
if (graspSuccess):
reward = 50 * reward
# elif(leftGripperCrashes or rightGripperCrashes): # "punishement" for crashing into the box
# reward = reward / 5
reward = min(reward, 1000)
return reward, graspSuccess
# if
# calculateReward
def render(self, mode='human'):
if (self.firstRender):
# display stuff
plt.ion()
self.fig, self.ax = plt.subplots()
self.ax.axis("equal")
self.ax.set_xlim([
-self.box.gripperRadius - self.box.length - 0.2,
self.box.gripperRadius + self.box.length + 0.2
])
self.ax.set_ylim([
0 - self.box.length - 0.2,
self.box.gripperRadius + self.box.length + 0.2
])
self.gripperLeftPoly = patches.Polygon(
[self.gripper.a, self.gripper.b], closed=True, color="black")
self.gripperRightPoly = patches.Polygon(
[self.gripper.c, self.gripper.d], closed=True, color="black")
self.pickMeUpPoly = patches.Polygon(
[self.box.a, self.box.b, self.box.c, self.box.d],
closed=True,
color="red")
self.ax.add_artist(self.gripperLeftPoly)
self.ax.add_artist(self.gripperRightPoly)
self.ax.add_artist(self.pickMeUpPoly)
self.firstRender = False
# if
plt.ion()
plt.cla()
self.gripperLeftPoly.set_xy([self.gripper.a, self.gripper.b])
self.gripperRightPoly.set_xy([self.gripper.c, self.gripper.d])
self.pickMeUpPoly.set_xy(
[self.box.a, self.box.b, self.box.c, self.box.d])
self.ax.add_artist(self.gripperLeftPoly)
self.ax.add_artist(self.gripperRightPoly)
self.ax.add_artist(self.pickMeUpPoly)
# self.fig.canvas.draw()
plt.pause(0.0001)
plt.draw()
class KinectTestWindow(gtk.Window):
REFRESH_DELAY = 500 # ms
def __init__(self):
self._paused = True
self._kinect = Kinect()
gtk.Window.__init__(self)
self.set_default_size(1280, 960)
vbox = gtk.VBox()
self.add(vbox)
# Kinect info visualisation.
self._display = KinectDisplay(self._kinect)
vbox.pack_start(self._display, True, True, 0)
hbox = gtk.HBox()
vbox.pack_start(hbox)
# Game scheme representation.
game_scene = GameSceneArea(self._kinect)
self._display.add_observer(game_scene)
hbox.pack_start(game_scene)
button_vbox = gtk.VBox()
hbox.pack_start(button_vbox)
# Choose static data.
self.choose = gtk.Button('Open', gtk.STOCK_OPEN)
button_vbox.pack_start(self.choose)
self.choose.connect("clicked", self._choose_cb)
# Save button.
self.save = gtk.Button('Save', gtk.STOCK_SAVE)
self.save.set_sensitive(False)
button_vbox.pack_start(self.save)
self.save.connect("clicked", self._save_cb)
# Pause/Autorefresh button.
self.pause = gtk.Button('Pause', gtk.STOCK_MEDIA_PAUSE)
button_vbox.pack_start(self.pause)
self.pause.connect("clicked", self._pause_cb)
self.connect("destroy", gtk.main_quit)
self.show_all()
# Auto-refresh at 10 frames per seconds.
self.timer_id = gobject.timeout_add(self.REFRESH_DELAY,
self._timedout)
def _choose_cb(self, widget, data=None):
# Create file chooser.
dialog = gtk.FileChooserDialog("Open..",
None,
gtk.FILE_CHOOSER_ACTION_OPEN,
(gtk.STOCK_CANCEL, gtk.RESPONSE_CANCEL,
gtk.STOCK_OPEN, gtk.RESPONSE_OK))
dialog.set_default_response(gtk.RESPONSE_OK)
# Get only numpy arrays.
filter = gtk.FileFilter()
filter.set_name("Numpy arrays")
filter.add_pattern("*_depth.npy")
dialog.add_filter(filter)
response = dialog.run()
chosen = response == gtk.RESPONSE_OK
if chosen:
# Extract file basename.
filename = dialog.get_filename()[:-10]
basename = os.path.basename(filename)
self._kinect.set_filename(basename)
print basename, 'selected'
dialog.destroy()
# Refresh GUI if needed.
if chosen:
self._display.refresh_data()
self.queue_draw()
def _save_cb(self, widget, data=None):
rgb = self._kinect.latest_rgb
depth = self._kinect.latest_depth
fname_base = time.strftime('%Y-%m-%d_%H-%M-%S', time.localtime())
numpy.save(fname_base + '_rgb', rgb)
numpy.save(fname_base + '_depth', depth)
print 'Saved with "%s" base filename' % fname_base
def _pause_cb(self, widget, data=None):
self._paused = not self._paused
self.save.set_sensitive(self._paused)
self.choose.set_sensitive(self._paused)
if not self._paused:
self.pause.set_label(gtk.STOCK_MEDIA_PAUSE)
# Try to prevent unwanted redraw.
if not data:
self._display.refresh_data()
self.queue_draw()
self.timer_id = gobject.timeout_add(self.REFRESH_DELAY,
self._timedout)
else:
self.pause.set_label(gtk.STOCK_REFRESH)
def _timedout(self):
# Stop auto refresh if no Kinect is detected.
if self._kinect.latest_present:
self._display.refresh_data()
self.queue_draw()
else:
if not self._paused:
print 'No Kinect found, stopping auto-refresh'
self._pause_cb(None, True)
# Timer is repeated until False is returned.
return not self._paused
def run(self):
gtk.main()
from robot import Robot
from kinect2path import plan, World, coordTransform, getLocalGoal, local2global
# from matplotlib import pyplot as plt
TCP_IP = '192.168.10.7'
TCP_PORT = 50000
BUFFER_SIZE = 1024
PLAN_TIME = 100.0
x_g = [-1.5, 0] # Goal in global coordinates
th_g = 3.0* math.pi/4.0 # Goal in global coordinates (+CCW)
vicon = Vicon(TCP_IP, TCP_PORT, BUFFER_SIZE)
kinect = Kinect()
robot = Robot(obstacle_avoid=True,min_motor_speed=20,max_motor_speed=180)
robot.open('/dev/ttyTHS2', 19200)
robot.write_motors() # Writes initial value (0) to motors
traj_count = 1
t_init = time.time()
t_plan = t_init
world_size = [51,25]
local_start = [int(world_size[0]/2.0), 0]
for i in range(5):
raw_depth = kinect.get_raw_depth()
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)
# [0, 1, 0, 0],
# [0, 0, 1, 0],
# ])
if GRID_VS_LINE:
gridshape = (30, 60)
rows, cols = np.indices(gridshape)
rows = (1080 * rows / gridshape[0]).astype(np.int32)
cols = (1920 * cols / gridshape[1]).astype(np.int32)
y_coords = rows.flatten()
x_coords = cols.flatten()
else:
y_coords = np.ones([1920]) * 1080 / 2
x_coords = np.arange(1920)
k = Kinect()
k.start()
for i in range(500):
start = time.time()
rgb, d = k.get_current_rgbd_frame(copy=False)
rgb[..., 2] = np.logical_or(d > 5000, d < 150) * 255
print("kinect time: %f\t" % (time.time() - start), end="")
cv2.imshow('color', rgb)
cv2.waitKey(1)
start = time.time()
if GRID_VS_LINE:
d_coord = d[rows, cols].flatten()
else:
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()
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)
""" 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)
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)
#!/usr/bin/env python
# -*- coding: utf-8 -*-
import cv2
import numpy as np
import config
from kinect import Kinect
from body import BodyDetector
from hand import HandDetector, HandContourDetector
from pose import PoseClassifier, OpenCloseClassifier, MultiPoseClassifier
kinect = Kinect()
body = BodyDetector()
hand = HandDetector()
contour = HandContourDetector()
# pose = PoseClassifier(OpenCloseClassifier())
pose = PoseClassifier(MultiPoseClassifier())
for (depth, depth8, rgb) in kinect.get_data():
b = body.run(depth8)
(h, _) = hand.run(b)
# cv2.imshow('hand', h)
(ccc, box, hc) = contour.run(h)
if len(ccc) < 100:
