def __init__(self, apc_order): self._create_knowledge_base() self._load_apc_order(apc_order) self.manager = MotionManager(self.knowledge_base, parallelism=1) self.manager.start() self.perception = PerceptionServer(self.knowledge_base, parallelism=1)
class Master: def __init__(self, apc_order): self._create_knowledge_base() self._load_apc_order(apc_order) self.manager = MotionManager(self.knowledge_base, parallelism=1) self.manager.start() self.perception = PerceptionServer(self.knowledge_base, parallelism=1) def _create_knowledge_base(self): self.knowledge_base = KnowledgeBase() #import object dimension CSV file try: reader = csv.reader(open(os.path.join(knowledge_base_dir,'Object Dimensions.csv'))) header = None for line in reader: if header == None: header = line else: self.knowledge_base.object_minimum_dims[line[0]] = float(line[1])*0.01 self.knowledge_base.object_maximum_dims[line[0]] = float(line[2])*0.01 self.knowledge_base.object_median_dims[line[0]] = float(line[3])*0.01 except IOError: logger.info("Object dimensions unknown, try filling in Object Dimensions.csv") pass for k in [ 'bin_vantage_points', 'vantage_point_xforms', 'withdraw_point_xforms', 'bin_bounds' ]: path = os.path.join(knowledge_base_dir, '{}.json'.format(k)) logger.info('loading {} from {}'.format(k, path)) data = json.load(open(path)) setattr(self.knowledge_base, k, data) def _load_apc_order(self, apc_order): global translation_to_berkeley # populate knowledge base self.knowledge_base.starting_bin_contents = apc_order['bin_contents'].copy() #do translation to berkeley for (b,objects) in self.knowledge_base.starting_bin_contents.iteritems(): for i,o in enumerate(objects): if o in translation_to_berkeley: objects[i] = translation_to_berkeley[o] self.knowledge_base.bin_contents = self.knowledge_base.starting_bin_contents.copy() self.order = apc_order['work_order'][:] for i,orderitem in enumerate(self.order): if orderitem['item'] in translation_to_berkeley: self.order[i]['item'] = translation_to_berkeley[orderitem['item']] # TODO: sort order by priority def _move_initial(self): # start at the initial pose so that sensors are free to localize logger.debug('returning to initial pose') if not self.manager.moveToInitialPose(): logger.error('failed to move to initial pose') return False return True def _localize_shelf_and_order_bin(self): # find the shelf and order bin in parallel logger.debug('finding shelf and order bin xforms') shelf_task = self.perception.localizeShelf() order_bin_task = self.perception.localizeOrderBin() # this is accomplished by adding the localizeShelf and localizeOrderBin jobs into a single list task = JobManager(jobs=shelf_task.jobs + order_bin_task.jobs) # wait for both tasks to complete in parallel # ignore the wait result value since the outcome is determined by how many tasks were successful wait_task(task, lambda: (shelf_task.count_success() >= 1 and order_bin_task.count_success() >= 1) or task.all_done(), 15) if shelf_task.count_success() == 0 or order_bin_task.count_success() == 0: if shelf_task.count_success() == 0: logger.error('failed to localize shelf') if order_bin_task.count_success() == 0: logger.error('failed to localize order bin') return False # update the knowledge base with the shelf and order bin transforms self.knowledge_base.shelf_xform = shelf_task.get_success()[0].result self.knowledge_base.order_bin_xform = order_bin_task.get_success()[0].result logger.debug('found shelf at {}'.format(self.knowledge_base.shelf_xform)) logger.debug('found order bin at {}'.format(self.knowledge_base.order_bin_xform)) return True def _set_target(self, target_bin, target_object): # set the target in the knowledge base self.knowledge_base.target_bin = target_bin self.knowledge_base.target_object = target_object if target_bin and target_object: logger.info('attempting {} in {}'.format(target_object, target_bin)) def _move_vantage_point(self, vantage_point): logger.debug('moving to {}'.format(vantage_point)) if not self.manager.moveToVantagePoint(self.knowledge_base.target_bin, vantage_point): logger.error('failed to move to {}'.format(vantage_point)) return False return True def _localize_object(self, vantage_point): (target_bin, target_object) = (self.knowledge_base.target_bin, self.knowledge_base.target_object) # find the object from that vantage point task = self.perception.localizeSpecificObject(target_bin, target_object) # ignore the wait result value since the outcome is determined by how many tasks were successful wait_task(task, lambda: task.count_success() >= 1 or task.all_done(), 60) if task.count_success() == 0: logger.warn('could not find {} from {}'.format(target_object, vantage_point)) return False # update knowledge_base with localized object transform, point cloud, and confusion matrix (object_xform, object_cloud, confusion_matrix) = task.get_success()[0].result self.knowledge_base.object_xforms[target_object] = object_xform self.knowledge_base.object_clouds[target_object] = object_cloud self.knowledge_base.last_confusion_matrix = confusion_matrix logger.info('found {} at {}'.format(target_object, object_xform)) logger.debug('found {} with {} points'.format(target_object, len(object_cloud))) return True def _find_object(self): (target_bin, target_object) = (self.knowledge_base.target_bin, self.knowledge_base.target_object) # search for the object from all available vantage points object_found = False # vantage points are ordered by goodness for vantage_point in self.knowledge_base.bin_vantage_points[target_bin]: if not self._move_vantage_point(vantage_point): return False if self._localize_object(vantage_point): object_found = True break else: pass # try another vantage point if not object_found: logger.error('failed to find {} from any vantage point'.format(target_object)) return False return True def _grasp_object(self): (target_bin, target_object) = (self.knowledge_base.target_bin, self.knowledge_base.target_object) # grasp the object logger.debug('grasping {} in {}'.format(target_object, target_bin)) if not self.manager.graspObjectInBin(target_bin, target_object): logger.error('failed to grasp {} in {}'.format(target_object, target_bin)) return False # update knowledge base to reflect grasped object self.knowledge_base.grasped_object = target_object logger.info('grasped {}!'.format(target_object)) return True def _retrieve_object(self): (target_bin, target_object) = (self.knowledge_base.target_bin, self.knowledge_base.target_object) # move the grasped object to the order bin logger.debug('moving {} to order bin'.format(target_object)) if not self.manager.moveObjectToOrderBin(self.knowledge_base.active_limb): logger.error('failed to move {} to order bin'.format(target_object)) return False # update the knowledge base with the object removal self.knowledge_base.bin_contents[target_bin].remove(target_object) del self.knowledge_base.object_xforms[target_object] del self.knowledge_base.object_clouds[target_object] self.knowledge_base.grasped_object = None return True def _undo_failed_retrieve(self): logger.debug('undoing prior grasp due to failed retrieve') self.manager.undo_last_motion() self.knowledge_base.grasped_object = None def run(self, target_bin, target_object): if not self._move_initial() or not self._localize_shelf_and_order_bin(): return False self._set_target(target_bin, target_object) if not self._find_object() or not self._grasp_object(): return False if not self._retrieve_object(): self._undo_failed_retrieve() return False self._set_target(None, None) # this order item is completed logger.info('completed order for {}'.format(target_object)) return True