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 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_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 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 _generate_grasps(self,
graspable,
num_grasps,
check_collisions=False,
vis=False):
"""Returns a list of candidate grasps for graspable object.
Params:
graspable - (GraspableObject3D) the object to grasp
num_grasps - currently unused TODO
Returns:
list of ParallelJawPtGrasp3D objects
"""
# get surface points
ap_grasps = []
surface_points, _ = graspable.sdf.surface_points(grid_basis=False)
for x_surf in 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 = contacts.Contact3D(graspable, x1, in_direction=None)
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(cone1, 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')
# start searching for contacts
grasp, c2 = ParallelJawPtGrasp3D.grasp_from_contact_and_axis_on_grid(
graspable, x1, v, self.grasp_width, vis=vis)
if grasp is None or c2 is None:
continue
# make sure grasp is wide enough
x2 = c2.point
if np.linalg.norm(x1 - x2) < self.min_contact_dist:
continue
v_true = grasp.axis
# compute friction cone for contact 2
cone_succeeded, cone2, n2 = c2.friction_cone(
self.num_cone_faces, self.friction_coef)
if not cone_succeeded:
continue
if vis:
plt.figure()
ax = plt.gca(projection='3d')
c1_proxy = c1.plot_friction_cone(color='m')
c2_proxy = c2.plot_friction_cone(color='y')
ax.view_init(elev=5.0, azim=0)
plt.show(block=False)
time.sleep(0.5)
plt.close() # lol
# check friction cone
in_cone1, alpha1 = self.within_cone(cone1, n1, v_true.T)
in_cone2, alpha2 = self.within_cone(cone2, n2, -v_true.T)
within_cone_time = time.clock()
# add points if within friction cone
if in_cone1 and in_cone2:
# get moment arms
x1_world, x2_world = grasp.endpoints()
rho1 = np.linalg.norm(graspable.moment_arm(x1_world))
rho2 = np.linalg.norm(graspable.moment_arm(x2_world))
antipodal_grasp = AntipodalGraspParams(
graspable, grasp, alpha1, alpha2, rho1, rho2)
ap_grasps.append(antipodal_grasp)
# randomly sample max num grasps from total list
max_grasp_index = min(len(ap_grasps), self.max_num_grasps)
random.shuffle(ap_grasps)
ap_grasps = ap_grasps[:max_grasp_index]
# load openrave
if check_collisions:
rave.raveSetDebugLevel(rave.DebugLevel.Error)
grasp_checker = pgc.OpenRaveGraspChecker(view=vis)
# go back through grasps and threshold
grasps = []
pr2_grasps = []
alpha_thresh = self.alpha_thresh
rho_thresh = self.rho_thresh * graspable.sdf.max_dim()
while len(ap_grasps) > 0 and len(grasps) < self.min_num_grasps and len(pr2_grasps) < self.min_num_collision_free and \
alpha_thresh < np.pi / 2:
# prune grasps above thresholds
next_ap_grasps = []
for ap_grasp in ap_grasps:
if max(ap_grasp.alpha1, ap_grasp.alpha2) < alpha_thresh and \
max(ap_grasp.rho1, ap_grasp.rho2) < rho_thresh:
# convert grasps to PR2 gripper poses
rotated_grasps = ap_grasp.grasp.transform(
graspable.tf, self.theta_res)
# prune collision grasps if necessary
if check_collisions:
rotated_grasps = grasp_checker.prune_grasps_in_collision(
graspable,
rotated_grasps,
auto_step=True,
delay=0.0)
# only add grasp if at least 1 is collision free
if len(rotated_grasps) > 0:
grasps.append(ap_grasp.grasp)
pr2_grasps.extend(rotated_grasps)
else:
next_ap_grasps.append(ap_grasp)
# update alpha and rho thresholds
alpha_thresh = alpha_thresh + self.alpha_inc #np.arctan(friction_coef)
rho_thresh = rho_thresh + self.rho_inc
ap_grasps = list(next_ap_grasps)
return grasps
def generate_candidate_grasps(object_name, dataset, stable_pose, num_grasps,
gripper, config):
""" Add the best grasp according to PFC as well as num_grasps-1 uniformly at random from the remaining set """
grasp_set = []
grasp_set_ids = []
grasp_set_metrics = []
# read params
approach_dist = config['approach_dist']
delta_approach = config['delta_approach']
rotate_threshold = config['rotate_threshold']
table_clearance = config['table_clearance']
dist_thresh = config['grasp_dist_thresh']
# get sorted list of grasps to ensure that we get the top grasp
graspable = dataset.graspable(object_name)
graspable.model_name_ = dataset.obj_mesh_filename(object_name)
grasps = dataset.grasps(object_name, gripper=gripper.name)
all_grasp_metrics = dataset.grasp_metrics(object_name,
grasps,
gripper=gripper.name)
mn, mx = graspable.mesh.bounding_box()
alpha = 1.0 / np.max(mx - mn)
print alpha
# prune by collisions
rave.raveSetDebugLevel(rave.DebugLevel.Error)
collision_checker = gcc.OpenRaveGraspChecker(gripper, view=False)
collision_checker.set_object(graspable)
# add the top quality grasps for each metric
metrics = config['candidate_grasp_metrics']
for metric in metrics:
# generate metric tag
if metric == 'efc':
metric = db.generate_metric_tag('efcny_L1', config)
elif metric == 'pfc':
metric = db.generate_metric_tag('pfc', config)
elif metric == 'ppc':
metric = db.generate_metric_tag('ppc_%s' % (stable_pose.id),
config)
# sort grasps by the current metric
grasp_metrics = [all_grasp_metrics[g.grasp_id][metric] for g in grasps]
grasps_and_metrics = zip(grasps, grasp_metrics)
grasps_and_metrics.sort(key=lambda x: x[1])
grasps = [gm[0] for gm in grasps_and_metrics]
grasp_metrics = [gm[1] for gm in grasps_and_metrics]
# add grasps by quantile
logging.info('Adding best grasp for metric %s' % (metric))
i = len(grasps) - 1
grasp_candidate = grasps[i].grasp_aligned_with_stable_pose(stable_pose)
# check wrist rotation
psi = grasp_candidate.angle_with_table(stable_pose)
rotated_from_table = (psi > rotate_threshold)
# check distances
min_dist = np.inf
for g in grasp_set:
dist = grasp_module.ParallelJawPtGrasp3D.distance(
g, grasp_candidate)
if dist < min_dist:
min_dist = dist
# check collisions
while gripper.collides_with_table(grasp_candidate, stable_pose, table_clearance) \
or collides_along_approach(grasp_candidate, gripper, collision_checker, approach_dist, delta_approach) \
or rotated_from_table or grasp_candidate.grasp_id in grasp_set_ids \
or min_dist < dist_thresh:
# get the next grasp
i -= 1
if i < 0:
break
grasp_candidate = grasps[i].grasp_aligned_with_stable_pose(
stable_pose)
# check wrist rotation
psi = grasp_candidate.angle_with_table(stable_pose)
rotated_from_table = (psi > rotate_threshold)
# check distances
min_dist = np.inf
for g in grasp_set:
dist = grasp_module.ParallelJawPtGrasp3D.distance(
g, grasp_candidate)
if dist < min_dist:
min_dist = dist
# add to sequence
if i >= 0:
grasp_set.append(grasp_candidate)
grasp_set_ids.append(grasp_candidate.grasp_id)
grasp_set_metrics.append(
all_grasp_metrics[grasp_candidate.grasp_id])
# sample the remaining grasps uniformly at random
i = 0
random.shuffle(grasps)
while len(grasp_set) < num_grasps and i < len(grasps):
# random grasp candidate
logging.info('Adding grasp %d' % (len(grasp_set)))
grasp_candidate = grasps[i].grasp_aligned_with_stable_pose(stable_pose)
# check wrist rotation
psi = grasp_candidate.angle_with_table(stable_pose)
rotated_from_table = (psi > rotate_threshold)
# check distances
min_dist = np.inf
for g in grasp_set:
dist = grasp_module.ParallelJawPtGrasp3D.distance(
g, grasp_candidate)
if dist < min_dist:
min_dist = dist
# check collisions
while gripper.collides_with_table(grasp_candidate, stable_pose) \
or collides_along_approach(grasp_candidate, gripper, collision_checker, approach_dist, delta_approach) \
or rotated_from_table or grasp_candidate.grasp_id in grasp_set_ids \
or min_dist < dist_thresh:
# get the next grasp
i += 1
if i >= len(grasps):
break
grasp_candidate = grasps[i].grasp_aligned_with_stable_pose(
stable_pose)
# check wrist rotation
psi = grasp_candidate.angle_with_table(stable_pose)
rotated_from_table = (psi > rotate_threshold)
# check distances
min_dist = np.inf
for g in grasp_set:
dist = grasp_module.ParallelJawPtGrasp3D.distance(
g, grasp_candidate)
if dist < min_dist:
min_dist = dist
# add to sequence
if i < len(grasps):
grasp_set.append(grasp_candidate)
grasp_set_ids.append(grasp_candidate.grasp_id)
grasp_set_metrics.append(
all_grasp_metrics[grasp_candidate.grasp_id])
return grasp_set, grasp_set_ids, grasp_set_metrics
def _generate_grasps(self, graspable, num_grasps,
check_collisions=False, vis=False):
"""Returns a list of candidate grasps for graspable object.
Params:
graspable - (GraspableObject3D) the object to grasp
num_grasps - currently unused TODO
Returns:
list of ParallelJawPtGrasp3D objects
"""
# get surface points
ap_grasps = []
surface_points, _ = graspable.sdf.surface_points(grid_basis=False)
for x_surf in 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 = contacts.Contact3D(graspable, x1, in_direction=None)
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(cone1, 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')
# start searching for contacts
grasp, c2 = ParallelJawPtGrasp3D.grasp_from_contact_and_axis_on_grid(graspable, x1, v, self.grasp_width, vis = vis)
if grasp is None or c2 is None:
continue
# make sure grasp is wide enough
x2 = c2.point
if np.linalg.norm(x1 - x2) < self.min_contact_dist:
continue
v_true = grasp.axis
# compute friction cone for contact 2
cone_succeeded, cone2, n2 = c2.friction_cone(self.num_cone_faces, self.friction_coef)
if not cone_succeeded:
continue
if vis:
plt.figure()
ax = plt.gca(projection='3d')
c1_proxy = c1.plot_friction_cone(color='m')
c2_proxy = c2.plot_friction_cone(color='y')
ax.view_init(elev=5.0, azim=0)
plt.show(block=False)
time.sleep(0.5)
plt.close() # lol
# check friction cone
in_cone1, alpha1 = self.within_cone(cone1, n1, v_true.T)
in_cone2, alpha2 = self.within_cone(cone2, n2, -v_true.T)
within_cone_time = time.clock()
# add points if within friction cone
if in_cone1 and in_cone2:
# get moment arms
x1_world, x2_world = grasp.endpoints()
rho1 = np.linalg.norm(graspable.moment_arm(x1_world))
rho2 = np.linalg.norm(graspable.moment_arm(x2_world))
antipodal_grasp = AntipodalGraspParams(graspable, grasp, alpha1, alpha2, rho1, rho2)
ap_grasps.append(antipodal_grasp)
# randomly sample max num grasps from total list
max_grasp_index = min(len(ap_grasps), self.max_num_grasps)
random.shuffle(ap_grasps)
ap_grasps = ap_grasps[:max_grasp_index]
# load openrave
if check_collisions:
rave.raveSetDebugLevel(rave.DebugLevel.Error)
grasp_checker = pgc.OpenRaveGraspChecker(view=vis)
# go back through grasps and threshold
grasps = []
pr2_grasps = []
alpha_thresh = self.alpha_thresh
rho_thresh = self.rho_thresh * graspable.sdf.max_dim()
while len(ap_grasps) > 0 and len(grasps) < self.min_num_grasps and len(pr2_grasps) < self.min_num_collision_free and \
alpha_thresh < np.pi / 2:
# prune grasps above thresholds
next_ap_grasps = []
for ap_grasp in ap_grasps:
if max(ap_grasp.alpha1, ap_grasp.alpha2) < alpha_thresh and \
max(ap_grasp.rho1, ap_grasp.rho2) < rho_thresh:
# convert grasps to PR2 gripper poses
rotated_grasps = ap_grasp.grasp.transform(graspable.tf, self.theta_res)
# prune collision grasps if necessary
if check_collisions:
rotated_grasps = grasp_checker.prune_grasps_in_collision(graspable, rotated_grasps, auto_step=True, delay=0.0)
# only add grasp if at least 1 is collision free
if len(rotated_grasps) > 0:
grasps.append(ap_grasp.grasp)
pr2_grasps.extend(rotated_grasps)
else:
next_ap_grasps.append(ap_grasp)
# update alpha and rho thresholds
alpha_thresh = alpha_thresh + self.alpha_inc #np.arctan(friction_coef)
rho_thresh = rho_thresh + self.rho_inc
ap_grasps = list(next_ap_grasps)
return grasps
def generate_grasps(self, graspable, target_num_grasps=None, grasp_gen_mult=3, max_iter=3,
check_collisions=False, vis=False, **kwargs):
"""Generates an exact number of grasps.
Params:
graspable - (GraspableObject3D) the object to grasp
target_num_grasps - (int) number of grasps to return, defualts to
self.target_num_grasps
grasp_gen_mult - (int) number of additional grasps to generate
max_iter - (int) number of attempts to return an exact number of
grasps before giving up
"""
# setup collision checking
if USE_OPENRAVE and check_collisions:
rave.raveSetDebugLevel(rave.DebugLevel.Error)
collision_checker = gcc.OpenRaveGraspChecker(self.gripper, view=False)
collision_checker.set_object(graspable)
# get num grasps
if target_num_grasps is None:
target_num_grasps = self.target_num_grasps
num_grasps_remaining = target_num_grasps
grasps = []
k = 1
while num_grasps_remaining > 0 and k <= max_iter:
# generate more than we need
num_grasps_generate = grasp_gen_mult * num_grasps_remaining
new_grasps = self._generate_grasps(graspable, num_grasps_generate,
vis, **kwargs)
# prune grasps in collision
coll_free_grasps = []
if check_collisions and USE_OPENRAVE:
logging.info('Checking collisions for %d candidates' %(len(new_grasps)))
for grasp in new_grasps:
# construct a set of rotated grasps
for i in range(self.num_grasp_rots):
rotated_grasp = copy.copy(grasp)
rotated_grasp.set_approach_angle(2 * i * np.pi / self.num_grasp_rots)
if not collision_checker.in_collision(rotated_grasp):
coll_free_grasps.append(rotated_grasp)
break
else:
coll_free_grasps = new_grasps
grasps += coll_free_grasps
logging.info('%d/%d grasps found after iteration %d.',
len(grasps), target_num_grasps, k)
grasp_gen_mult *= 2
num_grasps_remaining = target_num_grasps - len(grasps)
k += 1
random.shuffle(grasps)
if len(grasps) > target_num_grasps:
logging.info('Truncating %d grasps to %d.',
len(grasps), target_num_grasps)
grasps = grasps[:target_num_grasps]
logging.info('Found %d grasps.', len(grasps))
return grasps
