def sample_block_poses(blocks, timeout=100):
block_poses = {}
for block, region in random_sequence(blocks):
for _ in irange(0, timeout):
x = sample_region_pose(region, block)
if not any(
are_colliding(block, x, block2, x2)
for block2, x2 in block_poses.iteritems()):
block_poses[block] = x
break
else:
return sample_block_poses(blocks)
return block_poses
def sample_block_poses(blocks, ls, lg, timeout=100):
block_poses = {}
for block in random_sequence(blocks):
for _ in irange(0, timeout):
x = sample_region_pose(block, ls, lg)
if not any(
are_colliding(block, x, block2, x2)
for block2, x2 in block_poses.iteritems()):
block_poses[block] = x
break
#else:
# import pdb;pdb.set_trace()
# return sample_block_poses(blocks)
return block_poses
def sample_block_poses(
blocks,
timeout=100
): # NOTE - might need to reset the full assignment if infeasible
block_poses = {}
for block, region in random_sequence(blocks):
for _ in irange(0, timeout):
x = sample_region_pose(region, block)
if not any(
are_colliding(block, x, block2, x2)
for block2, x2 in block_poses.iteritems()):
block_poses[block] = x
break
else:
return sample_block_poses(blocks)
return block_poses
def sample_block_poses(
blocks,
env,
obstacles,
timeout=100
): # NOTE - might need to reset the full assignment if infeasible
block_poses = []
placed = list(obstacles)
for block, region in random_sequence(blocks):
for _ in irange(0, timeout):
p = sample_contained_poly_rot(env.poly, block.poly)
poly = transform(block.poly, p)
if not any(collides(poly, body) for body in placed):
block_poses.append((block, p))
placed.append(poly)
break
else:
return sample_block_poses(blocks, env, obstacles)
return block_poses
def sample_placement(self, body, max_attempts=10):
with body.CreateKinBodyStateSaver():
for _ in irange(0, max_attempts):
quat = quat_from_axis_angle(0, 0, uniform(0, 2 * math.pi))
set_quat(body, quat)
aabb = body.ComputeAABB()
low = self.xy_min + aabb.extents()[:2]
high = self.xy_max - aabb.extents()[:2]
if np.any(low >= high):
continue
xy = (high - low) * np.random.rand(*low.shape) + low
z = self.z + aabb.extents()[2]
point = np.concatenate([xy, [z]
]) + (get_point(body) - aabb.pos())
pose = pose_from_quat_point(quat, point)
#set_pose(body, pose)
return pose
# TODO - collision here?
return None
def replan_hierarchy(problem,
selector=first_selector,
max_time=INF,
max_iterations=INF,
first_only=False,
execute=True,
verbose=False):
abs_goal = problem.goal_literals
goal_level = 1
if not isinstance(problem.goal_literals, AbsCondition):
abs_goal = AbsCondition([problem.goal_literals])
universe = Universe(problem,
use_ground=True,
make_stream_instances=False,
hierarchical=True) # Previously removed for hierarchy
solution = []
for i in irange(0, max_iterations):
#if i != 0: print separator(10)
print header('Replan iteration: %s' % i)
#if verbose: print universe.initial_fluents() # TODO - this includes ground and the refinement stuff...
if verbose: print solution
plan, _ = focused_subroutine(
universe, max_time=INF,
verbose=False) # Maybe just return this an update it?
# TODO - arguments for this?
if verbose: print convert_plan(plan)
#raw_input('Continue?')
if plan is None:
break
n = 1 if first_only else len(plan)
if not any(
isinstance(action, Refinable)
for action, _ in plan[:n]): # NOTE - don't need this anymore
if goal_level != len(abs_goal.conditions):
goal_level += 1
universe.goal_formula = And(*abs_goal.conditions[:goal_level])
universe.add_formula(universe.goal_formula)
continue
static_atoms = filter(
lambda a: a.predicate not in universe.fluent_predicates,
universe.initial_atoms)
universe.initial_atoms = set(universe.problem.initial_atoms) | set(
static_atoms) # Resets the initial state
return solution + plan, universe
selector(plan, universe)
if execute: # TODO - no incentive for actions to be in an order which allows executable first
subplan = []
for action, args in plan:
if isinstance(action, Refinable): break
subplan.append((action, args))
if subplan:
# TODO - when resetting the state, save the information on the later 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
return None, universe
def simultaneous_strips(problem,
max_time=INF,
max_iterations=INF,
frequency=100,
verbose=False,
debug=False,
optimal=False):
# TODO - prune any unnecessary cond_streams
universe = Universe(problem,
use_ground=False,
make_action_instances=True,
verbose=verbose)
stream_queue = deque()
add_streams(universe, stream_queue)
universe.root = Vertex(universe.initial_fluents())
universe.vertices = {universe.root}
universe.goal_vertices = set()
universe.vertex_queue = deque()
universe.state_map = defaultdict(list)
universe.action_map = defaultdict(list)
for vertex in universe.vertices:
for a in universe.name_to_action.values():
for key in get_vertex_keys(vertex, a, universe):
universe.state_map[key].append(vertex)
for _ in irange(0, max_iterations):
print_helper(universe)
if debug: debug_helper(universe)
universe.iterations += 1
stream_atoms = universe.initial_stream_atoms(
) # TODO - could always augment the state with the static atoms or just ignore them
new_axioms = filter(lambda op: isinstance(op.lifted, Axiom),
universe.new_instances)
new_actions = filter(lambda op: isinstance(op.lifted, Action),
universe.new_instances)
axioms = filter(lambda op: isinstance(op.lifted, Axiom),
universe.action_instances)
actions = filter(lambda op: isinstance(op.lifted, Action),
universe.action_instances)
#if verbose:
print 'New Actions: %d | New Axioms: %s | Total Actions: %s | Total Axioms: %s\n' % (
len(new_actions), len(new_axioms), len(actions), len(axioms))
# TODO - recombine with simultaneous
for instance in universe.new_instances:
universe.action_map[get_operator_key(instance,
universe)].append(instance)
t0 = time()
success = False
for instance in new_actions:
for vertex in get_vertices(instance, universe):
#print action.lifted.name, map(get_value, action.args), vertex
#raw_input('Continue?')
if apply_actions(vertex, [instance], stream_atoms, universe,
optimal):
success = True
break
if success: break
if success: break
for vertex in list(
universe.vertices): # Process new_axioms and actions
if apply_axioms(vertex, new_axioms, stream_atoms, universe) and \
apply_actions(vertex, actions, stream_atoms, universe, optimal):
break
#elif apply_actions(vertex, new_actions, stream_atoms, universe, optimal):
# break
universe.new_instances = [] # NOTE - didn't have this before
while universe.vertex_queue: # Process new_vertices
vertex = universe.vertex_queue.popleft()
apply_axioms(vertex, axioms, stream_atoms, universe)
#if apply_actions(vertex, actions, stream_atoms, universe, optimal):
if apply_actions_strips(vertex, stream_atoms, universe, optimal):
#if apply_actions(vertex, get_actions(vertex, universe), stream_atoms, universe, optimal):
break
universe.search_time += time() - t0
if (not optimal and universe.goal_vertices) or not call_queue(
stream_queue, universe, frequency, max_time):
break # TODO - call queue waves
dijkstra(universe)
best_v, best_plan = None, None
if universe.goal_vertices:
best_v = argmin(lambda v: v.cost, universe.goal_vertices)
best_plan = get_plan(best_v)
if verbose:
universe.print_statistics()
if best_plan is not None:
print_plan_stats(best_plan, universe)
print 'Cost:', best_v.cost
return best_plan, universe
def simultaneous(problem,
max_time=INF,
max_iterations=INF,
frequency=100,
verbose=False,
debug=False,
optimal=False):
# TODO - prune any unnecessary cond_streams
universe = Universe(problem,
use_ground=False,
make_action_instances=True,
verbose=verbose)
stream_queue = deque()
add_streams(universe, stream_queue)
universe.root = Vertex(universe.initial_fluents())
universe.vertices = {universe.root}
universe.goal_vertices = set()
universe.vertex_queue = deque()
for _ in irange(0, max_iterations):
print_helper(universe)
if debug: debug_helper(universe)
universe.iterations += 1
stream_atoms = universe.initial_stream_atoms(
) # TODO - could always augment the state with the static atoms or just ignore them
new_axioms = filter(lambda op: isinstance(op.lifted, Axiom),
universe.new_instances)
new_actions = filter(lambda op: isinstance(op.lifted, Action),
universe.new_instances)
axioms = filter(lambda op: isinstance(op.lifted, Axiom),
universe.action_instances)
actions = filter(lambda op: isinstance(op.lifted, Action),
universe.action_instances)
#if verbose:
print 'New Actions: %d | New Axioms: %s | Total Actions: %s | Total Axioms: %s\n' % (
len(new_actions), len(new_axioms), len(actions), len(axioms))
t0 = time()
for vertex in list(
universe.vertices): # Process new_axioms and actions
if apply_axioms(vertex, new_axioms, stream_atoms, universe) and \
apply_actions(vertex, actions, stream_atoms, universe, optimal):
break
elif apply_actions(vertex, new_actions, stream_atoms, universe,
optimal):
break
universe.new_instances = [] # NOTE - didn't have this before
while universe.vertex_queue: # Process new_vertices
vertex = universe.vertex_queue.popleft()
apply_axioms(vertex, axioms, stream_atoms, universe)
if apply_actions(vertex, actions, stream_atoms, universe, optimal):
break
universe.search_time += time() - t0
if (not optimal and universe.goal_vertices) or not call_queue(
stream_queue, universe, frequency, max_time):
break # TODO - call queue waves
dijkstra(universe)
best_v, best_plan = None, None
if universe.goal_vertices:
best_v = argmin(lambda v: v.cost, universe.goal_vertices)
best_plan = get_plan(best_v)
if verbose:
universe.print_statistics()
if best_plan is not None:
print_plan_stats(best_plan, universe)
print 'Cost:', best_v.cost
return best_plan, universe
def compile_problem(tamp_problem):
O = Param(OBJECT)
O1, O2 = Param(OBJECT), Param(OBJECT)
L = Param(LOCATION)
L_s, L_g = Param(LOCATION), Param(LOCATION) # generic location
Stove_l_s, Stove_l_g = Param(STOVE_L_S), Param(
STOVE_L_G) # locations for stove and sink
Sink_l_s, Sink_l_g = Param(SINK_L_S), Param(SINK_L_G)
actions = [
Action(name='wash',
parameters=[O],
condition=And(InSink(O)),
effect=And(Clean(O))),
Action(name='cook',
parameters=[O],
condition=And(InStove(O), Clean(O)),
effect=And(Cooked(O))),
Action(name='pickplace',
parameters=[O, L_s, L_g],
condition=And(EmptySweptVolume(O, L_s, L_g), AtPose(O, L_s)),
effect=And(AtPose(O, L_g),
Not(AtPose(O, L_s)))) # You should delete!
]
axioms = [
# For all objects in the world, either object is O1 or if not, then it is not in the region
Axiom(effect=EmptySweptVolume(O,L_s,L_g),condition=ForAll([O2],\
Or(Equal(O,O2),\
Exists([L],(And(AtPose(O2,L),OutsideRegion(O,O2,L,L_s,L_g))))))),
Axiom(effect=InStove(O),condition=Exists([L,L_s,L_g],And(AtPose(O,L), Contained(O,L,L_s,L_g), IsStove(L_s,L_g)))),
Axiom(effect=InSink(O),condition=Exists([L,L_s,L_g],And(AtPose(O,L), Contained(O,L,L_s,L_g), IsSink(L_s,L_g)))),
]
cond_streams = [
EasyGenStream(inputs=[O,L_s,L_g], outputs=[L], conditions=[IsSmaller(L_s,L_g)], effects=[Contained(O,L,L_s,L_g)],\
generator=lambda b, ls, lg: (sample_region_pose(b, ls, lg ) for _ in irange(0, INF))),
EasyTestStream(inputs=[L_s,L_g],conditions=[],effects=[IsSmaller(L_s,L_g)],test=is_smaller,eager=EAGER_TESTS),
EasyTestStream(inputs=[L_s,L_g,O,L],conditions=[IsSink(L_s,L_g)],effects=[Contained(O,L,L_s,L_g)],test=in_region,eager=EAGER_TESTS),
EasyTestStream(inputs=[L_s,L_g,O,L],conditions=[IsStove(L_s,L_g)],effects=[Contained(O,L,L_s,L_g)],test=in_region,eager=EAGER_TESTS),
EasyTestStream(inputs=[O,O2,L,L_s,L_g],conditions=[],effects=[OutsideRegion(O,O2,L,L_s,L_g)],test=not_in_region,eager=EAGER_TESTS),
# OutsideRegion tests if the block at L is outside of the region (Ls,Lg)
]
####################
# instantiate the environment region?
constants = [
STOVE_L_S(tamp_problem.stove_region_s),
STOVE_L_G(tamp_problem.stove_region_g),
SINK_L_S(tamp_problem.sink_region_s),
SINK_L_G(tamp_problem.sink_region_g),
]
# define initial state using initial poses of objects
initial_atoms = [
AtPose(block, pose)
for block, pose in tamp_problem.initial_poses.iteritems()
] + [IsSink(tamp_problem.sink_region_s, tamp_problem.sink_region_g)] + [
IsStove(tamp_problem.stove_region_s, tamp_problem.stove_region_g)
]
# static predicate but on steroid
# define goal state as target object to be cooked - can you infer that target object is on Stove
goal_literals = []
# goal_literals.append( AtPose(tamp_problem.blue_obj,1) ) #NOTE: This works; so planner knows how to clear the area
# goal_literals.append( AtPose(tamp_problem.target_obj,3.0) ) #NOTE: But doing this does not work
goal_literals.append(Cooked(tamp_problem.target_obj))
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, constants), static_pred_names
def config_generator():
for n in irange(*CONFIG_RANGE):
yield Config(n),
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 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 compile_problem(tamp_problem):
"""
Constructs a STRIPStream problem for the continuous TMP problem.
:param tamp_problem: a :class:`.TMPProblem`
:return: a :class:`.STRIPStreamProblem`
"""
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
Q1, Q2 = Param(CONF), Param(CONF)
R = Param(REGION)
actions = [
Action(name='pick',
parameters=[B1, P1, Q1],
condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1),
LegalKin(P1, Q1)),
effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),
Action(name='place',
parameters=[B1, P1, Q1],
condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1),
ForAll([B2], Or(Equal(B1, B2), Safe(B2, B1,
P1)))),
effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move',
parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))),
Action(name='clean',
parameters=[B1, R],
condition=And(InRegion(B1, R), IsSink(R)),
effect=And(Cleaned(B1))),
Action(name='cook',
parameters=[B1, R],
condition=And(InRegion(B1, R), IsStove(R)),
effect=And(Cooked(B1))),
]
axioms = [
Axiom(effect=InRegion(B1, R),
condition=Exists([P1], And(AtPose(B1, P1), Contained(B1, P1,
R)))),
Axiom(effect=Safe(B2, B1, P1),
condition=Exists([P2],
And(AtPose(B2, P2),
CollisionFree(B1, P1, B2, P2)))),
]
cond_streams = [
EasyGenStream(inputs=[R, B1],
outputs=[P1],
conditions=[CanPlace(B1, R)],
effects=[Contained(B1, P1, R)],
generator=lambda r, b:
(sample_region_pose(r, b) for _ in irange(0, INF))),
EasyGenStream(inputs=[P1],
outputs=[Q1],
conditions=[],
effects=[LegalKin(P1, Q1)],
generator=lambda p: iter([inverse_kinematics(p)])),
EasyTestStream(inputs=[R, B1, P1],
conditions=[],
effects=[Contained(B1, P1, R)],
test=in_region,
eager=EAGER_TESTS,
plannable=False),
EasyTestStream(inputs=[B1, P1, B2, P2],
conditions=[],
effects=[CollisionFree(B1, P1, B2, P2)],
test=lambda *args: not are_colliding(*args),
eager=EAGER_TESTS),
]
constants = [
REGION(tamp_problem.env_region),
]
initial_atoms = [
AtConf(tamp_problem.initial_config),
] + [
AtPose(block, pose)
for block, pose in tamp_problem.initial_poses.iteritems()
] + [
CanPlace(block, tamp_problem.env_region)
for block in tamp_problem.initial_poses
]
if tamp_problem.initial_holding is None:
initial_atoms.append(HandEmpty())
else:
initial_atoms.append(Holding(tamp_problem.initial_holding))
goal_literals = []
if tamp_problem.goal_config is not None:
goal_literals.append(AtConf(tamp_problem.goal_config))
if tamp_problem.goal_holding is False:
goal_literals.append(HandEmpty())
elif tamp_problem.goal_holding is not None:
goal_literals.append(Holding(tamp_problem.goal_holding))
for block, goal in tamp_problem.goal_poses:
goal_literals.append(AtPose(block, goal))
for block, goal in tamp_problem.goal_regions:
initial_atoms.append(CanPlace(block, goal))
goal_literals.append(InRegion(block, goal))
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, constants)
def compile_problem(tamp_problem):
"""
Constructs a STRIPStream problem for the countable TMP problem.
:param tamp_problem: a :class:`.TMPProblem`
:return: a :class:`.STRIPStreamProblem`
"""
# NOTE - the simple focused algorithm gives "Could not find instantiation for PNE!" when this is moved outside
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
Q1, Q2 = Param(CONF), Param(CONF)
actions = [
Action(name='pick',
parameters=[B1, P1, Q1],
condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1),
LegalKin(P1, Q1)),
effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),
Action(
name='place',
parameters=[B1, P1, Q1],
condition=And(
Holding(B1),
AtConf(Q1),
LegalKin(P1, Q1),
#ForAll([B2], Or(Equal(B1, B2), Safe(B2, B1, P1)))),
ForAll([B2], Or(Equal(B1, B2), Safe(B2, P1)))),
#*[Or(Equal(B1, BLOCK(b2)), Safe(b2, P1)) for b2 in tamp_problem.initial_poses]),
effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move',
parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))),
]
axioms = [
#Axiom(effect=Safe(B2, B1, P1), condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(B1, P1, B2, P2)))),
Axiom(effect=Safe(B2, P1),
condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(P1,
P2)))),
]
# NOTE - this needs to be inside the method so you make new streams each time
cond_streams = [
#EasyGenStream(inputs=[P2], outputs=[P1], conditions=[], effects=[],
# generator=lambda a: irange(0, NUM_POSES)),
EasyGenStream(inputs=[],
outputs=[P1],
conditions=[],
effects=[],
generator=lambda: irange(0, NUM_POSES)),
EasyGenStream(inputs=[P1],
outputs=[Q1],
conditions=[],
effects=[LegalKin(P1, Q1)],
generator=lambda p: iter([p])),
#EasyTestStream(inputs=[B1, P1, B2, P2], conditions=[], effects=[CollisionFree(B1, P1, B2, P2)],
# test=lambda b1, p1, b2, p2: p1 != p2, eager=EAGER_TESTS),
EasyTestStream(inputs=[P1, P2],
conditions=[],
effects=[CollisionFree(P1, P2)],
test=lambda p1, p2: p1 != p2,
eager=EAGER_TESTS),
]
constants = []
initial_atoms = [
AtConf(tamp_problem.initial_config),
] + [
AtPose(block, pose)
for block, pose in tamp_problem.initial_poses.iteritems()
]
if tamp_problem.initial_holding is False:
initial_atoms.append(HandEmpty())
else:
initial_atoms.append(Holding(tamp_problem.initial_holding, BLOCK))
goal_literals = []
if tamp_problem.goal_holding is False:
goal_literals.append(HandEmpty())
elif tamp_problem.goal_holding is not None:
goal_literals.append(Holding(tamp_problem.goal_holding))
for block, goal in tamp_problem.goal_poses.iteritems():
goal_literals.append(AtPose(block, goal))
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, constants)
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 solve_incrementally():
O = Param(OBJECT)
O1, O2 = Param(OBJECT), Param(OBJECT)
L = Param(LOCATION)
L_s, L_g = Param(LOCATION), Param(LOCATION) # generic location
Stove_l_s, Stove_l_g = Param(STOVE_L_S), Param(
STOVE_L_G) # locations for stove and sink
Sink_l_s, Sink_l_g = Param(SINK_L_S), Param(SINK_L_G)
actions = [
Action(name='wash',
parameters=[O],
condition=And(InSink(O)),
effect=And(Clean(O))),
Action(name='cook',
parameters=[O],
condition=And(InStove(O), Clean(O)),
effect=And(Cooked(O))),
Action(name='pickplace',
parameters=[O, L_s, L_g],
condition=And(EmptySweptVolume(O, L_s, L_g), AtPose(O, L_s)),
effect=And(AtPose(O, L_g),
Not(AtPose(O, L_s)))) # You should delete!
]
axioms = [
# For all objects in the world, either object is O1 or if not, then it is not in the region
Axiom(effect=EmptySweptVolume(O,L_s,L_g),condition=ForAll([O2],\
Or(Equal(O,O2),\
Exists([L],(And(AtPose(O2,L),OutsideRegion(O,O2,L,L_s,L_g))))))),
# Object is in the stove if it is at pose L for which Ls and Lg define stove
Axiom(effect=InStove(O),condition=Exists([L,L_s,L_g],And(AtPose(O,L), Contained(O,L,L_s,L_g), IsStove(L_s,L_g)))),
Axiom(effect=InSink(O),condition=Exists([L,L_s,L_g],And(AtPose(O,L), Contained(O,L,L_s,L_g), IsSink(L_s,L_g)))),
]
cond_streams = [
EasyGenStream(inputs=[O,L_s,L_g], outputs=[L], conditions=[IsSmaller(L_s,L_g)], effects=[Contained(O,L,L_s,L_g)],\
generator=lambda b, ls, lg: (sample_region_pose(b, ls, lg ) for _ in irange(0, INF))),
EasyTestStream(inputs=[L_s,L_g],conditions=[],effects=[IsSmaller(L_s,L_g)],test=is_smaller,eager=EAGER_TESTS),
# Generate static predicates that object is contained in sink for which Ls and Lg define the sink. If L was not continuous value,
# then we would define this in the intial condition and would not be changed by any of the actions (hence static predicate)
EasyTestStream(inputs=[L_s,L_g,O,L],conditions=[IsSink(L_s,L_g)],effects=[Contained(O,L,L_s,L_g)],test=in_region,eager=EAGER_TESTS),
EasyTestStream(inputs=[L_s,L_g,O,L],conditions=[IsStove(L_s,L_g)],effects=[Contained(O,L,L_s,L_g)],test=in_region,eager=EAGER_TESTS),
# OutsideRegion tests if O2 is is outside of the region (Ls,Lg)
EasyTestStream(inputs=[O,O2,L,L_s,L_g],conditions=[],effects=[OutsideRegion(O,O2,L,L_s,L_g)],test=not_in_region,eager=EAGER_TESTS),
]
####################
tamp_problem = sample_one_d_kitchen_problem()
# instantiate the environment region?
constants = [
STOVE_L_S(tamp_problem.stove_region_s),
STOVE_L_G(tamp_problem.stove_region_g),
SINK_L_S(tamp_problem.sink_region_s),
SINK_L_G(tamp_problem.sink_region_g),
]
# define initial state using initial poses of objects
initial_atoms = [
AtPose(block, pose)
for block, pose in tamp_problem.initial_poses.iteritems()
] + [IsSink(tamp_problem.sink_region_s, tamp_problem.sink_region_g)] + [
IsStove(tamp_problem.stove_region_s, tamp_problem.stove_region_g)
] # initial_atoms = static predicates, but on steroid
goal_literals = []
subgoal_list = [ InSink(tamp_problem.target_obj), \
InStove(tamp_problem.target_obj), Cooked(tamp_problem.target_obj) ]
for i in range(len(subgoal_list)):
goal_literals = [subgoal_list[0]]
stream_problem = STRIPStreamProblem(initial_atoms, goal_literals, \
actions+axioms, cond_streams, constants)
search = get_fast_downward('eager')
plan, universe = incremental_planner(stream_problem, search=search,\
frequency=1, verbose=True, max_time=200)
plan = convert_plan(plan)
# move the object to the new locations; todo: add new predicates
if len(plan) > 0: # if no action needed, keep last initial atoms
initial_atoms = []
for action in plan:
action_name = action[0].name
action_args = action[1]
if action_name == 'pickplace':
O = action_args[0].name
pick_l = action_args[1]
place_l = action_args[2]
block = [b for b in tamp_problem.blocks if b.name == O][0]
initial_atoms += [AtPose(block, place_l)]
if len(initial_atoms) == 1:
block = [b for b in tamp_problem.blocks if b.name != O][0]
initial_atoms += [AtPose(block, tamp_problem.initial_poses[block])]
initial_atoms += [
IsSink(tamp_problem.sink_region_s, tamp_problem.sink_region_g)
]
initial_atoms += [
IsStove(tamp_problem.stove_region_s, tamp_problem.stove_region_g)
]
def compile_problem(tamp_problem):
"""
Constructs a STRIPStream problem for the countable TMP problem.
:param tamp_problem: a :class:`.TMPProblem`
:return: a :class:`.STRIPStreamProblem`
"""
B1, B2 = Param(BLOCK), Param(BLOCK)
P1, P2 = Param(POSE), Param(POSE)
Q1, Q2 = Param(CONF), Param(CONF)
actions = [
Action(name='pick',
parameters=[B1, P1, Q1],
condition=And(AtPose(B1, P1), HandEmpty(), AtConf(Q1),
LegalKin(P1, Q1)),
effect=And(Holding(B1), Not(AtPose(B1, P1)), Not(HandEmpty()))),
Action(name='place',
parameters=[B1, P1, Q1],
condition=And(Holding(B1), AtConf(Q1), LegalKin(P1, Q1),
ForAll([B2], Or(Equal(B1, B2), Safe(B2, P1)))),
effect=And(AtPose(B1, P1), HandEmpty(), Not(Holding(B1)))),
Action(name='move',
parameters=[Q1, Q2],
condition=AtConf(Q1),
effect=And(AtConf(Q2), Not(AtConf(Q1)))),
]
axioms = [
Axiom(effect=Safe(B2, P1),
condition=Exists([P2], And(AtPose(B2, P2), CollisionFree(P1,
P2)))),
]
cond_streams = [
EasyGenStream(inputs=[],
outputs=[P1],
conditions=[],
effects=[],
generator=lambda: irange(0, NUM_POSES)),
EasyGenStream(inputs=[P1],
outputs=[Q1],
conditions=[],
effects=[LegalKin(P1, Q1)],
generator=lambda p: iter([p])),
EasyTestStream(inputs=[P1, P2],
conditions=[],
effects=[CollisionFree(P1, P2)],
test=lambda p1, p2: p1 != p2,
eager=EAGER_TESTS),
]
constants = []
initial_atoms = [
AtConf(tamp_problem.initial_config),
] + [
AtPose(block, pose)
for block, pose in tamp_problem.initial_poses.iteritems()
]
if tamp_problem.initial_holding is False:
initial_atoms.append(HandEmpty())
else:
initial_atoms.append(Holding(tamp_problem.initial_holding, BLOCK))
goal_literals = []
if tamp_problem.goal_holding is False:
goal_literals.append(HandEmpty())
elif tamp_problem.goal_holding is not None:
goal_literals.append(Holding(tamp_problem.goal_holding))
for block, goal in tamp_problem.goal_poses.iteritems():
goal_literals.append(AtPose(block, goal))
return STRIPStreamProblem(initial_atoms, goal_literals, actions + axioms,
cond_streams, constants)
def pose_generator():
for n in irange(*POSE_RANGE):
yield Pose(n),