def main():
yarp.Network.init()
rpc_client = yarp.RpcClient()
rpc_client.open('/test-omega/rpc:o')
yarp.NetworkBase.connect('/test-omega/rpc:o', '/yarp-omega3-server/rpc:i')
# Test a simple circular motion
r = 0.04
f = 0.5
dt = 0.01
t = 0.0
try:
while True:
send_position_reference\
(
rpc_client,
r * math.sin(2 * math.pi * f * t),
r * math.cos(2 * math.pi * f * t),
0.0
)
t += dt
time.sleep(dt)
except KeyboardInterrupt:
send_stop(rpc_client)
rpc_client.close()
def main():
yarp.Network.init()
rpc_client = yarp.RpcClient()
rpc_client.open('/test-omega/rpc:o')
yarp.NetworkBase.connect( '/test-omega/rpc:o', '/yarp-omega3-server/rpc:i')
# Test four poses
try:
send_position_reference(rpc_client, 0.04, 0.04, 0.0)
time.sleep(2.0)
send_position_reference(rpc_client, 0.04, -0.04, 0.0)
time.sleep(2.0)
send_position_reference(rpc_client, 0.0, -0.04, 0.0)
time.sleep(2.0)
send_position_reference(rpc_client, 0.0, 0.04, 0.0)
time.sleep(2.0)
send_position_reference(rpc_client, 0.0, 0.0, 0.0)
time.sleep(2.0)
send_stop(rpc_client)
time.sleep(2.0)
except KeyboardInterrupt:
send_stop(rpc_client)
rpc_client.close()
def createPorts(self):
self.imageDataInputPort = yarp.BufferedPortImageRgb()
self.outputFacePrection = yarp.Port()
self.speakStatusPort = yarp.RpcClient()
self.speakStatusOutBottle = yarp.Bottle()
self.speakStatusInBottle = yarp.Bottle()
self.imageInputBottle = yarp.Bottle()
def _getClient(self, output):
_rpc_client = yarp.RpcClient()
_port_name = "/" + self.__class__.__name__ + "/" + str(
id(self)) + "/cmd"
_rpc_client.open(_port_name)
_rpc_client.addOutput(output)
return _rpc_client, _port_name
def __init__(self, rpc_server_name):
self.__logger__ = YarpLogger.getLogger()
self.__rpc_client__ = yarp.RpcClient()
self.__rpc_client_port_name__ = rpc_server_name + "/rpc_client/commands"
self.__rpc_client__.open(self.__rpc_client_port_name__)
self.__logger__.debug("Connecting %s with %s" % (self.__rpc_client_port_name__, rpc_server_name))
res = self.__rpc_client__.addOutput(rpc_server_name)
self.__logger__.debug("Result: %s" % res)
def __init__(self, module_name, robot_name, task_name):
port_prefix = '/' + robot_name + '/' + module_name + '/' + task_name + '/'
self.set_ref = yarp.BufferedPortBottle()
self.set_ref.open(port_prefix + 'set_ref:o')
self.set_ref.addOutput(port_prefix + 'set_ref:i')
#yarp.Network.connect(port_prefix+'set_ref:o',
# port_prefix+'set_ref:i')
self.rpc = yarp.RpcClient()
self.rpc.open(port_prefix + 'rpc_client')
self.rpc.addOutput(port_prefix + 'rpc')
def __init__(self):
self._rpc_client = yarp.RpcClient()
self._port_name = "/WorldController-" + str(id(self)) + "/commands"
self._rpc_client.open(self._port_name)
self._rpc_client.addOutput("/icubSim/world")
# A dictionary to track simulator object IDs for all types of objects
self._sim_ids_counters = collections.defaultdict(lambda:0)
# A sequence to track internal object IDs. This list stores tuples (object type, simulator id)
# so that outside one does not have to remember the type of object.
self._objects = [ ]
def __createPort(self, name, target=None, mode='unbuffered'):
""" This method returns a port object.
@param name - yarp name for the port
@param obj - object for which the port is created
@param buffered - if buffered is True a buffered port will be used otherwise not;
default is True.
@result port
"""
# create port
if mode == 'buffered':
port = yarp.BufferedPortBottle()
elif mode == 'rpcclient':
port = yarp.RpcClient()
elif mode == 'rpcserver':
port = yarp.RpcServer()
else:
port = yarp.Port()
# build port name
port_name = ['']
# prefix handling
if hasattr(self, 'prefix') and self.prefix:
port_name.append(self.prefix)
port_name.append(self.__class__.__name__)
port_name.append(name)
# open port
if not port.open('/'.join(port_name)):
raise RuntimeError, EMSG_YARP_NOT_FOUND
# add output if given
if target:
port.addOutput(target)
if hasattr(self, '_ports'):
self._ports.append(port)
return port
def __init__(self):
yarp.RFModule.__init__(self)
# Right Arm device
self.rightArmOptions = yarp.Property()
self.rightArmDevice = None
self.rightArmIEncoders = None
self.numRightArmJoints = 0
self.rightArmIPositionControl = None
self.rightArmIPositionControl = None
self.rightArmIControlMode = None
self.rightArmIPositionDirect = None
self.rightArmIRemoteVariables = False
self.rigthArmIControlLimits = None
# Trunk device
self.trunkOptions = yarp.Property()
self.trunkDevice = None
self.trunkIEncoders = None
self.numTrunkJoints = 0
self.trunkIPositionControl = None
self.trunkIControlMode = None
self.trunkIPositionDirect = None
self.trunkIRemoteVariables = False
self.trunkIControlLimits = None
# tts client
self.rpcClientTts = yarp.RpcClient()
# Trunk and Right Arm solver device
self.trunkRightArmSolverOptions = yarp.Property()
self.trunkRightArmSolverDevice = None
self.jointsGoalPositionPort = yarp.BufferedPortBottle()
self.demo_object_input_port_name = "/executeTrajectoryDMP/goalJoints:i"
self.q_min = []
self.q_max = []
self.dmp = None
self.dt = 0.001
self.execution_time = 1.0
self.n_features = 50
def main():
yarp.Network.init()
rpc_client = yarp.RpcClient()
rpc_client.open('/test-omega/rpc:o')
yarp.NetworkBase.connect('/test-omega/rpc:o', '/yarp-omega3-server/rpc:i')
print('##################################################################')
print('# Warning: the robot will try to exert a force along the y axis. #')
print('# Please hold the end-effector firmly before continuing. #')
print('# Press any key to continue. #')
print('# Press Ctrl-C at any time to stop robot. #')
print('##################################################################')
input()
# Test a simple sinusoidal reference
a = 3.0
f = 0.5
dt = 0.01
t = 0.0
try:
while True:
send_force_reference(rpc_client, 0.0,
a * math.sin(2 * math.pi * f * t), 0.0)
t += dt
time.sleep(dt)
except KeyboardInterrupt:
send_stop(rpc_client)
rpc_client.close()
def main():
# The state machine has the following states:
# [VOID, INIT, FIND, SHOW, KEY, ICUB, WHICH, QUIT]
#
# VOID: Empty state, to use for test
# INIT: It is called only once for the initialisation
# FIND: use the hardware face and landmark detection libraries
# SHOW: Print the image on screen using OpenCV
# KEY: Check which key is pressed
# ICUB: Pressing the (h) button the robot look in front of itself
# WHICH: Pressing the (w) button is like asking to the robot to look to a object on the table
# QUIT: Pressing (q) unsubscribe and close the script
#Configuration Variables, adjust to taste
#ICUB_IP = "192.168.0.100"
#ICUB_PORT = 9559
RECORD_VIDEO = True #If True record a video from the ICUB camera
USE_FESTIVAL_TTS = True #To use the Festival Text To Speach
#If you want to use Acapela TTS you have to fill the
#following variable with the correct values
#USE_ACAPELA_TTS = False
#ACCOUNT_LOGIN = '******'
#APPLICATION_LOGIN = '******'
#APPLICATION_PASSWORD = '******'
#SERVICE_URL = 'http://vaas.acapela-group.com/Services/Synthesizer'
#The initial state
STATE = "VOID"
while(True):
#Empty STATE, to be used for test
if(STATE == "VOID"):
STATE = "INIT"
# The zero state is an init phase
# In this state all the proxies are
# created and tICUB subscribe to the services
#
elif(STATE=="INIT"):
#Init some generic variables
#This counter allows continuing the program flow
#without calling a sleep
which_counter = 0 #Counter increased when WHICH is called
which_counter_limit = 30 #Limit for the which counter
#Init YARP
print("[STATE " + str(STATE) + "] " + "YARP init" + "\n")
yarp.Network.init()
#Camera connection
try:
print("[STATE " + str(STATE) + "] " + "Waiting for '/icubSim/cam/left' ..." + "\n")
cam_w = 320 #640
cam_h = 240 #480
input_port = yarp.Port()
input_port.open("/pyera-image-port")
yarp.Network.connect("/icubSim/cam/left", "/pyera-image-port")
img_array = np.zeros((cam_h, cam_w, 3), dtype=np.uint8)
yarp_image = yarp.ImageRgb()
yarp_image.resize(cam_w, cam_h)
yarp_image.setExternal(img_array, img_array.shape[1], img_array.shape[0])
except BaseException, err:
print("[ERROR] connect To Camera catching error " + str(err))
return
#Head connection and Reset
print("[STATE " + str(STATE) + "] " + "Waiting for '/icubSim/head/rpc:i' ..." + "\n")
rpc_client = yarp.RpcClient()
rpc_client.addOutput("/icubSim/head/rpc:i")
print("[STATE " + str(STATE) + "] " + "Reset the head position..." + "\n")
set_icub_head_pose(rpc_client, roll=0, pitch=0, yaw=0)
#Initialise the OpenCV video recorder
if(RECORD_VIDEO == True):
print("[STATE " + str(STATE) + "] " + "Starting the video recorder..." + "\n")
fourcc = cv2.cv.CV_FOURCC(*'XVID')
video_out = cv2.VideoWriter("./output.avi", fourcc, 20.0, (cam_w,cam_h))
#Init dlib Face detector
#my_face_detector = dlib.get_frontal_face_detector()
#Init the Deepgaze face detector
my_cascade = haarCascade("./haarcascade_frontalface_alt.xml", "./haarcascade_profileface.xml")
#Talking
if(USE_FESTIVAL_TTS == True):
print("[STATE " + str(STATE) + "] " + "Trying the TTS Festival..." + "\n")
say_something("Hello World, I'm ready!")
#if(USE_ACAPELA_TTS == True):
#print("[ACAPELA] Downloading the mp3 file...")
#tts_acapela = acapela.Acapela(account_login=ACCOUNT_LOGIN, application_login=APPLICATION_LOGIN, application_password=APPLICATION_PASSWORD,
#service_url=SERVICE_URL, quality='22k', directory='/tmp/')
#tts_acapela.prepare(text="Hello world, I'm ready!", lang='US', gender='M', intonation='NORMAL')
#output_filename = tts_acapela.run()
#subprocess.Popen(["play","-q","/tmp/" + str(output_filename)])
#print "[ACAPELA] Recorded TTS to %s" % output_filename
#Swithc to STATE > 1
print("[STATE " + str(STATE) + "] " + "Switching to next state" + "\n")
time.sleep(2)
STATE = "FIND"
# Get data from landmark detection and
# face detection.
#
elif(STATE=="FIND"):
#Get Data for Face Detection
#if(face_data and isinstance(face_data, list) and len(face_data) > 0):
#print("[ICUB] Face detected!")
#else:
#print("No face detected...")
#is_face_detected = False
input_port.read(yarp_image)
'''
faces_array = my_face_detector(img_array, 1)
print("Total Faces: " + str(len(faces_array)))
for i, pos in enumerate(faces_array):
face_x1 = pos.left()
face_y1 = pos.top()
face_x2 = pos.right()
face_y2 = pos.bottom()
text_x1 = face_x1
text_y1 = face_y1 - 3
cv2.putText(img_array, "FACE " + str(i+1), (text_x1,text_y1), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0), 1);
cv2.rectangle(img_array,
(face_x1, face_y1),
(face_x2, face_y2),
(0, 255, 0),
2)
'''
gray = cv2.cvtColor(img_array, cv2.COLOR_BGR2GRAY)
# Return code: 1=Frontal, 2=FrontRotLeft, 3=FronRotRight,
# 4=ProfileLeft, 5=ProfileRight.
my_cascade.findFace(gray, runFrontal=True, runFrontalRotated=False, runLeft=True, runRight=True,
frontalScaleFactor=1.18, rotatedFrontalScaleFactor=1.18, leftScaleFactor=1.18,
rightScaleFactor=1.18, minSizeX=70, minSizeY=70, rotationAngleCCW=30,
rotationAngleCW=-30, lastFaceType=my_cascade.face_type)
face_x1 = my_cascade.face_x
face_y1 = my_cascade.face_y
face_x2 = my_cascade.face_x + my_cascade.face_w
face_y2 = my_cascade.face_y + my_cascade.face_h
text_x1 = face_x1
text_y1 = face_y1 - 3
if(my_cascade.face_type == 1 or my_cascade.face_type == 2 or my_cascade.face_type == 3): cv2.putText(img_array, "FRONTAL", (text_x1,text_y1), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0), 1);
elif(my_cascade.face_type == 4): cv2.putText(img_array, "LEFT", (text_x1,text_y1), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0), 1);
elif(my_cascade.face_type == 5): cv2.putText(img_array, "RIGH", (text_x1,text_y1), cv2.FONT_HERSHEY_SIMPLEX, 0.5, (0,255,0), 1);
cv2.rectangle(img_array,
(face_x1, face_y1),
(face_x2, face_y2),
(0, 255, 0),
2)
is_face_detected = False
STATE = "SHOW"
print("[ICUB][ERROR] connect To iKinGazeCtrl/xd catching error " +
str(err))
return
try:
self.port_cart_leftarm = yarp.Port()
self.port_cart_leftarm.open("/pyera-cart-leftarm")
yarp.Network.connect("/pyera-cart-leftarm",
"/cartesianSolver/left_arm/in")
except BaseException, err:
print(
"[ICUB][ERROR] connect To /cartesianSolver/left_arm/in catching error "
+ str(err))
return
self.rpc_client_head = yarp.RpcClient()
self.rpc_client_head.addOutput(icub_root + "/head/rpc:i")
self.rpc_client_head_ikin = yarp.RpcClient()
self.rpc_client_head_ikin.addOutput("/iKinGazeCtrl/rpc")
def close(self):
"""Close all the services
"""
self.port_left_camera.close()
self.port_right_camera.close()
self.rpc_client_head.close()
def check_connection(self):
"""Check if the internet connection is present or not
) # create an instance of Property, a nice YARP class for storing name-value (key-value) pairs
optionsH.put('device', 'remote_controlboard'
) # we add a name-value pair that indicates the YARP device
optionsH.put('remote',
robot + '/head') # we add info on to whom we will connect
optionsH.put(
'local', '/demo' + robot +
'/head') # we add info on how we will call ourselves on the YARP network
ddH = yarp.PolyDriver(
optionsH) # create a YARP multi-use driver with the given options
posH = ddH.viewIPositionControl(
) # make a position controller object we call 'pos'
axesH = posH.getAxes() # retrieve number of joints
#-- Text-to-speech (TTS)
tts = yarp.RpcClient()
tts.open('/demo/tts/rpc:c')
yarp.Network.connect('/demo/tts/rpc:c', '/tts/rpc:s')
def ttsLang(language):
cmd = yarp.Bottle()
res = yarp.Bottle()
cmd.addString('setLanguage')
cmd.addString(language)
tts.write(cmd, res)
def ttsSay(sayStr):
cmd = yarp.Bottle()
res = yarp.Bottle()
clientName = '/testSender'
dataPortName = '/dataPort'
serverName = '/yarpdataplayer/rpc:i'
labelStorePath = '/home/daniel/WYSIWYD_PROJECT/actionRecognitionDataset/push-pull-right_arm/labels/data.log'
dataStorePath = '/home/daniel/WYSIWYD_PROJECT/actionRecognitionDataset/push-pull-right_arm/data/data.log'
pygame.init()
screen = pygame.display.set_mode((640, 100))
pygame.display.set_caption('Dataset Labeller')
pygame.mouse.set_visible(1)
pygame.key.set_repeat(100,100)
yarp.Network.init()
messagePort = yarp.RpcClient()
messagePort.open(clientName)
dataPort = yarp.BufferedPortBottle()
dataPort.open(dataPortName)
connected = False
step = False
inputBottle = yarp.Bottle()
outputBottle = yarp.Bottle()
dataInBottle = yarp.Bottle()
pressed = False
seclabel = ''
previousIdx = 0
currIdx = 0
timeString = ''
def __init__(self):
yarp.RFModule.__init__(self)
# handle port for the RFModule
self.module_name = None
self.handle_port = None
self.process = False
# Define vars to receive audio
self.audio_in_port = None
self.eventPort = None
self.is_voice = False
# Predictions parameters
self.label_outputPort = None
self.predictions = []
self.database = None
# Speaker module parameters
self.model_audio = None
self.dataset_path = None
self.db_embeddings_audio = None
self.threshold_audio = None
self.length_input = None
self.resample_trans = None
self.speaker_emb = []
# Parameters for the audio
self.sound = None
self.audio = []
self.np_audio = None
self.nb_samples_received = 0
self.sampling_rate = None
# Define port to receive an Image
self.image_in_port = yarp.BufferedPortImageRgb()
self.face_coord_port = yarp.BufferedPortBottle()
# Port to query and update the memory (OPC)
self.opc_port = yarp.RpcClient()
# Image parameters
self.width_img = None
self.height_img = None
self.input_img_array = None
self.frame = None
self.coord_face = None
self.threshold_face = None
self.face_emb = []
# Model face recognition modele
self.modele_face = None
self.db_embeddings_face = None
self.trans = None
self.faces_img = []
self.face_coord_request = None
self.face_model_path = None
# Model for cross-modale recognition
self.model_av = None
self.sm = torch.nn.Softmax(dim=1)
self.threshold_multimodal = None
self.device = None
self.save_face = False
self.name = ""
self.predict = False
interactionFunction = [
s for s in interactionFunction for g in functionsList
if s == g
]
if (len(interactionFunction) != 0):
parser2 = SafeConfigParser()
parser2.read(interactionConfPath)
if (parser2.has_section(j[0].lower())
): #checking current model allows this memory section
#create interface ports
interfacePortName = parser2.get(
j[0].lower(), 'rpcBase') + ':o'
callSignList = parser2.get(j[0].lower(),
'callSign').replace(
' ', '').split(',')
interfacePort = yarp.RpcClient()
interfacePort.open(interfacePortName)
rpcConnections.append([
j[0].lower(), interfacePort,
interfacePortName[:-1], callSignList
])
args = ' '.join([
join(dataPath, j[0]),
join(modelPath, j[4]), interactionConfPath
])
command = 'ipython ' + join(
trainingFunctionsPath,
interactionFunction[0] + '.py') + ' -- ' + args
command = "bash -c \"" + command + "; exec bash\""
#command = "bash -c \"" + command + "\""
subprocess.Popen(['gnome-terminal', '-e', command])
'WYSIWYD_DIR'] + '/share/wysiwyd/contexts/sightcorp/sightcorp.ini'
print("Will trying to open the .ini file in ", configFileName)
config = cp.ConfigParser()
config.readfp(open(configFileName))
app_key = config.get('API_Security', 'app_key')
client_id = config.get('API_Security', 'client_id')
print("app_key = ", app_key)
print("client_id = ", client_id)
# Initialise YARP
yarp.Network.init()
# Preparing port to connect to ABM
abm_port = yarp.RpcClient()
abm_local = '/sightcorp/abm:o'
abm_remote = '/autobiographicalMemory/rpc'
abm_port.open(abm_local)
# Loop to connect to ABM
while (not yarp.Network.connect(abm_local, abm_remote)):
print("Waiting for connection to ABM")
time.sleep(1)
# Ask where the images are stored
cmd = yarp.Bottle()
resp = yarp.Bottle()
cmd.addString('getStoringPath')
abm_port.write(cmd, resp)
storing_path = resp.get(0).asString()
img_array = numpy.zeros((240, 320, 3), dtype=numpy.uint8)
#imgplot = plt.imshow(img_array)
cv2.imshow('image',img_array)
cv2.waitKey(0)
cv2.destroyAllWindows()
'''
#yarp.Network.init() # Initialise YARP
#output_port = yarp.Port()
#output_port.open("/icubSim/head/rpc:i")
#position = (0.2, 0.2, 0.2, 0.2, 0.2, 0.2 )
#output_port.write(position)
yarp.Network.init() # Initialise YARP
_rpc_client = yarp.RpcClient()
#_rpc_client.open("/icubSim/head/rpc:i")
_rpc_client.addOutput("/icubSim/head/rpc:i")
bottle = yarp.Bottle()
result = yarp.Bottle()
bottle.clear()
bottle.addString("set")
bottle.addString("pos")
bottle.addInt(1) #Specifies the head Joint
bottle.addInt(15) # Specifies the Joint angle
print("Sending", bottle.toString())
_rpc_client.write(bottle, result)
print("Return", result.toString())
def configure(self, rf):
# # --------------------------------Testing---------------------------------
# self.list_of_actions = ["left_arm_gun_pt1", "fast_long_wave", "left_arm_outstretched",
# "hand_up_5_fingers_palm_outward", "left_arm_kill", "left_arm_outstretched_you_got_it",
# "hand_up_5_fingers_palm_inward", "left_arm_outstretched_thumbs_up", "left_arm_kill"]
# self.num_actions = len(self.list_of_actions)
#
# self.list_of_emotions = ["neutral", "talking", "happy", "sad", "surprised", "evil", "angry", "shy", "cunning"]
# self.num_emotions = len(self.list_of_emotions)
#
# for n in self.list_of_actions:
# self.prepared_action_dict[n] = self.prepare_movement(n)
#
# r = self.get_chatbot_reply("This is a sentence")
# self.parse_chatbot_reply(r)
# # ------------------------------------------------------------------------
self.withProactive = rf.find(
'withProactive').toString_c().lower() == "true"
# Open BBC rpc port
self.portsList["rpc"] = yarp.Port()
self.portsList["rpc"].open("/BBC_Demo/rpc:i")
self.attach(self.portsList["rpc"])
yarp.Network.connect("/sentence_tokenizer/audio:o",
self.portsList["rpc"].getName())
# yarp.Network.connect("/deepSpeechToText/text:o", self.portsList["rpc"].getName())
# Open Body control port
self.portsList["body_control"] = yarp.RpcClient()
self.portsList["body_control"].open("/BBC_Demo/body_control/rpc:o")
yarp.Network.connect(self.portsList["body_control"].getName(),
"/body_control/rpc:i")
# Open speech dev control port
self.portsList["speech_control"] = yarp.RpcClient()
self.portsList["speech_control"].open("/BBC_Demo/speech_control/rpc:o")
yarp.Network.connect(self.portsList["speech_control"].getName(),
"/icub/speech:rpc")
# Open tokenizer control port
self.portsList["tokenizer_control"] = yarp.RpcClient()
self.portsList["tokenizer_control"].open(
"/BBC_Demo/sentence_tokenizer/rpc:o")
yarp.Network.connect(self.portsList["tokenizer_control"].getName(),
"/sentence_tokenizer/rpc:i")
self.list_of_actions = [
"left_arm_gun_pt1", "fast_long_wave", "left_arm_outstretched",
"hand_up_5_fingers_palm_outward", "left_arm_kill",
"left_arm_outstretched_you_got_it",
"hand_up_5_fingers_palm_inward", "left_arm_outstretched_thumbs_up",
"left_arm_kill"
]
self.num_actions = len(self.list_of_actions)
self.list_of_emotions = [
"neutral", "talking", "happy", "sad", "surprised", "evil", "angry",
"shy", "cunning"
]
self.num_emotions = len(self.list_of_emotions)
rep = yarp.Bottle()
for n in self.list_of_actions:
rep.clear()
self.prepared_action_dict[n] = self.prepare_movement(n)
cmd = self.prepare_movement(n, duration=True)
self.portsList["body_control"].write(cmd, rep)
self.duration_action_dict[n] = rep.get(1).asDouble()
if self.withProactive:
self.portsList["toHomeo"] = yarp.Port()
self.portsList["toHomeo"].open(self.homeoPortName)
yarp.Network.connect(self.homeoPortName, self.homeoRPC)
# Setup icub client
self.iCub = icubclient.ICubClient("BBC_Demo", "icubClient",
"BBC_demo.ini")
if not self.iCub.connect():
print "iCub not connected"
# return False
if not self.iCub.getSpeechClient():
print "Speech not connected"
# return False
else:
self.speech_client = self.iCub.getSpeechClient()
self.speech_client.Connect()
if not self.iCub.getEmotionClient():
print "Emotion not connected"
# return False
else:
self.emotion_client = self.iCub.getEmotionClient()
self.emotion_client.Connect()
# if not self.iCub.getSAMClient():
# print "SAM not connected"
# return False
# else:
# self.sam_client = self.iCub.getSAMClient()
# self.sam_client.Connect()
# Setting up speech if available
# Setting up Rasa Grammar
if self.grammar_mode == "rasa":
search_dir = join(self.rasa_root_dir, self.rasa_model_dir)
model_dirs = [
join(search_dir, d) for d in os.listdir(search_dir)
if os.path.isdir(join(search_dir, d))
]
self.rasa_model_dir = max(model_dirs, key=os.path.getmtime)
self.rasa_config = RasaNLUConfig(
join(self.rasa_root_dir, self.rasa_config_file))
self.rasa_interpreter = Interpreter.load(
join(self.rasa_root_dir, self.rasa_model_dir),
self.rasa_config)
else:
with open(self.TalkML_grammar_file) as f:
content = f.readlines()
# you may also want to remove whitespace characters like `\n` at the end of each line
content = [x.strip() for x in content]
for j in content:
parts = j.split('.*')
key = parts[0].replace('\t', '')
vals = parts[1][1:-1].split('|')
for v in vals:
if "*" in v:
try:
self.grammars_mult[key].append(v.split("*"))
except:
self.grammars_mult[key] = [v.split("*")]
else:
try:
self.grammars[key].append(v)
except:
self.grammars[key] = [v]
# Setting up chat interface
if self.chat_interface == "TalkML":
# Reading in tkml file
with open(self.TalkML_tkml_file, "r") as tkml_file:
self.tkml_def = tkml_file.read().replace("\n", " ")
# Setting up upload request body
startup_request = \
{"version": "1.0",
"action": self.TKML_Actions.upload.value,
"tkml": self.tkml_def}
# Sending startup request
startup_reply = self.TalkML_Send(startup_request)
if str(startup_reply) == "<Response [200]>":
print "Upload successful"
# Sending start
start_request = \
{
"action": "start",
"version": "1.0"
}
start_reply = self.TalkML_Send(start_request)
self.TalkML_enabled = self.parse_chatbot_reply(start_reply)
if self.TalkML_enabled:
print "Start successful"
else:
print "TalkML reply: ", start_reply
print "Start unsuccessful"
else:
print "TalkML reply: ", startup_reply
if startup_reply is not None:
print startup_reply._content
print "TalkML upload unsuccessful"
return False
elif self.chat_interface == "testing":
r, _ = self.get_chatbot_reply("Hello")
self.parse_chatbot_reply(r)
return True
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
yarp.Network.init()
yarp.Network.setLocalMode(True)
# Create (Controlboard + environment + OpenraveYarpPluginLoader + viewer)
controlboardOptions = yarp.Property()
controlboardOptions.put('device', 'YarpOpenraveControlboard')
controlboardOptions.put('robotIndex', 0)
controlboardOptions.put('manipulatorIndex', 0)
controlboardOptions.put('env', 'data/testwamcamera.env.xml')
controlboardOptions.put('orPlugin', 'OpenraveYarpPluginLoader')
controlboardOptions.put('view', 1)
controlboardDevice = yarp.PolyDriver(controlboardOptions)
# Connect to OpenraveYarpPluginLoader and obtain environment pointer (penv)
rpcClient = yarp.RpcClient()
rpcClient.open('/OpenraveYarpPluginLoader/rpc:c')
rpcClient.addOutput('/OpenraveYarpPluginLoader/rpc:s')
cmd = yarp.Bottle()
res = yarp.Bottle()
cmd.addString('getPenv')
rpcClient.write(cmd, res)
penvValue = res.get(0) # penvValue.isBlob()
print(penvValue.toString())
# Create Grabber using penv
grabberOptions = yarp.Property()
grabberOptions.put('penv', penvValue)
grabberOptions.put('device', 'YarpOpenraveGrabber')
grabberOptions.put('robotIndex', 0)
grabberOptions.put('sensorIndex', 0)
#!/usr/bin/python3
# Copyright
import yarp
import TaskPlanning
yarp.Network.init()
p = yarp.RpcClient()
p.open("/test")
yarp.Network.connect(p.getName(), "/textTaskPlanningServer")
tp = TaskPlanning.TaskPlanning()
tp.yarp().attachAsClient(p)
print(tp.help())
print(tp.help("stepTo"))
print(tp.help("grasp"))
print(tp.help("place"))
print(tp.help("help"))
tp.stepTo(1.0, 2.0, 3.0)
tp.grasp(1)
tp.place(2, 3)
def loadModel(self, reply, command):
parser = SafeConfigParser()
if command.size() != 2:
reply.addString("Model name required. e.g. load Actions")
elif command.get(1).asString() in self.trainingListHandles:
reply.addString("Cannot load model. Model in training")
elif command.get(1).asString() in self.noModelsNames:
reply.addString(
"Cannot load model. Model training available but not yet trained."
)
elif command.get(1).asString(
) in self.uptodateModelsNames + self.updateModelsNames:
ret = parser.read(
join(self.dataPath,
command.get(1).asString(), "config.ini"))
if len(ret) > 0:
# OLD
# if(parser.has_option('model_options', 'interaction')):
# interactionFunction = parser.get('model_options', 'interaction').split(',')
# NEW
if parser.has_option('model_options',
'driver') and parser.has_option(
'model_options', 'modelNameBase'):
interactionFunction = parser.get('model_options',
'driver').split(',')
modelNameBase = parser.get('model_options',
'modelNameBase')
interactionFunction = [
s for s in interactionFunction
for g in self.functionsList if s == g
]
if len(interactionFunction) != 0:
j = [
s for s in self.trainableModels
if s[0] == command.get(1).asString()
][0]
interfacePortName = self.interactionParser.get(
j[0], 'rpcBase') + ':o'
callSignList = self.interactionParser.get(
j[0], 'callSign').replace(' ', '').split(',')
# check if the model is already loaded
alreadyOpen = False
conn = -1
for k in range(len(self.rpcConnections)):
if self.rpcConnections[k][0] == j[0]:
alreadyOpen = True
conn = k
print "Loading ", interfacePortName, " with ", callSignList
if alreadyOpen:
if self.verbose:
print "Model already open"
# check it is functioning correctly
correctOperation = True if self.rpcConnections[
conn][1].getOutputCount() > 0 else False
if self.verbose:
print "correct operation = ", correctOperation
print
else:
if self.verbose:
print "Model not open"
if alreadyOpen and correctOperation:
rep = yarp.Bottle()
cmd = yarp.Bottle()
cmd.addString("reload")
# print self.rpcConnections[conn][0], 'reload'
self.rpcConnections[conn][1].write(cmd, rep)
if rep.get(0).asString() == 'ack':
reply.addString(
command.get(1).asString() +
" model re-loaded correctly")
else:
reply.addString(
command.get(1).asString() +
" model did not re-loaded correctly")
elif alreadyOpen and not correctOperation:
# terminate model by finding process in self.rpcConnections[4]
alreadyOpen = False
rep = yarp.Bottle()
cmd = yarp.Bottle()
cmd.addString("close")
cmd.addString(command.get(1).asString())
self.closeModel(rep, cmd)
reply.addString(
command.get(1).asString() +
" model terminated ")
if not alreadyOpen:
interfacePort = yarp.RpcClient()
interfacePort.open(interfacePortName)
# OLD
# args = ' '.join([join(self.dataPath,j[0]), join(self.modelPath, j[4]),
# self.interactionConfFile])
# cmd = 'ipython ' + join(self.trainingFunctionsPath, interactionFunction[0]+'.py') + \
# ' -- ' + args
# NEW
args = ' '.join([
join(self.dataPath, j[0]),
join(self.modelPath,
j[4]), self.interactionConfFile,
interactionFunction[0]
])
cmd = 'interactionSAMModel.py' + ' -- ' + args
if self.verbose:
print
print "cmd = ", cmd
print
if self.persistence:
command = "bash -c \"" + cmd + "; exec bash\""
else:
command = "bash -c \"" + cmd + "\""
if self.windowed:
c = subprocess.Popen(['xterm', '-e', command],
shell=False)
else:
c = subprocess.Popen([cmd],
shell=True,
stdout=self.devnull,
stderr=self.devnull)
self.rpcConnections.append([
j[0], interfacePort, interfacePortName[:-1],
callSignList, c
])
# pause here
noConn = True
iters = 0
if self.verbose:
print 'connecting ' + self.rpcConnections[-1][2]+'o' + \
' with ' + self.rpcConnections[-1][2]+'i'
while noConn:
noConn = yarp.Network.connect(
self.rpcConnections[-1][2] + 'o',
self.rpcConnections[-1][2] + 'i')
noConn = not noConn
time.sleep(1)
iters += 1
if iters >= 20:
break
if noConn:
reply.addString("Failure to load " +
str(interactionFunction[0]) +
" model")
rep = yarp.Bottle()
cmd = yarp.Bottle()
cmd.addString("close")
cmd.addString(self.rpcConnections[-1][0])
self.closeModel(rep, cmd)
else:
# then execute an interaction model check to verify correct startup
reply.addString(
str(interactionFunction[0]) +
" model loaded at " + interfacePortName +
" with call signs " + str(callSignList))
else:
reply.addString('No interaction function found in ' +
command.get(1).asString() +
' model path. Skipping model')
else:
reply.addString(
'Parameters "driver" and "modelBaseName" not found in config.ini'
)
else:
reply.addString("Failed to retrieve " +
command.get(1).asString() +
" model. Model not trained")
else:
reply.addString(
command.get(1).asString() + " model does not exist")
del parser
def configure(self, rf):
persistence_val = rf.find('persistence').toString_c().lower()
windowed_val = rf.find('windowed').toString_c().lower()
robot_name_val = rf.find('robot').toString_c()
if persistence_val != '':
self.persistence = persistence_val == 'true'
if windowed_val != '':
self.windowed = windowed_val == 'true'
if robot_name_val != '':
self.robot_name = robot_name_val
# Setting up rpc port
self.portsList['rpc'] = yarp.Port()
self.portsList['rpc'].open("/body_control/rpc:i")
self.attach(self.portsList['rpc'])
for part in self.parts:
# Assemble command
cmd_args = [
'ctpService', '--robot', self.robot_name, '--part', part
]
# Start ctpService process and store pid
ret = self.start_process(' '.join(cmd_args))
# Connect to ctpService process
if ret is not None:
self.ctp_processes.append(ret)
portName_control = "/body_control/" + part + "/rpc:o"
self.controlPorts[part] = yarp.RpcClient()
self.controlPorts[part].open(portName_control)
self.controlBottles[part] = yarp.Bottle()
portName_monitor = "/body_control/" + part + "/monitor:i"
self.monitorPorts[part] = yarp.BufferedPortBottle()
self.monitorPorts[part].open(portName_monitor)
print "/" + join(self.robot_name, part, "state:o"), join(
"/ctpservice", part, "rpc")
time.sleep(0.5)
yarp.Network.connect(portName_control,
join("/ctpservice", part, "rpc"))
yarp.Network.connect(
"/" + join(self.robot_name, part, "state:o"),
portName_monitor, "udp")
else:
print "Error setting up ctpServices"
return False
# Load gestures from xml files into dictionary
self.load_gestures()
print "loaded gestures"
# te = yarp.Bottle()
# te.addString("move")
# te.addString("left_arm_kill")
# te.addString("mirror")
# te.addString("delay=1.8")
#
# self.do_action("left_arm_kill", args=te)
return True
def configure(self, rf):
# command format will be the following:
# trainPGClassifier selfName networkStructure
print sys.argv
# read network structure and make graph
# labels in networkStructure identical to model names
# networkStructure as a string containing a list of tuples
# selfName = 'actionPGN'
# netStructureString = "[('Actions3 exp','actionPGN'), ('Actions4','actionPGN')]"
selfName = sys.argv[1]
netStructureString = sys.argv[2]
netStructure = ast.literal_eval(netStructureString)
print netStructure
# collect all model names in a list to extract a unique set
modelList = []
for k in netStructure:
modelList += list(k)
print list(set(modelList))
# create a port to connect to /sam/rpc:i to query model path for each model name
portsList = []
querySupervisorPort = yarp.RpcClient()
querySupervisorPortName = '/sam/' + selfName + '/queryRpc'
querySupervisorPort.open(querySupervisorPortName)
portsList.append({
'name': querySupervisorPortName,
'port': querySupervisorPort
})
yarp.Network.connect(querySupervisorPortName, '/sam/rpc:i')
# ---------------------------------------------------------------------------------------------------------------
modelDict = dict()
failFlag = False
for j in modelList:
if j != selfName:
modNameSplit = j.split(' ')
cmd = yarp.Bottle()
cmd.addString('dataDir')
for l in modNameSplit:
cmd.addString(l)
reply = yarp.Bottle()
querySupervisorPort.write(cmd, reply)
if reply.get(0).asString() != 'nack':
modelDict[modNameSplit[0]] = {
'filename': reply.get(1).asString(),
'pickleData': None
}
# try:
# load pickle for the model file
currPickle = pickle.load(
open(reply.get(1).asString(), 'rb'))
# try loading labelComparisonDict from the pickle
if 'labelComparisonDict' in currPickle.keys():
modelDict[modNameSplit[0]]['pickleData'] = currPickle[
'labelComparisonDict']
print j, 'labelComparisonDict loaded'
else:
print modNameSplit[0], 'labelComparisonDict not found'
failFlag = True
if 'overallPerformanceLabels' in currPickle.keys():
modelDict[modNameSplit[0]]['labels'] = currPickle[
'overallPerformanceLabels']
print j, 'overallPerformanceLabels loaded'
else:
print j, 'overallPerformanceLabels not found'
failFlag = True
# except:
# failFlag = True
else:
failFlag = True
print 'FAIL?', failFlag
if failFlag:
return False
modelList = modelDict.keys()
print modelList
# ---------------------------------------------------------------------------------------------------------------
# extract unique lists from the collected data
# the unique list of pickleData[original] represents the possibleClassifications for each model
modelDict[selfName] = dict()
modelDict[selfName]['labels'] = []
selfModelCol = 1
for j in modelList:
modelDict[j]['CPD'] = np.zeros([1, len(modelDict[j]['labels'])])
print j, 'unique labels:', modelDict[j]['labels']
print j, 'CPD shape', modelDict[j]['CPD'].shape
modelDict[selfName]['labels'] += modelDict[j]['labels']
selfModelCol *= len(modelDict[j]['labels'])
print
# the possibleClassifications for both models (outputs of the PGN)
# are the unique list of the model specific labels for all models
modelDict[selfName]['labels'] = list(set(
modelDict[selfName]['labels']))
modelDict[selfName]['actualLabels'] = modelDict[j]['pickleData'][
'original']
modelDict[selfName]['CPD'] = np.zeros(
[len(modelDict[selfName]['labels']), selfModelCol])
print selfName, 'unique labels:', modelDict[selfName]['labels']
print selfName, 'CPD shape', modelDict[selfName]['CPD'].shape
# check that original classifications of both are identical
# otherwise cannot combine them with a single node.
# This is currently a big limitation that will be removed later
print modelDict[selfName]['labels']
for j in modelList:
print j,
for k in range(len(modelDict[j]['pickleData']['original'])):
print modelDict[j]['pickleData']['original'][k]
if modelDict[j]['pickleData']['original'][k] not in modelDict[
selfName]['labels']:
modelDict[j]['pickleData']['original'][k] = 'unknown'
for j in modelList:
if modelDict[j]['pickleData']['original'] != modelDict[selfName][
'actualLabels']:
failFlag = True
print 'original classifications of', j, 'are not identical to those of', selfName
if failFlag:
return False
# Update netStructureString to reflect changes in the modelList names
strSections = netStructureString.split("'")
for k in range(len(strSections)):
if len(strSections[k]) > 2 and ',' not in strSections[k]:
strSections[k] = strSections[k].split(' ')[0]
netStructureString = "'".join(strSections)
netStructure = ast.literal_eval(netStructureString)
# ---------------------------------------------------------------------------------------------------------------
# iterate through actual labels
# for each actual label, iterate through models
# for each model find classification label of this model for current actual label
# get the index of the current classification and add it to its CPD
# also calculate which item in the joint CPD needs to be incremented
for j in range(len(modelDict[selfName]['actualLabels'])):
currActualLabel = modelDict[selfName]['actualLabels'][j]
row = modelDict[selfName]['labels'].index(currActualLabel)
colVar = np.zeros([len(modelList)])
for k in range(len(modelList)):
cmod = modelList[k]
if k != 0:
pmod = modelList[k - 1]
colVar *= len(modelDict[pmod]['labels'])
colVar[k] = modelDict[cmod]['labels'].index(
modelDict[cmod]['pickleData']['results'][j])
modelDict[cmod]['CPD'][0, colVar[k]] += 1
col = sum(colVar)
modelDict[selfName]['CPD'][row, col] += 1
# take all CPD's and normalise the matrices
evidenceCard = copy.deepcopy(modelList)
for j in modelDict:
if j == selfName:
# this is a joint CPD matrix
# normalise columns to have sum = 1
modelDict[j]['CPD'] = normalize(modelDict[j]['CPD'],
axis=0,
norm='l1')
else:
# normalise sum of matrix = 1
modelDict[j]['CPD'] /= np.sum(modelDict[j]['CPD'])
evidenceCard[evidenceCard.index(j)] = len(
modelDict[j]['labels'])
print modelDict[j]['CPD']
model = BayesianModel(netStructure)
# create TabularCPD data structure to nest calculated CPD
for j in modelDict:
if j == selfName:
modelDict[j]['cpdObject'] = TabularCPD(
variable=j,
variable_card=len(modelDict[j]['labels']),
values=modelDict[j]['CPD'],
evidence=modelList,
evidence_card=evidenceCard)
else:
modelDict[j]['cpdObject'] = TabularCPD(
variable=j,
variable_card=len(modelDict[j]['labels']),
values=modelDict[j]['CPD'])
# Associating the CPDs with the network
for j in modelDict:
model.add_cpds(modelDict[j]['cpdObject'])
# check_model checks for the network structure and CPDs and verifies that the CPDs are correctly
# defined and sum to 1.
if not model.check_model():
print 'Model check returned unsuccessful'
return False
infer = VariableElimination(model)
confMatrix = np.zeros(len(modelDict[selfName]['labels']))
# iterate over all original data and perform classifications to calculate if accuracy with PGN has increased
for j in range(len(modelDict[selfName]['actualLabels'])):
currEvidenceDict = dict()
for k in modelList:
currEvidenceDict[k] = modelDict[k]['labels'].index(
modelDict[k]['pickleData']['results'][j])
q = infer.query([selfName], currEvidenceDict)
inferenceClass = modelDict[selfName]['labels'][np.argmax(
q[selfName].values)]
actualClass = modelDict[selfName]['actualLabels'][j]
confMatrix[modelDict[selfName].index(actualClass),
modelDict[selfName].index(inferenceClass)] += 1
print "%Accuracy with PGN"
dCalc = SAMTesting.calculateData(modelDict[selfName]['actualLabels'],
confMatrix)
return True
