def pddlstream_from_tamp(tamp_problem):
initial = tamp_problem.initial
assert(initial.holding is None)
known_poses = list(initial.block_poses.values()) + \
list(tamp_problem.goal_poses.values())
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'))
q100 = np.array([100, 100])
constant_map = {
'q100': q100, # As an example
}
init = [
#Type(q100, 'conf'),
('CanMove',),
('Conf', q100),
('Conf', initial.conf),
('AtConf', initial.conf),
('HandEmpty',),
Equal((TOTAL_COST,), 0)] + \
[('Block', b) for b in initial.block_poses.keys()] + \
[('Pose', p) for p in known_poses] + \
[('AtPose', b, p) for b, p in initial.block_poses.items()]
# [('Pose', p) for p in known_poses + tamp_problem.poses] + \
goal = And(*[
('AtPose', b, p) for b, p in tamp_problem.goal_poses.items()
])
# TODO: convert to lower case
stream_map = {
#'sample-pose': from_gen_fn(lambda: ((np.array([x, 0]),) for x in range(len(poses), n_poses))),
'sample-pose': from_gen_fn(lambda: ((p,) for p in tamp_problem.poses)),
'inverse-kinematics': from_fn(lambda p: (p + GRASP,)),
'test-cfree': from_test(lambda *args: not collision_test(*args)),
'collision': collision_test,
'distance': distance_fn,
}
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def pddlstream_from_tamp(tamp_problem):
initial = tamp_problem.initial
assert (initial.holding is None)
known_poses = list(initial.block_poses.values()) + \
list(tamp_problem.goal_poses.values())
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 = {}
init = [
('CanMove',),
('Conf', initial.conf),
('AtConf', initial.conf),
('HandEmpty',),
Equal((TOTAL_COST,), 0)] + \
[('Block', b) for b in initial.block_poses.keys()] + \
[('Pose', p) for p in known_poses] + \
[('AtPose', b, p) for b, p in initial.block_poses.items()] + \
[('GoalPose', p) for p in tamp_problem.goal_poses.values()]
goal = And(('AtConf', initial.conf),
*[('AtPose', b, p) for b, p in tamp_problem.goal_poses.items()])
# TODO: convert to lower case
stream_map = {
'push-target': from_list_fn(push_target_fn),
'push-direction': push_direction_gen_fn,
'test-cfree': from_test(lambda *args: not collision_test(*args)),
'distance': distance_fn,
}
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)