Ejemplo n.º 1
0
    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)       
Ejemplo n.º 2
0
    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)