def __init__(self, _port='COM3', _flagSpike=False):
self.NROWS = 4 #number of rows
self.NCOLS = 4 #number of columns
self.NPATCH = 5 #number of sensors
self.port = _port #port name
self.dataQueue = deque() #data queue
self.thAcqLock = Lock() #lock for data acquisition
self.thProcLock = Lock() #lock for data processing
#serial handler for receiving data
self.serialHandler = SerialHandler(self.port,
7372800,
_header=0x24,
_end=0x21,
_numDataBytes=160,
_thLock=self.thAcqLock)
#thread for receiving packages
self.thAcq = ThreadHandler(self.serialHandler.readPackage)
#thread for processing the packages
self.thProc = ThreadHandler(self.processData)
#moving average for all taxels of each tactile sensor patch
self.mva = [[] for k in range(self.NPATCH * self.NROWS * self.NCOLS)]
for k in range(len(self.mva)):
self.mva[k] = copy(
MovingAverage(_windowSize=10)) #10 Hz cut-off frequency
self.taxelCounter = 0 #counter for moving average across all taxels
#matrix for storing previous values --> necessary for integrate and fire spikes
self.prevTactile = np.zeros((self.NPATCH, self.NROWS, self.NCOLS),
dtype=float)
#threshold for spike detection
self.threshold = 100
#flag that determines whether spikes or raw values should be used
self.flagSpike = _flagSpike
def __init__(self):
super(Main, self).__init__()
self.setupUi(self)
# IntnesityData : Array to store data to be displayed on Interface
self.intensityData = []
for i in range(4):
self.intensityData.append(np.full((4, 4, 3), 0))
# Two variables to avoid mutliple starting and stopping of thread
self.start = 0
self.stop = 0
# initialise Interface to blue
for i in range(4):
for j in range(4):
for k in range(4):
self.intensityData[i][j][k][2] = 255
# Thread which updates the plot based on received data from Micro Controller
self.thr = ThreadHandler(self.processData)
print("Intitialisation")
self.init()
def doStartStop(self):
if not self.avr.acqThread.isAlive:
try:
self.avr.start()
#Thread that handles the data acquisition
self.dataProc = ThreadHandler(self.runAquisition)
#Start the threads
self.avr.acqThread.start()
self.dataProc.start()
self.btnStartStop.setText("Stop")
self.btnConnectDisconnect.setEnabled(False)
except Exception as e:
self.show_error_msg("Erro ao iniciar aquisicao.\nError Log: " +
str(e))
else:
try:
self.doStop()
self.btnStartStop.setText("Start")
self.btnConnectDisconnect.setEnabled(True)
except Exception as e:
self.show_error_msg(
"Erro ao finalizar aquisicao.\nError Log: " + str(e))
class Main(QMainWindow, Ui_MainWindow):
def __init__(self):
super(Main, self).__init__()
self.setupUi(self)
# IntnesityData : Array to store data to be displayed on Interface
self.intensityData = []
for i in range(4):
self.intensityData.append(np.full((4, 4, 3), 0))
# Two variables to avoid mutliple starting and stopping of thread
self.start = 0
self.stop = 0
# initialise Interface to blue
for i in range(4):
for j in range(4):
for k in range(4):
self.intensityData[i][j][k][2] = 255
# Thread which updates the plot based on received data from Micro Controller
self.thr = ThreadHandler(self.processData)
print("Intitialisation")
self.init()
# init() Contains other initialisations
def init(self):
print("Adding ViewBoxes")
# displays are the viewboxes (one for each patch of tactile sensors)
self.display1 = self.patch1.addViewBox()
self.display2 = self.patch2.addViewBox()
self.display3 = self.patch3.addViewBox()
self.display4 = self.patch4.addViewBox()
# Image items to be displayed on the viewboxes
self.currImage1 = pg.ImageItem(self.intensityData[0])
self.display1.addItem(self.currImage1)
self.currImage2 = pg.ImageItem(self.intensityData[1])
self.display2.addItem(self.currImage2)
self.currImage3 = pg.ImageItem(self.intensityData[2])
self.display3.addItem(self.currImage3)
self.currImage4 = pg.ImageItem(self.intensityData[3])
self.display4.addItem(self.currImage4)
# Functions of Start and Stop buttons
self.startButton.clicked.connect(self.doStart)
self.stopButton.clicked.connect(self.doStop)
def doStart(self):
# starting the thread to update the Interface
global recvThread, ser
if self.start == 0:
ser.flushInput()
self.start = 1
self.thr.start()
recvThread.start()
def doStop(self):
# stop the thread which updates the Interface
global recvThread
if self.stop == 0:
print("Stopped")
self.stop = 1
self.thr.pause()
recvThread.pause()
self.thr.kill()
recvThread.kill()
# The function to update the Interface in real time. This function is ran in a thread.
def processData(self):
global update, dataQueue
while True:
#print(update)
if update == 1:
#print("First update")
for pos in range(64):
patchNum = pos // 16
col = (pos % 16) // 4
row = (pos % 16) % 4
if patchNum == 0:
self.intensityData[0][row][col][0] = max(
0,
2 * int(dataQueue[patchNum][row][col] / 256) - 255)
self.intensityData[0][row][col][2] = max(
0,
255 - 2 * int(dataQueue[patchNum][row][col] / 256))
self.intensityData[0][row][col][
1] = 255 - self.intensityData[0][row][col][
2] - self.intensityData[0][row][col][0]
self.currImage1.setImage(self.intensityData[0],
levels=(0, 255))
elif patchNum == 1:
self.intensityData[1][row][col][0] = max(
0,
2 * int(dataQueue[patchNum][row][col] / 256) - 255)
self.intensityData[1][row][col][2] = max(
0,
255 - 2 * int(dataQueue[patchNum][row][col] / 256))
self.intensityData[1][row][col][
1] = 255 - self.intensityData[1][row][col][
2] - self.intensityData[1][row][col][0]
self.currImage2.setImage(self.intensityData[1],
levels=(0, 255))
elif patchNum == 2:
self.intensityData[2][row][col][0] = max(
0,
2 * int(dataQueue[patchNum][row][col] / 256) - 255)
self.intensityData[2][row][col][2] = max(
0,
255 - 2 * int(dataQueue[patchNum][row][col] / 256))
self.intensityData[2][row][col][
1] = 255 - self.intensityData[2][row][col][
2] - self.intensityData[2][row][col][0]
self.currImage3.setImage(self.intensityData[2],
levels=(0, 255))
elif patchNum == 3:
self.intensityData[3][row][col][0] = max(
0,
2 * int(dataQueue[patchNum][row][col] / 256) - 255)
self.intensityData[3][row][col][2] = max(
0,
255 - 2 * int(dataQueue[patchNum][row][col] / 256))
self.intensityData[3][row][col][
1] = 255 - self.intensityData[3][row][col][
2] - self.intensityData[3][row][col][0]
self.currImage4.setImage(self.intensityData[3],
levels=(0, 255))
update = 0
2] - self.intensityData[2][row][col][0]
self.currImage3.setImage(self.intensityData[2],
levels=(0, 255))
elif patchNum == 3:
self.intensityData[3][row][col][0] = max(
0,
2 * int(dataQueue[patchNum][row][col] / 256) - 255)
self.intensityData[3][row][col][2] = max(
0,
255 - 2 * int(dataQueue[patchNum][row][col] / 256))
self.intensityData[3][row][col][
1] = 255 - self.intensityData[3][row][col][
2] - self.intensityData[3][row][col][0]
self.currImage4.setImage(self.intensityData[3],
levels=(0, 255))
update = 0
#print(self.intensityData[0])
# Thread to receive data
#recvThread = threading.Thread(target = receive)
recvThread = ThreadHandler(receive)
# Parallely update the display based on received data. The class for interface( Main )
# itself runs another thread.
if __name__ == '__main__':
app = QtWidgets.QApplication(sys.argv)
main = Main()
main.show()
sys.exit(app.exec_())
def __init__(self,
_port='COM3',
_flagSpike=False,
_sensitivity=TBCONSTS.HIGH_SENS):
self.NROWS = 1 #number of rows
self.NCOLS = 1 #number of columns
self.NPATCH = 2 #number of sensors
self.port = _port #port name
self.dataQueue = deque() #data queue
self.thAcqLock = Lock() #lock for data acquisition
self.thProcLock = Lock() #lock for data processing
#serial handler for receiving data
self.serialHandler = SerialHandler(self.port,
7372800,
_header=0x24,
_end=0x21,
_numDataBytes=4,
_thLock=self.thAcqLock)
#thread for receiving packages
self.thAcq = ThreadHandler(self.serialHandler.readPackage)
#thread for processing the packages
self.thProc = ThreadHandler(self.processData)
#moving average for all taxels of each tactile sensor patch
self.mva = [[] for k in range(TBCONSTS.NPATCH * TBCONSTS.NROWS *
TBCONSTS.NCOLS)]
for k in range(len(self.mva)):
self.mva[k] = copy(
MovingAverage(_windowSize=5)) #10 Hz cut-off frequency
self.taxelCounter = 0 #counter for moving average across all taxels
#matrix for storing previous values --> necessary for integrate and fire spikes
self.prevTactile = np.zeros(
(TBCONSTS.NPATCH, TBCONSTS.NROWS, TBCONSTS.NCOLS), dtype=float)
#threshold for spike detection
self.threshold = 100
#flag that determines whether spikes or raw values should be used
self.flagSpike = _flagSpike
#flag that determines whether the slip sensor should be used
self.useSlipSensor = False
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
#NORMALIZATION --> adjusts sensitivity of the tactile sensors
#-----------------------------------------------------------------------
self.sensitivity = _sensitivity #determines which sensitivity should be used
self.vmax = 2.46 #maximum possible voltage
self.vhigh = 0.85 #voltage range for high sensitivity
self.vmed = 0.6 #voltage range for medium sensitivity
self.vlow = 0.3 #voltage range for low sensitivity
self.vdef = 0 #voltage range for default sensitivity (no normalization)
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
#CALIBRATION
#-----------------------------------------------------------------------
#temporary matrix to store the calibration values
self.calibMatrix = None #initializes the calibration matrix
#initializes the temporary matrix containing the calibration values
self.tempCalib = [
np.zeros((TBCONSTS.NROWS, TBCONSTS.NCOLS))
for k in range(TBCONSTS.NPATCH)
]
self.flagCalib = False #determines whether calibration is ongoing
self.calibCount = 0 #counter necessary for keeping track of the number of samples
self.useCalib = False #determines whether calibration should be used
self.calibStatus = TBCONSTS.CALIBRATION_IDLE #initializes calibration as idle
class TactileBoard():
def __init__(self,
_port='COM3',
_flagSpike=False,
_sensitivity=TBCONSTS.HIGH_SENS):
self.NROWS = 1 #number of rows
self.NCOLS = 1 #number of columns
self.NPATCH = 2 #number of sensors
self.port = _port #port name
self.dataQueue = deque() #data queue
self.thAcqLock = Lock() #lock for data acquisition
self.thProcLock = Lock() #lock for data processing
#serial handler for receiving data
self.serialHandler = SerialHandler(self.port,
7372800,
_header=0x24,
_end=0x21,
_numDataBytes=4,
_thLock=self.thAcqLock)
#thread for receiving packages
self.thAcq = ThreadHandler(self.serialHandler.readPackage)
#thread for processing the packages
self.thProc = ThreadHandler(self.processData)
#moving average for all taxels of each tactile sensor patch
self.mva = [[] for k in range(TBCONSTS.NPATCH * TBCONSTS.NROWS *
TBCONSTS.NCOLS)]
for k in range(len(self.mva)):
self.mva[k] = copy(
MovingAverage(_windowSize=5)) #10 Hz cut-off frequency
self.taxelCounter = 0 #counter for moving average across all taxels
#matrix for storing previous values --> necessary for integrate and fire spikes
self.prevTactile = np.zeros(
(TBCONSTS.NPATCH, TBCONSTS.NROWS, TBCONSTS.NCOLS), dtype=float)
#threshold for spike detection
self.threshold = 100
#flag that determines whether spikes or raw values should be used
self.flagSpike = _flagSpike
#flag that determines whether the slip sensor should be used
self.useSlipSensor = False
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
#NORMALIZATION --> adjusts sensitivity of the tactile sensors
#-----------------------------------------------------------------------
self.sensitivity = _sensitivity #determines which sensitivity should be used
self.vmax = 2.46 #maximum possible voltage
self.vhigh = 0.85 #voltage range for high sensitivity
self.vmed = 0.6 #voltage range for medium sensitivity
self.vlow = 0.3 #voltage range for low sensitivity
self.vdef = 0 #voltage range for default sensitivity (no normalization)
#-----------------------------------------------------------------------
#-----------------------------------------------------------------------
#CALIBRATION
#-----------------------------------------------------------------------
#temporary matrix to store the calibration values
self.calibMatrix = None #initializes the calibration matrix
#initializes the temporary matrix containing the calibration values
self.tempCalib = [
np.zeros((TBCONSTS.NROWS, TBCONSTS.NCOLS))
for k in range(TBCONSTS.NPATCH)
]
self.flagCalib = False #determines whether calibration is ongoing
self.calibCount = 0 #counter necessary for keeping track of the number of samples
self.useCalib = False #determines whether calibration should be used
self.calibStatus = TBCONSTS.CALIBRATION_IDLE #initializes calibration as idle
#method to load text file containing calibration parameters for a specific
#tactile board
#file format has 5 lines, with 16 values each
#each line corresponds to a patch --> from patch 1 to 5 following the schematics
#each line (patch) contains 16 values (taxels) arranged with respect to
#columns
def loadCalibration(self):
strport = ''.join([x for x in self.port if x is not '/'])
filepath = 'tactileboard_' + strport + '_calib.cfg'
#check if file exists
if os.path.isfile(filepath):
#load the file
self.calibValues = np.loadtxt(filepath)
if len(self.calibValues.shape) == 1:
# 1D array
self.calibValues = self.calibValues.reshape([-1, 1])
#calibration matrix
#list containing 4x4 matrices
self.calibMatrix = [
np.zeros((TBCONSTS.NROWS, TBCONSTS.NCOLS))
for k in range(TBCONSTS.NPATCH)
]
for n in range(TBCONSTS.NPATCH):
auxcounter = 0
for i in range(TBCONSTS.NROWS):
for j in range(TBCONSTS.NCOLS):
#create the calibration matrix
self.calibMatrix[n][i][j] = self.calibValues[n][
auxcounter]
auxcounter += 1 #increment counter
return True
else:
return False #file not found
#saves the calibration result to a text file
def saveCalibration(self):
try:
strport = ''.join([x for x in self.port if x is not '/'])
filepath = 'tactileboard_' + strport + '_calib.cfg'
filehandler = open(filepath, 'w')
for n in range(TBCONSTS.NPATCH):
strline = ''
for i in range(TBCONSTS.NROWS):
for j in range(TBCONSTS.NCOLS):
strline = strline + str(self.tempCalib[n][i][j]) + ' '
strline += '\n'
filehandler.write(strline)
filehandler.close()
return True
except:
return False
#start data acquisition
def start(self):
self.serialHandler.open() #open the serial port
self.thAcq.start() #start data acquisition
self.thProc.start() #start data processing
#stop acquisition
def stop(self):
self.thAcq.kill() #kill thread
self.thProc.kill() #kill thread
#if nsamples == 0, then calibration will only stop when the method
#doStopCalib is explicitly called. otherwise, the calibration process
#will stop automatically once 'nsamples' have been acquired for each
#taxel of all patches
def startCalibration(self, nsamples=0):
self.flagCalib = True
self.calibCount = 0
self.calibMaxSamples = nsamples
self.calibStatus = TBCONSTS.CALIBRATION_ONGOING
#stops calibration
def stopCalibration(self):
self.flagCalib = False
self.calibCount = 0
self.calibStatus = TBCONSTS.CALIBRATION_FINISHED
th = Thread(target=self.saveCalibration)
#th.daemon = True
th.start()
#method that takes the raw data coming from the serial port and rearranges
#them into proper 4x4 matrices
def processData(self):
#x = datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f')[:-3]
self.thAcqLock.acquire()
n = len(self.serialHandler.dataQueue)
q = copy(self.serialHandler.dataQueue)
self.serialHandler.dataQueue.clear()
self.thAcqLock.release()
#y = datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f')[:-3]
#if n > 0:
# print(x,y,n)
#3d array containing the 4x4 matrix of each tactile patch
self.tactileSensors = np.zeros(
(TBCONSTS.NPATCH, TBCONSTS.NROWS, TBCONSTS.NCOLS), dtype=float)
for k in range(n):
data = q.popleft()
# data = data[2:] + data[0:2]
for z in range(0, 4, 2):
patchNum = int((z / 2) % TBCONSTS.NPATCH)
row = int((z / 2) % (TBCONSTS.NROWS * TBCONSTS.NPATCH) //
TBCONSTS.NPATCH)
row = TBCONSTS.ROW_IDX[row]
col = int((z / 2) // (TBCONSTS.NCOLS * TBCONSTS.NPATCH))
#print(z,z/2,patchNum,row,col) #debugging
#re-arrange the adc sample
sample = data[z] << 8 | data[z + 1]
#if output format is spikes
if self.flagSpike:
if (sample - self.prevTactile[patchNum][col][row] >
self.threshold):
self.tactileSensors[patchNum][col][row] = 1
#print(sample,self.prevTactile[patchNum][col][row])
elif (self.prevTactile[patchNum][col][row] - sample >
self.threshold):
self.tactileSensors[patchNum][col][row] = 0
else:
self.tactileSensors[patchNum][col][row] = 0.5
self.prevTactile[patchNum][col][row] = sample
#if output format is normalized amplitude values
else:
filtsample = self.mva[self.taxelCounter].getSample(sample)
if self.flagCalib:
auxmean = self.tempCalib[patchNum][col][row]
auxsample = self.tactileSensors[patchNum][col][row]
auxmean = auxmean * (self.calibCount /
(self.calibCount + 1)) + (
filtsample /
(self.calibCount + 1))
self.tempCalib[patchNum][col][row] = auxmean
self.tactileSensors[patchNum][col][row] = self.normalize(
filtsample, patchNum, col, row)
self.taxelCounter += 1
if self.taxelCounter >= (self.NPATCH * self.NROWS *
self.NCOLS):
self.taxelCounter = 0
#calibration procedure
if self.flagCalib:
#increment the counter of samples for
self.calibCount += 1
#check if the number of specified samples have been acquired or not
if self.calibMaxSamples > 0 and self.calibCount >= self.calibMaxSamples:
self.stopCalibration()
#print('finished') #debugging
#print(self.tempCalib[patchNum]) #debugging
#slip sensor data
# slipSensor = (data[160]<<8 | data[161]) * (3.3/4096)
#print('slip sensor', slipSensor) #debugging
self.thProcLock.acquire()
for w in range(self.NPATCH):
#self.tactileSensors[w] = self.tactileSensors[w]
self.tactileSensors[w] = np.flip(
np.flip(self.tactileSensors[w], 0), 1)
#if the slip sensor is being used, the data queue will be different
if self.useSlipSensor is True:
#create a list where the first position refers to the tactile data
#and the second position to the slip sensor data
listdata = []
listdata.append(copy(self.tactileSensors))
listdata.append(copy(slipSensor))
self.dataQueue.append(copy(listdata))
else: #default -- no slip sensor
self.dataQueue.append(copy(self.tactileSensors))
self.thProcLock.release()
# print(self.dataQueue)
time.sleep(0.001) #necessary to prevent really fast thread access
#returns the data queue
def getData(self):
self.thProcLock.acquire()
q = copy(self.dataQueue)
self.dataQueue.clear()
self.thProcLock.release()
return q
#normalize the tactile sensor signal according to the sensitivity specified
def normalize(self, adcsample, patchNum=0, col=0, row=0):
if self.sensitivity == TBCONSTS.HIGH_SENS: #high sens
vsens = self.vhigh
elif self.sensitivity == TBCONSTS.MEDIUM_SENS: #medium sens
vsens = self.vmed
elif self.sensitivity == TBCONSTS.LOW_SENS: #low sens
vsens = self.vlow
else:
vsens = self.vdef #no normalization
#if the calibration matrix is instantiated, use it as parameter for
#normalization in a pixel by pixel fashion
if self.calibMatrix is not None and self.useCalib is True:
vmax = self.calibMatrix[patchNum][col][row] * (3.3 / 4096)
#print(vmax) #debugging
#otherwise, let default normalization take place where voltage values are
#fixed without considering taxel behavior
else:
vmax = self.vmax
#adjusts sensitivity based on the proper maximum value that is either
#given by the calibration matrix or the fixed default value
vsens *= vmax
#convert the adc value back to volts
vsample = adcsample * (3.3 / 4096.)
#convert from volts to a normalized value given the sensitivity
vnorm = ((vsample - vsens) * -1) / (vmax - vsens) + 1
#check if the normalization values are within range (0 to 1)
if vnorm > 1:
vnorm = 1
elif vnorm < 0:
vnorm = 0
#print(vnorm,vsample,vmax,vsens) #debugging
#return the normalized value
return vnorm
def conv2raw(self, vnorm, patchNum=0, col=0, row=0):
if self.sensitivity == TBCONSTS.HIGH_SENS: #high sens
vsens = self.vhigh
elif self.sensitivity == TBCONSTS.MEDIUM_SENS: #medium sens
vsens = self.vmed
elif self.sensitivity == TBCONSTS.LOW_SENS: #low sens
vsens = self.vlow
else:
vsens = self.vdef #no normalization
#if the calibration matrix is instantiated, use it as parameter for
#normalization in a pixel by pixel fashion
if self.calibMatrix is not None and self.useCalib is True:
vmax = self.calibMatrix[patchNum][col][row] * (3.3 / 4096)
#print(vmax) #debugging
#otherwise, let default normalization take place where voltage values are
#fixed without considering taxel behavior
else:
vmax = self.vmax
#adjusts sensitivity based on the proper maximum value that is either
#given by the calibration matrix or the fixed default value
vsens *= vmax
vsample = -1 * (((vmax - vsens) * (vnorm - 1)) - vsens)
return vsample
def conv2sens(self, vnorm, desiredSens, patchNum=0, col=0, row=0):
if desiredSens == TBCONSTS.HIGH_SENS: #high sens
vsens = self.vhigh
elif desiredSens == TBCONSTS.MEDIUM_SENS: #medium sens
vsens = self.vmed
elif desiredSens == TBCONSTS.LOW_SENS: #low sens
vsens = self.vlow
else:
vsens = self.vdef #no normalization
#if the calibration matrix is instantiated, use it as parameter for
#normalization in a pixel by pixel fashion
if self.calibMatrix is not None and self.useCalib is True:
vmax = self.calibMatrix[patchNum][col][row] * (3.3 / 4096)
#print(vmax) #debugging
#otherwise, let default normalization take place where voltage values are
#fixed without considering taxel behavior
else:
vmax = self.vmax
#adjusts sensitivity based on the proper maximum value that is either
#given by the calibration matrix or the fixed default value
vsens *= vmax
vraw = self.conv2raw(vnorm, patchNum, col, row)
newvnorm = ((vraw - vsens) * -1) / (vmax - vsens) + 1
return newvnorm
vmax = self.vmax
#adjusts sensitivity based on the proper maximum value that is either
#given by the calibration matrix or the fixed default value
vsens *= vmax
vraw = self.conv2raw(vnorm, patchNum, col, row)
newvnorm = ((vraw - vsens) * -1) / (vmax - vsens) + 1
return newvnorm
#-------------------------------------------------------------------------------
#-------------------------------------------------------------------------------
if __name__ == '__main__':
def update():
global tactileBoard
q = tactileBoard.getData()
n = len(q)
for k in range(n):
data = q.popleft()
print(data[0])
thMain = ThreadHandler(update)
tactileBoard = TactileBoard('COM3')
tactileBoard.start()
thMain.start()
a = input()
class Main(QMainWindow, Ui_MainWindow):
def __init__(self):
super(Main, self).__init__()
self.setupUi(self)
self.avr = None
self.threshold1 = 1.0
self.threshold2 = 2.0
self.threshold3 = 3.0
self.threshold1reached = False
self.threshold2reached = False
self.threshold3reached = False
self.oldthreshold1reached = self.threshold1reached
self.oldthreshold2reached = self.threshold2reached
self.oldthreshold3reached = self.threshold3reached
self.stateChanged = False
self.populateSerialPorts()
self.btnConnectDisconnect.clicked.connect(self.doConnect)
self.btnStartStop.clicked.connect(self.doStartStop)
self.sliderLimiar1.valueChanged.connect(self.threshold1Changed)
self.sliderLimiar2.valueChanged.connect(self.threshold2Changed)
self.sliderLimiar3.valueChanged.connect(self.threshold3Changed)
self.scene = QGraphicsScene(self)
self.graphicsLeds.setScene(self.scene)
self.normalColor = QBrush(QColor.fromRgb(162, 178, 245))
self.hilightColor = QBrush(QColor.fromRgb(57, 255, 77))
self.outlinePen = QPen(Qt.black)
self.outlinePen.setWidth(1)
h = self.scene.height()
w = self.scene.width()
self.circleLed1 = self.scene.addEllipse(w / 2, (h / 2) - 150, 100, 100,
self.outlinePen,
self.normalColor)
self.circleLed2 = self.scene.addEllipse(w / 2, h / 2, 100, 100,
self.outlinePen,
self.normalColor)
self.circleLed3 = self.scene.addEllipse(w / 2, (h / 2) + 150, 100, 100,
self.outlinePen,
self.normalColor)
self.textLed1 = self.scene.addText("Led 1", QFont("Arial", 12))
self.textLed2 = self.scene.addText("Led 2", QFont("Arial", 12))
self.textLed3 = self.scene.addText("Led 3", QFont("Arial", 12))
self.textLed1.setPos((w / 2) + 25, 35 + (h / 2) - 150)
self.textLed2.setPos((w / 2) + 25, 35 + (h / 2))
self.textLed3.setPos((w / 2) + 25, 35 + (h / 2) + 150)
def show_error_msg(self, msg_to_show):
msg = QMessageBox()
msg.setIcon(QMessageBox.Warning)
msg.setText(msg_to_show)
msg.setWindowTitle("Erro")
retval = msg.exec_()
def show_info_msg(self, msg_to_show):
msg = QMessageBox()
msg.setIcon(QMessageBox.Information)
msg.setText(msg_to_show)
msg.setWindowTitle("Mensagem Info")
retval = msg.exec_()
def populateSerialPorts(self):
if len(serial_tools.comports()) == 0:
self.show_error_msg("Nenhuma porta Serial Disponivel")
self.cbSerialPorts.setEnabled(False)
self.btnStartStop.setEnabled(False)
else:
for serial_port in serial_tools.comports():
self.cbSerialPorts.addItem(serial_port.device)
self.cbSerialPorts.setCurrentIndex(self.cbSerialPorts.count() -
1)
if ("Arduino" in serial_port.description):
self.doConnect()
def doConnect(self):
if self.avr == None:
self.avr = Arduino(
self.cbSerialPorts.itemText(self.cbSerialPorts.currentIndex()))
if self.avr != None:
if self.avr.serialPort == None:
try:
if self.avr.open():
#self.show_info_msg("Porta serial %s aberta com sucesso!" % (self.avr.port))
self.btnConnectDisconnect.setText("Desconectar")
self.cbSerialPorts.setEnabled(False)
self.avr.stop()
except Exception as e:
self.show_error_msg("Erro ao abrir a porta serial")
else:
try:
if self.avr.close():
self.show_info_msg(
"Porta serial %s fechada com sucesso!" %
(self.avr.port))
self.btnConnectDisconnect.setText("Conectar")
self.cbSerialPorts.setEnabled(True)
self.avr.serialPort = None
except Exception as e:
self.show_error_msg("Erro ao fechar a porta serial")
def doStartStop(self):
if not self.avr.acqThread.isAlive:
try:
self.avr.start()
#Thread that handles the data acquisition
self.dataProc = ThreadHandler(self.runAquisition)
#Start the threads
self.avr.acqThread.start()
self.dataProc.start()
self.btnStartStop.setText("Stop")
self.btnConnectDisconnect.setEnabled(False)
except Exception as e:
self.show_error_msg("Erro ao iniciar aquisicao.\nError Log: " +
str(e))
else:
try:
self.doStop()
self.btnStartStop.setText("Start")
self.btnConnectDisconnect.setEnabled(True)
except Exception as e:
self.show_error_msg(
"Erro ao finalizar aquisicao.\nError Log: " + str(e))
def doStop(self):
self.avr.stop()
time.sleep(1)
#Kill the threads
self.avr.acqThread.kill()
self.dataProc.kill()
def runAquisition(self):
if self.avr.dataQueue.qsize() > 0:
self.disassemblePacket()
def disassemblePacket(self):
n = self.avr.dataQueue.qsize()
for i in range(n):
data = self.avr.dataQueue.get()
#print data
#print "data:\t%.2f" % (data)
self.showReadValue(data[0])
self.threshold1reached = data[0] > self.threshold1
self.threshold2reached = data[0] > self.threshold2
self.threshold3reached = data[0] > self.threshold3
if not self.oldthreshold1reached and self.threshold1reached:
print 'Subiu Limiar 1'
self.stateChanged = True
elif self.oldthreshold1reached and not self.threshold1reached:
print 'Desceu Limiar 1'
self.stateChanged = True
if not self.oldthreshold2reached and self.threshold2reached:
print 'Subiu Limiar 2'
self.stateChanged = True
elif self.oldthreshold2reached and not self.threshold2reached:
print 'Desceu Limiar 2'
self.stateChanged = True
if not self.oldthreshold3reached and self.threshold3reached:
print 'Subiu Limiar 3'
self.stateChanged = True
elif self.oldthreshold3reached and not self.threshold3reached:
print 'Desceu Limiar 3'
self.stateChanged = True
self.runStateMachine()
self.oldthreshold1reached = self.threshold1reached
self.oldthreshold2reached = self.threshold2reached
self.oldthreshold3reached = self.threshold3reached
def runStateMachine(self):
if self.stateChanged:
self.stateChanged = False
#State 1 0 0 -> primeiro led
if not self.threshold1reached:
print "Apagado"
self.clearGraphView()
elif not self.threshold2reached:
print "Led 1"
self.updadeGraphView(self.circleLed1)
elif not self.threshold3reached:
print "Led 2"
self.updadeGraphView(self.circleLed2)
else:
print "Led 3"
self.updadeGraphView(self.circleLed3)
def threshold1Changed(self):
self.labelLimiar1.setText('Limiar 1: %.2f V' %
(self.sliderLimiar1.value() / 100.00))
self.threshold1 = (self.sliderLimiar1.value() / 100.00)
def threshold2Changed(self):
self.labelLimiar2.setText('Limiar 2: %.2f V' %
(self.sliderLimiar2.value() / 100.00))
self.threshold2 = (self.sliderLimiar2.value() / 100.00)
def threshold3Changed(self):
self.labelLimiar3.setText('Limiar 3: %.2f V' %
(self.sliderLimiar3.value() / 100.00))
self.threshold3 = (self.sliderLimiar3.value() / 100.00)
def showReadValue(self, valorLido):
self.sliderValorLido.setValue(int(np.round(valorLido * 100)))
self.lbTitleValorLido.setText('Valor Lido: %.2f V' % (valorLido))
self.dialValorLido.setValue(int(np.round(valorLido * 100)))
def clearGraphView(self):
self.circleLed1.setBrush(self.normalColor)
self.circleLed2.setBrush(self.normalColor)
self.circleLed3.setBrush(self.normalColor)
def updadeGraphView(self, shapetoupdate):
self.clearGraphView()
shapetoupdate.setBrush(self.hilightColor)
class Main(QMainWindow, Ui_MainWindow):
def __init__(self):
super(Main, self).__init__()
self.setupUi(self)
self.avr = None
self.thresholdCh0 = 1.0
self.thresholdCh1 = 1.0
self.ch0Contraido = False
self.ch1Contraido = False
self.stateChanged = False
self.ch0State = False
self.ch1State = False
fluidsynth.init('FluidR3_GM.sf2', 'alsa')
self.soundCommand1 = Note('C', 4)
self.soundCommand2 = Note('E', 4)
self.soundCommand3 = Note('G', 4)
self.ihmMIDIinstrument = MidiInstrument()
self.populateSerialPorts()
self.populateInstruments()
self.populateNotesAndScales()
self.btnConnectDisconnect.clicked.connect(self.doConnect)
self.btnStartStop.clicked.connect(self.doStartStop)
self.sliderCh0.valueChanged.connect(self.emgValueChanged)
self.sliderCh1.valueChanged.connect(self.emgValueChanged)
self.sliderThCh0.valueChanged.connect(self.thresholdChanged)
self.sliderThCh1.valueChanged.connect(self.thresholdChanged)
self.connect(self.cbInstruments, SIGNAL('currentIndexChanged(int)'),
self.cbInstrumentsChanged)
self.connect(self.cbNota1, SIGNAL('currentIndexChanged(int)'),
self.cbNotasChanged)
self.connect(self.cbNota2, SIGNAL('currentIndexChanged(int)'),
self.cbNotasChanged)
self.connect(self.cbNota3, SIGNAL('currentIndexChanged(int)'),
self.cbNotasChanged)
self.connect(self.cbEscala1, SIGNAL('currentIndexChanged(int)'),
self.cbNotasChanged)
self.connect(self.cbEscala2, SIGNAL('currentIndexChanged(int)'),
self.cbNotasChanged)
self.connect(self.cbEscala3, SIGNAL('currentIndexChanged(int)'),
self.cbNotasChanged)
self.scene = QGraphicsScene(self)
self.graphResult.setScene(self.scene)
self.normalColor = QBrush(QColor.fromRgb(162, 178, 245))
self.hilightColor = QBrush(QColor.fromRgb(57, 255, 77))
self.outlinePen = QPen(Qt.black)
self.outlinePen.setWidth(1)
self.circleSilence = self.scene.addEllipse(-100, -100, 100, 100,
self.outlinePen,
self.hilightColor)
self.circleNota2 = self.scene.addEllipse(-100, 100, 100, 100,
self.outlinePen,
self.normalColor)
self.circleNota1 = self.scene.addEllipse(100, -100, 100, 100,
self.outlinePen,
self.normalColor)
self.circleNota3 = self.scene.addEllipse(100, 100, 100, 100,
self.outlinePen,
self.normalColor)
self.textSilence = self.scene.addText("Silencio", QFont("Arial", 12))
self.textNota1 = self.scene.addText("Nota 1", QFont("Arial", 12))
self.textNota2 = self.scene.addText("Nota 2", QFont("Arial", 12))
self.textNota3 = self.scene.addText("Nota 3", QFont("Arial", 12))
self.textSilence.setPos(-80, -62.5)
self.textNota2.setPos(-75, 138.5)
self.textNota1.setPos(125, -62.5)
self.textNota3.setPos(125, 138.5)
def show_error_msg(self, msg_to_show):
msg = QMessageBox()
msg.setIcon(QMessageBox.Warning)
msg.setText(msg_to_show)
msg.setWindowTitle("Erro")
retval = msg.exec_()
def show_info_msg(self, msg_to_show):
msg = QMessageBox()
msg.setIcon(QMessageBox.Information)
msg.setText(msg_to_show)
msg.setWindowTitle("Mensagem Info")
retval = msg.exec_()
def populateSerialPorts(self):
for serial_port in serial_tools.comports():
self.cbSerialPorts.addItem(serial_port.device)
if len(serial_tools.comports()) == 0:
self.show_error_msg("Nenhuma porta Serial Disponivel")
self.cbSerialPorts.setEnabled(False)
self.btnStartStop.setEnabled(False)
else:
self.cbSerialPorts.setCurrentIndex(
len(serial_tools.comports()) - 1)
def doConnect(self):
if self.avr == None:
self.avr = Arduino(
self.cbSerialPorts.itemText(self.cbSerialPorts.currentIndex()))
if self.avr != None:
if self.avr.serialPort == None:
try:
if self.avr.open():
self.show_info_msg(
"Porta serial %s aberta com sucesso!" %
(self.avr.port))
self.btnConnectDisconnect.setText("Desconectar")
self.cbSerialPorts.setEnabled(False)
except Exception as e:
self.show_error_msg("Erro ao abrir a porta serial")
else:
try:
if self.avr.close():
self.show_info_msg(
"Porta serial %s fechada com sucesso!" %
(self.avr.port))
self.btnConnectDisconnect.setText("Conectar")
self.cbSerialPorts.setEnabled(True)
self.avr.serialPort = None
except Exception as e:
self.show_error_msg("Erro ao fechar a porta serial")
def doStartStop(self):
if not self.avr.acqThread.isAlive:
try:
self.avr.start()
#Thread that handles the data acquisition
self.dataProc = ThreadHandler(self.runAquisition)
#Start the threads
self.avr.acqThread.start()
self.dataProc.start()
self.btnStartStop.setText("Stop")
self.btnConnectDisconnect.setEnabled(False)
except Exception as e:
self.show_error_msg("Erro ao iniciar aquisicao.\nError Log: " +
str(e))
else:
try:
self.doStop()
self.btnStartStop.setText("Start")
self.btnConnectDisconnect.setEnabled(True)
except Exception as e:
self.show_error_msg(
"Erro ao finalizar aquisicao.\nError Log: " + str(e))
def doStop(self):
self.avr.stop()
time.sleep(1)
#Kill the threads
self.avr.acqThread.kill()
self.dataProc.kill()
def runAquisition(self):
if self.avr.dataQueue.qsize() > 0:
self.disassemblePacket()
def disassemblePacket(self):
n = self.avr.dataQueue.qsize()
for i in range(n):
data = self.avr.dataQueue.get()
#print "data:\t%.2f\t%.2f" % (data[0], data[1])
self.showEmgValues(data[0], data[1])
if not self.ch0Contraido and data[0] > self.thresholdCh0:
print 'Contraindo EMG0'
self.ch0State = True
self.stateChanged = True
elif self.ch0Contraido and data[0] < self.thresholdCh0:
print 'Relaxando EMG0'
self.ch0State = False
self.stateChanged = True
if not self.ch1Contraido and data[1] > self.thresholdCh1:
print 'Contraindo EMG1'
self.ch1State = True
self.stateChanged = True
elif self.ch1Contraido and data[1] < self.thresholdCh1:
print 'Relaxando EMG1'
self.ch1State = False
self.stateChanged = True
self.runStateMachine()
self.ch0Contraido = data[0] > self.thresholdCh0
self.ch1Contraido = data[1] > self.thresholdCh1
def runStateMachine(self):
if self.stateChanged:
self.stateChanged = False
self.cbCh0.setCheckState(
Qt.Checked if self.ch0State else Qt.Unchecked)
self.cbCh1.setCheckState(
Qt.Checked if self.ch1State else Qt.Unchecked)
#self.cbCh0.setCheckState(self.ch0State)
#State 1 1
if self.ch1State and self.ch0State:
fluidsynth.stop_Note(self.soundCommand1, 0)
fluidsynth.stop_Note(self.soundCommand2, 0)
fluidsynth.play_Note(self.soundCommand3, 0, 100)
self.lbStatus.setText("Status: Comando 3")
self.updadeGraphView(self.circleNota3)
#State 0 1
elif not self.ch1State and self.ch0State:
fluidsynth.play_Note(self.soundCommand1, 0, 100)
fluidsynth.stop_Note(self.soundCommand2, 0)
fluidsynth.stop_Note(self.soundCommand3, 0)
self.lbStatus.setText("Status: Comando 1")
self.updadeGraphView(self.circleNota1)
#State 1 0
elif self.ch1State and not self.ch0State:
fluidsynth.stop_Note(self.soundCommand1, 0)
fluidsynth.play_Note(self.soundCommand2, 0, 100)
fluidsynth.stop_Note(self.soundCommand3, 0)
self.lbStatus.setText("Status: Comando 2")
self.updadeGraphView(self.circleNota2)
#State 1 1
else:
#fluidsynth.stop_everything()
fluidsynth.stop_Note(self.soundCommand1, 0)
fluidsynth.stop_Note(self.soundCommand2, 0)
fluidsynth.stop_Note(self.soundCommand3, 0)
self.lbStatus.setText("Status: Silencio")
self.updadeGraphView(self.circleSilence)
def emgValueChanged(self):
self.lbCh0.setText('CH0: %.2f V' % (self.sliderCh0.value() / 100.00))
self.lbCh1.setText('CH1: %.2f V' % (self.sliderCh1.value() / 100.00))
def thresholdChanged(self):
self.lbThCh0.setText('Limiar CH0: %.2f V' %
(self.sliderThCh0.value() / 100.00))
self.lbThCh1.setText('Limiar CH1: %.2f V' %
(self.sliderThCh1.value() / 100.00))
self.thresholdCh0 = (self.sliderThCh0.value() / 100.00)
self.thresholdCh1 = (self.sliderThCh0.value() / 100.00)
def showEmgValues(self, valorCh0, valorCh1):
self.sliderCh0.setValue(int(np.round(valorCh0 * 100)))
self.sliderCh1.setValue(int(np.round(valorCh1 * 100)))
def populateInstruments(self):
for instrumento in self.ihmMIDIinstrument.names:
self.cbInstruments.addItem(instrumento)
self.cbInstruments.setCurrentIndex(0)
def cbInstrumentsChanged(self, idx):
fluidsynth.set_instrument(0, idx)
def populateNotesAndScales(self):
Notas = "C D E F G A B"
for possivel_nota in Notas.split():
self.cbNota1.addItem(possivel_nota)
self.cbNota2.addItem(possivel_nota)
self.cbNota3.addItem(possivel_nota)
for possivel_nota in Notas.split():
self.cbNota1.addItem(possivel_nota + "#")
self.cbNota2.addItem(possivel_nota + "#")
self.cbNota3.addItem(possivel_nota + "#")
for possivel_nota in Notas.split():
self.cbNota1.addItem(possivel_nota + "b")
self.cbNota2.addItem(possivel_nota + "b")
self.cbNota3.addItem(possivel_nota + "b")
for n in range(0, 9):
self.cbEscala1.addItem(str(n))
self.cbEscala2.addItem(str(n))
self.cbEscala3.addItem(str(n))
self.cbNota1.setCurrentIndex(0)
self.cbNota2.setCurrentIndex(2)
self.cbNota3.setCurrentIndex(4)
self.cbEscala1.setCurrentIndex(4)
self.cbEscala2.setCurrentIndex(4)
self.cbEscala3.setCurrentIndex(4)
def cbNotasChanged(self, idx):
n1 = str(self.cbNota1.itemText(self.cbNota1.currentIndex()))
n2 = str(self.cbNota2.itemText(self.cbNota2.currentIndex()))
n3 = str(self.cbNota3.itemText(self.cbNota3.currentIndex()))
e1 = self.cbEscala1.currentIndex()
e2 = self.cbEscala2.currentIndex()
e3 = self.cbEscala3.currentIndex()
print "Notas: %s%d - %s%d - %s%d" % (n1, e1, n2, e2, n3, e3)
self.soundCommand1 = Note(n1, e1)
self.soundCommand2 = Note(n2, e2)
self.soundCommand3 = Note(n3, e3)
def updadeGraphView(self, shapetoupdate):
self.circleSilence.setBrush(self.normalColor)
self.circleNota1.setBrush(self.normalColor)
self.circleNota2.setBrush(self.normalColor)
self.circleNota3.setBrush(self.normalColor)
shapetoupdate.setBrush(self.hilightColor)
#debugging
#print(correctedData) #print the read values
return copy(
self.dataQueue) #return a copy of the queued spike event
else:
return False
#-------------------------------------------------------------------------------
#Run the appo
if __name__ == '__main__':
#create an object
hdarray = HDNerArray()
def read():
global hdarray
z = hdarray.getData()
n = len(z)
for k in range(n):
print(z.popleft())
#thread to read
th = ThreadHandler(read)
#waits for input to start
input('press ENTER to start...')
hdarray.start()
th.start()
#wait for input to finish
input('press ENTER to finish...')
th.kill()
def __init__(self, parent=None):
# Constructor
super(FormMain, self).__init__()
self.setupUi(self)
# Handlers
self.ur10 = None
self.iLimb = None
# Set available ports
self.port_ur10.setText("10.1.1.6")
self.port_iLimb.addItems(list_serial_ports())
self.port_tactile.addItems(list_serial_ports())
# For tactile values
self.ser = None
self.dataQueue = deque([], maxlen=1000)
self.receiveThread = ThreadHandler(_worker=self.receive)
self.MIN = np.array([0, 0])
self.MAX = np.array([4096, 4096])
self.mvaFilt = [
MovingAverage(_windowSize=1000),
MovingAverage(_windowSize=1000)
]
# Connect buttons to callback functions
self.initialize.clicked.connect(self.init_handlers)
self.configure.clicked.connect(self.configure_handlers)
self.init_main.clicked.connect(self.start_main)
self.stop_main.clicked.connect(self.break_main)
self.toHome.clicked.connect(self.move_to_home)
self.toBase.clicked.connect(lambda: self.move_to_base(t=5))
self.moveUp.clicked.connect(lambda: self.move_vertical(_dir=1))
self.moveDown.clicked.connect(lambda: self.move_vertical(_dir=-1))
self.cw.clicked.connect(self.rotate_hand_CW)
self.ccw.clicked.connect(self.rotate_hand_CCW)
self.toPose.clicked.connect(self.move_iLimb_to_pose)
self.pinch.clicked.connect(lambda: self.close_hand())
self.moveAway.clicked.connect(lambda: self.move_away())
self.startSensors.clicked.connect(
lambda: [self.sensors_timer.start(0),
self.receiveThread.start()])
self.stopSensors.clicked.connect(
lambda: [self.sensors_timer.stop(),
self.receiveThread.pause()])
self.calibrate.clicked.connect(self.calibrate_sensors)
self.visualize.clicked.connect(
lambda: [self.save_points(),
render3D('run.pcd', stage=1)])
self.convexHull.clicked.connect(
lambda: [self.save_points(),
render3D('run.pcd', stage=2)])
self.detectShape.clicked.connect(self.recognize_shape)
self.clear.clicked.connect(self.reset_stored_values)
# Initialize PoV graphs
views = [
self.view0, self.view1, self.view2, self.view3, self.view4,
self.view5
]
self.povBoxes = []
self.povPlots = []
for view in views:
view.ci.layout.setContentsMargins(0, 0, 0, 0)
view.ci.layout.setSpacing(0)
self.povBoxes.append(view.addPlot())
self.povPlots.append(pg.ScatterPlotItem())
self.povBoxes[-1].addItem(self.povPlots[-1])
# Initialize tactile sensor graphs
self.timestep = [0]
self.sensorData = [[], []]
self.sensorBoxes = []
self.sensorPlots = []
for view in [self.sensor_index, self.sensor_thumb]:
view.ci.layout.setContentsMargins(0, 0, 0, 0)
view.ci.layout.setSpacing(0)
self.sensorBoxes.append(view.addPlot())
self.sensorPlots.append(
pg.PlotCurveItem(pen=pg.mkPen('b', width=1)))
self.sensorBoxes[-1].addItem(self.sensorPlots[-1])
self.sensorBoxes[-1].setXRange(min=0, max=20, padding=0.1)
# self.sensorBoxes[-1].setYRange(min=0, max=1, padding=0.1)
self.pov_timer = QtCore.QTimer()
self.pov_timer.timeout.connect(self.update_pov)
self.sensors_timer = QtCore.QTimer()
self.sensors_timer.timeout.connect(self.update_sensor_readings)
self.main_thread = Thread(target=self._palpation_routine)
self.main_thread.daemon = True
# Redirect console output to textBrowser
sys.stdout = port(self.textBrowser)
# Create TensorFlow session and load pretrained model
self.load_session()
class FormMain(QMainWindow, Ui_MainWindow):
def __init__(self, parent=None):
# Constructor
super(FormMain, self).__init__()
self.setupUi(self)
# Handlers
self.ur10 = None
self.iLimb = None
# Set available ports
self.port_ur10.setText("10.1.1.6")
self.port_iLimb.addItems(list_serial_ports())
self.port_tactile.addItems(list_serial_ports())
# For tactile values
self.ser = None
self.dataQueue = deque([], maxlen=1000)
self.receiveThread = ThreadHandler(_worker=self.receive)
self.MIN = np.array([0, 0])
self.MAX = np.array([4096, 4096])
self.mvaFilt = [
MovingAverage(_windowSize=1000),
MovingAverage(_windowSize=1000)
]
# Connect buttons to callback functions
self.initialize.clicked.connect(self.init_handlers)
self.configure.clicked.connect(self.configure_handlers)
self.init_main.clicked.connect(self.start_main)
self.stop_main.clicked.connect(self.break_main)
self.toHome.clicked.connect(self.move_to_home)
self.toBase.clicked.connect(lambda: self.move_to_base(t=5))
self.moveUp.clicked.connect(lambda: self.move_vertical(_dir=1))
self.moveDown.clicked.connect(lambda: self.move_vertical(_dir=-1))
self.cw.clicked.connect(self.rotate_hand_CW)
self.ccw.clicked.connect(self.rotate_hand_CCW)
self.toPose.clicked.connect(self.move_iLimb_to_pose)
self.pinch.clicked.connect(lambda: self.close_hand())
self.moveAway.clicked.connect(lambda: self.move_away())
self.startSensors.clicked.connect(
lambda: [self.sensors_timer.start(0),
self.receiveThread.start()])
self.stopSensors.clicked.connect(
lambda: [self.sensors_timer.stop(),
self.receiveThread.pause()])
self.calibrate.clicked.connect(self.calibrate_sensors)
self.visualize.clicked.connect(
lambda: [self.save_points(),
render3D('run.pcd', stage=1)])
self.convexHull.clicked.connect(
lambda: [self.save_points(),
render3D('run.pcd', stage=2)])
self.detectShape.clicked.connect(self.recognize_shape)
self.clear.clicked.connect(self.reset_stored_values)
# Initialize PoV graphs
views = [
self.view0, self.view1, self.view2, self.view3, self.view4,
self.view5
]
self.povBoxes = []
self.povPlots = []
for view in views:
view.ci.layout.setContentsMargins(0, 0, 0, 0)
view.ci.layout.setSpacing(0)
self.povBoxes.append(view.addPlot())
self.povPlots.append(pg.ScatterPlotItem())
self.povBoxes[-1].addItem(self.povPlots[-1])
# Initialize tactile sensor graphs
self.timestep = [0]
self.sensorData = [[], []]
self.sensorBoxes = []
self.sensorPlots = []
for view in [self.sensor_index, self.sensor_thumb]:
view.ci.layout.setContentsMargins(0, 0, 0, 0)
view.ci.layout.setSpacing(0)
self.sensorBoxes.append(view.addPlot())
self.sensorPlots.append(
pg.PlotCurveItem(pen=pg.mkPen('b', width=1)))
self.sensorBoxes[-1].addItem(self.sensorPlots[-1])
self.sensorBoxes[-1].setXRange(min=0, max=20, padding=0.1)
# self.sensorBoxes[-1].setYRange(min=0, max=1, padding=0.1)
self.pov_timer = QtCore.QTimer()
self.pov_timer.timeout.connect(self.update_pov)
self.sensors_timer = QtCore.QTimer()
self.sensors_timer.timeout.connect(self.update_sensor_readings)
self.main_thread = Thread(target=self._palpation_routine)
self.main_thread.daemon = True
# Redirect console output to textBrowser
sys.stdout = port(self.textBrowser)
# Create TensorFlow session and load pretrained model
self.load_session()
""" Function to initialize handler objects """
def init_handlers(self):
# Create handlers
print('Initializing handlers ...')
self.ur10 = UR10Controller(self.port_ur10.text())
print('UR10 done.')
self.iLimb = iLimbController(self.port_iLimb.currentText())
self.iLimb.connect()
print('iLimb done.')
self.ser = serial.Serial(self.port_tactile.currentText(), 117964800)
self.ser.flushInput()
self.ser.flushOutput()
print('TactileBoard done')
""" Functions to set all handlers to default configuration """
def move_to_home(self):
print('Setting UR10 to default position ...')
UR10pose = URPoseManager()
UR10pose.load('shape_recog_home.urpose')
UR10pose.moveUR(self.ur10, 'home_j', 5)
time.sleep(5.2)
def move_iLimb_to_pose(self):
print('Setting iLimb to default pose ...')
self.iLimb.setPose('openHand')
time.sleep(3)
self.iLimb.control(['thumbRotator'], ['position'], [700])
time.sleep(3)
def calibrate_sensors(self):
self.receiveThread.start()
print('Calibrating tactile sensors ...')
# Clear data queue
self.dataQueue.clear()
# Wait till queue has sufficient readings
while len(self.dataQueue) < 500:
pass
# Calculate lower and upper bounds
samples = np.asarray(copy(self.dataQueue))
self.MIN = np.mean(samples, axis=0)
self.MAX = self.MIN + 500
self.dataQueue.clear()
# Set Y-range
for box in self.sensorBoxes:
box.setYRange(min=0, max=1, padding=0.1)
print("Done")
def configure_handlers(self):
self.move_to_home()
self.move_iLimb_to_pose()
self.calibrate_sensors()
print('Done.')
""" Function to create and load pretrained model """
def load_session(self):
self.model = vCNN()
self.session = tf.Session(graph=self.model.graph)
with self.session.as_default():
with self.session.graph.as_default():
saver = tf.train.Saver(max_to_keep=3)
saver.restore(
self.session,
tf.train.latest_checkpoint('../shape_recognition/save'))
""" Function to clear all collected values """
def reset_stored_values(self):
STATE.NUM_POINTS = 0
STATE.CONTACT_POINTS = []
STATE.CONTROL_POS = [0 for _ in range(5)]
STATE.FINGER_POS = {'index': [], 'thumb': []}
STATE.XYZR = []
STATE.UNIT_VECTOR = []
""" Function to close fingers until all fingers touch surface """
def close_hand(self, fingers=['index', 'thumb']):
touched = [False] * len(fingers)
touched_once = False
fingerArray = [[x, MAPPING[x], THRESHOLD[x]] for x in fingers]
while not all(touched):
time.sleep(0.005)
q = self.get_sensor_data()
for _ in range(len(q)):
tactileSample = q.popleft()
touched = self.iLimb.doFeedbackPinchTouch(
tactileSample, fingerArray, 1)
# update control_pos for fingers that have touched a surface
for i in range(len(fingerArray)):
if touched[i]:
touched_once = True
STATE.CONTROL_POS[fingerArray[i]
[1]] = self.iLimb.controlPos
#----------------------------------------------------------
# Collect information
STATE.FINGER_POS[fingerArray[i][0]].append(
self.iLimb.controlPos)
#----------------------------------------------------------
# Self-touching condition
# Can be modified later
if self.iLimb.controlPos > 200 and not touched_once:
return False
elif self.iLimb.controlPos > 200 and touched_once:
for i in range(len(fingerArray)):
if not touched[i]:
#----------------------------------------------------------
# Collect information
STATE.FINGER_POS[fingerArray[i][0]].append(-1)
#----------------------------------------------------------
return True
if all(touched):
return True
else:
# update fingerArray
fingerArray = [
fingerArray[i] for i in range(len(touched))
if not touched[i]
]
""" Function to calculate coordinates of points of contact """
def compute_coordinates(self):
self.ur10.read_joints_and_xyzR()
xyzR = copy(self.ur10.xyzR)
joints = copy(self.ur10.joints)
sim = ur10_simulator()
sim.set_joints(joints)
_ = sim.joints2pose()
_, rm = sim.get_Translation_and_Rotation_Matrix()
# Calculate the direction in which the end effector is pointing
# aVlue corresponding to z-direction is ignored
direction = rm[:2, 2] # x and y direction vector only
direction /= np.linalg.norm(direction)
# Calculate unit vector direction
dir_ang = np.arctan(abs(direction[1] / direction[0]))
if direction[0] < 0:
if direction[1] < 0:
dir_ang += np.pi
else:
dir_ang = np.pi - dir_ang
else:
if direction[1] < 0:
dir_ang = 2 * np.pi - dir_ang
# Find point of contact for index finger
idx_control = STATE.CONTROL_POS[MAPPING['index']]
if idx_control > 0:
theta = 30 + 60 / 500 * idx_control
if idx_control < 210:
# Normal circular motion
rel_theta = 30
else:
rel_theta = 30 + 60 / 290 * (idx_control - 210)
# rel_theta = 30 + 60/500 * idx_control
axis = IDX_0 * np.cos(np.deg2rad(theta)) + IDX_1 * np.cos(
np.deg2rad(theta + rel_theta))
perp = IDX_0 * np.sin(np.deg2rad(theta)) + IDX_1 * np.sin(
np.deg2rad(theta + rel_theta))
axis += IDX_TO_BASE
pt_1 = [
axis * np.cos(dir_ang) - perp * np.sin(dir_ang) + xyzR[0],
axis * np.sin(dir_ang) + perp * np.cos(dir_ang) + xyzR[1],
xyzR[2]
]
STATE.NUM_POINTS += 1
STATE.CONTACT_POINTS.append(pt_1)
# Find point of contact for thumb
thb_control = STATE.CONTROL_POS[MAPPING['thumb']]
if thb_control > 0:
theta = 90 * (1 - thb_control / 500)
axis = THB * np.cos(np.deg2rad(theta)) + THB_TO_BASE
perp = THB * np.sin(np.deg2rad(theta))
pt_2 = [
axis * np.cos(dir_ang) - perp * np.sin(dir_ang) + xyzR[0],
axis * np.sin(dir_ang) + perp * np.cos(dir_ang) + xyzR[1],
xyzR[2]
]
STATE.NUM_POINTS += 1
STATE.CONTACT_POINTS.append(pt_2)
#--------------------------------------------------
# Collect information
STATE.XYZR.append(xyzR)
STATE.UNIT_VECTOR.append(direction)
#--------------------------------------------------
""" Functions to rotate hand for next reading """
def rotate_hand_CCW(self):
self.ur10.read_joints()
joints = copy(self.ur10.joints)
if STATE.ROTATION_POS < 180 // STATE.ROTATION_ANGLE - 1:
STATE.ROTATION_POS += 1
joints[4] += STATE.ROTATION_ANGLE * -1
xyzR = self.ur10.move_joints_with_grasp_constraints(
joints, dist_pivot=220, grasp_pivot=60, constant_axis='z')
self.ur10.movej(xyzR, 3)
time.sleep(3.2)
def rotate_hand_CW(self):
self.ur10.read_joints()
joints = copy(self.ur10.joints)
if STATE.ROTATION_POS > 0:
STATE.ROTATION_POS -= 1
joints[4] += STATE.ROTATION_ANGLE * 1
xyzR = self.ur10.move_joints_with_grasp_constraints(
joints, dist_pivot=220, grasp_pivot=60, constant_axis='z')
self.ur10.movej(xyzR, 3)
time.sleep(3.2)
def rotate_hand(self):
# Boundary checks
if STATE.ROTATION_POS == 0 and STATE.ROTATION_DIR == -1:
STATE.ROTATION_DIR = 1
if STATE.ROTATION_POS == 180 // STATE.ROTATION_ANGLE - 1 and STATE.ROTATION_DIR == 1:
STATE.ROTATION_DIR = -1
# Rotate the hand according to direction
if STATE.ROTATION_DIR == 1:
self.rotate_hand_CCW()
else:
self.rotate_hand_CW()
""" Function to move hand in vertical direction """
def move_vertical(self, _dir=1):
# move one step up while palpating
self.ur10.read_joints_and_xyzR()
x, y, z, rx, ry, rz = copy(self.ur10.xyzR)
new_joint_pos = np.array([x, y, z + 10 * _dir, rx, ry, rz])
self.ur10.movej(new_joint_pos, 0.5)
time.sleep(0.7)
STATE.HEIGHT += 10 * _dir
""" Function to move hand away from the object """
def move_away(self, fingers=['thumb', 'index']):
self.iLimb.control(fingers, ['position'] * len(fingers),
[0] * len(fingers))
time.sleep(1)
""" Function to move UR10 to base """
def move_to_base(self, t=1):
self.ur10.read_joints_and_xyzR()
x, y, z, rx, ry, rz = copy(self.ur10.xyzR)
new_joint_pos = np.array([x, y, -200, rx, ry, rz])
self.ur10.movej(new_joint_pos, t)
time.sleep(t + .2)
STATE.HEIGHT = 0
STATE.ESTIMATED_HEIGHT = 200
""" Function to pause of main loop """
def break_main(self):
STATE.STOP = True
""" Function to resume/start standard palpation """
def start_main(self):
if STATE.STARTED:
STATE.STOP = False
else:
self.main_thread.start()
STATE.STARTED = True
self.pov_timer.start(0)
""" Main routine """
def _palpation_routine(self):
print("Starting standard palpation routine ...")
for i in range(180 // STATE.ROTATION_ANGLE):
print("Rotation pos : %d" % (i + 1))
while STATE.HEIGHT < STATE.ESTIMATED_HEIGHT:
# Pause condition
print("Height : %d" % STATE.HEIGHT)
while STATE.STOP is True:
time.sleep(0.05)
touched = self.close_hand(['thumb', 'index'])
time.sleep(0.1)
if touched:
self.compute_coordinates()
else:
STATE.ESTIMATED_HEIGHT = STATE.HEIGHT
self.iLimb.resetControl()
time.sleep(0.5)
self.move_away()
self.move_vertical()
self.move_to_base()
self.rotate_hand()
self.recognize_shape()
""" Function to detect shape """
def recognize_shape(self):
self.save_points()
detect_shape(self.session, self.model, 'run.pcd')
""" Function to save point cloud """
def save_points(self):
# Convert collected points to a PCD file
pts = np.asarray(STATE.CONTACT_POINTS)
finger_pos = np.asarray(
[STATE.FINGER_POS['index'], STATE.FINGER_POS['thumb']])
np.savetxt('controlpos.txt', finger_pos)
np.savetxt('xyzr.txt', np.asarray(STATE.XYZR))
np.savetxt('uv.txt', np.asarray(STATE.UNIT_VECTOR))
save_point_cloud(pts, 'run.pcd')
""" Function to update the 6 PoV images """
def update_pov(self):
idx = STATE.FINGER_POS['index']
if len(idx) > 0:
thb = STATE.FINGER_POS['thumb']
xyzr = STATE.XYZR
uv = STATE.UNIT_VECTOR
# Get lists of coordinates
x1, y1, z1, x2, y2, z2 = get_coords(idx, thb, xyzr, uv)
x = x1 + x2
y = y1 + y2
z = z1 + z2
# Store projections
pts = np.empty((6, 2, len(x)))
pts[0][0] = x
pts[0][1] = y
pts[1][0] = x
pts[1][1] = z
pts[2][0] = y
pts[2][1] = z
pts[3][0] = 2 * np.mean(x) - x
pts[3][1] = y
pts[4][0] = 2 * np.mean(x) - x
pts[4][1] = z
pts[5][0] = 2 * np.mean(y) - y
pts[5][1] = z
for i, plot in enumerate(self.povPlots):
plot.clear()
plot.addPoints(pts[i][0], pts[i][1])
""" Function to update the tactile sensor readings """
def update_sensor_readings(self):
if len(self.dataQueue) > 0:
data = self.dataQueue[-1] # take the latest reading
val_1 = (data[0] - self.MIN[0]) / (
self.MAX[0] - self.MIN[0]) # reading of first sensor (index)
val_2 = (data[1] - self.MIN[1]) / (
self.MAX[1] - self.MIN[1]) # reading of second sensor (thumb)
val_1, val_2 = np.clip([val_1, val_2], 0, 1)
t = self.timestep[-1] + 0.005
self.sensorData[0].append(val_1)
self.sensorData[1].append(val_2)
self.timestep.append(t)
for i, plot in enumerate(self.sensorPlots):
plot.setData(self.timestep[1:], self.sensorData[i])
if t > 20:
self.timestep = [0]
self.sensorData = [[], []]
""" Function to populate the data queue with raw values """
def receive(self):
recvLength = 6
waiting = self.ser.inWaiting()
if waiting >= recvLength:
rawQueue = [x for x in self.ser.read(recvLength)]
reading_0 = rawQueue[1] * 256 + rawQueue[2]
reading_1 = rawQueue[3] * 256 + rawQueue[4]
self.dataQueue.append([
self.mvaFilt[0].getSample(reading_0),
self.mvaFilt[1].getSample(reading_1)
])
""" Function to procure data from data queue """
def get_sensor_data(self):
# Select last (atmost) 50 readings
data = np.asarray(copy(self.dataQueue))[-20:]
self.dataQueue.clear()
# Normalize data
data = (data - self.MIN) / (self.MAX - self.MIN)
data = np.clip(data, 0, 1)
# Convert data to standard format (list of [patchno][col][row] values)
processed_data = deque([x.reshape(2, 1, 1) for x in data])
return processed_data
f.close()
if __name__ == "__main__":
# Port 0 means to select an arbitrary unused port
HOST, PORT = "localhost", 8888
server = SocketServer.TCPServer((HOST, PORT), ThreadedTCPRequestHandler)
ip, port = server.server_address
# Start a thread with the server -- that thread will then start one
# more thread for each request
server_thread = threading.Thread(target=server.serve_forever)
# Exit the server thread when the main thread terminates
server_thread.daemon = True
server_thread.start()
print "Server loop running in thread:", server_thread.name
#client(ip, port, "Hello World 1")
#client(ip, port, "Hello World 2")
#client(ip, port, "Hello World 3")
fileThread = ThreadHandler(updateFile)
fileThread.start()
x = raw_input('type any key to finish....\n')
fileThread.kill()
server.shutdown()
server.server_close()
class RawTactileBoard():
def __init__(self, _port='COM3', _flagSpike=False):
self.NROWS = 4 #number of rows
self.NCOLS = 4 #number of columns
self.NPATCH = 5 #number of sensors
self.port = _port #port name
self.dataQueue = deque() #data queue
self.thAcqLock = Lock() #lock for data acquisition
self.thProcLock = Lock() #lock for data processing
#serial handler for receiving data
self.serialHandler = SerialHandler(self.port,
7372800,
_header=0x24,
_end=0x21,
_numDataBytes=160,
_thLock=self.thAcqLock)
#thread for receiving packages
self.thAcq = ThreadHandler(self.serialHandler.readPackage)
#thread for processing the packages
self.thProc = ThreadHandler(self.processData)
#moving average for all taxels of each tactile sensor patch
self.mva = [[] for k in range(self.NPATCH * self.NROWS * self.NCOLS)]
for k in range(len(self.mva)):
self.mva[k] = copy(
MovingAverage(_windowSize=10)) #10 Hz cut-off frequency
self.taxelCounter = 0 #counter for moving average across all taxels
#matrix for storing previous values --> necessary for integrate and fire spikes
self.prevTactile = np.zeros((self.NPATCH, self.NROWS, self.NCOLS),
dtype=float)
#threshold for spike detection
self.threshold = 100
#flag that determines whether spikes or raw values should be used
self.flagSpike = _flagSpike
#start data acquisition
def start(self):
self.serialHandler.open() #open the serial port
self.thAcq.start() #start data acquisition
self.thProc.start() #start data processing
#stop acquisition
def stop(self):
self.thAcq.kill() #kill thread
self.thProc.kill() #kill thread
def processData(self):
#x = datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f')[:-3]
self.thAcqLock.acquire()
n = len(self.serialHandler.dataQueue)
q = copy(self.serialHandler.dataQueue)
self.serialHandler.dataQueue.clear()
self.thAcqLock.release()
#y = datetime.datetime.now().strftime('%Y-%m-%d %H:%M:%S.%f')[:-3]
#if n > 0:
# print(x,y,n)
#3d array containing the 4x4 matrix of each tactile patch
self.tactileSensors = np.zeros((self.NPATCH, self.NROWS, self.NCOLS),
dtype=float)
for k in range(n):
data = q.popleft()
for z in range(0, 160, 2):
patchNum = int((z / 2) % self.NPATCH)
row = int((z / 2) % (self.NROWS * self.NPATCH) // self.NPATCH)
col = int((z / 2) // (self.NCOLS * self.NPATCH))
#print(z,z/2,patchNum,row,col)
sample = data[z] << 8 | data[z + 1]
if self.flagSpike:
if (sample - self.prevTactile[patchNum][col][row] >
self.threshold):
self.tactileSensors[patchNum][col][row] = 1
#print(sample,self.prevTactile[patchNum][col][row])
elif (self.prevTactile[patchNum][col][row] - sample >
self.threshold):
self.tactileSensors[patchNum][col][row] = 0
else:
self.tactileSensors[patchNum][col][row] = 0.5
self.prevTactile[patchNum][col][row] = sample
else:
self.tactileSensors[patchNum][col][row] = self.mva[
self.taxelCounter].getSample(sample)
self.taxelCounter += 1
if self.taxelCounter >= (self.NPATCH * self.NROWS *
self.NCOLS):
self.taxelCounter = 0
self.thProcLock.acquire()
for w in range(self.NPATCH):
self.tactileSensors[w] = np.flip(self.tactileSensors[w], 1)
self.dataQueue.append(copy(self.tactileSensors))
self.thProcLock.release()
time.sleep(0.001) #necessary to prevent really fast thread access
def getData(self):
self.thProcLock.acquire()
q = copy(self.dataQueue)
self.dataQueue.clear()
self.thProcLock.release()
return q
def __init__(self,_port='/dev/ttyUSB0',_baud=115200): self.acqThread = ThreadHandler(self.readPackage) self.isConnected = False self.dataQueue = Queue() self.flagAcq = False SerialHandler.__init__(self,_port,_baud)
return unpack('f', binF)[0]
#-------------------------------------------------------------------------------
if __name__ == '__main__':
from threading import Lock
from threadhandler import ThreadHandler
l = Lock()
s = SerialHandler(_header=0x24, _end=0x21, _numDataBytes=2, _thLock=l)
def run():
global s, l
l.acquire()
n = len(s.dataQueue)
for k in range(n):
data = s.dataQueue.popleft()
print(n, k, data[0] << 8 | data[1])
l.release()
s.open()
t = ThreadHandler(s.readPackage)
t.start()
p = ThreadHandler(run)
p.start()
input()
t.kill()
p.kill()
s.close()
#-------------------------------------------------------------------------------