def test_force_closure(self):
of = ObjFile(OBJ_FILENAME)
sf = SdfFile(SDF_FILENAME)
mesh = of.read()
sdf = sf.read()
obj = GraspableObject3D(sdf, mesh)
for i in range(NUM_TEST_CASES):
contacts, normals, _, mu, _ = random_force_closure_test_case(
antipodal=True)
c1 = Contact3D(obj, contacts[:, 0])
c1.normal = normals[:, 0]
c2 = Contact3D(obj, contacts[:, 1])
c2.normal = normals[:, 1]
self.assertTrue(
PointGraspMetrics3D.force_closure(c1,
c2,
mu,
use_abs_value=False))
for i in range(NUM_TEST_CASES):
contacts, normals, _, mu, _ = random_force_closure_test_case(
antipodal=False)
c1 = Contact3D(obj, contacts[:, 0])
c1.normal = normals[:, 0]
c2 = Contact3D(obj, contacts[:, 1])
c2.normal = normals[:, 1]
self.assertFalse(
PointGraspMetrics3D.force_closure(c1,
c2,
mu,
use_abs_value=False))
def find_contact(line_of_action, obj, vis=False, strict=False):
"""
Find the point at which a point traveling along a given line of action hits a surface.
Parameters
----------
line_of_action : :obj:`list` of 3x1 :obj:`numpy.ndarray`
the points visited as the fingers close (grid coords)
obj : :obj:`GraspableObject3D`
to check contacts on
vis : bool
whether or not to display the contact check (for debugging)
Returns
-------
contact_found : bool
whether or not the point contacts the object surface
contact : :obj:`Contact3D`
found along line of action (None if contact not found)
"""
contact_found = False
pt_zc = None
pt_zc_world = None
contact = None
num_pts = len(line_of_action)
sdf_here = 0
sdf_before = 0
pt_grid = None
pt_before = None
# step along line of action, get points on surface when possible
i = 0
while i < num_pts and not contact_found:
# update loop vars
pt_before_before = pt_before
pt_before = pt_grid
sdf_before_before = sdf_before
sdf_before = sdf_here
pt_grid = line_of_action[i]
# visualize
if vis:
ax = plt.gca(projection='3d')
ax.scatter(pt_grid[0], pt_grid[1], pt_grid[2], c='r')
# check surface point
on_surface, sdf_here = obj.sdf.on_surface(pt_grid)
if on_surface:
contact_found = True
if strict:
return contact_found, None
# quadratic approximation to find actual zero crossing
if i == 0:
pt_after = line_of_action[i + 1]
sdf_after = obj.sdf[pt_after]
pt_after_after = line_of_action[i + 2]
sdf_after_after = obj.sdf[pt_after_after]
pt_zc = Sdf3D.find_zero_crossing_quadratic(
pt_grid, sdf_here, pt_after, sdf_after, pt_after_after,
sdf_after_after)
# contact not yet found if next sdf value is smaller
if pt_zc is None or np.abs(sdf_after) < np.abs(sdf_here):
contact_found = False
elif i == len(line_of_action) - 1:
pt_zc = Sdf3D.find_zero_crossing_quadratic(
pt_before_before, sdf_before_before, pt_before,
sdf_before, pt_grid, sdf_here)
if pt_zc is None:
contact_found = False
else:
pt_after = line_of_action[i + 1]
sdf_after = obj.sdf[pt_after]
pt_zc = Sdf3D.find_zero_crossing_quadratic(
pt_before, sdf_before, pt_grid, sdf_here, pt_after,
sdf_after)
# contact not yet found if next sdf value is smaller
if pt_zc is None or np.abs(sdf_after) < np.abs(sdf_here):
contact_found = False
i = i + 1
# visualization
if vis and contact_found:
ax = plt.gca(projection='3d')
ax.scatter(pt_zc[0], pt_zc[1], pt_zc[2], s=80, c='g')
if contact_found:
pt_zc_world = obj.sdf.transform_pt_grid_to_obj(pt_zc)
in_direction_grid = line_of_action[-1] - line_of_action[0]
in_direction_grid = in_direction_grid / np.linalg.norm(
in_direction_grid)
in_direction = obj.sdf.transform_pt_grid_to_obj(in_direction_grid,
direction=True)
contact = Contact3D(obj, pt_zc_world, in_direction=in_direction)
if contact.normal is None:
contact_found = False
return contact_found, contact
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):
"""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