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 sample_grasps(self, graspable, openning_ratio_id, openning_ratios, vis=False):
"""Returns a list of candidate grasps for graspable object.
Parameters
----------
graspable : :obj:`GraspableObject3D`
the object to grasp
openning_ratio_id : int
initial gripper openning ratio for sampling; not actual grasp openning ratio
openning_ratios : list
all possible opening ratios
vis : bool
whether or not to visualize progress, for debugging
Returns
-------
:obj:`list` of :obj:`ParallelJawPtGrasp3D`
the sampled grasps
"""
# get surface points
grasps = []
surface_points, _ = graspable.sdf.surface_points(grid_basis=False)
np.random.shuffle(surface_points)
shuffled_surface_points = surface_points[:min(self.max_num_surface_points_, len(surface_points))]
logging.info('Num surface: %d' %(len(surface_points)))
for k, x_surf in enumerate(shuffled_surface_points):
start_time = time.clock()
# perturb grasp for num samples
for i in range(self.num_samples):
# perturb contact (TODO: sample in tangent plane to surface)
x1 = self.perturb_point(x_surf, graspable.sdf.resolution)
# compute friction cone faces
c1 = Contact3D(graspable, x1, in_direction=None)
_, tx1, ty1 = c1.tangents()
cone_succeeded, cone1, n1 = c1.friction_cone(self.num_cone_faces, self.friction_coef)
if not cone_succeeded:
continue
cone_time = time.clock()
# sample grasp axes from friction cone
v_samples = self.sample_from_cone(n1, tx1, ty1, num_samples=1)
sample_time = time.clock()
for v in v_samples:
if vis:
x1_grid = graspable.sdf.transform_pt_obj_to_grid(x1)
cone1_grid = graspable.sdf.transform_pt_obj_to_grid(cone1, direction=True)
plt.clf()
h = plt.gcf()
plt.ion()
ax = plt.gca(projection = '3d')
for i in range(cone1.shape[1]):
ax.scatter(x1_grid[0] - cone1_grid[0], x1_grid[1] - cone1_grid[1], x1_grid[2] - cone1_grid[2], s = 50, c = u'm')
# # random axis flips since we don't have guarantees on surface normal directoins
# if random.random() > 0.5:
# v = -v
# randomly pick grasp width & angle
grasp_width = openning_ratios[openning_ratio_id] * self.gripper.max_width
grasp_angle = np.random.rand() * np.pi * 2
# start searching for contacts
grasp, c1, c2 = ParallelJawPtGrasp3D.grasp_from_contact_and_axis_on_grid(graspable, x1, v, grasp_width,
grasp_angle=grasp_angle,
min_grasp_width_world=self.gripper.min_width,
vis=vis)
if grasp is None or c2 is None:
continue
# get true contacts (previous is subject to variation)
success, c = grasp.close_fingers(graspable)
if not success:
continue
c1 = c[0]
c2 = c[1]
# make sure grasp is wide enough
if np.linalg.norm(c1.point - c2.point) < self.min_contact_dist:
continue
# update grasp center
grasp.center = ParallelJawPtGrasp3D.center_from_endpoints(c1.point, c2.point)
# compute friction cone for new contacts
cone_succeeded, cone1, n1 = c1.friction_cone(self.num_cone_faces, self.friction_coef)
if not cone_succeeded:
continue
cone_succeeded, cone2, n2 = c2.friction_cone(self.num_cone_faces, self.friction_coef)
if not cone_succeeded:
continue
# check friction cone
if PointGraspMetrics3D.force_closure(c1, c2, self.friction_coef):
# try to find minimum possible openning width
original_max_width = grasp.max_grasp_width_
for index in range(openning_ratio_id):
grasp.max_grasp_width_ = openning_ratios[index] * self.gripper.max_width
success, _ = grasp.close_fingers(graspable)
if success:
break
else:
grasp.max_grasp_width_ = original_max_width
grasps.append(grasp)
# randomly sample max num grasps from total list
random.shuffle(grasps)
return grasps