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