def test_wrench_in_positive_span(self):
# simple test for in positive span
wrench_basis = np.eye(6)
force_limit = 1000
num_fingers = 1
# truly in span, with force limits
for i in range(NUM_TEST_CASES):
target_wrench = np.random.rand(6)
in_span, norm = PointGraspMetrics3D.wrench_in_positive_span(wrench_basis,
target_wrench,
force_limit,
num_fingers)
self.assertTrue(in_span)
# not in span, but within force limits
for i in range(NUM_TEST_CASES):
target_wrench = -np.random.rand(6)
in_span, norm = PointGraspMetrics3D.wrench_in_positive_span(wrench_basis,
target_wrench,
force_limit,
num_fingers)
self.assertFalse(in_span)
# truly in span, but not with force limits
force_limit = 0.1
for i in range(NUM_TEST_CASES):
target_wrench = np.random.rand(6)
target_wrench[0] = 1000
in_span, norm = PointGraspMetrics3D.wrench_in_positive_span(wrench_basis,
target_wrench,
force_limit,
num_fingers)
self.assertFalse(in_span)
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 sample(self, size=1):
""" Samples deterministic quasi-static point grasp quality metrics.
Parameters
----------
size : int
number of samples to take
"""
# sample grasp
cur_time = time.time()
grasp_sample = self.grasp_rv_.rvs(size=1, iteration=self.sample_count_)
grasp_time = time.time()
# sample object
obj_sample = self.obj_rv_.rvs(size=1, iteration=self.sample_count_)
obj_time = time.time()
# sample params
params_sample = None
if self.params_rv_ is not None:
params_sample = self.params_rv_.rvs(size=1, iteration=self.sample_count_)
params_time = time.time()
logging.debug('Sampling took %.3f sec' % (params_time - cur_time))
# compute deterministic quality
start = time.time()
q = PointGraspMetrics3D.grasp_quality(grasp_sample, obj_sample,
params_sample)
quality_time = time.time()
logging.debug('Quality comp took %.3f sec' % (quality_time - start))
self.sample_count_ = self.sample_count_ + 1
return q
def test_min_norm_vector_in_facet(self):
# zero in facet
facet = np.c_[np.eye(6), -np.eye(6)]
min_norm, _ = PointGraspMetrics3D.min_norm_vector_in_facet(facet)
self.assertLess(min_norm, 1e-5)
# simplex
facet = np.c_[np.eye(6)]
min_norm, v = PointGraspMetrics3D.min_norm_vector_in_facet(facet)
true_v = (1.0/6.0)*np.ones(6)
self.assertTrue(np.allclose(min_norm, np.linalg.norm(true_v)))
self.assertTrue(np.allclose(v, true_v))
# single data point, edge case
facet = np.ones([6,1])
min_norm, v = PointGraspMetrics3D.min_norm_vector_in_facet(facet)
self.assertTrue(np.allclose(min_norm, np.linalg.norm(facet)))
self.assertTrue(np.allclose(v, facet))
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 quality(self, grasp):
""" Compute grasp quality using a quasistatic method.
Parameters
----------
grasp : :obj:`GraspableObject3D`
grasp to quality quality on
Returns
-------
:obj:`GraspQualityResult`
result of quality computation
"""
if not isinstance(grasp, Grasp):
raise ValueError('Must provide Grasp object to compute quality')
quality = PointGraspMetrics3D.grasp_quality(grasp, self.graspable_,
self.quality_config_)
return GraspQualityResult(quality, quality_config=self.quality_config_)
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
def antipodal_grasp_sampler(self):
#of = ObjFile(OBJ_FILENAME)
#sf = SdfFile(SDF_FILENAME)
#mesh = of.read()
#sdf = sf.read()
#obj = GraspableObject3D(sdf, mesh)
mass = 1.0
CONFIG['obj_rescaling_type'] = RescalingType.RELATIVE
mesh_processor = MeshProcessor(OBJ_FILENAME, CONFIG['cache_dir'])
mesh_processor.generate_graspable(CONFIG)
mesh = mesh_processor.mesh
sdf = mesh_processor.sdf
obj = GraspableObject3D(sdf, mesh)
gripper = RobotGripper.load(GRIPPER_NAME)
ags = AntipodalGraspSampler(gripper, CONFIG)
grasps = ags.sample_grasps(obj, num_grasps=100)
print(len(grasps))
quality_config = GraspQualityConfigFactory.create_config(CONFIG['metrics']['robust_ferrari_canny'])
#quality_config = GraspQualityConfigFactory.create_config(CONFIG['metrics']['force_closure'])
quality_fn = GraspQualityFunctionFactory.create_quality_function(obj, quality_config)
vis.figure()
#vis.points(PointCloud(nparraypoints), scale=0.001, color=np.array([0.5,0.5,0.5]))
#vis.plot3d(nparraypoints)
metrics = []
vis.mesh(obj.mesh.trimesh, style='surface')
for grasp in grasps:
success, c = grasp.close_fingers(obj)
if success:
c1, c2 = c
#fn_fc = quality_fn(grasp).quality
true_fc = PointGraspMetrics3D.grasp_quality(grasp, obj, quality_config)
true_fc = true_fc
metrics.append(true_fc)
#color = quality_fn(grasp).quality
#if (true_fc > 1.0):
# true_fc = 1.0
#color = (1.0-true_fc, true_fc, 0.0)
low = np.min(metrics)
high = np.max(metrics)
print(low)
print(high)
if low == high:
q_to_c = lambda quality: CONFIG['quality_scale']
else:
q_to_c = lambda quality: CONFIG['quality_scale'] * (quality - low) / (high - low)
print(len(metrics))
for grasp, metric in zip(grasps, metrics):
color = plt.get_cmap('hsv')(q_to_c(metric))[:-1]
print(color)
vis.grasp(grasp, grasp_axis_color=color,endpoint_color=color)
vis.show(False)
def antipodal_grasp_sampler(visual=False, debug=False):
mass = 1.0
CONFIG['obj_rescaling_type'] = RescalingType.RELATIVE
mesh_processor = MeshProcessor(OBJ_FILENAME, CONFIG['cache_dir'])
mesh_processor.generate_graspable(CONFIG)
mesh = mesh_processor.mesh
sdf = mesh_processor.sdf
stable_poses = mesh_processor.stable_poses
obj = GraspableObject3D(sdf, mesh)
stable_pose = stable_poses[0]
if visual:
vis.figure()
T_table_world = RigidTransform(from_frame='table', to_frame='world')
T_obj_world = Visualizer3D.mesh_stable_pose(obj.mesh.trimesh,
stable_pose.T_obj_world,
T_table_world=T_table_world,
color=(0.5, 0.5, 0.5),
style='surface',
plot_table=True,
dim=0.15)
if debug:
print(len(stable_poses))
#for stable_pose in stable_poses:
# print(stable_pose.p)
# print(stable_pose.r)
# print(stable_pose.x0)
# print(stable_pose.face)
# print(stable_pose.id)
# glass = 22 is standing straight
if debug:
print(stable_pose.r)
print(stable_pose.T_obj_world)
gripper = RobotGripper.load(
GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers')
ags = AntipodalGraspSampler(gripper, CONFIG)
stable_pose.id = 0
grasps = ags.generate_grasps(obj,
target_num_grasps=20,
max_iter=5,
stable_pose=stable_pose.r)
quality_config = GraspQualityConfigFactory.create_config(
CONFIG['metrics']['robust_ferrari_canny'])
metrics = []
result = []
#grasps = map(lambda g : g.perpendicular_table(stable_pose), grasps)
for grasp in grasps:
c1, c2 = grasp.endpoints
true_fc = PointGraspMetrics3D.grasp_quality(grasp, obj, quality_config)
metrics.append(true_fc)
result.append((c1, c2))
if debug:
success, c = grasp.close_fingers(obj)
if success:
c1, c2 = c
print("Grasp:")
print(c1.point)
print(c2.point)
print(true_fc)
low = np.min(metrics)
high = np.max(metrics)
if low == high:
q_to_c = lambda quality: CONFIG['quality_scale']
else:
q_to_c = lambda quality: CONFIG['quality_scale'] * (quality - low) / (
high - low)
if visual:
for grasp, metric in zip(grasps, metrics):
#grasp2 = grasp.perpendicular_table(stable_pose)
#c1, c2 = grasp.endpoints
#axis = ParallelJawPtGrasp3D.axis_from_endpoints(c1, c2)
#angle = np.dot(np.matmul(stable_pose.r, axis), [1,0,0])
#angle = math.tan(axis[1]/axis[0])
#angle = math.degrees(angle)%360
#print(angle)
#print(angle/360.0)
color = plt.get_cmap('hsv')(q_to_c(metric))[:-1]
vis.grasp(grasp,
T_obj_world=T_obj_world,
grasp_axis_color=color,
endpoint_color=color)
#axis = np.array([[0,0,0], point])
#points = [(x[0], x[1], x[2]) for x in axis]
#Visualizer3D.plot3d(points, color=(0,0,1), tube_radius=0.002)
vis.show(False)
pose_matrix = np.eye(4, 4)
pose_matrix[:3, :3] = T_obj_world.rotation
pose_matrix[:3, 3] = T_obj_world.translation
return pose_matrix, result
