def solve_single(env):
use_viewer = (env.GetViewer() is not None)
robot, occupancy = load_env(env)
arm = robot.GetActiveManipulator()
#get_problem = get_problem_1_0
#get_problem = get_problem_1_1
#get_problem = get_problem_1_2
get_problem = get_problem_1_3
#get_problem = get_problem_2_1
#get_problem = get_problem_2_2
#get_problem = get_problem_3_2
#get_problem = get_problem_4
object_meshes = get_object_meshes(MESHES_DIR)
belief, task = get_problem(object_meshes)
print SEPARATOR
print 'Belief:', belief
print 'Task:', task
avaliable_actions = None
#avaliable_actions = ['pick']
stream_problem, belief_from_name = get_ground_problem(
arm,
object_meshes,
belief,
task,
occupancy,
available_actions=avaliable_actions)
print stream_problem
env.Lock()
plan, universe = incremental_planner(stream_problem,
search=get_fast_downward(
'astar', verbose=False),
optimal=False,
frequency=1,
waves=True,
debug=False,
max_time=5.0)
env.Unlock()
print SEPARATOR
length = plan_length(universe, plan)
cost = plan_cost(universe, plan)
plan = convert_plan(plan)
print 'Plan: {}\nLength: {} | Cost: {}'.format(plan, length, cost)
if use_viewer and (plan is not None):
trajectories = process_plan(arm, belief_from_name, plan)
print trajectories
raw_input('Start?')
while True:
with EnvironmentStateSaver(env):
simulate_trajectories(env, trajectories)
response = get_input('Replay?', ('y', 'n'))
if response != 'y':
break
# simulate_single(arm, plan)
if use_viewer:
raw_input('Finish?')
def update_best_plan(universe, search, best_plan, best_cost, max_time):
if max_time is None:
return best_plan, best_cost
t0 = time()
plan = search(universe, max_time, best_cost)
universe.search_time += time() - t0
if plan is not None:
cost = plan_cost(universe, plan) # plan_cost | plan_length
if cost < best_cost:
best_plan, best_cost = plan, cost
if universe.verbose:
print best_cost, convert_plan(best_plan)
return best_plan, best_cost
def fn(belief):
problem = observable_problem(belief, goal)
#print problem
# NOTE - the cost sensitivity starts to factor into this...
# NOTE - max cost prevents extremely large cost actions
search = get_fast_downward('astar', verbose=False, max_cost=max_cost) # dijkstra | astar | wastar1 | wastar2 | wastar3 | eager | lazy
plan, universe = incremental_planner(problem, search=search, frequency=1)
cost = plan_cost(universe, plan)
plan = convert_plan(plan)
print 'Plan:', plan
print 'Cost:', cost
print 'Length:', len(plan)
if plan is None or not plan:
return None
action, params = plan[0]
print 'Action:', action, params
print
return OPERATOR_MAP[action.name](operators, *params)
def policy(estimator):
problem, command_from_action = compile_problem(estimator, task)
print problem
plan, universe = incremental_planner(
problem,
search=get_fast_downward('astar'),
frequency=100,
waves=False,
optimal=True,
debug=False
) # TODO: need to make sure I do all costs (they are always eager)
print 'Plan: {}\nLength: {} | Cost: {}'.format(
convert_plan(plan), plan_length(universe, plan),
plan_cost(universe, plan))
if plan is None:
raise RuntimeError('Unable to find a plan')
if not plan:
return None
return command_from_action(plan[0])
def simple_focused(problem,
search=DEFAULT_SEARCH,
max_time=INF,
max_iterations=INF,
optimal=False,
stream_cost=10,
check_feasible=False,
max_level=0,
greedy=True,
shared=True,
dfs=True,
verbose=False,
debug=False):
universe = Universe(
problem,
use_ground=False,
make_stream_instances=True,
make_action_instances=True) # Need to assign a cost to all
#universe = Universe(problem, use_ground=False, make_stream_instances=True, make_action_instances=not optimal)
initialize_universe(universe)
universe.action_to_function = get_stream_functions(
universe) if stream_cost is not None else {}
####################
for _ in irange(0, max_iterations):
if verbose: print
print_status(universe)
if (time() - universe.start_time) >= max_time:
break
universe.iterations += 1
if debug:
focused_debug(universe)
if check_feasible: # Checks if the problem is feasible with no parameters
plan = solve_real(universe, search, max_time)
if plan is not None:
assert is_real_solution(universe, plan)
return plan, universe
make_streams(universe,
max_level) # TODO - can also do these in iterations/waves
if debug:
abstract_focused_debug(universe)
####################
atom_nodes = determine_reachable(universe)
for instance in universe.action_instances: # TODO - do I need to reset this ever?
if isinstance(instance, ActionInstance):
add_stream_cost(
universe, atom_nodes, instance, stream_cost
) # NOTE - need to add these before the state resets
plan, goals = solve_abstract(universe, atom_nodes, search, max_time)
if verbose:
print 'Cost:', plan_cost(universe, plan), 'Plan:', convert_plan(
plan) # TODO - use an upper bound and continue?
raw_input('Continue?')
if plan is None:
if not universe.temp_blocked:
break
universe.temp_blocked = set()
universe.resets += 1
continue
####################
constants = set_union(set(args) for _, args in plan)
abstract_constants = filter(lambda c: isinstance(c, AbstractConstant),
constants)
new_goals = goals.copy()
new_goals.update(
map(Concrete, abstract_constants)
) # NOTE - could do this for all supporting goals without distinction
order = extract_streams(atom_nodes, new_goals)
#visualize_streams(goals, abstract_constants)
if verbose:
print 'Goals:', len(goals)
print 'Abstract constants:', len(abstract_constants)
print 'Order:', order
####################
bound_plan = produce_bindings(universe, order, plan, greedy, shared,
dfs)
if verbose: print 'Bound plan:', bound_plan
if bound_plan is not None:
assert is_real_solution(universe, bound_plan)
return bound_plan, universe
return None, universe
def simple_focused(problem,
search=DEFAULT_SEARCH,
max_time=INF,
max_iterations=INF,
optimal=False,
stream_cost=10,
check_feasible=False,
max_level=0,
greedy=True,
shared=True,
dfs=True,
verbose=False,
debug=False):
universe = Universe(problem,
use_ground=False,
make_stream_instances=True,
make_action_instances=True)
initialize_universe(universe)
universe.action_to_function = get_stream_functions(
universe) if stream_cost is not None else {}
for _ in irange(0, max_iterations):
if verbose:
print
print_status(universe)
if time() - universe.start_time >= max_time:
break
universe.iterations += 1
if debug:
focused_debug(universe)
if check_feasible:
plan = solve_real(universe, search, max_time)
if plan is not None:
assert is_real_solution(universe, plan)
return plan, universe
make_streams(universe, max_level)
if debug:
abstract_focused_debug(universe)
atom_nodes = determine_reachable(universe)
for instance in universe.action_instances:
if isinstance(instance, ActionInstance):
add_stream_cost(universe, atom_nodes, instance, stream_cost)
plan, goals = solve_abstract(universe, atom_nodes, search, max_time,
stream_cost)
if verbose:
print 'Cost:', plan_cost(universe,
plan), 'Plan:', convert_plan(plan)
raw_input('Continue?')
if plan is None:
if not universe.temp_blocked:
break
universe.temp_blocked = set()
universe.resets += 1
continue
constants = set_union(set(args) for _, args in plan)
abstract_constants = filter(lambda c: isinstance(c, AbstractConstant),
constants)
new_goals = goals.copy()
new_goals.update(map(Concrete, abstract_constants))
order = extract_streams(atom_nodes, new_goals)
if verbose:
print 'Goals:', len(goals)
print 'Abstract constants:', len(abstract_constants)
print 'Order:', order
bound_plan = produce_bindings(universe, order, plan, greedy, shared,
dfs)
if verbose:
print 'Bound plan:', bound_plan
if bound_plan is not None:
assert is_real_solution(universe, bound_plan)
return bound_plan, universe
return None, universe
def incremental_planner(problem,
search=DEFAULT_SEARCH,
max_time=INF,
max_iterations=INF,
max_calls=INF,
waves=True,
frequency=1,
optimal=False,
verbose=False,
debug=False):
"""
Incremental algorithm.
:param problem: :class:`.STRIPStreamProblem`
:param search: python function of the search subroutine
:param max_time: numeric maximum planning time
:param max_iterations: int maximum subroutine calls
:param waves: boolean flag when ``True`` considers each stream evaluation iteration to be the current queue rather than a single stream
:param frequency: numeric number of evaluation iterations given by ``waves`` (``float('inf')`` implies to evaluate all streams per iteration)
:param optimal: boolean flag which saves the best current solution
:param verbose: boolean flag which toggles the print output
:param debug: boolean flag which prints the objects on each iteration
:return: a sequence of :class:`.Action` and tuple of :class:`.Constant` pairs as well as :class:`.Universe`
"""
universe = Universe(problem, use_ground=False, verbose=verbose)
queue = deque()
add_streams(universe, queue)
best_plan, best_cost = None, INF
for _ in irange(0, max_iterations):
print 'Iteration: %d | Time: %s | Calls: %s | Search Time: %s | Solved: %s | Cost: %s' % (
universe.iterations, round(time() - universe.start_time,
3), universe.calls,
round(universe.search_time, 3), best_plan is not None, best_cost)
if debug:
for type in universe.type_to_objects:
print type, len(universe.type_to_objects[type]), map(
get_value, universe.type_to_objects[type])[:10]
raw_input('Continue?\n')
universe.iterations += 1
t0 = time()
plan = search(universe, max_time - (time() - universe.start_time),
best_cost)
universe.search_time += time() - t0
if plan is not None:
cost = plan_cost(universe, plan)
if cost < best_cost:
best_plan, best_cost = plan, cost
if verbose:
print best_cost, convert_plan(best_plan)
if not optimal:
break
if waves and not call_queue_wave(queue, universe, frequency, max_time,
max_calls):
break
if not waves and not call_queue(queue, universe, frequency, max_time,
max_calls):
break
if verbose:
universe.print_statistics()
if best_plan is not None:
print_plan_stats(best_plan, universe)
print 'Cost:', best_cost
print 'Length:', len(best_plan)
return best_plan, universe
def focused_subroutine(universe,
search=DEFAULT_SEARCH,
max_time=INF,
max_iterations=INF,
stream_cost=10,
check_feasible=False,
greedy=True,
dfs=True,
verbose=False,
debug=False,
stream_limit=INF,
sort_order=True,
replace_initial=False,
optimal=False):
assert not optimal or stream_cost is None or stream_cost == 0
# TODO - should really add these in focused_planner
for operator in universe.name_to_action.values() + universe.axioms:
bound_parameters = set(p for a in operator.condition.get_atoms()
for p in a.args
if a.predicate in universe.stream_predicates
| set(universe.derived_predicates))
operator.condition = And(
operator.condition, *[
Concrete(param) for param in operator.parameters
if param not in bound_parameters
])
# NOTE - objects are grounded. This is harmless but annoying
stream_actions = [StreamAction(cs) for cs in universe.problem.cond_streams]
for stream_action in stream_actions:
for obj in stream_action.out_consts:
universe.add_object(obj)
plannable_streams = filter(lambda a: a.cond_stream.plannable,
stream_actions)
#universe.cost_to_function = get_cost_functions(universe)
universe.action_to_function = get_stream_functions(
universe) if stream_cost is not None else {}
solution = []
for _ in irange(0, max_iterations):
if verbose: print
print_status(universe)
if time() - universe.start_time >= max_time:
break
universe.iterations += 1
if debug:
for type in universe.type_to_objects:
print type, len(universe.type_to_objects[type]), \
map(get_value, filter(lambda o: not isinstance(o, AbstractConstant), universe.type_to_objects[type]))[:10]
raw_input('Continue?\n')
if check_feasible: # Checks if the problem is feasible with no parameters
universe.temporary_atoms = set()
t0 = time()
plan = search(universe, max_time - (time() - universe.start_time),
INF)
universe.search_time += time() - t0
if plan is not None:
#if verbose: print_solved(universe, plan)
print_status(universe)
return solution if replace_initial else plan, universe
t0 = time()
process_eager_streams(universe)
if DYNAMIC_INSTANCES:
l_costs, s_costs, static_atoms = dynamic_compute_costs(
plannable_streams, universe)
else:
l_costs, s_costs, static_atoms = get_stream_instances(
plannable_streams, universe)
if verbose:
print 'compute_costs | predicates: %s, streams: %s, time: %s' % (
len(l_costs), len(s_costs), time() - t0)
if stream_cost is not None and search != strips_planner: # TODO - make this specific to FastDownward
add_stream_costs(universe, l_costs, stream_cost)
t0 = time()
plan = search(universe, max_time - (time() - universe.start_time), INF)
universe.search_time += time() - t0
if verbose:
print 'Cost:', plan_cost(universe, plan), 'Plan:', convert_plan(
plan) # TODO - use an upper bound and continue?
if plan is None:
if len(universe.temp_blocked) == 0:
break
universe.temp_blocked = set()
universe.resets += 1
continue
plan_goals = get_target_atoms(universe, plan, static_atoms)
goals = {goal for level in plan_goals for goal in level}
if len(goals) != 0:
relaxed_plan, achievers = extract_plan(goals, l_costs, s_costs)
t0 = time()
if dfs:
groundings = dfs_groundings(plan_goals,
relaxed_plan,
achievers,
sort_order,
stream_limit,
universe,
greedy=greedy)
#groundings = queue_bindings(plan_goals, relaxed_plan, achievers, sort_order, problem)
else:
groundings = single_groundings(plan_goals,
relaxed_plan,
achievers,
sort_order,
stream_limit,
universe,
greedy=greedy)
if verbose: print 'groundings | time:', time() - t0
#for cs in problem.cond_streams:
# print cs, cs.calls, cs.call_time
#raw_input('Continue?')
else:
groundings = {}
#print groundings
#print universe.perm_blocked
#print universe.temp_blocked
#raw_input('awefawfe')
if groundings is not None and (
not optimal
or len(goals) == 0): # Prove optimal once have final costs
plan = [(action, tuple(groundings.get(arg, arg) for arg in args))
for action, args in plan]
if replace_initial:
subplan, success = feasible_subplan(universe, plan)
state = universe.initial_fluents()
for action, args in subplan:
state = action.instantiate(args).apply(
state, universe.type_to_objects)
fluent_atoms = filter(
lambda a: a.predicate in universe.fluent_predicates,
state) # NOTE - might be unnecessary
static_atoms = filter(
lambda a: a.predicate not in universe.fluent_predicates,
universe.initial_atoms)
universe.initial_atoms = set(fluent_atoms) | set(static_atoms)
solution += subplan
if not is_solution(universe, plan):
if verbose:
print plan
#raw_input('Failed plan')
continue
#if verbose: print_solved(universe, plan)
print_status(universe)
return solution if replace_initial else plan, universe
#if verbose: raw_input('Continue?')
#if verbose: universe.print_statistics()
print_status(universe)
return None, universe
def solve_sequence(env):
# TODO: simulate the work for real
use_viewer = (env.GetViewer() is not None)
robot, occupancy = load_env(env)
arm = robot.GetActiveManipulator()
# Two environments doesn't work because at robot can only be in one
get_world = get_world_1
#get_world = get_world_2
#get_world = get_world_3
#get_world = get_world_4
object_meshes = get_object_meshes(MESHES_DIR)
world, task = get_world(object_meshes)
prior = unknown(world)
#prior = fully_observable(world)
print SEPARATOR
print world
print task
# TODO: could try using two display windows
while True:
print SEPARATOR
add_world(robot, object_meshes, world)
#env.UpdatePublishedBodies()
saver = EnvironmentStateSaver(env) # TODO: won't help if removed...
#print observe_env(robot)
belief = belief_from_task(
prior, task) # TODO: don't recreate this on each step...
print belief
#with env:
env.Lock()
stream_problem, belief_from_name = get_ground_problem(
arm, object_meshes, belief, task, occupancy)
print stream_problem
# TODO: most of the overhead comes in translation still
plan, universe = incremental_planner(stream_problem,
search=get_fast_downward(
'ff-astar', verbose=False),
optimal=True,
frequency=1,
waves=True,
debug=False,
max_time=5.0)
env.Unlock()
print SEPARATOR
print 'Plan: {}\nLength: {} | Cost: {}'.format(
convert_plan(plan), plan_length(universe, plan),
plan_cost(universe, plan))
if plan is None:
raise RuntimeError('Unable to find a plan')
if not plan:
break
plan_prefix = get_plan_prefix(convert_plan(plan))
print plan_prefix
trajectories = process_plan(arm, plan_prefix)
print trajectories
if use_viewer:
raw_input('Execute?')
saver.Restore()
simulate_trajectories(env, trajectories)
saver.Restore()
action, args = plan_prefix[0]
# TODO: update the belief entirely from the env
world, prior = update_world(robot, world, prior, action, args)
#world, prior = update_world(robot, world, belief, action, args)
if use_viewer:
raw_input('Finish?')
