env.StopSimulation() env.Load("robots/pr2-beta-static.zae") robot = env.GetRobots()[0] robot.SetDOFValues(dof_vals) viewer = trajoptpy.GetViewer(env) try: import IPython IPython.lib.inputhook.set_inputhook(viewer.Step) except Exception: pass # Plotting original point cloud cloud_orig_colored = cloudprocpy.readPCDXYZRGB( osp.join(trajoptpy.bigdata_dir, args.scene_name, "cloud.pcd")) rgbfloats = cloud_orig_colored.to2dArray()[:, 4] rgb0 = np.ndarray(buffer=rgbfloats.copy(), shape=(480 * 640, 4), dtype='uint8') xyz = get_xyz_world_frame(cloud_orig) goodinds = np.isfinite(xyz[:, 0]) cloud_handle = env.plot3(xyz[goodinds, :], 2, (rgb0[goodinds, :3][:, ::-1]) / 255.) viewer.Idle() del cloud_handle ###### handles = [] # BEGIN addtoenv if args.geom_type == "mesh": mesh = generate_mesh(cloud_orig)
env.Load("robots/pr2-beta-static.zae") robot = env.GetRobots()[0] robot.SetDOFValues(dof_vals) viewer = trajoptpy.GetViewer(env) try: import IPython IPython.lib.inputhook.set_inputhook(viewer.Step) except Exception: pass # Plotting original point cloud cloud_orig_colored = cloudprocpy.readPCDXYZRGB(osp.join(trajoptpy.bigdata_dir,args.scene_name,"cloud.pcd")) rgbfloats = cloud_orig_colored.to2dArray()[:,4] rgb0 = np.ndarray(buffer=rgbfloats.copy(),shape=(480*640,4),dtype='uint8') xyz=get_xyz_world_frame(cloud_orig) goodinds = np.isfinite(xyz[:,0]) cloud_handle = env.plot3(xyz[goodinds,:], 2,(rgb0[goodinds,:3][:,::-1])/255. ) viewer.Idle() del cloud_handle ###### handles = [] # BEGIN addtoenv if args.geom_type == "mesh": mesh = generate_mesh(cloud_orig)
return simple_mesh env = openravepy.Environment() env.StopSimulation() filenames = [] for root, dirs, files in os.walk('/home/annal/src/RavenDebridement/pc_data/'): for f in files: if f.endswith('.pcd'): filenames.append(f) break filenames.sort() cloud_orig = cloudprocpy.readPCDXYZ(osp.join('/home/annal/src/RavenDebridement/pc_data/', filenames[len(files)-1])) mesh = generate_mesh(cloud_orig) mesh_body = mk.create_trimesh(env, get_xyz_world_frame(mesh.getCloud()), np.array(mesh.getFaces()), name="simple_mesh") viewer = trajoptpy.GetViewer(env) cloud_orig_colored = cloudprocpy.readPCDXYZRGB(osp.join('/home/annal/src/RavenDebridement/pc_data/', filenames[len(files)-1])) rgbfloats = cloud_orig_colored.to2dArray()[:,4] rgb0 = np.ndarray(buffer=rgbfloats.copy(),shape=(480*640,4),dtype='uint8') xyz=get_xyz_world_frame(cloud_orig) goodinds = np.isfinite(xyz[:,0]) cloud_handle = env.plot3(xyz[goodinds,:], 2,(rgb0[goodinds,:3][:,::-1])/255. ) viewer.Idle() del cloud_handle