def generateBodies(cloud, id):
# BEGIN addtoenv
names = []
if args.geom_type == "mesh":
mesh = generate_mesh(cloud)
name = "simple_mesh_%i" % id
mesh_body = mk.create_trimesh(env,
get_xyz_world_frame(mesh.getCloud()),
np.array(mesh.getFaces()),
name=name)
mesh_body.SetUserData(
"bt_use_trimesh", True
) # Tell collision checker to use the trimesh rather than the convex hull of it
names.append(name)
elif args.geom_type == "cd":
big_mesh = generate_mesh(cloud)
convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, 30)
for (i, mesh) in enumerate(convex_meshes):
name = "mesh_%i_%i" % (id, i)
verts = get_xyz_world_frame(mesh.getCloud())
mk.create_trimesh(env, verts, mesh.getTriangles(), name=name)
randcolor = np.random.rand(3)
print 'vertices'
print mesh.getVertices()
if not isValidMesh(mesh):
print('Mesh invalid. Removing kinbody')
env.RemoveKinBody(env.GetKinBody(name))
continue
env.GetKinBody(name).GetLinks()[0].GetGeometries(
)[0].SetAmbientColor(randcolor)
env.GetKinBody(name).GetLinks()[0].GetGeometries(
)[0].SetDiffuseColor(randcolor)
names.append(name)
# END addtoenv
elif args.geom_type == "spheres":
raise Exception("don't use spheres--there's a performance issue")
cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
name = "spheres_%i" % id
mk.create_spheres(env, get_xyz_world_frame(cloud_ds), .02, name=name)
names.append(name)
elif args.geom_type == "boxes":
cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
name = "boxes_%i" % id
mk.create_boxes(env, get_xyz_world_frame(cloud_ds), .02, name=name)
names.append(name)
return names
def add_convexified_pointcloud_to_env(sim, pc2, transform=np.eye(4), num_cd_components=20, point_cloud_filter=lambda point: True):
"""
Convexifies point cloud and adds to openrave environment
:param sim: pr2_sim.simulator.Simulator
:param pc2: the point cloud to read from
:type pc2: sensor_msgs.PointCloud2
:param transform: 4x4 np.ndarray of transform for cloud in frame base_link
:param point_cloud_filter: True if keep point
"""
transform_world = sim.transform_from_to(transform, 'base_link', 'world')
full_cloud = pc2_to_cloudprocpy(pc2, transform_world, point_cloud_filter)
cloud = cloudprocpy.downsampleCloud(full_cloud, .005) # .005
big_mesh = generate_mesh(cloud)
convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, num_cd_components)
for i, mesh in enumerate(convex_meshes):
sim.add_kinbody(mesh.getVertices(), mesh.getTriangles(), name='mesh_{0}'.format(i), check_collision=True)
def add_convexified_pointcloud_to_env(sim, pc2, transform=np.eye(4), num_cd_components=20, point_cloud_filter=lambda point: True):
"""
Convexifies point cloud and adds to openrave environment
:param sim: pr2_sim.simulator.Simulator
:param pc2: the point cloud to read from
:type pc2: sensor_msgs.PointCloud2
:param transform: 4x4 np.ndarray of transform for cloud in frame base_link
:param point_cloud_filter: True if keep point
"""
transform_world = sim.transform_from_to(transform, 'base_link', 'world')
full_cloud = pc2_to_cloudprocpy(pc2, transform_world, point_cloud_filter)
cloud = cloudprocpy.downsampleCloud(full_cloud, .005) # .005
big_mesh = generate_mesh(cloud)
convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, num_cd_components)
for i, mesh in enumerate(convex_meshes):
sim.add_kinbody(mesh.getVertices(), mesh.getTriangles(), name='mesh_{0}'.format(i), check_collision=True)
def generateBodies(cloud, id):
# BEGIN addtoenv
names = []
if args.geom_type == "mesh":
mesh = generate_mesh(cloud)
name = "simple_mesh_%i"%id
mesh_body = mk.create_trimesh(env, get_xyz_world_frame(mesh.getCloud()), np.array(mesh.getFaces()), name=name)
mesh_body.SetUserData("bt_use_trimesh", True) # Tell collision checker to use the trimesh rather than the convex hull of it
names.append(name)
elif args.geom_type == "cd":
big_mesh = generate_mesh(cloud)
convex_meshes = cloudprocpy.convexDecompHACD(big_mesh,30)
for (i,mesh) in enumerate(convex_meshes):
name = "mesh_%i_%i" % (id,i)
verts = get_xyz_world_frame(mesh.getCloud())
mk.create_trimesh(env, verts, mesh.getTriangles(), name=name)
randcolor = np.random.rand(3)
print 'vertices'
print mesh.getVertices()
if not isValidMesh(mesh):
print('Mesh invalid. Removing kinbody')
env.RemoveKinBody(env.GetKinBody(name))
continue
env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor)
env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor)
names.append(name)
# END addtoenv
elif args.geom_type == "spheres":
raise Exception("don't use spheres--there's a performance issue")
cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
name = "spheres_%i"%id
mk.create_spheres(env, get_xyz_world_frame(cloud_ds), .02, name=name)
names.append(name)
elif args.geom_type == "boxes":
cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
name = "boxes_%i"%id
mk.create_boxes(env, get_xyz_world_frame(cloud_ds), .02, name=name)
names.append(name)
return names
def generateBodies(self, cloud, id):
# BEGIN addtoenv
names = []
if self.geom_type == "mesh":
mesh = self.generate_mesh(cloud)
name = "simple_mesh_%i"%id
mesh_body = mk.create_trimesh(self.env, self.get_xyz_world_frame(mesh.getCloud()), np.array(mesh.getFaces()), name=name)
mesh_body.SetUserData("bt_use_trimesh", True) # Tell collision checker to use the trimesh rather than the convex hull of it
names.append(name)
elif self.geom_type == "cd":
big_mesh = self.generate_mesh(cloud)
#name = 'big_mesh'
#verts = self.get_xyz_world_frame(big_mesh.getCloud())
#mk.create_trimesh(self.env, verts, big_mesh.getTriangles(), name=name)
convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, self.num_cd_components)
for (i,mesh) in enumerate(convex_meshes):
name = "mesh_%i_%i" % (id,i)
verts = self.get_xyz_world_frame(mesh.getCloud())
mk.create_trimesh(self.env, verts, mesh.getTriangles(), name=name)
randcolor = np.random.rand(3)
self.env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor)
self.env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor)
names.append(name)
# END addtoenv
elif self.geom_type == "spheres":
raise Exception("don't use spheres--there's a performance issue")
cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
name = "spheres_%i"%id
mk.create_spheres(self.env, self.get_xyz_world_frame(cloud_ds), .02, name=name)
names.append(name)
elif self.geom_type == "boxes":
cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
name = "boxes_%i"%id
mk.create_boxes(self.env, self.get_xyz_world_frame(cloud_ds), .02, name=name)
names.append(name)
return names
def generateBodies(self, cloud, id):
# BEGIN addtoenv
names = []
if self.geom_type == "mesh":
mesh = self.generate_mesh(cloud)
name = "simple_mesh_%i"%id
mesh_body = mk.create_trimesh(self.env, self.get_xyz_world_frame(mesh.getCloud()), np.array(mesh.getFaces()), name=name)
mesh_body.SetUserData("bt_use_trimesh", True) # Tell collision checker to use the trimesh rather than the convex hull of it
names.append(name)
elif self.geom_type == "cd":
big_mesh = self.generate_mesh(cloud)
convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, self.num_cd_components)
for (i,mesh) in enumerate(convex_meshes):
name = "mesh_%i_%i" % (id,i)
verts = self.get_xyz_world_frame(mesh.getCloud())
mk.create_trimesh(self.env, verts, mesh.getTriangles(), name=name)
randcolor = np.random.rand(3)
self.env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor)
self.env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor)
names.append(name)
# END addtoenv
elif self.geom_type == "spheres":
raise Exception("don't use spheres--there's a performance issue")
cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
name = "spheres_%i"%id
mk.create_spheres(self.env, self.get_xyz_world_frame(cloud_ds), .02, name=name)
names.append(name)
elif self.geom_type == "boxes":
cloud_ds = cloudprocpy.downsampleCloud(cloud, .04)
name = "boxes_%i"%id
mk.create_boxes(self.env, self.get_xyz_world_frame(cloud_ds), .02, name=name)
names.append(name)
return names
handles = []
try:
import IPython
IPython.lib.inputhook.set_inputhook(viewer.Step)
except Exception:
pass
# BEGIN addtoenv
if args.geom_type == "mesh":
mesh = generate_mesh(cloud_orig)
mesh_body = mk.create_trimesh(env, get_xyz_world_frame(mesh.getCloud()), np.array(mesh.getFaces()), name="simple_mesh")
mesh_body.SetUserData("bt_use_trimesh", True) # Tell collision checker to use the trimesh rather than the convex hull of it
elif args.geom_type == "cd":
big_mesh = generate_mesh(cloud_orig)
convex_meshes = cloudprocpy.convexDecompHACD(big_mesh,30)
for (i,mesh) in enumerate(convex_meshes):
name = "mesh%i"%i
verts = get_xyz_world_frame(mesh.getCloud())
mk.create_trimesh(env, verts, mesh.getTriangles(), name=name)
randcolor = np.random.rand(3)
env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(randcolor)
env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(randcolor)
# END addtoenv
elif args.geom_type == "spheres":
raise Exception("don't use spheres--there's a performance issue")
cloud_nofloor = remove_floor(cloud_orig)
cloud_ds = cloudprocpy.downsampleCloud(cloud_nofloor, .04)
mk.create_spheres(env, get_xyz_world_frame(cloud_ds), .02)
elif args.geom_type == "boxes":
cloud_nofloor = remove_floor(cloud_orig)
handles = []
# BEGIN addtoenv
if args.geom_type == "mesh":
mesh = generate_mesh(cloud_orig)
mesh_body = mk.create_trimesh(env,
get_xyz_world_frame(mesh.getCloud()),
np.array(mesh.getFaces()),
name="simple_mesh")
mesh_body.SetUserData(
"bt_use_trimesh", True
) # Tell collision checker to use the trimesh rather than the convex hull of it
elif args.geom_type == "cd":
big_mesh = generate_mesh(cloud_orig)
convex_meshes = cloudprocpy.convexDecompHACD(big_mesh, 30)
for (i, mesh) in enumerate(convex_meshes):
name = "mesh%i" % i
verts = get_xyz_world_frame(mesh.getCloud())
mk.create_trimesh(env, verts, mesh.getTriangles(), name=name)
randcolor = np.random.rand(3)
env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetAmbientColor(
randcolor)
env.GetKinBody(name).GetLinks()[0].GetGeometries()[0].SetDiffuseColor(
randcolor)
# END addtoenv
elif args.geom_type == "spheres":
raise Exception("don't use spheres--there's a performance issue")
cloud_nofloor = remove_floor(cloud_orig)
cloud_ds = cloudprocpy.downsampleCloud(cloud_nofloor, .04)
mk.create_spheres(env, get_xyz_world_frame(cloud_ds), .02)