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"))
