def main(): model_id = 'Rs' trav_map_original_size = 1000 trav_map_size = 200 trav_map_erosion = 2 floor_map = [] floor_graph = [] with open(os.path.join(get_model_path(model_id), 'floors.txt'), 'r') as f: floors = sorted(list(map(float, f.readlines()))) print('floors', floors) for f in range(len(floors)): trav_map = Image.open( os.path.join(get_model_path(model_id), 'floor_trav_{}.png'.format(f))) obstacle_map = Image.open( os.path.join(get_model_path(model_id), 'floor_{}.png'.format(f))) trav_map = np.array(trav_map.resize((trav_map_size, trav_map_size))) obstacle_map = np.array( obstacle_map.resize((trav_map_size, trav_map_size))) trav_map[obstacle_map == 0] = 0 trav_map = cv2.erode(trav_map, np.ones((trav_map_erosion, trav_map_erosion))) plt.figure(f, figsize=(12, 12)) plt.imshow(trav_map) plt.show()
def load_scene_mesh(self): """ Load scene mesh """ if self.is_interactive: filename = os.path.join(get_model_path(self.model_id), "mesh_z_up_cleaned.obj") else: filename = os.path.join(get_model_path(self.model_id), "mesh_z_up_downsampled.obj") if not os.path.isfile(filename): filename = os.path.join(get_model_path(self.model_id), "mesh_z_up.obj") collision_id = p.createCollisionShape( p.GEOM_MESH, fileName=filename, flags=p.GEOM_FORCE_CONCAVE_TRIMESH) if self.pybullet_load_texture: visual_id = p.createVisualShape(p.GEOM_MESH, fileName=filename) texture_filename = get_texture_file(filename) texture_id = p.loadTexture(texture_filename) else: visual_id = -1 texture_id = -1 self.mesh_body_id = p.createMultiBody( baseCollisionShapeIndex=collision_id, baseVisualShapeIndex=visual_id) p.changeDynamics(self.mesh_body_id, -1, lateralFriction=1) if self.pybullet_load_texture: p.changeVisualShape(self.mesh_body_id, -1, textureUniqueId=texture_id)
def main(): if len(sys.argv) > 1: model_path = sys.argv[1] else: model_path = os.path.join(get_model_path('Rs'), 'mesh_z_up.obj') renderer = MeshRendererG2G(width=512, height=512, device_idx=0) renderer.load_object(model_path) renderer.add_instance(0) print(renderer.visual_objects, renderer.instances) print(renderer.materials_mapping, renderer.mesh_materials) camera_pose = np.array([0, 0, 1.2]) view_direction = np.array([1, 0, 0]) renderer.set_camera(camera_pose, camera_pose + view_direction, [0, 0, 1]) renderer.set_fov(90) for i in range(3000): with Profiler('Render'): frame = renderer.render(modes=('rgb', 'normal', '3d')) print(frame) img_np = frame[0].flip(0).data.cpu().numpy().reshape( renderer.height, renderer.width, 4) normal_np = frame[1].flip(0).data.cpu().numpy().reshape( renderer.height, renderer.width, 4) plt.imshow(np.concatenate([img_np, normal_np], axis=1)) plt.show()
def load_floor_metadata(self): """ Load floor metadata """ floor_height_path = os.path.join(get_model_path(self.model_id), 'floors.txt') if not os.path.isfile(floor_height_path): raise Exception('floors.txt cannot be found in model: {}'.format( self.model_id)) with open(floor_height_path, 'r') as f: self.floors = sorted(list(map(float, f.readlines()))) logging.debug('Floors {}'.format(self.floors))
def main(): if len(sys.argv) > 1: model_path = sys.argv[1] else: model_path = os.path.join(get_model_path('Rs'), 'mesh_z_up.obj') p.connect(p.GUI) p.setGravity(0, 0, -9.8) p.setTimeStep(1. / 240.) # Load scenes collision_id = p.createCollisionShape(p.GEOM_MESH, fileName=model_path, meshScale=1.0, flags=p.GEOM_FORCE_CONCAVE_TRIMESH) visual_id = p.createVisualShape(p.GEOM_MESH, fileName=model_path, meshScale=1.0) texture_filename = get_texture_file(model_path) texture_id = p.loadTexture(texture_filename) mesh_id = p.createMultiBody(baseCollisionShapeIndex=collision_id, baseVisualShapeIndex=visual_id) # Load robots turtlebot_urdf = os.path.join(gibson2.assets_path, 'models/turtlebot/turtlebot.urdf') robot_id = p.loadURDF(turtlebot_urdf, flags=p.URDF_USE_MATERIAL_COLORS_FROM_MTL) # Load objects obj_visual_filename = os.path.join( gibson2.assets_path, 'models/ycb/002_master_chef_can/textured_simple.obj') obj_collision_filename = os.path.join( gibson2.assets_path, 'models/ycb/002_master_chef_can/textured_simple_vhacd.obj') collision_id = p.createCollisionShape(p.GEOM_MESH, fileName=obj_collision_filename, meshScale=1.0) visual_id = p.createVisualShape(p.GEOM_MESH, fileName=obj_visual_filename, meshScale=1.0) object_id = p.createMultiBody(baseCollisionShapeIndex=collision_id, baseVisualShapeIndex=visual_id, basePosition=[1.0, 0.0, 1.0], baseMass=0.1) for _ in range(10000): p.stepSimulation() p.disconnect()
def load_floor_planes(self): if self.is_interactive: for f in range(len(self.floors)): # load the floor plane with the original floor texture for each floor plane_name = os.path.join(get_model_path(self.model_id), "plane_z_up_{}.obj".format(f)) collision_id = p.createCollisionShape(p.GEOM_MESH, fileName=plane_name) visual_id = p.createVisualShape(p.GEOM_MESH, fileName=plane_name) texture_filename = get_texture_file(plane_name) if texture_filename is not None: texture_id = p.loadTexture(texture_filename) else: texture_id = -1 floor_body_id = p.createMultiBody( baseCollisionShapeIndex=collision_id, baseVisualShapeIndex=visual_id) if texture_id != -1: p.changeVisualShape(floor_body_id, -1, textureUniqueId=texture_id) floor_height = self.floors[f] p.resetBasePositionAndOrientation(floor_body_id, posObj=[0, 0, floor_height], ornObj=[0, 0, 0, 1]) # Since both the floor plane and the scene mesh have mass 0 (static), # PyBullet seems to have already disabled collision between them. # Just to be safe, explicit disable collision between them. p.setCollisionFilterPair(self.mesh_body_id, floor_body_id, -1, -1, enableCollision=0) self.floor_body_ids.append(floor_body_id) else: # load the default floor plane (only once) and later reset it to different floor heiights plane_name = os.path.join(pybullet_data.getDataPath(), "mjcf/ground_plane.xml") floor_body_id = p.loadMJCF(plane_name)[0] p.resetBasePositionAndOrientation(floor_body_id, posObj=[0, 0, 0], ornObj=[0, 0, 0, 1]) p.setCollisionFilterPair(self.mesh_body_id, floor_body_id, -1, -1, enableCollision=0) self.floor_body_ids.append(floor_body_id)
def main(): if len(sys.argv) > 1: model_path = sys.argv[1] else: model_path = os.path.join(get_model_path('Rs'), 'mesh_z_up.obj') renderer = MeshRenderer(width=512, height=512) renderer.load_object(model_path) renderer.add_instance(0) camera_pose = np.array([0, 0, 1.2]) view_direction = np.array([1, 0, 0]) renderer.set_camera(camera_pose, camera_pose + view_direction, [0, 0, 1]) renderer.set_fov(90) frames = renderer.render(modes=('rgb', 'normal', '3d')) frames = cv2.cvtColor(np.concatenate(frames, axis=1), cv2.COLOR_RGB2BGR) cv2.imshow('image', frames) cv2.waitKey(0)
def load_scene_objects(self): if not self.is_interactive: return self.scene_objects = [] self.scene_objects_pos = [] scene_path = get_model_path(self.model_id) urdf_files = [ item for item in os.listdir(scene_path) if item[-4:] == 'urdf' and item != 'scene.urdf' ] position_files = [ item[:-4].replace('alignment_centered', 'pos') + 'txt' for item in urdf_files ] for urdf_file, position_file in zip(urdf_files, position_files): logging.info('Loading urdf file {}'.format(urdf_file)) with open(os.path.join(scene_path, position_file)) as f: pos = np.array( [float(item) for item in f.readlines()[0].strip().split()]) obj = InteractiveObj(os.path.join(scene_path, urdf_file)) obj.load() self.scene_objects.append(obj) self.scene_objects_pos.append(pos)
def has_scene_urdf(self): return os.path.exists( os.path.join(get_model_path(self.model_id), 'scene.urdf'))
def load_scene_urdf(self): self.mesh_body_id = p.loadURDF( os.path.join(get_model_path(self.model_id), 'scene.urdf'))
def load_trav_map(self): self.floor_map = [] self.floor_graph = [] for f in range(len(self.floors)): trav_map = np.array( Image.open( os.path.join(get_model_path(self.model_id), 'floor_trav_{}.png'.format(f)))) obstacle_map = np.array( Image.open( os.path.join(get_model_path(self.model_id), 'floor_{}.png'.format(f)))) if self.trav_map_original_size is None: height, width = trav_map.shape assert height == width, 'trav map is not a square' self.trav_map_original_size = height self.trav_map_size = int(self.trav_map_original_size * self.trav_map_default_resolution / self.trav_map_resolution) trav_map[obstacle_map == 0] = 0 trav_map = cv2.resize(trav_map, (self.trav_map_size, self.trav_map_size)) trav_map = cv2.erode( trav_map, np.ones((self.trav_map_erosion, self.trav_map_erosion))) trav_map[trav_map < 255] = 0 if self.build_graph: graph_file = os.path.join(get_model_path(self.model_id), 'floor_trav_{}.p'.format(f)) if os.path.isfile(graph_file): logging.info("Loading traversable graph") with open(graph_file, 'rb') as pfile: g = pickle.load(pfile) else: logging.info("Building traversable graph") g = nx.Graph() for i in range(self.trav_map_size): for j in range(self.trav_map_size): if trav_map[i, j] > 0: g.add_node((i, j)) # 8-connected graph neighbors = [(i - 1, j - 1), (i, j - 1), (i + 1, j - 1), (i - 1, j)] for n in neighbors: if 0 <= n[0] < self.trav_map_size and 0 <= n[ 1] < self.trav_map_size and trav_map[ n[0], n[1]] > 0: g.add_edge(n, (i, j), weight=l2_distance( n, (i, j))) # only take the largest connected component largest_cc = max(nx.connected_components(g), key=len) g = g.subgraph(largest_cc).copy() with open(graph_file, 'wb') as pfile: pickle.dump(g, pfile, protocol=pickle.HIGHEST_PROTOCOL) self.floor_graph.append(g) # update trav_map accordingly trav_map[:, :] = 0 for node in g.nodes: trav_map[node[0], node[1]] = 255 self.floor_map.append(trav_map)
def main(): global _mouse_ix, _mouse_iy, down, view_direction if len(sys.argv) > 1: model_path = sys.argv[1] else: model_path = os.path.join(get_model_path('Rs'), 'mesh_z_up.obj') renderer = MeshRenderer(width=512, height=512) renderer.load_object(model_path) renderer.add_instance(0) print(renderer.visual_objects, renderer.instances) print(renderer.materials_mapping, renderer.mesh_materials) camera_pose = np.array([0, 0, 1.2]) view_direction = np.array([1, 0, 0]) renderer.set_camera(camera_pose, camera_pose + view_direction, [0, 0, 1]) renderer.set_fov(90) px = 0 py = 0 _mouse_ix, _mouse_iy = -1, -1 down = False def change_dir(event, x, y, flags, param): global _mouse_ix, _mouse_iy, down, view_direction if event == cv2.EVENT_LBUTTONDOWN: _mouse_ix, _mouse_iy = x, y down = True if event == cv2.EVENT_MOUSEMOVE: if down: dx = (x - _mouse_ix) / 100.0 dy = (y - _mouse_iy) / 100.0 _mouse_ix = x _mouse_iy = y r1 = np.array([[np.cos(dy), 0, np.sin(dy)], [0, 1, 0], [-np.sin(dy), 0, np.cos(dy)]]) r2 = np.array([[np.cos(-dx), -np.sin(-dx), 0], [np.sin(-dx), np.cos(-dx), 0], [0, 0, 1]]) view_direction = r1.dot(r2).dot(view_direction) elif event == cv2.EVENT_LBUTTONUP: down = False cv2.namedWindow('test') cv2.setMouseCallback('test', change_dir) while True: with Profiler('Render'): frame = renderer.render(modes=('rgb', 'normal', '3d')) cv2.imshow( 'test', cv2.cvtColor(np.concatenate(frame, axis=1), cv2.COLOR_RGB2BGR)) q = cv2.waitKey(1) if q == ord('w'): px += 0.05 elif q == ord('s'): px -= 0.05 elif q == ord('a'): py += 0.05 elif q == ord('d'): py -= 0.05 elif q == ord('q'): break camera_pose = np.array([px, py, 1.2]) renderer.set_camera(camera_pose, camera_pose + view_direction, [0, 0, 1]) renderer.release()
def main(): global _mouse_ix, _mouse_iy, down, view_direction ################################################################################### # load the scene to get traversable area (disable the whole area for performance) # ################################################################################### p.connect(p.GUI) p.setGravity(0, 0, -9.8) p.setTimeStep(1. / 240.) scene = BuildingScene('Allensville', build_graph=True, pybullet_load_texture=True) objects = scene.load() random_floor = scene.get_random_floor() p1 = scene.get_random_point_floor(random_floor)[1] p2 = scene.get_random_point_floor(random_floor)[1] shortest_path, geodesic_distance = scene.get_shortest_path( random_floor, p1[:2], p2[:2], entire_path=True) print('random point 1:', p1) print('random point 2:', p2) print('geodesic distance between p1 and p2', geodesic_distance) print('shortest path from p1 to p2:', shortest_path) p.disconnect() ################################################################################### if len(sys.argv) > 1: model_path = sys.argv[1] else: model_path = os.path.join(get_model_path('Allensville'), 'mesh_z_up.obj') # set width, height (has to be equal for panorama) renderer = MeshRenderer(width=512, height=512) renderer.load_object(model_path) renderer.add_instance(0) print(renderer.visual_objects, renderer.instances) print(renderer.materials_mapping, renderer.mesh_materials) # set camera_pose / view direction camera_pose = np.array([0, 0, 1.2]) view_direction = np.array([1, 0, 0]) renderer.set_camera(camera_pose, camera_pose + view_direction, [0, 0, 1]) renderer.set_fov(90) px = 0 py = 0 _mouse_ix, _mouse_iy = -1, -1 down = False def change_dir(event, x, y, flags, param): global _mouse_ix, _mouse_iy, down, view_direction if event == cv2.EVENT_LBUTTONDOWN: _mouse_ix, _mouse_iy = x, y down = True if event == cv2.EVENT_MOUSEMOVE: if down: dx = (x - _mouse_ix) / 100.0 dy = (y - _mouse_iy) / 100.0 _mouse_ix = x _mouse_iy = y r1 = np.array([[np.cos(dy), 0, np.sin(dy)], [0, 1, 0], [-np.sin(dy), 0, np.cos(dy)]]) r2 = np.array([[np.cos(-dx), -np.sin(-dx), 0], [np.sin(-dx), np.cos(-dx), 0], [0, 0, 1]]) view_direction = r1.dot(r2).dot(view_direction) elif event == cv2.EVENT_LBUTTONUP: down = False cv2.namedWindow('test') cv2.setMouseCallback('test', change_dir) while True: with Profiler('Render'): frame = renderer.render(modes=('rgb', 'normal', '3d')) cv2.imshow( 'test', cv2.cvtColor(np.concatenate(frame, axis=1), cv2.COLOR_RGB2BGR)) q = cv2.waitKey(1) if q == ord('w'): px += 0.05 elif q == ord('s'): px -= 0.05 elif q == ord('a'): py += 0.05 elif q == ord('d'): py -= 0.05 elif q == ord('q'): break camera_pose = np.array([px, py, 1.2]) renderer.set_camera(camera_pose, camera_pose + view_direction, [0, 0, 1]) renderer.release()