def pddlstream_from_problem(robot, movable=[], teleport=False, grasp_name='top'):
#assert (not are_colliding(tree, kin_cache))
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
print('Robot:', robot)
conf = BodyConf(robot, get_configuration(robot))
init = [('CanMove',),
('Conf', conf),
('AtConf', conf),
('HandEmpty',)]
fixed = get_fixed(robot, movable)
print('Movable:', movable)
print('Fixed:', fixed)
for body in movable:
pose = BodyPose(body, get_pose(body))
init += [('Graspable', body),
('Pose', body, pose),
('AtPose', body, pose)]
for surface in fixed:
init += [('Stackable', body, surface)]
if is_placement(body, surface):
init += [('Supported', body, pose, surface)]
for body in fixed:
name = get_body_name(body)
if 'sink' in name:
init += [('Sink', body)]
if 'stove' in name:
init += [('Stove', body)]
body = movable[0]
goal = ('and',
('AtConf', conf),
#('Holding', body),
#('On', body, fixed[1]),
#('On', body, fixed[2]),
#('Cleaned', body),
('Cooked', body),
)
stream_map = {
'sample-pose': from_gen_fn(get_stable_gen(fixed)),
'sample-grasp': from_gen_fn(get_grasp_gen(robot, grasp_name)),
'inverse-kinematics': from_fn(get_ik_fn(robot, fixed, teleport)),
'plan-free-motion': from_fn(get_free_motion_gen(robot, fixed, teleport)),
'plan-holding-motion': from_fn(get_holding_motion_gen(robot, fixed, teleport)),
'TrajCollision': get_movable_collision_test(),
}
if USE_SYNTHESIZERS:
stream_map.update({
'plan-free-motion': empty_gen(),
'plan-holding-motion': empty_gen(),
})
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def fn(outputs, certified):
assert (len(outputs) == 1)
q0, _, q1 = find_unique(lambda f: f[0] == 'freemotion', certified)[1:]
obstacles = fixed + place_movable(certified)
free_motion_fn = get_free_motion_gen(robot, obstacles, teleport)
return free_motion_fn(q0, q1)