def __init__(self):
rf = yarp.ResourceFinder()
rf.setVerbose(True)
rf.configure(sys.argv)
try:
width = rf.find('width').asInt()
except:
pass
if width == 0:
width = 640
try:
height = rf.find('height').asInt()
except:
pass
if height == 0:
height = 480
try:
self.channels = rf.find('channels').asInt()
except:
pass
if self.channels == 0:
self.channels = 3
self.PORTS = [('input', 'image',
'image %sx%sx%s' % (width, height, self.channels))]
print self.PORTS
BaseModule.__init__(self)
def main():
yarp.Network.init()
rf = yarp.ResourceFinder()
rf.setDefaultContext("online_recognition")
rf.setDefaultConfigFile("default.ini")
rf.configure(sys.argv)
app = QtWidgets.QApplication(sys.argv)
form = Cockpit()
form.show()
app.exec_()
def __init__(self):
"""Initialize a SpeechRecognition1 object"""
rf = yarp.ResourceFinder()
rf.setVerbose(True)
rf.setDefaultContext('speechRecognition1')
rf.setDefaultConfigFile('speechRecognition1.ini')
self.my_lm = rf.findFileByName('words-20150720.lm')
self.my_dic = rf.findFileByName('words-20150720.dic')
self.outPort = yarp.Port()
self.outPort.open('/speechRecognition1:o')
self.init_gst()
def main():
yarp.Network.init()
rf = yarp.ResourceFinder()
rf.setDefaultContext("online_recognition")
rf.setDefaultConfigFile("eglove_only.ini")
rf.configure(sys.argv)
contact_module = DetectionContact(rf)
while (True):
try:
contact_module.update()
except KeyboardInterrupt:
break
def main(module_cls):
""" This is a main method to run a module from command line.
@param module_cls - an EZModule based class that can be started as a standalone module.
"""
args = createArgParser()
yarp.Network.init()
resource_finder = yarp.ResourceFinder()
resource_finder.setVerbose(True)
# resource_finder.configure(argc,argv);
module = module_cls(args.ip, args.port, args.name)
module.runModule(resource_finder)
yarp.Network.fini()
def main(module_cls):
""" This is a main method to run a module from command line.
@param module_cls - class inheriting BaseModule that can be started as a standalone module.
"""
assert issubclass(module_cls, BaseModule), EMSG_WRONG_CLS % module_cls.__name__
import sys
yarp.Network.init()
resource_finder = yarp.ResourceFinder()
resource_finder.setVerbose(True)
resource_finder.configure(sys.argv)
module_cls().runModule(resource_finder)
yarp.Network.fini()
return max([m[-1][0] for k, m in msg_list.iteritems()])
def getCurrPosition(self, part_name):
currPos = self.monitorPorts[part_name].read()
tuplePos = []
for p in range(currPos.size()):
tuplePos.append(round(currPos.get(p).asDouble(), 2))
return tuplePos
def interruptModule(self):
print "Interrupting"
self.close()
return True
def getPeriod(self):
return 0.1
def updateModule(self):
time.sleep(0.05)
return True
if __name__ == '__main__':
yarp.Network.init()
mod = ICubBodyControl()
rf = yarp.ResourceFinder()
rf.setVerbose(True)
rf.configure(sys.argv)
mod.runModule(rf)
mod.close()
def generate_3d_positions(args):
# Load yarp ResourceFinder
rf = yarp.ResourceFinder();
rf.setVerbose();
# Set the same default context of the iCubSkinGui for convenience loading
# .ini position containing the 2D location of the patches
rf.setDefaultContext("skinGui/skinGui");
prop = yarp.Property();
skinGuiConfFile = args.skinGui_conf_file[0];
skinGuiConfFullName = rf.findFileByName(skinGuiConfFile);
prop.fromConfigFile(skinGuiConfFullName);
print("Reading 2D taxel positions from " + skinGuiConfFullName)
sens_group = prop.findGroup("SENSORS")
completePart = triangleCluster();
maxTriangleNumber = -100;
completePart.trianglesNumbersList = [];
for i in range(1,sens_group.size()):
triangle_group = sens_group.get(i).asList();
triangle = {}
triangle["type"] = triangle_group.get(0).asString();
triangle["number"] = triangle_group.get(1).asInt();
maxTriangleNumber = max(triangle["number"],maxTriangleNumber);
completePart.trianglesNumbersList.append(triangle["number"]);
triangle["u"] = triangle_group.get(2).asInt();
triangle["v"] = triangle_group.get(3).asInt();
triangle["orient"] = triangle_group.get(4).asInt();
triangle["gain"] = triangle_group.get(5).asInt();
triangle["mirror"] = triangle_group.get(6).asInt();
completePart.triangles[triangle["number"]] = triangle
# taxel positions in triangle frame (expressed in millimeters)
taxelsPosInTriangle = []
# taxel 0
taxelsPosInTriangle.append(np.array([6.533, 0.0]))
# taxel 1
taxelsPosInTriangle.append(np.array([9.8, -5.66]))
# taxel 2
taxelsPosInTriangle.append(np.array([3.267, -5.66]))
# taxel 3
taxelsPosInTriangle.append(np.array([0.0, 0.0]))
# taxel 4
taxelsPosInTriangle.append(np.array([-3.267, -5.66]))
# taxel 5
taxelsPosInTriangle.append(np.array([-9.8, -5.66]))
# taxel 6 (thermal pad!)
taxelsPosInTriangle.append(np.array([-6.533, -3.75]))
# taxel 7
taxelsPosInTriangle.append(np.array([-6.533, 0]))
# taxel 8
taxelsPosInTriangle.append(np.array([-3.267, 5.66]))
# taxel 9
taxelsPosInTriangle.append(np.array([0.0, 11.317]))
# taxel 10 (thermal pad)
taxelsPosInTriangle.append(np.array([0, 7.507]))
# taxel 11
taxelsPosInTriangle.append(np.array([3.267, 5.66]))
# generate taxel list, we allocate a list of the total number of triangles
# dummy values (for the foot are 32) and then we overwrite the taxels
# for the real triangles
dummy_taxel = get_dummy_taxel();
# the total number of the triangles is composed by both real triangles
# and dummy triangles, is given by the length of the yarp vector published
# on the port, divided by 12 (for the torso: 384/12 = 32).
# Alternativly the total number of triangle for a skin part can be computed
# from the number of patches present in the skin part: each skin patch contains
# (a maximum of) 16 triangles, and for each skin patch 16*12 = 192 taxels are always
# streamed in the YARP ports, regardless of the actual presense of a physical triangle
# in the skin, so the total number of triangles is given by number_of_patches*16 .
# For more info see http://wiki.icub.org/wiki/Tactile_sensors_(aka_Skin)
# If we do not know apriori the number of patches in this part, we can easily get
# the total number of triangle from the skinGui configuration file: we just need to find the triangle with
# the maximum number, and then find the lowest multiple of 16 bigger then the maximum triangle number (plus one
# because the triangle number is 0-based
total_number_of_patches = (maxTriangleNumber/16)+1;
total_number_of_triangles = total_number_of_patches*16;
# number of taxels for triangle
taxel_per_triangle = 12
# list of taxels (from 0 to taxel_per_triangle) that are thermal
thermal_taxels_list = [6,10]
# taxel that is the center of the triangle
center_taxel = 3;
# pre-populate the taxels vector with all dummy taxels
taxelsList = total_number_of_triangles*taxel_per_triangle*[dummy_taxel]
for triangleNumber in completePart.triangles:
triangle = completePart.triangles[triangleNumber];
for i in range(0,taxel_per_triangle):
theta = np.pi*triangle["orient"]/180
rotMatrix = np.array([[np.cos(theta), -np.sin(theta)],
[np.sin(theta), np.cos(theta)]])
offset = rotMatrix.dot(taxelsPosInTriangle[i])
taxel = {}
# index of the taxel in the skin part YARP port
taxel["index"] = triangle["number"]*taxel_per_triangle+i;
taxel["triangleNumber"] = triangle["number"]
if( i in thermal_taxels_list ):
taxel["type"] = "thermal"
# u,v are the coordinates in millimeters of the taxels in
# the iCubSkinGui
# compute the offset of the taxel with respect to the triangle center
taxel["u"] = triangle["u"] + offset[0]
taxel["v"] = triangle["v"] + offset[1]
# the taxel x, y, z position in skin frame will be filled by
# the interpolation procedure
else:
taxel["type"] = "tactile"
taxel["u"] = triangle["u"] + offset[0]
taxel["v"] = triangle["v"] + offset[1]
taxel["x"] = None
taxel["y"] = None
taxel["z"] = None
taxelsList[taxel["index"]] = taxel
completePart.taxels = taxel_list_to_taxel_dict(taxelsList);
# Get triangle centers from CAD (passing through the URDF)
centersAndNormals = get_triangle_centers_from_urdf(args.urdf[0],args.link[0],args.skin_frame[0],completePart.trianglesNumbersList);
# Build interpolation cluster (for now depending on indipendent_patches switch)
interpolationClusters = [];
if( args.indipendent_patches ):
for patchId in range(0,total_number_of_patches):
interpolationClusters.append(completePart.getTrianglesInPatch(patchId))
else:
interpolationClusters.append(completePart)
completePartUnknownX = [];
completePartUnknownY = [];
completePartUnknownZ = [];
completePartCentersX = [];
completePartCentersY = [];
completePartCentersZ = [];
completePartNormX = [];
completePartNormY = [];
completePartNormZ = [];
for cluster in interpolationClusters:
trainingPointsU = []
trainingPointsV = []
unknownPointsU = []
unknownPointsV = []
valuesX = []
valuesY = []
valuesZ = []
for triangleNumber in cluster.triangles:
trainingPointsU.append(cluster.triangles[triangleNumber]["u"]);
trainingPointsV.append(cluster.triangles[triangleNumber]["v"]);
valuesX.append(centersAndNormals['centers'][triangleNumber][0])
valuesY.append(centersAndNormals['centers'][triangleNumber][1]);
valuesZ.append(centersAndNormals['centers'][triangleNumber][2]);
for taxelId in cluster.taxels:
taxel = cluster.taxels[taxelId];
unknownPointsU.append(taxel["u"]);
unknownPointsV.append(taxel["v"]);
assert(len(trainingPointsU) == len(trainingPointsV))
assert(len(trainingPointsU) == len(valuesX))
assert(len(trainingPointsU) == len(valuesY))
assert(len(trainingPointsU) == len(valuesZ))
ret = interpolate_using_griddata(trainingPointsU,trainingPointsV,valuesX,valuesY,valuesZ,unknownPointsU,unknownPointsV,cluster.taxels,centersAndNormals,cluster.taxel_offset);
# Plot results
if( args.plot ):
plot_points_in_3d(valuesX, valuesY, valuesZ, ret.unknownX, ret.unknownY, ret.unknownZ);
# Append the results
completePartUnknownX.extend(ret.unknownX);
completePartUnknownY.extend(ret.unknownY);
completePartUnknownZ.extend(ret.unknownZ);
completePartNormX.extend(ret.normX);
completePartNormY.extend(ret.normY);
completePartNormZ.extend(ret.normZ);
completePartCentersX.extend(valuesX);
completePartCentersY.extend(valuesY);
completePartCentersZ.extend(valuesZ);
# Export results
exportSkinManagerPositionTxtFile(completePart.taxels,completePartUnknownX,completePartUnknownY,completePartUnknownZ,completePartNormX,completePartNormY,completePartNormZ,args.link[0],args.skinManager_conf_file[0],taxel_per_triangle,center_taxel);
def configure(self, rf):
yarp.Network.init()
self.SIGNALS_TO_NAMES_DICT = dict((getattr(signal, n), n) \
for n in dir(signal) if n.startswith('SIG') and '_' not in n )
self.terminal = 'xterm'
rootPath = rf.check("root_path")
interactionConfPath = rf.check("config_path")
if (interactionConfPath == False and rootPath == False):
print "Cannot find .ini settings"
return False
else:
self.rootPath = rf.find("root_path").asString()
self.interactionConfPath = rf.find("config_path").asString()
persistence = rf.check("persistence",
yarp.Value("False")).asString()
windowed = rf.check("windowed", yarp.Value("True")).asString()
verbose = rf.check("verbose", yarp.Value("True")).asString()
self.persistence = True if (persistence == "True") else False
self.windowed = True if (windowed == "True") else False
self.verbose = True if (verbose == "True") else False
print 'Root supervisor path: \t', self.rootPath
print 'Model configuration file: \t', self.interactionConfPath
print 'Bash Persistence set to: \t', self.persistence
print 'Windowed set to: \t', self.windowed
print 'Verbose set to: \t', self.verbose
self.modelPath = self.rootPath + '/Models'
self.dataPath = self.rootPath + '/Data'
#OLD
# self.trainingFunctionsPath = os.environ.get("WYSIWYD_DIR")+"/bin"
#NEW
self.trainingFunctionsPath = SAM.SAM_Drivers.__path__
self.trainingListHandles = dict(
) #make this a dict to have a label attached to each subprocess
self.loadedListHandles = dict()
self.iter = 0
self.rpcConnections = []
self.inputBottle = yarp.Bottle()
self.sendingBottle = yarp.Bottle()
self.responseBottle = yarp.Bottle()
self.outputBottle = yarp.Bottle()
if (not self.windowed): self.devnull = open('/dev/null', 'w')
out = yarp.Bottle()
self.checkAvailabilities(out)
if (self.verbose): print out.toString()
self.supervisorPort = yarp.Port()
self.supervisorPort.open('/sam/rpc:i')
self.attach(self.supervisorPort)
cmd = 'ipcluster start -n 4'
command = "bash -c \"" + cmd + "\""
if self.windowed:
c = subprocess.Popen([self.terminal, '-e', command],
shell=False)
else:
c = subprocess.Popen([cmd],
shell=True,
stdout=self.devnull,
stderr=self.devnull)
self.trainingListHandles['Cluster'] = c
if len(self.uptodateModels) + len(self.updateModels) > 0:
if self.verbose:
print "Loading models according to " + self.interactionConfPath
# start loading model configuration according to interactionConfPath file
rfModel = yarp.ResourceFinder()
rfModel.setVerbose(self.verbose)
rfModel.setDefaultContext("samSupervisor")
self.interactionConfFile = rfModel.findFile(
self.interactionConfPath)
# Iterate over all sections within the interactionConfPath,
# create a list and check against the available models
# warn if model specified in interactionConfPath not loadable
self.interactionParser = SafeConfigParser()
self.interactionParser.read(self.interactionConfFile)
self.interactionSectionList = self.interactionParser.sections()
if self.verbose:
print
print self.dataPath
print self.interactionSectionList
print
for j in self.interactionSectionList:
command = yarp.Bottle()
command.addString("load")
command.addString(j)
if self.verbose:
print command.toString()
reply = yarp.Bottle()
self.loadModel(reply, command)
if self.verbose:
print reply.toString()
print "-----------------------------------------------"
print
elif len(self.noModels) > 0:
if self.verbose:
print "Models available for training."
# Train a model according to ineractionConfPath file
else:
if self.verbose:
print "No available models to load or train"
# wait for a training command
return True
import yarp
import icubclient
import sys
import time
if __name__ == '__main__':
yarp.Network.init()
logger = yarp.Log()
iCub = icubclient.ICubClient("AREiCubClientExample", "icubClient", "example_ARE.ini")
rfClient = yarp.ResourceFinder()
rfClient.setVerbose(True)
rfClient.setDefaultContext("icubClient")
rfClient.setDefaultConfigFile("example_ARE.ini")
rfClient.configure(sys.argv)
# we connect to both ARE and OPC
if not iCub.connect():
logger.error() << "[ARE_KARMAiCubClientExample] ARE and/or OPC seems unavailabe!"
if rfClient.check("target"):
target = rfClient.find("target").asString()
logger.info() << "target name set to %s"
logger.info() << target
else:
target = "octopus"
logger.info() << "target name set to default, i.e. "
logger.info() << target
def test_modelWithParams(modelName, driverName, datacollectionOnly,
randomRecall, probRecall, bufferLen, recency,
transmissionDelay):
# constant variables
automaticOpen = True
connectDirect = True
frameLen = 15
yrf = yarp.ResourceFinder()
yrf.setVerbose(True)
yrf.setDefaultContext("samSupervisor")
yrf.setDefaultConfigFile("default.ini")
yrf.configure([])
rootPath = yrf.find("root_path").asString()
name = rootPath + '/Models/' + modelName + '__' + driverName + '*.pickle'
fname = glob.glob(name)
assert len(fname) > 0, 'model file not found'
modelFileName = fname[0]
interactionConfFile = yrf.find("config_path").asString()
interactionConfFile = yrf.findFile(interactionConfFile)
# copyfile(interactionConfFile, interactionConfFile+'.bkp')
parser = SafeConfigParser()
parser.optionxform = str
parser.read(interactionConfFile)
assert modelName in parser.sections(), 'model name not in parser sections'
items = parser.items(modelName)
for j in items:
if j[0] == 'dataIn':
dataInPort = j[1].split(' ')[0]
elif j[0] == 'rpcBase':
rpcInPort = j[1]
elif j[0] == 'callSign':
askCommand = [k for k in j[1].split(',') if 'label' in k][0]
elif j[0] == 'collectionMethod':
collectionMethod = j[1].split(' ')[0]
if collectionMethod == 'continuous':
parser.set(modelName, 'collectionMethod',
'continuous ' + str(bufferLen))
elif j[0] == 'recency' and collectionMethod == 'continuous':
parser.set(modelName, 'recency', str(recency))
parser.write(open(interactionConfFile, 'wb'))
def checkRecall(rpcPort, askCommand):
# check correct response
cmBottle = yarp.Bottle()
rpBottle = yarp.Bottle()
cmBottle.addString(askCommand)
rpcPort.write(cmBottle, rpBottle)
results.append(rpBottle.toString())
return rpBottle
networkFound = yarp.Network.checkNetwork()
assert networkFound, 'Yarpserver not found'
processesList = []
# open data port
dataPortName = '/modelTest/data:o'
dataPort = yarp.BufferedPortBottle()
dataPort.open(dataPortName)
# open query port
rpcPortName = '/modelTest/rpc'
rpcPort = yarp.RpcClient()
rpcPort.open(rpcPortName)
# open interactionSAMModel
if connectDirect:
if automaticOpen:
args = ' '.join([
rootPath + '/Data/' + modelName,
modelFileName.replace('.pickle', ''), interactionConfFile,
driverName, 'False'
])
pyFile = 'interactionSAMModel.py'
interactionCMD = ' '.join([pyFile, args])
print interactionCMD
windowedCMD = "bash -c \"" + interactionCMD + "\""
interactionProcess = subprocess.Popen(['xterm', '-e', windowedCMD],
shell=False)
# wait until model loaded
time.sleep(7)
else:
# check interactionSAMModel present
pass
yarp.Network.connect(rpcPortName, rpcInPort + ':i')
else:
yarp.Network.connect(rpcPortName, '/sam/rpc:i')
# send load Actions3
# connect data to /sam/actions/actionData:i
yarp.Network.connect(dataPortName, dataInPort)
# yarp.Network.connect(dataPortName, '/reader')
# check length of data log file
dataFile = open(join('./noisyActionData/recordedData', 'data.log'), 'r')
for i, l in enumerate(dataFile):
pass
lenDataFile = i + 1
print lenDataFile
dataFile.close()
# send each line with a pause of 0.1s and query model every frameLen data points.
# check correct response and continued operation os samSupervisor
processBreak = False
dataFile = open(join('./noisyActionData/recordedData', 'data.log'), 'r')
results = []
for curr in range(lenDataFile):
line = dataFile.readline()
lineParts = line.split(' ')
line = ' '.join(lineParts[2:])
dataBottle = dataPort.prepare()
dataBottle.fromString(line)
dataPort.write()
time.sleep(transmissionDelay)
if randomRecall and random.random(
) < probRecall and not datacollectionOnly:
rpBottle = checkRecall(rpcPort, askCommand)
pollResult = interactionProcess.poll()
print "{0:.2f}".format(
curr * 100.0 / lenDataFile
) + '%\t', 'Request Response:', rpBottle.toString(), pollResult
if curr % frameLen == 0:
if not datacollectionOnly and not randomRecall:
rpBottle = checkRecall(rpcPort, askCommand)
# check reply and status of process
pollResult = interactionProcess.poll()
print "{0:.2f}".format(
curr * 100.0 / lenDataFile
) + '%\t', 'Request Response:', rpBottle.toString(), pollResult
else:
pollResult = interactionProcess.poll()
print "{0:.2f}".format(
curr * 100.0 / lenDataFile) + '%\t', pollResult
if pollResult is not None:
processBreak = True
break
dataFile.close()
try:
print 'Terminating process'
interactionProcess.send_signal(signal.SIGINT)
except:
pass
retCode = interactionProcess.wait()
if processBreak:
testResult = False
else:
testResult = True
return testResult
