def showgraspfrompickle(self, object_name,metric_name,onlynagative,onlypositive,openravechecker):
object = self.dataset[object_name]
labelgrasps,labelmetrics=self.readdatafrompickle(object_name)
vis.figure()
vis.mesh(object.mesh.trimesh ,style='surface')
config=self._get_config(None)
low =0.0
high = np.max([labelmetrics[i][metric_name] for i in range(len(labelmetrics))])
# print 'high',high,config['quality_scale']
if low == high:
q_to_c = lambda quality: config['quality_scale']
else:
q_to_c = lambda quality: config['quality_scale'] * (quality - low) / (high - low)
recalculate_grasp=[]
for i in range(len(labelgrasps)):
if labelmetrics[i][metric_name]==0 :
if not onlypositive:
vis.graspwithapproachvectorusingcenter_point(labelgrasps[i] ,approaching_color=(1,0,0), grasp_axis_color=(1,0,0) )
elif labelmetrics[i][metric_name]==-1 :
if not onlypositive:
vis.shownormals(labelgrasps[i][1],labelgrasps[i][0],color=(1,0,0) )
recalculate_grasp.append(labelgrasps[i])
elif labelmetrics[i][metric_name]>0 :
if not onlynagative:
color = plt.get_cmap('hsv')(q_to_c(labelmetrics[i][metric_name]))[:-1]
vis.graspwithapproachvectorusingcenter_point(labelgrasps[i] ,approaching_color=color, grasp_axis_color=color )
recalculate_grasp.append(labelgrasps[i])
print i # ,high
vis.pose(RigidTransform(), alpha=0.1)
vis.show(animate=False)
def showgraspfrompicklewithcandidate(self, object_name,metric_name,onlynagative,onlypositive,openravechecker):
object = self.dataset[object_name]
labelgrasps,labelmetrics,candidate_labelgrasps,candidate_labelmetrics=self.readdatafrompicklewithcandidate(object_name)
config=self._get_config(None)
low =0.0 #np.min(metrics)
high = np.max([labelmetrics[i][metric_name] for i in range(len(labelmetrics))])
# print 'high',high,config['quality_scale']
if low == high:
q_to_c = lambda quality: config['quality_scale']
else:
q_to_c = lambda quality: config['quality_scale'] * (quality - low) / (high - low)
recalculate_grasp=[]
vis.figure()
vis.mesh(object.mesh.trimesh ,style='surface')
for i in range(len(labelgrasps)):
if labelmetrics[i][metric_name]==0 :
if not onlypositive:
vis.figure()
vis.mesh(object.mesh.trimesh ,style='surface')
color = plt.get_cmap('hsv')(q_to_c(labelmetrics[i][metric_name]))[:-1]
vis.graspwithapproachvectorusingcenter_point(labelgrasps[i] ,approaching_color=(1,0,0), grasp_axis_color=(1,0,0) )
for kk in range(len(candidate_labelgrasps[i] )):
print 'candidate_labelgrasps[i][kk]',candidate_labelgrasps[i][kk]
color = plt.get_cmap('hsv')(q_to_c(candidate_labelmetrics[i][candidate_labelgrasps[i][kk].id][metric_name]))[:-1]
vis.graspwithapproachvectorusingcenter_point(candidate_labelgrasps[i][kk] ,approaching_color=color, grasp_axis_color=color )
recalculate_grasp.append(labelgrasps[i])
print 'maxtrics',i,labelmetrics[i][metric_name] # ,high
vis.pose(RigidTransform(), alpha=0.1)
vis.show(animate=False)
elif labelmetrics[i][metric_name]==-1 :
if not onlypositive:
vis.shownormals(labelgrasps[i][1],labelgrasps[i][0],color=(0,0,1) )
else:
if not onlynagative:
vis.figure()
vis.mesh(object.mesh.trimesh ,style='surface')
color = plt.get_cmap('hsv')(q_to_c(labelmetrics[i][metric_name]))[:-1]
if len(candidate_labelgrasps[i])>0:
A=np.array(labelgrasps[i].rotated_full_axis[:,0] )
B=np.array(candidate_labelgrasps[i][0].rotated_full_axis[:,0] )
num = np.dot(A.T, B)
print num
if num<0.99:
labelgrasps[i].approach_angle_= labelgrasps[i]._angle_aligned_with_table( -candidate_labelgrasps[i][0].rotated_full_axis[:,0])
vis.graspwithapproachvectorusingcenter_point(labelgrasps[i] ,approaching_color=color, grasp_axis_color=(0,0,1) )
print 'length',len(candidate_labelgrasps[i] )
for kk in range(len(candidate_labelgrasps[i] )):
color = plt.get_cmap('hsv')(q_to_c(candidate_labelmetrics[i][candidate_labelgrasps[i][kk].id][metric_name]))[:-1]
vis.graspwithapproachvectorusingcenter_point(candidate_labelgrasps[i][kk] ,approaching_color=(0,1,1), grasp_axis_color=color )
recalculate_grasp.append(labelgrasps[i])
print 'maxtrics',i,labelmetrics[i][metric_name] # ,high
vis.pose(RigidTransform(), alpha=0.1)
vis.show(animate=False)
