def render_robot_cameras(self, modes=('rgb')):
frames = []
for instance in self.instances:
if isinstance(instance, Robot):
camera_pos = instance.robot.eyes.get_position()
orn = instance.robot.eyes.get_orientation()
mat = quat2rotmat(xyzw2wxyz(orn))[:3, :3]
view_direction = mat.dot(np.array([1, 0, 0]))
self.set_camera(camera_pos, camera_pos + view_direction,
[0, 0, 1])
self.set_fov(60)
for item in self.render(modes=modes, hidden=[instance]):
frames.append(item)
if instance.robot.wrist_eyes is not None:
wrist_camera_pos = instance.robot.wrist_eyes.get_position()
wrist_orn = instance.robot.wrist_eyes.get_orientation()
wrist_mat = quat2rotmat(xyzw2wxyz(wrist_orn))[:3, :3]
wrist_view_direction = wrist_mat.dot(np.array([1, 0, 0]))
self.set_camera(wrist_camera_pos,
wrist_camera_pos + wrist_view_direction,
[0, 0, 1])
self.set_fov(80)
for item in self.render(modes=modes, hidden=[instance]):
frames.append(item)
return frames
def transform_pose(self, pose):
pose_rot = quat2rotmat(pose[3:])
pose_trans = xyz2mat(pose[:3])
pose_cam = self.V.dot(pose_trans.T).dot(pose_rot).T
return np.concatenate(
[mat2xyz(pose_cam),
safemat2quat(pose_cam[:3, :3].T)])
def set_rotation(self, rot):
"""
Set rotations for each part of this InstanceGroup
:param rot: New rotation matrices
"""
self.pose_rot = np.ascontiguousarray(quat2rotmat(rot))
def set_rotation(self, quat):
"""
Set rotations for each part of this InstanceGroup
:param quat: New quaternion in w,x,y,z
"""
self.pose_rot = np.ascontiguousarray(quat2rotmat(quat))
def render_robot_cameras(self, modes=('rgb')):
frames = []
for instance in self.instances:
if isinstance(instance, Robot):
camera_pos = instance.robot.eyes.get_position()
orn = instance.robot.eyes.get_orientation()
mat = quat2rotmat(xyzw2wxyz(orn))[:3, :3]
view_direction = mat.dot(np.array([1, 0, 0]))
self.set_camera(camera_pos, camera_pos + view_direction, [0, 0, 1])
for item in self.render(modes=modes):
frames.append(item)
return frames
def set_pose(self, pose, idx):
self.instances[idx].pose_rot = np.ascontiguousarray(
quat2rotmat(pose[3:]))
self.instances[idx].pose_trans = np.ascontiguousarray(xyz2mat(
pose[:3]))
def load_object(self,
obj_path,
scale=np.array([1, 1, 1]),
transform_orn=None,
transform_pos=None,
input_kd=None,
texture_scale=1.0,
load_texture=True):
"""
Load a wavefront obj file into the renderer and create a VisualObject to manage it.
:param obj_path: path of obj file
:param scale: scale, default 1
:param transform_orn: rotation for loading, 3x3 matrix
:param transform_pos: translation for loading, it is a list of length 3
:param input_kd: If loading texture is not successful, the color to use, it is a list of length 3
:param texture_scale: texture scale for the object, downsample to save memory.
:param load_texture: load texture or not
:return: VAO_ids
"""
reader = tinyobjloader.ObjReader()
ret = reader.ParseFromFile(obj_path)
if ret == False:
print("Warn:", reader.Warning())
print("Err:", reader.Error())
print("Failed to load : ", obj_path)
sys.exit(-1)
if reader.Warning():
print("Warn:", reader.Warning())
attrib = reader.GetAttrib()
print("attrib.vertices = ", len(attrib.vertices))
print("attrib.normals = ", len(attrib.normals))
print("attrib.texcoords = ", len(attrib.texcoords))
materials = reader.GetMaterials()
print("Num materials: ", len(materials))
for m in materials:
print(m.name)
print(m.diffuse)
shapes = reader.GetShapes()
print("Num shapes: ", len(shapes))
material_count = len(self.materials_mapping)
materials_fn = {}
for i, item in enumerate(materials):
is_texture = False
kd = item.diffuse
if item.diffuse_texname != '':
is_texture = True
if load_texture:
materials_fn[i + material_count] = item.diffuse_texname
dir = os.path.dirname(obj_path)
texture = loadTexture(os.path.join(dir,
item.diffuse_texname),
scale=texture_scale)
material = Material('texture', texture_id=texture)
self.textures.append(texture)
else:
material = Material('color', kd=kd)
self.materials_mapping[i + material_count] = material
if not is_texture:
self.materials_mapping[i + material_count] = Material('color',
kd=kd)
if input_kd is not None: # urdf material
self.materials_mapping[len(materials) + material_count] = Material(
'color', kd=input_kd)
elif len(materials
) == 0: # urdf material not specified, but it is required
self.materials_mapping[len(materials) + material_count] = Material(
'color', kd=[0.5, 0.5, 0.5])
print(self.materials_mapping)
VAO_ids = []
vertex_position = np.array(attrib.vertices).reshape(
(len(attrib.vertices) // 3, 3))
vertex_normal = np.array(attrib.normals).reshape(
(len(attrib.normals) // 3, 3))
vertex_texcoord = np.array(attrib.texcoords).reshape(
(len(attrib.texcoords) // 2, 2))
print(vertex_position.shape, vertex_normal.shape,
vertex_texcoord.shape)
for shape in shapes:
print(shape.name)
material_id = shape.mesh.material_ids[
0] # assume one shape only have one material
print("material_id = {}".format(material_id))
print("num_indices = {}".format(len(shape.mesh.indices)))
n_indices = len(shape.mesh.indices)
np_indices = shape.mesh.numpy_indices().reshape((n_indices, 3))
shape_vertex_index = np_indices[:, 0]
shape_normal_index = np_indices[:, 1]
shape_texcoord_index = np_indices[:, 2]
shape_vertex = vertex_position[shape_vertex_index]
if len(vertex_normal) == 0:
shape_normal = np.zeros(
(shape_vertex.shape[0],
3)) #dummy normal if normal is not available
else:
shape_normal = vertex_normal[shape_normal_index]
if len(vertex_texcoord) == 0:
shape_texcoord = np.zeros(
(shape_vertex.shape[0],
2)) #dummy texcoord if texcoord is not available
else:
shape_texcoord = vertex_texcoord[shape_texcoord_index]
vertices = np.concatenate(
[shape_vertex * scale, shape_normal, shape_texcoord], axis=-1)
faces = np.array(range(len(vertices))).reshape(
(len(vertices) // 3, 3))
if not transform_orn is None:
orn = quat2rotmat([
transform_orn[-1], transform_orn[0], transform_orn[1],
transform_orn[2]
])
vertices[:, :3] = vertices[:, :3].dot(orn[:3, :3].T)
if not transform_pos is None:
vertices[:, :3] += np.array(transform_pos)
vertexData = vertices.astype(np.float32)
VAO = GL.glGenVertexArrays(1)
GL.glBindVertexArray(VAO)
# Need VBO for triangle vertices and texture UV coordinates
VBO = GL.glGenBuffers(1)
GL.glBindBuffer(GL.GL_ARRAY_BUFFER, VBO)
GL.glBufferData(GL.GL_ARRAY_BUFFER, vertexData.nbytes, vertexData,
GL.GL_STATIC_DRAW)
# enable array and set up data
positionAttrib = GL.glGetAttribLocation(self.shaderProgram,
'position')
normalAttrib = GL.glGetAttribLocation(self.shaderProgram, 'normal')
coordsAttrib = GL.glGetAttribLocation(self.shaderProgram,
'texCoords')
GL.glEnableVertexAttribArray(0)
GL.glEnableVertexAttribArray(1)
GL.glEnableVertexAttribArray(2)
GL.glVertexAttribPointer(positionAttrib, 3, GL.GL_FLOAT,
GL.GL_FALSE, 32, None)
GL.glVertexAttribPointer(normalAttrib, 3, GL.GL_FLOAT, GL.GL_FALSE,
32, ctypes.c_void_p(12))
# the last parameter is a pointer
GL.glVertexAttribPointer(coordsAttrib, 2, GL.GL_FLOAT, GL.GL_TRUE,
32, ctypes.c_void_p(24))
GL.glBindBuffer(GL.GL_ARRAY_BUFFER, 0)
GL.glBindVertexArray(0)
self.VAOs.append(VAO)
self.VBOs.append(VBO)
self.faces.append(faces)
self.objects.append(obj_path)
if material_id == -1: # if no material, use urdf color as material
self.mesh_materials.append(len(materials) + material_count)
else:
self.mesh_materials.append(material_id + material_count)
print('mesh_materials', self.mesh_materials)
VAO_ids.append(self.get_num_objects() - 1)
#release(scene)
new_obj = VisualObject(obj_path, VAO_ids, len(self.visual_objects),
self)
self.visual_objects.append(new_obj)
return VAO_ids
def set_rotation(self, rot):
self.pose_rot = np.ascontiguousarray(quat2rotmat(rot))
def load_object(self,
obj_path,
scale=np.array([1, 1, 1]),
transform_orn=None,
transform_pos=None,
input_kd=None,
texture_scale=1.0,
load_texture=True):
"""
Load a wavefront obj file into the renderer and create a VisualObject to manage it.
:param obj_path: path of obj file
:param scale: scale, default 1
:param transform_orn: rotation quaternion, convention xyzw
:param transform_pos: translation for loading, it is a list of length 3
:param input_kd: if loading material fails, use this default material. input_kd should be a list of length 3
:param texture_scale: texture scale for the object, downsample to save memory.
:param load_texture: load texture or not
:return: VAO_ids
"""
reader = tinyobjloader.ObjReader()
logging.info("Loading {}".format(obj_path))
ret = reader.ParseFromFile(obj_path)
if ret == False:
logging.error("Warning: {}".format(reader.Warning()))
logging.error("Error: {}".format(reader.Error()))
logging.error("Failed to load: {}".format(obj_path))
sys.exit(-1)
if reader.Warning():
logging.warning("Warning: {}".format(reader.Warning()))
attrib = reader.GetAttrib()
logging.debug("Num vertices = {}".format(len(attrib.vertices)))
logging.debug("Num normals = {}".format(len(attrib.normals)))
logging.debug("Num texcoords = {}".format(len(attrib.texcoords)))
materials = reader.GetMaterials()
logging.debug("Num materials: {}".format(len(materials)))
if logging.root.level <= logging.DEBUG: #Only going into this if it is for logging --> efficiency
for m in materials:
logging.debug("Material name: {}".format(m.name))
logging.debug("Material diffuse: {}".format(m.diffuse))
shapes = reader.GetShapes()
logging.debug("Num shapes: {}".format(len(shapes)))
material_count = len(self.materials_mapping)
materials_fn = {}
for i, item in enumerate(materials):
if item.diffuse_texname != '' and load_texture:
materials_fn[i + material_count] = item.diffuse_texname
obj_dir = os.path.dirname(obj_path)
#texture = loadTexture(os.path.join(dir, item.diffuse_texname), scale=texture_scale)
texture = self.r.loadTexture(
os.path.join(obj_dir, item.diffuse_texname))
self.textures.append(texture)
material = Material('texture', texture_id=texture)
else:
material = Material('color', kd=item.diffuse)
self.materials_mapping[i + material_count] = material
if input_kd is not None: # append the default material in the end, in case material loading fails
self.materials_mapping[len(materials) + material_count] = Material(
'color', kd=input_kd)
else:
self.materials_mapping[len(materials) + material_count] = Material(
'color', kd=[0.5, 0.5, 0.5])
VAO_ids = []
vertex_position = np.array(attrib.vertices).reshape(
(len(attrib.vertices) // 3, 3))
vertex_normal = np.array(attrib.normals).reshape(
(len(attrib.normals) // 3, 3))
vertex_texcoord = np.array(attrib.texcoords).reshape(
(len(attrib.texcoords) // 2, 2))
for shape in shapes:
logging.debug("Shape name: {}".format(shape.name))
material_id = shape.mesh.material_ids[
0] # assume one shape only has one material
logging.debug("material_id = {}".format(material_id))
logging.debug("num_indices = {}".format(len(shape.mesh.indices)))
n_indices = len(shape.mesh.indices)
np_indices = shape.mesh.numpy_indices().reshape((n_indices, 3))
shape_vertex_index = np_indices[:, 0]
shape_normal_index = np_indices[:, 1]
shape_texcoord_index = np_indices[:, 2]
shape_vertex = vertex_position[shape_vertex_index]
if len(vertex_normal) == 0:
shape_normal = np.zeros(
(shape_vertex.shape[0],
3)) #dummy normal if normal is not available
else:
shape_normal = vertex_normal[shape_normal_index]
if len(vertex_texcoord) == 0:
shape_texcoord = np.zeros(
(shape_vertex.shape[0],
2)) #dummy texcoord if texcoord is not available
else:
shape_texcoord = vertex_texcoord[shape_texcoord_index]
vertices = np.concatenate(
[shape_vertex * scale, shape_normal, shape_texcoord], axis=-1)
faces = np.array(range(len(vertices))).reshape(
(len(vertices) // 3, 3))
if not transform_orn is None:
orn = quat2rotmat(xyzw2wxyz(transform_orn))
vertices[:, :3] = vertices[:, :3].dot(orn[:3, :3].T)
if not transform_pos is None:
vertices[:, :3] += np.array(transform_pos)
vertexData = vertices.astype(np.float32)
[VAO,
VBO] = self.r.load_object_meshrenderer(self.shaderProgram,
vertexData)
self.VAOs.append(VAO)
self.VBOs.append(VBO)
self.faces.append(faces)
self.objects.append(obj_path)
self.vertex_data.append(vertexData)
self.shapes.append(shape)
if material_id == -1: # if material loading fails, use the default material
self.mesh_materials.append(len(materials) + material_count)
else:
self.mesh_materials.append(material_id + material_count)
logging.debug('mesh_materials: {}'.format(self.mesh_materials))
VAO_ids.append(self.get_num_objects() - 1)
#release(scene)
new_obj = VisualObject(obj_path, VAO_ids, len(self.visual_objects),
self)
self.visual_objects.append(new_obj)
return VAO_ids
def set_rotation(self, quat):
"""
:param quat: New quaternion in w,x,y,z
"""
self.pose_rot = np.ascontiguousarray(quat2rotmat(quat))
def load_object(self,
obj_path,
scale=np.array([1, 1, 1]),
transform_orn=None,
transform_pos=None,
input_kd=None,
texture_scale=1.0):
scene = load(obj_path)
material_count = len(self.materials_mapping)
materials_fn = {}
for i, item in enumerate(scene.materials):
is_texture = False
kd = [0.5, 0.5, 0.5]
for k, v in item.properties.items():
if k == 'file':
materials_fn[i + material_count] = v
dir = os.path.dirname(obj_path)
texture = loadTexture(os.path.join(dir, v),
scale=texture_scale)
material = Material('texture', texture_id=texture)
self.materials_mapping[i + material_count] = material
self.textures.append(texture)
is_texture = True
if k == 'diffuse':
kd = v
if not is_texture:
self.materials_mapping[i + material_count] = Material('color',
kd=kd)
if not input_kd is None: # urdf material
self.materials_mapping[len(scene.materials) +
material_count] = Material('color',
kd=input_kd)
VAO_ids = []
for mesh in scene.meshes:
faces = mesh.faces
if mesh.normals.shape[0] == 0:
mesh.normals = np.zeros(mesh.vertices.shape,
dtype=mesh.vertices.dtype)
if mesh.texturecoords.shape[0] == 0:
mesh.texturecoords = np.zeros(
(1, mesh.vertices.shape[0], mesh.vertices.shape[1]),
dtype=mesh.vertices.dtype)
vertices = np.concatenate([
mesh.vertices * scale, mesh.normals,
mesh.texturecoords[0, :, :2]
],
axis=-1)
if not transform_orn is None:
orn = quat2rotmat([
transform_orn[-1], transform_orn[0], transform_orn[1],
transform_orn[2]
])
vertices[:, :3] = vertices[:, :3].dot(orn[:3, :3].T)
if not transform_pos is None:
vertices[:, :3] += np.array(transform_pos)
vertexData = vertices.astype(np.float32)
VAO = GL.glGenVertexArrays(1)
GL.glBindVertexArray(VAO)
# Need VBO for triangle vertices and texture UV coordinates
VBO = GL.glGenBuffers(1)
GL.glBindBuffer(GL.GL_ARRAY_BUFFER, VBO)
GL.glBufferData(GL.GL_ARRAY_BUFFER, vertexData.nbytes, vertexData,
GL.GL_STATIC_DRAW)
# enable array and set up data
positionAttrib = GL.glGetAttribLocation(self.shaderProgram,
'position')
normalAttrib = GL.glGetAttribLocation(self.shaderProgram, 'normal')
coordsAttrib = GL.glGetAttribLocation(self.shaderProgram,
'texCoords')
GL.glEnableVertexAttribArray(0)
GL.glEnableVertexAttribArray(1)
GL.glEnableVertexAttribArray(2)
GL.glVertexAttribPointer(positionAttrib, 3, GL.GL_FLOAT,
GL.GL_FALSE, 32, None)
GL.glVertexAttribPointer(normalAttrib, 3, GL.GL_FLOAT, GL.GL_FALSE,
32, ctypes.c_void_p(12))
# the last parameter is a pointer
GL.glVertexAttribPointer(coordsAttrib, 2, GL.GL_FLOAT, GL.GL_TRUE,
32, ctypes.c_void_p(24))
GL.glBindBuffer(GL.GL_ARRAY_BUFFER, 0)
GL.glBindVertexArray(0)
self.VAOs.append(VAO)
self.VBOs.append(VBO)
self.faces.append(faces)
self.objects.append(obj_path)
if mesh.materialindex == 0: # if there is no material, use urdf color as material
self.mesh_materials.append(
len(scene.materials) + material_count)
else:
self.mesh_materials.append(mesh.materialindex + material_count)
VAO_ids.append(self.get_num_objects() - 1)
release(scene)
new_obj = VisualObject(obj_path, VAO_ids, len(self.visual_objects),
self)
self.visual_objects.append(new_obj)
return VAO_ids
