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
def addCloudToEnvironment(self, cloud, cubeSize=0.02): ''' Add a point cloud to the OpenRAVE environment as a collision obstacle. @param cloud: list of 3-tuples, [(x1,y1,z1),...,(xn,yn,zn)] ''' # remove existing cloud, if there is one self.removeCloudFromEnvironment() # convert point cloud to expected format cloud = ascontiguousarray(cloud) # cloudprocpy assumes this cloud = concatenate((cloud, zeros((cloud.shape[0], 1))), axis=1) cloudTrajopt = cloudprocpy.CloudXYZ() cloudTrajopt.from2dArray(cloud) # downsample and add to environment dsCloud = cloudprocpy.downsampleCloud(cloudTrajopt, cubeSize) self.obstacleCloud = make_kinbodies.create_boxes( self.env, dsCloud.to2dArray()[:, :3], cubeSize / 2.0)
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) cloud_ds = cloudprocpy.downsampleCloud(cloud_nofloor, .04) mk.create_boxes(env, get_xyz_world_frame(cloud_ds), .02) def full_body_drive_and_reach(robot, link_name, xyz_targ, quat_targ): request = { "basic_info" : { "n_steps" : 10, "manip" : "active", "start_fixed" : True }, "costs" : [ { "type" : "collision", "params" : {"coeffs" : [10],"dist_pen" : [0.025]} },
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) cloud_ds = cloudprocpy.downsampleCloud(cloud_nofloor, .04) mk.create_boxes(env, get_xyz_world_frame(cloud_ds), .02) def full_body_drive_and_reach(link_name, xyz_targ, quat_targ): request = { "basic_info": { "n_steps": 10, "manip": "active", "start_fixed": True }, "costs": [{ "type": "collision", "params": { "coeffs": [10], "dist_pen": [0.025]