def observable_problem(belief, goal, costs=True): # NOTE - costs is really important here
#objects = belief.objLoc.keys()
locations = belief.occupancies.keys()
states = [dirty, clean, dry, wet]
initial_atoms = []
occupied = set()
for obj in belief.objLoc.keys():
dist = belief.objLocDist(obj)
loc, p = dist.computeMPE() # TODO - concentration inequalities for Gaussian
if p >= LOC_CONFIDENCE:
initial_atoms.append(At(obj, loc))
occupied.add(loc)
else:
initial_atoms.append(UnsureLoc(obj))
for loc in locations:
p = dist.prob(loc)
cost = int(COST_SCALE*1./max(p, MIN_P)) if costs else COST_SCALE*1 # TODO - set to infinity if too large (equivalent to blocking)
initial_atoms.append(Initialize(FindCost(OBJ(obj), LOC(loc)), cost))
#if p < MIN_CONFIDENCE:
# initial_atoms.append(NotAtLoc(obj, loc))
for obj in belief.objState.keys():
dist = belief.objState[obj]
state, p = dist.computeMPE()
if p >= STATE_CONFIDENCE:
initial_atoms.append(HasState(obj, state))
else:
initial_atoms.append(UnsureState(obj))
for state in states:
p = dist.prob(state)
cost = int(COST_SCALE*1./max(p, MIN_P)) if costs else COST_SCALE*1
initial_atoms.append(Initialize(InspectStateCost(OBJ(obj), STATE(state)), cost))
#if p < MIN_CONFIDENCE:
# initial_atoms.append(NotHasState(obj, loc))
for loc in belief.occupancies.keys():
if loc == 'washer':
initial_atoms.append(IsWasher(loc))
elif loc == 'painter':
initial_atoms.append(IsPainter(loc))
elif loc == 'dryer':
initial_atoms.append(IsDryer(loc))
goal_literals = []
for fluent in goal.fluents:
if isinstance(fluent, Bd):
literal, arg, prob = fluent.args
if isinstance(literal, ObjLoc):
goal_literals.append(At(literal.args[0], literal.value))
elif isinstance(literal, ObjState):
goal_literals.append(HasState(literal.args[0], arg))
elif isinstance(literal, toyTest.Clear):
goal_literals.append(Clear(literal.args[0]))
else:
raise NotImplementedError(literal)
else:
raise NotImplementedError(fluent)
return STRIPStreamProblem(initial_atoms, goal_literals, actions, [], [])
def add_stream_cost(universe, atom_nodes, action, stream_cost):
if action.lifted not in universe.action_to_function:
for cost in get_cost_atoms(action):
if any(isinstance(arg, AbstractConstant) for arg in cost.args):
initialize = Initialize(cost, 1)
#universe.temporary_atoms.add(initialize) # TODO - more generically use lower-bound on cost?
universe.add_temp_initialize(initialize)
#param_map = dict(zip(action.lifted.parameters, action.args))
# for cost in get_cost_atoms(action.lifted):
# cost_map = {param: param_map[param] for param in cost.args}
# if any(isinstance(arg, AbstractConstant) for arg in cost_map.values()):
# universe.temporary_atoms.add(Initialize(function, int(ceil(min_cost)))) # NOTE - can only have one of these here
return
min_cost = INF
#for literals in action.condition.get_literals(): # TODO - avoid recomputing this again (smart_instantiate_operators)
for literals in [action.condition.get_atoms()]:
cost = action.lifted.cost if action.lifted.cost is not None else 0
for atom in literals:
if isinstance(
atom,
Atom) and atom.predicate in universe.stream_predicates:
cost += stream_cost * atom_nodes[atom][
0] if atom in atom_nodes else INF # NOTE - could also do max
min_cost = min(cost, min_cost)
if min_cost < INF:
function = universe.action_to_function[action.lifted](*action.args)
initialize = Initialize(function, int(ceil(min_cost)))
#universe.temporary_atoms.add(initialize) # NOTE - can only have one of these here
universe.add_temp_initialize(
initialize) # NOTE - can only have one of these here
def get_values(self, **kwargs):
self.enumerated = True
cost = self.cond_stream.function(*unwrap_inputs(self.inputs))
assert isinstance(cost, numbers.Number) and (0 <= cost)
cost = int(self.cond_stream.scale * cost)
return map(lambda f: Initialize(f, cost),
self.cond_stream.instantiate_effects(self.inputs, []))
def get_values(self, **kwargs):
self.enumerated = True
prior, loc = map(get_value, self.inputs)
#_, prob = look_update(loc, prior, True)
prob = prior.prob(loc) #*(1 - glob.failProbs['Look'])
cost = int(COST_SCALE * geom_cost(1, prob)) if not UNIT else 1
return [Initialize(LookCost(*self.inputs), cost)]
def get_values(self, **kwargs):
self.enumerated = True
loc, prior = map(get_value, self.inputs)
#loc1, prior, loc2 = map(get_value, self.inputs)
#post, prob = move_update(loc1, prior, loc2)
prob = prior.prob(loc) # *(1 - glob.failProbs['Move'])
cost = int(COST_SCALE * geom_cost(1, prob)) if not UNIT else 1
return [Initialize(MoveCost(*self.inputs[:2]), cost)]
def get_values(self, **kwargs):
self.enumerated = True
loc1, prior, loc2 = map(get_value, self.inputs)
post, prob = move_update(loc1, prior, loc2)
cost = int(COST_SCALE * geom_cost(1, prob)) if not UNIT else 1
return [
IsMoveUpdate(loc1, prior, loc2, post),
Initialize(MoveCost(*self.inputs[:2]), cost)
] # TODO - make this a test stream
def get_values(self, **kwargs):
self.enumerated = True
prior, loc = map(get_value, self.inputs)
_, prob = look_update(loc, prior, True)
perfect = DDist({loc: 1.})
cost = int(COST_SCALE * geom_cost(1, prob)) if not UNIT else 1
return [
IsLookUpdate(prior, loc, perfect),
Initialize(LookCost(*self.inputs), cost)
] # Could include satisfies here...
def get_values(self, **kwargs):
self.enumerated = True
prior, loc = map(get_value, self.inputs)
post, prob = look_update(loc, prior, True)
if prior.prob(loc) >= .95:
return [] # Set a maximum probability update
#post = BELIEF(post)
cost = int(COST_SCALE * geom_cost(1, prob))
return [
IsLookUpdate(prior, loc, post),
Initialize(LookCost(*self.inputs), cost)
]
def get_values(self, **kwargs):
self.enumerated = True
q1, q2 = self.inputs
d = self.cond_stream.distance(q1.value.value, q2.value.value)
if d > self.max_distance:
return []
for q in self.cond_stream.extend(q1.value.value, q2.value.value):
if self.cond_stream.collision(q):
return []
self.handles.append(
draw_edge(self.cond_stream.oracle.env,
q1.value.value,
q2.value.value,
color=(1, 0, 0, .5)))
cost = int(self.scale * d + 1)
#return [IsCollisionFree(q1, q2), Initialize(MoveCost(q1, q2), )]
return [
IsCollisionFree(q1, q2),
IsCollisionFree(q2, q1),
Initialize(MoveCost(q1, q2), cost),
Initialize(MoveCost(q2, q1), cost)
]
def get_values(self, **kwargs):
self.enumerated = True # NOTE - could return one action that does the full look
prior, loc, p_goal = map(get_value, self.inputs)
if prior.prob(loc) >= p_goal:
return [BSatisfies(prior, loc, p_goal)]
post, cost = prior, 0
while post.prob(loc) < p_goal: # NOTE - could also move one step
post, prob = look_update(loc, post, True)
cost += int(COST_SCALE * geom_cost(1, prob))
return [
IsLookUpdate(prior, loc, post),
BSatisfies(post, loc, p_goal),
Initialize(LookCost(*self.inputs[:2]), cost)
]
def get_values(self, **kwargs):
self.enumerated = True
obj, loc = map(get_value, self.inputs)
prob = self.cond_stream.prior.prob(loc)
cost = int(COST_SCALE * geom_cost(1, prob)) if not UNIT else 1
return [Initialize(MoveCost(*self.inputs[:2]), cost)]
def initial_pddl(self, costs):
atoms = list(self.get_initial_atoms())
#atoms = list(apply_axioms(set(atoms), self.type_to_objects, instantiate_axioms(self)))
if costs: atoms.append(Initialize(TotalCost(), self.initial_cost))
return '(' + '\n\t'.join([':init'] +
sorted([atom.pddl() for atom in atoms])) + ')'
def temp_initialize(self):
return {
Initialize(atom, value)
for atom, value in self.temp_functions.iteritems()
}
def perm_initialize(self):
return {
Initialize(atom, self.evaluate_func(atom))
for atom in self.perm_functions
}
