class MotionManager: def __init__(self, knowledge_base, **params): self.knowledge_base = knowledge_base params = params or {} self.planner = PlanningServer(knowledge_base, params.get('parallelism', 5)) self.control = ControlServer(knowledge_base) self.pre_execute_robot_state = None self.last_executed_plan = None methods = [ 'moveToVantagePoint', 'graspObjectInBin', 'moveObjectToOrderBin', 'moveToInitialPose', 'moveToXform', 'PickUpOrderTray', 'MoveTrayToBin', 'MoveTrayToOrderBin' ] # set up the interface so that calling "MotionManager.methodName" like # "MotionManager.moveToVantagePoint" or "self.manager.moveToVantagePoint" inside Master class # invokes the "MotionManager._call(method, args ,kwargs)" function for method in methods: # method contains a string of methodName # this outher wrapping function is necessary to capture the method variable def make_proxy(method): # see here for *args and **kwargs # http://stackoverflow.com/questions/3394835/args-and-kwargs def proxy(*args, **kwargs): return self._call(method, args, kwargs) # returns the function handle "self._call(method, args, kwargs)" return proxy # equivalent to self.method = make_proxy(method) # = self._call(method, *args, **kwargs) # ie.) self.moveToVantagePoint = function_handle of self._call(method, args, kwargs) setattr(self, method, make_proxy(method)) def start(self): logger.info('starting control server') task = self.control.start() # check that control server started wait_task(task, lambda: task.done, 10) if task.error: logger.error('failed to start control server') return False else: logger.info('control server started') return True self.pre_execute_robot_state = None self.last_executed_path = None def _check_control(self): if self.control.running: return True else: logger.error('control server has died -> restarting') self._restart_control() return False def _restart_control(self): self.control.safe_close() self.start() def undo_last_motion(self): if not self.last_executed_path: logger.error('Trying to undo last motion, but no motion was previously executed') return False #get the gripper commands leading up to the end of the motion previous_gripper_commands = [baxter.get_q_gripper_command(self.pre_execute_robot_state.commanded_config,'left')] for (type, command, waitBefore, waitAfter) in self.last_executed_path: if type.endswith('gripper'): if (type.startswith('left')): previous_gripper_commands.append(command) #go through the previous path reverse_path = list(reversed(self.last_executed_path)) ngripper_commands = 0 for index,(type, command, waitBefore, waitAfter) in enumerate(reverse_path): if(type == 'path' or type == 'fast_path'): #reverse the path command = command[::-1] elif(type.endswith('gripper')): #go to the prior gripper config command = previous_gripper_commands[-2-ngripper_commands] if (type.startswith('left')): ngripper_commands += 1 else: logger.error('You messed up! Not a path or gripper command') raise Exception reverse_path [index] = type,command,waitAfter,waitBefore self.pre_execute_robot_state = copy.deepcopy(self.knowledge_base.robot_state) self.last_executed_path = reverse_path task = self.control.execute(reverse_path) # check that the plan executes safely wait_task(task, lambda: task.done, 600) # check that the task finished successfully (task.result) and did not have an error (task.error) if not task.result or task.error: if task.error: logger.error('plan execution failed with error: {} -> restarting'.format(task.error)) elif not task.result: logger.error('plan execution failed non-specifically -> restarting') # restart the control server for good measure self._restart_control() return False return True # TODO: NEED TO UNDERSTAND THIS FUNCTION def _call(self, method, args, kwargs): # check that the control server is running if not self._check_control(): return False # < 1. request a robot state update > task = self.control.update() # = MotionManager.control.update() = ControlServer(KB).update() = ControlServer._call(None,None,None) # wait for task to finish, or time-out, using task.done as the condition for determining whether the task is completed, and 10s as timeout wait_task(task, lambda: task.done, 10) if task.error: logger.warn('robot state updated failed... planning from old state') # < 2. Invoke the planner to plan for a task (plan_method) > # map to the plan name (capitalize the first letter of the method: ie. blah --> planBlah) plan_method = 'plan' + method[0].upper() + method[1:] # invoke the planner # task = PlanningServer.planBlah(*args, **kwargs) # = JobServer.planBlah(*args, **kwargs) # = JobServer._call(planBlah, *args, **kwargs) # = create, run and return JobManager(factory=lambda: self.factory(self.init_args, method, args, kwargs), # count=self.parallelism) # = create, run, and return JobManager(factory = function handle of PlanningJob(knowledgeBase, method, args, kwargs), # count = self.parallelism) # JobManager.run creates a Process(target = _handler) in the BaseJob class task = getattr(self.planner, plan_method)(*args, **kwargs) target_plan_count = 3 # let the planner do its job, count number of successes # the failed wait is only a problem if no plans were returned so ignore its return value wait_task(task, lambda: task.count_success() >= target_plan_count or task.all_done(), 600) if task.count_success() == 0: logger.warn('no plans available for {}'.format(method)) return False logger.debug('got {}/{} plans'.format(task.count_success(), target_plan_count)) # choose which plan to execute plans = [ job.result for job in task.get_success() ] # plans are tuples of (plan, goodness, limb) # limit to the first two tuple values since not all plans return an active limb best_plan = sorted(plans, key=lambda p: p[1])[0] logger.debug('best plan: {} ({})'.format(best_plan[0], best_plan[1])) # update the knowledge base with the active limb if len(best_plan) > 2: self.knowledge_base.active_limb = best_plan[2] else: self.knowledge_base.active_limb = None # update the knowledge base with the active grasp self.knowledge_base.active_grasp = best_plan[3] logger.debug('active limb changed to {}'.format(self.knowledge_base.active_limb)) #save state in case you want to undo self.pre_execute_robot_state = copy.deepcopy(self.knowledge_base.robot_state) self.last_executed_path = best_plan[0] # execute the plan task = self.control.execute(best_plan[0]) # check that the plan executes safely wait_task(task, lambda: task.done, 600) # check that the task finished successfully (task.result) and did not have an error (task.error) if not task.result or task.error: if task.error: logger.error('plan execution failed with error: {} -> restarting'.format(task.error)) elif not task.result: logger.error('plan execution failed non-specifically -> restarting') # restart the control server for good measure self._restart_control() return False return True
class MotionManager: def __init__(self, knowledge_base, **params): self.knowledge_base = knowledge_base params = params or {} self.planner = PlanningServer(knowledge_base, params.get('parallelism', 5)) self.control = ControlServer(knowledge_base) self.pre_execute_robot_state = None self.last_executed_plan = None methods = [ 'moveToVantagePoint', 'graspObjectInBin', 'moveObjectToOrderBin', 'moveToInitialPose', 'moveToXform', 'PickUpOrderTray', 'MoveTrayToBin', 'MoveTrayToOrderBin' ] # set up the interface for method in methods: # this outher wrapping function is necessary to capture the method variable def make_proxy(method): def proxy(*args, **kwargs): return self._call(method, args, kwargs) return proxy setattr(self, method, make_proxy(method)) def start(self): logger.info('starting control server') task = self.control.start() # check that control server started wait_task(task, lambda: task.done, 10) if task.error: logger.error('failed to start control server') return False else: logger.info('control server started') return True self.pre_execute_robot_state = None self.last_executed_path = None def _check_control(self): if self.control.running: return True else: logger.error('control server has died -> restarting') self._restart_control() return False def _restart_control(self): self.control.safe_close() self.start() def undo_last_motion(self): if not self.last_executed_path: logger.error('Trying to undo last motion, but no motion was previously executed') return False #get the gripper commands leading up to the end of the motion previous_gripper_commands = [baxter.get_q_gripper_command(self.pre_execute_robot_state.commanded_config,'left')] for (type, command, waitBefore, waitAfter) in self.last_executed_path: if type.endswith('gripper'): if (type.startswith('left')): previous_gripper_commands.append(command) #go through the previous path reverse_path = list(reversed(self.last_executed_path)) ngripper_commands = 0 for index,(type, command, waitBefore, waitAfter) in enumerate(reverse_path): if(type == 'path' or type == 'fast_path'): #reverse the path command = command[::-1] elif(type.endswith('gripper')): #go to the prior gripper config command = previous_gripper_commands[-2-ngripper_commands] if (type.startswith('left')): ngripper_commands += 1 else: logger.error('You messed up! Not a path or gripper command') raise Exception reverse_path [index] = type,command,waitAfter,waitBefore self.pre_execute_robot_state = copy.deepcopy(self.knowledge_base.robot_state) self.last_executed_path = reverse_path task = self.control.execute(reverse_path) # check that the plan executes safely wait_task(task, lambda: task.done, 600) # check that the task finished successfully (task.result) and did not have an error (task.error) if not task.result or task.error: if task.error: logger.error('plan execution failed with error: {} -> restarting'.format(task.error)) elif not task.result: logger.error('plan execution failed non-specifically -> restarting') # restart the control server for good measure self._restart_control() return False return True def _call(self, method, args, kwargs): # check that the control server is running if not self._check_control(): return False # request a robot state update task = self.control.update() wait_task(task, lambda: task.done, 10) if task.error: logger.warn('robot state updated failed... planning from old state') # map to the plan name plan_method = 'plan' + method[0].upper() + method[1:] # invoke the planner task = getattr(self.planner, plan_method)(*args, **kwargs) target_plan_count = 3 # the failed wait is only a problem if no plans were returned so ignore its return value wait_task(task, lambda: task.count_success() >= target_plan_count or task.all_done(), 600) if task.count_success() == 0: logger.warn('no plans available for {}'.format(method)) return False logger.debug('got {}/{} plans'.format(task.count_success(), target_plan_count)) # choose which plan to execute plans = [ job.result for job in task.get_success() ] # plans are tuples of (plan, goodness, limb) # limit to the first two tuple values since not all plans return an active limb best_plan = sorted(plans, key=lambda p: p[1])[0] logger.debug('best plan: {} ({})'.format(best_plan[0], best_plan[1])) # update the knowledge base with the active limb if len(best_plan) > 2: self.knowledge_base.active_limb = best_plan[2] else: self.knowledge_base.active_limb = None # update the knowledge base with the active grasp self.knowledge_base.active_grasp = best_plan[3] logger.debug('active limb changed to {}'.format(self.knowledge_base.active_limb)) #save state in case you want to undo self.pre_execute_robot_state = copy.deepcopy(self.knowledge_base.robot_state) self.last_executed_path = best_plan[0] # execute the plan task = self.control.execute(best_plan[0]) # check that the plan executes safely wait_task(task, lambda: task.done, 600) # check that the task finished successfully (task.result) and did not have an error (task.error) if not task.result or task.error: if task.error: logger.error('plan execution failed with error: {} -> restarting'.format(task.error)) elif not task.result: logger.error('plan execution failed non-specifically -> restarting') # restart the control server for good measure self._restart_control() return False return True