def grasps(data):
""" Return a list of grasp objects from the data provided in the HDF5 dictionary """
# need to read in a bunch of grasps but also need to know what kind of grasp it is
grasps = []
num_grasps = data.attrs[NUM_GRASPS_KEY]
for i in range(num_grasps):
# get the grasp data y'all
grasp_key = GRASP_KEY + '_' + str(i)
if grasp_key in list(data.keys()):
grasp_id = data[grasp_key].attrs[GRASP_ID_KEY]
grasp_type = data[grasp_key].attrs[GRASP_TYPE_KEY]
configuration = data[grasp_key].attrs[GRASP_CONFIGURATION_KEY]
frame = data[grasp_key].attrs[GRASP_RF_KEY]
# create object based on type
g = None
if grasp_type == 'ParallelJawPtGrasp3D':
g = ParallelJawPtGrasp3D(configuration=configuration, frame=frame, grasp_id=grasp_id)
grasps.append(g)
else:
logging.debug('Grasp %s is corrupt. Skipping' %(grasp_key))
return grasps
def sample_grasps(self, graspable, num_grasps,
vis=False, direction=None, stable_pose=None):
"""Returns a list of candidate grasps for graspable object.
Parameters
----------
graspable : :obj:`GraspableObject3D`
the object to grasp
num_grasps : int
number of grasps to sample
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)
_, 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()
# ------------ CHECK GRASP IS PARALLEL TO TABLE ----------------------
if stable_pose != None:
angle = np.dot(np.matmul(stable_pose, v_samples[0]), np.array([0,0,1]))
# don't save if angle greater than 36 degrees
if abs(angle) > 0.2:
continue
# --------------------------------------------------------------------
for v in v_samples:
# random axis flips since we don't have guarantees on surface normal directoins
if random.random() > 0.5:
v = -v
# start searching for contacts
grasp, c1, c2 = ParallelJawPtGrasp3D.grasp_from_contact_and_axis_on_grid(graspable, x1, v, self.gripper.max_width,
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
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
# check friction cone
if PointGraspMetrics3D.force_closure(c1, c2, self.friction_coef):
grasps.append(grasp)
# randomly sample max num grasps from total list
random.shuffle(grasps)
return grasps
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
def generate_grasps(self, graspable, target_num_grasps=None, grasp_gen_mult=5, max_iter=3,
sample_approach_angles=False, vis=False, direction=None, stable_pose=None, **kwargs):
"""Samples a set of grasps for an object.
Parameters
----------
graspable : :obj:`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
sample_approach_angles : bool
whether or not to sample approach angles
Return
------
:obj:`list` of :obj:`ParallelJawPtGrasp3D`
list of generated grasps
"""
# 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:
# SAMPLING: generate more than we need
num_grasps_generate = grasp_gen_mult * num_grasps_remaining
new_grasps = self.sample_grasps(graspable, num_grasps_generate,
vis, direction=direction, stable_pose=stable_pose, **kwargs)
# COVERAGE REJECTION: prune grasps by distance
pruned_grasps = []
for grasp in new_grasps:
min_dist = np.inf
for cur_grasp in grasps:
dist = ParallelJawPtGrasp3D.distance(cur_grasp, grasp)
if dist < min_dist:
min_dist = dist
for cur_grasp in pruned_grasps:
dist = ParallelJawPtGrasp3D.distance(cur_grasp, grasp)
if dist < min_dist:
min_dist = dist
if min_dist >= self.grasp_dist_thresh_:
pruned_grasps.append(grasp)
# ANGLE EXPANSION sample grasp rotations around the axis
candidate_grasps = []
if sample_approach_angles:
for grasp in pruned_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(i * delta_theta)
candidate_grasps.append(rotated_grasp)
else:
candidate_grasps = pruned_grasps
# add to the current grasp set
grasps += candidate_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
# shuffle computed grasps
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
def sample_grasps(self, graspable, num_grasps, vis=False):
"""Returns a list of candidate grasps for graspable object.
Parameters
----------
graspable : :obj:`GraspableObject3D`
the object to grasp
num_grasps : int
number of grasps to sample
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) # #这个不是approaching vector而是抓取的闭合方向
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
# start searching for contacts
grasp, c1, c2 = ParallelJawPtGrasp3D.grasp_from_contact_and_axis_on_grid(
graspable,
x1,
v,
self.gripper.max_width,
min_grasp_width_world=0.0, #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
x2 = c2.point
# if np.linalg.norm(x1 - x2) < self.min_contact_dist:
# continue
v_true = grasp.axis #这个不是approaching vector而是抓取的闭合方向
# 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
if PointGraspMetrics3D.force_closure(
c1, c2, self.friction_coef):
print 'c2.point', c2.point
grasp.touch_pointc1_ = c1.point
grasp.touch_pointc2_ = c2.point
grasps.append(grasp)
#需要在这里加入一个点，也就是根据grasp.center，grasp.axis，copy.deepcopy(grasp), theta = self._angle_aligned_with_table(table_normal)
#生成目标angle
#中心也要变
##########################################new added
#先求点法式方程：
# edge_margin=0.004 #设定为边界余量
# common_perpendicular_threshold=0.0001 #设定为公垂线距离阈值，阈值越小，精度越高，但得到的点也越少
# gn= c1.point-c2.point
# unit_gn=gn/np.linalg.norm(gn)
# gcenterpoint=(c1.point+c2.point)/2.0
# gA=gn[0]
# gB=gn[1]
# gC=gn[2]
# gD=-gA*gcenterpoint[0]-gB*gcenterpoint[1]-gC*gcenterpoint[2]
#
# if (np.linalg.norm(c1.point-c2.point)/2.0<edge_margin):#太窄
# #则这时候限制一个edge_margin意义不大
# uplimit=np.linalg.norm(c1.point-c2.point)/2.0
# else:
# uplimit=np.linalg.norm(c1.point-c2.point)/2.0-edge_margin
#
#
#
# print 'new grasp contact'
# for gi in range(0,len(surface_points)):
# #每个表面的点求点到面的距离
# distance= math.fabs(surface_points[gi][0]*gA+surface_points[gi][1]*gB+surface_points[gi][2]*gC+gD)/(math.sqrt(gA*gA+gB*gB+gC*gC))
# if distance > uplimit:
# continue
# else:#在这个uplimit内，则可以开始考虑其法线了
# #获得这个点的法线
# normalvalue=Contact3D(graspable, surface_points[gi], in_direction=None).normal
# if len(normalvalue)==0:
# print 'no normal value'
# continue
# #获得过grasp.axis且平行于normalvalue的平面（以便求公垂线的距离）
# #先求这个面的法线：
# unit_normalvalue=normalvalue/np.linalg.norm(normalvalue)
# planenorm=np.cross(unit_normalvalue,unit_gn)
# unit_planenorm=planenorm/np.linalg.norm(planenorm)
#
# #故有点法式面方程：法线：unit_planenorm，点：gcenterpoint
# ggA=unit_planenorm[0]
# ggB=unit_planenorm[1]
# ggC=unit_planenorm[2]
# ggD=-ggA*gcenterpoint[0]-ggB*gcenterpoint[1]-ggC*gcenterpoint[2]
# distance_common_perpendicular= math.fabs(surface_points[gi][0]*ggA+surface_points[gi][1]*ggB+surface_points[gi][2]*ggC+ggD)/(math.sqrt(ggA*ggA+ggB*ggB+ggC*ggC))
# if (distance_common_perpendicular>common_perpendicular_threshold):
# continue
# else:#否则，这个点surface_points[gi]则可被视为approaching点，下面可以进一步计算其法线对应的approaching angle
#
# #加入碰撞检测！！！！！！！！！！
#
#
# newgrasp=copy.deepcopy(grasp)
# print 'surface_points[gi]',surface_points[gi]
# newgrasp.approach_point_=surface_points[gi]
# #print 'approach_point_[gi]',newgrasp.approach_point_
# #这里需要重新求grasp.center： 也就是grasp.axis
#
# newgrasp.approach_angle_=grasp._angle_aligned_with_table(unit_normalvalue)#注意，这个angle未必会过grasp.center
# print 'approach_angle_',newgrasp.approach_angle_
# grasps.append(newgrasp)
# print len(grasps),' in ',self.target_num_grasps
# if len(grasps) >self.target_num_grasps:
# break
# if len(grasps) >self.target_num_grasps:
# break
#
#
# if len(grasps) >self.target_num_grasps:
# break
#对每个点，判断该点到grasp.center的向量（设为approaching vector）与grasp.axis是否垂直
#再看这个点的法线和approaching vector是否重合
# randomly sample max num grasps from total list
random.shuffle(grasps)
print 'the number:', len(grasps)
return grasps
def sample_grasps(self, graspable, num_grasps, vis=False):
"""Returns a list of candidate grasps for graspable object.
Parameters
----------
graspable : :obj:`GraspableObject3D`
the object to grasp
num_grasps : int
number of grasps to sample
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
# start searching for contacts
grasp, c1, c2 = ParallelJawPtGrasp3D.grasp_from_contact_and_axis_on_grid(
graspable,
x1,
v,
self.gripper.max_width,
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
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
if PointGraspMetrics3D.force_closure(
c1, c2, self.friction_coef):
grasps.append(grasp)
# randomly sample max num grasps from total list
random.shuffle(grasps)
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