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(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
robot.SetDOFValues(dof_vals)
viewer = trajoptpy.GetViewer(env)
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)
point_cloud = cloudprocpy.readPCDXYZ("cloud.pcd")
"""
elif (point_cloud == None) and (point_cloud_2 == None):
print "No point cloud"
elif point_cloud_2 != None:
point_cloud = point_cloud_2
"""
#print "Point Cloud ROS: "
#print point_cloud_2
print "Cloud Grabber: "
print xyzrgb
#cloud_orig = cloudprocpy.readPCDXYZ("cloud.pcd")
mesh = generate_mesh(point_cloud)
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
print "After making mesh"
joint_start = resp.position
print "Joint start: " + str(joint_start)
robot.SetDOFValues(joint_start) # robot.GetManipulator('rightarm').GetArmIndices())
time.sleep(3)
roll = 0.0
pitch = 0.0
yaw = 0
quat = tf.transformations.quaternion_from_euler(roll, pitch, yaw)
quat_target = [quat[3], quat[0], quat[1], quat[2]] # wxyz
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)
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(