def vis_graspable_grasp(graspable, grasp):
graspable.mesh.visualize()
mv.axes()
x, v = graspable.sdf.surface_points(grid_basis = False)
mv.points3d(x[:,0], x[:,1], x[:,2], scale_factor = 0.01)
g1, g2 = grasp.endpoints()
mv.points3d(g1[0], g1[1], g1[2], scale_factor = 0.01, color=(0.5,0.5,0.5))
mv.points3d(g2[0], g2[1], g2[2], scale_factor = 0.01, color=(0.5,0.5,0.5))
mv.points3d(grasp.center[0], grasp.center[1], grasp.center[2], scale_factor = 0.01, color=(0.5,0.5,0.5))
mv.show()
def vis_graspable_grasp(graspable, grasp):
graspable.mesh.visualize()
mv.axes()
x, v = graspable.sdf.surface_points(grid_basis=False)
mv.points3d(x[:, 0], x[:, 1], x[:, 2], scale_factor=0.01)
g1, g2 = grasp.endpoints()
mv.points3d(g1[0], g1[1], g1[2], scale_factor=0.01, color=(0.5, 0.5, 0.5))
mv.points3d(g2[0], g2[1], g2[2], scale_factor=0.01, color=(0.5, 0.5, 0.5))
mv.points3d(grasp.center[0],
grasp.center[1],
grasp.center[2],
scale_factor=0.01,
color=(0.5, 0.5, 0.5))
mv.show()
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, 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
