def call_planner_sp(planner, domain, problem, args, pwd, verbose):
'''
Call an external deterministic planner in single processing.
Arguments:
@domain : path to a given domain
@problem : path to a given problem
@planner : the name of the external planner
@args : the default args of the external planner
@pwd : the current working directory
@verbose : if True, prints statistics before returning
'''
## FF planner ##
if 'ff' in planner.lower():
return call_ff(domain, problem, args, pwd, verbose)
## Madagascar (M) planner ##
elif 'm' in planner.lower():
return call_m(domain, problem, args, pwd, verbose)
## PROBE planner ##
elif 'probe' in planner.lower():
return call_probe(domain, problem, args, pwd, verbose)
## optic-clp planner ##
elif 'optic-clp' in planner.lower() or 'optic' in planner.lower():
return call_optic_clp(domain, problem, args, pwd, verbose)
## optic-clp planner ##
elif 'vhpop' in planner.lower():
return call_vhpop(domain, problem, args, pwd, verbose)
## lpg-td planner ##
elif 'lpg-td' in planner.lower():
return call_lpg_td(domain, problem, args, pwd, verbose)
## lpg-td planner ##
elif 'lpg' in planner.lower():
return call_lpg(domain, problem, args, pwd, verbose)
## lpg-td planner ##
elif 'fd' in planner.lower():
return call_fd(domain, problem, args, pwd, verbose)
## no planner ##
print(color.fg_red("\n[There is not yet a function for parsing the outputs of '{0}'!]\n".format(planner)))
# exit()
return -1
def json_ma_plan(policy, agents=[], full=False, verbose=False):
'''
Convert given plan into a concurrent plan for execution by multi-robot.
The output is partial parallel plan iff the given plan is partial-order.
'''
# get the first pre-order path
try:
if full:
path = policy.plan()
else:
path = policy.get_paths(policy.plan())[0]
except:
return None
if verbose:
print(color.fg_red('[EXPERIMENTAL JSON PLAN!]'))
print(
color.fg_red(
'[APPLIED ONLY TO THE FIRST PRE-ORDER PATH OF THE PLAN]'))
policy.print_plan(path)
# get concurrent executions in concurrent clusters
concurrent_subplans_lists = concurrent_subplans(policy, path, agents)
main_list = OrderedDict()
# temporary: ignore outcomes of every step in subplans
for key, subplans in concurrent_subplans_lists.items():
new_subplans = []
for subplan in subplans:
new_subplan = OrderedDict()
for k, (actions, outcomes) in subplan.items():
new_subplan[k] = list(actions)
new_subplans.append(new_subplan)
main_list[key] = new_subplans
plan_json = OrderedDict()
action_descriptions_json = OrderedDict()
plan_json['main'] = OrderedDict({'list': [], 'ordering': 'total'})
for n, (key, subplans) in enumerate(main_list.items()):
# -------------------------------------------------------------------------------
if len(subplans) == 1: # there is only one parallel subplan
# ----------------------------------------------------
if len(subplans[0]) == 1: # there is only one step in this subplan
# ----------------------------------------------------
if len(subplans[0]
[key]) == 1: # there is only one action in this step
# make a reference to action (key+i)
action_ref = 'action_{}'.format(n)
action_ref = '_'.join(subplans[0][key][0].sig)
# add ref to main list of the plan
plan_json['main']['list'].append(action_ref)
# add ref and its description into the action_descriptions_json
action_descriptions_json[action_ref] = action_json(
subplans[0][key][0])
# ----------------------------------------------------
else: # there are more actions in this step
# make a reference to subplan (n)
subplan_ref = 'subplan_{}'.format(n)
plan_json['main']['list'].append(subplan_ref)
plan_json[subplan_ref] = OrderedDict({
'list': [],
'ordering': 'partial'
})
for i, action in enumerate(subplans[0][key]):
# make a reference to action (n+i+j)
action_ref = 'action_{}_{}'.format(n, i)
action_ref = '_'.join(action.sig)
# add ref to main list of the plan
plan_json[subplan_ref]['list'].append(action_ref)
# add ref and its description into the action_descriptions_json
action_descriptions_json[action_ref] = action_json(
action)
# ----------------------------------------------------
else: # there are more steps in this subplan
# make a reference to subplan (n)
subplan_ref = 'subplan_{}'.format(n)
plan_json['main']['list'].append(subplan_ref)
plan_json[subplan_ref] = OrderedDict({
'list': [],
'ordering': 'total'
})
for i, (k, step) in enumerate(subplans[0].items()):
# ----------------------------------------------------
if len(step) == 1: # there is only one action in this step
# make a reference to action (key+i)
action_ref = 'action_{}_{}'.format(n, i)
action_ref = '_'.join(step[0].sig)
# add ref to main list of the plan
plan_json[subplan_ref]['list'].append(action_ref)
# add ref and its description into the action_descriptions_json
action_descriptions_json[action_ref] = action_json(
step[0])
# ----------------------------------------------------
else: # there are more actions in this step
# make a reference to subplan (n)
subsubplan_ref = 'subplan_{}_{}'.format(n, i)
plan_json[subplan_ref]['list'].append(subsubplan_ref)
plan_json[subsubplan_ref] = OrderedDict({
'list': [],
'ordering':
'partial'
})
for j, action in enumerate(step):
# make a reference to action (n+i+j)
action_ref = 'action_{}_{}_{}'.format(n, i, j)
action_ref = '_'.join(action.sig)
# add ref to main list of the plan
plan_json[subsubplan_ref]['list'].append(
action_ref)
# add ref and its description into the action_descriptions_json
action_descriptions_json[action_ref] = action_json(
action)
# -------------------------------------------------------------------------------
else: # there are more parallel subplans
# make a reference to subplan (n)
subplan_ref = 'subplan_{}'.format(n)
plan_json['main']['list'].append(subplan_ref)
plan_json[subplan_ref] = OrderedDict({
'list': [],
'ordering': 'partial'
})
for i, subplan in enumerate(subplans):
# ----------------------------------------------------
if len(subplan) == 1: # there is one step in this subplan
# ----------------------------------------------------
if len(list(subplan.values())
[0]) == 1: # there is only one action in this step
# make a reference to action (key+i)
action_ref = 'action_{}_{}'.format(n, i)
action_ref = '_'.join(list(subplan.values())[0][0].sig)
# add ref to main list of the plan
plan_json[subplan_ref]['list'].append(action_ref)
# add ref and its description into the action_descriptions_json
action_descriptions_json[action_ref] = action_json(
list(subplan.values())[0][0])
# ----------------------------------------------------
else: # there are more actions in this step
# make a reference to subplan (n)
subsubplan_ref = 'subplan_{}_{}'.format(n, i)
plan_json[subplan_ref]['list'].append(subsubplan_ref)
plan_json[subsubplan_ref] = OrderedDict({
'list': [],
'ordering':
'partial'
})
for j, action in enumerate(list(subplan.values())[0]):
# make a reference to action (n+i+j)
action_ref = 'action_{}_{}_{}'.format(n, i, j)
action_ref = '_'.join(action.sig)
# add ref to main list of the plan
plan_json[subsubplan_ref]['list'].append(
action_ref)
# add ref and its description into the action_descriptions_json
action_descriptions_json[action_ref] = action_json(
action)
# ----------------------------------------------------
else: # there are more steps in this subplan
# make a reference to subplan (n+i)
subsubplan_ref = 'subplan_{}_{}'.format(n, i)
plan_json[subplan_ref]['list'].append(subsubplan_ref)
plan_json[subsubplan_ref] = OrderedDict({
'list': [],
'ordering': 'total'
})
for j, (k, step) in enumerate(subplan.items()):
# ----------------------------------------------------
if len(step
) == 1: # there is only one action in this step
# make a reference to action (key+i)
action_ref = 'action_{}_{}_{}'.format(n, i, j)
action_ref = '_'.join(step[0].sig)
# add ref to main list of the plan
plan_json[subsubplan_ref]['list'].append(
action_ref)
# add ref and its description into the action_descriptions_json
action_descriptions_json[action_ref] = action_json(
step[0])
# ----------------------------------------------------
else: # there are more actions in this step
# make a reference to subplan (n)
subsubsubplan_ref = 'subplan_{}_{}_{}'.format(
n, i, j)
plan_json[subsubplan_ref]['list'].append(
subsubsubplan_ref)
plan_json[subsubsubplan_ref] = OrderedDict({
'list': [],
'ordering':
'partial'
})
for l, action in enumerate(step):
# make a reference to action (n+i+j)
action_ref = 'action_{}_{}_{}_{}'.format(
n, i, j, l)
action_ref = '_'.join(action.sig)
# add ref to main list of the plan
plan_json[subsubsubplan_ref]['list'].append(
action_ref)
# add ref and its description into the action_descriptions_json
action_descriptions_json[
action_ref] = action_json(action)
plan_json['actions'] = list(action_descriptions_json.keys())
# make json files for plan and actions descriptions
plan_json_str = json.dumps(plan_json, indent=4)
action_json_str = json.dumps(action_descriptions_json, indent=4)
if policy.problem_file is not None:
problem_file = policy.problem_file
else:
problem_file = policy.domain_file
plan_json_file = '{}.plan.json'.format(os.path.splitext(problem_file)[0])
actions_json_file = '{}.actions.json'.format(
os.path.splitext(problem_file)[0])
with open(plan_json_file, 'w') as outfile:
json.dump(json.loads(plan_json_str, object_pairs_hook=OrderedDict),
outfile,
sort_keys=False,
indent=4)
with open(actions_json_file, 'w') as outfile:
json.dump(json.loads(action_json_str, object_pairs_hook=OrderedDict),
outfile,
sort_keys=False,
indent=4)
return plan_json_file, actions_json_file
def Plan(planners, domain, problem, pwd, verbose):
'''
calls multiple planners to solve a given problem and as soon as
the first planner finds a plan, terminates all other planners and returns
'''
try:
# if only one planner then no need multiprocessing
if len(set(planners)) == 1:
plan = call_planner_sp(planners[0], domain, problem, args_profiles[planners[0]][0], pwd, verbose)
if plan == -1:
if not verbose:
print(color.fg_red('[some error by external planner -- run again with parameter \'-v 2\']'))
sys.exit(0)
return plan
# create a shared Queue to store the output plan
returned_plan = Queue()
# a shared Array to store the failed planners
failed_planners = Array('I', len(planners))
# store the running processes
process_lst = []
# run in multiprocessing
for pidx, planner in enumerate(planners):
# proc = Process(target=call_planner_mp, \
# args=(planner, domain, problem, args_profiles[planner][0], pwd, returned_plan, failed_planners, verbose),
# daemon=True)
proc = Process(target=call_planner_mp, \
args=(planner, domain, problem, args_profiles[planner][0], pwd, returned_plan, failed_planners, verbose))
proc.daemon = True
process_lst.append(proc)
proc.start()
# wait until one process completes and returns a plan
while returned_plan.empty():
# if all processes (planners) failed to solve the problem
if sum(failed_planners) == len(planners):
print(color.fg_red('[error by all external planners: run with \'-v 2\''))
sys.exit(0)
# kill running planners (subprocesses) if they are running
kill_jobs(pwd, planners)
# make sure processes terminate gracefully
while process_lst:
proc = process_lst.pop()
while proc.is_alive():
try:
proc.terminate()
proc.join()
except: pass
# return the plan
return returned_plan.get()
# make sure all processes are terminated when KeyboardInterrupt received
except KeyboardInterrupt:
if len(planners) > 1:
kill_jobs(pwd, planners)
print(color.bg_red('ALL JOBS TERMINATED'))
raise
def call_optic_clp(domain, problem, args='-b -N', pwd='/tmp', verbose=0):
'''
Call an external planner
@domain : path to a given domain
@problem : path to a given problem
@verbose : if True, prints statistics before returning
@return plan : the output plan is a list of actions as tuples,
e.g., [[('move_to_grasp', 'arm1', 'box1', 'base1', 'box2'), ('move_to_grasp', 'arm2', 'box2', 'cap1', 'box1')],
[('vacuum_object', 'arm2', 'cap1', 'box1'), ('vacuum_object', 'arm1', 'base1', 'box2')],
...]
'''
cmd = 'timeout 1800 ./planners/optic-clp {} {} {} & echo $! >> {}/optic-clp-pid.txt'.format(args, domain, problem, pwd)
## call command ##
process = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True, preexec_fn=os.setsid)
(output, err) = process.communicate()
# try:
# (output, err) = process.communicate(timeout=1800)
# except subprocess.TimeoutExpired:
# if verbose == 2: print(color.fg_red('\n-- planning timeout (30m)'))
# return -1
## Wait for cmd to terminate. Get return returncode ##
if process.wait() < 0: return -1
## bytes to string ##
# shell = ''.join(map(chr, output))
shell = to_str(output)
if verbose == 2:
print(color.fg_voilet('\n-- planner stdout'))
print(shell)
if to_str(err):
print(color.fg_voilet('-- planner stderr'))
print(to_str(err))
# Permission denied
if 'Permission denied' in to_str(err):
print(to_str(err))
print(color.fg_voilet('-- run \'chmod +x planners/optic-clp\''))
return -1
## if solution already exists in the problem ##
if "has to terminate" in to_str(err) \
or "error while loading shared libraries" in to_str(err) \
or "Segmentation Fault" in shell:
# print(color.fg_yellow("[planning failed due to some error in the pddl description]"))
# print(color.fg_voilet('\n-- planner stdout'))
# print(shell)
# if to_str(err):
# print(color.fg_voilet('-- planner stderr'))
# print(to_str(err))
# # exit()
return -1
## if problem is unsolvable by EHC remove -b (activate best-first search)
if "Problem unsolvable by EHC, and best-first search has been disabled" in shell:
## calling 'optic-clp' again removing '-b' option ##
cmd = 'timeout 1800 ./planners/optic-clp -N {0} {1}'.format(domain, problem)
process = subprocess.Popen(cmd, stdout=subprocess.PIPE, stderr=subprocess.PIPE, shell=True, preexec_fn=os.setsid)
(output, err) = process.communicate()
# try:
# (output, err) = process.communicate(timeout=1800)
# except subprocess.TimeoutExpired:
# if verbose == 2: print(color.fg_red('\n-- planning timeout (30m)'))
# return -1
# shell = ''.join(map(chr, output))
shell = to_str(output)
## if no solution exists try the next domain ##
if "problem has been deemed unsolvable" in shell or "Problem unsolvable" in shell:
return None
## if no solution exists try the next domain ##
if "file appear to violate part of the PDDL" in to_str(err) or\
"problem has been encountered, and the planner has to terminate" in to_str(err):
if not verbose == 2:
print(color.fg_red('[error by external planner: run with \'-v 2\''))
return None
## if solution already exists in the problem ##
if ("; Plan empty" in shell and "; Plan found" in shell) \
or "The empty plan is optimal" in shell:
return list()
## refine the output screen and build a plan of actions' signatures ##
# extract plan from ';<problem_name>' to 'Time:<value>'
shell = shell[shell.find('; Time'):].strip()
# split shell into a list of actions and ignore ';<problem_name>'
shell = shell.split('\n')[1:]
plan = OrderedDict()
for action in shell:
action = re.split('[, ) (]+', action)[:-1]
plan.setdefault(action[0], []).append(tuple(action[1:]))
# print(list(plan.values()))
return list(plan.values())
def call_planner_mp(planner, domain, problem, args, pwd, returned_plan, failed_planners, verbose):
'''
Call an external deterministic planner in multi processing.
Arguments:
@domain : path to a given domain
@problem : path to a given problem
@planner : the name of the external planner
@args : the default args of the external planner
@pwd : the current working directory
@returned_plan : is a shared queue and stores the returned plan for every planner
@failed_planners : is a shared array and stores the failed result for every planner
@verbose : if True, prints statistics before returning
'''
def return_plan(planner, plan=-1):
if not plan == -1:
returned_plan.put(plan)
else:
try: failed_planners[sum(failed_planners)] = 1
except: pass
return
## FF planner ##
if 'ff' in planner.lower():
plan = call_ff(domain, problem, args, pwd, verbose)
return_plan(planner, plan)
## Madagascar (M) planner ##
elif 'm' in planner.lower():
plan = call_m(domain, problem, args, pwd, verbose)
return_plan(planner, plan)
## PROBE planner ##
elif 'probe' in planner.lower():
plan = call_probe(domain, problem, args, pwd, verbose)
return_plan(planner, plan)
## optic-clp planner ##
elif 'optic-clp' in planner.lower() or 'optic' in planner.lower():
plan = call_optic_clp(domain, problem, args, pwd, verbose)
return_plan(planner, plan)
## optic-clp planner ##
elif 'vhpop' in planner.lower():
plan = call_vhpop(domain, problem, args, pwd, verbose)
return_plan(planner, plan)
## lpg-td planner ##
elif 'lpg-td' in planner.lower():
plan = call_lpg_td(domain, problem, args, pwd, verbose)
return_plan(planner, plan)
## lpg-td planner ##
elif 'lpg' in planner.lower():
plan = call_lpg(domain, problem, args, pwd, verbose)
return_plan(planner, plan)
## lpg-td planner ##
elif 'fd' in planner.lower():
plan = call_fd(domain, problem, args, pwd, verbose)
return_plan(planner, plan)
## no planner ##
else:
print(color.fg_red("\n[There is not yet a function for parsing the outputs of '{0}'!]\n".format(planner)))
return_plan(planner)
return
def action_execution_verification(action_msg):
global args, domain, problem, state, plan, action_ids
#############################
## simulate and execute the plan
for level, step in plan.items():
## if all actions in the plan are visited
if step == 'GOAL':
## check if goal is achieved then terminate planner
if state.is_true(problem.goals):
# goal state is achieved
print(color.fg_voilet('@ GOAL'))
sub_proc.unregister()
rospy.signal_shutdown('finished')
else:
# unfold step into a tuple of actions and outcomes
(actions, outcomes) = step
for action in actions:
## find an action matching the received action
if '_'.join(action.sig) == action_msg.id:
# if action was already visited
if '_'.join(action.sig) in action_ids: return
# add action's id to the visited action_ids
action_ids.append('_'.join(action.sig))
# check if action succeeded
if action_msg.succeed:
## print out succeeded action
print(color.fg_yellow(' + ') + str(action))
# apply action to the state and update the state
state = state.apply(action)
## check if goal is achieved then terminate planner
if state.is_true(problem.goals):
# goal state is achieved
print(color.fg_voilet('@ GOAL'))
sub_proc.unregister()
rospy.signal_shutdown('finished')
else:
## print out failed action
print(color.fg_red(' - ') + str(action))
for monitor in action_msg.monitors:
print(
color.fg_red(' ---- {} {}'.format(
monitor.predicate, monitor.arguments[0])))
## update the state with the action violence and make a re-plane
## convert the predicates frozenset to a list and update the state
## i.e., remove ('collision_free', 'left_arm')
state_predicates = list(state.predicates)
for monitor in action_msg.monitors:
if 'collision' in monitor.predicate:
if ('collision_free', monitor.arguments[0]
) in state_predicates:
state_predicates.remove(
('collision_free',
monitor.arguments[0]))
if not ('collision_detected', monitor.
arguments[0]) in state_predicates:
state_predicates.append(
('collision_detected',
monitor.arguments[0]))
elif 'admittance' in monitor.predicate:
if ('admittance_free', monitor.arguments[0]
) in state_predicates:
state_predicates.remove(
('admittance_free',
monitor.arguments[0]))
if not ('admittance_detected', monitor.
arguments[0]) in state_predicates:
state_predicates.append(
('admittance_detected',
monitor.arguments[0]))
## convert back to frozenset
state.predicates = frozenset(state_predicates)
#############################
## create a new pddl problem file at /tmp/safe-planner
problem_pddl = pddl.pddl(problem, state=state)
## call planner to make an initial policy given the domain, problem and planners
(policy, plan, plan_json_file, actions_json_file) = \
make_plan(domain = domain, \
problem = problem_pddl, \
planners = args.planners, \
agents = args.agents, \
temporal_actions = args.temporal_actions, \
rank=False, \
verbose=args.verbose)
#############################
## call the execution engine giving the initial plan
# plan_json_file actions_json_file
send_json_plan(plan_json_file, actions_json_file)
return
return
def main():
## parse arguments
parser = parse()
args = parser.parse_args()
if args.domain == None:
parser.print_help()
sys.exit()
## make a policy given domain and problem
policy = planner.Planner(args.domain, args.problem, args.planners,
args.safe_planner, args.rank, args.all_outcome,
args.verbose)
## transform the produced policy into a contingency plan
plan = policy.plan()
## print out the plan in a readable form
policy.print_plan(del_effect_inc=True, det_effect_inc=False)
## print out sub-paths in the plan
if args.path:
paths = policy.get_paths(plan)
policy.print_paths(paths=paths, del_effect_inc=True)
# for path in paths:
# policy.print_plan(plan=path, del_effect_inc=True)
## generate graphs of sub-paths too
if args.dot:
for i, path in enumerate(paths):
dot_file = dot_plan.gen_dot_plan(plan=path)
print(
color.fg_yellow('-- path{} dot file: ').format(str(i +
1)) +
dot_file)
# os.system('xdot %s &' % dot_file)
dot_file = dot_plan.gen_dot_plan(plan=paths[0])
# os.system('xdot %s &' % dot_file)
print('')
## generate a graph of the policy as a dot file in graphviz
if args.dot:
plan = policy.plan(tree=True)
dot_file = dot_plan.gen_dot_plan(plan=plan,
del_effect=True,
domain_file=args.domain,
problem_file=args.problem)
print(color.fg_yellow('-- dot file: ') + dot_file + '\n')
# os.system('xdot %s &' % dot_file)
# os.system('dot -T pdf %s > %s.pdf &' % (dot_file, dot_file))
# os.system('evince %s.pdf &' % dot_file)
## transform the policy into a json file
if args.json:
import json_ma_plan
import dot_ma_plan
json_output = json_ma_plan.json_ma_plan(policy, verbose=args.verbose)
if json_output is not None:
plan_json_file, actions_json_file = json_output
print(
color.fg_yellow('-- plan_json_file:') + plan_json_file +
color.fg_red(' [EXPERIMENTAL!]'))
print(
color.fg_yellow('-- actions_json_file:') + actions_json_file +
color.fg_red(' [EXPERIMENTAL!]'))
os.system('cd lua && lua json_multiagent_plan.lua ../%s &' %
plan_json_file)
print(
color.fg_yellow('-- plan_json_dot_file:') +
('%s.dot' % plan_json_file) + color.fg_red(' [EXPERIMENTAL!]'))
# transform the plan into a parallel plan
dot_file, tred_dot_file = dot_ma_plan.parallel_plan(
policy, verbose=args.verbose)
print(color.fg_yellow('-- graphviz file: ') + dot_file)
print(color.fg_yellow('-- transitive reduction: ') + tred_dot_file)
# os.system('xdot %s.dot &' % plan_json_file)
## transform the policy into a json file
if args.json:
import json_plan
plan = policy.plan(tree=False)
json_file, plan_json = json_plan.json_plan(policy)
print(
color.fg_yellow('\n-- json file: ') + json_file +
color.fg_red(' [EXPERIMENTAL!]'))
print(
color.fg_yellow('-- try: ') + 'lua json_plan.lua ' + json_file +
color.fg_red(' [EXPERIMENTAL!]\n'))
if args.store:
stat_file = policy.log_performance(plan)
print(color.fg_yellow('-- planner performance: ') + stat_file)
# print out resulting info
if args.problem is not None:
print('\nPlanning domain: %s' % policy.domain_file)
print('Planning problem: %s' % policy.problem_file)
print('Arguments: %s' % ' '.join(sys.argv[3:]))
else:
print('Planning problem: %s' % policy.domain_file)
print('Arguments: %s' % ' '.join(sys.argv[2:]))
print('Policy length: %i' % len(policy.policy))
print('Plan length: %i' % (len(plan) - 1))
print('Compilation time: %.3f s' % policy.compilation_time)
print('Planning time: %.3f s' % policy.planning_time)
print('Planning iterations (all-outcome): %i' %
policy.alloutcome_planning_call)
print('Total number of replannings (single-outcome): %i' %
policy.singleoutcome_planning_call)
print('Total number of unsolvable states: %i' %
len(policy.unsolvable_states))