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)
