def test_init_grasp(self): # random grasp g1 = np.random.rand(3) g2 = np.random.rand(3) x = (g1 + g2) / 2 v = g2 - g1 width = np.linalg.norm(v) v = v / width configuration = ParallelJawPtGrasp3D.configuration_from_params(x, v, width) # test init random_grasp = ParallelJawPtGrasp3D(configuration) read_configuration = random_grasp.configuration self.assertTrue(np.allclose(configuration, read_configuration)) self.assertTrue(np.allclose(x, random_grasp.center)) self.assertTrue(np.allclose(v, random_grasp.axis)) self.assertTrue(np.allclose(width, random_grasp.open_width)) read_g1, read_g2 = random_grasp.endpoints self.assertTrue(np.allclose(g1, read_g1)) self.assertTrue(np.allclose(g2, read_g2)) # test bad init configuration[4] = 1000 caught_bad_init = False try: random_grasp = ParallelJawPtGrasp3D(configuration) except: caught_bad_init = True self.assertTrue(caught_bad_init)
def sample(self, size=1): """ Sample random variables from the model. Parameters ---------- size : int number of sample to take Returns ------- :obj:`list` of :obj:`ParallelJawPtGrasp3D` sampled grasps in various poses """ 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 open_width = max(self.open_width_rv_.rvs(size=1), 0) close_width = max(self.close_width_rv_.rvs(size=1), 0) approach = self.approach_rv_.rvs(size=1) # transform grasp by pose grasp_sample = ParallelJawPtGrasp3D(ParallelJawPtGrasp3D.configuration_from_params(t, v, open_width, approach, self.grasp_.jaw_width, close_width)) samples.append(grasp_sample) if size == 1: return samples[0] return samples
def sample_grasps(self, graspable, num_grasps, vis=False, max_num_samples=1000): """ Returns a list of candidate grasps for graspable object using uniform point pairs from the SDF Parameters ---------- graspable : :obj:`GraspableObject3D` the object to grasp num_grasps : int the number of grasps to generate Returns ------- :obj:`list` of :obj:`ParallelJawPtGrasp3D` list of generated grasps """ # get all surface points surface_points, _ = graspable.sdf.surface_points(grid_basis=False) num_surface = surface_points.shape[0] i = 0 grasps = [] # get all grasps while len(grasps) < num_grasps and i < max_num_samples: # get candidate contacts indices = np.random.choice(num_surface, size=2, replace=False) c0 = surface_points[indices[0], :] c1 = surface_points[indices[1], :] if np.linalg.norm(c1 - c0) > self.gripper.min_width and np.linalg.norm( c1 - c0) < self.gripper.max_width: # compute centers and axes grasp_center = ParallelJawPtGrasp3D.center_from_endpoints( c0, c1) grasp_axis = ParallelJawPtGrasp3D.axis_from_endpoints(c0, c1) g = ParallelJawPtGrasp3D( ParallelJawPtGrasp3D.configuration_from_params( grasp_center, grasp_axis, self.gripper.max_width)) # keep grasps if the fingers close success, contacts = g.close_fingers(graspable) if success: grasps.append(g) i += 1 return grasps
def test_init_grasp(self): # random grasp g1 = np.random.rand(3) g2 = np.random.rand(3) x = (g1 + g2) / 2 v = g2 - g1 width = np.linalg.norm(v) v = v / width configuration = ParallelJawPtGrasp3D.configuration_from_params( x, v, width) # test init random_grasp = ParallelJawPtGrasp3D(configuration) read_configuration = random_grasp.configuration read_g1, read_g2 = random_grasp.endpoints # test bad init configuration[4] = 1000 caught_bad_init = False try: random_grasp = ParallelJawPtGrasp3D(configuration) except: caught_bad_init = True
def sample_grasps(self, graspable, num_grasps, vis=False, sigma_scale=2.5): """ Returns a list of candidate grasps for graspable object by Gaussian with variance specified by principal dimensions. Parameters ---------- graspable : :obj:`GraspableObject3D` the object to grasp num_grasps : int the number of grasps to generate sigma_scale : float the number of sigmas on the tails of the Gaussian for each dimension Returns ------- :obj:`list` of obj:`ParallelJawPtGrasp3D` list of generated grasps """ # get object principal axes center_of_mass = graspable.mesh.center_of_mass principal_dims = graspable.mesh.principal_dims() sigma_dims = principal_dims / (2 * sigma_scale) # sample centers grasp_centers = stats.multivariate_normal.rvs( mean=center_of_mass, cov=sigma_dims**2, size=num_grasps) # samples angles uniformly from sphere u = stats.uniform.rvs(size=num_grasps) v = stats.uniform.rvs(size=num_grasps) thetas = 2 * np.pi * u phis = np.arccos(2 * v - 1.0) grasp_dirs = np.array([np.sin(phis) * np.cos(thetas), np.sin(phis) * np.sin(thetas), np.cos(phis)]) grasp_dirs = grasp_dirs.T # convert to grasp objects grasps = [] for i in range(num_grasps): grasp = ParallelJawPtGrasp3D(ParallelJawPtGrasp3D.configuration_from_params(grasp_centers[i,:], grasp_dirs[i,:], self.gripper.max_width)) contacts_found, contacts = grasp.close_fingers(graspable) # add grasp if it has valid contacts if contacts_found and np.linalg.norm(contacts[0].point - contacts[1].point) > self.min_contact_dist: grasps.append(grasp) # visualize if vis: for grasp in grasps: plt.clf() h = plt.gcf() plt.ion() grasp.close_fingers(graspable, vis=vis) plt.show(block=False) time.sleep(0.5) grasp_centers_grid = graspable.sdf.transform_pt_obj_to_grid(grasp_centers.T) grasp_centers_grid = grasp_centers_grid.T com_grid = graspable.sdf.transform_pt_obj_to_grid(center_of_mass) plt.clf() ax = plt.gca(projection = '3d') graspable.sdf.scatter() ax.scatter(grasp_centers_grid[:,0], grasp_centers_grid[:,1], grasp_centers_grid[:,2], s=60, c=u'm') ax.scatter(com_grid[0], com_grid[1], com_grid[2], s=120, c=u'y') ax.set_xlim3d(0, graspable.sdf.dims_[0]) ax.set_ylim3d(0, graspable.sdf.dims_[1]) ax.set_zlim3d(0, graspable.sdf.dims_[2]) plt.show() return grasps