def plot_stable_pose(mesh, stable_pose, T_table_world=stf.SimilarityTransform3D(from_frame='world', to_frame='table'), d=0.5, style='wireframe', color=(0.5,0.5,0.5)): T_mesh_stp = stf.SimilarityTransform3D(pose=tfx.pose(stable_pose.r)) mesh_tf = mesh.transform(T_mesh_stp) mn, mx = mesh_tf.bounding_box() z = mn[2] x0 = np.array([0,0,-z]) T_table_obj = stf.SimilarityTransform3D(pose=tfx.pose(stable_pose.r, x0), from_frame='obj', to_frame='table') T_world_obj = T_table_world.inverse().dot(T_table_obj) MayaviVisualizer.plot_mesh(mesh, T_world_obj.inverse(), style=style, color=color) MayaviVisualizer.plot_table(T_table_world, d=d) return T_world_obj.inverse()
def __init__(self, sdf, mesh=None, features=None, tf=stf.SimilarityTransform3D(tfx.identity_tf(), 1.0), key='', model_name='', category='', mass=1.0): if not isinstance(tf, stf.SimilarityTransform3D): raise ValueError( 'Must initialize graspable objects with 3D similarity transform' ) self.sdf_ = sdf self.mesh_ = mesh self.tf_ = tf self.key_ = key self.model_name_ = model_name # for OpenRave usage, gross! self.category_ = category self.mass_ = mass self.features_ = features # shot features # make consistent poses, scales self.sdf_.tf = self.tf_ if self.mesh_ is not None: self.mesh_.tf = self.tf_
def plot_gripper(grasp, T_obj_world=stf.SimilarityTransform3D(from_frame='world', to_frame='obj'), gripper=None, color=(0.5,0.5,0.5)): if gripper is None: gripper = FANUC_GRIPPER T_gripper_obj = grasp.gripper_pose(gripper).inverse() T_mesh_obj = gripper.T_mesh_gripper.dot(T_gripper_obj) T_mesh_world = T_mesh_obj.dot(T_obj_world) MayaviVisualizer.plot_mesh(gripper.mesh, T_mesh_world, style='surface', color=color)
def __init__(self, grasp_center, grasp_axis, grasp_width, jaw_width = 0, grasp_angle = 0, tf = stf.SimilarityTransform3D(tfx.identity_tf(from_frame = 'world'), 1.0)): """ Create a point grasp for parallel jaws with given center and width (relative to object) Params: (Note: all in meters!) grasp_center - numpy 3-array for center of grasp grasp_axis - numpy 3-array for grasp direction (should be normalized) grasp_width - how wide the jaws open in meters jaw_width - the width of the jaws tangent to the axis (0 means classical point grasp) grasp_angle - the angle of approach for parallel-jaw grippers (the 6th DOF for point grasps) wrt the orthogonal dir to the axis in the XY plane tf - the similarity tf of the grasp wrt the world """ if not isinstance(grasp_center, np.ndarray) or grasp_center.shape[0] != 3: raise ValueError('Center must be numpy ndarray of size 3') if not isinstance(grasp_axis, np.ndarray) or grasp_axis.shape[0] != 3: raise ValueError('Axis must be numpy ndarray of size 3') if jaw_width != 0: raise ValueError('Nonzero jaw width not yet supported') self.center_ = grasp_center self.axis_ = grasp_axis / np.linalg.norm(grasp_axis) self.grasp_width_ = grasp_width self.jaw_width_ = jaw_width self.approach_angle_ = grasp_angle self.tf_ = tf self.quality_ = None
def __init__(self, sdf, tf=stf.SimilarityTransform3D(tfx.identity_tf(), 1.0)): """ 2D objects are initialized with sdfs only""" if not isinstance(sdf, s.Sdf2D): raise ValueError('Must initialize graspable object 2D with 2D sdf') GraspableObject.__init__(self, sdf, tf=tf)
def __init__(self, sdf, mesh=None, features=None, tf=stf.SimilarityTransform3D(tfx.identity_tf(), 1.0), key='', category='', model_name='', mass=1.0): """ 2D objects are initialized with sdfs only""" if not isinstance(sdf, s.Sdf3D): raise ValueError('Must initialize graspable object 3D with 3D sdf') if mesh is not None and not (isinstance(mesh, m.Mesh3D) or isinstance(mesh, mx.Mesh)): raise ValueError('Must initialize graspable object 3D with 3D sdf') self.center_of_mass_ = sdf.center_world() # use SDF bb center for now GraspableObject.__init__(self, sdf, mesh=mesh, features=features, tf=tf, key=key, category=category, model_name=model_name, mass=mass)
def __init__(self, sdf_data, origin, resolution, tf=stf.SimilarityTransform3D(tfx.identity_tf(), scale=1.0), frame=None, use_abs=True): self.data_ = sdf_data self.origin_ = origin self.resolution_ = resolution self.dims_ = self.data_.shape # set up surface params self.surface_thresh_ = self.resolution_ * np.sqrt( 2 ) / 2 # resolution is max dist from surface when surf is orthogonal to diagonal grid cells self.center_ = np.array([(self.dims_[0] - 1) / 2, (self.dims_[1] - 1) / 2, (self.dims_[2] - 1) / 2]) self.points_buf_ = np.zeros([Sdf3D.num_interpolants, 3], dtype=np.int) self.coords_buf_ = np.zeros([ 3, ]) # set up tf self.tf_ = tf # tranform sdf basis to grid (X and Z axes are flipped!) R_sdf_mesh = np.eye(3) self.tf_grid_sdf_ = stf.SimilarityTransform3D( tfx.canonical.CanonicalTransform(R_sdf_mesh, -R_sdf_mesh.T.dot(self.center_)), 1.0 / self.resolution_) self.tf_sdf_grid_ = self.tf_grid_sdf_.inverse() # optionally use only the absolute values (useful for non-closed meshes in 3D) if use_abs: self.data_ = np.abs(self.data_) self._compute_flat_indices() self._compute_gradients() self.feature_vector_ = None #Kmeans feature representation
def makegrasp(originalgrasp, bmat=None, center=None, direction=None): if bmat is not None: axis = bmat[:, 1][:-1] center = bmat[:, 3][:-1] return ParallelJawPtGrasp3D(center, axis, originalgrasp.grasp_width, originalgrasp.jaw_width, originalgrasp.approach_angle, tf=stf.SimilarityTransform3D( tfx.identity_tf(), 1.0)) else: axis = direction[:, 1] return ParallelJawPtGrasp3D(center, axis, originalgrasp.grasp_width, originalgrasp.jaw_width, originalgrasp.approach_angle, tf=stf.SimilarityTransform3D( tfx.identity_tf(), 1.0))
def test_fanuc_gripper(): mesh_filename = '/home/jmahler/jeff_working/GPIS/data/grippers/fanuc_lehf/gripper.obj' of = objf.ObjFile(mesh_filename) gripper_mesh = of.read() gripper_mesh.center_vertices_bb() oof = objf.ObjFile(mesh_filename) oof.write(gripper_mesh) T_mesh_world = stf.SimilarityTransform3D(pose=tfx.pose(np.eye(4)), from_frame='world', to_frame='mesh') R_grasp_gripper = np.array([[0, 0, 1], [0, -1, 0], [1, 0, 0]]) R_mesh_gripper = np.array([[0, 1, 0], [-1, 0, 0], [0, 0, 1]]) t_mesh_gripper = np.array([0.0, 0.0, 0.065]) T_mesh_gripper = stf.SimilarityTransform3D(pose=tfx.pose(R_mesh_gripper, t_mesh_gripper), from_frame='gripper', to_frame='mesh') T_gripper_world = T_mesh_gripper.inverse().dot(T_mesh_world) T_grasp_gripper = stf.SimilarityTransform3D(pose=tfx.pose(R_grasp_gripper), from_frame='gripper', to_frame='grasp') T_grasp_world = T_grasp_gripper.dot(T_gripper_world) #T_mesh_gripper.save('/home/jmahler/jeff_working/GPIS/data/grippers/fanuc_lehf/T_mesh_gripper.stf') #T_grasp_gripper.save('/home/jmahler/jeff_working/GPIS/data/grippers/fanuc_lehf/T_grasp_gripper.stf') gripper_params = {} gripper_params['min_width'] = 0.015 gripper_params['max_width'] = 0.048 #f = open('/home/jmahler/jeff_working/GPIS/data/grippers/fanuc_lehf/params.json', 'w') #json.dump(gripper_params, f) MayaviVisualizer.plot_pose(T_mesh_world, alpha=0.05, tube_radius=0.0025, center_scale=0.005) #MayaviVisualizer.plot_pose(T_gripper_world, alpha=0.05, tube_radius=0.0025, center_scale=0.005) MayaviVisualizer.plot_pose(T_grasp_world, alpha=0.05, tube_radius=0.0025, center_scale=0.005) MayaviVisualizer.plot_mesh(gripper_mesh, T_mesh_world, style='surface', color=(1,1,1)) mv.axes()
def gripper_pose(self, gripper=None): """ Returns the pose from the object frame to the gripper frame as a similarity transform 3D object """ R_grip_p_grip = self.rotated_full_axis R_grip_robot = np.eye(3) if gripper is not None: R_grip_robot = gripper.T_grasp_gripper.rotation R = R_grip_p_grip.dot(R_grip_robot) t = self.center if USE_ALAN: return RigidTransform(R, t, from_frame='gripper', to_frame='obj') else: return stf.SimilarityTransform3D(tfx.pose(R, t), from_frame='gripper', to_frame='obj')
def write_mesh_stable_poses(self, mesh, filename, min_prob=0, vis=False): prob_mapping, cv_hull = st.compute_stable_poses(mesh), mesh.convex_hull() R_list = [] for face, p in prob_mapping.items(): if p >= min_prob: x0 = np.array(cv_hull.vertices()[face[0]]) R_list.append([p, st.compute_basis([cv_hull.vertices()[i] for i in face], cv_hull), x0]) if vis: print 'P', R_list[0][0] mv.figure() mesh.visualize() mv.axes() mv.figure() cv_hull_tf = cv_hull.transform(stf.SimilarityTransform3D(tfx.transform(R_list[0][1], np.zeros(3)))) cv_hull_tf.visualize() mv.axes() mv.show() f = open(filename[:-4] + ".stp", "w") f.write("#############################################################\n") f.write("# STP file generated by UC Berkeley Automation Sciences Lab #\n") f.write("# #\n") f.write("# Num Poses: %d" %len(R_list)) for _ in range(46 - len(str(len(R_list)))): f.write(" ") f.write(" #\n") f.write("# Min Probability: %s" %str(min_prob)) for _ in range(40 - len(str(min_prob))): f.write(" ") f.write(" #\n") f.write("# #\n") f.write("#############################################################\n") f.write("\n") # adding R matrices to .stp file pose_index = 1 for i in range(len(R_list)): f.write("p %f\n" %R_list[i][0]) f.write("r %f %f %f\n" %(R_list[i][1][0][0], R_list[i][1][0][1], R_list[i][1][0][2])) f.write(" %f %f %f\n" %(R_list[i][1][1][0], R_list[i][1][1][1], R_list[i][1][1][2])) f.write(" %f %f %f\n" %(R_list[i][1][2][0], R_list[i][1][2][1], R_list[i][1][2][2])) f.write("x0 %f %f %f\n" %(R_list[i][2][0], R_list[i][2][1], R_list[i][2][2])) f.write("\n\n")
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 sdf_3d(data): """ Converts HDF5 data provided in dictionary |data| to an SDF object """ sdf_data = np.array(data[SDF_DATA_KEY]) origin = np.array(data.attrs[SDF_ORIGIN_KEY]) resolution = data.attrs[SDF_RES_KEY] # should never be used, really pose = tfx.identity_tf() scale = 1.0 if SDF_POSE_KEY in data.attrs.keys(): pose = data.attrs[SDF_POSE_KEY] if SDF_SCALE_KEY in data.attrs.keys(): scale = data.attrs[SDF_SCALE_KEY] tf = stf.SimilarityTransform3D(pose, scale) frame = None if SDF_FRAME_KEY in data.attrs.keys(): frame = data.attrs[SDF_FRAME_KEY] return sdf.Sdf3D(sdf_data, origin, resolution, tf, frame)
def sample(self, size=1): """ Sample |size| random variables from the model """ samples = [] for i in range(size): # sample random pose xi = self.r_xi_rv_.rvs(size=1) S_xi = skew(xi) R = scipy.linalg.expm(S_xi).dot(self.obj_.tf.rotation) s = self.s_rv_.rvs(size=1)[0] t = self.t_rv_.rvs(size=1) sample_tf = stf.SimilarityTransform3D(tfx.transform(R.T, t), s) # transform object by pose obj_sample = self.obj_.transform(sample_tf) samples.append(obj_sample) # not a list if only 1 sample if size == 1: return samples[0] return samples
def test_3d_transform(): np.random.seed(100) sdf_3d_file_name = 'data/test/sdf/Co_clean_dim_25.sdf' s = sf.SdfFile(sdf_3d_file_name) sdf_3d = s.read() # transform pose_mat = np.matrix([[0, 1, 0, 0], [-1, 0, 0, 0], [0, 0, 1, 0], [0, 0, 0, 1]]) tf = tfx.transform(pose_mat, from_frame='world') tf = tfx.random_tf() tf.position = 0.01 * np.random.rand(3) start_t = time.clock() s_tf = stf.SimilarityTransform3D(tf, scale=1.2) sdf_tf = sdf_3d.transform(s_tf) end_t = time.clock() duration = end_t - start_t logging.info('3D Transform took %f sec' % (duration)) logging.info('Transformed resolution %f' % (sdf_tf.resolution)) start_t = time.clock() sdf_tf_d = sdf_3d.transform(s_tf, detailed=True) end_t = time.clock() duration = end_t - start_t logging.info('Detailed 3D Transform took %f sec' % (duration)) logging.info('Transformed detailed resolution %f' % (sdf_tf_d.resolution)) # display plt.figure() sdf_3d.scatter() plt.title('Original') plt.figure() sdf_tf.scatter() plt.title('Transformed') plt.figure() sdf_tf_d.scatter() plt.title('Detailed Transformed') plt.show()
def plot_grasp(grasp, T_obj_world=stf.SimilarityTransform3D(from_frame='world', to_frame='obj'), plot_approach=False, alpha=0.5, tube_radius=0.002, endpoint_color=(0,1,0), endpoint_scale=0.004, grasp_axis_color=(0,1,0), palm_axis_color=(0,0,1), stp=None): g1, g2 = grasp.endpoints() center = grasp.center g1_tf = T_obj_world.inverse().apply(g1) g2_tf = T_obj_world.inverse().apply(g2) center_tf = T_obj_world.inverse().apply(center) grasp_axis_tf = np.array([g1_tf, g2_tf]) T_gripper_obj = grasp.gripper_pose(gripper=ZEKE_GRIPPER).inverse() palm_axis = T_gripper_obj.inverse().rotation[:,1] axis_tf = np.array([g1_tf, g2_tf]) palm_axis_tf = T_obj_world.inverse().apply(palm_axis, direction=True) palm_axis_tf = np.array([center_tf, center_tf + alpha * palm_axis_tf]) mv.points3d(g1_tf[0], g1_tf[1], g1_tf[2], color=endpoint_color, scale_factor=endpoint_scale) mv.points3d(g2_tf[0], g2_tf[1], g2_tf[2], color=endpoint_color, scale_factor=endpoint_scale) mv.plot3d(grasp_axis_tf[:,0], grasp_axis_tf[:,1], grasp_axis_tf[:,2], color=grasp_axis_color, tube_radius=tube_radius) if plot_approach: mv.plot3d(palm_axis_tf[:,0], palm_axis_tf[:,1], palm_axis_tf[:,2], color=palm_axis_color, tube_radius=tube_radius)
stable_pose.r[1, :] = -stable_pose.r[1, :] R_stp_obj = stable_pose.r n = stable_pose.r[2, :] # read in mesh obj_filename = dataset.obj_mesh_filename(object_key) of = objf.ObjFile(obj_filename) mesh = of.read() gripper = gr.RobotGripper.load(config['gripper']) # form transformation matrices R_stp_cb = np.array([[0, -1, 0], [1, 0, 0], [0, 0, 1]]) t_stp_cb = np.array([delta_x, delta_y, delta_z]) T_stp_cb = stf.SimilarityTransform3D(pose=tfx.pose(R_stp_cb, t_stp_cb), from_frame='cb', to_frame='stp') T_obj_stp = stf.SimilarityTransform3D(pose=tfx.pose( R_stp_obj.T, np.zeros(3)), from_frame='stp', to_frame='obj') T_obj_cb = T_obj_stp.dot(T_stp_cb) T_cb_obj = T_obj_cb.inverse() R_cb_world = np.eye(3) t_cb_world = np.zeros(3) T_cb_world = stf.SimilarityTransform3D(pose=tfx.pose( R_cb_world, t_cb_world), to_frame='cb') # transformations from FANUC to Dex-Net conventions
def plot_mesh(mesh, T_mesh_world=stf.SimilarityTransform3D(from_frame='world', to_frame='mesh'), style='wireframe', color=(0.5,0.5,0.5), opacity=1.0): mesh_tf = mesh.transform(T_mesh_world.inverse()) mesh_tf.visualize(style=style, color=color, opacity=opacity)
def test_yumi_gripper(): # build new mesh finger_filename = '/home/jmahler/jeff_working/GPIS/data/grippers/yumi/finger.obj' base_filename = '/home/jmahler/jeff_working/GPIS/data/grippers/yumi/base.obj' of = objf.ObjFile(finger_filename) finger1_mesh = of.read() finger2_mesh = copy.copy(finger1_mesh) of = objf.ObjFile(base_filename) base_mesh = of.read() R_finger1_world = np.eye(3) t_finger1_world = np.array([-0.025, -0.005, 0.082]) T_finger1_world = stf.SimilarityTransform3D(pose=tfx.pose(R_finger1_world, t_finger1_world), from_frame='mesh', to_frame='world') R_finger2_world = np.array([[-1,0,0], [0,-1,0], [0,0,1]]) t_finger2_world = np.array([0.025, 0.005, 0.082]) T_finger2_world = stf.SimilarityTransform3D(pose=tfx.pose(R_finger2_world, t_finger2_world), from_frame='mesh', to_frame='world') T_mesh_world = stf.SimilarityTransform3D(pose=tfx.pose(np.eye(4)), from_frame='world', to_frame='mesh') finger1_mesh = finger1_mesh.transform(T_finger1_world) finger2_mesh = finger2_mesh.transform(T_finger2_world) offset = 0 vertices = [] triangles = [] meshes = [base_mesh, finger1_mesh, finger2_mesh] for i, mesh in enumerate(meshes): vertices.extend(mesh.vertices()) offset_tris = [[t[0]+offset,t[1]+offset,t[2]+offset] for t in mesh.triangles()] triangles.extend(offset_tris) offset += len(mesh.vertices()) gripper_mesh = m.Mesh3D(vertices, triangles) gripper_mesh.center_vertices_bb() of = objf.ObjFile('/home/jmahler/jeff_working/GPIS/data/grippers/yumi/gripper.obj') #of.write(gripper_mesh) # frames of reference R_grasp_gripper = np.array([[0, 0, -1], [0, 1, 0], [1, 0, 0]]) R_mesh_gripper = np.array([[1, 0, 0], [0, 1, 0], [0, 0, 1]]) t_mesh_gripper = np.array([0.0, -0.002, 0.058]) T_mesh_gripper = stf.SimilarityTransform3D(pose=tfx.pose(R_mesh_gripper, t_mesh_gripper), from_frame='gripper', to_frame='mesh') T_gripper_world = T_mesh_gripper.inverse().dot(T_mesh_world) T_grasp_gripper = stf.SimilarityTransform3D(pose=tfx.pose(R_grasp_gripper), from_frame='gripper', to_frame='grasp') #T_mesh_gripper.save('/home/jmahler/jeff_working/GPIS/data/grippers/yumi/T_mesh_gripper.stf') #T_grasp_gripper.save('/home/jmahler/jeff_working/GPIS/data/grippers/yumi/T_grasp_gripper.stf') gripper_params = {} gripper_params['min_width'] = 0.0 gripper_params['max_width'] = 0.05 #f = open('/home/jmahler/jeff_working/GPIS/data/grippers/yumi/params.json', 'w') #json.dump(gripper_params, f) MayaviVisualizer.plot_mesh(gripper_mesh, T_mesh_world, style='surface', color=(0,0,1)) MayaviVisualizer.plot_pose(T_gripper_world, alpha=0.05, tube_radius=0.0025, center_scale=0.005) mv.axes()
def plot_patch(window, T_patch_obj, window_dim_obj=(1.0,1.0), T_obj_world=stf.SimilarityTransform3D(from_frame='world', to_frame='obj'), patch_color=(1,1,1), contact_color=(1,1,0), contact_scale=0.005, delta_z=0.001, edge_len_thresh=3.0, dist_thresh=0.05, grad_thresh=0.025): """ Plot a patch defined by a window and a contact """ # extract dimensions proj_window = window.proj_win_2d grad_x_window = window.grad_x_2d grad_y_window = window.grad_y_2d win_height, win_width = proj_window.shape res_x = window_dim_obj[1] / win_width res_y = window_dim_obj[0] / win_height offset = 0 t_patch_render = np.array([0, 0, -delta_z]) T_patch_render = stf.SimilarityTransform3D(pose=tfx.pose(np.eye(3), t_patch_render), from_frame='render', to_frame='contact') # x,y lists for triangulation x_list = [] y_list = [] # convert patch into 3d points points_3d = np.zeros([0,3]) for y in range(win_height): for x in range(win_width): # convert to 3d points x_3d = (x - win_width/2) * res_x y_3d = (y - win_height/2) * res_y # add the point if the distance and gradients are appropriate if np.abs(proj_window[y, x]-offset) < dist_thresh and \ np.abs(grad_x_window[y, x]) < grad_thresh and \ np.abs(grad_y_window[y, x]) < grad_thresh: x_list.append(x) y_list.append(y) points_3d = np.r_[points_3d, np.array([[x_3d, y_3d, proj_window[y, x] - offset]])] # abort if too few points if len(x_list) <= 3 and len(y_list) <= 3: logging.warning('Too few points for triangulation') # triangulate and prune large tris tri = mtri.Triangulation(x_list, y_list) points_2d = np.array([x_list, y_list]).T triangles = [] for t in tri.triangles.tolist(): v = points_2d[t,:] largest_dist = np.max(ssd.pdist(v)) if largest_dist < edge_len_thresh: triangles.append(t) # transform into world reference frame points_3d_obj = T_patch_obj.inverse().dot(T_patch_render).apply(points_3d.T) points_3d_world = T_obj_world.inverse().apply(points_3d_obj).T contact_world = T_obj_world.inverse().dot(T_patch_obj.inverse()).dot(T_patch_render).apply(np.zeros(3)) # plot mesh_background = mlab.triangular_mesh(points_3d_world[:,0], points_3d_world[:,1], points_3d_world[:,2], triangles, representation='surface', color=patch_color) mesh_tris = mlab.triangular_mesh(points_3d_world[:,0], points_3d_world[:,1], points_3d_world[:,2], triangles, representation='wireframe', color=(0,0,0)) points = mlab.points3d(contact_world[0], contact_world[1], contact_world[2], color=contact_color, scale_factor=contact_scale)
def perform_stf(self, meshtotransform): self.source_mesh = meshtotransform tftransform = tfx.canonical.pose(self.transform(self.source_points)) similaritytf = stf.SimilarityTransform3D(tftransform) self.source_mesh = self.source_mesh.transform(similaritytf)
if os.path.exists(grasp_ids_filename): logging.warning( 'Candidate grasps already exist for object %s in stable pose %s for gripper %s. Skipping...' % (object_key, stp_id, gripper.name)) continue # generate candidate grasps logging.info( 'Computing candidate grasps for object {}'.format(object_key)) candidate_grasp_set, candidate_grasp_ids, candidate_grasp_metrics = \ generate_candidate_grasps(object_key, dataset, stable_pose, config['num_grasp_candidates'], gripper, config) # visualize grasp candidates T_obj_stp = stf.SimilarityTransform3D(pose=tfx.pose(stable_pose.r)) T_table_world = stf.SimilarityTransform3D(from_frame='world', to_frame='table') object_mesh = obj.mesh object_mesh_tf = object_mesh.transform(T_obj_stp) for i, grasp in enumerate(candidate_grasp_set): logging.info('') logging.info('Grasp %d (%d of %d)' % (grasp.grasp_id, i, len(candidate_grasp_set))) metrics = config['candidate_grasp_metrics'] for metric in metrics: if metric == 'efc': metric = db.generate_metric_tag('efcny_L1', config) elif metric == 'pfc': metric = db.generate_metric_tag('pfc', config)