def update_position(instance):
"""
Update position for an object or a robot in renderer.
:param instance: Instance in the renderer
"""
if isinstance(instance, Instance):
pos, orn = p.getBasePositionAndOrientation(
instance.pybullet_uuid) # orn is in x,y,z,w
instance.set_position(pos)
instance.set_rotation(xyzw2wxyz(orn))
elif isinstance(instance, InstanceGroup):
poses_rot = []
poses_trans = []
for link_id in instance.link_ids:
if link_id == -1:
pos, orn = p.getBasePositionAndOrientation(
instance.pybullet_uuid)
else:
_, _, _, _, pos, orn = p.getLinkState(
instance.pybullet_uuid, link_id)
poses_rot.append(
np.ascontiguousarray(quat2rotmat(xyzw2wxyz(orn))))
poses_trans.append(np.ascontiguousarray(xyz2mat(pos)))
instance.poses_rot = poses_rot
instance.poses_trans = poses_trans
def update_position(instance):
if isinstance(instance, Instance):
pos, orn = p.getBasePositionAndOrientation(instance.pybullet_uuid)
instance.set_position(pos)
instance.set_rotation([orn[-1], orn[0], orn[1], orn[2]])
elif isinstance(instance, InstanceGroup):
poses_rot = []
poses_trans = []
for link_id in instance.link_ids:
if link_id == -1:
pos, orn = p.getBasePositionAndOrientation(
instance.pybullet_uuid)
else:
_, _, _, _, pos, orn = p.getLinkState(
instance.pybullet_uuid, link_id)
poses_rot.append(
np.ascontiguousarray(
quat2rotmat([orn[-1], orn[0], orn[1], orn[2]])))
poses_trans.append(np.ascontiguousarray(xyz2mat(pos)))
#print(instance.pybullet_uuid, link_id, pos, orn)
instance.poses_rot = poses_rot
instance.poses_trans = poses_trans
def import_articulated_object(self, obj, class_id=None):
"""
Import an articulated object into the simulator
:param obj: Object to load
:param class_id: Class id for rendering semantic segmentation
:return: pybulet id
"""
if class_id is None:
class_id = self.next_class_id
self.next_class_id += 1
ids = obj.load()
visual_objects = []
link_ids = []
poses_rot = []
poses_trans = []
for shape in p.getVisualShapeData(ids):
id, link_id, type, dimensions, filename, rel_pos, rel_orn, color = shape[:
8]
if type == p.GEOM_MESH:
filename = filename.decode('utf-8')
if filename not in self.visual_objects.keys():
self.renderer.load_object(filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=np.array(dimensions))
self.visual_objects[filename] = len(
self.renderer.visual_objects) - 1
visual_objects.append(self.visual_objects[filename])
link_ids.append(link_id)
elif type == p.GEOM_SPHERE:
filename = os.path.join(gibson2.assets_path,
'models/mjcf_primitives/sphere8.obj')
self.renderer.load_object(filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=[
dimensions[0] / 0.5,
dimensions[0] / 0.5,
dimensions[0] / 0.5
])
visual_objects.append(len(self.renderer.visual_objects) - 1)
link_ids.append(link_id)
elif type == p.GEOM_CAPSULE or type == p.GEOM_CYLINDER:
filename = os.path.join(gibson2.assets_path,
'models/mjcf_primitives/cube.obj')
self.renderer.load_object(filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=[
dimensions[1] / 0.5,
dimensions[1] / 0.5,
dimensions[0]
])
visual_objects.append(len(self.renderer.visual_objects) - 1)
link_ids.append(link_id)
elif type == p.GEOM_BOX:
filename = os.path.join(gibson2.assets_path,
'models/mjcf_primitives/cube.obj')
self.renderer.load_object(filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=np.array(dimensions))
visual_objects.append(len(self.renderer.visual_objects) - 1)
link_ids.append(link_id)
if link_id == -1:
pos, orn = p.getBasePositionAndOrientation(id)
else:
_, _, _, _, pos, orn = p.getLinkState(id, link_id)
poses_rot.append(np.ascontiguousarray(quat2rotmat(xyzw2wxyz(orn))))
poses_trans.append(np.ascontiguousarray(xyz2mat(pos)))
self.renderer.add_instance_group(object_ids=visual_objects,
link_ids=link_ids,
pybullet_uuid=ids,
class_id=class_id,
poses_rot=poses_rot,
poses_trans=poses_trans,
dynamic=True,
robot=None)
return ids
def import_drawer_handle(self):
self.drawer_id = self.drawer.load()
# render drawer
class_id = self.simulator.next_class_id
self.simulator.next_class_id += 1
visual_objects = []
link_ids = []
poses_rot = []
poses_trans = []
for shape in p.getVisualShapeData(self.drawer_id):
id, link_id, type, dimensions, filename, rel_pos, rel_orn, color = shape[:
8]
link_name = p.getJointInfo(self.drawer_id, link_id)[12]
if link_name != b'handle_left' and link_name != b'handle_right' and link_name != b'handle_grip':
filename = os.path.join(gibson2.assets_path,
'models/mjcf_primitives/cube.obj')
self.simulator.renderer.load_object(filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=np.array(dimensions))
visual_objects.append(
len(self.simulator.renderer.visual_objects) - 1)
link_ids.append(link_id)
_, _, _, _, pos, orn = p.getLinkState(id, link_id)
poses_rot.append(
np.ascontiguousarray(quat2rotmat(xyzw2wxyz(orn))))
poses_trans.append(np.ascontiguousarray(xyz2mat(pos)))
self.simulator.renderer.add_instance_group(
object_ids=visual_objects,
link_ids=link_ids,
pybullet_uuid=self.drawer_id,
class_id=class_id,
poses_rot=poses_rot,
poses_trans=poses_trans,
dynamic=True,
robot=None)
# render drawer handle
class_id = self.simulator.next_class_id
self.simulator.next_class_id += 1
visual_objects = []
link_ids = []
poses_rot = []
poses_trans = []
for shape in p.getVisualShapeData(self.drawer_id):
id, link_id, type, dimensions, filename, rel_pos, rel_orn, color = shape[:
8]
link_name = p.getJointInfo(self.drawer_id, link_id)[12]
if link_name == b'handle_left' or link_name == b'handle_right' or link_name == b'handle_grip':
filename = os.path.join(gibson2.assets_path,
'models/mjcf_primitives/cube.obj')
self.simulator.renderer.load_object(filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=np.array(dimensions))
visual_objects.append(
len(self.simulator.renderer.visual_objects) - 1)
link_ids.append(link_id)
_, _, _, _, pos, orn = p.getLinkState(id, link_id)
poses_rot.append(
np.ascontiguousarray(quat2rotmat(xyzw2wxyz(orn))))
poses_trans.append(np.ascontiguousarray(xyz2mat(pos)))
self.simulator.renderer.add_instance_group(
object_ids=visual_objects,
link_ids=link_ids,
pybullet_uuid=self.drawer_id,
class_id=class_id,
poses_rot=poses_rot,
poses_trans=poses_trans,
dynamic=True,
robot=None)
def import_interactive_object(self, obj):
ids = obj.load()
visual_objects = []
link_ids = []
poses_rot = []
poses_trans = []
for shape in p.getVisualShapeData(ids):
id, link_id, type, dimensions, filename, rel_pos, rel_orn, color = shape[:
8]
if type == p.GEOM_MESH:
filename = filename.decode('utf-8')
if not filename in self.visual_objects.keys():
print(filename, rel_pos, rel_orn, color, dimensions)
self.renderer.load_object(filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=np.array(dimensions))
visual_objects.append(
len(self.renderer.visual_objects) - 1)
self.visual_objects[filename] = len(
self.renderer.visual_objects) - 1
else:
visual_objects.append(self.visual_objects[filename])
link_ids.append(link_id)
elif type == p.GEOM_SPHERE:
filename = os.path.join(gibson2.assets_path,
'models/mjcf_primitives/sphere8.obj')
print(filename, dimensions, rel_pos, rel_orn, color)
self.renderer.load_object(filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=[
dimensions[0] / 0.5,
dimensions[0] / 0.5,
dimensions[0] / 0.5
])
visual_objects.append(len(self.renderer.visual_objects) - 1)
# self.visual_objects[filename] = len(self.renderer.visual_objects) - 1
link_ids.append(link_id)
elif type == p.GEOM_CAPSULE or type == p.GEOM_CYLINDER:
filename = os.path.join(gibson2.assets_path,
'models/mjcf_primitives/cube.obj')
print(filename, dimensions, rel_pos, rel_orn, color)
self.renderer.load_object(filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=[
dimensions[1] / 0.5,
dimensions[1] / 0.5,
dimensions[0]
])
visual_objects.append(len(self.renderer.visual_objects) - 1)
link_ids.append(link_id)
elif type == p.GEOM_BOX:
filename = os.path.join(gibson2.assets_path,
'models/mjcf_primitives/cube.obj')
print(filename, dimensions, rel_pos, rel_orn, color)
self.renderer.load_object(
filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=[dimensions[0], dimensions[1], dimensions[2]])
visual_objects.append(len(self.renderer.visual_objects) - 1)
link_ids.append(link_id)
if link_id == -1:
pos, orn = p.getBasePositionAndOrientation(id)
else:
_, _, _, _, pos, orn = p.getLinkState(id, link_id)
poses_rot.append(
np.ascontiguousarray(
quat2rotmat([orn[-1], orn[0], orn[1], orn[2]])))
poses_trans.append(np.ascontiguousarray(xyz2mat(pos)))
self.renderer.add_instance_group(object_ids=visual_objects,
link_ids=link_ids,
pybullet_uuid=ids,
poses_rot=poses_rot,
poses_trans=poses_trans,
dynamic=True,
robot=None)
return ids
def import_robot(self, robot, class_id=0):
"""
Import a robot into Simulator
:param robot: Robot
:param class_id: class_id to show for semantic segmentation mask
:return: id for robot in pybullet
"""
ids = robot.load()
visual_objects = []
link_ids = []
poses_rot = []
poses_trans = []
self.robots.append(robot)
for shape in p.getVisualShapeData(ids[0]):
id, link_id, type, dimensions, filename, rel_pos, rel_orn, color = shape[:
8]
if type == p.GEOM_MESH:
filename = filename.decode('utf-8')
if not filename in self.visual_objects.keys():
print(filename, rel_pos, rel_orn, color, dimensions)
self.renderer.load_object(filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=np.array(dimensions))
visual_objects.append(
len(self.renderer.visual_objects) - 1)
self.visual_objects[filename] = len(
self.renderer.visual_objects) - 1
else:
visual_objects.append(self.visual_objects[filename])
link_ids.append(link_id)
elif type == p.GEOM_SPHERE:
filename = os.path.join(gibson2.assets_path,
'models/mjcf_primitives/sphere8.obj')
print(filename, dimensions, rel_pos, rel_orn, color)
self.renderer.load_object(filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=[
dimensions[0] / 0.5,
dimensions[0] / 0.5,
dimensions[0] / 0.5
])
visual_objects.append(len(self.renderer.visual_objects) - 1)
link_ids.append(link_id)
elif type == p.GEOM_CAPSULE or type == p.GEOM_CYLINDER:
filename = os.path.join(gibson2.assets_path,
'models/mjcf_primitives/cube.obj')
print(filename, dimensions, rel_pos, rel_orn, color)
self.renderer.load_object(filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=[
dimensions[1] / 0.5,
dimensions[1] / 0.5,
dimensions[0]
])
visual_objects.append(len(self.renderer.visual_objects) - 1)
link_ids.append(link_id)
elif type == p.GEOM_BOX:
filename = os.path.join(gibson2.assets_path,
'models/mjcf_primitives/cube.obj')
print(filename, dimensions, rel_pos, rel_orn, color)
self.renderer.load_object(
filename,
transform_orn=rel_orn,
transform_pos=rel_pos,
input_kd=color[:3],
scale=[dimensions[0], dimensions[1], dimensions[2]])
visual_objects.append(len(self.renderer.visual_objects) - 1)
link_ids.append(link_id)
if link_id == -1:
pos, orn = p.getBasePositionAndOrientation(id)
else:
_, _, _, _, pos, orn = p.getLinkState(id, link_id)
poses_rot.append(
np.ascontiguousarray(
quat2rotmat([orn[-1], orn[0], orn[1], orn[2]])))
poses_trans.append(np.ascontiguousarray(xyz2mat(pos)))
self.renderer.add_robot(object_ids=visual_objects,
link_ids=link_ids,
pybullet_uuid=ids[0],
class_id=class_id,
poses_rot=poses_rot,
poses_trans=poses_trans,
dynamic=True,
robot=robot)
return ids