def pddlstream_from_tamp(tamp_problem):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
stream_map = {
#'s-motion': from_fn(plan_motion),
't-reachable': from_test(test_reachable),
's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)),
't-region': from_test(get_region_test(tamp_problem.regions)),
's-ik': from_fn(lambda b, p, g: (inverse_kin(p, g),)),
'dist': distance_fn,
}
init, goal = create_problem(tamp_problem)
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def pddlstream_from_tamp(tamp_problem):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
# TODO: algorithm that prediscretized once
constant_map = {}
stream_map = {
's-motion': from_fn(plan_motion),
't-reachable': from_test(test_reachable),
's-region': from_gen_fn(get_pose_gen(tamp_problem.regions)),
't-region': from_test(get_region_test(tamp_problem.regions)),
's-ik': from_fn(lambda b, p, g: (inverse_kin(p, g),)),
'dist': distance_fn,
}
init, goal = create_problem(tamp_problem)
init.extend(('Grasp', b, GRASP) for b in tamp_problem.initial.block_poses)
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def gen(mode1, mode2):
assert set(mode1) == set(mode2)
unchanged = {block for block in mode1 if mode1[block].frame == mode2[block].frame} # TODO: pose as well?
changed = set(mode1) - unchanged
[block] = changed
transit_mode, transfer_mode = mode1, mode2
if mode2[block].frame == GROUND:
transit_mode, transfer_mode = mode2, mode1
grasp, robot = transfer_mode[block]
pose, _ = transit_mode[block]
conf = inverse_kin(pose, grasp)
state = {robot: conf}
for block in transfer_mode: # Should be equivalent when using mode2
pose, frame = transfer_mode[block]
if frame == GROUND:
state[block] = pose
else:
state[block] = forward_kin(state[frame], pose)
# No kinematic chain => only one state in the intersection
if test_state(problem, state):
yield (state,)