def __init__( self, n_bins=20, sensor_range=10., sensor_span=math.pi, maze_id=0, length=1, maze_height=0.5, maze_size_scaling=4, coef_inner_rew=0., # a coef of 0 gives no reward to the maze from the wrapped env. goal_rew=1000., # reward obtained when reaching the goal death_reward=0., random_start=False, direct_goal=False, velocity_field=True, visualize_goal=False, *args, **kwargs): Serializable.quick_init(self, locals()) self._n_bins = n_bins self._sensor_range = sensor_range self._sensor_span = sensor_span self._maze_id = maze_id self.length = length self.coef_inner_rew = coef_inner_rew self.goal_rew = goal_rew self.death_reward = death_reward self.direct_goal = direct_goal self.velocity_field = velocity_field self.algo = None # will be added in set_algo! self.visualize_goal = visualize_goal model_cls = self.__class__.MODEL_CLASS # print("model_cls", model_cls) if model_cls is None: raise "MODEL_CLASS unspecified!" xml_path = osp.join(MODEL_DIR, model_cls.FILE) # print("xml_path", xml_path) self.tree = tree = ET.parse(xml_path) self.worldbody = worldbody = tree.find(".//worldbody") self.MAZE_HEIGHT = height = maze_height self.MAZE_SIZE_SCALING = size_scaling = maze_size_scaling self.init_actual_x = self.init_actual_y = 0 self.random_start = random_start if self.random_start: # in kwargs # define: actual coordinates: origin at center of struct (3, 1) # define: mujoco coordinates: origin at the first ant self.pool, self.p, structure, x_relative, y_relative \ = construct_maze_random(maze_id=self._maze_id, length=self.length) # x, y_relative: x, y index in struct, relative to (3, 1) self.MAZE_STRUCTURE = structure self.init_actual_x = y_relative * self.MAZE_SIZE_SCALING # map x direction is list y direction! self.init_actual_y = x_relative * self.MAZE_SIZE_SCALING # print('self init:', self.init_actual_x, self.init_actual_y) self.x_r_prev, self.y_r_prev = x_relative + 3, y_relative + 1 # maintain the previous map for later update for i, tmp_line in enumerate(structure): for j, tmp_member in enumerate(tmp_line): if tmp_member == 'g': self.x_g_prev, self.y_g_prev = i, j break # self.x_g_prev, self.y_g_prev = 3, 3 # hard code here! - (1, j) in big maze structure in maze_env_utils.py self.x_g, self.y_g = self.x_g_prev, self.y_g_prev else: self.x_r_prev, self.y_r_prev = 3, 1 self.x_g_prev, self.y_g_prev = 1, 1 self.MAZE_STRUCTURE = structure = construct_maze( maze_id=self._maze_id, length=self.length) self.tot = 0 torso_x, torso_y = self._find_robot() self._init_torso_x = torso_x # this is not the actual init x, just convenient for goal pos computing self._init_torso_y = torso_y # torso pos in index coords! self.init_torso_x = self._init_torso_x # make visible from outside self.init_torso_y = self._init_torso_y for i in range(len(structure)): for j in range(len(structure[0])): if str(structure[i][j]) == '1': # offset all coordinates so that robot starts at the origin ET.SubElement( worldbody, "geom", name="block_%d_%d" % (i, j), pos="%f %f %f" % (j * size_scaling - torso_x, i * size_scaling - torso_y, height / 2 * size_scaling), size="%f %f %f" % (0.5 * size_scaling, 0.5 * size_scaling, height / 2 * size_scaling), type="box", material="", contype="1", conaffinity="1", rgba="0.4 0.4 0.4 1") if self.visualize_goal and str( structure[i][j] ) == 'g': # visualize goal! uncomment this block when testing! # offset all coordinates so that robot starts at the origin self.goal_element =\ ET.SubElement( self.worldbody, "geom", name="block_%d_%d" % (i, j), pos="%f %f %f" % (j * size_scaling - self._init_torso_x, i * size_scaling - self._init_torso_y, self.MAZE_HEIGHT / 2 * size_scaling), size="%f %f %f" % (0.2 * size_scaling, 0.2 * size_scaling, self.MAZE_HEIGHT / 2 * size_scaling), type="box", material="", contype="1", conaffinity="1", rgba="1.0 0.0 0.0 0.5" ) self.args = args self.kwargs = kwargs torso = tree.find(".//body[@name='torso']") geoms = torso.findall(".//geom") for geom in geoms: # print("geom", geom.attrib) if 'name' not in geom.attrib: raise Exception("Every geom of the torso must have a name " "defined") if self.__class__.MAZE_MAKE_CONTACTS: contact = ET.SubElement(tree.find("."), "contact") for i in range(len(structure)): for j in range(len(structure[0])): if str(structure[i][j]) == '1': for geom in geoms: ET.SubElement(contact, "pair", geom1=geom.attrib["name"], geom2="block_%d_%d" % (i, j)) _, file_path = tempfile.mkstemp(text=True) tree.write( file_path ) # here we write a temporal file with the robot specifications. Why not the original one?? self._goal_range = self._find_goal_range() self.goal = np.array([(self._goal_range[0] + self._goal_range[1]) / 2, (self._goal_range[2] + self._goal_range[3]) / 2]) # print ("goal_range", self._goal_range) # print("x_y", self.wrapped_env.model.data.qpos.flat[0:2]) # print("goal", self.goal) self._cached_segments = None self.gradient_pool = [ (1, 0), (0.707, 0.707), (0, 1), (-0.707, 0.707), (-1, 0), (-0.707, -0.707), (0, -1), (0.707, -0.707) ] # added gradient pool for train_low_with_v_gradient inner_env = model_cls( *args, file_path=file_path, **kwargs) # file to the robot specifications; model_cls is AntEnv ProxyEnv.__init__( self, inner_env) # here is where the robot env will be initialized
def __init__( self, n_bins=20, sensor_range=10., sensor_span=math.pi, maze_id=0, length=1, maze_height=0.5, maze_size_scaling=2, coef_inner_rew=0., # a coef of 0 gives no reward to the maze from the wrapped env. goal_rew=1., # reward obtained when reaching the goal *args, **kwargs): Serializable.quick_init(self, locals()) Serializable.quick_init(self, locals()) self._n_bins = n_bins self._sensor_range = sensor_range self._sensor_span = sensor_span self._maze_id = maze_id self.length = length self.coef_inner_rew = coef_inner_rew self.goal_rew = goal_rew model_cls = self.__class__.MODEL_CLASS if model_cls is None: raise "MODEL_CLASS unspecified!" xml_path = osp.join(MODEL_DIR, model_cls.FILE) tree = ET.parse(xml_path) worldbody = tree.find(".//worldbody") self.MAZE_HEIGHT = height = maze_height self.MAZE_SIZE_SCALING = size_scaling = maze_size_scaling self.MAZE_STRUCTURE = structure = construct_maze(maze_id=self._maze_id, length=self.length) torso_x, torso_y = self._find_robot() self._init_torso_x = torso_x self._init_torso_y = torso_y for i in range(len(structure)): for j in range(len(structure[0])): if str(structure[i][j]) == '1': # offset all coordinates so that robot starts at the origin ET.SubElement( worldbody, "geom", name="block_%d_%d" % (i, j), pos="%f %f %f" % (j * size_scaling - torso_x, i * size_scaling - torso_y, height / 2 * size_scaling), size="%f %f %f" % (0.5 * size_scaling, 0.5 * size_scaling, height / 2 * size_scaling), type="box", material="", contype="1", conaffinity="1", rgba="0.4 0.4 0.4 1") torso = tree.find(".//body[@name='torso']") geoms = torso.findall(".//geom") for geom in geoms: if 'name' not in geom.attrib: raise Exception("Every geom of the torso must have a name " "defined") if self.__class__.MAZE_MAKE_CONTACTS: contact = ET.SubElement(tree.find("."), "contact") for i in range(len(structure)): for j in range(len(structure[0])): if str(structure[i][j]) == '1': for geom in geoms: ET.SubElement(contact, "pair", geom1=geom.attrib["name"], geom2="block_%d_%d" % (i, j)) _, file_path = tempfile.mkstemp(text=True) tree.write( file_path ) # here we write a temporal file with the robot specifications. Why not the original one?? self._goal_range = self._find_goal_range() self._cached_segments = None inner_env = model_cls(*args, file_path=file_path, **kwargs) # file to the robot specifications ProxyEnv.__init__( self, inner_env) # here is where the robot env will be initialized