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 grasp_quality_calculator(mesh, sdf, grasps, metric):
obj = GraspableObject3D(sdf, mesh)
quality_config = GraspQualityConfigFactory.create_config(
CONFIG['metrics'][metric])
quality_function = GraspQualityFunctionFactory.create_quality_function(
obj, quality_config)
metrics = []
for grasp in grasps:
quality = quality_function.quality(grasp)
metrics.append(copy.deepcopy(quality.quality))
return metrics
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 antipodal_grasp_sampler_for_storing_stable_poses(mesh, sdf, stable_poses):
mass = 1.0
CONFIG['obj_rescaling_type'] = RescalingType.RELATIVE
obj = GraspableObject3D(sdf, mesh)
gripper = RobotGripper.load(
GRIPPER_NAME, gripper_dir='/home/silvia/dex-net/data/grippers')
ags = AntipodalGraspSampler(gripper, CONFIG)
quality_config = GraspQualityConfigFactory.create_config(
CONFIG['metrics']['force_closure'])
quality_function = GraspQualityFunctionFactory.create_quality_function(
obj, quality_config)
max_poses = len(stable_poses)
grasps = [None] * max_poses
metrics = [None] * max_poses
all_grasps = ags.generate_grasps(obj, target_num_grasps=200, max_iter=4)
for id, stable_pose in enumerate(stable_poses):
print('sampling for stable pose: ', id)
if id == max_poses:
break
grasps_pose = filter(
lambda x: grasp_is_parallel_to_table(x.axis, stable_pose.r),
all_grasps)
#grasps_pose = ags.generate_grasps(obj,target_num_grasps=20, max_iter=5, stable_pose=stable_pose.r)
grasps[id] = []
metrics[id] = []
for grasp in grasps_pose:
quality = quality_function.quality(grasp)
#quality = PointGraspMetrics3D.grasp_quality(grasp, obj, quality_config)
grasps[id].append(copy.deepcopy(grasp))
metrics[id].append(copy.deepcopy(quality.quality))
return grasps, metrics
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)