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)
