def to_pddlstream(belief_problem, collisions=True):
locations = {l for (_, l, _) in belief_problem.initial + belief_problem.goal} | \
set(belief_problem.locations)
observations = [True, False]
uniform = UniformDist(locations)
initial_bel = {o: MixtureDD(DeltaDist(l), uniform, p) for o, l, p in belief_problem.initial}
max_p_collision = 0.25 if collisions else 1.0
# TODO: separate pick and place for move
init = [('Obs', obs) for obs in observations] + \
[('Location', l) for l in locations]
for o, d in initial_bel.items():
init += [('Dist', o, d), ('BLoc', o, d)]
for (o, l, p) in belief_problem.goal:
init += [('Location', l), ('GoalProb', l, p)]
goal_literals = [('BLocGE', o, l, p) for (o, l, p) in belief_problem.goal]
goal = And(*goal_literals)
directory = os.path.dirname(os.path.abspath(__file__))
domain_pddl = read(os.path.join(directory, 'domain.pddl'))
stream_pddl = read(os.path.join(directory, 'stream.pddl'))
constant_map = {}
stream_map = {
'BCollision': get_collision_test(max_p_collision),
'GE': from_test(ge_fn),
'prob-after-move': from_fn(get_move_fn(belief_problem.p_move_s)),
'MoveCost': move_cost_fn,
'prob-after-look': from_fn(get_look_fn(belief_problem.p_look_fp, belief_problem.p_look_fn)),
'LookCost': get_look_cost_fn(belief_problem.p_look_fp, belief_problem.p_look_fn),
#'PCollision': from_fn(prob_occupied), # Then can use GE
}
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def fn(l):
# P(s2 | s1=loc1, a=(control_loc1, control_loc2))
perfect_d = DeltaDist(loc2)
fail_d = DeltaDist(l)
# fail_d = UniformDist(locations)
# return MixtureDD(perfect_d, fail_d, p_move_s)
if loc1 == l:
return MixtureDD(perfect_d, fail_d, p_move_s)
return fail_d
def set_uniform_belief(task, b_on, body, p_other=0.5):
# TODO: option to bias towards particular bottom
other = DeltaDist(OTHER)
uniform = UniformDist(task.get_supports(body))
b_on[body] = MixtureDD(other, uniform, p_other)
def set_uniform_belief(task, b_on, body, p_other=0.):
# p_other is the probability that it doesn't actually exist
# TODO: option to bias towards particular bottom
other = DeltaDist(OTHER)
uniform = UniformDist(task.get_supports(body))
b_on[body] = MixtureDD(other, uniform, p_other)