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 test_grasp_quality_functions(self):
num_grasps = NUM_TEST_CASES
of = ObjFile(OBJ_FILENAME)
sf = SdfFile(SDF_FILENAME)
mesh = of.read()
sdf = sf.read()
obj = GraspableObject3D(sdf, mesh)
gripper = RobotGripper.load(GRIPPER_NAME)
ags = UniformGraspSampler(gripper, CONFIG)
grasps = ags.generate_grasps(obj, target_num_grasps=num_grasps)
# test with grasp quality function
quality_config = GraspQualityConfigFactory.create_config(
CONFIG['metrics']['force_closure'])
quality_fn = GraspQualityFunctionFactory.create_quality_function(
obj, quality_config)
for i, grasp in enumerate(grasps):
success, c = grasp.close_fingers(obj)
if success:
c1, c2 = c
fn_fc = quality_fn(grasp).quality
true_fc = PointGraspMetrics3D.force_closure(
c1, c2, quality_config.friction_coef)
self.assertEqual(fn_fc, true_fc)
def test_antipodal_grasp_sampler(self):
of = ObjFile(OBJ_FILENAME)
sf = SdfFile(SDF_FILENAME)
mesh = of.read()
sdf = sf.read()
obj = GraspableObject3D(sdf, mesh)
gripper = RobotGripper.load(GRIPPER_NAME)
ags = AntipodalGraspSampler(gripper, CONFIG)
grasps = ags.generate_grasps(obj, target_num_grasps=NUM_TEST_CASES)
# test with raw force closure function
for i, grasp in enumerate(grasps):
success, c = grasp.close_fingers(obj)
if success:
c1, c2 = c
self.assertTrue(PointGraspMetrics3D.force_closure(c1, c2, CONFIG['sampling_friction_coef']))
def antipodal_grasp_sampler(self):
of = ObjFile(OBJ_FILENAME)
sf = SdfFile(SDF_FILENAME)
mesh = of.read()
sdf = sf.read()
obj = GraspableObject3D(sdf, mesh)
gripper = RobotGripper.load(GRIPPER_NAME)
ags = AntipodalGraspSampler(gripper, CONFIG)
grasps = ags.generate_grasps(obj, target_num_grasps=10)
quality_config = GraspQualityConfigFactory.create_config(
CONFIG['metrics']['force_closure'])
quality_fn = GraspQualityFunctionFactory.create_quality_function(
obj, quality_config)
q_to_c = lambda quality: CONFIG['quality_scale']
i = 0
vis.figure()
vis.mesh(obj.mesh.trimesh, style='surface')
for grasp in grasps:
print(grasp.frame)
success, c = grasp.close_fingers(obj)
if success:
c1, c2 = c
fn_fc = quality_fn(grasp).quality
true_fc = PointGraspMetrics3D.force_closure(
c1, c2, quality_config.friction_coef)
#print(grasp)
T_obj_world = RigidTransform(from_frame='obj', to_frame='world')
color = plt.get_cmap('hsv')(q_to_c(CONFIG['metrics']))[:-1]
T_obj_gripper = grasp.gripper_pose(gripper)
#vis.grasp(grasp,grasp_axis_color=color,endpoint_color=color)
i += 1
#T_obj_world = RigidTransform(from_frame='obj',to_frame='world')
vis.show(False)
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
