def test_window_distance(width, num_steps, plot=None): import sdf_file, obj_file 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 = GraspableObject3D(sdf, mesh) grasp_axis = np.array([0, 1, 0]) grasp_width = 0.1 grasp1_center = np.array([0, 0, -0.025]) grasp1 = g.ParallelJawPtGrasp3D(grasp1_center, grasp_axis, grasp_width) grasp2_center = np.array([0, 0, -0.030]) grasp2 = g.ParallelJawPtGrasp3D(grasp2_center, grasp_axis, grasp_width) w1, w2 = graspable.surface_information(grasp1, width, num_steps) v1, v2 = graspable.surface_information(grasp2, width, num_steps) # IPython.embed() if plot: plot(w1.proj_win, num_steps) plot(w2.proj_win, num_steps) plot(v1.proj_win, num_steps) plot(v2.proj_win, num_steps) plt.show() IPython.embed() return
def generate_window_for_figure(width, num_steps, plot=None): import sdf_file, obj_file np.random.seed(100) mesh_file_name = '/mnt/terastation/shape_data/MASTER_DB_v2/SHREC14LSGTB/M000385.obj' sdf_3d_file_name = '/mnt/terastation/shape_data/MASTER_DB_v2/SHREC14LSGTB/M000385.sdf' sdf = sdf_file.SdfFile(sdf_3d_file_name).read() mesh = obj_file.ObjFile(mesh_file_name).read() graspable = GraspableObject3D(sdf, mesh) grasp_axis = np.array([1, 0, 0]) grasp_width = 0.15 grasp1_center = np.array([0, 0, 0.075]) grasp1 = g.ParallelJawPtGrasp3D(grasp1_center, grasp_axis, grasp_width) _, contacts1 = grasp1.close_fingers(graspable, vis=True) contact1 = contacts1[0] grasp2_center = np.array([0, 0, 0.025]) grasp2 = g.ParallelJawPtGrasp3D(grasp2_center, grasp_axis, grasp_width) _, contacts2 = grasp2.close_fingers(graspable, vis=True) contact2 = contacts2[0] grasp3_center = np.array([0, 0.012, -0.089]) grasp3 = g.ParallelJawPtGrasp3D(grasp3_center, grasp_axis, grasp_width) _, contacts3 = grasp3.close_fingers(graspable, vis=True) contact3 = contacts3[0] if plot: plt.figure() contact1.plot_friction_cone() contact2.plot_friction_cone() print 'aligned projection window' aligned_window1 = contact1.surface_window_projection(width, num_steps, vis=True) aligned_window2 = contact2.surface_window_projection(width, num_steps, vis=True) aligned_window3 = contact3.surface_window_projection(width, num_steps, vis=True) plt.show() IPython.embed() if plot: plot(sdf_window1, num_steps, 'SDF Window 1') plot(unaligned_window1, num_steps, 'Unaligned Projection Window 1') plot(aligned_window1, num_steps, 'Aligned Projection Window 1') plot(sdf_window2, num_steps, 'SDF Window 2') plot(unaligned_window2, num_steps, 'Unaligned Projection Window 2') plot(aligned_window2, num_steps, 'Aligned Projection Window 2') 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 test_window_curvature(width, num_steps, plot=None): import sdf_file, obj_file 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 = GraspableObject3D(sdf, mesh) grasp_axis = np.array([0, 1, 0]) grasp_width = 0.1 for i, z in enumerate([-0.030, -0.035, -0.040, -0.045], 1): print 'w%d' % (i) grasp_center = np.array([0, 0, z]) grasp = g.ParallelJawPtGrasp3D( ParallelJawPtGrasp3D.configuration_from_params( grasp_center, grasp_axis, grasp_width)) w, _ = graspable.surface_information(grasp, width, num_steps) for info in (w.proj_win_, w.gauss_curvature_): print 'min:', np.min(info), np.argmin(info) print 'max:', np.max(info), np.argmax(info) if plot: plot(w.proj_win_, num_steps, 'w%d proj_win' % (i), save=True)
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 grasps(data): """ Return a list of grasp objects from the data provided in the HDF5 dictionary """ # need to read in a bunch of grasps but also need to know what kind of grasp it is grasps = [] num_grasps = data.attrs[NUM_GRASPS_KEY] for i in range(num_grasps): # get the grasp data y'all grasp_key = GRASP_KEY + '_' + str(i) if grasp_key in data.keys(): grasp_id = data[grasp_key].attrs[GRASP_ID_KEY] grasp_type = data[grasp_key].attrs[GRASP_TYPE_KEY] configuration = data[grasp_key].attrs[GRASP_CONFIGURATION_KEY] frame = data[grasp_key].attrs[GRASP_RF_KEY] timestamp = data[grasp_key].attrs[GRASP_TIMESTAMP_KEY] # create object based on type g = None if grasp_type == 'ParallelJawPtGrasp3D': g = grasp.ParallelJawPtGrasp3D(configuration=configuration, frame=frame, timestamp=timestamp, grasp_id=grasp_id) grasps.append(g) else: logging.debug('Grasp %s is corrupt. Skipping' % (grasp_key)) return grasps
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 sample(self, size=1): samples = [] for i in range(size): # sample random pose xi = self.r_xi_rv_.rvs(size=1) S_xi = skew(xi) v = scipy.linalg.expm(S_xi).dot(self.grasp_.axis) t = self.t_rv_.rvs(size=1) # transform object by pose #grasp_sample = copy.copy(self.grasp_) grasp_sample = gr.ParallelJawPtGrasp3D(t, v, self.grasp_.grasp_width) samples.append(grasp_sample) if size == 1: return samples[0] return samples
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 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 sample(self, size=1): samples = [] for i in range(size): # sample random pose xi = self.r_xi_rv_.rvs(size=1) S_xi = skew(xi) axis_sigma = self.R_sample_sigma_.T.dot(self.grasp_.axis) v = self.R_sample_sigma_.dot( scipy.linalg.expm(S_xi).dot(axis_sigma)) t = self.R_sample_sigma_.dot(self.t_rv_.rvs(size=1).T).T # transform object by pose grasp_sample = gr.ParallelJawPtGrasp3D( gr.ParallelJawPtGrasp3D.configuration_from_params( t, v, self.grasp_.grasp_width)) samples.append(grasp_sample) if size == 1: return samples[0] return samples
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_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()
def test_windows(width, num_steps, plot=None): import sdf_file, obj_file 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 = GraspableObject3D(sdf, mesh) grasp_axis = np.array([0, 1, 0]) grasp_width = 0.1 grasp1_center = np.array([0, 0, -0.025]) grasp1 = g.ParallelJawPtGrasp3D( ParallelJawPtGrasp3D.configuration_from_params(grasp1_center, grasp_axis, grasp_width)) _, contacts1 = grasp1.close_fingers(graspable) contact1 = contacts1[0] grasp2_center = np.array([0, 0, -0.030]) grasp2 = g.ParallelJawPtGrasp3D( ParallelJawPtGrasp3D.configuration_from_params(grasp2_center, grasp_axis, grasp_width)) _, contacts2 = grasp2.close_fingers(graspable) contact2 = contacts2[0] if plot: plt.figure() contact1.plot_friction_cone() contact2.plot_friction_cone() print 'sdf window' sdf_window1 = contact1.surface_window_sdf(width, num_steps) sdf_window2 = contact2.surface_window_sdf(width, num_steps) print 'unaligned projection window' unaligned_window1 = contact1.surface_window_projection_unaligned( width, num_steps) unaligned_window2 = contact2.surface_window_projection_unaligned( width, num_steps) print 'aligned projection window' aligned_window1 = contact1.surface_window_projection(width, num_steps) aligned_window2 = contact2.surface_window_projection(width, num_steps) print 'proj, sdf, proj - sdf at contact' contact_index = num_steps // 2 if num_steps % 2 == 0: contact_index += 1 contact_index = (contact_index, contact_index) print aligned_window1[contact_index], sdf_window1[ contact_index], aligned_window1[contact_index] - sdf_window1[ contact_index] print aligned_window2[contact_index], sdf_window2[ contact_index], aligned_window2[contact_index] - sdf_window2[ contact_index] if plot: plot(sdf_window1, num_steps, 'SDF Window 1') plot(unaligned_window1, num_steps, 'Unaligned Projection Window 1') plot(aligned_window1, num_steps, 'Aligned Projection Window 1') plot(sdf_window2, num_steps, 'SDF Window 2') plot(unaligned_window2, num_steps, 'Unaligned Projection Window 2') plot(aligned_window2, num_steps, 'Aligned Projection Window 2') plt.show()