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 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)
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
