def pddlstream_from_belief(initial_belief):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
constant_map = {}
stream_pddl = None
stream_map = {}
init = [
#('CanMove',),
Equal(('RegisterCost', ), scale_cost(1)),
Equal(('PickCost', ),
scale_cost(1)), # TODO: imperfect transition model
Equal(('PlaceCost', ), scale_cost(1)),
]
for item, dist in initial_belief.items():
support = dist.support()
if len(support) == 1:
init += [('On', item, support[0]), ('Localized', item)]
else:
init += [('Unknown', item)]
for i2 in support:
p_obs = dist.prob(i2)
cost = revisit_mdp_cost(
1, 1, p_obs) # TODO: imperfect observation model
if cost == INF:
continue
if i2 in initial_belief:
init += [('FiniteScanCost', i2, item),
Equal(('ScanCost', i2, item), scale_cost(cost))]
graspable_classes = [SOUP, GREEN]
for item in initial_belief:
for cl in filter(lambda c: is_class(item, c), CLASSES):
init += [('Class', item, cl)]
if cl in graspable_classes:
init += [('Graspable', item)] # TODO: include hand?
stackable_classes = [(TABLE, ROOM), (SOUP, TABLE), (GREEN, TABLE)]
for cl1, cl2 in stackable_classes:
for i1 in filter(lambda i: is_class(i, cl1), initial_belief):
for i2 in filter(lambda i: is_class(i, cl2), initial_belief):
init += [('Stackable', i1, i2)]
arms = ['left', 'right']
for arm in arms:
init += [('Arm', arm), ('HandEmpty', arm)]
goal_literals = [('On', 'soup0', 'table1'), ('Registered', 'soup0'),
('HoldingClass', 'green')]
#goal_literals = [('Holding', 'left', 'soup0')]
#goal_literals = [('HoldingClass', soup)]
#goal_literals = [Nearby('table1')]
#goal_literals = [('HoldingClass', 'green'), ('HoldingClass', 'soup')]
goal = And(*goal_literals)
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def pddlstream_from_state(state, teleport=False):
task = state.task
robot = task.robot
# TODO: infer open world from task
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {
'base': 'base',
'left': 'left',
'right': 'right',
'head': 'head',
}
#base_conf = state.poses[robot]
base_conf = Conf(robot, get_group_joints(robot, 'base'),
get_group_conf(robot, 'base'))
scan_cost = 1
init = [
('BConf', base_conf),
('AtBConf', base_conf),
Equal(('MoveCost', ), 1),
Equal(('PickCost', ), 1),
Equal(('PlaceCost', ), 1),
Equal(('ScanCost', ), scan_cost),
Equal(('RegisterCost', ), 1),
]
holding_arms = set()
holding_bodies = set()
for attach in state.attachments.values():
holding_arms.add(attach.arm)
holding_bodies.add(attach.body)
init += [('Grasp', attach.body, attach.grasp),
('AtGrasp', attach.arm, attach.body, attach.grasp)]
for arm in ARM_NAMES:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf), ('AtAConf', arm, conf)]
if arm in task.arms:
init += [('Controllable', arm)]
if arm not in holding_arms:
init += [('HandEmpty', arm)]
for body in task.get_bodies():
if body in holding_bodies:
continue
# TODO: no notion whether observable actually corresponds to the correct thing
pose = state.poses[body]
init += [
('Pose', body, pose),
('AtPose', body, pose),
('Observable', pose),
]
init += [('Scannable', body) for body in task.rooms + task.surfaces]
init += [('Registerable', body) for body in task.movable]
init += [('Graspable', body) for body in task.movable]
for body in task.get_bodies():
supports = task.get_supports(body)
if supports is None:
continue
for surface in supports:
p_obs = state.b_on[body].prob(surface)
cost = revisit_mdp_cost(0, scan_cost,
p_obs) # TODO: imperfect observation model
init += [('Stackable', body, surface),
Equal(('LocalizeCost', surface, body), clip_cost(cost))]
#if is_placement(body, surface):
if is_center_stable(body, surface):
if body in holding_bodies:
continue
pose = state.poses[body]
init += [('Supported', body, pose, surface)]
for body in task.get_bodies():
if state.is_localized(body):
init.append(('Localized', body))
else:
init.append(('Uncertain', body))
if body in state.registered:
init.append(('Registered', body))
goal = And(*[('Holding', a, b) for a, b in task.goal_holding] + \
[('On', b, s) for b, s in task.goal_on] + \
[('Localized', b) for b in task.goal_localized] + \
[('Registered', b) for b in task.goal_registered])
stream_map = {
'sample-pose':
get_stable_gen(task),
'sample-grasp':
from_list_fn(get_grasp_gen(task)),
'inverse-kinematics':
from_gen_fn(get_ik_ir_gen(task, teleport=teleport)),
'plan-base-motion':
from_fn(get_motion_gen(task, teleport=teleport)),
'test-vis-base':
from_test(get_in_range_test(task, VIS_RANGE)),
'test-reg-base':
from_test(get_in_range_test(task, REG_RANGE)),
'sample-vis-base':
accelerate_list_gen_fn(from_gen_fn(get_vis_base_gen(task, VIS_RANGE)),
max_attempts=25),
'sample-reg-base':
accelerate_list_gen_fn(from_gen_fn(get_vis_base_gen(task, REG_RANGE)),
max_attempts=25),
'inverse-visibility':
from_fn(get_inverse_visibility_fn(task)),
}
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
def compute_detect_cost(prob):
success_cost = DETECT_COST
failure_cost = success_cost
cost = revisit_mdp_cost(success_cost, failure_cost, prob)
return cost
