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
