def get_goal_test(tamp_problem):
region_test = get_region_test(tamp_problem.regions)
def test(s):
for r in tamp_problem.initial.robot_confs:
if not at_conf(s, r, tamp_problem.goal_conf): # or not hand_empty(s, r):
return False
#return holding(s, 'r0', 'A')
for b, r in tamp_problem.goal_regions.items():
if not (b in s.block_poses and region_test(b, s.block_poses[b], r)):
return False
return True
return test
def pddlstream_from_tamp(tamp_problem, use_stream=True, use_optimizer=False, collisions=True):
initial = tamp_problem.initial
assert(initial.holding is None)
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
external_paths = []
if use_stream:
external_paths.append(get_file_path(__file__, 'stream.pddl'))
if use_optimizer:
external_paths.append(get_file_path(__file__, 'optimizer.pddl'))
external_pddl = [read(path) for path in external_paths]
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', b, p) for b, p in initial.block_poses.items()] + \
[('AtPose', b, p) for b, p in initial.block_poses.items()] + \
[('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()] + \
[('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()] + \
[('Region', r) for r in tamp_problem.goal_regions.values() + [GROUND_NAME]]
goal_literals = [('In', b, r) for b, r in tamp_problem.goal_regions.items()] #+ [('HandEmpty',)]
if tamp_problem.goal_conf is not None:
goal_literals += [('AtConf', tamp_problem.goal_conf)]
goal = And(*goal_literals)
stream_map = {
's-motion': from_fn(plan_motion),
'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(inverse_kin_fn),
#'s-ik': from_gen_fn(unreliable_ik_fn),
'distance': distance_fn,
't-cfree': from_test(lambda *args: implies(collisions, not collision_test(*args))),
#'posecollision': collision_test, # Redundant
'trajcollision': lambda *args: False,
}
if use_optimizer:
stream_map.update({
'gurobi': from_fn(get_optimize_fn(tamp_problem.regions)),
'rrt': from_fn(cfree_motion_fn),
})
#stream_map = 'debug'
return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map, init, goal)
def pddlstream_from_tamp(tamp_problem,
use_stream=True,
use_optimizer=False,
collisions=True):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
external_paths = []
if use_stream:
external_paths.append(get_file_path(__file__, 'stream.pddl'))
if use_optimizer:
external_paths.append(
get_file_path(
__file__,
'optimizer/optimizer.pddl')) # optimizer | optimizer_hard
external_pddl = [read(path) for path in external_paths]
constant_map = {}
stream_map = {
's-grasp':
from_fn(lambda b: (GRASP, )),
's-region':
from_gen_fn(get_pose_gen(tamp_problem.regions)),
's-ik':
from_fn(inverse_kin_fn),
#'s-ik': from_gen_fn(unreliable_ik_fn),
's-motion':
from_fn(plan_motion),
't-region':
from_test(get_region_test(tamp_problem.regions)),
't-cfree':
from_test(
lambda *args: implies(collisions, not collision_test(*args))),
'dist':
distance_fn,
'duration':
duration_fn,
}
if use_optimizer:
# To avoid loading gurobi
stream_map.update({
'gurobi':
from_list_fn(
get_optimize_fn(tamp_problem.regions, collisions=collisions)),
'rrt':
from_fn(cfree_motion_fn),
})
#stream_map = 'debug'
init, goal = create_problem(tamp_problem)
return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map,
init, goal)
def get_goal_test(tamp_problem):
region_test = get_region_test(tamp_problem.regions)
def test(s):
if not at_conf(s, tamp_problem.goal_conf):
return False
return holding(s, 'block0')
if not hand_empty(s):
return False
for b, r in tamp_problem.goal_regions.items():
if (s[b] is None) or not region_test(b, s[b], r):
return False
return True
return test
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):
initial = tamp_problem.initial
assert(initial.holding is None)
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
external_pddl = [
read(get_file_path(__file__, 'stream.pddl')),
#read(get_file_path(__file__, 'optimizer.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', b, p) for b, p in initial.block_poses.items()] + \
[('AtPose', b, p) for b, p in initial.block_poses.items()] + \
[('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()] + \
[('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()]
goal_literals = [('In', b, r) for b, r in tamp_problem.goal_regions.items()] #+ [('HandEmpty',)]
if tamp_problem.goal_conf is not None:
goal_literals += [('AtConf', tamp_problem.goal_conf)]
goal = And(*goal_literals)
stream_map = {
's-motion': from_fn(plan_motion),
'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(inverse_kin_fn),
'distance': distance_fn,
't-cfree': from_test(lambda *args: not collision_test(*args)),
'posecollision': collision_test, # Redundant
'trajcollision': lambda *args: False,
'gurobi': from_fn(get_optimize_fn(tamp_problem.regions)),
'rrt': from_fn(cfree_motion_fn),
#'reachable': from_test(reachable_test),
#'Valid': valid_state_fn,
}
#stream_map = 'debug'
return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map, init, goal)
def pddlstream_from_tamp(tamp_problem):
initial = tamp_problem.initial
assert(initial.holding is None)
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
# TODO: can always make the state the set of fluents
#state = tamp_problem.initial
state = {
R: tamp_problem.initial.conf,
H: tamp_problem.initial.holding,
}
for b, p in tamp_problem.initial.block_poses.items():
state[b] = p
init = [
('State', state),
('AtState', state),
('Conf', initial.conf)] + \
[('Block', b) for b in initial.block_poses.keys()] + \
[('Pose', b, p) for b, p in initial.block_poses.items()] + \
[('Region', r) for r in tamp_problem.regions.keys()] + \
[('AtPose', b, p) for b, p in initial.block_poses.items()] + \
[('Placeable', b, GROUND) for b in initial.block_poses.keys()] + \
[('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()]
goal = ('AtGoal',)
stream_map = {
'plan-motion': from_fn(plan_motion),
'sample-pose': from_gen_fn(get_pose_gen(tamp_problem.regions)),
'test-region': from_test(get_region_test(tamp_problem.regions)),
'inverse-kinematics': from_fn(inverse_kin_fn),
#'posecollision': collision_test,
#'trajcollision': lambda *args: False,
'forward-move': from_fn(move_fn),
'forward-pick': from_fn(pick_fn),
'forward-place': from_fn(place_fn),
'test-goal': from_test(get_goal_test(tamp_problem)),
}
#stream_map = 'debug'
return 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 pddlstream_from_tamp(tamp_problem):
initial = tamp_problem.initial
assert (initial.holding is None)
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, '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', b, p) for b, p in initial.block_poses.items()] + \
[('Region', r) for r in tamp_problem.regions.keys()] + \
[('AtPose', b, p) for b, p in initial.block_poses.items()] + \
[('Placeable', b, GROUND) for b in initial.block_poses.keys()] + \
[('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()]
goal_literals = [('HandEmpty',)] + \
[('In', b, r) for b, r in tamp_problem.goal_regions.items()]
if tamp_problem.goal_conf is not None:
goal_literals += [('AtConf', tamp_problem.goal_conf)]
goal = And(*goal_literals)
stream_map = {
'plan-motion': from_fn(plan_motion),
'sample-pose': from_gen_fn(get_pose_gen(tamp_problem.regions)),
'test-region': from_test(get_region_test(tamp_problem.regions)),
'inverse-kinematics': from_fn(inverse_kin_fn),
'collision-free': from_test(lambda *args: not collision_test(*args)),
'cfree': lambda *args: not collision_test(*args),
'posecollision': collision_test,
'trajcollision': lambda *args: False,
'distance': distance_fn,
#'Valid': valid_state_fn,
}
#stream_map = 'debug'
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def pddlstream_from_tamp(tamp_problem):
initial = tamp_problem.initial
assert(not initial.holding) # is None
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
# TODO: can always make the state a set of fluents
init = [
('State', initial),
('AtState', initial)] + \
[('Robot', r) for r in initial.robot_confs.keys()] + \
[('Conf', q) for q in initial.robot_confs.values()] + \
[('Block', b) for b in initial.block_poses.keys()] + \
[('Pose', b, p) for b, p in initial.block_poses.items()] + \
[('Grasp', b, GRASP) for b in initial.block_poses.keys()] + \
[('Region', r) for r in tamp_problem.regions.keys()] + \
[('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()] + \
[('Placeable', b, r) for b, r in tamp_problem.goal_regions.items()]
# TODO: could populate AtConf, AtPose, HandEmpty, Holding
goal = ('AtGoal',)
stream_map = {
'plan-motion': from_fn(plan_motion),
'sample-pose': from_gen_fn(get_pose_gen(tamp_problem.regions)),
'test-region': from_test(get_region_test(tamp_problem.regions)),
'inverse-kinematics': from_fn(inverse_kin_fn),
#'posecollision': collision_test,
#'trajcollision': lambda *args: False,
'forward-move': from_fn(move_fn),
'forward-pick': from_fn(pick_fn),
'forward-place': from_fn(place_fn),
'test-goal': from_test(get_goal_test(tamp_problem)),
}
#stream_map = 'debug'
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def test_goal_state(problem):
region_test = get_region_test(problem.regions)
def test(mode, conf):
return all(region_test(block, conf[block], region)
for block, region in problem.goal_regions.items())
return test
def pddlstream_from_tamp(tamp_problem,
use_stream=True,
use_optimizer=False,
collisions=True):
initial = tamp_problem.initial
assert (not initial.holding)
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
external_paths = []
if use_stream:
external_paths.append(get_file_path(__file__, 'stream.pddl'))
if use_optimizer:
external_paths.append(get_file_path(
__file__, 'optimizer.pddl')) # optimizer | optimizer_hard
external_pddl = [read(path) for path in external_paths]
constant_map = {}
init = [
('Region', GROUND_NAME),
Equal((TOTAL_COST,), 0)] + \
[('Block', b) for b in initial.block_poses.keys()] + \
[('Pose', b, p) for b, p in initial.block_poses.items()] + \
[('Grasp', b, GRASP) for b in initial.block_poses] + \
[('AtPose', b, p) for b, p in initial.block_poses.items()] + \
[('Placeable', b, GROUND_NAME) for b in initial.block_poses.keys()]
goal_literals = []
for r, q in initial.robot_confs.items():
init += [
('Robot', r),
('CanMove', r),
('Conf', q),
('AtConf', r, q),
('HandEmpty', r),
]
if tamp_problem.goal_conf is not None:
#goal_literals += [('AtConf', tamp_problem.goal_conf)]
goal_literals += [('AtConf', r, q)]
for b, r in tamp_problem.goal_regions.items():
if isinstance(r, str):
init += [('Region', r), ('Placeable', b, r)]
goal_literals += [('In', b, r)]
else:
init += [('Pose', b, r)]
goal_literals += [('AtPose', b, r)]
#goal_literals += [Not(('Unsafe',))] # ('HandEmpty',)
goal = And(*goal_literals)
stream_map = {
's-motion':
from_fn(plan_motion),
'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(inverse_kin_fn),
#'s-ik': from_gen_fn(unreliable_ik_fn),
'dist':
distance_fn,
'duration':
duration_fn,
't-cfree':
from_test(
lambda *args: implies(collisions, not collision_test(*args))),
}
if use_optimizer:
# To avoid loading gurobi
stream_map.update({
'gurobi':
from_list_fn(
get_optimize_fn(tamp_problem.regions, collisions=collisions)),
'rrt':
from_fn(cfree_motion_fn),
})
#stream_map = 'debug'
return PDDLProblem(domain_pddl, constant_map, external_pddl, stream_map,
init, goal)
