class Mitsos():
# Webots-to-environment-agnostic
def __init__(self,max_steps=200,ACTIONS='DISCRETE'):
self.name = "Mitsos"
self.max_steps = max_steps
self.robot = Supervisor() # create the Robot instance
self.timestep = int(self.robot.getBasicTimeStep()) # get the time step of the current world.
# crash sensor
self.bumper = self.robot.getDeviceByIndex(36) #### <---------
self.bumper.enable(self.timestep)
# camera sensor
self.camera = self.robot.getDevice("camera")
self.camera.enable(self.timestep)
# ir sensors
IR_names = ["ps0", "ps1", "ps2", "ps3", "ps4", "ps5", "ps6", "ps7"]
self.InfraredSensors = [self.robot.getDevice(s) for s in IR_names]
for ir in self.InfraredSensors: ir.enable(self.timestep)
# wheels motors
motors = ["left wheel motor","right wheel motor"]
self.wheels = []
for i in range(len(motors)):
self.wheels.append(self.robot.getDevice(motors[i]))
self.wheels[i].setPosition(float('inf'))
self.wheels[i].setVelocity(0)
self.robot.step(self.timestep)
self.cam_shape = (self.camera.getWidth(),self.camera.getHeight())
self.sensors_shape = (14,)
self.stepCounter = 0
self.shaping = None
# Actions
self.ACTIONS = ACTIONS
self.RELATIVE_ROTATIONS = True
self.FIXED_ORIENTATIONS = False
# State
self.POSITION = True
self.IRSENSORS = True
self.CAMERA = False
if self.FIXED_ORIENTATIONS:
self.discrete_actions = [0,1,2,3]
if self.RELATIVE_ROTATIONS:
self.discrete_actions = [0,1,2]
if self.ACTIONS=='DISCRETE':
self.action_size = len(self.discrete_actions)
else:
self.action_size = 2
self.path = []
self.substeps = 10
self.n_obstacles = 25
self.d = 0.3
self.create_world()
self.map = DynamicMap(self.START[0],self.START[1],0.02)
def reset(self,reset_position=True,reset_world=True):
self.stepCounter = 0
if reset_world:
self.create_world()
self.map.reset(self.START[0],self.START[1],0.02)
self.path = [(self.START[0],self.START[1])]
else:
x,y,z = self.get_robot_position()
if D((x,y,z),self.GOAL) < 0.05:
rho = d
phi = random.random()*2*np.pi
xg,yg = pol2cart(rho,phi)
self.GOAL = xg,yg
if reset_position:
self.set_position(self.START[0],self.START[1],0.005)
self.set_orientation(np.random.choice([0,np.pi/2,np.pi,-np.pi/2]))
self.shaping = None
if self.ACTIONS=='DISCRETE':
state,_,_,_ = self.step(1)
else:
state,_,_,_ = self.step([1,0])
return state
def step(self,action):
[xg,yg,_] = self.GOAL
x,y,z = self.get_robot_position()
self.path.append((x,y))
self.map.visit(x,y)
if self.ACTIONS=='DISCRETE':
if self.FIXED_ORIENTATIONS:
# Take action
if action == 0:
a = 0
if action == 1:
a = 90
if action == 2:
a = 180
if action == 3:
a = -90
self.turn0(a)
self.set_wheels_speed(1,0)
elif self.RELATIVE_ROTATIONS:
if action == 0:
a = -45
if action == 1:
a = 0
if action == 2:
a = 45
self.turn(a)
self.set_wheels_speed(1,0)
elif self.ACTIONS=='CONTINUOUS':
u,w = action
self.set_wheels_speed(u,w)
camera_stack = np.zeros(shape=self.cam_shape+(4,))
sensor_data = []
position_data = []
sensor_data = list(self.read_ir())
for i in range(self.substeps):
self.robot.step(self.timestep)
x1,y1,z1 = self.get_robot_position()
collision = self.collision()
position_data = [x-xg,y-yg,x1-xg,y1-yg]
sensor_data += list(self.read_ir())
# rho0,phi0 = cart2pol(x-xg,y-yg)
# rho1,phi1 = cart2pol(x1-xg,y1-yg)
# state = [rho0,phi0,rho1,phi1]
state = []
if self.POSITION:
state += position_data
if self.IRSENSORS:
state += sensor_data
if self.CAMERA:
state = [camera_stack,state]
# REWARD
reward,done,self.shaping = reward_function(position_data,self.shaping,collision)
if reward == 100: print('goal')
if self.stepCounter >= self.max_steps:
done = True
if done:
vars = [self.path,self.GOAL,self.obstacles]
vars = np.array(vars,dtype=object)
f = open(episode_filename,'wb')
np.save(f,vars)
f.close()
self.stepCounter += 1
info = ''
return state,reward,done,info
def create_world(self):
mode = 1
if mode == 0:
self.GOAL = [0,0,0]
self.START = [0.2,0.2,0]
if mode == 1:
self.GOAL = [0,0,0]
a = random.random()*np.pi*2
x,y = pol2cart(self.d,a)
self.START = [x+self.GOAL[0],y+self.GOAL[1],0]
while(True):
try:
obs = self.robot.getFromDef('OBS')
obs.remove()
except:
break
self.set_obstacle_positions()
p = self.obstacles
for pos in p:
nodeString = self.get_object_proto(pos=pos)
root = self.robot.getRoot()
node = root.getField('children')
node.importMFNodeFromString(-1,nodeString)
def set_obstacle_positions(self):
n = self.n_obstacles
self.obstacles = []
while len(self.obstacles) < n:
# r = (random.random() + 0.25) / 1.25 # 0.2 < r < 0.8
# d = D(self.GOAL,self.START) * r
d = random.uniform(0.15,1)
a = random.random()*np.pi*2
x,y = pol2cart(d,a)
self.obstacles.append([x+self.GOAL[0],y+self.GOAL[1],0])
def render(self):
return
def read_ir(self):
ir_sensors = np.array([ i.getValue() for i in self.InfraredSensors])
max_ = 2500.0
for i in range(len(ir_sensors)):
if ir_sensors[i] < 0:
ir_sensors[i] = 0.0
elif ir_sensors[i] > max_:
ir_sensors[i] = 1.0
else:
ir_sensors[i] = ir_sensors[i] / max_
return ir_sensors
def read_camera(self):
image = np.uint8(self.camera.getImageArray())
gray = cv2.cvtColor(image, cv2.COLOR_BGR2GRAY)
grayN = gray / 255.0
return gray
def collision(self):
return bool(self.bumper.getValue())
def set_position(self,x,y,z):
#return
object = self.robot.getFromDef(self.name)
positionField = object.getField("translation")
Field.setSFVec3f(positionField,[y,z,x])
for _ in range(5): self.robot.step(self.timestep) # if not nan values in first iteration
def set_orientation(self,a):
a += np.pi
object = self.robot.getFromDef(self.name)
rotationField = object.getField("rotation") #object.getPosition()
Field.setSFRotation(rotationField,[0,1,0,a])
self.robot.step(self.timestep) # if not nan values in first iteration
def set_wheels_speed(self,u,w):
# u: velocity
# w: angular velocity
u1 = u + w
u2 = u - w
self.wheels[0].setVelocity(float(u1))
self.wheels[1].setVelocity(float(u2))
def turn(self,a):
phi = np.rad2deg(self.get_robot_rotation()[-1])
phi1 = phi + a
w=-2
w = int(w * np.sign(a))
self.set_wheels_speed(0,w)
while(np.abs(phi - phi1)%360 > 5):
self.robot.step(self.timestep)
phi = np.rad2deg(self.get_robot_rotation()[-1])
def turn0(self,a):
phi = np.rad2deg(self.get_robot_rotation()[-1])
w = 5
if phi - a > 180:
w = -w
self.set_wheels_speed(0,w)
while( np.abs(phi - a) >= 3 ):
self.robot.step(self.timestep)
phi = np.rad2deg(self.get_robot_rotation()[-1])
def get_robot_position(self):
object = self.robot.getFromDef(self.name)
y,z,x = object.getPosition()
return [x,y,z]
def get_robot_rotation(self):
object = self.robot.getFromDef(self.name)
rotationField = object.getField("rotation")
a=rotationField.getSFRotation()
return a
def rotation_to_goal(self,G,X1,X2):
(xg,yg),(x1,y1),(x2,y2) = G,X1,X2
if xg == x1:
theta1 = np.pi/2 + (yg<y1)*np.pi
else:
lambda1 = (yg-y1)/(xg-x1)
if xg > x1:
theta1 = np.arctan(lambda1)
else:
theta1 = np.arctan(lambda1) + np.pi
if x2 == x1:
theta2 = np.pi/2 + (y2<y1)*np.pi
else:
lambda2 = (y2-y1)/(x2-x1)
if x2 > x1:
theta2 = np.arctan(lambda2)
else:
theta2 = np.arctan(lambda2) + np.pi
theta = theta1 - theta2
return theta
def wait(self,timesteps):
for _ in range(timesteps):
self.robot.step(self.timestep)
return
def random_position(self):
x = (random.random()*2 - 1) * 0.95
y = (random.random()*2 - 1) * 0.95
z = 0
return [x,y,z]
def get_object_proto(self,object='',pos=[0,0,0]):
translation = str(pos[1])+' '+str(pos[2]+0.025)+' '+str(pos[0])
return "DEF OBS SolidBox { translation "+translation+" size 0.05 0.05 0.05}"
# # needs change
# objects = {'Chair':'0.2 ',
# 'Ball':'1.6 ',
# 'ComputerMouse':'1.4 ',
# 'OilBarrel':'0.17 ',
# 'Toilet':'0.13 ',
# 'SolidBox':''}
# if object=='':
# object = np.random.choice(list(objects.keys()))
# if object=='SolidBox':
# translation = str(pos[1])+' '+str(pos[2]+0.05)+' '+str(pos[0])
# return "DEF OBS SolidBox { translation "+translation+" size 0.05 0.05 0.05}"
# translation = str(pos[1])+' '+str(pos[2])+' '+str(pos[0])
# scale = objects[object]*3
# proto = "DEF OBS Solid { translation "+translation+" scale "+scale+" children [ "+object+" { } ]}"
# return proto
def store_path(self):
filename = 'paths'
keep_variables = [self.path,self.START,self.GOAL]
keep_variables = np.array(keep_variables,dtype=object)
f = open(filename,'a')
np.save(f,keep_variables)
f.close()
print("\x1b[2J\x1b[1;1H")
from controller import Supervisor
supervisor = Supervisor()
timeStep = int(4 * supervisor.getBasicTimeStep())
motors = []
fingers = []
for index in range(0, supervisor.getNumberOfDevices()):
device = supervisor.getDeviceByIndex(index)
#print(device.getName())
name = device.getName()
if 'sensor' in name:
continue
elif 'motor' in name:
device.setVelocity(1.0)
position_sensor = device.getPositionSensor()
position_sensor.enable(timeStep)
motors.append(device)
elif 'finger' in name:
device.setVelocity(1.0)
position_sensor = device.getPositionSensor()
position_sensor.enable(timeStep)
fingers.append(device)
#print(f'motors ({len(motors )}): {[motor.getName() for motor in motors]}')
#print(f'fingers ({len(fingers)}): {[finger.getName() for finger in fingers]}')
rad = 0.01745329251
