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(