def run(self):
     """
     Publishes the pot angles.
     """
     global finished
     while True and not finished:
         self.langle_pub.publish(gmt.get_pot_angle('l'))
         self.rangle_pub.publish(gmt.get_pot_angle('r'))
         time.sleep(1 / self.rate)
         
     yellowprint("PublishPotAngles thread has finished running.")
    def process_observation (self, n_avg=5):
        self.iterations += 1
        raw_input(colorize("Iteration %d: Press return when ready to capture transforms."%self.iterations, "red", True))
        
        sleeper = rospy.Rate(30)
        avg_tfms = {}
        j = 0
        thresh = n_avg*2
        pot_avg = 0.0
        while j < n_avg:
            blueprint('\tGetting averaging transform : %d of %d ...'%(j,n_avg-1))    

            tfms = {}
            # F**k the frames bullshit
            ar_tfm = self.cameras.get_ar_markers(markers=self.ar_markers)
            hyd_tfm = gmt.get_hydra_transforms(parent_frame=self.parent_frame, hydras = self.hydras)
            pot_angle = gmt.get_pot_angle()
            
            if not ar_tfm or (not hyd_tfm and self.hydras):
                yellowprint('Could not find all required transforms.')
                thresh -= 1
                if thresh == 0: return False
                continue
            
            pot_avg += pot_angle
            
            j += 1
            tfms.update(ar_tfm)
            tfms.update(hyd_tfm)

            # The angle relevant for each finger is only half the angle.

                        
            for marker in tfms:
                if marker not in avg_tfms:
                    avg_tfms[marker] = []
                avg_tfms[marker].append(tfms[marker])
                
            sleeper.sleep()
            
        pot_avg /= n_avg
        
        greenprint("Average angle found: %f"%pot_avg)

        for marker in avg_tfms:
            avg_tfms[marker] = utils.avg_transform(avg_tfms[marker])

        update_groups_from_observations(self.masterGraph, avg_tfms, pot_angle)
        return True
Example #3
0
 def get_specific_tooltip_tfm (self, m_type='AR'):
     """
     Not used in any places
     Get the estimate of the tool tip from either ar_markers or hydras.
     """
     if m_type not in ['AR', 'hydra']:
         redprint('Not sure what estimate %s gives.' % m_type)
         return
     
     if m_type == 'AR':
         if len(self.ar_markers) == 0:
             redprint('No AR markers to give you an estimate.')
             return
         marker_tfms = self.cameras.get_ar_markers(markers=self.ar_markers)
     else:
         if len(self.hydra_markers) == 0:
             redprint('No hydras to give you an estimate.')
             return
         marker_tfms = gmt.get_hydra_transforms(self.parent_frame, self.hydra_markers)
     
     theta = gmt.get_pot_angle(self.lr)
     return self.get_tooltip_transform(marker_tfms, theta)
    def process_observation (self, n_avg=5):
        self.iterations += 1
        raw_input(colorize("Iteration %d: Press return when ready to capture transforms."%self.iterations, "red", True))
        
        avg_tfms = {}
        for j in xrange(n_avg):
            print colorize('\tGetting averaging transform : %d of %d ...'%(j,n_avg-1), "blue", True)            

            tfms = {}
            tfms.update(gmt.get_ar_markers_from_cameras(self.cameras, parent_frame=self.parent_frame, markers=self.ar_markers))
            tfms.update(gmt.get_hydra_transforms(parent_frame=parent_frame, hydras = self.hydras))

            pot_angle = gmt.get_pot_angle()
                        
            for marker in tfms:
                if marker not in avg_tfms:
                    avg_tfms[marker] = []
                avg_tfms[marker].append(tfms[marker])
        
        for marker in avg_tfms:
            avg_tfms[marker] = utils.avg_transform(avg_tfms[marker])

        update_groups_from_observations(self.masterGraph, avg_tfms, pot_angle)
Example #5
0
    def get_obs_new_marker(self, marker, n_tfm=10, n_avg=10):
        """
        Store a bunch of readings seen for new marker being added.
        Returns a list of dicts each of which has marker transforms found
        and potentiometer angles.
        """
        
        all_obs = []
        
        i = 0
        thresh = n_avg * 2
        sleeper = rospy.Rate(30)
        
        while i < n_tfm:
            raw_input(colorize("Getting transform %i out of %i. Hit return when ready." % (i, n_tfm), 'yellow', True))
            
            j = 0
            j_th = 0
            pot_angle = 0
            found = True
            avg_tfms = []
            while j < n_avg:
                blueprint("Averaging transform %i out of %i." % (j, n_avg))
                ar_tfms = self.cameras.get_ar_markers();
                hyd_tfms = gmt.get_hydra_transforms(parent_frame=self.parent_frame, hydras=None);
                curr_angle = gmt.get_pot_angle(self.lr)
                
                if not ar_tfms and not hyd_tfms:
                    yellowprint("Could not find any transform.")
                    j_th += 1
                    if j_th < thresh:
                        continue
                    else:
                        found = False
                        break
                                
                tfms = ar_tfms
                pot_angle += curr_angle
                tfms.update(hyd_tfms)

                avg_tfms.append(tfms)
                j += 1
                sleeper.sleep()
            
            if found is False:
                yellowprint("Something went wrong; try again.")
                continue
            
            tfms_found = {}
            for tfms in avg_tfms:
                for m in tfms:
                    if m not in tfms_found:
                        tfms_found[m] = []
                    tfms_found[m].append(tfms[m])

            if marker not in tfms_found:
                yellowprint("Could not find marker to be added; try again.")
                continue
            
            for m in tfms_found:
                tfms_found[m] = utils.avg_transform(tfms_found[m])
            pot_angle = pot_angle / n_avg
            
            all_obs.append({'tfms':tfms_found, 'pot_angle':pot_angle})
            i += 1
            
        return all_obs
Example #6
0
    def get_all_transforms(self, parent_frame, diff_cam=False):
        """
        From marker transforms given in parent_frame, get all transforms
        of all markers on gripper.
        """
    	if self.cameras is None:
    	    redprint("Cameras not initialized. Could not get transforms.")
    	    return

        ar_tfms = self.cameras.get_ar_markers()

        if diff_cam:
            ar_tfms_cam = {}
            for i in range(self.cameras.num_cameras):
                ar_tfms_cam[i] = self.cameras.get_ar_markers(camera=i, parent_frame=True)

        hyd_tfms = gmt.get_hydra_transforms(self.parent_frame, None)
        theta = gmt.get_pot_angle(self.lr)

        marker_tfms = ar_tfms
        marker_tfms.update(hyd_tfms)
        
        ret_tfms = []
        
        cor_avg_tfms = []
        cor_hyd_avg = []
        cor_ar_avg = []
        ar_found = False
        hyd_found = False
        for m, tfm in marker_tfms.items():
            if m in self.ar_markers:
                c_tfm = tfm.dot(self.get_rel_transform(m, 'cor', theta)) 
                cor_ar_avg.append(c_tfm)
                cor_avg_tfms.append(c_tfm)
                ar_found = True
            elif m in self.hydra_markers:
                c_tfm = tfm.dot(self.get_rel_transform(m, 'cor', theta)) 
                cor_hyd_avg.append(c_tfm)
                cor_avg_tfms.append(c_tfm)
                hyd_found = True
        
        if diff_cam:
            cor_ar_cams = {}
            cam_found = {i:False for i in ar_tfms_cam}
            for i in ar_tfms_cam:
                for m, tfm in ar_tfms_cam[i].items():
                    if m in self.ar_markers:
                        c_tfm = tfm.dot(self.get_rel_transform(m, 'cor', theta))
                        if i not in cor_ar_cams:
                            cor_ar_cams[i] = []
                            cam_found[i] = True 
                        cor_ar_cams[i].append(c_tfm)
                if cam_found[i]:
                    cor_ar_cams[i] = utils.avg_transform(cor_ar_cams[i])


        if len(cor_avg_tfms) == 0: return ret_tfms
        
        cor_tfm = utils.avg_transform(cor_avg_tfms)
        cor_h_tfm = utils.avg_transform(cor_hyd_avg)
        cor_a_tfm = utils.avg_transform(cor_ar_avg)
        
        ret_tfms.append({'parent':parent_frame,
                         'child':'%sgripper_%s' % (self.lr, 'cor'),
                         'tfm':cor_tfm})

        for m in self.allmarkers:
            if m != 'cor' and m != 'tool_tip':
                tfm = self.get_rel_transform('cor', m, theta)
                ret_tfms.append({'parent':parent_frame,
                                 'child':'%sgripper_%s' % (self.lr, m),
                                 'tfm':cor_tfm.dot(tfm)})

        if self.tooltip_calculated:
            tfm = self.get_rel_transform('cor', 'tool_tip', theta)
            ret_tfms.append({'parent':parent_frame,
                             'child':'%sgripper_tooltip' % self.lr,
                             'tfm':cor_tfm.dot(tfm)})
            if hyd_found:
                ret_tfms.append({'parent':parent_frame,
                                 'child':'%sgripper_tooltip_hydra' % self.lr,
                                 'tfm':cor_h_tfm.dot(tfm)})
            if ar_found:
                ret_tfms.append({'parent':parent_frame,
                                 'child':'%sgripper_tooltip_ar' % self.lr,
                                 'tfm':cor_a_tfm.dot(tfm)})
            
            if diff_cam:
                for i, cor_tfm in cor_ar_cams.items():
                    ret_tfms.append({'parent':parent_frame,
                                     'child':'%sgripper_tooltip_camera%i' % (self.lr, i + 1),
                                     'tfm':cor_tfm.dot(tfm)})
                    
        
        return ret_tfms