class SupervisorEmitterReceiver(SupervisorEnv): def __init__(self, emitter_name="emitter", receiver_name="receiver", time_step=None): super(SupervisorEmitterReceiver, self).__init__() self.supervisor = Supervisor() if time_step is None: self.timestep = int(self.supervisor.getBasicTimeStep()) else: self.timestep = time_step self.initialize_comms(emitter_name, receiver_name) def initialize_comms(self, emitter_name, receiver_name): self.emitter = self.supervisor.getEmitter(emitter_name) self.receiver = self.supervisor.getReceiver(receiver_name) self.receiver.enable(self.timestep) return self.emitter, self.receiver def step(self, action): self.supervisor.step(self.timestep) self.handle_emitter(action) return ( self.get_observations(), self.get_reward(action), self.is_done(), self.get_info(), ) @abstractmethod def handle_emitter(self, action): pass @abstractmethod def handle_receiver(self): pass def get_timestep(self): return self.timestep
# Starting scores # score0 = 0 # score1 = 0 # How long the game has been running for timeElapsed = 0 lastTime = -1 # Send message to robot window to perform setup supervisor.wwiSendText("startup") # For checking the first update with the game running first = True receiver = supervisor.getReceiver('receiver') receiver.enable(32) # Get the starting tile minimum node and translation starting_PointMin = supervisor.getFromDef("start0min") starting_minPos = starting_PointMin.getField("translation") # Get maximum node and translation starting_PointMax = supervisor.getFromDef("start0max") starting_maxPos = starting_PointMax.getField("translation") # Get the vector positons starting_minPos = starting_minPos.getSFVec3f() starting_maxPos = starting_maxPos.getSFVec3f() starting_centerPos = [(starting_maxPos[0]+starting_minPos[0])/2,starting_maxPos[1],(starting_maxPos[2]+starting_minPos[2])/2]
burger = robot.getFromDef('Burger') # Supervisor translationField = robot.getFromDef('ROBOT_POSITION') rotationField = burger.getField('rotation') groundtruth = np.zeros((1, 3)) burgerData = np.zeros((1, 4)) lidarData = np.zeros((1, 128)) freq = 0 keyboard = robot.getKeyboard() keyboard.enable(timestep) receiver = robot.getReceiver("receiver") receiver.enable(timestep) lastDist = 0 np.set_printoptions(suppress=True) while robot.step(timestep) != -1: if freq == 10: translation = translationField.getPosition() orientation = translationField.getOrientation() zOri = np.array([orientation[6], orientation[8]]) angle = -np.arctan2(zOri[0], zOri[1]) b = np.array([translation[2], translation[0], angle])
"""pyServer controller.""" from controller import Supervisor, Receiver TIME_STEP = 256 supervisor = Supervisor() receiver = supervisor.getReceiver("receiver") receiver.enable(TIME_STEP) receiver.setChannel(-1) while supervisor.step(TIME_STEP) != -1: print('in while loop') if (receiver.getQueueLength() > 0): incoming = receiver.getData() print('incoming') dataList = struct.unpack('d', incoming) print(dataList) receiver.nextPacket()
class SimpleWebotsPeripheralSystem(): def __init__(self,SOLIDS,emitterName,*args): if(type(SOLIDS[0])==tuple): self.SOLIDS = [i for (i,v) in SOLIDS] self.MASSES = [v for (i,v) in SOLIDS] else: self.SOLIDS = SOLIDS self.MASSES = np.zeros(len(self.SOLIDS)) self.supervisor = Robot() self.fast_mode = True self.modeSwitched = False self.time = 0 self.timestep = int(self.supervisor.getBasicTimeStep()) # Receiver is not mandatory, view it as local information from your spinal cord # that you want to send back to the brain self.receiver = None if len(args) > 1: self.client = SimpleRPCClient(args[1]) else: self.client = SimpleRPCClient() if len(args) > 0: self.receiver = self.supervisor.getReceiver(args[0]) if self.receiver != None: self.receiver.enable(self.timestep) # Emitter is mandatory, view it as the local information your spinal cord # needs self.emitter = self.supervisor.getEmitter(emitterName) def sendToPremotorCortex(self,obs): self.client.send(obs) def sendToSpinalCord(self,actions,**kwargs): if 'encrypt_with' in kwargs: if kwargs['encrypt_with'] == 'json': self.emitter.send(str.encode(json.dumps(actions))) return self.emitter.send(pickle.dumps(actions, protocol=0)) return def receiveFromSensoryCortex(self): extraInfo = [] if self.receiver != None: while self.receiver.getQueueLength() > 0: extraInfo = json.loads(self.receiver.getData()) # do something with the map self.receiver.nextPacket() return { 'extrainfo' : extraInfo, 'qpos' : self.getPosition(), 'qvel' : self.getVelocity(), 'com' : self.getCenterOfMass(), 'mass' : self.getMasses(), 'phase' : np.mod(2.0 * pi * 1.0 * self.time,2.0 * pi), # TODO put real phase, 'time' : self.time, 'timestep' : int(self.time/(self.timestep/1000.0)) } def getFromMotorCortex(self): msg = self.client.get() if "fast_mode" in msg: if msg["fast_mode"] != self.fast_mode or not self.modeSwitched: self.modeSwitched = True self.fast_mode = msg["fast_mode"] print("Fast mode switched to {} by an external source".format(self.fast_mode)) if(self.fast_mode): self.supervisor.simulationSetMode(3) else: self.supervisor.simulationSetMode(1) else: print("No fast_mode in msg from rl") if "reset_model" in msg : if msg["reset_model"] == True: self.supervisor.simulationQuit(1) time.sleep(10.0) else: print("No reset_model in msg from rl") if "act" in msg: return msg["act"] else: print("No action in msg from rl") return np.zeros(10) def getPosition(self): # for m in self.SOLIDS: # print "{}:{}".format(m,self.supervisor.getFromDef(m)) return([ self.supervisor.getFromDef(m).getPosition() for m in self.SOLIDS ]) def getVelocity(self): return([ self.supervisor.getFromDef(m).getVelocity() for m in self.SOLIDS ]) def getCenterOfMass(self): return([ self.supervisor.getFromDef(m).getCenterOfMass() for m in self.SOLIDS ]) def getMasses(self): return(self.MASSES) def step(self): self.time += self.timestep/1000.0 return self.supervisor.step(self.timestep)