def read_datum(self, key): """Read in the GraspableObject3D corresponding to given key.""" if key not in self.data_keys_: raise ValueError('Key %s not found in dataset %s' % (key, self.name)) file_root = os.path.join(self.dataset_root_dir_, key) sdf_filename = Dataset.sdf_filename(file_root) obj_filename = Dataset.obj_filename(file_root) features_filename = Dataset.features_filename(file_root) # read in data sf = sdf_file.SdfFile(sdf_filename) sdf = sf.read() of = obj_file.ObjFile(obj_filename) mesh = of.read() if os.path.exists(features_filename): ff = feature_file.LocalFeatureFile(features_filename) features = ff.read() else: features = None return go.GraspableObject3D(sdf, mesh=mesh, features=features, key=key, model_name=obj_filename, category=self.data_categories_[key])
def test_gaussian_grasp_sampling(vis=False): np.random.seed(100) h = plt.figure() ax = h.add_subplot(111, projection='3d') sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf' sf = sdf_file.SdfFile(sdf_3d_file_name) sdf_3d = sf.read() mesh_name = 'data/test/meshes/Co_clean.obj' of = obj_file.ObjFile(mesh_name) m = of.read() graspable = graspable_object.GraspableObject3D(sdf_3d, mesh=m, model_name=mesh_name) config_file = 'cfg/correlated.yaml' config = ec.ExperimentConfig(config_file) sampler = GaussianGraspSampler(config) start_time = time.clock() grasps = sampler.generate_grasps(graspable, target_num_grasps=200, vis=False) end_time = time.clock() duration = end_time - start_time logging.info('Gaussian grasp candidate generation took %f sec' % (duration))
def test_grasp_collisions(): np.random.seed(100) h = plt.figure() ax = h.add_subplot(111, projection='3d') sdf_3d_file_name = 'data/test/sdf/Co.sdf' # sdf_3d_file_name = '/mnt/terastation/shape_data/MASTER_DB_v0/amazon_picking_challenge/dove_beauty_bar.sdf' sf = sdf_file.SdfFile(sdf_3d_file_name) sdf_3d = sf.read() mesh_name = 'data/test/meshes/Co.obj' # mesh_name = '/mnt/terastation/shape_data/MASTER_DB_v0/amazon_picking_challenge/dove_beauty_bar.obj' of = obj_file.ObjFile(mesh_name) m = of.read() graspable = graspable_object.GraspableObject3D(sdf_3d, mesh=m, model_name=mesh_name) rave.raveSetDebugLevel(rave.DebugLevel.Error) grasp_checker = OpenRaveGraspChecker() center = np.array([0, 0, 0.02]) axis = np.array([1, 0, 0]) axis = axis / np.linalg.norm(axis) width = 0.1 grasp = g.ParallelJawPtGrasp3D(center, axis, width) grasp.close_fingers(graspable, vis=True) grasp_checker.prune_grasps_in_collision(graspable, [grasp], auto_step=True, close_fingers=True, delay=30)
def test_grasp_from_contacts(): """ Should visually check for reasonable contacts (large green circles) """ np.random.seed(100) sdf_3d_file_name = 'data/test/sdf/Co_clean_dim_25.sdf' sf3 = sf.SdfFile(sdf_3d_file_name) sdf_3d = sf3.read() # create point on sdf surface obj_3d = go.GraspableObject3D(sdf_3d) surf_pts, surf_sdf = obj_3d.sdf.surface_points() rand_pt_ind = np.random.choice(surf_pts.shape[0]) rand_surf_pt_grid = surf_pts[rand_pt_ind, :] rand_surf_pt = obj_3d.sdf.transform_pt_grid_to_obj(rand_surf_pt_grid) # get grasp direction axis = -obj_3d.sdf.gradient(rand_surf_pt) axis = axis / np.linalg.norm(axis) axis = obj_3d.sdf.transform_pt_grid_to_obj(axis, direction=True) plt.figure() test_grasp_width = 0.8 ax = plt.gca(projection='3d') ax.scatter(rand_surf_pt_grid[0], rand_surf_pt_grid[1], rand_surf_pt_grid[2], s=80, c=u'g') g, _ = ParallelJawPtGrasp3D.grasp_from_contact_and_axis_on_grid( obj_3d, rand_surf_pt, axis, test_grasp_width, vis=True) plt.show()
def test_plot_friction_cone(): import sdf_file, obj_file, grasp as g, graspable_object np.random.seed(100) mesh_file_name = 'data/test/meshes/Co_clean.obj' sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf' sdf = sdf_file.SdfFile(sdf_3d_file_name).read() mesh = obj_file.ObjFile(mesh_file_name).read() graspable = graspable_object.GraspableObject3D(sdf, mesh) grasp_axis = np.array([0, 1, 0]) grasp_width = 0.1 grasp_center = np.array([0, 0, -0.025]) grasp = g.ParallelJawPtGrasp3D( ParallelJawPtGrasp3D.configuration_from_params(grasp_center, grasp_axis, 0, grasp_width, 0)) _, (c1, c2) = grasp.close_fingers(graspable) plt.figure() c1_proxy = c1.plot_friction_cone(color='m') c2_proxy = c2.plot_friction_cone(color='y') plt.legend([c1_proxy, c2_proxy], ['Cone 1', 'Cone 2']) plt.show() IPython.embed()
def getprobability(object, grasps): obj_name = object[1] sdf_name = object[2] obj_mesh = of.ObjFile(obj_name).read() sdf_ = sf.SdfFile(sdf_name).read() obj = go.GraspableObject3D(sdf_, mesh=obj_mesh, key=object[0].replace("_features.txt", ""), model_name=obj_name) config_name = "cfg/correlated.yaml" config = ec.ExperimentConfig(config_name) np.random.seed(100) brute_force_iter = config['bandit_brute_force_iter'] max_iter = config['bandit_max_iter'] confidence = config['bandit_confidence'] snapshot_rate = config['bandit_snapshot_rate'] tc_list = [ tc.MaxIterTerminationCondition(max_iter), # tc.ConfidenceTerminationCondition(confidence) ] # run bandits! graspable_rv = pfc.GraspableObjectGaussianPose(obj, config) f_rv = scipy.stats.norm(config['friction_coef'], config['sigma_mu']) # friction Gaussian RV # compute feature vectors for all grasps feature_extractor = ff.GraspableFeatureExtractor(obj, config) all_features = feature_extractor.compute_all_features(grasps) candidates = [] for grasp, features in zip(grasps, all_features): grasp_rv = pfc.ParallelJawGraspGaussian(grasp, config) pfc_rv = pfc.ForceClosureRV(grasp_rv, graspable_rv, f_rv, config) if features is None: pass else: pfc_rv.set_features(features) candidates.append(pfc_rv) def phi(rv): return rv.features nn = kernels.KDTree(phi=phi) kernel = kernels.SquaredExponentialKernel(sigma=config['kernel_sigma'], l=config['kernel_l'], phi=phi) objective = objectives.RandomBinaryObjective() # uniform allocation for true values ua = das.UniformAllocationMean(objective, candidates) ua_result = ua.solve( termination_condition=tc.MaxIterTerminationCondition(brute_force_iter), snapshot_rate=snapshot_rate) estimated_pfc = models.BetaBernoulliModel.beta_mean( ua_result.models[-1].alphas, ua_result.models[-1].betas) return estimated_pfc
def graspable(self, key): """Read in the GraspableObject3D corresponding to given key.""" if key not in self.object_keys: raise ValueError('Key %s not found in dataset %s' % (key, self.name)) # read in data (need new interfaces for this.... sdf = hfact.Hdf5ObjectFactory.sdf_3d(self.sdf_data(key)) mesh = hfact.Hdf5ObjectFactory.mesh_3d(self.mesh_data(key)) mass = self.object(key).attrs[MASS_KEY] return go.GraspableObject3D(sdf, mesh=mesh, key=key, model_name=self.obj_mesh_filename(key), mass=mass)
def test_antipodal_grasp_sampling(vis=False): np.random.seed(100) h = plt.figure() ax = h.add_subplot(111, projection='3d') sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf' sf = sdf_file.SdfFile(sdf_3d_file_name) sdf_3d = sf.read() mesh_name = 'data/test/meshes/Co_clean.obj' of = obj_file.ObjFile(mesh_name) m = of.read() graspable = graspable_object.GraspableObject3D(sdf_3d, mesh=m, model_name=mesh_name) config_file = 'cfg/correlated.yaml' config = ec.ExperimentConfig(config_file) sampler = AntipodalGraspSampler(config) start_time = time.clock() grasps = sampler.generate_grasps(graspable, vis=False) end_time = time.clock() duration = end_time - start_time logging.info('Antipodal grasp candidate generation took %f sec' % (duration)) if vis: plt.close() # lol for i, grasp in enumerate(grasps, 1): plt.figure() ax = plt.gca(projection='3d') found, (c1, c2) = grasp.close_fingers(graspable) c1_proxy = c1.plot_friction_cone(color='m') c2_proxy = c2.plot_friction_cone(color='y') ax.set_xlim([5, 20]) ax.set_ylim([5, 20]) ax.set_zlim([5, 20]) plt.title('Grasp %d' % (i)) plt.axis('off') plt.show(block=False) for angle in range(0, 360, 10): ax.view_init(elev=5.0, azim=angle) plt.draw() plt.close()
def test_find_contacts(): """ Should visually check for reasonable contacts (large green circles) """ sdf_3d_file_name = 'data/test/sdf/Co_clean_dim_25.sdf' sf3 = sf.SdfFile(sdf_3d_file_name) sdf_3d = sf3.read() # create grasp test_grasp_center = np.zeros(3) test_grasp_axis = np.array([1, 0, 0]) test_grasp_width = 1.0 obj_3d = go.GraspableObject3D(sdf_3d) grasp = ParallelJawPtGrasp3D(test_grasp_center, test_grasp_axis, test_grasp_width) contact_found, _ = grasp.close_fingers(obj_3d, vis=True) plt.ioff() plt.show() assert(contact_found)
def test_feature_extraction(): # load object sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf' sf = sdf_file.SdfFile(sdf_3d_file_name) sdf_3d = sf.read() mesh_name = 'data/test/meshes/Co_clean.obj' of = obj_file.ObjFile(mesh_name) m = of.read() graspable = go.GraspableObject3D(sdf_3d, mesh=m, model_name=mesh_name) shot_features_name = 'data/test/features/Co_clean_features.txt' feature_extractor = SHOTFeatureExtractor() feature_extractor.extract(graspable, shot_features_name) IPython.embed()
def grasp_model_figure(): np.random.seed(100) h = plt.figure() ax = h.add_subplot(111, projection='3d') sdf_3d_file_name = '/mnt/terastation/shape_data/MASTER_DB_v2/Cat50_ModelDatabase/5c7bf45b0f847489181be2d6e974dccd.sdf' sf = sdf_file.SdfFile(sdf_3d_file_name) sdf_3d = sf.read() mesh_name = '/mnt/terastation/shape_data/MASTER_DB_v2/Cat50_ModelDatabase/5c7bf45b0f847489181be2d6e974dccd.obj' of = obj_file.ObjFile(mesh_name) m = of.read() clean_mesh_name = 'data/test/meshes/flashlight.obj' mc = mesh_cleaner.MeshCleaner(m) mc.rescale_vertices(0.07) oof = obj_file.ObjFile(clean_mesh_name) oof.write(mc.mesh()) graspable = graspable_object.GraspableObject3D( sdf_3d, mesh=m, model_name=clean_mesh_name, tf=stf.SimilarityTransform3D(tfx.identity_tf(), 0.75)) rave.raveSetDebugLevel(rave.DebugLevel.Error) grasp_checker = OpenRaveGraspChecker() center = np.array([0, 0.01, 0]) axis = np.array([1, 0, 0]) axis = axis / np.linalg.norm(axis) width = 0.1 grasp = g.ParallelJawPtGrasp3D(center, axis, width) rotated_grasps = grasp.transform(graspable.tf, 2.0 * np.pi / 20.0) print len(rotated_grasps) grasp_checker.prune_grasps_in_collision(graspable, rotated_grasps, auto_step=False, close_fingers=True, delay=1)
def test_quality_metrics(vis=True): np.random.seed(100) mesh_file_name = 'data/test/meshes/Co_clean.obj' sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf' sf = sdf_file.SdfFile(sdf_3d_file_name) sdf_3d = sf.read() of = obj_file.ObjFile(mesh_file_name) mesh_3d = of.read() graspable = go.GraspableObject3D(sdf_3d, mesh=mesh_3d) z_vals = np.linspace(-0.025, 0.025, 3) for i in range(z_vals.shape[0]): print 'Evaluating grasp with z val %f' % (z_vals[i]) grasp_center = np.array([0, 0, z_vals[i]]) grasp_axis = np.array([0, 1, 0]) grasp_width = 0.1 grasp_params = g.ParallelJawPtGrasp3D.configuration_from_params( grasp_center, grasp_axis, grasp_width) grasp = g.ParallelJawPtGrasp3D(grasp_params) qualities = [] metrics = [ 'force_closure', 'force_closure_qp', 'min_singular', 'wrench_volume', 'grasp_isotropy', 'ferrari_canny_L1' ] for metric in metrics: q = PointGraspMetrics3D.grasp_quality(grasp, graspable, metric, soft_fingers=True) qualities.append(q) print 'Grasp quality according to %s: %f' % (metric, q) if vis: cf, contacts = grasp.close_fingers(graspable, vis=True) contacts[0].plot_friction_cone(color='y', scale=-2.0) contacts[1].plot_friction_cone(color='y', scale=-2.0) plt.show() IPython.embed()
def getgrasps(object): obj_name = object[1] sdf_name = object[2] obj_mesh = of.ObjFile(obj_name).read() sdf_ = sf.SdfFile(sdf_name).read() obj = go.GraspableObject3D(sdf_, mesh=obj_mesh, key=object[0].replace("_features.txt", ""), model_name=obj_name) config_name = "cfg/correlated.yaml" config = ec.ExperimentConfig(config_name) np.random.seed(100) if config['grasp_sampler'] == 'antipodal': sampler = ags.AntipodalGraspSampler(config) grasps = sampler.generate_grasps( obj, check_collisions=config['check_collisions'], vis=False) num_grasps = len(grasps) min_num_grasps = config['min_num_grasps'] if num_grasps < min_num_grasps: target_num_grasps = min_num_grasps - num_grasps gaussian_sampler = gs.GaussianGraspSampler(config) gaussian_grasps = gaussian_sampler.generate_grasps( obj, target_num_grasps=target_num_grasps, check_collisions=config['check_collisions'], vis=False) grasps.extend(gaussian_grasps) else: sampler = gs.GaussianGraspSampler(config) grasps = sampler.generate_grasps( obj, check_collisions=config['check_collisions'], vis=False, grasp_gen_mult=6) max_num_grasps = config['max_num_grasps'] if len(grasps) > max_num_grasps: np.random.shuffle(grasps) grasps = grasps[:max_num_grasps] return grasps
def test_grasp_poses(): sdf_3d_file_name = '/mnt/terastation/shape_data/MASTER_DB_v0/amazon_picking_challenge/munchkin_white_hot_duck_bath_toy.sdf' sf = sdf_file.SdfFile(sdf_3d_file_name) sdf_3d = sf.read() mesh_name = '/mnt/terastation/shape_data/MASTER_DB_v0/amazon_picking_challenge/munchkin_white_hot_duck_bath_toy.obj' of = obj_file.ObjFile(mesh_name) m = of.read() graspable = graspable_object.GraspableObject3D(sdf_3d, mesh=m, model_name=mesh_name) center = np.array([0, 0, 0.02]) axis = np.array([1, 0, 0]) axis = axis / np.linalg.norm(axis) grasp = g.ParallelJawPtGrasp3D(center, axis, 0.1) rotated_grasps = grasp.transform(graspable.tf, 2 * np.pi / 10) grasp_checker = OpenRaveGraspChecker() rotated_grasps = grasp_checker.prune_grasps_in_collision(graspable, rotated_grasps, auto_step=True)
def test_quality_metrics(vis=True): np.random.seed(100) mesh_file_name = 'data/test/meshes/Co_clean.obj' sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf' sf = sdf_file.SdfFile(sdf_3d_file_name) sdf_3d = sf.read() of = obj_file.ObjFile(mesh_file_name) mesh_3d = of.read() graspable = go.GraspableObject3D(sdf_3d, mesh=mesh_3d) z_vals = np.linspace(-0.025, 0.025, 3) for i in range(z_vals.shape[0]): print 'Evaluating grasp with z val %f' % (z_vals[i]) grasp_center = np.array([0, 0, z_vals[i]]) grasp_axis = np.array([0, 1, 0]) grasp_width = 0.1 grasp = g.ParallelJawPtGrasp3D(grasp_center, grasp_axis, grasp_width) qualities = [] metrics = [ 'force_closure', 'min_singular', 'wrench_volume', 'grasp_isotropy', 'ferrari_canny_L1' ] for metric in metrics: q = PointGraspMetrics3D.grasp_quality(grasp, graspable, metric, soft_fingers=True) qualities.append(q) print 'Grasp quality according to %s: %f' % (metric, q) if vis: grasp.visualize(graspable) graspable.visualize() mv.show()
def test_antipodal_grasp_thompson(): np.random.seed(100) # h = plt.figure() # ax = h.add_subplot(111, projection = '3d') # load object sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf' sf = sdf_file.SdfFile(sdf_3d_file_name) sdf_3d = sf.read() mesh_name = 'data/test/meshes/Co_clean.obj' of = obj_file.ObjFile(mesh_name) m = of.read() graspable = go.GraspableObject3D(sdf_3d, mesh=m, model_name=mesh_name) config = { 'grasp_width': 0.1, 'friction_coef': 0.5, 'num_cone_faces': 8, 'grasp_samples_per_surface_point': 4, 'dir_prior': 1.0, 'alpha_thresh_div': 32, 'rho_thresh': 0.75, # as pct of object max moment 'vis_antipodal': False, 'min_num_grasps': 20, 'alpha_inc': 1.1, 'rho_inc': 1.1, 'sigma_mu': 0.1, 'sigma_trans_grasp': 0.001, 'sigma_rot_grasp': 0.1, 'sigma_trans_obj': 0.001, 'sigma_rot_obj': 0.1, 'sigma_scale_obj': 0.1, 'num_prealloc_obj_samples': 100, 'num_prealloc_grasp_samples': 0, 'min_num_collision_free_grasps': 10, 'grasp_theta_res': 1 } sampler = ags.AntipodalGraspSampler(config) start_time = time.clock() grasps, alpha_thresh, rho_thresh = sampler.generate_grasps(graspable, vis=False) end_time = time.clock() duration = end_time - start_time logging.info('Antipodal grasp candidate generation took %f sec' % (duration)) # convert grasps to RVs for optimization graspable_rv = GraspableObjectGaussianPose(graspable, config) f_rv = scipy.stats.norm(config['friction_coef'], config['sigma_mu']) candidates = [] for grasp in grasps: grasp_rv = ParallelJawGraspGaussian(grasp, config) candidates.append(ForceClosureRV(grasp_rv, graspable_rv, f_rv, config)) objective = objectives.RandomBinaryObjective() # run bandits eps = 5e-4 ua_tc_list = [tc.MaxIterTerminationCondition(1000) ] #, tc.ConfidenceTerminationCondition(eps)] ua = das.UniformAllocationMean(objective, candidates) ua_result = ua.solve( termination_condition=tc.OrTerminationCondition(ua_tc_list), snapshot_rate=100) logging.info('Uniform allocation took %f sec' % (ua_result.total_time)) ts_tc_list = [ tc.MaxIterTerminationCondition(1000), tc.ConfidenceTerminationCondition(eps) ] ts = das.ThompsonSampling(objective, candidates) ts_result = ts.solve( termination_condition=tc.OrTerminationCondition(ts_tc_list), snapshot_rate=100) logging.info('Thompson sampling took %f sec' % (ts_result.total_time)) true_means = models.BetaBernoulliModel.beta_mean( ua_result.models[-1].alphas, ua_result.models[-1].betas) # plot results plt.figure() plot_value_vs_time_beta_bernoulli(ua_result, true_means, color='red') plot_value_vs_time_beta_bernoulli(ts_result, true_means, color='blue') plt.show() das.plot_num_pulls_beta_bernoulli(ua_result) plt.title('Observations Per Variable for Uniform allocation') das.plot_num_pulls_beta_bernoulli(ts_result) plt.title('Observations Per Variable for Thompson sampling') plt.show()
def test_window_correlation(width, num_steps, vis=True): import scipy import sdf_file, obj_file import discrete_adaptive_samplers as das import experiment_config as ec import feature_functions as ff import graspable_object as go # weird Python issues import kernels import models import objectives import pfc import termination_conditions as tc np.random.seed(100) mesh_file_name = 'data/test/meshes/Co_clean.obj' sdf_3d_file_name = 'data/test/sdf/Co_clean.sdf' config = ec.ExperimentConfig('cfg/correlated.yaml') config['window_width'] = width config['window_steps'] = num_steps brute_force_iter = 100 snapshot_rate = config['bandit_snapshot_rate'] sdf = sdf_file.SdfFile(sdf_3d_file_name).read() mesh = obj_file.ObjFile(mesh_file_name).read() graspable = go.GraspableObject3D(sdf, mesh) grasp_axis = np.array([0, 1, 0]) grasp_width = 0.1 grasps = [] for z in [-0.030, -0.035, -0.040, -0.045]: grasp_center = np.array([0, 0, z]) grasp = g.ParallelJawPtGrasp3D( ParallelJawPtGrasp3D.configuration_from_params( grasp_center, grasp_axis, grasp_width)) grasps.append(grasp) graspable_rv = pfc.GraspableObjectGaussianPose(graspable, config) f_rv = scipy.stats.norm(config['friction_coef'], config['sigma_mu']) # friction Gaussian RV # compute feature vectors for all grasps feature_extractor = ff.GraspableFeatureExtractor(graspable, config) all_features = feature_extractor.compute_all_features(grasps) candidates = [] for grasp, features in zip(grasps, all_features): logging.info('Adding grasp %d' % len(candidates)) grasp_rv = pfc.ParallelJawGraspGaussian(grasp, config) pfc_rv = pfc.ForceClosureRV(grasp_rv, graspable_rv, f_rv, config) pfc_rv.set_features(features) candidates.append(pfc_rv) if vis: _, (c1, c2) = grasp.close_fingers(graspable) plt.figure() c1_proxy = c1.plot_friction_cone(color='m') c2_proxy = c2.plot_friction_cone(color='y') plt.legend([c1_proxy, c2_proxy], ['Cone 1', 'Cone 2']) plt.title('Grasp %d' % (len(candidates))) objective = objectives.RandomBinaryObjective() ua = das.UniformAllocationMean(objective, candidates) logging.info('Running uniform allocation for true pfc.') ua_result = ua.solve( termination_condition=tc.MaxIterTerminationCondition(brute_force_iter), snapshot_rate=snapshot_rate) estimated_pfc = models.BetaBernoulliModel.beta_mean( ua_result.models[-1].alphas, ua_result.models[-1].betas) print 'true pfc' print estimated_pfc def phi(rv): return rv.features kernel = kernels.SquaredExponentialKernel(sigma=config['kernel_sigma'], l=config['kernel_l'], phi=phi) print 'kernel matrix' print kernel.matrix(candidates) if vis: plt.show()