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)
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,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
TODO: add other motors here as needed with their correct address"""
self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
port_num = self.dxlbus.port()
base = DXL_MX(port_num, 1)
shld = DXL_MX(port_num, 2)
elbw = DXL_MX(port_num, 3)
wrst = DXL_AX(port_num, 4)
wrst2 = DXL_AX(port_num, 5)
wrst3 = DXL_XL(port_num, 6)
gripper = DXL_XL(port_num, 7)
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2,wrst3),gripper)
self.tp = TrajectoryPlanner(self.rexarm)
self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
"""
Attach Functions to Buttons & Sliders
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btn_exec.clicked.connect(self.execute)
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
self.ui.sldrBase.valueChanged.connect(self.sliderChange)
self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist2.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist3.valueChanged.connect(self.sliderChange)
#self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
self.ui.rdoutStatus.setText("Waiting for input")
"""Team10 section for buttons"""
self.ui.btnUser2.setText("teach")
self.ui.btnUser2.clicked.connect(partial(self.sm.set_next_state, "teach"))
self.ui.btnUser3.setText("repeat")
self.ui.btnUser3.clicked.connect(partial(self.sm.set_next_state, "repeat"))
self.ui.btnUser4.setText("Set ROI")
self.ui.btnUser4.clicked.connect(partial(self.sm.set_next_state, "set_roi"))
self.ui.btnUser5.setText("Set Exclusion")
self.ui.btnUser5.clicked.connect(partial(self.sm.set_next_state, "set_exclusion"))
self.ui.btnUser6.setText("Save frames")
self.ui.btnUser6.clicked.connect(partial(self.sm.set_next_state, "save_frames"))
self.ui.btn_task3.clicked.connect(partial(self.sm.set_next_state, "task3"))
self.ui.btnUser7.setText("Click & Grab")
self.ui.btnUser7.clicked.connect(partial(self.sm.set_next_state, "ClickandGrab"))
"""initalize manual control off"""
self.ui.SliderFrame.setEnabled(False)
"""initalize rexarm"""
self.rexarm.initialize()
"""Setup Threads"""
self.videoThread = VideoThread(self.kinect)
self.videoThread.updateFrame.connect(self.setImage)
self.videoThread.start()
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
self.displayThread = DisplayThread(self.rexarm, self.sm)
self.displayThread.updateJointReadout.connect(self.updateJointReadout)
self.displayThread.updateEndEffectorReadout.connect(self.updateEndEffectorReadout)
self.displayThread.updateStatusMessage.connect(self.updateStatusMessage)
self.displayThread.start()
"""
Setup Timer
this runs the trackMouse function every 50ms
"""
self._timer = QTimer(self)
self._timer.timeout.connect(self.trackMouse)
self._timer.start(50)
def __init__(self,parent=None):
QWidget.__init__(self,parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Set GUI to track mouse """
QWidget.setMouseTracking(self,True)
"""dynamixel bus -- add other motors here"""
self.dxlbus = DXL_BUS(DEVICENAME, BAUDRATE)
port_num = self.dxlbus.port()
base = DXL_MX(port_num, 0)
shld = DXL_MX(port_num, 1)
elbw = DXL_MX(port_num, 2)
wrst = DXL_AX(port_num, 3)
wrst2 = DXL_XL(port_num, 4)
grip = DXL_XL(port_num, 5)
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm((base,shld,elbw,wrst,wrst2,grip),0)
self.tp = TrajectoryPlanner(self.rexarm)
self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
"""
Attach Functions to Buttons & Sliders
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btn_exec.clicked.connect(partial(self.sm.set_next_state, "execute"))
self.ui.btn_cali.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
# self.ui.btnUser1.setText("Calibrate")
# self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "calibrate"))
self.ui.btnUser1.setText("Teach and Repeat")
self.ui.btnUser1.clicked.connect(partial(self.sm.set_next_state, "teachNRepeat"))
self.ui.btnUser2.setText("Record Waypoints")
self.ui.btnUser2.clicked.connect(partial(self.sm.set_next_state, "recordWaypoint"))
self.ui.btnUser3.setText("Play")
self.ui.btnUser3.clicked.connect(partial(self.sm.set_next_state, "play"))
self.ui.btnUser4.setText("Block Detection Start")
self.ui.btnUser4.clicked.connect(partial(self.sm.set_next_state, "blockDetectionStart"))
self.ui.btnUser5.setText("Block Message")
self.ui.btnUser5.clicked.connect(partial(self.sm.set_next_state, "blockMessage"))
self.ui.btnUser6.setText("Block Detection End")
self.ui.btnUser6.clicked.connect(partial(self.sm.set_next_state, "blockDetectionEnd"))
self.ui.btnUser7.setText("Click and Grab")
self.ui.btnUser7.clicked.connect(partial(self.sm.set_next_state, "clickNGrab"))
self.ui.btnUser8.setText("Pick n' Place")
self.ui.btnUser8.clicked.connect(partial(self.sm.set_next_state, "pickNPlace"))
self.ui.btnUser9.setText("Pick n' Stack")
self.ui.btnUser9.clicked.connect(partial(self.sm.set_next_state, "pickNStack"))
self.ui.btnUser10.setText("Line 'em Up!")
self.ui.btnUser10.clicked.connect(partial(self.sm.set_next_state, "lineUp"))
self.ui.btnUser11.setText("Stack 'em High!")
self.ui.btnUser11.clicked.connect(partial(self.sm.set_next_state, "stackHigh"))
self.ui.btnUser12.setText("Pyramid Builder!")
self.ui.btnUser12.clicked.connect(partial(self.sm.set_next_state, "buildPyramid"))
self.ui.sldrBase.valueChanged.connect(self.sliderChange)
self.ui.sldrShoulder.valueChanged.connect(self.sliderChange)
self.ui.sldrElbow.valueChanged.connect(self.sliderChange)
self.ui.sldrWrist.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip1.valueChanged.connect(self.sliderChange)
self.ui.sldrGrip2.valueChanged.connect(self.sliderChange)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
self.ui.rdoutStatus.setText("Waiting for input")
"""initalize manual control off"""
self.ui.SliderFrame.setEnabled(False)
"""initalize rexarm"""
self.rexarm.initialize()
"""Setup Threads"""
self.videoThread = VideoThread(self.kinect)
self.videoThread.updateFrame.connect(self.setImage)
self.videoThread.start()
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
self.displayThread = DisplayThread(self.rexarm, self.sm)
self.displayThread.updateJointReadout.connect(self.updateJointReadout)
self.displayThread.updateEndEffectorReadout.connect(self.updateEndEffectorReadout)
self.displayThread.updateStatusMessage.connect(self.updateStatusMessage)
self.displayThread.start()
"""
Setup Timer
this runs the trackMouse function every 50ms
"""
self._timer = QTimer(self)
self._timer.timeout.connect(self.trackMouse)
self._timer.start(50)
# [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:
def __init__(self, parent=None):
QWidget.__init__(self, parent)
self.ui = Ui_MainWindow()
self.ui.setupUi(self)
""" Groups of ui commonents """
self.error_lcds = [
self.ui.rdoutBaseErrors, self.ui.rdoutShoulderErrors,
self.ui.rdoutElbowErrors, self.ui.rdoutWristErrors,
self.ui.rdoutWrist2Errors, self.ui.rdoutWrist3Errors
]
self.joint_readouts = [
self.ui.rdoutBaseJC, self.ui.rdoutShoulderJC, self.ui.rdoutElbowJC,
self.ui.rdoutWristJC, self.ui.rdoutWrist2JC, self.ui.rdoutWrist3JC
]
self.joint_slider_rdouts = [
self.ui.rdoutBase, self.ui.rdoutShoulder, self.ui.rdoutElbow,
self.ui.rdoutWrist, self.ui.rdoutWrist2, self.ui.rdoutWrist3
]
self.joint_sliders = [
self.ui.sldrBase, self.ui.sldrShoulder, self.ui.sldrElbow,
self.ui.sldrWrist, self.ui.sldrWrist2, self.ui.sldrWrist3
]
"""Objects Using Other Classes"""
self.kinect = Kinect()
self.rexarm = Rexarm()
self.tp = TrajectoryPlanner(self.rexarm)
self.sm = StateMachine(self.rexarm, self.tp, self.kinect)
self.sm.is_logging = False
"""
Attach Functions to Buttons & Sliders
TODO: NAME AND CONNECT BUTTONS AS NEEDED
"""
# Video
self.ui.videoDisplay.setMouseTracking(True)
self.ui.videoDisplay.mouseMoveEvent = self.trackMouse
self.ui.videoDisplay.mousePressEvent = self.calibrateMousePress
# Buttons
# Handy lambda function that can be used with Partial to only set the new state if the rexarm is initialized
nxt_if_arm_init = lambda next_state: self.sm.set_next_state(
next_state if self.rexarm.initialized else None)
self.ui.btn_estop.clicked.connect(self.estop)
self.ui.btn_init_arm.clicked.connect(self.initRexarm)
self.ui.btnUser1.setText("Calibrate")
self.ui.btnUser1.clicked.connect(partial(nxt_if_arm_init, 'calibrate'))
##OUR_CODE
self.ui.btn_exec.clicked.connect(self.execute)
self.ui.btnUser2.clicked.connect(self.record)
self.ui.btnUser3.clicked.connect(self.playback)
self.ui.btnUser4.clicked.connect(self.execute_tp)
self.ui.btnUser5.clicked.connect(self.toggle_logging)
self.ui.btnUser1.clicked.connect(self.calibrate)
self.ui.btnUser6.clicked.connect(self.blockDetect)
self.ui.btnUser7.clicked.connect(self.openGripper)
self.ui.btnUser8.clicked.connect(self.closeGripper)
self.ui.btnUser9.clicked.connect(self.clickGrab)
# Sliders
for sldr in self.joint_sliders:
sldr.valueChanged.connect(self.sliderChange)
self.ui.sldrMaxTorque.valueChanged.connect(self.sliderChange)
self.ui.sldrSpeed.valueChanged.connect(self.sliderChange)
# Direct Control
self.ui.chk_directcontrol.stateChanged.connect(self.directControlChk)
# Status
self.ui.rdoutStatus.setText("Waiting for input")
# Auto exposure
self.ui.chkAutoExposure.stateChanged.connect(self.autoExposureChk)
"""initalize manual control off"""
self.ui.SliderFrame.setEnabled(False)
"""Setup Threads"""
# Rexarm runs its own thread
# Video
self.videoThread = VideoThread(self.kinect)
self.videoThread.updateFrame.connect(self.setImage)
self.videoThread.start()
# State machine
self.logicThread = LogicThread(self.sm)
self.logicThread.start()
# Display
self.displayThread = DisplayThread(self.rexarm, self.sm)
self.displayThread.updateJointReadout.connect(self.updateJointReadout)
self.displayThread.updateEndEffectorReadout.connect(
self.updateEndEffectorReadout)
self.displayThread.updateStatusMessage.connect(
self.updateStatusMessage)
self.displayThread.updateJointErrors.connect(self.updateJointErrors)
self.displayThread.start()
def __init__(self, parent=None):
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)
#!/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)
def __init__(self):
self._kinect = Kinect()
self._body = BodyDetector()
self._hand = HandDetector(HandOtsu())
self._contour = HandContourDetector()
self._palm = PalmDetector()
def __init__(self):
super(MainWidget1, self).__init__()
self.phase = 0 # 0=tempo tracking, 1=pitch tracking, 2=performance
self.pitchTracker = PitchTracker()
self.audio = Audio(2, None, self.pitchTracker.audio_input_func)
self.tempo_map = SimpleTempoMap(100)
self.tempoProcessor = TempoProcessor(self.change_tempo, self.tempo_map)
self.playbackSystem = PlaybackSystem(self.audio, self.tempo_map, self.tempoProcessor, self.pitchTracker)
# self.gestureRecognizer = GestureRecognizer(self.quantize_time_to_beat, self.play_sound, self.tempoProcessor, self.tempo_map)
# user interface objects
self.metro_offset_y = -Window.height*0.135
metro_anchor_bg = Obj(CEllipse(cpos=(Window.width*0.5, (Window.height*0.25)+self.metro_offset_y), csize=(Window.width*0.035, Window.width*0.035)), (1.0, 0.0, 0.0))
self.canvas.add(metro_anchor_bg)
metro_anchor = Obj(CEllipse(cpos=(Window.width*0.5, (Window.height*0.25)+self.metro_offset_y), csize=(Window.width*0.025, Window.width*0.025)), (1.0, 1.0, 1.0))
self.canvas.add(metro_anchor)
self.metro_line = SolidLine((Window.width*0.5, (Window.height*0.25)+self.metro_offset_y), (Window.width*0.5, (Window.height*0.75)+self.metro_offset_y), (1.0, 1.0, 1.0), 6.0)
self.metro_anim_x = 0
self.canvas.add(self.metro_line)
# measure indicators
measure_indicator_size = (Window.width*0.09, Window.width*0.09)
self.measure_offset_y = -Window.height*0.145
self.measure_1_indicator = Obj(Rectangle(pos=((Window.width*2.0/7.0) - measure_indicator_size[0]/2, (Window.height*0.825) + self.measure_offset_y - measure_indicator_size[1]/2), size=measure_indicator_size), (1.0, 1.0, 1.0), "graphics/beat_1.png")
self.canvas.add(self.measure_1_indicator)
self.measure_2_indicator = Obj(Rectangle(pos=((Window.width*3.0/7.0) - measure_indicator_size[0]/2, (Window.height*0.825) + self.measure_offset_y - measure_indicator_size[1]/2), size=measure_indicator_size), (1.0, 1.0, 1.0), "graphics/beat_2.png")
self.canvas.add(self.measure_2_indicator)
self.measure_3_indicator = Obj(Rectangle(pos=((Window.width*4.0/7.0) - measure_indicator_size[0]/2, (Window.height*0.825) + self.measure_offset_y - measure_indicator_size[1]/2), size=measure_indicator_size), (1.0, 1.0, 1.0), "graphics/beat_3.png")
self.canvas.add(self.measure_3_indicator)
self.measure_4_indicator = Obj(Rectangle(pos=((Window.width*5.0/7.0) - measure_indicator_size[0]/2, (Window.height*0.825) + self.measure_offset_y - measure_indicator_size[1]/2), size=measure_indicator_size), (1.0, 1.0, 1.0), "graphics/beat_4.png")
self.canvas.add(self.measure_4_indicator)
self.measure_indicators = [self.measure_1_indicator, self.measure_2_indicator, self.measure_3_indicator, self.measure_4_indicator]
# phase indicator
indicator_size = (Window.width*0.5, Window.width*0.5*117.0/853.0)
self.phase_indicator = Obj(Rectangle(pos=((Window.width*0.5) - indicator_size[0]/2, (Window.height*0.865) - indicator_size[1]/2), size=indicator_size), (1.0, 1.0, 1.0), "graphics/signal_phase_1.png")
self.phase_ind_anim_x = 100
self.canvas.add(self.phase_indicator)
self.objects = [self.phase_indicator, metro_anchor, self.metro_line, self.measure_1_indicator, self.measure_2_indicator, self.measure_3_indicator, self.measure_4_indicator]
self.kinect = Kinect(1)
self.kinect.add_listener(self.on_kinect_update)
self.skeleton = SkeletonModel()
self.time_since_downbeat = 0
self.last_instruments = [None]
self.time_since_last_instrument = 0
self.kinect.start()
# initial logo
logo_size = (Window.width*0.6, Window.width*0.6*294/782)
self.logo_bg = Obj(Rectangle(pos=(0, 0), size=(Window.width, Window.height)), (0.0, 0.0, 0.0))
self.canvas.add(self.logo_bg)
self.logo = Obj(Rectangle(pos=((Window.width*0.5)-(logo_size[0]*0.5), (Window.height*0.53)-(logo_size[1]*0.5)), size=logo_size), (1.0, 1.0, 1.0), "graphics/logo_white.png")
self.canvas.add(self.logo)
self.objects.append(self.logo_bg)
self.objects.append(self.logo)
self.intro_timer = 0
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)
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()
