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