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