def get_pddlstream():
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
constant_map = {}
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
stream_map = {
'test-pose': from_test(empty_test), # universe_test | empty_test
'sample-pose': from_constant((np.array([2, 0]),)),
'inv-kin': from_fn(ik_fn),
'motion': from_fn(motion_fn),
}
block = 'block1'
region = 'region1'
pose = np.array([1, 0])
conf = np.array([0, 0])
init = [
('Conf', conf),
('AtConf', conf),
('HandEmpty',),
('Block', block),
('Pose', pose),
('AtPose', block, pose),
('Region', region),
]
goal = ('In', block, region)
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def examine_instantiated(problem,
constraints=PlanConstraints(),
unit_costs=False,
max_time=INF,
verbose=False,
**search_args):
domain_pddl, constant_map, stream_pddl, _, init, goal = problem
stream_map = DEBUG
problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
evaluations, goal_exp, domain, externals = parse_problem(
problem, constraints=constraints, unit_costs=unit_costs)
store = SolutionStore(evaluations,
max_time,
success_cost=INF,
verbose=verbose)
#externals = compile_fluents_as_attachments(domain, externals) #
instantiator = Instantiator(externals, evaluations)
process_stream_queue(instantiator,
store,
complexity_limit=INF,
verbose=verbose)
#plan, cost = solve_finite(evaluations, goal_exp, domain, max_cost=max_cost, **search_args)
debug = False
assert not isinstance(domain, SimplifiedDomain)
problem = get_problem(evaluations, goal_exp, domain, unit_costs)
task = task_from_domain_problem(domain, problem)
with Verbose(debug):
instantiated = instantiate_task(task)
instantiated = convert_instantiated(instantiated)
return instantiated
def random_restart(belief, args, problem, max_time=INF, max_iterations=INF,
max_planner_time=INF, **kwargs):
domain_pddl, constant_map, _, _, init, goal_formula = problem
start_time = time.time()
task = belief.task
world = task.world
iterations = 0
while (elapsed_time(start_time) < max_time) and (iterations < max_iterations):
iterations += 1
try:
stream_pddl, stream_map = get_streams(world, teleport_base=task.teleport_base,
collisions=not args.cfree, teleport=args.teleport)
problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)
remaining_time = min(max_time - elapsed_time(start_time), max_planner_time)
plan, plan_cost, certificate = solve_pddlstream(belief, problem, args, max_time=remaining_time, **kwargs)
if plan is not None:
return plan, plan_cost, certificate
except KeyboardInterrupt as e:
raise e
except MemoryError as e:
traceback.print_exc()
if not REPLAN_FAILURES:
raise e
break
except Exception as e:
traceback.print_exc()
if not REPLAN_FAILURES:
raise e
# FastDownward translator runs out of memory
return None, INF, Certificate(all_facts=[], preimage_facts=[])
def get_problem1():
value = 10
constant_map = {
#'c1': value,
}
stream_pddl = None
stream_map = {}
init = [
#('Goal',),
#('A',),
(
'B', ),
('P1', value),
#('P2', value),
]
goal = Or(
('Goal', ),
#Exists([], ('P2', value)),
#Exists(['?p'], ('P2', '?p')),
(
'Unachievable', ),
)
return PDDLProblem(DOMAIN_PDDL, constant_map, stream_pddl, stream_map,
init, goal)
def get_problem():
constant_map = {}
stream_map = {
'sample-split':
from_sampler(lambda: to_tuple(round(random.random(), 2))),
'compute-split':
from_fn(lambda (x1, x2), s: [(x1, convex_combo(x1, x2, s)),
(convex_combo(x1, x2, s), x2)]),
#'SplitCost': fn_from_constant(2),
'SplitCost':
lambda (x1, x2), s: round(
1. /
(min(convex_combo(x1, x2, s) - x1, x2 - convex_combo(x1, x2, s))
), 3)
}
locations = ['loc0', 'loc1', 'loc2']
init_carrot = (0., 1.)
init = [
#('Split', 0.5),
('Carrot', init_carrot),
('AtLoc', init_carrot, 'loc0'),
] + [('Loc', loc) for loc in locations]
goal = And(*[
Exists(['?c'], And(('Loc', loc), ('AtLoc', '?c', loc)))
for loc in locations
])
return PDDLProblem(DOMAIN_PDDL, constant_map, STREAM_PDDL, stream_map,
init, goal)
def examine_instantiated(problem, constraints=PlanConstraints(), unit_costs=False, unique=False, verbose=False, debug=False):
# TODO: refactor to an analysis file
domain_pddl, constant_map, stream_pddl, _, init, goal = problem
stream_map = DEBUG if unique else SHARED_DEBUG # DEBUG_MODES
problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
evaluations, goal_exp, domain, externals = parse_problem(
problem, constraints=constraints, unit_costs=unit_costs)
assert not isinstance(domain, SimplifiedDomain)
# store = SolutionStore(evaluations, max_time, success_cost=INF, verbose=verbose)
# instantiator = Instantiator(externals, evaluations)
# process_stream_queue(instantiator, store, complexity_limit=INF, verbose=verbose)
# results = [] # TODO: extract from process_stream_queue
#set_unique(externals)
# domain.actions[:] = [] # TODO: only instantiate axioms
# TODO: drop all fluents and instantiate
# TODO: relaxed planning version of this
results, exhausted = optimistic_process_streams(evaluations, externals, complexity_limit=INF, max_effort=None)
evaluations = evaluations_from_stream_plan(evaluations, results, max_effort=None)
problem = get_problem(evaluations, goal_exp, domain, unit_costs)
task = task_from_domain_problem(domain, problem)
with Verbose(debug):
instantiated = instantiate_task(task)
if instantiated is None:
return None
# TODO: reinstantiate actions?
instantiated.axioms[:] = [reinstantiate_axiom(axiom) for axiom in instantiated.axioms]
instantiated = convert_instantiated(instantiated)
return results, instantiated
def get_problem1(n_regions=0, n_cylinders=3, n_boxes=3):
constant_map = {}
stream_map = DEBUG
initial = 'initial'
shelfA = 'shelfA'
prepushA = 'prepushA'
shelfB = 'shelfB'
regions = [initial, shelfA, prepushA, shelfB] + \
['region{}'.format(i) for i in range(n_regions)]
cylinders = ['cylinder{}'.format(i) for i in range(n_cylinders)]
boxes = ['box{}'.format(i) for i in range(n_boxes)]
init = [
('HandEmpty',),
('Prepush', prepushA, shelfA),
#('Stackable', cylinders[2], cylinders[1]),
]
init += [('Region', region) for region in regions]
init += [('Cylinder', cylinder) for cylinder in cylinders]
init += [('Box', box) for box in boxes]
init.extend(flatten([('Movable', movable), ('On', movable, initial), ('Clear', movable)]
for movable in (cylinders + boxes)))
init += [('Thing', thing) for thing in (regions + cylinders + boxes)]
goal = And(
('In', cylinders[0], shelfA),
('On', cylinders[2], cylinders[1]),
)
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'))
constant_map = {}
initial = tamp_problem.initial
mode = {b: Mode(p, None) for b, p in initial.block_poses.items()}
conf = conf_from_state(initial)
init = [
('CanMove',),
('Mode', mode),
('AtMode', mode),
('Conf', mode, conf),
('AtConf', conf),
]
goal = Exists(['?m', '?q'], And(('GoalState', '?m', '?q'),
('AtMode', '?m'), ('AtConf', '?q')))
stream_map = {
's-forward': from_gen_fn(sample_forward(tamp_problem)),
's-intersection': from_gen_fn(sample_intersection(tamp_problem)),
's-connection': from_gen_fn(sample_connection(tamp_problem)),
't-goal': from_test(test_goal_state(tamp_problem)),
}
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def get_pddlstream(trajectories, element_bodies, ground_nodes):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
constant_map = {}
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
stream_map = {
'test-cfree': from_test(get_test_cfree(element_bodies)),
}
init = []
for n in ground_nodes:
init.append(('Connected', n))
for t in trajectories:
init.extend([
('Node', t.n1),
('Node', t.n2),
('Element', t.element),
('Traj', t),
('Connection', t.n1, t.element, t, t.n2),
])
goal_literals = [('Printed', e) for e in element_bodies]
goal = And(*goal_literals)
# TODO: weight or order these in some way
# TODO: instantiation slowness is due to condition effects. Change!
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
def get_pddlstream_test(node_points, elements, ground_nodes):
# stripstream/lis_scripts/run_print.py
# stripstream/lis_scripts/print_data.txt
domain_pddl = read(get_file_path(__file__, 'pddl/domain.pddl'))
constant_map = {}
stream_pddl = read(get_file_path(__file__, 'pddl/stream.pddl'))
#stream_pddl = None
stream_map = {
'test-cfree': from_test(get_test_cfree({})),
}
nodes = list(range(len(node_points))) # TODO: sort nodes by height?
init = []
for n in nodes:
init.append(('Node', n))
for n in ground_nodes:
init.append(('Connected', n))
for e in elements:
init.append(('Element', e))
n1, n2 = e
t = None
init.extend([
('Connection', n1, e, t, n2),
('Connection', n2, e, t, n1),
])
#init.append(('Edge', n1, n2))
goal_literals = [('Printed', e) for e in elements]
goal = And(*goal_literals)
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
def get_pddlstream_problem():
# TODO: bug where a trajectory sample could be used in a different state than anticipated (don't return the sample)
# TODO: enforce positive axiom preconditions requiring the state to be exactly some given value
# then, the can outputs can be used in other streams only present at that state
# TODO: explicitly don't let the outputs of one fluent stream be the input to another on a different state
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
constant_map = {}
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
stream_map = {
'sample-pickable': from_fn(feasibility_fn),
'test-cleanable': from_test(universe_test),
#'test-cleanable': from_fn(lambda o, fluents=set(): None if fluents else (TRAJ,)),
}
# Currently tests whether one can clean twice
init = [
('Block', 'b1'),
('Block', 'b2'),
('OnTable', 'b1'),
('OnTable', 'b2'),
]
#goal = ('Holding', 'b1')
goal = And(
('Clean', 'b1'),
('Cooked', 'b1'),
)
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def solve_first_goal(initial_problem, **kwargs):
domain_pddl, constant_map, stream_pddl, stream_map, init, goal_parts = initial_problem
achieved_parts = []
unachieved_parts = []
for task_part in goal_parts:
# TODO: store any stream evaluations (tests) and limit complexity
problem = create_simplified_problem(initial_problem,
new_goal=task_part)
solution = solve_restart(problem, **kwargs)
plan, _, _ = solution
if plan is None:
unachieved_parts.append(task_part)
elif len(plan) == 0:
achieved_parts.append(task_part)
else:
raise RuntimeError(task_part)
# print(achieved_parts)
# print(unachieved_parts)
# TODO: reset to initial state if not achieved
goal_formula = And(*achieved_parts)
if unachieved_parts:
goal_formula = And(Or(*unachieved_parts), goal_formula)
print(solve_all_goals.__name__, goal_formula)
problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal_formula)
return solve_restart(problem, **kwargs)
def solve_pddlstream_satisfaction(stream_pddl,
stream_map,
init,
constraints,
incremental=False,
**kwargs):
# TODO: prune set of streams based on constraints
domain, goal = planning_from_satisfaction(init, constraints)
constant_map = {}
problem = PDDLProblem(domain, constant_map, stream_pddl, stream_map, init,
goal)
if incremental:
plan, cost, facts = solve_incremental(problem, **kwargs)
else:
plan, cost, facts = solve_focused(problem, **kwargs)
if plan is None:
return None, cost, facts
assert len(plan) == len(domain.actions)
bindings = {}
for action, (name, args) in safe_zip(domain.actions, plan):
assert action.name == name
for param, arg in safe_zip(action.parameters, args):
name = param.name
assert bindings.get(name, arg) is arg
bindings[name] = arg
return bindings, cost, facts
def get_pddlstream(world,
debug=False,
collisions=True,
teleport=False,
parameter_fns={}):
domain_pddl = read(get_file_path(__file__, 'pddl/domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'pddl/stream.pddl'))
# TODO: increase number of attempts when collecting data
constant_map, initial_atoms, goal_formula = get_initial_and_goal(world)
stream_map = {
'sample-motion':
from_fn(get_motion_fn(world, collisions=collisions,
teleport=teleport)),
'sample-pick':
from_gen_fn(get_pick_gen_fn(world, collisions=collisions)),
'sample-place':
from_fn(get_place_fn(world, collisions=collisions)),
'sample-pose':
from_gen_fn(get_stable_pose_gen_fn(world, collisions=collisions)),
#'sample-grasp': from_gen_fn(get_grasp_gen_fn(world)),
'sample-pour':
from_gen_fn(
get_pour_gen_fn(world,
collisions=collisions,
parameter_fns=parameter_fns)),
'sample-push':
from_gen_fn(
get_push_gen_fn(world,
collisions=collisions,
parameter_fns=parameter_fns)),
'sample-stir':
from_gen_fn(
get_stir_gen_fn(world,
collisions=collisions,
parameter_fns=parameter_fns)),
'sample-scoop':
from_gen_fn(
get_scoop_gen_fn(world,
collisions=collisions,
parameter_fns=parameter_fns)),
'sample-press':
from_gen_fn(get_press_gen_fn(world, collisions=collisions)),
'test-reachable':
from_test(get_reachable_test(world)),
'ControlPoseCollision':
get_control_pose_collision_test(world, collisions=collisions),
'ControlConfCollision':
get_control_conf_collision_test(world, collisions=collisions),
'PosePoseCollision':
get_pose_pose_collision_test(world, collisions=collisions),
'ConfConfCollision':
get_conf_conf_collision_test(world, collisions=collisions),
}
if debug:
# Uses an automatically constructed debug generator for each stream
stream_map = DEBUG
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
initial_atoms, goal_formula)
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)),
'test-cfree-pose-pose': from_test(get_cfree_pose_pose_test()),
'test-cfree-approach-pose': from_test(get_cfree_obj_approach_pose_test()),
'test-cfree-traj-pose': from_test(negate_test(get_movable_collision_test())), #get_cfree_traj_pose_test()),
'TrajCollision': get_movable_collision_test(),
}
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal)
def solve_all_goals(initial_problem, **kwargs):
domain_pddl, constant_map, stream_pddl, stream_map, init, goal_parts = initial_problem
# TODO(caelan): cleaner specification of goal ordering
goal_formula = And(*goal_parts)
print(solve_all_goals.__name__, goal_formula)
problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal_formula)
return solve_restart(problem, **kwargs)
def create_problem(goal, obstacles=(), regions={}, max_distance=.5):
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 = {}
q0 = array([0, 0])
init = [
('Conf', q0),
('AtConf', q0),
] + [('Region', r) for r in regions]
if isinstance(goal, str):
goal = ('In', goal)
else:
init += [('Conf', goal)]
goal = ('AtConf', goal)
np.set_printoptions(precision=3)
samples = []
def region_gen(region):
lower, upper = regions[region]
area = np.product(upper - lower)
# TODO: sample proportional to area
while True:
q = array(sample_box(regions[region]))
samples.append(q)
yield (q, )
# http://citeseerx.ist.psu.edu/viewdoc/download?doi=10.1.1.419.5503&rep=rep1&type=pdf
#d = 2
#vol_free = (1 - 0) * (1 - 0)
#vol_ball = math.pi * (1 ** 2)
#gamma = 2 * ((1 + 1. / d) * (vol_free / vol_ball)) ** (1. / d)
roadmap = []
def connected_test(q1, q2):
#n = len(samples)
#threshold = gamma * (math.log(n) / n) ** (1. / d)
threshold = max_distance
are_connected = (get_distance(q1, q2) <= threshold) and \
is_collision_free((q1, q2), obstacles)
if are_connected:
roadmap.append((q1, q2))
return are_connected
stream_map = {
'sample-region': from_gen_fn(region_gen),
'connect': from_test(connected_test),
'distance': get_distance,
}
problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
return problem, samples, roadmap
def get_pddlstream2(robot,
obstacles,
node_points,
element_bodies,
ground_nodes,
trajectories=[]):
domain_pddl = read(get_file_path(
__file__, 'regression.pddl')) # progression | regression
constant_map = {}
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
stream_map = {
'test-cfree':
from_test(get_test_cfree(element_bodies)),
#'sample-print': from_gen_fn(get_print_gen_fn(robot, obstacles, node_points, element_bodies, ground_nodes)),
'sample-print':
get_wild_print_gen_fn(robot, obstacles, node_points, element_bodies,
ground_nodes),
}
# TODO: assert that all elements have some support
init = []
for n in ground_nodes:
init.append(('Grounded', n))
for e in element_bodies:
for n in e:
if element_supports(e, n, node_points):
init.append(('Supports', e, n))
if is_start_node(n, e, node_points):
init.append(('StartNode', n, e))
for e in element_bodies:
n1, n2 = e
init.extend([
('Node', n1),
('Node', n2),
('Element', e),
('Printed', e),
('Edge', n1, e, n2),
('Edge', n2, e, n1),
#('StartNode', n1, e),
#('StartNode', n2, e),
])
#if is_ground(e, ground_nodes):
# init.append(('Grounded', e))
#for e1, neighbors in get_element_neighbors(element_bodies).items():
# for e2 in neighbors:
# init.append(('Supports', e1, e2))
for t in trajectories:
init.extend([
('Traj', t),
('PrintAction', t.n1, t.element, t),
])
goal = And(*[('Removed', e) for e in element_bodies])
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
def pddlstream_from_problem(problem):
# TODO: push and attach to movable objects
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
# TODO: action to generically connect to the roadmap
# TODO: could check individual vertices first
# TODO: dynamically generate the roadmap in interesting parts of the space
# TODO: visibility graphs for sparse roadmaps
# TODO: approximate robot with isotropic geometry
# TODO: make the effort finite if applied to the roadmap vertex
samples = []
init = []
for robot, conf in problem.initial_confs.items():
samples.append(conf)
init += [('Robot', robot), ('Conf', robot, conf),
('AtConf', robot, conf), ('Free', robot)]
for body, pose in problem.initial_poses.items():
init += [('Body', body), ('Pose', body, pose), ('AtPose', body, pose)]
goal_literals = []
goal_literals += [('Holding', robot, body)
for robot, body in problem.goal_holding.items()]
for robot, base_values in problem.goal_confs.items():
q_goal = Conf(robot, get_base_joints(robot), base_values)
samples.append(q_goal)
init += [('Conf', robot, q_goal)]
goal_literals += [('AtConf', robot, q_goal)]
goal_formula = And(*goal_literals)
# TODO: assuming holonomic for now
[body] = problem.robots
with LockRenderer():
init += create_vertices(problem, body, samples)
#init += create_edges(problem, body, samples)
stream_map = {
'test-cfree-conf-pose': from_test(get_test_cfree_conf_pose(problem)),
'test-cfree-traj-pose': from_test(get_test_cfree_traj_pose(problem)),
# TODO: sample pushes rather than picks/places
'sample-grasp': from_gen_fn(get_grasp_generator(problem)),
'compute-ik': from_fn(get_ik_fn(problem)),
'compute-motion': from_fn(get_motion_fn(problem)),
'test-reachable': from_test(lambda *args: False),
'Cost': get_cost_fn(problem),
}
#stream_map = 'debug'
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal_formula)
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', ), 1),
Equal(('PickCost', ), 1), # TODO: imperfect transition model
Equal(('PlaceCost', ), 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), cost)]
for item in initial_belief:
for cl in filter(lambda c: is_class(item, c), CLASSES):
init += [('Class', item, cl)]
if cl in GRASPABLE:
init += [('Graspable', item)] # TODO: include hand?
for cl1, cl2 in STACKABLE:
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)]
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 PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
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_problem():
constant_map = {}
stream_pddl = None
stream_map = {}
init = [
('on-table', 'a'),
('on', 'b', 'a'),
('arm-empty', ),
]
goal = ('on', 'a', 'b')
return PDDLProblem(DOMAIN_PDDL, constant_map, stream_pddl, stream_map,
init, goal)
def create_problem(initial_poses):
block_goal = (-25, 0, 0)
initial_atoms = [
('IsPose', COASTER, block_goal),
('Empty', ROBOT),
('CanMove', ROBOT),
('HasSugar', 'sugar_cup'),
('HasCream', 'cream_cup'),
('IsPourable', 'cream_cup'),
('Stackable', CUP, COASTER),
('Clear', COASTER),
]
goal_literals = [
('AtPose', COASTER, block_goal),
('On', CUP, COASTER),
('HasCoffee', CUP),
('HasCream', CUP),
('HasSugar', CUP),
('Mixed', CUP),
('Empty', ROBOT),
]
for name, pose in initial_poses.items():
if 'gripper' in name:
initial_atoms += [('IsGripper', name)]
if 'cup' in name:
initial_atoms += [('IsCup', name)]
if 'spoon' in name:
initial_atoms += [('IsSpoon', name), ('IsStirrer', name)]
if 'stirrer' in name:
initial_atoms += [('IsStirrer', name)]
if 'block' in name:
initial_atoms += [('IsBlock', name)]
initial_atoms += [
('IsPose', name, pose),
('AtPose', name, pose),
('TableSupport', pose),
]
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
stream_map = DEBUG
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
initial_atoms, And(*goal_literals))
def get_problem():
domain_pddl = read_pddl('domain.pddl')
constant_map = {}
stream_pddl = None
stream_map = {}
init = [
('on-table', 'a'),
('on', 'b', 'a'),
('clear', 'b'),
('arm-empty',),
]
goal = ('on', 'a', 'b')
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'))
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 create_simplified_problem(problem,
use_actions=False,
use_streams=False,
new_goal=None):
# TODO: check whether goal is a conjunction
domain_pddl, constant_map, stream_pddl, stream_map, init, goal_parts = problem
if not use_streams:
stream_pddl = None
if new_goal is None:
new_goal = goal_parts
domain = parse_domain(
domain_pddl
) # TODO: Constant map value @base not mentioned in domain :constants
if not use_actions:
domain.actions[:] = [] # No actions
return PDDLProblem(domain, constant_map, stream_pddl, stream_map, init,
new_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)
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)
