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
