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 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_fn(lambda o, fluents=set(): (TRAJ, )),
#'test-cleanable': from_fn(lambda o, fluents=set(): None if fluents else (TRAJ,)),
}
init = [
('Block', 'b1'),
('Block', 'b2'),
('OnTable', 'b1'),
('OnTable', 'b2'),
]
#goal = ('Holding', 'b1')
goal = And(('Clean', 'b1'), ('Cooked', 'b1'))
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
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 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 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 pddlstream_from_problem(problem, teleport=False, movable_collisions=False):
robot = problem.robot
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {
'world': 'world',
}
world = 'world'
initial_bq = Pose(robot, get_pose(robot))
init = [
('CanMove',),
('BConf', initial_bq), # TODO: could make pose as well...
('AtBConf', initial_bq),
('AtAConf', world, None),
('AtPose', world, world, None),
] + [('Sink', s) for s in problem.sinks] + \
[('Stove', s) for s in problem.stoves] + \
[('Connected', b, d) for b, d in problem.buttons] + \
[('Button', b) for b, _ in problem.buttons]
#for arm in ARM_JOINT_NAMES:
for arm in problem.arms:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf),
('HandEmpty', arm), ('AtAConf', arm, conf), ('AtPose', arm, arm, None)]
if arm in problem.arms:
init += [('Controllable', arm)]
for body in problem.movable:
pose = Pose(body, get_pose(body))
init += [('Graspable', body), ('Pose', body, pose),
('AtPose', world, body, pose)]
for surface in problem.surfaces:
init += [('Stackable', body, surface)]
if is_placement(body, surface):
init += [('Supported', body, pose, surface)]
goal = ['and']
if problem.goal_conf is not None:
goal_conf = Pose(robot, problem.goal_conf)
init += [('BConf', goal_conf)]
goal += [('AtBConf', goal_conf)]
goal += [('Holding', a, b) for a, b in problem.goal_holding] + \
[('On', b, s) for b, s in problem.goal_on] + \
[('Cleaned', b) for b in problem.goal_cleaned] + \
[('Cooked', b) for b in problem.goal_cooked]
stream_map = {
'sample-pose': get_stable_gen(problem),
'sample-grasp': from_list_fn(get_grasp_gen(problem)),
'inverse-kinematics': from_gen_fn(get_ik_ir_gen(problem, teleport=teleport)),
'plan-base-motion': from_fn(get_motion_gen(problem, teleport=teleport)),
#'plan-base-motion': empty_gen(),
'BTrajCollision': fn_from_constant(False),
}
# get_press_gen(problem, teleport=teleport)
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def pddlstream_from_problem(problem, teleport=False, movable_collisions=False):
robot = problem.robot
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
#initial_bq = Pose(robot, get_pose(robot))
initial_bq = Conf(robot, get_group_joints(robot, 'base'), get_group_conf(robot, 'base'))
init = [
('CanMove',),
('BConf', initial_bq),
('AtBConf', initial_bq),
Equal(('PickCost',), scale_cost(1)),
Equal(('PlaceCost',), scale_cost(1)),
] + [('Sink', s) for s in problem.sinks] + \
[('Stove', s) for s in problem.stoves] + \
[('Connected', b, d) for b, d in problem.buttons] + \
[('Button', b) for b, _ in problem.buttons]
for arm in ARM_NAMES:
#for arm in problem.arms:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf), ('HandEmpty', arm), ('AtAConf', arm, conf)]
if arm in problem.arms:
init += [('Controllable', arm)]
for body in problem.movable:
pose = Pose(body, get_pose(body))
init += [('Graspable', body), ('Pose', body, pose),
('AtPose', body, pose)]
for surface in problem.surfaces:
init += [('Stackable', body, surface)]
if is_placement(body, surface):
init += [('Supported', body, pose, surface)]
goal = [AND]
if problem.goal_conf is not None:
goal_conf = Pose(robot, problem.goal_conf)
init += [('BConf', goal_conf)]
goal += [('AtBConf', goal_conf)]
goal += [('Holding', a, b) for a, b in problem.goal_holding] + \
[('On', b, s) for b, s in problem.goal_on] + \
[('Cleaned', b) for b in problem.goal_cleaned] + \
[('Cooked', b) for b in problem.goal_cooked]
stream_map = {
'sample-pose': get_stable_gen(problem),
'sample-grasp': from_list_fn(get_grasp_gen(problem)),
'inverse-kinematics': from_gen_fn(get_ik_ir_gen(problem, teleport=teleport)),
'plan-base-motion': from_fn(get_motion_gen(problem, teleport=teleport)),
'MoveCost': move_cost_fn,
'TrajPoseCollision': fn_from_constant(False),
'TrajArmCollision': fn_from_constant(False),
'TrajGraspCollision': fn_from_constant(False),
}
if USE_SYNTHESIZERS:
stream_map['plan-base-motion'] = empty_gen(),
# get_press_gen(problem, teleport=teleport)
return 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(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 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_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 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 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_belief(initial_belief):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
constant_map = {}
stream_pddl = None
stream_map = {}
init = [
#('CanMove',),
Equal(('RegisterCost', ), scale_cost(1)),
Equal(('PickCost', ),
scale_cost(1)), # TODO: imperfect transition model
Equal(('PlaceCost', ), scale_cost(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), scale_cost(cost))]
graspable_classes = [SOUP, GREEN]
for item in initial_belief:
for cl in filter(lambda c: is_class(item, c), CLASSES):
init += [('Class', item, cl)]
if cl in graspable_classes:
init += [('Graspable', item)] # TODO: include hand?
stackable_classes = [(TABLE, ROOM), (SOUP, TABLE), (GREEN, TABLE)]
for cl1, cl2 in stackable_classes:
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)]
arms = ['left', 'right']
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 domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def get_pddlstream_problem():
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
constant_map = {}
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
stream_map = {
#'test-feasible': from_test(test_feasible),
'test-feasible': from_fn(feasibility_fn),
}
init = [
('Block', 'b1'),
('Block', 'b2'),
('OnTable', 'b1'),
('OnTable', 'b2'),
]
goal = ('Holding', 'b1')
return 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 pddlstream_from_belief():
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
constant_map = {}
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
stream_map = {
'inv-kin': from_fn(ik_fn),
'motion': from_fn(motion_fn),
}
# Options
# - observation produces one of several poses
# - always at the pose, observation just makes it observable
# - object has a unobserved fluent
block = 'block1'
#pose = None # Unknown
pose = Uncertain()
#pose = np.array([1, 0])
conf = np.array([0, 1])
booth = np.array([0, 2])
init = [
('Initial',), # Forces move first
('Conf', conf),
#('Booth', conf),
#('Conf', booth),
#('Booth', booth),
('AtConf', conf),
('HandEmpty',),
('Block', block),
('Pose', pose),
('AtPose', block, pose),
#('Observable', pose),
#('Latent', pose),
]
goal = ('Holding', block)
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def get_problem(robot):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = None
constant_map = {}
stream_map = {}
robot_name = 'ur5'
robot_conf = robot.get_current_pose()
init = [
('Robot', robot_name),
('AtConf', robot_name, robot_conf),
('Conf', robot_name, robot_conf),
('CanMove', robot_name),
('HandEmpty', robot_name),
('FreePos', robot_conf)
]
objects = robot.get_object_6d_pose()
for obj_name in objects.keys():
init += [
('Obj', obj_name), # Currently, we only care about translation
('Pose', obj_name, objects[obj_name][0]),
('AtPose', obj_name, objects[obj_name][0]),
]
print('The robot detects the following objects: ')
obj_names = list(objects.keys())
for i, obj_name in enumerate(obj_names):
print('\t%d %s' % (i + 1, obj_name))
print('Which one do you want to grasp?')
idx = raw_input()
idx = int(idx)
while idx <= 0 or idx > len(obj_names):
print('Wrong input, try again: ')
idx = raw_input()
idx = int(idx)
goal = [
('AtConf', robot_name, robot_conf),
('AtGrasp', robot_name, obj_names[idx - 1]),
]
goal = And(*goal)
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def pddlstream_from_problem(problem, teleport=False):
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {'{}'.format(name).lower(): name
for name in problem.initial_confs.keys()}
edges = set()
init = [
]
for name, conf in problem.initial_confs.items():
init += [
('Safe',),
('Robot', name),
('Conf', conf),
('AtConf', name, conf),
Equal(('Speed', name), DIST_PER_TIME),
Equal(('BatteryCapacity', name), MAX_ENERGY),
Equal(('RechargeRate', name), CHARGE_PER_TIME),
Equal(('ConsumptionRate', name), BURN_PER_TIME),
Equal(('Energy', name), INITIAL_ENERGY),
]
goal_literals = [
#('Safe',),
#('Unachievable',),
]
for name, base_values in problem.goal_confs.items():
body = problem.get_body(name)
joints = get_base_joints(body)
#extend_fn = get_extend_fn(body, joints, resolutions=5*BASE_RESOLUTIONS)
#q_init = problem.initial_confs[name]
#path = [q_init] + [Conf(body, joints, q) for q in extend_fn(q_init.values, base_values)]
#edges.update(zip(path, path[1:]))
#q_goal = path[-1]
q_goal = Conf(body, joints, base_values)
init += [('Conf', q_goal)]
goal_literals += [('AtConf', name, q_goal)]
goal_formula = And(*goal_literals)
robot = list(map(problem.get_body, problem.initial_confs))[0]
with LockRenderer():
init += [('Conf', q) for _, n, q in create_vertices(problem, robot, [], samples_per_ft2=6)]
#vertices = {v for edge in edges for v in edge}
#handles = []
#for vertex in vertices:
# handles.extend(draw_point(point_from_conf(vertex.values), size=0.05))
#for conf1, conf2 in edges:
# traj = linear_trajectory(conf1, conf2)
# init += [
# ('Conf', conf1),
# ('Traj', traj),
# ('Conf', conf2),
# ('Motion', conf1, traj, conf2),
# ]
#draw_edges(edges)
cfree_traj_traj_test = get_test_cfree_traj_traj(problem)
cfree_traj_traj_list_gen_fn = from_test(cfree_traj_traj_test)
traj_traj_collision_test = negate_test(cfree_traj_traj_test)
stream_map = {
'test-cfree-conf-conf': cfree_traj_traj_list_gen_fn,
'test-cfree-traj-conf': cfree_traj_traj_list_gen_fn,
'test-cfree-traj-traj': cfree_traj_traj_list_gen_fn,
'ConfConfCollision': traj_traj_collision_test,
'TrajConfCollision': traj_traj_collision_test,
'TrajTrajCollision': traj_traj_collision_test,
'compute-motion': from_fn(get_motion_fn2(problem)),
'TrajDistance': get_distance_fn(),
}
#stream_map = 'debug'
problem = PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)
return problem, edges
def pddlstream_from_state(state, teleport=False):
task = state.task
robot = task.robot
# TODO: infer open world from task
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {
'base': 'base',
'left': 'left',
'right': 'right',
'head': 'head',
}
#base_conf = state.poses[robot]
base_conf = Conf(robot, get_group_joints(robot, 'base'),
get_group_conf(robot, 'base'))
scan_cost = 1
init = [
('BConf', base_conf),
('AtBConf', base_conf),
Equal(('MoveCost', ), 1),
Equal(('PickCost', ), 1),
Equal(('PlaceCost', ), 1),
Equal(('ScanCost', ), scan_cost),
Equal(('RegisterCost', ), 1),
]
holding_arms = set()
holding_bodies = set()
for attach in state.attachments.values():
holding_arms.add(attach.arm)
holding_bodies.add(attach.body)
init += [('Grasp', attach.body, attach.grasp),
('AtGrasp', attach.arm, attach.body, attach.grasp)]
for arm in ARM_NAMES:
joints = get_arm_joints(robot, arm)
conf = Conf(robot, joints, get_joint_positions(robot, joints))
init += [('Arm', arm), ('AConf', arm, conf), ('AtAConf', arm, conf)]
if arm in task.arms:
init += [('Controllable', arm)]
if arm not in holding_arms:
init += [('HandEmpty', arm)]
for body in task.get_bodies():
if body in holding_bodies:
continue
# TODO: no notion whether observable actually corresponds to the correct thing
pose = state.poses[body]
init += [
('Pose', body, pose),
('AtPose', body, pose),
('Observable', pose),
]
init += [('Scannable', body) for body in task.rooms + task.surfaces]
init += [('Registerable', body) for body in task.movable]
init += [('Graspable', body) for body in task.movable]
for body in task.get_bodies():
supports = task.get_supports(body)
if supports is None:
continue
for surface in supports:
p_obs = state.b_on[body].prob(surface)
cost = revisit_mdp_cost(0, scan_cost,
p_obs) # TODO: imperfect observation model
init += [('Stackable', body, surface),
Equal(('LocalizeCost', surface, body), clip_cost(cost))]
#if is_placement(body, surface):
if is_center_stable(body, surface):
if body in holding_bodies:
continue
pose = state.poses[body]
init += [('Supported', body, pose, surface)]
for body in task.get_bodies():
if state.is_localized(body):
init.append(('Localized', body))
else:
init.append(('Uncertain', body))
if body in state.registered:
init.append(('Registered', body))
goal = And(*[('Holding', a, b) for a, b in task.goal_holding] + \
[('On', b, s) for b, s in task.goal_on] + \
[('Localized', b) for b in task.goal_localized] + \
[('Registered', b) for b in task.goal_registered])
stream_map = {
'sample-pose':
get_stable_gen(task),
'sample-grasp':
from_list_fn(get_grasp_gen(task)),
'inverse-kinematics':
from_gen_fn(get_ik_ir_gen(task, teleport=teleport)),
'plan-base-motion':
from_fn(get_motion_gen(task, teleport=teleport)),
'test-vis-base':
from_test(get_in_range_test(task, VIS_RANGE)),
'test-reg-base':
from_test(get_in_range_test(task, REG_RANGE)),
'sample-vis-base':
accelerate_list_gen_fn(from_gen_fn(get_vis_base_gen(task, VIS_RANGE)),
max_attempts=25),
'sample-reg-base':
accelerate_list_gen_fn(from_gen_fn(get_vis_base_gen(task, REG_RANGE)),
max_attempts=25),
'inverse-visibility':
from_fn(get_inverse_visibility_fn(task)),
}
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
def get_pddlstream(robot,
brick_from_index,
obstacle_from_name,
collisions=True,
teleport=False):
domain_pddl = read(get_file_path(__file__, 'retired.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
conf = np.array(get_configuration(robot))
init = [
('CanMove', ),
('Conf', conf),
('AtConf', conf),
('HandEmpty', ),
]
goal_literals = [
('AtConf', conf),
#('HandEmpty',),
]
#indices = brick_from_index.keys()
#indices = range(2, 5)
indices = [4]
for index in list(indices):
indices.append(index + 6)
for index in indices:
brick = brick_from_index[index]
init += [
('Graspable', index),
('Pose', index, brick.initial_pose),
('AtPose', index, brick.initial_pose),
('Pose', index, brick.goal_pose),
]
goal_literals += [
#('Holding', index),
('AtPose', index, brick.goal_pose),
]
for index2 in brick.goal_supports:
brick2 = brick_from_index[index2]
init += [
('Supported', index, brick.goal_pose, index2,
brick2.goal_pose),
]
goal = And(*goal_literals)
if not collisions:
obstacle_from_name = {}
stream_map = {
'sample-grasp':
from_gen_fn(get_grasp_gen_fn(brick_from_index)),
'inverse-kinematics':
from_gen_fn(get_ik_gen_fn(robot, brick_from_index,
obstacle_from_name)),
'plan-motion':
from_fn(
get_motion_fn(robot,
brick_from_index,
obstacle_from_name,
teleport=teleport)),
'TrajCollision':
get_collision_test(robot, brick_from_index, collisions=collisions),
}
#stream_map = 'debug'
return domain_pddl, constant_map, stream_pddl, stream_map, init, goal
def get_pddlstream(robots,
static_obstacles,
node_points,
element_bodies,
ground_nodes,
layer_from_n,
initial_confs={},
printed=set(),
removed=set(),
additional_init=[],
additional_orders=set(),
trajectories=[],
temporal=True,
sequential=False,
local=False,
can_print=True,
can_transit=False,
checker=None,
**kwargs):
try:
get_tfd_path()
except RuntimeError:
temporal = False
print(
'Temporal Fast Downward is not installed. Disabling temporal planning.'
)
# TODO: TFD submodule
assert not removed & printed
remaining = set(element_bodies) - removed - printed
element_obstacles = {element_bodies[e] for e in printed}
obstacles = set(static_obstacles) | element_obstacles
max_layer = max(layer_from_n.values())
directions = compute_directions(remaining, layer_from_n)
if local:
# makespan seems more effective than CEA
partial_orders = compute_local_orders(
remaining, layer_from_n) # makes the makespan heuristic slow
else:
partial_orders = compute_global_orders(remaining, layer_from_n)
partial_orders.update(additional_orders)
# draw_model(supporters, node_points, ground_nodes, color=RED)
# wait_if_gui()
domain_pddl = read(
get_file_path(
__file__,
os.path.join('pddl',
'temporal.pddl' if temporal else 'domain.pddl')))
stream_pddl = read(get_file_path(__file__, 'pddl/stream.pddl'))
constant_map = {}
# TODO: don't evaluate TrajTrajCollision until the plan is retimed
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-move':
get_wild_move_gen_fn(robots,
obstacles,
element_bodies,
partial_orders=partial_orders,
**kwargs),
'sample-print':
get_wild_print_gen_fn(robots,
obstacles,
node_points,
element_bodies,
ground_nodes,
initial_confs=initial_confs,
partial_orders=partial_orders,
removed=removed,
**kwargs),
#'test-stiffness': from_test(test_stiffness),
#'test-cfree-traj-conf': from_test(lambda *args: True),
#'test-cfree-traj-traj': from_test(get_cfree_test(**kwargs)),
'TrajTrajCollision':
get_collision_test(robots, **kwargs),
'TrajConfCollision':
lambda *args, **kwargs:
False, # TODO: could treat conf as length 1 traj
'Length':
lambda e: get_element_length(e, node_points),
'Distance':
lambda r, t: t.get_link_distance(),
'Duration':
lambda r, t: t.get_link_distance() / TOOL_VELOCITY,
'Euclidean':
lambda n1, n2: get_element_length((n1, n2), node_points),
}
assignments = compute_assignments(robots, remaining, node_points,
initial_confs)
transits = compute_transits(layer_from_n, directions)
init = [
('Stationary', ),
Equal(('Speed', ), TOOL_VELOCITY),
]
init.extend(additional_init)
if can_print:
init.append(('Print', ))
if can_transit:
init.append(('Move', ))
if sequential:
init.append(('Sequential', ))
for name, conf in initial_confs.items():
robot = index_from_name(robots, name)
#init_node = -robot
init_node = '{}-q0'.format(robot)
init.extend([
#('Node', init_node),
('BackoffConf', name, conf),
('Robot', name),
('Conf', name, conf),
('AtConf', name, conf),
('Idle', name),
#('CanMove', name),
#('Start', name, init_node, None, conf),
#('End', name, None, init_node, conf),
])
for (n1, e, n2) in directions:
if layer_from_n[n1] == 0:
transits.append((None, init_node, n1, e))
if layer_from_n[n2] == max_layer:
transits.append((e, n2, init_node, None))
#init.extend(('Grounded', n) for n in ground_nodes)
init.extend(('Direction', ) + tup for tup in directions)
init.extend(('Order', ) + tup for tup in partial_orders)
# TODO: can relax assigned if I go by layers
init.extend(
('Assigned', r, e) for r in assignments for e in assignments[r])
#init.extend(('Transit',) + tup for tup in transits)
# TODO: only move actions between adjacent layers
for e in remaining:
n1, n2 = e
#n1, n2 = ['n{}'.format(i) for i in e]
init.extend([
('Node', n1),
('Node', n2),
('Element', e),
('Printed', e),
])
assert not trajectories
# for t in trajectories:
# init.extend([
# ('Traj', t),
# ('PrintAction', t.n1, t.element, t),
# ])
goal_literals = []
if can_transit:
goal_literals.extend(
('AtConf', r, q) for r, q in initial_confs.items())
goal_literals.extend(('Removed', e) for e in remaining)
goal = And(*goal_literals)
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map,
init, goal)
def pddlstream_from_problem(problem, collisions=True, teleport=False):
#rovers = problem.rovers
domain_pddl = read(get_file_path(__file__, 'domain.pddl'))
stream_pddl = read(get_file_path(__file__, 'stream.pddl'))
constant_map = {}
camera = 'RGBD'
mode = 'color' # highres | lowres | color | depth
init = []
goal_literals = [
#('Calibrated', camera, problem.rovers[0])
#('Analyzed', problem.rovers[0], problem.rocks[0])
#('ReceivedAnalysis', problem.rocks[0])
]
init += [('Lander', l) for l in problem.landers]
init += [('Camera', camera), ('Supports', camera, mode), ('Mode', mode)]
for rover in problem.rovers:
base_joints = get_group_joints(rover, 'base')
bq0 = Conf(rover, base_joints)
head_joints = get_group_joints(rover, 'head')
hq0 = Conf(rover, head_joints)
init += [('Rover', rover), ('OnBoard', camera, rover),
('BConf', bq0), ('AtBConf', rover, bq0),
('HConf', hq0), ('AtHConf', rover, hq0)]
for name in problem.stores:
init += [('Store', name)]
init += [('Free', rover, name) for rover in problem.rovers]
for name in problem.objectives:
init += [('Objective', name)]
#initial_atoms += [IsClass(name, cl) for cl in CLASSES if cl in name]
goal_literals += [('ReceivedImage', name, mode)]
for name in problem.rocks + problem.soils:
# pose = Pose(name, get_pose(env.GetKinBody(name)))
init += [('Rock', name)] # , IsPose(name, pose), AtPose(name, pose)]
#init += [('IsClass', name, cl) for cl in CLASSES if cl in name]
#if problem.rocks:
# goal_literals.append(Exists(['?rock'], And(('Rock', '?rock'),
# ('ReceivedAnalysis', '?rock'))))
#if problem.soils:
# goal_literals.append(Exists(['?soil'], And(('Soil', '?soil'),
# ('ReceivedAnalysis', '?soil')))) # TODO: soil class
goal_formula = And(*goal_literals)
custom_limits = {}
if problem.limits is not None:
for rover in problem.rovers:
custom_limits.update(get_base_custom_limits(rover, problem.limits))
stream_map = {
'test-reachable': from_test(get_reachable_test(problem)),
'obj-inv-visible': from_gen_fn(get_inv_vis_gen(problem, custom_limits=custom_limits)),
'com-inv-visible': from_gen_fn(get_inv_com_gen(problem, custom_limits=custom_limits)),
'sample-above': from_gen_fn(get_above_gen(problem, custom_limits=custom_limits)),
'sample-base-motion': from_fn(get_base_motion_fn(problem, custom_limits=custom_limits, teleport=teleport)),
'sample-head-motion': from_fn(get_head_motion_fn(problem, teleport=teleport)),
}
#stream_map = 'debug'
return PDDLProblem(domain_pddl, constant_map, stream_pddl, stream_map, init, goal_formula)
from collections import namedtuple, defaultdict
from time import time
from pddlstream.language.constants import EQ, NOT, Head, Evaluation, get_prefix, get_args, OBJECT, TOTAL_COST, Action, Not
from pddlstream.language.conversion import is_atom, is_negated_atom, objects_from_evaluations, pddl_from_object, \
pddl_list_from_expression, obj_from_pddl
from pddlstream.utils import read, write, INF, clear_dir, get_file_path, MockSet, find_unique, int_ceil, safe_remove, safe_zip
from pddlstream.language.write_pddl import get_problem_pddl
filepath = os.path.abspath(__file__)
if ' ' in filepath:
raise RuntimeError('The path to pddlstream cannot include spaces')
#CERBERUS_PATH = '/home/caelan/Programs/cerberus' # Check if this path exists
CERBERUS_PATH = '/home/caelan/Programs/fd-redblack-ipc2018' # Check if this path exists
FD_PATH = get_file_path(__file__, '../../FastDownward/')
USE_CERBERUS = False
USE_FORBID = False
FORBID_PATH = '/Users/caelan/Programs/external/ForbidIterative'
FORBID_TEMPLATE = 'plan.py --planner topk --number-of-plans {num} --domain {domain} --problem {problem}' # topk,topq,topkq,diverse
FORBID_COMMAND = os.path.join(FORBID_PATH, FORBID_TEMPLATE)
def find_build(fd_path):
for release in ['release64', 'release32']: # TODO: list the directory
path = os.path.join(fd_path, 'builds/{}/'.format(release))
if os.path.exists(path):
return path
# TODO: could also just automatically compile
raise RuntimeError(