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 main():
video_thread = VideoServer()
control_thread = ControlServer(video_thread)
try:
video_thread.start()
control_thread.start()
while True:
time.sleep(0.1)
except KeyboardInterrupt:
print('Keyboard interrupt')
control_thread.stop()
video_thread.stop()
video_thread.join()
control_thread.join()
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))
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
