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
# Supervisor setup supervisor = Supervisor() TIMESTEP = int(supervisor.getBasicTimeStep()) COMM_CHANNEL = 1 # Supervisor interpret world soccerball = supervisor.getFromDef("BALL") trans_field = soccerball.getField("translation") ball_radius = 0.113 # soccerball.getField("radius") INITIAL_TRANS = [0, ball_radius, 0] # Emitter setup emitter = supervisor.getEmitter('emitter') emitter.setChannel(COMM_CHANNEL) tInitial = supervisor.getTime() while supervisor.step(TIMESTEP) != -1: values = trans_field.getSFVec3f() t = supervisor.getTime() # Emit ball position if (t - tInitial) >= 1: # print(t-tInitial) print("Ball is at position: %g %g %g" % (values[0], values[1], values[2])) message = struct.pack("ddd", values[0], values[1], values[2])
sys.stderr.write("Warning: 'robotbenchmark' module not found.\n") sys.exit(0) # Get random generator seed value from 'controllerArgs' field seed = 1 if len(sys.argv) > 1 and sys.argv[1].startswith('seed='): seed = int(sys.argv[1].split('=')[1]) robot = Supervisor() timestep = int(robot.getBasicTimeStep()) jointParameters = robot.getFromDef("PENDULUM_PARAMETERS") positionField = jointParameters.getField("position") emitter = robot.getEmitter("emitter") time = 0 force = 0 forceStep = 800 random.seed(seed) run = True while robot.step(timestep) != -1: if run: time = robot.getTime() robot.wwiSendText("time:%-24.3f" % time) robot.wwiSendText("force:%.2f" % force) # Detect status of inverted pendulum position = positionField.getSFFloat() if position < -1.58 or position > 1.58:
ls.getField('intensity').setSFFloat(GLOBALS.sim_light_intensity) # hyperuranium = demiurge.getFromDef(GLOBALS.webots_nodes_defs['WorldInfo']) # hyperuranium.getField('basicTimeStep').setSFFloat(GLOBALS.sim_timestep_ms) # print(hyperuranium.getField('basicTimeStep').getSFFloat()) except Exception as ex: print(ex) n_steps = 0 max_steps = int((GLOBALS.sim_run_time_s * 1000) / timestep) trigger_step = int((GLOBALS.sim_event_timer_s * 1000) / timestep) # step @ which the event should be triggered t_step = trigger_step # ------------------------------------------------- e = demiurge.getEmitter('righthand') signal = False # r_bool() # # First phototaxis, in order to have comparable results n_signal = 0 # Main loop: # - perform simulation steps until Webots is stopping the controller while demiurge.step(timestep) != -1 and n_steps != max_steps: # -------------------- PERFORM SIMULATION STEP ------------------------ if n_steps == t_step: # trigger event modifying the simulation world t_step += trigger_step if e is not None: if n_signal == 2: e.send(bytes(bin(-1), 'ASCII'))
ds = [] dsNames = ['ds_right', 'ds_left', 'ds_back_left', 'ds_back_right'] for i in range(4): ds.append(supervisor.getDistanceSensor(dsNames[i])) ds[i].enable(TIME_STEP) #initializing wheels wheels = [] wheelsNames = ['wheel1', 'wheel2', 'wheel3', 'wheel4'] for i in range(4): wheels.append(supervisor.getMotor(wheelsNames[i])) wheels[i].setPosition(float('inf')) wheels[i].setVelocity(0.0) avoidObstacleCounter = 0 #initializing communications fasheqi = supervisor.getEmitter('emitter') fasheqi.setChannel(1) fasheqi.setRange(-1) #initializing IMU imu = supervisor.getInertialUnit('imu_angle') imu.enable(TIME_STEP) count = 0 #imu stuff pI = 3.14159265 curr_angle = 0 #initialize GPS gps = supervisor.getGPS('dummy_gps') gps.enable(TIME_STEP) prev_data = [0, 0, 0]
"""pyExitEmitter controller.""" import sys from controller import Supervisor, Emitter import struct TIME_STEP = 256 supervisor = Supervisor() emitter = supervisor.getEmitter("exitEmitter") emitter.setChannel(2) emitter.setRange(0.1) counter = 0 noEntrance = struct.pack('is', 162, b'exit') #print("Before loop") while supervisor.step(TIME_STEP) != -1: # print("before the if") if (counter >= 16): emitter.send(noEntrance) counter = 0 # print(counter) else: counter += 1 # print(counter) #print("after loop")
"""my_supervisor controller.""" # You may need to import some classes of the controller module. Ex: # from controller import Robot, LED, DistanceSensor from controller import Supervisor, Node, Display # create the Robot instance. super = Supervisor() root = super.getRoot() emitter = super.getEmitter('emitter') # emitter.setChannel(emitter.CHANNEL_BROADCAST) print(super.getFromDef("e-puck")) names = ["robo_1", "robo_2"] robots = [super.getFromDef(name) for name in names] # get the time step of the current world. timestep = int(super.getBasicTimeStep()) print("Timestep:", timestep) # Main loop: # - perform simulation steps until Webots is stopping the controller while super.step(timestep) != -1: for _id, robot in enumerate(robots): pos = robot.getField("translation") orientation = robot.getField("rotation")
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)