def test_maze_env_utils(self): """Test hbaselines/envs/efficient_hrl/maze_env_utils.py.""" # test construct_maze for maze_id in ["Maze", "Push", "Fall", "Block", "BlockMaze", "FourRooms"]: construct_maze(maze_id) self.assertRaises(NotImplementedError, construct_maze, maze_id="error") # test point_distance p1 = (0, 0) p2 = (2, 2) self.assertAlmostEqual(point_distance(p1, p2), np.sqrt(2**2 + 2**2)) # test line_intersect p1 = (0, 0) p2 = (2, 2) p3 = (0, 2) p4 = (2, 0) x, y, *_ = line_intersect(p1, p2, p3, p4) self.assertAlmostEqual(x, 1) self.assertAlmostEqual(y, 1)
def __init__(self, maze_id=None, maze_height=0.5, maze_size_scaling=8, n_bins=0, sensor_range=3., sensor_span=2 * math.pi, observe_blocks=False, put_spin_near_agent=False, top_down_view=False, image_size=64, manual_collision=False, ant_fall=False, evaluate=False, num_levels=1, *args, **kwargs): """Instantiate the environment. Parameters ---------- maze_id : str the type of maze being simulated. Can be 'Maze', 'Push', 'Fall', or 'Block'. maze_height : float, optional height of the floor maze_size_scaling : float, optional scaling factor for the maze. Specifies the size of one block. n_bins : int, optional number of viable objects sensor_range : float, optional distance whereby objects can be perceived. Must be within the span as well. sensor_span : float, optional degrees of visibility observe_blocks : bool, optional whether to add the movable blocks to the observations put_spin_near_agent : bool, optional specifies whether the agent can spin blocks top_down_view : bool, optional if set to True, the top-down view is provided via the observations image_size: int determines the width and height of the rendered image manual_collision : bool, optional if set to True, collisions cause the agent to return to its prior position ant_fall : bool specifies whether you are using the AntFall environment. The agent in this environment is placed on a block of height 4; the "dying" conditions for the agent need to be accordingly offset. evaluate : bool whether to run an evaluation. In this case an additional goal agent is placed in the environment for visualization purposes. num_levels : int number of levels in the policy. 1 refers to non-hierarchical models """ self._maze_id = maze_id model_cls = self.__class__.MODEL_CLASS if model_cls is None: raise AssertionError("MODEL_CLASS unspecified!") if evaluate and num_levels == 2: xml_file = "ant.xml" elif evaluate and num_levels == 3: xml_file = "triple_ant.xml" else: xml_file = "ant.xml" xml_path = os.path.join(MODEL_DIR, xml_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._n_bins = n_bins self._sensor_range = sensor_range * size_scaling self._sensor_span = sensor_span self._observe_blocks = observe_blocks self._put_spin_near_agent = put_spin_near_agent self._top_down_view = top_down_view self._manual_collision = manual_collision self.MAZE_STRUCTURE = structure = maze_env_utils.construct_maze( maze_id=self._maze_id) # Elevate the maze to allow for falling. self.elevated = any(-1 in row for row in structure) # Are there any movable blocks? self.blocks = any( any(maze_env_utils.can_move(r) for r in row) for row in structure) torso_x, torso_y = self._find_robot() self._init_torso_x = torso_x self._init_torso_y = torso_y self._init_positions = [(x - torso_x, y - torso_y) for x, y in self._find_all_robots()] self._xy_to_rowcol = lambda x, y: ( 2 + (y + size_scaling / 2) / size_scaling, 2 + (x + size_scaling / 2) / size_scaling) # walls (immovable), chasms (fall), movable blocks self.image_size = image_size self._view = np.zeros([5, 5, 3]) height_offset = 0. if self.elevated: # Increase initial z-pos of ant. height_offset = height * size_scaling torso = tree.find(".//body[@name='torso']") torso.set('pos', '0 0 %.2f' % (0.75 + height_offset)) if self.blocks: # If there are movable blocks, change simulation settings to # perform better contact detection. default = tree.find(".//default") default.find('.//geom').set('solimp', '.995 .995 .01') self.movable_blocks = [] for i in range(len(structure)): for j in range(len(structure[0])): struct = structure[i][j] if struct == 'r' and self._put_spin_near_agent: struct = maze_env_utils.Move.SpinXY if self.elevated and struct not in [-1]: # Create elevated platform. ET.SubElement( worldbody, "geom", name="elevated_%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.9 0.9 0.9 1", ) if struct == 1: # Unmovable block. # 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_offset + 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 1".format(i / len(structure), j / len(structure[0])) if top_down_view else "0.4 0.4 0.4 1", ) elif maze_env_utils.can_move(struct): # Movable block. # The "falling" blocks are shrunk slightly and increased in # mass to ensure that it can fall easily through a gap in # the platform blocks. name = "movable_%d_%d" % (i, j) self.movable_blocks.append((name, struct)) falling = maze_env_utils.can_move_z(struct) spinning = maze_env_utils.can_spin(struct) x_offset = 0.25 * size_scaling if spinning else 0.0 y_offset = 0.0 shrink = 0.1 if spinning else 0.99 if falling else 1.0 height_shrink = 0.1 if spinning else 1.0 movable_body = ET.SubElement( worldbody, "body", name=name, pos="%f %f %f" % (j * size_scaling - torso_x + x_offset, i * size_scaling - torso_y + y_offset, height_offset + height / 2 * size_scaling * height_shrink), ) ET.SubElement( movable_body, "geom", name="block_%d_%d" % (i, j), pos="0 0 0", size="%f %f %f" % (0.5 * size_scaling * shrink, 0.5 * size_scaling * shrink, height / 2 * size_scaling * height_shrink), type="box", material="", mass="0.001" if falling else "0.0002", contype="1", conaffinity="1", rgba="0.9 0.1 0.1 1") if maze_env_utils.can_move_x(struct): ET.SubElement(movable_body, "joint", armature="0", axis="1 0 0", damping="0.0", limited="true" if falling else "false", range="%f %f" % (-size_scaling, size_scaling), margin="0.01", name="movable_x_%d_%d" % (i, j), pos="0 0 0", type="slide") if maze_env_utils.can_move_y(struct): ET.SubElement(movable_body, "joint", armature="0", axis="0 1 0", damping="0.0", limited="true" if falling else "false", range="%f %f" % (-size_scaling, size_scaling), margin="0.01", name="movable_y_%d_%d" % (i, j), pos="0 0 0", type="slide") if maze_env_utils.can_move_z(struct): ET.SubElement(movable_body, "joint", armature="0", axis="0 0 1", damping="0.0", limited="true", range="%f 0" % (-height_offset), margin="0.01", name="movable_z_%d_%d" % (i, j), pos="0 0 0", type="slide") if maze_env_utils.can_spin(struct): ET.SubElement(movable_body, "joint", armature="0", axis="0 0 1", damping="0.0", limited="false", name="spinable_%d_%d" % (i, j), pos="0 0 0", type="ball") 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") _, file_path = tempfile.mkstemp(text=True, suffix='.xml') tree.write(file_path) try: self.wrapped_env = model_cls(*args, ant_fall=ant_fall, top_down_view=top_down_view, file_path=file_path, **kwargs) except AssertionError: # for testing purposes pass
def __init__(self, maze_id=None, maze_height=0.5, maze_size_scaling=8, n_bins=0, sensor_range=3., sensor_span=2 * math.pi, observe_blocks=False, put_spin_near_agent=False, top_down_view=False, manual_collision=False, *args, **kwargs): """Instantiate the environment. Parameters ---------- maze_id : str TODO maze_height : float, optional TODO maze_size_scaling : float, optional TODO n_bins : int, optional TODO sensor_range : float, optional TODO sensor_span : float, optional TODO observe_blocks : bool, optional TODO put_spin_near_agent : bool, optional TODO top_down_view : bool, optional TODO manual_collision : bool, optional TODO """ self._maze_id = maze_id model_cls = self.__class__.MODEL_CLASS if model_cls is None: raise AssertionError("MODEL_CLASS unspecified!") xml_path = os.path.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._n_bins = n_bins self._sensor_range = sensor_range * size_scaling self._sensor_span = sensor_span self._observe_blocks = observe_blocks self._put_spin_near_agent = put_spin_near_agent self._top_down_view = top_down_view self._manual_collision = manual_collision self.MAZE_STRUCTURE = structure = maze_env_utils.construct_maze( maze_id=self._maze_id) # Elevate the maze to allow for falling. self.elevated = any(-1 in row for row in structure) # Are there any movable blocks? self.blocks = any( any(maze_env_utils.can_move(r) for r in row) for row in structure) torso_x, torso_y = self._find_robot() self._init_torso_x = torso_x self._init_torso_y = torso_y self._init_positions = [(x - torso_x, y - torso_y) for x, y in self._find_all_robots()] self._xy_to_rowcol = lambda x, y: ( 2 + (y + size_scaling / 2) / size_scaling, 2 + (x + size_scaling / 2) / size_scaling) # walls (immovable), chasms (fall), movable blocks self._view = np.zeros([5, 5, 3]) height_offset = 0. if self.elevated: # Increase initial z-pos of ant. height_offset = height * size_scaling torso = tree.find(".//body[@name='torso']") torso.set('pos', '0 0 %.2f' % (0.75 + height_offset)) if self.blocks: # If there are movable blocks, change simulation settings to # perform better contact detection. default = tree.find(".//default") default.find('.//geom').set('solimp', '.995 .995 .01') self.movable_blocks = [] for i in range(len(structure)): for j in range(len(structure[0])): struct = structure[i][j] if struct == 'r' and self._put_spin_near_agent: struct = maze_env_utils.Move.SpinXY if self.elevated and struct not in [-1]: # Create elevated platform. ET.SubElement( worldbody, "geom", name="elevated_%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.9 0.9 0.9 1", ) if struct == 1: # Unmovable block. # 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_offset + 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", ) elif maze_env_utils.can_move(struct): # Movable block. # The "falling" blocks are shrunk slightly and increased in # mass to ensure that it can fall easily through a gap in # the platform blocks. name = "movable_%d_%d" % (i, j) self.movable_blocks.append((name, struct)) falling = maze_env_utils.can_move_z(struct) spinning = maze_env_utils.can_spin(struct) x_offset = 0.25 * size_scaling if spinning else 0.0 y_offset = 0.0 shrink = 0.1 if spinning else 0.99 if falling else 1.0 height_shrink = 0.1 if spinning else 1.0 movable_body = ET.SubElement( worldbody, "body", name=name, pos="%f %f %f" % (j * size_scaling - torso_x + x_offset, i * size_scaling - torso_y + y_offset, height_offset + height / 2 * size_scaling * height_shrink), ) ET.SubElement( movable_body, "geom", name="block_%d_%d" % (i, j), pos="0 0 0", size="%f %f %f" % (0.5 * size_scaling * shrink, 0.5 * size_scaling * shrink, height / 2 * size_scaling * height_shrink), type="box", material="", mass="0.001" if falling else "0.0002", contype="1", conaffinity="1", rgba="0.9 0.1 0.1 1") if maze_env_utils.can_move_x(struct): ET.SubElement(movable_body, "joint", armature="0", axis="1 0 0", damping="0.0", limited="true" if falling else "false", range="%f %f" % (-size_scaling, size_scaling), margin="0.01", name="movable_x_%d_%d" % (i, j), pos="0 0 0", type="slide") if maze_env_utils.can_move_y(struct): ET.SubElement(movable_body, "joint", armature="0", axis="0 1 0", damping="0.0", limited="true" if falling else "false", range="%f %f" % (-size_scaling, size_scaling), margin="0.01", name="movable_y_%d_%d" % (i, j), pos="0 0 0", type="slide") if maze_env_utils.can_move_z(struct): ET.SubElement(movable_body, "joint", armature="0", axis="0 0 1", damping="0.0", limited="true", range="%f 0" % (-height_offset), margin="0.01", name="movable_z_%d_%d" % (i, j), pos="0 0 0", type="slide") if maze_env_utils.can_spin(struct): ET.SubElement(movable_body, "joint", armature="0", axis="0 0 1", damping="0.0", limited="false", name="spinable_%d_%d" % (i, j), pos="0 0 0", type="ball") 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") _, file_path = tempfile.mkstemp(text=True, suffix='.xml') tree.write(file_path) self.wrapped_env = model_cls(*args, file_path=file_path, **kwargs)