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(