def __init__(self, fname, idn=0): # Always run super init first super().__init__(fname, idn=idn) # key: gripper name and value: gripper model self.grippers = OrderedDict() # Grab hand's offset from final robot link (string -> np.array -> elements [1, 2, 3, 0] (x, y, z, w)) # Different case based on whether we're dealing with single or bimanual armed robot if self.arm_type == "single": hand_element = find_elements(root=self.root, tags="body", attribs={"name": self.eef_name}, return_first=True) self.hand_rotation_offset = string_to_array( hand_element.get("quat", "1 0 0 0"))[[1, 2, 3, 0]] else: # "bimanual" case self.hand_rotation_offset = {} for arm in ("right", "left"): hand_element = find_elements( root=self.root, tags="body", attribs={"name": self.eef_name[arm]}, return_first=True) self.hand_rotation_offset[arm] = string_to_array( hand_element.get("quat", "1 0 0 0"))[[1, 2, 3, 0]] # Get camera names for this robot self.cameras = self.get_element_names(self.worldbody, "camera")
def parse_cameras(self): """ Parse cameras and initialize the cameras. """ robot = MujocoRobot() for cam in self.xml_root.iter("camera"): camera_name = cam.get("name") # get parent body name to find out where the camera is attached. parent_body_name = self.parent_map[cam].get("name", "worldbody") pos = string_to_array(cam.get("pos", "0 0 0")) quat = string_to_array(cam.get("quat", "1 0 0 0")) fov = float(cam.get("fovy", "45")) quat = np.array([quat[1], quat[2], quat[3], quat[0]]) camera = MujocoCamera(parent_body_name, pos, quat, active=False, mujoco_env=self.env, camera_name=camera_name, fov=fov) robot.cameras.append(camera) self.renderer.add_robot([], [], [], [], None, 0, dynamic=False, robot=robot)
def merge_arena(self, mujoco_arena): """ Adds arena model to the MJCF model. Args: mujoco_arena (Arena): arena to merge into this MJCF model """ self.arena = mujoco_arena self.floor_pos = string_to_array(mujoco_arena.floor.get("pos")) self.floor_size = string_to_array(mujoco_arena.floor.get("size")) self.merge(mujoco_arena)
def parse_materials(self): """ Parse all materials and use texture mapping to initialize materials """ self.material_attributes = {} self.material_mapping = {} self.normal_id = get_texture_id( np.array([127, 127, 255]).reshape(1, 1, 3).astype(np.uint8), "normal", self) for material in self.xml_root.iter("material"): material_name = material.get("name") texture_name = material.get("texture") rgba = string_to_array(material.get("rgba", "0.5 0.5 0.5")) self.material_attributes[material_name] = material.attrib if texture_name is not None: texture_id, _ = self.texture_id_mapping[texture_name] specular = material.get("specular") shininess = material.get("shininess") roughness_id = -1 if specular is None else get_texture_id( 1 - float(specular), "roughness", self) metallic_id = -1 if shininess is None else get_texture_id( float(shininess), "metallic", self) if isinstance(texture_id, int): repeat = string_to_array(material.get("texrepeat", "1 1")) self.material_mapping[material_name] = Material( "texture", texture_id=texture_id, transform_param=[repeat[0], repeat[1], 0], metallic_texture_id=metallic_id, roughness_texture_id=roughness_id, normal_texture_id=self.normal_id, ) else: # texture id in this case will be a numpy array of rgb values # If color are present both in material and texture, prioritize material color. if material.get("rgba") is None: self.material_mapping[material_name] = Material( "color", kd=texture_id) else: self.material_mapping[material_name] = Material( "color", kd=rgba[:3]) else: # color can either come from texture, or could be defined in the material itself. self.material_mapping[material_name] = Material("color", kd=rgba[:3])
def table_top_abs(self): """ Grabs the absolute position of table top Returns: np.array: (x,y,z) table position """ return string_to_array(self.floor.get("pos")) + self.table_offset
def set_origin(self, offset): """Applies a constant offset to all objects.""" offset = np.array(offset) for node in self.worldbody.findall("./*[@pos]"): cur_pos = string_to_array(node.get("pos")) new_pos = cur_pos + offset node.set("pos", array_to_string(new_pos))
def _load_model(self): PandaEnv._load_model(self) self.mujoco_robot.set_base_xpos([0, 0, 0]) # load model for table top workspace self.mujoco_arena = BinsArenaNoWall( table_full_size=self.table_full_size, table_friction=self.table_friction) if self.use_indicator_object: self.mujoco_arena.add_pos_indicator() # The sawyer robot has a pedestal, we want to align it with the table self.mujoco_arena.set_origin([.5, -0.3, 0]) self.item_names_org = list(self.item_names) self.obj_to_use = (self.item_names[0] + "{}").format(0) lst = [] for i in range(len(self.ob_inits)): ob = self.ob_inits[i]() lst.append((str(self.item_names[i]) + "0", ob)) self.mujoco_objects = OrderedDict(lst) self.n_objects = len(self.mujoco_objects) # task includes arena, robot, and objects of interest self.model = PickPlaceTask(self.mujoco_arena, self.mujoco_robot, self.mujoco_objects, []) self.model.place_objects() self.model.place_visual() self.bin_pos = string_to_array(self.model.bin2_body.get("pos")) self.bin_size = self.model.bin_size
def place_visual(self): """Places visual objects randomly until no collisions or max iterations hit.""" index = 0 bin_pos = string_to_array(self.bin2_body.get("pos")) bin_size = self.bin_size for _, obj_mjcf in self.visual_objects: bin_x_low = bin_pos[0] bin_y_low = bin_pos[1] if index == 0 or index == 2: bin_x_low -= bin_size[0] / 2 if index < 2: bin_y_low -= bin_size[1] / 2 bin_x_high = bin_x_low + bin_size[0] / 2 bin_y_high = bin_y_low + bin_size[1] / 2 bottom_offset = obj_mjcf.get_bottom_offset() bin_range = [bin_x_low + bin_x_high, bin_y_low + bin_y_high, 2 * bin_pos[2]] bin_center = np.array(bin_range) / 2.0 pos = bin_center - bottom_offset self.visual_obj_mjcf[index].set("pos", array_to_string(pos)) index += 1
def __init__(self, fname, idn=0): super().__init__(fname) # Set id and add prefixes to all body names to prevent naming clashes self.idn = idn # Define other variables that get filled later self.mount = None # Parse element tree to get all relevant bodies, joints, actuators, and geom groups self._elements = sort_elements(root=self.root) assert len(self._elements["root_body"]) == 1, "Invalid number of root bodies found for robot model. Expected 1," \ "got {}".format(len(self._elements["root_body"])) self._elements["root_body"] = self._elements["root_body"][0] self._elements["bodies"] = [self._elements["root_body"]] + self._elements["bodies"] if \ "bodies" in self._elements else [self._elements["root_body"]] self._root_body = self._elements["root_body"].get("name") self._bodies = [ e.get("name") for e in self._elements.get("bodies", []) ] self._joints = [ e.get("name") for e in self._elements.get("joints", []) ] self._actuators = [ e.get("name") for e in self._elements.get("actuators", []) ] self._sites = [e.get("name") for e in self._elements.get("sites", [])] self._sensors = [ e.get("name") for e in self._elements.get("sensors", []) ] self._contact_geoms = [ e.get("name") for e in self._elements.get("contact_geoms", []) ] self._visual_geoms = [ e.get("name") for e in self._elements.get("visual_geoms", []) ] self._base_offset = string_to_array(self._elements["root_body"].get( "pos", "0 0 0")) # Update all xml element prefixes add_prefix(root=self.root, prefix=self.naming_prefix, exclude=self.exclude_from_prefixing) # Recolor all collision geoms appropriately recolor_collision_geoms(root=self.worldbody, rgba=self.contact_geom_rgba) # Add default materials if macros.USING_INSTANCE_RANDOMIZATION: tex_element, mat_element, _, used = add_material( root=self.worldbody, naming_prefix=self.naming_prefix) # Only add if material / texture was actually used if used: self.asset.append(tex_element) self.asset.append(mat_element)
def _load_model(self): super()._load_model() self.mujoco_robot.set_base_xpos([0, 0, 0]) # load model for table top workspace self.mujoco_arena = BinPackingArena( table_full_size=self.table_full_size, table_friction=self.table_friction) if self.use_indicator_object: self.mujoco_arena.add_pos_indicator() # The sawyer robot has a pedestal, we want to align it with the table self.mujoco_arena.set_origin([.5, -0.3, 0]) self.ob_inits = [MilkObject, BreadObject, CerealObject, CanObject] self.vis_inits = [ MilkVisualObject, BreadVisualObject, CerealVisualObject, CanVisualObject, ] self.item_names = ["Milk", "Bread", "Cereal", "Can"] self.item_names_org = list(self.item_names) self.obj_to_use = (self.item_names[0] + "{}").format(0) lst = [] for j in range(len(self.vis_inits)): lst.append((str(self.vis_inits[j]), self.vis_inits[j]())) self.visual_objects = lst lst = [] for i in range(len(self.ob_inits)): ob = self.ob_inits[i]() lst.append((str(self.item_names[i]) + "0", ob)) self.mujoco_objects = OrderedDict(lst) self.n_objects = len(self.mujoco_objects) # task includes arena, robot, and objects of interest self.model = BinPackingTask( self.mujoco_arena, self.mujoco_robot, self.mujoco_objects, self.visual_objects, ) self.model.place_objects() if self.baseline == 'random': self.model.move_objects_random() elif self.baseline == 'oracle': self.model.move_objects_oracle() # self.model.place_visual() self.bin_pos = string_to_array(self.model.bin2_body.get("pos")) self.bin_size = self.model.bin_size
def _load_model(self): super()._load_model() self.mujoco_robot.set_base_xpos([0, 0, 0]) # load model for table top workspace self.mujoco_arena = PegsArena(table_full_size=self.table_full_size, table_friction=self.table_friction) if self.use_indicator_object: self.mujoco_arena.add_pos_indicator() # The sawyer robot has a pedestal, we want to align it with the table self.mujoco_arena.set_origin([.5, -0.15, 0]) # define mujoco objects self.ob_inits = [SquareNutObject, RoundNutObject] self.item_names = ["SquareNut", "RoundNut"] self.item_names_org = list(self.item_names) self.obj_to_use = (self.item_names[1] + "{}").format(0) self.ngeoms = [5, 9] lst = [] for i in range(len(self.ob_inits)): ob = self.ob_inits[i]() lst.append((str(self.item_names[i]) + "0", ob)) self.mujoco_objects = OrderedDict(lst) self.n_objects = len(self.mujoco_objects) # task includes arena, robot, and objects of interest self.model = NutAssemblyTask( self.mujoco_arena, self.mujoco_robot, self.mujoco_objects, self.placement_initializer, ) self.model.place_objects() self.table_pos = string_to_array(self.model.table_body.get("pos")) self.peg1_pos = string_to_array( self.model.peg1_body.get("pos")) # square self.peg2_pos = string_to_array( self.model.peg2_body.get("pos")) # round
def set_origin(self, offset): """ Applies a constant offset to all objects. Args: offset (3-tuple): (x,y,z) offset to apply to all nodes in this XML """ offset = np.array(offset) for node in self.worldbody.findall("./*[@pos]"): cur_pos = string_to_array(node.get("pos")) new_pos = cur_pos + offset node.set("pos", array_to_string(new_pos))
def _load_model(self): # replace BaxterEnv _load_model call with custom robot MujocoEnv._load_model(self) self.mujoco_robot = BaxterRobot() if self.has_gripper_right: self.gripper_right = gripper_factory(self.gripper_right_name) if not self.gripper_visualization: self.gripper_right.hide_visualization() self.mujoco_robot.add_gripper("right_hand", self.gripper_right) if self.has_gripper_left: self.gripper_left = gripper_factory(self.gripper_left_name) if not self.gripper_visualization: self.gripper_left.hide_visualization() self.mujoco_robot.add_gripper("left_hand", self.gripper_left) self.mujoco_robot.set_base_xpos([0, 0, 0]) # load model for table top workspace self.mujoco_arena = BinsArena(table_full_size=self.table_full_size, table_friction=self.table_friction) if self.use_indicator_object: self.mujoco_arena.add_pos_indicator() # The sawyer robot has a pedestal, we want to align it with the table self.mujoco_arena.set_origin([.5, -0.4, 0]) self.ob_inits = [MilkObject, BreadObject, CerealObject, CanObject] self.item_names = ["Milk", "Bread", "Cereal", "Can"] self.item_names_org = list(self.item_names) self.obj_to_use = (self.item_names[0] + "{}").format(0) lst = [] for i in range(len(self.ob_inits)): ob = self.ob_inits[i]() lst.append((str(self.item_names[i]) + "0", ob)) self.mujoco_objects = OrderedDict(lst) self.n_objects = len(self.mujoco_objects) # task includes arena, robot, and objects of interest self.model = PickPlaceTask( self.mujoco_arena, self.mujoco_robot, self.mujoco_objects, [], ) self.model.place_objects() self.model.place_visual() self.bin_pos = string_to_array(self.model.bin2_body.get("pos")) self.bin_size = self.model.bin_size
def set_goal_xpos(self, x_delta, y_delta): """ Sets x,y position of goal site in door model with x and y offset from door center""" door_center_site = self.worldbody.find( "./body/body/body/site[@name='door_center']") door_center_pos = string_to_array(door_center_site.get("pos")) goal_site = self.worldbody.find("./body/body/body/site[@name='goal']") goal_site.set( "pos", array_to_string([ door_center_pos[0] + x_delta, door_center_pos[1] + y_delta, -1.0 ]))
def _load_model(self): super()._load_model() self.mujoco_robot.set_base_xpos([0, 0, 0]) # load model for table top workspace self.mujoco_arena = BinSqueezeArena( table_full_size=self.table_full_size, table_friction=self.table_friction ) if self.use_indicator_object: self.mujoco_arena.add_pos_indicator() # The sawyer robot has a pedestal, we want to align it with the table self.mujoco_arena.set_origin([.5, -0.3, 0]) # make objects self._make_objects() lst = [] for j in range(len(self.vis_inits)): lst.append((str(self.vis_inits[j]), self.vis_inits[j]())) self.visual_objects = lst lst = [] for i in range(len(self.ob_inits)): ob = self.ob_inits[i]() lst.append((str(self.item_names[i]), ob)) self.mujoco_objects = OrderedDict(lst) self.object_names = list(self.mujoco_objects.keys()) self.object_to_choose = self.object_names.copy() self.n_objects = len(self.mujoco_objects) # task includes arena, robot, and objects of interest self.model = BinSqueezeTask( self.mujoco_arena, self.mujoco_robot, self.mujoco_objects, self.visual_objects, self.obj_poses ) # self.model.place_objects() self.bin_pos = string_to_array(self.model.bin2_body.get("pos")) self.bin_size = self.table_target_size
def parse_textures(self): """ Parse and load all textures and store them """ self.texture_attributes = {} self.texture_id_mapping = {} for texture in self.xml_root.iter('texture'): texture_type = texture.get('type') texture_name = texture.get('name') texture_file = texture.get('file') texture_rgb = texture.get('rgb1') if texture_file is not None: self.texture_attributes[texture_name] = texture.attrib else: color = np.array(string_to_array(texture_rgb)) self.texture_id_mapping[texture_name] = (color, texture_type)
def parse_textures(self): """ Parse and load all textures and store them """ self.texture_attributes = {} self.texture_id_mapping = {} for texture in self.xml_root.iter("texture"): texture_type = texture.get("type") texture_name = texture.get("name") texture_file = texture.get("file") texture_rgb = texture.get("rgb1") if texture_file is not None: self.texture_attributes[texture_name] = texture.attrib self.texture_id_mapping[texture_name] = ( self.renderer.load_texture_file(texture_file), texture_type) else: color = np.array(string_to_array(texture_rgb)) self.texture_id_mapping[texture_name] = (color, texture_type)
def table_top_abs(self): """ Returns the absolute position of table top. """ return string_to_array(self.table_body.get("pos"))
def table_top_abs(self): """Returns the absolute position of table top""" table_height = np.array([0, 0, self.table_full_size[2]]) return string_to_array(self.floor.get("pos")) + table_height
def get_horizontal_radius(self): horizontal_radius_site = self.worldbody.find( "./body/site[@name='horizontal_radius_site']" ) return string_to_array(horizontal_radius_site.get("pos"))[0]
def get_top_offset(self): top_site = self.worldbody.find("./body/site[@name='top_site']") return string_to_array(top_site.get("pos"))
def get_bottom_offset(self): bottom_site = self.worldbody.find("./body/site[@name='bottom_site']") return string_to_array(bottom_site.get("pos"))
def parse_geometries(self): """ Iterate through each goemetry and load it in the NVISII renderer. """ self.parse_meshes() element_id = 0 repeated_names = {} block_rendering_objects = [ 'VisualBread_g0', 'VisualCan_g0', 'VisualCereal_g0', 'VisualMilk_g0' ] self.entity_id_class_mapping = {} for geom_index, geom in enumerate(self.xml_root.iter('geom')): parent_body = self.parent_map.get(geom) parent_body_name = parent_body.get('name', 'worldbody') geom_name = geom.get('name') geom_type = geom.get('type') rgba_str = geom.get('rgba') geom_rgba = string_to_array( rgba_str) if rgba_str is not None else None if geom_name is None: if parent_body_name in repeated_names: geom_name = parent_body_name + str( repeated_names[parent_body_name]) repeated_names[parent_body_name] += 1 else: geom_name = parent_body_name + '0' repeated_names[parent_body_name] = 1 if (geom.get('group') != '1' and geom_type != 'plane') or ('collision' in geom_name): continue if 'floor' in geom_name or 'wall' in geom_name or geom_name in block_rendering_objects: continue geom_quat = string_to_array(geom.get('quat', '1 0 0 0')) geom_quat = [ geom_quat[0], geom_quat[1], geom_quat[2], geom_quat[3] ] # handling special case of bins arena if 'bin' in parent_body_name: geom_pos = string_to_array(geom.get( 'pos', "0 0 0")) + string_to_array( parent_body.get('pos', '0 0 0')) else: geom_pos = string_to_array(geom.get('pos', "0 0 0")) if geom_type == 'mesh': geom_scale = string_to_array(self.meshes[geom.get('mesh')].get( 'scale', '1 1 1')) else: geom_scale = [1, 1, 1] geom_size = string_to_array(geom.get('size', "1 1 1")) geom_mat = geom.get('material') tags = ['bin'] dynamic = True if self.tag_in_name(geom_name, tags): dynamic = False geom_tex_name = None geom_tex_file = None if geom_mat is not None: geom_tex_name = self.material_texture_mapping[geom_mat] if geom_tex_name in self.texture_attributes: geom_tex_file = self.texture_attributes[geom_tex_name][ 'file'] class_id = self.get_class_id(geom_index, element_id) # load obj into nvisii obj, entity_ids = load_object( geom=geom, geom_name=geom_name, geom_type=geom_type, geom_quat=geom_quat, geom_pos=geom_pos, geom_size=geom_size, geom_scale=geom_scale, geom_rgba=geom_rgba, geom_tex_name=geom_tex_name, geom_tex_file=geom_tex_file, class_id=class_id, # change meshes=self.meshes) element_id += 1 for entity_id in entity_ids: self.entity_id_class_mapping[entity_id] = class_id self.components[geom_name] = Components( obj=obj, geom_index=geom_index, element_id=element_id, parent_body_name=parent_body_name, geom_pos=geom_pos, geom_quat=geom_quat, dynamic=dynamic) self.max_elements = element_id
def parse_geometries(self): """ Iterate through each goemetry and load it in the iGibson renderer. """ self.parse_meshes() element_id = 0 for geom_index, geom in enumerate(self.xml_root.iter("geom")): geom_name = geom.get("name", "NONAME") geom_type = geom.get("type") if (geom.get("group") != "1" and geom_type != "plane") or ("collision" in geom_name): continue parent_body = self.parent_map.get(geom) # [1, 0, 0, 0] is wxyz, we convert it back to xyzw. geom_orn = string_to_array(geom.get("quat", "1 0 0 0")) geom_orn = [geom_orn[1], geom_orn[2], geom_orn[3], geom_orn[0]] geom_pos = string_to_array(geom.get("pos", "0 0 0")) if geom_type == "mesh": geom_scale = string_to_array(self.meshes[geom.get("mesh")].get( "scale", "1 1 1")) else: geom_scale = [1, 1, 1] geom_size = string_to_array(geom.get("size", "1 1 1")) geom_rgba = string_to_array(geom.get("rgba", "1 1 1 1")) geom_material = self.material_mapping.get(geom.get("material")) if geom_material is None: color = geom_rgba[:3].reshape(1, 1, 3) # TODO: check converting the below texture to color material dummy_texture_id = get_texture_id(color, "texture", self) geom_material = Material( "texture", texture_id=dummy_texture_id, metallic_texture_id=get_texture_id(1, "metallic", self), roughness_texture_id=get_texture_id(1, "roughness", self), normal_texture_id=self.normal_id, ) # Flag to check if default material is used geom_material._is_set_by_parser = True else: geom_material._is_set_by_parser = False # saving original params because transform param will be overwritten if not hasattr(geom_material, "_orig_transform_param"): geom_material._orig_transform_param = geom_material.transform_param # setting material_ids so that randomized material works geom_material.material_ids = 0 class_id = self.get_class_id(geom_index, element_id) load_object( renderer=self.renderer, geom=geom, geom_name=geom_name, geom_type=geom_type, geom_orn=geom_orn, geom_pos=geom_pos, geom_rgba=geom_rgba, geom_size=geom_size, geom_scale=geom_scale, geom_material=geom_material, parent_body=parent_body, class_id=class_id, visual_objects=self.visual_objects, meshes=self.meshes, ) element_id += 1 self.max_elements = element_id
def horizontal_radius(self): horizontal_radius_site = self.worldbody.find( "./body/site[@name='{}horizontal_radius_site']".format( self.naming_prefix)) return string_to_array(horizontal_radius_site.get("pos"))[0]
def top_offset(self): top_site = self.worldbody.find( "./body/site[@name='{}top_site']".format(self.naming_prefix)) return string_to_array(top_site.get("pos"))
def parse_geometries(self): """ Iterate through each goemetry and load it in the NVISII renderer. """ self.parse_meshes() element_id = 0 repeated_names = {} block_rendering_objects = ["VisualBread_g0", "VisualCan_g0", "VisualCereal_g0", "VisualMilk_g0"] self.entity_id_class_mapping = {} for geom_index, geom in enumerate(self.xml_root.iter("geom")): parent_body = self.parent_map.get(geom) parent_body_name = parent_body.get("name", "worldbody") geom_name = geom.get("name") geom_type = geom.get("type") rgba_str = geom.get("rgba") geom_rgba = string_to_array(rgba_str) if rgba_str is not None else None if geom_name is None: if parent_body_name in repeated_names: geom_name = parent_body_name + str(repeated_names[parent_body_name]) repeated_names[parent_body_name] += 1 else: geom_name = parent_body_name + "0" repeated_names[parent_body_name] = 1 if (geom.get("group") != "1" and geom_type != "plane") or ("collision" in geom_name): continue if "floor" in geom_name or "wall" in geom_name or geom_name in block_rendering_objects: continue geom_quat = string_to_array(geom.get("quat", "1 0 0 0")) geom_quat = [geom_quat[0], geom_quat[1], geom_quat[2], geom_quat[3]] # handling special case of bins arena if "bin" in parent_body_name: geom_pos = string_to_array(geom.get("pos", "0 0 0")) + string_to_array(parent_body.get("pos", "0 0 0")) else: geom_pos = string_to_array(geom.get("pos", "0 0 0")) if geom_type == "mesh": geom_scale = string_to_array(self.meshes[geom.get("mesh")].get("scale", "1 1 1")) else: geom_scale = [1, 1, 1] geom_size = string_to_array(geom.get("size", "1 1 1")) geom_mat = geom.get("material") tags = ["bin"] dynamic = True if self.tag_in_name(geom_name, tags): dynamic = False geom_tex_name = None geom_tex_file = None if geom_mat is not None: geom_tex_name = self.material_texture_mapping[geom_mat] if geom_tex_name in self.texture_attributes: geom_tex_file = self.texture_attributes[geom_tex_name]["file"] class_id = self.get_class_id(geom_index, element_id) # load obj into nvisii obj, entity_ids = load_object( geom=geom, geom_name=geom_name, geom_type=geom_type, geom_quat=geom_quat, geom_pos=geom_pos, geom_size=geom_size, geom_scale=geom_scale, geom_rgba=geom_rgba, geom_tex_name=geom_tex_name, geom_tex_file=geom_tex_file, class_id=class_id, # change meshes=self.meshes, ) element_id += 1 for entity_id in entity_ids: self.entity_id_class_mapping[entity_id] = class_id self.components[geom_name] = Components( obj=obj, geom_index=geom_index, element_id=element_id, parent_body_name=parent_body_name, geom_pos=geom_pos, geom_quat=geom_quat, dynamic=dynamic, ) self.max_elements = element_id
def parse_geometries(self): """ Iterate through each goemetry and load it in the iGibson renderer. """ self.parse_meshes() element_id = 0 for geom_index, geom in enumerate(self.xml_root.iter('geom')): geom_name = geom.get('name', 'NONAME') geom_type = geom.get('type') if (geom.get('group') != '1' and geom_type != 'plane') or ('collision' in geom_name): continue parent_body = self.parent_map.get(geom) # [1, 0, 0, 0] is wxyz, we convert it back to xyzw. geom_orn = string_to_array(geom.get('quat', '1 0 0 0')) geom_orn = [geom_orn[1], geom_orn[2], geom_orn[3], geom_orn[0]] geom_pos = string_to_array(geom.get('pos', "0 0 0")) if geom_type == 'mesh': geom_scale = string_to_array(self.meshes[geom.get('mesh')].get( 'scale', '1 1 1')) else: geom_scale = [1, 1, 1] geom_size = string_to_array(geom.get('size', "1 1 1")) geom_rgba = string_to_array(geom.get('rgba', "1 1 1 1")) geom_material = self.material_mapping.get(geom.get('material')) if geom_material is None: color = geom_rgba[:3].reshape(1, 1, 3) #TODO: check converting the below texture to color material dummy_texture_id = get_texture_id(color, 'texture', self) geom_material = Material( 'texture', texture_id=dummy_texture_id, metallic_texture_id=get_texture_id(1, 'metallic', self), roughness_texture_id=get_texture_id(1, 'roughness', self), normal_texture_id=self.normal_id) # Flag to check if default material is used geom_material._is_set_by_parser = True else: geom_material._is_set_by_parser = False # saving original params because transform param will be overwritten if not hasattr(geom_material, '_orig_transform_param'): geom_material._orig_transform_param = geom_material.transform_param # setting material_ids so that randomized material works geom_material.material_ids = 0 class_id = self.get_class_id(geom_index, element_id) load_object(renderer=self.renderer, geom=geom, geom_name=geom_name, geom_type=geom_type, geom_orn=geom_orn, geom_pos=geom_pos, geom_rgba=geom_rgba, geom_size=geom_size, geom_scale=geom_scale, geom_material=geom_material, parent_body=parent_body, class_id=class_id, visual_objects=self.visual_objects, meshes=self.meshes) element_id += 1 self.max_elements = element_id
def __init__(self, fname, idn=0): super().__init__(fname) # Set id and add prefixes to all body names to prevent naming clashes self.idn = idn # Define other variables that get filled later self.mount = None # Define filter method to automatically add a default name to visual / collision geoms if encountered group_mapping = { None: "col", "0": "col", "1": "vis", } ctr_mapping = { "col": 0, "vis": 0, } def _add_default_name_filter(element, parent): # Run default filter filter_key = _element_filter(element=element, parent=parent) # Also additionally modify element if it is (a) a geom and (b) has no name if element.tag == "geom" and element.get("name") is None: group = group_mapping[element.get("group")] element.set("name", f"g{ctr_mapping[group]}_{group}") ctr_mapping[group] += 1 # Return default filter key return filter_key # Parse element tree to get all relevant bodies, joints, actuators, and geom groups self._elements = sort_elements(root=self.root, element_filter=_add_default_name_filter) assert len(self._elements["root_body"]) == 1, "Invalid number of root bodies found for robot model. Expected 1," \ "got {}".format(len(self._elements["root_body"])) self._elements["root_body"] = self._elements["root_body"][0] self._elements["bodies"] = [self._elements["root_body"]] + self._elements["bodies"] if \ "bodies" in self._elements else [self._elements["root_body"]] self._root_body = self._elements["root_body"].get("name") self._bodies = [ e.get("name") for e in self._elements.get("bodies", []) ] self._joints = [ e.get("name") for e in self._elements.get("joints", []) ] self._actuators = [ e.get("name") for e in self._elements.get("actuators", []) ] self._sites = [e.get("name") for e in self._elements.get("sites", [])] self._sensors = [ e.get("name") for e in self._elements.get("sensors", []) ] self._contact_geoms = [ e.get("name") for e in self._elements.get("contact_geoms", []) ] self._visual_geoms = [ e.get("name") for e in self._elements.get("visual_geoms", []) ] self._base_offset = string_to_array(self._elements["root_body"].get( "pos", "0 0 0")) # Update all xml element prefixes add_prefix(root=self.root, prefix=self.naming_prefix, exclude=self.exclude_from_prefixing) # Recolor all collision geoms appropriately recolor_collision_geoms(root=self.worldbody, rgba=self.contact_geom_rgba) # Add default materials if macros.USING_INSTANCE_RANDOMIZATION: tex_element, mat_element, _, used = add_material( root=self.worldbody, naming_prefix=self.naming_prefix) # Only add if material / texture was actually used if used: self.asset.append(tex_element) self.asset.append(mat_element)
def bin_abs(self): """Returns the absolute position of table top""" return string_to_array(self.bin1_body.get("pos"))